江维等：带电作业机器人机械臂动力学建模与运动规划 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 ·