自动驾驶路径规划之随机采样算法
一、PRM 概率路线图算法
PRM（Probabilistic Roadmap）概率路线图算法是一种经典的基于随机采样的路径搜索算法 Error! Reference source not found.。PRM 算法将路径规划分为两个步骤，首先通过随机采样和碰撞检测构建地图空间的完整连通属性，使用稀疏的无向图描述，其次采用基于图搜索的算法规划可行驶路径。PRM 算法的关键在于将地图空间抽样为无向图，使其在保留地图原有障碍物特征以及连通性特征的基础上大大压缩数据量，从而提高路径搜索的速度。
在 MATLAB 中使用 Robotic Tool Box 工具箱构建一个 30*30 的地图，并设置起点与终点，采样点数量为 100，步长限制为 6 ，使用 PRM 算法抽样地图构建无向图，使用 Dijkstra 算法搜寻最短路径。绿色点为起点，黄色点为终点，品红色连线为最终搜寻的路径。
图1 PRM算法路径规划
从图1中可以看出，使用PRM算法能够在起点与终点之间规划出一条可行使的路径，PRM算法中有两个关键参数：采样点数量和采样步长。采样点用于抽样原始栅格地图，数量越多找到最优路径的概率就越大，但同时计算与搜索的时间也会更长；采样步长用于限制随机点的分布范围以及子路线的长度，步长限制越小，随机点的分布就会相对更加集中，最终生成的路径也会由一段一段拼接而成，而非一长段的路线，相对更加平滑。PRM算法的优点在于其一它是概率完备的，构建图中若存在解，则一定能够找到；其二通过对地图空间的抽样简化，具有更高的搜索效率。然而由于PRM采点的随机性，当地图中存在狭长通道时，可能会规划失败，且每次规划的结果都不一致，不适用于对路径一致性要求较高的场景。
二、RRT快速扩展随机树算法
RRT(Rapidly Exploring Random Trees)快速扩展随机树算法与PRM算法类似，也是一种概率完备的非最优路径规划算法，且都是基于随机采样的规划算法Error!Referencesourcenotfound.，而不同点在于PRM抽样地图空间生成的是“图”，而RRT算法抽样生成的是“树”。RRT算法的特点在于具备探索空间的能力，即从一个或两个根结点出发，通过随机采样、碰撞检测的方式向外随机增加叶子结点，最终生成一颗随机树。简单的RRT算法有单树RRT和双树RRT两种类型，单树RRT算法将规划起点作为根结点，向外随即扩展直到规划终点被纳入随机树；双树RRT则将规划起点和终点同时作为根结点，以同样的方式向外探索，直到两棵随机树相遇，较之单树RRT具有更高的规划效率。
单树RRT算法初始时设置规划起点为根结点，其迭代的流程如下：
1.生成一个随机采样点NewNode，并判断该采样点是否位于自由区域；
2.遍历当前随机树，使用距离函数找到距离NewNode最近的结点ClosetNode；
3.步长限制：判断NewNode和ClosetNode是否在步长限制范围内，否则以二者的间点代替
NewNode并回到步骤1；
4.碰撞检测：判断NewNode和ClosetNode之间是否存在障碍物，若存在则舍弃NewNode并回到步骤1；
5.若NewNode满足以上约束条件，则将其纳入随机树，并设置ClosetNode为其父结点；
6.循环步骤1至步骤5，直到NewNode在规划终点的步长限制范围内，且满足碰撞检测，则将规划终点纳入随机树并将NewNode设为规划终点的父结点，结束迭代。
双树RRT算法的迭代流程与单树RRT算法别无二致，二者算法流程上的差异仅在：
1.双树RRT算法以规划的起始点和规划的终止点同时作为根结点，交替地向外扩展，从而产生两棵随机树；
2.双树RRT算法每轮迭代会判断两棵随机树是否相遇。设以规划起点为根结点的随机树成功扩展新增结点NewNodeA，此时算法会遍历以规划终点为根结点的随机树，找到距离NewNodeA最近的结点ClosetNodeB，并判断这两个结点是否满足步长限制和碰撞检测，若满足，则认为两树相遇，迭代结束。
在MATLAB中使用Robotic Tool Box工具箱构建一个100*100的地图，绿色点为规划起点，黄色点为规划终点，不限制采样点数量，步长限制为3，品红色连线为单树RRT算法搜索的路径，绿色与黄色的连线为双树RRT算法搜索的路径。
图 2 RRT 算法路径规划
从图 2 中可以看出，单树 RRT 和双树 RRT 算法都能够找到一条连接起点与终点的路径，且单树 RRT 算法用时为 86.73 秒，双树 RRT 算法用时为 6.21 秒。RRT 算法的优势在于其概率完备，当路径规划有解时总能找到有效路径，但该算法不考虑运动学约束。生成的路径转弯较多，不够平滑。
