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

刘 冲,刘本学,吕 桉,李 霞

(郑州大学机械与动力工程学院,郑州 450001)

0 引言

随着各行业对自动化要求的不断提高,诸如扫地机器人、物流机器人等室内移动机器人的应用越来越广泛。路径规划是保证移动机器人完成各项任务的前提,是移动机器人研究领域的一个重要基础性问题[1]。路径规划是指根据已知的环境地图等条件,在满足特定条件的情况下,规划出一条从起点到终点的无碰撞路径[2]。路径规划算法的性能优劣对于机器人能否顺利完成任务有较大的影响。常见的路径规划算法包括Dijkstra算法、A*算法、RRT算法[3]、RRT*算法、遗传算法、蚁群算法等。Dijkstra算法一定能找到环境中的最短路径,但是路径规划过程中遍历的节点过多,计算复杂,算法效率较低。A*算法在Dijkstra算法的基础上设置了估价函数,提高了算法的搜索效率。遗传算法的计算量较大,对于高维度问题难以处理和优化。

RRT算法是一种概率完备、结构简单且具有灵活搜索能力的路径规划算法。具有较快的扩展和搜索速度,适用于各种复杂环境下的路径规划。但因其随机采样的机制而存在节点利用率低、路径复杂度高等问题[4-5]。针对RRT算法的不足,许多学者提出了优化方案。POHL[6]提出了双向扩展随机树算法(Bidirectional RRT),通过构造两棵分别以起点和终点为根节点的随机树来完成路径规划,提高了算法的收敛速度。JR等[7]将贪心算法应用于Bi-RRT算法,提出了RRT-Connect算法,提高了随机树的扩展效率。WANG等[8]提出了一种基于强化学习的LM-RRT算法,提高了随机树的局部空间探索能力,使机器人在狭窄通道的路径规划效率得到提高。QI等[9]提出了MOD RRT*算法,通过对生成的路径进行重新规划,提高了路径的质量。李金良等[10]通过改变RRT算法的度量函数并添加角度约束,减少了路径规划过程中的搜索范围,提高了算法的效率。王硕等[11]通过引入动态权重系数使树尽可能地在向目标点进行扩展的同时又能够及时地避开障碍物,减少了冗余节点数。张玉伟等[12]提出了IBI-RRT*算法,通过引入基于状态子集直接采样的反向扩展策略,使机器人在障碍物边界区时能够获得接近最优路径成本的可行路径。

本文通过优化采样点选择策略,并提出了可变步长的随机树扩展策略,以及对初始路径平滑处理3个方面对RRT算法进行改进。使用MATLAB进行仿真分析,在搭载ROS系统的移动机器人上进行实验,验证了改进RRT算法的优越性与可靠性。

1 RRT算法介绍

在RRT算法中,设置机器人所在状态空间为X,起点为Xstart,终点为Xgoal。算法初始化时将起点加入随机树中。每次采样开始时,从状态空间中随机选取采样点Xrandom,依次遍历当前随机树中的所有节点,找到距Xrandom距离最短的节点Xnearest,从Xnearest起向Xrandom扩展一个步长得到新节点Xnew。若Xnearest与Xnew之间的路径没有障碍物,则将Xnew加入随机树中,否则重新开始采样,直至随机树扩展到终点Xgoal。最后,从Xgoal回溯至Xstart,即可得到完整的路径。算法的流程如图1所示。

图1 RRT算法流程

2 改进RRT算法

2.1 基于动态概率的采样点选择策略

RRT算法总是在机器人状态空间中随机选取采样点,虽然具有概率完备性,但在实际搜索路径的过程中,这种随机选取采样点的策略会使大量的采样点最终不会出现在规划的路径中,导致计算资源的浪费。为了减少采样过程的随机性,URMSON等[13]提出了目标偏向RRT(Goal-biased RRT)算法。该算法按式(1)所示方式选取采样点。

(1)

式中:Xrandom为采样点,Xgoal为终点,P为随机数,0

Goal-biased RRT算法既保留了RRT算法的概率完备性,也提高了随机树的扩展速度。但Goal-biased RRT算法中概率阈值Pbias为固定值。当采样点在障碍物附近时,Goal-biased RRT算法会使机器人不易绕开障碍物。若继续采用目标点作为扩展点,机器人会陷入局部极小值,增加路径规划的时间。在实际路径规划过程中,一般将采样次数设置为有限值。这就有可能导致路径规划失败,如图2所示。

图2 路径规划失败

本文提出了基于动态概率的采样点选择策略:在算法开始时设置概率阈值初始值为Pini。当随机树扩展到障碍物附近时,则认为其已陷入局部极小值,此时通过式(2)计算Pbias的值。随着采样点落入障碍物次数的增加,Pbias的值会逐渐减小。此时的采样策略会增加随机树逃脱障碍物附近区域的概率,也就会更快的跳出局部极小值。

(2)

式中:Pbias为概率阈值,Pini为概率阈值的初始值,n为上一次采样成功至此次采样期间,采样失败的次数。

在如图2所示的环境地图中,取不同的概率阈值初始值Pini,分别对Goal-biased RRT算法与基于动态概率采样的RRT算法进行仿真分析。以采样点数量、路径长度作为评价指标,记录1000次仿真的平均值,仿真结果如图3所示。

(a) 概率阈值初始值Pini对路径长度的影响 (b) 概率阈值初始值Pini对采样点数量的影响图3 概率阈值初始值Pini对两种算法性能的影响

由图3a可知,概率阈值初始值Pini的大小对两种算法规划得到的路径长度都有影响。随着Pini的增大两种算法规划的路径长度变短,这是由于Pini值越大,采样点越具有目标偏向性。在Pini<0.8时两种算法所规划出的路径长度差在1%以内。可以认为两种算法在路径长度这一评价指标上性能大致相同。由图3b可知,Goal-biased RRT算法的采样点数量始终大于基于动态概率的RRT算法,并且随着Pini值的增大,前者的采样点数量急剧增加。过多的采样点会导致算法的计算时间增加,降低路径规划的速率。说明基于动态概率采样的RRT算法能提高路径规划的效率。

2.2 基于可变步长的随机树扩展策略

RRT算法中随机树的扩展步长为固定值。步长过大时,会增加Xnew与Xnearest之间的路径经过障碍物的概率,降低采样的成功率,导致采样次数增加、算法的效率降低;步长过小时,不仅会增加采样的次数,还会使路径过于曲折,不利用机器人移动。

本文提出了可变步长的随机树扩展策略。在算法初始化时,设置最小步长Slim和基本步长S两个参数,两参数的关系如式(3)所示。

S=n×Slim

(3)

式中:n为距离因子,其值由环境地图的大小确定。

在未遇到障碍物时,随机树的扩展步长等于S。在遇到障碍物时,采用变步长的策略进行扩展。其主要思路如下:当Xnearest与Xnew两点之间的路径经过障碍物时,并不是直接舍弃Xnew开始下一次采样。而是将Xnearest与Xnew之间的路径分为n+1份,每一段的端点分别设置为Xi,如图4所示。若存在某一个Xi使得Xnearest与Xi之间的路径不经过障碍物且Xnearest与Xi+1之间的路径经过障碍物,则将Xi作为新的节点加入随机树中。按照此规则,在图4中应将X3作为新的节点加入随机树中。基于可变步长的随机树扩展策略提高了采样点的利用效率,有利于减少采样的次数,提高算法的收敛速度。

图4 可变步长的随机树扩展策略

图4中各节点坐标的计算式如式(4)和式(5)所示:

xi=x0+i×Slim×cosθ

(4)

yi=y0+i×Slim×sinθ

(5)

式中:xi、yi分别为节点Xi的横、纵坐标,x0、y0分别为节点Xnearest的横、纵坐标,θ为节点Xnearest与节点Xnew连线与水平正方向的夹角。

2.3 初始路径优化

RRT算法按照从Xgoal回溯至Xstart的方法生成路径。最终生成的路径中会包含许多不必要的采样点。一方面会增加最终路径的长度,另一方面会导致路径过于曲折,不利于机器人移动。采用如下方法对机器人的路径进行优化:从起点Xstart开始,依次检查Xstart与初始路径中的各个节点Xi之间是否存在障碍物。如果不存在障碍物,则将Xi的父节点变更为Xstart;如果存在障碍物,则用Xi的父节点代替Xstart继续遍历,直至遍历至Xgoal。路径优化的示意图如图5所示。优化前的路径为Xstart→X1→X2→X3→Xgoal。优化后的路径为Xstart→X2→Xgoal。

图5 初始路径优化

图5中相邻节点之间的转角过大,不利用机器人移动,需要进行路径平滑处理。贝塞尔曲线是一种连续光滑的曲线,具有曲率连续,控制结构简单等特点[14]。本文使用五次贝塞尔曲线对路径进行平滑处理。五次贝塞尔曲线和6个控制点之间的关系如式(6)所示。五次贝塞尔曲线如图6所示。

图6 五次贝塞尔曲线

B5(t)=(1-t)5P0+5t(1-t)4P1+10t2(1-t)3P2+
10t3(1-t)2P3+5t4(1-t)P4+t5P5

(6)

式中:t为控制参数,t∈[0,1];P0～P5为6个控制点。

3 基于MATLAB的仿真分析

为验证本文提出算法的性能,在图2所示环境地图中,对RRT算法、Goal-biased RRT算法、改进RRT算法进行仿真分析。仿真使用计算机的处理器为AMD R5-4650G,仿真软件为MATLAB R2019a。

设置起点坐标为(10,10),终点坐标为(80,90),概率阈值初始值Pini设置为0.2。采用路径长度、采样点数量、路径规划时间作为评价指标进行仿真。每种算法分别进行1000次路径规划,取平均值作为仿真结果。图7为3种算法规划出的路径,3种算法的仿真数据如表1所示。

(a) RRT算法 (b) Goal-biased RRT算法

(c) 改进RRT算法图7 仿真环境下3种算法规划的路径

表1 3种算法仿真数据对比

由图7可知,3种算法都规划出了一条从起点到终点的无障碍路径。由于RRT算法的采样策略不具有方向性,RRT算法规划过程中的采样点的数量明显多于Goal-biased RRT算法和改进RRT算法。并且RRT算法规划的路径的长度也比Goal-biased RRT算法和改进RRT算法长。由于改进RRT算法采用了变步长的采样点扩展策略以及路径优化,改进RRT算法规划出的路径长度也明显短于RRT算法和Goal-biased RRT算法,并且路径更为平滑。

由表1可知,改进RRT算法相比于RRT算法和Goal-biased RRT算法,规划路径的长度分别减少了23.09%和15.45%。规划路径的时间分别减少了87.16%和46.55%。由于Goal-biased RRT算法的概率阈值为固定值,在遇到障碍物后可能会陷入局部极小值,增加了路径规划的时间。

4 基于ROS平台的实验验证

为验证算法在实际环境中的性能,将算法应用于如图8所示的基于ROS的移动机器人。该移动机器人硬件组成为树莓派4B、思岚A1激光雷达、STM32开发板。分别在实验室环境和Gazebo虚拟环境中进行实验。图9a为实验室环境,图9b为Gazebo软件中的虚拟环境。

图8 基于ROS的移动机器人

(a) 实验室环境 (b) Gazebo环境图9 实验环境

使用Gmapping算法建立二维地图,在可视化工具Rviz中设置相同的起点和终点,分别在两个场景中使用3种算法各进行100次路径规划。3种算法的路径规划结果如图10和图11所示。实验数据如表2和表3所示。

(a) RRT算法 (b) Goal-biased RRT算法

(c) 改进RRT算法图10 实验室环境中3种算法规划的路径

(a) RRT算法 (b) Goal-biased RRT算法

(c) 改进RRT算法图11 Gazebo环境中3种算法规划的路径

表2 实验室环境中3种算法实验数据对比

表3 Gazebo环境中3种算法实验数据对比

由表2和表3可知,在真实环境的实验中,改进RRT算法在路径长度、采样次数、规划时间3项指标中,均有更好的性能,与仿真得到的结论一致。同时考虑机器人在真实环境中的移动过程,由图10和图11可知,改进RRT算法规划出的路径具有更少的转折点,更有利于机器人的移动。进一步验证了算法的有效性和优越性。

5 结论

针对RRT算法采样时间长,规划路径曲折等问题,提出了一种改进RRT算法。通过采用基于动态概率的采样点选择策略和可变步长的随机树扩展策略,减少了采样点数量,提高了随机树的搜索效率,最后使用五次贝塞尔曲线对初始路径进行优化,使最终生成的路径更适合机器人移动。仿真和实验结果均表明改进RRT算法能有效提高机器人路径规划的效率,并且规划的轨迹更符合机器人运动学规律。