空间描述和变换

傷城~ 2022-12-08 12:55 226阅读 0赞

# 空间描述和变换 #

### 文章目录 ###

*  空间描述和变换
 *   *  1.1位置描述
     *  1.2 姿态描述
     *  1.3 坐标系的描述
     *  1.4坐标系的一般映射：齐次矩阵变换
     *  1.5 变换算法
     *   *  1.5.1混合变换
         *  1.5.2 逆变换
     *  1.6变换方程
     *  1.7 姿态的其他描述方法
     *   *  1.7.1固定角坐标系
         *  1.7.2 X-Y-Z欧拉角
         *  1.7.3 Z-Y-Z欧拉角
         *  1.7.4 等效坐标轴表示法
         *  1.7.5 欧拉参数
     *  参考文献

## 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 姿态描述 ##

![在这里插入图片描述][2020091911054046.png_pic_center]  
 点的位置可以用矢量描述，物体的姿态可以用固定在其上的坐标系描述。描述坐标系\{B\}的一种方法是用\{A\}坐标系的三个主轴单位矢量表示。

用 X ^ B , Y ^ B , Z ^ B \{ \{\\widehat\{\\mathbf\{X\}\}\}\_\{B\}\},\{ \{\\widehat\{\\mathbf\{Y\}\}\}\_\{B\}\},\{ \{\\widehat\{\\mathbf\{Z\}\}\}\_\{B\}\} XB,YB,ZB来表示\{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\}\} AXB,AYB,AZB，\{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=\[AXBAYBAZB\]=⎣⎡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=\[AXBAYBAZB\]=⎣⎡XB⋅XAXB⋅YAXB⋅ZAYB⋅XAYB⋅YAYB⋅ZAZB⋅XAZB⋅YAZB⋅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=\[AXB,AYB,AZB\]=\[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混合变换 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]

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变换方程 ##

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 1]

坐标系\{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固定角坐标系 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 2]

首先将坐标系\{B\}和一个已知的参考坐标系\{A\}重合，先将\{B\}绕 X ^ A \\widehat\{X\}\_\{A\} XA轴旋转 γ \\gamma γ角，再绕 Y ^ A \{\\widehat\{Y\}\}\_\{A\} YA轴旋转 β \\beta β，最后绕 Z ^ A \{\\widehat\{Z\}\}\_\{A\} ZA轴旋转 α \\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欧拉角 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 3]

坐标系\{B\}的描述：

首先将坐标系\{B\}与一个已知坐标系\{A\}重合，先将\{B\}绕 Z ^ B \{\\widehat\{Z\}\}\_\{B\} ZB旋转 α \\alpha α角，再将\{B\}绕 Y ^ B \{\\widehat\{Y\}\}\_\{B\} YB旋转 β \\beta β角，最后将\{B\}绕 X ^ B \{\\widehat\{X\}\}\_\{B\} XB旋转 γ \\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\} ZB旋转 α \\alpha α角，再将\{B\}绕 Y ^ B \{\\widehat\{Y\}\}\_\{B\} YB旋转 β \\beta β角，最后将\{B\}绕 Z ^ B \{\\widehat\{Z\}\}\_\{B\} ZB旋转 γ \\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为  
![在这里插入图片描述][20200919141659425.PNG_pic_center]  
已知一个矩阵，得到对应的欧拉参数为  
 ε 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.

[2020091911054046.png_pic_center]: /images/20221123/13dee9be41704ea5841d03b7511798cd.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]: /images/20221123/c6a64021043a46ae955e92b90f08df55.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20221123/66b57d93edbe479b92f907c6d1301f70.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 2]: /images/20221123/27801982fa1b4f1cb80bdea388a69620.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center 3]: /images/20221123/014c0bb756b54653a79de63d1f82d96a.png
[20200919141659425.PNG_pic_center]: /images/20221123/ab1dd1217ee84a85aefa798286c1f171.png