说透互补滤波 (1) - 线性互补滤波器从原理到实现

为什么开源代码看不懂?

说起互补滤波,之前非常的流行,在那个算力不够的年代,这种短小精干的融合算法,风靡一时。

原理也非常简单:

我有两路信号,一个带有高频噪声,一个带有低频噪声,所以我把他们的噪声分别滤除,然后合并,就得到了没有噪声的原始信号。

简单直接,清晰明了,看到这你顿时觉得这个算法你已经会了,甚至连算法的流程都设计好了。

于是你在网上找到了著名的 Madgwick实现的 MahonyAHRS 算法,准备对比一下自己的思路。

function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
    q = obj.Quaternion; 
    if(norm(Accelerometer) == 0), return; end  
    Accelerometer = Accelerometer / norm(Accelerometer);
    v = [2*(q(2)*q(4) - q(1)*q(3))
         2*(q(1)*q(2) + q(3)*q(4))
         q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
    e = cross(Accelerometer, v);
    if(obj.Ki > 0)
        obj.eInt = obj.eInt + e * obj.SamplePeriod;   
    else
        obj.eInt = [0 0 0];
    end            
    Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
    qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
    q = q + qDot * obj.SamplePeriod;
    obj.Quaternion = q / norm(q); 
end

你看着开源的代码和自己画的流程图,反复对照了20遍,突然想到了自己上数学课的样子。

于是你又上网看了无数的互补滤波解读教程,始终不理解,为什么算法原理和代码可以没有任何关系?,那这个算法是怎么写成代码的呢?

我直接给出结论吧,造成这样的原因是因为:

网上大部分互补滤波原理介绍的是传统的线性互补滤波(Classical Complementary Filters)

而Mahony用来算解姿态的滤波是经过改进的非线性互补滤波,

非线性互补滤波里有两种形式:直接互补滤波(Direct complementary filter)无源互补滤波(Passive complementary filter)

你在网上看到的开源代码都是基于无源互补滤波器的显式误差版本-显式互补滤波器(Explicit complementary filter).

你拿着两个完全不一样的东西,那肯定对应不上呀。

先从传感器开始说起

但是我就纳闷了,为啥就没人循序渐进的写清楚呢?既然没人那就我来吧。

我有两路姿态,一个带有高频噪声,一个带有低频噪声,所以我把他们的噪声分别滤除,然后合并,就得到了没有噪声的姿态信息。

我有哪两路姿态信息呢?

但凡你在网络上查过资料,他们一定会告诉你,一路陀螺仪的获取的姿态,一路加计获取的姿态。

真的是这样吗?

那我们先从陀螺仪开始吧,陀螺仪可以获得三轴角速度,积分一下就可以得到角度,在小角度假设下,这个角度可以认为就是姿态角。

思路没有任何问题,只是我们可能缺个初始状态。

我们求速度的时候是:

$$v=v_0 + at$$

那我们求角度的时候自然是:

所以如果我们却个初始状态angle0,陀螺仪只有在发生转动的时候才能有数据,所以它是无法提供这个初始状态angle0的。

没关系,起飞前的初始状态非常好分析,因为这时候飞机是受力平衡的,也就是说在地理系下的加速度的读数是[0;0;g]

所以我们可以列出方程:

得到:

那么根据加计得到的初始的roll,pitch角度正确时,我们可以通过公式,得到yaw:

这样,我们有了angle0 和 w ,终于和通过陀螺仪来获取姿态了。

ps:详细推导思路参考:《基于加速度计与磁力计的姿态解算方法》

等等,这样的话我们不就只有一路输入了吗?怎我三个传感器用完才能得到一路姿态呀?

加计不是可以求姿态吗?这时我才想起来,网上流传的加计求姿态公式,其实只能在飞机受力平衡的时候使用,当飞机受力不平衡,飞机在地理系受到的加速度不是[0;0;g],而是个未知数,上面的方程是没有解的。

连两路输入都没有,还怎么互补滤波?

必要的假设

所以在讨论互补滤波器之前我们要做出几个假设:

1.姿态的更新是线性的即满足公式

2.飞行过程基本受力平衡,接近匀速直线运动,或者悬停,即飞机在地理系下的加计读数为[0;0;g]

3.传感器的测量数据只涉及高频或者低频噪声,即,传感器测量方程如下:

$$\begin{aligned} acc_m(t) &= acc(t) + n_H(t) \\ \omega _m(t) &= \omega (t) + n_L(t) \\ \end{aligned}$$

m下标为传感器测量值,等于真实值加上噪声,所以可以推导出,传感器测量出的角度也满足以下测量方程。

$$\begin{aligned} acc\_ang_m(t) &= acc\_ang_(t) + n_H(t)\\ \omega\_ang_m(t) &= \omega\_ang_(t) + n_L(t) \end{aligned}$$

4.假设初始欧拉角为[0;0;0]

所以传统的线性互补滤波结构如下。

从低通滤波器开始分析

低通滤波器是我们比较熟悉的,之前我们分析过一阶低通滤波器,但是低通滤波器有很多种,为了讨论不同的状态,令低通滤波器函数为LPF。

那高通滤波器是什么呢?这里我们的低通滤波和高通滤波合并后希望能够通过完整的波形,也就是波形完全不变,那这个全通的函数其实就是1,所以我们高通滤波器就可以设计为1-LPF。

以我们最熟悉的一阶低通滤波器为例,它的函数为:

那高通滤波就是:

那这个结论对不对呢?令截止频率wc=1HZ,画出两个函数的伯德图,完全符合预期,一个低通一个高通且截止评率1hz.

ps:一阶滤波器详细分析参考《一阶RC低通滤波算法》

我们再试试二阶滤波器,二阶低通滤波器函数为:

二阶高通滤波器为:

令所有系数为1,a1=a2=a3=b1=b2=b3=1,画出滤波器的伯德图

结果也是符合预期的。

所以,可以看见很过论文把这个过程进行了总结,给出了通用的低通滤波函数:

当C(s) = 常数时,对应的就是一阶滤波器。

当C(s) = a+ b/s 时,对应的就是二阶滤波器 。

(二阶的这个形式正好和kp,ki形式一致,很多地方都会强调这里的KP,Ki有什么做用,又是消除误差,又是调整截止频率,但是我认为在线性互补滤波器中,这里的参数的作用就是构成二阶滤波器,这里的参数也仅仅是二阶滤波器的参数)

所以我们也用这种通用形式来继续分析。

根据框图可得:

是不是有内味了,再根据这个公式,把框图化简成反馈形式。

这就是论文里经常看见的框图,我们来对比一下

是不是一模一样。

那为什么要把它转变成反馈形式呢?这种反馈结构简化了滤波器在代码上的实现过程,同时可以地适应更多的测量,可以很方便的扩展结构如下图:

什么时候会需要扩展呢?例如当你有GPS的时候:

算法实现

ok,我们终于把线性互补滤波器的原理说完了,那就把它实现了吧。

之前我们已经很详细的分析过低通滤波器了,这次还是以一阶滤波器的形式实现。

令C=1/τ

一阶向后差分:

离散化

转换为离散时间的差分形式

截止频率为:

用MATLAB实现算法如下:

function obj = UpdateIMU(obj, w_m,angle_m )
   obj.angle =  (obj.tao/(obj.tao + obj.Ts)) * ( obj.last_angle + obj.Ts*w_m) 
            + (obj.Ts/(obj.tao + obj.Ts)) * angle_m;
   obj.last_angle = obj.angle;
end

仿真对比

找一组数据验证一下:

可以看到直接通过加计得到的图形确实噪声很大,符合加计容易受噪声影响的特点。

零初始状态只用陀螺仪积分得到的陀螺仪,则非常平滑,无法感知到高频部分的数据,但是静止时得到的角度有偏离,不是0度,符合陀螺仪动态响应效果好的特点。

传统互补滤波器和Mahony滤波器确实都能够结合两者的特点,在15s处能得到更多的姿态信息。

ps:Mahony算法和测试数据来源于https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

思考

让我们再看一眼,滤波器的框图:

这个框图可以很直接的看到,核心思路就是如果获取一个更精确的角速度,因为本质上我们想使用的就是角速度积分的到姿态,而角速度积分得到姿态就是姿态更新的线性化过程,我想这就是为什么这叫线性互补滤波器的原因,因为一切的起因都这个线性姿态更新算法。

关于经典的线性互补滤波器的原理,算法,结果测试已经全部讲完了,下一次我们就要开始非线性互补滤波器了,算法的改进往往是把之前算法的假设修改的更贴近与实际。

ok,今天就讲这么多,我是zing,一个有趣的飞控算法工程师,我们下期见。

PS:所有的论文资料和测试代码,公众号回复【互补滤波】即可获取。