《视觉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}$

• 外积（叉乘）：

$$\begin{array}{rl}\mathbf{a}\times \mathbf{b}=\Vert \begin{array}{ccc}{\mathbf{e}}_1& {\mathbf{e}}_2& {\mathbf{e}}_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert & =(a_2{b}_3-a_3{b}_2){\mathbf{e}}_1+(a_3{b}_1-a_1{b}_3){\mathbf{e}}_2+(a_1{b}_3-a_3{b}_1){\mathbf{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]\mathbf{b}\stackrel{\text{ def }}{=}{\mathbf{a}}^{\wedge }\mathbf{b}\end{array}\text{\hspace{1em}(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}$

为便于后续表达，记

$${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\text{\hspace{1em}(2-2)}$$
这是一个反对称矩阵，满足$A^T=-A$。

2.1.2 坐标系变换

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

$$\mathbf{a}=\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{\mathbf{e}}_1+a_2{\mathbf{e}}_2+a_3{\mathbf{e}}_3\text{\hspace{1em}(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$，由于向量本身的绝对位置没有改变，则

$$\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\text{\hspace{1em}(2-4)}$$

将上式两端分别左乘$\left[\begin{array}{l}{\mathbf{e}}_1^{\mathbf{T}}\\ {\mathbf{e}}_2^T\\ {\mathbf{e}}_3^T\end{array}\right]$得到，

$$\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{lll}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{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 }}{=}\mathbf{R}{\mathbf{a}}^{\prime }\text{\hspace{1em}(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}}$。从真实世界到相机照片的变换可以看做是射影变换：原本正方形的地砖，在照片中将不再是方形，由于近大远小，甚至可能是不规则的四边形。