·658· 智能系统学报 第14卷 隆 于齐次变换原理，可得 (19) 0=0 式中k=A2+B2+C。 x'i=r.cos (23) y轴方向可由右手法则确定，其方向为0=a×n。 y':=r·sin0 令p=[o%「，由齐次变换原理回，可求得坐标系 2=0 对k进行$型速度规划便可完成圆弧运动 o-xyz到局部坐标系o-yz的齐次变换矩阵T: 的插补计算，根据弧长和速度等约束条件可分为 n o a p T=0000 (20) 七段、六段、五段、四段4种情况，可参考直线部分。 令局部坐标系内的任意一点P的齐次坐标为 2.2 基于四元数的姿态规划 [xyz,则P点在坐标系o-xyt的坐标为 四元数具有无数据冗余、不存在万向节锁死 [xy,有 等优点1。设q=s,(a,bc由机器人正运动学齐 次矩阵可求对应的两个互补的单位四元数： yI]=T[xy 1T (21) s=±Vn.+0,+a:+1/2 a=(o2-a)/4s b=(a.-n-)/4s (24) P c=(n,-0x)/4s 设工业机器人的初始姿态和目标姿态分别 为R,和R2,根据式(24)可求出其对应的四元数 9和92。由于每个姿态矩阵对应两个互补的四元 数，考虑到时间最优，选择与前一姿态夹角较小 的一组四元数。定义两个四元数夹角为2，则： 图2基于局部坐标系的圆弧插补 cos2=q1·92 (25) Fig.2 Arc interpolation based on local coordinate system 对于两点间的姿态可由Slerp公式1求出： 3)求圆弧的夹角0。由式(21)计算出空间三 点P、P2、P3在坐标系o-yz下相应坐标为： Slerp()=9 sin(1+sin(a) (26) sin O P1(xy,0)、P3(,0)、P(,0)。 2.3轨迹规划流程 整个圆弧轨迹插补过程的角度为0。得到： 轨迹规划需通过插补算法来实现。本文采用 arctan 2(y3',x3)+2,y3'<O; 弧长增量法的插补技术，即在每个插补周期内， 0= (22) arctan2(y3',x3),y3>0. 按照进给速度用微小线段去逼近目标轮廓线。 4)在圆弧所在平面上Ⅱ1进行平面规划。基 本文设计的插补算法程序流程如图3所示。 获得起始点位姿、速 度、加速度等相关参数 5≥253 vmax s<2s, Y Y Y Y 七段式 六段式 五段式 四段式 +(44a3 Vma-Jfi S型速度规划 位姿规划 逆解求得各关节角度 插补运动☐ 图3插补算法程序流程 Fig.3 Program flow of interpolation algorithma = [ A k B k C k ]T (19) k = √ 式中 A2 + B2 +C2。 y ′ o = a× n p = [ x0 y0 z0 ]T o ′ − x ′ y ′ z ′ o− xyz T 轴方向可由右手法则确定，其方向为 。 令 ，由齐次变换原理[12] ，可求得坐标系 到局部坐标系 的齐次变换矩阵 ： T = [ n o a p 0 0 0 0 ] (20) P [ x y z 1 ]T P o ′ − x ′ y ′ z ′ [ x ′ y ′ z ′ 1 ]T 令局部坐标系内的任意一点 的齐次坐标为 ， 则 点在坐标系 的坐标为 ，有 [ x ′ y ′ z ′ 1 ]T = T −1 [ x ′ y ′ z ′ 1 ]T (21) P3 ′ Pk ′ z′ x′ y′ o′ x y z o P1 ′ Π1 λk θ θ 图 2 基于局部坐标系的圆弧插补 Fig. 2 Arc interpolation based on local coordinate system θ P1 P2 P3 o ′ − x ′ y ′ z ′ P ′ 1 (x ′ 1 , y ′ 1 ,0) P ′ 2 (x ′ 2 , y ′ 2 ,0) P ′ 3 (x ′ 3 , y ′ 3 ,0) 3) 求圆弧的夹角 。由式 (21) 计算出空间三 点 、 、 在坐标系 下相应坐标为： 、 、 。 整个圆弧轨迹插补过程的角度为 θ 。得到： θ = { arctan 2(y3 ′ , x3 ′ )+2π, y3 ′ < 0; arctan 2(y3 ′ , x3 ′ ), y3 ′ ⩾ 0. (22) 4) 在圆弧所在平面上 Π1 进行平面规划。基 于齐次变换原理，可得    θk = λk · θ x ′ i = r· cos θk y ′ i = r·sinθk z ′ i = 0 (23) 对 λk 进行 S 型速度规划便可完成圆弧运动 的插补计算，根据弧长和速度等约束条件可分为 七段、六段、五段、四段 4 种情况，可参考直线部分。 2.2 基于四元数的姿态规划 q = [s,(a,b, c)] 四元数具有无数据冗余、不存在万向节锁死 等优点[13]。设 ，由机器人正运动学齐 次矩阵可求对应的两个互补的单位四元数：    s = ± √ nx +oy +az +1/2 a = (oz −ay)/4s b = (ax −nz)/4s c = (ny −ox)/4s (24) Ω 设工业机器人的初始姿态和目标姿态分别 为 R1 和 R2，根据式 (24) 可求出其对应的四元数 q1 和 q2。由于每个姿态矩阵对应两个互补的四元 数，考虑到时间最优，选择与前一姿态夹角较小 的一组四元数。定义两个四元数夹角为 ，则： cos Ω = q1 · q2 (25) 对于两点间的姿态可由 Slerp 公式[13] 求出： Slerp(q1 ,q2 , λk) = q1 sin(1−λk)Ω+q2 sin(q2Ω) sin Ω (26) 2.3 轨迹规划流程 轨迹规划需通过插补算法来实现。本文采用 弧长增量法的插补技术，即在每个插补周期内， 按照进给速度用微小线段去逼近目标轮廓线[14]。 本文设计的插补算法程序流程如图 3 所示。 获得起始点位姿、速 度、加速度等相关参数 s>2s1 s<2s1 s≥2s  2   s<2s2 逆解求得各关节角度 插补运动 位姿规划 a 2 max vmax≤ J vmax s>2vmax J    S 型速度规划 七段式 六段式 五段式 四段式 Y Y Y Y amax, vmax 2 sJ2 amax=3 , vmax=Jt2 v 1 maxJ, vmax amax= amax, vmax= a 2 max ( 2 a 2 max J − + J ) 2+4amaxs 图 3 插补算法程序流程 Fig. 3 Program flow of interpolation algorithm ·658· 智 能 系 统 学 报 第 14 卷