STM32学习笔记----MPU6050的互补滤波

互补滤波

  多个传感器，以2个为例，分别是传感器A、B，两者各有长处短处，将AB的短处克服，长处互补，即A的长处补B的短处，B的长处补A的短处，此种方法为互补滤波。最常见的是陀螺仪和加速度计的互补。

常用例

加速度计，输出加速度大小，由其测量原理导致对高频信号敏感，在震动环境中干扰较大。

陀螺仪，输出角速度，对其积分得出角度，但在积分作用下导致低频干扰和漂移。

磁力计，输出机体与地磁场夹角，低频特性好，易受磁场干扰。

  在四轴中，加速度计/磁力计对振动敏感，取其值计算倾角误差大，而陀螺仪积分的角度受高频振动影响小，但随时间增长有漂移。互补滤波就是在短时间内以陀螺仪为主，定时用加速度计均值校正陀螺仪；长时间以加速度计为主，加大它的比重，这就是互补；由于陀螺仪高频信号好，低频存在干扰，故将陀螺仪信号通过一个高通滤波器，同样加速度计信号通过一个低通滤波器，这就是滤波，当然条件不足时也可以软件滤波；滤波后再互补。

互补公式（信号已滤波）

陀螺仪积分角度 += 加速度*dt；
融合角度 = A * 陀螺仪积分角度 + B * 加速度角度；
（A + B = 1）
（互补是给他们不同的权重，加权求和）

以MPU6050为例的程序

在读取数据后需要进行一下步骤:
A、读取数据；
B、去除零偏；
C、先期处理；
D、融合算法。
这里 先期处理 包含多种形式，我总结了二种在下面程序中标注出来了；融合算法 一般有DMP、互补滤波、卡尔曼滤波，下面使用的是一阶互补滤波。

//第一种方法：加速度计 角度计算+陀螺仪 单位转换
void MPU_HubuLvbo()
{
	float Q =0.02;
	float ax11_angle=0,ay11_angle=0,az11_angle=0;
	float gx11_rate=0,gy11_rate=0,gz11_rate=0;
	float acc_x=0,acc_y=0,acc_z=0;

	MPU_AD();//读取数据
	MMA7361_x11 = MMA7361_x11 - MMA7361_x11_off;//去除零偏
	ax11_angle = atan2(MMA7361_x11,MMA7361_z11)*180/3.14;
	//使用加速度计计算角度，反三角函数的数值单位是弧度，所以180/3.14是转换成°为单位

	ENC03_x11 = ENC03_x11 - ENC03_x11_off;//去除零偏
	gx11_rate = ENC03_x11/131.0;
	//陀螺仪的读取值除以灵敏度可以把单位转换为°/s，131可以从数据手册里得到，它和量程有关。
	
	angle_x = Q*(angle_x + gx11_rate*0.2) + (1-Q)*(ax11_angle);    //互补滤波
}

//第二种方法：加速度计 区间映射+陀螺仪 单位转换
void MPU_HubuLvbo()
{
	float Q =0.02;
	float ax11_angle=0,ay11_angle=0,az11_angle=0;
	float gx11_rate=0,gy11_rate=0,gz11_rate=0;
	float acc_x=0,acc_y=0,acc_z=0;
	float T=0.23,DT=0.2;//DT是采样周期
	MPU_AD();//读取数据
	MMA7361_x11 = MMA7361_x11 - MMA7361_x11_off;//去除零偏
	ax11_angle = MMA7361_x11*180/(MMA7361_x11_MAX - MMA7361_x11_MIN);//区间映射 -90°~90°

	ENC03_x11 = ENC03_x11 - ENC03_x11_off;//去除零偏
	gx11_rate = ENC03_x11/131.0;
	//陀螺仪的读取值除以灵敏度可以把单位转换为°/s，131可以从数据手册里得到，它和量程有关。

	angle_x += (gx11_rate + (ax11_angle - angle_x)/T)*DT;//互补滤波
}

上面程序，第一个是基础的互补滤波公式，第二个也是互补滤波公式，两者在读取加速度计和陀螺仪数据后均进行了先期处理而后才使用互补滤波算法。
第一种方式的波形如下：

  上图是传感器绕Y轴旋转整圈X轴与水平面的夹角的变化，红色是加速度计计算的角度值ax11_angle，棕色是融合角度数据angle_x 。可以看出，角度能够从180°~-180°连续变化，但是在180°和-180°的临界面会有数值的突然变化。这种方式陀螺仪作用较小，融合角度几乎完全逼近加速度计计算的角度值。
第二种方式的波形如下

  上图是传感器绕Y轴旋转整圈X轴与水平面的夹角的变化，红色是加速度计计算的角度值ax11_angle，棕色是融合角度数据angle_x 。可以看出，角度能够从90°~-90°连续变化。这种方式陀螺仪作用较大，抗干扰能力强，但是数据逼近加速度计计算的角度值的程度减小。

第二种方法的说明

  第二种滤波方式来自比赛时的学习文档 “4、直立平衡车模参考设计方案（2011）” 文档不上传了，具体算法框图如下：

VOLTAGE_GRAVITY：加速度计采集值；
GRAVITY_OFFSET：加速度计零偏；
GRAVITY_ANGLE_RATIO：加速度计归一化系数，
  GRAVITY_ANGLE_RATIO=180/(MMA7361_x11_MAX - MMA7361_x11_MIN)
VOLTAGE_GYRO：陀螺仪采集值；
GYROSCOPE_OFFSET：陀螺仪零偏；
GYROSCOPE_ANGLE_RATIO：陀螺仪系数，
  GYROSCOPE_ANGLE_RATIO=131；
GRAVITY_ADJUST_TIME_CONSTANT：加速度计时间常数，其值=1/Tg，Tg越大角度输出跟踪越慢，但可以有效抑制加速度计上的噪声，Tg过大会放大陀螺仪输出误差，一般取 Tg=1~4.
GYROSCOPE_ANGLE_SIGMA_FREQUENCY:积分时间常数，是原始数据采集的时间间隔的倒数。

滤波的最终目的和效果

1、以陀螺仪的特性消除加速度计在测量角度时的噪声和误差，实际上就是用加速度计测量角度，然而这个角度容易被干扰，所以用陀螺仪的角度对其进行优化。

2、最终可以得到较为稳定的，变化较为平滑的加速度计角度，该角度以加速度计的测量角度值为主；
3、通过滤波后加速度计角度对滤波前加速度计角度的追踪对比，可看出滤波后的角度对震动反应变慢，对突然的变化反应具有一定延迟，整体变化平滑；
4、单纯的互补滤波得到的3轴角度，是XYZ轴相对于重力加速度的（即垂直向下的方向的）夹角，与常说的姿态角俯仰角Pitch、偏航角Yaw、翻滚角Roll不一样。