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

路径规划方法综述

• 概述
• 主要方法
• 基于图搜索的规划方法
• D*
• LPA*
• D* Lite 和Field D*
• 基于采样的规划方法
• Probabilistic Roadmap Method (PRM)
• Rapidly-Exploring Random Tree (RRT)
• RRT connected/Bi-RRT
• RRT\*
• Interpolating Curve Planners 插值曲线规划
• 直线和圆
• 回旋曲线
• 多项式曲线
• Bezier（贝塞尔）曲线
• 总结
• 参考文献：

概述

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

完备性：是指若是在起始点和目标点间有路径解存在，那么必定能够获得解，若是得不到解那么必定说明没有解存在；

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

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

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

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

主要方法

基于图搜索的规划方法

基于图搜索的方法是最多见的路径规划方法，不只在机器人，甚至在网络中（如路由的寻路转发中）也有普遍的应用。
几种经典的方法，包括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_1t+a_2t^2+a_3t^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