第27卷第4期2021年4月
计算机集成制造系统
Computer Integrated Manufacturing Systems
Vol.27No.4
Apr.2021
DOI：10.13196/j.cims.2021.04.003
陈满意1，张桥叫张弓％3+，梁济民3，侯至丞2，杨文林2，徐征2，王建2
（X武汉理工大学机电工程学院，湖北武汉430070；
2.广州中国科学院先进技术研究所，广东广州511458；
3.中国科学院深圳先进技术研究院，广东深圳518055）
摘要：为提高协作机器人在多障碍环境下的避障路径规划的成功率和效率，针对机械臂和障碍物提出碰撞检测方法，并提出低振荡人工势场一自适应快速扩展随机树（ARRT）混合算法进行路径规划，机械臂先采用低振荡人工势场法进行搜索，当遇到局部极小、碰撞等情况时切换成ARRT进行逃离，宜至到达目标点。另外，为了在每个步长都取得最优的逆运动学关节角，保证前后步长对应关节角度值变化的连续性，提出最短行程逆解算法。 为了提高规划后的路径质量，提出一种冗余路径节点删除策略，并使用四次贝塞尔曲线对路径进行拟合。经过仿真分析，机械臂在多障碍环境下对于环境复杂度的适应性强，路径搜索成功率高于经典算法，其平均路径搜索时间相比于经典RRT算法从26.1s下降到3.6s，算法搜索成功率和效率都得到显著改善。
关键词：协作机器人；机械臂；避障路径规划；低振荡人工势场法；自适应快速扩展随机树法
中图分类号：TP242文献标识码：A
Obstacle avoidance path planning of manipulator in multiple obstacles environment
CHENManyi1,ZHANG Qiao1'2,ZHANG Gong2r3+,LIANG Jimin3,HOUZhicheng2,
YANG Wenlin2,XU Zheng2,WANG Jian2
(1.School of Mechanical Engineering,Wuhan University of Technology,Wuhan4.30070,China；
2.Guangzhou Institute of Advanced Technology,Chinese Academy of Sciences,Guangzhou511458,China；
3.Shenzhen Institute of Advanced Technology,Chinese Academy of Sciences,Shenzhen518055,China)
Abstract:To improve the success rate and efficiency of the obstacle avoidance path planning of the cooperative robot in multiple obstacles environment,a collision detection method was proposed,and a low-oscillation-artificial-poten-tial-field&Adaptive-Rapidly-exploring-Random-Tree(ARRT)hybrid algorithm was created.Low-oscillation-arti-ficial-potential-field method was used to search,and ARRT was used for switching to escape with the conditions such as local minimum situation and collision until the target point was reached.In addition,to ensure the continuity of the joint angle between two continuously step,the shortest stroke algorithm was proposed to obtain the optimal inverse kinematic joint angle.To improve the quality of the planned path,a redundant path node deletion strategy was proposed and the path was fitted by four-time Bessel curve.Through simulation analysis,the manipulator had strong adaptability to adapt multiple obstacles environment,and the success rate of
path search was higher than the traditional algorithm.The average time of path search was reduced from26.Is to3.6s compared with the classical RRT algorithm.Therefore,the algorithm search success rate and efficiency had been significantly improved.
Keywords:collaborative robots；manipulator；obstacle avoidance path planning；low-oscillation-artificial-potential-field method j adaptive-rapidly-exploring-random-tree method
收稿日期：2019-07-05；修订日期：2019-10-17o Received05July2019；accepted17Oct.2019.
基金项目：国家重点研发计划资助项目(2018YFA0902903)；国家自然科学基金资助项目(62073092〉；广东省自然科学基金资助项目(2021A1515012638)；广州市基础研究计划资助项目(202002030320)。Foundation items：Project supported by the National Key Research and Development Program.,China(No.2018YFA0902903),the National Natural Science Foundation,China(No.62073092), the Natural Science Foundation of Guangdong Province,China(No.2021A1515012638),and the Basic Research Program of Guangzhou City,China(No.202002030320).
第4期陈满意等：多障碍环境下机械臂避障路径规划991
1问题的描述
如今，协作机器人发展迅速，在手机等3C行业发挥的作用越来越大，如图1所示为多机器人协作进行手机配件装盒。由于机器人布置密集，工作的时候存在很多干涉碰撞问题，因此协作机器人避障路径规划成为了一大关键突破点，众多学者都对其进行了研究。避障路径规划是指在工作空间存在障碍物的情况下，机器人在有限的时间内规划出一条无碰撞的路径"幻，由于工业机械臂的应用环境越来越复杂，避障路径规划的作用也越来越重要。
图1协作机器人
人工势场法由Khatib⑷于1985年提出，其规划的路径质量高，但是容易陷入局部极小而发生振荡°快速扩展随机树(Rapidly-exploring Random Tree,RRT)算法由Lavalle1-4-1于1998年正式提出，其在多障碍物环境下搜索成功率很高，但是搜索随机性大、效率低。很多学者对这两种算法分别进行了改进，姬伟等旳提出一种基于虚拟目标点的人工势场法用于机械臂逃离局部极小值；Wang等皈提出一种基于改进吸引力势函数的人工势场法，实现机械臂关节角速度的平滑过渡;Seyyed等⑵提出一种基于代价函数的人工势场法，用于解决路径搜索容易陷入局部极小值和目标不可达的问题；陈晋音等凶提出改进的EB_RRT算法，解决障碍物附近路径搜索迭代次数过高的问题；Wei等⑷提出一种S-RRT路径规划算法，提高路径节点扩展效率；何兆楚等［炖提出人工势场-RRT算法，解决路径搜索容易陷入局部极小值的问题。
以上研究都是在机械臂的关节空间进行规划，需要将障碍物转换到关节空间，计算复杂。为此，本文在笛卡尔坐标空间进行机械臂避障路径规划，为了在每个步长都取得最优的逆运动学关节角，保证前后步长对应关节角度值变化连续性，参考Craig［山对机械臂运动学逆解的处理，提岀最短行程逆解算法；参考Ketchel等口幻提出的包围球和圆柱包络法对障碍物和机械臂连杆进行近似模拟，检测机械臂连杆和障碍物的碰撞，提出一种低振荡人工势场一自适应快速扩展随机树(Adaptive RRT,ARRT)混合算法，先采用低振荡人工势场法进行搜索，当遇到局部极小、碰撞等情况时，切换成ARRT算法进行搜索，两种算法结合使用，直至到达目标点。低振荡人工势场法降低了路径的振荡程度，arrt算法则在经典RRT算法的基础上加入自适应避障策略，提高搜索效率。另外，为提高路径质量，提出冗余路径节点删除策略，并使用四次贝塞尔曲线
对路径进行拟合。
2算法原理
2.1碰撞检测方法
获取信息的方法本文研究对象为六自由度机械臂，碰撞检测指的是障碍物与机械臂连杆的碰撞检测，而实际工程应用中障碍物的外形一般不规则，很难通过精确的数学公式进行表达。因此，本文采用包围球对障碍物进行近似模拟，采用圆柱包络法对机械臂连杆进行简化，如图2所示。该方法虽然增加了障碍物和机器
人本体的区域，但是大大简化了计算量。
图2障碍物和机器人碰撞检测模型
在三维空间内计算障碍物中心点和每根连杆所在直线的距离dis，比较dis和JR+r的大小，若dis 则障碍物和连杆没有发生碰撞；若障碍物与机器人各连杆均满足无碰撞条件，则机器人当前状态满足无碰撞条件
。
992计算机集成制造系统第27卷
2.2最短行程解算法
本次路径规划研究在笛卡尔空间完成，根据机械臂末端位姿采用逆运动学求解最多可求得八组解，为了在每个步长都取得最优的逆运动学关节角，保证前后步长对应关节角度值变化连续性，提出了最短行程逆解算法，因此两个步长之间每个关节的角度变化需尽可能小，即实现“最短行程”。机械臂空间搜索每移动一个步长的最短行程解即使得每一个关节角度变化最小，求出关节角变量的L1范数最小值即可，又因为机械臂6个连杆中靠近末端的连杆主要决定机械臂末端执行器的姿态，远离末端的连杆主要决定机械臂末端执行器的位置，所以求最短行程解时需要加权，满足式(1)的各关节角度值即为最短行程解：
min£I|«(1)式中：△已为每个关节角度值的变化量；砂为每个关节角度值变化量的加权系数。
2・3低振荡人工势场法
人工势场法是局部路径规划方法，目标点对机械臂产生引力作用，障碍物对机械臂产生排斥力作用，引力和斥力的合力方向决定机器人运动方向，如图3所示。
机器人在路径搜索过程中，由于目标点附近可能存在障碍物，导致机器人很难到达目标点，本文为人
工势场法的引力势函数进行修改，添加最小引力势能^(min)o对于斥力势函数，斥力场作用范围半径为Z。，故在斥力场边缘处会存在斥力突变的情况，路径规划容易发生振荡，因此本文为斥力场势函数添加了一个加权系数(詈)二使得斥力场从边
缘向中心位置逐渐以三次增长趋势过渡，减缓路径振荡，如式(2)和式(3)所示为改进的引力场和斥力场势函数：
Uatt=max(
*&|Probot—Pg,Uatt(min))；风淋房
(2)
[0 l>l0
其中：九为引力势函数；爲为引力系数，尺遇为机器人当前位置；P&为目标点位置;U衬表示斥力势函数；怎为斥力系数弭为机器人与障碍物之间的距离；为斥力场作用范围。对于如何选择其中的常量值，文献口4]进行了详细的论述。
机器人的势能总和
[7=4+4。(4)如图4所示为经典人工势场法和低振荡人工势场法的仿真效果对比。在障碍物、步长等其他条件均相同的情况下，图中粗线代表低振荡人工势场法规划的路径，其振荡幅度相比于经典人工势场法路径明显降低。
图“经典人I：势场法和低振荡人【沏场法对比
2.4ARRT算法
钻孔电视经典RRT算法的原理如图5所示，机器人每一次路径捜索都需要寻一个随机点工迪d，在已经存在的路径节点中选取距离工心最近的点竝eax并以步长aep朝着随机点工心的方向进行扩展，以此方式搜索路径直至到达终点。
快速扩展随机树法的路径搜索方向始终是云=，具有很强的随机性，当路径延伸到障碍物附近时，新生成的节点可能与障碍物发生干涉而被舍弃，使这次迭代未插入新的节点，这种情况在多障碍物环境下比较容易发生，因此本文提出自适应避障策略，如图6所示。RRT算法在三维直角坐标空间进行规划，当前节点为条曲,％罰为扩展的新的路径节点，
但是由于翻与障碍物碰撞(此处为了使得
第4期陈满意等：多障碍环境下机械臂避障路径规划993
图5快速扩展随机树法则通过式（5）可以计算
z E=（©—乞。）5+cosO—说）+
（0—夕0）（了+血仍—夕0）/勺—No+zo。（6）
已知算法搜索的步长为必则采用ARRT算法计算的新节点QnewZ位于直线gendE'方向上，距离
的距离为一个步长几设幺比的坐标为（怎"2，勺），如式⑺所示:
82=兀0+
示意图便于理解，障碍物并未画出被舍弃，算法需重新寻新的节点。图6中：Qend-1为Qend的上一个节点，即父节点；S为过gend节点且与Qend张wl垂直的平面;O。为S平面上以为中心的椭圆；O 为O。在XOY面上的投影且为单位圆；宜线X’为过O1圆心且与X轴平行的直线;『竝为路径规划的起始点在XOY面上的投影点;g'嗣为路径规划的终点在XOY面上的投影点；直线AB为过两点的直线；CD为过Oi圆心且垂直于直线AE的直线；垂足
自适应RRT的原理是在单位圆Q上选取点E，选择原则是在圆Q上被直线CD分割的且靠近『嗣的那一半圆弧上的点。已知张d的坐标为（氐，必，颅），则Q的坐标为（・o，yo,O），E点的坐标是Qo+ co彫，夕。+衣皿,0）。点£为圆Q）上对应于E点的点，则£点的坐标是（ho+co殆,M+sin^,ZE）,ZE未知。已知Qnewl的坐标为（刃15，勺），因为平面S过节点且与4凶二垂直，因此平面S的方程如下：（xi—x0）（x—x0）+（3?i—）（夕—yo）+
金丝雀定位（引一zo）（z—荻）=0。（5）
如表1所示为经典RRT箕法和ARRT算法经过MATLAB仿真分析之后的迭代次数对比，在障碍物、步长等其他条件均相同的情况下仿真分析20次,ARRT算法的平均迭代次数相比于经典的RRT 算法的平均迭代次数从143.8次降低到了89.8次，有效地提高了障碍物附近机器人避障路径搜索的效率。
表1经典RRT和ARRT平均迭代次数对比
经典RRT ARRT 20次试验平均迭代次数143.889>8
3低振荡人工势场・ARRT混合算法
3.1人工势场法和RRT算法的结合
利用一种算法的优点来弥补另外一种算法的缺点是学术研究的重要思路皿-词，本文提出低振荡人工势场一ARRT混合算法，机器人先采用低振荡人工势场法进行搜索，当遇到局部极小、碰撞等情况时，切换成ARRT算法进行搜索，两种方法结合使用，直至到达目标点.判断碰撞的方法已经在前文叙述，判断机器人由于局部极小值点等引起的振荡则采用以下策略。
当前节点为条皿，其前一个节点为Qend-l，张w为当前确定的新的路径节点。由图7中夹角0可以看出熄罰-1卫罰、9唤这3个节点已经有出现振荡的趋
994
计算机集成制造系统第27卷
势，因此判断夹角6的大小即可判断人工势场法是
否出现振荡，在平面内和三维空间都适用.设g 说 坐标为A (“0，歹0，No ), Ld-1坐标为B (初，，为)，
坐标为C(d ，歹2 ，乂2)，则由余弦定理：
具体阈值取足兀/6时，判定人工势场法可能出
现振荡，切换为ARRT 算法进行搜索，逃离之后再 切换回低振荡人工势场法进行搜索。
算法伪代码如下：
low-oscillation artificial potential field-ARRT hybrid
I. T-^-initCqinit) >
2・ qnEw^Low-Oscillation-ArtificiaLPotentiaKx)；
乱 Collosion_detection (
) or Oscillation_d etection (
, q^d ，
—i ) then 4・ Qtiew'*-
5. qnew^ARRTCx)；
6・ If Collosion_detectioti(qn tw )7・ Return advanced j
& Else
9・ If q aew = qend then 10. Return Reached
II. Else
塑料静电分离机靠谱吗
12. ExtendCT^q^)
13. Return Low-Oscillation_Artificial_Potential(x) ?14. Else
15. If q a€w = qend then
16 ・ Return Reached 17. Else
1& ExtendCTtqnev) j
算法流程图如图8所示。使用最短行程解算法 来保证机械臂在笛卡尔空间规划的关节角度变化连 续性，规划过程在笛卡尔空间进行，不需要将障碍物
转化到关节空间，可节省很大的计算量。2.2路径优化
2. 2. 1冗余路径节点删除策略
由于RRT 算法的随机性，Sudhakara 等［怕提 出RRT *算法进行节点路径的优化。但是RRT *
的算法所使用的路径优化策略只针对于单独使用
图8低振荡人」：势场-ARRT 混合算法渝程图
RRT 算法的情况，因此本文提出一种冗余路径节点 删除策略，其步骤描述如下：
(1) 低振荡人工势场法切换到ARRT 算法。
(2) 定义Tree 数组，7>ee 相当于一个节点容
器用来收集低振荡人工势场一ARRT 混合算法
的节点。定义KRT 歼ee 数组，该数组用来收集
ARRT 算法的节点，确定本次ARRT 扩展的新节
点 Qnew o
(3) 新增节点Qnew 到Tree 数组，张w 的前一个节
点，也就是当前节点为，将 细和Qnew 到RRTtree 数组。
(4) 按照节点添加的时间先后顺序，在RRTtree 数组中从第一个节点开始计算和q 心的距离刃$-
血nee，已知步长厶假设满足如下条件：
distance d o
(9)
则定义该节点为药心，并直接连接(?唤和Winter, 若没有节点和“ew 的距离小于一个步长，则直接转 步骤(6)。
(5) 删除Tree 数组和RRTtree 数组中血论和 Qnew 之间的所有节点。
(6) 切换回人工势场法。删除振荡中间点，即当
前节点Qd 的前一个节点 g^nd-1 o
如图9所示，由节点2、3、4等可判断路径出现
振荡，因此删除节点3，并连接节点2和节点4。又 因节点4和12之间无碰撞，且距离接近一个步长，
连接节点4和12并删除它们之间的节点5、6、7、8、彩油墨
9、10、11。
如图10所示为在低振荡人工势场-ARRT
混合
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议