3、坐标系与姿态表示：地球坐标系、机体坐标系、欧拉角、旋转矩阵、四元数及其转换
飞控算法的本质是解算“飞行器相对于目标参考系的姿态与位置”，而姿态描述的数学工具直接决定了算法的精度、计算效率与工程可实现性。在工程实践中，我们既需要直观的欧拉角用于调试与显示，也需要适合计算机迭代的旋转矩阵与四元数用于姿态解算和控制律设计。本章将系统梳理地球坐标系与机体坐标系的定义、欧拉角与旋转矩阵的数学关系、四元数的构造方法，以及三者之间的转换流程，并结合飞控固件中的典型实现给出工程建议。
3.1 坐标系定义与工程约定
3.1.1 地球坐标系（导航坐标系）
地球坐标系是描述飞行器位置与姿态的参考基准。在飞控工程中，最常用的是NED（North-East-Down，北东地）坐标系：X轴指向地理北极，Y轴指向正东，Z轴垂直指向地心。NED坐标系符合右手定则，且与多数惯性导航传感器（加速度计、陀螺仪）的输出方向一致，因此成为PX4、ArduPilot等开源飞控的默认导航坐标系。另一种ENU（东-北-天）坐标系多用于地面站显示与GPS坐标转换，两者之间可通过简单的轴交换实现转换。
工程中需注意：地球坐标系的原点通常选取为飞行器起飞点或任务规划点，其Z轴方向与重力方向重合。在姿态解算中，地球坐标系为惯性系近似，忽略地球自转与曲率影响（短航时、小范围任务场景下误差可接受）。
3.1.2 机体坐标系（Body Frame）
机体坐标系与飞行器固连，原点位于飞行器质心。标准定义：X轴指向机头前进方向，Y轴指向机体右侧（右翼方向），Z轴垂直于机体平面向下，满足右手定则。该坐标系直接与IMU（惯性测量单元）的安装方向对应，传感器输出的角速度与加速度均在该坐标系下表达。
在飞控固件中，机体坐标系与地球坐标系之间的转换是姿态解算的核心。例如，加速度计测量的是机体坐标系下的比力，而控制律需要的是地球坐标系下的重力分量，因此必须通过姿态矩阵进行变换。
3.2 欧拉角与旋转矩阵
3.2.1 欧拉角定义与旋转顺序
欧拉角用三个角度描述刚体相对于参考系的姿态：横滚角φ（Roll）绕X轴旋转，俯仰角θ（Pitch）绕Y轴旋转，偏航角ψ（Yaw）绕Z轴旋转。工程中约定旋转顺序为Z-Y-X（先偏航、再俯仰、最后横滚），该顺序与飞控中“先对地定向、再调整俯仰、最后修正横滚”的控制逻辑一致。
欧拉角的物理意义直观，但存在两个固有缺陷：万向锁（Gimbal Lock）——当俯仰角接近±90°时，横滚与偏航的旋转轴重合，导致自由度丢失；角度奇异性——在±180°边界处角度跳变，不利于连续控制。
3.2.2 旋转矩阵的构造
从地球坐标系到机体坐标系的旋转矩阵R（方向余弦矩阵）可由三个基本旋转矩阵连乘得到：
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φ]旋转矩阵R为正交矩阵（RT = R-1），其逆矩阵表示从机体坐标系到地球坐标系的变换。在飞控代码中，旋转矩阵常用于将加速度计测量的机体比力转换到地球坐标系，以计算重力补偿。
| 表示方法 | 优点 | 缺点 | 典型应用 |
|---|---|---|---|
| 欧拉角 | 直观、易调试 | 万向锁、角度跳变 | 地面站显示、遥控器输入 |
| 旋转矩阵 | 无奇异性、适合坐标变换 | 9个参数冗余、计算量大 | 向量旋转、控制律解算 |
| 四元数 | 紧凑、无奇异性、易插值 | 不够直观 | 姿态解算、滤波融合 |
3.3 四元数及其数学基础
3.3.1 四元数定义
四元数是由一个实数部分和三个虚数部分组成的超复数，表示为 q = w + xi + yj + zk，其中w为标量部分，(x, y, z)为矢量部分。单位四元数满足 w² + x² + y² + z² = 1，可用于表示三维空间中的旋转。四元数旋转的物理意义：绕单位向量(nx, ny, nz)旋转角度θ，对应的四元数为：
q = [cos(θ/2), n_x·sin(θ/2), n_y·sin(θ/2), n_z·sin(θ/2)]四元数乘法满足结合律但不满足交换律，其乘法规则为：
q1 ⊗ q2 = (w1w2 - v1·v2, w1v2 + w2v1 + v1×v2)3.3.2 四元数与旋转矩阵的转换
由单位四元数构造旋转矩阵的公式为：
R = [1-2(y²+z²) 2(xy-wz) 2(xz+wy);
2(xy+wz) 1-2(x²+z²) 2(yz-wx);
2(xz-wy) 2(yz+wx) 1-2(x²+y²)]反之，从旋转矩阵提取四元数时，需根据矩阵迹（trace）的大小选择不同公式以避免数值不稳定。工程中常用Shepperd方法：先计算最大对角元素对应的分量，再通过代数余子式求解其余分量。
3.4 姿态表示转换的工程实现
3.4.1 转换流程与代码实现
在飞控固件中，姿态解算通常采用四元数作为核心状态量，而在控制律与地面站通信时转换为欧拉角。典型转换流程如下：
- 初始化：上电时通过加速度计与磁力计估计初始欧拉角，并转换为四元数。
- 预测：陀螺仪积分角速度更新四元数（一阶龙格-库塔法）。
- 校正：利用加速度计与磁力计测量值构造误差，通过互补滤波或卡尔曼滤波修正四元数。
- 输出：将最终四元数转换为欧拉角用于控制律，或转换为旋转矩阵用于向量变换。
// 四元数转欧拉角（Z-Y-X顺序）
roll = atan2(2(wx + yz), 1 - 2(x² + y²));
pitch = asin(2(wy - zx));
yaw = atan2(2(wz + xy), 1 - 2(y² + z²));3.4.2 案例拆解：姿态解算中的数值稳定性问题
场景：某四旋翼在高速俯仰机动时（俯仰角达80°），飞控输出姿态出现剧烈跳变，导致电机指令异常。
分析过程：通过日志检查发现，姿态解算模块在俯仰角接近90°时，欧拉角转换函数中的asin参数超出[-1,1]范围（因浮点截断误差），导致返回NaN值。进一步检查发现，旋转矩阵提取四元数时使用了固定公式，未考虑矩阵迹接近-1的退化情况。
解决方案：
- 在四元数转欧拉角前，对asin输入进行饱和处理（clamp到[-1,1]）。
- 改用Shepperd方法提取四元数，根据矩阵迹的最大值选择不同分支。
- 在控制律中增加欧拉角速率限制，避免瞬时大角度变化。
结果对比：修复后，在相同机动条件下姿态输出平滑，控制指令无异常跳变。该案例说明，数学转换的数值稳定性与算法选择同等重要。
3.5 常见误区与总结复盘
| 误区 | 后果 | 正确做法 |
|---|---|---|
| 混淆NED与ENU坐标系 | 姿态解算错误，飞行器反向控制 | 统一使用NED，转换时明确轴序 |
| 忽略旋转顺序 | 欧拉角与旋转矩阵不匹配 | 固定使用Z-Y-X顺序并写入文档 |
| 四元数未归一化 | 姿态漂移、控制精度下降 | 每次更新后强制归一化 |
| 直接使用欧拉角做控制 | 万向锁导致失控 | 控制律内部使用四元数或旋转矩阵 |
总结：坐标系与姿态表示是飞控算法的数学基石。工程实践中，应以四元数作为姿态解算的核心状态量，利用旋转矩阵进行向量坐标变换，仅在需要人机交互时转换为欧拉角。理解三种表示方法的数学本质与转换关系，是编写稳定、可靠飞控代码的前提。建议开发者在实现转换函数时，参考PX4或ArduPilot的成熟代码，并针对边界条件（如大角度、奇异点）进行专项测试。