高速公路绿篱修剪机器人手臂避障路径规划

工程科学学报，第41卷，第1期：134-142,2019年1月 Chinese Journal of Engineering,Vol.41,No.I:134-142,January 2019 D0L:10.13374/j.issn2095-9389.2019.01.015;htp:/journals.ustb.edu.cm 高速公路绿篱修剪机器人手臂避障路径规划 罗天洪12)，唐果)四，马翔宇3》，周军超) 1)重庆交通大学机电与车辆工程学院，重庆4000742)重庆文理学院机械工程学院，重庆402160 3)西安航空学院机械工程学院，西安7100774)人工智能四川省重点实验室，自贡643000 区通信作者，E-mail:401628977@qq.com 摘要针对非结构环境下高速公路绿篱修剪机器人手臂实时准确避障问题，提出一种基于扰动人工势场法的避障路径规 划解决方法.根据绿篱隔离带与障碍物分布情况，设计智能修剪机器人执行机构，构建包络障碍物简化模型，分析机械臂与障 碍物的碰撞条件，求解机械臂在修剪过程中的避碰空间.引入斥力场调节策略来优化势场模型，建立斥力场扰动机制调整斥 力影响方式，消除传统算法中的局部极小点、目标不可达等现象.在避碰空间内，应用扰动人工势场法对机械臂进行路径规划 仿真，仿真结果表明，机械臂跳出局部极小点，灵活顺利避障，成功抵达目标点，验证了该方法的有效性和可行性 关键词修剪机械臂；非结构环境；路径规划；避碰空间；扰动人工势场法 分类号TP241.3 Obstacle avoidance path planning for expressway hedgerow pruning robot manipulator LUO Tian-hong),TANG Guo,MA Xiang-yu,ZHOU Jun-chao 1)School of Mechanotronies and Vehicle Engineering,Chongqing Jiaotong University,Chongqing 400074,China 2)College of Mechanical Engineering,Chongqing University of Arts and Sciences,Chongqing 402160,China 3)College of Mechanical Engineering,Xi'an Aeronautical University,Xi'an 710077.China 4)Artificial Intelligence Key Laboratory of Sichuan Province,Zigong 643000,China Corresponding author,E-mail:401628977@qq.com ABSTRACT Expressway hedgerow pruning robots need be able to recognize hedgerow and position themselves real-time,to plan an obstacle avoidance trajectory from the starting point to target point based on the position relationship between hedgerows and obstacles. Compared with the traditional industry manipulator,the expressway hedgerow pruning robot manipulator frequently works in unstruc- tured environments with unknown obstacles and irregular scales.It is difficult to establish a mathematical model of obstacles precisely and comprehensively.The problem of real-time obstacle avoidance can be solved by path planning.Thus,aiming at the problem of re- al-time obstacle avoidance for expressway hedgerows pruning robot manipulator in an unstructured environment,a novel path planning method to avoid obstacle based on perturbed artificial potential field(PAPF)was proposed.According to the distribution of hedgerows and obstacles,simplified models of intelligent pruning robot and sphere enveloping obstacle were established.By considering the geo- metric relationship between manipulator and obstacle,the collision conditions of manipulator and obstacles were analyzed,and then, the collision avoidance space of manipulator was solved.The traditional artificial potential field method was associated with some prob- lems such as local minimum point (LMP)and goals nonreachable with obstacles nearby(GNRON).In this study,a repulsion field ad- justment strategy was presented to optimize the function model of potential field,and a repulsion field perturbation mechanism was in- troduced to adjust the effect of repulsion in order to flexibly avoid obstacles and successfully reach the target point.The path planning simulation of the designed manipulator was carried out in the collision avoidance space using PAPF.The simulation result shows that the manipulator smoothly jumps out of the LMP and reaches the target point successfully by accurately avoiding obstacles in real time, 收稿日期：2017-12-18 基金项目：国家自然科学基金资助项目(51375519)：人工智能四川省重点实验室资助项目(2017RYY04)

工程科学学报,第 41 卷,第 1 期:134鄄鄄142,2019 年 1 月 Chinese Journal of Engineering, Vol. 41, No. 1: 134鄄鄄142, January 2019 DOI: 10. 13374 / j. issn2095鄄鄄9389. 2019. 01. 015; http: / / journals. ustb. edu. cn 高速公路绿篱修剪机器人手臂避障路径规划 罗天洪1,2) , 唐 果1)苣 , 马翔宇3) , 周军超4) 1) 重庆交通大学机电与车辆工程学院, 重庆 400074 2) 重庆文理学院机械工程学院, 重庆 402160 3) 西安航空学院机械工程学院, 西安 710077 4) 人工智能四川省重点实验室, 自贡 643000 苣 通信作者, E鄄mail: 401628977@ qq. com 摘 要 针对非结构环境下高速公路绿篱修剪机器人手臂实时准确避障问题,提出一种基于扰动人工势场法的避障路径规 划解决方法. 根据绿篱隔离带与障碍物分布情况,设计智能修剪机器人执行机构,构建包络障碍物简化模型,分析机械臂与障 碍物的碰撞条件,求解机械臂在修剪过程中的避碰空间. 引入斥力场调节策略来优化势场模型,建立斥力场扰动机制调整斥 力影响方式,消除传统算法中的局部极小点、目标不可达等现象. 在避碰空间内,应用扰动人工势场法对机械臂进行路径规划 仿真,仿真结果表明,机械臂跳出局部极小点,灵活顺利避障,成功抵达目标点,验证了该方法的有效性和可行性. 关键词 修剪机械臂; 非结构环境; 路径规划; 避碰空间; 扰动人工势场法 分类号 TP241郾 3 收稿日期: 2017鄄鄄12鄄鄄18 基金项目: 国家自然科学基金资助项目(51375519); 人工智能四川省重点实验室资助项目(2017RYY04) Obstacle avoidance path planning for expressway hedgerow pruning robot manipulator LUO Tian鄄hong 1,2) , TANG Guo 1)苣 , MA Xiang鄄yu 3) , ZHOU Jun鄄chao 4) 1) School of Mechanotronics and Vehicle Engineering, Chongqing Jiaotong University, Chongqing 400074, China 2) College of Mechanical Engineering, Chongqing University of Arts and Sciences, Chongqing 402160, China 3) College of Mechanical Engineering, Xi蒺an Aeronautical University, Xi蒺an 710077,China 4) Artificial Intelligence Key Laboratory of Sichuan Province, Zigong 643000, China 苣 Corresponding author, E鄄mail: 401628977@ qq. com ABSTRACT Expressway hedgerow pruning robots need be able to recognize hedgerow and position themselves real鄄time, to plan an obstacle avoidance trajectory from the starting point to target point based on the position relationship between hedgerows and obstacles. Compared with the traditional industry manipulator, the expressway hedgerow pruning robot manipulator frequently works in unstruc鄄 tured environments with unknown obstacles and irregular scales. It is difficult to establish a mathematical model of obstacles precisely and comprehensively. The problem of real鄄time obstacle avoidance can be solved by path planning. Thus, aiming at the problem of re鄄 al鄄time obstacle avoidance for expressway hedgerows pruning robot manipulator in an unstructured environment, a novel path planning method to avoid obstacle based on perturbed artificial potential field (PAPF) was proposed. According to the distribution of hedgerows and obstacles, simplified models of intelligent pruning robot and sphere enveloping obstacle were established. By considering the geo鄄 metric relationship between manipulator and obstacle, the collision conditions of manipulator and obstacles were analyzed, and then, the collision avoidance space of manipulator was solved. The traditional artificial potential field method was associated with some prob鄄 lems such as local minimum point (LMP) and goals nonreachable with obstacles nearby(GNRON). In this study, a repulsion field ad鄄 justment strategy was presented to optimize the function model of potential field, and a repulsion field perturbation mechanism was in鄄 troduced to adjust the effect of repulsion in order to flexibly avoid obstacles and successfully reach the target point. The path planning simulation of the designed manipulator was carried out in the collision avoidance space using PAPF. The simulation result shows that the manipulator smoothly jumps out of the LMP and reaches the target point successfully by accurately avoiding obstacles in real time

罗天洪等：高速公路绿篱修剪机器人手臂避障路径规划 ·135· which verifies the effectiveness and feasibility of the proposed method. KEY WORDS pruning manipulator;unstructured environment;path planning;collision avoidance space;perturbed artificial poten- tial field 高速公路绿篱修剪机器人与外部环境若实现共 1 高速公路绿篱修剪机器人构型设计及 融，需要具有识别绿篱、障碍以及准确实时定位的能 建模 力，且能根据绿篱与障碍位置关系，规划一条由起始 点到目标点的避障轨迹.与传统工业机器人相比， 1.1结构设计 高速公路绿篱修剪机器人手臂常处于障碍未知、尺 根据绿篱隔离带与障碍物分布实际情况以及修 形变化不规律等非结构工作环境，难以建立精确、全 剪要求，设计一种高速公路绿篱修剪机器人，其机械 面的障碍物数学模型，且存在路径规划避障性差、准 结构如图1所示 确性低、实时性弱等问题 迄今为止，国内外学者针对机械臂路径规划已 提出了一些相关解决方案).其中，比较典型的包 12 括几何模型搜索法[2-5)、生物智能法[6-刊以及虚拟势 13 4 场法[8)谢碧云等)提出了双树随机树搜索(bi-di- rectional rapidly-exploring random tree,bi-RRT)算法， 7777777777777 777 通过关节自运动以及末端姿态的调整来生成目标 1一轮式小车：2一回转台：3一控制板电源箱：4一伺服电机：5一谐 树，并运用在机械臂的轨迹规划中.汪首坤等[o)通 波减速器：6一传动轴：7一腰部：8一大臂：9一小臂：10一腕部： 过导航势函数预先消除了局部极小点(local mini- 11一修剪电机：12一多传感器系统刀具盘：13一减速齿轮组：14一 mum point,.LMP),解决人工势场法(artificial poten- 回转刀具 tial field,APF)目标不可达(goals nonreachable with 图1修剪机器人结构简图 Fig.1 Structure of pruning robot obstacle nearby,GNRON)问题，使机械臂顺利到达目 标点.姬伟等通过在人工势场算法中引入虚拟 高速公路绿篱修剪机器人主体分为行走机构 目标点，使机械臂在避障过程中跳出局部极小点，从 与执行机构，其中行走机构由两个自由度的轮式 而灵活避障并到达目标点.文献[12-13]用遗传算 小车组成，执行机构由五个自由度的旋转关节机 法优化适应度评价函数，进行机械臂无碰撞轨迹规 械手臂组成.图1中，从腰部起到腕部止，第一个 划.文献[14-151通过模糊逻辑和神经网络算法求 翻转关节控制腰部旋转，轴线沿铅垂线方向，第二 解机械臂逆运动学，来规划出满足运动学约束的运 至四的俯仰关节分别控制大臂、小臂、腕部沿水平 动轨迹.上述方法虽然能够实现机械臂的避障路径 面方向摆动，第五个翻转关节控制修剪刀具盘上 规划，但方法过于复杂、计算量较大、难以达到在复 多传感器的检测位置.其中，轮式小车控制修剪机 杂环境下避障的实时性与准确性，且忽略了臂杆与 械臂的行进，机械臂前三个关节控制修剪刀具位 障碍之间的碰撞，同时也少有学者在避碰空间内应 置，后两个关节控制其姿态，从而实现对绿篱的 用改进人工势场算法对修剪机械臂进行实时准确避 修剪 障规划研究. 1.2修剪机械臂模型 本文针对修剪机器人手臂路径规划中避障性 所设计的修剪机械臂结构运用D-H(Denavit-- 差、准确性低、实时性弱等问题，提出一种扰动人工 Hartenberg)法创建连杆模型，如图2所示，具体连杆 势场法(perturbed artificial potential field,PAPF),对 参数如表1所示.图中，坐标系XY,Z(i=0~5)分 机械臂和障碍物简化建模，结合两者空间位置关系 别为基坐标系和五个连杆坐标系，，为末端刀具与 以及尺形特征，分析相互之间碰撞条件，求解避碰空 腕部的偏置距离，尺寸为0.7m,机械臂外形轮廓为 间，通过引入斥力场调节策略，建立扰动机制，避免 圆柱体结构，直径为0.2m. 传统算法中的局部极小点、目标不可达等现象，实现 根据图2和表1创建的D-H连杆简化模型和 机械臂在避碰空间内灵活避障并成功准确抵达目 相应连杆参数，可以由式(1)得到机械臂修剪刀具 标点 相对于基坐标系的总位姿变换矩阵为

罗天洪等: 高速公路绿篱修剪机器人手臂避障路径规划 which verifies the effectiveness and feasibility of the proposed method. KEY WORDS pruning manipulator; unstructured environment; path planning; collision avoidance space; perturbed artificial poten鄄 tial field 高速公路绿篱修剪机器人与外部环境若实现共 融,需要具有识别绿篱、障碍以及准确实时定位的能 力,且能根据绿篱与障碍位置关系,规划一条由起始 点到目标点的避障轨迹. 与传统工业机器人相比, 高速公路绿篱修剪机器人手臂常处于障碍未知、尺 形变化不规律等非结构工作环境,难以建立精确、全 面的障碍物数学模型,且存在路径规划避障性差、准 确性低、实时性弱等问题. 迄今为止,国内外学者针对机械臂路径规划已 提出了一些相关解决方案[1] . 其中,比较典型的包 括几何模型搜索法[2鄄鄄5] 、生物智能法[6鄄鄄7]以及虚拟势 场法[8] . 谢碧云等[9]提出了双树随机树搜索(bi鄄di鄄 rectional rapidly鄄exploring random tree,bi鄄RRT)算法, 通过关节自运动以及末端姿态的调整来生成目标 树,并运用在机械臂的轨迹规划中. 汪首坤等[10] 通 过导航势函数预先消除了局部极小点( local mini鄄 mum point,LMP),解决人工势场法( artificial poten鄄 tial field,APF) 目标不可达( goals nonreachable with obstacle nearby,GNRON)问题,使机械臂顺利到达目 标点. 姬伟等[11] 通过在人工势场算法中引入虚拟 目标点,使机械臂在避障过程中跳出局部极小点,从 而灵活避障并到达目标点. 文献[12鄄鄄13]用遗传算 法优化适应度评价函数,进行机械臂无碰撞轨迹规 划. 文献[14鄄鄄15]通过模糊逻辑和神经网络算法求 解机械臂逆运动学,来规划出满足运动学约束的运 动轨迹. 上述方法虽然能够实现机械臂的避障路径 规划,但方法过于复杂、计算量较大、难以达到在复 杂环境下避障的实时性与准确性,且忽略了臂杆与 障碍之间的碰撞,同时也少有学者在避碰空间内应 用改进人工势场算法对修剪机械臂进行实时准确避 障规划研究. 本文针对修剪机器人手臂路径规划中避障性 差、准确性低、实时性弱等问题,提出一种扰动人工 势场法(perturbed artificial potential field,PAPF),对 机械臂和障碍物简化建模,结合两者空间位置关系 以及尺形特征,分析相互之间碰撞条件,求解避碰空 间,通过引入斥力场调节策略,建立扰动机制,避免 传统算法中的局部极小点、目标不可达等现象,实现 机械臂在避碰空间内灵活避障并成功准确抵达目 标点. 1 高速公路绿篱修剪机器人构型设计及 建模 1郾 1 结构设计 根据绿篱隔离带与障碍物分布实际情况以及修 剪要求,设计一种高速公路绿篱修剪机器人,其机械 结构如图 1 所示. 1—轮式小车;2—回转台;3—控制板电源箱;4—伺服电机;5—谐 波减速器;6—传动轴;7—腰部;8—大臂;9—小臂;10—腕部; 11—修剪电机;12—多传感器系统刀具盘;13—减速齿轮组;14— 回转刀具 图 1 修剪机器人结构简图 Fig. 1 Structure of pruning robot 高速公路绿篱修剪机器人主体分为行走机构 与执行机构,其中行走机构由两个自由度的轮式 小车组成,执行机构由五个自由度的旋转关节机 械手臂组成. 图 1 中,从腰部起到腕部止,第一个 翻转关节控制腰部旋转,轴线沿铅垂线方向,第二 至四的俯仰关节分别控制大臂、小臂、腕部沿水平 面方向摆动,第五个翻转关节控制修剪刀具盘上 多传感器的检测位置. 其中,轮式小车控制修剪机 械臂的行进,机械臂前三个关节控制修剪刀具位 置,后两个关节控制其姿态,从而实现对绿篱的 修剪. 1郾 2 修剪机械臂模型 所设计的修剪机械臂结构运用 D鄄H(Denavit鄄 Hartenberg)法创建连杆模型,如图 2 所示,具体连杆 参数如表 1 所示. 图中,坐标系 XiYiZi(i = 0 ~ 5)分 别为基坐标系和五个连杆坐标系,l 4 为末端刀具与 腕部的偏置距离,尺寸为 0郾 7 m,机械臂外形轮廓为 圆柱体结构,直径为 0郾 2 m. 根据图 2 和表 1 创建的 D鄄H 连杆简化模型和 相应连杆参数,可以由式(1)得到机械臂修剪刀具 相对于基坐标系的总位姿变换矩阵为 ·135·

·136 工程科学学报，第41卷，第1期 P.7 [1.3c192-0.7c1s234+1.7cc23 1.3s192-0.7s15234+1.7s1c23 (4) p.0.7c24-1.7s23-1.3s2+0.5 式中，c:=cos0,s:=sin0,c=cos(6+0+0), st=sin(0:+0+0). 2非结构环境下避碰空间的求解 图2修剪机械臂D-H连杆简化模型 2.1障碍物包络建模 Fig.2 D-H link simplified model of pruning manipulator 修剪机械臂养护时通常会遇到绿篱附近的硬质 表1修剪机械臂D-H连杆参数 放射状灌木、枝干、圆形指示牌等非结构化障碍物， Table 1 D-H link parameters of pruning manipulator 其尺形变化不规律、随机出现等特点严重影响了作 连杆 长度， 扭角，偏置，关节角，变量范围， 业时的避障准确性.因此采用球形完全包络近似形 序号，ia1-1/ma-1/()d,/m8/(） /() 容非结构化的障碍物6]，虽然该方式降低了求解的 1 0 0 0.5 -160-160 优越性，但简化了避障规划时的计算量，提升了 2 0 -90 0 % -225~45 效率. 3 1.3 0 0 -150120 障碍简化模型表示为B(P。,「)，如图3所示 1.7 0 0 -210~30 以障碍物空间距离最大的两个边缘顶点G(,少1， 0 -90 -180~180 1)、G2(x2,2,2)建立包络球形，P。(0,y0,0)为球 心坐标，「为球形半径，表达式如式(5)所示. n 0x P =2√(-)+(-2)+(，-)(5) T=TITTTTST= y (1) B(P 0: a: P 0 0 0 1 Gc,y,） 其中，-T为连杆i相对于连杆i-1的变换矩阵，通 式为 难碍物 c8: -s8 0 a:-1 i-1T= s0:coi-1 c0.co;-1 -S0-1 -d sa;-1 GrY s0sa:-1 c0:sai-1 C0:-1 dca- 图3障碍简化模型 0 0 0 1 Fig.3 Simplified model of obstacle (2) 2.2机械臂避碰空间 式中，c0:=cos0,s0:=sin0:,ca-1=c0sa:-1,sa-1= 根据上述所建立的机械臂D-H连杆以及障碍 sina-·省去中间计算步骤，可推导出机械臂修剪 简化模型，分析各连杆与障碍物之间的碰撞条件，求 刀具的空间姿态向量为 解修剪机械臂在非结构环境下的避碰空间，为基于 n s1s5+C1C5C234 扰动人工势场算法对机械臂实时准确避障提供理论 s1csC234 -c1ss 基础. n: -C5S234 2.2.1关节1引起机械臂碰撞的条件 s1Cs -cissC234 连杆1的连锁反应会引起后续连杆与障碍物碰 0, -C1C5-S15sC24 (3) 撞，因此由机械臂与障碍物的位置关系和尺形特征， 0 S5s234 推导出关节1引起碰撞时的旋转角限度.将后续连 -C1S234 杆和障碍物同时投影到关节1坐标系平面O,xy1 内，此时关节1刚好引起碰撞时的连杆极限位形如 -515234 图4所示. 0J -C234 由于回转刀具半径远远大于机械臂连杆的碰撞 机械臂修剪刀具的空间位置向量为

工程科学学报,第 41 卷,第 1 期 图 2 修剪机械臂 D鄄H 连杆简化模型 Fig. 2 D鄄H link simplified model of pruning manipulator 表 1 修剪机械臂 D鄄H 连杆参数 Table 1 D鄄H link parameters of pruning manipulator 连杆 序号,i 长度, ai - 1 / m 扭角, 琢i - 1 / (毅) 偏置, di / m 关节角, 兹i / (毅) 变量范围, / (毅) 1 0 0 0郾 5 兹1 - 160 ~ 160 2 0 - 90 0 兹2 - 225 ~ 45 3 1郾 3 0 0 兹3 - 150 ~ 120 4 1郾 7 0 0 兹4 - 210 ~ 30 5 0 - 90 0 兹5 - 180 ~ 180 0 TT = 0 1T 1 2T 2 3T 3 4T 4 5T 5 TT = nx ox ax px ny oy ay py nz oz az pz é ë ê ê ê ê ê ù û ú ú ú ú ú 0 0 0 1 (1) 其中, i - 1 i T 为连杆 i 相对于连杆 i - 1 的变换矩阵,通 式为 i -1 iT = c兹i - s兹i 0 ai -1 s兹i c琢i -1 c兹i c琢i -1 - s琢i -1 - di s琢i -1 s兹i s琢i -1 c兹i s琢i -1 c琢i -1 di c琢i -1 é ë ê ê ê ê ê ù û ú ú ú ú ú 0 0 0 1 (2) 式中,c兹i = cos 兹i,s兹i = sin 兹i,c琢i - 1 = cos 琢i - 1 ,s琢i - 1 = sin 琢i - 1 . 省去中间计算步骤,可推导出机械臂修剪 刀具的空间姿态向量为 nx ny nz ox oy oz ax ay a é ë ê ê ê ê ê ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ú ú ú ú ú z = s1 s5 + c1 c5 c234 s1 c5 c234 - c1 s5 - c5 s234 s1 c5 - c1 s5 c234 - c1 c5 - s1 s5 c234 s5 s234 - c1 s234 - s1 s234 - c é ë ê ê ê ê ê ê ê ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú ú ú ú ú ú ú ú 234 (3) 机械臂修剪刀具的空间位置向量为 px py p é ë ê ê ê ê ù û ú ú ú ú z = 1郾 3c1 c2 - 0郾 7c1 s234 + 1郾 7c1 c23 1郾 3s1 c2 - 0郾 7s1 s234 + 1郾 7s1 c23 0郾 7c234 - 1郾 7s23 - 1郾 3s2 é ë ê ê ê ê ù û ú ú ú + 0 ú 郾 5 (4) 式中,ci = cos 兹i,si = sin 兹i,cijk = cos( 兹i + 兹j + 兹k ), sijk = sin(兹i + 兹j + 兹k). 2 非结构环境下避碰空间的求解 2郾 1 障碍物包络建模 修剪机械臂养护时通常会遇到绿篱附近的硬质 放射状灌木、枝干、圆形指示牌等非结构化障碍物, 其尺形变化不规律、随机出现等特点严重影响了作 业时的避障准确性. 因此采用球形完全包络近似形 容非结构化的障碍物[16] ,虽然该方式降低了求解的 优越性,但简化了避障规划时的计算量,提升了 效率. 障碍简化模型表示为 B(P0 ,rB ),如图 3 所示. 以障碍物空间距离最大的两个边缘顶点 G1 ( x1 ,y1 , z1 )、G2 (x2 ,y2 ,z2 )建立包络球形,P0 ( x0 ,y0 ,z0 )为球 心坐标,rB 为球形半径,表达式如式(5)所示. rB = 1 2 (x1 - x2 ) 2 + (y1 - y2 ) 2 + (z1 - z2 ) 2 (5) 图 3 障碍简化模型 Fig. 3 Simplified model of obstacle 2郾 2 机械臂避碰空间 根据上述所建立的机械臂 D鄄H 连杆以及障碍 简化模型,分析各连杆与障碍物之间的碰撞条件,求 解修剪机械臂在非结构环境下的避碰空间,为基于 扰动人工势场算法对机械臂实时准确避障提供理论 基础. 2郾 2郾 1 关节 1 引起机械臂碰撞的条件 连杆 1 的连锁反应会引起后续连杆与障碍物碰 撞,因此由机械臂与障碍物的位置关系和尺形特征, 推导出关节 1 引起碰撞时的旋转角限度. 将后续连 杆和障碍物同时投影到关节 1 坐标系平面 O1 x1 y1 内,此时关节 1 刚好引起碰撞时的连杆极限位形如 图 4 所示. 由于回转刀具半径远远大于机械臂连杆的碰撞 ·136·

罗天洪等：高速公路绿篱修剪机器人手臂避障路径规划 ·137· 杆2 P △0， da 4, 绿篱 图4后续连杆极限位形 图5连杆2极限位形 Fig.4 Limit position of subsequent link Fig.5 Limit position of Link 2 外形结构尺寸，机械臂连杆的碰撞余量将不作考虑. 则，根据几何关系求得连杆2运动平面和障碍物所 在高速公路绿篱修剪的实际工况中，障碍物一般只 成的切圆半径r1=√(rB+r裕)-d5,以及圆心P1 分布于关节1空间平面的一个象限.p(P,P,)点为 到O,的距离d2关节2引起碰撞时的边界角△0， 障碍物球心P。在O1x1y1平面内的投影点，x轴与直 可表示为 线OP所成角度可以表示为 △02=arcsin(r,/d2z) (8) 0=arctan(p,/p,) (6) 通过障碍物在连杆2运动平面O,x,名，的投影 式中，01∈[-T/2,π/2].关节1引起后续连杆碰 关系，求得切圆中心P,在01x11平面的坐标(P., 撞时的边界角△8，可以表示为 P1x),以及直线O,P1与x轴所成的角度021=arctan △91=arcsin(d,/d1) (7) (P1./P1.).其中0∈[-T/2,/2],△02∈[0， m/2]. 式中，△0，e[0,T/2],d,为点p到原点O1的直 考虑到实际避障过程中，障碍物下方还有无须 线距离，d。为点p到机械臂连杆的空间距离.令 修剪的绿篱，为简化运算将障碍物下方全视为碰撞 8nmim1=811-△81,01mm1=011+△01，关节1引起后续 发生的运动范围.令02m=021+△02，关节2引起 连杆碰撞时的旋转角限度为0，∈[01miml,01m]. 连杆2碰撞时的旋转角限度为02∈[-π/2,02mm]. 而当障碍物分布于其余两个象限时，关节1的 当障碍物分布于连杆2运动空间平面内的其他两个 运动范围呈对称分布.令0mm2=T+日：-△81,82= 象限时，令02mim=T+02:-△02，则关节2另一部分 T+8,+△0，，因此关节1引起后续连杆碰撞时的另 旋转角限度为0，∈[0，m,3π/2].因此对于随意设 一部分旋转角限度为0，∈[01mi2,81m2]. 定的0，关节2引起连杆2碰撞时的总旋转角限度 综上所述，关节1引起后续连杆碰撞时的总旋 为02∈O2=[-T/2,02mm1]U[92i2,3π/2]. 转角限度为01∈O,=[01miml,81x1]U[81mim2, 由上述分析方法可得连杆3、4关节旋转角总限 01m2]. 度0，∈83=[-π/2,0i]U[8,i2,3π/2]、84∈ 2.2.2连杆2发生碰撞的条件 Θ4=[-T/2,04mm1]U[94mi2,3π/2]. 此处只需考虑在6：∈⊙，=[01mia1,01m]U 最后根据以上所推导出的各连杆碰撞条件，求 [01mi,9me]范围内的情况，即设定某一个日，根据 解出修剪机械臂在非结构环境下运动的避碰空间. 连杆2与障碍物的空间几何关系，来判断连杆2是 否会发生碰撞.若d2Tg+ 为零导致.针对此问题，采用一种斥力场调节策略， 「裕（「辎为机械臂连杆碰撞余量），则不在范围内：否 即加入距离尺度因子(X-X)”对斥力场函数模型

罗天洪等: 高速公路绿篱修剪机器人手臂避障路径规划 图 4 后续连杆极限位形 Fig. 4 Limit position of subsequent link 外形结构尺寸,机械臂连杆的碰撞余量将不作考虑. 在高速公路绿篱修剪的实际工况中,障碍物一般只 分布于关节 1 空间平面的一个象限. p( px,py)点为 障碍物球心 P0 在 O1 x1 y1 平面内的投影点,x 轴与直 线 O1 p 所成角度可以表示为 兹11 = arctan(py / px) (6) 式中,兹11沂[ - 仔/ 2,仔/ 2]. 关节 1 引起后续连杆碰 撞时的边界角 驻兹1 可以表示为 驻兹1 = arcsin(da / d1 ) (7) 式中,驻兹1沂[0,仔/ 2],d1 为点 p 到原点 O1 的直 线距离,da 为点 p 到机械臂连杆的空间距离. 令 兹1min1 = 兹11 - 驻兹1 ,兹1max1 = 兹11 + 驻兹1 ,关节 1 引起后续 连杆碰撞时的旋转角限度为 兹1沂[兹1min1 ,兹1max1 ]. 而当障碍物分布于其余两个象限时,关节 1 的 运动范围呈对称分布. 令 兹1min2 = 仔 + 兹11 - 驻兹1 ,兹1max2 = 仔 + 兹11 + 驻兹1 ,因此关节 1 引起后续连杆碰撞时的另 一部分旋转角限度为 兹1沂[兹1min2 ,兹1max2 ]. 综上所述,关节 1 引起后续连杆碰撞时的总旋 转角 限 度 为 兹1 沂 专1 = [ 兹1min1 , 兹1max1 ] 胰 [ 兹1min2 , 兹1max2 ]. 2郾 2郾 2 连杆 2 发生碰撞的条件 此处只需考虑在 兹1 沂 专1 = [ 兹1min1 , 兹1max1 ] 胰 [兹1min2 ,兹1max2 ]范围内的情况,即设定某一个 兹1 ,根据 连杆 2 与障碍物的空间几何关系,来判断连杆 2 是 否会发生碰撞. 若 d2 rB + r裕(r裕 为机械臂连杆碰撞余量),则不在范围内;否 图 5 连杆 2 极限位形 Fig. 5 Limit position of Link 2 则,根据几何关系求得连杆 2 运动平面和障碍物所 成的切圆半径 r1 = (rB + r裕) 2 - d 2 21 ,以及圆心 p1 到 O1 的距离 d22 . 关节 2 引起碰撞时的边界角 驻兹2 可表示为 驻兹2 = arcsin(r1 / d22 ) (8) 通过障碍物在连杆 2 运动平面 O1 x1 z1 的投影 关系,求得切圆中心 p1 在 O1 x1 z1 平面的坐标( p1z, p1x),以及直线 O1 p1 与 x 轴所成的角度 兹21 = arctan (p1z / p1x ). 其 中 兹21 沂 [ - 仔/ 2, 仔/ 2 ], 驻兹2 沂 [ 0, 仔/ 2]. 考虑到实际避障过程中,障碍物下方还有无须 修剪的绿篱,为简化运算将障碍物下方全视为碰撞 发生的运动范围. 令 兹2max1 = 兹21 + 驻兹2 ,关节 2 引起 连杆 2 碰撞时的旋转角限度为 兹2沂[ - 仔/ 2,兹2max1 ]. 当障碍物分布于连杆 2 运动空间平面内的其他两个 象限时,令 兹2min2 = 仔 + 兹21 - 驻兹2 ,则关节 2 另一部分 旋转角限度为 兹2沂[兹2min2 ,3仔/ 2]. 因此对于随意设 定的 兹1 ,关节 2 引起连杆 2 碰撞时的总旋转角限度 为 兹2沂专2 = [ - 仔/ 2,兹2max1 ]胰[兹2min2 ,3仔/ 2]. 由上述分析方法可得连杆 3、4 关节旋转角总限 度 兹3沂专3 = [ - 仔/ 2,兹3max1 ] 胰[ 兹3min2 ,3仔/ 2]、兹4 沂 专4 = [ - 仔/ 2,兹4max1 ]胰[兹4min2 ,3仔/ 2]. 最后根据以上所推导出的各连杆碰撞条件,求 解出修剪机械臂在非结构环境下运动的避碰空间. 3 基于扰动人工势场算法的路径规划 3郾 1 斥力场调节策略 人工势场算法因结构简单、计算量小、实时性 好,常运用于机器人的路径规划领域[17] ,但存在目 标不可达和局部极小点等问题[1] . 目标不可达问题 是由于目标点距离障碍物过近,在目标点处合力不 为零导致. 针对此问题,采用一种斥力场调节策略, 即加入距离尺度因子(X - Xg) n 对斥力场函数模型 ·137·

·138· 工程科学学报，第41卷，第1期 进行优化，促使机械臂在目标点处合力为零，变成全 臂的F.(X),如式(16)所示，以及沿目标点指向机 局最小点.人工势场算法中引力和斥力势函数分别 械臂的F(X),如式(17)所示. 表示为 3.2斥力场扰动机制 U(X)=0.5kp2(X,X) (9) 通常有两种情况导致人工势场算法出现局部 Up (X)= 极小点问题，造成机械臂无法成功抵达目标点或 0.5k 11)2 在一定范围内徘徊抖动.一是当障碍物位于机械 e(X.X).). p(X,X)≤pb 臂和目标点之间，且三者共线时发生；二是当机械 0 p(X,Xa）>Pb 臂处于多障碍物非结构环境中，受到多个斥力与 (10) 引力共同作用，达到合力平衡时发生.针对上述问 其中，k。与k,分别为引力和斥力增益尺度因子，且 题，提出一种斥力场扰动机制，当机械臂在运动过 k,与k均为正实数，X=[xy]、X。与X分别 程中陷入局部极小点问题时，随机在运动空间中 表示机械臂、目标点以及障碍物的空间位置向量，· 出现黑洞，对斥力产生扰动效应，使总斥力在此时 (X,X)=‖X-X.‖p(X,Xa)=d.分别表示机械 绕基坐标系z轴偏转一定的角度中，来调整斥力 臂与目标点、障碍物i的空间距离，P,为障碍物斥力 影响方式，再进行引力和斥力合成，以致对机械 场作用半径 臂施加的合力非零，解决此时的局部极小点问 对引力和斥力势函数进行负梯度计算，可推导 题.当多次陷入局部极小点问题时，可重复扰动 出对应的引力和斥力函数，表达式分别为 斥力：若没有，则无须扰动.扰动斥力与总合力表 F(X)=-VUm(X)=-k.(X-X.)(11) 达式分别为 Fi (X)=-VUpi (X) c0s中 -sin中0 Ft=Rp(中)Fp sin o cos d da≤pb 0 0 (12) (18) 0. dui>P Fiotal F rot +Fatt (19) 因此，当空间内有n个障碍物或威胁区时，对机 其中，p∈[-/2，/2]，R(中)为斥力旋转向量. 械臂施加的总合力可表示为 3.3扰动人工势场算法的步骤描述 R()=f.(W+会F(W） (13) 在进行机械臂路径规划过程中，利用数值解析 改进后的调节斥力场函数为 法进行运动学逆解，求得当前时刻对应关节角向量 0=[0,0266,9,]T的可行解.为减少机械臂 Upi (X)= 广(x-x八 路径规划中的能量消耗，降低关节角增量和，建立目 0 标优化模型函数minf(0),求取最优关节角向量. dui >Pu minf(g)函数表达式为 (14) minf8)=(0,-gm)2+ 其中，d表示机械臂与障碍物i最短空间距离，n表 示任意正实数，因此对应的斥力函数表达式为 含e-)-(0-,(20) (F:+Fi,dt≤p Fpi (X)=-VUpi (X)= 其中，和0：分别表示机械臂起始和当前时刻运动 0. doi>pu 的关节角i. (15) 本文基于扰动人工势场算法在避碰空间内进行 其中， 高速公路绿篱修剪机械臂的避障路径规划，算法流 6民是-)尝 程图如图6所示，具体流程描述如下： (16) 步骤1初始化算法参数、机械臂起始关节角 F2.(X)=0.5nk 以及目标点、障碍物位置，根据式(3)和式(4)获取 ax 机械臂末端位姿信息. (17) 步骤2根据式(11)和式(15)计算出机械臂 加入距离尺度因子调节斥力场函数模型后，对 所受合力，并由合力运动到下一位置，记下当前位置 应的斥力则分为了两个分量，即沿障碍物指向机械 信息

工程科学学报,第 41 卷,第 1 期 进行优化,促使机械臂在目标点处合力为零,变成全 局最小点. 人工势场算法中引力和斥力势函数分别 表示为 Uatt(X) = 0郾 5ka 籽 2 (X,Xg) (9) Urepi(X) = 0郾 5kb ( 1 籽(X,Xbi) - 1 籽 ) b 2 , 籽(X,Xbi)臆籽b 0, 籽(X,Xbi) > 籽 ì î í ïï ïï b (10) 其中,ka 与 kb 分别为引力和斥力增益尺度因子,且 ka 与 kb 均为正实数,X = [x y z] T 、Xg 与 Xb 分别 表示机械臂、目标点以及障碍物的空间位置向量,籽 (X,Xg) = 椰X - Xg椰、籽(X,Xbi) = dbi分别表示机械 臂与目标点、障碍物 i 的空间距离,籽b 为障碍物斥力 场作用半径. 对引力和斥力势函数进行负梯度计算,可推导 出对应的引力和斥力函数,表达式分别为 Fatt(X) = - 驻 Uatt(X) = - ka(X - Xg) (11) Frepi(X) = - 驻 Urepi(X) = kb ( 1 dbi - 1 籽 ) b 1 d 2 bi 鄣dbi 鄣X , dbi臆籽b 0, dbi > 籽 ì î í ïï ïï b (12) 因此,当空间内有 n 个障碍物或威胁区时,对机 械臂施加的总合力可表示为 Ftotal(X) = Fatt(X) + 移 n i = 1 Frepi(X) (13) 改进后的调节斥力场函数为 Urepi(X) = 0郾 5kb ( 1 dbi - 1 籽 ) b ( 2 X - Xg ) n , dbi臆籽b 0, dbi > 籽 ì î í ïï ïï b (14) 其中,dbi表示机械臂与障碍物 i 最短空间距离,n 表 示任意正实数,因此对应的斥力函数表达式为 Frepi(X) = - 驻 Urepi(X) = F1i + F2i, dbi臆籽b {0, dbi > 籽b (15) 其中, F1i(X) = kb ( 1 dbi - 1 籽 ) b 1 d 2 bi (X - Xg) n 鄣dbi 鄣X (16) F2i(X) = 0郾 5nkb ( 1 dbi - 1 籽 ) b 2 (X - Xg) n - 1 鄣(X - Xg) 鄣X (17) 加入距离尺度因子调节斥力场函数模型后,对 应的斥力则分为了两个分量,即沿障碍物指向机械 臂的 F1i(X),如式(16)所示,以及沿目标点指向机 械臂的 F2i(X),如式(17)所示. 3郾 2 斥力场扰动机制 通常有两种情况导致人工势场算法出现局部 极小点问题,造成机械臂无法成功抵达目标点或 在一定范围内徘徊抖动. 一是当障碍物位于机械 臂和目标点之间,且三者共线时发生;二是当机械 臂处于多障碍物非结构环境中,受到多个斥力与 引力共同作用,达到合力平衡时发生. 针对上述问 题,提出一种斥力场扰动机制,当机械臂在运动过 程中陷入局部极小点问题时,随机在运动空间中 出现黑洞,对斥力产生扰动效应,使总斥力在此时 绕基坐标系 z 轴偏转一定的角度 准,来调整斥力 影响方式,再进行引力和斥力合成,以致对机械 臂施加的合力非零,解决此时的 局 部 极 小 点 问 题. 当多次陷入局部极小点问题时,可重复扰动 斥力;若没有,则无须扰动. 扰动斥力与总合力表 达式分别为 Frot = Rrep (准)Frep = cos 准 - sin 准 0 sin 准 cos 准 0 é ë ê ê ê ù û ú ú ú 0 0 1 Frep (18) Ftotal = Frot + Fatt (19) 其中,渍沂[ - 仔/ 2,仔/ 2],Rrep (准)为斥力旋转向量. 3郾 3 扰动人工势场算法的步骤描述 在进行机械臂路径规划过程中,利用数值解析 法进行运动学逆解,求得当前时刻对应关节角向量 兹 = [兹1 兹2 兹3 兹4 兹5 ] T 的可行解. 为减少机械臂 路径规划中的能量消耗,降低关节角增量和,建立目 标优化模型函数 min f( 兹),求取最优关节角向量. min f(兹)函数表达式为 min f(兹) = (兹1 - 兹 ini 1 ) 2 + 移 5 i = 2 [(兹i - 兹 ini i ) - (兹i - 1 - 兹 ini i - 1 )] 2 (20) 其中,兹 ini i 和 兹i 分别表示机械臂起始和当前时刻运动 的关节角 i. 本文基于扰动人工势场算法在避碰空间内进行 高速公路绿篱修剪机械臂的避障路径规划,算法流 程图如图 6 所示,具体流程描述如下: 步骤 1 初始化算法参数、机械臂起始关节角 以及目标点、障碍物位置,根据式(3)和式(4)获取 机械臂末端位姿信息. 步骤 2 根据式(11) 和式(15) 计算出机械臂 所受合力,并由合力运动到下一位置,记下当前位置 信息. ·138·

罗天洪等：高速公路绿篱修剪机器人手臂避障路径规划 ·139· 步骤3在关节角约束与避碰空间下，进行运 角向量，通过式(13)判断当前位置是否为局部极小 动学逆解，若关节角有可行解，直接转入下一步：否 点，若Fm为零，则是，通过式(19)解决，再转入下 则令当前位置为起始点，判断是否为局部极小点，若 一光. 是，则通过式(19)解决，再转入步骤2 步骤5判断机械臂是否到达目标点，若到达， 步骤4根据式(20)求取并保存当前最优关节 则输出关节角向量，否则转入步骤2 否+ 是否为局部极小点 尽 引入斥力场扰动机制解决 开始 初始化参数与环境信息 否 (机械臂初始关节角、 是否有关节角可行解 目标点及障碍物位置) 是 进行运动学正解，获取 求取并保存当前最优关节角 机械臂末端位姿信息 向量（目标优化模型函数） 以及与障碍物距离信息 进行机械臂路径规划 是 是否为局部极小点 引入斥力场扰动机制解决 计算机械臂在人工势场中 否上 所受合力，根据合力运动到 否 下一步位置 是否为目标点 是 在关节角自身约束与避碰 输出关节角向量 空间内利用数值解析法 进行运动学逆解 结束 图6扰动人工势场算法流程图 Fig.6 Flow chart of PAPF algorithm 本文基于扰动人工势场算法与人工势场算法 4 仿真研究 分别进行避障规划仿真，为保证仿真的通用性，检 为验证以上提出的在避碰空间内基于扰动人工 验该算法是否解决传统算法中的局部极小点、目 势场算法避障规划的可行性和有效性，将在MAT- 标不可达问题以及机械臂路径规划中的实时准确 LAB8.3环境中进行仿真.以设计的高速公路绿篱 避障问题，设置带有局部极小点条件的单障碍物 修剪机器人手臂为研究对象，连杆简化模型如图2 环境，即起始点、障碍物与目标点三者共线；带有 所示，具体的连杆参数如表1所示，球形包络障碍物 局部极小点条件的多障碍物环境，即三者共面：带 如图3所示，结合避碰空间规划出机械臂末端点的 有目标不可达条件的多障碍物环境，即目标点处 运动路径，利用数值解析法和式(20)中的目标优化 于障碍物斥力场影响范围内.具体算法参数设置 模型求解出最优关节角向量. 如表2所示. 表2算法基本参数设置 Table 2 Basic parameter setting of the algorithm 引力增益尺度 斥力增益尺度 斥力场作用 距离尺度因子 斥力偏 步长， 最大循环迭代 因子，k 因子，k 半径，PL/m 系数，n 转角，/() V/m 次数，N 3 0.5 30 0.1 % 实验一：带有局部极小点条件的单个障碍物环 实验二：带有局部极小点条件的多个障碍物环 境，障碍物中心坐标为(105,95,112)，半径为25 境，障碍物中心坐标分别为(100,70,112)、(180， cm.机械臂起始关节角为(0°，120°，-120°，60°， 105,112),半径分别为10cm和30cm.机械臂起始 180°，0°)，目标点坐标为(105,163,112). 关节角为(0°，120°，-120°，60°，180°，0)，目标点

罗天洪等: 高速公路绿篱修剪机器人手臂避障路径规划 步骤 3 在关节角约束与避碰空间下,进行运 动学逆解,若关节角有可行解,直接转入下一步;否 则令当前位置为起始点,判断是否为局部极小点,若 是,则通过式(19)解决,再转入步骤 2. 步骤 4 根据式(20)求取并保存当前最优关节 角向量,通过式(13)判断当前位置是否为局部极小 点,若 Ftotal为零,则是,通过式(19)解决,再转入下 一步. 步骤 5 判断机械臂是否到达目标点,若到达, 则输出关节角向量,否则转入步骤 2. 图 6 扰动人工势场算法流程图 Fig. 6 Flow chart of PAPF algorithm 4 仿真研究 为验证以上提出的在避碰空间内基于扰动人工 势场算法避障规划的可行性和有效性,将在 MAT鄄 LAB 8郾 3 环境中进行仿真. 以设计的高速公路绿篱 修剪机器人手臂为研究对象,连杆简化模型如图 2 所示,具体的连杆参数如表 1 所示,球形包络障碍物 如图 3 所示,结合避碰空间规划出机械臂末端点的 运动路径,利用数值解析法和式(20)中的目标优化 模型求解出最优关节角向量. 本文基于扰动人工势场算法与人工势场算法 分别进行避障规划仿真,为保证仿真的通用性,检 验该算法是否解决传统算法中的局部极小点、目 标不可达问题以及机械臂路径规划中的实时准确 避障问题,设置带有局部极小点条件的单障碍物 环境,即起始点、障碍物与目标点三者共线;带有 局部极小点条件的多障碍物环境,即三者共面;带 有目标不可达条件的多障碍物环境,即目标点处 于障碍物斥力场影响范围内. 具体算法参数设置 如表 2 所示. 表 2 算法基本参数设置 Table 2 Basic parameter setting of the algorithm 引力增益尺度 因子,ka 斥力增益尺度 因子,kb 斥力场作用 半径,籽b / m 距离尺度因子 系数,n 斥力偏 转角,准/ (毅) 步长, l / m 最大循环迭代 次数,N 1 2 0郾 5 2 30 0郾 1 50 实验一:带有局部极小点条件的单个障碍物环 境,障碍物中心坐标为(105,95,112),半径为 25 cm. 机械臂起始关节角为(0毅,120毅, - 120毅,60毅, 180毅,0毅),目标点坐标为(105,163,112). 实验二:带有局部极小点条件的多个障碍物环 境,障碍物中心坐标分别为(100,70,112)、(180, 105,112),半径分别为 10 cm 和 30 cm. 机械臂起始 关节角为(0毅,120毅, - 120毅,60毅,180毅,0毅),目标点 ·139·

·140. 工程科学学报，第41卷，第1期 坐标为(123,142,112) 臂避障规划仿真效果图.从图7(a)和图7(c)可以 实验三：带有目标不可达条件的多个障碍物环 看出，机械臂在运动过程中陷入了局部极小点问题， 境，障碍物中心坐标分别为(100,60,132)、(160， 并停止了运动，而图7(b)和7(d)中，机械臂在陷入 75,180)、(170,110,200),半径分别为18、12和15 局部极小点问题时，通过扰动机制顺利跳出，继续前 cm.机械臂起始关节角为(0°，120°，-120°，60°， 行，且成功躲避障碍到达目标点.图7()中，机械 180°，0°)，目标点的坐标为(163,129,205). 臂因目标点距离障碍物较近，发生目标不可达现象， 图7中，各直线段表示机械臂连杆，球形表示障 而图7()中，通过调节策略，机械臂最终成功躲避 碍物，两实心点分别表示起始点与目标点.图7 障碍，顺利抵达目标点.从图7(a)、(c)和(e)可 (b)、7(d)、7(f)与图7(a)、7(c)和7(e)分别表示 以看出，机械臂在多种类型的障碍物环境中，均能 基于扰动人工势场算法和基于人工势场算法的机械 够根据障碍物的不同特征参数而不断地在线搜索 a 150 150 100 100 50 0 -50 ● 150 50 150 0 100 0 100 X/cm 50 50 50 0 Yem 100 100 0 150 s100 150 50 50 100 200 7150 50 150 100 50 0 100 '50 50 X/cm 150 200 0 250 (e) 250 200 i(f) 200 150 100 150 50 100 208 100 0 100 50 150 200 100 100 -100-100 0 50 0 Xicm 图7机械臂避障规划仿真效果图.(a)实验一人工势场算法：(b)实验一扰动人工势场算法：（©）实验二人工势场算法：(d)实验二扰动 人工势场算法：()实验三人工势场算法：(f)实验三扰动人工势场算法 Fig.7 Path planning simulation for manipulator:(a)APF algorithm in Test 1;(b)PAPF algorithm in Test 1;(c)APF algorithm in Test 2;(d) PAPF algorithm in Test 2;(e)APF algorithm in Test 3;(f)PAPF algorithm in Test 3

工程科学学报,第 41 卷,第 1 期 坐标为(123,142,112). 实验三:带有目标不可达条件的多个障碍物环 境,障碍物中心坐标分别为(100,60,132)、(160, 75,180)、(170,110,200),半径分别为 18、12 和 15 cm. 机械臂起始关节角为(0毅,120毅, - 120毅,60毅, 180毅,0毅),目标点的坐标为(163,129,205). 图 7 机械臂避障规划仿真效果图. (a) 实验一人工势场算法;(b) 实验一扰动人工势场算法;(c) 实验二人工势场算法;( d) 实验二扰动 人工势场算法;(e) 实验三人工势场算法;(f) 实验三扰动人工势场算法 Fig. 7 Path planning simulation for manipulator: (a) APF algorithm in Test 1; (b) PAPF algorithm in Test 1; (c) APF algorithm in Test 2; (d) PAPF algorithm in Test 2; (e) APF algorithm in Test 3; (f) PAPF algorithm in Test 3 图 7 中,各直线段表示机械臂连杆,球形表示障 碍物,两实心点分别表示起始点与目标点. 图 7 (b)、7(d)、7(f)与图 7( a)、7( c)和 7( e)分别表示 基于扰动人工势场算法和基于人工势场算法的机械 臂避障规划仿真效果图. 从图 7( a)和图 7( c)可以 看出,机械臂在运动过程中陷入了局部极小点问题, 并停止了运动,而图 7(b)和 7(d)中,机械臂在陷入 局部极小点问题时,通过扰动机制顺利跳出,继续前 行,且成功躲避障碍到达目标点. 图 7( e) 中,机械 臂因目标点距离障碍物较近,发生目标不可达现象, 而图 7( f)中,通过调节策略,机械臂最终成功躲避 障碍,顺利抵达目标点. 从图 7 ( a) 、( c) 和( e) 可 以看出,机械臂在多种类型的障碍物环境中,均能 够根据障碍物的不同特征参数而不断地在线搜索 ·140·

罗天洪等：高速公路绿篱修剪机器人手臂避障路径规划 ·141· 出合理路径来躲避障碍物，体现了避障规划的实 动变化.综上可见，在避碰空间内基于扰动人工势 时性.机械臂在运动过程中，除了末端避开了障碍 场算法的机械臂路径规划，避障性强，实时性好， 物，其各臂杆也与障碍物并无发生碰撞，最终顺利 准确性高，运动稳定平滑，速度连续，路径轨迹安 到达目标点，体现了避障规划的准确性.图8中， 全、柔顺，有效解决了人工势场算法存在的局部极 图(a)、(b)和图(c)、(d)以及图(e)、(f)分别表 小点和目标不可达等问题，验证了该方法的可行 示图7(b)、(d)和()的机械臂关节角和角速度运 性与有效性. a 0.150 0.10 关节4 关节5 关节2 0.05 一关节3 关节1 邑 关节5 .) 0- 关 关节4 0 关节1 节2 -0.05 节3 关节2 一关节1 节4 …关节2 关节3 头 -0.10 关节3 一关节4 0.15 关节5 10 20 3 0.206 10 15 20 25 时间s 时间s 0.10 (e) d 关节I 0.08 关节4 关节2 0.06 关节1 节3 节4 0.04 关节3 关节 关节1 关节5 关节1 0.02 关节5 关节2 关节4 关节3 0.02 关节2 -0.04 0.06 10 15 20 25 30 10 15 20 35 时间/s 时间s ⊙ 0.10 关节4 0.08 节2 节3 关节2 0.06 节4 关节3 0.04 关节1 关节 0.02 关节5 关节5 关节4 关节3 -0.02 一X节3 关节1 关节4 -0.04 关节2 关节5 关节2 -0D6 10 15 20 25 30 10 15 20 30 时间s 时间/s 图8机械臂运动关节参数变化图.(a)实验一关节角：(b)实验一关节角速度：(c)实验二关节角：(d)实验二关节角速度：(e)实验三关 节角：()实验三关节角速度 Fig.8 Motion parameters of manipulator joints:(a)angle of joint in Test 1;(b)angular velocity of joint in Test 1;(c)angle of joint in Test 2; (d)angular velocity of joint in Test 2;(e)angle of joint in Test 3;(f)angular velocity of joint in Test 3 生碰撞的条件，求解机械臂的避碰空间.在传统人 5结论 工势场算法的基础上，引入斥力场调节策略与扰动 (1)以设计的高速公路绿篱修剪机器人手臂为 机制，解决其局部极小点和目标不可达等问题，从而 研究对象，针对其在非结构环境下的实时准确避障 实现机械臂在避碰空间内的避障路径规划. 问题，提出一种基于扰动人工势场算法进行避障路 (2)在MATLAB虚拟仿真平台中进行验证，仿 径规划的解决方法.按照修剪机械臂具体构型以及 真结果表明，在避碰空间内基于扰动人工势场算法 障碍物时空特征，创建简化模型.通过相互之间的 的避障路径规划，能够有效解决局部极小点和目标 位置关系将障碍物映射到关节空间，分析各连杆发 不可达等问题，使机械臂顺利灵活避障，成功抵达目

罗天洪等: 高速公路绿篱修剪机器人手臂避障路径规划 出合理路径来躲避障碍物,体现了避障规划的实 时性. 机械臂在运动过程中,除了末端避开了障碍 物,其各臂杆也与障碍物并无发生碰撞,最终顺利 到达目标点,体现了避障规划的准确性. 图 8 中, 图( a) 、( b)和图( c) 、( d) 以及图( e) 、( f) 分别表 示图 7( b) 、( d)和( f)的机械臂关节角和角速度运 动变化. 综上可见,在避碰空间内基于扰动人工势 场算法的机械臂路径规划,避障性强,实时性好, 准确性高,运动稳定平滑,速度连续,路径轨迹安 全、柔顺,有效解决了人工势场算法存在的局部极 小点和目标不可达等问题,验证了该方法的可行 性与有效性. 图 8 机械臂运动关节参数变化图. (a) 实验一关节角;(b) 实验一关节角速度;(c) 实验二关节角;(d) 实验二关节角速度;(e) 实验三关 节角;(f) 实验三关节角速度 Fig. 8 Motion parameters of manipulator joints: (a) angle of joint in Test 1; (b) angular velocity of joint in Test 1; (c) angle of joint in Test 2; (d) angular velocity of joint in Test 2; (e) angle of joint in Test 3; (f) angular velocity of joint in Test 3 5 结论 (1)以设计的高速公路绿篱修剪机器人手臂为 研究对象,针对其在非结构环境下的实时准确避障 问题,提出一种基于扰动人工势场算法进行避障路 径规划的解决方法. 按照修剪机械臂具体构型以及 障碍物时空特征,创建简化模型. 通过相互之间的 位置关系将障碍物映射到关节空间,分析各连杆发 生碰撞的条件,求解机械臂的避碰空间. 在传统人 工势场算法的基础上,引入斥力场调节策略与扰动 机制,解决其局部极小点和目标不可达等问题,从而 实现机械臂在避碰空间内的避障路径规划. (2)在 MATLAB 虚拟仿真平台中进行验证,仿 真结果表明,在避碰空间内基于扰动人工势场算法 的避障路径规划,能够有效解决局部极小点和目标 不可达等问题,使机械臂顺利灵活避障,成功抵达目 ·141·

·142· 工程科学学报，第41卷，第1期 标点，并且实时性好、准确性高，验证了该方法的有 (丑武胜，王朔.基于虚拟力的的冗余度机器人自主避障研 效性和可行性 究.中国机械工程，2011,22(24)：2899) (3)本文仅在仿真平台对该避障路径规划方法 [9]Xie B Y,Zhao J,Liu Y.Motion planning of reaching point move- ments for 7R robotic manipulators in obstacle environment based on 进行了验证，样机实验将在以后的工作中作进一步 rapidly-exploring random tree algorithm.Mech Eng,2012,48 研究 (3):63 (谢碧云，赵京，刘宇.基于快速扩展随机树的7R机械臂避 参考文献 障达点运动规划.机械工程学报.2012,48(3)：63) [10]Wang S K,Zhu L,Wang JZ.Path plan of 6-DOF robot manipu- [1]Gao H,Zhang M L,Zhang X J.A review of the space trajectory lators in obstacle environment based on navigation potential func- planning of redundant manipulator.J Mech Transmiss,2016,40 tion.Trans Beijing Inst Technol,2015,35(2):186 (10):176 (汪首坤，朱磊，王军政.基于导航势函数法的六自由度机 (高涵，张明路，张小俊.冗余机械臂空间轨迹规划综述.机 械臂避障路径规划.北京理工大学学报，2015,35(2)：186) 械传动.2016,40(10)：176) [11]Ji W,Cheng F Y,Zhao D A,et al.Obstacle avoidance method [2]Yin J J.Wu C Y,Yang S X,et al.Obstacle-avoidance path plan- of apple harvesting robot manipulator.Trans Chin Soc Agric ning of robot arm for tomato-picking robot.Trans Chin Soc Agric Mach,2013,44(11):253 Mach,2012,43(12):171 (姬伟，程风仪，赵德安，等.基于改进人工势场的苹果采摘 (尹建军，武传宇，Yang Simon X,等.番茄采摘机器人机械臂 机器人机械手避障方法.农业机械学报，2013,4(11)： 避障路径规划.农业机械学报，2012,43(12)：171) 253) [3]Jia QX,Zhang Q R.Gao X.et al.Dynamic obstacle avoidance [12]Qi R L,Zhou W J,Wang T J.An obstacle avoidance trajectory algorithm for redundant robots with pre-selected minimum distance planning scheme for space manipulators based on genetic algo- index.Robot,2013,35(1):17 rithm.Robot,2014,36(3):263 (贾庆轩，张倩茹，高欣，等.预选择最小距离指标的冗余机 (祁若龙，周维佳，王铁军。一种基于遗传算法的空间机械 器人动态避障算法.机器人，2013,35(1)：17) 臂避障轨迹规划方法.机器人，2014,36(3)：263) [4]Flacco F,De Luca A,Khatib O.Motion control of redundant ro- [13]Machmudah A,Parman S,Zainuddin A,et al.Polynomial joint bots under joint constraints:saturation in the null space /Pro- angle arm robot motion planning in complex geometrical obsta- ceedings of 2012 IEEE International Conference on Robotics and cles.Appl Soft Comput,2013,13(2):1099 Automation.Saint Paul,2012:285 [14]Toshani H,Farrokhi M.Real-time inverse kinematics of redun- [5]Chyan G S,Ponnambalam S G.Obstacle avoidance control of re- dant manipulators using neural networks and quadratic program- dundant robots using variants of particle swarm optimization.Rob ming:a Lyapunow-based approach.Rob Autonom Syst,2014,62 Comput Integr Manuf,2012,28(2):147 (6):766 [6]Wang S K,Di Z,Wang J Z,et al.Path planning method for ma- [15]Hasan A T,Ismail N,Hamouda A M S,et al.Artificial neural nipulator to avoid obstacle based on advanced A algorithm.Trans network-based kinematics Jacobian solution for serial manipulator Beijing Inst Technol,2011,31(11)1302 passing through singular configurations.Adr Eng Software, (汪首坤，邸智，王军政，等.基于A·改进算法的机械臂避 2010,41(2):359 障路径规划.北京理工大学学报，2011,31(11)：1302) [16]Jia Q X,Chen G,Sun H X,et al.Path planning for space ma- [7]Lin C J,Li T HS,Kuo P H,et al.Integrated particle swarm op- nipulator to avoid obstacle based on A'algorithm.Mech Eng, timization algorithm based obstacle avoidance control design for 2010.46(13):109 home service robot.Comput Electr Eng,2016,56:748 (贾庆轩，陈钢，孙汉旭，等.基于A·算法的空间机械臂避 [8]Chou W S,Wang S.Research on obstacle avoidance by virtual 障路径规划.机械工程学报，2010,46(13)：109) force for redundant robot.China Mech Eng,2011,22 (24): [17]Khatib 0.Real-time obstacle avoidance for manipulators and mo- 2899) bile robots.Int J Rob Res,1986,5(1):90

工程科学学报,第 41 卷,第 1 期 标点,并且实时性好、准确性高,验证了该方法的有 效性和可行性. (3)本文仅在仿真平台对该避障路径规划方法 进行了验证,样机实验将在以后的工作中作进一步 研究. 参 考 文 献 [1] Gao H, Zhang M L, Zhang X J. A review of the space trajectory planning of redundant manipulator. J Mech Transmiss, 2016, 40 (10): 176 (高涵, 张明路, 张小俊. 冗余机械臂空间轨迹规划综述. 机 械传动, 2016, 40(10): 176) [2] Yin J J, Wu C Y, Yang S X, et al. Obstacle鄄avoidance path plan鄄 ning of robot arm for tomato鄄picking robot. Trans Chin Soc Agric Mach, 2012, 43(12): 171 (尹建军, 武传宇, Yang Simon X, 等. 番茄采摘机器人机械臂 避障路径规划. 农业机械学报, 2012, 43(12): 171) [3] Jia Q X, Zhang Q R, Gao X, et al. Dynamic obstacle avoidance algorithm for redundant robots with pre鄄selected minimum distance index. Robot, 2013, 35(1): 17 (贾庆轩, 张倩茹, 高欣, 等. 预选择最小距离指标的冗余机 器人动态避障算法. 机器人, 2013, 35(1): 17) [4] Flacco F, De Luca A, Khatib O. Motion control of redundant ro鄄 bots under joint constraints: saturation in the null space / / Pro鄄 ceedings of 2012 IEEE International Conference on Robotics and Automation. Saint Paul, 2012: 285 [5] Chyan G S, Ponnambalam S G. Obstacle avoidance control of re鄄 dundant robots using variants of particle swarm optimization. Rob Comput Integr Manuf, 2012, 28(2): 147 [6] Wang S K, Di Z, Wang J Z, et al. Path planning method for ma鄄 nipulator to avoid obstacle based on advanced A * algorithm. Trans Beijing Inst Technol, 2011, 31(11): 1302 (汪首坤, 邸智, 王军政, 等. 基于 A * 改进算法的机械臂避 障路径规划. 北京理工大学学报, 2011, 31(11): 1302) [7] Lin C J, Li T H S, Kuo P H, et al. Integrated particle swarm op鄄 timization algorithm based obstacle avoidance control design for home service robot. Comput Electr Eng, 2016, 56: 748 [8] Chou W S, Wang S. Research on obstacle avoidance by virtual force for redundant robot. China Mech Eng, 2011, 22 ( 24 ): 2899) (丑武胜, 王朔. 基于虚拟力的的冗余度机器人自主避障研 究. 中国机械工程, 2011, 22(24): 2899) [9] Xie B Y, Zhao J, Liu Y. Motion planning of reaching point move鄄 ments for 7R robotic manipulators in obstacle environment based on rapidly鄄exploring random tree algorithm. J Mech Eng, 2012, 48 (3): 63 (谢碧云, 赵京, 刘宇. 基于快速扩展随机树的 7R 机械臂避 障达点运动规划. 机械工程学报, 2012, 48(3): 63) [10] Wang S K, Zhu L, Wang J Z. Path plan of 6鄄DOF robot manipu鄄 lators in obstacle environment based on navigation potential func鄄 tion. Trans Beijing Inst Technol, 2015, 35(2): 186 (汪首坤, 朱磊, 王军政. 基于导航势函数法的六自由度机 械臂避障路径规划. 北京理工大学学报, 2015, 35(2): 186) [11] Ji W, Cheng F Y, Zhao D A, et al. Obstacle avoidance method of apple harvesting robot manipulator. Trans Chin Soc Agric Mach, 2013, 44(11): 253 (姬伟, 程风仪, 赵德安, 等. 基于改进人工势场的苹果采摘 机器人机械手避障方法. 农业机械学报, 2013, 44 (11 ): 253) [12] Qi R L, Zhou W J, Wang T J. An obstacle avoidance trajectory planning scheme for space manipulators based on genetic algo鄄 rithm. Robot, 2014, 36(3): 263 (祁若龙, 周维佳, 王铁军. 一种基于遗传算法的空间机械 臂避障轨迹规划方法. 机器人, 2014, 36(3): 263) [13] Machmudah A, Parman S, Zainuddin A, et al. Polynomial joint angle arm robot motion planning in complex geometrical obsta鄄 cles. Appl Soft Comput, 2013, 13(2): 1099 [14] Toshani H, Farrokhi M. Real鄄time inverse kinematics of redun鄄 dant manipulators using neural networks and quadratic program鄄 ming: a Lyapunow鄄based approach. Rob Autonom Syst, 2014, 62 (6): 766 [15] Hasan A T, Ismail N, Hamouda A M S, et al. Artificial neural network鄄based kinematics Jacobian solution for serial manipulator passing through singular configurations. Adv Eng Software, 2010, 41(2): 359 [16] Jia Q X, Chen G, Sun H X, et al. Path planning for space ma鄄 nipulator to avoid obstacle based on A * algorithm. J Mech Eng, 2010, 46(13): 109 (贾庆轩, 陈钢, 孙汉旭, 等. 基于 A * 算法的空间机械臂避 障路径规划. 机械工程学报, 2010, 46(13): 109) [17] Khatib O. Real鄄time obstacle avoidance for manipulators and mo鄄 bile robots. Int J Rob Res, 1986, 5(1): 90 ·142·