基于采样的自动驾驶规划算法 - PRM，RRT，RRT*，CL-RRT

本文将讲解PRM，RRT，RRT*自动驾驶规划算法原理，不正之处望读者指正

0 前言

机器人运动规划的基本任务：从开始位置到目标位置的运动

（1）如何躲避构型空间出现的障碍物

（2）如何满足机器人本身在机械、传感方面的速度、加速度等限制

基于采样的运动规划算法就是解决如何躲避构型空间出现的障碍物。

配置空间

机器人规划的配置空间概念：一个空间包含所有机器人自由度的机器人配置，描述为 C − s p a c e C-space C−space

机器人配置 ：表示对机器人上点的位置的描述
机器人自由度 ：规划的时候用最少的坐标数量去表示机器人配置
机器人配置空间：一个空间包含所有机器人自由度的机器人配置，描述为C-space

机器人的位姿在C-space中描述为一个点

机器人配置空间的意义：

在工作空间中进行规划，机器人有不同的形状和大小，需要根据不同的形状大小去做碰撞检测，是费时费力的。

在配置空间中做规划

机器人在C-space中表示一个点，障碍物做特殊的处理，把工作空间中的障碍物变成配置空间中的障碍物C-obstacle，这个工作是在运动规划前完成的，一次完成的工作。

障碍物按照机器人尺寸进行膨胀，上面机器人被设置成了一个点，只要点在障碍物外面，就不会发生碰撞

C-space = C-obstacle + C-free
经过配置空间的处理，路径规划变成了在C-free中找到起点到终点的路径寻找

1 概率路线图（PRM）

1.1 核心思想

（1）学习预处理阶段

在配置空间中随机采样足够密集的点

如果可以相互到达，连接附近的点

（2）查询搜索阶段

采用图搜索算法对G搜索，如果能找到起始点S到终点G的路线，存在可行路径

1.2 PRM主要步骤

（1）采样足够密集的点学习地图结构

（2）对采样的点碰撞检查，只保留在C-free中的采样点

（3）每个点通过直线连接到最近的邻居

（4）删除碰撞连接

（5）无碰撞连接被保留为边构造图

（6）添加起点s和终点g到Graph中

（7）利用图搜索算法A*/Dijstra在路线图里面搜索出一条最优路径

1.3 算法流程

PRM算法流程

1 learning-phase阶段：
V V V：构建的图的所有顶点的集合
E E E：图中所有边的集合

2 采样点个数为n

3 通过某种采样策略，不同分布得到采样点

4 以采样点为中心， r r r为半径，在这个圆范围内的邻居节点，把它记录到 U U U中

5 把采样点加入到顶点集 V V V中

6 遍历邻居节点集 U U U的每个节点

7-8 定义一些规则滤除一些节点和边

7 采样点 x r a n d x_{rand} xrand和已有的节点处在相同的邻接元素下，跳过

8 碰撞检测，检测 x r a n d x_{rand} xrand和 u u u是不是发生碰撞，如果Free，就把 x r a n d x_{rand} xrand和 u u u连成的边加入到 E E E中

9 重复n次之后，就得到了一个完整的图 G = ( V , E ) G = (V,E) G=(V,E)

最后应用图搜索算法在G上找到一条最优路径

sPRM算法与PRM算法的区别：

只要采样到某个节点，就把以r为半径圆里面所有的节点都进行一个连接，边比PRM多，搜索消耗的资源更大

选择节点之间的连接方式：

（1）k近邻PRM

选择采样点周围最近的k个邻居
U ← k N e a r e s t ( G = ( V , E ) , v , k ) U\gets kNearest(G=(V,E),v,k) U←kNearest(G=(V,E),v,k)

（2）有界维度PRM

就是以常规的PRM算法为基础，如果圆里面采样点过多，就找采样点的k个邻居取交集
U ← N e a r ( G , x r a n d , r ) ∩ k N e a r e s t ( G = ( V , E ) , v , k ) U\gets Near(G,x_{rand},r)\cap kNearest(G=(V,E),v,k) U←Near(G,xrand,r)∩kNearest(G=(V,E),v,k)

（3）可变半径PRM

把r为半径的圆作为采样节点个数n的函数，采样点较少情况下，r可以取大一点，采样点足够多的时候，r取小一点

PRM*算法流程

d d d：维度
n n n：采样节点个数

1.4 PRM算法的优点和缺点

优点：

概率完备性，如果运行时间足够长（或者采集足够多的点），如果有解一定是最优解

缺点：

（1）在整个状态空间上构造图 ，需要连接特定的开始和目标，可能浪费一些不必要的资源

（2）使用直线连接不符合车辆运动学约束

（3）抽样方法的完备性很弱 ，即使空间存在合理的路径，由于抽样参数的设置，也可能无法找到路径。因为随机抽样，所以该方法稳定性也不好，对于同样的问题，前后两次解不一样，在严格要求稳定性的场合不适用

采样点的数量 和采样点存在通路的最大距离是路径规划成功的关键

2 RRT

2.1 RRT核心思想和特点

RRT是一种通过随机构建空间填充树来有效搜索非凸，高维空间的算法。

核心思想：RRT 算法首先将起点初始化为随机树的根节点，然后在机器人的可达空间中随机生成采样点，从树的根节点逐步向采样点扩展节点，节点和节点之间的连线构成了整个随机树，当某个节点与目标点的距离小于设定的阈值时，即可认为找到可行路径。

RRT的特点就是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划

2.2 算法流程

1 将 x i n i t x_{init} xinit加入到顶点集 V V V中

2 采样n次

3 随机采样得到 x r a n d x_{rand} xrand

4 图中距离 x r a n d x_{rand} xrand最近的节点 x n e a r e s t x_{nearest} xnearest

5 连接 x r a n d x_{rand} xrand和 x n e a r e s t x_{nearest} xnearest，之间的节点 x n e w x_{new} xnew

6-7 只有通过碰撞检测，才会把 x n e w x_{new} xnew加入顶点集 V V V，连接 x n e a r e s t x_{nearest} xnearest和 x n e w x_{new} xnew

2.3 RRT优缺点

优点：

（1）简单找到起点到终点的路径，比PRM更高效，该算法通过尽可能少地探索环境 ，来实现有效的单一路径规划，对未知环境适应能力强

（2）RRT 算法通过随机树向未观察的空间区域生长，并且不会回归到已经探索过的区域 ，这实现了对空间的快速探索

（3）搜索方法不是维持固定的栅格结构，而是在运行中构建随机树，通过随机树内部的节点的连接找到路径。

缺点：

（1）不满足概率完备性，只能连接最近的节点

（2）需要对输入空间进行离散化 ，采样次数太少，则生成的路径将表现出较差的性能 ，采样次数太多则会增加整个规划过程的计算量 ，降低路径规划的实时性

（3）RRT算法生成的路径存在冗余的节点，增加机器人实际运行中的路程