本文将讲解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算法生成的路径存在冗余的节点，增加机器人实际运行中的路程
2.4 RRG
RRT的变体，具有概率完备性
核心思想：
不要只连接 x n e w x_{new} xnew和 x n e a r e s t x_{nearest} xnearest
尝试连接到半径内的所有顶点
最后需要接入图搜索算法寻找一条最优路径，违背了RRT的初衷，没有把构造图和搜索步骤合二为一
2.5 基于运动学的RRT
区别在于5 使用基于运动学的方法来引导两个节点
3 RRT*
3.1 核心思想
（1）相比于RRG算法，维护树结构而不是图，会从图中删除多余的边
（2）相比于RRT算法，添加了“rewire"操作（每次采样到新的节点，会把以他为圆心，半径为r的圆内其他节点作为一个考量，对这些节点做一些修剪的操作）确保通过最小成本路径到达顶点
3.2 RRT*算法流程
前半部分与RRT相同
后半部分
（1）连接以r为半径的圆的所有顶点，在集合中选择cost最小的去连接
（2）得到了
x
n
e
a
r
x_{near}
xnear，依次遍历每一个节点，判断累计成本最小的，将
x
n
e
a
r
x_{near}
xnear标记为
x
m
i
n
x_{min}
xmin：保证
x
n
e
w
x_{new}
xnew本身的最优性
（3）对树做修剪：每次采样到
x
n
e
w
x_{new}
xnew之后，周围其他节点都会做一次检查，判断是否能找到cost最小的路径
4 CL-RRT
核心思想：
（1）相比于对车辆输入进行采样的标准的RRT，CL-RRT对控制器的输入进行采样
（2）通过前向模拟得到动态可行轨迹
（3）对于城市场景，优化算法策略：采样策略、节点选择策略
转向控制器：Pure-Pursuit Controller
速度控制器：PI Controller
采样策略：
n
r
、
n
θ
n_r、n_\theta
nr、nθ：具有高斯分布的随机变量
σ
r
\sigma_r
σr：径向标准差
σ
θ
\sigma_\theta
σθ：圆周方向标准差
根据车辆位置和道路规则改变这些参数
Node选择策略：
（1）RRT试图将样本连接到树中最近的节点，当RRT应用于转弯能力有限的车辆时，需要进行拓展
（2）CL-RRT算法使用节点和采样点之间的Dubins路径长度作为距离度量
Reeds-Shepp曲线和Dubins曲线
