Hybrid A*、Kinodynamic RRT*

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

---

Notes：从事无人驾驶决策规划或规划控制研究方向需要学习咨询或者致力于从事该研究方向需要学习指导的朋友们欢迎咨询交流。

一、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图分别是欧式距离、考虑动力学约束、考虑障碍物和动力学约束搜索路径的过程，可以看到明显考虑障碍物和动力学约束的启发函数采样更少的点，更快搜索到终点；

其