随着人类对未知星球探索的渴望逐渐增加，移动机器人导航技术的发展越来越重要。为了保证移动机器人正确执行各种任务，高效且实用的路径规划算法研究十分必要。很多路径规划算法如栅格法，可视图法，A*，D*等算法虽然有各自的优势，但是难以考虑移动机器人非完整约束限制。快速随机搜索树算法可以有效地考虑到移动机器人的非完整约束限制并且可以有效的搜索整个解空间，从而快速得到路径。

路径规划算法的定义是：移动机器人在具有障碍物的环境中按照一定的评价标准，寻找一条从起始状态到目标状态的无碰路径。路径规划算法的其中一种分类方法是分为全局路径规划和局部路径规划。

全局路径规划是根据环境全局的信息，这包括机器人在当前状态下探测不到的信息。全局规划将环境信息存储在一张图中，利用这张图找到可行的路径。全局算法往往需要耗费大量的计算时间，不适于快速变化的动态环境，同时由于全局路径规划需要事先获得全局环境信息，也不适于未知环境下的规划任务。

局部路径规划只考虑机器人的瞬时环境信息，因此计算量减小，速度大大提高。但是局部路径规划算法有时不一定能够使机器人到达目标点，造成算法全局不收敛。

对于移动机器人来说，考虑非完整微分约束的路径规划是该领域的难题之一。非完整微分约束限制机器人系统的运动速度并且约束不可积分，无法将这种约束转化为简单的几何约束。基于随机采样的路径规划算法特别是快速随机搜索树算法，可以将各种约束集成在算法本身之中，因此可以很有效的解决有非完整微分约束的路径规划问题。

快速搜索随机树算法（Rapidly-exploring Random Tree，RRT）由Lavalle提出，是一种采用增量方式增长的随机采样算法，用于解决有代数约束（障碍带来的）和微分约束（非完整性和动态环境带来的）的高维空间问题。RRT算法的优势在于无需对系统进行建模，无需对搜索区域进行几何划分，在搜索空间的覆盖率高，搜索的范围广，可以尽可能的探索未知区域。但同时也存在算法计算代价过大的问题。研究者们提出RRT的多种改进形式来解决这类问题。比如Goal-bias RRT算法，Bi-RRT算法，RRT-Connect算法，Extend RRT算法，Local-tree-RRT算法，Dynamic-RRT算法等。其中Goal-Bias算法将目标节点作为采样点出现，并且算法中还可以控制目标点出现的概率。Extend RRT算法，引入路径点集合，加快了收敛速度，提高了路径的稳定性。Bi-RRT算法和RRT-Connect算法从初始点和目标点生成两棵树，直到两棵树连在一起算法收敛，这种改进型提高了算法的效率。Local-tree-RRT算法针对随机采样算法狭窄通道难以迅速通过的问题，提出局部树方法加以解决。Dynamic RRT算法提出了修剪和合并操作，去除掉无效的节点后再继续进行搜索。

由于RRT算法采样的随机性，导致最终生成的路径往往只是可行路径而不是最优路径。因此，有些RRT的改进算法可以对生成的路径进行优化。RRT*算法也是RRT算法的一种改进型，由S.Karaman和E.Frazzoli于2011年提出。RRT*与基本RRT算法的主要区别在于RRT*算法引入了对新生成节点相邻节点的搜索，目的是选择低代价的父节点，除此之外还有重新布线的过程进一步减小路径代价，是解决高维的最优路径规划问题的一个突破性的方法。RRT*算法是渐进最优的，若给定足够的运行时间，RRT*算法总是收敛到最优解。尽管RRT*算法在一定程度上解决了RRT算法的优化问题，但是搜索新的父节点和重新布线过程也使得算法的效率大大减少。近年来，研究者们也提出了很多RRT*的改进型。

• Akgun和Stilman提出了Bidirectional RRT*(B-RRT*)算法可以提高收敛速度的同时使用容许启发式的方法来有选择性的产生新节点以完善路径。
• Nasir等人提出的RRT*-Smart算法主要有两个特点，分别是智能采样和路径优化。当初始路径生成后，采用三角不等式原理去除多余的节点。优化生成的路径时，依靠生成信标节点来减少路径代价。
• Lee等提出了基于样条曲线的RRT*算法(SRRT*)解决三维环境非完整约束的无人机路径规划问题。该算法通过B样条曲线扩展随机树，在可以实现动态规划同时也可以在树扩展阶段检查机器人几何碰撞。SRRT*算法可以为无人机产生平滑且代价最优的路径。

当移动机器人面临未知环境时，只能依靠及时地反馈信息逐步规划路径，这样虽然很难得到全局的最优路径，但可以在环境信息反馈的基础上尽可能的做到局部优化。《动态不确定环境下广义控制问题的预测控制》结合模型预测控制的基本思想，将滚动的思想推广到了不确定环境下的规划问题的求解，提出了充分利用已知信息进行规划，通过滚动窗口进行局部优化的思想，并提出用新获得的环境信息更新已有信息。滚动规划就是在这些理论基础上建立起来的。基于滚动窗口的路径规划算法依靠移动机器人实时探测到的局部环境信息，以滚动的方式进行在线规划。在滚动的每一步，机器人根据滚动窗口内部的信息，生成子目标，在当前滚动窗口内进行局部路径规划，然后随着移动机器人的移动，滚动窗口递进，不断更新窗口内信息，从而在滚动中实现规划与反馈的结合。《基于正反馈自适应遗传算法的机器人路径滚动规划》中针对传统遗传算法求解机器人路径规划收敛较慢的问题，将蚁群算法，模拟退火算法，滚动规划和遗传算法相结合，在未知环境下规划出了全局优化路径。《基于神经网络和人工势场的滚动规划》中，将规划算法增加了移动机器人的预测能力，减少了碰撞的可能性。

在机器人运动研究领域，对机器人的运动学分析是必不可少的。机器人运动学的正确分析依赖于对机器人所受各个约束的正确判断和计算。为研究的方便，将约束定义为：限制质点或质点系运动的条件，表示这些限制条件的数学方程称为约束方程。约束的分类有很多种。比如几何约束和运动约束，定常约束和非定常约束等。几何约束限制质点或质点系在空间的几何位置，满足几何上的原理。例如一个单摆系统，单摆的下端悬挂的小球可以看作是一个质点系，该质点系只能绕着单摆的固定点进行平面摆动，我们说该质点系受到几何约束。运动约束是限制质点系运动情况的运动学条件。例如一个车轮沿着直线轨道作纯滚动时，车轮受到限制，其轮心除了受到始终与地面保持车轮半径r这一几何约束外，还受到只滚不滑的运动学限制，这就是运动学约束。定常约束是不随时间变化的约束。非定常约束的约束条件随时间变化。除了上述约束的分类形式外，还可以将约束分为完整约束和非完整约束。这种分类方式对机器人研究领域来说十分重要。如果约束方程中包含坐标对时间的导数(如运动约束)，而且方程不可能积分为有限形式，这类约束称为非完整约束。非完整约束方程总是微分方程的形式。反之，如果约束方程中不包含坐标对时间的导数，或者约束方程中的微分项可以积分为有限形式，这类约束称为完整约束。

快速随机搜索树（RRT）算法是基于随机采样的路径规划算法，它相比于其他算法的一个优势在于可以有效地将非完整约束考虑在算法内部，从而避免了复杂的运动学约束的考虑，使得路径规划问题简单化。

• RANDOM_STATE()函数在设定的环境内部产生随机点
• NEAREST_NEIGHBOR()函数遍历随机树，找出距离随机点最近的节点
• SELECT_INPUT()函数按照已设定好的值扩展随机树
• NEW_STATE()函数生成
• judge()函数判断新生成的节点是否满足非完整约束
• T.add_vertex()插入
• T .add _ edge()为 到 之间加上一条边

具体过程为：

• 首先产生第一个节点 属于 ，在每一次循环中，产生一个随机点 ，随机点的生成是任意的，即可以在整个状态空间 内。
• 在产生随机点后，遍历随机树中的每一个节点，计算每一个节点与该循环生成的随机点之间的距离，找出距离此随机点最近的节点，记为 。
• 定义一个步进变量EPS，当找到 时， 向 与 连线方向扩展EPS步长，因此这里的EPS也就是状态方程的输入 ，扩展后产生新的节点 。
• 判断 是否满足非完整微分约束，如果不满足，舍弃 ，重新产生新的随机点。如果满足非完整微分约束，则加入 ，并在 到 之间加上一条边。
• 相应的，再插入新节点 的过程中，如果 、 和 到 之间的边任意一个位于 中或者与 相交，则此次循环不添加任何节点，在下一次循环中重新生成新的随机点 ，然后再进行判断，如果属于 ，则保留新节点。
• 总之，在加入新节点 时需要两次判断，分别为障碍物检测和非完整约束检测，当且仅当两者都满足要求时，才加入新节点。

RRT算法相较于其他路径规划算法如栅格法，A* ，D*算法等，一大特点在于对空间的随机搜索，这样的搜索尤其给高维空间的路径规划带来了巨大优势，但是这样的搜索同样带给RRT 算法一大缺点：算法的运算效率不高。随机树搜索漫无目的，在整个度量空间生成随机点，依据随机点进行随机树扩展，直到恰好有节点扩展到目标附近才结束搜索，生成最终路径。这样的方法虽然能够保证对整个度量空间有效的探索，使得路径解的可能性大大增加，但是在得到路径解之前，随机树经常扩展到一些离目标很远的地方，也就是扩展到离我们期待的目标区域较远的“无用区域”。为了高算法的效率，我们希望随机树的搜索并不是完全漫无目的的，在通常的路径规划任务中，快速到达目标是我们的追求，因此我们希望随机树尽可能向着目标方向搜索，以加快搜索速度。

具体的操作方法是：人为的引导随机点的生成。在产生随机点 时，以一定的概率选取目标点作为循环中的 ，即 。

在随机树扩展中相当于给定一个扩展的方向，以一定的概率将目标点作为，就等价于驱使随机树向着目标方向扩展，将图1展示的算法流程中RANDOM_STATE()函数改写为如下形式：

但为了保持随机树对未知空间的扩展能力，概率通常不宜选择的过大(通常0.05-0.1)。这种目标偏好的随机树扩展一个显著的缺点是，容易陷入局部搜索无法跳出，尤其是障碍物遮挡目标时。随着目标偏好的程度加大，跳出局部搜索困难越大。因此目标偏好概率的选取要折衷算法效率和RRT扩展能力。

尽管RRT算法是一个相对高效率，同时可以较好的处理带有非完整约束的路径规划问题的算法，并且在很多方面有很大的优势，但是RRT算法并不能保证所得出的可行路径是相对优化的。因此许多关于RRT算法的改进也致力于解决路径优化的问题，RRT*算法就是其中一个。RRT*算法的主要特征是能快速的找出初始路径，之后随着采样点的增加，不断地进行优化直到找到目标点或者达到设定的最大循环次数。RRT*算法是渐进优化的，也就是随着迭代次数的增加，得出的路径是越来越优化的，而且永远不可能在有限的时间中得出最优的路径。因此换句话说，要想得出相对满意的优化路径，是需要一定的运算时间的。所以RRT*算法的收敛时间是一个比较突出的研究问题。但不可否认的是，RRT*算法计算出的路径的代价相比RRT来说减小了不少。RRT*算法与RRT算法的区别主要在于两个针对新节点 的重计算过程，分别为：

• 重新为 选择父节点的过程
• 重布线随机树的过程

在新产生的节点 附近以定义的半径范围内寻找“近邻”，作为替换 父节点的备选。依次计算“近邻”节点到起点的路径代价加上 到每个“近邻”的路径代价，具体过程见图3。

图3 a)中表现的是随机树扩展过程中的一个时刻，节点标号表示产生该节点的顺序，0节点是初始节点，9节点是新产生的节点 ，4节点是产生9节点的 （抱歉这里6节点是9节点的 ，图中标错了），节点与节点之间连接的边上数字代表两个节点之间的欧氏距离(这里我们用欧氏距离来表示路径代价)。

在重新找父节点的过程中，以9节点 为圆心，以事先规定好的半径，找到在这个圆的范围内 的近邻，也就是4，5，8节点。

原来的路径0 - 4 - 6 - 9代价为10 + 5 + 1=16，备选的三个节点与 组成的路径0 - 1 - 5 - 9，0 - 4 - 9和0 - 1 - 5 - 8 - 9代价分别为3 + 5 + 3=11，10 + 4=14和3 + 5 + 1 + 3=12，因此如果5节点作为9节点的新父节点，则路径代价相对是最小的，因此我们把9节点的父节点由原来的节点4变为节点5，则重新生成的随机树如图3 b)所示。

在为 重新选择父节点之后，为进一步使得随机树节点间连接的代价尽量小，为随机树进行重新布线。过程示意如图4重布线的过程也可以被表述成：如果近邻节点的父节点改为 可以减小路径代价，则进行更改。

如图4 a)，9节点为新生成的节点 ，近邻节点分别为节点4 , 6 , 8 。它们父节点分别为节点0 , 4 , 5。路径分别为0 - 4，0 - 4 - 6，0 - 1 - 5 - 8，代价分别为10，10 + 5=15 和3 + 5 + 1=9。

如果将4节点的父节点改为9节点 ，则到达节点4的路径变为0 - 1 - 5 - 9 - 4，代价为3 + 5 + 3 + 4=15 大于原来的路径代价10，因此不改变4节点的父节点。

同理，改变了8节点的父节点，路径代价将由原来的9变为14，也不改变8节点的父节点。如果改变6节点的父节点为 则路径变为0 - 1 - 5 - 9 - 6，代价为3 + 5 + 3 + 1=12小于原来的路径代价15，因此将6的父节点改为节点9，生成的新随机树如图4 b)。

重布线过程的意义在于每当生成了新的节点后，是否可以通过重新布线，使得某些节点的路径代价减少。如果以整体的眼光看，并不是每一个重新布线的节点都会出现在最终生成的路径中，但在生成随机树的过程中，每一次的重布线都尽可能的为最终路径代价减小创造机会。

RRT*算法的核心在于上述的两个过程：重新选择父节点和重布线。这两个过程相辅相成，重新选择父节点使新生成的节点路径代价尽可能小，重布线使得生成新节点后的随机树减少冗余通路，减小路径代价。

其中部分函数与RRT算法中的定义和作用相同。

calculate(dist(,)+cost(,)) 中dist函数计算两点之间的距离，cost函数计算从给定点沿着随机树的各个边直到起点的路径代价。在这里路径代价是从给定点沿着随机树的各个边直到起点的欧氏距离之和。

min(dist( ,)+cost(,)) 表示选出使路径代价最小的 。

同理min(dist(,)+cost(,)) 表示选出使路径代价最小的。

为了简化问题，路径规划的障碍物取较为规则的几何形，如圆，多边形等。对于圆形障碍物来说，圆形边界的判断属于非线性问题，通常将圆形进行线性化处理(转化为多边形)。只需要判断 的横纵坐标是否在圆内，这里我们规定如果 位于圆形障碍物的外接正方形内，就视为碰撞。如果圆形障碍物的圆心坐标为 ，半径为 ，考虑移动机器人的尺寸，对障碍物进行膨化处理，膨胀尺寸inf，所以碰撞条件为：

因为圆形障碍物的避障策略相对简单，在仿真中并未设置圆形障碍物。

仿真环境中搭建的迷宫，主要选取长方形障碍物。长方形的碰撞机制研究相对复杂一些，具体碰撞机制阐述如下：在扩展随机树的过程中，由 到 连接的边不能与长方形障碍物的任何一边相交，即将长方形障碍物碰撞检测问题转化为直线与矩形相交问题。直线与矩形相交的判断主要分为两步：

第一步，判断 与 是否在矩形某一条边的一侧。如果 与 在矩形任意一条边的同侧，则不用进行后续判断， 与 连线必定不与矩形相交。这里不存在两点位于矩形内部的情况，因为 由 产生，而 必位于矩形外侧，如果 与 位于某一条边的两侧，则进行第二步判断。

第二步， 与 在矩形任意一边的不同侧时，分为两种情况：

• 第一种情况是 位于矩形内部，则 与 连线必定与矩形相交。
• 第二种情况是两点均在矩形外部。在这种情况下并不能保证两点连线不与矩形相交，图6情况所示，两点位于矩形外侧且位于矩形上边的两侧，但两点连线与矩形相交。在这种情况下，利用直线与矩形的性质进行避碰计算。

如图7所示， 与 位于矩形障碍物AB 边的两侧且均位于矩形的外侧，两点连线与矩形相交， 与 是两个边界，即当 与 连线位于 与 两直线下方时， 与 连线必定与矩形相交。反之，若不在 与 两直线下方则表现为不与障碍物发生碰撞。对于AD边来说必发生碰撞的过程可以用如下式子表示，

k 表示直线的斜率。

整个碰撞检测过程的逻辑如下:

对于矩形的一条边设定一个布尔值 ，当 时，表示发生碰撞，当 时，表示不发生碰撞。

定义一个判断函数 ，其中：

所以 函数是一个布尔函数，当等式右边为真时， ，反之 。则对于每一个边，判断逻辑可以写成

其中 表示矩形障碍物某一条边的两个定点。

针对图7中的具体情况:

布尔函数表示为：

首先判断且逻辑左半部分，

所以 ，意味着 与 位于AD边的两侧。

接着判断且逻辑的右半部分，

所以 ，意味着 与 连线位于 与 两直线下方。根据此判断条件发现， 与 属于同一种情况，不必再分情况讨论。

因此 ， 与 连线与矩形这一条边相交，发生碰撞。

bool 函数且逻辑左半部分与 相同，对于 而言，因为且逻辑左半部分判断两点是否在一条边两侧。且逻辑右半部分

所以，故 。

因此 与 连线不会与矩形这一条边相交，不会发生碰撞。

其他边的判断方法是相同的，当且仅当

才表明 与 连线不与矩形相交，不会发生碰撞，也将其视为有效点插入随机树中。

通过对每一个障碍物进行上述逻辑判断即可以使随机树的扩展避开障碍物，在 中搜索路径。

机器人路径规划的其中一种分类方式为全局规划和局部规划，当机器人获得先验的全局地图时，机器人可以离线的进行全局一次性规划得出路径解。但实际情况中，往往机器人需要工作在未知环境中，即没有先验的地图信息。比如在进行实时定位与地图构建(SLAM) 任务中，因为地图未知，机器人的移动路径并不能在执行任务之前就一次性规划好，而是随着机器人的移动进行实时在线多次重规划(滚动规划)来得到可行的路径。在机器人的导航系统中已经较为广泛的使用滚动规划来得到机器人可行路径。

机器人没有全局信息，因此在任意时刻只能实时探测到以其当前位置为中心，r 为半径区域内的环境信息，采用反复进行的局部优化规划代替一次性的全局优化的结果，并在每次局部优化规划中充分利用该时刻最新的局部环境信息。基于滚动窗口的路径规划算法的基本原理如下所述，流程图见图8。

在滚动的每一步，机器人依据窗口内(预先设定好范围、形状)局部环境信息(包括障碍物信息)，用启发式方法生成局部子目标。运算局部规划算法得到局部路径，沿着生成的局部路径前进一步或几步，机器人运动后滚动窗口随之移动。

滚动规划中较为关键的一步是子目标的选取。本文中采用启发式函数进行判定。在t 时刻，机器人的滚动窗口为 ，若此时全局目标点 ，则取 ，否则利用启发式函数F(P)选取使F(P)取最小值的点作为子目标 。

启发式函数: F(P)=G(P) + H(P) + J(P)

其中：

通常的启发式构造函数采用的多是F(P)=G(P) + H(P) 的形式，这种形式的一个缺点是有可能经过该函数算出的子目标点恰好是机器人当前的位置，增加的一项J(P) 目的就是排除这种情况。

当障碍物相对比较小的时候，如果障碍物完全被包含在滚动窗口范围中，如图9所示：

图中的圆表示滚动窗口，矩形代表障碍物，“ O ” 代表起点，通常位于滚动窗口的中心。“ 口” 代表子目标点(由启发式函数确定)。在此种情况下与全局静态环境中的路径搜索相同。

当障碍物相对较大时，以至于占据了一部分滚动窗口，如图10所示的情况：

图中“ O ” 和“ 口” 表示起点，五角星表示全局目标点，根据启发式算法，生成的子目标点本应该是三角所示的位置，但是子目标点位于几何平面中障碍物所占空间之内，即 。此种情况下无法得出解路径。因为算法中的避障策略无法搜索到 内，因此需要相应的更改子目标点。子目标点坐标的确定过程如下：已知滚动中心(机器人位置) ，矩形坐标 和边长a , b。因此可以得到矩形其他三个点的坐标分别为 , , 。

通过上述过程求得新子目标点，进行规划。

滚动窗口规划可以用于在相对未知的环境中，随着机器人的移动，传感器探测的范围发生变化，在不同的传感器范围中进行局部路径的规划。

完整约束忽略了移动机器人的转向轮转向角的限制，因此RRT的扩展并不考虑移动机器人的运动能力。仿真对比结果如图11。该仿真算例设置了10个矩形障碍物，起点用O表示(0,0)，终点用☆表示(1000,1000)。图11左所示的路径为完整约束下的RRT算法求解的路径，图11右所示的路径设置的机器人最大转向角30°。

可以看出，在考虑非完整约束条件下路径明显比完整约束条件下的路径光滑，图11左求解的路径有些节点的转变角度很大，很可能导致移动机器人无法跟踪路径，从而失控。

由于RRT算法搜索的随机性，导致出现很多无用的搜索，需要较长的时间才能搜索到可行路径，同时得到的路径往往代价不小。而基于目标的快速RRT算法，给路径的搜索引导方向同时也保留RRT的空间搜索能力，不仅提高了搜索效率，而且得到的路径代价也相应的优化。

为了更好的对比基于目标的快速RRT算法与RRT基本算法，同时探索选取不同概率对求解路径时间的影响。

程序运行在corei7-2630QM，主频2.0GHz，睿频2.0GHz的计算机上。

如图11右中的RRT算法，计算时间为539.75s。因为采样的随机性，即使随机树已经扩展到距离目标点很近的地方，仍需要花费很长时间才能到达，这样虽然随机树的扩展性大大增加，但是计算效率过低，影响实际应用。

完整约束下，随机树节点之间的夹角没有任何限制，在这种情况下，计算时间见表1：

计算结果如下图12所示，左上的图为概率0.05时的计算结果，右侧为概率0.06时的计算结果，中间一行左边为概率0.07时的计算结果，以此类推。

随着随机采样点选择目标点的概率增加，随机树向着目标扩展的趋势越明显，整个路径求解的时间也相对更少。但是路径光滑度较低，有很多处转折明显，对于受到非完整约束的机器人来说，较难跟踪轨迹。

为对比不同角度非完整约束下对基于目标的快速RRT 算法计算时间的影响，进行了两种角度的不同尝试。

首先设定机器人转动角度为(, )，各个概率下计算时间列于表2：

计算结果如下图13所示，左上的图为概率0.05时的计算结果，右侧为概率0.06时的计算结果，中间一行左边为概率0.07时的计算结果，以此类推。

可以看出，相比于完整约束的计算结果，非完整约束下的路径光滑很多，这样便于机器人进行路径跟踪。但是计算时间大大增加。主要因为采样的随机性，导致很多采样点不能满足角度限制，采样过程不断重复进行，直到找到合适的采样点为止。另外，尽管是基于目标的快速RRT 算法，有一定的概率采样点选为目标点，但是因为有非完整约束的限制，当某一次迭代中采样点选择了目标点，但是由于不满足非完整约束的限制，该采样点（目标点）需要舍弃。因此，目标引导性相较于完整约束来说大大削弱。

设定机器人转动角度为( , )，各个概率下计算时间列于表3：

计算结果如下图14所示，左上的图为概率0.05时的计算结果，右侧为概率0.06时的计算结果，中间一行左边为概率0.07时的计算结果，以此类推。

30度的角度约束对采样点的要求更为苛刻，因此计算时间也显著增加。但相比于基本RRT算法，计算时间还是减小不少。

RRT*算法相比于RRT算法对随机树进行了优化，在仿真实例中，分别将RRT*主要的两个优化过程通过不同颜色的线段表现出来。给出了RRT算法和RRT*算法求解路径的对比。

设置的环境为一个简易迷宫，迷宫的"墙壁"由矩形组成，矩形障碍物总数为10个。起点用O 表示（100,400），终点用☆表示（750,600）。设置的机器人最大转向角为45度。选取目标作为采样点的概率为0.1。这里将RRT*的两个核心优化过程简称为"修剪"。图15上是RRT算法在此环境下的仿真结果，图15下为RRT*算法在此环境下仿真结果。该图将RRT*算法的修剪过程都保留下来了，其中包括搜索过程中的所有修剪。随着搜索的进行，基本RRT算法用黑色的线表示，重选父节点的过程用绿线表示，重布线过程用蓝线表示。

可以看出，RRT*算法几乎将基本RRT算法扩展的分支全部进行了优化（图15下中几乎不存在黑色的线段）。绿色的线段表示新生成的节点重新选择父节点后的新分支。蓝色的线段表示重布线后的新分支。

RRT*算法是渐近收敛的，换句话说，随着迭代次数的增加，得出的路径是逐渐趋于全局最优的，但需要付出的运算时间代价是巨大的。因此，为了后续研究，算法的计算速度不能很长，因此引入了目标偏好的搜索策略，得出的路径计算结果相对于基本RRT算法来说是相对优化的，即在时间代价尽量小的前提下，做到相对的优化是路径规划的宗旨。

依据滚动规划的流程，设置滚动窗口，在每个滚动窗口，依据启发式算法设置子目标点，在每个滚动窗口内采用RRT* 算法求解路径，机器人向前移动，滚动窗口随之移动，再次规划，移动……直到达到全局目标点。

由于MATLAB 下模拟未知环境存在一定的困难，涉及到障碍物的探测和较为复杂的几何建模以及多种情况避障策略的讨论，本次仿真是在已有先验地图信息的条件下进行的。

该仿真是在1000X1000的环境下进行的，起点为（50,50），终点为（1000,800），滚动窗口半径为300，设置的机器人最大转向角为。仿真中设定机器人的移动规则是：每次移动都是移动到滚动窗口的子目标点后，滚动窗口才更新。图16为仿真计算得出的路径。

图中的椭圆形（实际为圆形，因图片显示比例所以显示椭圆）为设置的滚动窗口，每个滚动窗口中的随机树表示方式与前述仿真相同。由于设置的机器人转向角最大为，途中当路径搜索到中间障碍物时，为了向未知空间扩展，同时保证在机器人的转向能力之内，因此做了比较大的转向以至于随机树的扩展先向着目标反方向移动了一定距离。但最终移动机器人沿着滚动窗口顺利求解得到了路径。