对运动学实验教程中相关旋量法的解释（附讲义）

[mhuasong]

3.5.1.2旋量法
在第二章中我们曾经介绍过螺旋轴及罗德里格斯参数表示的姿态矩阵。
给定参考坐标系，螺旋轴S可写成一个六维向量，又称运动旋量(twist)。
S=[■(υ@ω)] ∈R^6x1 （3-30）
式(3-30)中，对于旋转关节，ω为沿关节轴正方向的单位向量，υ为线速度，υ=-ωxq，q为关节轴上任一点，其坐标值在基坐标系中进行度量，一般为了计算方便，可将重合坐标轴上的值取0。若关节为平移关节，则ω=0，υ为沿关节轴正方向的单位向量，θ表示移动的距离。
针对串联机器人，我们在第二章中描述过的指数积(PoE)公式，可以将其理解为每个关节产生的螺旋运动施加给了后面的连杆。假设机器人由n个单自由度关节串联而成，我们首先选择基坐标系{s}以及末端坐标{b}，将机器人置于初始位置（或零位，即所有关节变量初始值为0），已知末端坐标系的初始位形为M，当得到所有关节变量(θ_1,⋯θ_n)时，则该n个关节串联机器人末端的新位形可以指数积公式（3-31）进行表示。
T(θ)=e^([S_1 ] θ_1 )⋯e^([S_(n-1) ] θ_(n-1) ) e^([S_n ] θ_n ) M (3-31)
将矩阵指数e^([s]θ)进行级数展开，得：
e^([s]θ)=I+[S]θ+[S] θ^2/2!+〖[S]〗^2 θ^3/3!+⋯=[■(e^([ω ̂]θ)&G(θ)v@0&1)] (3-32)
G(θ)=Iθ+[ω ̂ ] θ^2/2!+〖[ω ̂]〗^2 θ^3/3!+⋯
利用单位向量的反对称矩阵特性，〖[ω ̂]〗^3=-[ω ̂ ],〖[ω ̂]〗^4=-[ω ̂ ]^2,〖[ω ̂]〗^5=[ω ̂ ],如此类推， G(θ)可进一步简化为：
G(θ)=I+(θ^2/2!-θ^4/4!+θ^6/6!-⋯)[ω ̂ ]+(θ^3/3!-θ^5/5!+θ^7/7!-⋯) [ω ̂ ]^2
=Iθ+(1-cos⁡〖θ)[ω ̂]〗+(θ-sin⁡〖θ)〗 〖[ω ̂]〗^2 (3-33)
（1）如果‖ω‖=1，该螺旋轴为旋转关节，则对于任意沿螺旋轴的旋转角度θ∈R，都有：
e^([s]θ)=[■(e^([ω ̂]θ)&(Iθ+(1-cos⁡〖θ)[ω ̂]〗+(θ-sin⁡〖θ)〗 [ω ̂ ]^2)v@0&1)] (3-34)
其中，e^([ω ̂]θ)表示刚体绕转轴ω ̂旋转 θ后的姿态，可以用如下的罗德里格斯公式计算：
e^([ω ̂]θ)=I+sin⁡θ [ω ̂ ]+(1-cos⁡〖θ)〖[ω ̂]〗^2 〗 （3-35）
[ω ̂ ]为单位向量ω的反对称矩阵，如果旋转轴在基坐标系{s}下的向量为(ω_x,ω_y,ω_z)，则：
[ω ̂ ]=[■(0&-ω_z&ω_y@ω_z&0&-ω_x@-ω_y&ω_x&0)] （3-36）
（2）如果ω=0，该螺旋轴为平移关节，则：
e^([s]θ)=[■(e^([ω ̂]θ)&vθ@0&1)] (3-37)

该方法与D-H表示法建模不同，被称为旋量法建模，其无须定义连杆坐标系，也不要求机器人必须满足Pieper准则。

如图3.19所示，我们可以对图3.15所示的机器人建立6个螺旋轴(screw axis)（{S1,S2,S3,S4,S5,S6}），假设图3.19为每个螺旋轴的零位位置，那么终端工具坐标系{b}在零位状态下的初始位姿矩阵见式（3-30）。

图3.19 AUBO-i5螺旋轴表示法, 右手法则表示正转，W1为S1螺旋轴与S5螺旋轴之间的距离，L1=0.408m，L2=0.376m，W1=121.5mm，W2=94mm，H1=98.5mm,H2=102.5mm
M=[■(■(1&0@0&0)&■(0&-(L1+L2)@-1&-(W1+W2))@■(0&1@0&0)&■(0& H1-H2@0&1))] （3-38）
其中，末端TCP姿态矩阵可以认为是终端坐标系{b}相对基坐标系{s}绕X轴旋转φ=90度，其3x3的姿态矩阵为：
R=[■(1&0&0@0&cos⁡φ&-sin⁡φ@0&sin⁡φ&cos⁡φ )]=[■(1&0&0@0&0&-1@0&1&0)]
AUBO-i5每个轴S_i都是旋转轴，其运动旋量(twist)可表示为式（3-39）。
S_i=[■(υ_i@ω_i )] ∈R^6×1 （3-39）
图3.19中，S_1的方向和基坐标系{s}的z轴重合，所以其瞬时角速度ω_1=(ω_1x,ω_1y,ω_1z )=(0,0,1), 线速度v_1=(v1,v2,v3)=(0,0,0)（为计算简便，将关节1上的点q_2选在基坐标系{s}的原点）；
S_2的方向和基坐标系{s}的y轴重合,方向相反，所以其瞬时角速度ω_2=(ω_2x,ω_2y,ω_2z )=(0,-1,0), 选择螺旋轴S_2的基点q_2=(0,0,H1),线速度v_2=-ω_2×q_2=(H_1,0,0)；
S_3的方向和基坐标系{s}的y轴重合,方向相反，所以其瞬时角速度ω_3=(ω_3x,ω_3y,ω_3z )=(0,-1,0), 选择螺旋轴S_3的基点q_3=(-L1,0,H1),,线速度v_3=-ω_3×q_3=(H_1,0,L1)；
S_4的方向和基坐标系{s}的y轴重合,方向相反，所以其瞬时角速度ω_4=(ω_4x,ω_4y,ω_4z )=(0,-1,0), 选择螺旋轴S_4的基点q_4=(-L1-L2,0,H1),,线速度v_4=-ω_4×q_4=(H_1,0,L1+L2)；
S_5的方向和基坐标系{s}的Z轴重合,方向相反，所以其瞬时角速度ω_5=(ω_5x,ω_5y,ω_5z )=(0,0,-1), 选择螺旋轴S_5的基点q_5=(-L1-L2,-W1,H1),线速度v_5=-ω_5×q_5=(W_1,-L1-L2,0)；
S_6的方向和基坐标系{s}的y轴重合,方向相反，所以其瞬时角速度ω_6=(ω_6x,ω_6y,ω_6z )=(0,-1,0), 选择螺旋轴S_6的基点q_6=(-L1-L2,0,H1-H2),线速度v_6=-ω_6×q_6=(H1-H2,0,L1+L2)。
由此，螺旋轴的S_i=(ω_i,ν_i)如表3.5所示。
表3.5 各旋转轴向量表
i ω_i ν_i
1 （0，0，1） （0，0，0）
2 （0，-1，0） （H1，0，0）
3 （0，-1，0） （H1，0，L1）
4 （0，-1，0） （H1，0，L1+L2）
5 （0，0，-1） （W1，-（L1+L2），0）
6 （0，-1，0） （H1-H2，0，L1+L2）
AUBO-i5有六个旋转轴，假设六个关节的旋转角为〖{θ〗_1,θ_2,θ_3,θ_4,θ_5,θ_6)，则末端工具坐标系的姿态矩阵T可以由PoE指数积公式(3-40)表达。
T(θ)=e^[S1]θ1 e^[S2]θ2 e^[S3]θ3 e^[S4]θ4 e^[S5]θ5 e^[S6]θ6 M (3-40)
其中指数积e^([s]θ)可采用公式（3-34）和（3-37）进行计算。
公式（3-40）就是正运动学求解方程，将每个关节的旋转角度代入该PoE公式，即可得到末端TCP经过各关节旋转运动后的姿态矩阵。