Hybrid-state A* path planner的一些理解

看过网上很多关于hybrid A*的介绍，感觉都没真正讲清楚核心的hybrid state。在这里简单记录一下我对hybrid-state A*的理解。

首先hybrid A*本质上也是图搜索，所以同样需要定义graph。在文献[1]中作者明确说明了graph中的结点（node）为离散2D栅格的中心。而每个node，或者说每个栅格又包含一个连续空间下的3D state。Graph中的node的key值与其包含的3D state的f值一致。

初始化阶段，通过当前连续空间下的位姿计算对应的2D栅格，将二者关联，并将这个2D栅格（node）加入到优先队列open list中。在搜索过程中，从open list中取出key值最小的node（2D栅格），对该2D栅格包含的3D state，基于运动学模型计算其在一定时间内前进或者后退到达的位姿（后面称之为模拟位姿），并计算模拟位姿的g值和h值，得到其f值。此外，计算模拟位姿对应的2D栅格。假如模拟位姿对应的2D栅格已经在open list中，且模拟位姿的f值小于其对应的2D栅格当前的key值，则用该模拟位姿取代其对应的2D栅格中包含的3D state，并更新2D栅格（node）的key值。

可以看到，hybrid-state A*是采用离散栅格对连续空间的search branch进行枝剪，以牺牲完备性和最优性来换取效率。

此外，论文中还提到在expand过程中，除了通过运动学模型计算模拟位姿外，还计算一条连接当前3D state和规划目标（也是3D state）的Reed-Sheep曲线，并进行碰撞检测。如果是无碰撞路径，则更新规划目标的f值并将其加入到open list中。当规划目标附近区域障碍物密度较小时，通过这种方式可以“一步到位”获得最终路径，而不用按照离散的栅格一步一步往目标搜索。

整体来看，hybrid-state A*是很启发式很工程化的路径规划方法，没有严格的理论基础，也难怪作者在其会议论文的题目中用practical search。如果不是有DARPA城市挑战赛的成绩和斯坦福Thrun实验室的背景，很难想象这种方法能够发表在IJRR上。

顺带说一下，在我看来A*及其变种方法，实现起来最难的部分其实是优先队列open list。open list一定要自己写（用二叉树去实现），不要调用boost库或者STL库。理由很简单，expand的过程中，假如待expand结点的successor的g值降低了，且该successor已经存在于open list中，此时应该更新该successor的g值和f值，并调整其在优先队列中的顺序，而不是将该successor又重复插入到open list中。

[1] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, "Path planning for autonomous vehicles in unknown semi-structured environments," International Journal of Robotics Research, 29(5), 485-501.