第二章 飞控系统核心原理：传感器融合、姿态解算与控制基础
各位同学，欢迎来到飞控系统的核心章节。说实话，这一章是整个无人机自主起降的「心脏」。我见过太多新手，代码写得飞起，但一上天就炸机——问题往往就出在没搞懂传感器在「骗」你，或者PID参数调得稀烂。
今天咱们就把这些硬骨头啃下来。我会尽量用大白话讲，但该有的公式和代码，一个都不会少。
2.1 传感器融合：别信任何一个传感器
无人机上有四个主要传感器：IMU（惯性测量单元）、GPS、气压计、磁力计。它们各有各的毛病。
- IMU（加速度计+陀螺仪）：短时间准，但长时间会飘。你想想看，积分一次就多一个误差，积分两次误差更大。飞个几分钟，姿态可能偏了十几度。
- GPS：绝对位置准，但更新慢（5-10Hz），而且高楼底下、树荫里信号会丢。
- 气压计：测高度还行，但风一吹、温度一变，数值就跳。我曾在海边试飞，气压计高度忽高忽低，差点把飞机往海里按。
- 磁力计：就是电子罗盘，但电机电流、周围铁磁物都会干扰它。嗯，这里要注意：磁力计校准不好，航向角能偏30度。
所以，飞控的核心思路是：用IMU做高频姿态更新，用GPS和气压计做低频修正，用磁力计校准航向。说白了，就是「信IMU的短期精度，信GPS的长期稳定」。
核心公式（卡尔曼滤波简化版）：
预测：x̂k|k-1 = A·x̂k-1 + B·uk
更新：x̂k|k = x̂k|k-1 + Kk·(zk - H·x̂k|k-1)
其中Kk是卡尔曼增益，决定了你更相信预测还是测量。
我个人习惯用Mahony互补滤波做姿态融合，因为它计算量小，在STM32F4上跑得飞起。卡尔曼滤波虽然更准，但调协方差矩阵太费劲了——我曾经调了一个星期，最后发现还是互补滤波更稳。
2.2 姿态解算：四元数 vs 欧拉角
姿态解算就是把传感器数据变成「飞机现在头朝哪、身子歪不歪」。有两种主流表示方法：欧拉角和四元数。
| 表示方法 | 优点 | 缺点 |
|---|---|---|
| 欧拉角（俯仰/横滚/偏航） | 直观，人脑能理解 | 有万向锁问题（俯仰90度时，横滚和偏航分不清） |
| 四元数（q0,q1,q2,q3） | 无万向锁，计算效率高 | 不直观，四个数代表啥？ |
实际飞控里，内部运算全用四元数，只在给地面站显示时转成欧拉角。为什么？因为四元数插值平滑，不会出现角度跳变。我遇到过一个问题：用欧拉角做姿态控制，飞机做大机动时突然失控——查了半天，万向锁导致的。
四元数转欧拉角的公式（避坑指南：注意atan2的象限）：
// 四元数转欧拉角
roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
pitch = asin(2*(q0*q2 - q3*q1));
yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));
我的经验：四元数归一化很重要！每次更新后都要做 q = q / norm(q)，否则数值误差会累积，姿态越算越歪。
2.3 PID控制基础：别让飞机像弹簧一样乱晃
姿态解算完了，接下来就是控制。PID是飞控的「大脑」——它告诉电机：你现在歪了，赶紧给我正过来！
PID公式很简单：
输出 = Kp * 误差 + Ki * 积分误差 + Kd * 误差变化率但调参才是真正的艺术。我刚开始做飞控时，P给大了，飞机像抽风一样高频抖动；D给大了，又像陷在泥里反应迟钝。后来总结了一套口诀：
- P（比例）：让飞机快速回正。P太小，飞机软绵绵；P太大，飞机抖成筛子。
- I（积分）：消除稳态误差。比如飞机一直往一边偏，I项会慢慢补上。但I太大，会引入低频振荡。
- D（微分）：抑制超调。相当于「预判」飞机的运动趋势。D太大，噪声会被放大。
警告：千万不要在地面调PID！螺旋桨离地效应会让参数完全失效。我见过有人在地面调好了，一上天就炸——因为地面有「地面效应」撑着，空中没了。
实际飞控中，我们通常用串级PID：内环控制角速度（快），外环控制角度（慢）。这样响应更快，抗干扰更强。
2.4 PWM信号与电机控制：最后一步
PID算出了控制量（比如-1到1），但电机听不懂。我们需要把它转成PWM信号——说白了，就是给电调一个特定宽度的脉冲，告诉它「转多快」。
标准PWM频率是50Hz（周期20ms），脉宽1ms-2ms对应油门0%-100%。但现在的电调很多支持400Hz甚至更高，响应更快。
// 将控制量(-1~1)映射到PWM脉宽(1000~2000us)
pwm_value = 1500 + control * 500; // 1500us是中点
// 限幅
if(pwm_value > 2000) pwm_value = 2000;
if(pwm_value < 1000) pwm_value = 1000;
这里有个坑：电机响应有延迟。从PWM信号发出到电机真正达到目标转速，大概有几十毫秒的滞后。所以PID的D项不能太大，否则会激发电机-螺旋桨的谐振。我曾经遇到过：飞机悬停时突然剧烈抖动，频率大概10Hz——查了半天，是PID的D项和电机响应延迟共振了。
解决方法是加一个低通滤波器，把D项的噪声滤掉：
// 一阶低通滤波
filtered_d = 0.8 * filtered_d + 0.2 * raw_d;知识体系总览
下面这张图，是我自己画的知识结构。你把它存脑子里，整个飞控的脉络就清楚了：
这张图从左到右，就是飞控的数据流：传感器采集原始数据 → 融合得到可靠估计 → 解算出姿态 → PID算出控制量 → PWM驱动电机。然后实际飞行效果再反馈回传感器，形成闭环。
一句话总结：传感器融合是「眼睛」，姿态解算是「大脑」，PID是「神经」，PWM和电机是「肌肉」。缺一个，飞机就飞不稳。
好了，这一章的内容就到这里。记住：理论是基础，但真正的功夫在调参和试飞。我建议你先把传感器融合的代码跑通，然后一点点加PID——别想一口吃成胖子。
公众号:蓝海资料掘金营，微信deep3321