【视觉SLAM十四讲】第3讲 三维空间刚体运动

3.1 旋转矩阵

首先这一章的题目是三维空间的刚体运动，所谓刚体，是不仅考虑位置还考虑姿态的物体，这要区别于以前立体几何的各种物体，以前的各种物体都是不考虑姿态的，一个位置就解决全部问题，现在加上姿态以后，物体就变成了刚体。对于一个相机而言，位置是指相机在空间中的哪个地方，而姿态则是指相机的朝向。

学习高数的时候有一部分上课讲过但是考研不考，是关于线性空间的基的知识，线性空间的基可以看作是一组向量的集合，其中每两个向量都互相垂直，对于一个三维空间，可以用一个基表示一个坐标系，需要三个向量表示坐标轴的指向，如果拿来多个基，那就是三维空间中的多个坐标系，在ROS的tf中也有讲过，不同坐标系下的位置是不同的，所以同一个向量，大小和方向一样，但是放在不同的坐标系下表示的内容是不同的，所以引入基的概念，用下面的方式表示向量在一组基下的坐标：

从式子里不难看出，一个坐标的具体取值，一方面和向量本身有关，另一方面和所处的坐标系有关。一般的坐标系都是由三个正交的坐标轴组成，可以用右手系定义。

这里补充一下内积和外积的知识：
①内积
内积本身还是一个数，可以用来表征两个向量之间的夹角以及一个向量在另一个向量上的投影，三维空间下的内积可以写作：

②外积
外积仍然是一个向量，向量的方向与两个原始向量的坐标平面垂直，即法向量的方向，而大小与两个向量的长度有关，大小为|a||b|sin<a,b>

图里这种写法是一种简化的版本，因为两个向量直接叉乘写起来不方便计算，所以将结果倒着转换回去，把向量a换成另一个形式，向量b保持不变，这样的新形式更加方便计算。

引入外积最大的好处是可以表示向量的旋转，向量a旋转到向量b的位置，可以用外积，也就是法向量表示旋转轴的方向，用移动前后两个向量的外积就可以直接计算出法向量的方向，从而可以用右手表示出整个旋转的过程，此外还可以用外积的值去衡量旋转的幅度。

向量的旋转之后，是坐标系的旋转，旋转加上平移，就可以组成坐标系之间的变换关系。在机器人运动过程中，常见的做法是设定一个惯性坐标系，也可以叫世界坐标系，这个坐标系是固定不动的，也就是一般说的上帝视角，而对于每个机器人或者相机而言，都会有一个单独的移动坐标系，也就是机器人视角下看到东西的坐标系。机器人视角并不一定是和上帝视角完全重合的，所以需要有一个坐标转换，这个转换可以是tf转换，在SLAM里面用高数里面的基的相关知识去表示坐标系的变换。

相机的运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化，这种变换称为欧式变换，一个欧式变换由旋转和平移两部分组成。旋转比平移要麻烦的多，首先我们用一个单位正交基来表示一个坐标系，单位正交基是一组向量，其中任意两个向量都是正交的，对于三维空间而言，三维空间下的单位正交基是由三个向量组成的，其中任意两个垂直，且长度都为1，也就是说用一个单位正交基表示了三维坐标下的坐标系。
对于一个坐标系，经过了一次旋转，变成另一个坐标系，在每个坐标系下都还有一个向量a，于是得到下面的关系：

等式左侧是向量a在旋转前坐标系下的坐标，等式右侧是向量a在旋转后坐标系下的坐标，在这个等式两侧同时乘以转制组成的基，得到下面的式子：

这个过程的推导就是上面的这张图，但是在第二行到第三行的过程中存在一些小疑问，想要凑出单位矩阵E，那就要求e1的转置乘以e1的结果是一个实数1，那也就是说e1的转置的形状应该是1×3，e1的形状是3×1，这样结果才是1，但是一般的写法应该是e1是1×3，e1的转置是3×1，正好反了过来。将红色框里面的内容单独拿出来定义成一个矩阵R，这个矩阵就是旋转矩阵，可以从最后一个式子里面看出来，向量a‘经过旋转矩阵的处理得到了向量a，所以这个矩阵R就是对坐标系旋转的描述。

旋转矩阵由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么旋转矩阵也是一样的，也就是说旋转矩阵R表示的是旋转的这个过程，与具体的矩阵无关。此外，旋转矩阵是一个行列式为1的正交矩阵（矩阵乘以矩阵的转置等于单位矩阵E，即矩阵的转置等于矩阵的逆），反之，行列式为1的正交矩阵也是一个旋转矩阵，于是有下面的定义：

通过旋转矩阵，就可以直接谈论两个坐标系之间的旋转变换，而不用从基开始弹起。因为旋转矩阵是正交矩阵，正交矩阵必定可逆，所以式子反过来就有了：

显然R的转置刻画了一个相反的旋转，拿实际意义来看的话，向量a‘可以经过R变换为a，那么反过来a也可以经过R的转置（逆）变为a’。

平移就要简单得多，直接在向量基础上加一个向量就可以表示平移，加的这个向量称为平移向量。将旋转和平移结合在一起，得到的就是欧式变换的式子：

这个式子完整地表示了欧式空间的旋转和平移，但是还是存在一个小问题，就是这个变换不是一个线性关系，如果同一个向量进行两次变换，也就是嵌套，那么形式会变得很麻烦，比如下面这样：

向量a经过R1和R2两次变换得到c，但是写法上来看会很别扭，嵌套的内容太多，仅仅是两次变换就已经这么复杂了，那么次数更多的情况必然写起来更加麻烦，所以引入齐次坐标和变换矩阵的重写式：

这算是一个数学技巧，在向量末尾加一个1，变成四维向量，称为齐次坐标，对于这个四维向量，就可以将旋转和平移写在同一个矩阵里面，从而变成了线性关系，此时称T为变换矩阵，这个变换矩阵左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1，与前面一样，变换矩阵也可以用逆矩阵表示反向的计算，只不过形式变得有点麻烦：

此外，对于添加了一维的三维向量，增加的一维相当于增加了一个自由度。在其次坐标中，一个点的每个分量同时乘以有一个相同的非零常数k之后，仍然表示同一个点，所以一个点的具体坐标值是不确定的，如果把所有的坐标都除以最后一项，也就是强制最后一项为1，此时才能得到一个唯一的坐标表示：

此时前面三维的坐标刚好就是增加之前的坐标，直接删去最后的1，就能得到三维下的坐标。利用这种方式，两次变换的累加就可以直接写成变换矩阵的连乘：

3.2 旋转向量和欧拉角

前面提到的旋转矩阵已经可以表示旋转，但是旋转矩阵存在一定的不足，一方面旋转矩阵有9个量，但是一次旋转只有三个自由度，所以存在冗余，如果是变换矩阵，则是用16个量表示了6个自由度的变换，冗余更多，所以需要一个更加紧凑的表示方法，此外旋转矩阵要求必须是行列式为1的正交矩阵，本身也存在约束。

因此引入了一种更加简单的旋转表示方法，利用外积的方法，用一个旋转轴和一个旋转角来表示一个旋转，这里使用一个向量，向量的方向与旋转轴一致，而长度等于旋转角，这种向量就是旋转向量，这种方法是需要使用一个三维向量即可，如果要表示变换则再补上一个平移向量即可。
假设一个旋转轴为向量n，进行角度为Θ的旋转，则它的旋转向量为Θn，旋转矩阵则可以根据罗德里格斯公式写出：

反之，从旋转矩阵到旋转向量也可以进行，对于旋转角Θ，有：

而转轴n为矩阵R特征值1对应的特征向量。

旋转矩阵和旋转向量虽然可以表示旋转，但是对于人类而言是十分抽象的，于是又引入了一种更简单的方法：欧拉角。欧拉角使用了三个分离的转角，将一个旋转表示成三次绕不同的轴的旋转，即使用“偏航-俯仰-滚转”三个角度来表示一个旋转，因为旋转的顺序不固定，所以会有很多种旋转角的表示方法，而最常用的是ZYX的旋转顺序，ZYX转角相当于把任意旋转分解为下面的三个角度：

于是可以使用任意一个三维向量去描述旋转，而且可以十分直观地表述出旋转的过程。

欧拉角最大的缺点是会碰到万向锁问题，即在俯仰角为±90°时，第一次旋转和第三次旋转会使用同一个轴，这会导致系统丢失一个自由度，这被称为奇异性问题，本身还是因为在第二次的旋转过程中，第三次的轴被旋转到了第一次轴的位置，而这个重叠是没法处理的。由于这个问题，欧拉角并不适合插值和迭代，一般用于人机交互。

3.3 四元数

上一部分提到的两种表示形式，虽然都可以表示旋转，但是各自都存在不足，旋转矩阵存在冗余，欧拉角和旋转矩阵存在奇异性问题，都是不完美的，所以最终引入了一个几乎完美的表示方法-四元数。四元数是一种扩展的复数，紧凑而且没有奇异性，如果要说缺点的话，四元数不够直观，运算稍微麻烦一些。

四元数是简单的超复数。复数是由实数加上虚数单位i组成，其中i²= -1。相似地，四元数都是由实数加上三个虚数单位i、j和k组成，而且它们有如下的关系：i²=j²=k²=-1，iº=jº=kº=1，每个四元数都是1、i、j和k的线性组合，四元数一般可表示为a+bi+cj+dk，其中a、b、c、d是实数。对于i、j和k本身的几何意义可以理解为一种旋转，其中i旋转代表Z轴与Y轴相交平面中Z轴正向向Y轴正向的旋转，j旋转代表X轴与Z轴相交平面中X轴正向向Z轴正向的旋转，k旋转代表Y轴与X轴相交平面中Y轴正向向X轴正向的旋转，-i、-j、-k分别代表i、j、k旋转的反向旋转。

如果将实数和复数的部分拆开，可以得到下面的形式：

其中s记为实部，向量v记为虚部，如果一个四元数虚部为0向量，那么称这个四元数为实四元数，如果一个四元数实部为0，那么称这个四元数为虚四元数。四元数实际上和虚数很像，区别在于它有三个虚的部分，具体的四元数的理论很复杂，我们需要记住的是，单位四元数能够表达三维空间的旋转，而这个旋转也可以用旋转矩阵、旋转向量表示。

假设某个旋转绕着向量n=（nx,ny,nz）进行了角度为Θ的旋转，那么旋转的四元数形式为：

按照这个转换关系，当旋转角变为0的时候，整个虚部都变成0，这个四元数变成一个实四元数。另外，对式中的Θ加上2π，得到一个相同的旋转，但是四元数变成了-q，因此任意的旋转都可以有互为相反数的四元数表示。
同理，从单位四元数中也可以提取出旋转轴和夹角：

四元数也可以进行运算，可以进行的运算如下：
①加减法
加减法最简单，直接对应位置上相加即可：

②乘法
这里的乘法还不同于数乘点乘，这个乘法写法是直接连在一起，将两个四元数的每一项都相乘，最后相加：

③共轭
也可以看作一种特殊的取反，将虚部全部取成相反数：

④模长
可以看作求距离的变形：

两个四元数乘积的模即为模的乘积。
⑤逆
一个四元数的逆为：

四元数和自己的逆相乘，结果是实四元数的1。
⑥数乘和点乘
数乘就简单得多，直接将四元数的每部分乘以相同的数即可，而点乘则是四元数上每个位置对应相乘：


现在就可以用一个四元数表示旋转，假设一个空间三维点p=[x,y,z]，进行一个由轴角n，Θ指定的旋转，使用旋转矩阵的话应该是p’=Rp，如果是四元数表示，首先用一个虚四元数表示三维空间点，让三个虚部与空间的三个轴对应，之后利用前面的式子，将旋转轴和旋转角表示成四元数的形式，记作q，那么旋转后的结果为p’=qpq(逆)，计算的结果是一个纯虚四元数，三个分量表示旋转后3D点的坐标。

既然都可以表示旋转，那么四元数和旋转矩阵之间也可以转换，对于四元数q=q0+q1i+q2j+q3k对应的旋转矩阵为：

反过来矩阵到四元数的转换为：

最后做一个小总结，这一章实际上是介绍了四种表示旋转的方法：旋转矩阵、旋转向量、欧拉角和四元数。
旋转矩阵是最原始的方法，用基与向量的关系建立等式，之后消去一侧总结出来一个矩阵称为旋转矩阵，这个矩阵的进阶是变换矩阵，将平移也整合了进去。虽然能够表示旋转，但是一方面很抽象，人无法直观地想象，另一方面一个旋转矩阵是9个数，而旋转的自由度是3，所以存在冗余。
旋转向量是用向量去表示旋转，利用外积的思想，让旋转向量的方向正好对应旋转轴的方向，而旋转的角度刚好等于旋转向量的大小，这样的向量就称为旋转向量。旋转向量消去了冗余，但是存在奇异性问题，并且也很抽象。
欧拉角是最好理解的一种方法，将一个旋转拆分成三个方向上的三次旋转，但是这种方法存在奇异性问题，一般用在人机交互中。
四元数是理论上最优的选择，但是理解起来很麻烦，计算上有一些复杂，但是表示旋转的能力最好，没有冗余而且不存在奇异性问题。

关于旋转矩阵、欧拉角和四元数等内容的转换，见下面的博客

https://zhuanlan.zhihu.com/p/45404840
https://zhuanlan.zhihu.com/p/79894982