1. 旋转矩阵

一个刚体在三维空间中的运动由一次旋转加一次平移组成。本文主要介绍旋转矩阵、欧拉角、四元数的意义，以及他们是如何运算和转化的。

1.1 点和向量、坐标系

在三维空间中，刚体不仅有位置还有姿态。比如相机，我们可以说相机位于世界坐标系的（0， 0， 0）处（位置），朝向正前方（姿态）。

向量（也称为欧几里得向量、几何向量、矢量），指具有大小和方向的量。三维空间中某个向量的坐标可以用 $R^3$ 中的3个数来描述。如果确定了坐标系，也就是线性空间的基 $\left(\begin{matrix}e_1,e_2,e_3 \end{matrix}\right)$ ，那么向量 $\vec{a}$ 在这组基下的坐标可以表示为：

\begin{matrix} (3.1) & \vec{a} = [\begin{matrix} e_{1}, e_{2}, e_{3} \end{matrix}] [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}] = a_{1} \cdot e_{1} + a_{2} \cdot e_{2} + a_{3} \cdot e_{3} . \end{matrix}

$\vec{a}=\begin{bmatrix} e_1, e_2, e_3 \end{bmatrix} \begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix}=a_1\cdot e_1+a_2\cdot e_2+a_3\cdot e_3.\tag{3.1}$
对于

\vec{a} ， \vec{b} \in R^{3}

$\vec{a}，\vec{b}\in R^3$ ，向量的内积可以表示为：

\begin{matrix} (3.2) & \vec{a} \cdot \vec{b} = {\vec{a}}^{T} \cdot \vec{b} = \sum_{i = 1}^{3} a_{i} \cdot b_{i} = | a | \cdot | b | \cdot \cos < a, b > . \end{matrix}

$\vec{a}\cdot\vec{b}=\vec{a}^T\cdot\vec{b}=\sum_{i=1}^3 a_i\cdot b_i=|a|\cdot |b|\cdot \cos \lt a,b\gt.\tag{3.2}$
内积可以表示向量间的投影关系。
向量的外积表示为：

\begin{matrix} (3.3) & \vec{a} \times \vec{b} = | | \begin{matrix} i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{matrix} | | = [\begin{matrix} a_{2} \cdot b_{3} - a_{3} \cdot b_{2} \\ a_{3} \cdot b_{1} - a_{1} \cdot b_{3} \\ a_{1} \cdot b_{2} - a_{2} \cdot b_{1} \end{matrix}] = [\begin{matrix} 0 & - a_{3} & a_{2} \\ a_{3} & 0 & - a_{1} \\ - a_{2} & a_{1} & 0 \end{matrix}] \cdot \vec{b} = {\vec{a}}^{s} \cdot \vec{b} . \end{matrix}

$\vec{a}\times\vec{b}=\left|\left| \begin{matrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{matrix} \right|\right| =\left[ \begin{matrix} a_2\cdot b_3-a_3 \cdot b_2 \\ a_3\cdot b_1-a_1 \cdot b_3 \\ a_1\cdot b_2-a_2 \cdot b_1 \end{matrix} \right] =\left[ \begin{matrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{matrix} \right]\cdot \vec{b}=\vec{a}^s \cdot \vec{b}. \tag{3.3}$
外积的方向垂直于上面两个向量，大小为

| \vec{a} | \cdot | \vec{b} | \cdot \sin < a, b >

$|\vec{a}|\cdot |\vec{b}|\cdot \sin \lt a,b\gt$ 。对于外积，本文引入了

{\vec{a}}^{s}

$\vec{a}^s$ 来表示基于

\vec{a}

$\vec{a}$ 的 反对称矩阵。外积只对三维向量存在定义，此外还能使用外积来表示旋转。
反对称矩阵的定义：设

A

$A$ 为

n

$n$ 阶方阵，若有

A^{T} = - A

$A^T=-A$ ，则称方阵

A

$A$ 为反对称矩阵。

1.2 坐标系间的欧式变换

我们以旋转加平移来描述两个坐标系之间的变换关系。在刚体的运动过程中，常见的做法是设定一个世界坐标系（或者叫惯性坐标系），可以认为它是固定不动的。同时，刚体则是一个移动坐标系。
刚体运动保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。其在各个坐标系之间的变换称为欧式变换。欧式变换由一个旋转加一个平移组成。
首先考虑旋转。设某个单位正交基 $\left(\begin{matrix}e_1,e_2,e_3\end{matrix}\right)$ 经过一次旋转变成了 $\left(\begin{matrix}e '_1,e'_2,e'_3\end{matrix}\right)$ 。那么对于同一个向量 $\vec{a}$ （该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 $\left[\begin{matrix}a_1,a_2,a_3\end{matrix}\right]^T$ 和 $\left[\begin{matrix}a'_1,a'_2,a'_3\end{matrix}\right]^T$ 。根据坐标的定义有：

\begin{matrix} (3.4) & [\begin{matrix} e_{1}, e_{2}, e_{3} \end{matrix}] [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}] = (\begin{matrix} e_{1}^{'}, e_{2}^{'}, e_{3}^{'} \end{matrix}) [\begin{matrix} a_{1}^{'} \\ a_{2}^{'} \\ a_{3}^{'} \end{matrix}] . \end{matrix}

$\left[\begin{matrix}e_1,e_2,e_3\end{matrix}\right] \left[\begin{matrix}a_1\\a_2\\a_3\end{matrix}\right]= \left(\begin{matrix}e '_1,e'_2,e'_3\end{matrix}\right) \left[\begin{matrix}a'_1\\a'_2\\a'_3\end{matrix}\right].\tag{3.4}$
对上述

3.4

$3.4$ 式左右两边同时左乘

[\begin{matrix} e_{1} \\ e_{2} \\ e_{3} \end{matrix}]

$\left[\begin{matrix}e_1\\e_2\\e_3\end{matrix}\right]$ ，可得：

\begin{matrix} (3.5) & [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}] = [\begin{matrix} e_{1}^{T} \cdot e_{1}^{'} & e_{1}^{T} \cdot e_{2}^{'} & e_{1}^{T} \cdot e_{3}^{'} \\ e_{2}^{T} \cdot e_{1}^{'} & e_{2}^{T} \cdot e_{2}^{'} & e_{2}^{T} \cdot e_{3}^{'} \\ e_{3}^{T} \cdot e_{1}^{'} & e_{3}^{T} \cdot e_{2}^{'} & e_{3}^{T} \cdot e_{3}^{'} \end{matrix}] [\begin{matrix} a_{1}^{'} \\ a_{2}^{'} \\ a_{3}^{'} \end{matrix}] = R \cdot {\vec{a}}^{'} . \end{matrix}

$\left[\begin{matrix}a_1\\a_2\\a_3\end{matrix}\right]= \left[\begin{matrix}e^T_1\cdot e'_1 & e^T_1\cdot e'_2 & e^T_1\cdot e'_3 \\ e^T_2\cdot e'_1 & e^T_2\cdot e'_2 & e^T_2\cdot e'_3 \\ e^T_3\cdot e'_1 & e^T_3\cdot e'_2 & e^T_3\cdot e'_3 \end{matrix}\right] \left[\begin{matrix}a'_1\\a'_2\\a'_3\end{matrix}\right]=R\cdot \vec{a}'. \tag{3.5}$
由式

3.5

$3.5$ 可知

R

$R$ 描述了旋转关系，因此被称为 旋转矩阵。事实上，

R

$R$ 是一个行列式为1的 正交矩阵（逆为其自身转置的矩阵），反之，行列式为1的正交矩阵也是一个旋转矩阵。综上，旋转矩阵的集合定义如下：

\begin{matrix} (3.6) & S O (n) = {R \in R^{n * n} | R R^{T} = I, d e t (R) = 1} . \end{matrix}

$SO(n)=\lbrace R \in R^{n*n}|RR^T=I,det(R)=1\rbrace.\tag{3.6}$

S O (n)

$SO(n)$ 是特殊正交群（Special Orthogonal Group）的意思。特别地，

S O (3)

$SO(3)$ 就是三维空间的旋转了。也就是，旋转矩阵可以描述刚体的旋转。
由于旋转矩阵是正交矩阵，它的逆描述了一个相反的旋转。因此可得：

\begin{matrix} (3.7) & {\vec{a}}^{'} = R^{- 1} \vec{a} = R^{T} \vec{a} \end{matrix}

$\vec{a}'=R^{-1}\vec{a}=R^T\vec{a}\tag{3.7}$
显然

R^{T}

$R^T$ 是一个相反的旋转。
对于欧式变换来说，除了旋转外一般还有平移。我们使用平移向量

t

$t$ 来表示平移，则一个完整的欧式空间的坐标变换如下：

\begin{matrix} (3.8) & {\vec{a}}^{'} = \vec{a} = R \vec{a} + t . \end{matrix}

$\vec{a}'=\vec{a}=R\vec{a}+t.\tag{3.8}$

1.3 变换矩阵与齐次坐标

式 $3.8$ 完整的表达了欧式空间的旋转与平移，但是该变换并不是一个线性变换。假如我们进行了两次变换： $R_1,t_1$ 和 $R_2,t_2$ ，满足：

\vec{b} = R_{1} \cdot \vec{a} + \vec{t_{1}}, \vec{c} = R_{2} \cdot \vec{b} + \vec{t_{2}} .

$\vec{b}=R_1\cdot \vec{a}+\vec{t_1},\quad \vec{c}=R_2\cdot \vec{b}+\vec{t_2}.$
则

a

$a$ 到

c

$c$ 的变换为：

\vec{c} = R_{2} (R_{1} \cdot \vec{a} + \vec{t_{1}}) + \vec{t_{2}} .

$\vec{c}=R_2(R_1\cdot \vec{a}+\vec{t_1})+\vec{t_2}.$
在经过多次变换后上式会显得非常复杂。因此，本文引入齐次坐标和变换矩阵重写式

(3.8)

$(3.8)$ ：

\begin{matrix} (3.9) & [\begin{matrix} \vec{a^{'}} \\ 1 \end{matrix}] = [\begin{matrix} R & \vec{t} \\ 0^{T} & 1 \end{matrix}] [\begin{matrix} \vec{a} \\ 1 \end{matrix}] = T [\begin{matrix} \vec{a} \\ 1 \end{matrix}] . \end{matrix}

$\left[\begin{matrix}\vec{a'}\\1\end{matrix}\right]= \left[\begin{matrix}R & \vec{t} \\ 0^T & 1 \end{matrix}\right] \left[\begin{matrix}\vec{a}\\1\end{matrix}\right]= T\left[\begin{matrix}\vec{a}\\1\end{matrix}\right].\tag{3.9}$
如式

(3.9)

$(3.9)$ 所示，我们在每一个三维向量的末尾添加1，将其变为四维向量，称为 齐次坐标。其中矩阵

T

$T$ 称为 变换矩阵（Transform Matrix），变换矩阵左上角为旋转矩阵，右侧为平移向量，左下角为

0

$0$ 向量，右下角为1。我们暂时用

\vec{\bar{a}}

$\vec{\overline{a}}$ 表示

\vec{a}

$\vec{a}$ 的齐次坐标。由齐次坐标和变换矩阵可得：

\begin{matrix} (3.10) & \vec{\bar{b}} = T_{1} \cdot \vec{\bar{a}}, \vec{\bar{c}} = T_{2} \cdot \vec{\bar{b}} = T_{2} \cdot T_{1} \cdot \vec{\bar{a}} . \end{matrix}

$\vec{\overline{b}}=T_1\cdot \vec{\overline{a}},\quad \vec{\overline{c}}=T_2\cdot \vec{\overline{b}}=T_2\cdot T_1\cdot \vec{\overline{a}}.\tag{3.10}$
此外由式

(3.9)

$(3.9)$ 表示的变换矩阵

T

$T$ 又称为 特殊欧式群（Special Euclidean Group）：

\begin{matrix} (3.11) & S E (3) = {T = [\begin{matrix} R & \vec{t} \\ 0^{T} & 1 \end{matrix}] \in R^{4 * 4} | R \in S O (3), \vec{t} \in R^{3}} . \end{matrix}

$SE(3)=\lbrace T=\left[\begin{matrix}R & \vec{t} \\ 0^T & 1 \end{matrix}\right]\in R^{4*4}|R\in SO(3),\vec{t}\in R^3\rbrace.\tag{3.11}$

2. 旋转向量和欧拉角

$SO(3)$ 的旋转矩阵有9个量，但是一次旋转只有3个自由度。同样，变换矩阵用16个量来表示6个自由度（旋转加平移）的变换。因此这种方式是冗余的。此外旋转矩阵和变换矩阵都带了其他一些额外约束。

2.1 旋转向量

对于坐标系的旋转，可以用一个旋转轴和一个旋转角来刻画。于是，我们使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这样的向量称为旋转向量（或轴角）。
对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。假设有一个旋转轴为 $n$ ，角度为 $\theta$ 的旋转，那么它对应的旋转向量为 $\theta n$ 。
从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明，如下为转换结果：

\begin{matrix} (3.12) & R = \cos θ \cdot \vec{I} + (1 - \cos θ) \cdot n n^{T} + \sin θ \cdot n^{s} \end{matrix}

$R=\cos \theta \cdot\vec{I}+(1-\cos \theta)\cdot nn^T+\sin \theta \cdot n^s\tag{3.12}$
符号

s

$s$ 是向量到反对称的转换符。
对于转角

θ

$\theta$ ，有：

\begin{matrix} (3.13) & θ = \arccos (t r (R) - 1 / 2) \end{matrix}

$\theta=\arccos(tr(R)-1/2)\tag{3.13}$
关于转轴

n

$n$ ，由于旋转轴上的向量在旋转后不发生改变，有以下：

\begin{matrix} (3.14) & R \cdot n = n \end{matrix}

$R\cdot n=n\tag{3.14}$
由此可见，转轴

n

$n$ 是矩阵

R

$R$ 特征值为1对顶的特征向量。

2.2 欧拉角

欧拉角提供了一种直观的方式来描述旋转，它使用3个分离的转角，把一个旋转分解成3次绕不同的轴的旋转。欧拉角当中比较常用的一种便是用“偏航-俯仰-滚转（yaw-pitch-roll）”3个角度来描述一个旋转的，该旋转等价于ZYX轴的旋转。ZYX转角相当于把任意旋转分解成以下3个轴上的轴角：
1. 绕物体的Z轴旋转，得到偏航角yaw；
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch；
3. 绕旋转之后的X轴旋转，得到滚转角roll；
欧拉角的一个重大确定是会碰到著名的万向锁问题：当俯仰角为 $\pm90^{。}$ 时，第一次旋转与第三次旋转将使用同一个轴，这会导致系统丢失一个自由度。

2.3 四元数

关于四元数的定义以及运算可参考如下两篇博客：
1. 中文版 https://www.qiujiawei.com/understanding-quaternions/
2. 英文版 https://www.3dgep.com/understanding-quaternions/

2.3.1 用四元数表示旋转

我们可以用四元数表达对一个点的旋转。假设一个空间三维点 $p=[x, y, z]\in R^{3}$ ，以及一个轴角 $n,\theta$ 指定的旋转。三维点 $p$ 经过旋转之后变为 $p^{'}$ 。使用四元数来描述这个旋转。首先，将三维空间点用一个虚四元数来表示：

p = [0, x, y, z] = [0, v] .

$p=[0,x,y,z]=[0, \boldsymbol v].$
然后用四元数

q

$q$ 来表示这个旋转：

q = [\cos \frac{θ}{2}, n \cdot \sin \frac{θ}{2}]

$q=[\cos\frac{\theta}{2},n\cdot \sin\frac{\theta}{2}]$
那么，旋转后的

p^{^{'}}

$p^{'}$ 即可表示为如下式：

\begin{matrix} (3.15) & p^{^{'}} = q p q^{- 1} \end{matrix}

$p^{'}=qpq^{-1}\tag{3.15}$
可以验证。式

(3.15)

$(3.15)$ 计算得出的结果为

0

$0$ ，故为纯虚四元数。其虚部的3个分量表示旋转后3D点的坐标。

2.3.2 四元数到旋转矩阵的转换

设四元数 $q=q_0+q_1\cdot i+q_2\cdot j+q_3\cdot k$ ，则对应的旋转矩阵 $R$ 为

\begin{matrix} (3.16) & R = [\begin{matrix} 1 - 2 q_{2}^{2} - 2 q_{3}^{2} & 2 q_{1} q_{2} - 2 q_{0} q_{3} & 2 q_{1} q_{3} + 2 q_{0} q_{2} \\ 2 q_{1} q_{2} + 2 q_{0} q_{3} & 1 - 2 q_{1}^{2} - 2 q_{3}^{2} & 2 q_{2} q_{3} - 2 q_{0} q_{1} \\ 2 q_{1} q_{3} - 2 q_{0} q_{2} & 2 q_{2} q_{3} + 2 q_{0} q_{1} & 1 - 2 q_{1}^{2} - 2 q_{2}^{2} \end{matrix}] \end{matrix}

$R=\left[\begin{matrix}1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2\\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1\\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{matrix}\right]\tag{3.16}$
反之，由旋转矩阵到四元数的转换如下。假设矩阵

R = m_{i j}, i, j \in [1, 2, 3]

$R={m_{ij}},i,j\in [1,2,3]$ ，其对应的四元数

q

$q$ 由下式给出：

\begin{matrix} (3.17) & q_{0} = \frac{\sqrt{t r (R) + 1}}{2}, q_{1} = \frac{m_{23} - m_{32}}{4 q_{0}}, q_{2} = \frac{m_{31} - m_{13}}{4 q_{0}}, q_{3} = \frac{m_{12} - m_{21}}{4 q_{0}} \end{matrix}

$q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0}\tag{3.17}$

视觉slam14讲读书笔记(3)--三维空间刚体运动