打印
[开发工具]

MPU6050一阶互补滤波方法

[复制链接]
1538|13
手机看帖
扫描二维码
随时随地手机跟帖
跳转到指定楼层
楼主
vivilyly|  楼主 | 2025-5-26 13:01 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
什么是一阶互补滤波以及为什么要用一阶互补滤波
假设我们需要测定一个物理量,这个物理量可以通过两个传感器测得,而且其中一个传感器的测量噪声主要集中在高频段,另一个传感器的噪声主要集中在低频段,那么我们可以构造出两个滤波器,其中一个是低通滤波器,另一个是高通滤波器。然后让这两个传感器测得数据分别通过这两个滤波器再进行加权求和:
Y=αx1+(1-α)x2 (1)
式(1)中Y即为输出的预测值,α为加权系数,如果这个时候α可变,并且时时刻刻保持估算出的Y的方差最小那么α还有一个名字——卡尔曼增益,此时转化为最佳线性滤波——卡尔曼滤波,只是在卡尔曼滤波算法中x1或者x2有一个是实测的,另一个是通过其他传感器再结合数学模型推测的结果,这和我们MPU6050姿态解算很像,比方说我们要解算俯仰角,那么我们需要知到x轴方向的加速度,绕y轴的角速度,这样我们可以通过:
pitch1=asin(ax/g); (2)
pitch2 =pitch(0)+ ∫Wydt; (3)
来算出两个俯仰角。其中ax为x轴方向加速度,g为重力加速度,ax除以重力加速度再求反正弦函数即为从加速度计中推出的俯仰角1(pitch1)。
pitch(0)为初始俯仰角(由第一次测得的pitch1得出),Wy为y轴角速度,角速度对时间积分再加上初始俯仰角即为从陀螺仪推出的俯仰角2(pitch2)。
我们来看看这两个角度是否满足一个噪声集中在高频段,一个噪声集中在低频段:

图中黄线代表加速度测得角度,绿线代表真实的角度,可以看出黄线基本跟随绿线,说明没有漂移误差,但是存在较为严重的毛刺信号,说明存在高频噪声;
图中蓝线代表陀螺仪的积分,可以看出它几乎没有尖锐的毛刺信号,但是随着积分时间的累加,陀螺仪解算出的角度存在极大的漂移,这个漂移可以看做是一个低频噪声,比如漂移了一个常数值,那么就存在一个很大的频率为0的噪声分量,我们利用matlab的快速傅里叶变换来看看是不是这个情况:
        
          
clear;
clc;
x=load('mpu6050.txt');
x1=[];
for n=0:1131
    j=6*n+1;
    x1(n+1)=x(j);
end
k=0:0.01:11.31;
figure
plot(k,x1);
fs=100;
T=1/fs;
L=1132;
t=(0:L-1)*T;
Y=fft(x1,L);
P2=abs(Y/L);
P1=P2(1:L/2+1);
f=fs*(0:(L/2))/L;
plot(f,P1);


没标横纵坐标单位和图例嗯,坏习惯要改,这里横坐标为频率,单位为Hz,纵坐标为对应频率所占的幅值大小。
从图中可以看出在第低频段存在严重的噪声,在获得测试数据时我是以5Hz为频率对传感器进行摆动,当然不可能精确到5Hz,所以我们要获得的数据频率应集中在3-5Hz的频段中,但是由于积分漂移,0hz(常量)-2Hz的干扰1信号占了绝大部分,所以我们应该使用高通滤波器把这些低频信号给滤除。
同理可得,我们也可以用快速傅里叶变换来分析加速度计解算出的角度信号的幅频特性,这里就不过多展开了,后面有机会讲卡尔曼滤波算法时再写。
得到这两个俯仰角信号的特性后,我们知道它满足互补滤波算法对信号的要求,那么如何设计互补滤波算法呢?

具体的实现流程为:

3.一阶互补滤波的数学推导
一阶低通惯性滤波:LPF=1/(τ*s+1) (4)
这是一个低通滤波器,它的截止频率为1/τ,假如我们要让该滤波器的截止频率为3,那么τ为1/3,我们在matlab中用bode图看一看是不是这样的:## 标题
num=[1];
den=[1/3 1];
sys=tf(num,den);
bode(sys);

得到:

看完了低通滤波,我们还差一个高通滤波,简单:HPF=1-LPF就是高通滤波器:
一阶高通滤波:HPF=1-1/(τ*s+1) (5)
同样设τ为1/3,来看看bode图:
num=[1];
den=[1/3 1];
sys=1-tf(num,den);
bode(sys);


现在再次贴出具体设计方案:

我们看上图左边部分,首先忽略掉陀螺仪,B(s)为输出,加速度计输出角度A(s)为输入,得到闭环传递函数为:
B(s)/A(s)=1/(TAs+1); (6)
同理,以陀螺仪输出角速度G(s)为输入,B(s)为输出,得到闭环传递函数为:
B(s)/G(s)=TA/(TAs+1); (7)
这里有的同志们就要问了,这不是一阶高通滤波的表达式啊,没关系,因为我们一阶高通滤波器输入的是角度,而这里陀螺仪输出的G(s)为角速度啊,角速度积分就是角度,将(7)式变形为:
B(s)=TAs/(TAs+1)*G(s)/s;
这样来看就是一个一阶高通滤波器了!
到此为止1我们已经知道如何在数学上得到一个一阶互补滤波器了,美汁汁!下面我们来看看代码上如何实现,上来先贴出代码:
/*一阶补偿*/
float Angle_offset(float acx1,float gyroy2)
{
        float hk;
        hk=-100.0f/(8192-100)*(acx1-100)+100;
        //MPU_Get_Raw_data(&aacx,&aacy,&aacz,&gyrox,&gyroy,&gyroz);
        Angle_ax = asin((acx1-hk) /8192);   //去除零点偏移,计算得到角度(弧度)
        Angle_ax = Angle_ax*180/3.14f;     //弧度转换为度
        gyroy=-1*(gyroy2+3)/16.4f;
        //if(abs(gyroy)<1.2)
        Angle1=0.95f*Angle_ax+0.05f*(Angle1 + gyroy*0.01);
        /*else
        Angle1=0.9f*Angle_ax+0.1f*(Angle1 + gyroy*0.01);        */
        if(Angle1>=90)
                Angle1=90;
        else if(Angle1<=-90)
                Angle1=-90;
        pitIsrCnt1=true;
        return Angle1;
}

这个函数的输入参数1为x轴加速度,参数2为y轴角速度,其余代码不多解释,我们直奔主题,看最关键的一句:Angle1=0.95fAngle_ax+0.05f(Angle1 + gyroy*0.01);为什么要这样写,怎么从数学方面来进行推导?0.95这个系数怎么得来?(我用的是试凑法,0.95为系数试出来刚好收敛速度快且静态误差较小。)下面我们来简单谈一谈。
数学到算法的推导:
拉式域方程为:
变换到时域并且离散化为:

将其变形可得:

好了,到这一步我们可以看出来算法与该公式的关系了,前面提到的0.95这个系数其实是T/(T+TA),那么如果我们知道一阶互补滤波每一次迭代时间T,然后又能确定我们需要设定的截止频率1/TA是多少,我们就可以定量计算出这个系数,当然不同的系统肯定会标定出不同的系数。

使用特权

评论回复
沙发
daichaodai| | 2025-5-26 19:19 | 只看该作者
MPU6050在很多飞控上都有应用

使用特权

评论回复
板凳
tpgf| | 2025-5-27 09:04 | 只看该作者
其核心思想是利用加速度计的长期稳定性和陀螺仪的短期高精度特性,通过加权平均来抑制各自噪声

使用特权

评论回复
地板
probedog| | 2025-5-27 11:23 | 只看该作者
MPU6050一阶互补滤波方法是一种结合加速度计和陀螺仪数据以获取可靠姿态角的算法。

使用特权

评论回复
5
liu96jp| | 2025-6-17 12:41 | 只看该作者
MPU6050是一款集成了三轴加速度计和三轴陀螺仪的惯性测量单元(IMU),常用于姿态解算和运动检测。然而,加速度计和陀螺仪的数据分别存在高频噪声和低频漂移问题。一阶互补滤波方法通过结合两者的优势,可实现低延迟、低噪声的姿态解算。

使用特权

评论回复
6
ex7s4| | 2025-6-17 13:50 | 只看该作者
陀螺仪数据:短期稳定性好,但存在低频漂移(积分误差)。加速度计数据:长期稳定性好,但存在高频噪声(动态干扰)。

使用特权

评论回复
7
lix1yr| | 2025-6-17 15:08 | 只看该作者
通过一阶低通滤波器和高通滤波器分别处理陀螺仪和加速度计数据,然后加权融合

使用特权

评论回复
8
g0d5xs| | 2025-6-17 18:01 | 只看该作者
航向角无法通过加速度计准确计算,需结合陀螺仪或磁力计数据。

使用特权

评论回复
9
d1ng2x| | 2025-6-17 20:16 | 只看该作者
α 越大,陀螺仪权重越高,动态响应快但易漂移。α 越小,加速度计权重越高,静态稳定性好但动态响应慢。通常 α 取 0.9~0.98,需根据实际场景调整。

使用特权

评论回复
10
cen9ce| | 2025-6-17 21:09 | 只看该作者
确保加速度计数据(g)和陀螺仪数据(°/s)的单位正确。积分时需将角速度转换为弧度/秒(rad/s),但代码中直接使用 °/s 也可,因为滤波系数是比例关系。

使用特权

评论回复
11
zhizia4f| | 2025-6-18 09:11 | 只看该作者
动态场景(如快速运动)时,增大 α(如 0.98)。静态场景(如静止)时,减小 α(如 0.9)。

使用特权

评论回复
12
q1ngt12| | 2025-6-18 10:20 | 只看该作者
陀螺仪需进行静态校准(去偏置)。加速度计需进行水平校准(确保静止时姿态角为 0)。

使用特权

评论回复
13
suw12q| | 2025-6-18 12:14 | 只看该作者
航向角无法通过加速度计准确计算,需结合磁力计数据或陀螺仪积分(但会漂移)。

使用特权

评论回复
14
t1ngus4| | 2025-6-18 16:27 | 只看该作者
从MPU6050读取加速度计和陀螺仪的原始数据。对加速度计数据进行归一化处理,转换为单位向量。对陀螺仪数据进行去偏置处理(静态校准)。

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

101

主题

1912

帖子

1

粉丝