3. 坐标系与姿态表示：机体坐标系、地面坐标系、欧拉角、四元数、旋转矩阵的定义与转换关系，以及姿态解算的数学基础
飞控系统的核心任务之一，是实时、准确地获取飞行器在三维空间中的姿态信息。无论是姿态稳定、航向保持，还是自主导航与轨迹跟踪，都依赖于对“飞行器现在朝向哪里”这一问题的精确回答。本章将系统梳理姿态描述的数学语言体系：从两个基础坐标系（地面坐标系与机体坐标系）的定义出发，深入剖析欧拉角、旋转矩阵与四元数三种主流表示方法的数学本质、相互转换关系及其在飞控工程中的适用场景。理解这些概念之间的内在联系，是后续学习姿态解算算法（如互补滤波、卡尔曼滤波）与姿态控制律设计的必要前提。
3.1 坐标系定义与物理意义
在飞控系统中，坐标系是描述飞行器运动状态的参考基准。若坐标系定义不统一，所有测量数据与计算结果都将失去可比性。工程上最常用的两个坐标系为地面坐标系（Ground Frame, 又称NED坐标系）与机体坐标系（Body Frame, 简称BRF）。
3.1.1 地面坐标系（NED）
地面坐标系是固定于地球表面的惯性参考系（忽略地球自转影响时近似为惯性系）。其原点通常取飞行器起飞点或地面站位置，三轴定义遵循右手定则：X轴指向正北（North），Y轴指向正东（East），Z轴垂直指向地心（Down）。这种“北-东-地”的排列方式使得在水平面上，航向角可以直接以正北为基准进行度量，极大简化了导航解算中的角度换算。NED坐标系是飞控中所有导航数据（位置、速度、加速度）的最终参考基准。
3.1.2 机体坐标系（BRF）
机体坐标系与飞行器固连，原点位于飞行器质心。其定义同样遵循右手定则：X轴沿机体纵轴指向机头方向，Y轴垂直于纵轴指向右翼，Z轴垂直于机体平面指向下方。机体坐标系是惯性测量单元（IMU）直接测量的参考系——陀螺仪输出的角速度、加速度计输出的比力，均是在机体坐标系下的物理量。飞控算法需要将机体坐标系下的测量值转换到地面坐标系，才能进行导航与制导解算。
3.2 姿态表示方法：欧拉角、旋转矩阵与四元数
飞行器的姿态本质上是机体坐标系相对于地面坐标系的旋转关系。数学上，这种旋转关系可以用多种方式描述，各有优劣。
3.2.1 欧拉角（Euler Angles）
欧拉角通过三次绕不同坐标轴的旋转来定义姿态，飞控中通常采用Z-Y-X（偏航-俯仰-横滚）顺序，即先绕Z轴旋转偏航角（Yaw, ψ），再绕新Y轴旋转俯仰角（Pitch, θ），最后绕新X轴旋转横滚角（Roll, φ）。欧拉角的物理意义直观，便于人机交互与调试显示。然而，欧拉角存在著名的万向节锁（Gimbal Lock）问题：当俯仰角接近±90°时，第一次与第三次旋转的轴重合，导致系统丢失一个旋转自由度，姿态解算出现奇异。此外，欧拉角的三角函数运算涉及大量超越函数，计算效率较低，且不便于进行姿态插值。
3.2.2 旋转矩阵（Rotation Matrix）
旋转矩阵是一个3×3的正交矩阵（行列式为+1），属于特殊正交群SO(3)。它将机体坐标系下的向量映射到地面坐标系：v_N = R · v_B。旋转矩阵的优点是：无奇异问题，且可以直接用于向量坐标变换，是连接不同坐标系测量的“桥梁”。其缺点是：矩阵包含9个元素，但实际自由度只有3个，存在冗余；且连续旋转的矩阵乘法运算量较大，不适合在嵌入式MCU上高频执行。
3.2.3 四元数（Quaternion）
四元数是哈密顿扩展的复数系统，由一个实部与三个虚部组成：q = q0 + q1·i + q2·j + q3·k，其中满足 i² = j² = k² = ijk = -1。单位四元数（模长为1）可以唯一表示三维空间中的旋转。相比欧拉角，四元数完全避免了万向节锁问题；相比旋转矩阵，四元数存储量小（4个浮点数）、运算速度快（乘法仅需16次标量乘法），且便于姿态插值（球面线性插值，SLERP）。因此，现代飞控系统（如PX4、ArduPilot）内部普遍采用四元数作为姿态状态量的核心表示。
| 表示方法 | 存储量 | 奇异性 | 计算效率 | 直观性 | 飞控应用场景 |
|---|---|---|---|---|---|
| 欧拉角 | 3个浮点数 | 有（俯仰±90°） | 低（三角函数多） | 高 | 地面站显示、遥控器输入 |
| 旋转矩阵 | 9个浮点数 | 无 | 中（矩阵乘法） | 低 | 坐标变换、外参标定 |
| 四元数 | 4个浮点数 | 无 | 高（乘加运算少） | 中 | 姿态解算核心、控制律内部状态 |
3.3 三种表示方法的数学转换
在飞控软件中，不同模块对姿态表示的需求不同：传感器驱动输出原始角速度，姿态解算模块输出四元数，控制模块可能需要旋转矩阵进行误差计算，而地面站则需要欧拉角进行可视化。因此，掌握三者之间的转换公式是飞控开发的必备技能。
3.3.1 欧拉角 → 旋转矩阵
按照Z-Y-X顺序，旋转矩阵可由三个基本旋转矩阵连乘得到：R = Rz(ψ) · Ry(θ) · Rx(φ)。展开后得到：
R = | cosθ·cosψ, sinφ·sinθ·cosψ - cosφ·sinψ, cosφ·sinθ·cosψ + sinφ·sinψ |
| cosθ·sinψ, sinφ·sinθ·sinψ + cosφ·cosψ, cosφ·sinθ·sinψ - sinφ·cosψ |
| -sinθ, sinφ·cosθ, cosφ·cosθ |3.3.2 旋转矩阵 → 欧拉角
从旋转矩阵提取欧拉角时，可利用矩阵元素的比值关系。设矩阵元素为 R[i][j]，则：
俯仰角 θ = asin(-R[2][0])
横滚角 φ = atan2(R[2][1], R[2][2])
偏航角 ψ = atan2(R[1][0], R[0][0])注意：当 θ = ±90° 时，R[2][1] 与 R[2][2] 同时趋近于0，此时 atan2 参数退化，需要特殊处理（通常将 φ 强制置0）。
3.3.3 欧拉角 → 四元数
利用半角公式，可以直接从欧拉角构造四元数：
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.3.4 四元数 → 旋转矩阵
由四元数构造旋转矩阵的公式为：
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²) |3.3.5 四元数 → 欧拉角
结合上述公式，可先由四元数计算旋转矩阵，再提取欧拉角。工程上更常用的直接公式为：
θ = asin( 2(q0·q2 - q1·q3) )
φ = atan2( 2(q0·q1 + q2·q3), 1 - 2(q1² + q2²) )
ψ = atan2( 2(q0·q3 + q1·q2), 1 - 2(q2² + q3²) )q = q / ||q||。该操作应放在姿态解算循环的末尾，确保进入下一轮迭代前四元数始终为单位四元数。
3.4 姿态解算的数学基础与工程实现
姿态解算（Attitude Estimation）的目标是融合IMU（陀螺仪、加速度计、磁力计）的测量数据，实时估计飞行器的姿态四元数。其数学基础是刚体运动学方程与多传感器数据融合理论。
3.4.1 陀螺仪积分与运动学方程
陀螺仪测量的是机体坐标系相对于惯性系的角速度 ω = (p, q, r)。四元数的运动学微分方程为：
dq/dt = 0.5 · q ⊗ ω_quat其中 ω_quat = (0, p, q, r) 为角速度的四元数表示，⊗ 表示四元数乘法。在离散时间系统中，常用一阶龙格-库塔法（RK1）进行数值积分：
q_new = q_old + (dq/dt) · Δt陀螺仪积分在短时间内精度极高（高频响应好），但受零偏漂移影响，长时间运行会产生累积误差。因此，需要利用加速度计和磁力计的测量信息进行修正。
3.4.2 互补滤波与梯度下降法
互补滤波器的核心思想是：陀螺仪积分负责高频姿态变化，加速度计/磁力计负责低频基准。通过高通滤波处理陀螺仪积分结果，低通滤波处理加速度计/磁力计解算结果，再相加得到融合后的姿态。经典的马霍尼（Mahony）互补滤波算法通过比例-积分（PI）控制器修正陀螺仪零偏，实现简单、计算量小，非常适合STM32等MCU平台。
另一种主流方法是梯度下降法（Madgwick算法），其思想是：将姿态解算转化为一个优化问题——寻找使“参考向量（重力、地磁）经旋转后与测量向量误差最小”的四元数。通过迭代梯度下降步长来逼近最优解，并与陀螺仪积分结果进行加权融合。
3.4.3 卡尔曼滤波在姿态解算中的应用
对于要求更高精度的应用（如无人机载高精度测绘），常采用扩展卡尔曼滤波器（EKF）。EKF以四元数（或误差四元数）为状态量，以陀螺仪输出为控制输入，以加速度计和磁力计输出为观测向量。其优势在于能够显式建模传感器噪声统计特性，并在线估计陀螺仪零偏。但EKF需要线性化处理，且矩阵运算量大，对MCU算力要求较高。
3.5 案例拆解：四元数姿态解算的完整实现流程
下面以一个典型的四旋翼飞控姿态解算任务为例，展示从原始传感器数据到最终姿态输出的完整工程链路。
背景与约束：某四旋翼飞行器搭载BMI088六轴IMU（陀螺仪量程±2000dps，加速度计量程±6g），主控为STM32F405（168MHz）。要求姿态更新率500Hz，姿态角静态误差小于1°，动态跟踪延迟小于20ms。
初始现象：在飞行器静止平放时，通过串口读取的欧拉角显示俯仰角在-2°~+2°之间缓慢漂移，且伴有高频抖动。分析发现，陀螺仪零偏未校准，且加速度计数据未进行低通滤波。
关键输入数据：
- 陀螺仪原始输出（静止时）：X轴 +0.02 rad/s，Y轴 -0.015 rad/s，Z轴 +0.005 rad/s
- 加速度计原始输出（静止水平）：X轴 0.02g，Y轴 -0.01g，Z轴 0.99g
分析判断过程：陀螺仪零偏导致积分后姿态角以约1°/s的速度漂移；加速度计Z轴读数小于1g，说明存在安装倾斜或噪声干扰。需要先进行陀螺仪零偏静态校准（采集1000次取平均），并对加速度计数据进行滑动窗口滤波（窗口大小10）。
实际操作步骤：
- 传感器校准：上电后保持飞行器静止，采集2000组陀螺仪数据求平均，得到零偏值
bias = (0.02, -0.015, 0.005)，后续每次读取后减去该值。 - 初始化四元数：利用加速度计计算初始横滚与俯仰角，磁力计计算初始偏航角，再通过欧拉角转四元数公式得到初始
q0。 - 姿态更新循环（500Hz）：读取陀螺仪数据（减零偏），代入四元数微分方程进行RK1积分，得到预测四元数
q_pred。 - 加速度计修正：将重力向量 (0,0,1) 通过
q_pred旋转到机体坐标系，得到期望加速度a_expected。计算实际加速度测量值a_meas与a_expected的叉积误差e = a_meas × a_expected。 - PI补偿：将误差
e经过PI控制器（Kp=2.0, Ki=0.05）叠加到陀螺仪角速度上，修正积分漂移。 - 四元数归一化：更新后的四元数进行归一化处理。
- 输出转换：将最终四元数转换为欧拉角，通过MAVLink协议发送至地面站。
结果对比：校准与滤波后，静态俯仰角稳定在0.1°以内，动态飞行时姿态跟踪延迟约15ms，满足设计要求。
风险与代价：PI参数调节不当会导致姿态振荡；加速度计在剧烈机动时（如自由落体）比力不再指向重力方向，此时应降低加速度计修正权重（可通过检测加速度计模长是否接近1g来判断）。
复盘经验：姿态解算的精度不仅取决于算法本身，更依赖于传感器校准质量。建议在每次上电时执行快速校准流程，并在飞行日志中记录陀螺仪零偏变化趋势，以便早期发现传感器老化问题。
3.6 常见误区与总结复盘
在姿态表示与解算的工程实践中，以下误区值得特别注意：
- 误区一：混淆旋转方向。旋转矩阵
R表示的是“将机体坐标系向量转换到地面坐标系”，而R^T表示反向转换。在代码中必须明确注释，避免将加速度计数据转换方向搞反。 - 误区二：忽视四元数符号歧义。
q与-q表示相同的旋转。在融合多个传感器数据时，若两个四元数点积为负，应先取反再融合，否则会导致插值路径错误。 - 误区三：欧拉角更新直接积分。欧拉角的微分方程与旋转顺序强相关，且包含大量三角函数，直接积分极易导致数值发散。正确做法是使用四元数或旋转矢量进行积分，最后再转换到欧拉角。
- 误区四：忽略坐标系对齐。IMU在PCB上的安装方向可能与机体坐标系不一致（如旋转90°安装）。必须在驱动层进行轴重映射，否则所有姿态数据将产生固定偏差。
总结而言，坐标系与姿态表示是飞控系统最底层、最关键的数学基础。四元数以其无奇异、计算高效的特性成为飞控内部姿态表示的首选；旋转矩阵是连接不同坐标系测量的桥梁；欧拉角则作为人机交互的直观接口。掌握三者之间的转换关系，并理解姿态解算中“积分+修正”的核心思想，是设计稳定可靠飞控系统的第一步。在实际开发中，务必重视传感器校准、坐标系对齐与数值归一化等工程细节，这些看似微小的环节往往决定了系统最终性能的上限。