描述

为了学习一下混合A*算法，我前面介绍了车辆运动学及非完整约束、差速轮及阿克曼运动学模型、Dubins曲线和RS曲线，现在终于可以看一下混合A*算法的相关内容了。

原文名称：Practical Search Techniques in Path Planning for Autonomous Driving

关于非完整性约束：https://blog.csdn.net/qq_42568675/article/details/116116994

我之前也写了几篇文章，链接如下：
机器人运动学中的非完整约束与运动模型推导
机器人常见的差速轮模型与阿克曼模型运动学方程解析
车辆路径规划之Dubins曲线与RS曲线简述

简介

这篇文章描述了一种实用的路径规划算法，适用于在未知环境（在线检测到障碍物）中运行的自动驾驶汽车。这种算法分两个主要步骤：第一步是使用一种应用于车辆3D运动状态空间的变体A*搜索算法，但通过修改状态更新规则，将车辆的连续状态捕获到A*的离散节点中，从而保证运动学上可行的路径；第二步是通过数值非线性优化来提高解决方案的质量，从而得到局部（并经常是全局）最优解。这种路径规划算法在DARPA城市挑战赛中得到了验证，其中自动驾驶汽车需要自由地在停车场中导航，并在遇到障碍物时进行在线重新规划。该算法在执行U型转弯等复杂的通用路径规划任务时表现出了无懈可击的性能，具有典型的全周期重新规划时间在50-300ms之间。此外，该算法还具有很好的扩展性，可以轻松地与其他高级算法集成，如用于局部路径规划的RRT*算法和用于全局路径规划的分层图搜索算法等。这种算法在实时性和鲁棒性方面表现出色，可以应用于各种自动驾驶汽车和其他移动机器人平台。

介绍和相关工作

解决问题被明确为：在未知环境中运行的自主车辆的路径规划问题。进行了如下的假设：假设机器人具有足够的感知和定位能力，并必须在在线时重新规划，同时逐步构建障碍图。这种场景在一定程度上受到了DARPA城市挑战赛的启发，其中车辆需要在停车场等环境中自由导航。文本中提到的路径规划算法是由斯坦福赛车队的机器人Junior在Urban Challenge中使用的算法。Junior在完成复杂的通用路径规划任务中表现出色，包括在停车场导航、执行U型转弯、处理堵塞的道路和交叉口等任务，以及在PC上执行的全周期重新规划时间为50-300ms。

需要车本身拥有定位能力（知道车辆实时位姿），并且能够实时感知障碍物信息（位置及类别）

Junior的图如下：

对于自由导航区的实际路径规划器开发，主要挑战之一在于机器人的控制空间和轨迹空间是连续的，从而导致连续变量优化的问题变得复杂。文章列举了一些同行的研究成果，这里略过。

混合A*算法建立在上述现有工作的基础上，由两个主要阶段组成。第一步是使用连续坐标中的启发式搜索，保证计算轨迹的运动可行性。虽然没有理论最优性保证，但实际上，第一步通常会生成位于全局最优解邻域内的轨迹。第二步使用共轭梯度（CG）下降来局部改善解决方案的质量，生成至少局部最优的路径，但通常也会达到全局最优解。

另一个实际挑战是设计一个路径上的成本函数，以产生所需的驾驶行为。困难在于我们希望获得的路径在长度上接近最优，但同时又平滑且与障碍物保持舒适的距离。惩罚接近障碍物的一种常见方法是使用潜在场（Andrews和Hogan 1983；Khatib 1986；Pavlov和Voronin 1984；Miyazaki和Arimoto 1985）。潜在场的一个缺点是它们会在狭窄通道中创建高潜力区域，从而使这些通道变得实际上无法通行。为了解决这个问题，我们引入了一个潜力场，它基于工作空间的几何形状重新调整场，从而允许在狭窄通道中进行精确导航，同时也可以有效地将机器人推离在开阔区域的障碍物。

潜力场的方法避免规划进入看似走向终点的死胡同，并且尽可能远离障碍物

混合A搜索

方法的第一阶段使用了一种变体的的A* 算法，该算法应用于车辆的3D运动状态空间，但是具有修改后的状态更新规则，可以在离散搜索节点中捕获连续状态数据。与传统的A* 一样，搜索空间(x，y，θ)被离散化，但与传统的A*只允许访问单元格的中心不同，我们的混合状态A*将每个网格单元格与车辆的连续3D状态相关联，如图2所示。

图2解释一下。左图是A* 算法，它的状态量就是网格中心，这样就只有（x，y）。中间是 Field D（Ferguson和Stentz 2005）和Theta（Nash等人2007）提出的方法，允许连接到每个格子的角点，同时每个格子之间可以用任意路径的线连接。混合A* 算法用连续状态来连接格子（仔细看会发现连接线可以是曲线了），格子的分数是它关联的连续状态的损失。

如上图所述，由于其合并了在离散空间中占据相同单元的连续坐标状态（由于坐标连续，实际上一个格子内有无数个坐标点，但我们现在只能通过某一点来代表车辆到达了某个格子），因此我们的混合状态A*算法不能保证找到最小代价解决方案。但是，由此产生的路径是可驾驶的（而不是像标准A*那样分段线性的）。此外，在实践中，混合状态A*的解决方案通常位于全局最优解的邻域内，使我们能够通过我们算法的第二阶段（使用梯度下降来局部改进路径，如下所述）渐进地达到全局最优解。

混合状态A*的主要优势在于在狭小空间中的机动性，其中离散化误差变得至关重要。

我们的算法计划前进和后退运动，并对后退以及改变运动方向施加惩罚。

我认为A*的优势，就在于其生成轨迹可驾驶。算法的简述，接下来会详细介绍

1. 启发

我们的搜索算法由两种启发式方法指导，如图3所示。这些启发式方法不依赖于混合状态A的任何属性，也适用于其他搜索方法（例如离散A）。

A*算法的公式是F=G+H，G是实际消耗，而H就是估计消耗，它的函数表达可以被称为启发函数

第一个启发式方法——我们称之为“非完整约束无障碍”-忽略了障碍物，但考虑了汽车的非完整约束。

为了计算它，我们假设目标状态为 (x_g, y_g, θ_g)=(0,0,0)，假设没有障碍物时，计算每个点(x, y, θ)到离散化后的目标点邻域中间的最短路径。显然，这个启发式方法的成本并不高。然后，我们使用非完整约束无障碍成本和2D欧几里得距离中的最大值作为我们的启发式方法。这种启发式方法的效果是修剪那些以错误方向接近目标的搜索分支。请注意，由于此启发式方法不依赖于运行时的传感器信息，因此可以完全离线计算，然后使用简单的变换和旋转来匹配当前目标。在我们的实际驾驶场景实验中，与直接的2D欧几里得距离成本相比，此启发式方法提供了接近一个数量级的改进，减少了扩展的节点数量。

第一种启发函数：不考虑障碍物，只考虑车辆的运动约束。每个点计算一下到终点的RS曲线的长度，这是H1

第二个启发式方法是第一个启发式方法的对抗，它忽略了汽车的非完整约束，但使用障碍图通过执行2D动态规划来计算到目标的最短距离。

此启发式方法的优点是它发现了所有U形障碍物和死胡同，并在2D中进行指导，然后引导更昂贵的3D搜索远离这些区域。

第二种启发函数：不考虑运动约束，只考虑障碍物，每个点计算到终点并且无障碍物的路径长度（显然规避了障碍物且理论意义上最短），这是H2

两种启发函数在A*意义下都是可接受的，因此可以将两者中的最大值用于指导搜索。

第一种启发函数结果为H1，第二种启发函数结构为H2，最终取的启发函数为H=max(H1, H2)

图3。A*的启发。欧几里得距离方法在2D空间中扩展了21515个节点（图a），而非完整约束无障碍启发式方法在节点扩展上有了显著改善，只扩展了1465个节点（图b）。然而，在更复杂的场景中，这种方法可能导致浪费地对死胡同进行探索，例如在（图c）中扩展了68730个节点。为了解决这个问题，可以使用完整约束带障碍启发式方法（图d），将后一种方法与前一种方法结合使用，只扩展了10588个节点。

提出了两个启发式方法，目的都是尽可能减少路径节点的探索。第一种方法：只考虑车的非完整约束，不考虑障碍物。第二种方法：只考虑障碍物，不考虑车的非完整约束。节点搜索时，取这两种方法的最大值。

2. 分析拓展

在上述的搜索过程中，使用控制动作的离散空间。这意味着搜索永远无法达到精确的连续坐标目标状态（精度取决于A*网格的分辨率）。

到位精度是受限的，取决于A*算法状态的分辨率

为了解决这个精度问题，并进一步提高搜索速度，我们使用Reed-Shepp模型（Reeds和Shepp，1990）进行解析扩展来增强搜索。在上述搜索中，通过使用特定的控制动作在一小段时间内（对应于网格的分辨率）模拟汽车的运动学模型来扩展树中的节点。

H1中使用RS曲线来搜索，并根据车辆模型的一些离散控制动作，生成树中的节点。比如左打轮5°、右打轮5°等，而根据车辆运动学模型，显然一个节点在当前位置的侧方是不能生成的，车辆不可以平移过去。

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

当到达某些节点时，如果能够生成直接到达终点的RS曲线，就可以生成额外的一个子节点。为了简化运算量，可以每100步计算一下RS拓展节点，接着每80步、每60步……依次类推

图4中显示了带有Reed-Shepp扩展的搜索树。由节点短期增量扩展生成的搜索树显示为黄绿色范围，而Reed-Shepp扩展显示为通向目标的单一紫色线条。我们发现，这种对搜索树进行的分析扩展在准确性和规划时间方面带来了重大益处。

刚开始由RS曲线生成的节点较多，而在靠近终点时利用RS曲线节点显著变少，节点甚至直接成为一条直通终点的线，大大减少了规划时间

总结

这篇文章将清楚了混合A*算法的路径搜索的方法，接下来会对生成路径及路径平滑优化进行分析