3. 坐标系与姿态表示：常用坐标系定义、欧拉角、旋转矩阵、四元数及其相互转换

在陀螺仪与加速度计融合应用中，姿态解算的第一步并非算法选择，而是坐标系定义与姿态描述方法的统一。工程实践中，坐标系定义混乱、欧拉角奇异、四元数更新错误是导致姿态漂移、数据跳变甚至系统失控的首要原因。本章将系统梳理导航坐标系与载体坐标系的定义，深入对比欧拉角、旋转矩阵、四元数三种姿态表示方法的数学本质、工程适用场景及相互转换关系，为后续传感器标定、数据融合与姿态解算奠定严格的数学基础。

3.1 常用坐标系定义与工程约定

3.1.1 导航坐标系（n系）

导航坐标系是描述载体位置与姿态的参考基准。在无人机、机器人、惯性导航领域，最常用的是北东地（NED）坐标系：X轴指向地理北，Y轴指向地理东，Z轴垂直指向地心。该坐标系遵循右手定则，与地球重力方向对齐，便于直接解算加速度计的重力分量。在IMU（惯性测量单元）出厂时，加速度计与陀螺仪的敏感轴通常已按载体坐标系定义，但导航解算必须先将测量值转换到n系。

另一种常见选择是东北天（ENU）坐标系，多用于航空与大地测量。NED与ENU的差异在于Z轴方向相反，且X/Y轴互换。工程中若混用两套坐标系，会导致航向角偏差180°、俯仰角符号反转等严重错误。因此，项目启动时必须明确记录坐标系约定，并在代码注释与接口文档中强制标注。

3.1.2 载体坐标系（b系）

载体坐标系与IMU硬件固连，原点位于传感器中心。典型定义为：X轴指向载体前方（横滚轴），Y轴指向载体右方（俯仰轴），Z轴指向载体下方（航向轴），构成右-前-下（RFD）右手系。对于无人机，X轴沿机头方向；对于手持设备，X轴沿屏幕长边方向。载体坐标系的定义直接影响陀螺仪角速度的符号约定，例如绕X轴正方向旋转对应右滚转。

在传感器融合中，还需区分机体坐标系与传感器坐标系。当IMU安装位置偏离载体中心或存在安装角度误差时，必须进行杆臂补偿与安装角标定，否则融合结果会引入周期性误差。工程上建议在PCB设计阶段就明确IMU的安装方向，并在结构件上标注参考箭头。

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

3.2.1 定义与旋转顺序

欧拉角通过三次绕不同坐标轴的旋转来描述姿态，常用组合为Z-Y-X（航向-俯仰-横滚）。设载体从n系出发，先绕Z轴旋转航向角ψ，再绕新Y轴旋转俯仰角θ，最后绕新X轴旋转横滚角φ。该顺序下，旋转矩阵可分解为三个基本旋转矩阵的乘积：

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

其中各基本旋转矩阵为：

Rx(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]
Ry(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
Rz(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]

欧拉角的物理意义直观，适合用于操作员显示与简单控制逻辑。例如，飞控手看到俯仰角为10°时，能立即理解机头抬升了10°。但欧拉角存在万向节锁问题：当俯仰角接近±90°时，航向与横滚的旋转轴重合，导致自由度丢失，姿态解算出现奇异。

3.2.2 从旋转矩阵提取欧拉角

已知旋转矩阵R的元素，可通过以下公式提取欧拉角（Z-Y-X顺序）：

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

当θ接近±90°时，atan2的参数趋于0/0，导致数值不稳定。工程中应检测|R[2][0]|是否接近1，若超过阈值（如0.999），则采用退化公式处理，并标记姿态为奇异状态。

3.3 旋转矩阵：精确但参数冗余

3.3.1 性质与用途

旋转矩阵R是一个3×3的正交矩阵，满足R^TR = I且det(R)=1。它既能表示坐标变换（将b系向量转换到n系），也能表示向量旋转。在传感器融合中，旋转矩阵常用于：

• 将加速度计测量的比力从b系转换到n系，以分离重力分量。
• 将陀螺仪角速度积分得到的姿态增量更新到当前姿态。
• 多传感器外参标定时的坐标系对齐。

旋转矩阵的优点是无奇异，且组合旋转只需矩阵乘法。缺点是9个参数存在6个约束方程，直接存储和更新会引入数值误差，导致矩阵不再正交。因此，工程中需定期进行正交化修正（如采用Gram-Schmidt正交化或SVD分解）。

3.3.2 旋转矩阵与欧拉角的转换

由欧拉角构造旋转矩阵时，按Z-Y-X顺序展开可得完整矩阵表达式。反之，从旋转矩阵提取欧拉角已在3.2.2节给出。在代码实现中，建议使用atan2而非atan，以正确处理象限问题。

3.4 四元数：高效无奇异的姿态表示

3.4.1 四元数定义与运算

四元数由一个实部和三个虚部组成，记为q = q0 + q1·i + q2·j + q3·k，其中i² = j² = k² = ijk = -1。单位四元数满足q0² + q1² + q2² + q3² = 1，可用于表示三维旋转。四元数乘法定义为：

q ⊗ p = (q0p0 - q1p1 - q2p2 - q3p3) +
         (q0p1 + q1p0 + q2p3 - q3p2)·i +
         (q0p2 - q1p3 + q2p0 + q3p1)·j +
         (q0p3 + q1p2 - q2p1 + q3p0)·k

四元数的共轭q* = q0 - q1·i - q2·j - q3·k，表示逆旋转。向量v的旋转可通过v' = q ⊗ v ⊗ q*实现，其中v以纯四元数(0, vx, vy, vz)表示。

3.4.2 四元数更新方程

陀螺仪输出角速度ω = (ωx, ωy, ωz)，四元数随时间更新的微分方程为：

dq/dt = 0.5 · q ⊗ ω

其中ω以纯四元数(0, ωx, ωy, ωz)表示。离散化后常用一阶龙格-库塔法：

q_new = q_old + dt · 0.5 · q_old ⊗ ω

更新后需对四元数进行归一化，防止模长漂移。相比旋转矩阵，四元数仅需4个参数，无奇异，且计算量更小，是IMU融合算法（如Mahony、Madgwick）的首选。

3.4.3 四元数与欧拉角的转换

从四元数到欧拉角（Z-Y-X顺序）的转换公式为：

φ = atan2(2(q0q1 + q2q3), 1 - 2(q1² + q2²))
θ = asin(2(q0q2 - q3q1))
ψ = atan2(2(q0q3 + q1q2), 1 - 2(q2² + q3²))

反向转换时，由欧拉角构造四元数：

q0 = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2)
q1 = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2)
q2 = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2)
q3 = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2)

3.5 案例拆解：从原始数据到姿态输出的完整链路

某四旋翼无人机项目采用BMI088 IMU，陀螺仪量程±2000°/s，加速度计量程±6g。飞控主控为STM32F405，运行频率400Hz。初始需求：实现姿态解算，输出欧拉角供姿态控制与地面站显示。

关键约束：俯仰角可能达到±90°（特技飞行模式），且要求解算延迟小于2ms。若直接使用欧拉角积分，在俯仰角接近90°时会出现数值发散。因此，方案选择四元数作为内部状态。

实施步骤：

1. 坐标系对齐：确认IMU安装方向与机体坐标系一致，若不一致，在初始化时通过静态标定获取安装角误差，并构造校正旋转矩阵。
2. 初始化四元数：上电静止时，利用加速度计计算初始横滚与俯仰角，利用磁力计（若有）计算初始航向，再通过欧拉角→四元数公式得到初始q。
3. 陀螺仪积分：以400Hz读取角速度，执行四元数更新方程，并归一化。
4. 加速度计修正：将重力向量从n系转换到b系，与加速度计实测值求叉积，得到误差项，用于补偿陀螺仪漂移（即互补滤波或Mahony算法）。
5. 输出转换：每10ms将四元数转换为欧拉角，通过MAVLink协议发送至地面站。

结果对比：在俯仰角85°时，欧拉角直接积分法输出跳变至-95°，而四元数法输出稳定在85.2°±0.3°。在持续飞行10分钟后，四元数法航向漂移小于2°，满足设计要求。

复盘经验：四元数更新频率需至少为陀螺仪带宽的10倍，否则高频角振动会混叠；加速度计修正系数需根据振动环境调整，过大则引入噪声，过小则漂移明显。

3.6 常见误区与总结复盘