论文阅读之混合A*

《Practical Search Techniques in Path Planning for Autonomous Driving》
路径规划算法，可以在未知环境中行进的自动驾驶车辆生成平滑的轨迹。

混合状态A搜索，Hybrid-State A Search

我们将A算法应用于车辆的3D动力学状态空间。搜索空间$(x,y,\theta )$是离散的，但每个离散状态不局限于空间网格的中心。

三种图搜索算法的比较：A算法访问的状态在每个网格的中心位置（左侧）；Field D和Theta算法在每个网格的角点处计算成本，并允许网格之间的任意线性路径（中间）；混合A计算网格内连续状态的成本，并将该成本作为单元格的成本。
因为一个单元格合并了多个连续状态，混合A算法无法保证找到最优路径。但是混合A算法能够保证结果路径是可行使的。且通常，混合A算法得到的结果路径通常在最优路径附近，我们可以通过优化算法获得全局最优解。
算法允许倒车，但惩罚倒车以及切换运动方向。

启发函数

算法参考了两种启发函数：non-holonomicwithout-obstacles，即忽略障碍物，但考虑车辆的non-holonomic约束。我们假设目标状态为$(x_g,y_g,{\theta }_g)=(0,0,0)$，并计算每个点$(x,y,\theta )$到达目标点所需的最短路径作为启发函数。
算法使用max(2D欧式距离，non-holonomicwithout-obstacles成本)作为启发函数。
而欧氏距离启发函数，由于考虑了障碍物，它可以发现2D中所有的U型障碍物和死角，然后产生更昂贵的代价函数，引导搜索远离这些区域。

A*算法使用欧几里得距离作为启发函数：a，扩展了21515个结点，c，扩展了68730个结点（复杂环境导致了大量的无用搜索）
non-holonomic-without-obstacles作为启发函数：b，只扩展了1465个结点，d，只扩展了10588个结点。

扩展结点

上述正向搜索使用控制动作转向的离散空间。
使用控制动作的离散空间进行前向搜索，且使用基于Reed-Shepp曲线。通过使用特定的控制动作在短时间内（对应于网格的分辨率）模拟汽车的运动模型来扩展树中的节点。

使用Voronoi场的路径成本函数

使用Voronoi场来权衡路径长度与距离障碍物的距离。


a)voronoi场，b)vorinoi图，c)其他势场（标准电势场）。可以看出，其他势场中，狭窄隘口有很高的势，不利于通行。

局部优化和平滑

混合A算法产生的路径并非最优的，进行两个阶段进行优化：1. 对轨迹上的顶点进行非线性优化，以改善解的长度和平滑度。2. 执行非参数插值。

对轨迹上每个结点进行位姿优化。

红色线是混合A产生的轨迹，蓝线是对结点优化后的轨迹。
使用上述优化方法（CG平滑），我们得到的路径比混合A*得到的解更平滑，但它仍然是分段线性的。
我们需要对结点进行曲线插值，因此，我们使用非参数插值。下图显示了对路径结点进行插值的结果。