五、Hybrid A * 算法总结

     （1）首先，将得到的起始点、目标点的位姿信息，以及环境中障碍物的信息处理成我们需要的格式，并得到一个包含障碍物信息的栅格地图obsmap ，为后续的规划做准备。

  （2）然后在栅格地图obsmap上，调用传统的 A * 算法，从目标点开始不断地进行拓展（往open_set中添加节点），同时将扩展过的节点从open_set移到closed_set中。直至open_set为空，此时栅格地图obsmap中所有非障碍物的节点都被拓展过了，存放在closed_set中，且这些节点中都储存了从目标点到该节点的实际距离（代价值），并将代价值存储在一个地图中，至此，我们得到了一个启发式地图hmap，对于障碍物点启发式地图hmap中对应位置为inf，否则则存放从该点到达目标点的实际代价值，作为Hybrid A * 算法中考虑障碍物但不考虑动力学约束的启发式函数值。

  （3）将开集合open_set初始化为起始点，闭集合初始化为空集，并初始化优先级队列，以节点的总代价值作为优先级评判依据，总代价值越低，优先级越高，即每次从open_set中弹出总代价值最小的节点作为拓展点。至此，准备工作已完成。

  （4）开始主循环，循环执行以下步骤，直至找到路径或open_set集合为空时，结束循环。

        ① 判断open_set集合是否为空，若为空，则结束循环，规划失败。若不为空，则执行下一步

   ②、选取本轮循环拓展点，从open_set集合弹出总代价值最小的节点作为本轮循环的拓展点，并将该点添加到闭集合closed_set中(表示该节点已经拓展过了，或者即将进行扩展），

   ③、以一定的概率尝试调用Reeds_sheep算法查找48种reeds_sheep曲线中从当前扩展点到目标点的所有可行曲线，若不存在可行曲线，则继续下一步，若存在可行曲线，则对所有可行曲线的代价值进行计算（前面介绍的程序中这里的代价计算考虑了倒车惩罚代价、转向惩罚代价、角度变换惩罚代价等各种惩罚值），然后将其存放在一个优先级队列中，然后依次从该优先级队列中选取代价值最小的路径，采样路径点，进行碰撞检查，直至找到通过碰撞检查的路径或所有的路径均没有通过碰撞检测。对于前者的情形，我们成功找到了从当前拓展点到目标点的路径，将该路径与从当前拓展点回溯到起点所得到的路径拼接起来，即得到了从起始点到目标点的可行路径，停止循环。对于后者的情况，说明没有找到从当前拓展点到目标点的无碰撞Reeds_sheep曲线，继续执行下一步。

   ④、对当前拓展点进行拓展，从左转的最大角度到右转的最大角度中选取n个采样点，即选取n个前轮转向角，对于每个转向角，再考虑车辆前进后退两种运动形式，即在当前拓展点处朝着2n个可能的方向进行拓展。

   对于每个方向均执行以下操作：

   沿着该方向向前模拟一段路径，判断是否超出地图边界或者与障碍物相交，若是则放弃向该方向拓展，若不是，则计算该方向模拟路径的代价值（这里的代价值同样可以考虑倒车惩罚、切换方向惩罚、转向角惩罚等），从而得到从当前拓展点到该模拟路径末端节点的成本。

   接下来检查该末端节点所属的栅格是否已经闭集合closed_set中，若是则说明该栅格早就已经拓展完成了，不用再加入到open_set中。 若不是，则进一步判断其是否在open_set中，若不是，则计算该节点的总代价值（从起点到该节点已花费的实际代价+从该节点到目标点预估启发代价），并将其添加到open_set集合中，若其已经在open_set集合中，则判断从起点到该末端节点的代价值是否小于从起点到该栅格中存储的之前的末端节点的代价值，若是，则对该栅格中存储的末端节点及代价值进行更新，否则，不更新。

   对所有方向执行完以上操作后，当前拓展点的拓展就完成了，同时本轮循环也完成了，跳转到第①步，继续进行下一轮循环。

  （5）进行目标点开始进行路径回溯，提取出从起点到目标点的可行路径。
 参考资料：  
https://blog.csdn.net/qq_44339029/article/details/132466521