无人机飞控三大核心算法：捷联惯导、卡尔曼滤波与PID控制详解

1. 无人机飞控到底在算什么从三个算法说起很多人第一次接触无人机飞控看到捷联惯导、卡尔曼滤波、PID控制这三个词第一反应是“这仨到底啥关系”。我刚开始玩开源飞控的时候也一样翻了一堆论文越看越懵。后来自己焊板子、调参数、炸了几次机之后才慢慢理清楚这三个算法其实是一条流水线上的三道工序缺一不可。简单说捷联惯导负责“我现在在哪、朝哪个方向、转多快”它靠陀螺仪和加速度计推算姿态卡尔曼滤波负责“把传感器那些带噪声的数据揉在一起给出一个靠谱的估计”因为惯导会漂、磁力计会被干扰、GPS会跳PID控制负责“我知道偏差了该给电机发多少指令去纠正”。三者串起来就是飞控最核心的一条链路感知、估计、控制。这篇文章适合谁看如果你刚入坑无人机飞控或者做嵌入式控制想了解飞控算法怎么落地又或者你只是好奇“为什么无人机能悬停不飘”那这篇内容应该能帮你把这三个算法的骨架搭起来。我不打算堆公式而是从工程实现的角度讲清楚每个算法在飞控里到底干什么活、参数怎么调、坑在哪里。10分钟不一定能让你成为飞控专家但至少能让你看懂飞控代码里那些变量名到底在折腾什么。2. 捷联惯导飞控的“内耳”是怎么工作的2.1 为什么叫“捷联”它和平台惯导差在哪捷联惯导这个词听起来很唬人其实“捷联”就是“绑定”的意思。早期的惯导系统是平台式的陀螺仪和加速度计装在一个物理稳定平台上平台通过万向节保持水平传感器永远在一个固定的参考系里测量。这种方式精度高但体积大、成本高、结构复杂导弹和潜艇用得起消费级无人机想都别想。捷联惯导的思路完全不同把陀螺仪和加速度计直接“绑”在机体上跟着飞机一起翻滚俯仰。传感器测出来的就是机体坐标系下的角速度和加速度然后靠算法把这些数据转换到导航坐标系里去。这个转换过程就是姿态解算核心是四元数或者方向余弦矩阵。我刚开始看四元数的时候也觉得莫名其妙四个数怎么能表示旋转后来想通了一个类比你可以把四元数想象成“绕某个轴转某个角度”的紧凑表达比欧拉角少了很多麻烦。欧拉角有万向节死锁问题俯仰角到90度的时候滚转和偏航会耦合算出来的姿态会跳变。四元数没这个问题而且计算量小适合单片机跑。2.2 姿态解算的核心流程与实操要点飞控里捷联惯导的姿态解算一般跑在几百赫兹到几千赫兹的频率上。以常见的MPU6000为例陀螺仪输出的是角速度单位是度每秒或者弧度每秒加速度计输出的是比力单位是g或者米每二次方秒。解算流程大致是这样的读取陀螺仪和加速度计的原始数据做零偏校准和温度补偿。用陀螺仪积分更新四元数得到预测姿态。用加速度计测量的重力方向去修正陀螺仪的积分漂移。如果有磁力计再用磁力计修正偏航角。归一化四元数转换成欧拉角或者旋转矩阵供后续使用。这里面最关键的一步是加速度计修正陀螺仪漂移。陀螺仪有个毛病它测角速度积分之后角度会慢慢漂。哪怕你静止不动积分出来的角度也会越走越偏。加速度计虽然噪声大但它长期来看是准的因为它测的是重力方向。所以工程上常用互补滤波的思路高频信陀螺仪低频信加速度计。注意加速度计修正只在飞机接近水平、且没有大加速度的情况下才靠谱。如果飞机在做剧烈机动加速度计测到的就不只是重力还有运动加速度这时候修正反而会把姿态带偏。所以很多飞控会判断加速度计的模长是否接近1g只有接近的时候才用加速度计做修正。实操中还有一个坑传感器安装误差。加速度计和陀螺仪的轴不可能完全对齐如果安装有偏差修正的时候会引入额外的误差。我一般会在校准阶段做六面校准把每个轴的零偏和比例因子都算出来。开源飞控里通常有校准脚本按照提示把飞机翻六个面就行。2.3 捷联惯导的误差来源与应对策略捷联惯导的误差主要来自三个方面传感器噪声、积分漂移、以及坐标系转换的数值误差。传感器噪声是物理层面的陀螺仪的随机游走、加速度计的振动噪声都会影响解算精度。应对办法一是选好的传感器二是做低通滤波。但低通滤波会引入相位延迟延迟大了控制环路会震荡所以截止频率要折中。我一般把陀螺仪低通设在80到100赫兹加速度计设在30到50赫兹具体看机架振动情况。积分漂移前面说了靠加速度计和磁力计修正。但修正不能太猛太猛了会把陀螺仪的高频信息压掉姿态响应变迟钝。互补滤波的系数一般取0.01到0.05之间卡尔曼滤波则通过协方差矩阵自动调节权重。坐标系转换的数值误差主要来自四元数归一化。四元数在积分过程中模长会慢慢偏离1如果不归一化旋转矩阵会变形。所以每次更新完四元数都要重新归一化这个操作计算量很小但必须做。3. 卡尔曼滤波把一堆不准的传感器揉成一个靠谱的估计3.1 卡尔曼滤波在飞控里到底解决什么问题捷联惯导给出了姿态但那个姿态是“预测”出来的带着漂移和噪声。卡尔曼滤波的作用就是融合多个传感器的数据给出最优估计。注意“最优”这个词它是有数学定义的在系统模型和噪声统计特性已知的情况下卡尔曼滤波是最小均方误差意义下的最优估计器。飞控里最常见的卡尔曼滤波应用是姿态估计。状态量一般是姿态四元数或者欧拉角再加上陀螺仪的零偏。观测量是加速度计和磁力计的数据。预测步骤用陀螺仪积分更新步骤用加速度计和磁力计修正。整个过程是递归的每个周期算一次计算量可控。我见过很多新手把卡尔曼滤波当成“万能滤波器”觉得只要上了卡尔曼数据就准了。其实不是。卡尔曼滤波的效果高度依赖系统模型和噪声矩阵的准确性。如果过程噪声Q和观测噪声R设得不对滤波效果可能还不如互补滤波。Q设大了滤波器更相信观测结果噪声大Q设小了滤波器更相信预测结果漂移大。R也是同样的道理。3.2 卡尔曼滤波的五个核心方程与参数整定卡尔曼滤波的数学形式看起来吓人但拆开就是五个方程状态预测用上一时刻的状态和系统模型预测当前时刻的状态。协方差预测预测估计的不确定性。卡尔曼增益计算根据预测不确定性和观测不确定性算出一个权重。状态更新用观测值和卡尔曼增益修正预测状态。协方差更新更新估计的不确定性供下一轮使用。在飞控姿态估计里状态量通常是四元数和陀螺仪零偏系统模型是四元数微分方程观测模型是重力方向和磁场方向。过程噪声Q反映陀螺仪的随机游走和零偏不稳定性观测噪声R反映加速度计和磁力计的噪声水平。参数整定是个经验活。我的做法是先用传感器手册上的噪声密度算出初始Q和R然后在地上静止测试看姿态输出的波动和收敛速度。如果姿态抖动大就增大R或者减小Q如果姿态收敛慢就减小R或者增大Q。实际调的时候我一般会把Q和R都做成可调参数飞的时候用遥控器通道实时调找到合适的值再固化。提示卡尔曼滤波的观测更新有个前提就是观测噪声是高斯白噪声。但加速度计在振动环境下噪声不是高斯的磁力计在电机附近会被干扰。所以工程上通常会在观测更新之前做卡方检验把异常观测剔除掉。这个技巧在开源飞控里很常见能显著提升滤波鲁棒性。3.3 扩展卡尔曼滤波与误差状态卡尔曼滤波的取舍标准的卡尔曼滤波只适用于线性系统但姿态估计是非线性的因为四元数微分方程和旋转矩阵都是非线性的。所以实际用的是扩展卡尔曼滤波通过一阶泰勒展开把非线性系统线性化。扩展卡尔曼滤波的问题是线性化误差。当姿态偏差大的时候一阶近似不够准滤波可能发散。后来有人提出了误差状态卡尔曼滤波把状态量定义成“真实状态和名义状态的误差”误差状态是小量线性化更准确。而且误差状态卡尔曼滤波的协方差矩阵维度更低计算量更小。我在实际项目里两种都用过。扩展卡尔曼滤波实现简单代码量少适合资源受限的单片机。误差状态卡尔曼滤波精度更高适合对姿态要求高的场景比如云台增稳或者室内导航。选哪个看你的MCU性能和精度需求没有绝对的好坏。4. PID控制让无人机听话的最后一道工序4.1 从单级PID到级联PID为什么飞控要用串级控制PID控制是飞控里最直观的部分你知道目标姿态和当前姿态的偏差用比例、积分、微分三项算出电机指令。但飞控里的PID不是简单的单级PID而是级联PID通常是角度环加角速度环。为什么用级联因为单级PID响应慢、抗扰能力差。角度环的输出是期望角速度角速度环的输出是电机指令。角度环负责“我要转到哪个角度”角速度环负责“我要用多快的速度转过去”。角速度环在内响应快能及时抑制扰动角度环在外保证最终角度准确。我刚开始调PID的时候只调角度环结果飞机反应迟钝风一吹就飘。后来加了角速度环飞机立马稳了很多。角速度环的带宽比角度环高一般角度环跑在100到200赫兹角速度环跑在500到1000赫兹。这样内环能快速跟踪外环的指令外环不用管高频扰动。4.2 PID三个参数在飞控里的物理意义与整定顺序比例项P偏差乘以一个系数直接产生纠正力。P越大响应越快但太大会震荡。在角速度环里P相当于“阻尼”P太小飞机会晃P太大飞机会高频抖动。积分项I偏差的累积乘以一个系数消除稳态误差。在飞控里I主要用来抵消重力、风阻等持续扰动。I太大会导致“积分饱和”飞机在长时间偏差后突然过冲。所以工程上会做积分限幅和积分分离偏差大的时候不积分偏差小的时候才积分。微分项D偏差的变化率乘以一个系数提供阻尼。D能抑制震荡但会放大噪声。飞控里D项通常要加低通滤波否则电机噪声会通过D项放大导致电机发热甚至烧毁。整定顺序一般是先调角速度环的P再调D最后调I然后调角度环的P再调I。角速度环的P从0开始慢慢加加到飞机能快速响应但不过冲。D从0开始加加到震荡消失。I最后加加到稳态误差消除。角度环的P加到飞机能回到目标角度I加到角度无静差。注意调PID一定要在自稳模式下调不要一上来就调定高或者定点。自稳模式只涉及姿态控制变量少容易定位问题。调好自稳再调高度和位置一层一层来。4.3 实际调参中的常见问题与排查思路调PID最常遇到的问题就是震荡。震荡分两种低频震荡和高频震荡。低频震荡一般是P太小或者I太大飞机像喝醉酒一样晃来晃去。高频震荡一般是P太大或者D太小飞机像得了帕金森一样抖。排查的时候先看日志把期望姿态和实际姿态画出来看相位差和幅值。另一个常见问题是电机发热。如果D项太大电机指令会高频抖动电机电流忽大忽小发热严重。这时候要么减小D要么给D加低通滤波。我一般会把D项的低通截止频率设在50到80赫兹既能保留阻尼效果又能滤掉高频噪声。还有积分饱和的问题。飞机在地面的时候如果油门推起来但飞机没离地角度环的积分会一直累积离地瞬间飞机会猛跳。解决办法是加积分限幅或者用“起飞前清零积分”的逻辑。开源飞控里通常有“地面检测”检测到飞机在地面就清零积分。5. 三个算法怎么串起来飞控主循环里的协作关系5.1 从传感器读到电机输出的完整数据流飞控的主循环一般跑在1kHz左右每个周期做以下几件事读取IMU数据包括陀螺仪和加速度计。捷联惯导解算用陀螺仪积分得到预测姿态。卡尔曼滤波更新用加速度计和磁力计修正姿态。读取遥控器指令解析出目标姿态或目标角速度。级联PID计算角度环输出期望角速度角速度环输出电机指令。混控输出把电机指令分配到各个电机。输出PWM或DShot信号给电调。这个流程里捷联惯导和卡尔曼滤波是感知层PID是控制层。感知层的输出是姿态估计控制层的输入是姿态估计和遥控指令。感知层的延迟和噪声会直接影响控制效果所以感知层的滤波参数和控制层的PID参数要联合调。我见过有人姿态估计做得很糙然后拼命调PID怎么调都调不好。后来把卡尔曼滤波的参数重新整定姿态输出平滑了PID随便调调就稳了。这说明感知是控制的基础感知不准控制再牛也没用。5.2 时序、延迟与计算资源的分配飞控对实时性要求很高主循环的周期抖动要小。捷联惯导和卡尔曼滤波的计算量不大但PID和混控的计算量也不小加起来在STM32F4上大概占30%到50%的CPU。如果跑双精度浮点或者复杂的滤波算法可能占更多。时序上IMU读取和姿态解算要放在最高优先级PID和混控可以稍低。我一般会把IMU读取放在定时器中断里保证采样周期严格固定。姿态解算和PID放在主循环里用中断标志位触发。这样能保证姿态解算的频率稳定不会因为主循环的其他任务而抖动。延迟是另一个关键因素。从传感器采样到电机输出整个链路的延迟要尽量小。陀螺仪的低通滤波会引入延迟卡尔曼滤波的更新也会引入延迟PID的计算还有延迟。这些延迟加起来如果超过控制周期的四分之一控制环路就可能震荡。所以滤波器的截止频率不能设太低卡尔曼滤波的更新频率不能太低。提示如果你发现飞机在高频抖动但PID参数已经调得很保守了那可能是延迟太大导致的。这时候要检查滤波器的截止频率和主循环的周期把延迟降下来。6. 常见问题速查与避坑经验6.1 姿态漂移、震荡、发热的排查表现象可能原因排查方法解决措施姿态缓慢漂移陀螺仪零偏未校准静止时看陀螺仪输出是否为零重新做零偏校准检查温度补偿姿态高频抖动加速度计噪声大或D项太大看加速度计原始数据和D项输出增大加速度计低通滤波减小D或加D低通飞机低频晃动角度环P太小或I太大看期望姿态和实际姿态的相位差增大角度环P减小角度环I电机发热严重D项太大或电机指令抖动看电机指令的频谱减小D给D加低通滤波起飞瞬间猛跳积分饱和看地面时积分项是否累积加积分限幅地面检测清零积分卡尔曼滤波发散Q或R设置不当看协方差矩阵是否收敛重新整定Q和R加卡方检验6.2 我踩过的几个坑第一个坑是磁力计干扰。我一开始把磁力计装在电机附近结果偏航角一直跳。后来把磁力计移到GPS模块上远离电机和电源线偏航角才稳。磁力计对磁场很敏感电机电流、电源线、甚至螺丝刀都会干扰它。所以磁力计一定要远离干扰源而且要做硬铁和软铁校准。第二个坑是加速度计修正过度。我一开始把互补滤波的系数设得很大结果飞机一机动姿态就偏。后来把系数降到0.02机动时姿态稳多了。加速度计修正只在低频有效高频要靠陀螺仪。第三个坑是PID调参顺序搞反。我一开始先调角度环结果角度环怎么调都震荡。后来先调角速度环角速度环稳了角度环随便调调就稳了。级联PID一定要从内环往外环调内环不稳外环不可能稳。第四个坑是主循环周期抖动。我一开始把姿态解算放在主循环里主循环里还有其他任务导致姿态解算周期不固定。后来把姿态解算放在定时器中断里周期固定了姿态输出也平滑了。6.3 给新手的三个实用建议第一先仿真再实飞。MATLAB或者Python里搭个简单的飞控模型把捷联惯导、卡尔曼滤波、PID都跑一遍看看参数变化对系统的影响。仿真里炸机不花钱实飞炸机花钱还费时间。第二日志是你的朋友。飞控一定要有日志功能把传感器数据、姿态估计、PID中间变量都记下来。出了问题看日志比瞎猜快得多。我一般会用SD卡记录或者用无线数传实时看。第三一次只改一个参数。调参的时候不要同时改多个参数否则出了问题不知道是哪个参数导致的。改一个飞一次看日志再改下一个。慢就是快。7. 从算法到飞行一些个人体会飞控这三个算法捷联惯导是基础卡尔曼滤波是核心PID是手段。捷联惯导决定了你能不能感知姿态卡尔曼滤波决定了你感知得准不准PID决定了你能不能控制住。三者环环相扣任何一个环节出问题飞机都飞不好。我刚开始学的时候总想找“最优算法”觉得卡尔曼滤波比互补滤波高级误差状态卡尔曼滤波比扩展卡尔曼滤波高级。后来发现工程上没有最优只有最合适。互补滤波在资源受限的平台上跑得比卡尔曼滤波还稳因为卡尔曼滤波的参数整定太依赖模型准确性。选算法要看平台资源、精度需求和开发周期不要盲目追新。另外飞控是个系统工程算法只是一部分。传感器的选型、减震设计、电源质量、电磁兼容都会影响算法效果。我见过有人算法写得很好但机架振动太大加速度计噪声爆表卡尔曼滤波怎么调都不行。后来换了减震硅胶垫问题迎刃而解。所以不要只盯着算法硬件和结构同样重要。最后分享一个小技巧如果你在调PID的时候找不到方向可以把角速度环的P和D先调好然后用手拿着飞机快速转动感受电机的响应。如果电机响应跟手说明角速度环调好了。然后再调角度环把飞机放在地上轻轻推一下看它能不能快速回正。这个方法比看日志直观适合新手找感觉。