AR眼镜 AHRS 算法技术文档 (二)

详细数学推导

1. 四元数基础

1.1 四元数定义

四元数是一种扩展的复数系统,用于表示三维空间中的旋转:

$q = q_w + q_x i + q_y j + q_z k$

或向量表示: $\\begin{bmatrix} q_w q_x q_y q_z \\end{bmatrix} = \\begin{bmatrix} q_w \\mathbf{q_v} \\end{bmatrix}$

其中:

$q_w$ : 标量部分 (scalar part)
$\\mathbf{q_v} = [q_x, q_y, q_z]^T$ : 向量部分 (vector part)

虚数单位性质: $i^2 = j^2 = k^2 = ijk = -1$

$\\quad jk = i, \\quad ki = j$ $\\quad kj = -i, \\quad ik = -j$

1.2 四元数基本运算

加法

$q_1 + q_2 = \\begin{bmatrix} q_{1w} + q_{2w} q_{1x} + q_{2x} q_{1y} + q_{2y} q_{1z} + q_{2z} \\end{bmatrix}$

乘法 (Hamilton Product)

$q_1 \\otimes q_2 = \\begin{bmatrix} q_{1w}q_{2w} – q_{1x}q_{2x} – q_{1y}q_{2y} – q_{1z}q_{2z} q_{1w}q_{2x} + q_{1x}q_{2w} + q_{1y}q_{2z} – q_{1z}q_{2y} q_{1w}q_{2y} – q_{1x}q_{2z} + q_{1y}q_{2w} + q_{1z}q_{2x} q_{1w}q_{2z} + q_{1x}q_{2y} – q_{1y}q_{2x} + q_{1z}q_{2w} \\end{bmatrix}$

向量形式: $q_1 \\otimes q_2 = \\begin{bmatrix} q_{1w}q_{2w} – \\mathbf{q_{1v}} \\cdot \\mathbf{q_{2v}} q_{1w}\\mathbf{q_{2v}} + q_{2w}\\mathbf{q_{1v}} + \\mathbf{q_{1v}} \\times \\mathbf{q_{2v}} \\end{bmatrix}$

注意: 四元数乘法不满足交换律: $q_1 \\otimes q_2 \\neq q_2 \\otimes q_1$

共轭 (Conjugate)

$q^* = \\begin{bmatrix} q_w -q_x -q_y -q_z \\end{bmatrix} = \\begin{bmatrix} q_w -\\mathbf{q_v} \\end{bmatrix}$

模长 (Norm)

$\\sqrt{q_w^2 + q_x^2 + q_y^2 + q_z^2}$

逆 (Inverse)

$q^{-1} = \\frac{q^*}{||q||^2}$

对于单位四元数 ( $∣∣ q ∣∣ = 1$ ): $q^{-1} = q^*$

AR眼镜 AHRS 算法技术文档 (二)