四元数基础（一）
定义
作为旋转的一种重要表示，四元数在SLAM中有着比较重要的地位，所以这篇文章打算来做个简要介绍。引入四元数可以有五花八门的形式，但是比较优雅的一种形式是Cayley-Dickson构造，该形式如下：
假定我们有两个复数 $A = a + bi$ 和 $C = c + di$，我们再引入一个虚部$j$，定义$Q = A+ Cj$ 并且记虚部相乘的结果$k = ij$，那么，$$ Q = a+bi+cj +dk \in \mathbb{H}$$ 其中，实部 $a, b, c, d \in \mathbb{R}$, $i, j, k$ 为单位虚部。进一步，我们定义虚部运算如下: $$i^2 =j^2 = k^2 = ijk = -1$$ 从这个定义不难得出：$$ij = -ji = k, \quad jk = -kj = i, \quad ki = -ik =j $$
从以上定义中，我们可以清楚看到，实数，虚数，复数本质都是四元数，因为$$ Q = a \in \mathbb{R} \subset \mathbb{H}, \qquad Q = bi \in \mathbb{I} \subset \mathbb{H}, \qquad Q = a+bi \in \mathbb{Z} \subset \mathbb{H} $$ 同复数中定义纯虚数一样，我们定义纯虚四元数: $$ Q = bi + cj +dk \in \mathbb{H_{p}} \subset \mathbb{H}$$
现在，我们再介绍另一种四元数的常用表示。我们知道一个复数 $z = a+bi$ 在复平面内表示一个点，其坐标是$(a, b)$。同样地，四元数也可以这么玩，定义: $$Q = q_w +q_x i+ q_y j + q_z k \qquad \Leftrightarrow \qquad Q = q_w + \mathbf{q}_v $$ 其中，$q_w$称之为实部，而$\mathbf{q}_v =q_x i+ q_y j + q_z k = (q_x, q_y, q_z)$ 称之为虚部。
我们把四元数$Q$记作一个四维空间向量$\mathbf{q}$，这样，我们把这个向量进一步可以写成如下形式：$$\mathbf{q} = \left [ \begin{array} {*{20}{c}} q_w \\ \mathbf{q}_v \end{array} \right] = \left [ \begin{array} {*{20}{c}} q_w \\ q_x \\ q_y \\ q_z \end{array} \right] $$
最后说明一下，这里我们遵循的四元数定义是哈密顿约定，当然也有一些文献采用JPL约定的。不同定义并没有本质区别，只是formulation不太一致，注意区别就好。
简单性质
- 加法性质
两个四元数相加减，有：$$\mathbf{p} \pm \mathbf{q} = \left [ \begin{array} {*{20}{c}} p_w \\ \mathbf{p}_v \end{array} \right] \pm \left [ \begin{array} {*{20}{c}} q_w \\ \mathbf{q}_v \end{array} \right] = \left [ \begin{array} {*{20}{c}} p_w \pm q_w \\ \mathbf{p}_v \pm \mathbf{q}_v\end{array} \right] $$ 不难验证，四元数加法满足交换律和结合律： \begin{equation*} { \mathbf{p} + \mathbf{q} = \mathbf{q} + \mathbf{p} \\ \mathbf{p} + (\mathbf{q} + \mathbf{r}) = (\mathbf{q} + \mathbf{p}) + \mathbf{r} } \end{equation*} - 乘积性质
四元数乘法是我们用的最多的性质，当然口舌也必须多费点。用记号$\otimes$ 表示四元数乘法，如果我们利用四元数最原始的记法以及定义好的虚部运算规则，两个四元数相乘便会有如下结果：$$\mathbf{q} \otimes \mathbf{p} = \left [ \begin{array} {*{20}{c}} p_w q_w - p_x q_x - p_y q_y - p_z q_z \\ p_wq_x + p_xq_w + p_yq_z -p_zq_y \\ p_wq_y - p_xq_z + p_yq_w + p_zq_x \\ p_wq_z +p_xq_y -p_yq_x + p_zq_w \end{array} \right] = \left [ \begin{array} {*{20}{c}} p_w q_w - \mathbf{p}_{v}^{T} \mathbf{q}_{v} \\ p_w\mathbf{q}_v + q_w \mathbf{p}_v + \mathbf{p}_v \times \mathbf{q}_v \end{array} \right] $$ 从相乘结果的虚部中，我们可以看到包含叉乘运算，由于叉乘本身是不可交换的，所以，一般意义上来说，两个四元数相乘不满足交换律：$$\mathbf{p} \otimes \mathbf{q} \neq \mathbf{q} \otimes \mathbf{p} $$ 当然，也有一些特殊情况，如两个四元数的虚部向量是平行的，这样叉乘的结果就是0，这时候四元数相乘满足交换律。又如，两个四元数的虚部向量均为0，只有实部，四元数退化为实数，这种特殊情况下，四元数相乘当然也满足交换律。
然而，四元数乘法满足结合律: $$ (\mathbf{p} \otimes \mathbf{q}) \otimes \mathbf{r} = \mathbf{p} \otimes (\mathbf{p} \otimes \mathbf{r} )$$ 同时，也满足分配率: $$\mathbf{p} \otimes (\mathbf{q} + \mathbf{r}) = \mathbf{p} \otimes \mathbf{q} + \mathbf{p} \otimes \mathbf{r} \quad and \quad (\mathbf{p} + \mathbf{q}) \otimes \mathbf{r} = \mathbf{p} \otimes \mathbf{r} + \mathbf{q} \otimes \mathbf{r} $$
现在，我们把四元数相乘的结果重新整理一下，写成如下形式：$$\left [ \begin{array} {*{20}{c}} p_w q_w - p_x q_x - p_y q_y - p_z q_z \\ p_wq_x + p_xq_w + p_yq_z -p_zq_y \\ p_wq_y - p_xq_z + p_yq_w + p_zq_x \\ p_wq_z +p_xq_y -p_yq_x + p_zq_w \end{array} \right] = \left [ \begin{array} {*{20}{c}} p_w & -p_x & -p_y & -p_z\\ p_x & p_w & -p_z & p_y \\ p_y & p_z & p_w & -p_x \\ p_z & -p_y & p_x & p_w \end{array} \right] \left [ \begin{array} {*{20}{c}} q_w \\ q_x \\ q_y \\ q_z \end{array} \right] $$ 那么两个四元数的相乘就可以用如下简洁的矩阵乘法形式来表示了：$$\mathbf{p} \otimes \mathbf{q} = [\mathbf{p}]_L \mathbf{q}$$ 其中， $$ [\mathbf{p}]_L = \left [ \begin{array} {*{20}{c}} p_w & -p_x & -p_y & -p_z\\ p_x & p_w & -p_z & p_y \\ p_y & p_z & p_w & -p_x \\ p_z & -p_y & p_x & p_w \end{array} \right] $$ 同理，我们也可以这样表示两个四元数相乘：$$\mathbf{p} \otimes \mathbf{q} = [\mathbf{q}]_R \mathbf{p}$$ 其中，$$ [\mathbf{q}]_R = \left [ \begin{array} {*{20}{c}} q_w & -q_x & -q_y & -q_z\\ q_x & q_w & q_z & -q_y \\ q_y & -q_z & q_w & q_x \\ q_z & q_y & -q_x & q_w \end{array} \right] $$ 我们再来把上面两个矩阵写得更简洁一点：$$ [\mathbf{p}]_L = p_w \mathbf{I} + \left [ \begin{array} {*{20}{c}} 0 & -\mathbf{p}_{v}^{T} \\ \mathbf{p}_v & [\mathbf{p}_v]^{\wedge} \end{array} \right], \qquad [\mathbf{q}]_R = q_w \mathbf{I} + \left [ \begin{array} {*{20}{c}} 0 & -\mathbf{q}_{v}^{T} \\ \mathbf{q}_v & -[\mathbf{q}_v]^{\wedge} \end{array} \right] $$ 其中，$[\cdot]^{\wedge}$ 这个操作是各种文献上都经常看到的操作，它将一个向量写成了反对称形式：$$[\mathbf{a}]^{\wedge} = \left [ \begin{array} {*{20}{c}} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{array} \right]$$ 最后，关于四元数左右乘机矩阵，我们还有一个性质：$$[\mathbf{p}]_R [\mathbf{q}]_L = [\mathbf{q}]_L [\mathbf{p}]_R$$ 关于这个性质的证明，用一用乘法结合律很显然就可以知道了：$$ (\mathbf{q} \otimes \mathbf{r}) \otimes \mathbf{p} = [\mathbf{p}]_R [\mathbf{q}]_L \mathbf{r} \qquad and \qquad \mathbf{q} \otimes (\mathbf{r} \otimes \mathbf{p}) = [\mathbf{q}]_L [\mathbf{p}]_R \mathbf{r}$$ - 共轭
四元数$\mathbf{q}$的共轭$\mathbf{q}^*$定义为：$$\mathbf{q}^* = q_w - \mathbf{q}_v = \left [ \begin{array} {*{20}{c}} q_w \\ -\mathbf{q}_v \end{array} \right]$$ 四元数和其共轭的乘法是可交换的：$$\mathbf{q}^* \otimes \mathbf{q} = \mathbf{q} \otimes \mathbf{q}^* = q_{w}^2 + q_{x}^2 + q_{y}^2 +q_{z}^2 = \left [ \begin{array} {*{20}{c}} q_{w}^2 + q_{x}^2 + q_{y}^2 +q_{z}^2\\ \mathbf{0}_v \end{array} \right]$$ 上面的结果可以用两个普通四元数相乘的结果做一下简单替换就很容易知道了。另外，四元数乘积的共轭还有性质：$$(\mathbf{p} \otimes \mathbf{q})^* = \mathbf{q}^* \otimes \mathbf{p}^*$$ 这个性质有点儿类似于矩阵相乘然后取转置或者逆的性质。 - 模
四元数的模定义如下：$$\| \mathbf{q} \|= \sqrt{\mathbf{q} \otimes \mathbf{q}^*} = \sqrt{\mathbf{q}^* \otimes \mathbf{q}} = \sqrt{q_{w}^2 + q_{x}^2 + q_{y}^2 +q_{z}^2}$$ 根据模长定义以及四元数乘法的交换律，我们不难得出：$$ \| \mathbf{p} \otimes \mathbf{q} \| = \| \mathbf{q} \otimes \mathbf{p} \| = \| \mathbf{p}\| \| \mathbf{q} \|$$ - 逆
四元数的逆 $\mathbf{q}^{-1}$ 定义也比较简单，那就是一个四元数和它的逆相乘结果为1，公式表示为：$$\mathbf{q}^{-1} \otimes \mathbf{q} = \mathbf{q} \otimes \mathbf{q} ^ {-1} = 1$$ 四元数的逆可以通过下式计算：$$\mathbf{q}^{-1} = \frac{\mathbf{q}^*}{\|\mathbf{q}\|^2}$$ - 单位四元数
所谓单位四元数，就是模长为1的四元数：$\|\mathbf{q}\| = 1$，这样的限制使得它刚好具有三个自由度。我们知道在三维欧氏空间中，旋转具有三个自由度。表达旋转可以采用各种不同的表达形式：
欧拉角：Yaw, Pitch, Roll，共三个自由度；
旋转矩阵：虽然这是个3维方阵，貌似有9个自由度，但它本身也受到正交性等限制。它也只有三个自由度；
旋转向量：旋转方向向量只有两个自由度，外加一个旋转角度，所以，它也肯定是三个自由度；
单位四元数：当一个四元数被限制为单位四元数时，它也只剩下了三个自由度，我们可以用它来表达旋转。
任意单位四元素总可以表达成下面的形式：$$\mathbf{q} = \left [ \begin{array} {*{20}{c}} \cos \theta \\ \mathbf{u} \sin \theta \end{array} \right]$$ 其中，$\mathbf{u} = u_x i + u_y j +u_z k$ 是一个单位矢量，$\theta$则是一个标量。另外，根据求逆的表达式，我们很容易知道，单位四元数的逆和共轭相等：$$\mathbf{q}^{-1} = \mathbf{q}^*$$ - 单位纯虚四元数
对于单位纯虚四元数$\mathbf{u} = [0, u_x, u_y, u_z ]^T \in \mathbb{H}_p$，$\| \mathbf{u} \| = 1$，我们可以从两个四元数乘积最一般的结果得到：$$\mathbf{u} \otimes \mathbf{u} = -1$$ 这个结果可以类比复数虚部自乘结果：$i \cdot i = -1$
