HybridA* 论文解读

本文旨在对原论文进行翻译，对混合A*有一个大概的理解
论文题目：Practical Search Techniques in Path Planning for Autonomous Driving

1 摘要

本文描述了一个实用的路径规划算法，无人驾驶汽车在未知的环境中，障碍物通过机器人的传感器实时检测产生平滑的路径。这项工作的动机和实验验证了在2007年DARPA城市挑战赛，机器人必须在停车场自主导航。本文的方法有两个主要步骤：
① 使用众所周知的A* 搜索算法的变体，应用于车辆的3D运动状态空间，但是具有修改的状态更新规则，其在A* 的离散节点中捕获车辆的连续状态-保证路径的运动可行性 。
② 通过数值非线性优化来提高解决方案的质量，从而获得局部（通常是全局）最优解。
本文描述的路径规划算法用于斯坦福大学城市挑战赛。Junior在复杂的一般路径规划任务中表现出完美的性能，例如在停车场导航和在堵塞的道路上执行U形转弯，典型的全周期重新规划时间为50- 300毫秒。

2 引言和相关的工作

本文解决的路径规划问题：无人驾驶车辆在未知的环境中运行。假设机器人有足够的传感和定位能力，必须能在线重规划当一步一步建立障碍物地图时。这种情况的部分动机是DARPA城市挑战赛，其中车辆必须自由地在停车场导航。下面描述的路径规划算法被斯坦福大学赛车队的机器人Junior in the Urban Challenge（DARPA 2007）使用。Junior（图1）在复杂的一般路径规划任务中表现出了完美的性能，其中许多任务涉及倒车，例如在停车场导航、执行U形转弯以及处理堵塞的道路和十字路口，在现代PC上的典型全周期重新规划时间为50- 300 ms。

在开发一个实用的路径规划器的自由导航区的主要挑战之一来自于这样的事实，即所有机器人控制的空间是连续的 - 因此轨迹导致一个复杂的连续变量优化景观。

许多关于搜索算法的先前工作：路径规划（Ersson和Hu 2001; Koenig和Likhachev 2002; Ferguson和Stentz 2005; Nash等2007）产生离散状态空间的快速算法，但是这些算法往往产生非光滑的路径，并且通常不满足飞行器的非完整约束。保证运动学可行性的替代方法是连续坐标中的前向搜索，例如，使用快速探索随机树（RRT）（Kavraki et al. 1996; LaValle 1998; Plaku，Kavraki，and Vardi 2007）。使这种连续搜索算法在在线实现中实用的关键在于有效的引导启发式。另一种方法是将路径规划问题直接公式化为控制或参数化曲线空间中的非线性优化问题（Cremean et al. 2006），但在实践中，由于局部最小值，保证此类程序的快速收敛是困难的。

本文的算法建立在上述现有工作的基础上，包括两个主要阶段。
① 使用连续坐标中的启发式搜索，以保证计算轨迹的运动学可行性。虽然缺乏理论上的最优性保证，但在实践中，这第一步通常产生位于全局最优值附近的轨迹。
② 使用共轭梯度（CG）下降来提高局部解的质量，产生至少局部最优的路径，但通常也达到全局最优。

另一个实际的挑战是设计一个成本函数的路径，产生所需的驾驶行为。困难源于这样一个事实：即我们希望获得长度接近最优的路径，但同时是平滑的，并与障碍物保持合适的距离。一种常用的惩罚接近障碍物的方法是使用势场(Andrews and Hogan 1983; Khatib 1986;Pavlov and Voronin 1984; Miyazaki and Arimoto 1985)。然而，正如许多研究人员所观察到的那样(Tilove1990; Koren and Borenstein 1991)，势场的缺点之一是，它们在狭窄的通道中产生高电势区域，从而使得这些通道实际上不可通过。为了解决这个问题，本文引入了基于几何的工作空间重新缩放的设想，允许在狭窄的通道中精确导航，同时有效地将机器人推离障碍物到更宽更开放的区域。

3 混合状态A* 搜索

本文方法第一阶段使用了一个著名的A* 算法的变体应用到三维运动状态空间的车辆，但是这是修改后的状态更新规则，捕捉连续状态数据的离散搜索节点的A*。正如在传统A* 中一样，搜索空间$(x,y,\theta )$是离散化的，但与传统A* 不同的是，传统A* 只允许访问单元的中心，我们的混合状态A* 与每个网格单元相关联，车辆的连续3D状态，如图2所示。

如上所述，混合状态A* 不能保证找到最小成本解决方案，因为它合并了在离散空间中占据相同单元的连续坐标状态。然而，所得到的路径保证是可驱动的（而不是像标准A* 那样是分段线性的）。此外，在实践中，混合A * 解决方案通常位于全局最优解的邻域中，允许通过第二阶段（其使用梯度下降以局部地改进路径，如下所述）。
混合状态A* 的主要优点体现在紧凑空间中的机动中，其中离散化误差变得至关重要。算法计划向前和向后运动，并对反向驾驶以及切换运动方向进行处罚。

图3：A* 算法。二维欧氏距离扩展了21，515个节点（a）。非完整无障碍启发法是一个重大的改进：它扩展了（b）中的1465个节点，但在更复杂的设置中可能会导致浪费地探索死胡同：（c）中的68730个节点。这一点可以通过将后者与有障碍的完整性启发法结合使用来纠正：（d）中有10588个节点。

第一种启发式：“non-holonomic-without-obstacles” - 忽略障碍，但考虑到汽车的非完整性质。为了计算它，我们假设目标状态为$(x_g,y_g,{\theta }_g)$=$(0,0,0)$，并计算从目标的某个离散邻域中的每个点$(x,y,\theta )$到目标的最短路径，假设完全没有障碍。然后，使用一个最大的non-holonomic-without-obstacles成本和二维欧氏距离作为我们的启发式。这种启发式的效果是，它修剪了接近目标的错误标题的搜索分支。请注意，由于此启发式算法不依赖于运行实时的传感器信息，因此可以完全离线预先计算，然后简单地进行转换和旋转以匹配当前目标。
第二种启发式：第一个启发式算法的对偶，因为它忽略了汽车的非完整性，但**使用障碍图通过在2D中执行动态规划来计算到目标的最短距离。**这种启发式搜索的好处是，它可以在2D中发现所有U形障碍物和死胡同，然后引导更昂贵的3D搜索远离这些区域。
这两种算法在A* 意义上都是数学上可接受的，因此可以使用两者中的最大值。

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

除了以这种方式生成的子节点之外，对于某些节点，通过计算从当前状态到目标的最佳Reed-and-Shepp路径来生成额外的子节点（假设无障碍环境）。然后检查Reed-andShepp路径是否与当前障碍物贴图发生冲突，并且仅将子节点添加到树中如果路径是无碰撞的。出于计算的原因，不希望将Reed-Shepp展开应用于每个节点（特别是远离目标的节点，其中大多数此类路径可能会穿过障碍物）。在我们的实现中，我们使用了一个简单的选择规则，其中Reed-Shepp展开应用于每N个节点中的一个，其中N作为目标成本启发式的函数而减小（随着我们接近目标，导致更频繁的分析展开）。图4显示了一个带有Reed-Shepp展开的搜索树。由节点的短增量扩展生成的搜索树显示在黄绿色范围内，Reed-Shepp扩展显示为通向目标的单条紫色线。我们发现，这种分析扩展的搜索树导致在准确性和规划时间有着显着的好处。

4 使用Voronoi场的路径代价函数

我们使用下面的势场，我们称之为Voronoi场，来定义路径长度和障碍物接近度之间的权衡。Voronoi域定义如下：

其中$d_0$和$d_V$是到最近障碍物和广义Voronoi图（GVD）的边缘的距离，$\alpha >0$，$d_O>0$是控制场的衰减速率和最大有效范围的常数。在(1)式中，$d_O\le d_O^{max}$，否则$p_V(x,y)=0$。

这个势有以下性质：i）当$d_O\ge d_O^{max}$时它为零; ii）${\rho }_V(x，y)\in [0，1]$并且在$(x,y)$上连续，因为我们不能同时使$d_O=d_V=0$; iii）它只在障碍物内达到最大值。iv）它仅在GVD的边缘上达到其最小值。

Voronoi场相对于传统势场的主要优点是，场值与导航的总可用间隙成比例。因此，即使是狭窄的开口仍然可以航行，而标准势场并不总是如此。图5说明了这个属性。图5a显示了Voronoi场的二维投影，图5b给出了相应的广义Voronoi图。请注意，相互靠近的障碍物之间的狭窄通道不会被电势阻挡，它们之间总是有一条连续的${\rho }_V=0$路径。与图5c所示的自然势场$\rho (x,y)=\alpha (\alpha +d_O(x,y))-1$相比，它在窄通道障碍物与障碍物之间具有高电位区。

图6示出了Voronoi场和用于真实的停车场的驱动轨迹。

应该注意到，Voronoi图和势场的使用早已在机器人运动规划的背景下提出。例如，Voronoi图可以用来推导自由空间的非线性化（Choset and Burdick 2000）。然而，导航沿着Voronoi图是不可能的非完整的汽车。
导航函数（Koditschek 1987; Rimon and Koditschek 1992）和拉普拉斯势（Connolly，Burns，and韦斯1990）也与我们的Voronoi场相似，因为它们构造的势函数没有局部极小值，可以进行全局导航。我们不使用Voronoi Field进行全局导航。然而，我们观察到，对于具有凸障碍物的机器人，Voronoi场可以用全局吸引力势来增强，从而产生一个没有局部极小值的场，因此适合于全局导航。

5 局部优化与平滑

混合状态A* 产生的路径通常仍然是次优的，值得进一步改进。根据经验，我们发现这样的路径是可驾驶的，但可能包含不自然的转向，需要不必要的转向。因此，我们通过应用以下两阶段优化程序对混合状态A* 解进行后处理。
① 制定了一个非线性优化程序的顶点的路径，提高了解决方案的长度和光滑度的坐标。
② 使用具有更高分辨率的路径离散化的共轭梯度的另一迭代来执行非参数插值。

给定一个顶点序列$X_i=(x_i,y_i),i\in [1，N]$，我们定义几个量：$o_i$，最接近顶点的障碍物的位置; $△x_i=x_i-x_{i-1}$，顶点处的位移向量; $△{\phi }_i=|ta{n}^{-1}\frac{△y_i+1}{△x_i+1}-ta{n}^{-1}\frac{△y_i}{△x_i}|$，顶点处的切向角的变化。
目标函数：

其中${\rho }_V$是Voronoi场; ${\kappa }_{max}$是路径的最大允许曲率（由汽车的转弯半径定义），${\sigma }_o$和${\sigma }_{\kappa }$是惩罚函数（根据经验，我们发现简单的二次惩罚效果很好）; $w_{\rho }$，$w_o$，$w_{\kappa }$，$w_s$是权重。
成本函数的第一项有效地引导机器人远离狭窄和宽阔通道中的障碍物。第二项惩罚与障碍物的碰撞。第三项上限的瞬时曲率的轨迹在每个节点和强制执行的非完整约束的车辆。第四项是衡量道路的平滑度。

以如下所述的直接方式计算上述成本函数的梯度。对于Voronoi场项，当$d_0\le d_0^{max}$时有：

其中$v_i$是广义Voronoi图（GVD）的边缘上最接近顶点$i$的点的坐标的2D向量。我们通过维护所有障碍点和GVD边缘点的kd树并在每次共轭梯度迭代时更新顶点的最近邻居来计算最近障碍$oi$和最近GVD边缘点$v_i$。
对于具有二次${\sigma }_0$的碰撞惩罚，如果$|Xi-oi|\le dmax$：

对于顶点$i$处的最大曲率项，我们必须对影响点$i$处曲率的三个点求导数：$i-1$、$i$和$i+1$。对于该计算，节点$i$处的切向角的变化最好表示为



引入以下归一化正交补：

图7显示了第二次优化和平滑步骤的效果：红线是A* 解决方案，蓝线是CG优化获得的路径。

使用上面描述的CG平滑，我们获得了比A* 解决方案更平滑的路径，但它仍然是分段线性的，顶点之间的距离很大（在我们的实现中大约为0.5m-1 m）。这可能导致物理车辆上的非常突然的转向。因此，我们使用CG解决方案的顶点之间的插值来进一步平滑路径。许多参数内插技术对输入中的噪声非常敏感并且加剧输出中的任何这样的噪声（例如，当输入顶点彼此靠近时，三次样条可能导致输出中任意大的振荡）。
因此，我们使用非参数插值，其中我们通过添加新顶点来对路径进行超采样，并使用CG来最小化路径的曲率，同时保持原始顶点固定。图8b中示出了对图8a中的路径进行插值的结果。

6 Results

图9描绘了Junior在DARPA城市挑战赛中的几个轨迹。图9a至图9c示出了在阻塞道路上的U形转弯，图9c示出了涉及在停车场中导航的任务。

图10显示了在模拟中计算的更复杂的迷宫状环境的解决方案。在http://ai.stanford.edu/ ddolgov/gpp maze.avi上有一段视频，显示了机器人在逐步检测障碍物并构建图10场景中的障碍物地图时的重新规划。

规划器使用了以下参数：障碍物地图尺寸为160 m × 160 m，分辨率为0.15 cm; A* 使用尺寸为160 m × 160 m ×360 m的网格，x-y分辨率为0.5 m，航向θ分辨率为5 m。涉及混合A* 搜索、CG平滑和插值的完整重新规划周期的典型运行时间为50- 300 ms的量级。

---

到这结束，基本上是对混合A*有了基本的理解了。不正之处，望读者积极指正。