通俗易懂！视觉slam第七部分——四元数

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式 [19]。这有点类似于，当我们想用两个坐标表示地球表面时（如经度和纬度），必定存在奇异性（纬度为 ±90 ◦ 时经度无意义）。三维旋转是一个三维流形，想要无奇异性地表达它，用三个量是不够的。

在表达三维空间旋转时，也有一种类似于复数的代数：四元数（Quaternion）。四元数是 Hamilton 找到的一种扩展的复数. 它既是紧凑的，也没有奇异性。

一个四元数 q 拥有一个实部和三个虚部。

其中 i,j,k 为四元数的三个虚部。这三个虚部满足关系式：

由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：

这和复数非常相似。考虑到三维空间需要三个轴，四元数也有三个虚部，那么，一个虚四元数能不能对应到一个空间点呢？事实上我们就是这样做的。同理，我们知道一个模长为 1 的复数，可以表示复平面上的纯旋转（没有长度的缩放），那么，三维空间中的旋转是否能用单位四元数表达呢？答案也是肯定的。 

我们知道单位四元数能够表达三维空间的旋转。这种表达方式和旋转矩阵、旋转向量有什么关系呢？我们不妨先来看旋转向量。假设某个旋转是绕单位向量进行了角度为 θ 的旋转，那么这个旋转的四元数形式为：

反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：

在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取 θ 为 0，则得到一个没有任何旋转的实四元数： 

运算 

 四元数和通常复数一样，可以进行一系列的运算。常见的有四则运算、数乘、求逆、共轭等等。

两个四元数为：

1. 加法和减法
四元数 的加减运算为：

 2.乘法

 3.共轭

四元数的共轭是把虚部取成相反数：

4.模

5.逆

一个四元数的逆为： 

四元数和自己的逆的乘积为实四元数的 1：

6.数乘与点乘 

用四元数表示旋转

我们可以用四元数表达对一个点的旋转。假设一个空间三维点 ，以及一个由轴角 n,θ 指定的旋转。三维点 p 经过旋转之后变成为 p ′ 。如果使用矩阵描述，那么有 p ′ = Rp。

如果用四元数描述旋转，把三维空间点用一个虚四元数来描述:

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后用四元数 q 表示这个旋转： 

旋转后的点 p ′ 即可表示为这样的乘积： 

四元数与矩阵的转换

现在看来，把四元数转换为矩阵的最直观方法，是先把四元数 q 转换为轴角 θ 和 n，然后再根据罗德里格斯公式转换为矩阵。
不过那样要计算一个 arccos 函数，代价较大。实际上这个计算是可以通过一定的技巧绕过的。我们省略过程中的推导，直接给出四元数到旋转矩阵的转换方式。 

设四元数 ，对应的旋转矩阵 R 为：

反之，由旋转矩阵到四元数的转换如下。假设矩阵为 ，其对应的四元数 q 由下式给出： 

值得一提的是，由于 q 和 −q 表示同一个旋转，事实上一个 R 对应的四元数表示并不是惟一的。同时，除了上面给出的转换方式之外，还存在其他几种计算方法。实际编程中，当 q0 接近 0 时，其余三个分量会非常大，导致解不稳定，此时我们再考虑使用其他的方式进行转换。
最后，无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。我们应该在实际中选择对我们最为方便的形式，而不必拘泥于某种特定的样子。