视觉SLAM十四讲学习笔记——三维空间刚体运动（ch3~四元数）

一、四元数的定义

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；
欧拉角和旋转向量是紧凑的，但具有奇异性。
事实上，我们找不到不带奇异性的三维向量描述方式。三维旋转是一个三维流形，想要无奇异性地表达它，用三个量是不够的。

回忆我们以前学习过的复数。我们用复数集$\mathbb{C}:$ 表示复平面上的向量，而复数的乘法则能表示复平面上的旋转: 例如，乘上复数 相当于逆时针把一个复向量旋转 90 度。类似的，在表达三维空间旋转时，也有一种类似于复数的代数:四元数 (Quaternion)。四元数是 Hamilton 找到的一种扩展的复数它既是紧凑的，也没有奇异性。如果说缺点的话四元数不够直观，其运算稍为复杂一些。

个四元数q拥有一个实部和三个虚部。像这样:
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k,$$
其中$i,j,k$ 为四元数的三个虚部。这三个虚部满足关系式:
$$\begin{array}{rl}& \{\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}\end{array}$$
由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数:
$$q=[s,\mathbf{v}],s=q_0\in \mathbb{R},v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3,$$
这里，$s$ 称为四元数的实部，而$\mathbf{v}$称为它的虚部。如果一个四元数虚部为0，称之为实四元数。反之，若它的实部为 0，称之为虚四元数。

我们能用单位四元数表示三维空间中任意一个旋转，不过这种表达方式和复数有着微妙的不同。
这种表达方式和旋转矩阵、旋转向量有什么关系呢?
先看旋转向量。
假设某个旋转是绕单位向量$\mathbf{n}={\left[n_x,n_y,n_z\right]}^T$进行了角度为$\theta$的旋转，那么这个旋转的四元数形式为:
$$\mathbf{q}={\left[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}\right]}^T.$$
反之，亦可从单位四元数中计算出对应旋转轴与夹角:
$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^T={\left[q_1,q_2,q_3\right]}^T/\sin \frac{\theta }{2}\end{array}$$
这式子给我们一种微妙的“转了一半”的感觉。同样，对式$\mathbf{q}={\left[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}\right]}^T$的$\theta$加上 $2\pi$，我们得到一个相同的旋转，但此时对应的四元数变成了$-\mathbf{q}$。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取$\theta$为0，则得到一个没有任何旋转的实四元数:
$$q_0=[\pm 1,0,0,0{]}^T\cdot$$

二、四元数的运算

四元数和通常复数一样，可以进行一系列的运算。常见的有四则运算、数乘、求逆、共钜等等。
现有两个四元数 $q_a$,$q_b$，它们的向量表示为$[s_a,v_a]$,$[s_b,v_b]$，或者原始四元数表示为:
$${\mathbf{q}}_a=s_a+x_{a}i+y_{a}j+z_{a}k,{\mathbf{q}}_b=s_b+x_{b}i+y_{b}j+z_{b}k.$$
那么，运算如下：

2.1 加减法

$${\mathbf{q}}_a\pm {\mathbf{q}}_b=[s_a\pm s_b,{\mathbf{v}}_a\pm {\mathbf{v}}_b].$$

2.2 乘法

乘法是把$q_a$的每一项与 $q_b$每项相乘，最后相加.
$$\begin{array}{rl}{\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}=& s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i\\ & +(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{b}_b)j\\ & +\left(s_a{z}_b+x_a{y}_b-x_b{y}_a+z_a{s}_b\right)k.\end{array}$$
简洁表达：
$${\mathbf{q}}_a{\mathbf{q}}_b=\left[\begin{array}{c}{s}_a{s}_b-{\mathbf{v}}_a^T{\mathbf{v}}_b,s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\end{array}\right].$$

2.3 共轭

2.4 模长

2.5 逆

2.6 数乘与点乘