4、姿态控制原理：PID控制、角速度环与角度环的级联结构、控制分配基础
PX4 的姿态控制是整个飞控系统中最核心、也是与飞行品质直接相关的模块。它接收来自姿态估计器（如 EKF2）的当前四元数/欧拉角与角速度，结合用户期望的姿态指令，输出期望的力矩指令，最终通过混控器映射到各个执行机构（电机、舵机）。理解这一章的内容，是后续进行源码逐行解析和调参实战的基石。本章将深入剖析PX4姿态控制器的内部机理，重点讲解PID控制、内外环级联结构以及控制分配的基本原理。
上图清晰地展示了PX4姿态控制器的级联结构。从期望姿态输入到最终电机指令输出，信号依次经过角度环、角速度环和控制分配三个主要阶段。其中，角度环负责“纠正方向偏差”，角速度环负责“抑制旋转速率”，而控制分配则负责“执行力矩指令”。下面我们逐一深入解析。
4.1 核心原理：PID控制与级联架构
PID（比例-积分-微分）控制是姿态控制的基础。在PX4中，它被巧妙地拆分为两个级联的控制器，而不是一个复杂的单环PID。这种设计源于物理直觉：角度是角速度的积分，角速度是力矩的积分。通过分别控制角速度和角度，可以更精细地调节飞行器的动态响应。
- 角度环（外环）：输入是期望姿态角（如期望横滚角30°）和当前姿态角（由EKF2估计）。其输出是期望的角速度。由于角度环的响应速度要求相对较慢，通常只使用比例（P）控制，以避免超调和振荡。P值越大，纠正姿态偏差的“力度”越大，但过大会导致角速度指令过大，引发内环饱和。
- 角速度环（内环）：输入是角度环输出的期望角速度（如期望横滚角速度200°/s）和当前角速度（由陀螺仪直接测量）。其输出是期望的力矩。角速度环是姿态控制的核心，必须使用完整的PID控制。P项提供主要阻尼，I项消除稳态误差（如重心偏移导致的持续倾斜），D项则用于抑制角速度的快速变化，提高系统稳定性。
为什么不用一个大的PID直接控制角度？
如果直接用角度误差计算力矩，系统相当于一个二阶系统，其阻尼特性难以独立调节。而级联结构将二阶系统拆分为两个一阶系统，使得我们可以先稳定内环（角速度），再稳定外环（角度），大大简化了调参难度，并且对陀螺仪噪声的敏感度更低。
4.2 判断方法与操作流程：从期望到力矩的完整链路
在PX4源码中，姿态控制的核心函数是 MulticopterAttitudeControl::control_attitude()。其处理流程可以归纳为以下步骤：
- 姿态误差计算：将期望姿态四元数（
attitude_setpoint）与当前姿态四元数（attitude）做差，得到姿态误差四元数。该误差四元数被转换为机体坐标系下的旋转向量，并乘以角度环的P增益（params->mc_att_p），得到期望角速度（rates_setpoint）。 - 角速度误差计算：将期望角速度与当前角速度（
rates）做差，得到角速度误差向量。 - PID控制律：对角速度误差分别进行比例、积分、微分运算，并相加得到期望力矩向量（
torque）。其中，积分项带有抗积分饱和（anti-windup）机制，微分项通常使用角速度反馈而非误差微分，以避免噪声放大。 - 推力与力矩合成：期望推力（
thrust）由油门杆量直接映射，与期望力矩一起作为控制分配模块的输入。
| 环节 | 输入 | 输出 | 控制器类型 | 关键参数 |
|---|---|---|---|---|
| 角度环 | 期望姿态角、当前姿态角 | 期望角速度 | P控制器 | MC_ATT_P |
| 角速度环 | 期望角速度、当前角速度 | 期望力矩 | PID控制器 | MC_ROLLRATE_P/I/D, MC_PITCHRATE_P/I/D, MC_YAWRATE_P/I/D |
| 控制分配 | 期望力矩、期望推力 | 电机PWM指令 | 混控矩阵 | PWM_RATE, 机架类型 |
4.3 案例拆解：穿越机横滚通道的调参与振荡分析
假设我们正在调试一架250mm轴距的穿越机，用户反馈在急转弯时飞机出现高频抖动（约15Hz），且横滚轴回中缓慢。我们通过黑匣子日志（.ulg文件）分析，发现横滚角速度（vehicle_rates_setpoint 与 vehicle_rates）之间存在明显的相位滞后和振荡。
初始参数：MC_ROLLRATE_P = 0.08, MC_ROLLRATE_I = 0.05, MC_ROLLRATE_D = 0.001。
分析过程：
- 高频抖动：15Hz的抖动通常与角速度环的P增益过高或D增益不足有关。P增益过高导致系统对误差响应过激，而D增益不足则无法提供足够的阻尼来抑制振荡。
- 回中缓慢：这通常意味着角度环的P增益（
MC_ATT_P）偏低，或者角速度环的I增益不足，无法消除稳态误差。
操作步骤：
- 首先降低
MC_ROLLRATE_P至 0.06，观察抖动是否减弱。若减弱，则确认是P过高。 - 逐步增加
MC_ROLLRATE_D至 0.003，每次增加0.0005，观察高频噪声是否被抑制。注意D项对陀螺仪噪声敏感，过大会导致电机发热。 - 若回中仍然缓慢，将
MC_ATT_P从默认的6.5提升至7.5，并观察角度跟踪效果。 - 最后，检查I项。在悬停时观察横滚角是否有缓慢漂移，若有，则适当增加
MC_ROLLRATE_I至 0.08。
结果对比：调整后，高频抖动消失，横滚回中时间从1.2秒缩短至0.6秒，且无超调。代价是D项增加后，电机温度略有上升，但在可接受范围内。
风险提示：调参时务必遵循“先内环后外环”的原则。如果内环不稳定，外环调得再好也无济于事。每次只改变一个参数，并记录日志，以便回溯。
4.4 控制分配基础：从力矩到PWM
控制分配是姿态控制的“最后一公里”。PX4使用混控矩阵将期望的滚转力矩（L）、俯仰力矩（M）、偏航力矩（N）和推力（T）映射到各个电机的转速指令。对于常见的四旋翼X型布局，其混控矩阵如下：
// 电机1(右前) 电机2(左后) 电机3(左前) 电机4(右后)
// 滚转力矩 L = 0.5 * (T1 - T2 - T3 + T4)
// 俯仰力矩 M = 0.5 * (T1 + T2 - T3 - T4)
// 偏航力矩 N = -T1 + T2 - T3 + T4
// 推力 T = T1 + T2 + T3 + T4在源码中，mixer 模块会根据机架类型（如 QUAD_X）自动生成对应的混控矩阵。该矩阵的逆矩阵用于将期望力矩和推力转换为各电机的期望转速。值得注意的是，当某个电机达到输出饱和（如PWM达到最大值）时，控制分配会进行“饱和处理”，优先保证推力，而牺牲部分力矩，这可能导致飞行器在极限机动时出现响应迟钝。
实战经验：在调试大型无人机或自定义机架时，务必检查混控器输出是否合理。可以通过MAVLink地面站（如QGroundControl）的电机测试界面，逐个电机测试转向和转速，确保与混控矩阵一致。
4.5 常见误区与总结复盘
- 误区一：盲目增大角速度环P增益。P增益过大会导致高频振荡，甚至损坏电机和机架。应结合D增益和滤波器（如陀螺仪低通滤波）综合考虑。
- 误区二：忽略积分项的限幅。积分项在长时间偏载或风扰下会累积过大，导致超调。PX4中通过
MC_RR_INT_LIM等参数限制积分输出，调参时不应随意增大该限幅。 - 误区三：内外环带宽不匹配。如果角度环响应过快，会不断给内环发送“激进”的角速度指令，导致内环饱和。通常内环带宽应为外环的5~10倍。