4. 姿态控制算法：从解算到控制的完整链路
姿态控制，说白了就是让无人机知道「我现在是什么姿势」，然后告诉它「你应该摆什么姿势」。这个环节要是出问题，飞控再牛也白搭。我最早做飞控调试时，就吃过姿态解算不准的亏——飞机悬停时莫名其妙地往一边飘，查了三天才发现是陀螺仪零偏没处理好。
这一章我们重点聊四个东西：姿态解算（互补滤波、Mahony滤波）、姿态PID控制、前馈控制，以及抗饱和策略。嗯，内容不少，但都是实战中绕不开的硬骨头。
4.1 姿态解算：让传感器「说实话」
姿态解算的本质，就是把加速度计、陀螺仪、磁力计这些传感器的原始数据，融合成一个可靠的姿态估计。你想想看，陀螺仪短期准但会漂，加速度计长期稳但怕震动，磁力计容易受干扰——每个传感器都有自己的「脾气」。
4.1.1 互补滤波：简单粗暴但有效
互补滤波的思路很直接：低频相信加速度计，高频相信陀螺仪。为什么？因为加速度计在低频段（静态或慢速运动）很准，但高频震动会引入噪声；陀螺仪正好相反，动态响应快，但积分久了会漂。
核心公式其实就一行：
// 互补滤波核心
angle = 0.98 * (angle + gyro * dt) + 0.02 * acc_angle;这里的0.98和0.02就是权重系数。系数怎么调？我个人习惯先设成0.98/0.02，然后看悬停时的角度波动。如果波动太大，说明加速度计权重不够；如果响应太慢，说明陀螺仪权重太高。
4.1.2 Mahony滤波：更优雅的互补方案
Mahony滤波本质上还是互补滤波，但它用上了四元数和PI调节器来修正陀螺仪偏差。说白了，它比普通互补滤波多了一个「纠偏」环节。
它的工作流程是这样的：
- 用加速度计和磁力计算出参考方向
- 对比当前四元数估计的方向，算出误差
- 用PI控制器修正陀螺仪的角速度测量值
- 更新四元数
代码实现也不复杂，核心部分大概这样：
// Mahony滤波更新步骤（简化版）
void MahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) {
// 1. 计算重力方向与加速度计方向的误差
// 2. 用PI控制器修正陀螺仪偏差
// 3. 更新四元数
// 4. 归一化四元数
}4.2 姿态PID控制：飞控的「大脑」
姿态解算搞定了，接下来就是怎么控制。姿态PID控制，说白了就是告诉电机怎么转，才能让飞机到达目标姿态。
标准的姿态PID控制是串级结构：外环是角度环，内环是角速度环。为什么用串级？因为角速度环响应快，能先抑制外部扰动；角度环响应慢，负责跟踪目标。
| 控制环 | 输入 | 输出 | 典型带宽 |
|---|---|---|---|
| 角度环（外环） | 目标角度 - 实际角度 | 目标角速度 | 5-10 Hz |
| 角速度环（内环） | 目标角速度 - 实际角速度 | 电机控制量 | 30-80 Hz |
调参顺序我建议：先内环后外环。先把角速度环调稳，再调角度环。否则两个环一起调，出了问题你根本不知道是哪个环在捣乱。
我的调参经验：
- 角速度环P：从小往大加，直到飞机出现轻微高频抖动，然后退回70%
- 角速度环D：加D可以抑制超调，但D太大会引入噪声
- 角度环P：从0.5开始，看响应速度，一般1.0-3.0之间
4.3 前馈控制：让响应更快一步
纯PID控制有个问题：它总是「看到误差才行动」。比如你要快速横滚30度，PID得等到实际角度偏离目标了才开始用力。前馈控制就是解决这个问题的——提前给一个「预判」的控制量。
前馈的公式很简单：
// 前馈控制
feedforward = Kff * target_angular_velocity;这里的Kff是前馈系数，target_angular_velocity是目标角速度（由遥控器或位置控制器给出）。
加了前馈之后，控制量变成：
control_output = PID_output + feedforward;这样做的好处是：响应速度明显提升，而且PID的负担减轻了。我曾在一次竞速无人机项目里试过，加了前馈后，滚转响应延迟从80ms降到了40ms。
4.4 抗饱和策略：别让积分「积死」
PID控制里有个经典问题：积分饱和。什么意思？就是当执行器（电机）已经满油门了，但误差还在，积分项还在不断累积。等你需要反向控制时，积分项还卡在最大值，导致响应延迟。
抗饱和策略主要有三种：
- 积分限幅：给积分项设个上限，比如±油门量的30%
- 积分冻结：当执行器饱和时，停止积分
- 反计算法：根据饱和量反向修正积分
我个人最常用的是积分冻结，实现简单，效果也够用：
// 积分冻结实现
if (output >= output_max || output <= output_min) {
// 执行器饱和，冻结积分
integral = integral; // 保持当前值不变
} else {
// 正常积分
integral += error * dt;
}4.5 实战中的坑与对策
讲了这么多理论，最后分享几个我踩过的坑：
- 陀螺仪噪声：有一次飞机悬停时高频抖动，查了半天发现是陀螺仪数据没做低通滤波。加了个20Hz的截止频率，问题解决。
- 加速度计校准：新换的飞控没校准加速度计，导致姿态解算一直有偏差。嗯，这个坑我踩了两次才长记性。
- PID参数耦合：滚转和俯仰的PID参数其实会互相影响，尤其是四旋翼的X型布局。调参时记得两个轴一起看。
姿态控制这块，说白了就是传感器融合 + 控制策略 + 工程经验的三位一体。理论搞懂了，剩下的就是多试、多炸、多总结。下一章我们会聊位置控制，那又是另一片天地了。