带电作业机器人机械臂动力学建模与运动规划

工程科学学报，第38卷，第6期：867-875,2016年6月 Chinese Journal of Engineering,Vol.38,No.6:867-875,June 2016 DOI:10.13374/j.issn2095-9389.2016.06.018:http://journals.ustb.edu.cn 带电作业机器人机械臂动力学建模与运动规划 江 维2，吴功平12》，王伟12”，张颉2) 1)武汉大学动力与机械学院，武汉4300722)武汉大学机器人研究所，武汉430072 ☒通信作者，E-mail:jiangwei.2013@whu.edu.cm 摘要研制了一种双臂、双机械手的绝缘子串更换带电作业机器人·分解了机器人作业过程中机械臂的运动.利用拉格朗 日法并结合关节电机电枢电压方程对机器人机械臂基本动作进行动力学建模，得出机械臂基本动作的统一动力学模型，计算 出机械臂2的动力学方程，提出基于五次多项式插值的机械臂运动轨迹规划方法，计算出机械臂2各关节运动方程.在 ADAMS:环境下进行机器人机械臂动力学和运动学仿真，结果验证了动力学模型的正确性，同时各关节运动轨迹规划满足运动 学要求.最后在实际线路上进行机器人带电更换绝缘子串实验，结果表明机器人机械臂各关节运动获得了较好的动态性能， 验证了本文提出的五次多项式插值机械臂运动轨迹规划具有较强的工程实用性，进一步提高了机器人的作业效率和稳定性. 关键词机器人：机械手；动力学建模；运动规划 分类号TP242.3 Manipulator dynamic modeling and motion planning for live working robot JIANG Wei,WU Gong-ping),WANG Wei,ZHANG Jie 1)School of Power and Mechanical Engineering,Wuhan University,Wuhan 430072,China 2)Robot Research Institute,Wuhan University,Wuhan 430072,China Corresponding author,E-mail:jiangwei2013@whu.edu.cn ABSTRACT This article is focused on a double-arm and double-manipulator live working robot.The arm motion of the robot for insulator string replacement during the operation process was decomposed.The basic operation dynamic model was established by the Lagrange method combined with the armature voltage equation,the dynamic equations of manipulator 2 were calculated,the manipula- tor motion trajectory planning method was proposed based on five-polynomial interpolation,and the kinematics equations of manipulator 2 each joint were calculated.Robot manipulator dynamics and kinematics simulations were performed in ADAMS environment.The re- sults verify the correctness of the dynamic model,and the joint trajectory planning meets the kinematic requirements.Finally,insulator string replacement experiments of the robot were tested on actual line.The results show that each robot manipulator joint movement ob- tains a better dynamic performance,which validates that the proposed five-polynomial interpolation manipulator trajectory planning has strong practicability,and can further improve the robot operational efficiency and stability. KEY WORDS robot:manipulator:dynamic modeling:motion planning 带电作业·是在高压电气设备上不停电进行检修、测试的一种作业方法，其对电网稳定运行，确 收稿日期：201507-10 基金项目：中央高校基本业务费资助项目(2104005)：国家电网湖南省电力公司科技资助项目(5216A01400B1)

工程科学学报，第 38 卷，第 6 期: 867--875，2016 年 6 月 Chinese Journal of Engineering，Vol． 38，No． 6: 867--875，June 2016 DOI: 10． 13374 /j． issn2095--9389． 2016． 06． 018; http: / /journals． ustb． edu． cn 带电作业机器人机械臂动力学建模与运动规划 江 维1，2) ，吴功平1，2) ，王 伟1，2) ，张 颉1，2) 1) 武汉大学动力与机械学院，武汉 430072 2) 武汉大学机器人研究所，武汉 430072  通信作者，E-mail: jiangwei2013@ whu． edu． cn 摘 要 研制了一种双臂、双机械手的绝缘子串更换带电作业机器人． 分解了机器人作业过程中机械臂的运动． 利用拉格朗 日法并结合关节电机电枢电压方程对机器人机械臂基本动作进行动力学建模，得出机械臂基本动作的统一动力学模型，计算 出机械臂 2 的动力学方程，提出基于五次多项式插值的机械臂运动轨迹规划方法，计算出机械臂 2 各关节运动方程． 在 ADAMS环境下进行机器人机械臂动力学和运动学仿真，结果验证了动力学模型的正确性，同时各关节运动轨迹规划满足运动 学要求． 最后在实际线路上进行机器人带电更换绝缘子串实验，结果表明机器人机械臂各关节运动获得了较好的动态性能， 验证了本文提出的五次多项式插值机械臂运动轨迹规划具有较强的工程实用性，进一步提高了机器人的作业效率和稳定性． 关键词 机器人; 机械手; 动力学建模; 运动规划 分类号 TP242. 3 收稿日期: 2015--07--10 基金项目: 中央高校基本业务费资助项目( 2104005) ; 国家电网湖南省电力公司科技资助项目( 5216A01400B1) Manipulator dynamic modeling and motion planning for live working robot JIANG Wei1，2)  ，WU Gong-ping1，2) ，WANG Wei1，2) ，ZHANG Jie1，2) 1) School of Power and Mechanical Engineering，Wuhan University，Wuhan 430072，China 2) Ｒobot Ｒesearch Institute，Wuhan University，Wuhan 430072，China  Corresponding author，E-mail: jiangwei2013@ whu． edu． cn ABSTＲACT This article is focused on a double-arm and double-manipulator live working robot． The arm motion of the robot for insulator string replacement during the operation process was decomposed． The basic operation dynamic model was established by the Lagrange method combined with the armature voltage equation，the dynamic equations of manipulator 2 were calculated，the manipula￾tor motion trajectory planning method was proposed based on five-polynomial interpolation，and the kinematics equations of manipulator 2 each joint were calculated． Ｒobot manipulator dynamics and kinematics simulations were performed in ADAMS environment． The re￾sults verify the correctness of the dynamic model，and the joint trajectory planning meets the kinematic requirements． Finally，insulator string replacement experiments of the robot were tested on actual line． The results show that each robot manipulator joint movement ob￾tains a better dynamic performance，which validates that the proposed five-polynomial interpolation manipulator trajectory planning has strong practicability，and can further improve the robot operational efficiency and stability． KEY WOＲDS robot; manipulator; dynamic modeling; motion planning 带电作业［1--3］是在高压电气设备上不停电进行 检修、测试的一种作业方法，其对电网稳定运行，确

·868· 工程科学学报，第38卷，第6期 保稳定供电具有极其重要的意义.绝缘子串更换是 走轮及其夹持机构、导轨等几个部分构成.机械臂1 电力部门带电作业的重要内容之一，传统更换方 和机械臂2分别为三自由度和四自由度机构，机械 法4主要是通过携带工器具登塔完全依靠人工独 臂1固定于机体上，机械臂2与机械臂1一样除具有 立完成，然而高空和高压环境使得完全依靠人工不 旋转、伸缩和纵移三个关节外，还另有一横移关节， 仅劳动强度大和作业效率低，而且危险性高.基于此 可沿机体横向移动.机器人作业末端由W销推送机 本文研制出一种高压线路绝缘子串更换带电作业机 构和绝缘子串取装机构构成.W销推送机构由三个 器人，通过搭载的双机械臂及其作业末端完成关键 关节构成，一个移动关节携带夹爪实现碗头挂板的 位置的定位，并在人工辅助下实现绝缘子串的更换. 夹持，另两个移动关节分别实现W销的推出和推入， 机器人机械臂动力学模型是机械臂运动控制的基 绝缘子串取装机构由以曲柄摇杆机构为工作原理的 础，目前国内外学者利用不同的方法对机械臂动力 夹爪对钢帽进行夹持，由横移关节的前后运动对绝 学建模进行相关研究.文献78]基于拉格朗日方 缘子串推出和装入. 程分别与有限元法、假设模态法和奇异摄动法相结 碗头挂板夹持机构 绝缘子夹持机构 W销推块 合得到不同形式的双连杆机械臂动力学模型.文献 机械手1 机械手2 9-lO]利用Newton--Euler法推导多连杆可重构机 机械臂2 械臂的动力学模型，该方法利用动量矩定理，物理意 奇睁行走轮 机械臂1· ·夹爪 义明确，能够清晰地表征系统完整受力关系，但该方 奇臂· 偶臂 法方程数量较多且计算效率较低.文献1]基于 Kane方程在惯性参考坐标系中建立刚柔机械臂的动 力学模型，Kane方程其特点是建模过程中不出现内 力项，实现了动力学方程的计算机推导.文献2] 导轨控制箱 基于Hamilton原理并结合假设模态法建立了具有旋 图1带电作业机器人实体结构图 转关节的单连杆机械臂动力学模型，Hamilton原理采 Fig.1 Entity structure of the live working robot 用能量方式建模，避免建模过程中的内力项，适合结 构较简单的机械臂系统，对于结构复杂的系统 1.2绝缘子串更换作业原理 Hamilton函数变分运算计算量将大幅增加.因此，机 单分裂导线上的悬垂绝缘子串联接结构如图2所 械臂的动力学建模大多是利用上述方法并结合系统 示.实现绝缘子串更换作业的关键是推出位于碗头挂 特征而形成的一种复合建模方法.针对带电作业机 板凹槽内的联接部件W销，然后将绝缘子串的球头从 器人特定作业环境，为尽可能地减小作业环境对机 碗头挂板凹槽内推出，使得绝缘子串的下端处于自由 械臂运动的影响，本文将机械臂作业动作分解，利用 状态以便于人工辅助更换，更换完毕后将下端处于自 拉格朗日法并结合关节电机的电枢方程，建立了机 由状态的新绝缘子串球头推入碗头挂板凹槽，并推入 械臂的动力学模型.该模型考虑了高压电磁环境对 W销即完成一次完整更换.在作业过程中机器人主要 机械臂关节电机的影响，在一定程度上可以提高机 负责碗头挂板的夹持、绝缘子的夹持、W销的推进与 械臂运动控制的精度.基于该模型提出五次多项式 推出、球头的推进与推出，人工主要负责收紧放松导 插值的机械臂运动规划方法.该方法与三次多项式 线、更换绝缘子串，通过这种人机协调工作的方式实现 插值相比，除了满足关节位移和速度性能要求外，进 绝缘子串的更换 一步满足了关节加速度的动态性能要求，消除了在 起始点和终止点加速度的突变对各关节冲击的影 2带电作业机器人机械臂动力学建模 响，提高了机器人作业过程的稳定性 在绝缘子串更换作业过程中机械臂的基本动作有 1带电作业机器人的实体结构与绝缘子更 旋转、伸缩、横移和机械手的纵移，由于机械臂2比机 换作业原理 械臂1多一个横移关节，因此本文将以机械臂2为例 1.1带电作业机器人的实体结构 对基本动作进行动力学建模 带电作业机器人的实体结构如图1所示.机器 2.1机械臂的横移 人主要由控制箱、双机械臂、双机械手及其末端、行 设x是机械臂2质心横坐标，m,为其质量，则机械

工程科学学报，第 38 卷，第 6 期 保稳定供电具有极其重要的意义． 绝缘子串更换是 电力部门带电作业的重要内容之一，传 统 更 换 方 法［4--6］主要是通过携带工器具登塔完全依靠人工独 立完成，然而高空和高压环境使得完全依靠人工不 仅劳动强度大和作业效率低，而且危险性高． 基于此 本文研制出一种高压线路绝缘子串更换带电作业机 器人，通过搭载的双机械臂及其作业末端完成关键 位置的定位，并在人工辅助下实现绝缘子串的更换． 机器人机 械 臂 动 力 学 模 型 是 机 械 臂 运 动 控 制 的 基 础，目前国内外学者利用不同的方法对机械臂动力 学建模进行相关研究． 文献［7--8］基于拉格朗日方 程分别与有限元法、假设模态法和奇异摄动法相结 合得到不同形式的双连杆机械臂动力学模型． 文献 ［9 － 10］利用 Newton--Euler 法推导多连杆可重构机 械臂的动力学模型，该方法利用动量矩定理，物理意 义明确，能够清晰地表征系统完整受力关系，但该方 法方程数量 较 多 且 计 算 效 率 较 低． 文 献［11］基 于 Kane 方程在惯性参考坐标系中建立刚柔机械臂的动 力学模型，Kane 方程其特点是建模过程中不出现内 力项，实现了动力学方程的计算机推导． 文献［12］ 基于 Hamilton 原理并结合假设模态法建立了具有旋 转关节的单连杆机械臂动力学模型，Hamilton 原理采 用能量方式建模，避免建模过程中的内力项，适合结 构较 简 单 的 机 械 臂 系 统，对于结构复杂的系统 Hamilton函数变分运算计算量将大幅增加． 因此，机 械臂的动力学建模大多是利用上述方法并结合系统 特征而形成的一种复合建模方法． 针对带电作业机 器人特定作业环境，为尽可能地减小作业环境对机 械臂运动的影响，本文将机械臂作业动作分解，利用 拉格朗日法并结合关节电机的电枢方程，建立了机 械臂的动力学模型． 该模型考虑了高压电磁环境对 机械臂关节电机的影响，在一定程度上可以提高机 械臂运动控制的精度． 基于该模型提出五次多项式 插值的机械臂运动规划方法． 该方法与三次多项式 插值相比，除了满足关节位移和速度性能要求外，进 一步满足了关节加速度的动态性能要求，消除了在 起始点和 终 止 点 加 速 度 的 突 变 对 各 关 节 冲 击 的 影 响，提高了机器人作业过程的稳定性． 1 带电作业机器人的实体结构与绝缘子更 换作业原理 1. 1 带电作业机器人的实体结构 带电作业机器人的实体结构如图 1 所示． 机器 人主要由控制箱、双机械臂、双机械手及其末端、行 走轮及其夹持机构、导轨等几个部分构成． 机械臂 1 和机械臂 2 分别为三自由度和四自由度机构，机械 臂 1 固定于机体上，机械臂 2 与机械臂 1 一样除具有 旋转、伸缩和纵移三个关节外，还另有一横移关节， 可沿机体横向移动． 机器人作业末端由 W 销推送机 构和绝缘子串取装机构构成． W 销推送机构由三个 关节构成，一个移动关节携带夹爪实现碗头挂板的 夹持，另两个移动关节分别实现 W 销的推出和推入， 绝缘子串取装机构由以曲柄摇杆机构为工作原理的 夹爪对钢帽进行夹持，由横移关节的前后运动对绝 缘子串推出和装入． 图 1 带电作业机器人实体结构图 Fig． 1 Entity structure of the live working robot 1. 2 绝缘子串更换作业原理 单分裂导线上的悬垂绝缘子串联接结构如图 2 所 示． 实现绝缘子串更换作业的关键是推出位于碗头挂 板凹槽内的联接部件 W 销，然后将绝缘子串的球头从 碗头挂板凹槽内推出，使得绝缘子串的下端处于自由 状态以便于人工辅助更换，更换完毕后将下端处于自 由状态的新绝缘子串球头推入碗头挂板凹槽，并推入 W 销即完成一次完整更换． 在作业过程中机器人主要 负责碗头挂板的夹持、绝缘子的夹持、W 销的推进与 推出、球头的推进与推出，人工主要负责收紧放松导 线、更换绝缘子串，通过这种人机协调工作的方式实现 绝缘子串的更换． 2 带电作业机器人机械臂动力学建模 在绝缘子串更换作业过程中机械臂的基本动作有 旋转、伸缩、横移和机械手的纵移，由于机械臂 2 比机 械臂 1 多一个横移关节，因此本文将以机械臂 2 为例 对基本动作进行动力学建模． 2. 1 机械臂的横移 设 x 是机械臂 2 质心横坐标，m2为其质量，则机械 · 868 ·

江维等：带电作业机器人机械臂动力学建模与运动规划 ·869· 缩时所满足的动力学方程式(5) (b) may +mag =T2. (5) 若k,为机械臂2伸缩电机角速度与位移转换常 数，由y与丝杆角速度6，关系可得式(6) W销 mk202 +m2g =Kiz (6) 由状态 的绝缘子 碗头挂板 凹后 结合伸缩关节电机电枢电压平衡方程可得 m2k202 +m2g=Kiz, (7) 图2绝缘子串结构示意图.(a)联接状态的绝缘子串：(b)由状 +R5+Ua=0, La di 态的绝缘子 式中，g为重力加速度，K为伸缩电机常数，L2和R2 Fig.2 Schematic illustration of the insulator strings:(a)connection 分别为伸缩电机电枢回路的电感和电阻，2为伸缩电 state insulator strings:(b)free state insulator 机电枢电流，U,为伸缩电机电枢电压，U。为伸缩电机 1 臂2横移时速度：=，横移时动能E,=2m,(),由 电枢逆电势，U。=K。吧，K为伸缩电机常数. 于横移时机械臂2质心纵坐标不变，其势能变化E2= 2.3机械臂的旋转 0,T,为机械臂2横移关节上外力总和，将E,、E2和T, 设机械臂2在旋转时的转动惯量为J,机械臂2旋 代入拉格朗日方程式(1)可得机械臂2横移时所满足 转电机旋转角为6，T3为机械臂2旋转关节上外力总 的动力学方程为式(2). 和，根据转动惯量与转动力矩的关系有 (1) J62=t3 (8) 由旋转电机的特性方程可知T3=K3,结合旋转 m,x =T (2) 关节电机电枢电压平衡方程可得 若k,为机械臂2横移电机角速度与位移转换常 J03=Kmi3 数，由x与丝杆角速度日，关系可得 m2k,01=T1 (3) +R,5+U。=U (9) La di 由横移电机的特性方程可知T1=K,结合横移 式中，K如为旋转电机常数，L,和R,分别为旋转电机电 关节电机电枢电压平衡方程可得 枢回路的电感和电阻，3为旋转电机电枢电流，，为 rmzk 01 Kmin, 旋转电机电枢电压，U为旋转电机电枢逆电势，。= i (4) L di +Ri+U=U 飞曾K,为旋转电机常数 式中，1为时间，K为横移电机常数，i为横移电机电 2.4机械臂基本动作动力学方程的统一表达形式 枢电流，L和R,分别为横移电机电枢回路的电感和 为便于机械臂的动力学分析，将式(4)、式(7)和 电阻，U,为横移电机电枢电压，U为横移电机电枢逆 式(9)机械臂基本动作的动力学方程用一组统一的表 电势，儿。=人盟K为横移电机常数 达式来描述如式(10)所示，其中P和Q为常数，0为关 节电机转角，ⅱ为关节电机电枢电流，U为关节电机电 由于机械手2纵移时质心纵坐标不变，其动力学 枢电压，L和R分别是电枢回路的电感和电阻，K,和 方程推导过程和结果与上述完全相似，此处不再赘述. K。是与关节电机有关的常数. 2.2机械臂的伸缩 P0+Q=Kxi, 机械臂伸缩运动和机械臂横移运动唯一不同是运 ++水盟=儿 di (10) 动过程中势能有变化.设y是机械臂2质心纵坐标， L ai (a)当P=m2k,Q=0时式(10)可以表示机械臂2 机械臂2在伸缩运动时动能为E,=2m,(G),势能变 横移运动的动力学方程： 化为E2=m28,T2为机械臂2横移关节上外力总和， (b)当P=m,k,Q=0时式(10)可以表示机械手 将E,和E2代入拉格朗日方程式(1)可得机械臂2伸 2纵移运动的动力学方程：

江 维等: 带电作业机器人机械臂动力学建模与运动规划 图 2 绝缘子串结构示意图． ( a) 联接状态的绝缘子串; ( b) 由状 态的绝缘子 Fig． 2 Schematic illustration of the insulator strings: ( a) connection state insulator strings; ( b) free state insulator 臂 2 横移时速度 v = x ·，横移时动能 E1 = 1 2 m2 ( x ·) 2 ，由 于横移时机械臂 2 质心纵坐标不变，其势能变化 E2 = 0，τ1 为机械臂 2 横移关节上外力总和，将 E1、E2 和 τ1 代入拉格朗日方程式( 1) 可得机械臂 2 横移时所满足 的动力学方程为式( 2) ． d d ( t E1 x · ) － E1 x + E2 x = τ， ( 1) m2 x ·· = τ1 ． ( 2) 若 k1 为机械臂 2 横移电机角速度与位移转换常 数，由 x 与丝杆角速度 θ1 关系可得 m2 k1 θ ·· 1 = τ1 ． ( 3) 由横移电机的特性方程可知 τ1 = KM1 i1，结合横移 关节电机电枢电压平衡方程可得 m2 k1 θ ·· 1 = KM1 i1， L1 di1 dt + Ｒ1 i { 1 + Ua1 = U1 ． ( 4) 式中，t 为时间，KM1为横移电机常数，i1 为横移电机电 枢电流，L1 和 Ｒ1 分别为横移电机电枢回路的电感和 电阻，U1 为横移电机电枢电压，Ua1为横移电机电枢逆 电势，Ua1 = Ka1 dθ1 dt ，Ka1为横移电机常数． 由于机械手 2 纵移时质心纵坐标不变，其动力学 方程推导过程和结果与上述完全相似，此处不再赘述． 2. 2 机械臂的伸缩 机械臂伸缩运动和机械臂横移运动唯一不同是运 动过程中势能有变化． 设 y 是机械臂 2 质心纵坐标， 机械臂 2 在伸缩运动时动能为 E1 = 1 2 m2 ( y ·) 2 ，势能变 化为 E2 = m2 gy，τ2 为机械臂 2 横移关节上外力总和， 将 E1 和 E2 代入拉格朗日方程式( 1) 可得机械臂 2 伸 缩时所满足的动力学方程式( 5) ． m2 y ·· + m2 g = τ2 ． ( 5) 若 k2 为机械臂 2 伸缩电机角速度与位移转换常 数，由 y 与丝杆角速度 θ2 关系可得式( 6) ． m2 k2 θ ·· 2 + m2 g = KM2 i2 ． ( 6) 结合伸缩关节电机电枢电压平衡方程可得 m2 k2 θ ·· 2 + m2 g = KM2 i2， L2 di2 dt + Ｒ2 i { 2 + Ua2 = U2 ． ( 7) 式中，g 为重力加速度，KM2 为伸缩电机常数，L2 和 Ｒ2 分别为伸缩电机电枢回路的电感和电阻，i2 为伸缩电 机电枢电流，U2 为伸缩电机电枢电压，Ua2为伸缩电机 电枢逆电势，Ua2 = Ka2 dθ2 dt ，Ka2为伸缩电机常数． 2. 3 机械臂的旋转 设机械臂 2 在旋转时的转动惯量为 J，机械臂 2 旋 转电机旋转角为 θ3，τ3 为机械臂 2 旋转关节上外力总 和，根据转动惯量与转动力矩的关系有 J θ ·· 3 = τ3 ． ( 8) 由旋转电机的特性方程可知 τ3 = KM3 i3，结合旋转 关节电机电枢电压平衡方程可得 J θ ·· 3 = KM3 i3， L3 di3 dt + Ｒ3 i { 3 + Ua3 = U3 ． ( 9) 式中，KM3为旋转电机常数，L3 和 Ｒ3 分别为旋转电机电 枢回路的电感和电阻，i3 为旋转电机电枢电流，U3 为 旋转电机电枢电压，Ua3为旋转电机电枢逆电势，Ua3 = Ka3 dθ3 dt ，Ka3为旋转电机常数． 2. 4 机械臂基本动作动力学方程的统一表达形式 为便于机械臂的动力学分析，将式( 4) 、式( 7) 和 式( 9) 机械臂基本动作的动力学方程用一组统一的表 达式来描述如式( 10) 所示，其中 P 和 Q 为常数，θ 为关 节电机转角，i 为关节电机电枢电流，U 为关节电机电 枢电压，L 和 Ｒ 分别是电枢回路的电感和电阻，KM 和 Ka 是与关节电机有关的常数． P θ ·· + Q = KM i， L di dt + Ｒi + Ka dθ dt { = U． ( 10) ( a) 当 P = m2 k，Q = 0 时式( 10) 可以表示机械臂2 横移运动的动力学方程; ( b) 当 P = m* 2 k，Q = 0 时式( 10) 可以表示机械手 2 纵移运动的动力学方程; · 968 ·

·870· 工程科学学报，第38卷，第6期 (c)当P=m2k,Q=m2g时式(10)可以表示机械 0 臂2伸缩运动的动力学方程： P x=w,i],A (d)当P=J,Q=0时式(10)可以表示机械臂2 K 旋转运动的动力学方程 令关节电机转速为w,则0=0，因此式(10)可化 为式(11)，取w和i为状态变量，则式(11)可化为状 1 态空间表达式，如式(12)所示 [P@+Q=Kyi, (11) 则式(12)可简化为 Li+Ri+Kω=U. =Ax Bu. (13) 令x=i n] ,n=1,2,3,4分别表示机械臂的旋转、伸 (12) 缩、横移和机械手纵移运动的状态变量，则机械臂2的 动力学模型为 0 0 K R 01 0 0 01 L 0 0 0 0 0 (14) 0 3 0 0 0 0 4 L L:l 0 0 m"k 0 0 R L 由关节电机参数K。=0.5V·s·rad,K= 0 0.21「3.921 (16) 1.00Nm…A,R=1,L=0.1H,电枢电压为24V,机 械臂2的质量m2=4kg,机械手2的质量m?=1kg,机 (c)横移关节： 械臂2长0.47m,旋转关节的转动惯量为J=(m2+ m)2=(4+1)·(0.47)2=1.105kgm2,关节电机角 0.081 速度与位移转换常数k=2.5,重力加速度常数g= 9.8ms2.将上述参数代入式(12)，可分别计算出机 (17) 械臂2各关节基本动作动力学方程为(15)~(18). (a)旋转关节： (d)纵移关节： =+62 「00.911 +01 -5-10+240 (15) (18) (b)伸缩关节： =900+691I31- 将式(15)~式(18)合并到一个矩阵中可得机械 臂2动力学模型的完整表达式：

工程科学学报，第 38 卷，第 6 期 ( c) 当 P = m2 k，Q = m2 g 时式( 10) 可以表示机械 臂 2 伸缩运动的动力学方程; ( d) 当 P = J，Q = 0 时式( 10) 可以表示机械臂 2 旋转运动的动力学方程． 令关节电机转速为 ω，则 ω = θ · ，因此式( 10) 可化 为式( 11) ，取 ω 和 i 为状态变量，则式( 11) 可化为状 态空间表达式，如式( 12) 所示． P ω · + Q = KM i， L i · { + Ｒi + Kaω = U． ( 11) ω · i [ · ] = 0 KM P － Ka L － Ｒ          L  ω [ ] i + 1 0 [ ] 0 1 － Q P U          L  ． ( 12) 令 x =［ω，i］T ，A = 0 KM P － Ka L － Ｒ          L  ， B = 1 0 [ ] 0 1 ，u = － Q P U          L  ， 则式( 12) 可简化为 x · = Ax + Bu． ( 13) 令 xn = ωn [ i ] n ，n = 1，2，3，4 分别表示机械臂的旋转、伸 缩、横移和机械手纵移运动的状态变量，则机械臂 2 的 动力学模型为 ω · 1 i · 1 ω · 2 i · 2 ω · 3 i · 3 ω · 4 i ·                             4 = 0 KM J 0 0 0 0 0 0 － Ka L － Ｒ L 0 0 0 0 0 0 0 0 0 KM m2 k 0 0 0 0 0 0 － Ka L － Ｒ L 0 0 0 0 0 0 0 0 0 KM m2 k 0 0 0 0 0 0 － Ka L － Ｒ L 0 0 0 0 0 0 0 0 0 KM m* k 0 0 0 0 0 0 － Ka L － Ｒ                                            L  ω1 i1 ω2 i2 ω3 i3 ω4 i                         4 + 0 U L g k U L 0 U L 0 U                                   L ． ( 14) 由关节电机参数 Ka = 0. 5 V·s·rad － 1，KM = 1. 00 N·m·A － 1，Ｒ = 1 Ω，L = 0. 1H，电枢电压为 24 V，机 械臂 2 的质量 m2 = 4 kg，机械手 2 的质量 m* 2 = 1 kg，机 械臂 2 长 0. 47 m，旋转关节的转动惯量为 J = ( m2 + m* 2 ) r 2 = ( 4 + 1)·( 0. 47) 2 = 1. 105 kg·m2 ，关节电机角 速度与位移转换常数 k = 2. 5，重力加速度常数 g = 9. 8 m·s － 2 ． 将上述参数代入式( 12) ，可分别计算出机 械臂 2 各关节基本动作动力学方程为( 15) ～ ( 18) ． ( a) 旋转关节: x · 1 = 0 0. 91 [ ] － 5 － 10 x1 + 1 0 [ ] 0 1 0 [ ] 240 = 0 0. 91 [ ] － 5 － 10 x1 + 0 [ ] 240 ; ( 15) ( b) 伸缩关节: x · 2 = 0 0. 2 [ ] － 5 － 10 x2 + 1 0 [ ] 0 1 3. 92 [ ] 240 = 0 0. 2 [ ] － 5 － 10 x2 + 3. 92 [ ] 240 ; ( 16) ( c) 横移关节: x · 3 = 0 0. 08 [ ] － 5 － 10 x3 + 1 0 [ ] 0 1 0 [ ] 240 = 0 0. 08 [ ] － 5 － 10 x3 + 0 [ ] 240 ; ( 17) ( d) 纵移关节: x · 4 = 0 0. 4 [ ] － 5 － 10 x4 + 1 0 [ ] 0 1 0 [ ] 240 = 0 0. 4 [ ] － 5 － 10 x4 + 0 [ ] 240 ． ( 18) 将式( 15) ～ 式( 18) 合并到一个矩阵中可得机械 臂 2 动力学模型的完整表达式: · 078 ·

江维等：带电作业机器人机械臂动力学建模与运动规划 871 ①1 0 0.91 00 0 0 0 0 0 -5 -10 0 0 0 0 0 0 240 02 0 0 0 0.2 0 0 0 0 3.92 0 0 -5 -10 0 0 0 0 240 (19) 03 0 0 0 0 0.08 0 0 0 0 0 0 0 -5 -10 0 3 240 0 0 0 0 0 0 0 0.4 0 W 0 0 0 0 0 0 -5 -10」 240」 i 下方程组： 3基于多项式插值的机械臂运动轨迹规划 [at+b后+c后+di+e+f=x(o) 机器人在作业过程中，作业末端随着机械臂由初 at+b+ct+di+el+f=x(t), 始位姿运动至工作位姿，在此过程中需要避让导线及 5ald+4bt后+3ct6+2d。+e=x(lo), 线路上的各种障碍物，在已知机械臂各关节变量的初 (23) 值和终值，要求末端执行机构按照预定的轨迹运动，这 5ati +4bi +3cti+2dt +e=x(t), 就是机器人机械臂轨迹规划问题. 20al。+12bi+6ct。+2d=x(o), 3.1机械臂多项式插值运动规划算法基本原理 20at+12b+6c4,+2d=x(l,). 机械臂运动规划除了一些智能算法-外，还有 为便于MATLAB求解方程及插值算法的推广将 较常用的多项式插值算法5-.关节轨迹插值函数主 式(23)改写为矩阵的形式如下所示： 要有三次多项式插值、高阶多项式插值、抛物线过渡线 66.11 「x(to)1 a 性插值、抛物线过渡路径点线性插值等.其中三次多 b x(t) 项式插值方法只适用于关节起始、终止速度为零的运 5 x (ta) 动情况，而高阶多项式插值适用于对路径的起始点和 436210 终止点都规定了关节的位置、速度和加速度的情况 5 4t32,10 x() 结合对机械臂运动的控制要求及机械臂起点和终点的 200 12% 6l。 2 0 0 e (to) 六个初始条件，本文采用五次多项式插值方法对每个 203122 2 00 (t) 动作规划单元的关节进行插值可以满足机器人机械臂 (24) 的轨迹运动要求. 式(24)两边同时左乘系数矩阵的逆可得 设起始点的位移、速度和加速度分别为x。、元。和 to 11「x()1 元。，终点的位移、速度和加速度分别为x、x和元，插值 a] t l 1 x() 函数为x(),则x(t)满足初始条件 5to 4后 Tx(lo）=xo, 36 2to 1 0 (to) x()=x 5r 4 3 2,1 0 x(t,) e i(to)=o 203 12后 6 2 0 0 元(lo) (20) 120r 122 6t 00 (t)= Lx() x(。)=0 (25) 通过MATLAB软件可解得插值多项式的系数，由 (t)= 于结果较复杂本文只列出了系数a,b的值. 由式(20)的六个初始条件可唯一确定一个五次 多项式 a=[)-)+,)-)+ x (t)=at+b+ct+dt2+et+f. (21) 对式(21)求导可得运动轨迹上的关节速度和加 6)-(,)+6x。）-6x()-3,）+ 速度为 [(t)=5at+4bt+3ct2+2dt+e, (22) 3W-3ww+30]a-(2w） x(t）=20at3+12bm2+6t+2d. 将式(20)初始条件代入式(21)和式(22)可得如 6=[2(,)+2()-()-

江 维等: 带电作业机器人机械臂动力学建模与运动规划 ω · 1 i · 1 ω · 2 i · 2 ω · 3 i － · 3 ω · 4 i ·                             4 = 0 0. 91 0 0 0 0 0 0 － 5 － 10 0 0 0 0 0 0 0 0 0 0. 2 0 0 0 0 0 0 － 5 － 10 0 0 0 0 0 0 0 0 0 0. 08 0 0 0 0 0 0 － 5 － 10 0 0 0 0 0 0 0 0 0 0. 4                      0 0 0 0 0 0 － 5 － 10 ω1 i1 ω2 i2 ω3 i3 ω4 i                         4 + 0 240 3. 92 240 0 240 0                      240  ． ( 19) 3 基于多项式插值的机械臂运动轨迹规划 机器人在作业过程中，作业末端随着机械臂由初 始位姿运动至工作位姿，在此过程中需要避让导线及 线路上的各种障碍物，在已知机械臂各关节变量的初 值和终值，要求末端执行机构按照预定的轨迹运动，这 就是机器人机械臂轨迹规划问题． 3. 1 机械臂多项式插值运动规划算法基本原理 机械臂运动规划除了一些智能算法［13--14］外，还有 较常用的多项式插值算法［15--16］． 关节轨迹插值函数主 要有三次多项式插值、高阶多项式插值、抛物线过渡线 性插值、抛物线过渡路径点线性插值等． 其中三次多 项式插值方法只适用于关节起始、终止速度为零的运 动情况，而高阶多项式插值适用于对路径的起始点和 终止点都规定了关节的位置、速度和加速度的情况． 结合对机械臂运动的控制要求及机械臂起点和终点的 六个初始条件，本文采用五次多项式插值方法对每个 动作规划单元的关节进行插值可以满足机器人机械臂 的轨迹运动要求． 设起始点的位移、速度和加速度分别为 x0、x · 0 和 x ·· 0，终点的位移、速度和加速度分别为 xf、x · f 和 x ·· f，插值 函数为 x( t) ，则 x( t) 满足初始条件 x( t0 ) = x0， x( tf ) = xf， x ·( t0 ) = x · 0， x ·( tf ) = x · f， x ·· ( t0 ) = x ·· 0， x ·· ( tf ) = x ·· f          ． ( 20) 由式( 20) 的六个初始条件可唯一确定一个五次 多项式 x( t) = at5 + bt4 + ct3 + dt2 + et + f． ( 21) 对式( 21) 求导可得运动轨迹上的关节速度和加 速度为 x ·( t) = 5at4 + 4bt3 + 3ct2 + 2dt + e， x ·· ( t) = 20at3 + 12bt2 { + 6ct + 2d． ( 22) 将式( 20) 初始条件代入式( 21) 和式( 22) 可得如 下方程组: at5 0 + bt4 0 + ct3 0 + dt2 0 + et0 + f = x( t0 ) ， at5 f + bt4 f + ct3 f + dt2 f + etf + f = x( tf ) ， 5at4 0 + 4bt3 0 + 3ct2 0 + 2dt0 + e = x ·( t0 ) ， 5at4 f + 4bt3 f + 3ct2 f + 2dtf + e = x ·( tf ) ， 20at3 0 + 12bt2 0 + 6ct0 + 2d = x ·· ( t0 ) ， 20at3 f + 12bt2 f + 6ctf + 2d = x ·· ( tf )          ． ( 23) 为便于 MATLAB 求解方程及插值算法的推广将 式( 23) 改写为矩阵的形式如下所示: t 5 0 t 4 0 t 3 0 t 2 0 t0 1 t 2 f t 4 f t 3 f t 2 f tf 1 5t 4 0 4t 3 0 3t 2 0 2t0 1 0 5t 4 f 4t 3 f 3t 2 f 2tf 1 0 20t 3 0 12t 2 0 6t0 2 0 0 20t 3 f 12t 2 f 6tf                    2 0 0 a b c d e                f = x( t0 ) x( tf ) x ·( t0 ) x ·( tf ) x ·· ( t0 ) x ·· ( tf                    )  ． ( 24) 式( 24) 两边同时左乘系数矩阵的逆可得 a b c d e                f = t 5 0 t 4 0 t 3 0 t 2 0 t0 1 t 2 f t 4 f t 3 f t 2 f tf 1 5t 4 0 4t 3 0 3t 2 0 2t0 1 0 5t 4 f 4t 3 f 3t 2 f 2tf 1 0 20t 3 0 12t 2 0 6t0 2 0 0 20t 3 f 12t 2 f 6tf                    2 0 0 － 1 x( t0 ) x( tf ) x ·( t0 ) x ·( tf ) x ·· ( t0 ) x ·· ( tf                    )  ． ( 25) 通过 MATLAB 软件可解得插值多项式的系数，由 于结果较复杂本文只列出了系数 a，b 的值． a [ = 1 2 t 2 0 x ·· ( t0 ) － t0 tf x ·· ( t0 ) + 1 2 t 2 f x ·· ( t0 ) － 1 2 t 2 0 x ·· ( tf ) + t0 tf x ·· ( tf ) － 1 2 t 2 f x ·· ( tf ) + 6x( t0 ) － 6x( tf ) － 3t0 x ·( t0 ) + 3tf x ·( t0 ) － 3t0 x ·( tf ) + 3tf x ·( tf ] ) · 1 ( t0 － tf ) 5 ． ( 26) b [ = 1 2 t 2 0 tf x ·· ( t0 ) + 2t0 t 2 f x ·· ( t0 ) － t 3 0 x ·· ( t0 ) － · 178 ·

·872· 工程科学学报，第38卷，第6期 )+6W-2)-宁)+ 可得到初始点到终止点的五次多项式轨迹 3.2机械臂2运动规划 x(,)-15ox(a)+15ox()-15x(a)+ 机械臂2由初始位姿到绝缘子夹持位姿的运动规 15px(,)+7ix(o)+lolt(o)-8x()+ 划为：机械臂2后转至传感器位，机械臂2伸长至传感 8)-4)-7]- 1 (27) 器位，机械臂2横移至绝缘子钢帽位于绝缘子夹持末 端正中间处，机械手2纵移内移，理想情况下此时机械 为简化系数的运算结果，当起始时间。为0、起始 手2末端可以执行夹紧操作实现绝缘子的夹持.本文 点和终止点加速度为0时，多项式系数如下： 主要研究机械臂2轨迹运动规划，因此省略机械臂1 a=[6(x(,)-x(0))-3(t(0)+x()]/, 运动的时间段，设定机械臂2各关节的运动时序及相 b=[-15(x()-x(0)+(8x(0)+7x())]/, 应的关节变量值如表1所示. c=00(x()-x(0))-(6x(0)+4x())]/, 根据绝缘子串更换作业运动规划可知机械臂2由 d=0, 初始位姿至绝缘子夹持位姿，旋转关节后转角位移为 e=x(0), π，伸缩关节伸长位移为150cm,横移关节前移位移为 /=x(0). 176cm,纵移关节位移为157.5cm.按照机器人关节五 (28) 次多项式轨迹插值方法，结合表1中关节变量的初始 将解得的系数代入式(21)即可得到关节变量的 条件及初始时刻和终点时刻加速度为零的六个初始条 五次多项式运动轨迹函数.由系数可知只要知道初始 件，按照机械臂2基本动作的先后顺序可计算得到各 和终止时间、初始点和终止点的位移、速度、加速度即 关节的运动函数的数学表达式如下. 表1机械臂2运动时序及关节变量值 Table 1 Manipulator 2 motion sequence in time and joint variable values 初始条件 旋转关节变量， 伸缩关节变量， 横移关节变量， 纵移关节变量， 时刻 序号 62/rad d3/cm d/em da/cm 1 0 -r2 315.5 8.5 -158.5 2 0:0:5 π/2 465.5 8.5 -158.5 3 0:0:10 EA 465.5 186.5 -158.5 4 0:0:15 r/2 465.5 186.5 -1 (a)旋转关节： (d)纵移关节： 「受-+爱- (0≤t<5), x(）= x(t)= -158.5 (0≤1<10)， T 12 (t≥5). -158.5+18.9(t-10)2- (29) 12,6(u-10)3-袋(-10)+ 5 25 (b)伸缩关节： 129 50 (t-10)5 (t≥10). x(t)= 315.5+18r-号+ (0≤t<10), (32) 465.5 (t≥10). (30) 4 仿真实验与现场运行实验 (c)横移关节： 4.1动力学仿真实验 x(t)= 基于所建立的机械臂2基本动作动力学方程 8.5 (0≤t<5), 式(15)~(18)，利用ADAMS的仿真功能模块进行动 8.5+534(4-5)2-3561-5)3- 25 -125 力学仿真，设置EndTime为l5s,步长为500.四个关节 -+認-5列 在作业运动过程中所受关节力与关节轨迹变化的关系 (5≤1<10)， 如图3所示. 186.5 (t≥10). 由仿真结果可知，各关节在驱动力的作用下，均能 (31) 按照轨迹规划所确定的运动到达最终的位姿.对于旋

工程科学学报，第 38 卷，第 6 期 3 2 t 3 f x ·· ( t0 ) + 3 2 t 3 0 x ·· ( tf ) － 2t 2 0 tf x ·· ( tf ) － 1 2 t0 t 2 f x ·· ( tf ) + t 3 f x ·· ( tf ) － 15t0 x( t0 ) + 15t0 x( tf ) － 15tfx( t0 ) + 15tfx( tf ) + 7t 2 0 x ·( t0 ) + t0 tf x ·( t0 ) － 8t 2 f x ·( t0 ) + 8t 2 0 x ·( tf ) － t0 tf x ·( tf ) － 7t 2 f x ·( tf ] ) · 1 ( t0 － tf ) 5 ． ( 27) 为简化系数的运算结果，当起始时间 t0为 0、起始 点和终止点加速度为 0 时，多项式系数如下: a =［6( x( tf ) － x( 0) ) － 3( x ·( 0) + x ·( tf ) ) tf ］/t5 f ， b =［－ 15( x( tf ) － x( 0) ) + ( 8 x ·( 0) + 7 x ·( tf ) ) tf ］/t4 f ， c =［10( x( tf ) － x( 0) ) － ( 6 x ·( 0) + 4 x ·( tf ) ) tf ］/t3 f ， d = 0， e = x ·( 0) ， f = x( 0)          ． ( 28) 将解得的系数代入式( 21) 即可得到关节变量的 五次多项式运动轨迹函数． 由系数可知只要知道初始 和终止时间、初始点和终止点的位移、速度、加速度即 可得到初始点到终止点的五次多项式轨迹． 3. 2 机械臂 2 运动规划 机械臂 2 由初始位姿到绝缘子夹持位姿的运动规 划为: 机械臂 2 后转至传感器位，机械臂 2 伸长至传感 器位，机械臂 2 横移至绝缘子钢帽位于绝缘子夹持末 端正中间处，机械手 2 纵移内移，理想情况下此时机械 手 2 末端可以执行夹紧操作实现绝缘子的夹持． 本文 主要研究机械臂 2 轨迹运动规划，因此省略机械臂 1 运动的时间段，设定机械臂 2 各关节的运动时序及相 应的关节变量值如表 1 所示． 根据绝缘子串更换作业运动规划可知机械臂 2 由 初始位姿至绝缘子夹持位姿，旋转关节后转角位移为 π，伸缩关节伸长位移为 150 cm，横移关节前移位移为 176 cm，纵移关节位移为 157. 5 cm． 按照机器人关节五 次多项式轨迹插值方法，结合表 1 中关节变量的初始 条件及初始时刻和终点时刻加速度为零的六个初始条 件，按照机械臂 2 基本动作的先后顺序可计算得到各 关节的运动函数的数学表达式如下． 表 1 机械臂 2 运动时序及关节变量值 Table 1 Manipulator 2 motion sequence in time and joint variable values 初始条件 序号 时刻 旋转关节变量， θ2 /rad 伸缩关节变量， d3 /cm 横移关节变量， d1 /cm 纵移关节变量， d4 /cm 1 0 － π/2 315. 5 8. 5 － 158. 5 2 0: 0: 5 π/2 465. 5 8. 5 － 158. 5 3 0: 0: 10 π/2 465. 5 186. 5 － 158. 5 4 0: 0: 15 π/2 465. 5 186. 5 － 1 ( a) 旋转关节: x( t) = － π 2 － 2π 125t 3 + 3π 25 t 4 － 6π 75 t 5 ( 0≤t ＜ 5) ， π 2 { ( t≥5) ． ( 29) ( b) 伸缩关节: x( t) = 315. 5 + 18t 3 － 12 5 t 4 + 7 10t 5 ( 0≤t ＜ 10) ， 465. 5 ( t≥10) { ． ( 30) ( c) 横移关节: x( t) = 8. 5 ( 0≤t ＜ 5) ， 8. 5 + 534 25 ( t － 5) 2 － 356 125( t － 5) 3 － 224 75 ( t － 5) 4 + 337 100( t － 5) 5 ( 5≤t ＜ 10) ， 186. 5 ( t≥10)        ． ( 31) ( d) 纵移关节: x( t) = － 158. 5 ( 0≤t ＜ 10) ， － 158. 5 + 18. 9( t － 10) 2 － 12. 6 5 ( t － 10) 3 － 48 25( t － 10) 4 + 129 50 ( t － 10) 5 ( t≥10)          ． ( 32) 4 仿真实验与现场运行实验 4. 1 动力学仿真实验 基于所 建 立 的 机 械 臂 2 基本动作动力学方程 式( 15) ～ ( 18) ，利用 ADAMS 的仿真功能模块进行动 力学仿真，设置 EndTime 为15 s，步长为500． 四个关节 在作业运动过程中所受关节力与关节轨迹变化的关系 如图 3 所示． 由仿真结果可知，各关节在驱动力的作用下，均能 按照轨迹规划所确定的运动到达最终的位姿． 对于旋 · 278 ·

江维等：带电作业机器人机械臂动力学建模与运动规划 873 200 49.5 150 30 49.0 48.5 48.0 20 100 47.5 15≤ 47.0 10 46.5 50 46.0 45.5 0 10 1550 10 时间s 时间s 200 49.5 200 11.05 150 489 (d) 10.95 150 10.85 00 47.5 10.75≤ 47.0 系 10.655 46.5 46.0 10.55 45.5 10.45 50 10.35 5 10 时间/s 时间/s 图3关节力与位移关系曲线.(a)旋转：(b)伸缩：()横移：(d)纵移 Fig.3 Joint force and displacement curves:(a)rotation:(b)stretch;(c)lateral movement;(d)vertical movement 转关节，所受的力矩主要为重力相对于旋转轴的转矩， 臂2伸缩、横移和纵移关节所选用电机可提供的工 在0s和5s,由于机械臂2均呈竖直状态，重力相对于 作转矩为1163.244N·mm.机械臂2旋转关节所选 旋转轴的转矩近似为零；在0~5s内，由于伸缩关节的 用电机可以提供的工作转矩为13815.36Nmm.经对 运动，重力相对于旋转轴的力臂不断变化，因此受力存 比可知机械臂2几个关节处所选用电机的实际驱动 在波动的现象，且在机械臂处于水平姿态时，转矩最 力矩均大于在ADAMS仿真过程中所测得的力矩，满 大.对于伸缩关节，所受的力主要为伸缩臂移动的摩 足实际要求，从而验证了本文各关节基本动作动力 擦力，由于旋转关节的运动，重力的分力呈正弦曲线变 学模型的正确性. 化，相应的摩擦力也呈正弦曲线的规律变化，与图3的 4.2运动学仿真实验 受力曲线一致.对于横移关节，所受的力主要为横移 基于已建立的机械臂2五次多项式插值关节运动 滑台移动的摩擦力，在0~5s内，虽然横移关节没有运 函数式(29)~(32)，在ADAMS环境下进行运动学仿 动，但是由于旋转关节和伸缩关节的运动对其产生耦 真，得到机械臂2作业末端运动由初始位姿到绝缘子 合力，因此在0s就开始受力且受力存在波动的现象并 夹持位姿全过程的路径曲线如图4(a)所示.机械手2 呈对称分布.对于纵移关节，所受的力主要为纵移滑 相对于机器人奇臂行走轮质心在X(竖向)、Y(纵向) 台移动的摩擦力，在0~5s内，虽然纵移关节没有运 和Z(横向)三个方向上的运动特性曲线如图4(b) 动，但是由于旋转关节的运动对其产生耦合力，中立的 所示 分力呈余弦规律变化，使得摩擦力相应变化， 图4(a)末端运动路径仿真过程，机械臂2由初 由仿真结果还可得知旋转关节、伸缩关节、横移 始位姿无碰运动到绝缘子夹持位姿.由图4(b)可 关节、纵移关节运动所需的力矩或力分别为431.837 知，按照既定的关节轨迹规划，在前5s,旋转关节和 N·mm、27.8153N、47.1257N和10.6796N.而机械 伸缩关节运动，使机械臂2的作业末端在X方向上 (a) 700 b 500 300Ly向 100 -100 -300 -500 -7006 时间/s 图4机械手2运动学仿真.()末端运动轨迹：(b)末端运动特性曲线 Fig.4 Manipulator 2 kinematics simulation:(a)end trajectory:(b)end motion characteristic curves

江 维等: 带电作业机器人机械臂动力学建模与运动规划 图 3 关节力与位移关系曲线． ( a) 旋转; ( b) 伸缩; ( c) 横移; ( d) 纵移 Fig． 3 Joint force and displacement curves: ( a) rotation; ( b) stretch; ( c) lateral movement; ( d) vertical movement 转关节，所受的力矩主要为重力相对于旋转轴的转矩， 在 0 s 和 5 s，由于机械臂 2 均呈竖直状态，重力相对于 旋转轴的转矩近似为零; 在 0 ～ 5 s 内，由于伸缩关节的 运动，重力相对于旋转轴的力臂不断变化，因此受力存 图 4 机械手 2 运动学仿真． ( a) 末端运动轨迹; ( b) 末端运动特性曲线 Fig． 4 Manipulator 2 kinematics simulation: ( a) end trajectory; ( b) end motion characteristic curves 在波动的现象，且在机械臂处于水平姿态时，转矩最 大． 对于伸缩关节，所受的力主要为伸缩臂移动的摩 擦力，由于旋转关节的运动，重力的分力呈正弦曲线变 化，相应的摩擦力也呈正弦曲线的规律变化，与图 3 的 受力曲线一致． 对于横移关节，所受的力主要为横移 滑台移动的摩擦力，在 0 ～ 5 s 内，虽然横移关节没有运 动，但是由于旋转关节和伸缩关节的运动对其产生耦 合力，因此在 0 s 就开始受力且受力存在波动的现象并 呈对称分布． 对于纵移关节，所受的力主要为纵移滑 台移动的摩擦力，在 0 ～ 5 s 内，虽然纵移关节没有运 动，但是由于旋转关节的运动对其产生耦合力，中立的 分力呈余弦规律变化，使得摩擦力相应变化． 由仿真结果还可得知旋转关节、伸缩关节、横移 关节、纵移关节运动所需的力矩或力分别为 431. 837 N·mm、27. 8153 N、47. 1257 N 和 10. 6796 N． 而机械 臂 2 伸缩、横移和纵移关节所选用电机可提供的工 作转矩为 1163. 244 N·mm． 机械臂 2 旋转关节所选 用电机可以提供的工作转矩为 13815. 36 Nmm． 经对 比可知机械臂 2 几个关节处所选用电机的实际驱动 力矩均大于在 ADAMS 仿真过程中所测得的力矩，满 足实际要求，从而验证了本文各关节基本动作动力 学模型的正确性． 4. 2 运动学仿真实验 基于已建立的机械臂 2 五次多项式插值关节运动 函数式( 29) ～ ( 32) ，在 ADAMS 环境下进行运动学仿 真，得到机械臂 2 作业末端运动由初始位姿到绝缘子 夹持位姿全过程的路径曲线如图 4( a) 所示． 机械手 2 相对于机器人奇臂行走轮质心在 X( 竖向) 、Y( 纵向) 和 Z( 横向) 三个方向上的运动特性曲线如图 4 ( b) 所示． 图 4( a) 末端运动路径仿真过程，机械臂 2 由初 始位姿无碰运动到绝缘子夹持位姿． 由 图 4 ( b) 可 知，按照既定的关节轨迹规划，在前 5 s，旋转关节和 伸缩关节运动，使机械臂 2 的作业末端在 X 方向上 · 378 ·

·874· 工程科学学报，第38卷，第6期 产生位移，在其后的时间里，旋转关节和伸缩关节停 12 止运动，故X方向的位移恒定不变：Z方向上产生的 ·三次多项式插值 位移由旋转关节和横移关节的运动决定，在前5s旋 ·五次多项式插值 转关节的转角为180°，相应的Z方向上的位移先增 0 加而后减少，5~10s由于横移关节的运动，Z方向上 的位移逐步增加，10s之后保持恒定不变：Y方向上 产生的位移主要由纵移关节决定，在10~15s内，随 着纵移关节的移动，Y方向上的位移发生变化.在机 20 406080100120140160180200 械手2运动的过程中质心同样也会产生微小的运 时间s 动，前10s可以很清楚地观察到作业末端在Y方向 图5不同算法对机器人稳定性的影响 上位移的抖动.以上结果表明五次多项式插值轨迹 Fig.5 Effect of different algorithms on the robot stability 规划方法，机械臂获得较平滑的运动轨迹，具有较好 角范围更小，机器人运行更为稳定.从图5的仿真结 的连续性，同时各关节运动轨迹规划满足运动学要 果也可直观的看出三次多项式插值机械臂运动轨迹 求，机器人的执行末端具有良好的稳定性，机器人在 规划机器人倾角传感器所测的倾角值变化范围大， 作业过程中各关节运行平稳，冲击力小，使得驱动器 机器人抖动更厉害，此外五次多项式插值机械臂由 的控制较容易实现.根据所期望的运动轨迹规划，利 初始位姿到绝缘子夹持位姿耗时较短，由此可见五 用已建立的机器人动力学模型，可实现机器人的动 次多项式插值算法机器人获得较好的稳定性和较高 态控制，完成绝缘子更换作业任务 的作业效率 4.3现场作业实验 为进一步验证五次多项式插值机械臂运动轨迹规 为验证五次多项式插值轨迹规划较三次多项式插 划方法的工程实用性，在实际线路上进行机器人带电 值轨迹规划机械臂运动过程的稳定性，通过机器人携 更换绝缘子串实验，初始状态下机械臂2为前位如 带的倾角传感器测量的倾角值变化范围来量化机械臂 图6(a),旋转π角度至后位再伸长如图6(b),机器人 运动过程机器人的稳定程度，实际线路坡度为10°，在 前进定位到悬垂线夹后横移前移至绝缘子夹持末端与 机械臂2由初始位姿运动到绝缘子夹持位姿的过程中 绝缘子钢帽对齐如图6（©），机械手2纵移内移实现绝 所记录的倾角值变化范围如表2所示.为更加直观反 缘子末端的夹持如图6(d). 映倾角变化对机器人稳定性的影响，在MATLAB中进 通过现场作业实验机器人机械臂2由初始状态进 行了仿真研究，结果如图5所示 入到工作状态，再到绝缘子的夹持整个过程，机器人机 表2机械臂2不同动作倾角值变化范围 械臂2及其末端各关节动作流畅、连续且平稳，由此可 Table 2 Angle ranges of manipulator 2 under different actions 见机器人五次插值轨迹规划方法具有较强的工程实用 倾角值变化范围/() 性，进一步提高了作业效率并且一定程度上体现了机 机器人动作 三次多项式插值五次多项式插值 器人作业的智能性 机械臂旋转（后转） 7-12 9w11 5 结论 机械臂伸缩（伸长） 10 10 行走轮定位悬垂线夹 6~12 8~11 (1)研制出高压输电线路绝缘子串更换带电作业 机械臂横移（前移） 10~11 10 机器人实验样机，较人工作业大大提高了作业效率 机械臂纵移（内移） 10~11 10 (2)分解机械臂作业过程的基本动作，推导出机 械臂基本动作动力学模型的统一表达形式，求出机械 通过表2机器人基本动作所记录的倾角传感器 臂2基本动作的完整动力学模型，提出基于五次多项 所测得的倾角范围可知，在机器人奇臂行走轮定位 式插值的机械臂运动规划方法，求出机械臂2基本动 到悬垂线夹时由于行走轮与悬垂线夹需要碰检，此 作的五次多项式运动轨迹. 时对机器人的振动冲击最大，此时倾角值范围最大， (3)动力学仿真实验验证了机械臂基本动作动 其次是旋转运动对机器人的稳定性有一定影响，伸 力学模型的正确性，运动学仿真实验验证了五次多 缩、横移和纵移运动对机器人稳定性基本不产生影 项式插值算法机械臂运动轨迹规划的有效性，现场 响.通过对比可以得知在相同动作情况下，五次多项 作业实验进一步验证了五次多项式插值算法的工程 式插值机械臂运动较三次多项式插值机械臂运动倾 实用性

工程科学学报，第 38 卷，第 6 期 产生位移，在其后的时间里，旋转关节和伸缩关节停 止运动，故 X 方向的位移恒定不变; Z 方向上产生的 位移由旋转关节和横移关节的运动决定，在前 5 s 旋 转关节的转角为 180°，相应的 Z 方向上的位移先增 加而后减少，5 ～ 10 s 由于横移关节的运动，Z 方向上 的位移逐步增加，10 s 之后保持恒定不变; Y 方向上 产生的位移主要由纵移关节决定，在 10 ～ 15 s 内，随 着纵移关节的移动，Y 方向上的位移发生变化． 在机 械手 2 运 动 的 过 程 中 质 心 同 样 也 会 产 生 微 小 的 运 动，前 10 s 可以很清楚地观察到作业末端在 Y 方向 上位移的抖动． 以上结果表明五次多项式插值轨迹 规划方法，机械臂获得较平滑的运动轨迹，具有较好 的连续性，同时各关节运动轨迹规划满足运动学要 求，机器人的执行末端具有良好的稳定性，机器人在 作业过程中各关节运行平稳，冲击力小，使得驱动器 的控制较容易实现． 根据所期望的运动轨迹规划，利 用已建立的机器人动力学模型，可实现机器人的动 态控制，完成绝缘子更换作业任务． 4. 3 现场作业实验 为验证五次多项式插值轨迹规划较三次多项式插 值轨迹规划机械臂运动过程的稳定性，通过机器人携 带的倾角传感器测量的倾角值变化范围来量化机械臂 运动过程机器人的稳定程度，实际线路坡度为 10°，在 机械臂 2 由初始位姿运动到绝缘子夹持位姿的过程中 所记录的倾角值变化范围如表 2 所示． 为更加直观反 映倾角变化对机器人稳定性的影响，在 MATLAB 中进 行了仿真研究，结果如图 5 所示． 表 2 机械臂 2 不同动作倾角值变化范围 Table 2 Angle ranges of manipulator 2 under different actions 机器人动作 倾角值变化范围/( °) 三次多项式插值 五次多项式插值 机械臂旋转( 后转) 7 ～ 12 9 ～ 11 机械臂伸缩( 伸长) 10 10 行走轮定位悬垂线夹 6 ～ 12 8 ～ 11 机械臂横移( 前移) 10 ～ 11 10 机械臂纵移( 内移) 10 ～ 11 10 通过表 2 机器人基本动作所记录的倾角传感器 所测得的倾角范围可知，在机器人奇臂行走轮定位 到悬垂线夹时由于行走轮与悬垂线夹需要碰检，此 时对机器人的振动冲击最大，此时倾角值范围最大， 其次是旋转运动对机器人的稳定性有一定影响，伸 缩、横移和纵移运动对机器人稳定性基本不产生影 响． 通过对比可以得知在相同动作情况下，五次多项 式插值机械臂运动较三次多项式插值机械臂运动倾 图 5 不同算法对机器人稳定性的影响 Fig． 5 Effect of different algorithms on the robot stability 角范围更小，机器人运行更为稳定． 从图 5 的仿真结 果也可直观的看出三次多项式插值机械臂运动轨迹 规划机器人倾角传感器所测的倾角值变化范围大， 机器人抖动更厉害，此外五次多项式插值机械臂由 初始位姿到绝缘子夹持位姿耗时较短，由此可见五 次多项式插值算法机器人获得较好的稳定性和较高 的作业效率． 为进一步验证五次多项式插值机械臂运动轨迹规 划方法的工程实用性，在实际线路上进行机器人带电 更换绝缘子串实验，初始状态下机械臂 2 为前位如 图 6( a) ，旋转 π 角度至后位再伸长如图 6( b) ，机器人 前进定位到悬垂线夹后横移前移至绝缘子夹持末端与 绝缘子钢帽对齐如图 6( c) ，机械手 2 纵移内移实现绝 缘子末端的夹持如图 6( d) ． 通过现场作业实验机器人机械臂 2 由初始状态进 入到工作状态，再到绝缘子的夹持整个过程，机器人机 械臂 2 及其末端各关节动作流畅、连续且平稳，由此可 见机器人五次插值轨迹规划方法具有较强的工程实用 性，进一步提高了作业效率并且一定程度上体现了机 器人作业的智能性． 5 结论 ( 1) 研制出高压输电线路绝缘子串更换带电作业 机器人实验样机，较人工作业大大提高了作业效率． ( 2) 分解机械臂作业过程的基本动作，推导出机 械臂基本动作动力学模型的统一表达形式，求出机械 臂 2 基本动作的完整动力学模型，提出基于五次多项 式插值的机械臂运动规划方法，求出机械臂 2 基本动 作的五次多项式运动轨迹． ( 3) 动力学仿真实验验证了机械臂基本动作动 力学模型的正确性，运动学仿真实验验证了五次多 项式插值算法机械臂运动轨迹规划的有效性，现场 作业实验进一步验证了五次多项式插值算法的工程 实用性． · 478 ·

江维等：带电作业机器人机械臂动力学建模与运动规划 875 a (b) c 图6带电作业机器人现场实验.(a)初始位姿：(b)工作位姿：()定位悬垂线夹：(d)绝缘子夹持 Fig.6 Live working robot field test:(a)initial pose;(b)work pose:(c)suspension clamp location:(d)insulator holder 参考文献 SciA,2008,9(4):539 [1]Takaoka K,Kazuhiko,Wakisako H,et al.Development of the [10]Book W J,Maizza-Neto 0,Whitney D E.Feedback control of fully automatic live line maintenance robot Phase I//Proceedings two beam two joint system with distributed flexibility.Dyn Syst of the 4th IEEE International Symposium on assembly and Task Meas Control,1975,97(4):424 Planning Soft Research Park.Fukuoka,2001:423 [11]Jin GG,Liu Y W,Wang S X.Dynamic modeling and simula- 2]Aracil R,Pinto E,Ferre M.Robots for live power lines mainte- tion of flexible curve beam with a wide range of motion.I Tianjin nance and inspection task /15th World Congress of the Interna- Umim,2014,37(7):629 tional Federation of Automatic Control.Bacelona,2002:118 (金国光，刘又午，王树新.大范围运动的柔性曲线梁动力学 B]Hu Y.Research and development of live working technology on 建模及仿真.天津大学学报，2014,37(7)：629) transmission and distribution lines.High Voltage Eng,2006,32 02] Pei X W,Wang C Q.Free-floating space flexible manipulator (11):1 trajectory tracking and vibration suppression.J Huazhong Univ (胡毅.输配电线路带电作业技术的研究与发展.高电压技 Sci Technol Nat Sci Ed,2014,39(2):28 术，2006,32(11)：1) (裴希伍，王从庆.自由浮动空间柔性机械臂轨迹跟踪与振 4]Chen Z W,Wu W H,Sheng J.The new locking pin insulator pin 动抑制.华中科技大学学报（自然科学版），2014,39(2)：28) puller development.Zhejiang Power,2014,12(1):26 03] Peng Z H,Sun L,Chen J.UAV online low altitude penetration (陈忠华，邬韦华，沈佳.新型绝缘子锁紧销拔销器的研制.浙 trajectory plan based on improved differential evolution algorithm. 江电力，2014,12(1)：26) J Univ Sci Technol Beijing,2012,34(1):96 [5]Zhang H B.Jing W B.Hu Y H.The design of UHV DC line (彭志红，孙琳，陈杰.基于改进差分进化算法的无人机在线 electrifed insulator replacement jig.J Xihua Univ Nat Sci,2013, 低空突防航迹规划.北京科技大学学报，2012,34(1)：96) 32(6):73 04] Zhang C,Li Q.Dong J Y.Mobile robot path planning based on (张海兵，景文炳，胡应华.特高压直流线路带电更换绝缘子 chaos particle swarm algorithm special genetic programming switc- 卡具的设计.西华大学学报（自然科学版），2013,32(6)：73) hing strategies.J Univ Sci Technal Beijing,2013,35(6):826 6]Zhao Y L,Li Y C,Qi H.The study of high-voltage transmission (张超，李擎，董冀媛，等.基于混沌粒子群专用遗传算法切 lines live working intelligent automatic tool.Manuf Autom,2012, 换策略的移动机器人路径规划.北京科技大学学报，2013,35 34(2):12 (6):826) (赵玉良，李运厂，戚辉，等.高压输电线路带电作业智能化自 [15]Zhu S Q,Liu S G,Wang X Y.Time-optimal and jerk-continu- 动工具研究.制造业自动化，2012,34(2)：12) ous trajectory planning algorithm for manipulators.J Mech Eng, ]Boscariol P,Gasparetto A,Zanotto V.Active position and vibra- 2013.46(3):47 tion control of a flexible links mechanism using model-ased pre- (朱世强，刘松国，王宣银.机械手时间最优脉动连续轨迹规 dictive control.J Dyn Syst Meas Control,2010,132(1):014506 划算法.机械工程学报，2013,46(3)：47) 8] Kalyoncu M.Mathematical modelling and dynamic response of a [16]Liu X Q,Qin Z,Ni J.Three degrees of freedom hydraulic servo multi-straight-ine path tracing flexible robot manipulator with rota- manipulator trajectory optimization.J Zhejiang Univ Eng Sci, ting prismatic joint.Appl Math Modell,2008.32(2):1087 2015,19(3):472 9]Mian AA,Wang D.Dynamic modeling and nonlinear control (刘湘琪，秦臻，倪敬.三自由度液压伺服机械手轨迹优化浙 strategy for an underactuated quad rotor rotorcraft.Zhejiang Univ 江大学学报（工学版），2015,19(3)：472)

江 维等: 带电作业机器人机械臂动力学建模与运动规划 图 6 带电作业机器人现场实验． ( a) 初始位姿; ( b) 工作位姿; ( c) 定位悬垂线夹; ( d) 绝缘子夹持 Fig． 6 Live working robot field test: ( a) initial pose; ( b) work pose; ( c) suspension clamp location; ( d) insulator holder 参 考 文 献 ［1］ Takaoka K，Kazuhiko，Wakisako H，et al． Development of the fully automatic live line maintenance robot Phase Ⅲ/ / Proceedings of the 4th IEEE International Symposium on assembly and Task Planning Soft Ｒesearch Park． Fukuoka，2001: 423 ［2］ Aracil Ｒ，Pinto E，Ferre M． Ｒobots for live power lines mainte￾nance and inspection task / / 15th World Congress of the Interna￾tional Federation of Automatic Control． Bacelona，2002: 118 ［3］ Hu Y． Ｒesearch and development of live working technology on transmission and distribution lines． High Voltage Eng，2006，32 ( 11) : 1 ( 胡毅． 输配电线路带电作业技术的研究与发展． 高电压技 术，2006，32( 11) : 1) ［4］ Chen Z W，Wu W H，Sheng J． The new locking pin insulator pin puller development． Zhejiang Power，2014，12( 1) : 26 ( 陈忠华，邬韦华，沈佳． 新型绝缘子锁紧销拔销器的研制． 浙 江电力，2014，12( 1) : 26) ［5］ Zhang H B，Jing W B，Hu Y H． The design of UHV DC line electrified insulator replacement jig． J Xihua Univ Nat Sci，2013， 32( 6) : 73 ( 张海兵，景文炳，胡应华． 特高压直流线路带电更换绝缘子 卡具的设计． 西华大学学报( 自然科学版) ，2013，32( 6) : 73) ［6］ Zhao Y L，Li Y C，Qi H． The study of high-voltage transmission lines live working intelligent automatic tool． Manuf Autom，2012， 34( 2) : 12 ( 赵玉良，李运厂，戚辉，等． 高压输电线路带电作业智能化自 动工具研究． 制造业自动化，2012，34( 2) : 12) ［7］ Boscariol P，Gasparetto A，Zanotto V． Active position and vibra￾tion control of a flexible links mechanism using model-based pre￾dictive control． J Dyn Syst Meas Control，2010，132( 1) : 014506 ［8］ Kalyoncu M． Mathematical modelling and dynamic response of a multi-straight-line path tracing flexible robot manipulator with rota￾ting prismatic joint． Appl Math Modell，2008，32( 2) : 1087 ［9］ Mian A A，Wang D． Dynamic modeling and nonlinear control strategy for an underactuated quad rotor rotorcraft． J Zhejiang Univ Sci A，2008，9( 4) : 539 ［10］ Book W J，Maizza-Neto O，Whitney D E． Feedback control of two beam two joint system with distributed flexibility． J Dyn Syst Meas Control，1975，97( 4) : 424 ［11］ Jin G G，Liu Y W，Wang S X． Dynamic modeling and simula￾tion of flexible curve beam with a wide range of motion． J Tianjin Univ，2014，37( 7) : 629 ( 金国光，刘又午，王树新． 大范围运动的柔性曲线梁动力学 建模及仿真． 天津大学学报，2014，37( 7) : 629) ［12］ Pei X W，Wang C Q． Free-floating space flexible manipulator trajectory tracking and vibration suppression． J Huazhong Univ Sci Technol Nat Sci Ed，2014，39( 2) : 28 ( 裴希伍，王从庆． 自由浮动空间柔性机械臂轨迹跟踪与振 动抑制． 华中科技大学学报( 自然科学版) ，2014，39( 2) : 28) ［13］ Peng Z H，Sun L，Chen J． UAV online low altitude penetration trajectory plan based on improved differential evolution algorithm． J Univ Sci Technol Beijing，2012，34( 1) : 96 ( 彭志红，孙琳，陈杰． 基于改进差分进化算法的无人机在线 低空突防航迹规划． 北京科技大学学报，2012，34( 1) : 96) ［14］ Zhang C，Li Q，Dong J Y． Mobile robot path planning based on chaos particle swarm algorithm special genetic programming switc￾hing strategies． J Univ Sci Technol Beijing，2013，35( 6) : 826 ( 张超，李擎，董冀媛，等． 基于混沌粒子群专用遗传算法切 换策略的移动机器人路径规划． 北京科技大学学报，2013，35 ( 6) : 826) ［15］ Zhu S Q，Liu S G，Wang X Y． Time-optimal and jerk-continu￾ous trajectory planning algorithm for manipulators． J Mech Eng， 2013，46( 3) : 47 ( 朱世强，刘松国，王宣银． 机械手时间最优脉动连续轨迹规 划算法． 机械工程学报，2013，46( 3) : 47) ［16］ Liu X Q，Qin Z，Ni J． Three degrees of freedom hydraulic servo manipulator trajectory optimization． J Zhejiang Univ Eng Sci， 2015，19( 3) : 472 ( 刘湘琪，秦臻，倪敬． 三自由度液压伺服机械手轨迹优化． 浙 江大学学报( 工学版) ，2015，19( 3) : 472) · 578 ·