夜雨聆风
前三篇拆完了数据流、EKF2、uORB。传感器数据怎么进来的清楚了，大脑怎么融合的清楚了，消息怎么飞的清楚了。但一直没碰一个核心问题：EKF2算出"飞机在哪、朝哪"之后，谁在用它算"怎么飞"？
答案是一条四层嵌套的控制链：
位置设定值 → 位置控制器 → 速度设定值 → 速度控制器 → 推力+姿态设定值
→ 姿态控制器 → 角速率设定值 → 角速率控制器 → 执行器输出 → 混控器 → 电机PWM
四层PID，从外到内，一层套一层。外层的输出是内层的输入。位置控制器的输出喂给速度控制器，速度控制器的输出喂给姿态控制器，姿态控制器的输出喂给角速率控制器。角速率控制器的输出经过混控器变成四个电机的PWM。
这条链路是PX4飞行控制的核心骨架。理解了它，你就理解了PX4为什么能悬停、能定点、能飞航线。不理解它，调参就是瞎蒙。
这篇拆这条控制链的每一层：每层输入什么、输出什么、PID怎么算、参数怎么调、代码在哪。最后看混控器怎么把力矩变成PWM。
先搞清楚：为什么要四层
最直觉的想法：飞机要飞到某个位置，直接算一个力推过去不就完了？
不行。因为位置到力之间隔了太多东西。位置误差是3维的，但直接用力去消除位置误差会导致震荡——力作用在位置上是一个双积分关系（力→加速度→速度→位置），两个积分环节意味着180度相位滞后，纯比例控制必然震荡。
串级PID的思路是：不要让外层直接控制物理量，而是控制中间变量，让内层去稳定这个中间变量。
位置控制器不直接算力，算目标速度。速度是位置的一阶导数，控制速度比控制位置容易。 速度控制器不直接算力矩，算目标推力和目标姿态（倾斜角度）。推力控制高度，倾斜角度控制水平位移。 姿态控制器不直接算力矩，算目标角速率。角速率是姿态的一阶导数，控制角速率比控制姿态快。 角速率控制器直接输出力矩指令，这是最内层，响应最快。
每一层只管一层导数关系。外层管积分，内层管比例。这样每一层的闭环带宽可以独立设计，外层慢内层快，逐层稳定。
这是经典控制理论的思路，不是PX4的发明。直升机、固定翼、工业伺服都用串级PID。PX4的贡献是把这套东西在嵌入式上实现了，而且实现得相当干净。
第一层：位置控制器
位置控制器是整个串级的最外层，也是最慢的一层。它的任务很简单：给定目标位置和当前位置，算出应该以什么速度飞过去。
输入
vehicle_local_position | ||
vehicle_local_position | ||
trajectory_setpoint |
NED坐标系：North-East-Down，北-东-地。PX4的本地坐标系，原点是起飞点。
输出
trajectory_setpoint |
是的，输入输出用的同一个主题trajectory_setpoint。位置控制器从里面读位置设定值，往里面写速度设定值。这不是最好的设计——读写同一个主题容易混淆，但PX4就是这么干的，通过时序保证不会自读自写。
算法
// src/modules/mc_pos_control/MulticopterPositionControl.cpp
// 简化后的核心逻辑
voidMulticopterPositionControl::control_position()
{
// 位置误差
Vector3f pos_error = _pos_sp - _pos;
// 速度设定值 = 位置P增益 × 位置误差
// 注意：只有P，没有I和D
_vel_sp(0) = _params.pos_p(0) * pos_error(0); // North
_vel_sp(1) = _params.pos_p(1) * pos_error(1); // East
_vel_sp(2) = _params.pos_p(2) * pos_error(2); // Down
}
位置控制器只有P，没有I和D。这是经过实践的选择：
没有I：位置积分会导致超调和震荡。如果位置有稳态误差（比如风偏），速度控制器里的I项会补偿，不需要位置层做积分。 没有D：位置微分就是速度，而速度有独立的传感器测量（EKF2融合了GPS和IMU的速度估计），不需要在位置层做数值微分。
参数：MPC_XY_P（水平位置增益，默认0.95），MPC_Z_P（垂直位置增益，默认1.0）。这两个参数决定了位置响应的"软硬"——值越大，飞机越急切地往目标飞；值太大，会震荡。
还有速度限幅：
// 水平速度不超过MPC_XY_VEL_MAX
// 垂直速度不超过MPC_Z_VEL_MAX_UP / MPC_Z_VEL_MAX_DN
float vel_mag = _vel_sp.xy().norm();
if (vel_mag > _params.vel_max_xy) {
_vel_sp.xy() *= _params.vel_max_xy / vel_mag;
}
速度限幅不是控制需要，是安全需要。无人机速度太快会出事。
第二层：速度控制器
速度控制器是位置控制的下一层，也是连接位置和姿态的桥梁。它的输出不是直接力矩，而是推力大小和目标姿态——飞机要往哪倾斜、倾斜多少度、油门给多少。
这是整个控制链里最复杂的一层。因为它的输出跨越了两个物理量：推力（标量）和姿态（3D旋转），而这两个量是耦合的。
输入
vehicle_local_position | ||
trajectory_setpoint |
输出
vehicle_thrust_setpoint | ||
vehicle_attitude_setpoint |
算法
速度控制器的核心是PID，但不是简单的三个独立PID。因为多旋翼的推力方向和姿态耦合——推力总是沿机体Z轴（向上），要产生水平力就必须倾斜。
voidMulticopterVelocityControl::control_velocity()
{
// 速度误差
Vector3f vel_error = _vel_sp - _vel;
// PID
Vector3f vel_err_integral = _vel_err_int; // 积分项
Vector3f thrust_accel_sp; // 期望加速度
for (int i = 0; i < 3; i++) {
// P项
float p_term = _params.vel_p(i) * vel_error(i);
// I项
vel_err_integral(i) += vel_error(i) * dt;
float i_term = _params.vel_i(i) * vel_err_integral(i);
// D项（基于加速度估计的前馈，不是速度微分）
float d_term = _params.vel_d(i) * (-_vel(i));
thrust_accel_sp(i) = p_term + i_term + d_term;
}
// ---- 关键步骤：从期望加速度推导推力和姿态 ----
// 期望合力 = 重力补偿 + 期望加速度
Vector3f thrust_vector = -thrust_accel_sp;
thrust_vector(2) += 9.81f; // 补偿重力（NED坐标系Z轴向下）
// 推力大小 = 合力在当前偏航方向上的投影
float thrust_mag = thrust_vector.norm();
// 目标Z轴方向 = 合力方向（归一化）
Vector3f body_z_sp = thrust_vector / thrust_mag;
// 从目标Z轴和当前偏航角构造目标旋转矩阵 → 四元数
// 这一步是速度控制器和姿态控制器的接口
matrix::Quatf q_sp = compute_quaternion_from_z_and_yaw(body_z_sp, _yaw_sp);
// 推力归一化到0-1
float thrust_normalized = thrust_mag / (_params.thr_max);
thrust_normalized = math::constrain(thrust_normalized, 0.0f, 1.0f);
}
这几行代码藏着速度控制的核心物理：
重力补偿：悬停时加速度为0，但必须有推力抵消重力。NED坐标系Z轴向下，所以重力是+9.81。推力沿机体Z轴向上（-Z），所以补偿重力就是在Z分量上加9.81。
推力方向就是目标姿态：多旋翼的推力永远沿机体Z轴。要让飞机往北飞，就得让机体Z轴偏北倾斜——倾斜角由水平加速度需求决定。
body_z_sp就是目标机体Z轴方向，由它和目标偏航角一起唯一确定了目标姿态。推力大小和姿态是耦合输出：不是一个PID输出推力、另一个PID输出姿态。而是先算合力向量，再分解成大小（推力）和方向（姿态）。
参数：MPC_XY_VEL_P/MPC_XY_VEL_I/MPC_XY_VEL_D（水平），MPC_Z_VEL_P/MPC_Z_VEL_I/MPC_Z_VEL_D（垂直）。水平和垂直分开调，因为动力学特性不同——水平方向靠倾斜产生力，垂直方向直接靠推力。
第三层：姿态控制器
姿态控制器接收目标四元数，输出目标角速率。这一层也只需要P——因为角速率是姿态的导数，P增益乘姿态误差就是目标角速率。
输入
vehicle_attitude | ||
vehicle_attitude_setpoint |
输出
vehicle_rates_setpoint |
算法
姿态误差的计算不像位置或速度那样简单相减。四元数不能直接相减，因为四元数空间不是线性的——它覆盖在单位球面上，直接相减会走"直线"而不是球面上的"弧线"。
PX4的做法是：先把四元数误差转换成轴角表示，再用P增益映射到角速率。
voidMulticopterAttitudeControl::control_attitude()
{
// 当前姿态四元数
Quatf q = _vehicle_attitude.q; // 当前
Quatf qd = _attitude_setpoint.q_d; // 目标
// 四元数误差：q_err = qd * q^(-1)
// 这个误差四元数表示"从当前姿态旋转到目标姿态"的旋转
Quatf qe = qd * q.inversed();
// 从误差四元数提取轴角
// 四元数 q = [cos(θ/2), sin(θ/2)*axis]
// 轴角 = 2 * atan2(||虚部||, 实部) * 虚部/||虚部||
// 小角度近似：轴角 ≈ 2 * 虚部
Vector3f angle_error;
float qe_norm_imag = Vector3f(qe(1), qe(2), qe(3)).norm();
if (qe_norm_imag > 1e-4f) {
float angle = 2.0f * atan2f(qe_norm_imag, qe(0));
angle_error = angle * Vector3f(qe(1), qe(2), qe(3)) / qe_norm_imag;
} else {
// 小角度近似
angle_error = 2.0f * Vector3f(qe(1), qe(2), qe(3));
}
// P增益：角速率设定值 = P × 姿态误差
_rates_sp(0) = _params.att_p(0) * angle_error(0); // roll rate
_rates_sp(1) = _params.att_p(1) * angle_error(1); // pitch rate
_rates_sp(2) = _params.att_p(2) * angle_error(2); // yaw rate
}
小角度近似这一步值得多说。当姿态误差很小时（正常飞行状态），sin(θ/2) ≈ θ/2，cos(θ/2) ≈ 1，所以 2 × 虚部 ≈ θ × axis，就是轴角。这个近似在±30度以内误差小于1%，对飞控来说足够了。
为什么姿态控制器只有P没有I和D？
没有I：姿态稳态误差由角速率控制器的I项补偿。如果飞机有常值干扰力矩（比如重心偏移），角速率I会积分出一个补偿角速率，姿态误差最终收敛到0。 没有D：姿态微分就是角速率，有陀螺仪直接测量，不需要数值微分。
参数：MC_ROLL_P、MC_PITCH_P、MC_YAW_P。Roll和Pitch通常设2.5-4.0，Yaw通常小一些（1.5-2.5），因为偏航响应本来就慢，增益大了会震荡。
第四层：角速率控制器
角速率控制器是最内层，也是响应最快的一层。陀螺仪的测量频率250Hz（甚至更高），角速率控制器以同样的频率运行。它的输出直接是力矩指令——混控器把这个力矩指令变成四个电机的转速差。
这一层是完整PID。
输入
vehicle_angular_velocity | ||
vehicle_rates_setpoint |
输出
vehicle_torque_setpoint |
算法
voidMulticopterRateControl::control_rates()
{
// 角速率误差
Vector3f rates_error = _rates_sp - _rates;
// PID
for (int i = 0; i < 3; i++) {
// P项
float p_term = _params.rate_p(i) * rates_error(i);
// I项（带抗饱和）
_rates_err_int(i) += rates_error(i) * dt;
// 抗积分饱和：限制积分项不超过最大力矩的某个比例
float i_max = _params.rate_int_lim(i);
_rates_err_int(i) = math::constrain(_rates_err_int(i), -i_max, i_max);
float i_term = _params.rate_i(i) * _rates_err_int(i);
// D项（基于角加速度的前馈）
// 注意：这里不是rates_error的微分，而是rates的微分（即角加速度）
// rates_sp微分理论上应该加，但设定值变化太快会引入噪声
float d_term = _params.rate_d(i) * (_rates_sp_prev(i) - _rates(i)) / dt;
_rates_sp_prev(i) = _rates(i);
// 力矩输出
_torque(i) = p_term + i_term + d_term;
}
}
角速率PID有几个细节值得展开：
1. I项的anti-windup
角速率I项是最容易出问题的。如果飞机在地面上（电机没转），角速率控制器已经在积分误差——目标角速率是0，但陀螺仪有噪声，积分项会慢慢累积。等电机启动时，这个累积的积分项会输出一个大力矩，导致飞机突然翻转。
PX4在检测到推力接近0时（飞机没起飞或正在降落），会冻结积分项：
// 推力过小时冻结积分
if (_thrust_sp < 0.01f) {
_rates_err_int.zero();
}
更精细的做法是clamp（上面代码里的rate_int_lim），限制积分幅度。PX4默认限制在最大力矩的33%。
2. D项的实现
角速率D项不是对误差微分，而是对角速率微分——也就是角加速度。陀螺仪的测量噪声在微分后会放大，所以D增益一般很小（roll/pitch的D增益通常只有P增益的1/5到1/10），而且有的飞控干脆不用D。
PX4默认的角速率D增益：roll 0.004，pitch 0.004，yaw 0.0。Yaw轴没有D项，因为偏航轴的阻尼已经足够（反扭矩天然阻尼），不需要额外的微分阻尼。
3. 前馈项
PX4的角速率控制器还有一个容易被忽略的前馈项——角速率设定值直接乘一个增益加到输出上：
// 前馈：角速率设定值 → 力矩
// 物理含义：以目标角速率旋转需要的力矩 = I × α
// 简化假设：匀速旋转时α≈0，但加速阶段需要力矩
float ff_term = _params.rate_ff(i) * _rates_sp(i);
_torque(i) += ff_term;
前馈增益MC_ROLLRATE_FF、MC_PITCHRATE_FF、MC_YAWRATE_FF通常设0，除非你知道你飞机的转动惯量。设对了前馈可以显著减小跟踪误差，但设错了比不设更糟。
参数：MC_ROLLRATE_P/MC_ROLLRATE_I/MC_ROLLRATE_D，pitch和yaw类似。典型的250mm穿越机：P=0.15，I=0.2，D=0.004。大机架（450mm以上）：P=0.08，I=0.15，D=0.002。
混控器：力矩和推力怎么变成PWM
四层PID算出来的是推力标量（0-1）和三个力矩分量（roll/pitch/yaw，-1~1）。但电机只认PWM——一个0-100%的油门值。中间的转换就是混控器（Mixer）做的事。
X型四旋翼的混控矩阵
四旋翼四个电机编号（从上方俯视）：
前
3 1
X
2 4
后
1号和3号逆时针旋转，2号和4号顺时针旋转。
每个电机产生的效果：
写成矩阵形式：
[T] [1 1 1 1 ] [M1]
[τroll] = [-1 -1 +1 +1] [M2]
[τpitch] [+1 -1 +1 -1] [M3]
[τyaw] [-1 +1 +1 -1] [M4]
混控器要做的是逆运算：已知T、τroll、τpitch、τyaw，求M1-M4。求逆矩阵：
[M1] [1 -1 +1 -1] [T]
[M2] = 0.25×[1 -1 -1 +1] [τroll]
[M3] [1 +1 +1 +1] [τpitch]
[M4] [1 +1 -1 -1] [τyaw]
代码实现：
// src/modules/control_allocator/ControlAllocator.cpp
// 简化的X型四旋翼混控
voidControlAllocator::allocate_multicopter()
{
// 归一化的推力和力矩
float T = _thrust_sp; // 0-1
float t_roll = _torque_sp(0); // -1 to 1
float t_pitch = _torque_sp(1); // -1 to 1
float t_yaw = _torque_sp(2); // -1 to 1
// 混控矩阵（X型，归一化）
float m1 = 0.25f * ( T - t_roll + t_pitch - t_yaw);
float m2 = 0.25f * ( T - t_roll - t_pitch + t_yaw);
float m3 = 0.25f * ( T + t_roll + t_pitch + t_yaw);
float m4 = 0.25f * ( T + t_roll - t_pitch - t_yaw);
// 饱和限幅：0到1
m1 = math::constrain(m1, 0.0f, 1.0f);
m2 = math::constrain(m2, 0.0f, 1.0f);
m3 = math::constrain(m3, 0.0f, 1.0f);
m4 = math::constrain(m4, 0.0f, 1.0f);
// 转PWM：0-1 → PWM范围（典型1075-1950us）
_outputs[0] = PWM_MIN + m1 * (PWM_MAX - PWM_MIN);
_outputs[1] = PWM_MIN + m2 * (PWM_MAX - PWM_MIN);
_outputs[2] = PWM_MIN + m3 * (PWM_MAX - PWM_MIN);
_outputs[3] = PWM_MIN + m4 * (PWM_MAX - PWM_MIN);
}
饱和问题
混控器最大的麻烦是饱和。四个电机的输出都在0-1之间，但PID不知道这个限制——它可能算出一个需要某个电机输出1.5的指令，这不可能。
饱和会导致两个问题：
优先级冲突：推力和力矩抢资源。比如全油门爬升时（T≈1），没有余量给力矩——四个电机都在最大转速，无法通过转速差产生力矩。此时飞机失去姿态控制能力。
积分饱和：角速率I项持续积分，但混控器饱和导致实际力矩上不去。积分项越积越大，等推力降下来后，累积的积分项会产生巨大的力矩超调。
PX4的混控器处理饱和的方式比较粗暴但有效——优先保姿态，牺牲推力：
// 如果有电机饱和（>1），降低总推力来腾出余量
float max_motor = math::max(math::max(m1, m2), math::max(m3, m4));
if (max_motor > 1.0f) {
float scale = 1.0f / max_motor;
m1 *= scale;
m2 *= scale;
m3 *= scale;
m4 *= scale;
// 总推力被压缩了，但力矩比例保持不变
}
// 如果有电机饱和（<0），抬高总推力来避免负值
float min_motor = math::min(math::min(m1, m2), math::min(m3, m4));
if (min_motor < 0.0f) {
float offset = -min_motor;
m1 += offset;
m2 += offset;
m3 += offset;
m4 += offset;
// 增加推力来避免负值，力矩比例不变
}
这两步操作的本质：在推力和力矩冲突时，保持力矩比例不变，调整推力大小。 因为姿态失控比掉高度更危险——飞机翻倒后推力方向全错，直接坠机。掉高度至少还有时间纠正。
四层的时序：谁先跑谁后跑
四层控制不是同时跑的。PX4的调度优先级决定了执行顺序：
实际调度中，姿态控制器和角速率控制器在同一个模块mc_rate_control里串联执行，不是一个调另一个，而是按顺序调：
// 伪代码：每个250Hz周期
voidcontrol_step()
{
// 1. 读最新姿态（从uORB）
orb_copy(vehicle_attitude, &att);
// 2. 姿态控制器：attitude → rate setpoint
attitude_control(att, att_sp, &rates_sp);
// 3. 读最新角速率（从uORB）
orb_copy(vehicle_angular_velocity, &rates);
// 4. 角速率控制器：rate → torque
rate_control(rates, rates_sp, &torque);
}
速度控制器和位置控制器在另一个模块mc_pos_control里：
// 伪代码：每个100Hz/50Hz周期
voidcontrol_step()
{
// 1. 位置控制器：pos → vel setpoint
position_control(pos, pos_sp, &vel_sp);
// 2. 速度控制器：vel → thrust + attitude setpoint
velocity_control(vel, vel_sp, &thrust, &att_sp);
}
两个模块之间通过uORB连接：mc_pos_control发布vehicle_attitude_setpoint，mc_rate_control订阅它。发布/订阅的时序依赖uORB的generation机制——如果mc_rate_control跑得比mc_pos_control快（250Hz vs 100Hz），它会在5个周期里读到同一个设定值，然后每个周期都重新算角速率。这不是问题，因为角速率控制器看到的目标姿态没变，它就维持当前角速率。
调参指南：从内到外
串级PID的调参有一个铁律：从内到外，先调内层再调外层。 内层不稳定，外层调了也白调。
第一步：角速率环
把飞机固定在桌面上（或者去掉桨叶），用手给飞机一个角速率扰动，看角速率响应。
先只调P。从0.05开始，每次加0.01，直到角速率响应快速但不震荡。 加I。从0.05开始，给一个常值力矩（比如用手指轻推），看I项能不能消除稳态误差。I太大→震荡，太小→消除慢。 D一般不动。如果高频震荡，先降P，不是加D。
第二步：姿态环
装回桨叶，低速悬停。
P从2.0开始。打roll/pitch杆，看飞机跟上没有。响应慢→加大，震荡→减小。 Yaw的P通常比roll/pitch小，因为偏航响应天然慢。
第三步：速度环
定点悬停，观察位置保持。
P从0.5开始。推飞机一下，看它回来快不快。回不来→加大，回来过冲→减小。 I从0.1开始。有风时看能不能稳住。积分太大会超调。 D通常0（或很小的值）。
第四步：位置环
飞航线或定点。
P从0.95开始（默认值就不错）。 位置环只有P，没什么好调的。如果定点漂移，问题多半在速度环的I不够。
DShot：PWM之后的最后一公里
混控器输出0-1的归一化值，但电机最终需要的是电调（ESC）能理解的信号。PX4支持两种协议：
PWM（传统）：1000-2000us脉宽，分辨率1us，更新率通常400-490Hz。
DShot（现代）：数字协议，16位帧（11位油门值+1位遥测请求+4位CRC），更新率可达1kHz以上。DShot 600表示600kbps波特率，DShot 1200是1200kbps。
DShot的优势：
不需要校准：PWM需要校准油门范围（最小最大脉宽），DShot的0-2047直接对应0-100%油门。 更高更新率：1kHz以上，比PWM的400Hz快一倍多。 双向遥测：DShot 300/600/1200支持ESC回传RPM，可以用来做闭环转速控制（但目前PX4没启用这个功能）。 抗干扰：数字信号+CRC校验，PWM是模拟信号，长线容易受干扰。
代码层面，DShot和PWM的混控输出逻辑完全一样——都是算出0-1的归一化值。区别只在最后的信号编码：
// src/modules/px4fmu/fmu.cpp
// PWM模式
_output_pwm[i] = PWM_MIN + normalized_output * (PWM_MAX - PWM_MIN);
// DShot模式
_output_dshot[i] = (uint16_t)(normalized_output * 2047.0f);
如果你的电调支持DShot，用DShot。更简单、更快、更可靠。
代码阅读路径
控制器模块的代码比EKF2好读得多，因为控制逻辑是显式的数学公式，不需要猜算法意图。按这个顺序：
src/modules/mc_pos_control/— 位置和速度控制器。先看MulticopterPositionControl.cpp的control_position()和control_velocity()。src/modules/mc_rate_control/— 姿态和角速率控制器。先看MulticopterRateControl.cpp的control_attitude()和control_rates()。src/modules/control_allocator/— 混控器。看ControlAllocator.cpp的allocate_multicopter()。src/modules/px4fmu/— 底层PWM/DShot输出。如果只关心控制逻辑，这层可以跳过。
每个模块的结构都很相似：一个主类，一个Run()方法作为主循环，control_xxx()方法做具体控制。参数定义在对应的params.c里。
和ArduPilot的对比
PX4和ArduPilot都用串级PID，但实现差异不小：
最显著的差异在位置环——ArduPilot的位置控制器有I项，PX4没有。ArduPilot的理由是：有风时位置积分可以消除稳态偏差，避免持续漂移。PX4的理由是：位置积分会增加超调，速度环的I项已经能补偿风的影响。
两种做法都能飞。PX4的选择更保守，不容易调出危险的参数组合。ArduPilot的选择更灵活，但调参门槛更高。
小结
四层串级PID的控制链，压缩成一句话：外层算设定值，内层跟踪设定值，每一层只管一层导数关系，逐层递进。
核心机制：
位置控制器（P only）：位置误差 → 速度设定值 速度控制器（PID）：速度误差 → 推力+姿态设定值，重力补偿在NED坐标下Z轴+9.81 姿态控制器（P only）：四元数误差 → 轴角 → 角速率设定值 角速率控制器（PID+FF）：角速率误差 → 力矩，anti-windup冻结积分，D项基于角加速度 混控器：X型4×4逆矩阵，饱和时优先保力矩比例、牺牲推力 调参铁律：从内到外，先角速率再姿态再速度再位置
控制器不像EKF2那样藏着复杂的数学推导，也不像uORB那样有精巧的系统设计。它就是PID，加上一些工程上的取舍和妥协。但这些取舍恰恰是PX4能稳定飞行的原因——简单、可调、可预测。
代码基于PX4 v1.14，控制器模块路径：src/modules/mc_pos_control/、src/modules/mc_rate_control/、src/modules/control_allocator/
