移动机器人路径规划方法概览

概述

在路径规划中，几个名词的含义为：

完备性：是指如果在起始点和目标点间有路径解存在，那么一定可以得到解，如果得不到解那么一定说明没有解存在；

概率完备性：是指如果在起始点和目标点间有路径解存在，只要规划或搜索的时间足够长，就一定能确保找到一条路径解；

最优性：是指规划得到的路径在某个评价指标上是最优的（评价指标一般为路径的长度）；

渐进最优性：是指经过有限次规划迭代后得到的路径是接近最优的次优路径，且每次迭代后都与最优路径更加接近，是一个逐渐收敛的过程；

路径规划的过程大致如下图所示，主要包括信息获取-感知-通信-决策-控制-执行，一般狭义的路径规划指的是决策部分。

主要方法

基于图搜索的规划方法

基于图搜索的方法是最常见的路径规划方法，不仅在机器人，甚至在网络中（如路由的寻路转发中）也有广泛的应用。
几种经典的方法，包括Floyd，Bellman-Ford，Dijkstra，A*我在这里就不赘述了，相关的资料以及十分齐全了。这里主要介绍一些A*的改进版。

D*

D*是Anthony Stentz 1994年发表在ICRA上的， Optimal and Efficient Path Planning for Partially-KnownEnvironments 。关于D*算法我之前写了一篇博客，感兴趣可以戳链接。相比A-star算法，D-star的主要特点就是由目标位置开始向起始位置进行路径搜索，当物体由起始位置向目标位置运行过程中，发现路径中存在新的障碍时，对于目标位置到新障碍之间的范围内的路径节点，新的障碍是不会影响到其到目标的路径的。新障碍只会影响的是物体所在位置到障碍之间范围的节点的路径。通过将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播，能最小程度的减少计算开销。D*路径搜索的过程和Dijkstra算法比较像，A-star算法中f(n)=g(n)+h(n)，h(n)在D-star中并没有体现，路径的搜索并没有A-star所具有的方向感，即朝着目标搜索的感觉，这种搜索更多的是一种由目标位置向四周发散搜索，直到把起始位置纳入搜索范围为止，因此，D_star算法虽然能够在障碍物发生变化时找到一条路径，但不一定是一条最短的路径

LPA*

Lifelong Planning A*是Sven Koenig 和 Maxim Likhachev在2004发表在Artificial Intelligence上的。这个方法在A*的基础上，加了一步对cost变化的处理。定义了一个rhs函数

rhs函数的作用是通过前瞻一步的方式来判断cost的变化。同时定义了三种状态

处于局部一致状态就是说明环境中的最短路径没有发生改变，处于过一致状态说明可以通过改变父节点降低当前的cost，处于欠一致状态则说明由于某个父节点的cost发生变化导致最短路径的cost变大，需要重新规划。伪代码中对应的是下图红框部分

主要思想是当某个节点不可达时，将以其为父节点的子节点放入U中重新考察，同时利用之前探索的部分继续向目标拓展，直到重新规划出一条可以到达目标的路径。因为这一类方法是通过对以前搜索的信息进行再利用来减少搜索空间，所以又称为增量式搜索。
LPA*和A*一样，可以通过启发函数来减少搜索空间，同时可以解决环境发生变化的情况。缺点就是，同D*不同，它是从起点向终点搜索，这就意味着，当机器人移动后，他必须重新计算启发函数再进行搜索，会造成很大的浪费。

D* Lite 和Field D*

D* Lite还是上面两个老哥在2005年发表在Trans of Robotics上，Fast Replanning for Navigation in Unknown Terrain
为了解决之前所说的LPA的问题，很自然地想到是否可以将LPA和D结合起来，没错，这就是D Lite。D* Lite 的逻辑基本和LPA*差不多，不过是从终点向起点扩展，这里也不多介绍了。不过科研就是发现问题和解决问题，很快，又有人提出新的问题了，基于图的方法总是将图划分成栅格，再在栅格中心移动，这样实际限制了机器人的移动。举个例子，下图机器人从左下到右上肯定是走红线比较近，但是如果按照图搜索每次移动一个栅格来规划的话，得到的路径就是蓝线。

那理所应当的想法就是不一定非要走栅格中心，这就是Field D*了，2005年由David Ferguson 和 Anthony (Tony) Stentz 提出，主要思想是我不用之前那种从一个中心到另一个中心来计算cost了，我换成从一个中心到边界，再从边界到另一个中心的方式。这样，就可以规划出红线了。

总的来说，基于图的搜索方法都需要全局信息。如果你没有所有的信息，A*可能会出错；D*的贡献在于，它能纠正那些错误而不用过多的时间。LPA*用于代价会改变的情况。在A*中，当地图发生改变时，路径将变得无效；LPA*可以重新使用之前A*的计算结果并产生新的路径。然而，D*和LPA*都需要很多内存——用于运行A*并保存它的内部信息（OPEN和CLOSED集，路径树，g值等），当地图发生改变时，D*或者LPA*会告诉你，是否需要就地图的改变对路径作调整。在一个有许多运动者的物体的游戏中，你经常不希望保存所有这些信息，所以D*和LPA*在这里并不适用。它们是为机器人技术而设计的，这种情况下只有一个机器人——你不需要为别的机器人寻路而重用内存。

基于采样的规划方法

图搜索的基于遍历或者改进遍历的逻辑，对于高维的情况，比如多自由度机械臂等情况，很容易出现指数爆炸。而且，在实际场景中，能找到最优的路径固然是最好的，但更多的情况是，能找到一条次优或者可行的较优路径就行。这种情况下，基于采样的规划方法应运而生，其中最典型的就是PRM和RRT。

Probabilistic Roadmap Method (PRM)

PRM全称是Probabilistic Roadmap Method，上世纪90年代初由M.H.Overmars等人提出的，伪代码如下

算法十分简洁有效，主要思想是现在全局建立一个道路拓扑图，然后利用这些道路拓扑来规划路径。好比全国修了若干条铁路，之后不管你要去哪，只要上离起点最近的车站，再在终点附近的车站下车即可。

缺点也很明显，首先很难保证最优，其次，路径的性能很大程度取决于生成道路图的质量，而道路图的质量又和节点数以及边数相关，也就是算法中的n和k，一个典型的trade-off问题。

Rapidly-Exploring Random Tree (RRT)

RRT算法的伪代码如下

核心思想是每次随机从全局选中一个点，并使随机树向该方向生长，当到达终点或者终点附近时停止生长。单向的随机树其实效率并不高，而且对特定情况效率很差，比如下图所示狭窄路口

对RRT的改进一般从以下三个方面着手
1）如何选取随机点？
2）如何选择距离？
3）如何扩展随机树？
第一个问题很好理解，完全随机肯定是低效的，最简单的逻辑就是往终点优先选取。第二个问题可以体现RRT的一个优点，能够考虑机器人的非完整约束（如车辆的最大转弯半径和动量等），在图搜索中，无论用欧式距离，曼哈顿距离或者切比雪夫距离等，其实影响不太大。但是考虑下图情况

以差分机器人为例，前进显然是最容易实现的，原地转向则需要考虑转弯半径，侧向移动则是完全无法实现。那么单纯用欧式距离显然是不合理的，一种朴素的想法是通过剪枝把不合理的边裁掉，但是在代码中也不是特别好实现，另一种常见的方法是对不同的移动通过赋予不同的权值，来尽量避免不合理的运动。
第三个问题，如何扩展？扩展距离过大，容易造成震荡，过小则增大了计算量。
对RRT的改进有两种比较经典的方法，RRT Connected和RRT*

RRT connected/Bi-RRT

为了使随机树能迅速扩展到终点附近，采取双向搜索的方法（这个思想很常见，Dijkstra和A*也有类似的改进）。不过这里还是有一个小细节，起码一开始是我没想到的。双向搜索正常逻辑就是随机取一个点，然后两棵随机树往该处生长，直到相遇或者小于一定阈值。这样做虽然是比单随机树要快，但是还有很大改进空间，因为随机取点很可能并不是我们要拓展的发现，最好的逻辑是什么呢？就是我下图红框处，既然已经找到一个中间节点，那么我就铆足了劲往这冲就对了，直到遇见障碍物或者相交。

当然，这样做的弊端是什么呢？RRT本身会拓展出一些绕远路的路径，而上述这种死循环会无限放大错误的路径。那怎么办呢？于是就有人提出了一种办法，我每次拓展完都修剪一下，把cost优化一下，这样最后得到的路径起码是较优的，这就是RRT*。

RRT*

伪代码如下

具体来讲，RRT*做了两件事，也就是上面红框的部分
下图中的序号表示随机树拓展的顺序，9号是新拓展的，原始父节点为4号，我们以9号为圆心，半径为事先设好的参数，画圆。
1）试着将9号挂到圆内的其他节点上，最后留下使9号cost最小的，作为新的父节点，并替换原边，比如这里为5号。

2）还是在该圆范围内，若其他节点以9号为新的父节点可以降低cost，则替换其原父节点，如下图中的6号。

我们之所以将RRT*称作渐近最优原因就是，当节点足够多，迭代次数足够多的时候，RRT*是可以收敛到全局最优的。

Interpolating Curve Planners 插值曲线规划

插值法主要是利用计算机图形技术来对一系列的路标点形成的路径做平滑处理。逻辑就是在已知一系列轨迹点的情况下，考虑动力学约束和环境约束，利用一些曲线生成一条可行的平滑的路径。
常见的插值曲线有

直线和圆

这个方法比较直观，假设有若干个点，直线没啥好说的，圆的话，只要相邻三个点不共线，总能找到一段圆弧同时过这三个点。

回旋曲线

回旋曲线一般设置在直线与圆之间或大圆与小圆之间，由较大圆向较小圆过渡的一种曲线，在道路设计中应用较为广泛。回旋曲线的曲率半径连续变化，曲率变化速率可依据需要灵活设置，能平稳的在直线和圆弧之间过渡而不引起曲率的突变。回旋曲线进行路径规划具有以下优点：
1）曲线曲率连续，无突变
2）减小了离心力的变化，增加了机器人稳定性；
回旋线的基本公式为
$Rl=s^2$
其中：
$R$是回旋线上某点的曲率半径
$l$是该段回旋线上某点到该段起点的曲线长
$s$为常数参数

多项式曲线

常用的是三次曲线和五次曲线，为什么呢？这要考虑实际背景，插值的逻辑是在一系列机器人的轨迹点上做平滑，隐含的条件就是这些点首先机器人是可达的，一系列剧烈震荡的点再怎么平滑也没有用。所以，我们考虑假设有若干个点，我们可能获得哪些条件。比如，起点的初速度，位移肯定为0，终点的速度位移也应该为0，这就是最简单的约束，一共四个条件，对应的就是一条三次曲线
$s(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3$
更进一步地，如果对加速度也有相应的约束，那么就可以拟合成一条五次曲线。

Bezier（贝塞尔）曲线

考虑一种情况，n个顶点连接成为平滑的曲线。那肯定得在这些顶点之间插值了，最简单的办法是用直线全连起来，当然，如果用上述曲线也行，但这些插值的方法都有一个问题，局部可能是光滑的，整体上未必是平滑曲线，那么是否存在一个曲线方程，根据这个曲线方程来找到这些插值的点，而且这条曲线方程不仅过原来条件中规定的n个顶点，还是全局光滑的。这就是贝塞尔曲线了。如下图所示，锚点（anchor）就是我们要过的顶点，控制点（control）是用来控制贝塞尔曲线的参数。通过调整控制点的位置，可以实现全局光滑。怎么确定这些控制点大家可以参考这个博客。

贝塞尔插值的效果这个博主画了很多漂亮的实现。

总结

这里主要介绍了三类机器人路径规划方法：基于图搜索，基于采样和基于插值，但是实际上这些方法都有许多局限性，而且目前机器人（甚至自动驾驶）规划考虑的问题越来越接近实际，比如上述方法说是可以动态避障，实际上并不是真动态避障，更多是依赖于先验的环境信息做的决策，而非向DWA那样，实时根据环境改变。另一个点就是实际上除了考虑环境，更多要考虑其他机器人（车辆）的行为，基于行为的预测来做决策，这是更难的。当然，科研有很大一个方向就是没事找事，比如机器人感知受限，控制受限这种，在附加约束情况下去做规划，也是目前许多人做的一个方向。

参考文献：

https://www.cnblogs.com/21207-iHome/
https://blog.csdn.net/lqzdreamer
A Review of Motion Planning Techniques for Automated Vehicles