描述

上一篇文章混合A*算法详解（一）路径搜索

路径损失函数使用Voroni势能图

根据之前的文章分析，决定A*路径长度的有两点：路径长度和距离障碍物远近。Voroni图用于权衡这两者。之前我在记录二维点云的阿尔法形状算法时简单介绍过维诺图，链接在这里。
阿尔法形状算法与2D点云的C++实现

维诺图可以将每个障碍物作为一个多边形的中心。障碍物与障碍物之间，被等分的线段分割开，使得整个环境构成了一张维诺图。它有如下几个性质：
1.每个多边形中只有一个节点；
2.相邻多边形中的节点到达他们公共边的距离相等；
3.多边形内的点距离其中心，比距离其他多边形的中心都要近

论文在维诺图的基础上提出了维诺势能（Voroni Field）。它的公式如下：

p_v(x,y)代表节点在某一位置的势能，\alpha代表下降速率，d_O代表节点到最近障碍物的距离，d_V代表节点到维诺图多边形边的距离。d_O^{max}代表势能值的最大有效范围。根据该势能会生成一个维诺图（the Generalized Voronoi Diagram，GVD）

该势能有以下几个特点：

1.超出势能值范围的势能都为0；
2.势能值p_V(x,y)\subseteq[0,1]，且在(x,y)范围内是连续的；
3.只有在障碍物内部才会达到最大值；
4.在GVD的边上达到最小值。

Voronoi场相对于传统势场的关键优势在于，势能值是根据总可用导航间隙按比例缩放的。因此，即使很窄的开口仍然是可导航的，这在标准势场中并不总是如此。在传统势场中，障碍物周围的场值通常被设置为无穷大，这意味着车辆无法穿越这些区域。相比之下，Voronoi场根据可用的导航间隙来调整势能值，这意味着即使在狭窄的空间内，车辆也可以找到可穿越的路径。

论文给出了几张图，如下：

图a显示了一个模拟停车场对应的Voroni势能图，图b是其对应的GVD图。注意到障碍物之间的狭窄空隙没有被势能阻挡，在他们之间总会有一条p_V = 0的势能为0路径。图c显示了一个反例，当势能公式为p(x,y)=\alpha(\alpha+d_O(x,y))^{-1}时，障碍物之间仍然有比较高的势能。

下图显示了一个真实停车场的维诺地图中的一条驾驶轨迹。

Voronoi图和势场的使用早已在机器人运动规划的背景下提出。例如，Voronoi图可用于导出自由空间的骨架化（Choset和Burdick 2000）。然而，对于具有非完整约束的汽车来说，沿着Voronoi图导航是不可能的（由于运动学约束某些路径是车辆是无法驾驶通过的）。导航函数（Koditschek 1987；Rimon和Koditchek 1992）和拉普拉斯势（Connolly，Burns和Weiss 1990）也与我们的Voronoi域相似，因为它们构造了用于全局路径规划的不存在局部极小值的势函数。我们不使用Voronoi势能图进行全局路径规划。然而，我们观察到，对于具有凸障碍物的工作空间，Voronoi势能图可以用全局吸引势能来增强，产生一个没有局部极小值的势能图，因此适合于全局导航。我的理解是对于图a，可以看到凸障碍物内部的势能也比较高，该障碍物小还好说，当障碍物非常大时，应用Voroni势能可以避免在障碍物内部产生局部最小值的势能，导致规划多占用算力。

轨迹优化及平滑

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

给定一个节点序列 \mathbf{x_i}=(x_i,y_i),i\in[1,N]

距离每个节点最近的障碍物位置为\mathbf{o_i}

节点之间的移动向量为\triangle\mathbf{x_{i}}=\mathbf{x_i}-\mathbf{x_{i-1}}

节点切向角度的变化为\triangle \phi_i=|tan^{-1}\frac{\triangle y_{i+1}}{\triangle x_{i+1}}-tan^{-1}\frac{\triangle y_{i}}{\triangle x_{i}}|

然后作者设计了这样一个损失函数，如下：

式子中p_V代表节点的势能，k_{max}代表路径允许的最大曲率，\sigma_o和\sigma_k代表惩罚函数（根据经验，我们发现简单的平方惩罚就很有效），

w_p、w_o、w_k、w_s都是权重参数。

损失函数的第一项有效地引导机器人远离狭窄和宽阔通道中的障碍物。第二项惩罚与障碍物的碰撞。第三项上限是每个节点处轨迹的瞬时曲率，并强制车辆的非完整约束。第四项是路径平滑度的度量。

1. 势能惩罚的偏导

计算一下第一项损失函数的偏导如下：

式子中 v_i是一个二维矢量，是节点i到广义Voronoi图（GVD）边缘上最靠近节点i的坐标点之间的矢量。我们通过维护所有障碍物和GVD边缘点的KD树，并在每次共轭梯度迭代时更新节点的最近相邻节点，来计算最近的障碍物和最近的GVD边缘点。

2. 碰撞惩罚的偏导

作者采用了二次项的惩罚函数，因此第二项的偏导函数如下：

3. 曲率惩罚的偏导

为了计算曲率变化，需要考虑节点i-1、i、i+1三个点。这个公式比较繁琐，但很简单，把论文的公式贴在这里

4.路径平滑惩罚的偏导

可以用公式的定义推到出，第四项为常数

效果

通过上面提到的CG

下图显示了这一节提出算法的意义

可以看出混合A*的路径为红色，通过CG（共轭梯度算法）优化后的曲线为蓝色。

通过文章提出的CG算法后，获得的路径比A*解决方案平滑很多，但它仍然是分段线性的，节点之间的距离很大（在作者的实现中大约为0.5米或1米）。这可能会导致实体车辆转向非常突然。因此，我们使用CG解出来的顶点之间，需要插值来进一步平滑路径。许多参数插值技术对输入噪声非常敏感，并加剧该噪声的输出（例如，随着输入顶点彼此越来越近，三次样条曲线可能导致输出中出现大幅振荡）。

因此，作者使用非参数插值，通过添加新的顶点并使用CG来最小化路径的曲率，同时保持原始顶点不变，对路径进行了超级采样。对路径进行插值的结果如下图

图a中的红色是输入路径，图b是插值后的路径，图中将前轴和后轴的规划路径一起显示了。这张图得去论文里放大看，就比较好理解了。图a中有一条红色线，是分段线性的（每一个小红点之间都是条直线）。我们发现红色点都按照上一个线段延长出一条短短的绿色线，这可以理解为上一时刻的航向角。我们的目标就是要让整个轨迹变成连续的曲线，因此应该插值出的每个点所体现的航向角，都应该在前后两个红点的航向角之间连续变化。因此图b中产生了一条浅蓝色连续的曲线，就是经由CG插值后的结果，该蓝色曲线的曲率是连续变化的。（或者，左图红色和蓝色分别代表前轴和后轴，右图的绿色曲线和蓝色曲线代表差值后的结果，我还得再看看）

结果

下图描绘了Junior在DARPA城市挑战赛中的几个驾驶轨迹。图a和图b显示了在堵塞的道路上掉头，图c显示了涉及停车场导航的任务。

在模拟中计算的更复杂的迷宫状环境的解决方案如图10所示。在图10的场景中，机器人在逐步检测障碍物并构建障碍物地图时重新规划。

作者使用了以下参数：障碍物地图的尺寸为160m×160m，分辨率为0.15cm；A*使用尺寸为160m×160m×360的网格◦ 具有0.5米的x-y分辨率和5◦的航向θ的分辨率。涉及混合A*搜索、CG平滑和插值的整个重新规划周期的运行时间约为50–300ms。