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

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

0 前言

机器人运动规划的基本任务：从开始位置到目标位置的运动
（1）如何躲避构型空间出现的障碍物
（2）如何满足机器人本身在机械、传感方面的速度、加速度等限制

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

配置空间

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

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

机器人的位姿在C-space中描述为一个点
机器人配置空间的意义：

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

在配置空间中做规划

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

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

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

1 概率路线图（PRM）

1.1 核心思想

（1）学习预处理阶段

1. 在配置空间中随机采样足够密集的点
2. 如果可以相互到达，连接附近的点

（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$：构建的图的所有顶点的集合
$E$：图中所有边的集合
2 采样点个数为n
3 通过某种采样策略，不同分布得到采样点
4 以采样点为中心，$r$为半径，在这个圆范围内的邻居节点，把它记录到$U$中
5 把采样点加入到顶点集$V$中
6 遍历邻居节点集$U$的每个节点
7-8 定义一些规则滤除一些节点和边
7 采样点$x_{rand}$和已有的节点处在相同的邻接元素下，跳过
8 碰撞检测，检测$x_{rand}$和$u$是不是发生碰撞，如果Free，就把$x_{rand}$和$u$连成的边加入到$E$中
9 重复n次之后，就得到了一个完整的图$G=(V,E)$
最后应用图搜索算法在G上找到一条最优路径

sPRM算法与PRM算法的区别：

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

选择节点之间的连接方式：
（1）k近邻PRM
选择采样点周围最近的k个邻居
$$U\leftarrow kNearest(G=(V,E),v,k)$$
（2）有界维度PRM
就是以常规的PRM算法为基础，如果圆里面采样点过多，就找采样点的k个邻居取交集
$$U\leftarrow Near(G,x_{rand},r)\cap kNearest(G=(V,E),v,k)$$
（3）可变半径PRM
把r为半径的圆作为采样节点个数n的函数，采样点较少情况下，r可以取大一点，采样点足够多的时候，r取小一点

PRM*算法流程

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

1.4 PRM算法的优点和缺点

优点：

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

缺点：

（1）在整个状态空间上构造图，需要连接特定的开始和目标，可能浪费一些不必要的资源
（2）使用直线连接不符合车辆运动学约束
（3）抽样方法的完备性很弱，即使空间存在合理的路径，由于抽样参数的设置，也可能无法找到路径。因为随机抽样，所以该方法稳定性也不好，对于同样的问题，前后两次解不一样，在严格要求稳定性的场合不适用

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

2 RRT

2.1 RRT核心思想和特点

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

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

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

2.2 算法流程

1 将$x_{init}$加入到顶点集$V$中
2 采样n次
3 随机采样得到$x_{rand}$
4 图中距离$x_{rand}$最近的节点$x_{nearest}$
5 连接$x_{rand}$和$x_{nearest}$，之间的节点$x_{new}$
6-7 只有通过碰撞检测，才会把$x_{new}$加入顶点集$V$，连接$x_{nearest}$和$x_{new}$

2.3 RRT优缺点

优点：

（1）简单找到起点到终点的路径，比PRM更高效，该算法通过尽可能少地探索环境，来实现有效的单一路径规划，对未知环境适应能力强
（2）RRT 算法通过随机树向未观察的空间区域生长，并且不会回归到已经探索过的区域，这实现了对空间的快速探索
（3）搜索方法不是维持固定的栅格结构，而是在运行中构建随机树，通过随机树内部的节点的连接找到路径。

缺点：

（1）不满足概率完备性，只能连接最近的节点
（2）需要对输入空间进行离散化，采样次数太少，则生成的路径将表现出较差的性能，采样次数太多则会增加整个规划过程的计算量，降低路径规划的实时性
（3）RRT算法生成的路径存在冗余的节点，增加机器人实际运行中的路程

2.4 RRG

RRT的变体，具有概率完备性

核心思想：

不要只连接$x_{new}$和$x_{nearest}$
尝试连接到半径内的所有顶点

最后需要接入图搜索算法寻找一条最优路径，违背了RRT的初衷，没有把构造图和搜索步骤合二为一

2.5 基于运动学的RRT

区别在于5 使用基于运动学的方法来引导两个节点

3 RRT*

3.1 核心思想

（1）相比于RRG算法，维护树结构而不是图，会从图中删除多余的边
（2）相比于RRT算法，添加了“rewire"操作（每次采样到新的节点，会把以他为圆心，半径为r的圆内其他节点作为一个考量，对这些节点做一些修剪的操作）确保通过最小成本路径到达顶点

3.2 RRT*算法流程

前半部分与RRT相同

后半部分
（1）连接以r为半径的圆的所有顶点，在集合中选择cost最小的去连接

（2）得到了$x_{near}$，依次遍历每一个节点，判断累计成本最小的，将$x_{near}$标记为$x_{min}$：保证$x_{new}$本身的最优性
（3）对树做修剪：每次采样到$x_{new}$之后，周围其他节点都会做一次检查，判断是否能找到cost最小的路径

4 CL-RRT

核心思想：
（1）相比于对车辆输入进行采样的标准的RRT，CL-RRT对控制器的输入进行采样
（2）通过前向模拟得到动态可行轨迹
（3）对于城市场景，优化算法策略：采样策略、节点选择策略

转向控制器：Pure-Pursuit Controller

速度控制器：PI Controller


采样策略：

$n_r、n_{\theta }$：具有高斯分布的随机变量
${\sigma }_r$：径向标准差
${\sigma }_{\theta }$：圆周方向标准差

根据车辆位置和道路规则改变这些参数

Node选择策略：
（1）RRT试图将样本连接到树中最近的节点，当RRT应用于转弯能力有限的车辆时，需要进行拓展
（2）CL-RRT算法使用节点和采样点之间的Dubins路径长度作为距离度量

Reeds-Shepp曲线和Dubins曲线