Kinodynamic Path Finding

一、引言

1.概念

Kinodynamic = kinematic(avoid obs)+dynamic(bounds to v、a、force)

2.motivation

既然后端可以轨迹优化为什么前端还需要做符合动力学约束的轨迹？

经典motion plan框架:前端搜索+后端轨迹优化

答：

plan是一个从粗糙到细致的过程；后端轨迹优化只能局限在局部；不可行的参考路径对于非完整约束系统来无意义

例1：

                                                       

对于左图无人机规划问题，在不考虑动力学约束的情况下（初速度约束）规划出的path应该为紫色实线，kinodynamic planning则规划绿色实线，后端优化只局限在局部，不能改变整体趋势，所以紫色实线在优化为虚线后会拐弯，对比之下绿色虚线轨迹更加平滑。

例2：

对于右图车辆规划问题，车辆是非完整约束系统，无法横移，如若按照质点考虑的path则毫无意义

二、Lattice Search

1.Basic idea

(1)原则

graph中机器人的移动必须符合机器人运动规则，而不再可以随意移动

(2)控制空间采样与状态空间采样

①正向方法

离散control space,我感觉可以理解为dwa的思想，离散许多控制量，然后前向执行一段时间，获得离散connection

②逆向方法

从机器人周围离散许多state space，计算从当前状态到下一个状态的连接(connection)

(3)两类实例

①基于搜索的规划中获得节点之间的连接性就是状态空间离散

②基于采样的规划中获取边就是状态空间离散

2.基于差速机器人对比两种采样

首先建立差速模型，设置初始速度，后通过两种方式获取可行plan

(1)控制空间采样

给与系统不同的输入u,保持一段时间T，前向积分，获得控制空间离散

但是这种离散没有任务导向，使得效率低下

(2)状态空间采样

在空间中采样一个末尾状态(包括位置与姿态信息)，根据初始状态与末尾状态解出中间这条边

这种规划具有到导向性，但是很难计算

3.控制空间离散无人机实例

（1）建模

如图所示，系统状态为位置与速度，输入为加速度，得到A、B矩阵，是一个积分模型

（2）高阶拓展

也可以拓展为更高阶的模型，将jerk(加速度的导数)作为u，则状态为位置、速度、加速，得到一个更高维度的A、B矩阵

（3）幂零矩阵

但是！！无论是几维度的A，A矩阵都是幂零矩阵，也就是说A的某次方为零阵

（4）状态转移方程

前向sample后获得末尾状态，如何通过初始状态与末尾状态获得轨迹，此时需要状态转移方程s(t),这时候A是幂零矩阵就有助于求解状态转移矩阵了

4.Lattice graph构建

给定一个初始的状态，给定不同的控制输入，让系统到达不同的位置，继续离散控制输入，又会获得不同的位置

U离散九份、二十五份：

注意：不一定是做所有的graph做完之后再进行搜索，也可以具有导向性，也可以边搜索边建立graph，这样可以节省计算时间

5.控制空间离散车辆实例

选取一个初始状态，离散u，前向积分，选取不与障碍物碰撞的结果

6.状态空间离散

（1）离散的是末尾状态

（2）状态空间离散受初始状态影响小

两层lattice graph在不同初始状态下的sample：只有第一层不同，第二层相同

7.控制空间采样与状态空间采样性质

控制空间采样无目的性，容易违反边界条件、而且容易受到初始条件影响大(状态空间采样只有第一层不同，第二层相同)、控制空间受初始状态影响导致采样同质化严重

状态空间采样是由于反算，所以具有目的性，但是bvp问题很难计算