飞控高度融合实战：互补滤波与卡尔曼的工程抉择

简介《采用惯导、GPS与气压计数据的飞控系统高度滤波算法》是一份学术论文PDF面向无人机飞控开发者、惯性导航研究人员与嵌入式系统爱好者。内容聚焦高度测量精度系统讲解基于惯导、GPS与气压计的高度观测模型构建并利用互补滤波对导航坐标系下的高度、速度、位置进行联合优化。文中包含姿态解算的四元数微分方程、一阶龙格-库塔解法以及GPS与气压计融合的权重模型同时针对单一传感器信号弱或数据差的情形给出容错处理策略。通过静态与模拟动态实验验证算法相比传统方法具有更高准确度与稳定性可为低成本飞控平台的高度解算提供直接工程参考。资料为单个PDF文档共2.13MB便于阅读与标注目前已有336人学习下载。作者来自北京航空航天大学兼具学术深度与实用价值适合课程设计、算法研究或实际开发时对照查阅。1. 为什么高度是飞控系统里最难融合的变量做飞控的人都知道这么一句话姿态好解高度难缠。姿态有陀螺和加速度计互补短时间内就能收敛得很好但高度这个量牵涉到三个完全不同物理原理的传感器各有各的脾气谁都不能单独指望。先说惯导。惯导里负责高度通道的主要是加速度计加上数学积分说白了就是对垂直加速度做两次积分得到位移。问题在于加速度计零偏是随温度和时间漂移的哪怕零点零几g的偏置二次积分之后也会变成每秒几厘米、每十秒几米的抛物线式误差。我见过不少自研飞控在新板子上跑高度融合静止不动却看到估计高度在慢悠悠往下“坠落”后来一查就是加速度计零偏没标定干净。另外惯导的垂直通道在数学上本来就是不稳定的积分只会让误差持续累积所以必须靠外部观测量去持续“拉扯”修正。再看GPS。GPS高度用的是WGS-84椭球模型下的高度接收机输出的海拔数据本质是卫星测距解算出来的水平精度好的时候垂直精度通常要打折扣PDOP里垂直分量本来就比水平大。再加上多路径效应、卫星几何分布变化、以及城市峡谷里的反射GPS高度经常能跳出一两米甚至更大的毛刺。而且GPS更新率一般也就是5Hz到10Hz光靠它自己的输出去控制高度飞控根本跟不上短时间的姿态扰动。气压计则拥有三者中最好的短期稳定性。气压高度通过标准大气模型换算短时间内读数平滑能反映厘米级到分米级的变化趋势。但它有两个让人头痛的毛病一是出厂时每个芯片的基准气压都不一样且随温度漂移必须做动态校准二是无人机旋翼下洗气流会直接冲击气压计桨叶一转表压猛增高度读数瞬间往下跳几米。如果把裸露的气压计直接装在飞控板上不搞海绵遮罩和风道隔离这种“桨效高度负偏移”问题会伴随你整个调试周期。结论很清楚这三种传感器的误差特性是互补的——惯导短时响应快但长时发散气压计稳但受气流干扰严重GPS绝对误差有界但噪声大且更新慢。高度滤波算法的本质就是设计一个机制让每个传感器在自己擅长的频段发挥主导作用同时把各自不擅长的部分压制下去。2. 算法选型为什么我不推荐在飞控里硬扛纯卡尔曼2.1 互补滤波与卡尔曼滤波的边界做高度融合大家第一反应总是“上卡尔曼”。确实标准的线性卡尔曼滤波器理论上是最优的但工程实现和数学形式是两回事。这里必须先分清楚你是在哪种平台上干活是PX4/ArduPilot这类成熟飞控还是自己写的精简飞控框架又或者是做算法仿真验证的Matlab脚本。在STM32这类单片机平台上我见过很多同学照着博客抄了一个七阶卡尔曼位置、速度、加速度偏差三个状态加两个量测输入气压高度和GPS高度跑起来倒是没问题但问题出在调参和鲁棒性上。卡尔曼的观测量和过程噪声矩阵如果设得不好融合出来的高度反而比单一气压计还飘。而互补滤波结构上就是两个一阶环节一个低通作用于GPS/气压计一个高通作用于惯导积分参数少、物理意义清晰、极端情况下容易降级。我不否认卡尔曼在学术上的优越性但作为多年实际调飞控的人我推荐大家在大多数飞控场景下先做“惯导气压计互补”作为内环高度源再把GPS的输出通过另一级校正网络逐步修正绝对基准。2.2 我最终选用的融合架构我这里给出的方案是我在自研飞控上验证过的一套“两级级联高度解算”架构适合在实时嵌入式系统里跑计算量很低而且每个中间量都有明确的物理意义方便调试时观察。第一级是纵向通道的惯导气压互补部分思路是这样的惯导垂直速度 v_z 由加速度计积分得到坐标变换后这份信号经过了高通滤波响应快但不长期可信气压计换算高度经过低通滤波长期稳定但短时有延迟和冲击偏差两者相加得到融合高度估计其本质是一个一阶互补结构交叉频率大约在0.05Hz到0.2Hz之间。写成伪代码大致是这样的float baro_alt; // 气压计换算高度 float accel_z; // Z轴加速度已做重力补偿单位 m/s^2 float vel_z; // 惯导垂直速度 float alt_fused; // 融合高度输出 const float tau 0.3f; // 互补时间常数,单位s const float dt 0.004f; // 控制周期,单位s float alpha tau / (tau dt); // 每次循环执行 vel_z accel_z * dt; alt_fused alpha * (alt_fused vel_z * dt) (1.0f - alpha) * baro_alt;这段代码的物理含义值得细说。α越接近1说明越信任惯导的短时积分但一旦加速度计零偏没校准好高度会缓慢漂移α越小说明越信任气压计但气压计本身的噪声和风场扰动会直接串到高度输出里。tau取0.3秒是我在四旋翼悬停场景下调出来的折中值——既保证机动时高度响应跟得上又不会被桨叶气流带跑太多。第二级再引入GPS绝对高度修正。GPS高度单独用来做控制肯定不够平滑但它有一个不可替代的优势长时间尺度上没有漂移。所以我把GPS高度经过一个非常慢的低通滤波时间常数5到10秒得到基准高度。再将第一级互补滤波的融合高度与GPS基准的差值通过一个比例积分环节缓慢回灌到第一级的高度状态中。这样做的好处是机动和抗风时惯导提供即时响应气压计提供短时稳定GPS负责锚定绝对基准三个传感器各干各的活互不打架。最终的高度估计即使在GPS被遮挡比如树荫下或气压计被桨流干扰的情况下都还有可用的输出。3. 传感器预处理融合算法跑得好不好七成看这里很多人把注意力全放在滤波参数上忽略了传感器数据的预处理。实际上我踩坑最深的恰恰是这个环节。3.1 惯导数据的坐标系转换与重力补偿加速度计输出的是机体坐标系下的比力要得到导航系下的垂直加速度必须先经过姿态矩阵旋转。这里有个很容易犯的错直接用姿态角欧拉角去投影在俯仰角超过45度时欧拉角奇异会导致垂直加速度计算明显失真。我一般建议用四元数方向余弦矩阵来做旋转稳定性好而且纯粹是乘法运算在MCU上开销也不大。重力补偿一定要做准。静止状态下加速度计三轴模长应该等于9.8左右如果差得远先查标定标定没问题但滤波后高度还是飘再回头看姿态估计是否收敛因为姿态角哪怕偏差半度在水平加速度大的时候也会投影出一个不小的虚假垂直加速度。3.2 气压计温度补偿与动静压处理气压计型号不同处理方式略有不同。以MS5611为例芯片内部自带ADC和温度补偿直接读出的气压值已经比较干净了。但飞控板上的其他发热元件特别是电调、电源模块、主控会显著抬升传感器局部温度。我的做法是PCB布局上尽量让气压计远离发热源同时底部开孔连通外界大气在固件里维护一个缓变的气压基值每次上电预热五分钟之后再记录基准气压而不是上电就校准如果要做高精度定高建议加一个第二气压计做温度差动测量。关于下洗气流干扰我在实际试飞中发现转速变化瞬间气压高度会先向下跳然后缓慢恢复类似一个一阶惯性响应。解决办法除了物理遮罩外还可以在飞控里检测油门突变当检测到垂直加速度异常时临时增大互补滤波中惯导的权重减小tau等气流稳定后再恢复。这种“动态权值”思路虽然不复杂但对悬停精度提升立竿见影。3.3 GPS高度数据的坐标转换与异常值剔除GPS输出的高度通常是基于WGS-84椭球的椭球高而飞控真正需要的是海拔高正高。两者之间的差叫大地水准面差距不同地区差异很大国内大部分地方在-30米到30米之间。如果你只是做相对定高这个偏差不影响但如果你要做起飞降落阶段的绝对高度判断就必须查当地的大地水准面模型或者在地面站里做一个固定偏移量校准。此外GPS数据里偶尔会出现明显跳变这类异常值如果直接喂给滤波系统会把整个高度估计拉偏。我的做法是在GPS回调函数里加一个简单的卡方检测当新的GPS高度与上一有效值的差超过设定阈值比如5米且持续超过3个周期不回落才判定为有效更新否则丢弃。这样能过滤掉大部分多路径和失锁造成的离群点。4. 实测过程中我踩过的高度滤波大坑这一节全部来自我自己的试飞记录每一个问题都是真实发生过的而且都有对应的现象和根因你很可能也会遇到。4.1 “地面静止时高度在匀速下降”现象飞控上电GPS定位正常气压计读数稳定但地面站里显示的融合高度以每秒0.2米左右的速度匀速下降。排查链路先观察气压计原始高度稳定排除气压计问题。再看惯导垂直速度一直在负方向缓慢增长。最后检查机体系Z轴加速度分量发现有0.02g的负向偏置。根因是加速度计零偏未标定而这个零偏被惯导积分持续累积成虚假的垂直速度。解法重新做六面静态标定并且在融合代码里增加一个可在线修正的加速度计零偏项用一个慢速积分器持续修正垂直速度估计。从那以后我再也不相信出厂标定尤其是新板子焊接后应力引起的零偏变化。4.2 “悬停时高度周期性波浪状波动”现象四旋翼定点悬停遥控器不动融合高度却有规律地上下起伏周期约3到5秒幅度接近0.5米。排查链路先怀疑气压计遮罩问题但做了海绵罩后没有改善。然后查看GPS高度发现同样存在周期性波动与高度波动周期高度相关。进一步检查卫星数量和PDOP值发现当时卫星几何分布极差导致GPS高度本身就有长周期慢漂移。而我当时把GPS修正环的时间常数设得太短约1秒GPS的这个慢漂移被完整地注入了融合高度。解法把GPS修正环时间常数从1秒改到8秒让GPS只负责锚定长期基准不再参与短周期高度变化。修改之后波浪状波动立刻消失。这个坑给我最大的教训是每个传感器修正通道的时间常数必须与其信号有效带宽错开。GPS慢漂移就用大时间常数去修气压计动态偏差就用小时间常数去补惯导只负责秒级以下的高频响应。三者频段互不重叠融合结果才干净。4.3 “降落时高度读数突然跳变2米”现象自动降落过程中飞机接近地面时高度估计突然向上跳变约2米导致降落后的高度显示为正值而非零。排查链路首先检查是否触发地面效应。地面效应会让气压分布发生变化但通常在距离地面0.5米以内才明显2米的跳变幅度不太像。继续检查超声波/激光定高传感器发现这个项目里并没有装载近距离传感器。最后看日志发现跳变发生的时刻正好是油门完全关闭的瞬间电机停转下洗气流消失气压计读数迅速回升到真实环境气压。由于我的气压基准是起飞前记录的固定值气流消失后气压恢复到正常值气压高度计算自然就跳了。解法在降落阶段引入“近地逻辑”——检测到油门低于阈值且垂直速度持续为负时将融合高度缓慢锚定到当前近地传感器如果有或强制把高度估计向零收紧。更重要的是要把气压计基准做成滑动窗口内的动态校准值而不是固定的起飞前值这样即使地面效应导致局部气压异常也能在几步内自动恢复。4.4 “高速前飞时高度剧烈发散”现象切换到固定翼模式或者穿越机做大机动时融合高度在数秒内剧烈发散数值完全不可信。排查链路这种情况几乎可以断定是惯导垂直通道被水平机动加速度污染。四旋翼在高速前飞时需要大角度倾斜重力加速度在机体系Z轴的分量减小如果姿态估计滞后或者加速度计标度因数不准投影到导航系Z轴的加速度就会出现巨大的虚假正/负值。气压计此时也存在动压效应——气流流过气压计通气孔时产生动压使静压测量偏低高度读数偏高。解法在融合算法中引入“机动检测器”当水平加速度绝对值超出阈值时自动降低惯导通道的权重同时将气压计动压修正项根据空速和攻角估算引入高度计算。对于四旋翼定高场景更简单粗暴的办法是直接限制最大垂直加速度输入让异常大的加速度值不可能进入积分器。5. 参数整定与验证从仿真到实飞的自检清单我把高度融合算法的调参和验证流程梳成了一个相对标准化的清单每次换飞机平台时直接照着跑一遍效率很高。5.1 仿真参数怎么定先做静态数据回放。把实际飞行的IMU、气压计、GPS原始数据录制下来在MATLAB或Python里离线跑你的高度融合算法反复调整以下参数参数初始建议范围调整方向互补滤波时间常数 tau0.2s ~ 0.5s悬停震动大时加大机动响应慢时减小加速度计零偏修正速率0.01 ~ 0.05温漂大时调快但过快会吃真实加速度GPS修正时间常数5s ~ 15sGPS信号差时加大信号好时可适度减小气压计基准滑动窗口30s ~ 120s气温变化大或起降频繁时缩短离线回放的好处是你不需要反复去外场试飞几分钟就能跑完一组参数效率比刻意去飞一个起降高得多。5.2 实飞验证的四个科目参数离线初调后必须做外场验证我一般固定测四个科目静止悬停测试原地解锁悬停3分钟看融合高度波动幅度应小于±0.15米手动爬升下降测试以2m/s速度升降看高度跟踪是否平滑不应有阶梯跳变前飞-悬停切换测试全速前飞两个来回后切悬停看高度是否快速收敛且无超调GPS遮蔽测试在树荫或楼旁悬停人为遮挡GPS天线确认融合高度仍能靠惯导气压计维持可接受的漂移率GPS恢复后能在数秒内平滑拉回。这四个科目全部通过我才会把算法固化成正式版本。没通过也没关系哪个科目挂了就顺着那张排查表对应去查传感器的原始信号基本都能定位到具体环节。5.3 我个人习惯的“冗余兜底”策略不管算法调得多好我始终保留一条底线逻辑如果气压计和GPS的高度偏差超过设定阈值比如10米且持续一段时间立即切换融合高度为单一气压计输出同时在地面站报“高度传感器失效”。千万不要在所有传感器都异常的情况下还让融合算法硬扛着飞行该降级就降级这是对飞机负责。6. 落地到具体飞控平台时的几点补充建议如果你用的是PX4或ArduPilot这类现成飞控其实它们内部已经实现了比较完整的多传感器高度融合你不需要也不应该去改核心算法更重要的是做好传感器安装和参数配置。比如PX4里EKF2的高度来源可以选择气压计、GPS或两者的混合其内部的容错逻辑比自研方案周全得多。如果你和我一样是自研飞控那么这套方案几乎可以直接移植。我建议把互补级、GPS修正级和参数配置做成三个独立的模块方便对不同机型单独调参。代码结构层面只需要维护一个高度状态结构体包含融合高度、垂直速度、加速度计零偏估计、气压计基准、GPS基准等字段模块之间通过函数接口传递调试时可以随时打印中间量。最后再分享一个我自己总结的小技巧每次外场试飞前做一次电机怠速和满油门的静态测试观察气压计高度读数的跳变幅度。如果跳变超过1米说明气压计的物理遮罩或者风道设计还有问题先去解决硬件再谈算法。很多人整天纠结融合参数实际是被一个粗糙的传感器安装环境带偏了方向。高度滤波算法的核心从来不是“用多高级的滤波器”而是你对每个传感器误差特性的理解深度以及你在工程上留了多少余量。把基础的数据清洗、频段分离、异常检测做扎实哪怕用一个一阶互补滤波也能达到比盲目堆卡尔曼更好的实际飞行效果。本文还有配套的精品资源点击获取