RRT路径规划算法概述

本文主要记录本人之前调研过在三维复杂环境下的路径规划算法。

RRT快速随机搜索树

快速扩展随机树(Rapidly-exploring Random Trees，RRT)算法，是近十几年得到广泛发展与应用的基于采样的运动规划算法，它由美国爱荷华州立大学的Steven M. LaValle教授在1998 年提出。RRT 算法是一种在多维空间中有效率的规划方法。原始的RRT 算法是通过一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由树节点组成的从初始点到目标点的路径。

RRT是一种通过随机构建空间填充树来有效搜索非凸，高维空间的算法。树是从搜索空间中随机抽取的样本逐步构建的，并且本质上倾向于朝向大部分未探测区域生长。由于它可以轻松处理障碍物和差分约束（非完整和动力学）的问题，并被广泛应用于自主机器人运动规划。

RRT也可以被看作是一种为具有状态约束的非线性系统生成开环轨迹的技术。一个RRT也可以被认为是一个蒙特卡罗方法。用来将搜索偏向一个配置空间中图形的最大Voronoi区域。一些变化甚至可以被认为是随机分形。

改进的RRT算法：

1. 基于概率P的RRT;

2. RRT_Connect’

3. RRT*;

4. Parallel-RRT;

5. Real-time RRT;

6. Dynamic domain RRT;

 

 

基于RRT的运动规划算法综述

介绍：

在过去的十多年中， 机器人的运动规划问题已经收到了大量的关注，因为机器人开始成为现代工业和日常生活的重要组成部分。最早的运动规划的问题只是考虑如何移动一架钢琴从一个房间到另一个房间而没有碰撞任何物体。早期的算法则关注研究一个最完备的运动规划算法（完备性指如果存在这么一条规划的路径，那么算法一定能够在有限时间找到它），例如用一个多边形表示机器人，其他的多边形表示障碍物体， 然后转化为一个代数问题去求解。 但是这些算法遇到了计算的复杂性问题，他们有一个指数时间的复杂度。在 1979 年，Reif则证明了钢琴搬运工问题的运动规划是一个 PSPACE-hard 问题[1]。所以这种完备的规划算法无法应用在实际中。

 

在实际应用中的运动规划算法有胞分法[2]，势场法[3]，路径图法[4]等。这些算法在参
数设置的比较好的时候，可以保证规划的完备性，在复杂环境中也可以保证花费的时间上限。然而，这些算法在实际应用中有许多缺点。 例如在高维空间中这些算法就无法使用， 像胞分法会使得计算量过大。 势场法会陷入局部极小值，导致规划失败[5],[6]。

 

基于采样的运动规划算法是最近十几年提出的一种算法，并且已经吸引了极大的关注。
概括的讲，基于采样的运动规划算法一般是连接一系列从无障碍的空间中随机采样的点，试图建立一条从初始状态到目标状态的路径。 与最完备的运动规划算法相反，基于采样的方法通过避免在状态空间中显式地构造障碍物来提供大量的计算节省。即使这些算法没有实现完整性， 但是它们是概率完备， 这意味着规划算法不能返回解的概率随着样本的数量趋近无穷而衰减到零[7]，并且这个下降速率是指数型的。

快速扩展随机树(Rapidly-exploring Random Trees， RRT)算法，是近十几年得到广泛发展
与应用的基于采样的运动规划算法，它由美国爱荷华州立大学的 Steven M. LaValle教授在1998 年提出， 他一直从事 RRT 算法的改进和应用研究，他的相关工作奠定了 RRT 算法的基础。 RRT 算法是一种在多维空间中有效率的规划方法。原始的 RRT 算法是通过一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由树节点组成的从初始点到目标点的路径。

 

快速扩展随机树（RRT） 也是一种数据结构和算法，其设计用途是用来有效搜索高维非
凸空间，可应用于路径规划、虚拟现实等研究。 RRT 是一种基于概率采样的搜索方法，它采用一种特殊的增量方式进行构造，这种方式能迅速缩短一个随机状态点与树的期望距离。该方法的特点是能够快速有效的搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径。它通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效的解决高维空间和复杂约束的路径规划问题。 RRT算法适合解决多自由度机器人在复杂环境下和动态环境中的路径规划问题[8]。与其他的随机路径规划方法相比， RRT 算法更适用于非完整约束和多自由度的系统中[9]。

 

相比于最原始的 RRT 算法的一些缺点，又提出了许多改进的 RRT 算法，例如：

1. 基于概率 P 的 RRT

为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率（0.0 到 1.0 的随机值 p）来选择生长方向是目标点还是随机点。2001 年，LaValle在采样策略方面引入 RRT GoalBias与 RRT GoalZoom， RRT GoalBias方法中，规划器随机采样的同时,以一定概率向最终目标运动；RRTGoalZoom方法中，规划器分别在整个空间和目标点周围的空间进行采样[10]。

1. RRT_Connect

RRT_Connect即连接型 RRT， 2000 年由 LaValle教授和日本东京大学的 Kuffner教授联合提出。该算法一开始同时从初始状态点和目标状态点生长两棵随机树，每一次迭代过程中，其中一棵树进行扩展，尝试连接另一棵树的最近节点来扩展新节点。然后，两棵树交换次序重复上一迭代过程[10]。这种双向的 RRT 技术具有良好的搜索特性，相比原始快速扩展随机树算法，在搜索速度、搜索效率有了显著提高。

1. RRT*

 

2010 年， MIT 的 Sertac和 Emilio 证明了在基于采样的运动规划算法中，随着 RRT 算法采样点趋向于无穷，其收敛到最优解的概率为 0，为此他们提出了渐进最优（asymptoticoptimality）的 RRT*算法[11]。该算法在原有 RRT 算法基础上，改进了父节点选择的方式，采用代价函数来选取扩展节点邻域内最小代价的节点为父节点，同时，每次迭代后都会重新连接现有树上的节点，从而保证计算复杂度和渐进最优解。

 

定义：

位姿空间：

 

运动规划的状态空间是应用于机器人变换的集合，称为位姿空间（configuration space），
引入了 C-空间、 C-空间障碍物、自由空间等一系列概念， Latombe在他的著作[12]中对路径规划的文献进行了总结统一。对于路径规划问题，位姿空间的引入是一次划时代的革命，一旦清楚的理解了位姿空间的概念和意义，许多诸如几何学、运动学等各种以不同形式出现的运动规划问题都可以采用相同的规划算法加以解决，这种层次的抽象是非常重要的。 下面介绍一些概念：

 

 

定义 2.1 位姿（configuration） 机器人一个位姿指的是一组相互独立的参数集，它能完
全确定机器人上所有的点在工作空间 W 中的位置，这些参数用来完整描述机器人在工作空间 W 中的状态。一个位姿通常表示为带有位置和方向参数的一个向量(vector)，用 q 表示。

定义 2.2 自由度（degrees of freedom） 机器人的自由度定义为机器人运动过程中决定其运动状态的所有独立参数的数目，即 q 的维数。

定义 2.3 位姿空间（configuration space） 位姿空间是机器人所有可能位姿组成的集合，
代表了机器人所有可能的运动结果，称为 C-空间，也可简记为 C。

定义 2.4 距离函数（distance function） C-空间中的距离函数定义为该空间中的一个映射。记为。

障碍物空间和自由空间

假设在工作空间中包含所有的障碍物区域，定义 A 为机器人， A的具体的含义可以理解为从机器人位姿空间到机器人工作空间的一一映射，它将位姿空间中的任意一个点映射成 2 维或者3 维工作空间中机器人各刚体段的位姿状态。

 

定义 2.5 障碍物空间（obstacle space） 表达式定义了位姿空间当中的障碍物空间。

位姿空间中的无碰撞区域通常称为自由空间，可用与集合运算定义（表示集合 A 与 B 的差集）

 

 

定义 2.6 自由空间（free space） 表达式定义了位姿空间中的自由空间。

定义 2.7 路径长度（path cost） 定义pc:为一条路径的非负长度。表示所有自由空间中的路径集合。

 

2.3 运动规划的基本定义

用 C-空间的思路，运动规划问题在概念上变得非常简单：任务是在中寻找一条从起始
位姿到目标位姿的路径。 运动规划问题的示意图如图 2.1 所示,图中整个水滴表示位姿空间

 

图1运动规划示意图

 

有了上述概念， C-空间中的运动规划问题可描述如下：

 

1. 定义一个工作空间 W；

2. 定义 W 中的障碍物区域 O 和机器人 A；

3. 所有可能的机器人位姿构成 C-空间，并且划分为和；

4. 指定机器人初始位姿和目标目标位姿；

5. 可行规划（Feasible planning） 一个完整的算法必须计算一条连续的路径，使得，或者正确报告这样的路径不存在。

6. 最优规划（Optimal planning） 在所有的可行的规划的路径里面花费代价最少的一条路径，或者报告这样的路径不存在。

算法：

 

在介绍 RRT 算法之前，先说明一下路径的表示方法。 我们用一个有向图来表示路径 G=（V,E）， 那么一条可行的路径就是一个顶点的序列，，。同时，表示边。 这样子问题就变成了使用采样到的点来扩展图 G，使之能找到一条从初始节点到达目标节点的路径。

 

3.1 原始的 RRT 算法

 

算法1：RRT算法主体部分

 

 

While do

 

 

 

算法2：原始RRT算法的Extend函数

 

 

 

If then

 

 

 

这里可以看到两个算法， 一个是算法的主体部分，还有一个是 RRT 算法的 Extend 函数，主要是如何利用从采样到点扩展图 G。下面详细介绍每一步骤：

 

初始化顶点为，边集E；

进入while循环，迭代N次停止；

设置了为新的图G；

Sample(i)采样一个新的点；

利用新的点扩展图G.

原始RRT算法Extend函数的步骤：

把V,E暂存

函数表示求图G 中离欧式距离最近的点;一般情况下会采用来存储图中的节点，这样会节约搜索的时间。

表示存在一个点它将最小化但是，为我们人为设定的一个值， 其实就是往 q 方向步进了一段距离；

 

进行碰撞检测， 然后判断这一段路径，是否与障碍物发生碰撞即判断路径是否属于中；

把加到顶点集中；

把加入到边集中；

返回扩展后的图。

从算法中可以看出， RRT 的扩展能够趋向于位姿空间中没有扩展到的部分。这就决定了RRT 一开始能够快速的进行扩展，而且能够形成对空间的全面覆盖。 RRT 顶点是分配在位姿空间中是一致均匀的，如果路径存在，在顶点数目一定的条件下是肯定可以找到一条路径的。
当然 RRT 算法也有一些缺点，它是一种纯粹的随机搜索算法对环境类型不敏感，当 C 中包含大量障碍物和狭窄通道约束时，算法的收敛速度慢， 效率会大幅下降。 为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率（0.0 到1.0 的随机值 p）来选择生长方向是目标点还是随机点。

 

基于概率P的RRT

算法3：基于概率P的RRT算法的Extend函数

1.

 

 

 

If  then

 

 

Return

算法主要的改变就是在于 Extend 函数时， 增加了一个ChoseTarget函数。这个函数不会全都使用作为扩展的方向，而是一个概率P来选择进行扩展，以1-P的概率选择来进行扩展。这样的好处就会加快了收敛速度， 不过需要选择好对应的 P 概率。

3.3 RRT_Connect算法

上述的 RRT 每次搜索都只有从初始状态点生长的快速扩展随机树来搜索整个状态空间，
如果从初始状态点和目标状态点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。为此， 在 2000 年由 LaValle教授和日本东京大学的 Kuffner教授联合提出了基于双向扩展平衡的连结型双树RRT 算法，即 RRT_Connect算法。 算法如下所示：

 

算法4：RRT_Connet算法

 

 

 

While do

 

 

 

If   then

 

 

 

;

If  then

 

 

Do

 

If  then

 

 

 

Else break;

While not

If  then return ;

If  then Swap;

该算法与原始 RRT 相比， 在目标点区域建立第二棵树进行扩展。 每一次迭代中， 开始步骤与原始的 RRT 算法一样，都是采样随机点然后进行扩展。 然后扩展完第一棵树的新节点后，以这个新的目标点作为第二棵树扩展的方向。同时第二棵树扩展的方式略有不同，首先它会扩展第一步得到，如果没有碰撞，继续往相同的方向扩展第二步，直到扩展失败或者表示与第一棵树相连了， 即 connect 了，整个算法结束。 当然每次迭代中必须考虑两棵树的平衡性，即两棵树的节点数的多少（也可以考虑两棵树总共花费的路径长度）， 交换次序选择“小”的那棵树进行扩展。 这种双向的 RRT 技术具有良好的搜索特性，比原始 RRT 算法的搜索速度、搜索效率有了显著提高，被广泛应用。首先， Connect 算法较之前的算法在扩展的步长上更长，使得树的生长更快；其次，两棵树不断朝向对方交替扩展，而不是采用随机扩展的方式，特别当起始位姿和目标位姿处于约束区域时，两棵树可以通过朝向对方快速扩展而逃离各自的约束区域。这种带有启发性的扩展使得树的扩展更加贪婪和明确，使得双树RRT 算法较之单树RRT 算法更加有效。

 

3.4 RRT*算法

 

2010 年， MIT 的 Sertac和 Emilio 提出了 RRT*算法。这个算法与上面的算法相比，不但可以找到可行解，还可以找到一条相对次优的算法。 Extend 函数的算法如下：

 

算法5：RRT*算法的Extend

 

 

 

If  then

 

 

 

For all  do

If  then

 

If  then

 

For all  do

If  and

Return

这个算法增加了一些函数， 先来讲解下增加的函数的作用。表示这两个点
直接相连的一条路径。函数表示从到的路径花费。函数则返回 q 节点的父亲节点。 near 函数是 RRT*里最关键的一步， 他会返回一个顶点的集合，表示这些顶点是靠近的。它以为球心的，为半径的超球体，这些顶点就包括在超球体的内部，一般取，和是常数，表示节点维度的数，表示所有节点的个数[11]。

然后来分析这个算法的步骤： 前几步采样扩展步长与之前类似，只是当扩展出新的步长的时候，用 near 函数求邻近点。遍历邻近点集合，寻找花费代价最小的节点，把加入到边集中。 最后还要进行一步回溯的操作， 即如果存在这么一条路径经过到达所需的花费少于经过的parent 到达的花费，那么就可以说明原来到的路径不是最优的， 需要选择经过到达的路径作为新路径。 对所有的邻近点集合内的点都做相同的操作。 这样经过 N 次迭代后得到就是接近最优的次优路径了。

4.结语

RRT 算法是目前流行的基于增量采样的运动规划算法。 本文介绍了它的原始算法和其他RRT 算法的变种。 为了优化 RRT 算法的收敛速度问题提出概率 P 选择的 RRT 和 RRT-Connect算法。 还有为止还有仍在研究的基于多树扩展的 RRT 算法。 针对 RRT 难以得到最优解， 而提出的 RRT*算法可以得到接近最优的次优解。 RRT 算法的优势简单易实现，可以运用在高维的运动规划中，并且采样数量足够多情况下总可以找到可行解，因为是随机采样可以避免陷入局部极小。 当然他也有不足，比如 RRT 算法收敛的速率是未知的， 找树最邻近点的效率上，同时还存在一个“长尾”效应比如刚开始扩展的一些点是有用的，后面随机采样的点可能完全用不到完全是浪费时间的。 还有许多 RRT 算法的变种本文未介绍，比如利用当下流行的 GPU 并行计算加速 RRT 求解速度的 Parallel-RRT 算法，改进实时性的 real-time RRT，针对动态环境的 dynamic domain RRT 算法等等。 希望以后对 RRT 算法开展更深入的研究。

 

 

 

参考文献：

https://en./wiki/Rapidly-exploring_random_tree

http://planning.cs./