2. PX4姿态控制模块概览

PX4 作为当前最活跃的开源飞行控制软件之一，其姿态控制模块是连接“感知”与“执行”的核心枢纽。理解该模块的代码位置、文件组织方式以及数据流转路径，是进行源码级调参与二次开发的前提。本章将带领大家从宏观架构入手，逐步拆解姿态控制模块在 PX4 系统中的坐标、其内部文件的功能划分，以及从传感器原始数据到执行器PWM输出的完整信号链。同时，我们也会重点厘清姿态控制与姿态估计（EKF2/互补滤波）之间的边界与协作关系，为后续逐行解析控制算法打下坚实的工程认知基础。

上图清晰地展示了 PX4 姿态控制模块在整体系统中的位置。从物理世界到执行器，数据流经三个关键阶段：传感器采集、姿态估计、姿态控制。值得注意的是，姿态控制模块并非直接读取传感器原始数据，而是消费姿态估计模块（主要是 EKF2）输出的融合状态量（四元数、角速度、加速度）。这种解耦设计使得控制算法可以独立于传感器特性进行开发和调优。

2.1 模块位置与源码文件结构

核心原理：PX4 采用模块化设计，所有功能组件均以独立进程（或任务）形式运行，通过 uORB 消息总线进行通信。姿态控制模块的官方名称为 mc_att_control（Multicopter Attitude Control），其源码位于 src/modules/mc_att_control/ 目录下。该模块主要负责将期望姿态（来自遥控器、任务规划器或外部指令）转换为期望力矩和推力，供混控器（Mixer）进一步处理。

文件结构解析：下表列出了该模块的核心文件及其职责，这是后续逐行阅读源码的“地图”。

文件名	核心职责	关键类/函数
mc_att_control_main.cpp	模块入口，负责初始化、参数读取、uORB主题订阅与发布、任务循环	MulticopterAttitudeControl::task_main()
mc_att_control.cpp	核心控制逻辑，包含外环角度控制与内环角速率控制的调用与协调	control_attitude(), control_rate()
AttitudeControl.cpp	外环角度控制器，将四元数误差转换为期望角速率	AttitudeControl::control_attitude()
RateControl.cpp	内环角速率控制器，将期望角速率转换为力矩指令（PID）	RateControl::control_rate()
ControlMath.cpp	数学工具集，包括推力限幅、向量旋转、轴角转换、平滑函数等	ControlMath::thrust_limiting(), ControlMath::smooth_yaw()
parameters.c	定义模块所有可调参数（如 MC_ROLL_P, MC_RATE_P 等）	参数宏定义

2.2 数据流全景：从传感器到执行器

判断方法/操作流程：理解数据流是排障的基础。我们可以将整个链路划分为四个阶段，每个阶段都有明确的输入输出主题（uORB Topic）。

1. 传感器数据采集：IMU（陀螺仪+加速度计）以 1kHz 以上频率输出原始数据，经传感器驱动校正后，发布到 sensor_gyro 和 sensor_accel 主题。磁力计发布到 sensor_mag。
2. 姿态估计（EKF2）：EKF2 模块订阅上述传感器主题，结合 GPS/气压计进行 31 维状态估计。其输出为 vehicle_attitude（四元数姿态）、vehicle_angular_velocity（角速度）、vehicle_acceleration（加速度）。这些是姿态控制的“测量值”。
3. 姿态控制（mc_att_control）：订阅 vehicle_attitude 和 vehicle_angular_velocity，同时订阅 vehicle_attitude_setpoint（期望姿态，来自位置控制器或遥控器）。经过外环和内环计算后，输出 actuator_controls_0 主题，包含期望滚转、俯仰、偏航力矩和推力。
4. 混控与执行：混控器（Mixer）模块订阅 actuator_controls_0，根据机型（四旋翼、六旋翼等）将力矩和推力映射为每个电机的 PWM/DShot 指令，最终通过 IO 或 FMU 输出到电调。

案例拆解：假设无人机悬停时受到一阵侧风干扰，机身向右倾斜。此时数据流如下：

• 传感器：陀螺仪检测到向右的角速度，加速度计检测到重力分量变化。
• 估计：EKF2 融合数据，更新四元数，使估计姿态向右倾斜约 5 度。
• 控制：姿态控制模块发现当前姿态（右倾 5°）与期望姿态（水平 0°）存在偏差。外环角度控制器计算出一个期望角速率（向左旋转），内环角速率控制器对比当前角速度与期望角速度，输出一个向左的滚转力矩。
• 执行：混控器将滚转力矩转换为左侧电机转速降低、右侧电机转速升高，产生反向力矩，无人机恢复水平。

整个过程在 10-20ms 内完成，体现了姿态控制的高频特性。

2.3 与姿态估计（EKF2）的关系：边界与协作

核心原理：姿态控制与姿态估计是“控制”与“感知”的关系，二者通过 uORB 主题解耦，但存在紧密的时序与质量依赖。姿态控制的质量上限由姿态估计的精度和延迟决定。如果 EKF2 输出的姿态有 10ms 延迟，那么控制环的相位裕度会显著下降，导致系统震荡。

对比分析：下表总结了二者的核心区别与协作点。

维度	姿态估计 (EKF2)	姿态控制 (mc_att_control)
输入	IMU、Mag、Baro、GPS	估计姿态、期望姿态
输出	四元数、角速度、加速度	力矩、推力指令
频率	250-500 Hz（EKF2 内部 1kHz）	250-500 Hz（与估计同步）
核心算法	扩展卡尔曼滤波（EKF）	PID + 四元数误差控制
调参关注点	噪声协方差、传感器延迟补偿	P/I/D 增益、积分限幅、前馈

常见误区：初学者常犯的错误是，当飞机高频抖动时，直接去调控制器的 PID 增益，而忽略了检查 EKF2 的 innovation 序列是否异常。如果估计的姿态本身包含大量噪声或延迟，无论怎么调 PID 都无法根治。正确的排障顺序是：先通过 ekf2_status 主题检查估计质量，再通过 vehicle_attitude 与真实姿态对比，最后才调整控制参数。

2.4 实战场景：如何快速定位姿态异常模块

案例背景：一架四旋翼在手动模式下起飞后，出现持续的低频振荡（约 2Hz），飞行员感觉飞机“点头”。初始现象是俯仰轴来回摆动，幅度逐渐增大。

分析过程：我们按照数据流顺序排查：

1. 检查传感器：通过 listener sensor_gyro 命令查看陀螺仪原始数据，发现噪声水平正常（±0.5°/s），排除传感器故障。
2. 检查估计：查看 listener vehicle_attitude，发现估计姿态与真实姿态（通过水平仪目测）基本一致，但存在约 30ms 的延迟。进一步检查 ekf2_status，发现磁力计 innovation 偏大，但俯仰轴主要依赖 IMU，影响较小。
3. 检查控制：查看 listener actuator_controls_0，发现俯仰力矩指令在 ±0.2 之间振荡，频率与控制频率一致。初步判断是控制增益过高。
4. 参数调整：将 MC_PITCH_P 从 6.5 降低至 5.5，同时将 MC_PITCHRATE_P 从 0.15 降低至 0.12。振荡幅度明显减小。
5. 结果对比：调整后，振荡频率从 2Hz 降至 0.5Hz 以下，飞机恢复稳定。但发现响应变慢，于是适当增加 MC_PITCHRATE_I 以消除稳态误差。

复盘经验：低频振荡通常与外环角度 P 增益过高有关，而高频抖动（>10Hz）则多与内环速率 P 增益或陀螺仪噪声有关。通过数据流定位，可以避免盲目调参。此外，记录调参前后的 actuator_controls 输出曲线，是判断系统稳定性的有效手段。

通过本章的学习，我们建立了 PX4 姿态控制模块的完整认知框架。下一层次，我们将深入 mc_att_control_main.cpp 的代码细节，逐行解析模块的初始化、循环调度和 uORB 交互逻辑。请务必在本地源码中对照阅读，以加深理解。