空间描述和变换

时间 2021-01-17 标签机器人空间描述齐次变换旋转矩阵刚体变换欧拉角

空间描述和变换

文章目录

空间描述和变换

1.1位置描述

在坐标系{A}中，空间中一个点的位置用一个3×1的矢量描述
A P = [ p x p y p z ] ( 1 − 1 ) ^{A} \mathbf {P}= \left[ \begin{matrix} {{p}_{x}} \\ {{p}_{y}} \\ {{p}_{z}} \\ \end{matrix} \right] \ \ \ \ \ \ \ {(1-1)} AP=⎣⎡pxpypz⎦⎤ (1−1)

1.2 姿态描述

点的位置可以用矢量描述，物体的姿态可以用固定在其上的坐标系描述。描述坐标系{B}的一种方法是用{A}坐标系的三个主轴单位矢量表示。

用 X ^ B , Y ^ B , Z ^ B {{\widehat{\mathbf{X}}}_{B}},{{\widehat{\mathbf{Y}}}_{B}},{{\widehat{\mathbf{Z}}}_{B}} X B,Y B,Z B来表示{B}主轴方向的单位矢量，当用坐标系{A}的坐标来表达时，被写成 A X ^ B , A Y ^ B , A Z ^ B {}^{A}{{\widehat{\mathbf{X}}}_{B}},{}^{A}{{\widehat{\mathbf{Y}}}_{B}},{}^{A}{{\widehat{\mathbf{Z}}}_{B}} AX B,AY B,AZ B，{B}相对于{A}的表达为
B A R = [ A X ^ B A Y ^ B A Z ^ B ] = [ r 11 r 12 r 12 r 21 r 22 r 23 r 31 r 32 r 33 ] ( 1 − 2 ) {}_{B}^{A}\mathbf {R}=\left[ \begin{matrix} {}^{A}{{\widehat{\mathbf{X}}}_{B}} & {}^{A}{{\widehat{\mathbf{Y}}}_{B}} & {}^{A}{{\widehat{\mathbf{Z}}}_{B}} \\ \end{matrix} \right]=\left[ \begin{matrix} {{r}_{11}} & {{r}_{12}} & {{r}_{12}} \\ {{r}_{21}} & {{r}_{22}} & {{r}_{23}} \\ {{r}_{31}} & {{r}_{32}} & {{r}_{33}} \\ \end{matrix} \right] \ \ \ \ \ \ \ {(1-2)} BAR=[AX BAY BAZ B]=⎣⎡r11r21r31r12r22r32r12r23r33⎦⎤ (1−2)
标量 r i j r_{ij} rij可用每个矢量在其参考坐标系中单位方向上的投影来表示，投影可通过点积得到，因此
B A R = [ A X ^ B A Y ^ B A Z ^ B ] = [ X ^ B ⋅ X A Y ^ B ⋅ X A Z ^ B ⋅ X A X ^ B ⋅ Y A Y ^ B ⋅ Y A Z ^ B ⋅ Y A X ^ B ⋅ Z A Y ^ B ⋅ Z A Z ^ B ⋅ Z A ] ( 1 − 3 ) {}_{B}^{A}\mathbf{R}=\left[ \begin{matrix} {}^{A}{{\widehat{\mathbf{X}}}_{B}} & {}^{A}{{\widehat{\mathbf{Y}}}_{B}} & {}^{A}{{\widehat{\mathbf{Z}}}_{B}} \\ \end{matrix} \right]=\left[ \begin{matrix} {{\widehat{\mathbf{X}}}_{B}}\cdot {{\mathbf{X}}_{A}} & {{\widehat{\mathbf{Y}}}_{B}}\cdot {{\mathbf{X}}_{A}} & {{\widehat{\mathbf{Z}}}_{B}}\cdot {{\mathbf{X}}_{A}} \\ {{\widehat{\mathbf{X}}}_{B}}\cdot {{\mathbf{Y}}_{A}} & {{\widehat{\mathbf{Y}}}_{B}}\cdot {{\mathbf{Y}}_{A}} & {{\widehat{\mathbf{Z}}}_{B}}\cdot {{\mathbf{Y}}_{A}} \\ {{\widehat{\mathbf{X}}}_{B}}\cdot {{\mathbf{Z}}_{A}} & {{\widehat{\mathbf{Y}}}_{B}}\cdot {{\mathbf{Z}}_{A}} & {{\widehat{\mathbf{Z}}}_{B}}\cdot {{\mathbf{Z}}_{A}} \\ \end{matrix} \right] \ \ \ \ \ \ \ {(1-3)} BAR=[AX BAY BAZ B]=⎣⎡X B⋅XAX B⋅YAX B⋅ZAY B⋅XAY B⋅YAY B⋅ZAZ B⋅XAZ B⋅YAZ B⋅ZA⎦⎤ (1−3)
上式中矢量的右上标被省略了。实际上，只要点积的对矢量是在同一坐标系中描述，那么坐标系的选择是任意的。观察上式，也可发现矩阵中行向量是单位矢量{A}在{B}中的表达。即
B A R = [ A X ^ B , A Y ^ B , A Z ^ B ] = [ B X T A B X T A B Y T A B Y T A ] (1-4) _B^AR=\left[ ^A\widehat{X}_B ,^A\widehat{Y}_B, ^A\widehat{Z}_B \right ] =\left [ ^BX^A_T \\ ^BX^A_T\\ ^BY^A_T\\ ^BY^A_T \right ] \tag{1-4} BAR=[AX B,AY B,AZ B]=[BXTABXTABYTABYTA](1-4)

因此，坐标系{A}相对于{B}的描述，可由 B A R _B^A\mathbf{R} BAR转置得到，即
A B R = B A R T (1-5) ^B_AR=^A_BR^T\tag{1-5} ABR=BART(1-5)
这表明旋转矩阵的逆可由它的转置得到，实际上旋转矩阵也是正交矩阵。

1.3 坐标系的描述

在机器人学中，位置和姿态经常成对出现，我们将此组合称作坐标系。因此一个坐标系可等价的用一个位置矢量和一个旋转矩阵来描述，例如
{ B } = [ B A R , A P ] ( 1 − 6 ) \{\rm{B}\}=\left[\begin{matrix} _B^A\mathbf{R},^A\mathbf{P} \end{matrix}\right] \ \ \ \ (1-6) {B}=[BAR,AP] (1−6)

1.4坐标系的一般映射：齐次矩阵变换

齐次变换矩阵有三方面的含义：

（1）它是坐标系的描述。 B A T _B^A\mathbf{T} BAT表示相对于坐标系{A}的坐标系{B}。特别是， B A R _B^A\mathbf{R} BAR 的各列是坐标系{B}主轴方向的单位矢量。确定了{B}的原点。

（2）它是变换映射。 B A T _B^A\mathbf{T} BAT是映射 B P → A P {}^{B}\mathbf{P}\to{}^{A}\mathbf{P} BP→AP。

（3）它是变换算子。 T \mathbf{T} T将 A P 1 ^A\mathbf{P}_1 AP1变换为 A P 2 ^A\mathbf{P}_2 AP2 。

其次变换矩阵是旋转和平移的组合，具体可写成
[ A P 1 ] = [ B A R A P B O R G 0 , 0 , 0 1 ] ( 1 − 7 ) \left[ \begin{matrix} {}^{A}\mathbf{P} \\ 1 \\ \end{matrix} \right]=\left[ \begin{matrix} {}_{B}^{A}\mathbf{R} & {}^{A}{{\mathbf{P}}_{BORG}} \\ 0,0,0 & 1 \\ \end{matrix} \right] \ \ \ \ (1-7) [AP1]=[BAR0,0,0APBORG1] (1−7)
由此可见，坐标系和变换都可以通过位置矢量和姿态来描述。一般坐标系主要用于描述，而变换用来表示映射或算子。

1.5 变换算法

1.5.1混合变换

A P = B A T C B T C P ( 1 − 8 ) {}^A{\bf{P}} = {}_B^A{\bf{T}}{}_C^B{\bf{T}}{}^C{\bf{P}} \ \ \ \ \ \ (1-8) AP=BATCBTCP (1−8)
由此定义
C A T = B A T C B T ( 1 − 9 ) {}_C^A{\bf{T}} = {}_B^A{\bf{T}}{}_C^B{\bf{T}} \ \ \ \ \ \ (1-9) CAT=BATCBT (1−9)
由变换的定义，可得

C A T = [ B A R C B R B A R B P C O R G + A P B O R G 0 , 0 , 0 1 ] _C^AT =\left[ \begin{matrix} _B^AR_C^BR&_B^AR^BP_{CORG} + ^AP_{BORG}\\ 0,0,0 &1 \end{matrix} \right ] CAT=[BARCBR0,0,0BARBPCORG+APBORG1]

1.5.2 逆变换

通过一般的方法求矩阵的逆可以得到逆变换，但是结合旋转矩阵的性质，可以更简便得到逆变换，编程时分开计算可降低维数，减少求逆的计算时间。
A B T = B A T − 1 = [ B A R T − B A R T A P B O R G 0 , 0 , 0 1 ] ( 1 − 11 ) {}_A^B{\bf{T}} = {}_B^A{{\bf{T}}^{ - 1}} = \left[ \begin{matrix} {{}_B^A{{\bf{R}}^T}}&{ - {}_B^A{{\bf{R}}^T}{}^A{{\bf{P}}_{BORG}}}\\ {0,0,0}&1 \end{matrix} \right] \ \ \ \ (1-11) ABT=BAT−1=[BART0,0,0−BARTAPBORG1] (1−11)

1.6变换方程

坐标系{D}可以通过两个不同的变换得到
D U T = A U T D A T ( 1 − 12 ) D U T = B U T C B T D C T ( 1 − 13 ) {}_D^U{\bf{T}} = {}A^U{\bf{T}}{}_D^A{\bf{T}} \ \ \ \ \ \ (1-12)\\ {}_D^U{\bf{T}} = {}_B^U{\bf{T}}{}_C^B{\bf{T}}{}_D^C{\bf{T}} \ \ \ \ (1-13) DUT=AUTDAT (1−12)DUT=BUTCBTDCT (1−13)
两个表达式构成一个方程
A U T D A T = B U T C B T D C T ( 1 − 14 ) {}_A^U{\bf{T}}{}_D^A{\bf{T}} = {}_B^U{\bf{T}}{}_C^B{\bf{T}}{}_D^C{\bf{T}} \ \ \ \ \ (1-14) AUTDAT=BUTCBTDCT (1−14)
如果有n个未知变换和n个变换方程，这个变换可以从变换方程解出。

1.7 姿态的其他描述方法

1.7.1固定角坐标系

首先将坐标系{B}和一个已知的参考坐标系{A}重合，先将{B}绕 X ^ A \widehat{X}_{A} X A轴旋转 γ \gamma γ角，再绕 Y ^ A {\widehat{Y}}_{A} Y A轴旋转 β \beta β，最后绕 Z ^ A {\widehat{Z}}_{A} Z A轴旋转 α \alpha α角。每次旋转都是绕着固定参考坐标系{A}的轴。

把每个旋转看做算子，等价旋转矩阵为
B A R X Y Z ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( α ) = [ c α − s α 0 s α c α 0 0 0 1 ] [ c β 0 s β 0 1 0 − s β 0 c β ] [ 1 0 0 0 c γ − s γ 0 s γ c γ ] ( 1 − 15 ) {}_B^A{{\bf{R}}_{XYZ}}(\gamma ,\beta ,\alpha ) = {{\bf{R}}_Z}(\alpha ){{\bf{R}}_Y}(\beta ){{\bf{R}}_X}(\alpha )\\ = \left[ \begin{matrix} {c\alpha }&{ - s\alpha }&0\\ {s\alpha }&{c\alpha }&0\\ 0&0&1 \end{matrix}\right] \left[\begin{matrix} {c\beta }&0&{s\beta }\\ 0&1&0\\ { - s\beta }&0&{c\beta } \end{matrix} \right] \left[ \begin{matrix} 1&0&0\\ 0&{c\gamma }&{ - s\gamma }\\ 0&{s\gamma }&{c\gamma } \end{matrix} \right] \ \ (1-15) BARXYZ(γ,β,α)=RZ(α)RY(β)RX(α)=⎣⎡cαsα0−sαcα0001⎦⎤⎣⎡cβ0−sβ010sβ0cβ⎦⎤⎣⎡1000cγsγ0−sγcγ⎦⎤ (1−15)
计算得到
B A R X Y Z ( γ , β , α ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α c β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] ( 1 − 16 ) {}_B^A{{\bf{R}}_{XYZ}}(\gamma ,\beta ,\alpha ) = \left[ \begin{matrix} {c\alpha c\beta }&{c\alpha s\beta s\gamma - s\alpha c\gamma }&{c\alpha s\beta c\gamma + s\alpha s\gamma }\\ {s\alpha c\beta }&{s\alpha s\beta s\gamma + c\alpha c\gamma }&{s\alpha s\beta c\gamma - c\alpha s\gamma }\\ { - s\beta }&{c\beta s\gamma }&{c\beta c\gamma } \end{matrix}\right] \ \ (1-16) BARXYZ(γ,β,α)=⎣⎡cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤ (1−16)

1.7.2 X-Y-Z欧拉角

坐标系{B}的描述：

首先将坐标系{B}与一个已知坐标系{A}重合，先将{B}绕 Z ^ B {\widehat{Z}}_{B} Z B旋转 α \alpha α角，再将{B}绕 Y ^ B {\widehat{Y}}_{B} Y B旋转 β \beta β角，最后将{B}绕 X ^ B {\widehat{X}}_{B} X B旋转 γ \gamma γ角。每次都是绕运行坐标系{B}的各轴旋转。

把每个旋转看成坐标系的描述，可立即写出
B A R = B ′ A R B ′ ′ B ′ R B B ′ ′ R (1-17) {}_B^A{\bf{R}} = {}_{B'}^A{\bf{R}}{}_{B''}^{B'}{\bf{R}}{}_B^{B''}{\bf{R}} \tag{1-17} BAR=B′ARB′′B′RBB′′R(1-17)
即{B}相对于{A}的最终姿态为

B A R X ′ Y ′ Z ′ ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( α ) = [ c α − s α 0 s α c α 0 0 0 1 ] [ c β 0 s β 0 1 0 − s β 0 c β ] [ 1 0 0 0 c γ − s γ 0 s γ c γ ] (1-18) {}_B^A{{\bf{R}}_{X'Y'Z'}}(\gamma ,\beta ,\alpha ) = {{\bf{R}}_Z}(\alpha ){{\bf{R}}_Y}(\beta ){{\bf{R}}_X}(\alpha )\\ = \left[ \begin{matrix} {c\alpha }&{ - s\alpha }&0\\ {s\alpha }&{c\alpha }&0\\ 0&0&1 \end{matrix}\right] \left[\begin{matrix} {c\beta }&0&{s\beta }\\ 0&1&0\\ { - s\beta }&0&{c\beta } \end{matrix}\right ] \left[\begin{matrix} 1&0&0\\ 0&{c\gamma }&{ - s\gamma }\\ 0&{s\gamma }&{c\gamma } \end{matrix} \right] \tag{1-18} BARX′Y′Z′(γ,β,α)=RZ(α)RY(β)RX(α)=⎣⎡cαsα0−sαcα0001⎦⎤⎣⎡cβ0−sβ010sβ0cβ⎦⎤⎣⎡1000cγsγ0−sγcγ⎦⎤(1-18)
计算得到
B A R X ′ Y ′ Z ′ ( γ , β , α ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α c β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] (1-19) {}_B^A{{\bf{R}}_{X'Y'Z'}}(\gamma ,\beta ,\alpha ) = \left[ \begin{matrix} {c\alpha c\beta }&{c\alpha s\beta s\gamma - s\alpha c\gamma }&{c\alpha s\beta c\gamma + s\alpha s\gamma }\\ {s\alpha c\beta }&{s\alpha s\beta s\gamma + c\alpha c\gamma }&{s\alpha s\beta c\gamma - c\alpha s\gamma }\\ { - s\beta }&{c\beta s\gamma }&{c\beta c\gamma } \end{matrix}\right] \tag{1-19} BARX′Y′Z′(γ,β,α)=⎣⎡cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤(1-19)
带有“撇”号表示是由X-Y-Z欧拉角参数化的旋转矩阵。发现和X-Y-Z固定轴旋转矩阵是完全相同的。这两种变换的矩阵一样，因此求解欧拉角的方法一样。由旋转矩阵求欧拉角，设
B A R X Y Z ( γ , β , α ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α c β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] ( 1 − 20 ) {}_B^A{{\bf{R}}_{XYZ}}(\gamma ,\beta ,\alpha ) = \left[\begin{matrix} {c\alpha c\beta }&{c\alpha s\beta s\gamma - s\alpha c\gamma }&{c\alpha s\beta c\gamma + s\alpha s\gamma }\\ {s\alpha c\beta }&{s\alpha s\beta s\gamma + c\alpha c\gamma }&{s\alpha s\beta c\gamma - c\alpha s\gamma }\\ { - s\beta }&{c\beta s\gamma }&{c\beta c\gamma } \end{matrix} \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] \ \ (1-20) BARXYZ(γ,β,α)=⎣⎡cαcβsαcβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤ (1−20)
当 c β ≠ 0 ( β ≠ ± 90 ∘ ) c\beta \ne 0(\beta \ne \pm 90{}^\circ ) cβ=0(β=±90∘)时
β = A tan ⁡ 2 ( − r 31 , r 11 2 + r 21 2 ) α = A t a n 2 ( r 21 / c β , r 11 / c β ) γ = A t a n 2 ( r 32 / c β , r 33 / c β ) (1-21) \begin{array}{l} \beta = A\tan 2( - {r_{31}},\sqrt {r_{11}^2 + r_{21}^2} )\\ \alpha = Atan2({r_{21}}/c\beta ,{r_{11}}/c\beta )\\ \gamma = Atan2({r_{32}}/c\beta ,{r_{33}}/c\beta ) \end{array} \tag{1-21} β=Atan2(−r31,r112+r212 )α=Atan2(r21/cβ,r11/cβ)γ=Atan2(r32/cβ,r33/cβ)(1-21)
当 c β = 0 ( β = ± 90 ∘ ) c\beta =0(\beta =\pm 90{}^\circ ) cβ=0(β=±90∘)时，一般取 α = 0 \alpha =0 α=0，结果如下

若$\beta =90{}^\circ $，
β = 9 0 ∘ α = 0 ∘ γ = A tan ⁡ 2 ( r 12 , r 22 ) ( 1-22) \begin{array}{l} \beta = 90^\circ \\ \alpha = 0^\circ \\ \gamma = A\tan 2({r_{12}},{r_{22}}) \end{array} \tag{ 1-22} β=90∘α=0∘γ=Atan2(r12,r22)( 1-22)
若$\beta =-90{}^\circ $，
β = − 9 0 ∘ α = 0 ∘ γ = − A tan ⁡ 2 ( r 12 , r 22 ) (1-23) \begin{array}{l} \beta = - 90^\circ \\ \alpha = 0^\circ \\ \gamma = - A\tan 2({r_{12}},{r_{22}}) \end{array} \tag{1-23} β=−90∘α=0∘γ=−Atan2(r12,r22)(1-23)

1.7.3 Z-Y-Z欧拉角

坐标系{B}的描述：

首先将坐标系{B}与一个已知坐标系{A}重合，先将{B}绕 Z ^ B {\widehat{Z}}_{B} Z B旋转 α \alpha α角，再将{B}绕 Y ^ B {\widehat{Y}}_{B} Y B旋转 β \beta β角，最后将{B}绕 Z ^ B {\widehat{Z}}_{B} Z B旋转 γ \gamma γ角。每次都是绕运行坐标系{B}的各轴旋转。

等效旋转矩阵为
B A R Z ′ Y ′ Z ′ ( α , β , γ ) = [ c α s β c γ − s α s γ − c α c β s γ − s α c γ c α s β s α c β c γ + c α c β s γ − s α c β s γ + c α c γ s α s β − s β c γ s β s γ c β ] (1-24) {}_B^A{R_{Z'Y'Z'}}(\alpha ,\beta ,\gamma ) = \left[ \begin{matrix} {c\alpha s\beta c\gamma - s\alpha s\gamma }&{ - c\alpha c\beta s\gamma - s\alpha c\gamma }&{c\alpha s\beta }\\ {s\alpha c\beta c\gamma + c\alpha c\beta s\gamma }&{ - s\alpha c\beta s\gamma + c\alpha c\gamma }&{s\alpha s\beta }\\ { - s\beta c\gamma }&{s\beta s\gamma }&{c\beta } \end{matrix} \right] \tag{1-24} BARZ′Y′Z′(α,β,γ)=⎣⎡cαsβcγ−sαsγsαcβcγ+cαcβsγ−sβcγ−cαcβsγ−sαcγ−sαcβsγ+cαcγsβsγcαsβsαsβcβ⎦⎤(1-24)

1.7.4 等效坐标轴表示法

坐标系{B}的描述：

首先将坐标系{B}与一个已知的参考坐标系{A}重合，将{B}绕矢量 A K ^ {}^{A}\widehat{\mathbf{K}} AK 按右手定则旋转$\theta $角。

则{B}相对于{A}的姿态可用 B A R ( K ^ , θ ) {}_{B}^{A}\mathbf{R}(\widehat{\mathbf{K}},\theta) BAR(K ,θ)或 B A R K ( θ ) {}_{B}^{A}{{\mathbf{R}}_{\mathbf{K}}}(\theta) BARK(θ)表示，称作等效轴角坐标表示法。

1.7.5 欧拉参数

另一种姿态表示法通过四个数值来表示，称为欧拉参数法。根据等效旋转轴和等效旋转角，可以得到欧拉参数如下
ε 1 = k x sin ⁡ θ 2 ε 2 = k y sin ⁡ θ 2 ε 3 = k z sin ⁡ θ 2 ε 4 = cos ⁡ θ 2 (1-25) {\varepsilon _1} = {k_x}\sin \frac{\theta }{2}\\ {\varepsilon _2} = {k_y}\sin \frac{\theta }{2}\\ {\varepsilon _3} = {k_z}\sin \frac{\theta }{2}\\ {\varepsilon _4} = \cos \frac{\theta }{2} \tag{1-25} ε1=kxsin2θε2=kysin2θε3=kzsin2θε4=cos2θ(1-25)
这四个参数不是相互独立的
ε 1 + ε 2 + ε 3 + ε 4 = 1 (1-26) {\varepsilon _1} + {\varepsilon _2} + {\varepsilon _3} + {\varepsilon _4} = 1 \tag{1-26} ε1+ε2+ε3+ε4=1(1-26)
使用欧拉参数表示的旋转矩阵 R e {{\mathbf{R}}_{e}} Re为

已知一个矩阵，得到对应的欧拉参数为
ε 1 = r 32 − r 23 4 ε 4 ε 2 = r 13 − r 31 4 ε 4 ε 3 = r 21 − r 12 4 ε 4 ε 4 = 1 2 1 + r 11 + r 22 + r 33 (1-28) {\varepsilon _1} = \frac{{{r_{32}} - {r_{23}}}}{{4{\varepsilon _4}}}\\ {\varepsilon _2} = \frac{{{r_{13}} - {r_{31}}}}{{4{\varepsilon _4}}}\\ {\varepsilon _3} = \frac{{{r_{21}} - {r_{12}}}}{{4{\varepsilon _4}}}\\ {\varepsilon _4} = \frac{1}{2}\sqrt {1 + {r_{11}} + {r_{22}} + {r_{33}}} \tag{1-28} ε1=4ε4r32−r23ε2=4ε4r13−r31ε3=4ε4r21−r12ε4=211+r11+r22+r33 (1-28)

参考文献

[1] JOHN J.CRAIG. 机器人学导论: 第3版[M]. 机械工业出版社, 2006.