旋转矩阵
基本概念
三个主轴，可以看作是三个向量，为b在a的表达，以a为基准
旋转矩阵
B相对于A的姿态：
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 ] ^A_BR=\begin{bmatrix} ^A\hat{X_B} & ^A\hat{Y_B} & ^A\hat{Z_B}\\ \end{bmatrix} =\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} BAR=[AXB^AYB^AZB^]= 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
用三个列向量分别来表示B坐标系每一个转轴的方向，每一个列向量的组成：即每一个轴的向量在A坐标系的投影，即通过点乘得到投影。
以下面的图为例子，蓝色为A坐标系，红色为B坐标系。
如果要求 B A R = ? ^A_BR=? BAR=? ，需要分析B的三轴与A的三轴的关系。B的X轴与A的Z轴反向，则 A X B ^ = [ 0 0 − 1 ] ^A\hat{X_B}=\begin{bmatrix}0\\0\\-1\end{bmatrix} AXB^= 00−1 ，如果去分析投影计算方法，则可知是向量 X B ^ = [ 0 0 − 1 ] \hat{X_B}=\begin{bmatrix}0\\0\\-1\end{bmatrix} XB^= 00−1 分别与 X A ^ = [ 1 0 0 ] \hat{X_A}=\begin{bmatrix}1\\0\\0\end{bmatrix} XA^= 100 、 Y A ^ = [ 0 1 0 ] \hat{Y_A}=\begin{bmatrix}0\\1\\0\end{bmatrix} YA^= 010 、 Z A ^ = [ 0 0 1 ] \hat{Z_A}=\begin{bmatrix}0\\0\\1\end{bmatrix} ZA^= 001 ，就可以得到 A X B ^ = [ 0 0 − 1 ] ^A\hat{X_B}=\begin{bmatrix}0\\0\\-1\end{bmatrix} AXB^= 00−1 了。
B的Y轴与A的Y轴重叠，即 A Y B ^ = [ 0 1 0 ] ^A\hat{Y_B}=\begin{bmatrix}0\\1\\0\end{bmatrix} AYB^= 010
B的Z轴与A的X轴重叠，即 A Z B ^ = [ 1 0 0 ] ^A\hat{Z_B}=\begin{bmatrix}1\\0\\0\end{bmatrix} AZB^= 100
可以得到
B A R = [ 0 0 1 0 1 0 − 1 0 0 ] ^A_BR=\begin{bmatrix}0 &0 &1\\ 0&1&0\\ -1&0&0 \end{bmatrix} BAR= 00−1010100
特性
特性1
由于点乘得到的是一个数，左右交换顺序并不改变得到的结果
B A R = [ 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 ] = [ 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 ] ^A_BR =\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}=\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} BAR= 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 = 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
B A R = [ B X A ^ T B Y A ^ T B Z A ^ T ] = [ B X A ^ B Y A ^ B Z A ^ ] T = A B R T ^A_BR=\begin{bmatrix}^B\hat{X_A}^T\\^B\hat{Y_A}^T\\^B\hat{Z_A}^T\end{bmatrix} =\begin{bmatrix}^B\hat{X_A}&^B\hat{Y_A}&^B\hat{Z_A}\end{bmatrix}^T =^B_AR^T BAR= BXA^TBYA^TBZA^T =[BXA^BYA^BZA^]T=ABRT
特性2
B A R T ⋅ B A R = [ A X B ^ T A Y B ^ T A Z B ^ T ] [ A X B ^ A Y B ^ A Z B ^ ] = [ 1 0 1 0 1 0 0 0 1 ] = I 3 = B A R − 1 ⋅ B A R ^A_BR^T \cdot^A_BR=\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} =\begin{bmatrix} 1 & 0 & 1\\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} =I_3=^A_BR^{-1} \cdot^A_BR BART⋅BAR= AXB^TAYB^TAZB^T [AXB^AYB^AZB^]= 100010101 =I3=BAR−1⋅BAR
即：
B A R T ⋅ B A R = = B A R − 1 ⋅ B A R ^A_BR^T \cdot^A_BR==^A_BR^{-1} \cdot^A_BR BART⋅BAR==BAR−1⋅BAR
称之为单位正交矩阵。
该矩阵有九个数字，但是有六个限制条件（单位正交、每一行或者列的长度为1，共有三行六列），因此只有三个自由度，与转动只有三个自由度相符。
特性3
坐标点的转换，在B坐标系的P点通过与旋转矩阵相乘，得到A坐标系的P点
A P = B A R B P ^AP=^A_BR^BP AP=BARBP
对于将旋转拆解成三次旋转的连乘
- 需要明确多次旋转的前后顺序
- 旋转转轴也需要明确定义，是对固定不动的转轴旋转（Fixed angles），还是对转动之后坐标系下的转轴旋转（Euler angles）
旋转矩阵可以描述物体旋转的状态（以逆时针为正）,
以 Z A ^ \hat{Z_A} ZA^为旋转轴，旋转角度为 θ \theta θ
R Z A ^ ( θ ) = [ c o s θ − s i n θ 0 s i n θ c o s θ 0 0 0 1 ] R_{\hat{Z_A}}(\theta)=\begin{bmatrix}cos\theta&-sin\theta&0\\sin\theta&cos\theta&0\\0&0&1\end{bmatrix} RZA^(θ)= cosθsinθ0−sinθcosθ0001
通过旋转，得到下面的坐标 A P ′ = R ( θ ) A P {}^AP^{\prime}=R(\theta)^AP AP′=R(θ)AP 。
Fix Angles
依次按照XYZ轴旋转，可得：
B A R X Y Z ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( γ ) _B^AR_{XYZ}(\gamma,\beta,\alpha)=R_Z(\alpha)R_Y(\beta)R_X(\gamma) BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)
先转的在后面，即先与矩阵相乘（需要乘的矩阵在最右侧），先进行变换，后转在前面。
在fix angles下，先对X轴旋转60°、后对Y轴旋转30°与先对Y轴旋转30°，在对X轴旋转60°的旋转矩阵。
先对X轴旋转60°、后对Y轴旋转30°：
B A R X Y Z ( γ , β , α ) = R Z ( 0 ) R Y ( 30 ) R X ( 60 ) = [ 0.866 0.433 0.25 0 0.5 − 0.866 − 0.5 0.75 0.433 ] ^A_BR_{XYZ}(\gamma,\beta,\alpha)=R_Z(0)R_Y(30)R_X(60)=\begin{bmatrix}0.866&0.433&0.25\\0&0.5&-0.866\\-0.5&0.75&0.433\end{bmatrix} BARXYZ(γ,β,α)=RZ(0)RY(30)RX(60)= 0.8660−0.50.4330.50.750.25−0.8660.433
先对Y轴旋转30°，在对X轴旋转60°的旋转矩阵:
B A R X Y Z ( γ , β , α ) = R Z ( 0 ) R X ( 60 ) R Y ( 30 ) = [ 0.866 0 0.5 0.433 0.5 − 0.75 − 0.25 0.866 0.433 ] _B^AR_{XYZ}(\gamma,\beta,\alpha)=R_Z(0)R_X(60)R_Y(30)=\begin{bmatrix}0.866&0&0.5\\0.433&0.5&-0.75\\-0.25&0.866&0.433\end{bmatrix} BARXYZ(γ,β,α)=RZ(0)RX(60)RY(30)= 0.8660.433−0.2500.50.8660.5−0.750.433
可以通过旋转矩阵倒推角度：
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 ] ^A_BR_{XYZ}(\gamma,\beta,\alpha)=\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} 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
当 β ≠ 90 \beta \neq 90 β=90 :
β = A t a n 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 β ) \begin{aligned} &\beta=Atan2(-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{aligned} β=Atan2(−r31,r112+r212)α=Atan2(r21/cβ,r11/cβ)γ=Atan2(r32/cβ,r33/cβ)
I f β = 9 0 ∘ α = 0 ∘ γ = A t a n 2 ( r 12 , r 22 ) \begin{aligned}\mathsf{If}\beta&=90^\circ\\\alpha&=0^\circ\\\gamma&=Atan2(r_{12},r_{22})\end{aligned} Ifβαγ=90∘=0∘=Atan2(r12,r22)
I f β = − 9 0 ∘ α = 0 ∘ γ = − A t a n 2 ( r 12 , r 22 ) \begin{aligned}\mathsf{If}\beta&=-90^\circ\\\alpha&=0^\circ\\\gamma&=-Atan2(r_{12},r_{22})\end{aligned} Ifβαγ=−90∘=0∘=−Atan2(r12,r22)
Euler Angles
B A R Z ′ Y ′ X ′ ( α , β , γ ) = B ′ A R B ′ ′ B ′ R B ′ ′ B R = R Z ′ ( α ) R Y ′ ( β ) R X ′ ( γ ) {}_{B}^{A}R_{Z^{\prime}Y^{\prime}X^{\prime}}(\alpha,\beta,\gamma)={}^A_{B^{\prime}}R_{B^{\prime\prime}}^{B^{\prime}}R^{B^{\prime\prime}}{}_{B}R=R_{Z^{\prime}}(\alpha)R_{Y^{\prime}}(\beta)R_{X^{\prime}}(\gamma) BARZ′Y′X′(α,β,γ)=B′ARB′′B′RB′′BR=RZ′(α)RY′(β)RX′(γ)
先对哪个轴转，哪个轴的旋转矩阵在最前面。大概理解为，如果要从旋转后的坐标系恢复至原来的坐标系，需要从最后一个旋转的轴，依次往前复原
如果三个角度相同，那么最终结果与FixAngles相同。
可以通过多种旋转方法得到相同的旋转矩阵，即旋转到相同的姿态
Euler和Fix各有十二种方法，第一个旋转的轴有三种选择，第二个有两种（排除掉第一个旋转的轴），第三个有两种（排除掉第二个旋转的轴）
还存在四元数法，之后会去介绍。
（设置标志位）)
芯片手册(I2C通信)-翻译版)
