视觉SLAM中的数学基础 第二篇 四元数

视觉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$为四元数的三个虚部。这三个虚部满足关系式:

\[\begin{equation}
\label{eq:quaternionVirtual}
\left\{ \begin{array}{l}
{i^2} = {j^2} = {k^2} = - 1\\
ij = k,ji = - k\\
jk = i,kj = - i\\
ki = j,ik = - j
\end{array} \right.
\end{equation}\]

  由于它的这种特殊表示形式,有时人们也用一个标量和一个向量来表达四元数:
$$ \mathbf{q} = \left[ s, \mathbf{v} \right], \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 \right]^T$进行了角度为$\theta$的旋转,那么这个旋转的四元数形式为:

\[\begin{equation}
\label{eq:ntheta2quaternion}
\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
\end{equation}\]

  事实上,这还是一个模长为1的四元数,称为单位四元数。反之,我们亦可通过任意一个长度为1的四元数,计算对应旋转轴与夹角:

\[\begin{equation}
\begin{cases}
\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{cases}
\end{equation}\]

  若某个四元数长度不为1,我们可以通过归一化将它转换为一个模长为1的四元数。

  对式$\ref{eq:ntheta2quaternion}$的$\theta$加上$2\pi$,我们得到一个相同的旋转,但此时对应的四元数变成了$-\mathbf{q}$。因此,在四元数中,任意的旋转都可以由两个互为相反数的四元数表示。同理,取$\theta$为$0$,则得到一个没有任何旋转的四元数:

\[\begin{equation}
\mathbf{q}_0 = \left[ { \pm 1,0,0,0} \right]^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$的加减运算为:
\[\begin{equation}
\mathbf{q}_a \pm \mathbf{q}_b = \left[ s_a \pm s_b, \mathbf{v}_a \pm \mathbf{v}_b \right].
\end{equation}\]

  • 乘法

  乘法是把$\mathbf{q}_a$的每一项与$\mathbf{q}_b$每项相乘,最后相加,虚部要按照式~\ref{eq:quaternionVirtual}~进行:
\[\begin{equation}
\begin{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}} \right)i\\
&&+ \left( {{s_a}{y_b} - {x_a}{z_b} + {y_a}{s_b} + {z_a}{b_b}} \right)j\\
&&+ \left( {{s_a}{z_b} + {x_a}{y_b} - {x_b}{y_a} + {z_a}{s_b}} \right)k
\end{array}
\end{equation}\]
  虽然稍为复杂,但形式上也是整齐有序的。如果写成向量形式并利用内外积运算,该表达会更加简洁:
\[\begin{equation}
\mathbf{q}_a \mathbf{q}_b = \left[ s_a s_b - \mathbf{v}_a \cdot \mathbf{v}_b, s_a\mathbf{v}_b + s_b\mathbf{v}_a + \mathbf{v}_a \times \mathbf{v}_b \right]
\end{equation}\]
  这里我们就不帮读者复习什么叫外积了。在该乘法定义下,两个实的四元数乘积仍是实的,这与复数也是一致的。然而,注意到,由于最后一项外积的存在,该乘法通常是不可交换的,除非$\mathbf{v}_a$和$\mathbf{v}_b$在$\mathbb{R}^3$中共线。

  • 共轭

  四元数的共轭为:
\[\begin{equation}
\mathbf{q}_a^* = s_a - x_ai - y_aj - z_ak = [s_a, -\mathbf{v}_a]
\end{equation}\]
  即把虚部取成相反数。四元数共轭与自己本身相乘,会得到一个实四元数,其实部为模长的平方:
\[\begin{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}\]

  • 模长

  四元数的模长定义为:
\[\begin{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}\]
  可以验证,两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。
\[\begin{equation}
\| \mathbf{q}_a \mathbf{q}_b \| = \|\mathbf{q}_a \| \| \mathbf{q}_b \|
\end{equation}\]

  一个四元数的逆为:
\[\begin{equation}
\mathbf{q}^{-1} = \mathbf{q}^* / \| \mathbf{q} \| ^2
\end{equation}\]
  按此定义,四元数和自己的逆的乘积为实四元数的1:
\[\begin{equation}
\mathbf{q} \mathbf{q}^{-1} = \mathbf{q}^{-1} \mathbf{q} = 1
\end{equation}\]
  同时,乘积的逆有和矩阵相似的性质:
\[\begin{equation}
\left( \mathbf{q}_a \mathbf{q}_b \right)^{-1} = \mathbf{q}_b^{-1} \mathbf{q}_a^{-1}
\end{equation}\]
  对于单位四元数,即$\|\mathbf{q}\|=1$,它的逆即是它的共轭四元数。

  • 数乘与点乘

  和向量相似,四元数可以与数相乘:
\[\begin{equation}
k \mathbf{q} = \left[ ks, k\mathbf{v} \right]
\end{equation}\]

  点乘是指两个四元数每个位置上的数值分别相乘:
\[\begin{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 \theta}$表示2D的旋转,类似的,3D空间也可以用单位四元数表示旋转。假设一个空间三维点$\mathbf{v} = [x,y,z]\in \mathbb{R}^3$,以及一个由旋转轴和夹角$\mathbf{n}, \theta$ 指定的旋转,下面讨论如何用四元数表示它们。

  首先,我们把三维空间点用一个虚四元数来描述:$$\mathbf{p} = [0, x, y, z] = [0, \mathbf{v}]. $$

  然后,参照式\ref{eq:ntheta2quaternion},用另一个四元数$\mathbf{q}$表示这个旋转:$$ \mathbf{q} = [\cos \frac{\theta}{2}, \mathbf{n} \sin \frac{\theta}{2} ]. $$

  那么,旋转后的点$\mathbf{p}'$即可表示为这样的乘积:
\[\begin{equation}
\mathbf{p}' = \mathbf{q} \mathbf{p} \mathbf{q}^{-1}
\end{equation}\]

  可以验证,计算结果的实部为$\mathbf{n}^T(\mathbf{n} \times \mathbf{v})=0$,故计算结果为纯虚四元数。其虚部的三个分量表示旋转后3D点的坐标。


 

四元数到旋转矩阵的转换

  由于任意单位四元数都可表示为一个3D旋转,即$SO(3)$中的元素,我们可以找到一个旋转矩阵与之对应。最简单的方式是由四元数$\mathbf{q}$解出旋转角$\theta$和旋转轴$\mathbf{n}$,但那样要计算一个$\arccos$函数,代价较大。实际上这个计算是可以通过一定的计算技巧绕过的。为省略篇幅,我们直接给出四元数到旋转矩阵的转换方式。

  设四元数$\mathbf{q} = q_0+q_1i+q_2j+q_3k$,对应的旋转矩阵$\mathbf{R}$为:

\[\begin{equation}
\mathbf{R} = \left[ {\begin{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}} \right]
\end{equation}\]

  反之,由旋转矩阵到四元数的转换如下。假设矩阵为$\mathbf{R}=\{ m_{ij}\}, i, j \in \left[ 1, 2,3 \right] $,其对应的四元数$\mathbf{q}$由下式给出:
\[\begin{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空间中的变换,除了欧氏变换之外,还存在其他几种变换(事实上欧氏变换是最简单的)。它们有一部分和测量几何有关,我们之后的讲解中会提到,在此先罗列出来。

  • 相似变换

  相似变换比欧氏变换多了一个自由度,它允许物体进行自由地缩放。
\[\begin{equation}
T_S = \left[ {\begin{array}{*{20}{c}}
{s \mathbf{R}}& \mathbf{t}\\
{{ \mathbf{0}^T}}&1
\end{array}} \right]
\end{equation}\]

  注意到旋转部分多了一个缩放因子$s$,它在$x,y,z$三个坐标上形成均匀的缩放。类似的,相似变换的乘法也构成群,称为$Sim(3)$。由于含有缩放,相似变换不再保持图形的面积不变。

  • 仿射变换

  仿射变换的矩阵形式如下:
\[\begin{equation}
T_A = \left[ {\begin{array}{*{20}{c}}
\mathbf{A} & \mathbf{t}\\
{{\mathbf{0}^T}} & 1
\end{array}} \right]
\end{equation}\]

  与欧氏变换不同的是,仿射变换只要求$\mathbf{A}$是一个可逆矩阵,而不必是正交矩阵。在仿射变换下,直线的夹角会发生改变,但平行性质不变。这即是说,仿射变换把平行四边形变为平行四边形。

  • 射影变换

  射影变换是最一般的变换,它的矩阵形式为:

\[\begin{equation}
{\mathbf{T}_P} = \left[ {\begin{array}{*{20}{c}}
\mathbf{A} & \mathbf{t}\\
{{\mathbf{a}^T}} & v
\end{array}} \right]
\end{equation}\]

  它左上角为可逆矩阵$\mathbf{A}$,右上为平移$\mathbf{t}$,左下缩放$\mathbf{a}^T$。由于采用齐坐标,当$v \neq 0$时,我们可以对整个矩阵除以$v$得到一个右下角为1的矩阵; 否则,则得到右下角为$0$的矩阵。因此,这个矩阵在2D中一共有8个自由度,而在3D中一共有15个自由度,是现在提到的变换中最为一般的。

  下表总结了目前讲到的几种变换的性质。注意在“不变性质”中,从上到下是有包含关系的。例如,欧氏变换除了保体积之外,也具有保平行、相交等性质。

 


  如果你觉得我的博客有帮助,可以进行几块钱的小额赞助,帮助我把博客写得更好。

  

posted @ 2016-01-11 09:30 半闲居士 阅读(...) 评论(...) 编辑 收藏