随着时代的飞速发展，高度自主化的机器人在人类社会中的地位与作用越来越大。而机械臂作为机器人的一个最主要操作部件，其运动规划问题，例如准确抓取物体，在运动中躲避障碍物等，是现在研究的热点，对其运动规划的不断深入研究是非常必要的。
机械臂的运动规划主要在高维空间中进行。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算法机械臂路径规划
刘建宇，范平清
上海工程技术大学机械与汽车工程学院，上海201620
摘要：基于双向渐进最优的RRT*-connect算法，对高维的机械臂运动规划进行分析，从而使规划过程中的搜索路径更短，效率更高。将目标偏向策略引入采样过程，同时对采样点区域进行约束，保证每次采样都能朝着目标方向搜索，使得搜索路径更优。在此基础上，采用梯度下降法优化搜索出的路径，将整个路径做平滑处理，去除大角度转弯。利用Matlab对改进后的RRT*-connect算法进行仿真对比分析，从而证明该算法在各种复杂环境下都能保证搜索的概率完备性以及渐进最优性，并且搜索路径更短，用时更少。在ROS平台使用UR5机械臂进行仿真实验，验证该算法的实用性与有效性。
关键词：机械臂；路径规划；改进RRT*-connect；梯度下降法
文献标志码：A中图分类号：TP241doi：10.3778/j.issn.1002-8331.1911-0316
Path Planning of Manipulator Based on Improved RRT*-connect Algorithm
LIU Jianyu,FAN Pingqing
School of Mechanical and Automotive Engineering,Shanghai University of Engineering Science,Shanghai201620,China
Abstract：Based on the bi-directional asymptotically optimal RRT*-connect algorithm,the motion planning of the high-dimensional manipulator is analyzed,so as to make the search path in the planning process shorter and more efficient.
Firstly,the target bias strategy is introduced into the sampling process,and the sampling area is constrained to ensure that each sample can be searched towards the target direction,therefore the optimal searching path can be obtained.Secondly, based on the above operation,the gradient descent method is adopted to optimize the searched path,smoothing the whole path and removing the large angle turn.Then,the improved RRT*-connect algorithm is simulated and analyzed by Matlab. It is demonstrated that the algorithm can guarantee the probabilistic completeness and asymptotic optimality of searching under various complex environment,and the shorter searching path is obtained in shorter time.Finally,a simulation experi-ment is conducted on the ROS platform with UR5manipulator to verify the practicability and effectiveness of the algorithm. Key words：manipulator;path planning;improved RRT*-connect;gradient descent method
基金项目：上汽基金会项目（1910）。
作者简介：刘建宇（1994—），男，硕士研究生，研究领域为机械臂的运动规划研究；范平清（1980—），通信作者，女，副教授，硕士生导师，研究领域为机械系统动力学和智能控制，E-mail：*****************。
收稿日期：2019-11-21修回日期：2020-01-08文章编号：1002-8331（2021）06-0274-05
索，提出使搜索路径朝理论最优解收敛的RRT*-connect 算法[10]。2016年王道威等提出动态步长的RRT算法[11]，但是只考虑简单障碍物环境。2017年朱宏辉等提出加入规避步长延伸法的改进RRT*算法[12]，虽然防止陷入局部最小解，但是对于复杂狭小路径，不容易获得较优解。2018年陈波芝等人提出用于双机械臂避障的改进RRT算法[13]，但只考虑了球形障碍物环境。2019年王坤等提出在B-RRT*中加入采样函数，并结合快速扩展策略使搜索速度加快的EB-RRT*算法[14]。
本文基于上述研究成果，针对RRT*-connect算法的采样速度较慢以及产生路径较粗糙的问题，首先在
目标偏向采样的基础上加入约束条件，保证每次采样趋近目标点，减少不必要区域的搜索，使采样更加高效。然后采用梯度下降法对搜索形成的路径做平滑处理，对比分析改进前后的RRT*-connect算法，证明经过优化处理的路径更平滑。最后在ROS平台进行仿真，验证该算法更适合实际机器人操作，路径更短且用时更少。
1RRT*-connect算法
2015年Klemm提出了RRT*-connect算法，该算法利用RRT*算法的渐进最优思想，在传统的RRT基础上进行优化，使得搜索出的路径是渐进最优的。
如图1所示，q init为路径的起始点，向目标点q goal搜索，q rand是在自由空间得到的随机采样点，q nearest是随机树上到离q rand最近的节点，q new为q nearest向q rand延伸一定距离l得到的下一个拓展点。利用算法的渐进最优特性，为q new重新选择父节点。
如图2（a）所示，首先以q new为圆心，以一定半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合Q near。其次对路径进行代价判断，将从起始点经过潜在父节点到q new的路径长度与经过原q nearest节点的长度比较，不断对比筛选出路径代价最小的节点q new_parent。然后将路径添加进随机树并删除原q nearest与q new连接路径，如图2（b）所示。同时该算法利用RRT-connect算法的贪婪搜索来实现双向拓展。
如图3所示，从起点q init向目标点q goal搜索的过程中，目标点q goal也在向起始点q init搜索。在两棵随机树空间V1、V2中，q opt为起始点随机树V1中的一个拓展点，V2随机树将设法拓展到q opt节点。q opt节点利用路径代价函数进行选择，如式（1）所示：
q opt=arg min
q∈Va⋂Vb
Cost(line(q init,q opt)+line(q opt,q goal))（1）通过代价判断得到最优节点q opt，将两棵随机树连接，
2改进RRT*-connect算法
2.1采样约束
上述RRT*-connect算法利用双向随机树搜索，加快
搜索速度。但是在搜索过程中存在采样点的随机性较
大和采样区域太分散的问题，容易造成搜索效率低和路
径较粗糙。
针对这个问题，本文将目标偏向思想[15]加入到
RRT*-connect中，目标偏向思想是指在采样过程中首先
设定一个基准值P standard，然后随机树每次在自由空间
中采样，按均匀概率分配，随机产生一个概率值P。如
果P小于原先设定的基准值，则采样点选择为目标点：
P←rand(0,1)；
if(P<P standard)q rand=q goal
然后本文在P大于基准值时，
对随机树搜索的随
机点进行控制（此处仅以起始点拓展随机树为例，目标
点拓展随机树的设置相同），如图4所示，利用采样约束
策略，具体设置如下。
采样约束设置：首先将起始点与目标点进行连接，
将两者之间的区域定义为全局的采样区域Vq，即图4
黄区域。然后当P大于基准值时，开始在空间进行
随机采样，并对采样点的位置进行判断，保证每一次的
采样要比前一次更接近终点，否则重新采样。即通过函
数约束，使每次的采样点都在采样区间Vq内，并且每
次的采样都在前一次采样点的一定角度内，左右角度
θ≤900。如图4中的q rand2为第二次随机采样点，位于第
q goal
图1
压缩空气汽车自由空间采样
（a）潜在父节点筛选（b）连接新节点
图2父节点重构
图3两棵随机树连接过程
一次采样点q rand1的上方θ角度内，q rand3位于q rand2的下方θ角度内，如下所示：
if (P ≥P standard )q rand(i )←Sample ()；While q rand(i )≤q rand(i -1)
q rand(i )←Sample ()；End
采样约束设置确保每次采样趋于目标点，有效防止随机采样点的反向搜索，从而保证采样的方向性，减少资源浪费。同时在约束环境下不限制采样点在纵向空间的随机选择，保留了采样的随机性，确保遇到障碍物时，能够高效搜索出可行路径。
最后在加入采样约束后，整个改进的RRT*-connect 算法流程如图5所示。
改进的RRT*-connect 算法，首先从起始点和目标点初始化两棵随机树，然后开始随机获取概率值并进行判断。如果小于基准值，则将目标点设置为采样点，否则在约束区域进行采样，将经过约束设置的采样点加入随机树。最后利用算法的渐进最优特性重新选择父节点，重构随机树，不断重复操作，直到某一节点同时存在两随机树上，则路径完成。
2.2平滑处理
上文将RRT*-connect 算法的采样点进行约束设置，明确采样方向性，从而节约搜索资源，使得路径更优且用时更短。但依旧存在规划出的路径较陡，大角度弯折较多的问题。针对这个问题，采用梯度下降法对搜索出的路径进行优化，使其在实际运用中更加合理，进而增强在实际运行中的可操作性。
梯度下降法常应用在A*、D*算法上，用于将搜索出的路径做平滑处理，其优点为计算量较少，能够最小化所有样本的损失函数值，并且使结果是全局最优解或者在最优解附近。梯度下降法是一种依靠迭代求解最小值的算法，主要是通过误差最小化求解出最佳数据，从而可以达到拟合曲线的目的。
将路径看成曲线并用函数表示，如式（2）所示：y =θ0x 0+θ1x 1+θ2x 2+⋯+θi x i （2）式中，x i 为处理数据中的特征值，θi 为权重值。引入假设函数，使用一组特征值代入式（2），由于权重值θi 未知，令得到结果为y'，如式（3）所示：
y'=h θ(x )=∑i =0n
θi x i
（3）
引入损失函数概念，即式（3）得到的y'与真正的y 值间存在误差。损失函数ΔT (θ)如式（4）所示：
ΔT (θ)=(y'-y )2=(h θ(x )-y )2
（4）
此差值为方差，损失ΔT (θ)越小意味两者越接近，故ΔT (θ)越小越好。针对路径存在数据较多的问题，将采用均方差科学表示损失值，如式（5）所示：
J (θ)=1m ∑i =1
m
[]
h θ(x (i ))-y
(i )
2
（5）
式中汇总所有样本数据，J (θ)为均方损失差。
梯度下降法利用微分思想求解，将θ取值范围划分多份，每份宽度为动态∂，实际宽度由微分步长α决定，通过改变θ的值直至损失值趋于0。
根据微分公式变形得到的迭代公式如式（6）所示：θj :=θj -α∂∂θj J (θ)（6）
迭代使得每次损失最小，对于路径而言只需控制权重值θi 的大小和迭代次数，使得目标函数取最小值，即可以对路径进行平滑处理，如图6所示。
图6红圈划线为原始曲线，黑虚线为经过梯度
下降法平滑处理后的曲线，可以明显看出处理后的曲线弱化了原路径的棱角，同时能够控制平滑处理的程度，保证路径的可靠性。
3仿真结果对比分析
3.1规划时间与路径长度分析
本文提出RRT*-connect 改进算法并运用仿真软件对比原规划算法。首先选择两幅复杂程度较高的地图进行测试，如图7，地图范围为500×500，地图中的黑为障碍物，起点坐标（0，0），目标坐标（500，500），红部分为搜索形成的随机树，蓝部分为规划出的路径。然后对比分析两者搜索时间与路径长度上的差异，来验证改进算法的性能。
通过两个算法在不同地图中的对比，可以很明显看出改进后RRT*-connect 算法在复杂环境下迭代次数更少，同时减少无效区域搜索，使得搜索更加高效，目的性更强。
经过20次重复实验，将路径搜索时间和路径长度汇总成表格。
图8为两幅测试图20次实验的搜索时间对比，可以很明显看出，改进后RRT*-connect 算法在不同的地图环
图5优化算法流程图
5
10
15
20
25
30
x /dm
7
654321y /m
原始路径
平滑处理后路径
图6平滑处理对比
境下，搜索时间都有较大程度的缩小，并且更加稳定，不
存在原RRT*-connect 算法搜索时间随机性大的问题。测试环境1中原算法20次搜索的平均耗时2.88s ，改进RRT*-connect 算法平均耗时为1.70s ，时间缩短40.97%。测试环境2中原算法20次搜索的平均耗时4.35s ，改进RRT*-connect 算法平均耗时2.68s ，时间缩短38.39%。
通过数据可以清晰看出，径搜索时间缩短图9为两幅测试图20次实验的路径长度对比，可以
看出改进后的RRT*-connect 算法路径长度普遍更短，路径更优，并且多次搜索的路径相比原算法更加稳定。测试环境1中原算法20次的平均路径长度为858.41mm ，改进RRT*-connect 算法平均路径长度为758.29mm ，路径缩短11.66%。地图2中原算法20次的平均路径长度860.38mm ，改进RRT*-conne
ct 算法平均路径长度为
780.36mm ，路径缩短9.30%。通过数据可以清晰看出，改进后的10%综上，不同复杂环境下都更具目的性和方向性，且比原RRT*-connect 算法搜索更加精确，
路径长度更短，耗时更少。但在实际的运行操作中还是存在路径较粗糙的问题，因此需要进行平滑处理。
3.2平滑处理效果分析
针对上述问题，采用梯度下降法对路径进行优化处
理，如图10所示，分别为四种不同障碍物地形，设置如上文所述，黑路径为改进后RRT*-connect
算法路径，蓝路径为经过平滑处理后的路径。
通过图10可以明显看出，在不同地图环境下经过
平滑处理，路径大角度棱角明显减少，更加平坦，且路径长度也明显缩短。具体数据见表1。
100200300400
室外路径
500
500400300200100x /mm y /m m
RRT*-connect
100200300
400500
500400300200100x /mm y /m m
改进后RRT*-connect
100200300400500
500400300200100x /mm
y /m m
RRT*-connect
100200300400500
500400
300
200100x /mm
y /m m
改进后RRT*-connect
（a ）测试环境1
（b ）测试环境2图7
算法测试对比
气吹
滤波装置
图8
两种算法时间数据对比图
1路径长度/m m
1路径长度/m m
100200300400500
500400300200100x /mm
y /m m
100200300400500
500400300200100
x /mm
y /m m
100200300400500
500
清肺排毒颗粒的功效与作用
400300200100x /mm
y /m m
100200300400500
500400300200100x /mm
y /m m
（a ）测试环境1
（b ）测试环境2
（c ）测试环境3
（d ）测试环境4
图10
四种地图下平滑处理
通过表1可以明显看出，在不同的障碍物环境下，经过平滑处理，路径在原有的基础上进一步改善，路线长度有明显减少，平均减少4%左右，路径趋于最优。证明本优化处理是十分有效且必要的。
3.3仿真测试
本文改进的RRT*-connect 算法在ROS 开发平台下，
通过使用UR5机械臂在moveit 环境下进行运动规划测试。如图11所示，图中绿柱体为障碍物，将改进算法作为插件加载进moveit 进行测试，指定其由平置的down 位姿，如图11（a ）所示，绕过障碍物到达上方指定的up 位姿，如图11（b ）所示。
利用ROS 平台，可以直观看出算法的规划路线，并对其进行分析，如图12所示。
通过图12可以明显看出，该算法很好地绕过障碍物，且路径较优，证明该改进算法的实用性与有效性。
4结论
本文将RRT 的变种算法RRT*-connect 算法加以改进，首先通过引入目标偏向思想，对随机采样点加以控制，使得全局的采样点趋向目标点进行采样，确保采样更加精确，避免了不必要区域的搜索，大
大节省搜索时间。同时通过概率偏置，防止出现局部最小解，使得路径渐进更优。其次通过仿真软件在多种复杂地图环境下进行测试，将改进的RRT*-connect 算法与原算法相比，搜索时间减少40%左右，路径长度缩短10%左右，从而表明路径更优，搜索速度更快。然后再利用梯度下降法对路径进行平滑处理，减少路径的大角度棱角，同时路径长度减少
4%左右。最后在ROS 平台，使用UR5机械臂在多个障碍物的仿真环境下进行了无碰撞运动规划。综上得出结论，本文改进的RRT*-connect 算法搜索时间相比原RRT*-connect 算法大幅度减少，同时路径长度更短，趋于最优解。仿真实验表明改进算法更加适合机械臂的实际运行与操作，进而证明该算法的可行性与实用性。
参考文献：
[1]LA V ALLE S M ，KUFFNER J J.Randomized kinodynamic
布线槽planning[J].The International Journal of Robotics Research ，2001，20（5）：378-400.[2]KUFFNER J J ，LA V ALLE S M.RRT-connect ：an efficient
approach to single-query path planning[C]//IEEE International Conference on Robotics and Automation ，2000：995-1001.[3]BRUCE J ，VELOSO M M.Real-time randomized path
planning for robot navigation[C]//Robot Soccer World Cup.Berlin ，Heidelberg ：Springer ，2002：288-295.[4]FERGUSON D ，KALRA N ，STENTZ A.Replanning with
RRTs[C]//2006IEEE International Conference on Robotics and Automation ，2006：1243-1248.[5]KARAMAN S ，FRAZZOLI E.Sampling-based algorithms
for optimal motion planning[J].The International Journal of Robotics Research ，2011，30（7）：846-894.[6]ISLAM F ，NASIR J ，MALIK U ，et al.RRT*-smart ：rapid
convergence implementation of RRT*towards optimal solution[C]//2012IEEE International Conference on Mecha-tronics and Automation ，2012：1651-1656.[7]JORDAN M ，PEREZ A.Optimal bidirectional rapidly-exploring random trees[R].Cambridge ：Computer Science
and Artificial Intelligence Laboratory ，Massachusetts In-stitute of Technology ，2013.[8]SALZMAN O ，HALPERIN D.Asymptotically near-optimal
RRT for fast ，high-quality ，motion planning[J].IEEE Trans-actions on Robotics ，2013，32（3）：473-483.[9]QURESHI A H ，AYAZ Y.Intelligent bidirectional rapidly-exploring random trees for optimal motion planning in
complex cluttered environments[J].Robotics and Autono-mous Systems ，2015，68：1-11.[10]KLEMM S ，OBERLÄNDER J ，HERMANN A ，et al.RRT*-connect ：faster ，asymptotically optimal motion planning[C]//
2015IEEE International Conference on Robotics and Biomimetics ，2015：1670-1677.[11]王道威，朱明富，刘慧.动态步长的RRT 路径规划算法[J].
计算机技术与发展，2016，26（3）：105-107.[12]朱宏辉，明瑞冬，朱轶.基于改进RRT~*算法的路径规划[J].
武汉理工大学学报，2017，39（2）：72-76.[13]陈波芝，陆亮，雷新宇，等.基于改进快速扩展随机树算法
的双机械臂协同避障规划方法[J].中国机械工程，2018，29（10）：1220-1226.[14]王坤，曾国辉，鲁敦科，等.基于改进渐进最优的双向快速
扩展随机树的移动机器人路径规划算法[J].计算机应用，2019，39（5）：1312-1317.[15]徐娜，陈雄，孔庆生，等.非完整约束下的机器人运动规划
算法[J].机器人，2011，33（6）：
666-672.
（a ）down 位姿
（b ）up 位姿
图11
机械臂位姿
图12机械臂运动轨迹
地图图10（a ）图10（b ）图10（c ）图10（d ）改进前路径长度/mm 829.98812.98893.95866.39
改进后路径长度/mm
803.16781.80855.20822.61
改进/%3.233.844.335.05
表1
不同地图下路径长度数据
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议