四元数 | Quaternion

1 定义

1.1 四元数

四元数是复数的不可交换延伸。如把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。
PS：四元数有 Hamiton (Left Hand) 和 JPL (Right Hand) 两种。在这里我们后面公式表示都对应更常用的 Hamiton 四元数。
其表示为：
\begin{equation}\mathbf{q}=[w, x, y, z]^{T}\end{equation}
也可以记为：
\begin{equation}\mathbf{q} = w+xi+yj+zk\end{equation}
其中实部 w 为标量，虚部 x, y, z 为矢量。
三个虚部满足如下性质：
\begin{equation}\left\{\begin{matrix}i^2 + j^2 + k^2 = 1\\ij=k, ji=-k \\jk=i, kj=-i \\ki=j, ik=-j\end{matrix}\right.\end{equation}
也可以记为：
\begin{equation}\mathbf{q} = [s,\mathbf{v}], \mathbf{v} = [x,y,z]^\mathrm{T} \in \mathbb{R}^3\end{equation}
其中 s 为标量，v 为虚部的矢量。

1.2 单位四元数

三维空间中的任意旋转可以用单位四元数表示（注意是单位不是任意四元数），其满足条件 w^2+x^2+y^2+z^2=1。

1.3 单位四元数与角轴的关系

单位四元数和角轴的转换关系：设角轴为 \omega 和 \theta，那么它对应的四元数为：
\begin{equation}\mathbf{q}=\left[\cos \frac{\theta}{2}, \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}\end{equation}

2 四元数运算

给定两个四元数 \mathbf{q}_{a} = w_{a} + x_{a} \cdot i + y_{a} \cdot j + z_{a} \cdot k 和 \mathbf{q}_{b} = w_{b} + x_{b} \cdot i + y_{b} \cdot j + z_{b} \cdot k，则可以定义下面运算：

2.1 加减法

四元数的加法定义如下：
\begin{equation}\mathbf{q}_{a} \oplus \mathbf{q}_{b} =\left(w_{a} + w_{b}\right) + \left(x_{a} + x_{b}\right) i + \left(y_{a} + y_{b}\right) j + \left(z_{a} + z_{b}\right) k\end{equation}
四元数的减法定义如下：
\begin{equation}\mathbf{q}_{a} \ominus \mathbf{q}_{b} =\left(w_{a} - w_{b}\right) + \left(x_{a} - x_{b}\right) i + \left(y_{a} - y_{b}\right) j + \left(z_{a} - z_{b}\right) k\end{equation}

2.2 乘法

四元数的乘法定义如下：
\begin{equation}\begin{aligned}\mathbf{q}_{a} \otimes \mathbf{q}_{b} &=w_{a} w_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\&+\left(w_{a} x_{b}+x_{a} w_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\&+\left(w_{a} y_{b}-x_{a} z_{b}+y_{a} w_{b}+z_{a} x_{b}\right) j \\&+\left(w_{a} z_{b}+x_{a} y_{b}-y_{a} x_{b}+z_{a} w_{b}\right) k\end{aligned}\end{equation}

或者：
\begin{equation}\mathbf{q}_{a} \otimes \mathbf{q}_{b}=\left[s_{a} s_{b}-\mathbf{v}_{a}^{\mathrm{T}} \mathbf{v}_{b}, s_{a} \mathbf{v}_{b}+s_{b} \mathbf{v}_{a}+\mathbf{v}_{a} \times \mathbf{v}_{b}\right]^{\mathrm{T}}\end{equation}

2.3 共轭与逆

共轭：
\begin{equation}\boldsymbol{q}^{*}=s-x i-y j-z k=\left[s,-\boldsymbol{v}\right]\end{equation}

逆：
\begin{equation}\boldsymbol{q}^{-1}=\boldsymbol{q}^{*} /\|\boldsymbol{q}\|^{2}\end{equation}

四元数还有很多其他运算与复数一致，参见维基百科。

3 四元数与空间旋转

3.1 四元数矩阵

两个旋转叠加可以表示为四元数相乘：
\begin{equation}\mathbf{q} \otimes \mathbf{p}=\left[\begin{array}{c}q_{w} p_{x}-q_{z} p_{y}+q_{y} p_{z}+q_{x} p_{w} \\q_{z} p_{x}+q_{w} p_{y}-q_{x} p_{z}+q_{y} p_{w} \\-q_{y} p_{x}+q_{x} p_{y}+q_{w} p_{z}+q_{z} p_{w} \\-q_{x} p_{x}-q_{y} p_{y}-q_{z} p_{z}+q_{w} p_{w}\end{array}\right]=[\mathbf{q}]_{L} \mathbf{p}=[\mathbf{p}]_{R} \mathbf{q}\end{equation}
其中，左乘四元数矩阵定义为：
\begin{equation}[\mathbf{q}]_{L}=\left[\begin{array}{cccc}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]=q_{w} \mathbf{I}+\left[\begin{array}{cc}{\left[\mathbf{q}_{v}\right]_{\times}} & \mathbf{q}_{v}^{T} \\-\mathbf{q}_{v} & 0\end{array}\right]\end{equation}

右乘四元数矩阵定义为：
\begin{equation}[\mathbf{q}]_{R}=\left[\begin{array}{cccc}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]=q_{w} \mathbf{I}+\left[\begin{array}{cc}-\left[\mathbf{q}_{v}\right] \times & \mathbf{q}_{v}^{T} \\-\mathbf{q}_{v} & 0\end{array}\right]\end{equation}

3.2 四元数旋转三维点

已知一个三维点 \mathbf{p}，进行旋转 \mathbf{q} 后的三维点 \mathbf{p}^{\prime}
\begin{equation}\mathbf{p}^{\prime}=\mathbf{R} \mathbf{p} \Leftrightarrow \mathbf{p}^{\prime}=\mathbf{q} \otimes \mathbf{p} \otimes \mathbf{q}^{-1}=\mathbf{q} \otimes \mathbf{p} \otimes \mathbf{q}^{*}=[\mathbf{q}]_{L}\left[\mathbf{q}^{*}\right]_{R} \mathbf{p}=\left[\mathbf{q}^{*}\right]_{R}[\mathbf{q}]_{L} \mathbf{p}\end{equation}

显然我们也可以由此求出四元数与旋转矩阵的关系：
\begin{equation}\mathbf{R}=\left[\begin{array}{ccc}q_{w}^{2}+q_{w}^{2}-q_{w}^{2}-q_{w}^{2} & 2\left(q_{x} q_{y}-q_{w} q_{z}\right) & 2\left(q_{x} q_{z}+q_{w} q_{y}\right) \\2\left(q_{x} q_{y}+q_{w} q_{z}\right) & q_{w}^{2}-q_{w}^{2}+q_{w}^{2}-q_{w}^{2} & 2\left(q_{y} q_{z}-q_{w} q_{x}\right) \\2\left(q_{x} q_{z}-q_{w} q_{y}\right) & 2\left(q_{y} q_{z}+q_{w} q_{x}\right) & q_{w}^{2}-q_{w}^{2}-q_{w}^{2}+q_{w}^{2}\end{array}\right]\end{equation}

4 四元数求导

设初始旋转为 \mathbf{q} = [s,\mathbf{v}]，发生了角轴为 \omega，\theta 的旋转，右乘对应四元数记为 \Delta \mathbf{q} = [\theta,\mathbf{\omega}]，那么 \mathbf{q} 相对该旋转的导数推导如下：
\begin{equation}\begin{aligned}\lim _{\theta \rightarrow 0} \frac{\mathbf{q} \otimes \Delta \mathbf{q}-\mathbf{q}}{\theta} &=\lim _{\theta \rightarrow 0} \frac{\left[s \cos \frac{\theta}{2}-\mathbf{v}^{T} \boldsymbol{\omega} \sin \frac{\theta}{2}, s \omega \sin \frac{\theta}{2}+\cos \frac{\theta}{2} \mathbf{v}+\mathbf{v} \times \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}-\mathbf{q}}{\theta} \\&=\lim _{\theta \rightarrow 0} \frac{\left[s\left(\cos \frac{\theta}{2}-1\right)-\mathbf{v}^{T} \boldsymbol{\omega} \sin \frac{\theta}{2}, s \omega \sin \frac{\theta}{2}+\left(\cos \frac{\theta}{2}-1\right) \mathbf{v}+\mathbf{v} \times \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}}{\theta} \\&=\left[-\frac{1}{2} \mathbf{v}^{T} \boldsymbol{\omega}, \frac{1}{2} s \boldsymbol{\omega}+\frac{1}{2} \mathbf{v} \times \boldsymbol{\omega}\right]^{\mathrm{T}} \\&=\mathbf{q} \otimes\left[0, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}}\end{aligned}\end{equation}

则若角速度为 \omega 则四元数对时间的导数为：
\begin{equation}\dot{\mathbf{q}}=\mathbf{q} \otimes\left[0, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}}\end{equation}

参考文献

[1] 《视觉SLAM进阶：从零开始手写VIO》
[2] https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B8
[3] https://www.cnblogs.com/MerakXuan/p/12148013.html
[4] https://fzheng.me/2018/05/22/quaternion-matrix-so3-jacobians/
[5] https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B0%E4%B8%8E%E7%A9%BA%E9%97%B4%E6%97%8B%E8%BD%AC