第二章 空间描述与变换
1. 描述：位置姿态与位姿
1.1 位置描述
位置矢量描述空间中点的位置
1.2 姿态描述
在物体上固定一个坐标系并且给出此坐标系相对于参考矩阵的描述，借用旋转矩阵 B A R _{B}^{A}R BAR
1.3 位姿描述
位置和姿态成对出现，成为位姿。位姿中一个矢量表示指尖位置，三个矢量表示姿态，等价地用一个位置矢量和一个旋转矩阵描述，用 B A R _{B}^{A}R BAR和 A P B O R G ^{A}P_{BORG} APBORG来描述位姿{ B { {B} } B}，其中是确定位姿{ B { {B} } B}的原点的位置矢量
2. 映射：从一个坐标系到另一个坐标系
2.1 坐标平移
当{A}与{B}的姿态相同时，用矢量相加的方法求点P 相对{A}的表示 A P ^AP AP
A P = B P + A P B O R G (2-1) ^AP=^BP+^A{P}_{BORG} \tag{2-1} AP=BP+APBORG(2-1)
2.2 坐标旋转
2.2.1 旋转矩阵定义
旋转矩阵 B A R _{B}^{A}R BAR的列是{B}的单位矢量在{A}中的描述，旋转矩阵 B A R _{B}^{A}R BAR的行是{A]的单位矢量在{B}中的描述
B A R = [ ∣ ∣ ∣ A X ^ B A Y ^ B A Z ^ B ∣ ∣ ∣ ] B relative “column vector” = [ 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 ] “direct cosines” = [ X ^ A ⋅ X ^ B X ^ A ⋅ Y ^ B X ^ A ⋅ Z ^ B Y ^ A ⋅ X ^ B Y ^ A ⋅ Y ^ B Y ^ A ⋅ Z ^ B Z ^ A ⋅ X ^ B Z ^ A ⋅ Y ^ B Z ^ A ⋅ Z ^ B ] 前后向量互换 = [ − B X ^ A T − − B Y ^ A T − − B Z ^ A T − ] = A B R T = A B R − 1 A relative "row vector" \begin{aligned}_B^AR & =\begin{bmatrix}| & | & | \\ ^A\hat{X}_B & ^A\hat{Y}_B & ^A\hat{Z}_B \\ | & | & |\end{bmatrix}& \\ \text{B relative} & \text{``column vector''} \\&=\begin{bmatrix}\hat{X}_B\cdot\hat{X}_A & \hat{Y}_B\cdot\hat{X}_A & \hat{Z}_B\cdot\hat{X}_A \\ \hat{X}_B\cdot\hat{Y}_A & \hat{Y}_B\cdot\hat{Y}_A & \hat{Z}_B\cdot\hat{Y}_A \\ \hat{X}_B\cdot\hat{Z}_A & \hat{Y}_B\cdot\hat{Z}_A & \hat{Z}_B\cdot\hat{Z}_A \end{bmatrix} \\ & \text{``direct cosines''}\\ &= \begin{bmatrix}\hat{X}_{A}\cdot\hat{X}_{B}&\hat{X}_{A}\cdot\hat{Y}_{B}&\hat{X}_{A}\cdot\hat{Z}_{B}\\\hat{Y}_{A}\cdot\hat{X}_{B}&\hat{Y}_{A}\cdot\hat{Y}_{B}&\hat{Y}_{A}\cdot\hat{Z}_{B}\\\hat{Z}_{A}\cdot\hat{X}_{B}&\hat{Z}_{A}\cdot\hat{Y}_{B}&\hat{Z}_{A}\cdot\hat{Z}_{B}\end{bmatrix}\\ &\text{前后向量互换}\\&=\begin{bmatrix}-&{}^{B}\hat{X}_{A}^{T}&-\\-&{}^{B}\hat{Y}_{A}^{T}&-\\-&{}^{B}\hat{Z}_{A}^{T}&-\end{bmatrix} ={_A^B}R^T={_A^B}R^{-1}& \\ \text{A relative} & \text{ "row vector"} &\end{aligned} BARB relativeA relative= ∣AX^B∣∣AY^B∣∣AZ^B∣ “column vector”= X^B⋅X^AX^B⋅Y^AX^B⋅Z^AY^B⋅X^AY^B⋅Y^AY^B⋅Z^AZ^B⋅X^AZ^B⋅Y^AZ^B⋅Z^A “direct cosines”= X^A⋅X^BY^A⋅X^BZ^A⋅X^BX^A⋅Y^BY^A⋅Y^BZ^A⋅Y^BX^A⋅Z^BY^A⋅Z^BZ^A⋅Z^B 前后向量互换= −−−BX^ATBY^ATBZ^AT−−− =ABRT=ABR−1 "row vector"
其中， A X ^ B ^A\hat{X}_B AX^B表示{ B B B}的 X B X_B XB轴在{ A A A}中的描述， X ^ B ⋅ X ^ A \hat{X}_B\cdot\hat{X}_A X^B⋅X^A表示{ B B B}的 X B X_B XB轴在{ A A A}的 X A X_A XA轴上的投影， X ^ B ⋅ Y ^ A \hat{X}_B\cdot\hat{Y}_A X^B⋅Y^A表示{ B B B}的 X B X_B XB轴在{ A A A}的 Y A Y_A YA轴上的投影， X ^ B ⋅ Z ^ A \hat{X}_B\cdot\hat{Z}_A X^B⋅Z^A表示{ B B B}的 X B X_B XB轴在{ A A A}的 Z A Z_A ZA轴上的投影。
2.2.2 旋转矩阵特性
B A R T B A R = [ ∣ ∣ ∣ A X ^ B A Y ^ B A Z ^ B ∣ ∣ ∣ ] T [ ∣ ∣ ∣ A X ^ B A Y ^ B A Z ^ B ∣ ∣ ∣ ] = [ − A X ^ B T − − A Y ^ B T − − A Z ^ B T − ] [ ∣ ∣ ∣ A X ^ B A Y ^ B A Z ^ B ∣ ∣ ∣ ] = I 3 = B A R − 1 B A R \begin{aligned}_B^AR^T{}_B^AR&=\left[\begin{matrix}{|} & {|} & {|} \\ {A\hat{X}_B} & {A\hat{Y}_B} & {A\hat{Z}_B} \\ {|} & {|} & {|}\end{matrix}\right]^T\left[\begin{matrix}{|} & {|} & {|} \\ {A\hat{X}_B} & {A\hat{Y}_B} & {A\hat{Z}_B} \\ {|} & {|} & {|}\end{matrix}\right] & \\& =\begin{bmatrix}- & {^A\hat{X}_B}^T & - \\ - & {^A\hat{Y}_B}^T & - \\ - & {^A\hat{Z}_B}^T & -\end{bmatrix}\begin{bmatrix}| & | & | \\ A\hat{X}_B & {^A\hat{Y}_B} & {^A\hat{Z}_B} \\ | & | & |\end{bmatrix} & \\ &=I_3 & \\ &={}_B^AR^{-1}{}_B^AR & \end{aligned} BARTBAR= ∣AX^B∣∣AY^B∣∣AZ^B∣ T ∣AX^B∣∣AY^B∣∣AZ^B∣ = −−−AX^BTAY^BTAZ^BT−−− ∣AX^B∣∣AY^B∣∣AZ^B∣ =I3=BAR−1BAR
B A R T = B A R − 1 = A B R (2-2) {}_B^AR^T={}_B^AR^{-1}={}_A^BR \tag{2-2} BART=BAR−1=ABR(2-2)
2.2.2.3 描述 A P ^AP AP
将空间中某点P相对于{B}的描述 B P ^BP BP转换成了该点相对于{A}的描述 A P ^AP AP
A P = [ A p x A p y A p z ] = [ B X ^ A ⋅ B P B Y ^ A ⋅ B P B Z ^ A ⋅ B P ] = B A P B P (2-3) ^AP=\begin{bmatrix}^{A}p_x \\^Ap_y\\^Ap_z\end{bmatrix}=\begin{bmatrix}^B\hat{X}_A{\cdot}^{B}P\\^B\hat{Y}_A{\cdot}^{B}P\\^B\hat{Z}_A{\cdot}^{B}P\\\end{bmatrix}={{^{A}_{B}}P }\ {^BP} \tag{2-3} AP= ApxApyApz = BX^A⋅BPBY^A⋅BPBZ^A⋅BP =BAP BP(2-3)
2.2.3 一般变换
2.2.3.1 变换矩阵定义
B P ^BP BP表示P在{B}中的位置，首先将 B P ^BP BP变换到跟{A}姿态相同的中间坐标系，该坐标系的原点和{B}的原点重合，再将其平移到{A}的原点处。
A P = B A R B P + A P B O R G (2-4) ^AP={{^A_B}R}\ {{^B}P}+^AP_{BORG}\tag{2-4} AP=BAR BP+APBORG(2-4)
( A P 1 ) = [ B A R A P B O R G 0 0 0 1 ] ( B P 1 ) (2-5) {\binom{^AP}{1}}=\left[\begin{array}{cccc} & _B^AR & & ^AP_{BORG} \\ 0 & 0 & 0 & 1\end{array}\right]{\binom{^BP}{1}}\tag{2-5} (1AP)=[0BAR00APBORG1](1BP)(2-5)
将 A P ^AP AP增加一个分量变为 4 × 1 4\times1 4×1的位置矢量，引入新形式（ 4 × 4 4\times4 4×4的齐次变换矩阵 B A T ^A_BT BAT）：
A P = B A T B P (2-6) {}^AP={}_B^AT^BP\tag{2-6} AP=BATBP(2-6)
2.2.3.2 变换矩阵意义
1）位姿的描述。
B
A
T
^A_BT
BAT表示相对于坐标系{
A
A
A}的坐标系{
B
B
B}。特别地，
B
A
T
^A_BT
BAT的各列是定义{
B
B
B}主轴方向的单位矢量，
A
P
B
O
R
G
^AP_{BORG}
APBORG确定了{
B
B
B}的原点。
2）变换映射。
B
A
T
^A_BT
BAT是映射
B
P
→
A
P
^BP→^AP
BP→AP。
3）变换算子。
T
T
T将
A
P
1
^AP_1
AP1变换为
A
P
2
^AP_2
AP2.
2.3 算子：平移、旋转和变换
用于坐标系间点的映射的通用数学表达式称为算子，包括点的平移算子、矢量旋转算子和平移加旋转的算子，仅涉及一个坐标系。
2.3.1 平移算子
2.3.1.1 定义
平移将空间中的一个点沿着一个已知的矢量方向移动一定距离，仅涉及一个坐标系。空间点的平移与此点向另一坐标系的映射具有相同的数学描述。
矢量 A P 1 ^AP_1 AP1 通过 A Q ^AQ AQ平移得到矢量 A P 2 ^AP_2 AP2，计算如下
A P 2 = A P 1 + A Q (2-7) {}^AP_2={}^AP_1+{}^AQ \tag{2-7} AP2=AP1+AQ(2-7)
用矩阵平移算子表示平移变换
A P 2 = D Q ( q ) A P 1 (2-8) {}^AP_2=D_Q(q){}^AP_1 \tag{2-8} AP2=DQ(q)AP1(2-8)
D Q ( q ) = [ 1 0 0 q x 0 1 0 q y 0 0 1 q z 0 0 0 1 ] D_Q(q)=\left[\begin{matrix}1&0&0&q_x\\0&1&0&q_y\\0&0&1&q_z\\0&0&0&1\end{matrix}\right] DQ(q)= 100001000010qxqyqz1
其中，
q
x
,
q
y
,
q
z
q_x,q_y,q_z
qx,qy,qz是平移矢量
Q
Q
Q的分量，并且
q
=
q
x
2
+
q
y
2
+
q
z
2
q=\sqrt{q^2_x+q^2_y+q^2_z}
q=qx2+qy2+qz2
式（2-7）与（2-1）的数学表达式相同
2.3.1.2 注意
如果在上图中已经定义了 B P A O R G ^BP_{AORG} BPAORG({ A A A}相对于{ B B B}的位置），而不是 A P B O R G ^AP_{BORG} APBORG，则会有符号的变化，这区别了是矢量向前运动还是坐标系向后运动。
2.3.2 旋转算子
2.3.2.1 定义
矢量经某一旋转R得到的旋转矩阵与描述某个坐标系相对参考坐标系旋转R所得的旋转矩阵是相同的。
旋转矩阵可以用旋转算子来定义，将一个矢量 A P 1 ^AP_1 AP1用旋转矩阵R变幻成一个新的矢量 A P 2 ^AP_2 AP2，当旋转矩阵作为旋转算子时，无需写出上下标，因为她不涉及两个坐标系，可写为式（2-9），与式（2-3）的数学表达形式相同，但解释不同。
A P 2 = R A P 1 (2-9) ^AP_2=R^AP_1\tag{2-9} AP2=RAP1(2-9)
将旋转算子定义为绕广义 K ˆ \^K Kˆ轴旋转 θ \theta θ角度的形式
A P 2 = R K ( θ ) A P 1 (2-10) ^AP_2=R_K(\theta)^AP_1\tag{2-10} AP2=RK(θ)AP1(2-10)
2.3.2.2 注意
如果在式（2-3）中已经定义了 A B R ^B_AR ABR而不是 B A R ^A_BR BAR那么式（2-9）应该使用R的逆，这区别了是矢量往前旋转还是坐标系往后旋转。
2.3.3 变换算子
变换算子 T T T将矢量 A P 1 ^AP_1 AP1平移并旋转得到一个新矢量 A P 2 ^AP_2 AP2
A P 2 = T A P 1 (2-11) ^AP_2=T^AP_1\tag{2-11} AP2=TAP1(2-11)
式（2-11）与式（2-6）数学意义相同，解释不同。
包含旋转
R
R
R和平移
Q
Q
Q的变换与描述某个坐标系相对于参考坐标系旋转
R
R
R并平移
Q
Q
Q的变换是相同的。变换通常被认为是由广义旋转矩阵和位置矢量分量组成的齐次变换的形式。
2.4 变换乘法和逆运算
2.4.1 复合变换
已知坐标系｛C｝相对于坐标系｛B｝， 并且已知坐标系｛B｝相对于 坐标系｛A｝
C A T = B A T C B 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 ] \begin{align*}_C^AT &=_B^AT_C^BT \\&=\left[\begin{array}{cccc} & {_B^AR}{^B_CR} & & _B^AR^BP_{CORG}+^AP_{BORG} \\ 0 & 0 & 0 & 1\end{array}\right]\tag{2-12}\end{align*} CAT=BATCBT=[0BARCBR00BARBPCORG+APBORG1](2-12)
2.4.2 逆变换
已知坐标系｛B｝相对于坐标系｛A｝，即 B A T ^A_BT BAT的值已知，求 4 × 4 4\times4 4×4矩阵 A B T ^B_AT ABT
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 ] (2-13) \begin{align*}^B_AT &=^A_BT^{-1}\\&=\left[\begin{array}{cccc} & _B^AR^T & & {-_B^AR^T }{^AP_{BORG}} \\ 0 & 0 & 0 & 1\end{array}\right]\end{align*}\tag{2-13} ABT=BAT−1=[0BART00−BARTAPBORG1](2-13)
2.4.3 其他姿态描述
旋转矩阵是一个正交单位矩阵，9个矩阵元素有6个约束，因此可以用三个参数表示姿态。因为旋转矩阵不满足交换律，所以矩阵的顺序很重要。
三种表示姿态的常用方法： X-Y-Z 固定角、Z-Y-X 欧拉角和Z-Y-Z 欧拉角。 每个表示法均需要按一定顺序进行绕主轴的3个旋转。 这些表示法是24 种表示法中的典型方法，且都被称作转角排列设定法。 其中， 12 种为固定角设定法， 另 12 种为欧拉角设定法。
2.4.3.1 X-Y-Z固定角（绕着固定参考坐标系{ A A A}的XYZ轴旋转）
首先将坐标系｛B｝和一个已知参考坐标系｛A｝重合。 先将｛B｝绕 X ˆ A \^X_A XˆA旋转 γ γ γ角， 再绕 Y ˆ A \^Y_A YˆA 旋转 β \beta β角， 最后绕 Z ˆ A \^Z_A ZˆA 旋转 α α α角，则旋转矩阵 B A R X Y Z ( γ , β , α ) {_B^AR_{XYZ}\left(\gamma,\beta,\alpha\right)} BARXYZ(γ,β,α)为
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 γ ] = [ 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 ] (2-14) \begin{aligned}_B^AR_{XYZ}\left(\gamma,\beta,\alpha\right) & =R_Z(\alpha)R_Y(\beta)R_X(\gamma) \\ & =\begin{bmatrix}c\alpha & -s\alpha & 0 \\ s\alpha & c\alpha & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} c\beta & 0 & s\beta \\ 0 & 1 & 0 \\ -s\beta & 0 & c\beta\end{bmatrix}\begin{bmatrix}1 & 0 & 0 \\ 0 & c\gamma & -s\gamma \\ 0 & s\gamma & c\gamma\end{bmatrix}\\&=\begin{bmatrix}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{bmatrix}\\ &=\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix}\\&\tag{2-14}\end{aligned} BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)= cαsα0−sαcα0001 cβ0−sβ010sβ0cβ 1000cγsγ0−sγcγ = 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 (2-14)
β = Atan2 ( − r 31 , r 11 2 + r 21 2 ) α = Atan2 ( r 21 / c β , r 11 / c β ) γ = Atan2 ( r 32 / c β , r 33 / c β ) 其中 A t a n 2 ( y , x ) = tan − 1 ( y x ) (2-15) \begin{aligned}\beta&=\operatorname{Atan2}(-r_{31},\sqrt{r_{11}^2+r_{21}^2})\\\alpha&=\operatorname{Atan2}(r_{21}/c\beta,r_{11}/c\beta)\\\gamma&=\operatorname{Atan2}(r_{32}/c\beta,r_{33}/c\beta) & & & \text{其中}\mathrm{Atan2}(y,x)=\tan^{-1}\left(\frac{y}{x}\right)\tag{2-15}\end{aligned} βαγ=Atan2(−r31,r112+r212)=Atan2(r21/cβ,r11/cβ)=Atan2(r32/cβ,r33/cβ)其中Atan2(y,x)=tan−1(xy)(2-15)
2.4.3.2 Z-Y-X欧拉角（绕运动坐标系{ B B B}的ZYX轴依次旋转）
首先将坐标系｛B｝和一个已知参考坐标系｛A｝重合。 先将｛B｝绕 Z ˆ B \^Z_B ZˆB 旋转 α α α角，再绕 Y ˆ B \^Y_B YˆB 旋转 β \beta β角， 最后绕 X ˆ B \^X_B XˆB旋转 γ γ γ角，用中间坐标系{B’}和{B’'}来表达旋转矩阵 B A R X ′ Y ′ Z ′ ( α , β , γ ) {_B^AR_{X^{\prime}Y^{\prime}Z^{\prime}}\left(\alpha,\beta,\gamma\right)} BARX′Y′Z′(α,β,γ)为
B A R Z ′ Y ′ X ′ ( α , β , γ ) = 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 γ ] = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α c β s a s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] \begin{align}{}_B^AR_{Z^{\prime}Y^{\prime}X^{\prime}}(\alpha,\beta,\gamma)&=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\\&=\begin{bmatrix}c\alpha&-s\alpha&0\\s\alpha&c\alpha&0\\0&0&1\end{bmatrix}\begin{bmatrix}c\beta&0&s\beta\\0&1&0\\-s\beta&0&c\beta\end{bmatrix}\begin{bmatrix}1&0&0\\0&c\gamma&-s\gamma\\0&s\gamma&c\gamma\end{bmatrix}\\&=\begin{bmatrix}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 & sas\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{bmatrix}& \tag{2-16}\end{align} BARZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)= cαsα0−sαcα0001 cβ0−sβ010sβ0cβ 1000cγsγ0−sγcγ = cαcβsαcβ−sβcαsβsγ−sαcγsasβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ (2-16)
式（2-15）也可以用来求解同一个已知旋转矩阵对应的Z-Y-X欧拉角
2.4.3.3 Z-Y-Z欧拉角（绕运动坐标系{ B B B}的ZYZ轴旋转）
首先将坐标系｛B｝和一个已知参考坐标系｛A｝重合。 先将｛B｝绕 Z ˆ B \^Z_B ZˆB 旋转 α α α角，再绕 Y ˆ B \^Y_B YˆB 旋转 β \beta β角， 最后绕 Z ˆ B \^Z_B ZˆB旋转 γ γ γ角，用中间坐标系{B’}和{B’'}来表达旋转矩阵 B A R Z ′ Y ′ Z ′ ( α , β , γ ) {_B^AR_{Z^{\prime}Y^{\prime}Z^{\prime}}\left(\alpha,\beta,\gamma\right)} BARZ′Y′Z′(α,β,γ)为
B A R Z ′ Y Z ′ ( a , β , γ ) = [ α c β c γ − s α s γ − c α c β s γ − s α c γ c α s β s α c β c γ + c α s γ − s α c β s γ + c α c γ s α s β − s β c γ s β s γ c β ] = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] \begin{align*}_B^AR_{Z^{\prime}YZ^{\prime}}(a,\beta,\gamma)&=\begin{bmatrix}\alpha c\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 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{bmatrix}\\&=\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix}&\tag{2-17}\end{align*} BARZ′YZ′(a,β,γ)= αcβcγ−sαsγsαcβcγ+cαsγ−sβcγ−cαcβsγ−sαcγ−sαcβsγ+cαcγsβsγcαsβsαsβcβ = r11r21r31r12r22r32r13r23r33 (2-17)
β = Atan2 ( r 31 2 + r 32 2 , r 33 ) α = Atan2 ( r 23 / s β , r 13 / s β ) γ = Atan2 ( r 32 / s β , − r 31 / s β ) \begin{aligned}\beta=&\text{Atan2}(\sqrt{r_{31}^2+r_{32}^2},r_{33})\\{\alpha}=&\text{Atan2}(r_{23}/s\beta,r_{13}/s\beta)\\{\gamma}=&\text{Atan2}(r_{32}/s\beta,-r_{31}/s\beta)\end{aligned} β=α=γ=Atan2(r312+r322,r33)Atan2(r23/sβ,r13/sβ)Atan2(r32/sβ,−r31/sβ)
2.4.3.4 等效角度－轴线表示法（轴的方向是一般方向）
如果轴的方向是一般方向（而不是主轴方向）， 任何姿态都可通过选择适当的轴和角度来得到。
首先将坐标系｛B｝和一个已知的参考坐标系｛A｝重合。 将｛B｝绕矢量
A
K
ˆ
^A\^K
AKˆ按右手定则转
θ
\theta
θ角。矢量
K
^
\hat{K}
K^有时被称为有限旋转的等效轴。{B}相对于｛A｝的一般姿态可用
B
A
R
(
K
^
,
θ
)
^A_BR(\hat{K},\theta)
BAR(K^,θ)或
R
K
(
θ
)
R_{K}(\theta)
RK(θ)表示。矢量
A
K
^
^A\hat{K}
AK^只需要两个参数，
θ
\theta
θ确定了第三个参数。经常用旋转量。乘以单位方向矢量
K
K
K形成一个简单的3×1的矢量来描述姿态， 用
K
K
K表示。
R K ( θ ) = [ k x k x v θ + c θ k x k y v θ − k z s θ k x k z v θ + k y s θ k x k y v θ + k z s θ k y k y v θ + c θ k y k z v θ − k x s θ k x k z v θ − k y s θ k y k z v θ + k x s θ k z k z v θ + c θ ] (2-18) R_K(\theta)=\begin{bmatrix}k_xk_xv\theta+c\theta&k_xk_yv\theta-k_zs\theta&k_xk_zv\theta+k_ys\theta\\k_xk_yv\theta+k_zs\theta&k_yk_yv\theta+c\theta&k_yk_zv\theta-k_xs\theta\\k_xk_zv\theta-k_ys\theta&k_yk_zv\theta+k_xs\theta&k_zk_zv\theta+c\theta\end{bmatrix} ~~~~\tag{2-18} RK(θ)= kxkxvθ+cθkxkyvθ+kzsθkxkzvθ−kysθkxkyvθ−kzsθkykyvθ+cθkykzvθ+kxsθkxkzvθ+kysθkykzvθ−kxsθkzkzvθ+cθ (2-18)
其中 c θ = c o s θ , s θ = s i n θ , v θ = 1 − c o s θ , A K ˆ = ( k x k y k z ) T c\theta=\mathrm{cos}\theta,\quad s\theta=\mathrm{sin}\theta,\quad v\theta=1-\mathrm{cos}\theta,\quad {^A\^K=(k_x\quad k_y\quad k_z)^T} cθ=cosθ,sθ=sinθ,vθ=1−cosθ,AKˆ=(kxkykz)T。 θ \theta θ的符号由右手定则判定，即大拇指指向 A K ˆ ^A\^K AKˆ的正方向，四根手指指向的方向为 θ \theta θ的正向。
KaTeX parse error: \tag works only in display equations
对任意一对轴线-角度( A K ^ , θ ^A\hat{K},\theta AK^,θ)，存在另一对轴线-角度，即( − A K ^ , − θ -^A\hat{K},-\theta −AK^,−θ)，他们在空中的姿态相同。
空间中任一位置的轴角表示是唯一的，旋转轴可以通过单位向量 A K ^ = ( k x , k y , k z ) T ^A\hat{K}=(k_x, k_y, k_z)^T AK^=(kx,ky,kz)T和轴角 θ \theta θ表示，总共需要三个参数，两个表示旋转轴，一个表示旋转角，通常用 Θ = θ A K ^ \Theta=\theta ^A\hat{K} Θ=θAK^表示，这里的旋转轴称为欧拉轴，旋转向量称为欧拉向量。
2.4.3.5 欧拉参数（四元数）
另一种姿态表示法是通过4个数值来表示的，称为欧拉参数，可看作一个 4 × 1 4\times1 4×1的单位矢量，可被视为一个单位四元数。根据等效旋转轴 K ⃗ = ( k x k y k z ) T \vec{K}=(k_x\quad k_y\quad k_z)^T K=(kxkykz)T和等效旋转角 θ \theta θ得到欧拉参数如下，且 ε 1 2 + ε 2 2 + ε 3 2 + ε 4 2 = 1 {\varepsilon_1}^2+{\varepsilon_2}^2+{\varepsilon_3}^2+{\varepsilon_4}^2=1 ε12+ε22+ε32+ε42=1
ε 1 = k x sin θ 2 ε 2 = k y sin θ 2 ε 3 = k z sin θ 2 ε 4 = cos θ 2 (2-19) {\begin{aligned} & \varepsilon_1=k_x\sin\frac{\theta}{2} \\ & \varepsilon_2=k_y\sin\frac\theta2 \\ & \varepsilon_3=k_z\sin\frac\theta2 \\ & \varepsilon_4=\cos\frac\theta2\end{aligned}}\tag{2-19} ε1=kxsin2θε2=kysin2θε3=kzsin2θε4=cos2θ(2-19)
用欧拉参数表示旋转矩阵 R ε R_\varepsilon Rε为
R ε = [ 1 − 2 ε 2 2 − 2 ε 3 2 2 ( ε 1 ε 2 − ε 3 ε 4 ) 2 ( ε 1 ε 3 + ε 2 ε 4 ) 2 ( ε 1 ε 2 + ε 3 ε 4 ) 1 − 2 ε 1 2 − 2 ε 3 2 2 ( ε 2 ε 3 − ε 1 ε 4 ) 2 ( ε 1 ε 3 − ε 2 ε 4 ) 2 ( ε 2 ε 3 + ε 1 ε 4 ) 1 − 2 ε 1 2 − 2 ε 2 2 ] (2-20) {R_\varepsilon=\begin{bmatrix}1-2\boldsymbol{\varepsilon}_2^2-2\boldsymbol{\varepsilon}_3^2&2(\boldsymbol{\varepsilon}_1\boldsymbol{\varepsilon}_2-\boldsymbol{\varepsilon}_3\boldsymbol{\varepsilon}_4)&2(\boldsymbol{\varepsilon}_1\boldsymbol{\varepsilon}_3+\boldsymbol{\varepsilon}_2\boldsymbol{\varepsilon}_4)\\2(\boldsymbol{\varepsilon}_1\boldsymbol{\varepsilon}_2+\boldsymbol{\varepsilon}_3\boldsymbol{\varepsilon}_4)&1-2\boldsymbol{\varepsilon}_1^2-2\boldsymbol{\varepsilon}_3^2&2(\boldsymbol{\varepsilon}_2\boldsymbol{\varepsilon}_3-\boldsymbol{\varepsilon}_1\boldsymbol{\varepsilon}_4)\\2(\boldsymbol{\varepsilon}_1\boldsymbol{\varepsilon}_3-\boldsymbol{\varepsilon}_2\boldsymbol{\varepsilon}_4)&2(\boldsymbol{\varepsilon}_2\boldsymbol{\varepsilon}_3+\boldsymbol{\varepsilon}_1\boldsymbol{\varepsilon}_4)&1-2\boldsymbol{\varepsilon}_1^2-2\boldsymbol{\varepsilon}_2^2\end{bmatrix}}\tag{2-20} Rε= 1−2ε22−2ε322(ε1ε2+ε3ε4)2(ε1ε3−ε2ε4)2(ε1ε2−ε3ε4)1−2ε12−2ε322(ε2ε3+ε1ε4)2(ε1ε3+ε2ε4)2(ε2ε3−ε1ε4)1−2ε12−2ε22 (2-20)
给定旋转矩阵对应的欧拉参数为
ε 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 (2-21) \begin{aligned}&\varepsilon_1 =\frac{r_{32}-r_{23}}{4\boldsymbol{\varepsilon}_4} \\&\varepsilon_2=\frac{r_{13}-r_{31}}{4\boldsymbol{\varepsilon}_{4}} \\&\varepsilon_3 =\frac{r_{21}-r_{12}}{4\boldsymbol{\varepsilon}_4} \\&\varepsilon_4=\frac12\sqrt{1+r_{11}+r_{22}+r_{33}} \end{aligned}\tag{2-21} ε1=4ε4r32−r23ε2=4ε4r13−r31ε3=4ε4r21−r12ε4=211+r11+r22+r33(2-21)
2.5 matlab习题
1.
%ZYXeul2R
function R = ZYXeul2R(alpha,beta,gamma)
%输入欧拉角α，β，γ，输出旋转矩阵R
R=[cos(alpha)*cos(beta),cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma),cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma);...
sin(alpha)*cos(beta),sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma),sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma);...
-sin(beta),cos(beta)*sin(gamma),cos(beta)*cos(gamma)];
end
%R2ZYXeul
function [alpha,beta,gamma] = R2ZYXeul(R)
%输入旋转矩阵R,输出欧拉角α，β，γ
cbeta=sqrt(R(1,1)^2+R(2,1)^2);
if cbeta==0
beta=pi/2;
alpha=0;
gamma=atan2(R(1,2),R(2,2));
else
beta=atan2(-R(3,1),cbeta);
alpha=atan2(R(2,1)/cos(beta),R(1,1)/cos(beta));
gamma=atan2(R(3,2)/cos(beta),R(3,3)/cos(beta));
end
%(a)
ABR1=ZYXeul2R(deg2rad(10),deg2rad(20),deg2rad(30))
ABR2=ZYXeul2R(deg2rad(30),deg2rad(90),deg2rad(-55))
%(b)
[alpha1,beta1,gamma1]=R2ZYXeul(ABR1);
rad2deg([alpha1,beta1,gamma1])
[alpha2,beta2,gamma2]=R2ZYXeul(ABR2);
rad2deg([alpha2,beta2,gamma2])
%(c)
theta=20;
Ry=[cosd(theta) 0 sind(theta)
0 1 0
-sind(theta) 0 cosd(theta)];
BP=[1 0 1]';
AP=Ry*BP
ABR1=rpy2r(30,20,10)
ABR2=rpy2r(-55,90,30)
% 函数rpy2r()为输入绕 X-Y-Z固定角坐标系旋转的度数γ，β，α，输出旋转矩阵。
% 而该旋转矩阵等同于绕Z-Y-X欧拉角的旋转矩阵。即绕Z-Y-X欧拉角10，20，30等于绕X-Y-Z固定角30，20，10。
angles1=tr2rpy(ABR1,'deg')
angles2=tr2rpy(ABR2,'deg')
%函数tr2rpy为函数rpy2r()的逆运算，'deg'为角度制输出。
%注意到angles2并不是[-55,90,30]，是因为在β为90°的情况下，只能求出α和γ的和或差。
AP=roty(20)*[1 0 1].'%函数roty()为输入绕Y轴旋转的度数，输出旋转矩阵。
2.
% ZYXeulVector2Tr
function T = ZYXeulVector2Tr(alpha,beta,gamma,P)
%输入欧拉角α，β，γ和位置矢量P,输出齐次变换矩阵T
T=[ZYXeul2R(alpha,beta,gamma) P;
0 0 0 1];
end
%invT
function Tv = invT(T)
%输出用符号公式计算的齐次变换矩阵T的逆矩阵
Tv(1:3,1:3)=T(1:3,1:3).';
Tv(1:3,4)=-T(1:3,1:3).'*T(1:3,4);
Tv(4,:)=[0,0,0,1];
end
%% (a)
ABT1=ZYXeulVector2Tr(deg2rad(10),deg2rad(20),deg2rad(30),[1 2 3]')
ABT2=ZYXeulVector2Tr(0,deg2rad(20),0,[3 0 1]')
%% (b)
AP=ZYXeulVector2Tr(0,deg2rad(20),0,[3 0 1].')*[1 0 1 1].'
%% (c)
% 对(a)中的i)和ii)验证
ABT1*invT(ABT1)
invT(ABT1)*ABT1
ABT2*invT(ABT2)
invT(ABT2)*ABT2
% 所得结果均为单位矩阵
%% (d)
% i)
ABT=ABT1;
BCT=ABT2;
ACT=ABT*BCT
CAT=invT(BCT)*invT(ABT)
% ii),iii)
ABTii=ACT*invT(BCT)
ABTii-ABT;
BCTiii=invT(ABT)*ACT
BCTiii-BCT;
% 通过做矩阵减法可得结果与已知答案几乎一致（相差10^-15）
%% 工具箱检查
ABT1=rt2tr(rpy2r(30,20,10),[1,2,3].')
ABT2=rt2tr(rpy2r(0,20,0),[3,0,1].')% 函数rt2tr(R,t)将旋转矩阵R和平移量t合成齐次变换矩阵。
AP=ABT2*[1 0 1 1].'
T=transl(3,0,1)*troty(20)%trotx(),troty(),trotz()三个函数计算旋转变换矩阵，分别对应于X轴，Y轴，Z轴旋转一定的角度，得到的是一个4×4的矩阵。
%T=transl(x，y，z):表示能够获取一个分别沿着x，y，z轴平移一段距离得到的4×4齐次变换矩阵，而rotx(),roty(),rotz()是3×3矩阵
290
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
