Hybrid A* 路径规划

参考:

Hybrid A* 的使用场景​ 在斯坦福大学2007年参加的DARPA无人车城市挑战赛时使用的Junior，其在行为规划层提出了分层有限状态机的方式，如下图所示。

​ 其中，BAD_RNDF状态表示的是，当前道路与系统的路网图不同的时候，无人车将采用Hybrid A*来进行规划路径。

Hybrid A 与 AHybrid A* 的主要特点是：

考虑物体的实际运动方向约束，不像A假定所有的相邻节点都可以顺利转移
A 的物体总是出现在栅格中心，而 Hybrid A 则不一定
Hybrid A 是连续路径
Hybrid A 基于A

算法流程:主要流程如下图所示，需要说明的是：
​ 算法考虑了车辆的x,y坐标，偏航角。

G值更新策略如下：

H值更新策略如下：
可以支持倒车时计算当前节点到终点的，仅支持前向行驶时计算Dubins曲线；
H值为Reeds-Shepp曲线、Dubins曲线、曼哈顿距离三种cost解算出来的最大值。

代码参考：

Rviz实验效果