【机器人学的数学基础】（2）使用指数积公式对SCARA和拟人（肘）机械臂进行正运动学建模

一般的机器人学教材中，首先介绍的是使用DH方法对机械臂进行正运动学建模，DH方法是对每个连杆给定4个参数，建立齐次矩阵相乘后即可得到机械臂末端的位置和姿态的表达式，另外一种建模方法是指数积公式，这种方法的知名度不高，是因为它的前提是要掌握李群、李代数和螺旋理论，但是我觉得这种方法较DH方法来说，是更直观的。

本文介绍使用指数积方法对SCARA机械臂和拟人机械臂（有时也被称为肘机械臂）这两种构型的机械臂进行正运动学建模。

一、SCARA机械臂

step1:
求解 表示θ=0 的时候工具坐标在基坐标中的位形的齐次变换矩阵。

gst(0)=⎡⎣⎢⎢⎢I0⎛⎝⎜0l1+l2l0⎞⎠⎟1⎤⎦⎥⎥⎥

step2:
设关节1，2，3的角速度的方向为沿z轴，所以 w1=w2=w3 = [0 0 1 ]T ;
step3:
将 wi 写成反对称矩阵的形式：

w∧1=w∧2=w∧3=⎡⎣⎢010−100000⎤⎦⎥

step4:
相应的坐标系原点在基座标系中的位置：

q1=⎡⎣⎢000⎤⎦⎥;q2=⎡⎣⎢0l10⎤⎦⎥;q3=⎡⎣⎢0l1+l20⎤⎦⎥;

step5:
旋转关节1,2,3运动旋量坐标：

ξ1=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢000001⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥ξ2=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢l100001⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥ξ3=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢l1+l200001⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥;

step6:
移动关节4的运动旋量坐标：

ξ4=[v40]=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢001000⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥;

step7:
求解各个关节运动的刚体运动的李代数的矩阵形式：

ξ1∧=[w∧10v10]=⎡⎣⎢⎢⎢0100−100000000000⎤⎦⎥⎥⎥;

ξ2∧=[w∧20v20]=⎡⎣⎢⎢⎢0100−10000000l1000⎤⎦⎥⎥⎥;

ξ3∧=[w∧30v30]=⎡⎣⎢⎢⎢0100−10000000l1+l2000⎤⎦⎥⎥⎥;

ξ4∧=[w∧40v40]=⎡⎣⎢⎢⎢0000000000000010⎤⎦⎥⎥⎥;

step8:
求解各个关节的指数映射( se(3)→SE(3) )：

eξ∧1θ1=[ew∧1θ10(I−ew1∧θ1)(w1×v1)1]=⎡⎣⎢⎢⎢cosθ1sinθ100−sinθ1cosθ10000100001⎤⎦⎥⎥⎥

eξ∧2θ2=[ew∧2θ20(I−ew2∧θ2)(w2×v2)1]=⎡⎣⎢⎢⎢cosθ2sinθ200−sinθ2cosθ2000010l1sinθ2l1(1−cosθ2)01⎤⎦⎥⎥⎥;

eξ∧3θ3=[ew∧3θ30(I−ew3∧θ3)(w3×v3)1]=⎡⎣⎢⎢⎢⎢cosθ3sinθ300−sinθ3cosθ3000010(l1+l2)sinθ3(l1+l2)(1−cosθ3)01⎤⎦⎥⎥⎥⎥;

eξ∧4θ4=[ew∧4θ40(I−ew4∧θ4)(w4×v4)1]=⎡⎣⎢⎢⎢10000100001000θ41⎤⎦⎥⎥⎥;

step9:
根据指数积公式可求得机械臂的正运动学模型：

gst(θ)=eξ1∧θ1eξ2∧θ2eξ3∧θ3eξ4∧θ4gst(0)=[R(θ)0p(θ)1]

⇒ 机械臂末端的姿态矩阵：

R(θ)=⎡⎣⎢cos(θ1+θ2+θ3)sin(θ1+θ2+θ3)0−sin(θ1+θ2+θ3)cos(θ1+θ2+θ3)0001⎤⎦⎥

机械臂末端的位置向量：

p(θ)=⎡⎣⎢−l1sinθ1−l2sin(θ1+θ2)l1cosθ1+l2cos(θ1+θ2)l0+θ4⎤⎦⎥

二、拟人机械臂

拟人机械臂（肘机械臂）的正运动学建模方法和scara机械臂建模方法的步骤是一致的：
step1：
求解工具坐标系在基座标系中的位形的齐次矩阵：

gst(0)=⎡⎣⎢⎢⎢I0⎛⎝⎜0l1+l2l⎞⎠⎟1⎤⎦⎥⎥⎥;

step2-step4：
根据建立的坐标系系统来确定每个关节的轴的向量 wi 和每个坐标系在基座标系中的位置 pi 。
step5：
6个旋转关节的旋量坐标

ξ1=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢−⎛⎝⎜001⎞⎠⎟×⎛⎝⎜00l0⎞⎠⎟⎛⎝⎜001⎞⎠⎟⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢000001⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥;

ξ2=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢−⎛⎝⎜−101⎞⎠⎟×⎛⎝⎜00l0⎞⎠⎟⎛⎝⎜−100⎞⎠⎟⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢0−l00−100⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥;

ξ3=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢0−l0l1−100⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥ξ4=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢l1+l200001⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥ξ5=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢0−l0l1+l2−100⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥ξ6=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢−l000010⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥

step6:
该机械臂的构型中不含有移动关节，故省略。
step7:
将step5中的运动旋量坐标转换为矩阵形式。
step8:
根据指数积公式可求得机械臂的正运动学模型：

gst(θ)=eξ1∧θ1eξ2∧θ2⋯eξ6∧θ6gst(0)=[R(θ)0p(θ)1]

⇒
机械臂末端的姿态矩阵和位置向量：

R(θ)=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥

p(θ)=⎡⎣⎢−sinθ1(l1cosθ2+l2cos(θ2+θ3))cosθ1(l1cosθ2+l2cos(θ2+θ3))l0−l1sinθ2−l2sin(θ2+θ3)⎤⎦⎥

其中：

参考文献：
A Mathematical Introduction to Robotic Manipulation.