《视觉SLAM十四讲》-- 三维空间的刚体运动

02 三维空间的刚体运动

2.0 机器人位姿表述

（1）二维与三维空间中，机器人位姿的表述：

• 2D 的情况：横纵坐标 + 旋转角（机器人朝向），即 $[x,y,\theta ]$

• 3D 的情况：三维空间中的旋转和平移

2.1 点和坐标系

2.1.1 三维坐标系有关表述

（1）基本运算

• 加减法

• 内积（点乘）：$\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^T\mathbf{b}={\sum }_{i=1}^3{a}_i\times b_i=|\mathbf{a}||\mathbf{b}|cos<\mathbf{a},\mathbf{b}>$，且 $\mathbf{a}\cdot \mathbf{b}=\mathbf{b}\cdot \mathbf{a}$

• 外积（叉乘）：

a × b = ∥ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ∥ = ( a 2 b 3 − a 3 b 2 ) e 1 + ( a 3 b 1 − a 1 b 3 ) e 2 + ( a 1 b 3 − a 3 b 1 ) e 3 = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b =  def  a ∧ b (2-1) \begin{aligned} \boldsymbol{a} \times \boldsymbol{b}=\left\|\begin{array}{ccc} \boldsymbol{e}_{1} & \boldsymbol{e}_{2} & \boldsymbol{e}_{3} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{array}\right\| &=(a_2b_3-a_3b_2)\boldsymbol{e}_{1}+(a_3b_1-a_1b_3)\boldsymbol{e}_{2}+(a_1b_3-a_3b_1)\boldsymbol{e}_{3} \\ &=\left[\begin{array}{c} a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1} \end{array}\right]=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] \boldsymbol{b}\stackrel{\text { def }}{=}\boldsymbol{a}^{\wedge} \boldsymbol{b} \end{aligned} \tag{2-1} a×b= ​e1​a1​b1​​e2​a2​b2​​e3​a3​b3​​ ​​=(a2​b3​−a3​b2​)e1​+(a3​b1​−a1​b3​)e2​+(a1​b3​−a3​b1​)e3​= ​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​ ​= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​b= def a∧b​(2-1)
也即$\mathbf{a}\times \mathbf{b}=|\mathbf{a}||\mathbf{b}|sin<\mathbf{a},\mathbf{b}>$，方向垂直于向量$\mathbf{a}$、$\mathbf{b}$ 组成的平面，遵循右手定则。

性质：$\mathbf{a}\times \mathbf{b}=-\mathbf{b}\times \mathbf{a}$

为便于后续表达，记

a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] (2-2) \boldsymbol{a}^{\wedge}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] \tag{2-2} a∧= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​(2-2)
这是一个反对称矩阵，满足$A^T=-A$。

2.1.2 坐标系变换

（1）三维空间向量表示：

a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 (2-3) \boldsymbol{a}=\left[\boldsymbol{e_1},\boldsymbol{e_2},\boldsymbol{e_3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=a_1\boldsymbol{e_1}+a_2\boldsymbol{e_2}+a_3\boldsymbol{e_3} \tag{2-3} a=[e1​,e2​,e3​] ​a1​a2​a3​​ ​=a1​e1​+a2​e2​+a3​e3​(2-3)
其中，${\mathbf{e}}_1、{\mathbf{e}}_2、{\mathbf{e}}_3$为基向量。

（2）两个坐标系之间的运动由一个旋转加一个平移组成，这种运动称为刚体运动。

（3）对于坐标系旋转变换，假设原坐标系和现坐标系单位正交基底分别为$[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3]$和$[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }]$，

坐标分别为$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$，由于向量本身的绝对位置没有改变，则

[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] (2-4) \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] \tag{2-4} [e1​,e2​,e3​] ​a1​a2​a3​​ ​=[e1′​,e2′​,e3′​] ​a1′​a2′​a3′​​ ​(2-4)

将上式两端分别左乘 [ e 1 T e 2 T e 3 T ] \left[\begin{array}{l} \boldsymbol{e_{1}^T} \\ \boldsymbol{e}_{2}^T \\ \boldsymbol{e}_{3}^T \end{array}\right] ​e1T​e2T​e3T​​ ​得到，

[ 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 ′ ] =  def  R a ′ (2-5) \left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{lll} \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] \stackrel{\text { def }}{=} \boldsymbol{R} \boldsymbol{a}^{\prime} \tag{2-5} ​a1​a2​a3​​ ​= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​ ​ ​a1′​a2′​a3′​​ ​= def Ra′(2-5)

矩阵$\mathbf{R}$即为旋转矩阵。它有如下性质：

• $\mathbf{R}$ 行列式为 1;
• $\mathbf{R}$ 是一个正交矩阵，即满足${\mathbf{R}}^T\mathbf{R}=\mathbf{I}$。
• 坐标系 1 到坐标系 2 ，有${\mathbf{a}}_1={\mathbf{R}}_{12}{\mathbf{a}}_2$，反之有${\mathbf{a}}_2={\mathbf{R}}_{21}{\mathbf{a}}_1$，其中，
  ${\mathbf{R}}_{12}={{\mathbf{R}}_{21}}^{-1}={{\mathbf{R}}_{21}}^T$

将满足此性质的矩阵集合定义为特殊正交群：

$$\mathrm{SO}(n)=\left\{\mathbf{R}\in {\mathbb{R}}^{n\times n}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}\text{\hspace{1em}(2-6)}$$

（4）旋转加平移

可以将三维空间的刚体运动分解为平移以及旋转运动，满足：

$${\mathbf{a}}^{\prime }=\mathbf{Ra}+\mathbf{t}\text{\hspace{1em}(2-7)}$$

（5）齐次坐标与旋转矩阵（参考机器人学中的坐标变换）

为便于表达，写成齐次形式

$$\left[\begin{array}{l}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]\stackrel{\text{ def }}{=}\mathbf{T}\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]\text{\hspace{1em}(2-8)}$$

其中，$\mathbf{T}$即为变换矩阵（注意左乘右乘区别）。

（绕静坐标系（世界坐标系）旋转即左乘，绕自身坐标系旋转即右乘）

这种形式的矩阵集合定义为特殊欧式群，即

$$\mathrm{SE}(3)=\left\{\mathbf{T}=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in \mathrm{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}\text{\hspace{1em}(2-9)}$$

同样地，逆方向的变换为：

$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{T}}& -{\mathbf{R}}^{\mathrm{T}}\mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(2-10)}$$

2.2 旋转向量和欧拉角

2.2.1 旋转向量

（1）三维空间中刚体运动有六个自由度（旋转和平移各三个）。显然无论是上述的旋转矩阵（9个量）还是变换矩阵（16个量），都有很大的冗余；并且，矩阵中的元素相互关联，这不利于后续的非线性优化计算。因此我们希望找到一种更为紧凑的表达方式。

（2）任意一个旋转都可以用旋转轴和旋转角刻画，因此可以用旋转向量（也称为角轴或轴角）来表达，即向量方向为旋转轴，其模长为旋转角。

（3）罗德里格斯公式描述了旋转矩阵和旋转向量之间的关系。

$$\mathbf{R}=\cos (\theta \mathbf{I})+\left(1-\cos \theta \right)\mathbf{n}{\mathbf{n}}^T+\sin (\theta {\mathbf{n}}^{\wedge })\text{\hspace{1em}(2-11)}$$

反之，有
$$\theta =arccos(\frac{tr(\mathbf{R})-1}{2})\text{\hspace{1em}(2-12)}$$
$tr(\mathbf{R})$表示求迹，即矩阵对角线元素之和。

由于旋转轴在旋转过程中是不动的，则有
$$\mathbf{R}\mathbf{n}=\mathbf{n}\text{\hspace{1em}(2-13)}$$
这说明$\mathbf{n}$是矩阵$\mathbf{R}$特征值为 1 对应的特征向量，由此可以求出向量$\mathbf{n}$的值。

2.2.2 欧拉角

（1）将旋转分解为 X、Y、Z 三个方向上的转动，常用的是 ZYX 顺序的旋转：

• 绕 Z 轴旋转，得到偏航角 yaw；
• 绕旋转后的 Y 轴旋转，得到俯仰角 pitch；
• 绕旋转后的 X 轴旋转，得到滚转角 roll。

此时，使用$[y,p,r{]}^T$这样一个三维向量便可以描述任意旋转。

（2）存在万向锁问题：即当俯仰角为 ±90° 时，第三次旋转轴和第一次旋转轴重合，使系统丢失了一个自由度，这被称为奇异性问题。因此欧拉角在 SLAM 中较少使用。

（3）旋转向量也存在奇异性，当 $\theta$超过 $2\pi$时会产生周期性，这时将其限定在$2\pi$范围内便可以避免。

2.3 四元数

2.3.1 四元数的定义

（1）2D 情况下，可用单位复数表达旋转，在复数平面中，结合欧拉公式，如下图，向量$\mathbf{z}$旋转 90° ，相当于乘上 $i$。

（2）类似的，在三维空间中，采用四元数描述旋转。四元数有一个实部，三个虚部（分别对应 x、y、z轴）。

$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k\text{\hspace{1em}(2-14)}$$

写成向量形式

$$\mathbf{q}=[s,\mathbf{v}],s=q_0\in \mathbb{R},\mathbf{v}=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3\text{\hspace{1em}(2-15)}$$

并且，虚部之间满足

$$\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}\text{\hspace{1em}(2-16)}$$

（3）当四元数的实部为 0 时，称为虚四元数（分别对应三维坐标），此时，可以用来表示三维空间中的点。

2.3.2 四元数的计算

• 加法：对应部分相加

• 乘法：每一项相乘，最后相加

• 模长：各项系数平方和再开方；并且两个四元数乘积的模等于各自模的乘积，即$||{\mathbf{q}}_1{\mathbf{q}}_2||=||{\mathbf{q}}_1||\cdot ||{\mathbf{q}}_1||$。

• 共轭：实部相等，虚部互为相反数：${\mathbf{q}}_{\mathbf{a}}^{\ast }=[s_a,-{\mathbf{v}}_{\mathbf{a}}{]}^T$。并且，${\mathbf{q}}^{\ast }\mathbf{q}=\mathbf{q}{\mathbf{q}}^{\ast }=[s^2+{\mathbf{v}}^{\mathbf{T}}\mathbf{v},0{]}^T$

• 逆：${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/{||\mathbf{q}||}^2$，则${\mathbf{q}}^{-1}\mathbf{q}=\mathbf{q}{\mathbf{q}}^{-1}=1$，同时，$({\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}{)}^{-1}={{\mathbf{q}}_{\mathbf{a}}}^{-1}{{\mathbf{q}}_{\mathbf{b}}}^{-1}$

• 数乘：$k\mathbf{q}=[ks,k\mathbf{v}{]}^T$

2.3.3 四元数表示旋转

（1）三维空间中任意点均可用一个纯虚四元数表示即$\mathbf{p}=[0,\mathbf{v}{]}^T$，经一个单位四元数$\mathbf{q}$的旋转后，得到${\mathbf{p}}^{\prime }$，则

$${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}\text{\hspace{1em}(2-17)}$$

最终${\mathbf{p}}^{\prime }$的虚部即为旋转后点的坐标。

2.3.4 四元数与其他旋转表示法的转换

• 角轴到四元数：

$$\mathbf{q}=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}]\text{\hspace{1em}(2-18)}$$

• 四元数到角轴
  $$\{\begin{array}{c}\theta =2arccos{q}_0\\ \\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}\text{\hspace{1em}(2-19)}$$

• 四元数到旋转矩阵

• 旋转矩阵到四元数

2.4 相似、仿射、射影变换

欧式变换不改变向量本身，只是进行旋转或平移。

（1）相似变换

相似变换比欧式变换多了一个自由度，即相当于在旋转或平移后，各坐标再进行等比例缩放，表达式为

$${\mathbf{T}}_s=\left[\begin{array}{ll}s\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(2-20)}$$
$s$为缩放因子。

（2）仿射变换

表达式如下：
$${\mathbf{T}}_A=\left[\begin{array}{ll}\mathbf{A}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(2-21)}$$

这里不要求$\mathbf{A}$为正交矩阵，因此，变换后，正方形就不是方的了，但仍是平行四边形。

（3）射影变换

$${\mathbf{T}}_P=\left[\begin{array}{ll}\mathbf{A}& \mathbf{t}\\ {\mathbf{a}}^{\mathrm{T}}& v\end{array}\right]\text{\hspace{1em}(2-22)}$$

射影变换是最一般的变换，左上角$\mathbf{A}$为可逆矩阵，右上角$\mathbf{t}$为平移，左下角为缩放${\mathbf{a}}^{\mathbf{T}}$。从真实世界到相机照片的变换可以看做是射影变换：原本正方形的地砖，在照片中将不再是方形，由于近大远小，甚至可能是不规则的四边形。