考虑机器人运动学模型下的路径规划方法

考虑机器人运动学模型下的路径规划方法仍然可以分为基于搜索的以及基于采样的路径规划方法

什么是Kinodynamic？

Kinodynamic可以这样理解：Kinematic + Dynamic
其人话翻译就是：一种受生成机器人的运动同时受限于运动学的约束，比如说避障，以及动力学的约束比如说速度加速度力之类的上下限约束，这就是Kinodynamic的定义。
约束条件
做一个Kinodynamic都是要考虑如下点的

• 高阶模型微分条件的约束（非质点模型）
• 力的约束（加速度）

为什么要Kinodynamic？

因为现在机器人运动是无法当做一个质点来看待的，不符合运动学的约束，假如轨迹的生成是一条条折线，这样的路径直接交给机器人去执行肯定是不行的，所以之前的A*，Dijkstra，JPS，都是按照质点去执行的，且轨迹都是一条条折线组成的Path，机器人执行肯定不行。而且总是要有一个模型，自身具有的，不能当成一个质点来吹。

eg:

在前端找path的时候，如果只是去寻找一个最优化的路径，只考虑了最短路径的path，用主流的轨迹优化得到的轨迹就是一段段的折线，然后优化的轨迹就是不平滑的轨迹，但是在找path的时候，考虑了一些动力学约束的情况下，就会找到一个平滑的轨迹。

构建Feasible motion connections

Feasible motion connections
就是说路径当中的每两个节点，都能进行连接且满足机器人实际的路径需求。

正向方法

机器人的控制空间进行离散，驱动机器人往前运动，得到feasible motion connections。

• Forward simulation
• Fixed
• No mission guidance
• East to implement
• low planning efficiency

具体事例

无人机系统建模，输入加速度，状态是位置与速度。
状态空间建模：

A阵，叫做nilpotent，幂0矩阵，就是A的i次方= 0，A的i+1次方也等于0。

现在建模后，给定初始的运动状态，比如说是速度加速度时间之类的，就会使得激励推动系统运动，进而系统就演化成这样。

这些紫色的轨迹，中间的这个过程就是一开始所说的feasible motion connections。

同样的，对于高阶模型有微分条件的约束，现在也可以把系统阶数升高，弄成输入为x,y,z的三阶导数，初始条件就变为：
三阶积分系统，此时的状态S就有位置，速度，加速度。

现在feasible motion connections就成了下图所示：

在得到初始以及末端节点的时候，feasible motion connections该怎么进行连接以得到这种平滑的轨迹？

使用状态转移方程

前半部分是零输入响应，后面是零状态响应。
这方程就是，把一个线性系统在控制量Um激励下往前做前向积分函数的表达式。
一般计算的时候把这个状态转移矩阵进行展开，这时候A是一个幂0矩阵很重要了，因为高次方的A都是0，这时候计算就很便利了。

在控制空间中离散

此时不像A*，Dijkstra，JPS算法那样，栅格地图天生就在那里，是已经建立好了的，在控制空间当中，图不是栅格地图，而是lattice graph。

lattice graph不是天生存在的，是机器人在线搜索（启发式函数）并构建出来的。
搜索树扩展方法

反向方法

不去离散控制空间，离散机器人的状态空间，产生很多状态，再去找当前状态到某一状态的连接。

• Backward calculation
• Need calculate
• Good mission guidance
• Hard to implement
• High planning efficiency

离散状态空间

现在采用的是一个Reeds-Sheep Car Model，汽车是不能左右侧滑的，只能正向/反向进行运动。
在这里，就不是去离散控制了，是把汽车周围的状态进行离散，离散周围的状态，再去反算是怎么把状态与汽车连接起来。

最后构成了这个lattice graph

双层的lattice graph

一般做局部的规划一层就够了。

结尾

综上，在控制空间当中进行采样，是没有目的性的，效率很低的，采样出来的结果很多是不合格的。

比如说这两辆车，在控制空间当中都飞到外面去了，在马路上的有效采样很少。
在状态空间中采样，落在马路上的就很多。

但是，状态空间采样很难实现，这时候就要用Boundary Value Problem的这个思想去解算。