【SLAM笔记】三维刚体运动

SLAM笔记专栏：https://blog.csdn.net/weixin_44543463/category_10925276.html

---

一、旋转矩阵

1.1 向量

  向量与坐标是两个不同的概念。向量只是有大小和方向的量。只有当指定了三维空间中某个坐标系的时候，才能谈论向量在此坐标系下的坐标。因此向量的坐标，既和向量本身有关，也和坐标系的选取有关。
  向量的内积描述了向量之间的投影关系。

  向量的外积，方向垂直于这两个向量，大小为|a||b|sin<a,b>。

1.2 欧氏变换

  两个坐标系之间的旋转、平移关系，统称为坐标系之间的变换关系。机器人运动过程中，往往会设定一个惯性坐标系（即世界坐标系），可以认为它是固定不动的。机器人则是一个移动坐标系。
  如果需要知道某个向量在机器人坐标系中与世界坐标系中如何转换的，就需要先得到该向量对机器人坐标系的坐标值，再根据机器人位姿转换到世界坐标系中，这个转换关系用矩阵T来描述。
  机器人移动是一个刚体运动，即同一个向量在各个坐标系下的长度和夹角都不会发生变化，这种变化就是欧式变换。

（1）旋转矩阵
  假设某个单位正交基(${\mathbf{e}}_1$,${\mathbf{e}}_2$,${\mathbf{e}}_3$)经过了一次旋转，变成了(${{\mathbf{e}}_1}^{\prime }$,${{\mathbf{e}}_2}^{\prime }$,${{\mathbf{e}}_3}^{\prime }$)，则对于同一个向量$\mathbf{a}$（该向量没有随坐标系旋转而运动），则可知

用矩阵表示如下

  为了表示两个坐标之间的变换关系，等式两边同乘[${{\mathbf{e}}_1}^T$ ${{\mathbf{e}}_2}^T$ ${{\mathbf{e}}_3}^T$]T，则左侧矩阵变成了单位矩阵：

  中间这个行列式为1的正交矩阵，就是所谓的旋转矩阵。
  同时，此旋转矩阵的逆描述了一个相反的旋转。

（2）平移矩阵
  平移矩阵十分简单，只需要将平移量加到旋转之后的坐标上就可以了。

1.3 变换矩阵

  使用下面这种形式进行变换时，变化多次之后往往会变得过于复杂且不满足线性关系。
  因此通常我们会使用如下的齐次坐标和变换矩阵进行变换。即把三维向量末尾加一，变成四维向量，称为齐次坐标$\tilde{a}$。将旋转矩阵和平移矩阵写在同一个矩阵里面，这个矩阵T称为变换矩阵。
  引入齐次坐标就可以实现多个变换矩阵的连乘，得到一个总的变换矩阵，实现多次变换的累加。

二、角轴和欧拉角

2.1 问题提出

  旋转矩阵有九个量，但一次旋转只有三个自由度，变换矩阵用十六个量表达六自由度变换，表达方式可能冗余。同时旋转矩阵本身要求必须是正交矩阵，变换矩阵一样都需要约束条件，有些情况下这些约束会使求解变得困难。

2.2 角轴

（1）定义
  任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们可以使用一个向量，其方向与旋转轴一致，长度等于旋转角，这种向量就称为旋转向量或角轴。
  使用角轴表示方法只需要一个三维向量即可描述旋转。同样对于变换矩阵，我们使用一个角轴和一个平移向量即可表达。
（2）角轴与旋转矩阵的转换
  假设有一个旋转轴为$\mathbf{n}$，角度为θ的旋转，显然旋转向量为θ$\mathbf{n}$。由角轴转化为旋转矩阵，可以使用罗德里格斯公式：

  同样也可以计算从旋转矩阵到角轴的转换。（由于旋转轴上的向量旋转后不发生变化，因此旋转轴就是旋转矩阵$\mathbf{R}$的特征值1对应的特征向量）。

2.3 欧拉角

  旋转矩阵和角轴都不太直观，而欧拉角的表达方式比较利于人的理解。（但是在程序中不常用）
  欧拉将将旋转分解为三次不同轴上的转动，例如按Z-Y-X顺序转动，可以得到yaw-pitch-roll角。

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

  欧拉角存在万向锁的问题，例如在ZYX顺序中，第一次绕Z轴旋转，第二次绕Y轴旋转90°，这时候x轴和系统初始时的Z轴重合了，导致第三次旋转和第一次是绕同一个轴旋转，丢失了一个自由度。所以程序中很少用欧拉角表示机器人的位姿。

三、四元数

3.1 简介

  四元数是一种扩展的复数。我们知道复数可以表示复平面内的旋转，乘i表示复平面内逆时针转90度，单位圆上的复数可以表达二维平面的旋转。
  四元数有三个虚部，可以表达三维空间中的旋转。


  四元数的虚部相乘，类似于虚数i的相乘，也有对应的关系，且其关系很想三维空间中的叉积。

3.2 四元数的运算

运算	公式
加减法
乘法
共轭
模长
逆
数乘
点乘

3.3 用四元数表示旋转

（1）四元数与角轴、旋转矩阵的转换

转换	公式
角轴到四元数：
四元数到角轴
四元数到旋转矩阵

（2）使用四元数表示旋转
  四元数有三个虚部i, j, k，将三个坐标值作为三个虚部的系数，另实部为零，这样就将一个三维空间坐标转换为纯虚四元数表示的坐标。

  用一个四元数$\mathbf{q}$表示旋转。

  空间点的旋转可以用四元数的乘法表示，旋转后的点${\mathbf{p}}^{\prime }$如下

参考：高翔-视觉SLAM十四讲
相关Github：https://github.com/gaoxiang12/slambook