视觉SLAM十四讲 3-三维空间刚体运动

蓝色
紫色
红色

一、前言

  本章简要介绍 三维世界中刚体 运动的描述方式：旋转矩阵、旋转向量、欧拉角、四元数等。

二、SLAM问题的数学表述

用 数学语言 来描述：小萝卜正携带着某种 传感器 在未知环境里 运动

相机：在 某些时刻 采集数据   连续时间的运动 => 离散时刻

对于小萝卜：$t=1,...,K$ 时刻，小萝卜在各个时刻的位置：$x_1,...,x_K$   => 小萝卜的轨迹

对于地图：设置许多路标 (Landmark），每个时刻，传感器会测量到一部分路标点，得到他们的观测数据。

N 个路标点：$y_1,...,y_N$

运动：从 k = 1时刻 到 k时刻，小萝卜的 位置x 是如何变化的。

机器人会携带一个 测量自身运动 的传感器（eg.码盘或惯性传感器），这个传感器可以测量有关运动的读数，但不一定直接是 位置之差，还可能是 加速度、角速度 等信息。抽象成数学模型：(运动方程）

$u_k$ 是运动传感器的读数（输入），$w_k$ 是噪声。

观测：假设小萝卜在 k时刻，于 $x_k$ 处探测到了某一个路标 $y_j$，如何用数学语言描述。

观测方程： 描述当小萝卜在 $x_k$ 位置上看到某个路标点 $y_j$ ，产生了一个观测数据 $z_{k,j}$ 。$v_{k,j}$ 是观测里的噪声。

参数化   Parameterization

位姿：位置+姿态

如果小萝卜在 平面 中运动，那么它的位姿由 两个位置 和 一个转角 来描述，即 $x_k=[x,y,\theta {]}_k^T$ 。同时，运动传感器能够测量到小萝卜在每两个时间 间隔位置和转角的变化量 $u_k=[\Delta x,\Delta y,\Delta \theta {]}_k^T$，则运动方程可具体化为：

上述是简单的 线性关系 。但并不是所有的传感器都直接能测量出 位移和角度变化 ，所以会有更复杂的运动方程，需要进行动力学分析。

关于观测方程，比方说小萝卜携带着一个 二维激光传感器。我们知道激光传感器观测一个2D 路标点时，能够测到两个量：路标点与小萝卜本体之间的 距离r 和 夹角ϕ 。我们记 路标点 为 $y=[p_x,p_y{]}^T$（为保持简洁，省略了下标），观测数据 为 $z=[r,\varphi {]}^T$ ，那么观测方程就具体化为：

视觉SLAM的问题表述

视觉SLAM：传感器是 相机。

观测方程就是 “对路标点拍摄后，得到了图像中的像素” 的过程。针对不同的传感器，这两个方程有不同的参数化形式。

如果我们保持通用性，把它们取成通用的抽象形式。则SLAM 过程可总结为两个基本方程：

这两个方程描述了最基本的SLAM 问题：当我们知道运动测量的读数u，以及传感器的读数z 时，如何求解定位问题（估计 位置x ）和建图问题（估计 路标y ）？

SLAM问题 建模成 状态估计问题 ［通过带有噪声的测量数据，估计内部的、隐藏着的 状态变量]

三、线性代数相关知识

1. 点和向量、坐标系

刚体：内部任意两点间距离保持不变，不易发生形变的物体。

刚体变换：由旋转和平移组成

存在疑惑：刚体配准和非刚体配准区别？

在西电的一篇硕士论文第五章里看到这样的表述：

“非刚性物体的图像配准比刚体物体的复杂，因为非刚性物体图像配准中的变换必须考虑 非线性形变 ，这种形变可能是全局的也可能是局部的，也可能二者都有。”

三维空间由3个轴组成，所以空间点的位置可由3个坐标 [x,y,z] 指定。 注意区别 坐标 和 向量

向量只有在指定坐标系之后才可以谈论该向量在 此坐标系下的坐标，即找到 若干个实数 对应这个向量。

当指定一个坐标系，即给定一个线性空间的基 $(e_1,e_2,e_3)$，向量a 在这组基下的坐标：

$a,b\in R^3$

• 向量内积 描述向量间的投影关系：
  
• 向量外积 可表示向量的旋转：两个向量张成的四边形的有向面积，大小为 |a||b|sin<a,b>，方向垂直于这两个向量构成的平面
  
  把 a 写成反对称矩阵(Skew-symmetric)，变成 a^b (线性运算)

a 旋转到 b，可以由旋转向量 w 来描述，w 即是 a×b，其大小由 a 和 b 的夹角大小决定。

2. 坐标系间的欧氏变换

坐标系之间的变换关系：旋转和平移

惯性坐标系 / 世界坐标系    => 固定不动   $x_W,y_W,z_W$

移动坐标系            => 相机或机器人   $x_C,y_C,z_C$

相机视野中某个向量 $\mathbf{p}$ ，它的坐标为 ${\mathbf{p}}_{\mathbf{c}}$ ，而从世界坐标系下看，它的坐标 ${\mathbf{p}}_{\mathbf{w}}$ 。这两个坐标之间的转换过程如下：

1. 该点在相机坐标系下的 坐标值
2. 根据 相机位姿 把坐标转换到世界坐标系中
3. 该坐标变换由 矩阵 T 来描述
   

四、旋转的表达

欧式变换：同一个向量在各个坐标系下的长度和夹角都不会发生变化 平移和旋转

1. 旋转矩阵

设某个单位正交基 $(e_1,e_2,e_3)$ 经过一次旋转，变成了 $(e_1^{\prime },e_2^{\prime },e_3^{\prime })$ 。那么，对于同一个向量 a（该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 $[a_1,a_2,a_3{]}^T$ 和 $[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$ 。

为了描述两个坐标之间的关系，上式左右同时左乘 $\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$ :

旋转矩阵 R：描述旋转本身 由两组基之间的 内积 组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。

性质：旋转矩阵 ⇔ 行列式为1的正交矩阵

正交矩阵：$A^T=A^{-1}$

定义：设A是一个n 阶方阵，如果有 $A{A}^T=I$ ，则A是正交矩阵。

特殊正交群（Special Orthogonal Group）$SO(n)$

旋转矩阵的集合定义：

$SO(3)$ 表示三维空间的旋转。

相反的旋转

由于 旋转矩阵R 是正交矩阵，它的 逆 $R^{-1}$ / 转置 $R^T$ 描述了一个相反的旋转。

完整的欧式空间坐标变换

考虑世界坐标系中的向量 a，经过一次旋转（用 R 描述）和一次平移 t 后，得到了 a‘ ，那么把旋转和平移合到一起，有：

但是这样的变换关系不是一个 线性关系 ，多次变换之后会过于复杂。

引入 齐次坐标和变换矩阵T 重写式 (3.8)：

两次变换的累加 变为：

特殊欧氏群（Special Euclidean Group） $SE(n)$

与 $SO(3)$ 一样，求解 该矩阵的逆 表示一个反向的变换：

2. 旋转向量

变换矩阵 $T$  $DoF(T)=6$   (1+1)*3 三个维度上的旋转+平移

旋转矩阵 $R$  $DoF(R)=3$   1*3

矩阵表示法的缺点：

1. $SO(3)$ 的旋转矩阵有 九个量 $(3\ast 3)$ ，但一次旋转只有 三个自由度 。变换矩阵用 十六个量 $(4\ast 4)$ 表达了 六自由度 的变换。  冗余
2. 旋转矩阵自身带有 约束：它必须是个正交矩阵，且行列式为 1。  估计或优化一个 R/T 时，求解变得更困难

在前面我们注意到 外积可以表达两个向量的旋转关系，所以可以用一个三维向量表达旋转，六维向量表达变换。

任意旋转都可以用 一个旋转轴和一个旋转角 来刻画。

旋转向量（轴角， Axis-Angle）：其方向与旋转轴一致，长度等于旋转角。

对于变换矩阵 $T$，我们使用 一个旋转向量和一个平移向量 即可表达一次变换，这时的维数正好是六维。

旋转向量和旋转矩阵的相互转换

旋转向量 => 旋转矩阵

罗德里格斯公式（Rodrigues’s Formula ）：

符号∧ 是向量到反对称矩阵的转换符。

旋转矩阵 => 旋转向量

对于转角θ：

对于转轴 n，旋转轴上的向量旋转后不发生改变： 旋转轴经过旋转之后不变
$$\mathbf{R}\mathbf{n}=\mathbf{n}$$

转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

3. 欧拉角

欧拉角：使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。根据绕轴旋转的先后顺序不同一般分为：XYZ、ZYZ、ZYX等。还需要考虑绕 固定轴 旋转还是 绕旋转之后的轴 旋转。

欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”（yaw-pitch-roll）三个角度来描述一个旋转的。 ⇔ ZYX 轴的旋转   rpy角  $[r,p,y{]}^T$

1. 绕物体的 Z 轴旋转，得到偏航角 yaw
2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch
3. 绕旋转之后的 X 轴旋转，得到滚转角 roll

万向锁： 奇异性问题

在俯仰角为 $\pm 90°$ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。

理论上，用三个实数来表达三维旋转时，都会不可避免地碰到奇异性问题，所以欧拉角不适于插值和迭代，且一般不用于表达姿态，也不会在滤波或优化中使用。

4. 四元数  Quaternion

三维旋转是一个三维流形，想要无奇异性地表达它，三个量不够。

复数思想：

复数集 $C$ 表示复平面上的向量，复数的乘法则 能表示复平面上的旋转。eg. 乘上复数 i 相当于逆时针把一个复向量旋转 90° 。

四元数  紧凑且无奇异性  1个实部&3个虚部

一个模长为 1 的复数，可以表示复平面上的纯旋转（没有长度的缩放）⇒ 单位四元数表示三维空间中任意一个旋转

四元数中：

乘以 $i$ 应该对应着旋转 180° 这样才能保证 $ij=k$ 的性质。

$i^2=-1$ ，意味着绕 $i$ 轴旋转 360° 后，得到了一个相反的东西。旋转 2*360° 才能得到和原来一样的向量。

1) 四元数和旋转向量的关系

旋转向量 => 四元数

设某个旋转是绕单位向量 $\mathbf{n}=[n_x,n_y.n_z{]}^T$ 进行了角度为θ的旋转，则该旋转的四元数形式为：

在上式中，$\theta +2\pi$ 得到一个相同的旋转，但此时四元数变成了 $-\mathbf{q}$

⇒ 任意的旋转都可以由两个 互为相反数的四元数 表示

$\theta =0$ ，则得到一个没有任何旋转的实四元数 ${\mathbf{q}}_0=[\pm 1,0,0,0{]}^T$

四元数 => 旋转向量

2) 四元数表示旋转

设空间三维点 $\mathbf{p}=[x,y,z]\in R^3$ ，进行一个由轴角 $\mathbf{n},\theta$ 指定的旋转

矩阵形式： ${\mathbf{p}}^{\prime }=\mathbf{R}\mathbf{p}$

四元数形式：

1. 将三维空间点用一个虚四元数表达：
   
2. 把四元数的三个虚部与空间中的三个轴相对应，参照(3.19)用四元数 $\mathbf{q}$表示旋转：
   
3. 旋转后的三维空间点的四元数表达：
   
   计算结果的实部为 0，故为 纯虚四元数 ，其虚部的三个分量表示旋转后 3D 点的坐标。

3) 四元数和旋转矩阵的相互转换

四元数 => 旋转矩阵

直观方法：四元数q ⇒ 轴角 θ 和 n ⇒ 罗德里格斯公式转换为 R
（缺点：计算 arccos 代价较大）

四元数到旋转矩阵的转换方式：

旋转矩阵 => 四元数

假设矩阵为 R = { m i j } , i , j ∈ [ 1 , 2 , 3 ] R = \left \{m_{ij} \right \} , i, j ∈ [1, 2, 3] R={mij​},i,j∈[1,2,3] ，其对应的四元数 $q$ 由下式给出：

事实上一个 $\mathbf{R}$ 对应的四元数表示并不是唯一的，$\mathbf{q}$ 和 $-\mathbf{q}$ 表示同一个旋转。

4) 四元数的运算

  写成向量形式:

五、相似、放射、射影变换

1. 欧氏变换保持了向量的长度和夹角，相当于把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。
2. 相似变换允许物体进行均匀的缩放（缩放因子 s），不再保持图形的面积不变。
3. 仿射变换只要求 A 是一个 可逆矩阵 ，而不必是正交矩阵。平行四边形保持。
4. 射影变换 v≠0时，可对矩阵进行归一化，DoF(2D)=8，DoF(3D)=15

从真实世界到相机照片的变换是一个 射影变换 。如果相机的 焦距为无穷远 ，那么这个变换则为 仿射变换 。