混合A*算法详解（二）路径平滑

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

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

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

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

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