工程科学学报，第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是 否会发生碰撞.若d2<rg+a2(rg为包络球形障碍 3基于扰动人工势场算法的路径规划 物半径，d2为连杆2起点O,到障碍物球心P。的距 3.1斥力场调节策略 离，a2为连杆2长度)，则不会发生碰撞，无须进一 人工势场算法因结构简单、计算量小、实时性 步分析：否则，应进行如下计算.关节2引起碰撞时 好，常运用于机器人的路径规划领域)，但存在目 的连杆2极限位形如图5所示. 标不可达和局部极小点等问题).目标不可达问题 根据障碍物球心到连杆2运动平面的距离d21, 是由于目标点距离障碍物过近，在目标点处合力不 判断连杆2是否在可能碰撞的范围内，若d2!>Tg+ 为零导致.针对此问题，采用一种斥力场调节策略， 「裕（「辎为机械臂连杆碰撞余量），则不在范围内：否 即加入距离尺度因子(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 + a2 ( rB 为包络球形障碍 物半径,d2 为连杆 2 起点 O1 到障碍物球心 P0 的距 离,a2 为连杆 2 长度),则不会发生碰撞,无须进一 步分析;否则,应进行如下计算. 关节 2 引起碰撞时 的连杆 2 极限位形如图 5 所示. 根据障碍物球心到连杆 2 运动平面的距离 d21 , 判断连杆 2 是否在可能碰撞的范围内,若 d21 > 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·