Hybrid A*、Kinodynamic RRT*

本文主要讲解Hybrid A算法、Kinodynamic RRT算法，它们和前面讲的A*、RRT*算法流程相似，但有一些不同的地方。

---

一、Hybrid A*

1.1 基本思想

在之前的文章中，讲解了动力学约束下的State Lattice Planner，在线的生成稠密的Lattice graph会花费过多的时间，怎么样去做剪枝一些节点进行优化呢？
Hybrid A*的基本思想是就和Lattice Graph有关。它的基本思想是将栅格地图的路径搜索和Control Lattice Graph结合起来，让栅格地图中的每一个方格中只存在一个节点。

1.2 算法流程

选择不同的控制量u驱动向前去积分，积分得到的不同状态保持在每个栅格地图中的机器人可执行节点只有一个；如果有另外的控制量驱动向前积分的状态量落在一个栅格中而之前已经有了可执行节点，那么就比较这两条路径的代价函数，如果新的路径比之前的路径代价小，那就忘记之前的节点和路径，栅格更新节点为新的节点。

Hybrid A * 算法与A* 算法的算法流程有很多相似之处，这里我就只说一下它们有区别的地方；
首先，Hybrid A* 算法的g(n)、h(n)、f(n)和 A * 算法不同，Hybrid A* 算法的g(n)是到起点的路径的损失函数，但是它不仅包括A*算法的路径上的代价，还包括始末状态的误差函数、损耗的能量的大小等、h(n)也不再是A * 算法的欧氏距离等，而是不考虑动力学约束到终点的距离和不考虑障碍物到终点的距离的综合、f(n)是h(n)＋g(n)，那自然就不同了；a、b、c、d图分别是欧式距离、考虑动力学约束、考虑障碍物和动力学约束搜索路径的过程，可以看到明显考虑障碍物和动力学约束的启发函数采样更少的点，更快搜索到终点；

其次，Hybrid A* 算法在扩展寻找新的节点的过程也是不同的，它是通过选取不同的控制量u前向积分得到的；
再者，在扩展节点的时候，Hybrid A* 算法与A* 算法也是有差别的，在Hybrid A* 算法中，如果栅格中没有节点，那就把前向积分得到的状态记录在栅格中；如果栅格中已经存在节点了，就比较两个路径的损失，新的节点损失更小的话，就更新栅格里的节点。

1.3 Hybrid A* tricks

Hybrid A* 算法中的一个技巧就是one shot heuristic，思想就是在扩展搜索树的过程中，在当前节点计算与终点的理论最优路径，如果恰好满足动力学约束，就找到路径了，节省时间；
当然也不是每扩展一个新的节点就去做一次one shot heuristic，那样只会降低搜索速度，最好是设定一个参数N，比如N=20，每扩展20个节点后，做一次one shot heuristic，还可以随着扩展的过程，逐步降低N。

二、Kinodynamic RRT*

2.1 庞特里亚金极小值原理

Kinodynamic RRT* 与RRT*不同，它并不是用直接连接采样点，而是解一个Optimal Boundary Value Problem（OBVP 最优边界值）问题，求出解之后用曲线光滑地连接两点，OBVP问题的求解需要用到庞特里亚金极小值原理，下面来详细讲一讲庞特里亚金极小值原理，以及怎么应用它解OBVP问题；
1）首先说问题以及怎么对问题进行建模；

因为现在的问题是想要求连接两点的曲线的方程，把损失函数定义为从一个状态到另一个状态的跃度的平方最小，这样消耗的能量最少，状态量就是车辆当前的位置、车辆的速度、车辆的加速度，函数的方程就是状态量的导数也就是车辆的速度、车辆的加速度、车辆的跃度，这就是模型；

2）哈密尔顿函数；应用庞特里亚金的公式，还需要知道几个概念，先来把需要知道的概念介绍完，庞特里亚金极小值原理需要定义哈密尔顿函数，我们来看一看怎么定义哈密尔顿函数；

h（s（T））是对最终状态的惩罚，也就是未到达最终想要的状态的惩罚，分为软约束和硬约束，如果是硬约束哈密尔顿函数中就不需要有h（s（T））这一项了；另一项函数g是对状态转移过程的代价的计算；u和s代表的是最优的输入量和状态量；这道题目的哈密尔顿函数的定义如下图所示；

λ1，λ2，λ3称为协态变量，有几个状态量就会有几个协变量，哈密尔顿函数第一项是系统输入，后几项是协变量和系统模型乘积的函数；
3）庞特里亚金极小值原理；庞特里亚金极小值原理公式说的是协变量在下图的第一个方程中在最优状态、最优输入的时候，对状态s求偏导再加符号得到的就是协变量的导数；该公式满足终态的惩罚函数在T时刻的最优状态输入下对状态s求偏导再加负号（注意这个边界条件仅仅在软约束条件下有，如果是硬约束，也就是必须到达终点，那么就没有该项约束了）；
最后一个公式想说的是在最优状态s下，因为上个公式我们求出来了λ1，λ2，λ3，我们带入这个方程最小化哈密尔顿函数就可以得到最优的输入u了。

这就是庞特里亚金原理的原理。

2.2 Optimal Boundary Value Problem（OBVP ）Problem Example

上面讲解了庞特里亚金极小值原理，下面用一个例子来讲解庞特里亚金极小值原理的应用，搓搓小手开始吧；

1）协变量λ的导数为0，-λ1，-λ2，这里是因为我们的哈密尔顿函数状态s和输入u已经是最优的，那就是常数，此时对λ求导，加上负号就是（0，-λ1，-λ2），这里大家应该可以懂，比较简单；
2）因为λ1的导数为0所以它的函数就是常数，λ2的导数为-λ1，所以它就是对λ1积分得到的是关于时间t的一次函数，同理λ3是关于时间的二次函数，也就是下图所示的函数；

3）在最优状态s下，因为上个公式我们求出来了λ1，λ2，λ3，我们带入这个方程最小化哈密尔顿函数就可以得到最优的输入u了，此时因为状态s是最优状态s*，所以哈密尔顿函数里就有v和a了，那么我们最小化j（t）只需要对方称求导得到了（（2/T） * j + λ3 ）= 0，j = -0.5T * λ3， 此时就得到了如下的最优输入量u*；

因为状态s是加速度的导数跃度，所以求车辆的状态位置p、速度v、加速度a就是对状态u积分就得到了，得到了最优状态u*如下图所示；

现在还有α、β、γ未知，那么我们就用末尾时刻的边界条件就可以得到了α、β、γ都是关于T的表达式，从而得到了损失函数J就是一个只关于时间T的函数，如果我们给出了时间T那么所有的量就都求出来了，如果时间T位置就求出这个多项式的根然后就可以求出所有的量了，多项式就有太多种方法求根了，可以用二分法、牛顿法等等，讲解完毕。

2.3 算法流程

第一点不同之处在于，在采样的时候原先RRT*算法是车的位置x、y进行采样，Kinodynamic RRT *算法是对状态量进行采样，不仅包括车的位置x、y，还包括车的速度、加速度等量；

第二点不同之处在于，RRT*算法在连接两个状态的时候，把机器人当成了质点，用直线连接了两个状态，Kinodynamic RRT *算法在连接两个状态的时候，通过庞特里亚金最小值原理解决两点边界值最优的问题找出一条最优且适合机器人执行的路径，庞特里亚金最小值原理上面已经讲清楚了；

第三点不同之处在于RRT*算法中，节点在寻找它的父节点的时候，计算的是代价是到达起点的距离，在Kinodynamic RRT *算法中，节点在寻找它的父节点的时候，计算的代价不仅包括到达起点的距离，还有消耗的能量u的大小，以及始末终端误差等。

---

总结

以上就是今天要讲的内容，本文主要讲解了Hybrid A算法、Kinodynamic RRT算法，谢谢观看。