视觉SLAM中的数学基础 第二篇 四元数
什么是四元数
相比欧拉角，四元数(Quaternion)则是一种紧凑、易于迭代、又不会出现奇异值的表示方法。它在程序中广为使用，例如ROS和几个著名的SLAM公开数据集、g2o等程序都使用四元数记录机器人的姿态。因此，理解四元数的含义与用法，对学习SLAM来说是必须的。本节我们就来讲讲四元数。
首先，请读者不要对四元数有什么神秘的感觉。四元数仅是3D姿态的一种表达方式，我们用一个单位四元数表达原本用旋转矩阵表示的三维旋转。这样做一个直接的好处是省空间。一个旋转阵有9个分量，但只有三个自由度。那么，能不能用三个数来描述呢？可以是可以的，但不可避免会出现奇异的情况，欧拉角就是一个例子。而四元数，比三维向量多了一个分量，从而可以无奇异地表示各种姿态。下面我们来详细讲讲四元数。
四元数是Hamilton找到的一种扩展的复数。一个四元数拥有一个实部和三个虚部（故事上说他原先找了很久带两个虚部的，结果怎么也找不到，最后豁然开朗找到了三虚部的四元数）：
$$ mathbf{q} = q_0 + q_1 i + q_2 j + q_3 k $$
其中$i,j,k$为四元数的三个虚部。这三个虚部满足关系式：
[egin{equation}
label{eq:quaternionVirtual}
left{ egin{array}{l}
{i^2} = {j^2} = {k^2} = - 1\
ij = k,ji = - k\
jk = i,kj = - i\
ki = j,ik = - j
end{array}
ight.
end{equation}]
由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：
$$ mathbf{q} = left[ s, mathbf{v}
ight], quad s=q_0 in mathbb{R}, mathbf{v} = [q_1, q_2, q_3] in mathbb{R}^3. $$
这里，标量$s$称为四元数的实部，而向量$mathbf{v}$称为它的虚部。如果一个四元数虚部为$mathbf{0}$，称之为实四元数。反之，若它的实部为$0$，称之为虚四元数。该定义和复数是相似的。
四元数可以表示三维空间中任意一个旋转。与旋转矩阵中类似，我们仍假设某个旋转是绕单位向量$mathbf{n}=left[ n_x, n_y, n_z ight]^T$进行了角度为$ heta$的旋转，那么这个旋转的四元数形式为：
[egin{equation}
label{eq:ntheta2quaternion}
mathbf{q} = left[ cos frac{ heta}{2}, n_x sin frac{ heta}{2}, n_y sin frac{ heta}{2}, n_z sin frac{ heta}{2}
ight]^T
end{equation}]
事实上，这还是一个模长为1的四元数，称为单位四元数。反之，我们亦可通过任意一个长度为1的四元数，计算对应旋转轴与夹角：
[egin{equation}
egin{cases}
heta = 2arccos {q_0}\
{left[ {{n_x},{n_y},{n_z}}
ight]^T} = {{{left[ {{q_1},{q_2},{q_3}}
ight]}^T}}/{sin frac{ heta }{2}}
end{cases}
end{equation}]
若某个四元数长度不为1，我们可以通过归一化将它转换为一个模长为1的四元数。
对式$ ef{eq:ntheta2quaternion}$的$ heta$加上$2pi$，我们得到一个相同的旋转，但此时对应的四元数变成了$-mathbf{q}$。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取$ heta$为$0$，则得到一个没有任何旋转的四元数：
[egin{equation}
mathbf{q}_0 = left[ { pm 1,0,0,0}
ight]^T
end{equation}]
四元数的运算
四元数和通常复数一样，可以进行一系列的运算。常见的有四则运算、内积、求逆、共轭、求指数／对数等等。表示姿态时，它还可以进行插值。下面我们分别介绍。
现有两个四元数$mathbf{q}_a, mathbf{q}_b$，它们的向量表示为$[s_a, mathbf{v}_a], [s_b, mathbf{v}_b]$，或者原始四元数表示为：$$s_a+x_ai+y_aj+z_ak, s_b+x_bi+y_bj+z_bk.$$那么，它们的运算可表示如下。
- 加法和减法
四元数$mathbf{q}_a, mathbf{q}_b$的加减运算为：
[egin{equation}
mathbf{q}_a pm mathbf{q}_b = left[ s_a pm s_b, mathbf{v}_a pm mathbf{v}_b
ight].
end{equation}]
- 乘法
乘法是把$mathbf{q}_a$的每一项与$mathbf{q}_b$每项相乘，最后相加，虚部要按照式~
ef{eq:quaternionVirtual}~进行：
[egin{equation}
egin{array}{lll}
mathbf{q}_a mathbf{q}_b &=& {s_a}{s_b} - {x_a}{x_b} - {y_a}{y_b} - {z_a}{z_b}\
&&+ left( {{s_a}{x_b} + {x_a}{s_b} + {y_a}{z_b} - {z_a}{y_b}}
ight)i\
&&+ left( {{s_a}{y_b} - {x_a}{z_b} + {y_a}{s_b} + {z_a}{b_b}}
ight)j\
&&+ left( {{s_a}{z_b} + {x_a}{y_b} - {x_b}{y_a} + {z_a}{s_b}}
ight)k
end{array}
end{equation}]
虽然稍为复杂，但形式上也是整齐有序的。如果写成向量形式并利用内外积运算，该表达会更加简洁：
[egin{equation}
mathbf{q}_a mathbf{q}_b = left[ s_a s_b - mathbf{v}_a cdot mathbf{v}_b, s_amathbf{v}_b + s_bmathbf{v}_a + mathbf{v}_a imes mathbf{v}_b
ight]
end{equation}]
这里我们就不帮读者复习什么叫外积了。在该乘法定义下，两个实的四元数乘积仍是实的，这与复数也是一致的。然而，注意到，由于最后一项外积的存在，该乘法通常是不可交换的，除非$mathbf{v}_a$和$mathbf{v}_b$在$mathbb{R}^3$中共线。
- 共轭
四元数的共轭为：
[egin{equation}
mathbf{q}_a^* = s_a - x_ai - y_aj - z_ak = [s_a, -mathbf{v}_a]
end{equation}]
即把虚部取成相反数。四元数共轭与自己本身相乘，会得到一个实四元数，其实部为模长的平方:
[egin{equation}
mathbf{q}* mathbf{q} = mathbf{q} mathbf{q}* = [s_a^2+mathbf{v}^T mathbf{v}, mathbf{0} ] = s_a^2+mathbf{v}^T mathbf{v}
end{equation}]
- 模长
四元数的模长定义为：
[egin{equation}
| mathbf{q}_a | = sqrt{ s_a^2 + x_a^2 + y_a^2 + z_a^2 } = sqrt{mathbf{q}_a^{*T} mathbf{q}_a}
end{equation}]
可以验证，两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。
[egin{equation}
| mathbf{q}_a mathbf{q}_b | = |mathbf{q}_a | | mathbf{q}_b |
end{equation}]
- 逆
一个四元数的逆为：
[egin{equation}
mathbf{q}^{-1} = mathbf{q}^* / | mathbf{q} | ^2
end{equation}]
按此定义，四元数和自己的逆的乘积为实四元数的1：
[egin{equation}
mathbf{q} mathbf{q}^{-1} = mathbf{q}^{-1} mathbf{q} = 1
end{equation}]
同时，乘积的逆有和矩阵相似的性质：
[egin{equation}
left( mathbf{q}_a mathbf{q}_b
ight)^{-1} = mathbf{q}_b^{-1} mathbf{q}_a^{-1}
end{equation}]
对于单位四元数，即$|mathbf{q}|=1$，它的逆即是它的共轭四元数。
- 数乘与点乘
和向量相似，四元数可以与数相乘：
[egin{equation}
k mathbf{q} = left[ ks, kmathbf{v}
ight]
end{equation}]
点乘是指两个四元数每个位置上的数值分别相乘：
[egin{equation}
mathbf{q}_a cdot mathbf{q}_b = s_a s_b + x_a x_b i + y_a y_b j + z_a z_b k
end{equation}]
用四元数表示旋转
在复数域$mathbb{C}$，我们可以用一个复数$e^{i heta}$表示2D的旋转，类似的，3D空间也可以用单位四元数表示旋转。假设一个空间三维点$mathbf{v} = [x,y,z]in mathbb{R}^3$，以及一个由旋转轴和夹角$mathbf{n}, heta$ 指定的旋转，下面讨论如何用四元数表示它们。
首先，我们把三维空间点用一个虚四元数来描述：$$mathbf{p} = [0, x, y, z] = [0, mathbf{v}]. $$
然后，参照式 ef{eq:ntheta2quaternion}，用另一个四元数$mathbf{q}$表示这个旋转：$$ mathbf{q} = [cos frac{ heta}{2}, mathbf{n} sin frac{ heta}{2} ]. $$
那么，旋转后的点$mathbf{p}'$即可表示为这样的乘积：
[egin{equation}
mathbf{p}' = mathbf{q} mathbf{p} mathbf{q}^{-1}
end{equation}]
可以验证，计算结果的实部为$mathbf{n}^T(mathbf{n} imes mathbf{v})=0$，故计算结果为纯虚四元数。其虚部的三个分量表示旋转后3D点的坐标。
四元数到旋转矩阵的转换
由于任意单位四元数都可表示为一个3D旋转，即$SO(3)$中的元素，我们可以找到一个旋转矩阵与之对应。最简单的方式是由四元数$mathbf{q}$解出旋转角$ heta$和旋转轴$mathbf{n}$，但那样要计算一个$arccos$函数，代价较大。实际上这个计算是可以通过一定的计算技巧绕过的。为省略篇幅，我们直接给出四元数到旋转矩阵的转换方式。
设四元数$mathbf{q} = q_0+q_1i+q_2j+q_3k$，对应的旋转矩阵$mathbf{R}$为：
[egin{equation}
mathbf{R} = left[ {egin{array}{*{20}{c}}
{1 - 2q_2^2 - 2q_3^2}&{2{q_1}{q_2} + 2{q_0}{q_3}}&{2{q_1}{q_3} - 2{q_0}{q_2}}\
{2{q_1}{q_2} - 2{q_0}{q_3}}&{1 - 2q_1^2 - 2q_3^2}&{2{q_2}{q_3} + 2{q_0}{q_1}}\
{2{q_1}{q_3} + 2{q_0}{q_2}}&{2{q_2}{q_3} - 2{q_0}{q_1}}&{1 - 2q_1^2 - 2q_2^2}
end{array}}
ight]
end{equation}]
反之，由旋转矩阵到四元数的转换如下。假设矩阵为$mathbf{R}={ m_{ij}}, i, j in left[ 1, 2,3
ight] $，其对应的四元数$mathbf{q}$由下式给出：
[egin{equation}
{q_0} = frac{{sqrt {tr(R) + 1} }}{2},{q_1} = frac{{{m_{23}} - {m_{32}}}}{{4{q_0}}},{q_2} = frac{{{m_{31}} - {m_{13}}}}{{4{q_0}}},{q_3} = frac{{{m_{12}} - {m_{21}}}}{{4{q_0}}}
end{equation}]
值得一提的是，由于$mathbf{q}和mathbf{-q}$表示同一个旋转，事实上一个$mathbf{R}$的四元数表示并不是惟一的。存在其他三种与上式类似的计算方式，而本书省略了。实际编程中，当$q_0$接近0时，其余三个分量会非常大，导致解不稳定，此时会考虑使用剩下的几种方式计算。
其他几种变换
3D空间中的变换，除了欧氏变换之外，还存在其他几种变换（事实上欧氏变换是最简单的）。它们有一部分和测量几何有关，我们之后的讲解中会提到，在此先罗列出来。
- 相似变换
相似变换比欧氏变换多了一个自由度，它允许物体进行自由地缩放。
[egin{equation}
T_S = left[ {egin{array}{*{20}{c}}
{s mathbf{R}}& mathbf{t}\
{{ mathbf{0}^T}}&1
end{array}}
ight]
end{equation}]
注意到旋转部分多了一个缩放因子$s$，它在$x,y,z$三个坐标上形成均匀的缩放。类似的，相似变换的乘法也构成群，称为$Sim(3)$。由于含有缩放，相似变换不再保持图形的面积不变。
- 仿射变换
仿射变换的矩阵形式如下：
[egin{equation}
T_A = left[ {egin{array}{*{20}{c}}
mathbf{A} & mathbf{t}\
{{mathbf{0}^T}} & 1
end{array}}
ight]
end{equation}]
与欧氏变换不同的是，仿射变换只要求$mathbf{A}$是一个可逆矩阵，而不必是正交矩阵。在仿射变换下，直线的夹角会发生改变，但平行性质不变。这即是说，仿射变换把平行四边形变为平行四边形。
- 射影变换
射影变换是最一般的变换，它的矩阵形式为:
[egin{equation}
{mathbf{T}_P} = left[ {egin{array}{*{20}{c}}
mathbf{A} & mathbf{t}\
{{mathbf{a}^T}} & v
end{array}}
ight]
end{equation}]
它左上角为可逆矩阵$mathbf{A}$，右上为平移$mathbf{t}$，左下缩放$mathbf{a}^T$。由于采用齐坐标，当$v eq 0$时，我们可以对整个矩阵除以$v$得到一个右下角为1的矩阵； 否则，则得到右下角为$0$的矩阵。因此，这个矩阵在2D中一共有8个自由度，而在3D中一共有15个自由度，是现在提到的变换中最为一般的。
下表总结了目前讲到的几种变换的性质。注意在“不变性质”中，从上到下是有包含关系的。例如，欧氏变换除了保体积之外，也具有保平行、相交等性质。
如果你觉得我的博客有帮助，可以进行几块钱的小额赞助，帮助我把博客写得更好。
