改进RRT算法的室内移动机器人路径规划

刘紫燕，张 杰

贵州大学 大数据与信息工程学院，贵阳550025

1 引言

随着科学技术的不断发展，导航技术已成为机器人领域的一个研究热点，而路径规划算法是实现移动机器人导航的重要保障[1]。路径规划是指机器人按照一定的准则，在环境中搜索一条从起始点到目标点、可以避开障碍物的最优路径[2]。传统的路径规划算法有人工势场法、模拟退火算法、模糊控制法、神经网络算法等[3]，但这些算法的计算复杂度较大，在真实场景下的应用存在缺陷。遗传算法、蚁群算法、粒子群算法等智能仿生学算法需要大量的迭代来求解最优路径，因此收敛速度慢，而且极易陷入局部最优值[4-5]。

快速扩展随机树（Rapidly-exploring Random Tree，RRT）由于具有快速高效的优点，近几年在移动机器人路径规划中被广泛采用[6]。RRT 算法通过随机采样，把搜索导向空白区域，从而搜索到一条起点到终点的路径，适合解决复杂环境下多自由度机器人的路径规划问题[7]。但是由于传统RRT 算法扩展点具有随机性，使得算法搜索过于平均、规划结果偏离最优解[8]。针对以上缺陷，文献[9]提出的RRT2connect算法同时从起点和终点生长两棵随机树，在搜索效率方面有了显著提高，但在组态空间存在窄道时其性能会显著下降。Karaman和Frazzoli[10]提出了RRT*算法，用以改进由RRT 算法产生非最优解的问题，但存在收敛速度慢、结果不稳定的缺点。同时莫栋成等人[11]提出了任意时间RRT算法，通过减少搜索节点数来提高搜索效率，该算法的缺点是启发函数没有达到最优。刘成菊等人[12]提出了动态RRT算法，并将其应用于NAO 机器人，该算法能适应复杂障碍物环境，但是生成的路径并非理想平滑曲线。Wang[13]提出基于学习的LM-RRT（Learning-based Multi-RRT）算法，用于窄通道中的机器人路径规划，提高了局部探索能力。Du等人[14]将RRT 算法和人工势场法相结合，提出一种启发式采样方法，具有更高的采样效率和收敛速度。Chang 等人[15]提出了SARRT（Struc‐ture-Aware Rapidly-exploring Random Tree）算法，通过构建温度场使随机树生长偏向于目标区域，取得良好的效果，然而求解扩散方程计算成本昂贵，耗费了大量时间。

为了解决上述问题，本文结合目标偏向策略和气味扩散法对RRT算法加以改进，并对生成的路径进行优化处理，从而提高算法效率和路径质量。通过基于ROS的仿真实验和实验室环境下的Turtlebot2 实验，证实了改进RRT算法的优越性、实用性和可靠性。

2 机器人运动模型

机器人运动模型如图1 所示，可将运动视为通过旋转平移两种方式完成从一点到另一点的操作。假设机器人沿曲线运动，t 时刻机器人的线速度为vt，角速度为ωt。在(t-1,t]时刻内（时间间隔为Δt），机器人以r=v/ω为半径的圆形轨迹上移动，其瞬时速度为：

图1 机器人运动模型

令t-1 时刻机器人的位姿Xt-1=(xt-1,yt-1,θt-1)T，Δt 时间内以速度常量(v,ω)T进行运动，则t 时刻机器人的位姿为：

机器人所有时刻的位姿构成了机器人实际行走的路径。

3 基本RRT算法

RRT 算法由Lavalle 于1998 年提出，它是一种多维空间的高效规划方法[16]。RRT 算法复杂度小，可直接应用于非完整约束系统的规划。设机器人的位姿空间为C，初始节点为Qstart，目标节点为Qgoal，机器人最小步长为s，初始随机树为T。算法以Qstart为根节点，在位姿空间C 中随机产生采样点Qrand，然后遍历T 中的所有节点，寻找离Qrand最近的节点Qnear作为扩展节点。此时判断Qnear与Qrand的连接线是否与障碍物相交，若相交则在位姿空间C 中重新生成随机采样点Qrand；否则，机器人由Qnear向Qrand运动最小步长s到达新节点Qnew，同时将新节点Qnew添加到扩展树T 中。重复上述步骤，当机器人到达目标节点Qgoal或超过最大迭代次数时算法结束。此时，由目标节点Qgoal回溯到初始节点Qstart，即可得到路径。RRT算法伪代码如下[17]：

4 改进RRT算法

4.1 目标偏向策略

由基本RRT算法原理可知，其产生新节点的方式是在机器人状态空间中随机采样，导致搜索过于平均，浪费了大量计算时间和资源，算法效率低。而且算法输出的路径质量不高，偏离最优解。为了克服上述缺陷，高效地规划出移动机器人在复杂环境下的最优路径，本文在基本RRT 算法的框架中引入一种目标偏向策略。即提前设定一个偏向概率阈值pbias，在随机选取扩展节点时利用均匀概率分布产生一个随机概率p，若p ＞pbias，则Qrand在机器人状态空间中随机产生，否则Qrand=Qgoal。通过这种方式既保留了原算法的随机特性，又使得算法收敛速度加快，从而提升搜索效率。

在基本RRT算法中，规划问题的起始状态点是树的根节点，但目标状态点在搜索过程中并没有被考虑在内，只是被动地等待随机树扩展到目标区域。目标偏向策略将目标点的作用考虑在内，使得随机树以一定概率朝向目标点生长，改变了节点扩展方式，避免了原算法的无方向生长。当随机概率p ＜pbias时，探索树朝目标点生长；当p ＞pbias时，探索树在状态空间中随机选择采样点进行生长。探索树的目标偏向性和随机延展性受随机概率p 的影响，p 越大随机树朝目标点生长的趋势越强，随机性越弱。反之，p 越小随机树在整个状态空间中选择扩展点的机会越大，随机树的发散性也就越强，适用于复杂环境下的路径规划[18]。不论随机树是否朝目标点生长，当路径与障碍物发生碰撞时都会放弃当前节点重新产生采样点，这种节点扩展方式使得随机树在避开障碍物的同时朝向目标点生长，保证了生成路径的准确性。目标偏向RRT算法仍具有概率完备性，在理论上总能在地图中搜索到一条可行路径，且规划路径优于RRT 算法生成路径。RRT 算法与目标偏向RRT 路径生成对比如图2 所示，红线表示规划路径，蓝点表示采样节点。由图2 可知，基于目标偏向策略的RRT 算法采样点减少，可大大缩短路径长度和规划时间，生成路径也较为平整光滑，实验结果证明了目标偏向策略的有效性和准确性。

在目标偏向RRT中，概率阈值pbias的选取会影响算法规划性能。通过实验分析对概率阈值pbias的取值进行研究。环境地图如图2（a）所示，起点位置坐标为S(1,1)，终点位置坐标为G(90,90)。选取不同的概率阈值，在每一个概率阈值pbias下分别进行10 次路径规划。将pbias=0.05 作为第一组实验数据，pbias=0.1 作为第二组实验数据，之后以0.1为间隔进行实验。以路径长度、搜索时间作为评价指标，记录10 次实验结果的平均值。仿真实验结果如图3所示。

图2 RRT与目标偏向RRT对比

从图3（a）可知，随着概率阈值pbias逐渐增大，平均路径长度整体上呈下降趋势。表明pbias越大，随机树向终点生长的趋势愈加明显，使得路径更为直接，减少了无效搜索，缩短了路径长度。从图3（b）可知，路径平均搜索时间整体上呈现出先降低再上升的趋势，pbias取0.2用时最短。表明pbias较小时，RRT 算法偏向终点扩展的趋势小，随机树会向其他空白区域作无效搜索，导致搜索时间变长。而当pbias较大时，随机树会以较大概率朝终点生长，使得扩展节点选择单一。所以算法会花费较多时间避开障碍物，增加了路径搜索时间。

图3 概率阈值pbias 对算法性能指标影响

由于本文采用目标偏向策略主要目的是减少搜索时间，因而在后续实验中选取概率阈值pbias=0.2。

4.2 基于气味扩散法的路径搜索

气味扩散法灵感来源于机器人主动嗅觉目标搜索[19]，也称为气味源搜索。机器人利用传感器检测环境中的气味浓度，朝浓度增大方向行进并搜索出气味源的位置。假设机器人的终点为气味源，机器人所处环境为二维地图，且地图中的障碍物区域阻断了气味扩散，即气味不能以障碍物为介质传播。在地图中，气味源会呈辐射状地向周围扩散，离终点越远气味浓度越低，反之，气味浓度越高。由于障碍物阻断了扩散，气味只能在地图中的自由区域内完成扩散，而自由区域内与终点距离相同位置上的气味浓度应大致相同。由此可在二维地图上构建气味素浓度场，终点气味浓度最高且气味浓度只与终点位置和环境状况有关。无论机器人起点处于地图中的什么位置，只要沿着浓度增大的方向移动一定能到达终点，从而完成路径规划任务。

先将二维地图栅格化，排除障碍栅格，自由空间内每一个栅格可作为一个气味扩散单元。气味扩散采用四邻域模型，如图4 所示。在四邻域模型中，气味从当前栅格朝上下左右四个方向作扩散[20]，直至气味素扩散到自由空间中每一个栅格。将路径规划的终点作为气味源，即终点位置所处栅格为最高浓度，按照四邻域模型逐渐向外扩散，每扩散一次，气味浓度值减小一个等级，然后记录每个栅格的气味素浓度，当扩散完成后地图中的每个栅格都会分配到相应气味浓度值。气味扩散法可用下面的表达式表示：

式（3）中，i 为扩散批次，{O(i)}表示第i 批扩散栅格的气味浓度集合，{O(i+1)}为i+1批扩散栅格的气味浓度集合。O(1)表示终点所在栅格的气味浓度，将其值设为M。因为扩散中同一批次栅格的气味浓度相等，所以将它们称之为“等浓度集合”，气味的扩散是按照等浓度集合进行的[21]，每扩散一次，气味浓度值减1，如果此栅格已经标记有气味浓度值则放弃扩散。当每个自由栅格都标记好气味浓度值时扩散完成。

图4 机器人运动方向

由气味扩散法的原理可知，终点气味浓度最高，其值为M，所有栅格的浓度都由终点扩散而来。栅格气味浓度大小只与终点位置和环境状况有关，而与起始位置无关。每一个栅格都只能向上下左右四个栅格做扩散，而且任一栅格的气味浓度值都从比它浓度大1 的栅格扩散而来。所以任取一个浓度为X(X ＜M)的栅格，周围一定至少有一个栅格的浓度为X+1，依此类推：气味浓度为X+1的栅格周围一定至少有一个浓度为X+2的栅格，追溯到最后一定能找出浓度为M 的栅格，即是到达终点。将每个栅格的气味浓度值作为此栅格的代价，由此把二维地图转化为基于浓度的代价地图。因此，从起始栅格出发，每次向比当前栅格代价高的栅格移动，必然能找到一条通往终点位置的可行路径。这是将气味扩散法应用到RRT 算法中的重要基础。由气味扩散法生成的代价地图如图5 所示。机器人所处环境为8×10的栅格图，图中横向为x轴，纵向为y 轴，终点坐标为（1，1），起点坐标为（8，10），黑色区域为障碍物，白色区域为自由空间。设终点浓度值为30，可从图中看出经过气味扩散法处理后自由空间中每个栅格对应的代价值。机器人的一个可行路径为（8，10）—（8，9）—（7，9）—（7，8）—（6，8）—（6，7）—（6，6）—（5，6）—（5，5）—（4，5）—（3，5）—（2，5）—（1，5）—（1，4）—（1，3）—（1，2）—（1，1）。

图5 气味扩散法示意图

本文提出的改进RRT 算法在扩展节点过程中引入气味扩散法，可准确有效地指导随机树的生长：在RRT算法生成新节点时根据栅格代价来确定接受或是舍弃，使得随机树朝栅格代价增大的方向生长，从而到达终点，克服了原算法偏离最优解的缺陷。由于气味扩散法生成的代价地图中每个栅格周围都存在比当前栅格代价高的栅格，使得随机树趋向于目标点生长，提高了路径质量。改进算法不但采用气味扩散法改善扩展节点的选取，而且保留了原算法的随机特性，从而避免了陷入局部最优解。

4.3 改进RRT算法流程

改进RRT 算法采用目标偏向策略和气味扩散法来改善扩展节点的选取，加快了算法收敛速度，从而解决了随机点分布过于平均的缺点。算法流程如下：

步骤1 环境建模：对地图进行栅格化，利用气味扩散法得到与栅格地图相对应的代价地图。

步骤2 初始化探索树T。

步骤3 生成随机点Qrand，设定一个偏向概率阈值pbias，在随机选取扩展节点时利用均匀概率分布产生一个随机概率p，若p ＞pbias，则Qrand在机器人状态空间中随机产生，否则Qrand=Qgoal。

步骤4 选择树的生长点：

步骤5 求探索树的新节点Qnew：

步骤6 判断Qnew所在栅格代价是否比Qnear所在栅格代价高，若是则转步骤7，否则返回步骤3。

步骤7 更新探索树T，判定Qnew是否为探索失败节点，若不是，则Qnew加入到T 中。否则，放弃Qnew，返回步骤3。

步骤8 判断是否到达目标点。若‖ Qgoal-Qnew‖＜s，则到达目标，否则，返回步骤3，其中s 为机器人最短行走距离。

步骤9 从目标节点逆向回溯至探索树的根节点，并返回规划路径。

4.4 基于三次B样条曲线的路径平滑

通过改进RRT算法输出的路径由折线段构成，且路径长度不是最短，需要作进一步平滑处理。常用的路径平滑方法有多项式插值、曲线拟合、B样条插值等。高次插值易在区间两端产生震荡，造成较大的逼近误差，称为龙格现象。为了使生成路径逼近原路径，平滑过程中应避免龙格现象。采用龙格函数测试各种路径平滑方法的逼近性能，如图6所示。从图中可看出，三次曲线拟合的效果最差，所得图形与原曲线相差甚远；多项式插值在中间部分插值效果较好，对于两端的插值结果非常不理想；而B 样条曲线给出了光滑的插值曲线，逼近效果理想，稳定性好，收敛性强，能很好地回避龙格现象。

由于B 样条曲线能避免龙格现象，具有连续性和局部性[2]等优点，且能生成曲率连续的平滑路径，本文采用B 样条曲线对路径进行平滑处理。由于B 样条曲线局部平滑性好，不能在缩短路径方面取得理想效果，所以在采用B 样条曲线对路径平滑处理前先对路径作预处理。预处理的目的是缩短路径，以便后续平滑处理。

假设利用改进RRT 算法进行路径规划的输出结果由点序列S={( x1,y1),(x2,y2),… ,(xn,yn)}构成，对点序列S进行预处理，预处理后路径规划的点序列为要使路径长度缩短，必须满足以下两个条件：

图6 三种方法逼近性能比较

条件（1）的作用是使得平滑后的所有路径点与对应原始路径点的距离和最小；条件（2）保证了平滑后的点序列中前一个路径点与后一个路径点的距离和最小。所以路径最短的目标函数为：

式（6）中α、β 为权重因子，且满足α+β=1。路径平滑的实质即是求上式的最优解，求解最优解的方法采用梯度下降法（gradient descent），即通过多次迭代，调整P(i)使M 取得最小值，使得路径变短。通过式（6）得到的路径P 仍不够平滑，需对P 作进一步处理。

为了使路径变为理想的平滑曲线，采用三次B 样条曲线对P 作进一步优化处理。三次B 样条曲线和控制点间的关系如公式（7）所示：

式（7）中t ∈[0,1]，Pi、Pi+1、Pi+2、Pi+3为控制点。最后依次连接全部三阶B 样条曲线段即可生成最终路径。

5 实验结果及分析

分别在仿真环境和真实环境下进行实验。将本文改进RRT 算法与基本RRT、SARRT 等算法进行对比，验证提出算法的优越性和正确性。仿真实验硬件平台为笔记本电脑，真实环境实验平台为配备笔记本电脑的Turtlebot2。计算机配置为Ubuntu 操作系统，处理器为i3-3110M，主频2.4 GHz，运行内存为4 GB。

5.1 仿真实验

在ROS 的Stage 仿真平台中选取三种不同场景进行实验，图7（a）～（c）分别为地图1、地图2、地图3。三种地图均为10 m×10 m 的迷宫地图，地图大小为200×200像素。

图7 三种地图

为了方便处理、提高运行效率，把地图划分为40×40的栅格。改进RRT与SARRT[13]算法的相同点是都会在随机树生长前对地图进行预处理，利用扩散方法构造代价地图，把栅格代价大小作为产生新节点的依据。以地图2 为例，对两种算法的性能进行比较。图8（a）为改进RRT 算法得到的代价地图，图8（b）为SARRT 算法得到的代价地图。在图8 中，目标点用三角形标记，起点用圆形标记；红色区域表示高代价栅格，蓝色区域表示低代价栅格。观察发现，图8（b）左中右三个区域每个区域的颜色大致一样，表明SARRT 的扩散结果过于平均，因而局部指导能力较差。与图8（b）相比，图8（a）颜色层次分明，更能准确反映地图中自由空间的结构。改进RRT 的扩散过程采用气味扩散法，该方法局部性能优越，在复杂环境下的指向性更好，能对随机树的生长作精确指导。SARRT 的扩散过程采用菲克第二定律，计算复杂，且需要多次迭代才能在环境中完成扩散，因此耗费更多时间。由于气味扩散法计算简单，使得改进算法在效率和速度上得到极大的提升。气味扩散完成后开始执行路径规划任务，分别在地图1、地图2、地图3下进行实验。设定搜索步长s=0.30 m，偏向目标概率pbias=0.2，路径最短目标函数式（6）中的权重因子α=0.5，β=0.5。

图8 改进RRT与SARRT代价地图对比

地图1 的环境比较简单，设定机器人起点坐标为（2，2），终点坐标为（2，9）。图9（a）～（c）分别为基本RRT、SARRT 和本文提出的改进RRT 算法在地图1 中的输出结果。

图9 地图1中三种算法路径规划结果比较

地图2的场景比地图1复杂，设定机器人起点为（2，2），终点为（9，2）。图10（a）～（c）分别为基本RRT、SARRT和本文提出的改进RRT算法在地图2中的输出结果。

图10 地图2中三种算法路径规划结果比较

地图3 的场景最为复杂，设定机器人起点坐标为（1，1），终点坐标为（9，2）。在地图3 中，机器人可以选择不同的通道到达终点，路径长度与机器人选择的通道密切相关。图11（a）～（c）分别为基本RRT、SARRT 和本文提出的改进RRT算法在地图3中的输出结果。

图11 地图3中三种算法路径规划结果比较

由图9 和图10 知，地图1 和地图2 的共同特点是机器人只有一个通道到达终点，在此种情况下改进RRT算法表现最优。由图11 知，基本RRT 的路径没有方向性，机器人可能会选择距离最长的通道，导致路径长度增大。而SARRT 和改进RRT 的路径具有方向性，机器人会选择距离最短的通道到达终点，因而路径长度缩短，路径质量更高。比较图11（b）和（c）可知，与SARRT 相比，改进RRT的路径长度更短，路径平滑度得到提升，所以改进RRT算法的性能优于SARRT。

由于RRT算法具有随机性，为了客观评价算法的优劣，在三种地图场景中分别对三种算法进行20 次路径规划。设定三种算法的搜索步长s=0.30 m，改进RRT 算法的偏向目标概率pbias=0.2，路径最短目标函数式（6）中的权重因子α=0.5，β=0.5。将路径长度、搜索时间和采样节点数作为三种算法的评价指标，记录20 次实验的平均值。地图1、地图2、地图3 仿真实验数据如表1、表2、表3所示。

表1 地图1仿真实验数据对比

表2 地图2仿真实验数据对比

表3 地图3仿真实验数据对比

通过图9～11和表1～3中的仿真实验数据可知，与基本RRT 和SARRT 相比，改进RRT 算法的搜索时间更短，在效率和速度上得到极大提升，而且路径长度缩短、路径更为平滑，使得路径质量得到显著改善，证明改进RRT算法在复杂环境下进行路径规划具有优良的性能。

5.2 Turtlebot2实验

图12 改进RRT算法实验场景

为了测试实际场景下改进RRT算法的性能，将算法应用到基于ROS的移动机器人Turtlebot2上。本文的实验场景为实验室，如图12（a）所示。图12（b）为实验室的整体环境图。首先用无线控制器控制Turtlebot2扫描室内环境，利用gmapping 算法构建2 维地图，地图尺寸为576×512 像素。然后在建好的地图上进行路径规划实验，并将实验结果与基本RRT和SARRT进行对比。

在ROS 的Rviz 可视化工具中显示二维地图和路径规划结果，基本RRT 算法的输出结果如图13（a）所示，SARRT算法的输出结果如图13（b）所示，改进RRT算法的输出结果如图13（c）所示。图中的黑色区域表示障碍物，白色区域为机器人运动的自由空间。

图13 实验室环境下三种算法输出结果

表4 列出了三种算法在相同起始点下的路径长度、搜索时间和采样节点数。由实验数据可知，改进RRT算法优于SARRT 和基本RRT。与基本RRT 相比，改进RRT的路径长度大大缩减；与SARRT相比，改进RRT的搜索速度更快。考虑到算法各方面的性能，认为改进RRT算法能在真实环境下生成最优路径。

表4 实验室环境下三种算法实验数据对比

实验结果证明，结合了目标偏向策略和气味扩散法的改进RRT 算法能高效完成复杂环境下的移动机器人路径规划。相比于基本RRT 和SARRT 算法，改进RRT算法优化了节点扩展方式，因此搜索速度更快，路径质量更优。

6 结束语

基本RRT 算法不能满足真实环境下移动机器人路径规划的实际需求。为了提高路径规划的效率和质量，对RRT算法进行改进，在原算法的基础上引入目标偏向策略和气味扩散法，利用基于浓度场的成本地图精确指导随机树的生长，从而选择最优采样节点，提高搜索效率。为了使生成路径是理想平滑曲线，采用基于三次B样条曲线的路径平滑方法对生成的路径进行优化，进一步改善了路径质量。不同环境下的仿真实验表明，改进RRT算法能显著提高搜索效率和避障能力，而且路径长度更短、路径质量更优，尤其在复杂环境下表现出优良的性能。将改进RRT 算法应用到Turtlebot2 上并在真实环境下进行实验，结果表明改进RRT 算法可满足实际需求，能高效解决移动机器人在复杂环境下的路径规划问题。