旋转矩阵
基本概念
三个主轴，可以看作是三个向量，为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各有十二种方法，第一个旋转的轴有三种选择，第二个有两种（排除掉第一个旋转的轴），第三个有两种（排除掉第二个旋转的轴）
还存在四元数法，之后会去介绍。
