3. 坐标系与旋转表示

姿态控制是PX4飞控系统的核心，而姿态控制的前提是精确地描述飞行器在三维空间中的朝向。任何姿态控制算法，无论其控制律多么先进，都依赖于对坐标系和旋转表示的深刻理解。本章将系统梳理机体坐标系与世界坐标系的定义、欧拉角与四元数的数学原理、旋转矩阵的构建方法，并深入PX4源码，剖析这些数学工具在真实飞控代码中的具体实现与转换逻辑。理解本章内容，是后续逐行解析姿态控制器、调参以及排障的数学与工程基础。

3.1 核心原理：从NED世界系到FRD机体系的数学桥梁

在PX4中，世界坐标系采用NED（North-East-Down）右手系，即X轴指向地理北极，Y轴指向正东，Z轴垂直指向地心。机体坐标系则采用FRD（Front-Right-Down）右手系，X轴指向飞行器机头方向，Y轴指向右翼，Z轴指向机体下方。两个坐标系均为正交直角坐标系，但原点与轴向定义不同。飞行器的姿态，本质上就是机体坐标系相对于世界坐标系的旋转关系。这种旋转关系可以用三种数学工具表示：欧拉角、旋转矩阵和四元数。

欧拉角是最直观的表示方式，通过绕三个轴的依次旋转来定义姿态。PX4中约定旋转顺序为Z-Y-X，即先偏航（Yaw，绕Z轴），再俯仰（Pitch，绕Y轴），最后横滚（Roll，绕X轴）。这种顺序避免了万向节锁问题在常规飞行包线内的影响，且符合航空惯例。然而，欧拉角在俯仰角接近±90°时会出现奇异性，导致姿态解算失效，因此PX4内部核心计算并不直接使用欧拉角。

旋转矩阵（Direction Cosine Matrix, DCM）是一个3×3的正交矩阵，其列向量分别表示机体坐标系的三个轴在世界坐标系中的投影。DCM可以无奇异地将任意向量从机体系变换到世界系，反之亦然。但DCM包含9个元素，存在6个约束条件（正交性和单位模长），在数值迭代中容易产生漂移，需要频繁正交化修正。

四元数是一种超复数，由一个实部和三个虚部组成，形如 q = q0 + q1·i + q2·j + q3·k。单位四元数可以唯一表示三维空间中的旋转，且不存在奇异性，仅需4个参数，计算效率高，适合嵌入式实时运算。PX4的姿态估计与控制核心均采用四元数作为姿态的主要存储与运算形式。下表对比了三种表示方法的关键特性：

3.2 判断方法与操作流程：PX4中四元数、欧拉角与旋转矩阵的转换链路

在PX4源码中，姿态数据的流转遵循一条清晰的链路。首先，姿态估计器（如attitude_estimator_q）输出四元数形式的姿态估计。然后，控制模块（如mc_att_control）需要将四元数转换为旋转矩阵，以便将期望姿态（通常以欧拉角或四元数给定）与当前姿态进行误差计算。最后，在日志记录和地面站通信时，四元数会被转换为欧拉角供用户查看。

PX4使用matrix库中的Quatf、Eulerf和Dcmf类来封装这些数学运算。开发者无需手动实现转换公式，只需调用相应的成员函数。例如，从四元数获取旋转矩阵，调用 Dcmf R = Dcmf(q); 即可。从欧拉角构建四元数，则使用 Quatf q = Quatf(Eulerf(roll, pitch, yaw));。这些转换在底层均经过严格的数学推导与优化，保证了数值稳定性。

在实际调参过程中，理解转换关系至关重要。例如，当我们在QGroundControl中设置期望姿态角时，地面站会将欧拉角打包为MAVLink消息发送给飞控，飞控内部将其转换为四元数后送入控制器。若用户发现姿态响应方向错误，往往是因为欧拉角旋转顺序或坐标轴定义与PX4约定不一致所致。因此，任何自定义的外部控制代码，都必须严格遵循PX4的NED/FRD定义和Z-Y-X旋转顺序。

Quatf qd = _att_sp.q_d;
Quatf q = _att.q;
Quatf qe = qd * q.inversed();  // 误差四元数

Dcmf R = Dcmf(qe);
Vector3f euler_error = R.to_euler();  // 提取欧拉角误差

3.3 案例拆解：一次偏航角指令异常的分析与修复

某无人机在自动航线测试中，出现偏航角响应滞后且方向相反的现象。地面站显示期望偏航角为30°，但实际机头指向约为-30°。初步排查了舵机与电机响应，均正常。通过分析飞行日志，发现姿态估计器输出的四元数正确，但控制器计算出的期望角速度方向错误。

进一步检查代码，发现自定义的航线控制模块在生成期望姿态时，使用了错误的旋转顺序。该模块直接使用欧拉角构建四元数，但采用了X-Y-Z的旋转顺序，而PX4内部约定为Z-Y-X。这导致在非零横滚和俯仰状态下，偏航分量被错误耦合。具体数据如下：飞行器当前横滚10°，俯仰5°，期望偏航30°。使用错误的X-Y-Z顺序构建的四元数，其等效偏航角仅为约28.5°，且引入了额外的横滚/俯仰耦合。

修复方案是将自定义模块中的四元数构建方式改为PX4标准接口：

// 错误方式：手动按X-Y-Z顺序计算
// 正确方式：使用matrix库的Eulerf类
Eulerf euler(roll_rad, pitch_rad, yaw_rad);
Quatf q_des = Quatf(euler);

修改后重新测试，偏航响应恢复正常。此案例表明，在PX4生态中，任何涉及姿态的代码都必须复用其标准数学库，避免自行实现旋转转换，否则极易因约定不一致引入难以排查的隐性故障。

3.4 常见误区与总结复盘

误区一：混淆NED与ENU坐标系。PX4使用NED，而部分视觉定位或外部导航库使用ENU（X-东，Y-北，Z-上）。若直接混用，会导致所有轴上的姿态与位置控制反向。必须通过坐标变换矩阵进行转换。

误区二：忽视四元数的单位化。四元数必须保持单位模长（q0²+q1²+q2²+q3²=1）。在长时间运行或外部干扰下，四元数可能发生漂移。PX4在姿态估计器中会定期对四元数进行归一化处理，但用户自定义代码中若频繁进行四元数乘法，也应注意归一化。

误区三：将欧拉角用于控制回路内部。欧拉角的奇异性会导致控制器在特定姿态下输出跳变。PX4内部控制律完全基于四元数和旋转矩阵，欧拉角仅用于人机交互。调参时，应通过地面站观察欧拉角，但理解控制行为需回归四元数视角。

误区四：旋转矩阵的更新未做正交化修正。在纯软件仿真或自定义姿态解算中，若直接对DCM进行积分，矩阵会逐渐失去正交性。PX4的Dcmf类在每次更新后都会执行施密特正交化，确保矩阵始终有效。