关于旋转矩阵
在坐标系的变换中，主要分为旋转变换和平移变换。对于旋转变换来说，常常通过一个3×3的正交矩阵来描述坐标系的一个旋转过程，也可以用于描述一个空间物体的姿态。
旋转矩阵的特殊性在于，它与一个三维向量相乘时只改变其方向而不改变其长度。为了达到这种效果，旋转矩阵均为3*3的正交矩阵，属于一个特殊正交群SO(3)：
S
O
(
3
)
=
{
R
∈
R
3
×
3
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
+
1
}
SO(3)=\{R\in \R^{3\times3}|RR^T=I,det(R)=+1\}
SO(3)={R∈R3×3∣RRT=I,det(R)=+1}
进一步推广，对于三维空间中的6自由度运动，人们希望定义一个统一变换矩阵来同时描述坐标系的平移和旋转变换。这种统一的变换矩阵将包含在如下的特殊欧几里得空间里：
S
E
(
3
)
=
{
(
p
,
R
)
∣
p
∈
R
3
,
R
∈
S
O
(
3
)
}
=
R
3
×
S
O
(
3
)
SE(3)=\{ (p,R) | p \in \R^3, R \in SO(3) \} = \R^3 \times SO(3)
SE(3)={(p,R)∣p∈R3,R∈SO(3)}=R3×SO(3)
1.旋转矩阵
旋转矩阵一般可以通过以下几种方式获得。
1.1.根据定义
已知存在两个坐标系{A}和{B}，如何得到相对于{A}的{B}的姿态，即旋转矩阵 B A R ^A_BR BAR呢？
假设用 X B ^ \hat{X_B} XB^、 Y B ^ \hat{Y_B} YB^和 Z B ^ \hat{Z_B} ZB^来表示坐标系{B}主轴方向的单位矢量。当用坐标系{A}作为参考系时，它们被写作 A X B ^ \hat{^AX_B} AXB^、 A Y B ^ \hat{^AY_B} AYB^和 A Z B ^ \hat{^AZ_B} AZB^，分别表示坐标系{B}各个主轴在坐标系{A}描述下的单位矢量，也就是坐标系{B}各个主轴分别在坐标系{A}的三个主轴下的投影分量。
将这三个单位矢量按照顺序排列成一个3*3的矩阵，即为旋转矩阵。
B
A
R
=
(
A
X
B
^
A
Y
B
^
A
Z
B
^
)
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
^A_BR=(\hat{^AX_B}\quad\hat{^AY_B}\quad\hat{^AZ_B}) =\left[ \begin{matrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{matrix} \right]
BAR=(AXB^AYB^AZB^)=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
1.2.连续旋转
一个旋转矩阵也可以被视为多个基本旋转矩阵的乘积。基本旋转矩阵，即绕基本坐标系的某个主轴进行旋转后得到的旋转矩阵，根据绕X轴、Y轴和Z轴旋转可分别得到三种基本旋转矩阵。
R
X
(
θ
)
=
[
1
0
0
0
cos
θ
−
sin
θ
0
sin
θ
cos
θ
]
R_X(\theta)=\left[ \begin{matrix} 1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta \end{matrix} \right]
RX(θ)=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤
R Y ( θ ) = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_Y(\theta)=\left[ \begin{matrix} \cos\theta & 0 & \sin\theta\\ 0 & 1 & 0\\ -\sin\theta & 0 & \cos\theta \end{matrix} \right] RY(θ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤
R Z ( θ ) = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R_Z(\theta)=\left[ \begin{matrix} \cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{matrix} \right] RZ(θ)=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤
根据旋转的参考系不同，可分为绕固定坐标系和绕运动坐标系这两种旋转方式。下面介绍两种常见的连续旋转方式。
1.2.1.X-Y-Z固定角（静态欧拉角）
假设坐标系{B}与一个已知参考坐标系{A}重合。然后坐标系{B}经过以下次序进行旋转：
- 绕 X A ^ \hat{X_A} XA^旋转 γ \gamma γ角；
- 绕 Y A ^ \hat{Y_A} YA^旋转 β \beta β角；
- 绕 Z A ^ \hat{Z_A} ZA^旋转 α \alpha α角.
最后，根据旋转次序将三种基本旋转矩阵相乘即可得到对应的旋转矩阵
B
A
R
^A_BR
BAR。但需要注意的是矩阵相乘的方向，由于这三次旋转都是绕着固定坐标系进行的，因此矩阵相乘顺序为从右到左。
B
A
R
X
Y
Z
(
γ
,
β
,
α
)
=
R
Z
(
α
)
R
Y
(
β
)
R
X
(
γ
)
^A_BR_{XYZ}(\gamma,\beta,\alpha)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)
BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)
1.2.2.Z-Y-X欧拉角（动态欧拉角）
假设坐标系{B}与一个已知参考坐标系{A}重合。然后坐标系{B}经过以下次序进行旋转：
- 绕 Z B ^ \hat{Z_B} ZB^旋转 α \alpha α角；
- 绕 Y B ^ \hat{Y_B} YB^旋转 β \beta β角；
- 绕 X B ^ \hat{X_B} XB^旋转 γ \gamma γ角.
最后，根据旋转次序将三种基本旋转矩阵相乘即可得到对应的旋转矩阵
B
A
R
^A_BR
BAR。但需要注意的是矩阵相乘的方向，由于这三次旋转都是绕着运动坐标系进行的，每次旋转所绕的轴的姿态取决于上一次的旋转，因此矩阵相乘顺序为从左到右。
B
A
R
Z
′
Y
′
X
′
(
α
,
β
,
γ
)
=
R
Z
(
α
)
R
Y
(
β
)
R
X
(
γ
)
^A_BR_{Z'Y'X'}(\alpha,\beta,\gamma)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)
BARZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)
由于在整个旋转过程中，参考坐标系的选取一直在发生变换，因此会出现万向节锁的现象。
为什么会出现万向节锁的问题呢？原因在于参考系一直在变换。
假设一个初始坐标系{A}，根据其三个主轴可以进行三种不同的旋转，也就是说存在3个自由度。假设旋转序列为 X → Y → Z X\rightarrow Y\rightarrow Z X→Y→Z：
- 首先初始坐标系{A}绕X轴旋转任意角度得到坐标系{B}，假设旋转 0 o 0^o 0o；
- 然后坐标系{B}绕Y轴旋转 9 0 o 90^o 90o得到坐标系{C}，此时{C}的Z轴与{A}的X轴重合；
- 最后坐标系{C}绕Z轴旋转任意角度得到坐标系{D}。
从最后一次旋转中可以发现，无论坐标系{C}绕Z轴如何转动，都无法得到绕初始坐标系{A}的Z轴的效果，即此时旋转失去了一个自由度。这就是出现万向节锁的原因。
1.3. k − ϕ k-\phi k−ϕ过程
1.3.1.简介
除了上述两种方法外，还可以通过给定一个单位向量k和以k为轴旋转的角度 ϕ \phi ϕ，从而得到旋转矩阵。这种方法的好处是可以以任意向量为旋转轴，而不是使用一个连续的旋转。
在介绍
k
−
ϕ
k-\phi
k−ϕ过程前，需要先介绍一个重要的数学理论发现，即指数映射。
E
X
P
:
s
o
(
3
)
→
S
O
(
3
)
EXP:so(3)\rightarrow SO(3)
EXP:so(3)→SO(3)
该映射说明，对于每一个3*3的斜对称矩阵
S
∈
s
o
(
3
)
S\in so(3)
S∈so(3)，其指数函数
e
x
p
(
S
)
=
R
∈
S
O
(
3
)
exp(S)=R\in SO(3)
exp(S)=R∈SO(3)始终是旋转矩阵。下面正式开始介绍
k
−
ϕ
k-\phi
k−ϕ过程。
已知一个单位向量
k
=
(
k
1
k
2
k
3
)
T
k=(k_1 \quad k_2 \quad k_3)^T
k=(k1k2k3)T，可以给出相应的斜对称矩阵为
S
(
k
)
=
K
=
[
0
−
k
3
k
2
k
3
0
−
k
1
−
k
2
k
1
0
]
S(k)=K=\left[ \begin{matrix} 0 & -k_3 & k_2\\ k_3 & 0 & -k_1\\ -k_2 & k_1 & 0 \end{matrix} \right]
S(k)=K=⎣⎡0k3−k2−k30k1k2−k10⎦⎤
根据指数映射公式，可以通过斜对称矩阵
ϕ
K
∈
s
o
(
3
)
\phi K\in so(3)
ϕK∈so(3)得到：
e
x
p
(
ϕ
K
)
=
R
∈
S
O
(
3
)
exp(\phi K)=R\in SO(3)
exp(ϕK)=R∈SO(3)
然后将上式左边按照泰勒级数展开，可以得到以下公式，也就是著名的罗德里格斯公式：
I
+
sin
ϕ
K
+
(
1
−
cos
ϕ
)
K
2
=
R
I+\sin\phi K+(1-\cos\phi)K^2=R
I+sinϕK+(1−cosϕ)K2=R
1.3.2.两个参数与特征值和特征向量的联系
基于旋转矩阵R的特殊性，其特征值必然为
λ
1
=
1
,
λ
2
,
3
=
cos
θ
±
i
sin
θ
\lambda_1 = 1, \lambda_{2,3} = \cos\theta \pm i \sin\theta
λ1=1,λ2,3=cosθ±isinθ
而从特征值中，我们神奇的发现了
ϕ
\phi
ϕ的身影。通过MATLAB验证发现，
ϕ
=
θ
\phi = \theta
ϕ=θ。接下来，我们把目光转移到特征值
λ
1
\lambda_1
λ1，假设其特征向量为v。根据特征值与特征向量的基本等式，代入
λ
1
\lambda_1
λ1可以发现一个神奇的现象：
R
v
=
v
Rv=v
Rv=v
向量v左乘旋转矩阵后得到的新向量仍为v。这是为什么呢？向量v与向量k之间有什么联系呢?
事实上，通过MATLAB验证可以发现，向量v与向量k相等！也就是说，旋转矩阵R的旋转轴即为其特征值为1所对应的特征向量。有了这个结论，我们可以比较直观理解上述的等式了。由于旋转矩阵R可以分解成旋转轴k和旋转角度 ϕ \phi ϕ，于是等式左边可以理解为：向量v绕旋转轴k旋转 ϕ \phi ϕ，而当向量v与转轴k相等时，也就是说，向量v绕自身转动，最终得到的新向量自然就是向量v本身。
这也是一般性的矩阵的特征向量的厉害之处，从基本等式 A α = λ α = β A\alpha=\lambda \alpha = \beta Aα=λα=β可以看出，矩阵A右乘其特征向量 α \alpha α，其结果得到的新的向量 β \beta β与其本身方向相同，改变的仅是其模的大小。
1.4.单位四元数(Unit Quaternion)
四元数并不是一个新概念，其出现的历史已经超过了150年。但一直以来，四元数都饱受人们的争议，其其中一个反对理由就是四元数不支持乘法交换律。但直到机器人学的出现，让四元数逐渐受到重视且发挥着巨大的作用，比如单位四元数可用于描述旋转运动。
假设四元数由一个标量s和一个向量v组成，如下式所示：
q
=
s
+
v
⃗
=
s
+
v
1
i
+
v
2
j
+
v
3
k
q=s+\vec{v}=s+v_1i+v_2j+v_3k
q=s+v=s+v1i+v2j+v3k
q = s < v 1 , v 2 , v 3 > q = s<v_1,v_2,v_3> q=s<v1,v2,v3>
用单位四元数表示旋转与第三种方式有些相似，都用到了等效旋转轴k和等效旋转角
ϕ
\phi
ϕ，可表示为
q
=
s
+
v
⃗
,
s
=
cos
θ
2
,
v
⃗
=
(
sin
θ
2
)
k
^
q=s+\vec{v},s=\cos \frac{\theta}{2},\vec{v}=(\sin \frac{\theta}{2})\hat{k}
q=s+v,s=cos2θ,v=(sin2θ)k^
单位四元数一个由四个参数组成，可组成一个4*1的矢量，被称为欧拉参数。根据等效旋转轴
k
^
=
(
k
x
k
y
k
z
)
T
\hat{k}=(k_x \quad k_y \quad k_z)^T
k^=(kxkykz)T和等效旋转角
ϕ
\phi
ϕ，可以得到欧拉参数如下：
ε
1
=
k
x
sin
θ
2
\varepsilon_1=k_x\sin\frac{\theta}{2}
ε1=kxsin2θ
ε 2 = k y sin θ 2 \varepsilon_2=k_y\sin\frac{\theta}{2} ε2=kysin2θ
ε 3 = k z sin θ 2 \varepsilon_3=k_z\sin\frac{\theta}{2} ε3=kzsin2θ
ε 4 = cos θ 2 \varepsilon_4=\cos\frac{\theta}{2} ε4=cos2θ
比较欧拉参数和单位四元数的四个参数，可以发现：
s
=
ε
4
,
v
⃗
=
(
ε
1
,
ε
2
,
ε
3
)
s=\varepsilon_4,\vec{v}=(\varepsilon_1,\varepsilon_2,\varepsilon_3)
s=ε4,v=(ε1,ε2,ε3)
根据欧拉参数或者说单位四元数，可以得到对应的旋转矩阵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
]
R=\left[ \begin{matrix} 1-2\varepsilon^2_2-2\varepsilon^2_3 & 2(\varepsilon_1\varepsilon_2-\varepsilon_3\varepsilon_4) & 2(\varepsilon_1\varepsilon_3+\varepsilon_2\varepsilon_4)\\ 2(\varepsilon_1\varepsilon_2+\varepsilon_3\varepsilon_4) & 1-2\varepsilon^2_1-2\varepsilon^2_3 & 2(\varepsilon_2\varepsilon_3-\varepsilon_1\varepsilon_4)\\ 2(\varepsilon_1\varepsilon_3-\varepsilon_2\varepsilon_4) & 2(\varepsilon_2\varepsilon_3+\varepsilon_1\varepsilon_4) & 1-2\varepsilon^2_1-2\varepsilon^2_2 \end{matrix} \right]
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⎦⎤
使用四元数描述旋转矩阵有诸多优点：
- 计算速度相对较快；
- 可以避免万向节锁(gimbal lock)现象；
667
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
