基于改进RRT算法的移动机器人路径规划研究

巩 浩,谭向全,李佳欣,吴清文,2

(1.中国科学院 a.长春光学精密机械与物理研究所;b.空间光学系统在轨制造与集成系统重点实验室,长春 130033;2.中国科学院大学材料与光电研究中心,北京 100049)

0 引言

移动机器人被广泛应用于工业生产、运输、服务业等领域,已成为当下科技发展的重点研究方向[1]。路径规划就是机器人根据自身传感器对环境的感知,自行规划出一条符合机器人约束条件的安全无碰撞的运行路线[2-3],而路径长度、安全性、高效性则是衡量规划路径是否最优的重要指标[4]。

路径规划根据地图是否已知分为全局路径规划以及局部路径规划。常见的全局路径规划算法有A*算法、RRT算法、Dijkstra算法、Floyd算法等[5],局部路径规划算法有人工势场法[6]、动态窗口算法[7]等。

RRT算法是一种基于采样的路径规划算法,因其计算成本低,被广泛使用于移动机器人路径规划[8]以及机械臂轨迹规划[9]。RRT算法通过随机采样扩展搜索,算法结构简单、运算速度快。然而RRT算法也存在一些不足:盲目搜索,随机性强,收敛性差,再现性差;路径上冗余点多,曲折不光滑,一般不是最优路径[10-12]。针对这些问题,一些国内外学者提出了相关改进。KUFFNER等[13]提出RRT-Connect算法,通过采用起始点与目标点双向扩展随机树,大大降低了搜索时间。KANG等[14]提出改进的RRT-Connect算法,通过采用基于三角形不等式的重新排列树方法,改进了RRT算法规划路径上冗余点多的缺点。KARAMAN等[15]提出改进算法RRT*,通过重选父节点进行更新树,可以求解次优路径,但其计算成本高,效率低。刘慧等[16]提出改进的双向RRT*算法,通过结合动态末梢节点导向和势场导向进行偏置采样,改善了RRT算法随机性强的缺点。GAMMELL等[17]提出改进算法Informed RRT*,通过椭圆启发式搜索的最优采样,改进了RRT算法随机性强的缺点,但在复杂环境情况下容易陷入局部极值问题。栾添添等[18]通过划分采用区域,采用概率目标偏置策略和多级步长扩展进行路径规划,提升了节点搜索效率。杜传胜等[19]提出同根双向扩展的贪心RRT算法,将扩展方式改为由起始点与目标点连线的中间点同时向起始点和目标点进行双向扩展,加快路径生成,但是搜索过程容易陷入局部极值。

本文结合概率目标偏置与人工势场相结合的动态采样策略、步长函数、贪心思想、预留安全距离策略对RRT算法进行改进,提高了节点搜索的效率。对初步规划路径进行剪枝优化以及曲线拟合,减少路径上冗余点数,提高了路径质量,使最终路径满足移动机器人运动学特性,保证了最终路径的连续性和平滑性。

1 RRT算法原理

RRT算法是基于采样原理的一种典型路径规划算法。通过随机采样进行节点扩展,生成一个随机树,当寻找到目标点时,通过自由树的回溯思想,生成路径。节点扩展示意图如图1所示。

图1 RRT算法随机树扩展示意图

(a) 随机采样 (b) 人工势场

表1 RRT伪代码

2 改进RRT算法

2.1 采样优化

传统RRT算法采用随机采样的方式,采样节点盲目扩张,产生大量无效节点,导致算法搜索时间长、收敛性差。针对此问题,本文采用概率目标偏置与人工势场相结合的动态采样策略,采样函数为:

(1)

式中:Xrand1为随机采样的一点,P1与P2为0～1之间的随机数,α为目标权重因子,β为随机权重因子。首先在地图上随机采样一点Xrand1,然后通过两个随机数P1与P2决定选取哪种采样方式确定Xrand。α越大,越倾向于选择目标点作为采样点;α越小且β越大,越倾向于选择随机点作为采样点,保留一部分随机采样,帮助脱离局部极值;借鉴人工势场法,有(1-α)(1-β)概率选择目标点与随机点相结合的点作为采样点。对于不用场景,合理选择α与β的大小,得到较好的路径规划效果。

2.2 步长函数

考虑局部避障能力,且考虑快速到达目标点的要求,引入变步长函数。变步长函数包括两部分:

(1)快速扩展步长函数。考虑快速到达目标点,构造以Xnear、Xgoal之间的欧氏距离为自变量的函数,要求接近目标点时的步长为L1,根据起始点处四周无障碍时的步长要求Lmax,可以求得a,最后得到快速扩展步长函数:

(2)

式中:D为Xnear、Xgoal的欧氏距离。

(2)局部避障因子。考虑局部障碍物避障问题,构造以Xnear为圆心、r为半径的邻近圆区域内障碍物检测。

(3)

式中:Sobs为邻近圆区域内障碍物的面积,Sr为邻近圆区域总面积。

综合以上,设计步长函数为:

L=(1-ρ)LD

(4)

步长函数的添加会增加计算时间,合理设计局部无障碍区域的最大步长、快速扩展步长函数,可以在满足局部避障能力的前提下快速到达目标点,相对减少搜索时间。

2.3 贪心思想

随机采样思想实际应用时,新节点一般不能采样到目标点。针对这一问题,传统RRT算法设定新节点Xnew到达以Xgoal为圆心、单步长为半径的圆区域内,且Xnew、Xgoal连线无碰撞障碍物,此时认为新节点可直达目标点,则判断为完成路径搜索,返回路径。传统RRT算法在实际应用时,因为步长限制,存在新生长节点在目标点四周振荡现象[19],难以收敛。针对此问题,本文引入基于贪心思想的目标点检测,考虑到极度贪心思想可能会导致路径错过更优解[19],拟根据不同场景选取不同尺寸的目标点区域检测。即当有新节点被加入树中时,若新节点在目标点检测区域内,则对Xnew、Xgoal进行连线障碍物碰撞检测,若无碰撞,则直连Xnew、Xgoal,返回路径;若有碰撞,则继续采样搜索,重复进行。引入贪心思想的节点扩展示意图如图3所示。

图3 贪心思想的节点扩展示意图

图3中,新节点Xnew进入目标检测范围内,利用贪心思想,直连Xgoal,通过障碍物碰撞检测函数计算得Xnew、Xgoal连线之间无障碍物,直接将Xgoal加入随机树中,提前结束路径搜索。这种贪心思想可以很好地解决目标点附近新节点的振荡问题,减少迭代次数。针对不同场景,合理设置目标点检测区域半径的取值,能够有效提高算法的收敛速度。

2.4 安全距离

考虑传统RRT算法的随机采样性,存在新节点距离障碍物过近的现象,可能导致生成路径紧贴障碍物,并且考虑到移动机器人有外形尺寸限制,此时路径不符合移动机器人实际工作需求,故在路径规划过程考虑行车安全距离,安全距离如图4所示。

图4 预留安全距离示意图

当树节点生长过程中,与局部避障因子原理类似,在Xnew加入树节点之前加一步骤:检测以新生成节点Xnew为圆心、安全距离d为半径的圆区域障碍物检测,若不存在障碍物,则将Xnew加入树中,继续操作;若存在障碍物,则丢弃Xnew,重复采样。

3 路径优化

初步规划的路径中含有大量冗余点且拐点多,无法满足移动机器人的实际运动需求。因此本文通过对初始路径进行剪枝优化去除冗余点,然后通过对剪枝优化后的路径进行曲线优化去除拐点。

3.1 剪枝优化

传统RRT算法搜索出来的路径是由许多线段连接而成,考虑两点之间线段最短这一思想,对初步规划的路径进行剪枝优化,去除冗余点。具体做法是利用自由树的回溯思想对路径节点进行重选父节点,过程中考虑剪枝后路径的安全距离。剪枝优化示意图如图5所示。

图5 剪枝优化示意图

图5中,首先重选Xgoal的父节点。连接Xinit与Xgoal,如果连线未碰撞障碍物,则直接将Xinit视为Xgoal的父节点,去除Xinit与Xgoal之间的点,连接Xinit、Xgoal形成路径;如果连线碰撞障碍物,检测Xinit后的一点X1。连接X1与Xgoal,重复上述检测,检测到X5、Xgoal之间无碰撞,但连线距离障碍物过近,丢弃X5,直至检测到X6,满足X6、Xgoal之间无碰撞且连线距离障碍物有足够安全距离,将X6视为Xgoal的父节点。接着重选X6的父节点,重复上述操作。直至重选到某一点的父节点为Xinit,得到剪枝优化后的路径。

3.2 曲线优化

剪枝优化后的路径是由许多线段连接而成,在节点处存在航向角突变的问题,不符合移动机器人的运动学特性,无法满足实际应用需求。因此本文对剪枝优化后的路径进行平滑处理,使最终生成的轨迹满足移动机器人的运动学特性。

为了使优化后路径曲率连续,又考虑到实际应用中不可能为了曲率连续完全放弃直线行驶,故采用三次B样条曲线对路径转折点处进行拟合处理。

三次B样条曲线的表达式为:

(5)

式中:Pk(k=0,1,2,3)为控制点,Fk,3(t)为三次B样条基函数,推导公式为式(6)。

(6)

应用式(6),四点构造一段三次B样条曲线,公式具体化为:

(7)

最终,三次B样条的拟合曲线为:

P(t)=P0·F0,3(t)+P1·F1,3(t)+
P2·F2,3(t)+P3·F3,3(t)

(8)

4 仿真分析

为了验证本文改进算法的有效性,将RRT算法、RRT-Connect算法、文献[20]算法、本文改进RRT算法(曲线优化前)分别在无障碍场景、简单场景、复杂场景、狭窄通道场景下进行仿真分析,对仿真结果进行对比分析。仿真实验运行环境:系统windows 10,处理器Intel(R) Core(TM) i7-7700HQ CPU @2.80 GHz,内存双通道8.00 GB。

4.1 参数设置

仿真4种场景地图尺寸均为800×800,本文算法的其余参数如表2所示。每个算法在地图上进行10组实验,每组实验包括10次路径规划,规划结果以图表形式输出。

表2 参数设置

4.2 不同算法仿真分析

本小节对RRT算法、RRT-Connect算法、文献[20]算法、本文改进RRT算法(剪枝优化前)的仿真结果进行对比,验证改进算法的有效性。仿真结果如图6～图9所示。

(a) RRT算法 (b) RRT-Connect算法

从图6～图9中可以看出,4种算法均可以完成从起始点到目标点的全局路径规划,通过对比仿真结果可知:本文算法改进后,冗余节点大量减少;随机树扩展的避障能力得到增强;通过狭窄通道能力增强。对4种地图分别测试10组,每组测试10次,测试结果进行取平均值处理后如表3～表6所示。

表3 不同场景下路径搜索时间对比表

表4 不同场景下迭代次数对比表

表5 不同场景下路径长度对比表

表6 不同场景下路径上节点个数对比表

文献[20]算法在狭窄通道地图中迭代20 000次内规划路径成功率约70%。

对100*4次实验的路径搜索时间分析可知,本文算法可以很好地减少路径搜索时间,提升路径规划效率。4种场景下,本文算法的路径规划时间相对于RRT算法分别减少了98.06%、92.29%、84.74%、69.64%,相对于RRT-Connect算法分别减少了85.23%、42.90%、25.07%、59.38%,相对于文献[20]算法分别减少了4.17%、70.08%、92.08%、96.21%。

文献[20]算法在狭窄通道地图中迭代20 000次内规划路径成功率约70%。

对100*4次实验的迭代次数分析可知,本文算法可以很好地减少迭代次数,提升路径规划效率。4种场景下,本文算法的迭代次数相对于RRT算法分别减少了92.59%、72.93%、78.74%、41.83%,相对于RRT-Connect算法分别减少了63.79%、3.92%、35.87%、1.31%,相对于文献[20]算法分别减少了0.00%、85.96%、90.13%、91.42%。

文献[20]算法在狭窄通道地图中迭代20 000次内规划路径成功率约70%。

对100*4次实验生成的路径长度分析可知,本文算法可以很好地缩短路径长度。4种场景下,本文算法规划的路径长度相对于RRT算法分别减少了22.23%、16.96%、16.41%、7.02%,相对于RRT-Connect算法分别减少了20.26%、15.04%、15.07%、5.47%,相对于文献[20]算法分别增加了0.00%、0.18%、3.63%、8.50%。

文献[20]算法在狭窄通道地图中迭代20 000次内规划路径成功率约70%。

对100*4次实验生成路径上的节点个数分析可知,本文算法可以很好地优化路径节点个数。路径上节点个数可以反应采样函数的好坏。4种场景下,本文算法规划路径上的节点个数度相对于RRT算法分别减少了22.22%、17.54%、16.36%、6.25%,相对于RRT-Connect算法分别减少了20.76%、14.55%、16.36%、4.76%,相对于文献[20]算法分别增加了0.00%、4.08%、2.13%、-5.26%。

综上所述,针对不同地图环境,相比于RRT算法、RRT-Connect算法、文献[20]算法,本文改进算法有效提高了收敛速度、搜索效率以及稳定性,减短了路径长度。

4.3 路径优化仿真分析

路径优化包括剪枝优化和三次B样条曲线拟合。为了验证路径优化的效果,选择复杂障碍地图进行仿真实验,实验效果如图10所示。

(a) 剪枝优化前 (b) 剪枝优化后

图10a为初步规划路径,路径曲折不光滑且冗余点过多;图10b为剪枝优化后的路径,路径上冗余点大大减少,曲折性有所改善;图10c为三次B样条曲线拟合后的路径,路径光滑性得到优化。由实验结果可知,本文算法有效提高了路径平滑性、路径质量,符合移动机器人的运动学特性,提高了行车的安全性。

5 结论

为了解决传统RRT算法进行移动机器人路径规划时存在的问题,提出了一种改进RRT算法,通过理论分析以及仿真分析,得到以下结论:

(1)引入概率目标偏置与人工势场结合的采样策略,改善了节点生成过程,仿真结果表明,改进算法优化了节点扩展,降低了路径搜索的随机性,提高了路径搜索效率,解决了目标点四周生长节点时的振荡问题。目标权重因子、随机权重因子的引入使改进算法可以适应不同地图环境。

(2)引入动态变步长扩展策略,节点步长扩展过程中,在距离目标点远时进行快速扩展,在局部障碍密集区域进行权重减小步长,做到安全高效地扩展节点。

(3)引入基于安全距离的碰撞检测策略,使生成的节点与障碍物之间有一定安全距离,保证生成路径的质量以及实际移动过程中的安全性。

(4)采用考虑安全距离的剪枝优化以及拐点三次B样条曲线拟合对初步生成路径进行处理,解决了路径曲折不光滑的问题,使最终路径符合移动机器人运动学特性。