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

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

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

一、SCARA机械臂

step1:
求解 表示θ=0 <script type="math/tex" id="MathJax-Element-1">表示\theta =0</script>的时候工具坐标在基坐标中的位形的齐次变换矩阵。

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

<script type="math/tex; mode=display" id="MathJax-Element-2">{{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]</script>
step2:
设关节1，2，3的角速度的方向为沿z轴，所以 w1=w2=w3 <script type="math/tex" id="MathJax-Element-3">{{w}_{1}}={{w}_{2}}={{w}_{3}}</script>= [0 0 1 ]T <script type="math/tex" id="MathJax-Element-4">\left[ \begin{matrix} 0 \ 0 \ 1 \ \end{matrix} \right]^{T}</script>;
step3:
将 wi <script type="math/tex" id="MathJax-Element-5">{w_{i}}</script>写成反对称矩阵的形式：

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

<script type="math/tex; mode=display" id="MathJax-Element-6">{{\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]</script>
step4:
相应的坐标系原点在基座标系中的位置：

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

<script type="math/tex; mode=display" id="MathJax-Element-7">{{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];</script>
step5:
旋转关节1,2,3运动旋量坐标：

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

<script type="math/tex; mode=display" id="MathJax-Element-8">{{\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];</script>
step6:
移动关节4的运动旋量坐标：

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

<script type="math/tex; mode=display" id="MathJax-Element-9">{{\xi }_{4}}=\left[ \begin{matrix} {{v}_{4}} \\ 0 \\ \end{matrix} \right]=\left[ \begin{matrix} 0 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0 \\ \end{matrix} \right];</script>
step7:
求解各个关节运动的刚体运动的李代数的矩阵形式：

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

<script type="math/tex; mode=display" id="MathJax-Element-10">\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];</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-11">\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];</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-12">\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];</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-13">\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];</script>
step8:
求解各个关节的指数映射( se(3)→SE(3) <script type="math/tex" id="MathJax-Element-14">se\left( 3 \right)\to SE\left( 3 \right)</script>)：

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

<script type="math/tex; mode=display" id="MathJax-Element-15">{{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]</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-16">{{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];</script>

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⎤⎦⎥⎥⎥⎥;

<script type="math/tex; mode=display" id="MathJax-Element-17">{{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];</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-18">{{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];</script>
step9:
根据指数积公式可求得机械臂的正运动学模型：

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

<script type="math/tex; mode=display" id="MathJax-Element-19">{{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]</script>
⇒ <script type="math/tex" id="MathJax-Element-20">\Rightarrow </script>机械臂末端的姿态矩阵：

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

<script type="math/tex; mode=display" id="MathJax-Element-21">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]</script>
机械臂末端的位置向量：

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

<script type="math/tex; mode=display" id="MathJax-Element-22">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]</script>

二、拟人机械臂

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

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

<script type="math/tex; mode=display" id="MathJax-Element-23">{{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];</script>
step2-step4：
根据建立的坐标系系统来确定每个关节的轴的向量 wi <script type="math/tex" id="MathJax-Element-24">{w}_{i}</script>和每个坐标系在基座标系中的位置 pi <script type="math/tex" id="MathJax-Element-25">{p}_{i}</script>。
step5：
6个旋转关节的旋量坐标

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

<script type="math/tex; mode=display" id="MathJax-Element-26">{{\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];</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-27">{{\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];</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-28">{{\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]</script>
step6:
该机械臂的构型中不含有移动关节，故省略。
step7:
将step5中的运动旋量坐标转换为矩阵形式。
step8:
根据指数积公式可求得机械臂的正运动学模型：

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

<script type="math/tex; mode=display" id="MathJax-Element-29">{{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]</script>
⇒ <script type="math/tex" id="MathJax-Element-30">\Rightarrow</script>
机械臂末端的姿态矩阵和位置向量：

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

<script type="math/tex; mode=display" id="MathJax-Element-31">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]</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-32">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]</script>
其中：

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