视觉SLAM——三维空间刚体运动

让我们带着疑问来开启三维空间刚体运动的学习吧！

为什么要学习三位刚体运动？三维刚体运动描述的是什么呢？
我们研究的机器人的位姿是需要利用三维刚体运动（假设为刚体）的规律来求解。
那什么是位姿呢？
就是指当前机器人坐标系相对世界坐标系的旋转与位移。
为什么是旋转与位移呢？
自己脑补去 = =

以下涉及的是旋转有关的知识。因为平移操作在数学上就相当于在对坐标系旋转后加上了一个平移向量(列向量)。

一、旋转矩阵

1.1坐标系间的欧式变换

设某个正交基(e1,e2,e3) 经过一次旋转变成了(e’1,e’2,e’3)，因此坐标系由1旋转到了2，但是对于一个向量α，它在两个坐标系下的向量本身没有改变，则可以得到：

为了描述两坐标系间的转换，等式两边同时左乘[e1, e2, e3]的转置，所以：

这个R就是我们所求的旋转矩阵。
同时，我们可以看到旋转矩阵R是一个行列式为1的正交矩阵。

1.2变换矩阵与齐次坐标

为什么会讲变换矩阵与齐次坐标呢？
因为我们平时研究机器人位姿的时候，不仅仅要考虑旋转，还要考虑平移，则可用下式表示 ：

t就是平移向量为3 x 1的列向量。
而现实研究中，我们不可能只考虑一次旋转和平移，机器人的运动是进行了多次旋转和平移，可用下式表达（先只选取2个）：


这样经过多次变换后会显得罗嗦，则我们引入齐次坐标和变换矩阵，可写为：

T就是变换矩阵，它将旋转和平移整合到一个矩阵里了。
在三维向量末尾加1，增加了一个维度，称为齐次坐标。
经过这样的叠加，连续地旋转就可以用线性的式子表达出了：

二、旋转向量

为什么要引入旋转向量呢？用旋转矩阵它不香吗~
不香 … …
旋转矩阵有以下几个缺点：

1. 有9个自由度，然而一次旋转只有3个自由度。因此这种表达方式比较冗余。
2. 旋转矩阵自带的约束，它必须是一个正交矩阵，且行列式为1.

因此，我们就想用一种紧凑的结构来表达旋转。那就是旋转向量。旋转向量由一个旋转轴和一个旋转角组成。旋转向量就是一个3x1的向量，方向与旋转轴方向一致，长度等于旋转角。这样看来我们只用3个自由度就表达了一个旋转了。是吧！

那我们开始好奇了，既然旋转向量也能表示旋转，那他的效果是不是要和旋转矩阵一样呢？也就是说旋转矩阵和旋转向量间是不是有互相转化的关系呢？
答案是肯定的。
旋转向量和旋转矩阵之间的转换过程由罗德里格斯公式表明：

符号^是向量到反对称的转换符。反之，我们可以得到旋转矩阵到旋转向量的转换，对转角θ，取两边的迹（求矩阵对角线元素之和）：


我们又可以发现一点：关于转轴n，在旋转后不发生改变，也就是

因此，转轴n是矩阵R特征值1对应的特征向量。

三、欧拉角

为什么要引入欧拉角呢？旋转矩阵和旋转向量它不香吗？
不香~ … 因为它俩不直观。

因此为了直观的表达，我们引入了欧拉角，它使用了三个直观的分离的转角来表达旋转。偏航角yaw，俯仰角pitch，滚转角roll，即对应着三个旋转轴ZYX。而且我们还特意定义了一下规则，按 “ 偏航 - 俯仰 - 滚转 ” 的顺序来表达旋转。

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

但是欧拉角有一个重大的缺陷，就是会碰到万向锁问题。在俯仰角为 $\pm 90◦$ 的时候 ，第一次旋转与第三次旋转将使用同一个轴，使得系统失去了一个自由度。这就是奇异性问题 。

四、四元数

旋转矩阵用9个量描述3个自由度，具有冗余性；旋转向量和欧拉角紧凑，但是有奇异性。

4.1 四元数的引入

回忆以前学习过的复数。我们用复数集C表示复平面上的向量，而复数的乘法则表示复平面上 的旋转：例如，乘上复数 i 相当于逆时针把一个复向量旋转 90◦。类似地，在表达三维空间旋转时， 也有一种类似于复数的代数：四元数（Quaternion）。四元数是Hamilton找到的一种扩展的复数。它 既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。
它拥有一个实部和三个虚部：

三个虚部满足以下关系：

如果把 i,j,k 看成三个坐标轴，那么它们与自己的乘法和复数一样，相互之间的乘法和外积一样。有 时人们也用一个标量和一个向量来表达四元数：

可以用单位四元数表示三维空间中任意一个旋转。

4.2四元数运算

设有两个四元数：

1. 加法和减法
   

2. 乘法
   乘法是把qa的每一项与qb的每项相乘，相加。
   
   在整理一下，可表达为：
   
   可以注意到，由于有外积的存在，所以四元数的乘法是不可交换的。

3. 模长
   
   

4. 共轭
   
   

5. 逆
   
   
   

6. 数乘
   

4.3用四元数表示旋转

我们可以用四元数表达对一个点的旋转。
首先，把三维空间点用一个虚四元数来描述：

相当于把四元数的3个虚部与空间的3个轴相对应。那么旋转后的点p‘ 可表示为：

这里的乘法均为四元数乘法，结果为虚四元数。

4.4四元数到其他旋转的转换（摘抄自高翔slam十四讲）

同理可证：

然后，考虑shi’yong四元数对空间点进行旋转的问题。根据前面的说法，有：

带入两个符号对应的矩阵，得：

因为 p 和 p’ 都是虚四元数，那该矩阵得右下角也就表示旋转矩阵和四元数的关系：

为了得到四元数到旋转向量的转换公式，对上式两侧求迹，得：

至于旋转轴，如果在式(3.38)中用 q 的虚部代替 p，易知 q 的虚部组成的向量在旋转时是不动的，即 构成旋转轴。于是只要将它除掉它的模长，即得。总而言之，四元数到旋转向量的转换公式可列写 如下：