路径规划实用搜索技术的学习笔记

参考文献《Practical Search Techniques in Path Planning for Autonomous Driving》

作者介绍如下：－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－

Hybrid A*是首次由斯坦福于2010年提出：一个实用的路径规划算法，该算法可以为在未知环境中运行的自主车辆生成平滑的路径，其中障碍物由机器人的传感器在线检测。

因为不想让这篇交流笔记变为数学课，所以我省略了很多公式。另外，写这个的主要目的是希望能和各位大佬一起交流讨论。

方法包含两个主要步骤:
1、使用A star的变体，应用于车辆的3D运动状态空间（考虑了汽车的运动学特性），但使用修改后的状态更新规则，在A*的离散节点中捕获车辆的连续状态（从而保证路径的运动可行性）；

2、通过数值非线性优化来提高解的质量，从而得到局部（通常是全局）最优解。

左：A star 将成本与单元格中心关联，并且只访问与网格单元格中心相对应的状态。
中心：Field D*（Ferguson and Stentz 2005）和Theta* (Nash et al. 2007)将成本与单元格角点关联，并允许单元格之间的任意线性路径。
右：混合A*将一个连续状态与每个单元格关联，单元格的分数是其关联的连续状态的成本

此算法包含两个阶段：
1、在连续坐标中使用启发式搜索，以确保计算轨迹的运动学可行性。通常会产生一个位于全局最优值附近的轨迹；

2、使用共轭梯度（CG）下降来局部改善路径规划解的质量，至少产生局部最优的路径，但通常也会达到全局最优。

引入势场的问题：在狭窄的通道中产生高势区，从而使这些通道实际上不可规避。为了解决这一问题，我们引入了一种基于工作空间几何结构重新划分区域的可能性，允许在狭窄通道中精确导航，同时有效地将机器人推离更开阔区域中的障碍物。

Hybrid A star 不能保证找到最小代价解，因为它合并了在离散空间中占据同一栅格的连续坐标状态。但得到的路径保证是可驱动的（A*是分段线性的）。Hybrid A star 通常使用在全局规划中，是通过使用梯度下降来局部改善路径，从而让 Hybrid A star 在全局规划中最优解。

混合状态A*的主要优点体现在紧致空间的驱动中，此时离散化误差变得至关重要。

算法考虑正向和反向运动，并对反向和切换运动方向进行惩罚代价增加。

混合状态A*搜索

通过A star的变体，使用了一种改进的状态更新规则，该规则捕获A star 离散搜索节点中的连续状态数据。比如在传统A star 中，搜索空间（x，y，θ）是离散的，但又不像传统A star那样只访问单元中心。混合状态A*与每个网格单元关联车辆的连续3D状态。

此算法由两种启发式指导：

二维欧氏距离扩展21515个节点（a）。
无障碍非完整启发式算法是一个显著的改进：它在（b）中扩展了1465个节点，
但在更复杂的环境中可能导致浪费性的死胡同探索：在（c）中扩展了68730个节点。
这是通过使用后者结合0完整的障碍启发式：10588节点在（d）。

第一种启发式：non-holonomic-without-obstacles：忽略了障碍物，但计算了汽车的非完整性。

为了计算，将终点的状态（x_g, y_g, θ_g）设为(0,0,0), 并在完全没有障碍物的情况下，计算从目标的某个离散邻域中的每个点（x，y，θ）到目标的最短路径。然后，我们使用non-holonomic-without-obstacles 代价的最大值和二维欧几里的距离作为我们的启发式。这种启发式方法的效果是，它修剪了一些搜索分支with the wrong headings。（这种启发式方法不依赖于运行时传感器信息，因此可以离线完全预先计算，然后简单地转换和旋转来匹配当前目标点。在实际驾驶场景实验中，该启发式算法在直接2D欧式距离代价上，在节点扩张上提供了接近数量级的改进。）

第二种启发式：

是第一种启发式的双重性，它忽略了汽车的非完整性，但是利用障碍图在二维空间进行动态规划，计算到目标的最短距离，这种启发式方法的优点是在二维空间中发现所有的U形障碍物和死角，然后引导更昂贵的三维搜索远离这些区域。

最后，可以使用两种方法的最大值。

解析展开法

上述正向搜索使用离散化的控制动作空间（转向）。这意味着搜索永远不会达到精确的连续坐标终点状态（精度取决于A*中网格的分辨率）。为了解决这一精度问题，并进一步提高搜索速度，在Reed-Shepp模型基础上，采用解析展开法进行扩展。在上面描述的搜索中，树（主搜索路）中的一个节点通过使用一个特定的控制动作在一小段时间内（对应于网格的分辨率）模拟汽车的运动模型来扩展。

除了以这种方式生成的子节点外，对于某些节点，通过计算从当前状态到目标的最佳Reed-Shepp路径（假设环境无障碍）来生成额外的子节点。然后检查Reed-Shepp路径是否与当前障碍物地图发生碰撞，并且仅当路径无碰撞时，子节点才会添加到树中。由于计算原因，不希望将Reed-Shepp展开应用于每个节点（特别是远离目标的节点，因为大多数这样的路径都可能通过障碍）。在我们的实现中，我们使用了一个简单的选择规则，其中Reed-Shepp展开应用于每N个节点中的一个，其中N作为目标成本启发式的函数而减少（当我们接近目标时导致更频繁的分析展开）。

带有Reed-Shepp扩展的搜索树如下图所示。我们发现，这种搜索树的分析扩展在准确性和规划时间上都有显著的好处。

与短增量扩展对应的搜索树分支显示在黄绿色范围内，Reed-Shepp路径是指向目标的紫色段。

利用Voronoi代表路径代价函数
我们使用下面的势场，我们称之为Voronoi场，来定义路径长度和接近障碍物之间的权衡。
与常规势场相比，Voronoi场的主要优势在于，势场的值与导航时的可通过间隙成比例。因此，即使是狭窄的开口也可以通行，但这在标准势场中不总是可行。

局部优化与平滑

混合状态A star 产生的路径通常是次优的，就是发现这样的道路虽然是可以驾驶的，但可能包含不必要的非自然转向。因此，采用两步优化过程对混合状态A*解进行后处理：
第一阶段，在路径顶点的坐标上建立一个非线性优化程序，以提高解的长度和平滑度；
第二阶段，使用另一个具有高分辨率路径离散化的共轭梯度迭代来执行非参数插值。

另外，作者有指出了相应的代价函数表示式。其中有引导机器人远离狭窄和宽阔通道中的障碍物的代价；有与障碍物的碰撞的惩罚代价；强制执行车辆的非完整约束的代价；衡量道路平坦程度的标准的代价。

混合A*和CG路径，用于复杂机动，包括倒车进入停车位。混合状态路径（红色）和共轭梯度解（蓝色）

CG平滑后，虽然得到的路径比A*解要平滑得多，但它仍然是分段线性的，顶点之间的距离很大，这会导致车辆转向非常突然。因此，使用CG解的顶点之间的插值来进一步平滑路径。许多参数插值技术对输入中的噪声非常敏感，并且会加剧输出中的任何此类噪声（例如，随着输入顶点之间的距离越来越近，三次样条函数会导致输出中任意大的振荡）。 因此，作者使用非参数插值，通过添加新顶点对路径进行超级采样，并使用CG最小化路径的曲率，同时保持原始顶点不变。

最后，附上我用ROS对hybrid A*的实现视频（请大家看视频之前把音量调小或者戴上耳机，希望大家喜欢这个bgm）： https://www.bilibili.com/video/av89999064/