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

工程科学学报，第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 ·