視覺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個自由度，是現在提到的變換中最為一般的。
下表總結了目前講到的幾種變換的性質。注意在“不變性質”中，從上到下是有包含關係的。例如，歐氏變換除了保體積之外，也具有保平行、相交等性質。
如果你覺得我的部落格有幫助，可以進行幾塊錢的小額贊助，幫助我把部落格寫得更好。
