3、姿态表示基础：欧拉角、旋转矩阵、四元数的定义、转换关系及在飞控中的应用

姿态控制是飞控系统的核心闭环，而姿态表示则是该闭环的数学基石。PX4、ArduPilot 等主流飞控源码中，姿态数据在传感器融合（EKF2/互补滤波）、控制器解算（角度环/角速度环）以及指令生成（姿态设定值）之间频繁流转。理解欧拉角、旋转矩阵与四元数三者的定义、转换关系及各自的工程适用场景，是读懂飞控源码、定位姿态异常（如万向节锁、控制发散）的前提。本章将从数学定义出发，结合飞控源码中的实际数据结构与算法路径，系统梳理三种表示方法的本质联系与工程取舍。

上图展示了三种姿态表示方法在飞控系统中的核心定位与转换关系。欧拉角提供直观的物理意义，旋转矩阵擅长向量坐标变换，四元数则是现代飞控姿态解算与控制的内核。下面我们逐一深入剖析。

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

核心原理

欧拉角通过三次绕不同坐标轴的旋转来定义刚体姿态。飞控中通常采用 Z-Y-X 内旋顺序（即先偏航 Yaw，再俯仰 Pitch，最后横滚 Roll），对应机体坐标系相对于地面坐标系的旋转。其数学本质是三个基本旋转矩阵的乘积：R = Rz(ψ) · Ry(θ) · Rx(φ)。其中 φ（Roll）、θ（Pitch）、ψ（Yaw）分别对应横滚角、俯仰角和偏航角。PX4 源码中，vehicle_attitude_setpoint 消息直接使用欧拉角作为用户期望姿态的输入接口，而 vehicle_attitude 消息则输出当前欧拉角用于地面站显示。

判断方法与操作流程

在飞控调试中，判断欧拉角是否处于正常范围是第一步。通常约定：Roll 和 Pitch 限制在 ±90° 以内（实际飞行中极少超过 ±60°），Yaw 为 ±180°。当 Pitch 接近 ±90° 时，欧拉角描述会出现万向节锁现象——此时 Yaw 与 Roll 的旋转轴重合，导致自由度丢失。操作流程上，若地面站观察到 Pitch 接近 90° 且 Yaw/Roll 数据剧烈跳变，应首先检查是否触发了奇异条件，而非传感器故障。

案例拆解：PX4 中欧拉角设定值的异常跳变

某多旋翼在自动航线飞行中，地面站显示 Yaw 角在 179° 与 -179° 之间反复跳变。初步分析：航向控制指令在角度环绕处未做平滑处理。检查源码发现，mc_att_control 模块中偏航误差计算使用了直接相减：yaw_error = target_yaw - current_yaw，当目标为 179°、当前为 -179° 时，误差为 358°，控制器会错误地选择大幅旋转方向。修复方案是引入角度归一化函数：yaw_error = wrap_pi(target_yaw - current_yaw)，将误差映射到 -180°~+180° 区间。修改后航向平滑过渡，跳变消失。此案例说明欧拉角虽然直观，但在边界处理上需要额外逻辑。

3.2 旋转矩阵：坐标变换的万能工具

核心原理

旋转矩阵（Direction Cosine Matrix, DCM）是一个 3×3 的正交矩阵，满足 R^TR = I 且 det(R)=1。其每一列表示机体坐标轴在地面坐标系中的方向余弦。飞控中，DCM 最重要的作用是向量坐标变换：将加速度计测量的比力从机体坐标系转换到地面坐标系，或将期望推力向量从地面系转换到机体系。PX4 的 math::Matrix3f 类封装了 DCM 运算，EKF2 中维护的 R 矩阵即为当前姿态的 DCM 表示。

判断方法与操作流程

验证 DCM 是否有效，可通过正交性检查：计算 R·R^T 是否接近单位矩阵。在源码调试中，若发现加速度融合异常，可打印 DCM 的第三行（即地面系 Z 轴在机体系中的投影），正常情况下其模长应为 1。操作流程：在飞控日志中提取 R[2][0], R[2][1], R[2][2]，计算平方和，若偏差超过 0.01 则说明矩阵已退化，需要重新初始化。

案例拆解：DCM 在互补滤波中的正交化修正

经典互补滤波（如 Mahony 算法）中，每次更新后 DCM 会因积分漂移失去正交性。源码中常见的修正方法是：R = R * (1.5*I - 0.5*R*RT)，该公式通过牛顿迭代将矩阵投影回 SO(3) 流形。某飞控在长时间飞行后出现姿态漂移，日志显示 DCM 正交性误差从 0.001 增长至 0.05。加入上述修正后，误差稳定在 0.0005 以内。此案例表明，DCM 虽然计算量较大（9 个元素），但其数学性质便于检测和修正。

3.3 四元数：飞控姿态解算的工业标准

核心原理

四元数 q = w + xi + yj + zk 是一种超复数，其中 w 为标量部分，(x,y,z) 为矢量部分。单位四元数满足 w²+x²+y²+z²=1，可表示三维空间中的旋转。其核心优势在于：无奇异点、存储紧凑（4 个浮点数）、插值平滑（球面线性插值 Slerp）。PX4 的 vehicle_attitude 消息中 q[4] 数组即为当前姿态四元数，EKF2 的状态向量中直接包含四元数分量作为状态量。

判断方法与操作流程

四元数的有效性检查包括：模长是否接近 1（容差 1e-4），以及 w 分量是否为正（若 w<0，则 q 与 -q 表示同一旋转，但习惯上取 w>0 以减少计算歧义）。在飞控源码中，四元数更新通常采用一阶龙格-库塔积分：q_dot = 0.5 * q ⊗ ω，其中 ω 为机体角速度四元数（实部为 0）。操作流程上，每次更新后需重新归一化：q = q / norm(q)，防止模长漂移。

案例拆解：四元数在 EKF2 中的状态更新与故障恢复

某固定翼无人机在高速飞行中，EKF2 报告姿态发散。分析日志发现，四元数模长在 0.5 秒内从 1.0 衰减至 0.92，导致旋转矩阵非正交。根因是陀螺仪噪声过大，且更新步长 h=0.01s 时，一阶积分误差累积。修复措施：① 在源码中增加四元数模长监测，当 |1-norm|>0.01 时强制重置为最近有效值；② 将积分方法升级为二阶龙格-库塔（中点法），减少离散化误差。修改后，四元数模长稳定在 0.999~1.001 之间，姿态恢复平稳。此案例展示了四元数在飞控中的核心地位——其数值稳定性直接决定整个控制链路的可靠性。

3.4 三种表示的转换关系与工程选型

核心原理

转换关系是飞控源码中频繁调用的数学工具。欧拉角转四元数：q = q_yaw(ψ) ⊗ q_pitch(θ) ⊗ q_roll(φ)，其中各轴四元数可直接由半角公式计算。四元数转欧拉角：φ = atan2(2(wx+yz), 1-2(x²+y²))，θ = asin(2(wy-zx))，ψ = atan2(2(wz+xy), 1-2(y²+z²))。四元数转旋转矩阵：R = I + 2w·[v]× + 2[v]ײ，其中 [v]× 为矢量部分的反对称矩阵。

判断方法与操作流程

在飞控源码中，转换函数通常位于 math::Quaternion 类中。判断转换是否正确的快速方法是：将同一姿态分别用三种方式表示，然后互相转换，验证误差小于 1e-6。操作流程上，当需要从传感器融合模块（四元数）输出到地面站（欧拉角）时，应使用官方提供的 to_euler() 方法，而非自行推导公式，以避免符号错误。

案例拆解：PX4 中四元数转欧拉角的精度验证

在开发自定义飞控时，团队需要将 EKF2 输出的四元数转换为欧拉角用于日志记录。初始实现直接套用标准公式，但在 Pitch=85° 时发现 Roll 和 Yaw 出现微小抖动。排查发现，asin 函数在输入接近 ±1 时对数值误差敏感。解决方案：在调用 asin 前将输入限制在 [-1, 1] 区间内，并增加 M_PI_2 的边界处理。修正后，全姿态范围内转换误差小于 0.01°。此案例提醒我们，数学公式的数值稳定性在飞控这种实时系统中至关重要。

3.5 总结复盘：姿态表示的工程要点

本章的核心结论可归纳为三点：第一，欧拉角是“人机接口”，适合展示和简单设定，但绝不能用于控制内环；第二，旋转矩阵是“变换工具”，适合向量旋转，但需关注正交性维护；第三，四元数是“控制内核”，适合姿态估计与插值，但需注意归一化。在飞控源码阅读中，遇到姿态相关变量时，首先判断其数据类型（Quatf、Matrix3f 还是 Eulerf），再决定调用相应的转换函数。实际调试中，若发现姿态控制异常，优先检查四元数模长和 DCM 正交性，这两项指标能快速定位数值发散问题。掌握这三种表示方法的本质联系，是深入理解飞控姿态控制链路的第一步，也是后续学习控制律设计、传感器融合的基础。