旋转矩阵表示形式
??由欧拉旋转定理可得：刚体在三位空间里的一般运动可以分解为刚体上某一点的平移，以及绕过该点旋转轴的转动；
只考虑旋转：
??三维空间中的点p在坐标系
(
e
1
,
e
2
,
e
3
)
(e_1, e_2, e_3)
(e1?,e2?,e3?)中的坐标为
(
a
1
,
a
2
,
a
3
)
(a_1, a_2, a_3)
(a1?,a2?,a3?),当坐标系
(
e
1
,
e
2
,
e
3
)
(e_1, e_2, e_3)
(e1?,e2?,e3?)旋转到坐标系
(
e
1
′
,
e
2
′
,
e
3
′
)
(e_1^{\prime}, e_2^{\prime}, e_3^{\prime})
(e1′?,e2′?,e3′?),点p的坐标为
(
a
1
′
,
a
2
′
,
a
3
′
)
(a_1^{\prime}, a_2^{\prime}, a_3^{\prime})
(a1′?,a2′?,a3′?)，如式1所示：
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
,
e
2
′
,
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
(1)
\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right]\tag1
[e1?,e2?,e3?]???a1?a2?a3?????=[e1′?,e2′?,e3′?]???a1′?a2′?a3′?????(1)
等式两边同乘
[
e
1
,
e
2
,
e
3
]
T
\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]^T
[e1?,e2?,e3?]T，由于
e
1
,
e
2
,
e
3
\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}
e1?,e2?,e3?为正交向量有：
[
a
1
a
2
a
3
]
=
[
e
1
T
e
1
′
e
1
T
e
2
′
e
1
T
e
3
′
e
2
T
e
1
′
e
2
T
e
2
′
e
2
T
e
3
′
e
3
T
e
1
′
e
3
T
e
2
′
e
3
T
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
?
R
a
′
(2)
\left[\begin{array}{c} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{ccc} \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \triangleq \boldsymbol{R} \boldsymbol{a}^{\prime}\tag2
???a1?a2?a3?????=???e1T?e1′?e2T?e1′?e3T?e1′??e1T?e2′?e2T?e2′?e3T?e2′??e1T?e3′?e2T?e3′?e3T?e3′????????a1′?a2′?a3′??????Ra′(2)
则称 R R R 为旋转矩阵，其包含以下性质：
- R是正交矩阵: R ? 1 = R , ? R T R = I R^{-1}=R, \ R^TR = I R?1=R,?RTR=I
- R的行列是为1：
∣
R
∣
=
1
|R|=1
∣R∣=1
注：上式两个条件是充分必要条件，只要满足上式两个条件的矩阵都可以作为一个旋转矩阵，即：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ? ( R ) = 1 } (3) S O(n)=\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} \mid \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}\tag3 SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(3)
??将所有满足式3的矩阵称为 Special Orthognal Group （特殊正交群），下面只考虑三维空间中的特殊正交群，坐标系1到坐标系2的之间的旋转关系可由式4来表示：
a
1
=
R
12
a
2
a
2
=
R
21
a
1
(4)
a_1 = R_{12}a_2\\ a_2 = R_{21}a1\tag4
a1?=R12?a2?a2?=R21?a1(4)
其中
R
21
=
R
12
?
1
=
R
12
T
R_{21}=R_{12}^{-1}=R_{12}^{T}
R21?=R12?1?=R12T?，若一个旋转矩阵取转置或取逆，则表示一个相反方向的旋转；
旋转+平移：
??若同时考虑平移，则两个坐标系下p点的坐标关系如式5所示：
a
′
=
R
a
+
t
(5)
a^{\prime} = Ra+t\tag5
a′=Ra+t(5)
具体表示形式为：
a
2
=
R
21
?
a
1
+
t
21
a
1
=
R
12
?
a
2
+
t
12
a_{2}=R_{21} \cdot a_{1}+t_{21} \\ a_{1}=R_{12} \cdot a_{2}+t_{12}
a2?=R21??a1?+t21?a1?=R12??a2?+t12?
注：
t
12
≠
?
t
21
t_{12}\neq -t_{21}
t12??=?t21?
其次坐标表示：
由于刚体在三维空间中可能发生多次旋转与平移，如式6所示：
b
=
R
1
a
+
t
1
,
c
=
R
2
b
+
t
2
.
?
c
=
R
2
(
R
1
a
+
t
1
)
+
t
2
.
(6)
\boldsymbol{b}=\boldsymbol{R}_{1} a+t_{1}, \quad c=\boldsymbol{R}_{2} b+t_{2} . \quad \Longrightarrow \quad c=\boldsymbol{R}_{2}\left(\boldsymbol{R}_{1} a+t_{1}\right)+t_{2} .\tag6
b=R1?a+t1?,c=R2?b+t2?.?c=R2?(R1?a+t1?)+t2?.(6)
为了方便表达这种运动，引入了其次坐标表示形式如式7所示：
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
?
T
[
a
1
]
\left[\begin{array}{l} \boldsymbol{a}^{\prime} \\ 1 \end{array}\right]=\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{T} & 1 \end{array}\right]\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right] \triangleq \boldsymbol{T}\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right]
[a′1?]=[R0T?t1?][a1?]?T[a1?]
其中
T
=
[
R
t
0
T
1
]
T = \left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{T} & 1 \end{array}\right]
T=[R0T?t1?] 为变换矩阵(同时包含旋转与平移),变换矩阵的逆
T
?
1
\boldsymbol{T}^{-1}
T?1为：
T
?
1
=
[
R
T
?
R
T
t
0
T
1
]
\boldsymbol{T}^{-1}=\left[\begin{array}{cc} \boldsymbol{R}^{T} & -\boldsymbol{R}^{T} t \\ \mathbf{0}^{T} & 1 \end{array}\right]
T?1=[RT0T??RTt1?]
将变换矩阵的集合称为特殊欧式群SE(3) (Special Euclidean Group)：
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
(7)
\mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\}\tag7
SE(3)={T=[R0T?t1?]∈R4×4∣R∈SO(3),t∈R3}(7)
其中
[
a
′
1
]
\left[\begin{array}{l} \boldsymbol{a}^{\prime} \\ 1\end{array}\right]
[a′1?] 为齐次坐标，齐次坐标乘以任意非零常熟仍然表示三维空间中的同一个坐标：
a
~
=
[
a
1
]
=
k
[
a
1
]
(8)
\tilde{a}=\left[\begin{array}{l} a \\ 1 \end{array}\right]=k\left[\begin{array}{l} a \\ 1 \end{array}\right]\tag8
a~=[a1?]=k[a1?](8)
则多次的旋转与平移可以如式9所示：
b
~
=
T
1
a
~
,
c
~
=
T
2
b
~
?
c
~
=
T
2
T
1
a
~
(9)
\tilde{b}=\boldsymbol{T}_{1} \tilde{\boldsymbol{a}}, \tilde{\boldsymbol{c}}=\boldsymbol{T}_{2} \tilde{\boldsymbol{b}} \quad \Rightarrow \tilde{\boldsymbol{c}}=\boldsymbol{T}_{2} \boldsymbol{T}_{\mathbf{1}} \tilde{\boldsymbol{a}}\tag9
b~=T1?a~,c~=T2?b~?c~=T2?T1?a~(9)
旋转向量的表达形式
??用9个元素的旋转矩阵仅仅表示3个自由度的信息，表达形式过于复杂，不节省内存空间。从直观上看，旋转可以表示为绕某个轴
n
n
n 旋转一定的角度
θ
\theta
θ，为此引入了**旋转向量（Rotation Vector）**或角轴/轴角（Angle Axis）
旋转矩阵与旋转向量是等价的，可以互相转换：
旋转向量到旋转矩阵的变换： 罗德里格斯公式(Rodrigus’s Formula):
R
=
cos
?
θ
I
+
(
1
?
cos
?
θ
)
n
n
T
+
sin
?
θ
n
∧
(10)
\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T}+\sin \theta \boldsymbol{n}^{\wedge}\tag{10}
R=cosθI+(1?cosθ)nnT+sinθn∧(10)
其中
cos
?
θ
I
\cos \theta \boldsymbol{I}
cosθI 为3x3矩阵，
(
1
?
cos
?
θ
)
n
n
T
(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T}
(1?cosθ)nnT也为3x3的矩阵，
n
∧
\boldsymbol{n}^{\wedge}
n∧表示旋转向量
n
n
n 的叉积。
旋转矩阵到旋转向量：
θ
=
arccos
?
(
tr
?
(
R
)
?
1
2
)
R
n
=
n
(11)
\theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right)\\ Rn = n\tag{11}
θ=arccos(2tr(R)?1?)Rn=n(11)
n
n
n 为
R
R
R 矩阵对应特征值为1的特征向量；
欧拉角(Euler Angles)
??旋转向量表示的旋转方式不够直观，无法从旋转向量中直接看出旋转轴的位置，欧拉角将旋转分解为三个方向上的旋转（如Z-Y-X）方向，轴可以为定轴或者动轴，轴的顺序也可以改变，如最常用绕动轴的Z-Y-X旋转方式:
- 绕物体的Z轴转动，得到偏航角yaw；
- 绕旋转之后的Y轴旋转，得到俯仰角pitch；
- 绕旋转之后的X轴旋转，得到滚转角roll；
万向锁（Gimbal Lock）：欧拉角在某些旋转情况下，旋转自由度会减1，存在奇异性；由于万向锁的存在，欧拉角不适合做插值或迭代，应用不广泛；
四元数
四元素基础
??四元数是一种既节省空间，又不存在奇异性的刚体旋转表示方式；
在复数平面坐标系下，可以用单位复数来表达旋转操作：
z
=
x
+
i
y
=
ρ
e
i
θ
(12)
z=x+i y=\rho e^{i \theta}\tag{12}
z=x+iy=ρeiθ(12)
当
z
z
z 乘以一个纯虚数
i
i
i 表示原来的向量旋转90度：
z
i
=
(
x
+
i
y
)
i
=
ρ
e
i
(
θ
+
π
2
)
(13)
zi=(x+i y)i=\rho e^{i (\theta+\frac{\pi}{2})}\tag{13}
zi=(x+iy)i=ρei(θ+2π?)(13)
在三维情况下， 四元数可以作为复数的扩充：
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
(14)
\boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k\tag{14}
q=q0?+q1?i+q2?j+q3?k(14)
四元数包含1个实部与3个虚部，虚部之间满足以下的关系（自己与自己运算像复数运算，自己与别人运算像叉乘）：
{
i
2
=
j
2
=
k
2
=
?
1
i
j
=
k
,
j
i
=
?
k
j
k
=
i
,
k
j
=
?
i
k
i
=
j
,
i
k
=
?
j
(15)
\left\{\begin{array}{l} i^{2}=j^{2}=k^{2}=-1 \\ i j=k, j i=-k \\ j k=i, k j=-i \\ k i=j, i k=-j \end{array}\right.\tag{15}
????????i2=j2=k2=?1ij=k,ji=?kjk=i,kj=?iki=j,ik=?j?(15)
单位四元数可以表达旋转：
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
,
q
=
[
s
,
v
]
,
s
=
q
0
∈
R
,
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
(16)
\boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k, \quad \boldsymbol{q}=[s, \boldsymbol{v}], \quad s=q_{0} \in \mathbb{R}, \boldsymbol{v}=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3}\tag{16}
q=q0?+q1?i+q2?j+q3?k,q=[s,v],s=q0?∈R,v=[q1?,q2?,q3?]T∈R3(16)
基本运算如下所示：
加减法：
q
a
±
q
b
=
[
s
a
±
s
b
,
v
a
±
v
b
]
\begin{aligned} \boldsymbol{q}_{a} \pm \boldsymbol{q}_{b}=& {\left[s_{a} \pm s_{b}, \boldsymbol{v}_{a} \pm \boldsymbol{v}_{b}\right] }\end{aligned}
qa?±qb?=?[sa?±sb?,va?±vb?]?
乘法：
q
a
q
b
=
s
a
s
b
?
x
a
x
b
?
y
a
y
b
?
z
a
z
b
+
(
s
a
x
b
+
x
a
s
b
+
y
a
z
b
?
z
a
y
b
)
i
+
(
s
a
y
b
?
x
a
z
b
+
y
a
s
b
+
z
a
x
b
)
j
+
(
s
a
z
b
+
x
a
y
b
?
y
b
x
a
+
z
a
s
b
)
k
\begin{aligned}\boldsymbol{q}_{a} \boldsymbol{q}_{b}=& s_{a} s_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ &+\left(s_{a} x_{b}+x_{a} s_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\ &+\left(s_{a} y_{b}-x_{a} z_{b}+y_{a} s_{b}+z_{a} x_{b}\right) j \\ &+\left(s_{a} z_{b}+x_{a} y_{b}-y_{b} x_{a}+z_{a} s_{b}\right) k \end{aligned}
qa?qb?=?sa?sb??xa?xb??ya?yb??za?zb?+(sa?xb?+xa?sb?+ya?zb??za?yb?)i+(sa?yb??xa?zb?+ya?sb?+za?xb?)j+(sa?zb?+xa?yb??yb?xa?+za?sb?)k?
q
a
q
b
=
[
s
a
s
b
?
v
a
T
v
b
,
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
]
\begin{aligned}\boldsymbol{q}_{a} \boldsymbol{q}_{b}=\left[s_{a} s_{b}-\boldsymbol{v}_{a}^{T} \boldsymbol{v}_{b}, s_{a} \boldsymbol{v}_{b}+s_{b} \boldsymbol{v}_{a}+\boldsymbol{v}_{a} \times \boldsymbol{v}_{b}\right] \end{aligned}
qa?qb?=[sa?sb??vaT?vb?,sa?vb?+sb?va?+va?×vb?]?
共轭：
q
a
?
=
s
a
?
x
a
i
?
y
a
j
?
z
a
k
=
[
s
a
,
?
v
a
]
\boldsymbol{q}_{a}^{*}=s_{a}-x_{a} i-y_{a} j-z_{a} k=\left[s_{a},-\boldsymbol{v}_{a}\right]
qa??=sa??xa?i?ya?j?za?k=[sa?,?va?]
取模长：
∥
q
a
∥
=
s
a
2
+
x
a
2
+
y
a
2
+
z
a
2
\left\|\boldsymbol{q}_{a}\right\|=\sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}}
∥qa?∥=sa2?+xa2?+ya2?+za2??
取逆：
q
?
1
=
q
?
/
∥
q
∥
2
\boldsymbol{q}^{-1}=\boldsymbol{q}^{*} /\|\boldsymbol{q}\|^{2}
q?1=q?/∥q∥2
数乘：
k
q
=
[
k
s
,
k
v
]
k \boldsymbol{q}=[k s, k \boldsymbol{v}]
kq=[ks,kv]
点乘：
q
a
?
q
b
=
s
a
s
b
+
x
a
x
b
i
+
y
a
y
b
j
+
z
a
z
b
k
\boldsymbol{q}_a \cdot \boldsymbol{q}_{b}=s_{a} s_{b}+x_{a} x_{b} i+y_{a} y_{b} j+z_{a} z_{b} k
qa??qb?=sa?sb?+xa?xb?i+ya?yb?j+za?zb?k
旋转角转换为四元数：
q
=
[
cos
?
θ
2
,
n
x
sin
?
θ
2
,
n
y
sin
?
θ
2
,
n
z
sin
?
θ
2
]
T
(17)
\boldsymbol{q}=\left[\cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2}\right]^{T}\tag{17}
q=[cos2θ?,nx?sin2θ?,ny?sin2θ?,nz?sin2θ?]T(17)
四元数转为旋转向量：
{
θ
=
2
arccos
?
q
0
[
n
x
,
n
y
,
n
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
/
sin
?
θ
2
(18)
\left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]^{T}=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}} \end{array}\right.\tag{18}
{θ=2arccosq0?[nx?,ny?,nz?]T=[q1?,q2?,q3?]T/sin2θ??(18)
使用四元数表示三维空间中的旋转
??若点 p p p 经过一次以四元数 q q q 表示的旋转后得 p ′ p^{\prime} p′ ，包含以下两个步骤：
- 将 p p p 坐标用纯虚的四元数表示： p = [ 0 , x , y , z ] p =[0,x,y,z] p=[0,x,y,z]
- 使用四元数q将其旋转(前面乘以 q q q ，后面乘以 q ? 1 q^{-1} q?1 )： p ′ = q p q ? 1 (19) p^{\prime}=qpq^{-1}\tag{19} p′=qpq?1(19)
