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

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

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

一、SCARA机械臂

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

g s t (0) = ⎡ ⎣ ⎢ ⎢ ⎢ I 0 ⎛ ⎝ ⎜ 0 l 1 + l 2 l 0 ⎞ ⎠ ⎟ 1 ⎤ ⎦ ⎥ ⎥ ⎥

${{g}_{st}}\left( 0 \right)=\left[ \begin{matrix} I & \left( \begin{matrix} 0 \\ {{l}_{1}}+{{l}_{2}} \\ {{l}_{0}} \\ \end{matrix} \right) \\ 0 & 1 \\ \end{matrix} \right]$
step2:
设关节1，2，3的角速度的方向为沿z轴，所以

w1=w2=w3 ${{w}_{1}}={{w}_{2}}={{w}_{3}}$ =

[0 0 1 ]T $\left[ \begin{matrix} 0 \ 0 \ 1 \ \end{matrix} \right]^{T}$ ;
step3:
将

wi ${w_{i}}$ 写成反对称矩阵的形式：

w \land 1 = w \land 2 = w \land 3 = ⎡ ⎣ ⎢ 010 - 1 00 000 ⎤ ⎦ ⎥

${{\overset{\wedge }{\mathop{w}}\,}_{1}}={{\overset{\wedge }{\mathop{w}}\,}_{2}}={{\overset{\wedge }{\mathop{w}}\,}_{3}}=\left[ \begin{matrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \\ \end{matrix} \right]$
step4:
相应的坐标系原点在基座标系中的位置：

q 1 = ⎡ ⎣ ⎢ 000 ⎤ ⎦ ⎥; q 2 = ⎡ ⎣ ⎢ 0 l 1 0 ⎤ ⎦ ⎥; q 3 = ⎡ ⎣ ⎢ 0 l 1 + l 2 0 ⎤ ⎦ ⎥;

${{q}_{1}}=\left[ \begin{matrix} 0 \\ 0 \\ 0 \\ \end{matrix} \right];{{q}_{2}}=\left[ \begin{matrix} 0 \\ {{l}_{1}} \\ 0 \\ \end{matrix} \right];{{q}_{3}}=\left[ \begin{matrix} 0 \\ {{l}_{1}}+{{l}_{2}} \\ 0 \\ \end{matrix} \right];$
step5:
旋转关节1,2,3运动旋量坐标：

ξ 1 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 000001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ξ 2 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ l 1 00001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ξ 3 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ l 1 + l 2 00001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥;

${{\xi }_{1}}= \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 1 \\ \end{bmatrix}{{\xi }_{2}}=\begin{bmatrix} {{l}_{1}} \\ 0 \\ 0 \\ 0 \\ 0 \\ 1 \end{bmatrix}{{\xi }_{3}}=\left[ \begin{matrix} {{l}_{1}}+{{l}_{2}} \\ 0 \\ 0 \\ \begin{align} & 0 \\ & 0 \\ & 1 \\ \end{align} \\ \end{matrix} \right];$
step6:
移动关节4的运动旋量坐标：

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

${{\xi }_{4}}=\left[ \begin{matrix} {{v}_{4}} \\ 0 \\ \end{matrix} \right]=\left[ \begin{matrix} 0 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0 \\ \end{matrix} \right];$
step7:
求解各个关节运动的刚体运动的李代数的矩阵形式：

ξ 1 \land = [w \land 1 0 v 1 0] = ⎡ ⎣ ⎢ ⎢ ⎢ 0100 - 1 000 00000000 ⎤ ⎦ ⎥ ⎥ ⎥;

$\overset{\wedge }{\mathop{{{\xi }_{1}}}}\,=\left[ \begin{matrix} {{\overset{\wedge }{\mathop{w}}\,}_{1}} & {{v}_{1}} \\ 0 & 0 \\ \end{matrix} \right]=\left[ \begin{matrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \end{matrix} \right];$

ξ 2 \land = [w \land 2 0 v 2 0] = ⎡ ⎣ ⎢ ⎢ ⎢ 0100 - 1 000 0000 l 1 000 ⎤ ⎦ ⎥ ⎥ ⎥;

$\overset{\wedge }{\mathop{{{\xi }_{2}}}}\,=\left[ \begin{matrix} {{\overset{\wedge }{\mathop{w}}\,}_{2}} & {{v}_{2}} \\ 0 & 0 \\ \end{matrix} \right]=\left[ \begin{matrix} 0 & -1 & 0 & {{l}_{1}} \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \end{matrix} \right];$

ξ 3 \land = [w \land 3 0 v 3 0] = ⎡ ⎣ ⎢ ⎢ ⎢ 0100 - 1 000 0000 l 1 + l 2 000 ⎤ ⎦ ⎥ ⎥ ⎥;

$\overset{\wedge }{\mathop{{{\xi }_{3}}}}\,=\left[ \begin{matrix} {{\overset{\wedge }{\mathop{w}}\,}_{3}} & {{v}_{3}} \\ 0 & 0 \\ \end{matrix} \right]=\left[ \begin{matrix} 0 & -1 & 0 & {{l}_{1}}+{{l}_{2}} \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ \end{matrix} \right];$

ξ 4 \land = [w \land 4 0 v 4 0] = ⎡ ⎣ ⎢ ⎢ ⎢ 0000000000000010 ⎤ ⎦ ⎥ ⎥ ⎥;

$\overset{\wedge }{\mathop{{{\xi }_{4}}}}\,=\left[ \begin{matrix} {{\overset{\wedge }{\mathop{w}}\,}_{4}} & {{v}_{4}} \\ 0 & 0 \\ \end{matrix} \right]=\left[ \begin{matrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ \end{matrix} \right];$
step8:
求解各个关节的指数映射(

se(3)→SE(3) $se\left( 3 \right)\to SE\left( 3 \right)$ )：

e ξ \land 1 θ 1 = [e w \land 1 θ 1 0 (I - e w 1 \land θ 1) (w 1 \times v 1) 1] = ⎡ ⎣ ⎢ ⎢ ⎢ cos θ 1 sin θ 1 00 - sin θ 1 cos θ 1 00 00100001 ⎤ ⎦ ⎥ ⎥ ⎥

${{e}^{{{\overset{\wedge }{\mathop{\xi }}\,}_{1}}{{\theta }_{1}}}}=\left[ \begin{matrix} {{e}^{{{\overset{\wedge }{\mathop{w}}\,}_{1}}{{\theta }_{1}}}} & (I-{{e}^{\overset{\wedge }{\mathop{{{w}_{1}}}}\,{{\theta }_{1}}}})({{w}_{1}}\times {{v}_{1}}) \\ 0 & 1 \\ \end{matrix} \right]=\left[ \begin{matrix} \cos {{\theta }_{1}} & -\sin {{\theta }_{1}} & 0 & 0 \\ \sin {{\theta }_{1}} & \cos {{\theta }_{1}} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]$

e ξ \land 2 θ 2 = [e w \land 2 θ 2 0 (I - e w 2 \land θ 2) (w 2 \times v 2) 1] = ⎡ ⎣ ⎢ ⎢ ⎢ cos θ 2 sin θ 2 00 - sin θ 2 cos θ 2 00 0010 l 1 sin θ 2 l 1 (1 - c o s θ 2) 01 ⎤ ⎦ ⎥ ⎥ ⎥;

${{e}^{{{\overset{\wedge }{\mathop{\xi }}\,}_{2}}{{\theta }_{2}}}}=\left[ \begin{matrix} {{e}^{{{\overset{\wedge }{\mathop{w}}\,}_{2}}{{\theta }_{2}}}} & (I-{{e}^{\overset{\wedge }{\mathop{{{w}_{2}}}}\,{{\theta }_{2}}}})({{w}_{2}}\times {{v}_{2}}) \\ 0 & 1 \\ \end{matrix} \right]=\left[ \begin{matrix} \cos {{\theta }_{2}} & -\sin {{\theta }_{2}} & 0 & {{l}_{1}}\sin {{\theta }_{2}} \\ \sin {{\theta }_{2}} & \cos {{\theta }_{2}} & 0 & {{l}_{1}}(1-cos{{\theta }_{2}}) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right];$

e ξ \land 3 θ 3 = [e w \land 3 θ 3 0 (I - e w 3 \land θ 3) (w 3 \times v 3) 1] = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ cos θ 3 sin θ 3 00 - sin θ 3 cos θ 3 00 0010 (l 1 + l 2) sin θ 3 (l 1 + l 2) (1 - c o s θ 3) 01 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥;

${{e}^{{{\overset{\wedge }{\mathop{\xi }}\,}_{3}}{{\theta }_{3}}}}=\left[ \begin{matrix} {{e}^{{{\overset{\wedge }{\mathop{w}}\,}_{3}}{{\theta }_{3}}}} & (I-{{e}^{\overset{\wedge }{\mathop{{{w}_{3}}}}\,{{\theta }_{3}}}})({{w}_{3}}\times {{v}_{3}}) \\ 0 & 1 \\ \end{matrix} \right]=\left[ \begin{matrix} \cos {{\theta }_{3}} & -\sin {{\theta }_{3}} & 0 & ({{l}_{1}}+{{l}_{2}})\sin {{\theta }_{3}} \\ \sin {{\theta }_{3}} & \cos {{\theta }_{3}} & 0 & ({{l}_{1}}+{{l}_{2}})(1-cos{{\theta }_{3}}) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right];$

e ξ \land 4 θ 4 = [e w \land 4 θ 4 0 (I - e w 4 \land θ 4) (w 4 \times v 4) 1] = ⎡ ⎣ ⎢ ⎢ ⎢ 100001000010 00 θ 4 1 ⎤ ⎦ ⎥ ⎥ ⎥;

${{e}^{{{\overset{\wedge }{\mathop{\xi }}\,}_{4}}{{\theta }_{4}}}}=\left[ \begin{matrix} {{e}^{{{\overset{\wedge }{\mathop{w}}\,}_{4}}{{\theta }_{4}}}} & (I-{{e}^{\overset{\wedge }{\mathop{{{w}_{4}}}}\,{{\theta }_{4}}}})({{w}_{4}}\times {{v}_{4}}) \\ 0 & 1 \\ \end{matrix} \right]=\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & {{\theta }_{4}} \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right];$
step9:
根据指数积公式可求得机械臂的正运动学模型：

g s t (θ) = e ξ 1 \land θ 1 e ξ 2 \land θ 2 e ξ 3 \land θ 3 e ξ 4 \land θ 4 g s t (0) = [R (θ) 0 p (θ) 1]

${{g}_{st}}\left( \theta \right)={{e}^{\overset{\wedge }{\mathop{{{\xi }_{1}}}}\,{{\theta }_{1}}}}{{e}^{\overset{\wedge }{\mathop{{{\xi }_{2}}}}\,{{\theta }_{2}}}}{{e}^{\overset{\wedge }{\mathop{{{\xi }_{3}}}}\,{{\theta }_{3}}}}{{e}^{\overset{\wedge }{\mathop{{{\xi }_{4}}}}\,{{\theta }_{4}}}}{{g}_{st}}\left( 0 \right)=\left[ \begin{matrix} R\left( \theta \right) & p\left( \theta \right) \\ 0 & 1 \\ \end{matrix} \right]$

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

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

$R\left( \theta \right)=\left[ \begin{matrix} \cos \left( {{\theta }_{1}}+{{\theta }_{2}}+{{\theta }_{3}} \right) & -\sin \left( {{\theta }_{1}}+{{\theta }_{2}}+{{\theta }_{3}} \right) & 0 \\ \sin ({{\theta }_{1}}+{{\theta }_{2}}+{{\theta }_{3}}) & \cos ({{\theta }_{1}}+{{\theta }_{2}}+{{\theta }_{3}}) & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right]$
机械臂末端的位置向量：

p (θ) = ⎡ ⎣ ⎢ - l 1 sin θ 1 - l 2 sin (θ 1 + θ 2) l 1 cos θ 1 + l 2 cos (θ 1 + θ 2) l 0 + θ 4 ⎤ ⎦ ⎥

$p\left( \theta \right)=\left[ \begin{matrix} -{{l}_{1}}\sin {{\theta }_{1}}-{{l}_{2}}\sin ({{\theta }_{1}}+{{\theta }_{2}}) \\ {{l}_{1}}\cos {{\theta }_{1}}+{{l}_{2}}\cos ({{\theta }_{1}}+{{\theta }_{2}}) \\ {{l}_{0}}+{{\theta }_{4}} \\ \end{matrix} \right]$

二、拟人机械臂

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

g s t (0) = ⎡ ⎣ ⎢ ⎢ ⎢ I 0 ⎛ ⎝ ⎜ 0 l 1 + l 2 l ⎞ ⎠ ⎟ 1 ⎤ ⎦ ⎥ ⎥ ⎥;

${{g}_{st}}\left( 0 \right)=\left[ \begin{matrix} I & \left( \begin{matrix} 0 \\ {{l}_{1}}+{{l}_{2}} \\ l \\ \end{matrix} \right) \\ 0 & 1 \\ \end{matrix} \right];$
step2-step4：
根据建立的坐标系系统来确定每个关节的轴的向量

wi ${w}_{i}$ 和每个坐标系在基座标系中的位置

pi ${p}_{i}$ 。
step5：
6个旋转关节的旋量坐标

ξ 1 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ - ⎛ ⎝ ⎜ 001 ⎞ ⎠ ⎟ \times ⎛ ⎝ ⎜ 00 l 0 ⎞ ⎠ ⎟ ⎛ ⎝ ⎜ 001 ⎞ ⎠ ⎟ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 000001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥;

${{\xi }_{1}}=\left[ \begin{matrix} -\left( \begin{matrix} 0 \\ 0 \\ 1 \\ \end{matrix} \right)\times \left( \begin{matrix} 0 \\ 0 \\ {{l}_{0}} \\ \end{matrix} \right) \\ \left( \begin{matrix} 0 \\ 0 \\ 1 \\ \end{matrix} \right) \\ \end{matrix} \right]=\left[ \begin{matrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 1 \\ \end{matrix} \right];$

ξ 2 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ - ⎛ ⎝ ⎜ - 1 01 ⎞ ⎠ ⎟ \times ⎛ ⎝ ⎜ 00 l 0 ⎞ ⎠ ⎟ ⎛ ⎝ ⎜ - 1 00 ⎞ ⎠ ⎟ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 0 - l 0 0 - 1 00 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥;

${{\xi }_{2}}=\left[ \begin{matrix} -\left( \begin{matrix} -1 \\ 0 \\ 1 \\ \end{matrix} \right)\times \left( \begin{matrix} 0 \\ 0 \\ {{l}_{0}} \\ \end{matrix} \right) \\ \left( \begin{matrix} -1 \\ 0 \\ 0 \\ \end{matrix} \right) \\ \end{matrix} \right]=\left[ \begin{matrix} 0 \\ -{{l}_{0}} \\ 0 \\ -1 \\ 0 \\ 0 \\ \end{matrix} \right];$

ξ 3 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 0 - l 0 l 1 - 1 00 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ξ 4 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ l 1 + l 2 00001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ξ 5 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 0 - l 0 l 1 + l 2 - 1 00 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ξ 6 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ - l 0 00010 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

${{\xi }_{3}}=\left[ \begin{matrix} 0 \\ -{{l}_{0}} \\ {{l}_{1}} \\ -1 \\ 0 \\ 0 \\ \end{matrix} \right]{{\xi }_{4}}=\left[ \begin{matrix} {{l}_{1}}+{{l}_{2}} \\ 0 \\ 0 \\ 0 \\ 0 \\ 1 \\ \end{matrix} \right]{{\xi }_{5}}=\left[ \begin{matrix} 0 \\ -{{l}_{0}} \\ {{l}_{1}}+{{l}_{2}} \\ -1 \\ 0 \\ 0 \\ \end{matrix} \right]{{\xi }_{6}}=\left[ \begin{matrix} -{{l}_{0}} \\ 0 \\ 0 \\ 0 \\ 1 \\ 0 \\ \end{matrix} \right]$
step6:
该机械臂的构型中不含有移动关节，故省略。
step7:
将step5中的运动旋量坐标转换为矩阵形式。
step8:
根据指数积公式可求得机械臂的正运动学模型：

g s t (θ) = e ξ 1 \land θ 1 e ξ 2 \land θ 2 \dots e ξ 6 \land θ 6 g s t (0) = [R (θ) 0 p (θ) 1]

${{g}_{st}}\left( \theta \right)={{e}^{\overset{\wedge }{\mathop{{{\xi }_{1}}}}\,{{\theta }_{1}}}}{{e}^{\overset{\wedge }{\mathop{{{\xi }_{2}}}}\,{{\theta }_{2}}}}\cdots {{e}^{\overset{\wedge }{\mathop{{{\xi }_{6}}}}\,{{\theta }_{6}}}}{{g}_{st}}\left( 0 \right)=\left[ \begin{matrix} R\left( \theta \right) & p\left( \theta \right) \\ 0 & 1 \\ \end{matrix} \right]$

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

R (θ) = ⎡ ⎣ ⎢ r 11 r 21 r 31 r 12 r 22 r 32 r 13 r 23 r 33 ⎤ ⎦ ⎥

$R\left( \theta \right)=\left[ \begin{matrix} {{r}_{11}} & {{r}_{12}} & {{r}_{13}} \\ {{r}_{21}} & {{r}_{22}} & {{r}_{23}} \\ {{r}_{31}} & {{r}_{32}} & {{r}_{33}} \\ \end{matrix} \right]$

p (θ) = ⎡ ⎣ ⎢ - sin θ 1 (l 1 cos θ 2 + l 2 cos (θ 2 + θ 3)) cos θ 1 (l 1 cos θ 2 + l 2 cos (θ 2 + θ 3)) l 0 - l 1 sin θ 2 - l 2 sin (θ 2 + θ 3) ⎤ ⎦ ⎥

$p\left( \theta \right)=\left[ \begin{matrix} -\sin {{\theta }_{1}}\left( {{l}_{1}}\cos {{\theta }_{2}}+{{l}_{2}}\cos \left( {{\theta }_{2}}+{{\theta }_{3}} \right) \right) \\ \cos {{\theta }_{1}}\left( {{l}_{1}}\cos {{\theta }_{2}}+{{l}_{2}}\cos \left( {{\theta }_{2}}+{{\theta }_{3}} \right) \right) \\ {{l}_{0}}-{{l}_{1}}\sin {{\theta }_{2}}-{{l}_{2}}\sin ({{\theta }_{2}}+{{\theta }_{3}}) \\ \end{matrix} \right]$
其中：
这里写图片描述

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