路径规划/路径平滑涉及方法汇总

1. 背景与参考文献

路径生成与路径平滑相关问题之前了解过，这次重新捡起来。起因是为了实现Hybrid A*，该方法中涉及了较多模块，为了弄清楚模块之间的关系以便于后续改进，近期阅读了一些相关的文献，在本文中进行梳理。

Hybrid A* 文献：
这篇文章里细节较多。另一篇practical search 的细节不如这个丰富。

[1] Dolgov D, Thrun S, Montemerlo M, et al. Path planning for autonomous vehicles in unknown semi-structured environments[J]. The International Journal of Robotics Research, 2010, 29(5): 485-501.

Hybrid A* 代码：
这个包中包含了很多有用信息可以看，本文对Hybrid A的理解基本上也是基于这一版本的代码，百度Apollo中也实现了Hybrid A，但是并没有来得及看。

[2] https://github.com/teddyluo/hybrid-a-star-annotation

dynamic voronoi：

[3] Efficient Grid-Based Spatial Representationsfor Robot Navigation in Dynamic Environments

ridge detection in distance map

[4] Arcelli, Carlo, and Gabriella Sanniti Di Baja. “Ridge points in Euclidean distance maps.” Pattern Recognition Letters 13.4 (1992): 237-243.

2. Hybrid A* 搜索过程

Hybrid A*的搜索过程在许多博客文章中都有描述，本文针对阅读代码过程中的部分细节进行讨论。

在阅读Hybrid A*文献及代码前需要对A*的实现过程有一定了解，建议阅读下列博客：

https://www.redblobgames.com/pathfinding/a-star/introduction.html

该博客对Breadth First Search，Dijkstra’s Algorithm，以及A* search进行了详细的讲解并给出了格式清晰的python代码。

在了解了A*方法后，可以开始阅读1文献以及对应代码。

3. Hybrid A* 平滑过程

Hybrid A* 的平滑过程在文章[1]中有专门一章论述，涉及了如下几个部分：
（1）轨迹平滑目标函数
（2）保证平滑轨迹安全性
（3）利用voronoi field做平滑
（4）插值

3.1 轨迹平滑目标函数

轨迹平滑目标函数如下所示：

式中包含三项，分别实现三种不同的目的
（1）第一项作用是使路径点距离障碍物的距离不断接近$d_{max}$，注意这里是接近$d_{max}$，而不是远离障碍物。当路径点和障碍物之间距离小于$d_{max}$时，路径点会远离障碍物物，当路径点和障碍物之间距离大于$d_{max}$时，路径点会接近障碍物。
（2）第二项的作用是使路径的曲率接近${\kappa }_{max}$，这一项的效果与上一项类似。
（3）第三项会将路径“掰直”，并且使点变的均匀。

从这三项的作用可以看出，如果只有这一部分的话，并不能得到一个理想的曲线，比如第一项会将路径点向$d_{max}$方向移动，这意味着如果车辆起始位置远离障碍物，则会先改变方向冲着障碍物开一段，然后再环绕障碍物运动这显然不是一个合理的方式，尽管会使整体路径长度变短。

3.3 Voronoi field

3.3.1 github repo[2]中使用方法

为了解决3.1节中目标函数存在的问题，引入了Dynamic Voronoi目标项，该目标项计算方式如下：

从式中可以看出，需要实时更新的参数是路径点到最近的障碍物的距离以及路径点到最近的voronoi边的距离。要获得这些参数需要对整张地图进行实时更新。

在运行代码的过程中发现，如果地图的尺寸放大到1500x800，在刚刚开始建图的时候会出现计算较慢的情况，大约700ms/frame，这是由于在初始情况下，添加障碍物点后，需要更新的点较多，导致迭代的时间过长；而在添加了较多障碍物点后，再进行更新，速度就比较快，小于100ms/frame。

3.3.2 Voronoi field的优化可能

考虑对这个过程进行优化，采用其他的方法进行地图更新，缩短计算时间。地图更新的最终目标有两个：
（1）获得每个点到最近的障碍物的距离
（2）获得每个店到最近的边的距离
文献[1]中注释4提到可以用计算distance field的方法快速计算点到障碍物的距离。在图像领域，每个点到最近的障碍物距离其实是求一个distance transform。考虑使用下述链接中的方法：

https://stackoverflow.com/questions/7426136/fastest-available-algorithm-for-distance-transform

经过测试Meijster-distance方法可以在1500x800的map上实现小于70ms的更新速度

针对（2），有大约有两个类似的研究方向：skeletonize/medial axis transform，目前考虑有三种求法：
（i）基于distance transform求解
（ii）voronoi diagram
（iii）image thinning

测试过（iii）image thinning方法中的Zhang-Suen方法，特别慢13s/frame。（ii）voronoi diagram是[2]中使用方法，但是对应的文献[3]中使用的是brushfire方法，不知道和速度较快的sweepLine方法有没有什么关系，这里可以留待后续测试。（i）方法是目前首要考虑的，利用求解出的distance map，寻找局部最大值，获得完整的voronoi edge。求解局部最大值的过程利用文献[4]的方法。目前还没有找到现成的代码，有可能需要自己写。