3、坐标系与姿态表示：载体坐标系、世界坐标系、欧拉角、四元数、旋转矩阵

IMU（惯性测量单元）输出的原始数据是加速度计、陀螺仪和磁力计在载体坐标系下的测量值。然而，步数计数和运动检测算法最终需要的是设备在世界坐标系中的运动轨迹、姿态变化或步态特征。因此，理解坐标系定义以及姿态的数学表示方法，是连接原始传感器数据与高级运动语义的桥梁。本章将系统梳理载体坐标系与世界坐标系的定义、欧拉角与四元数的数学原理、旋转矩阵的构建方法，并重点分析在步数计数场景下如何选择合适的姿态表示方式。

在IMU算法开发中，坐标系与姿态表示是整个信号处理链路的基石。如果坐标系定义不清晰，后续的滤波、积分、步态特征提取都会产生系统性偏差。本节将深入剖析这些核心概念，并给出在步数计数场景下的工程选型建议。

3.1 载体坐标系与世界坐标系的定义与转换

3.1.1 载体坐标系（Body Frame, b系）

载体坐标系是固定于IMU传感器（或设备）本身的直角坐标系。其原点通常位于传感器的几何中心或芯片的参考点。在消费级设备（如手机、手环）中，载体坐标系的轴向定义遵循右手定则：Xb指向设备的右方（对于手机，为屏幕横向向右），Yb指向设备的前方（屏幕纵向向上），Zb垂直于屏幕平面指向设备正面（即屏幕朝上时Zb指向天空）。加速度计和陀螺仪输出的三轴分量（ax, ay, az）和（gx, gy, gz）均是在此坐标系下的投影值。

需要特别强调的是，当设备佩戴在手腕或放在裤兜中时，载体坐标系相对于人体是不断变化的。例如，手臂摆动时，手环的Xb轴可能从水平变为垂直。因此，直接对载体坐标系下的加速度进行阈值检测（如检测峰值）虽然简单，但极易受到设备姿态变化的影响，导致误检。

3.1.2 世界坐标系（Navigation Frame, n系）

世界坐标系（又称导航坐标系）是固定于地球表面的参考系。在步数计数应用中，我们通常采用东北天（ENU）坐标系：Xn指向地理东向，Yn指向地理北向，Zn指向天顶（即重力加速度的反方向）。世界坐标系的Zn轴方向与重力方向对齐，这使得在n系下，静止时加速度计的理论输出应为(0, 0, 1g)（归一化后）。

建立世界坐标系的意义在于：将载体坐标系下的测量值投影到n系后，我们可以将重力分量与人体运动加速度分离。步态的本质是人体质心在垂直方向（Zn轴）上的周期性起伏，因此，在n系下分析垂直方向的加速度波形，比在b系下分析任意轴向的波形要稳定得多。

3.1.3 坐标系转换的数学本质

从b系到n系的转换是一个三维旋转映射。假设b系中的向量为 v^b，n系中的向量为 vⁿ，则存在一个旋转矩阵 R_b→n 使得 vⁿ = R_b→n · v^b。该矩阵是正交矩阵（满足 R^TR = I），其行列式为+1。旋转矩阵的构建可以通过欧拉角、四元数或轴角表示法获得。

3.2 欧拉角：直观但存在万向锁风险

3.2.1 定义与旋转顺序

欧拉角通过三个绕不同坐标轴的旋转角来描述姿态，通常定义为：横滚角（Roll, φ）绕Xb轴旋转，俯仰角（Pitch, θ）绕Yb轴旋转，偏航角（Yaw, ψ）绕Zb轴旋转。旋转顺序对最终姿态有决定性影响，工程中常用Z-Y-X（偏航-俯仰-横滚）顺序，即先绕Z轴旋转ψ，再绕新Y轴旋转θ，最后绕新X轴旋转φ。

欧拉角的物理意义直观，便于调试人员理解当前设备的朝向。例如，当手机平放时，Roll≈0°，Pitch≈0°，Yaw为任意值。当手机竖立时，Pitch≈90°。

3.2.2 万向锁问题

当Pitch角接近±90°时，第一次旋转（Yaw）与第三次旋转（Roll）的旋转轴会重合，导致系统丢失一个旋转自由度，即产生万向锁。在万向锁状态下，Yaw和Roll的角速度无法唯一确定，姿态解算会出现奇异值。在步数计数场景中，如果用户将手机放在裤兜且弯腰（Pitch接近90°），则欧拉角表示会导致陀螺仪积分发散。

3.3 四元数：无奇异性的姿态表示

3.3.1 四元数定义

四元数是由一个实数部分和三个虚数部分组成的超复数，形式为 q = q0 + q1·i + q2·j + q3·k，其中满足 i² = j² = k² = i·j·k = -1。对于单位四元数（模长为1），它可以唯一表示三维空间中的旋转。其与旋转轴（单位向量 u = [ux, uy, uz]）和旋转角度 θ 的关系为：

q0 = cos(θ/2)
q1 = ux * sin(θ/2)
q2 = uy * sin(θ/2)
q3 = uz * sin(θ/2)

四元数避免了欧拉角的万向锁问题，且仅需4个参数（相比旋转矩阵的9个参数），计算效率更高。四元数的乘法运算可以复合旋转，且归一化操作简单（只需除以模长），非常适合在嵌入式MCU上实时运行。

3.3.2 四元数更新与坐标变换

在IMU姿态解算中，我们使用陀螺仪的角速度（ωx, ωy, ωz）对四元数进行时间积分更新。离散更新公式为：

q_new = q_old + 0.5 * dt * q_old ⊗ ω_vec

其中 ⊗ 表示四元数乘法，ω_vec = [0, ωx, ωy, ωz]。更新后需对四元数进行归一化处理，防止模长漂移。

利用四元数将b系向量转换到n系的公式为：

v_n = q ⊗ v_b ⊗ q*

其中 q* 是 q 的共轭四元数（实部不变，虚部取反）。在代码实现中，通常直接使用旋转矩阵的等价形式（由四元数元素计算得出）来避免重复的四元数乘法。

3.4 旋转矩阵：数学严谨但计算开销大

旋转矩阵是一个3×3的正交矩阵，其列向量分别表示b系各轴在n系中的方向余弦。由四元数构建旋转矩阵的公式如下：

R = | 1-2(q2²+q3²)   2(q1q2 - q0q3)   2(q1q3 + q0q2) |
    | 2(q1q2 + q0q3)   1-2(q1²+q3²)   2(q2q3 - q0q1) |
    | 2(q1q3 - q0q2)   2(q2q3 + q0q1)   1-2(q1²+q2²) |

旋转矩阵的优点是可直接用于向量旋转（矩阵乘法），且便于进行误差分析。但其缺点是参数冗余（9个元素），且必须满足正交性约束，在数值积分过程中容易因误差累积导致矩阵非正交，需要频繁进行正交化修正（如Gram-Schmidt过程），计算成本较高。

在步数计数算法中，旋转矩阵通常不直接作为状态量，而是作为四元数解算后的输出形式，用于一次性计算加速度的n系投影。

3.5 姿态表示方法对比与选型

特性维度	欧拉角	四元数	旋转矩阵
参数数量	3	4	9
万向锁问题	存在（Pitch=±90°）	不存在	不存在
计算开销（MCU）	低（但需处理三角函数）	低（仅乘加运算）	高（需正交化）
直观性	高	低	中
适合场景	静态姿态显示、简单阈值判断	实时姿态解算、数据融合	坐标变换计算、离线分析

3.6 案例拆解：手机放在裤兜中的步数检测姿态补偿

背景与约束：用户将手机随意放入裤兜，手机屏幕朝向大腿（载体坐标系Zb轴指向人体内侧），且手机在行走过程中会随腿部摆动而旋转。初始姿态未知，且无磁力计校准。

初始现象：直接对加速度计Zb轴进行峰值检测，发现步数计数准确率仅为62%。主要原因是Zb轴并不总是与重力方向对齐，当腿部摆动时，Zb轴上的重力分量变化剧烈，导致误检。

关键输入数据：加速度计采样率50Hz，陀螺仪采样率50Hz，量程±4g，陀螺仪零偏稳定性为10°/h。

分析判断过程：由于无磁力计，无法获得航向角（Yaw）。但步数检测主要依赖垂直方向（Zn轴）的加速度，而Zn轴与重力方向对齐，与Yaw无关。因此，我们只需解算Roll和Pitch，即可将加速度投影到n系。

实际操作步骤：

1. 初始化姿态：利用加速度计静态时的重力向量（归一化后为[0,0,1]）与当前b系加速度的叉积，粗略估计初始Roll和Pitch，构造初始四元数（Yaw设为0）。
2. 实时解算：使用Mahony互补滤波算法，以陀螺仪积分为主，加速度计修正Roll和Pitch漂移。算法输出四元数q。
3. 坐标变换：利用q将b系加速度a_b转换为n系加速度a_n = R(q)·a_b。
4. 步态特征提取：取a_n的Zn分量（垂直加速度），进行带通滤波（0.5Hz-3Hz），然后执行峰值检测与步数计数。

结果对比：经过姿态补偿后，步数计数准确率提升至94%。误检主要发生在快速上下楼梯场景，此时垂直加速度波形与平地行走有差异，需额外加入步态分类器。

风险与代价：由于Yaw未校准，无法提供行走方向信息，但不影响步数计数。若需检测转弯或方向变化，则需引入磁力计或使用GPS航向辅助。

复盘经验：在无磁力计场景下，利用重力向量初始化姿态并持续修正Roll/Pitch，是低成本实现姿态补偿的有效手段。四元数在此过程中避免了欧拉角在手机竖立（Pitch≈90°）时的奇异问题。

3.7 常见误区与总结

• 误区一：认为加速度计的Zb轴输出就是垂直方向。实际上，只有设备静止且水平放置时Zb才与重力对齐。运动状态下必须进行坐标变换。
• 误区二：在MCU中直接使用欧拉角进行积分。这会导致万向锁问题，且三角函数计算开销大。应使用四元数积分。
• 误区三：忽略四元数的归一化。连续乘法会导致四元数模长漂移，若不归一化，旋转矩阵将不再正交，坐标变换结果失真。
• 误区四：认为旋转矩阵是最高效的表示。在嵌入式平台上，旋转矩阵的9个参数和正交化修正会消耗大量CPU周期，而四元数仅需4个参数。