定义

$\mathbf q=w+xi+yj+zk=q_w+\mathbf q_v =\begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix} =\begin{bmatrix} q_w \\ \mathbf q_v \end{bmatrix}$

其中 $i,j,k$ 是虚根，满足

$i^2=j^2=k^2=ijk=-1$

即这里使用了Hamilton规范。由此可以推导出

$ij=-ji=k,\ jk=-kj=i,\ ki=-ik=j$

\ 实部 $q_w\neq 0$ ，虚部 $\mathbf q_v= 0$ 的四元数就等价为普通实数。

实部 $q_w=0$ ，虚部 $\mathbf q_v\neq 0$ 的四元数称为纯四元数。对于纯四元数，常常只使用一个三维矢量 $\mathbf q_v$ 表示。

加法

对应项直接相加即可

$\mathbf p \pm\mathbf q=\begin{bmatrix} p_w \pm q_m\\ \mathbf p_v \pm \mathbf q_v \end{bmatrix}$ $\mathbf p +\mathbf q = \mathbf q +\mathbf p$ $(\mathbf p +\mathbf q)+\mathbf r=\mathbf p +(\mathbf q+\mathbf r)$

乘法

利用定义 $\mathbf q=w+xi+yj+zk$ ，两个四元数相乘，并合并相同虚根的对应项，可以定义四元数的乘法。

$\mathbf p\otimes\mathbf q=\begin{bmatrix} p_wq_w-p_xq_x-p_yq_y-p_zq_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{bmatrix}$

用矢量表示形式，上式可以简化为

$\mathbf p\otimes\mathbf q=\begin{bmatrix} p_wq_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{bmatrix}$

四元数乘法类似于矩阵乘法，不满足交换律

$\mathbf p\otimes\mathbf q\neq\mathbf q\otimes\mathbf p$

但满足结合律和分配律

$(\mathbf p\otimes\mathbf q)\otimes\mathbf r=\mathbf p\otimes(\mathbf q\otimes\mathbf r)$ $\mathbf p\otimes(\mathbf q+\mathbf r)=\mathbf p\otimes\mathbf q+\mathbf p\otimes\mathbf r$ $(\mathbf p+\mathbf q)\otimes\mathbf r=\mathbf p\otimes\mathbf r+\mathbf q\otimes\mathbf r$

\ \ 四元数乘法可以进一步表示为矩阵和矢量的线性相乘，即

$\mathbf p\otimes\mathbf q=[\mathbf p]_L\mathbf q=[\mathbf q]_R\mathbf p$

其中 $[\bullet]_L, [\bullet]_R$ 分别为四元数的左乘矩阵和右乘矩阵，定义为

$[\mathbf q]_L=q_w\mathbf I+\begin{bmatrix} 0 & -\mathbf q_v^T \\ \mathbf q_v & [\mathbf q_v]_{\times} \end{bmatrix}$ $[\mathbf q]_R=q_w\mathbf I+\begin{bmatrix} 0 & -\mathbf q_v^T \\ \mathbf q_v & -[\mathbf q_v]_{\times} \end{bmatrix}$

其中 $[\mathbf q_v]_{\times}$ 代表 $\mathbf q_v$ 的叉乘矩阵，即

$[\mathbf q_v]_{\times}=\begin{bmatrix} 0 & -q_z & q_y \\ q_z & 0 & -q_x \\ -q_y & q_x & 0 \end{bmatrix}$

根据结合律可以推导出

$[\mathbf q]_L[\mathbf p]_R=[\mathbf p]_R[\mathbf q]_L$

幺元

$\mathbf q_1=\begin{bmatrix} 1 \\ \mathbf 0 \end{bmatrix}$ $\mathbf q_1\otimes\mathbf q=\mathbf q\otimes\mathbf q_1=\mathbf q$

共轭

实部不变，对虚部取逆

$\mathbf q^*=\begin{bmatrix} q_w \\ -\mathbf q_v \end{bmatrix}$ $(\mathbf p\otimes\mathbf q)^*=\mathbf q^*\otimes\mathbf p^*$

\ \ 与自己的共轭相乘得到纯实数，即

$\mathbf q\otimes\mathbf q^*=\mathbf q^*\otimes\mathbf q=\begin{bmatrix} q_w^2+q_x^2+q_y^2+q_z^2 \\ \mathbf 0 \end{bmatrix}$

模

$\|\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\|$

\ 模长等于1（ $\|\mathbf q\|=1$ ）的四元数称为单位四元数。

逆

$\mathbf q^{-1}=\frac{\mathbf q^*}{\|\mathbf q\|^2}$ $\mathbf q\otimes\mathbf q^{-1}=\mathbf q^{-1}\otimes\mathbf q=\mathbf q_1=\begin{bmatrix} 1 \\ \mathbf 0 \end{bmatrix}$

交换子

$[\mathbf p, \mathbf q]\overset{\vartriangle}{=}\mathbf p\otimes\mathbf q-\mathbf q\otimes\mathbf p=\begin{bmatrix} 0 \\ 2\mathbf p_v\times\mathbf q_v \end{bmatrix}$

四元数的幂

四元数的幂定义为

$\mathbf q^n=\mathbf q\otimes\mathbf q\otimes...\otimes\mathbf q$

若 $\mathbf q$ 是一个实部为0，虚部为 $\mathbf q_v$ 的纯四元数，并令 $\mathbf q_v=\mathbf u\theta$ ，其中 $\mathbf u$ 是单位矢量，则有循环模式

$\mathbf q_v^2=-\theta^2,\ \mathbf q_v^3=-\mathbf u\theta^3,\ \mathbf q_v^4=\theta^4, \ \mathbf q_v^5=\mathbf u\theta^5, \ \mathbf q_v^6=-\theta^6, \ ...$

指数映射

$e^{\mathbf q}=\sum_{k=0}^{\infty}\frac{1}{k!}\mathbf q^k$

若 $\mathbf q$ 是纯四元数，记为 $\mathbf q_v=\mathbf u\theta$ ，则根据指数映射的定义，上一节中的指数循环模式，以及三角函数的泰勒展开式，可得

$e^{\mathbf u\theta}=\cos\theta+\mathbf u\sin\theta=\begin{bmatrix} \cos\theta \\ \mathbf u\sin\theta \end{bmatrix}$

即纯四元数的指数映射是一个单位四元数。

上式也可以看做欧拉公式 $e^{xi}=\cos x+i\sin x$ 的推广。类似的有

$(\cos\theta+\mathbf u\sin\theta)^n=e^{\mathbf un\theta}=\cos (n\theta)+\mathbf u\sin( n\theta)=\begin{bmatrix} \cos (n\theta) \\ \mathbf u\sin (n\theta) \end{bmatrix}$

由此可知单位四元数的幂（其中 $n\in\mathbb{R}$ ）的计算方法

$\mathbf q^n=\begin{bmatrix} \cos (n\theta) \\ \mathbf u\sin (n\theta) \end{bmatrix}$

另外还有

$(e^{\mathbf u\theta})^*=\begin{bmatrix} \cos\theta \\ -\mathbf u\sin\theta \end{bmatrix}=e^{-\mathbf u\theta}$

\ 普通四元数的指数映射可以将实部和虚部分开处理，即

$e^{\mathbf q}=e^{q_w+\mathbf q_v}=e^{q_w}e^{q_v}=e^{q_w}\begin{bmatrix} \cos\|\mathbf q_v\| \\ \frac{\mathbf q_v}{\|\mathbf q_v\|}\sin\|\mathbf q_v\| \end{bmatrix}$

对数映射

设四元数 $\mathbf q$ 是单位四元数，则可以定义对数映射

$\log\mathbf q=\log(\cos\theta+\mathbf u\sin\theta)=\log(e^{\mathbf u \theta})=\mathbf u\theta=\begin{bmatrix} 0 \\ \mathbf u\theta \end{bmatrix}$

即单位四元数的对数映射是一个纯四元数。

其中 $\mathbf u=\mathbf q_v/\|\mathbf q_v\|$ ， $\theta=\arctan(\|\mathbf q_v\|/q_w)=\arccos(q_w)$ 。

\ 普通四元数的对数映射同样按照分离实部和虚部的思路，有

$\log\mathbf q=\log(\|\mathbf q\|\frac{\mathbf q}{\|\mathbf q\|})=\log\|\mathbf q\|+\log\frac{\mathbf q}{\|\mathbf q\|}=\log\|\mathbf q\|+\mathbf u\theta=\begin{bmatrix} \log\|\mathbf q\| \\ \mathbf u\theta \end{bmatrix}$

单位四元数与旋转

定义

单位四元数可用于表示旋转。

设旋转的轴为 $\mathbf u$ 且 $\|\mathbf u\|=1$ ，旋转角度为 $\phi$ ，则该旋转可用四元数表示为

$\mathbf q=\begin{bmatrix} \cos{\frac{\phi}{2}} \\ \mathbf u\sin{\frac{\phi}{2}} \end{bmatrix}$

要用四元数对某个三维矢量 $\mathbf x$ 进行旋转，首先将 $\mathbf x$ 变为纯四元数，即

$\mathbf x \leftarrow \begin{bmatrix} 0 \\ \mathbf x \end{bmatrix}$

然后

$\mathbf q\otimes\mathbf x\otimes \mathbf q^*$

可以证明上式的结果也是一个纯四元数，虚部对应旋转后的矢量。

从上式也可得出， $\mathbf q$ 与 $-\mathbf q$ 表示同一个旋转。

与轴角的关系

单位四元数与轴角可以通过指数映射和对数映射建立联系。

设某个旋转通过轴角表示为三维矢量 $\mathbf a=\mathbf u\phi$ ，其中 $\mathbf u$ 是单位矢量。则

$\text{Exp}(\mathbf a)\overset{\vartriangle}{=}e^{\mathbf a/2}=e^{\mathbf u\phi/2}=\begin{bmatrix} \cos{\frac{\phi}{2}} \\ \mathbf u\sin{\frac{\phi}{2}} \end{bmatrix}=\mathbf q$ $\text{Log}(\mathbf q)\overset{\vartriangle}{=}2\log(\mathbf q)=2(\mathbf u\frac{\phi}{2})=\mathbf u\phi = \mathbf a$

需要注意的是，一般而言

$\text{Exp}(\mathbf a+\mathbf b)\neq\text{Exp}(\mathbf a)\otimes\text{Exp}(\mathbf b)$

仅当 $\mathbf a,\mathbf b$ 旋转轴相同时，等式才成立，且满足交换律。

\ 当有轴角是小量（ $\delta\theta$ ）时，存在如下关系：

$\begin{aligned} \text{Exp}(\theta+\delta\theta)&\simeq\text{Exp}(\theta)\otimes\text{Exp}(J_r(\theta)\delta\theta) \\ \text{Exp}(\theta)\otimes\text{Exp}(\delta\theta)&\simeq\text{Exp}(\theta+J_r^{-1}(\theta)\delta\theta) \end{aligned}$

其中

$\begin{aligned} J_r(\theta)&=I-\frac{1-\cos\Vert\theta\Vert}{\Vert\theta\Vert^2}+\frac{\Vert\theta\Vert-\sin\Vert\theta\Vert}{\Vert\theta\Vert^3}[\theta]_{\times}^2 \\ J_r^{-1}(\theta)&=I+\frac{1}{2}[\theta]_{\times}+\Big(\frac{1}{\Vert\theta\Vert^2}-\frac{1+\cos\Vert\theta\Vert}{2\Vert\theta\Vert\sin\Vert\theta\Vert}\Big)[\theta]_{\times}^2 \end{aligned}$

需要指出的是，以上小量关系是正交群SO(3)的内在关系，与旋转的表示形式无关，即无论使用四元数还是旋转矩阵来表示旋转，上述关系都存在。使用旋转矩阵表示时，四元数乘法就变为普通矩阵乘法，上面的Exp映射就是罗德里格斯(Rodrigues)变换：

$\mathbf R = \text{Exp}(\mathbf a)\overset{\vartriangle}{=}\cos\phi\mathbf I+(1-\cos\phi)\mathbf u\mathbf u^T + \sin\phi[\mathbf u]_{\times}$

与旋转矩阵的关系

根据

$\mathbf q\otimes\mathbf x\otimes \mathbf q^*=[\mathbf q^*]_R[\mathbf q]_L\mathbf x=[\mathbf q]_L[\mathbf q^*]_R\mathbf x=\begin{bmatrix} 0 \\ \mathbf R\mathbf x \end{bmatrix}$

可得

$\mathbf R\{\mathbf q\}\overset{\vartriangle}{=}\mathbf R=(q_w^2-\mathbf q_v^T\mathbf q_v)\mathbf I+2\mathbf q_v\mathbf q_v^T+2q_w[\mathbf q_v]_{\times}$

以及一些常用性质

$\mathbf R\{-\mathbf q\}=\mathbf R\{\mathbf q\}$ $\mathbf R\{\mathbf q^*\}=\mathbf R\{\mathbf q\}^T$ $\mathbf R\{\mathbf p\otimes\mathbf q\}=\mathbf R\{\mathbf p\}\mathbf R\{\mathbf q\}$ $\mathbf R\{\mathbf q^n\}=\mathbf R\{\mathbf q\}^n$

四元数的导数

对时间求导

在动力学系统中，用四元数表示某个物体的姿态（旋转量）时，四元数是时间的函数，则其关于时间的变化率（导数）为

$\mathbf {\dot{q}}=\frac{1}{2}\mathbf q\otimes\mathbf \omega=\frac{1}{2}[\mathbf \omega]_R\mathbf q$

其中 $\mathbf \omega$ 是局部（Body）坐标系下物体的角速度，是一个轴角表示的三维矢量，带入上式计算时要先变为纯四元数。

如果角速度表示在全局（global）坐标系下，上式变为

$\mathbf {\dot{q}}=\frac{1}{2}\mathbf\omega\otimes\mathbf q=\frac{1}{2}[\mathbf \omega]_L\mathbf q$

四元数的乘积对时间求导满足二元运算求导法则，即

$\dot{(\mathbf p \otimes\mathbf q)}=\dot{\mathbf p} \otimes\mathbf q+\mathbf p \otimes\dot{\mathbf q}$

对四元数求导

对四元数求导可以利用矢量求导法则，比如

$\frac{\partial(\mathbf p \otimes\mathbf q)}{\partial\mathbf p}=\frac{\partial([\mathbf q]_R\mathbf p )}{\partial\mathbf p}=[\mathbf q]_R$

Reference

Quaternion kinematics for the error-state Kalman filter

四元数速查手册

Quaternion

定义

加法

乘法

幺元

共轭

模

逆

交换子