基于改进RRT-Connect算法的全局路径规划*
2024-04-29朱建军王明森
朱建军,王明森
(吉林化工学院信息与控制工程学院,吉林 132022)
0 引言
智能化的要求越来越高,路径规划作为移动机器人不可或缺的一部分,也在快速的发展[1]。路径规划的功能是让机器人在存在障碍物的复杂环境中,在两点之间规划出一条安全、平滑的路径,同时还要尽可能满足计算量小、规划时间短、路径较优等要求[2]。路径规划算法主要由A*算法[3]、遗传算法[4]、蚁群算法[5]组成。这些算法不仅耗时长、计算量大,而且在面对大量复杂障碍物时也容易陷入局部陷阱。
LAVALLE[6]提出快速扩展随机树算法(rapid exploring random tree,RRT),该算法既存在具有泛用性强,计算量小、寻迹时间短等特点,也存在随机性大、收敛速度慢等问题。针对RRT算法存在的缺点,JR等[7]提出了RRT-Connect算法,该算法以双树的形式进行路径规划,减少算法的路径代价和规划时间;张顺等[8]提出使用人工势场法和RRT-Connect算法结合,以此加快搜索效率;CHEN等[9]提出在RRT-Connect算法中配置第3节点,增加了探索效率;张丹萌等[10]通过加入混沌序列来控制RRT-Connect算法的采样范围和采样随机性,减少了算法的收敛时间和无效采样;杜传胜等[11]在引入第3节点和引力RRT-Connect算法基础上提出同根双向连接,以此提升算法的收敛性、减少搜索时间。
本文基于RRT-Connect算法进行改进。首先,通过产生高质量随机点来增强RRT-Connect算法中两颗随机树的收敛性和探索性;其次,使用动态步长来加快连接速度,减少规划时间和迭代次数;然后,引入正向寻优和逆向贪婪算法进行轨迹优化;最后,采用3次B样条进行平滑处理,从而获得一条质量更高、路径更优的路径。
1 RRT-Connect算法
RRT算法是基于采样的路径规划算法,该算法以单棵随机树的形式生长[12]。RRT-Connect算法是由RRT算法改进而来的算法,该算法以双树的形式进行交替生长和直线连接,极大的减少了路径规划时间和规划路径的长度[13];RRT-Connect算法生长示意图如图1所示[14]。
图1 RRT-Connect生长示意图
步骤1:初始化状态空间、起点、终点和障碍物坐标信息,以起点和终点生成T1和T2;
步骤2:以T1中的Xnearest为根节点向Xrand延伸生成Xnew;
步骤3:以T2中的Xprim_nearst为根节点向Xnew不断延伸生成Xprim_new,直到Xprim_new不满足状态空间约束或者Xprim_new和Xnew连接。
2 改进RRT-Connect算法
相较于RRT,RRT-Connect算法虽然提升了规划效率,但是随机性较大、扩展性不足、拐点多和存在大量冗余节点等问题依然存在。本文针对RRT-Connect算法中存在的问题,将从引入高质量随机点、引入动态步长、使用正弦寻优和逆向贪婪结合算法进行轨迹优化、3次B样条进行平滑处理4个方面对RRT-Connect算法进行改进。
2.1 引入高质量随机点
为了增加RRT-Connect算法的探索性和导向性,本文引入高质量随机点。高质量随机点的产生公式为:
(1)
式中:XH_rand为产生的高质量随机点,Q为节点选择概率,P为随机概率,Xexp为探索性随机点,Xcon为连接性随机点。
2.1.1 探索性随机点的生成
若随机点到最近节点的最短欧氏距离大于当前的可生成范围[15],说明该随机点和随机树的距离足够大,该随机点具有高探索性。本文通过上述方式,判断该随机点是否生成输出。探索性随机点生成步骤:
步骤1:初始化R值,随后进入循环;
步骤2:产生随机点Xrand,并计算Xrand和树的最短欧氏距离S;
步骤3:将最短距离S和R进行比较。如果R值较大,则返回步骤2重新生成Xrand;否则将生成的Xrand输出。
R为随机点的可生成范围的半径,计算公式为:
R=G(n)*Acol_len
(2)
式中:Acol_len为碰距值,G(n)为无碰撞系数,其计算公式为:
(3)
式中:Gin为碰撞增值,Gde为碰撞降值,G为无碰撞系数的预设值。探索性随机点生成示意图如图2所示。
图2 探索性随机点生成示意图
2.1.2 连接性随机点
RRT-Connect算法以双树形式进行拓展。若两棵树的末端节点连接,则表示路径规划成功。故本文提出以两棵树各自的末端节点作为连接性随机点引导双树的连接,连接性随机点数据list的更新步骤为:
步骤1:生成新节点Xnew;
步骤2:在连接性随机点数据list中添加新节点Xnew。若Xnew的父节点Xnear在list中,删除父节点Xnear;
步骤3:对list的数据长度进行删减,使其维持在设定数量内。
2.2 动态步长探索
RRT-Connect算法两棵树彼此生长时,往往存在冗余节点较多、容易陷入局部最优的问题。针对上述问题,本文提出采用动态步长来代替固定步长进行生长延伸。动态步长生长示意图如图3所示。
图3 动态步长生长示意图
由图可知,当T1生成新节点Xnew之后,在T2根据欧氏距离寻找距离Xnew最近的父节点Xprim_new。随后以父节点与随机点的连线方向为T2的连接方向,L*为动态步长拓展T2树,L*的计算公式为:
L*=N*L
(4)
式中:L为设定步长,N为动态系数,由是否满足空间约束决定。N的表达式为:
(5)
引入高质量随机点和动态步长后的改进RRT-Connect算法流程图如表1所示。
表1 改进RRT-Connect算法
3 轨迹优化、平滑处理
改进RRT-Connect算法很大程度上优化了原算法探索性和导向性弱、冗余节点多的问题,但拐点较多、路径代价较大、不满足机器人运动学规律的问题依旧存在。针对上述问题,本文提出了一种正向寻优和逆向贪婪结合的算法来进行轨迹优化,并对优化后的路径使用3次B样条优化处理,以此得到拐点较少、路径代价较小且更加平滑的路径。
3.1 正向寻优算法:
正向寻优算法示意图如图4所示,Xstart为起点,Xgoal为终点,Xn为已知路径点。首先,连接Xstart和Xgoal,如果两点之间没有障碍物,则由两点之间直线最短的原则,将两点作为最终路径输出。若两点之间存在障碍物,则判断Xstart和终点前一个有效路径点X9之间是否存在障碍物,若两点之间不存在障碍物,则重复上述判断,直到得到到满足条件的X6。连接X6和Xstart,然后以X6作为起点重复上述操作,直到满足新的起点和终点可以直接连接,输出路径。
图4 正向寻优算法示意图
3.2 “分裂化”处理
由于改进RRT-Connect规划出的路径经过正向寻优之后变为几个关键位置的点,在其连线上没有点的存在,无法使用逆向贪婪算法优化。故使用逆向贪婪算法之前,需要对已有路径进行“分裂化”处理:将原有路径下的每两个点之间分裂为若干个点,让原本只存在几个关键点路径重新变成一条有众多冗余节点、可以使用逆向贪婪优化的路径。
3.3 逆向贪婪算法
使用逆向贪婪算法对“分裂化”处理的路径进行轨迹优化。节点处理后的路径如图5所示。
图5 逆向贪婪算法
3.4 B样条平滑处理
为了保证路径的平滑性,本文使用3次B样条曲线方程对轨迹进行平滑处理。为了保证平滑处理后的轨迹满足空间状态,需要在路径点周围产生临近节点。以路径点Xn前后节点Xn-1和Xn+1为方向,step为步长产生新的临近节点,临近节点如图6中灰色节点所示。
图6 产生临近节点
3次B样条路径点计算公式为:
P(t)=P0G0,3(t)+P1G1,3(t)+P2G2,3(t)+P3G3,3(t)
(6)
式中:P(t)为路径点,P0、P1、P2、P3为控制曲线的特征点,特征点的基函数Gi,n(t)在3次B样条中的公式为:
(7)
式中:t为时间参数,取值为t∈[0,1]。
4 仿真校验
为了验证算法的可行性,本文使用PyCharm平台分别在少障碍物环境、狭窄通道环境、多障碍物环境和H型障碍物环境中对RRT-Connect算法和本文提出的改进RRT-Connect算法进行仿真对比实验。地图大小均为50*30,其中少障碍物环境、多障碍物环境和狭窄通道环境的起点坐标为(2,2),终点坐标为(49,24);H型陷阱环境的起点为(15,15),终点坐标为(30,15),起点和终点分别使用灰色点和黑色点表示,节点扩展步长为0.8,黑色方块和黑色圆形代表障碍物和边界。灰色线条代表生成的随机树,黑色线条代表规划出的最终路径。为了减小算法本身存在的随机性较大的问题,每次实验的结果为20次实验数据的平均值。
4.1 实验结果与分析
本文使用RRT-Connect算法和改进RRT-Connect算法分别在少障碍物环境、狭窄通道环境、多障碍物环境和H型障碍物环境中进行仿真实验,仿真结果如图7～图10所示,其中图7a～图10a为RRT-Connect算法的规划结果,图7b～图10b为改进RRT-Connect算法的规划结果。
(a) RRT-Connect算法 (b) 改进RRT-Connect算法
(a) RRT-Connect算法 (b) 改进RRT-Connect算法
由图7可知,RRT-Connect算法在少障碍物环境需要产生大量节点来探索路径,而改进RRT-Connect算法通过探索性随机点增强了对未知区域的探索,减少了迭代次数和冗余节点数量。
由图8可知,RRT-Connect算法在狭窄通道环境中存在盲目搜索的问题,而改进RRT-Connect算法通过引入连接性节点,增加了算法的导向性,减少了算法的随机性和规划时间。
由图9可知,RRT-Connect算法在多障碍物环境存在随机性较大的问题,而改进RRT-Connect算法通过引入高质量随机点,增加算法的探索性和导向性,提升了算法的规划效率。
(a) RRT-Connect算法 (b) 改进RRT-Connect算法
由图10可知,RRT-Connect算法在H型陷阱障碍物中存在容易陷入局部陷阱的问题,而改进RRT-Connect算法通过引入高质量随机点和动态步长,改善了算法容易陷入局部陷阱的问题,减少冗余节点的数量,提升了算法的规划质量。
(a) RRT-Connect算法 (b) 改进RRT-Connect算法
4.2 算法数据分析
使用两种算法在4种环境下进行20次实验,每次试验为20次实验的平均值,共400次实验。在实验后对规划时间、路径长度和节点个数进行整理统计。400次实验的数据结果如表2所示。每20次路径规划的平均时间、平均路径长度、平均节点个数的单位分别为秒、米、个。
表2 不同环境下算法数据对比
由表2可知,相比于RRT-Connect算法,改进RRT-Connect算法在少障碍物环境、狭窄通道环境、多障碍物环境和H型障碍物环境中进行400次实验后的平均时间分别减少了28.57%、35.64%、16.41%、25.01%;平均路径长度分别减少了21.65%、15.09%、14.02%、25.45%;平均节点个数分别减少了42.19%、51.07%、29.26%、45.1%。由上述数据可知,在不同环境中,改进RRT-Connect算法相比于RRT-Connect算法规划效率更高、路径代价更小、冗余节点数量更少。
5 结论
本文在RRT-Connect算法的基础上,提出了一种基于高质量随机点的动态步长改进RRT-Connect算法,改进算法在基础RRT-Connect的基础上通过生成高质量随机点,进而引导随机树的生成、同时引入动态步长来减少冗余节点的数量,随后使用正向寻优和逆向贪婪对轨迹进行优化,最后使用3次B样条进行平滑处理。通过两种算法在4种不同环境下的仿真实验,验证了改进算法相比于基础RRT-Connect效率更高、路径质量更优,从而表明了改进算法的可行性。未来的研究中会考虑在实际运行中加入局部路径规划,以此来提高算法的实用性。
