4、四元数运算：四元数乘法、共轭、归一化、旋转向量与四元数的转换
在IMU姿态识别系统中，四元数（Quaternion）是连接传感器原始数据与姿态解算结果的核心数学工具。与欧拉角相比，四元数避免了万向锁问题，且计算效率高、插值平滑，因此成为AHRS（航姿参考系统）算法中的标准姿态表示方法。本节将系统讲解四元数的基本运算规则，包括乘法、共轭、归一化，以及旋转向量与四元数之间的转换方法，并给出工程实现中的关键注意事项。
4.1 四元数基础与核心运算
4.1.1 四元数的数学定义
四元数是一个超复数，由实部和虚部组成，通常表示为 q = w + xi + yj + zk，其中 w 为实部（标量部分），(x, y, z) 为虚部（向量部分）。在姿态表示中，四元数常写作 q = [w, x, y, z] 或 q = [cos(θ/2), v·sin(θ/2)] 的形式，其中 v 为单位旋转轴，θ 为旋转角度。四元数的模长定义为 ||q|| = √(w² + x² + y² + z²)，单位四元数满足 ||q|| = 1，这是姿态表示的基本约束。
4.1.2 四元数乘法
四元数乘法（Hamilton积）是姿态复合的核心运算。设 q₁ = [w₁, x₁, y₁, z₁]，q₂ = [w₂, x₂, y₂, z₂]，则 q₁ ⊗ q₂ 的计算公式为：
w = w₁w₂ - x₁x₂ - y₁y₂ - z₁z₂
x = w₁x₂ + x₁w₂ + y₁z₂ - z₁y₂
y = w₁y₂ - x₁z₂ + y₁w₂ + z₁x₂
z = w₁z₂ + x₁y₂ - y₁x₂ + z₁w₂在姿态解算中，四元数乘法用于将两次旋转复合为一次等效旋转。例如，若先进行 q₁ 旋转，再进行 q₂ 旋转，则等效旋转为 q = q₂ ⊗ q₁。注意乘法不满足交换律，即 q₁ ⊗ q₂ ≠ q₂ ⊗ q₁，这对应了旋转顺序的不可交换性。
4.1.3 共轭与逆
四元数的共轭定义为 q* = [w, -x, -y, -z]，即虚部取反。对于单位四元数，其逆等于共轭：q⁻¹ = q*。共轭在姿态变换中用于实现旋转的逆操作。例如，若 q 表示从机体坐标系到世界坐标系的旋转，则 q* 表示从世界坐标系到机体坐标系的旋转。在IMU数据处理中，共轭常用于将向量从世界坐标系变换到机体坐标系，或反之。
4.1.4 归一化
归一化是将四元数除以它的模长，使 ||q|| = 1。由于浮点运算的累积误差，长时间运行后四元数会偏离单位模长，导致姿态漂移。因此，在每次姿态更新后必须进行归一化处理：
norm = sqrt(w*w + x*x + y*y + z*z)
w /= norm; x /= norm; y /= norm; z /= norm归一化不仅是数学上的要求，更是保证旋转矩阵正交性和姿态解算稳定性的关键步骤。工程上建议在每次四元数更新后立即执行归一化，避免误差累积。
4.2 旋转向量与四元数的转换
4.2.1 旋转向量定义
旋转向量（轴角表示）由旋转轴单位向量 v = [vx, vy, vz] 和旋转角度 θ 组成，记为 (θ, v)。它直观地描述了刚体绕某轴旋转一定角度的运动。旋转向量与四元数之间存在一一对应关系，转换公式如下：
- 旋转向量 → 四元数：q = [cos(θ/2), vx·sin(θ/2), vy·sin(θ/2), vz·sin(θ/2)]
- 四元数 → 旋转向量：θ = 2·acos(w)，v = [x, y, z] / sin(θ/2)
当 θ 接近 0 或 2π 时，sin(θ/2) 趋近于 0，直接计算会导致数值不稳定。此时应使用泰勒展开近似：v ≈ [x, y, z] / (0.5·θ)，或直接使用四元数的虚部作为旋转向量的近似。
4.2.2 转换的工程实现
在AHRS算法中，旋转向量到四元数的转换常用于将陀螺仪的角速度积分结果转换为姿态增量。设陀螺仪在 Δt 时间内的角速度为 ω = [ωx, ωy, ωz]，则旋转角度 θ = ||ω||·Δt，旋转轴 v = ω / ||ω||。代入转换公式即可得到姿态增量四元数：
// 输入：角速度(rad/s), 时间间隔dt
float theta = sqrt(wx*wx + wy*wy + wz*wz) * dt;
if (theta > 1e-6) {
float half = theta / 2.0f;
float sin_half = sin(half);
q_delta.w = cos(half);
q_delta.x = wx / theta * sin_half;
q_delta.y = wy / theta * sin_half;
q_delta.z = wz / theta * sin_half;
} else {
// 小角度近似
q_delta.w = 1.0f;
q_delta.x = wx * dt / 2.0f;
q_delta.y = wy * dt / 2.0f;
q_delta.z = wz * dt / 2.0f;
}然后通过四元数乘法更新当前姿态：q_current = q_delta ⊗ q_current。这种基于旋转向量的增量更新方式比直接积分四元数微分方程更稳定，且易于实现。
4.3 四元数运算在IMU姿态识别中的应用
4.3.1 姿态表示与坐标系变换
在IMU系统中，四元数主要用于描述机体坐标系（b系）相对于世界坐标系（n系）的旋转关系。设姿态四元数为 q_nb，则任意向量 r 在 n 系和 b 系之间的变换为：
r_b = q_nb ⊗ r_n ⊗ q_nb*其中 ⊗ 表示四元数乘法，q_nb* 为 q_nb 的共轭。这种变换避免了欧拉角变换中的三角函数计算，计算效率更高。在加速度计和磁力计数据融合中，正是利用这种变换将参考向量（重力、地磁）从世界坐标系变换到机体坐标系，与传感器测量值进行比较。
4.3.2 姿态解算中的四元数更新
AHRS算法（如Madgwick、Mahony）的核心是四元数姿态更新。以Madgwick算法为例，其梯度下降法通过最小化目标函数来修正四元数：
// 目标函数：f(q, d, s) = q* ⊗ d ⊗ q - s
// 其中 d 为参考向量（重力/地磁），s 为传感器测量值
// 梯度：∇f = J(q, d)^T · f(q, d, s)
// 修正：q_est = q_est - β · ∇f / ||∇f||这里涉及四元数的乘法、共轭、归一化等运算。β 为梯度下降步长，需要根据传感器噪声特性调整。该算法的优势在于计算量小，适合嵌入式实时系统。
4.3.3 姿态特征提取
在姿态识别任务中，四元数本身即可作为特征输入到机器学习模型。相比欧拉角，四元数特征具有连续性（无角度跳变），且对旋转顺序不敏感。常用的特征构造方式包括：
- 直接使用四元数的四个分量 [w, x, y, z] 作为特征
- 使用四元数差分（相邻时刻的四元数乘法结果）表示角速度
- 将四元数转换为旋转矩阵，提取欧拉角或旋转矩阵元素
- 使用四元数对数映射，将姿态映射到三维向量空间
| 特征类型 | 维度 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|---|
| 原始四元数 | 4 | 连续、无万向锁 | 维度冗余，需归一化 | 深度学习模型输入 |
| 四元数差分 | 4 | 反映角速度变化 | 对噪声敏感 | 动态手势识别 |
| 欧拉角 | 3 | 直观、维度低 | 存在万向锁、角度跳变 | 静态姿态分类 |
| 旋转矩阵 | 9 | 信息完整 | 维度高、冗余大 | 需要完整姿态信息 |
4.4 案例拆解：基于四元数的跌倒检测系统
4.4.1 场景与约束
设计一个基于IMU的老年人跌倒检测系统，要求：① 检测灵敏度 ≥ 95%；② 误报率 ≤ 5%；③ 响应时间 ≤ 500ms；④ 运行在Cortex-M4处理器上，主频 168MHz，RAM 128KB。传感器采用BMI160（加速度计+陀螺仪），采样率 100Hz。
4.4.2 初始现象与问题
初版算法直接使用欧拉角作为特征，发现两个问题：① 当设备接近垂直时，俯仰角接近 ±90°，导致横滚角计算不稳定（万向锁）；② 欧拉角在 ±180° 边界处发生跳变，造成特征不连续，影响分类器性能。跌倒检测的准确率仅为 82%，误报率高达 12%。
4.4.3 四元数方案设计
改用四元数作为姿态表示，具体步骤如下：
- 初始化：上电后静止 2 秒，利用加速度计计算初始姿态四元数 q0。
- 姿态更新：以 100Hz 频率读取陀螺仪数据，使用旋转向量法计算姿态增量四元数，并更新当前姿态。
- 数据融合：每 10 个周期（1Hz）使用加速度计数据通过梯度下降法修正累积漂移。
- 特征提取：计算相邻时刻四元数的差值 Δq = q(t) ⊗ q(t-1)*，提取旋转角度 θ = 2·acos(Δq.w) 和旋转轴方向。
- 跌倒判定：当 θ 在 200ms 内超过 60°，且后续 500ms 内姿态变化小于 10°，判定为跌倒。
4.4.4 结果与复盘
改进后系统在 50 组测试中（20 组跌倒、30 组日常活动）达到：灵敏度 96%，误报率 4%，响应时间 350ms。相比欧拉角方案，准确率提升 14 个百分点，误报率降低 8 个百分点。关键改进点：
- 四元数避免了万向锁问题，在任意姿态下保持数值稳定
- 四元数差分的旋转角度特征比欧拉角差分更平滑，减少了分类器的误判
- 归一化操作保证了姿态解算的长期稳定性，测试 2 小时漂移小于 2°
4.5 常见误区与总结
4.5.1 常见误区
- 忽略归一化：部分开发者认为四元数乘法后模长变化不大，跳过归一化步骤。实际上，经过数百次迭代后，模长偏差可达 5% 以上，导致姿态漂移明显。
- 乘法顺序错误：四元数乘法不满足交换律，q₁ ⊗ q₂ 与 q₂ ⊗ q₁ 表示不同的旋转顺序。在姿态更新中，必须明确是“世界坐标系下的旋转”还是“机体坐标系下的旋转”，选择正确的乘法顺序。
- 小角度近似滥用：当旋转角度较大时（>10°），小角度近似 sin(θ/2) ≈ θ/2 的误差会显著增大，导致姿态解算发散。应设置阈值，仅在角度小于 5° 时使用近似。
- 转换公式符号错误：旋转向量到四元数的转换中，虚部符号取决于旋转方向约定（右手定则）。若符号错误，会导致旋转方向相反，姿态解算结果完全错误。
4.5.2 总结与最佳实践
四元数运算是IMU姿态识别的数学基础，掌握其核心运算规则和转换方法是实现可靠姿态解算的前提。在实际工程中，建议遵循以下最佳实践：
- 始终使用单位四元数表示姿态，每次更新后立即归一化
- 明确旋转顺序约定，在代码注释中标注坐标系定义
- 使用旋转向量法进行姿态增量更新，避免直接积分四元数微分方程
- 在特征提取时，优先使用四元数差分或旋转角度，而非原始四元数分量
- 针对嵌入式平台，使用定点数或快速数学库优化四元数运算