3. 坐标系与姿态表示

在传感器数据融合与滤波算法实战中，坐标系与姿态表示是整个系统的“语言基础”。无论是加速度计、陀螺仪还是磁力计，其原始输出都定义在特定的坐标系内；而卡尔曼滤波、互补滤波等算法所估计的姿态，也必须在一个统一的参考框架下进行描述。如果坐标系定义混乱或姿态表示方法选择不当，轻则导致调试困难、数据“看起来不对”，重则引发姿态解算发散、控制回路振荡等严重问题。本章将系统梳理机体坐标系与世界坐标系的定义，深入讲解欧拉角、旋转矩阵、四元数、方向余弦矩阵（DCM）的数学本质与工程含义，并给出它们之间的转换关系与工程选型建议。

3.1 两大坐标系的定义与工程意义

3.1.1 世界坐标系（World Frame / Navigation Frame）

世界坐标系是描述物体在空间中位置的参考基准，通常记为 n系（导航系）或g系（地理系）。在无人机、机器人、车载导航等应用中，最常用的是东北天（ENU）或北东地（NED）两种约定。以NED为例：X轴指向地理北向，Y轴指向地理东向，Z轴垂直指向地心。该坐标系固定于地球表面某一点，不随载体运动而改变。在IMU数据处理中，加速度计和磁力计的“参考”意义正是相对于世界坐标系而言的——重力方向定义了Z轴（或负Z轴），地磁场方向则提供了北向的参考。

工程上选择世界坐标系时需注意：原点位置通常取起飞点、设备上电位置或某个已知经纬度点；轴向约定必须与后续算法中的旋转矩阵定义保持一致。例如，若采用NED约定，则重力加速度在静止时为 [0, 0, 9.8] m/s²（或 [0, 0, -9.8] 取决于Z轴方向定义）；若采用ENU约定，则重力为 [0, 0, -9.8] m/s²。这个细节直接影响加速度计读数在滤波算法中的使用方式。

3.1.2 机体坐标系（Body Frame）

机体坐标系固定于载体本身，记为 b系。其原点通常位于IMU芯片的几何中心或载体的质心，三个轴与载体的结构方向对齐。以四旋翼无人机为例：X_B轴指向机头方向，Y_B轴指向右侧机翼，Z_B轴垂直于机体平面向下（NED约定）或向上（ENU约定）。陀螺仪输出的角速度 [ωx, ωy, ωz] 就是绕机体三个轴的旋转速率；加速度计输出的比力 [ax, ay, az] 也是沿机体轴测量的。

需要特别强调的是，IMU的安装位置与方向直接决定了机体坐标系的定义。如果IMU芯片在PCB上的丝印方向与机头方向不一致，必须在软件中进行轴交换或符号翻转。例如，某款IMU芯片的X轴可能指向芯片的右侧而非前方，此时需要将原始数据映射到统一的机体坐标系中。这一步骤称为传感器坐标系校准，通常在姿态解算之前完成。

3.2 欧拉角：直观但存在奇异的姿态描述

3.2.1 欧拉角的定义与旋转顺序

欧拉角通过三次绕不同坐标轴的旋转来描述姿态，最常见的组合是Z-Y-X（偏航-俯仰-横滚）顺序，对应航空领域的Yaw（偏航角ψ）、Pitch（俯仰角θ）、Roll（横滚角φ）。其旋转过程为：先绕世界系Z轴旋转ψ得到中间系，再绕新的Y轴旋转θ，最后绕新的X轴旋转φ。这三次旋转的复合结果就是机体坐标系相对于世界坐标系的姿态。

欧拉角的物理意义非常直观：飞行员或飞手可以直接通过这三个角度理解飞行器的朝向。在STM32的串口调试助手中打印 yaw=30.5° pitch=-12.3° roll=5.8°，任何人都能立即想象出飞行器的姿态。因此，欧拉角广泛用于人机交互显示、日志记录、遥控遥测等场景。

3.2.2 万向节死锁（Gimbal Lock）问题

欧拉角最致命的缺陷是万向节死锁。当俯仰角θ接近±90°时，第一次旋转（偏航）和第三次旋转（横滚）的旋转轴重合，导致系统丢失一个自由度。此时偏航角和横滚角无法独立区分，姿态解算出现奇异。数学上表现为旋转矩阵中的某些元素出现除零或无穷大，导致从旋转矩阵反解欧拉角时出现 NaN 或跳变。

在工程实践中，万向节死锁并非仅仅是一个理论问题。例如，当无人机做垂直爬升或俯冲动作时，俯仰角可能接近±90°，此时如果滤波算法内部直接使用欧拉角进行积分，就会产生数值不稳定。因此，欧拉角只适合作为“显示层”的姿态表示，不适合作为“计算层”的滤波状态量。

3.3 旋转矩阵与方向余弦矩阵（DCM）

3.3.1 旋转矩阵的构造

旋转矩阵 R 是一个3×3的正交矩阵，满足 RTR = I 且 det(R) = 1。它将机体坐标系中的向量变换到世界坐标系：vn = R · vb。由欧拉角构造旋转矩阵时，按照Z-Y-X顺序，有：

R = Rz(ψ) · Ry(θ) · Rx(φ)

其中：
Rz(ψ) = [cosψ  -sinψ  0;
         sinψ   cosψ  0;
         0      0     1]

Ry(θ) = [cosθ   0     sinθ;
         0      1     0;
         -sinθ  0     cosθ]

Rx(φ) = [1      0     0;
         0      cosφ  -sinφ;
         0      sinφ   cosφ]

将三个矩阵相乘后，得到完整的旋转矩阵。该矩阵的每一列表示机体坐标系的三个轴在世界坐标系中的方向余弦。因此，旋转矩阵也被称为方向余弦矩阵（DCM）。DCM的9个元素中只有3个独立变量，但由于正交约束的存在，实际使用中需要定期进行正交化修正，否则积分漂移会导致矩阵不再满足正交性。

3.3.2 DCM在姿态解算中的角色

在早期的捷联惯导系统中，DCM是主要的姿态表示方法。其优点是：无奇异问题，可以描述任意姿态；向量旋转方便，直接矩阵乘法即可。缺点是：存储量大（9个元素），计算量大（矩阵乘法），且需要正交化修正。在STM32这类资源受限的MCU上，如果采样频率较高（如1kHz），DCM的运算开销可能成为瓶颈。

DCM与旋转矩阵本质上是同一个数学对象，只是名称强调的角度不同。在工程代码中，通常直接使用 float R[3][3] 或 Matrix3f 类型来存储。当需要将加速度计读数从机体系转换到世界系时，直接左乘DCM即可：an = R · ab。

3.4 四元数：滤波计算的核心工具

3.4.1 四元数的定义与优势

四元数是由一个实部和三个虚部组成的超复数，形式为 q = q0 + q1·i + q2·j + q3·k，其中 i² = j² = k² = ijk = -1。单位四元数（模长为1）可以表示三维空间中的旋转。相比欧拉角和DCM，四元数具有以下显著优势：

• 无奇异问题：可以平滑表示任意姿态，不存在万向节死锁。
• 存储量小：仅需4个浮点数，比DCM少5个。
• 计算效率高：四元数乘法比矩阵乘法运算量更小，适合嵌入式实时计算。
• 插值方便：球面线性插值（SLERP）可用于平滑过渡。

在卡尔曼滤波或互补滤波中，四元数通常被选为状态向量。例如，一个典型的姿态滤波器状态量为 x = [q0, q1, q2, q3, gyro_bias_x, gyro_bias_y, gyro_bias_z]，其中前四个元素就是姿态四元数。

3.4.2 四元数的基本运算

四元数的乘法定义为：

q ⊗ p = [q0*p0 - q1*p1 - q2*p2 - q3*p3;
         q0*p1 + q1*p0 + q2*p3 - q3*p2;
         q0*p2 - q1*p3 + q2*p0 + q3*p1;
         q0*p3 + q1*p2 - q2*p1 + q3*p0]

四元数表示旋转时，向量 v 的旋转通过 v' = q ⊗ v ⊗ q* 实现，其中 q* 是共轭四元数。在姿态更新中，陀螺仪角速度 ω = [ωx, ωy, ωz] 通过四元数微分方程进行积分：

dq/dt = 0.5 * q ⊗ ω_quat

其中 ω_quat = [0, ωx, ωy, ωz]

实际离散化时，常用一阶龙格-库塔法或解析法进行更新。需要注意的是，每次更新后必须对四元数进行归一化，否则模长漂移会导致旋转缩放误差。

3.5 四种表示方法的转换关系

3.5.1 欧拉角 → 旋转矩阵

按照Z-Y-X顺序，将三个基本旋转矩阵相乘即可得到完整的旋转矩阵。工程代码中通常直接展开为9个元素的表达式，避免矩阵乘法开销。例如，R[0][0] = cosθ·cosψ，R[0][1] = sinφ·sinθ·cosψ - cosφ·sinψ，等等。

3.5.2 旋转矩阵 → 欧拉角

从旋转矩阵反解欧拉角时，需要根据矩阵元素进行atan2运算。对于Z-Y-X顺序：

ψ = atan2(R[1][0], R[0][0])
θ = asin(-R[2][0])
φ = atan2(R[2][1], R[2][2])

当 θ = ±90° 时，R[0][0] 和 R[1][0] 同时接近0，atan2的参数退化，导致ψ和φ无法唯一确定。这正是万向节死锁的数学表现。

3.5.3 旋转矩阵 ↔ 四元数

从旋转矩阵到四元数的转换有多种方法，最常用的是基于矩阵迹（trace）的方法：

t = R[0][0] + R[1][1] + R[2][2]
if t > 0:
    s = 0.5 / sqrt(t + 1)
    q0 = 0.25 / s
    q1 = (R[2][1] - R[1][2]) * s
    q2 = (R[0][2] - R[2][0]) * s
    q3 = (R[1][0] - R[0][1]) * s
else:
    # 处理 t <= 0 的情况，选择最大对角元素分支
    ...

从四元数到旋转矩阵的转换则相对直接：

R[0][0] = 1 - 2*(q2² + q3²)
R[0][1] = 2*(q1*q2 - q0*q3)
R[0][2] = 2*(q1*q3 + q0*q2)
R[1][0] = 2*(q1*q2 + q0*q3)
R[1][1] = 1 - 2*(q1² + q3²)
R[1][2] = 2*(q2*q3 - q0*q1)
R[2][0] = 2*(q1*q3 - q0*q2)
R[2][1] = 2*(q2*q3 + q0*q1)
R[2][2] = 1 - 2*(q1² + q2²)

3.5.4 四元数 → 欧拉角

将四元数先转换为旋转矩阵，再按上述公式反解欧拉角。也可以直接使用四元数元素计算：

ψ = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2² + q3²))
θ = asin(2*(q0*q2 - q3*q1))
φ = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1² + q2²))

同样，当 θ = ±90° 时会出现奇异。

3.6 工程选型与实战案例

3.6.1 姿态表示方法选型对比

3.6.2 实战案例：基于STM32的四旋翼姿态解算框架

假设我们正在开发一款基于STM32F405的飞控，IMU为MPU6050（三轴陀螺仪+三轴加速度计），采样频率为500Hz。姿态解算采用互补滤波或卡尔曼滤波，内部状态量为四元数。以下是完整的工程流程：

1. 传感器数据读取与预处理：通过I2C读取MPU6050的原始数据，转换为物理单位（陀螺仪：°/s，加速度计：g或m/s²）。根据IMU的安装方向，进行轴映射和符号修正，统一到机体坐标系。
2. 初始化：上电时，利用加速度计静态读数计算初始横滚角和俯仰角：roll = atan2(ay, az)，pitch = atan2(-ax, sqrt(ay² + az²))。偏航角初始值可由磁力计提供（若有），否则设为0。将初始欧拉角转换为四元数作为滤波初值。
3. 姿态预测（陀螺仪积分）：在每个采样周期，读取陀螺仪角速度 ω = [ωx, ωy, ωz]，构造四元数微分方程并离散化更新：q_new = q_old + 0.5 * dt * q_old ⊗ ω_quat。更新后立即归一化。
4. 姿态观测（加速度计修正）：将加速度计读数归一化，得到机体坐标系下的重力方向估计 a_b = [ax, ay, az] / ||a||。同时，利用当前四元数计算理论上的重力方向（世界系Z轴在机体系中的投影）：v = R(q)^T · [0, 0, 1]。计算两者叉积 e = a_b × v，该叉积反映了姿态误差。
5. 滤波融合：将误差 e 通过PI控制器反馈到陀螺仪角速度上，修正积分漂移。互补滤波公式为：ω_corrected = ω_gyro + Kp * e + Ki * ∫e dt。卡尔曼滤波则将此误差作为观测更新量。
6. 姿态输出：将滤波后的四元数转换为欧拉角，用于控制律计算和地面站显示。转换时注意处理 θ = ±90° 的边界情况。

3.7 常见误区与总结

3.7.1 常见误区

• 误区一：欧拉角直接用于滤波积分。这会导致万向节死锁和数值不稳定，应使用四元数或DCM作为滤波状态量。
• 误区二：四元数未归一化。四元数模长漂移会导致旋转缩放，每次更新后必须归一化。
• 误区三：旋转顺序不一致。不同文献可能使用不同的旋转顺序（如Z-Y-X vs Z-X-Y），导致欧拉角与旋转矩阵的对应关系不同。在代码中必须明确注释旋转顺序。
• 误区四：忽略IMU安装方向。轴映射错误会导致姿态解算完全错误，且难以排查。
• 误区五：DCM未做正交化修正。长时间运行后，DCM的正交性会因积分误差而破坏，导致向量变换失真。

3.7.2 总结与选型建议

在STM32嵌入式系统中，推荐采用以下策略：滤波内部使用四元数作为状态量，利用其无奇异、计算高效的特点；坐标变换使用旋转矩阵，将加速度计、磁力计等向量在机体系与世界系之间转换；人机交互使用欧拉角，方便调试和显示。三者之间的转换函数应封装为独立的模块，并经过充分的单元测试。