四元素基础理论及其应用

四元素与旋转表示

原创已于 2025-07-14 00:25:48 修改 · 2k 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#几何学 #算法

于 2020-03-28 17:40:36 首次发布

数学基础同时被 2 个专栏收录

9 篇文章

订阅专栏

VSLAM

3 篇文章

订阅专栏

在阅读《Indirect Kalman Filter for 3D Attitude Estimation》一文时，发现文中对四元素的定义采用了非Hamilton方式，阅读有一些冲突，难以转换，至此，决定将该文章转换为Hamilton的表达形式。

1 四元素定义：
$\bm{q} = q_w + q_x \bm{i} + q_y\bm{j} + q_z\bm{k} \qquad (1)$
其中，虚数单位： $i\bm{i}$ ， $j\bm{j}$ ， $k\bm{k}$ 满足如下关系：
$\bm{i}^2 = \bm{j}^2 = \bm{k}^2 = \bm{ijk} = -1\qquad(2)$
从而：
$\bm{ij} = -\bm{ji} = \bm{k},\quad \bm{jk} = -\bm{kj} = \bm{i}, \quad \bm{ki} = -\bm{ik} = \bm{j} \qquad (3)$
此外，四元数还可以表达成如下式所示的形式：

常量+向量形式：
$\bm{q} = q_w + \bm{q}_v \qquad(4)$
其中： $qv=qxi+qyj+qzk\bm{q}_v = q_x \bm{i} + q_y\bm{j} + q_z\bm{k}$

向量形式
$\bm{q} = \begin{bmatrix} q_w\\ \bm{q}_v \end{bmatrix} = \begin{bmatrix}q_w\\ q_x \\ q_y \\ q_z \end{bmatrix} \qquad(5)$

2 四元素的主要性质
2.1 加法运算
$\bm{q}_1 \pm \bm{q}_2 = \begin{bmatrix}q_{w1} \\ \bm{q}_{v1} \end{bmatrix} \pm \begin{bmatrix}q_{w2} \\ \bm{q}_{v2} \end{bmatrix} = \begin{bmatrix}q_{w1} \pm q_{w2} \\ \bm{q}_{v1} \pm \bm{q}_{v2} \end{bmatrix} \qquad (6)$
加法运算满足交换律和结合律：
$\bm{q}_1 + \bm{q}_2 = \bm{q}_2 + \bm{q}_1 \qquad (7)$
$\bm{q}_1 + (\bm{q}_2 + \bm{q}_3) = (\bm{q}_1 + \bm{q}_2) + \bm{q}_3 \qquad(8)$

2.2 四元素数乘
$k\bm{q} = \begin{bmatrix} kq_w \\ k\bm{q}_v \end{bmatrix} = \begin{bmatrix} kq_w\\ kq_x\\ kq_y\\ kq_z \end{bmatrix} \qquad (9)$

2.3 四元素点乘
点乘指的是两个四元素每个对应位置上的数值相乘再求和，即：
$\bm{q} \cdot \bm{p} = q_w p_w + q_xp_x + q_yp_y + q_zp_z \qquad(10)$

2.4 乘法运算 $⊗\otimes$
$\bm{p} \otimes \bm{q} = \begin{bmatrix} p_w q_w - p_x q_x - p_y q_y - p_z q_z \\ p_w q_x + p_x q_w + p_y q_z - p_z q_y \\ p_w q_y - p_x q_z + p_y q_w + p_z q_x \\ p_w q_z + p_xq_y - p_y q_x + p_z q_w \end{bmatrix} = \begin{bmatrix} p_w q_w - \bm{p}_v ^\top \bm{q}_v\\ p_w \bm{q}_v + q_w \bm{p}_v + \bm{p}_v \times\bm{q}_v \end{bmatrix} \quad (11)$
因（11）式中含有叉乘项，故四元素乘法不满足交换律，即:
$\bm{p} \otimes \bm{q} \neq \bm{q} \otimes \bm{p} \qquad (12)$
例外：当 $pv×qv=0\bm{p}_v \times \bm{q}_v = 0$ 时，叉乘项为零时满足交换律，此时， $pv=0\bm{p}_v=0$ 或 $qv=0\bm{q}_v=0$ 或 $pv∣∣qv\bm{p}_v||\bm{q}_v$ (即两个向量平行)。

结合律：
$(\bm{q}_1 \otimes \bm{q}_2) \otimes \bm{q}_3 = \bm{q}_1 \otimes (\bm{q}_2 \otimes \bm{q}_3) \qquad(13)$

分配律:
$\bm{q}_1 \otimes (\bm{q}_2 + \bm{q}_3) = \bm{q}_1 \otimes \bm{q}_2 + \bm{q}_1 \otimes \bm{q}_3 \qquad(14)$
$(\bm{q}_1 + \bm{q}_2) \otimes \bm{q}_3 = \bm{q}_1 \otimes \bm{q}_3 + \bm{q}_2 \otimes \bm{q}_3 \qquad(15)$

矩阵乘法形式:
根据（11）式，提取出相应的四元素向量，便可以得到：
$\bm{q}_1 \otimes \bm{q}_2 = [\bm{q}_1]_L \bm{q}_2=\begin{bmatrix} q_{w1} & -q_{x1} & -q_{y1} & -q_{z1} \\ q_{x1} & q_{w1} & -q_{z1} & q_{y1} \\ q_{y1} & q_{z1} & q_{w1} & -q_{x1} \\ q_{z1} & -q_{y1} & q_{x1} & q_{w1} \end{bmatrix} \bm{q}_2 \qquad (16)$
$\bm{q}_1 \otimes \bm{q}_2 = [q_2]_R \bm{q}_1 = \begin{bmatrix} q_{w2} & -q_{x2} & -q_{y2} & -q_{z2} \\ q_{x2} & q_{w2} & q_{z2} & -q_{y2} \\ q_{y2} & -q_{z2} & q_{w2} & q_{x2} \\ q_{z2} & q_{y2} & -q_{x2} & q_{w2} \end{bmatrix} \bm{q}_1 \qquad (17)$
根据（16）和（17）可以得到：
$[\bm{q}]_L = \begin{bmatrix} 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{bmatrix}, \quad [\bm{q}]_R = \begin{bmatrix} 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{bmatrix} \bm{q}_1 \qquad (18)$
进一步可以表示为：
$[\bm{q}]_L = q_w \mathbf{I} + \begin{bmatrix} 0 & -\bm{q}_v^\top \\ \bm{q}_v & \bm{q}_v^\land \end{bmatrix}, \qquad [\bm{q}]_R = q_w \mathbf{I} + \begin{bmatrix} 0 & -\bm{q}_v^\top \\ \bm{q}_v & -\bm{q}_v^\land \end{bmatrix} \qquad (19)$
其中：
$\bm{q} ^ \land = \begin{bmatrix} 0 & -q_z & q_y \\ q_z & 0 & -q_x \\ -q_y & q_x & 0 \end{bmatrix} \qquad (20)$
是反对称矩阵，即 $q∧=−[q∧]⊤\bm{q}^\land = -[\bm{q}^\land]^\top$ ，可用于向量间的叉乘运算：
$\bm{a} \times \bm{b} = \bm{a}^\land\bm{b}, \quad \forall \bm{a}, \bm{b} \in \mathbb{R}^3 \qquad (21)$

因为：
$(\bm{q} \otimes \bm{x}) \otimes \bm{p} = [\bm{p}]_R[\bm{q}]_L\bm{x} \quad and \quad \bm{q} \otimes(\bm{x} \otimes \bm{p})=[\bm{q}]_L[\bm{p}]_R\bm{x} \qquad(22)$
从而，根据四元素乘法的结合律得：
$[\bm{p}]_R[\bm{q}]_L = [\bm{q}]_L[\bm{p}]_R \qquad (23)$

简化写法：
$[\bm{q}]_L = [\bm{q} \quad \mathbf{\Psi}(\bm{q})] \qquad(24)$
其中：
$\mathbf{\Psi}(\bm{q}) = \begin{bmatrix} -\bm{q}_v^\top \\ q_w\mathbf{I}_{3\times3} + \bm{q}_v^\land \end{bmatrix} \qquad (25)$
同理：
$[\bm{q}]_R = [\bm{q} \quad \mathbf{\Xi}(\bm{q})] \qquad(26)$
其中：
$\mathbf{\Xi}(\bm{q}) = \begin{bmatrix} -\bm{q}_v^\top \\ q_w\mathbf{I}_{3\times3} - \bm{q}_v^\land \end{bmatrix} \qquad (27)$

此外，对于unit quaternion $q\bm{q}$ 有：
$[\bm{q}^{-1}]_L = [\bm{q}]_L^\top \qquad (28)$
$[\bm{q}^{-1}]_R = [\bm{q}]_R^\top \qquad (29)$

2.5 Identity quaternion
$\bm{q}_1 = 1 = \begin{bmatrix} 1 \\ \bm{0}_v \end{bmatrix} \qquad (30)$
满足：
$\bm{q}_1 \otimes \bm{q} = \bm{q} \otimes \bm{q}_1 = \bm{q} \qquad(31)$

2.6 共轭
四元素的共轭为：
$\bm{q}^* = \begin{bmatrix} q_w\\ -\bm{q}_v \end{bmatrix} \qquad(32)$
共轭四元素的性质:
$\bm{q} \otimes \bm{q}^* = \bm{q}^* \otimes \bm{q} = \begin{bmatrix} q_w^2 + q_x^2 + q_y^2 + q_z^2 \\ \bm{0}_v \end{bmatrix} \qquad (32)$

以及：
$(\bm{p} \otimes \bm{q})^* = \bm{q}^* \otimes \bm{p}^* \qquad (33)$

2.7 四元素的模
定义:
$||\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} \qquad (34)$

模的性质:
$||\bm{p} \otimes \bm{q}|| = ||\bm{q} \otimes \bm{p}|| = ||\bm{p}|| ||\bm{q}|| \qquad (35)$
简单推导：
$||\bm{p} \otimes \bm{q}||^2 = (\bm{p} \otimes \bm{q}) \otimes (\bm{p} \otimes\bm{q})^* = \bm{p} \otimes\bm{q}\otimes\bm{q}^*\otimes\bm{p}^* = \bm{p}\otimes(\bm{q}\otimes\bm{q}^*)\otimes\bm{p}^*=\bm{p} \otimes\begin{bmatrix} ||\bm{q}||^2 \\ \bm{0}_v \end{bmatrix} \otimes\bm{p}^* = \bm{p} \otimes \bm{p}^* \otimes \begin{bmatrix} ||\bm{q}||^2 \\ \bm{0}_v \end{bmatrix} = (||\bm{p}||||\bm{q}||)^2$

2.8 四元素的逆
四元素的逆记为 $q−1\bm{q}^{-1}$ ,，其满足如下条件：
$\bm{q} \otimes \bm{q}^{-1} = \bm{q}^{-1} \otimes \bm{q} = \bm{q}_1 \qquad(36)$
其中 $q1\bm{q}_1$ 为"Identity quaternion“，逆通过下式计算：
$\bm{q}^{-1} = \bm{q}^*/||\bm{q}||^2 \qquad (37)$

2.9 Unit or normalized quaternion
对于unit quaternion有， $∣∣q∣∣=1||\bm{q}|| = 1$ ，所以：
$\bm{q}^{-1} = \bm{q}^* \qquad(38)$

3 旋转四元素
形式：
$\bm{q} = \begin{bmatrix} \cos (\theta / 2) \\ \bm{u} \sin (\theta/2) \end{bmatrix} = \begin{bmatrix} \cos (\theta/2) \\ u_x \sin (\theta/2) \\ u_y \sin (\theta/2) \\ u_z \sin (\theta/2) \end{bmatrix} \qquad (39)$
其中，旋转轴为 $u\bm{u}$ ，为单位向量，旋转角度为 $θ\theta$ ，将向量 $x\bm{x}$ 绕 $u\bm{u}$ 轴旋转 $θ\theta$ 即可表示为:
$\bm{x}' = \bm{q} \otimes \bm{x} \otimes \bm{q}^* \qquad (40)$
可以验证，旋转四元素为unit quaternion，跟unit quaternion一样具有如下性质：
$(\bm{p} \otimes \bm{q})^{-1} = \bm{q}^{-1}\otimes\bm{p}^{-1} \qquad(41)$

在介绍四元素基本的定义及相关性质之后，将对《Indirect Kalman Filter for 3D Attitude Estimation》一文当中的相关内容进行转义。

》1.3 Userful Identites
》1.3.1 叉乘反对称矩阵的相关属性
Anti-Commutativity
$\bm{w}^\land = -[\bm{w}^\land]^\top \qquad(42)$
$\bm{a}^\land\bm{b} = -\bm{b}^\land\bm{a} \quad \Leftrightarrow \quad -\bm{b}^\top \bm{a}^\land = \bm{a}^\top\bm{b}^\land \qquad (43)$

Distributivity orver Addition
$\bm{a}^\land + \bm{b}^\land = (\bm{a} + \bm{b})^\land \qquad(44)$

数乘
$\cdot \bm{w}^\land = (c\bm{w})^\land \qquad(45)$

平行向量的叉乘
$\bm{w} \times (c\bm{w}) = c \cdot\bm{w}^\land\bm{w} = -c \cdot(\bm{w}^\top\bm{w}^\land)^\top = \bm{0}_{3\times1} \qquad(46)$

拉格朗日公式
$\bm{a}^\land\bm{b}^\land=\bm{b}\bm{a}^\top-(\bm{a}^\top\bm{b})\mathbf{I}_{3\times3} \qquad (47)$
$\Leftrightarrow \bm{a} \times (\bm{b} \times \bm{c}) = \bm{b}(\bm{a}^\top\bm{c}) - \bm{c}(\bm{a}^\top\bm{b}) \qquad (48)$

$(\bm{a} \times \bm{b})^\land = \bm{b}\bm{a}^\top-\bm{a}\bm{b}^\top \qquad (49)$

Jacobi Identity
$\bm{a} \times (\bm{b} \times \bm{c}) + \bm{b} \times (\bm{c} \times \bm{a}) + \bm{c} \times (\bm{a} \times \bm{b}) = \bm{0} \qquad (49)$
$\qquad$ or
$\bm{a}^\land\bm{b}^\land\bm{c} + \bm{b}^\land\bm{c}^\land\bm{a} + \bm{c}^\land\bm{a}^\land\bm{b}= \bm{0} \qquad(50)$

Rotations
$(\bm{R}\bm{a})^\land = \bm{R}\bm{a}^\land\bm{R}^\top \qquad (51) \\ proof.\quad (\bm{Ra}) \times \bm{b} = (\bm{Ra}) \times (\bm{RR^\top b}) = \bm{R}[\bm{a} \times(\bm{R^\top b})]=\bm{R}\bm{a}^\land\bm{R}^\top\bm{b} \\ \Rightarrow (\bm{Ra})^\land = \bm{Ra}^\land\bm{R}^\top$
$\bm{R}(\bm{a} \times \bm{b}) = (\bm{R}\bm{a}) \times (\bm{R}\bm{b}) \qquad(52)$

叉乘反对称矩阵
$(\bm{w}^\land)^2 = \bm{ww}^\top - |\bm{w}|^2 \mathbf{I} \qquad(53)$

$\begin{aligned} (\bm{w}^\land)^3 & = (\bm{ww}^\top - |\bm{w}|^2\mathbf{I})\bm{w}^\land\\ & = \bm{w}\bm{w}^\top\bm{w}^\land-|\bm{w}|^2\bm{w}^\land\\ & =\bm{w}(-\bm{w}^\land\bm{w})^\top - |\bm{w}|^2\bm{w}^\land\\ & = -|\bm{w}|^2\bm{w}^\land \end{aligned} \qquad (54)$

$\begin{aligned} (\bm{w}^\land)^4 & = (\bm{w}^\land)^3\bm{w}^\land\\ & = -|\bm{w}|^2(\bm{w}^\land)^2 \end{aligned} \qquad (55)$

$\begin{aligned} (\bm{w}^\land)^5 & = (\bm{w}^\land)^4\bm{w}^\land \\ &=-|\bm{w}|^2(\bm{w}^\land)^3\\ &=-|\bm{w}|^2(-|\bm{w}|^2\bm{w}^\land)\\ &=|\bm{w}|^4\bm{w}^\land \qquad (56) \end{aligned}$

$\begin{aligned} (\bm{w}^\land)^6 & = (\bm{w}^\land)^5\bm{w}^\land\\ &= |\bm{w}|^4(\bm{w}^\land)^2 \qquad(57) \end{aligned}$

$\begin{aligned} (\bm{w}^\land)^7 &= (\bm{w}^\land)^6\bm{w}^\land\\ &=|\bm{w}|^4(\bm{w}^\land)^3\\ &=|\bm{w}|^4(-|\bm{w}|^2\bm{w}^\land)\\ &=-|\bm{w}|^6\bm{w}^\land \qquad(58)\\ &...... \end{aligned}$

》1.3.2 Properties of the matrix $Ω\mathbf{\Omega}$
$Ω\mathbf{\Omega}$ 矩阵出现在一个向量与四元素的乘积当中，可用于四元素的求导，它具有以下性质：
$\begin{aligned} \mathbf{\Omega}(\bm{w}) &=\begin{bmatrix} 0 & -w_x & -w_y & -w_z\\ w_x & 0 & w_z & -w_y\\ w_y & -w_z& 0 & w_x \\ w_z & w_y & -w_x & 0 \end{bmatrix}\\ & = \begin{bmatrix} 0 & -\bm{w}^\top\\ \bm{w} & -\bm{w}^\land \end{bmatrix} \end{aligned} \qquad (59)$

$\begin{aligned} \mathbf{\Omega}(\bm{w})^2 & = \begin{bmatrix} -\bm{w}^\top\bm{w} & \bm{w}^\top\bm{w}^\land\\ -\bm{w}^\land\bm{w} & -\bm{ww}^\top+\bm{w}^\land\bm{w}^\land\\ \end{bmatrix}\\ &=\begin{bmatrix} -||\bm{w}||^2 & \mathbf{0}_{1\times3} \\ \mathbf{0}_{3\times1}& -||\bm{w}||^2\mathbf{I}_{3\times3} \end{bmatrix}\\ &=-||\bm{w}||^2\mathbf{I}_{4\times4} \qquad\qquad\qquad\qquad (60) \end{aligned}$

$\mathbf{\Omega}(\bm{w})^3 = -||\bm{w}||^2\mathbf{\Omega}(\bm{w}) \qquad(61)$

$\mathbf{\Omega}(\bm{w})^4 = ||\bm{w}||^4\mathbf{I}_{4\times4} \qquad(62)$

$\mathbf{\Omega}(\bm{w})^5 = ||\bm{w}||^4\mathbf{\Omega}(\bm{w}) \qquad(63)$

$\mathbf{\Omega}(\bm{w})^6 = -||\bm{w}||^6\mathbf{I}_{4\times4} \qquad(64)$

1.3.3 Properties of the matrix $Ξ\mathbf{\Xi}$

$\mathbf{\Psi}(\bm{q}) = \begin{bmatrix} -\bm{q}_v^\top \\ q_w\mathbf{I}_{3\times3} + \bm{q}_v^\land \end{bmatrix}, \quad\mathbf{\Psi}^\top(\bm{q})=[-\bm{q}_v\quad q_w\mathbf{I}_{3\times3}-\bm{q}_v^\land] \qquad (65)$

$\begin{aligned} \Rightarrow \qquad \mathbf{\Psi}^\top(\bm{q})\mathbf{\Psi}(\bm{q}) & = ||\bm{q}||^2\mathbf{I}_{3\times3} \qquad(66)\\ \mathbf{\Psi}(\bm{q})\mathbf{\Psi}^\top(\bm{q}) &= ||\bm{q}||\mathbf{I}_{4\times4}-\bm{q}\bm{q}^\top\qquad(67)\\ \mathbf{\Psi}^\top(\bm{q})\bm{q}&=\mathbf{0}_{3\times1} \qquad (68) \end{aligned}$

$Ξ\mathbf{\Xi}$ 与 $Ω\mathbf{\Omega}$ 之间的关系为：
$\mathbf{\Omega}(\bm{w})\bm{q}=\mathbf{\Psi}(\bm{q})\bm{w} \qquad(69)$

》1.4 四元素与旋转矩阵之间的关系
给定旋转四元素 $q=cos⁡(θ/2)+usin⁡(θ/2)\bm{q}=\cos(\theta/2)+\bm{u}\sin(\theta/2)$ ，对应的旋转矩阵记为 $R(q)\mathbf{R}(\bm{q})$ ，则
$\mathbf{R}(\bm{q})=(2q_w^2-1)\mathbf{I} + 2\bm{q}_v\bm{q}_v^\top + 2q_w\bm{q}_v^\land \qquad(70)$
其中：
$q_w = \cos(\theta/2), \quad\bm{q}_v=\bm{u}\sin(\theta/2)$
或者表示为：
$\mathbf{R}(\bm{q})=\mathbf{\Xi}^\top(\bm{q})\mathbf{\Psi}(\bm{q}) \qquad(71)$

或者表示为：
$\mathbf{R}(\bm{q}) = \begin{bmatrix} 1-2q_y^2-2q_z^2 & 2q_xq_y - 2q_wq_z & 2q_xq_z + 2q_wq_y \\ 2q_xq_y+2q_wq_z & 1-2q_x^2-2q_z^2 & 2q_yq_z-2q_wq_x\\ 2q_xq_z-2q_wq_y& 2q_yq_z+2q_wq_x& 1-2q_x^2-2q_y^2 \end{bmatrix} \qquad(72)$

旋转矩阵的乘法与旋转四元素的乘法之间的关系：
$\mathbf{R}(\bm{^0q_1})\mathbf{R}(^1\bm{q}_2) = \mathbf{R}(^0\bm{q}_1 \otimes {^1}\bm{q}_2) \qquad(73)$
指数映射
$\mathbf{R}(\bm{q}) = exp(\theta\bm{u}^\land) \qquad (74)$
》1.5 四元素对时间的导数