混合A*算法详解（一）路径搜索

描述
为了学习一下混合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*算法详解（一）路径搜索 - 古月居可查看全文