汪世鹏等：基于EtherCAT总线的七自由度机械臂的隐蔽攻击技术 1655· 1000 式(2)中，[nx,ny,n=、[ox,o,o]、[ax,ay,a]为机 械臂末端的俯仰角、偏航角和滚转角，，， 875 为机械臂末端的位移.依据所建立的D-H坐标系 d 对七自由度机械臂逆运动学模型求解！由于构 768 型设计上符合存在封闭解的Pieper准则P),可求解 677 出各个关节角的解析解方程，得到解析的运动学 y 模型. 1.2机械臂PID控制器模型 526 机械臂的运动规划需要各个关节的控制器依 据模型的参数进行计算才能实施.机械臂运动控 400 d 制是依靠各个关节执行器来达到目标位姿.本文 所用机械臂的关节控制器是采用PD进行调节 234 的.具体PID控制框图如图2所示 0 图2中，y表示七自由度机械臂期望的末端位 d 100 置，表示七自由度机械臂期望的末端速度，表示 七自由度机械臂期望的末端加速度；P表示规划 轨迹的期望位置；w,=1…7)表示关节位置， (i=1…7)表示关节速度，0=1…7)表示关节加速 图1七自由度机械臂DH坐标系 度；x、太、分别表示七自由度机械臂末端期望位 Fig.1 7-DOF manipulator D-H coordinate system 置、期望速度和期望加速度 坐标系{}相对于坐标系{i-1}的坐标变换矩阵 图2所示关节控制器采用三闭环的位置随动 如下 系统，位置环使得各个关节到达指定的位置，速度 FT= 环使得各个关节的速度能够按照要求的量进行转 cos sini 0 动，电流环可以防止速度过载从而提升系统稳 ai-1 sin0icosai-1 cos0;cosai-1 -sinai-1-sinai-1di 定性 关节位置控制器是位置式PID控制器为 sin;sinai-1 cos;sinai-1 cosai-1 cosai-idi 0 0 0 1 (1) o=kk肉+20+eu-- 已知目标位姿(x,y,,,B,y),则建立逆运动学 求解等式 nx Ox dx 1 9T-T]T3TTSTST9T y Oy 4 (2) 0: a t (3) 0 0 0 1 式中，Ok)表示第k次的关节转动输出，表示可调 表1 机械臂D-H坐标系参数 Table 1 D-H coordinate system parameters of manipulator Joint,i Twist angle,a) Link length,a1 Joint angle,, Offset of connecting rod,d/mm Range/() 1 0 0 8 234 ±180 2 % 0 6 0 ±135 3 90 0 8的 292 ±180 4 -90 0 8 0 135 5 甲 0 0s 242 ±180 6 -90 0 9% 0 ±135 7 90 0 0 107 ±180坐标系{i}相对于坐标系{i–1}的坐标变换矩阵 如下 i−1 i T =   cos θi −sinθi 0 ai−1 sinθi cosαi−1 cos θi cosαi−1 −sinαi−1 −sinαi−1di sinθisinαi−1 cos θisinαi−1 cosαi−1 cosαi−1di 0 0 0 1   （1） (xt , yt ,zt 已知目标位姿 ,α, β, γ),则建立逆运动学 求解等式 0 7 T = 0 1 T 1 2 T 2 3 T 3 4 T 4 5 T 5 6 T 6 7 T =   nx ox ax xt ny oy ay yt nz oz az zt 0 0 0 1   （2） [nx, ny, nz] T [ox, oy, oz] T [ax, ay, az] T [xt , yt , zt] T 式（2）中， 、 、 为机 械臂末端的俯仰角、偏航角和滚转角， 为机械臂末端的位移. 依据所建立的 D-H 坐标系 对七自由度机械臂逆运动学模型求解[22] . 由于构 型设计上符合存在封闭解的 Pieper 准则[23] ，可求解 出各个关节角的解析解方程，得到解析的运动学 模型. 1.2    机械臂 PID 控制器模型 机械臂的运动规划需要各个关节的控制器依 据模型的参数进行计算才能实施. 机械臂运动控 制是依靠各个关节执行器来达到目标位姿. 本文 所用机械臂的关节控制器是采用 PID 进行调节 的. 具体 PID 控制框图如图 2 所示. y y˙ y¨ ωi(i = 1···7) ω˙ i (i = 1···7) ω¨ i(i = 1···7) x、 x˙、x¨ 图 2 中， 表示七自由度机械臂期望的末端位 置， 表示七自由度机械臂期望的末端速度， 表示 七自由度机械臂期望的末端加速度；Pd 表示规划 轨迹的期望位置 ； 表示关节位置 ， 表示关节速度， 表示关节加速 度； 分别表示七自由度机械臂末端期望位 置、期望速度和期望加速度. 图 2 所示关节控制器采用三闭环的位置随动 系统，位置环使得各个关节到达指定的位置，速度 环使得各个关节的速度能够按照要求的量进行转 动，电流环可以防止速度过载从而提升系统稳 定性. 关节位置控制器是位置式 PID 控制器为    O(k) = Kˆ p   e(k)+ Ts TI ∑n i e(i)+ Td Ts (e(k)−e(k−1))   Kˆ i = Kˆ p Ts TI Kˆ d = Kˆ p Td Ts （3） O(k) k Kˆ 式中， 表示第 次的关节转动输出， p 表示可调 1000 d7 d5 d3 d1 x0 z0 y0 z1 y2 z3 y4 z5 z7 y6 x2 x1 x4 x3 x6 x7 x5 y1 z2 y3 z4 y5 y7 z6 875 768 677 526 400 234 100 0 图 1    七自由度机械臂 D-H 坐标系 Fig.1    7-DOF manipulator D-H coordinate system 表 1 机械臂 D-H 坐标系参数 Table 1 D-H coordinate system parameters of manipulator Joint, i Twist angle, αi–1/(°) Link length, ai–1 Joint angle, θi Offset of connecting rod, di /mm Range/(°) 1 0 0 θ1 234 ±180 2 –90 0 θ2 0 ±135 3 90 0 θ3 292 ±180 4 –90 0 θ4 0 ±135 5 90 0 θ5 242 ±180 6 –90 0 θ6 0 ±135 7 90 0 θ7 107 ±180 汪世鹏等： 基于 EtherCAT 总线的七自由度机械臂的隐蔽攻击技术 · 1655 ·