常用规划算法解析 — 采样篇

这篇文章想和读者分享一下自动驾驶规划算法中一类较常用的算法，基于采样的规划算法。之前的文章中说到，基于图搜索的规划算法总是能给出全局范围内的最优解，而以此为代价的就是当地图过大，规划的维度过高时，它的搜索效率就会变得很慢。在某些场景下，我们不一定需要规划出的路径是全局最优解，只要是可行解就能满足我们的需求，而我们的关注点主要放在求解效率上。这时，我们就可以为了效率牺牲最优性，也就引出了这篇文章的内容——基于采样的路径规划算法。

PRM 算法

首先介绍的第一种就是经典的PRM算法（Probabilistic Road Map）。PRM主要包含了两个步骤，一是学习阶段，二是查询阶段。在学习阶段中，其会在地图空间中进行均匀的随机采样，并删除采样落在障碍物上的点，接着对相邻的点进行连接并做碰撞检测，剔除不是collision-free的连线。而在查询阶段中，就是利用上一步构建好的采样节点及连续，运用图搜索的方法（Dijkstra或者A*）来找出一条可行路径。 

考虑到上述的算法缺陷，有一种提升PRM的方法就是Lazy Collision-checking，即在学习的阶段不考虑障碍物的碰撞（不进行collision-checking的步骤），而在查询阶段，如果搜索出的路径中含有碰撞的部分，则删除该部分并只对这部分进行相邻节点的重新寻路。　

RRT算法

接下来，介绍第二种基于采样的算法，也是最常用的一种——RRT(Rapidly-exploring Random Tree)算法。RRT其实代表了一系列基于随机生长树思想的算法，也是目前机器人领域运用最广泛、优化变种最多的一类算法，首先介绍一下最原始的RRT算法。 

RRT的算法流程伪代码如上图所示，首先对地图进行随机采样，得到采样点x_rand。再从已有的树中找到距离x_rand最近的点x_near，从ｘ_near往x_rand的方向行驶距离StepSize得到x_new，并将x_near与x_new的连线添加进一个临时变量中。接着对这段路进行碰撞检测，如果无碰撞，则将x_new添加进树中。如此不停循环，直到x_new到达期望的终点时结束。 

最终的运行效果如上图所示，可以看出rrt算法虽然很快，但给出的解往往是曲折环绕的，控制模块往往也无法跟踪这样的路径。该算法的优缺点如下：

RRT Family 

上面说到RRT的速度是非常快的，但是针对Narrow Passage这种场景，传统的RRT算法就会显得比较笨拙，需要花费很长的时间才能找到可行路径，如下图所示。 

为了解决这个问题，需要对RRT算法进行一些改进。
1. Bidirectional RRT第一种改进思路就是不同于只从起点开始出发，而是从起点和终点同时出发，从而构建出两棵树，当两棵树相互连接至一起时，则说明已经找到路径，进而进行双回溯得到路径。 这种思路可以很好地解决当终点落在Narrow Passage内这种场景，由于从狭窄通道内直接出发寻点，采样到驶出通道点的概率也就高了许多。
2. RRT*在RRT算法中，每次都会选择距离最近的点作为新加入点的父节点，但显然这种做法不一定是最合理的。而RRT*所进行的改变主要为两点：重选父节点和重连接。RRT*的伪代码如下所示： 

RRT*的思想是，它在采样点加入进树结构之后，以其为圆心再画一个圆，在这个圆之内，搜索是否有其余点与采样点连接后，能够使得起点到采样点的距离更短。如果有的话，就对采样点的父节点进行更新，并重连采样点与其新的父节点。RRT*的优势就在于，不同于RRT算法只找到一条可行路径就结束，RRT*会不停的循环迭代，不停地更新当前路径，因此只要时间足够长的话，RRT*是可以求得最短路径的。下图中就可以很明显的看出RRT*求得的最终结果和RRT的最终结果，显然RRT*搜索出的路径是短许多的。 

3. Kinodynaimic RRT* 上面讲的这类RRT算法都是简单的将两点进行连线，这样导致最终生成的路径会相当不平滑，甚至相邻的线段都是无法满足机器人的动力学要求的。也就是说虽然规划出了路径，但是给到控制模块却没有办法执行。因此，不同于直线连接，而是通过曲线、多项式等来连接x_near和x_new，从而使得生成的路径能够符合车辆动力学的相关约束。 4. Any-Time RRT* Any-Time RRT*如名称所示，其会不停地更新路径，即不停地以当前位置作为规划起始点，来重新规划路径。这种方式可以更好地应对动态环境变化比较多的场景。

5. Informed RRT*前面说到RRT*在找到一条路径之后，继续不停地在全局空间中进行重采样从而更新路径。但是在已经找到一条路径的情况下，再在全局范围内随机撒点显然不是一种高效的行为，而Informed RRT*的主要思想就是在生成一条初始路径后，构建出一个椭圆区域，然后在椭圆区域内进行随机采样并更新路径，这样就避免了在无效区域内的浪费时间，从而极大提升了路径优化的效率。 

椭圆区域的构建方法为以起点和终点作为椭圆的焦点，以生成的路径的长度作为椭圆方程中的常数。这样随着路径不停地变短，生成的椭圆也不停地缩小，这样使得路径的收敛也更加快。 

以上就介绍完了几种比较常见的基于采样的路径规划方式以及它们的一些进阶版，欢迎大家交流讨论~- End -