【SLAM学习】（一）基础知识

基础知识篇

外积

外积大小为 $|a||b|sin<a,b>$，可以表示两个向量张成四边形的有向面积，引入符号，定义 $a$^表示为一个反对称矩阵，这样就把向量的外积变换成了矩阵和向量的乘法。

同时外积可以表示向量的旋转，在右手法则下，用右手的四个指头从 $a$ 转到 $b$ ，其大拇指所指向的方向就是旋转向量的方向，旋转向量的大小为 $a$ 与 $b$ 的夹角。

欧氏变换

在机器人的运动中，通常先固定一个坐标系，称为世界坐标系，如图中的 $x_w,y_w,z_w$ 定义的坐标系；同时相机也是一个坐标系，如图中的 $x_c,y_c,z_c$ 定义的坐标系。在相机视野中的某个向量 $p$ 坐标为 $p_c$ ，在世界坐标系下坐标则为 $p_w$ 。相机运动时，两个坐标之间的转换关系由变换矩阵 $T$ 来表示。

因为相机可以看作刚体，刚体运动时能保证同一个向量在各个坐标系下的长度和夹角不会发生变化，这种变换称作欧氏变换，一个欧式变换由一个旋转和一个平移两部分组成。

旋转矩阵

首先看旋转，设单位正交基 $(e_1,e_2,e_3)$ 经过一次旋转变成了 $(e_1^{\prime },e_2^{\prime },e_3^{\prime })$ ，对于同一个向量 $a$ ，它在两个坐标系下的的坐标分别为 $[a_1,a_2,a_3]$ 和 $[a_1^{\prime },a_2^{\prime },a_3^{\prime }]$ ，则有：

为了描述两个坐标之间的关系，对等式左右同乘 $[e_1^T,e_2^T,e_3^T{]}^T$，左式系数即变为单位矩阵，右式系数则变成由两组标准正交基的内积组成的矩阵：

称右式矩阵 $R$ 为旋转矩阵，旋转矩阵作用于向量 $a$ 的坐标得到向量 $a$ 旋转后的坐标。

仔细观察旋转矩阵 $R$ ，可以发现旋转矩阵为一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。

由于 $R$ 为正交矩阵，则 $R^T=R^{-1}$ ，所以 $R^T$ 可以描述一个相反的旋转：

欧式变换除了旋转还有平移，平移就简单多了，直接在旋转后的向量坐标上加一个平移向量 $t$ 就可以，把旋转和平移合在一起有：

通过旋转矩阵 $R$ 和平移向量 $t$ 可以完整描述一个欧氏变换。

变换矩阵和齐次坐标

虽然上式可以完整描述一个欧氏变换，但是这里的变换不是一个线性关系，当做两次变换时有：

那么用 $a$ 表述 $c$ 时则有：

可以想象在多次变换后式子会变得多么复杂，为了简化形式，引入了齐次坐标和变换矩阵：

给三维向量加一维变成四维向量，称为齐次坐标；把旋转和平移写在同一个矩阵里，称为变换矩阵。

首先介绍齐次坐标：

• 通过添加最后一维，用四个数表示一个三维向量，这样就多了一个自由度，多了一个自由度就可以写出线性的形式。

• 齐次坐标的每一个分量同时乘一个非零常数仍然表示同一个点，所以一个点的齐次坐标不唯一。

• 用齐次坐标运算得到的也是齐次坐标，将齐次坐标转换为非齐次坐标的方法为把所有坐标除以最后一项，强制最后一项为1，然后忽略最后一项即可得到欧氏空间中的非齐次坐标。

然后介绍变换矩阵：

• 变换矩阵左上角为旋转矩阵，右上角为平移向量，左下角为 $0$ 向量，右下角为1.
• 变换矩阵的逆也表示一个反向的变换

通过引入齐次坐标和旋转矩阵，就大大简化了欧氏变换的表示：

旋转向量

• 旋转矩阵用9个量来表示三个自由度的旋转，变换矩阵用16个量来表示6个自由度的欧式变换，这样会显得十分冗余。

• 旋转矩阵具有约束：它必须是一个行列式为1的正交矩阵，这使得估计或优化一个旋转矩阵时会使得求解变得困难。

其实最开始提到的外积就可以表示向量的旋转。坐标系的旋转可以由一个旋转轴和一个旋转角来刻画，我们选择方向与旋转轴相同、长度等于旋转角的向量，来描述旋转，称为旋转向量。易知旋转向量用三个量来表示三个自由度的旋转。

设旋转轴为 $n$ ，角度为 $\theta$ 的旋转用 $\theta n$ 来表示，则由旋转向量 $\theta n$ 到旋转矩阵的公式可由罗德里格斯公式得出：

反之也可得出从旋转矩阵到旋转向量的公式：

由于旋转轴上的向量在旋转后不发生改变，所以有:

​ $Rn=n$

因此转轴 $n$ 是旋转矩阵 $R$ 特征值为1对应的特征向量，所以求解方程可得旋转轴。

欧拉角

除了旋转矩阵和旋转向量，还可以用欧拉角来直观的表式旋转。

欧拉角使用三个分离的转角来表述旋转，分别为：

1. 绕物体的 Z 轴旋转，得到偏航角 yaw；

2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；

3. 绕旋转之后的 X 轴旋转，得到滚转角 roll。

这样我们就可以通过 $[r,p,y{]}^T$ 这样一个三维向量来描述任意旋转。

但是使用欧拉角会遇到著名的万向锁问题：当俯仰角 $pitch$ 为 $\pm 90°$ 时，第三次旋转与第一次旋转将会使用同一个轴，这样会使系统丢失一个自由度，称为奇异性问题。所以欧拉角不适用于插值和迭代，也很少用于SLAM程序，往往只用于人机交互中，方便直观理解。

四元数

旋转矩阵冗余，欧拉角奇异。事实上我们找不到不带奇异性的三维向量描述方式。比如，当使用两个坐标来表示地球表面时，如经纬度，当纬度为 $\pm 90°$ 时，经度没有意义。

回忆刚学过的复数，我们可以用复数集 $C$ 来表示复平面上的向量，乘上 $i$ 相当于把复向量旋转 $90$ 度。那么类似的，在表达三维空间的旋转时，也有一种类似于复数的代数：四元数。四元数是一种扩展复数，在表示旋转时具有紧凑性，缺点是运算时比较复杂。

一个四元数 $q$ 具有一个实部和三个虚部： $q=q_0+q_{1}i+q_{2}j+q_{3}k$，其中 $i,j,k$为三个虚部，且满足以下关系式：

有时也通过一个标量和一个向量来表达四元数：

$s$ 称为实部， $v$ 称为虚部。当虚部为0时称为实四元数，实部为0时称为虚四元数，虚四元数可以表示三维空间中的一个点。

为了满足 $ij=k$ 的关系，定义乘上一个虚部对应旋转180度。

四元数和旋转向量之间的转换关系为：

和

第一个式子中当 $\theta$ 加上 $2\pi$ 时，易得旋转没有改变，但是得到的四元数 $q$ 却变成了 $-q$ 。因此两个互为相反数的四元数表示着同一个旋转。

---

用四元数表示点的旋转时，设空间中一点 $p=[0,x,y,z]$ ；

若要表示点绕转轴 $n$ 旋转 $\theta$ ，由上式得 $q=[cos\frac{\theta }{2},nsin\frac{\theta }{2}]$ ；

旋转后的 $p^{,}=qp{q}^{-1}$ ，可验证得 $p^{,}$ 也是纯虚数，表示三维空间中的点。

---

最后给出四元数到旋转矩阵的转换公式，设四元数 $q=q_o+q_{1}i+q_{2}j+q_{3}k$，则有：

反之有：