高度和加速度计三阶互补融合滤波--无人机定高

高度和加速度计三阶互补融合滤波

作者：凌启科技

1 高度数据获取

无人机获取高度数据的方法有多种，目前常见的传感器有三种：激光测距、超声波测距、气压计测高，另外，凌启科技最近推出了一款双目测距及光流一体模块（LQ-S01），该模块就具备测距功能，可作为第四种主流测距传感器。这四种传感器各自特点不一样，可根据需求选择。

2 三阶互补融合滤波算法概述

2.1 为什么需要融合滤波

假设我们通过高度传感器获得了无人机的高度数据，这些数据中会有一些噪声，通过低通滤波器可以将大部分高频噪声虑除掉，高度数据整体上会平滑许多，但这样的数据放在无人机的控制算法里面是无法工作的，因为控制算法还需要获取高度的变化率，也就是高度的速度，如果直接用高度数据求解速度，高度数据再平滑，速度数据都会有很多毛刺，而且这些毛刺在趋势上会与实际速度一致，这种数据是无法用在控制算法上的，另外，如果高度数据本身质量就不好，加了低通滤波也不会很平滑，这就更加增加了控制难度，因此仅仅对单传感器进行低通滤波是无法解决问题的。

融合滤波是对多个传感器数据进行一种运算，这种运算结合每个传感器各自的特点，可以得到更好的高度数据、速度数据和加速度数据。

2.2 三阶互补融合滤波的原理

试想一下，假如我们现在需要知道无人机此时的高度，在没有任何参考数据的时候，我们只能凭感觉猜测，此时加入高度传感器，参考高度传感器的数据，可以估计出较为准确的高度数据，如果再加入一个传感器，如加速度传感器，理论上经过一些运算就能得到更加准确的高度数据，这就是融合滤波的思想。具体的三阶互补融合滤波过程可参考图1。

图1

图1中高度传感器实时采集高度数据，此数据与估计高度做差，得到了估计数据的误差，将这个误差除以运算周期的平方，就得到了加速度的误差，再乘以滤波系数K2，这个误差通过两次积分就能纠正估计高度，类似于一个负反馈系统，重复这样的步骤，估计高度就会不断趋近高度传感器的数据，同样的，将误差除以运算周期，即为速度的误差，再乘以滤波系数K1，加入到负反馈系统后就可加快趋近过程，也能提高估计数据的准确性和稳定性。

因为无人机不是静止在一个高度，为了估计高度能快速跟随无人机实际高度变化，在图1左侧中加入了加速度计，这样就完成了加速度计和高度数据的融合，求得的估计高度和估计速度可以直接参与控制算法的运算。

为什么互补融合滤波得到的高度数据和速度数据更加平滑稳定？有一个很重要的原因就是融合的高度和速度是从积分中得到的，而如果直接用高度数据求速度，是从微分中得到的，微分会将误差变大。

3 三阶互补融合滤波算法仿真

根据第二章，我们进行了matlab仿真，仿真结果如图2，如需仿真代码，可私聊或评论，该代码经过简单的修改就可以移植到飞控中。

图2

参考文章：

四旋翼定高篇之惯导加速度+速度+位置三阶互补融合方案-CSDN博客

TECS——ArduPilot——高度与爬升速度的估计（3阶互补滤波）_三阶互补滤波-CSDN博客