基于改进的RRT*-connect算法机械臂路径规划

刘建宇，范平清

上海工程技术大学 机械与汽车工程学院，上海 201620

随着时代的飞速发展，高度自主化的机器人在人类社会中的地位与作用越来越大。而机械臂作为机器人的一个最主要操作部件，其运动规划问题，例如准确抓取物体，在运动中躲避障碍物等，是现在研究的热点，对其运动规划的不断深入研究是非常必要的。

机械臂的运动规划主要在高维空间中进行。RRT（Rapidly-exploring Random Tree）算法[1]基于随机采样的规划方式，无需对构型空间的障碍物进行精确描述，同时不需要预处理，因此在高维空间被广为使用。

近些年人们对于RRT 算法的研究很多，2000 年Kuffner 等提出 RRT-connect 算法[2]，通过在起点与终点同时生成两棵随机树，加快了算法的收敛速度，但存在搜索路径步长较长的情况。2002 年Bruce 等提出了ERRT（Extend RRT）算法[3]。2006年Ferguson等提出DRRT（Dynamic RRT）算法[4]。2011 年 Karaman 和 Frazzoli 提出改进的RRT*算法[5]，在继承传统RRT 算法概率完备性的基础上，同时具备了渐进最优性，保证路径较优，但是会增加搜索时间。2012 年Islam 等提出快速收敛的RRT*-smart 算法[6]，利用智能采样和路径优化来迫近最优解，但是路径采样点较少，使得路径棱角较大，不利于实际运用。2013 年Jordan 等通过将RRT*算法进行双向搜索，提出B-RRT*算法[7]，加快了搜索速度。同年Salzman 等提出在下界树LBT-RRT 中连续插值的渐进优化算法[8]。2015 年Qureshi 等提出在B-RRT*算法中插入智能函数提高搜索速度的IB-RRT*算法[9]。同年Klemm等结合RRT*的渐进最优和RRT-connect的双向搜索，提出使搜索路径朝理论最优解收敛的RRT*-connect算法[10]。2016年王道威等提出动态步长的RRT算法[11]，但是只考虑简单障碍物环境。2017 年朱宏辉等提出加入规避步长延伸法的改进RRT*算法[12]，虽然防止陷入局部最小解，但是对于复杂狭小路径，不容易获得较优解。2018 年陈波芝等人提出用于双机械臂避障的改进RRT算法[13]，但只考虑了球形障碍物环境。2019年王坤等提出在B-RRT*中加入采样函数，并结合快速扩展策略使搜索速度加快的EB-RRT*算法[14]。

本文基于上述研究成果，针对RRT*-connect算法的采样速度较慢以及产生路径较粗糙的问题，首先在目标偏向采样的基础上加入约束条件，保证每次采样趋近目标点，减少不必要区域的搜索，使采样更加高效。然后采用梯度下降法对搜索形成的路径做平滑处理，对比分析改进前后的RRT*-connect算法，证明经过优化处理的路径更平滑。最后在ROS 平台进行仿真，验证该算法更适合实际机器人操作，路径更短且用时更少。

1 RRT＊-connect算法

2015 年 Klemm 提出了 RRT*-connect 算法，该算法利用RRT*算法的渐进最优思想，在传统的RRT 基础上进行优化，使得搜索出的路径是渐进最优的。

如图1所示，qinit为路径的起始点，向目标点qgoal搜索，qrand是在自由空间得到的随机采样点，qnearest是随机树上找到离qrand最近的节点，qnew为qnearest向qrand延伸一定距离l得到的下一个拓展点。利用算法的渐进最优特性，为qnew重新选择父节点。

图1 自由空间采样

如图2（a）所示，首先以qnew为圆心，以一定半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合Qnear。其次对路径进行代价判断，将从起始点经过潜在父节点到qnew的路径长度与经过原qnearest节点的长度比较，不断对比筛选出路径代价最小的节点qnew_parent。然后将路径添加进随机树并删除原qnearest与qnew连接路径，如图2（b）所示。同时该算法利用RRT-connect算法的贪婪搜索来实现双向拓展。

图2 父节点重构

如图3 所示，从起点qinit向目标点qgoal搜索的过程中，目标点qgoal也在向起始点qinit搜索。在两棵随机树空间V1、V2中，qopt为起始点随机树V1中的一个拓展点，V2随机树将设法拓展到qopt节点。qopt节点利用路径代价函数进行选择，如式（1）所示：

通过代价判断得到最优节点qopt，将两棵随机树连接，路径搜索完成。

图3 两棵随机树连接过程

2 改进RRT＊-connect算法

2.1 采样约束

上述RRT*-connect算法利用双向随机树搜索，加快搜索速度。但是在搜索过程中存在采样点的随机性较大和采样区域太分散的问题，容易造成搜索效率低和路径较粗糙。

针对这个问题，本文将目标偏向思想[15]加入到RRT*-connect中，目标偏向思想是指在采样过程中首先设定一个基准值Pstandard，然后随机树每次在自由空间中采样，按均匀概率分配，随机产生一个概率值P。如果P小于原先设定的基准值，则采样点选择为目标点：

然后本文在P大于基准值时，对随机树搜索的随机点进行控制（此处仅以起始点拓展随机树为例，目标点拓展随机树的设置相同），如图4所示，利用采样约束策略，具体设置如下。

图4 采样设置示意图

采样约束设置：首先将起始点与目标点进行连接，将两者之间的区域定义为全局的采样区域Vq，即图4黄色区域。然后当P大于基准值时，开始在空间进行随机采样，并对采样点的位置进行判断，保证每一次的采样要比前一次更接近终点，否则重新采样。即通过函数约束，使每次的采样点都在采样区间Vq内，并且每次的采样都在前一次采样点的一定角度内，左右角度θ≤900。如图4中的qrand2为第二次随机采样点，位于第一次采样点qrand1的上方θ角度内，qrand3位于qrand2的下方θ角度内，如下所示：

采样约束设置确保每次采样趋于目标点，有效防止随机采样点的反向搜索，从而保证采样的方向性，减少资源浪费。同时在约束环境下不限制采样点在纵向空间的随机选择，保留了采样的随机性，确保遇到障碍物时，能够高效搜索出可行路径。

最后在加入采样约束后，整个改进的RRT*-connect算法流程如图5所示。

图5 优化算法流程图

改进的RRT*-connect算法，首先从起始点和目标点初始化两棵随机树，然后开始随机获取概率值并进行判断。如果小于基准值，则将目标点设置为采样点，否则在约束区域进行采样，将经过约束设置的采样点加入随机树。最后利用算法的渐进最优特性重新选择父节点，重构随机树，不断重复操作，直到某一节点同时存在两随机树上，则路径完成。

2.2 平滑处理

上文将RRT*-connect算法的采样点进行约束设置，明确采样方向性，从而节约搜索资源，使得路径更优且用时更短。但依旧存在规划出的路径较陡，大角度弯折较多的问题。针对这个问题，采用梯度下降法对搜索出的路径进行优化，使其在实际运用中更加合理，进而增强在实际运行中的可操作性。

梯度下降法常应用在A*、D*算法上，用于将搜索出的路径做平滑处理，其优点为计算量较少，能够最小化所有样本的损失函数值，并且使结果是全局最优解或者在最优解附近。梯度下降法是一种依靠迭代求解最小值的算法，主要是通过误差最小化求解出最佳数据，从而可以达到拟合曲线的目的。

将路径看成曲线并用函数表示，如式（2）所示：

式中，xi为处理数据中的特征值，θi为权重值。

引入假设函数，使用一组特征值代入式（2），由于权重值θi未知，令得到结果为y′，如式（3）所示：

引入损失函数概念，即式（3）得到的y′与真正的y值间存在误差。损失函数ΔT(θ)如式（4）所示：

此差值为方差，损失ΔT(θ)越小意味两者越接近，故ΔT(θ)越小越好。针对路径存在数据较多的问题，将采用均方差科学表示损失值，如式（5）所示：式中汇总所有样本数据，J(θ)为均方损失差。

梯度下降法利用微分思想求解，将θ取值范围划分多份，每份宽度为动态∂，实际宽度由微分步长α决定，通过改变θ的值直至损失值趋于0。

根据微分公式变形得到的迭代公式如式（6）所示：

迭代使得每次损失最小，对于路径而言只需控制权重值θi的大小和迭代次数，使得目标函数取最小值，即可以对路径进行平滑处理，如图6所示。

图6 平滑处理对比

图6红色圈划线为原始曲线，黑色虚线为经过梯度下降法平滑处理后的曲线，可以明显看出处理后的曲线弱化了原路径的棱角，同时能够控制平滑处理的程度，保证路径的可靠性。

3 仿真结果对比分析

3.1 规划时间与路径长度分析

本文提出RRT*-connect 改进算法并运用仿真软件对比原规划算法。首先选择两幅复杂程度较高的地图进行测试，如图7，地图范围为500×500，地图中的黑色为障碍物，起点坐标（0，0），目标坐标（500，500），红色部分为搜索形成的随机树，蓝色部分为规划出的路径。然后对比分析两者搜索时间与路径长度上的差异，来验证改进算法的性能。

通过两个算法在不同地图中的对比，可以很明显看出改进后RRT*-connect算法在复杂环境下迭代次数更少，同时减少无效区域搜索，使得搜索更加高效，目的性更强。

经过20 次重复实验，将路径搜索时间和路径长度汇总成表格。

图8为两幅测试图20次实验的搜索时间对比，可以很明显看出，改进后RRT*-connect算法在不同的地图环境下，搜索时间都有较大程度的缩小，并且更加稳定，不存在原RRT*-connect 算法搜索时间随机性大的问题。测试环境1 中原算法20 次搜索的平均耗时2.88 s，改进RRT*-connect算法平均耗时为1.70 s，时间缩短40.97%。测试环境2 中原算法20 次搜索的平均耗时4.35 s，改进RRT*-connect 算法平均耗时2.68 s，时间缩短38.39%。通过数据可以清晰看出，改进后的RRT*-connect算法路径搜索时间缩短40%左右，减少搜索资源的浪费。

图7 算法测试对比

图8 两种算法时间数据对比图

图9为两幅测试图20次实验的路径长度对比，可以看出改进后的RRT*-connect算法路径长度普遍更短，路径更优，并且多次搜索的路径相比原算法更加稳定。测试环境1 中原算法20 次的平均路径长度为858.41 mm，改进RRT*-connect 算法平均路径长度为758.29 mm，路径缩短11.66%。地图2中原算法20次的平均路径长度860.38 mm，改进RRT*-connect 算法平均路径长度为780.36 mm，路径缩短9.30%。通过数据可以清晰看出，改进后的RRT*-connect 算法搜索得出的路径长度减少10%左右，从而表明改进后算法搜索出的路径更优。

图9 两种算法路径长度数据对比图

综上，本文通过设置采样约束，使改进后的路径在不同复杂环境下都更具目的性和方向性，且比原RRT*-connect 算法搜索更加精确，路径长度更短，耗时更少。但在实际的运行操作中还是存在路径较粗糙的问题，因此需要进行平滑处理。

3.2 平滑处理效果分析

针对上述问题，采用梯度下降法对路径进行优化处理，如图10 所示，分别为四种不同障碍物地形，设置如上文所述，黑色路径为改进后RRT*-connect 算法路径，蓝色路径为经过平滑处理后的路径。

图10 四种地图下平滑处理

通过图10 可以明显看出，在不同地图环境下经过平滑处理，路径大角度棱角明显减少，更加平坦，且路径长度也明显缩短。具体数据见表1。

表1 不同地图下路径长度数据

通过表1 可以明显看出，在不同的障碍物环境下，经过平滑处理，路径在原有的基础上进一步改善，路线长度有明显减少，平均减少4%左右，路径趋于最优。证明本优化处理是十分有效且必要的。

3.3 仿真测试

本文改进的RRT*-connect算法在ROS开发平台下，通过使用UR5 机械臂在moveit 环境下进行运动规划测试。如图11所示，图中绿色柱体为障碍物，将改进算法作为插件加载进moveit 进行测试，指定其由平置的down 位姿，如图11（a）所示，绕过障碍物到达上方指定的up位姿，如图11（b）所示。

图11 机械臂位姿

利用ROS平台，可以直观看出算法的规划路线，并对其进行分析，如图12所示。

图12 机械臂运动轨迹

通过图12 可以明显看出，该算法很好地绕过障碍物，且路径较优，证明该改进算法的实用性与有效性。

4 结论

本文将RRT 的变种算法RRT*-connect 算法加以改进，首先通过引入目标偏向思想，对随机采样点加以控制，使得全局的采样点趋向目标点进行采样，确保采样更加精确，避免了不必要区域的搜索，大大节省搜索时间。同时通过概率偏置，防止出现局部最小解，使得路径渐进更优。其次通过仿真软件在多种复杂地图环境下进行测试，将改进的RRT*-connect 算法与原算法相比，搜索时间减少40%左右，路径长度缩短10%左右，从而表明路径更优，搜索速度更快。然后再利用梯度下降法对路径进行平滑处理，减少路径的大角度棱角，同时路径长度减少4%左右。最后在ROS平台，使用UR5机械臂在多个障碍物的仿真环境下进行了无碰撞运动规划。综上得出结论，本文改进的RRT*-connect 算法搜索时间相比原RRT*-connect 算法大幅度减少，同时路径长度更短，趋于最优解。仿真实验表明改进算法更加适合机械臂的实际运行与操作，进而证明该算法的可行性与实用性。