混合A*算法论文原文：《Practical Search Techniques in Path Planning for Autonomous Driving》
Hybrid-State A*搜索
图2
图2中左边是A*，中间是场D*和Theta*，右边是混合A*。
和传统A*搜索不同的是，混合A*每个栅格关联了一个车辆的连续3D状态，如图2所示。
混合A*并不能保证一定能够找到最小代价的方案因为其把占据离散空间的连续坐标状态融合在一起。然而结果路径一定保证能够被驾驶。此外，在实践中，混合A*算法通常位于全局最优解的邻域，这使得我们可以通过算法的第二阶段（如下所述，该阶段使用梯度下降来局部改进路径）频繁地获得全局最优解。
混合状态 A* 的主要优势体现在狭小空间内的机动性上，在这种情况下，离散化误差变得至关重要。
我们的算法规划了正向和反向运动，并对反向行驶和切换运动方向进行了惩罚。
启发式 我们的搜索算法由两个启发式指导，如图3所示。这些启发式并不依赖于混合状态 A* 的任何特性，也适用于其他搜索方法（如离散 A*）。
图3
第一个启发式，我们叫做“无障碍非完整性约束（non-holonomic-without-obstacles）”——忽略了障碍物，但考虑到了汽车的非完整性约束特性。为了计算它，我们假设 \((x_g,y_g,\theta_g)=(0,0,0)\) 的一个目标状态状态，并计算从目标离散邻域中的每一点\((x, y, θ)\)到目标的最短路径，假设完全无障碍。1很明显，这一代价是一种可接受的启发式方法。然后，我们使用无障碍非完整性约束成本和二维欧氏距离的最大值作为启发式。这种启发式的效果是，它可以剪除以错误头部接近目标的搜索分支。请注意，由于该启发式不依赖于运行时的传感器信息，因此可以完全离线预计算，然后进行简单的转换和旋转，以匹配当前目标。在我们的真实驾驶场景实验中，与直接的二维欧氏距离成本相比，这种启发式方法在节点扩展数量上有接近数量级的改进。
drive robot 依据自由度（freedom）的可控性（controllable）划分为“nonholonomic or holonomic robot” holonomic robot：在所有的自由度上都可控的，比如基于万向轮的机器人（robot built on Omni-wheels ） nonholonomic robot：可控的自由度维数小于机器人自身的自由度，比如car自身有3的自由度（x，y，theta），即平面坐标和车身朝向，但car可控的自由度是2（加减速度，转动方向盘，不能随意的漂移运动） 比如，在一维空间中，只能在一个轴的前后方向移动。那么，对于有轨电车或火车，则可以认为是完整性的，因为它是可能朝着该空间任意方向移动的。但如果对于二维空间来说，就是非完整性约束了，因为有轨电车或火车无法朝轴的正交方向移动。同样地道理，对于地面移动机器人来说，通常我们所说的空间是指二维空间中，人在该二维空间中是完整性的约束，也就是所有能够像人一样在二维空间中自由移动的机器人，都是完整性约束。而类似无人车这种机器人，无法保证侧向移动，因此被认为是非完整性约束。 —— https://blog.csdn.net/zjpp2580369/article/details/100915045
第二种启发式是第一种启发式的翻版，它忽略了汽车非完整性约束的特性，而是利用障碍物地图，通过二维动态编程计算出到目标的最短距离。这种启发式的好处在于，它能在二维中发现所有U形障碍和死胡同，然后引导成本更高的三维搜索远离这些区域。
从数学意义上讲，这两种启发式方法都是可接受的A*，因此可以使用两者的最大值。
分析扩展 上述的前向搜索使用了离散化的控制动作空间（转向）。这意味着搜索永远不会达到精确的连续坐标目标状态（精度取决于A*中网格的分辨率）。为了解决这个精度问题，并进一步提高搜索速度，我们使用基于Reed-Shepp模型的解析扩展来增强搜索（Reeds和Shepp，1990）。在上述的搜索中，树中的一个节点通过模拟车辆的运动模型——使用特定的控制动作——在一小段时间内（对应于网格的分辨率）进行扩展。
除了以这种方式生成的子节点外，对于某些节点，我们通过计算从当前状态到目标的最优Reed-Shepp路径来生成一个额外的子节点（假设环境中没有障碍物）。然后，我们会检查Reed-Shepp路径是否与当前的障碍物地图发生碰撞，只有在路径无碰撞的情况下，子节点才会被添加到树中。出于计算原因，我们并不希望将Reed-Shepp扩展应用到每一个节点（尤其是离目标较远的地方，大部分这样的路径可能会穿过障碍物）。在我们的实现中，我们使用了一个简单的选择规则，即每N个节点中就有一个节点会应用Reed-Shepp扩展，其中N随着到目标成本的启发式函数而减小（这意味着当我们接近目标时，解析扩展的频率会增加）。
图4展示了一个带有Reed-Shepp扩展的搜索树。由节点的短增量扩展生成的搜索树显示为黄绿色范围，而Reed-Shepp扩展显示为通向目标的单一紫色线条。我们发现，这种搜索树的解析扩展在精度和规划时间方面都带来了显著的好处。
图4
使用Voronoi场的路径代价函数
我们使用以下势场，我们称之为Voronoi场，来定义路径长度和接近障碍物之间的权衡。Voronoi场定义如下：
\[ \rho_V(x,y)= \left ( \frac{\alpha}{\alpha+d_\mathcal{O}(x,y)} \right) \left( \frac{d_\mathcal{V}(x,y)}{d_\mathcal{O}(x,y)+d_\mathcal{V}(x,y)} \right) \frac{(d_\mathcal{O}-d_\mathcal{O}^{max})^2}{(d_\mathcal{O}^{max})^2}, \tag{1} \]
其中 \(d_\mathcal{O}\) 和 \(d_\mathcal{V}\) 分别是到最近障碍物和广义Voronoi图（GVD）边缘的距离，其中，\(\alpha>0,d_\mathcal{O}>0\)是控制场的衰减率和最大有效范围的常数。公式(1)中的表达式是对于 \(d_\mathcal{O} ≤ d_\mathcal{O}^{max}\) 的情况；否则，\(ρ_V(x, y) = 0\)。
这个势场具有以下特性:
i) 当 \(d_\mathcal{O} ≥
d_\mathcal{O}^{max}\) 时，它为零；
ii) \(ρ_V (x, y) ∈ [0, 1]\) 并且在
\((x, y)\)
上是连续的，因为我们不能同时有 \(d_\mathcal{O}
= d_\mathcal{V} = 0\)；
iii) 它只在障碍物内部达到最大值。
iv) 它只在GVD的边缘达到最小值。
Voronoi场相比于传统的势场的主要优势在于，场的值是按照导航可用的总清晰度进行缩放的。因此，即使是狭窄的开口也仍然可以导航，这在标准的势场中并不总是如此。
图5展示了这个特性。图5a显示了Voronoi场的二维投影，图5b给出了相应的广义Voronoi图。注意到，靠近彼此的障碍物之间的狭窄通道并没有被势场阻挡，而且总是存在一条连续的\(ρ_V = 0\)的路径在它们之间。将这个与图5c中显示的简单势场\(ρ(x, y) = α(α + d_\mathcal{O}(x, y))^{−1}\)进行比较，后者在障碍物之间的狭窄通道中有高势区域。
图5
图6显示了一个真实停车场的Voronoi场和驾驶轨迹。
图6
我们应该注意，Voronoi图和势场的使用在机器人运动规划的背景下已经被提出很长时间了。例如，Voronoi图可以用来推导出自由空间的骨架化（Choset和Burdick 2000）。然而，对于非全向车辆来说，沿着Voronoi图进行导航是不可能的。
局部优化和平滑
由混合状态A*产生的路径往往仍然是次优的，值得进一步改进。从经验上看，我们发现这样的路径是可以驾驶的，但可能包含不必要的转向的不自然的偏移。因此，我们通过应用以下两阶段优化程序对混合状态A*解决方案进行后处理。在第一阶段，我们对路径的顶点坐标提出一个非线性优化程序，以改进解决方案的长度和平滑度。第二阶段使用另一次共轭梯度迭代进行非参数插值，路径离散化的分辨率更高。
给定一系列顶点 \(\mathbf{x}_i=(x_i,y_i)，i∈[1,N]\) ，我们定义几个量：\(\mathbf{o}_i\) ，最接近顶点的障碍物的位置；\(∆\mathbf{x}_i =\mathbf{x}_i - \mathbf{x}_{i-1}\)，顶点处的位移向量；\(\Delta\phi_i = \left| \tan^-1 \frac{\Delta y_{i+1}}{\Delta x_{i+1}} - \tan^-1 \frac{\Delta y_i}{\Delta x_i} \right|\)，顶点处切线角的变化。目标函数是：
\[ \begin{align} &w\rho\sum^N_{i=1}\rho_V(x_i,y_i)+wo\sum^N_{i=1}\sigma_o(|\mathbf{x}_i-\mathbf{o}_i|-d_{max})+ \\ &w\kappa\sum^{N-1}_{i=1}\sigma_\kappa\left(\frac{\Delta \phi_i}{|\Delta \mathbf{x}_i|}-\kappa_{max}\right)+ws\sum^{N-1}_{i=1}(\Delta\mathbf{x}_{i+1}-\Delta\mathbf{x}_i)^2, \end{align} \]
其中，\(\rho_V\) 是Voronoi场；\(\kappa_{max}\) 是路径的最大允许曲率（由汽车的转弯半径定义），\(\theta_o\) 和 \(\theta_\kappa\) 是惩罚函数（从经验上看，我们发现简单的二次惩罚效果很好）；\(w_\rho，w_o，w_ \kappa，w_s\) 是权重。
代价函数的第一项有效地引导机器人在狭窄和宽阔的通道中远离障碍物。第二项对与障碍物的碰撞进行惩罚。第三项在每个节点处对轨迹的瞬时曲率进行上限约束，并强制执行车辆的非全向约束。第四项是路径平滑度的度量。
上述代价函数的梯度可以按照下面的方式直接计算。对于Voronoi场项，当 \(d_\mathcal{O} ≤ d^{max}_\mathcal{O}\) 时，我们有：
\[ \begin{align} \frac{\partial \rho_V}{\partial\mathbf{x}_i}=&\frac{\partial \rho_V}{\partial d_\mathcal{O}}\frac{\partial d_\mathcal{O}}{\partial \mathbf{x}_i}+\frac{\partial \rho_V}{\partial d_\mathcal{V}}\frac{\partial d_\mathcal{V}}{\partial \mathbf{x}_i}, \\ \frac{\partial d_\mathcal{O}}{\partial \mathbf{x}_i}=&\frac{\mathbf{x}_i-\mathbf{o}_i}{|\mathbf{x}_i-\mathbf{o}_i|}, \\ \frac{\partial d_\mathcal{V}}{\partial \mathbf{x}_i}=&\frac{\mathbf{x}_i-\mathbf{v}_i}{|\mathbf{x}_i-\mathbf{v}_i|}, \\ \frac{\partial \rho_V}{\partial d_\mathcal{V}}=&\frac{\alpha}{\alpha+d_\mathcal{O}}\frac{(d_\mathcal{O}-d^{max}_\mathcal{O})}{(d^{max}_\mathcal{O})^2}\frac{d_\mathcal{V}}{(d_\mathcal{O}+d_\mathcal{V})^2}, \\ \frac{\partial \rho_V}{\partial d_\mathcal{O}}=&\frac{\alpha}{\alpha+d_\mathcal{O}}\frac{d_\mathcal{V}}{d_\mathcal{O}+d_\mathcal{V}}\frac{(d_\mathcal{O}-d^{max}_\mathcal{O})}{(d^{max}_\mathcal{O})^2} \\ & \left[ \frac{-(d_\mathcal{O}-d_\mathcal{O}^{max})}{\alpha+d_\mathcal{O}}-\frac{d_\mathcal{O}-d_\mathcal{O}^{max}}{d_\mathcal{O}+d_\mathcal{V}} +2 \right] \end{align} \]
其中，\(\mathbf{v}_i\) 是广义Voronoi图（GVD）边缘上离顶点 \(i\) 最近的点的二维坐标向量。我们通过维护所有障碍点和GVD边缘点的kd树，并在每次共轭梯度迭代时更新顶点的最近邻点，来计算最近的障碍物 \(\mathbf{o}_i\) 和最近的GVD边缘点 \(\mathbf{v_i}\) 。
对于二次\(σ_o\)的碰撞惩罚，如果 \(|\mathbf{x}_i - \mathbf{o}_i| ≤ d_{max}\) ，我们有：
\[ \frac{\partial \sigma_o}{\partial \mathbf{x}_i}=2(|\mathbf{x}_i-\mathbf{o}_i|-d_{max})\frac{\mathbf{x}_i-\mathbf{o}_i}{|\mathbf{x}_i-\mathbf{o}_i|}. \]
对于顶点 \(i\) 处的最大曲率项，我们必须对影响点 \(i\) 处曲率的三个点： \(i-1\) ， \(i\) 和 \(i+1\) ，进行求导。对于这个计算，节点 \(i\) 处切线角的变化最好表示为：
\[ \Delta\phi_i = \cos^{-1}\frac{\Delta\mathbf{x}^T_i\Delta\mathbf{x}_{i+1}}{|\Delta\mathbf{x}_i||\Delta\mathbf{x}_{i+1}|}, \tag{2} \]
对于曲率 \(\kappa_i = \Delta\phi_i /|∆x_i|\)，关于这三个节点的坐标的导数则为：
\[ \begin{align} &\frac{\partial \kappa_i}{\partial \mathbf{x}_i}=-\frac{1}{|\Delta \mathbf{x}_i|}\frac{\partial \Delta\phi_i}{\partial \cos (\Delta \phi_i)}\frac{\partial \cos (\Delta \phi_i)}{\partial \mathbf{x}_i} - \frac{\Delta\phi_i}{(\Delta \mathbf{x}_i)^2}\frac{\partial \Delta \mathbf{x}_i}{\partial \mathbf{x}_i}, \\ &\frac{\partial \kappa_i}{\partial \mathbf{x}_{i-1}}=\frac{1}{|\Delta \mathbf{x}_i|}\frac{\partial \Delta\phi_i}{\partial \cos (\Delta \phi_i)}\frac{\partial \cos (\Delta \phi_i)}{\partial \mathbf{x}_{i-1}} - \frac{\Delta\phi_i}{(\Delta \mathbf{x}_i)^2}\frac{\partial \Delta \mathbf{x}_i}{\partial \mathbf{x}_{i-1}}, \\ &\frac{\partial \kappa_i}{\partial \mathbf{x}_{i+1}}=\frac{1}{|\Delta \mathbf{x}_i|}\frac{\partial \Delta\phi_i}{\partial \cos (\Delta \phi_i)}\frac{\partial \cos (\Delta \phi_i)}{\partial \mathbf{x}_{i+1}}, \end{align} \]
其中：
\[ \frac{\partial \Delta \phi_i}{\partial \cos (\Delta \phi_i)} = \frac{\cos^{-1}(\cos(\Delta \phi_i))}{\partial \cos(\Delta \phi_i)} = -\frac{1}{\sqrt{1-\cos^2(\Delta \phi_i)}}. \]
关于三个顶点坐标的 \(\cos(\Delta\phi_i)\) 的导数，最容易用正交补来表示：
\[ \mathbf{a} \perp \mathbf{b} = \mathbf{a} - \frac{\mathbf{a}^T\mathbf{b}}{|\mathbf{b}|}\frac{\mathbf{b}}{|\mathbf{b}|}. \tag{3} \]
引入以下标准化的正交补：
\[ \mathbf{p}_1 = \frac{\mathbf{x}_{i}\perp (-\mathbf{x}_{i+1})}{|\mathbf{x}_{i}||\mathbf{x}_{i+1}|};\quad \mathbf{p}_2 = \frac{(-\mathbf{x}_{i+1})\perp \mathbf{x}_{i}}{|\mathbf{x}_{i}||\mathbf{x}_{i+1}|}, \tag{4} \]
我们可以将导数表示为：
\[ \begin{align} &\frac{\partial \cos(\Delta \phi_i)}{\partial \mathbf{x}_i} = -\mathbf{p}_1 - \mathbf{p}_2; \\ &\frac{\partial \cos(\Delta \phi_i)}{\partial \mathbf{x}_{i-1}}=\mathbf{p}_2; \quad \frac{\partial \cos(\Delta \phi_i)}{\partial \mathbf{x}_{i+1}}=\mathbf{p}_1. \tag{5} \end{align} \]
图7显示了第二次优化和平滑步骤的效果：红线是A*解决方案，蓝线是通过CG优化获得的路径。
图7
使用上述的CG平滑，我们得到的路径比A*解决方案更加平滑，但它仍然是分段线性的，顶点之间的距离较大（在我们的实现中大约为0.5m-1m）。这可能导致物理车辆的转向非常突然。因此，我们使用插值在CG解决方案的顶点之间进一步平滑路径。许多参数插值技术对输入的噪声非常敏感，并在输出中加剧任何这样的噪声（例如，立方样条可以导致输出中的振荡在输入顶点彼此接近时任意大）。
因此，我们使用非参数插值，我们通过添加新的顶点对路径进行超采样，并使用CG来最小化路径的曲率，同时保持原始顶点固定。图8a中路径的插值结果显示在图8b中。
图8
实验结果
视频地址: https://ai.stanford.edu/~ddolgov/gpp_maze.avi
我们使用了一个160x160的网格，其在x-y方向上的分辨率为1m，角度分辨率为5度。↩︎