动力学约束下State Lattice planner

本文主要介绍动力学约束下的Lattice planner。

---

前言

动力学约束的概念

首先，我们要知道为什么需要Kinodynamic Planning（动力学约束下的规划）？因为我们虽然在后端有轨迹的优化，有如下的原因必须Kinodynamic Planning：其一，如果规划出的轨迹是满足最小化损失的，轨迹不一定适合机器人去执行，非常的反直觉，比如下图的紫色虚线的轨迹虽然是最小化损失函数的，它显然不适合机器人去执行,因为在起始位置规划的轨迹有一个急转弯，很不适合机器人去执行，如果对紫色的虚线进行优化，就得到了紫色的实线，紫色的实线显然也不适合机器人执行;而绿色的虚线是Kinodynamic Planning的结果，轨迹优化后得到的绿色实线是比较适合机器人执行的路径；其二，轨迹优化是对局部的轨迹进行优化，不可能对全局路径都进行优化，那样既不现实同时要求的算力过高了。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、State Lattice Planning

1.1 回顾

首先来回顾一下我之前讲的基于搜索的算法Dijsktra和A*，以及基于采样的算法RRT和RRT*。
1）基于搜索的算法是在搜索图中搜索一条从起点到终点可行的路径；
2）基于采样的算法是在空间中采样一些点，构建出一颗搜索树，然后从终点开始寻找父节点，直到找不到父节点，也就是搜索到了起点，这时候也就找到了一条从起点到终点的路径。

1.2 构建可行连接的图

State Lattice Planning也就是状态栅格搜索算法，它构建带有可行连接的图有两种方法，分别是正向方法：离散化机器人的控制空间，驱动机器人往前运动，得到控制空间许多的可行连接；另一种方法是反向连接：不离散机器人的控制空间，离散机器人的状态空间，在机器人周围的环境里离散出很多状态，然后再找当前状态到某一个状态的连接；

该图为机器人控制空间的离散，选择机器人的控制量，对它进行离散或者说采样都是一个的意思。

概率路图算法在空间中采样很多点，然后进行一个连接，这就是对状态空间的离散，当然这里的状态比较简单，就是机器人的坐标x和y，此时未考虑机器人的动力学约束，只是将其看成一个质点。

1.3 控制空间和状态空间采样的对比

首先我们需要已知一些东西，状态方程我们需要知道，状态s包含机器人的坐标x、y、x’、y’等，状态方程的u为控制量，可以认为是系统的输入量，我们还需要知道机器人初始的状态s0；
来看一看在控制空间的采样，给定一个固定的时间步长T，给定不同的控制量u，机器人会走到不同的表现，这就是前向的仿真，这里的仿真指的是状态的仿真，不是仿真器的仿真，是通过前向积分得到的；它的优点是易于实现，通过前向积分得到；它的缺点是没有任务的导向，同时导致产生很好的规划结果又较低的效率。
再看一看在状态空间的采样，采样了起始状态和末状态，状态包括车辆的位置和朝向角等姿态信息，我们要从两个状态中解出连接两个点的一条边，也就是这个曲线的方程，要把U和T都计算出来，如果U和T计算比较困难的话，可以给定固定的时间步T，去计算U；优点是它会有非常好的任务导向，会具有启发性或者贪心的性质，缺点在于比较难以实线；

下图展示分别是在控制空间对控制量U进行9次采样和25次采样，也可以认为是对控制量U进行了划分，划分为9或者25份，得到的Lattice graph；

那么这里我们值得说的一点是，我们在搜索起点到终点的路径的时候不是要非把图构建好再去找路径，而是可以在构建图的过程中就开始搜索了。

状态空间的离散化，考虑汽车模型（汽车不能侧滑），那我们就可以把想要到达的状态得到之后，去反算这些边，反算连接方式，然后继续去算这个状态与下一个想得到的状态之间的连接方式，如此循环下去，直到找到终点，就得到了State Lattice Graph；


来看一看控制空间的采样和状态空间采样带来的不同结果，离散控制量是不合格的，很多控制量都开到路外面去了，落在马路上的部分少，所以可用的部分较少；在状态空间采样，就是在马路道路线内寻找状态，反算连接状态的曲线。离散化控制空间的自由度比较小，可以看到控制空间采样的间隔即控制量的离散程度也是一个需要考量的指标，，它们具有同质化的特点，因为如果控制空间离散化比较多份，那么如果有障碍物在控制空间，那么它周围的采样也是会碰撞到障碍物的。

现在说了很多关于控制空间采样低效，状态空间采样高效，那大家可能会有疑惑，为什么不都用状态空间采样呢？这是因为状态空间采样虽然效果很好，但是很难去计算中间转移的局部状态，那么这就需要我们接下来讲的（Boundary Value Problem）两点边界最优控制问题来解决怎么计算中间的轨迹关于时间的参数，以及控制量关于时间的参数计算出来。

二、Boundary Value Problem

2.1 Boundary Value Problem

首先来看一看怎样解决Boundary Value Problem（边界值问题）

边界值问题会给定初始状态和末尾状态，我们用五次多项式来连接始末状态，我们可以看到始末状态的条件如下图所示，轨迹有6个参数，但是我们只有两个方程，我们可以得到很多解但不一定是最优的，而且我们不知道哪个是最优的。因此就引出了（Optimal Boundary Value Problem）边界最优控制问题来寻找最优的轨迹。

2.2 Optimal Boundary Value Problem

1）解决边界值最优控制问题，首先需要建立系统的模型，需要用到庞特里亚金最小值原理和哈密尔顿函数，这里会给出庞特里亚金最小值原理的公式以及怎样去定义哈密尔顿函数，这里只是用到，大家知道就行了，想知道怎么来的去看他们的论文看推导过程，这里不讲他们的原理。

系统的模型：J为损失函数，jk是加速度的导数跃度，Uk=jk，系统的状态方程就是对系统的状态求导；

2）庞特里亚金最小值原理

首先损失函数J的是由终端状态的约束和对状态转移成本两部分组成的；而下面定义哈密尔顿函数就是状态转移成本函数加上协变量乘以系统的状态方程得到的（协变量的个数取决于系统的状态的个数）；
其次来说一说左边的极小值原理部分，其一，协变量的导数是在状态s和输入u取最佳的时候，对哈密尔顿函数的状态s求导得到的；其二，协变量也必须满足终端状态的约束；其三，最佳状态u是在状态s取最佳状态时，同时把计算出来的协变量也带入哈密尔顿函数，然后哈密尔顿函数对控制量u求导，令导数等于0，得到最佳控制量u的值。

3）一个OBVP问题的例子
第一，因为损失函数J的加速度的导数跃度的平方的函数；所以哈密尔顿函数定义成如下的形式：

当状态s和控制量u取最佳时，对状态s求导得到了协变量的导数如上图所示；

第二步，我知道了协变量的导数，积分就可以得到协变量的表达式；
同时在状态s取最佳的时候，哈密尔顿函数里的v和a就变成了v* 和 a*，那么在最小化j（t）的时候，他们就是常量了，哈密尔顿函数对跃度j求导就得到了 j = -（T/2）* lambda3；代入上面得到的lambda3表达式就计算出了u*(t)，系统的状态p、v、a是对状态量积分得到的，因此对求到的控制量积分就得到了s*(t)，代入初始的状态，就得到了s*(t)，形式如下：

同时在代入终端状态求三个系数，得到了如下的方程：

我们得到了待求的三个系数α、β、γ是关于时间T的函数，损失函数J也是关于T的函数，那么我们如果给定一个固定的时间步T，所有的量就全部求解出来了。

4）一个小细节

可能有些同学有疑问，损失函数J在定义的时候为什么终端状态的约束条件没加到其中去呢，因为前面说了损失函数是由终端状态的约束和对状态转移成本两部分组成的呀！这里是因为终端状态的约束不是一个软约束，软约束的意思是你离我终端状态越近，这个损失函数越小，达到终端状态的时候这个损失函数就为0；这里的终端状态约束是一个硬约束，就是达到终端状态的时候这个损失函数就为0，没有达到终端状态的时候损失函数为∞，因此终端状态的约束这个函数是不可导的，也就无法将它加入损失函数中。这个问题解答完了，这个OBVP问题就解决了。^-

5）另外一个小例子是给定汽车的初始状态和末状态，如何去生成一条带有约束的轨迹，这里计算过程类似，只讲一下区别所在吧；汽车的约束包括不能侧滑等，以及汽车的模型很难线性化，因此我们求得的不是显示解，只是一个数值的解析解。

6）另外会有一个做法叫Trajectory Library（轨迹库），轨迹库其实就是单层的State Lattice Planner，这个做法的好处是非常简单，机器人所处环境不是特别复杂的情况时，轨迹库是非常好的选择，就去测试哪些轨迹运行效果比较好就选取那部分轨迹执行即可。

7）启发函数

在这个Latice Graph采样搜索过程中，我们同样希望能够像A*算法的启发函数一样加速搜索的过程，启发函数是用来估计到终点的距离；这无外乎两种：不考虑动力学约束、不考虑障碍物，工程上采取两个加在一起或者两者较大的作为启发函数，效果是非常好的。

8）Frenet坐标系下的Kinodynamic Planning
跟前面的比较类似，不同的地方在于汽车在道路上行驶，道路上有车道线，我们会希望车辆做一个车道线跟随的任务，对于这样的任务，这时候笛卡尔坐标系就变为了沿着车道线径向和切向的Frenet坐标系，对车辆的控制量用五次多项式进行参数化；接下来给定初始状态和末状态，解一个最优控制的问题即可。

---

总结

以上就是今天要讲的内容，本文介绍了动力学约束下State Lattice planner，谢谢观看。