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

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

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*的主要思想就是在生成一条初始路径后，构建出一个椭圆区域，然后在椭圆区域内进行随机采样并更新路径，这样就避免了在无效区域内的浪费时间，从而极大提升了路径优化的效率。