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

基于RRT的运动规划算法综述1.介绍在过去的十多年中，机器人的运动规划问题已经收到了大量的关注，因为机器人开始成为现代工业和日常生活的重要组成部分。

最早的运动规划的问题只是考虑如何移动一架钢琴从一个房间到另一个房间而没有碰撞任何物体。

早期的算法则关注研究一个最完备的运动规划算法（完备性指如果存在这么一条规划的路径，那么算法一定能够在有限时间找到它），例如用一个多边形表示机器人，其他的多边形表示障碍物体，然后转化为一个代数问题去求解。

但是这些算法遇到了计算的复杂性问题，他们有一个指数时间的复杂度。

在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]。

（2）RRT_ConnectRRT_Connect即连接型RRT，2000年由LaValle教授和日本东京大学的Kuffner教授联合提出。

该算法一开始同时从初始状态点和目标状态点生长两棵随机树，每一次迭代过程中，其中一棵树进行扩展，尝试连接另一棵树的最近节点来扩展新节点。

然后，两棵树交换次序重复上一迭代过程[10]。

这种双向的RRT技术具有良好的搜索特性，相比原始快速扩展随机树算法，在搜索速度、搜索效率有了显著提高。

（3）RRT*2010年，MIT的Sertac和Emilio证明了在基于采样的运动规划算法中，随着RRT算法采样点趋向于无穷，其收敛到最优解的概率为0，为此他们提出了渐进最优（asymptotic optimality）的RRT*算法[11]。

该算法在原有RRT算法基础上，改进了父节点选择的方式，采用代价函数来选取扩展节点邻域内最小代价的节点为父节点，同时，每次迭代后都会重新连接现有树上的节点，从而保证计算复杂度和渐进最优解。

2.定义2.1位姿空间运动规划的状态空间是应用于机器人变换的集合，称为位姿空间（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-空间中的距离函数定义为该空间中的一个映射ρ:(q1,q2)∈C2→ρ(q1,q2)。

记为ρ=||q1−q2||.2.2障碍物空间和自由空间假设在工作空间W:W=R d中包含所有的障碍物区域O:O⊂W，定义A为机器人，A 的具体的含义可以理解为从机器人位姿空间到机器人工作空间的一一映射，它将位姿空间中的任意一个点映射成2维或者3维工作空间中机器人各刚体段的位姿状态。

定义2.5障碍物空间（obstacle space）表达式C obs={q∈C|A(q)⋂O=∅}定义了位姿空间当中的障碍物空间C obs⊆C。

位姿空间中的无碰撞区域通常称为自由空间，可用C obs与C集合运算定义（A\B表示集合A与B的差集）定义2.6自由空间（free space）表达式定义了C free=C\C obs位姿空间中的自由空间。

定义2.7路径长度（path cost）定义pc:∑Cfree →R>0为一条路径的非负长度。

∑Cfree表示所有自由空间中的路径集合。

2.3运动规划的基本定义用C-空间的思路，运动规划问题在概念上变得非常简单：任务是在中寻找一条从起始位姿到目标位姿的路径。

运动规划问题的示意图如图 2.1所示,图中整个水滴表示位姿空间C=C free⋃ C obs.图2.1 运动规划示意图有了上述概念，C-空间中的运动规划问题可描述如下：（1）定义一个工作空间W；（2）定义W中的障碍物区域O和机器人A；（3）所有可能的机器人位姿构成C-空间，并且划分为C obs和C free；（4）指定机器人初始位姿q init和目标目标位姿q goal；（5）可行规划（Feasible planning）一个完整的算法必须计算一条连续的路径τ:[0,1]→C free，使得τ(0)=q init，τ(1)=q goal或者正确报告这样的路径不存在。

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

3.算法在介绍RRT算法之前，先说明一下路径的表示方法。

我们用一个有向图来表示路径G=（V,E），那么一条可行的路径就是一个顶点的序列(v1,v2 ,v3 ,…,v n),v1=q init,v n= q goal.同时(v i,v i+1)∈E,1≤i≤n−1表示边。

这样子问题就变成了使用采样到的点来扩展图G，使之能找到一条从初始节点到达目标节点的路径。

这里可以看到两个算法，一个是算法的主体部分，还有一个是RRT算法的Extend函数，主要是如何利用从采样到点扩展图G。

下面详细介绍每一步骤：（1）初始化顶点为q init，边集E；（2）进入while循环，迭代N次停止；（3）设置了为新的图G；（4）Sample(i)采样一个新的点q rand；（5）利用新的点扩展图G。

原始RRT算法Extend函数的步骤：（1）把V，E暂存（2）Nearst(G,q)函数表示求图G中离q欧式距离最近的点q nearst；一般情况下会采用kd-tree来存储图中的节点，这样会节约搜索的时间。

（3）Steer(q nearst,q)表示存在一个q new点它将最小化||q new−q||但是||q new−q nearst||<η, η为我们人为设定的一个值，其实就是往q方向步进了一段距离；（4）ObstacleFree(q nearst,q new)进行碰撞检测，然后判断这一段(q nearst,q new)路径，是否与障碍物发生碰撞即判断路径是否属于C free中；（5）把q new加到顶点集中；（6）把(q nearest,q new)加入到边集中；（7）返回扩展后的图G’。

从算法中可以看出，RRT的扩展能够趋向于位姿空间中没有扩展到的部分。

这就决定了RRT一开始能够快速的进行扩展，而且能够形成对空间的全面覆盖。

RRT顶点是分配在位姿空间中是一致均匀的，如果路径存在，在顶点数目一定的条件下是肯定可以找到一条路径的。

当然RRT算法也有一些缺点，它是一种纯粹的随机搜索算法对环境类型不敏感，当C中包含大量障碍物和狭窄通道约束时，算法的收敛速度慢，效率会大幅下降。

为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率（0.0到1.0的随机值p）来选择生长方向是目标点还是随机点。

算法主要的改变就是在于Extend 函数时，增加了一个ChoseTarget 函数。

这个函数不会全都使用q rand 作为扩展的方向，而是一个概率P 来选择进行扩展，以1-P 的概率选择q goal 来进行扩展。