基于改进RRT算法的6-DOF机器人路径规划

李 季 史晨发 邵 磊 刘宏利

(天津理工大学 天津 300384)

0 引 言

工业机器人较高的稳定性和工作效率在自动化生产中逐渐替代人工操作。随着多自由度机器人应用场景逐渐多样化，对工业机器人的运动规划和运动控制提出了更加智能化、高精度等要求。路径规划是工业机器人的研究重点，传统的局部规划算法并不能适应现阶段多自由度机器人MIMO高冗余、高耦合性复杂系统。全局规划算法是在自由空间内随机分布采样点，对采样点进行筛选判断后，将有效节点连接最终生成路径，将其应用在多自由度机器人路径规划中，提高了路径搜索成功率且降低6-DOF机器人资源损耗。

RRT算法是一种全局规划算法，具有概率完备性，针对RRT算法在机器人路径规划中的缺点，国内外学者对其进行改进。文献[1]利用双向扩展树，将启发函数引入至随机样本中，提高了算法收敛速度。文献[2]通过罚函数法建立以机械臂运动平稳性和机械臂末端最优性为代价适用度函数，使改进RRT算法应用于多自由度机器人在关节空间内路径规划。文献[3]提出了一种基于改进势场法的动态避障规划算法，并在离线、在线这两种状态中仿真验证。文献[4]基于光流的动态障碍物检测并结合人工势场理论完成机器人避障的方法，能实现障碍物精准定位及避障，但其对图像实时处理将耗费较多资源。本文利用RRT全局路径规划算法，结合局部路径优化，从随机采样点引入权重系数、设置动态距离增量、路径缩短、拐角平滑处理等多个方面提高路径搜寻成功率，加快收敛速度，在最短时间内生成连续平滑的最优路径[5]。

1 基本算法原理

1.1 械臂运动学模型建立

6-DOF工业机械臂是具有多自由度的开链式连杆机构，起始端与固定底座相连接，末端是机械手，中间由若干关节和连杆构成。机械臂的每个关节有自由旋转度与独立位置，需要借助数学模型建立机械臂末端在世界坐标系中坐标与机械臂关节角度的联系[6]。

本文中6-DOF机器人的连杆的数学模型采用Denavit-Hartenberg参数。先在固定底座上建立一个基础三维坐标系，再对机械臂的每个关节上的中心节点建立相对应坐标系，相邻关节上的坐标系之间的平移矢量和旋转矢量通过齐次变换方法求出。6-DOF机器人DH连杆坐标系如图1所示。

图1 6-DOF机器人DH连杆坐标系

4个DH参数的含义分别为：θi表示关节角，di表示连杆偏移量，ai表示连杆长度，αi表示连杆扭转角。这4个参数确定了相邻连杆间的状态空间描述，根据六自由度机械臂的运动学正解和逆解，能将机械臂所在自由空间内的世界坐标与机械臂的各关节相对运动量、旋转度用矩阵函数实现相互转换[7]。

1.2 机械臂障碍物检测

机械臂避障检测要先对障碍物定位，由于障碍物形状各异，若将每个障碍物进行精准建模，则将耗费大量计算时间，所以碰撞检测的核心思想就是将障碍物抽象化处理。

根据6-DOF的机械臂固有属性，将5个连杆简化成不同半径的圆柱体，文献[8]采用的是长方体包络检测法，这种方法会造成一些可规划的空间缺失，从而造成生成节点与最优节点偏差过大。同时选用成像法能精确描述障碍物信息，但其工作量大，实时性较差。所以本文将复杂障碍物经抽象处理后简化成球体和长方体进行预处理。

首先对球体障碍物提出基于包围球(Sphere)的碰撞检测方法，将障碍物碰撞检测问题转换成机械臂连杆中心线段至障碍物球心距离判断。球体障碍物在空间内做旋转运动时，若球心位置保持不变，此碰撞检测方法依然适用，这将节省一部分系统计算时间。

对长方体障碍物碰撞检测采用AABB包围盒碰撞检测算法[9]，该碰撞检测算法的核心是根据障碍物的6个平面与6-DOF工业机械臂连杆空间位置所对应关系进行计算，具体分为以下三类：机械臂连杆线段与障碍物平面相交、平行、异面。计算分析连杆线段和障碍物6个面的线面关系是否在所设定的安全距离区间内来判断机械臂是否发生碰撞。

2 传统RRT算法

2.1 原理简介

快速探索随机树(RRT)是一种通过随机构建空间填充树来有效搜索非凸高维空间的算法。从搜索空间中的生成随机样本来逐步构建树，并且趋向于大多数未检测区域生长。障碍物和差异约束的问题可以应用数学模型进行量化处理，广泛用于机器人运动规划中[10]，传统RRT算法扩展图如图2所示。

图2 传统RRT算法扩展图

传统RRT算法流程如下：

(1) 先对自由空间边界值限定，在拓扑空间X内会根据均匀随机分布函数生成随机节点Xrand。

(2) 遍历已生成的随机树上的节点寻找到与该随机点Xrand距离最近的节点Xnearest，沿着Xnearest至Xrand方向以固定距离增量步长eta生成新的节点Xnew。

(3) 利用碰撞检测函数筛选有效节点，在Xnew至Xnearest线段上插入若干点，依次检测每个点与障碍物的距离是否大于预设值。

(4) 循环以上新节点构建过程直至新节点到达目标节点Xgoal，最后依次搜寻并连接父节点构建出Xint到Xgoal路径。

2.2 优缺点分析

RRT算法是全局规划算法，具有概率完备性，理论上迭代次数趋于无穷时肯定能搜寻到路径。在避障过程中只需确定障碍物具体坐标信息和抽象化处理后的位姿形态，随机生成的节点根据碰撞检测函数指导随机树的扩展，和局部路径规划相比，系统不易陷入局部最小解。然而，传统RRT算法在6-DOF机器人路径规划中存在以下缺点：

(1) 随机性大。新节点是根据均匀随机分布函数生成的，缺少目标方向的引导，具有较大盲目性，迭代次数相差较大，在本文设定的迭代次数1 000内找寻路径成功率较低。

(2) 冗余节点多。根据搜寻父节点构建的路径中会有大量冗余点，传统RRT算法生成的节点并没有考虑路径成本，路径距离较长。

(3) 路径抖动大。在一些转折点处，两段相邻的线段转折角较大，对于机械臂关节会造成损伤同时也会造成抖动现象，影响系统精度。

3 改进RRT算法

3.1 算法流程

本文改进RRT算法，优先保证系统在设定迭代次数内的成功率，在新节点生成之前先判断与目标点的距离并配合不同距离增量，使路径能减小振荡，有效逃离局部最小值。同时在生成初始路径上再对节点进行二次处理，筛选距离最优节点，舍弃冗余节点，最后再对筛选过的节点利用平滑处理函数在与障碍物保持安全域量的前提下调整路径平滑程度。改进RRT算法流程图如图3所示。

图3 改进RRT算法流程图

3.2 引入权重系数

传统RRT算法随机采样点没有加入目标节点的引导信息，生成扩展树过程中大量节点都是无效节点，所以在搜寻路径时随机性较大。本文对随机分布节点引入权重系数，使之向目标节点逐渐趋近，使整体扩展树生长方向由传统随机发散式转变为目标偏置式扩展树[11]。引入权重系数节点扩展示意图如图4所示。

图4 引入权重系数节点扩展示意图

在随机函数的基础上引入权重系数，沿着Xnearest至Xrand和Xnearest至Xgoal两个方向分别取P1、P2权重系数，共同作用下生成全新节点。

(1)

当P2大于P1时，全新节点向Xgoal方向偏置；当P1大于P2时，全新节点更趋向于随机方向。王兆光[15]利用启发式的思想，在随机节点的扩展过程中引入两个参数，一个是目标偏置值bias，一个是随机系数p。同时为了避免目标偏置值大小设置不合理会造成陷入局部最小解，引入Extend函数，先将随机生成的节点Xrand与Xgoal连接，判断是否与障碍物发生碰撞，当存在障碍物时将目标偏置值bias设置为0，这样能有效地逃离局部最小，但此时新节点随机性较大，且步长不确定性会造成目标节点周围的振荡效果。谢龙[3]将改进人工势场法应用于RRT中的全新节点，启发思想和上述方法相似，引力和斥力这两个参数需在每个新节点生成时不断配置，这一部分将耗费大量时间。

本文与上述研究不同之处是将每一个随机节点都引入一个目标偏置的合力方向，且P1和P2在设定数值时需要根据寻找路径成功率、迭代次数、耗费时间等多方面综合考虑。当权重系数配置完成后配合动态距离增量，每次全新节点的生成计算量小，可以实现路径规划时间最短的指标。

3.3 动态距离增量

在新节点Xnew生成时会先判断是否到达目标节点Xgoal所设定的领域范围内。当Xnearest不在Xgoal邻域时，固定距离增量步长设置为eta1，当Xnearest进入Xgoal邻域，设定领域半径为R，若步长还是eta1则会造成Xnew在目标点附近发生振荡，容易使路径陷入局部死循环，所以需要将步长设置成eta2，且保证eta2

图5 动态距离增量示意图

3.4 路径缩短处理

在对随机节点引入权重系数后大大降低了随机性，生成的初始路径是由多个离散点组成的连续线段，其中包含较多的冗余点造成与最短路径相差较大，所以采用路径缩短处理将这些冗余点剔除[13]。路径缩短处理示意图如图6所示。

图6 路径缩短处理示意图

具体操作步骤如下：

(1) 经过权重函数处理过的随机节点到达目标点Xgoal时，从Xgoal点向前依次寻找所对应的父节点。

(2) 将其每个坐标数据信息顺序编号，从起始点Xint依次向下一个节点连线并再次进行障碍物检测。

(3) 当所选节点Xm和起始节点连线和障碍物发生碰撞，选取这个节点前一个节点Xm-1作为一个全新扩展树节点，并将Xint至Xm-1中所有节点剔除。

(4) 从Xm-1开始依次向后对每个节点重连判断直至到达目标点。

路径缩短处理示意图中，通过重新连线再进行碰撞检测后，只选取1、5、8、11，路径整体距离有效缩短。

3.5 路径平滑处理

RRT算法在经过以上路径缩短操作后的扩展树上节点之间会包含较多转折点，相邻节点拐角过大会造成路径成锯齿状，系统状态转换过快会降低机械臂运行的平稳性，还会引起电机的冲击和振动，加速机械零件的磨损，甚至对机械臂造成损坏。目前国内学者选用七次非均匀B样条插值法，虽然精度较高，急动度连续且光滑，规划的速度、加速度值较小[14]，但是计算量较大。本文算法基于时间最短的指标，在两段路径衔接点不连续处根据机械臂曲率约束进行路径平滑处理[14]，通过迭代操作使拐角处节点偏离原始位置，并不需要通过插值法添加新的节点，下面描述具体操作步骤。

假设经过权重系数引导新节点和路径缩短处理后扩展树中节点序列为[M1,M2,…,Mn]，对其路径平滑处理后的节点序列为[N1,N2,…,Nn]，路径平滑处理本质上是对扩展树上节点处理后满足以下这两种距离趋向最小解：

(1) 保证平滑处理后的Ni与原节点Mi偏离程度最小，即求解N(i)使‖M(i)-N(i)‖的平方值最小；(2) 保证平滑处理后的节点N(i)与N(i+1)之间距离取值最小，即N(i)满足‖N(i)-N(i+1)‖最小。

在曲线平滑函数中，采用梯度下降法，即通过设定迭代次数求解出最优解，不断调整N(i)，使目标函数取值最小化。先将扩展树节点初始化操作N(i)=M(i)，设定迭代次数为10次。

N(i)=N(i)+α×[M(i)-N(i)]+

β×[N(i-1)-2×N(i)+N(i+1)]

(2)

通过调整公式中α和β值大小控制路径的平滑程度，最后进行迭代操作。通过以上平滑处理后，满足约束条件，序列由[M1,M2,…,Mn]变为[N1,N2,…,Nn]，路径平滑处理示意图如图7所示。

图7 路径平滑处理示意图

4 仿真实验

在MATLAB仿真中，对机械臂在二维空间中进行路径轨迹，障碍物分别选取圆、长方形模拟障碍物在平面上的投影，起始点和目标点参数设置为(5,5)和(95,95)，扩展树有效节点用黑点表示。权重系数P1、P2都设置为0时，全新随机节点并没有引入启发信息，传统RRT算法仿真图如图8所示。

图8 传统RRT算法仿真图

可以看出，未经过优化处理的路径多次规划出来的路径完全不一样，迭代次数相差较大，冗余节点较多耗费较多时间计算且拐点多，路径不平滑、方向变化大。

在改进RRT算法中，先对随机生成节点引入权重系数，P1、P2这两系数的确定对路径规划的效率有较大影响，在仿真实验中将迭代次数设置为1 000次，规定次数内未到达目标点邻域则算是路径规划失败。分别选取不同权重系数P1、P2和变化步长eta1，每组数据样本经过100次仿真实验进行统计，测试结果如表1所示。

表1 测试结果

续表1

可以看出，P2大于P1合力方向整体偏向于目标节点，当P2大于P130%时，所设定迭代次数内成功率为100%，平均时间为0.9 s左右。当P2大于P150%时，所设定迭代次数内成功率为98%，平均时间为0.8 s左右。当P2大于P160%时，所设定迭代次数内成功率为95%，平均时间为1.2 s左右。

在权衡成功率和耗费时间后，Xnearest至Xgoal方向的权重系数P2取3，Xnearest至Xrand方向的权重系数P1取2，固定距离增量eta1=1，eta2=0.3，整体路径规划成功率和快速性达到最优状态，将参数配置后的仿真图如图9所示。

图9 引入权重系数仿真图

在路径缩短处理中，将引入权重系数后的路径中所有节点进行重连、判断、筛选后，去除冗余节点，能将路径距离有效缩短，同时也让机械臂关节转动次数降低，保证末端机械臂的平稳性。路径缩短过程是在已经生成的路径上进行二次优化[15]，所以这一部分将耗费一些计算时间，但综合考虑路径整体距离有效缩短后，这些计算资源的损耗对整个系统是有意义的。

在最短路径的基础上，两段路径衔接点不连续处根据机械臂曲率约束进行路径平滑处理。在转折点处将原有节点利用圆滑处理函数进行重新标定，前提条件是调整α和β值的大小，保证拐角处平滑处理后的曲线和障碍物之间存在着安全域量。经路径缩短和平滑处理后的最终优化路径仿真如图10所示。

图10 最终优化路径仿真图

在标定参数后，对本文算法在二维空间状态内的路径规划进行100次仿真实验，与传统RRT算法相比，在规定迭代次数1 000次以内，本文算法成功率为100%。将本文算法与RRT-star算法比较，RRT-star的核心也是在优化最优节点上，在此引入了贪婪思想，采用代价函数来选取拓展节点领域内最小代价的节点为父节点，同时，每次迭代后都会重新连接现有树上的节点，从而保证计算的复杂度和渐进最优解。RRT-star最终路径长度和本文算法相近，但是迭代次数较多，耗费时间过长。仿真结果表明，本文算法极大降低了传统RRT算法的随机性，整体路径里程缩短，且经处理后的路径斜率连续，拐角处平滑过渡使机械臂能更稳定地运行。算法比较结果如表2所示。

表2 算法比较结果

5 结 语

本文针对传统RRT算法的缺点提出了一些改进方法，首先建立6-DOF机械臂运动型模型，对机械臂连杆、障碍物抽象化处理，有效减少计算量。在RRT随机节点生成时引入权重系数，并配合不同距离增量减小趋近于目标节点的振荡，同时剔除初始路径中的冗余节点后将路径距离有效缩短，拐角平滑处理后减少了机械臂抖动。仿真表明，改进RRT算法降低了随机性，平均规划时间缩短75%，路径距离缩短25%，该算法在6-DOF机械臂全局路径规划中具有较好的应用前景。