4.6.c Hybrid A*代码解析

4.6.c.1 构图的代码实现

本节构建的图比较简单，主要包含map border（board_x和board_y）和obstacles（ox和oy），读者也可根据需求修改构图方式。

在Hybrid A*算法中，基于环境信息，起始点位置，目标点位置以及x, y ,yaw 的分辨率搜到最优路径：

start:起始点信息：x，y，yaw

goal:目标点信息：x，y，yaw

ox:障碍物和地图边界的x坐标序列

oy:障碍物和地图边界的x坐标序列

xy_resolution: x和y的分辨率，用来encoding坐标值

yaw_resolution:yaw的分辨率，用来encoding横摆角

最终返回一条路径：path: 包含了一系列的x, y和yaw信息

🌞Note: 在进行碰撞检测时会使用kdtree来加速

另外hybrid a star算法中，启发式函数时使用动态规划计算所有点到终点的距离，此时我们是考虑了障碍物的影响的。

在进行一个节点的analytic expansion时我们使用的时Reeds Shepp 曲线：

在main 函数中设置起始点，目标点，grid的分辨率和yaw的分辨率，在创建grid map之后，并运行Hybrid A*算法，即可找到最优路径。如下图所示，红色路径即为最终Hybrid A*搜出来的最优路径。