基于MOBDB-RRT*算法的移动机器人路径规划

张 瑞， 周 丽,2， 刘震锴

(1.南京信息工程大学，南京 210000； 2.江苏省大气环境与装备技术协同创新中心，南京 210000)

0 引言

21世纪是信息化与智能化的时代，而机器人作为现代高自动化产物，被广泛应用于人类的日常生活。机器人技术的快速发展，提高了生产效率，促进了经济发展，给人们的生活带来了便利。因而，近年来国内外对于机器人的研究也更加深入，其中，路径规划问题是机器人研究领域的一个重要分支[1]。考虑到机器人现实工作环境的复杂情况，需要事先为机器人规划一条从起点到终点的安全无碰撞最优路径。传统的路径规划算法有人工势场法[2]、A*算法[3]、可视图法[4]等。基于智能算法的有遗传算法[5]、粒子群算法[6]、蚁群算法[7]等。其中，传统的路径规划算法需要对复杂的规划空间和障碍物进行精确建模，而环境的复杂程度越高，算法的规划效率也就越低。智能算法虽然学习能力非常强，但实时性差、计算量较大，需要占用大量的存储空间[8]。

为了加强在复杂障碍物环境中的规划能力，基于采样的单一查询快速扩展随机树(Rapidly-exploring Random Tree，RRT)算法被提出[9]。RRT由于结构简单、概率完备性、强大的搜索扩展能力以及避免对复杂空间精确建模等优点，被广泛应用于机器人的路径规划。然而，现有的RRT算法有着规划效率低、易陷入局部极小值、路径长度不最优、路径不光滑等缺点。为了解决RRT算法存在的问题，诸多学者提出了改进方法。KUFFNER等[10]提出了RRT-connect算法，在起点和终点同时生成两棵随机树，通过双向扩展缩短了规划时间，但算法缺乏对路径成本的考虑；KARAMAN等[11]提出了RRT*算法，可在迭代次数充足的条件下找到最优路径，但算法搜索时所花费的时间较长；JORDAN等[12]提出了B-RRT*算法，在RRT*算法的基础上引入双向搜索思想，加快了RRT*算法的收敛速度，但采样较为随机，节点的利用率不高；NASIR等[13]提出了RRT*-smart算法，通过智能采样的方式提高了规划效率，但自适应能力较差；王兆光[14]提出了改进的概率偏向目标RRT算法，提高规划效率的同时有效解决了局部极小值的问题，但不能保证路径长度达到最优；张伟民等[15]提出了一种基于目标约束采样和目标偏置扩展的改进RRT*算法，加快算法搜索速度的同时路径也变得更加光滑，但没有考虑到机器人的安全运行距离。上述文献对RRT算法进行了有效的改进，但在复杂的环境中，障碍物的不规则性和狭窄的间距可能会降低算法的规划效率，而且对路径的安全性也提出了较高的要求。

本文结合已有的研究及存在的问题，提出了一种基于膨胀化障碍物环境的双向动态目标偏置RRT*(Magnified Obstacles-based Bidirectional Dynamic Goal Bias RRT*，MOBDB-RRT*)算法。针对RRT*算法规划的路径距离障碍物近的问题，采用对障碍物进行膨胀化处理方法，给机器人预留出一定的安全距离；为了提高算法的规划效率并解决易陷入局部极小值的问题，基于RRT*算法引入双向动态目标偏置策略，使生成的节点具有明确的方向性，减少了对不必要区域的搜索；为了得到更短、更光滑的路径，本文还对已规划好的路径采用修剪算法和三次贝塞尔曲线进行二次优化，使最终路径更有利于移动机器人安全运行。

1 RRT*算法

RRT算法在路径规划过程中只连接到最近的点，并不能保证路径的成本达到最低，因此KARAMAN等在2011年提出了RRT*算法。RRT*算法是一种渐近最优算法，在RRT算法的基础上加入了重新选择父节点以及重新布线的过程，这样既保留了RRT算法的概率完备性，又能使路径达到相对最优。

RRT*算法的第一层优化：重新选择父节点。具体过程如图1所示，首先以qnew为圆心和事先定义好的半径画圆，找到这个圆内qnew的所有近邻点作为其备选父节点，分别为点E，F，H，J。从图中可以看出，A-B-D-K这条路径为RRT算法规划的初始未优化路径，其路径成本为9。当备选父节点连接到qnew时，路径分别为A-E-K，A-B-D-F-K，A-E-H-K，A-B-D-J-K，路径成本分别为8，13，12，13。由于路径A-E-K的成本最低，所以将路径A-E-K替换掉原来路径A-B-D-K。

图1 重新选择父节点Fig.1 Reselect parent nodes

RRT*算法的第二层优化：随机树重新布线。为了进一步减小路径成本，试图为qnew寻找下一个可连接的近邻点，若此路径成本小于原先路径到达该点的成本，则将其替换。具体过程如图2所示，D，F，H，J为qnew的近邻点，其中路径A-E-K-D的成本为10，而原先路径A-B-D成本为7，不满足要求。同理，连接到其他近邻点的成本分别为10，11，11，与之对应的原先路径成本为11，9，10。因此，将路径A-E-K-F替换掉原先路径A-B-D-F，成为新的随机树。

图2 随机树的重新布线Fig.2 Rerouting of random trees

2 MOBDB-RRT*算法

RRT*是一种渐近优化算法，当路径中节点的数量不断增加时，算法的计算量也随之增加，因此需要较长的运算时间才能逼近最优解。虽然RRT*算法能使路径达到相对最优，但没有解决RRT算法盲目搜索带来规划效率低的问题，反而进一步增加了规划时间，且路径中还存在着冗余节点，无法使路径成本达到最低。考虑到安全性，线性连接路径的不光滑以及靠近障碍物也不利于机器人移动。针对以上RRT*算法存在的缺点，本文提出了一种MOBDB-RRT*算法，该算法将障碍物进行膨胀化处理，避免了路径距离障碍物太近，保证机器人安全运行；以双向动态偏向目标代替单向随机扩展，提高了RRT*算法的搜索效率；利用修剪算法和三次贝塞尔曲线对路径进行优化处理，最终生成一条有利于机器人移动的光滑路径。

2.1 设置膨胀化障碍物

为了获得成本最小的路径，RRT*算法可能会连接靠近障碍物的节点，虽然没有发生碰撞，但这样的路径显然不适合实体移动机器人运行。因此，需要为机器人设置安全运行距离(一般为机器人宽度的一半)。本文选择将障碍物进行膨胀化处理，在放大了的障碍物环境中进行路径规划，这样不仅可以避免路径紧贴着障碍物，而且为后续光滑处理后路径的避障带来了便利。

本文以多边形障碍物为例，为移动机器人设置复杂的运行环境。图3为膨胀化处理后的障碍物环境，其中,膨胀系数k即原障碍物边和膨胀后障碍物边之间的距离,可根据机器人的大小进行调整。

图3 膨胀化障碍物Fig.3 Magnified obstacles

2.2 双向动态目标偏置策略

由于RRT*算法的路径节点是朝随机点的方向生成，扩展方式为单向，在复杂的障碍物环境中，无疑增加了规划的难度。所以希望通过起点和终点同时扩展两棵随机树，并改变路径规划过程中节点的生成方向，让其尽可能朝着目标点方向扩展，从而达到缩短规划时间的目的。

首先定义两棵随机扩展树V1和V2，一棵从起点开始扩展，另一棵从终点开始扩展，每次迭代生成两个点。其中,新节点qnew以动态偏向目标的方式生成，在随机树的扩展过程中，首先对树节点中的最近点qnearest和随机点qrand的连线进行障碍物判断，有障碍物时按随机点方向生成新节点qnew，其算式为

(1)

若没有障碍物，取随机点qrand方向的步长为λ1，终点qgoal方向的步长为λ2，其中λ2=2λ1，根据矢量合成原理，可得到新节点qnew，其算式为

(2)

当碰撞检测失败即最近点qnearest和新节点qnew之间有障碍物，新节点qnew生成无效，此时将步长进行反转变换，让随机点qrand方向的步长为λ2，终点qgoal方向步长为λ1，原理同上可得到新节点qnew，其算式为

(3)

如果碰撞检测还是失败，无法得到有效的新节点qnew时，这说明偏向出现了困难，此时，只需让新节点qnew按随机点方向生成即可。图4为双向动态目标偏置的具体扩展示意图。

图4 双向动态目标偏置的具体扩展示意图Fig.4 Specific expansion diagram of bidirectional dynamic goal bias

上文描述的双向动态目标偏置策略，加快了RRT*算法的收敛速度，提高了路径的规划效率，其中，动态变步长的搜索方式有效避免了在复杂障碍物环境中陷入局部极小值的问题。

2.3 路径优化

2.3.1 路径缩短

RRT*算法的随机扩展会生成一些冗余节点，从而导致路径较为曲折，在满足避障要求的前提下，去除这些多余的节点，有利于进一步减小路径成本。路径缩短处理采用修剪算法，基本原理如下：首先让终点qgoal和起点qstart直接相连，如果无碰撞，最终路径就为起点连接终点的直线，如果有碰撞，选择终点qgoal的前一个点和起点qstart相连，按照这样的方式，依次对各个路径点和qstart进行障碍物碰撞判断，直至找到无碰撞的那个路径点，记为新起点q′start，重复上述过程，一旦找到能与终点qgoal直接相连且无碰撞的新起点就结束。最终的优化路径就由起点、中间这些新的起点和终点连接而成。

2.3.2 路径光滑

RRT*算法规划的路径最终是点与点的线性连接，而这种锯齿状不光滑路径不利于机器人移动，因此需要对上述规划好的路径进行光滑处理。为了平滑地拟合路径节点，本文采用贝塞尔曲线。设n次贝塞尔曲线有n+1个控制点(P0,P1,…,Pn)，其定义为

(4)

式中：s∈[0,1]；Bn,i(s)为Berstein多项式。

根据式(4)可知，当n越大时，算法的计算成本会随之增加，因此较为低阶的贝塞尔曲线是首选，而三次贝塞尔曲线是可以生成连续曲率轨迹的最小次数曲线。本文基于三次贝塞尔曲线对折线路径进行光滑处理。

3 仿真实验分析

利用Matlab R2016a 对本文提出的MOBDB-RRT*算法进行仿真实验验证。笔记本电脑的系统为Windows10，处理器为Intel®CoreTMi5-10210U CPU @1.6 GHz/2.11 GHz，运行内存为16 GiB。

为了验证所提出MOBDB-RRT*算法的优越性，本文将RRT*，B-RRT*和MOBDB-RRT*这3种算法在3种不同的二维仿真环境下做对比实验，地图大小为400 dm×400 dm，障碍物均设置为多边形，将3种算法在每个地图中各运行50次，表1记录了规划路径时所花费的平均扩展节点、平均规划时长和平均路径长度。

表1 实验数据Table 1 Experimental data

图5为狭窄不规则障碍物环境(地图1)规划对比图。起点为(10 dm，10 dm)，终点为(400 dm，400 dm)，地图特点是障碍物较大且间距狭窄。图5(a)为RRT*算法规划的一条可行路径，由于RRT*算法的盲目搜索特性，生长树会随机地分布在无障碍区域，虽然路径相对较优，但规划所需时间较长。图5(b)为B-RRT*算法规划的一条可行路径，虽然引入了双向扩展思想，改善了RRT*算法规划效率低的缺点，但生长树的扩展依然充满了随机性，无效节点较多。图5(c)为MOBDB-RRT*算法规划的一条可行路径，安全距离和双向动态偏向目标的引入使算法在较为狭窄的环境中也能快速找到一条较优的路径，扩展节点利用率高，减少了对不必要空间的探索。

图5 不规则障碍物环境规划对比图Fig.5 Comparison of path planning in an environment with irregular obstacles

从表1的地图1实验数据中可以看出:MOBDB-RRT*算法的平均扩展节点数为60，比RRT*算法减少97.06%，比B-RRT*算法减少70.44%；平均规划时长为0.580 2s，比RRT*算法缩短95.57%，比B-RRT*算法缩短53.62%；平均路径长度为637.790 1 dm，比B-RRT*算法减少2.35%。

图6为大量规则障碍物环境(地图2)规划对比图，起点为(400 dm,0 dm)，终点为(0 dm,400 dm)，地图特点是障碍物较多，起点到终点有多条可行路径。

图6 大量规则障碍物环境规划对比图Fig.6 Comparison of path planning in an environment with a large number of regular obstacles

图6(a)中，RRT*算法通过全图扩展随机树，利用自身的重布线特性，规划出一条相对较优路径，但扩展随机树所需节点较多，规划效率不高。图6(b)中，B-RRT*算法由于随机采样和双向扩展可能导致路径偏差较大，无法找到较优路径。图6(c)中，MOBDB-RRT*算法由于加入双向目标偏置策略，两棵生长树都朝着各自的目标扩展，在保证路径成本达到相对最小的同时提高了算法的规划效率，扩展节点明显减少，路径也变得更加平滑。

从表1的地图2实验数据中可以看出:MOBDB-RRT*算法的平均扩展节点数为63，比RRT*算法减少96.64%，比B-RRT*算法减少57.72%；平均规划时长为0.587 8 s，比RRT*算法缩短96.27%，比B-RRT*算法缩短64.51%；平均路径长度659.347 6 dm，比B-RRT*算法减少2.29%。

图7为单通道障碍物环境(地图3)规划对比图，起点为(30 dm,50 dm)，终点为(370 dm,350 dm)，地图特点是从起点到终点需要多次转折，偏向目标较为困难。

图7 单通道障碍物环境规划对比图Fig.7 Comparison of path planning in an environment with single channel obstacles

RRT*算法除了上述提到的规划速度慢之外，在障碍物较为稀疏的环境中易出现冗余节点，路径折线较多;B-RRT*算法规划的两端路径较优，但随机扩展特性使节点的利用率不高;而MOBDB-RRT*算法在偏向困难的障碍物环境中，依然能够以较快的速度找到一条光滑的安全无碰撞路径。

从表1的地图3实验数据中可以看出:MOBDB-RRT*算法的平均扩展节点数为272，比RRT*算法减少81.03%，比B-RRT*算法减少47.18%；平均规划时长为3.547 2 s，比RRT*算法缩短70.56%，比B-RRT*算法缩短48.22%；由于3种算法在图7这种单通道障碍物环境中探索方式相似，因此平均路径长度相差不大，其次MOBDB-RRT*算法设置了安全运行距离，所以会比B-RRT*算法规划的路径要长一点。

根据上述的仿真图和实验数据可以明显看出，RRT*算法虽然在路径长度上占有优势，但是时间耗费非常大，而B-RRT*算法虽然能在较短的时间内找到路径，但随机扩展导致无效节点较多，而且在有多条可行路径的障碍物环境中路径成本相对较大。除了规划时间和路径成本外，这两种算法规划的路径不光滑且靠近障碍物，机器人运行的安全性无法得到保障。本文所提出的MOBDB-RRT*算法在3种复杂的障碍物环境中，规划效率更高、路径更光滑且更安全。

4 结束语

本文在现有的RRT*算法的基础上提出了MOBDB-RRT*算法，该算法所设置的膨胀化障碍物，为机器人留出安全的运行距离，保证了路径的可行性。通过引入双向动态目标偏置的思想，加快了随机树的生长速度，提高了算法的规划效率，大大缩短了搜索路径的时间。结合修剪算法和三次贝塞尔曲线对规划好的路径进行优化处理，去除冗余节点的同时使路径更加光滑。通过与RRT*算法和B-RRT*算法进行仿真实验对比，证明了本文提出的MOBDB-RRT*算法在路径规划效率和路径质量上更具优越性，该算法对移动机器人的运动规划具有一定的参考意义。