关于MPU6050姿态解算的一阶互补滤波方法（从原理到代码实现）

关于MPU6050一阶互补滤波方法（从原理到代码实现）

1.写在前面
最近知道自己不用考研后便花了很多时间来准备机械创新设计大赛，在设计的多功能防台风窗中需要到MPU6050对窗户的姿态进行检测，用来反馈到步进电机控制电机转动（别问为什么不用编码器反馈来控制电机转动，问就是穷，当然也存在本项目用的电源电压低，丢步严重的问题。）在MPU6050传感器中一般读到的原始数据为三轴加速度和三轴角速度，那么如何利用这六个数据进行姿态解算便成为了一个首要的问题。可以利用官方的dmp来解算姿态，但是代码移植是个问题，我用的是恩智浦的RT1064，手头只有MSP430和STM32的dmp库，在采用卡尔曼滤波算法发现收敛太慢的情况下，我决定采用经典的一阶互补滤波算法和简单的自适应一阶互补滤波算法来试试看，也从数学原理到代码彻底的把一阶互补滤波原理捋一遍。
2.什么是一阶互补滤波以及为什么要用一阶互补滤波
假设我们需要测定一个物理量，这个物理量可以通过两个传感器测得，而且其中一个传感器的测量噪声主要集中在高频段，另一个传感器的噪声主要集中在低频段，那么我们可以构造出两个滤波器，其中一个是低通滤波器，另一个是高通滤波器。然后让这两个传感器测得数据分别通过这两个滤波器再进行加权求和：
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是多少，我们就可以定量计算出这个系数，当然不同的系统肯定会标定出不同的系数。
在单片机算力不足或者主频不高的情况下，一阶互补滤波可能是比较好的一个选择，当然，如果算力足够，还是推荐使用卡尔曼滤波，后面有空再写一写基于卡尔曼滤波的姿态解算以及自适应的一阶互补滤波，这篇帖子当做学习心得，后面可以留着复习，以此为记。
以上。