3、坐标系与姿态表示：欧拉角、旋转矩阵、四元数的定义与转换

在嵌入式传感器融合系统中，加速度计、陀螺仪与磁力计输出的原始数据都是相对于传感器自身坐标系的物理量。要利用这些数据描述载体的空间姿态，首先必须建立严格的数学表示框架。姿态描述的本质是回答“载体坐标系相对于参考坐标系如何旋转”这一问题。工程实践中，欧拉角、旋转矩阵与四元数是三种最常用的数学工具，它们各有优劣，适用于不同的计算阶段与硬件约束。本章将系统阐述三者的数学定义、物理含义、相互转换关系以及在嵌入式环境中的工程取舍。

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

欧拉角通过三次绕坐标轴的旋转来定义载体姿态，最常用的航空次序为 Z-Y-X（偏航-俯仰-横滚），分别对应偏航角 ψ（Yaw）、俯仰角 θ（Pitch）和横滚角 φ（Roll）。其物理意义非常直观：偏航角描述载体绕垂直轴（Z轴）的旋转，俯仰角描述绕横向轴（Y轴）的旋转，横滚角描述绕纵向轴（X轴）的旋转。这种分解方式与人类对空间方向的直觉高度吻合，因此在飞控地面站、姿态仪表显示和调试界面中广泛使用。

然而，欧拉角存在一个根本性缺陷——万向节锁（Gimbal Lock）。当俯仰角接近 ±90° 时，偏航与横滚的旋转轴重合，导致自由度丢失，姿态解算出现奇异。在嵌入式系统中，这意味着当飞行器垂直爬升或俯冲时，欧拉角表示会失效，无法唯一确定姿态。此外，欧拉角的微分方程包含三角函数运算，计算量较大，且角速度积分时容易产生累积误差。因此，欧拉角更适合作为最终显示输出，而非内部姿态解算的核心表示。

3.2 旋转矩阵：无奇异但冗余的坐标变换工具

旋转矩阵是一个 3×3 的正交矩阵，满足 R^TR = I 且 det(R) = 1。它将载体坐标系中的向量变换到参考坐标系，或反之。旋转矩阵的最大优势在于坐标变换的直接性——只需一次矩阵乘法即可完成向量在不同坐标系间的映射，无需任何三角函数计算。在传感器融合算法中，当需要将加速度计测量的比力从载体坐标系转换到导航坐标系时，旋转矩阵是最自然的选择。

旋转矩阵的缺点同样明显：9 个元素描述 3 个自由度，存在 6 个冗余约束。在嵌入式环境中，这意味着需要额外的存储空间（9 个浮点数），且每次更新后必须进行正交化修正（如 Gram-Schmidt 过程），否则矩阵会逐渐偏离正交性，导致姿态漂移。此外，旋转矩阵无法直观地表达姿态角，也不适合进行姿态插值。因此，旋转矩阵通常作为中间计算工具，而非长期存储的姿态状态量。

3.3 四元数：高效无奇异的姿态状态量

四元数是一种超复数形式，表示为 q = q₀ + q₁i + q₂j + q₃k，其中 q₀ 为标量部分，q₁、q₂、q₃ 为矢量部分，满足 i² = j² = k² = ijk = -1。单位四元数（模长为 1）可以唯一表示三维空间中的旋转，其物理意义为：绕单位轴向量 (nₓ, nᵧ, n_z) 旋转角度 θ 的四元数为：

q = [cos(θ/2), nₓ·sin(θ/2), nᵧ·sin(θ/2), n_z·sin(θ/2)]

四元数在嵌入式姿态解算中占据主导地位，原因有三：其一，无奇异问题，适用于全姿态范围；其二，仅需 4 个参数，存储和传输开销小；其三，四元数乘法可实现旋转复合，且微分方程仅含简单的线性运算，非常适合在 MCU 上实时计算。在 Mahony 互补滤波和 Madgwick 滤波等经典算法中，四元数都是核心的状态表示。

3.4 三种表示方法的转换关系与工程实现

在实际嵌入式系统中，三种表示方法往往需要相互转换。例如，IMU 内部解算使用四元数，但地面站显示需要欧拉角；或者外部控制指令以欧拉角形式下发，需要转换为四元数参与姿态控制。下表总结了三种表示方法的核心特性与典型应用场景：

特性维度	欧拉角	旋转矩阵	四元数
参数数量	3	9	4
奇异问题	有（俯仰±90°）	无	无
计算效率	低（含三角函数）	中（矩阵乘法）	高（四则运算）
姿态插值	不支持	不支持	支持（球面线性插值）
典型应用	显示、调试、人机交互	坐标变换、向量映射	姿态解算、控制律、数据融合

从欧拉角到四元数的转换遵循 Z-Y-X 次序，先分别构造三个基本旋转四元数，再按顺序相乘：

qx = [cos(φ/2), sin(φ/2), 0, 0]
qy = [cos(θ/2), 0, sin(θ/2), 0]
qz = [cos(ψ/2), 0, 0, sin(ψ/2)]
q = qz ⊗ qy ⊗ qx

从四元数到欧拉角的转换则利用四元数元素直接计算：

φ = atan2(2(q₀q₁ + q₂q₃), 1 - 2(q₁² + q₂²))
θ = asin(2(q₀q₂ - q₃q₁))
ψ = atan2(2(q₀q₃ + q₁q₂), 1 - 2(q₂² + q₃²))

在嵌入式实现中，需要注意 atan2 函数返回值的象限判断，以及 asin 函数在输入接近 ±1 时的数值稳定性。建议在转换前对四元数进行单位化处理，避免因累积误差导致三角函数输入越界。

3.5 案例拆解：四旋翼姿态解算中的表示方法选型

某四旋翼无人机项目采用 STM32F405 主控（168MHz），搭配 MPU9250 九轴传感器。初始设计时，工程师直接使用欧拉角作为姿态状态量，在俯仰角超过 60° 的机动飞行中，发现姿态解算结果出现剧烈跳变，甚至导致飞控系统误判为传感器故障。

问题分析：通过串口日志分析，当俯仰角接近 75° 时，偏航角输出出现 ±180° 的跳变，横滚角也出现异常波动。进一步检查发现，欧拉角微分方程中的 cos(θ) 项在分母位置，当 θ 接近 90° 时，该项趋近于零，导致数值放大和奇异。

解决方案：将姿态状态量从欧拉角切换为四元数，具体步骤如下：

1. 将初始欧拉角转换为四元数作为初始状态；
2. 陀螺仪角速度通过四元数微分方程 q̇ = ½·q⊗ω 进行积分；
3. 加速度计和磁力计数据通过互补滤波或卡尔曼滤波修正四元数漂移；
4. 仅在需要输出姿态角时，将四元数转换为欧拉角。

结果对比：切换后，在相同机动条件下，姿态解算稳定，无奇异跳变。俯仰角 85° 时，偏航角输出平滑，误差小于 2°。CPU 占用率从 42% 降至 31%，系统响应速度提升约 15%。

风险与代价：四元数需要额外的单位化处理，若单位化频率不足，长期运行后可能出现模长漂移。本案例中，在每个解算周期（10ms）内执行一次单位化，确保模长误差小于 0.001。

3.6 总结与工程选型建议

三种姿态表示方法各有明确的适用边界。欧拉角适合人机交互和可视化，旋转矩阵适合坐标变换计算，四元数适合作为姿态解算的核心状态量。在嵌入式传感器融合系统中，推荐采用“四元数内部解算 + 欧拉角外部输出”的架构。具体选型时，还需考虑 MCU 的浮点运算能力、存储资源以及实时性要求。对于资源受限的 8 位或 16 位 MCU，可考虑使用定点四元数运算，但需注意动态范围和精度损失。对于支持 FPU 的 32 位 MCU，直接使用浮点四元数即可获得良好性能。

此外，无论采用哪种表示方法，都必须建立严格的坐标系定义约定。建议在项目初期明确参考坐标系（如 NED 或 ENU）、旋转次序（如 Z-Y-X）以及角度正方向，并在代码注释中详细记录，避免团队协作时出现坐标混乱。姿态表示是传感器融合的基础，选择正确的表示方法并理解其数学本质，是构建可靠姿态解算系统的第一步。