机器人路径规划中快速扩展随机树算法的改进研究

2022-07-19王硕段蓉凯廖与禾

西安交通大学学报 2022年7期

王硕,段蓉凯,廖与禾

(1.西安交通大学现代设计及转子轴承系统教育部重点实验室,710049,西安;2.西安交通大学陕西省机械产品质量保障与诊断重点实验室,710049,西安)

随着社会和科学的不断进步,机器人技术得到了迅速的发展,智能机器人逐渐在医疗卫生、家庭服务、军事航天、工业生产等领域得到广泛的应用。与此同时,路径规划作为机器人技术中重要的研究领域,也开始迅速发展[1-4]。

路径规划的研究目的是在有障碍物的环境中,使机器人可通过算法自主规划出无碰撞的、从起点到目标点的可行路径,或满足某些约束条件的最优路径[5]。常用的算法有图搜索法[6]、栅格法[7]、人工势场法[8]、遗传算法[9]、蚁群算法[10]、RRT法[11]等。其中,RRT算法因具有泛用性好、适合移动机器人和多自由度工业机器人等特点而被广泛使用,但由于采样过程的随机性,基本RRT算法存在树的扩展随机性大、冗余节点多、容易在目标点周围发生振荡、规划的路径较长等问题。

为解决基本RRT算法所存在的问题,国内外学者进行了大量有针对性的研究工作。例如,文献[12]在基本RRT算法的基础上提出了双向随机搜索(从起点和终点同时扩展两颗树)的RRT-Connect算法,减少了路径规划的时间;文献[13]提出了引入贪婪策略的RRT-Connect算法,使树尽可能地向目标点方向生长,搜索路径更优;文献[14]提出了动态调整父节点从而对路线进行优化的RRT-star算法,缩短了规划路径的长度;文献[15]提出了Quick-RRT-star算法,扩大了RRT-star算法动态调整父节点的范围,对搜索路径进行了优化;文献[16]以一定的概率将目标点作为采样点,文献[17]引入了权重系数,增加了树向目标点方向的扩展分量,文献[18]引入了引力系数,使树在扩展时一直受到目标点方向的引力影响,这几种方法都能使树偏向目标点方向进行生长,从而减少了树扩展时的随机性。

这些研究在机器人路径规划方面进行了许多有益的探索,并取得了一定的进展。但目前对于树扩展随机性大的问题的改进研究大多只考虑了如何让树偏向目标点方向进行生长,而没有考虑障碍物的影响,因此造成树始终偏向目标点方向生长,无法根据障碍物信息灵活调整扩展方向并在障碍物附近产生大量无效节点的问题。此外,目前对于RRT算法容易在目标点附近发生振荡问题的研究也还存在不足。为进一步解决这些问题,本文提出了一种改进的RRT算法。首先,通过引入动态权重系数以提高树扩展时的灵活性;其次,针对扩展树容易在目标点附近产生振荡的问题,研究了扩展步长的自适应设置方法。最后,结合剪枝处理[19]和基于三次B样条曲线[20]的平滑处理完成了规划路径的优化。

1 RRT算法基本原理

基本RRT算法的扩展图如图1所示。

图1 基本RRT算法扩展图

该算法以起点为根节点,在状态空间中进行随机采样,通过特定的约束条件进行随机树的扩展。当随机树上的点到达目标点设定的邻域范围内时,即可找到一条从起点到目标点的唯一路径。

RRT算法的基本步骤如下。

(1)设在已知的空间环境中,有定义的起点xinit和目标点xgoal。

(2)在空间中随机生成一个点xrand并遍历已生成的扩展树,找到距离xrand最近的节点xnearest,沿着xrand向xnearest方向扩展一定的距离e得到新节点xnew,连接xnearest和xnew得到直线L;检测直线L是否和障碍物发生碰撞,如果未发生碰撞就将xnew加入树中。

(3)重复步骤(2)直到新节点到达目标点xgoal的邻域范围内,连接xnew和xgoal,从xnew开始依次寻找并连接父节点,即完成起点到目标点的路径规划。

基本的RRT算法存在以下缺点[21-23]。

(1)树扩展时随机性较大:由于随机采样点是根据均匀函数产生的,导致新节点的生成缺乏偏向性。

(2)冗余节点较多:树扩展时的随机性使得RRT算法会在非目标方向的区域生成大量冗余节点,从而产生不合理的规划路径。

(3)树容易在目标点周围发生振荡:为了提高树扩展的效率,树的扩展步长总是大于目标点邻域,这导致树扩展到目标点附近时容易发生振荡现象。

(4)路径平滑性不足:节点与节点之间通过直线连接,所规划路径的各段直线之间因此存在拐角,缺乏平滑性,难以满足机器人运动的平稳性要求。

2 改进RRT算法

基于上述对基本RRT算法和相关改进研究存在问题的分析,本文首先提出采用动态权重系数和自适应扩展步长的方式优化路径搜索过程,并在对初步规划后的路径进行剪枝处理以去除冗余节点后,利用平滑处理完成路径的最终规划,具体详述如下。

2.1 动态权重系数

基本RRT算法在进行树的扩展时没有考虑目标点的引导信息,因此生成了大量远离目标区域的无效节点,导致路径规划时具有较大的随机性。文献[17]提出了在目标点和随机点两个方向分别取固定权重系数p1和p2(p1和p2的最优值由仿真实验确定),再进行矢量合成确定扩展方向和扩展距离的方法。这种方法增加了树扩展时向目标点的偏向性,能够明显地降低路径规划的时间,但是p1总是取大于p2的固定值导致遇到障碍物时容易产生大量无效节点,这就增加了路径规划所需的迭代次数。针对这一问题,本文引入了动态权重系数,使树尽可能地在向目标点进行扩展的同时又能够即时地避开障碍物。为了提高算法对障碍物的敏感性,本文将目标点方向和随机点方向的权重系数取为p和1-p这两个相互制约的参数。不考虑权重系数的变化,引入权重系数后树的扩展示意图如图2所示。

图2 引入权重系数后树的扩展示意图

新节点xnew的坐标由以下公式决定

(1)

式中:p代表权重系数,00.5时,新节点将偏向目标点方向;当p<0.5时,新节点将偏向随机方向。

由于路径搜索开始前障碍物的信息未知,路径规划过程中p的取值如可根据障碍物的情况自适应地动态调整,对于路径规划而言是更合理的方式。文献[24]提出一种将人工势场法与RRT算法相结合的方法。该方法与动态权重系数法效果相似,只是由于需要在生成新节点时不断地更新引力和斥力这两个参数,计算效率并不理想。本文在上述工作的基础上,进一步提出一种p值动态调整的思路,即首先考虑目标点方向因素占主导的情况(p>0.5),使树偏向目标点进行扩展。如果得到的xnew与xnearest之间的连线L与障碍物没有发生碰撞,则将xnew加入扩展树。否则,考虑随机方向因素占主导的情况并调整p值使p<0.5,以便树能快速避开障碍物。如果新的xnew与xnearest之间的连线L与障碍物没有发生碰撞,则将xnew加入扩展树。依此进入下一轮迭代。这种动态调整p值的方式让树能够在偏向目标点扩展的同时,快速地避开障碍物。

2.2 自适应扩展步长

新节点xnew生成后会判断该节点和目标点xgoal之间的距离是否小于目标点的邻域λ,若满足条件就将xnew和xgoal相连。一般情况下,为了确保树扩展时的效率,扩展步长e总是大于λ,当xnearest和xgoal之间的距离为(λ,e)范围内的值时,如果树继续以步长e进行扩展,容易造成新的节点xnew在目标点周围振荡,陷入局部死循环,不仅增加路径规划的时间,而且使路径在目标点附近较为曲折,增加了路径的长度和路径复杂度。为解决这一问题,本文中e将根据扩展树与目标点之间的距离自适应地进行调整。记扩展步长的初始设定值为e1,当xnearest和xgoal之间的距离大于e1时,e=e1;当xnearest和xgoal之间距离为[λ,e]范围内的值时,e=e2=‖xgoal-xnearest‖。固定扩展步长和自适应扩展步长的扩展示意图如图3所示(示意图仅显示扩展步长的对比,未考虑动态权重系数)。

(a)固定扩展步长扩展示意图

2.3 路径剪枝和平滑处理

引入动态权重系数后树扩展的随机性大大减小,但是由于生成的节点众多,依然存在冗余节点。本文对初步路径规划得到的节点集合进行剪枝,从起点xinit开始尽量去除集合中不必要的节点。

剪枝处理的具体思路如下:对初步路径规划得到的节点集合,从起点xinit开始,依次遍历后续节点xk,如果xinit和xk之间的连线和障碍物之间没有发生碰撞,则将xinit和xk之间的所有节点都删除;如果xinit和xk之间的连线和障碍物之间发生了碰撞,则从节点xk-1开始重复以上步骤,直到遍历到目标点xgoal,最后将剩余的节点结合依次进行连接,得到新的路径。这种剪枝方式极大地减少了路径上的节点数量。

剪枝后路径的拐点位置仍然是分段的,不平滑。机器人按照这样的路径行走,会在拐点处因为行进方向变化产生冲击,影响机器人的运行平稳性。因此需要进一步对路径进行平滑处理。鉴于B样条曲线精度较高,能够较好地控制曲线的平滑度[25],且三次B样条拟合曲线的曲率半径较小,在拐点的过渡较为平滑,计算量也相对较少,本文选用三次B样条曲线对剪枝后的路径进行平滑处理。

K次B样条曲线可表示为

(2)

其中,di为曲线控制的顶点,即树的节点;Bi,K为K次B样条曲线的基函数。K次分段曲线由节点向量U=[u0,u1,…,un+K+1]决定。三次B样条曲线的基函数为

(3)

2.4 改进RRT算法流程

综合以上几种改进措施,改进的RRT算法流程图如图4所示。

图4 改进的RRT算法流程图

改进的RRT算法流程如下。

(1)在目标空间中,设定起点xinit和目标点xgoal,扩展步长e的初始值e1,目标点的邻域λ。

(2)生成随机采样点xrand,遍历扩展树找到最近节点xnearest,检测xnearest和xgoal之间的距离是否大于e1。

(3)如果步骤(2)条件满足,则在e1的基础上引入权重系数p1得到新节点xnew,否则将e的值设为新值e2,在e2的基础上引入权重系数p1得到新节点xnew。

(4)检测xnew和xnearest之间的连线L是否和障碍物发生碰撞,如果未发生碰撞,就将xnew加入扩展树,否则在当前e取值的基础上引入权重系数p2得到新节点xnew。

(5)检测xnew点和xnearest之间的连线L是否和障碍物发生碰撞,如果未发生碰撞,就将xnew加入扩展树,否则返回步骤(2)。

(6)检测xinit和xgoal之间的距离是否小于λ,如果满足条件,连接xnew和目标点,从xnew开始依次寻找并连接父节点得到起点到目标点的路径。

(7)对生成的初始路径进行剪枝处理和曲线拟合。

3 仿真验证

3.1 初始路径规划仿真

为了对改进的RRT算法的效果进行验证,在matlab中进行了二维空间环境下的仿真。设计的地图包含较多障碍物,以验证改进的算法能够快速地避开障碍物。环境场景的大小为800 mm×800 mm,设定起点坐标(50 mm,50 mm),目标点坐标(750 mm,750 mm),初始扩展步长e1=50 mm,目标点邻域λ=20 mm,分别使用RRT算法、RRT-star算法、固定权重系数的RRT算法(引入自适应扩展步长)和本文改进的RRT算法进行路径规划对比分析。为了确保树的偏向性,固定权重系数的RRT算法的权重系数p取值为0.8,本文改进的RRT算法的权重系数p取值为0.8(目标点方向因素占主导时)和0.2(随机方向因素占主导时)。黑色粗线代表规划出的路径,四种算法的规划结果如图5所示。

(a)基本RRT算法

表1为4种算法在较多障碍物环境下分别进行500次路径规划所得的平均数据。数据表明,在路径规划时间和路径长度这两个主要指标上,改进的RRT算法相较基本RRT算法和RRT-star算法都有明显的优势。改进的RRT算法和基本RRT算法相比,路径规划时间减少54.08%,路径长度减少19.56%;改进的RRT算法和RRT-star算法相比,路径长度的减小虽然不甚明显,但所需的路径规划时间大幅减少。改进的RRT算法相较使用固定权重系数的RRT算法,虽然路径长度略微增加了5.21%,但是路径规划时间大幅减少了60.31%,算法的改进是有效的。

表1 4种算法的路径规划数据

3.2 自适应扩展步长仿真

为进一步检验动态权重系数和自适应扩展步长的有效性,继续在保持仿真环境和其他参数不变的条件下,对仅引入自适应扩展步长的改进算法进行检验,仿真分析结果如图6所示。可以看到,基本RRT算法在目标点xgoal附近发生了振荡(图6(a)),而引入自适应扩展步长的RRT算法会根据xnearest和目标点之间的距离动态地调整扩展步长,有效地减少了振荡并迅速到达路径目标点(图6(b))。

(a)基本RRT算法

表2为基本RRT算法、仅引入自适应扩展步长的RRT算法和改进的RRT算法在较多障碍物环境下分别进行500次路径规划所得到的平均数据。数据表明,引入自适应扩展步长的RRT算法和基本RRT算法相比有明显提升,改进是有效的。引入自适应扩展步长的RRT算法和基本RRT算法相比,路径规划时间减少34.74%,路径长度减少2.08%。进一步的数据分析还表明,仅考虑自适应扩展步长的RRT算法在路径规划时间和路径长度这两个指标上与改进的RRT算法相比均仍有较大差距。因此,在路径规划中同时考虑动态权重系数和自适应扩展步长有其重要性。