四元数 | 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