基于STM32F407四旋翼无人机---MPU6050数据处理（滑动均值滤波）（二）

﹏ヽ暗。殇╰゛Y 2023-02-14 10:12 17阅读 0赞

### 基于STM32F407四旋翼无人机---MPU6050数据处理（滑动均值滤波）（二） ###

*  MPU6050介绍
 *  加速度计和陀螺仪做静止校准
 *  滑动均值滤波
 *   *  滑动均值滤波介绍

# MPU6050介绍 #

MPU6050模块是一个比较常用的一个姿态传感器，里面有三轴加速度计和三轴陀螺仪的六轴传感器模块，并可以利用DMP来直接输出姿态解算后的数据，通过IIC通信将姿态结算后的数据输出，但是看过大多数飞控中的姿态解算发现很少用DMP来做姿态解算的，==因为用DMP做姿态解算会有漂移，而且收敛速度较慢，==此模块自带温度传感器，这个温度传感器可不是测量机体周围环境的温度，而是像电脑中电脑管家里面经常看到的CPU温度一样，它测量的就是MPU6050此时的温度，那么这个温度有什么用呢？当MPU6050自身有温度的时候是对数据是有影响的，此时会有一个叫温漂的东西，，那么我们通常用补偿法将数据通过补偿使其更接近真实值。

对于MPU6050的初始化需要根据自己的需求，设置陀螺仪和加速度计的灵敏度。下图为MPU6050：![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center]

# 加速度计和陀螺仪做静止校准 #

可以看到MPU6050有三个轴X，Y，Z轴，这三个轴解算出来的姿态也就是Y对应横滚,X轴对应俯仰，Z轴对应航向，对于MPU6050的初始化就不在粘贴代码了，但是陀螺仪和加速度在未进行校准时，数据是有偏差的或者波动范围较大的，这时候就需要对陀螺仪和加速度计进行校准，像是加速度计和陀螺仪校准的话，一般情况都是静态校准就可以，如果想要校准的更加精确还可以使用六面校准，玩过pixhowk的朋友都知道，在进行加速度计校准的时候是六面校准，那么一般的话静止校准也可以满足我们的需求，校准过程：==比如说采集50个数据，然后进行相加，然后求出均值，那么此时就得到了一个加速度计和陀螺仪的偏移量，那么每次采集到的三轴数据减去偏移量就可以得到矫正后的数据，==话不多说上代码：

//加速度计校准
    if(ACC_Offset == 1&&ACC_Offset_ok)
    {  
       /* 计数及累加 */
       acc_sum_cnt++;
       sum_temp[A_X] += g_MPUManager.Acc_initX;
       sum_temp[A_Y] += g_MPUManager.Acc_initY;
       sum_temp[A_Z] += g_MPUManager.Acc_initZ - 4096;
       sum_temp[TEM] += g_MPUManager.Tempreature;
        /* 判断计数符合条件 */
        if( acc_sum_cnt >= OFFSET_AV_NUM )
        { 
           /* 计算校验数据 */
           g_MPUManager.Acc_Offset.x = sum_temp[A_X]/OFFSET_AV_NUM;
           g_MPUManager.Acc_Offset.y = sum_temp[A_Y]/OFFSET_AV_NUM;
           g_MPUManager.Acc_Offset.z = sum_temp[A_Z]/OFFSET_AV_NUM;
           g_MPUManager.Acc_Temprea_Offset = sum_temp[TEM]/OFFSET_AV_NUM;
           /* 清零过程变量 */
           acc_sum_cnt =0;
           ACC_Offset_ok = 0;
        }
    }
    /* 陀螺仪校准 */
    if(Gyro_Offest == 1 && Gyro_Offest_ok)
    { 
       /* 计数及累加 */
       gyro_sum_cnt++;
       sum_temp[G_X] += g_MPUManager.Gyro_initX;
       sum_temp[G_Y] += g_MPUManager.Gyro_initY;
       sum_temp[G_Z] += g_MPUManager.Gyro_initZ;
       sum_temp[TEM] += g_MPUManager.Tempreature;
       /* 判断计数符合条件 */
       if( gyro_sum_cnt >= OFFSET_AV_NUM )
       { 
          /* 计算校验数据 */
          g_MPUManager.Gyro_Offset.x = (float)sum_temp[G_X]/OFFSET_AV_NUM;
          g_MPUManager.Gyro_Offset.y = (float)sum_temp[G_Y]/OFFSET_AV_NUM;
          g_MPUManager.Gyro_Offset.z = (float)sum_temp[G_Z]/OFFSET_AV_NUM;
          g_MPUManager.Gyro_Temprea_Offset = sum_temp[TEM]/OFFSET_AV_NUM;
          /* 清零过程变量 */
          gyro_sum_cnt =0;
          Gyro_Offest_ok = 0;
       }
    }

# 滑动均值滤波 #

以上就是静止校准过程，那么加速度计和陀螺仪在机体静止的情况下输出波形看起来还是比较平稳的，但是机体在运动中可以观察到数据会有一些波动，这是因为加速度计比较对于机体的姿态非常敏感，由于电机的转动产生了高频的噪声，而陀螺仪由于是积分出来的，那么会有很大的积分误差，这个误差是无法消除的，那么我们通常使用一个低通滤波去滤除高频噪声，怎么通过波形看到呢，如果我们观察到波形存在尖峰噪声，那么就可以使用低通滤波或者滑动均值滤波去将尖峰滤除，之前是做过一个低通滤波和滑动均值滤波两个滤波算法的效果比较的，但是低通滤波的效果图貌似被我误删了，只剩下了滑动均值滤波的效果图，可以看一下：  
这个是加速度计的原始数据，看着上下波动是不是很大。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 1]

进行滑动均值滤波算法之后加速度计数据非常平稳了！  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 2]

陀螺仪数据的效果截图我就不放了，效果其实和加速度计的一样。

## 滑动均值滤波介绍 ##

什么是滑动均值滤波呢？==是指建立一个数据缓冲区，按照顺序存放N个采样数据，每采样一次新数据就将最早采样的数据丢掉，然后求N个数据的算术平均值或加权平均值。每次进行一次采样，就可以计算出一个新的平均值，这样就形成了一种环型队列结构的模式，==由于陀螺仪和加速度数据容易受电机转动引起的震动造成数据中存在噪声数据，使数据上下波动较大，其适用于高频震荡的系统，则加入该滤波算法后对周期性干扰有着良好的抑制作用，使加速度数据和陀螺仪数据变得更加平滑，==但是依然有他的缺点，灵敏度较低，对于偶然出现的脉冲性感染的抑制作用较差，不使用脉冲干扰比较严重的场合，但是相比之下比MPU6050的内部低通滤波器效果好很多，==可以将先使用MPU6050的内部的低通滤波先处理一下输出数据，再使用滑动均值滤波去处理输出的数据，干货要来了，对于逻辑性较好的可以自己自行编写代码，也可以参考我的。

#define FILTER_NUM 8 // //滑动平均滤波数值个数
    /* 更新滤波滑动窗口临时数组 */
    FILT_BUF[A_X][filter_cnt] = mpu6050_tmp[A_X];//存放加速度数据
    FILT_BUF[A_Y][filter_cnt] = mpu6050_tmp[A_Y];
    FILT_BUF[A_Z][filter_cnt] = mpu6050_tmp[A_Z];
    FILT_BUF[G_X][filter_cnt] = mpu6050_tmp[G_X];//存放陀螺仪数据
    FILT_BUF[G_Y][filter_cnt] = mpu6050_tmp[G_Y];
    FILT_BUF[G_Z][filter_cnt] = mpu6050_tmp[G_Z];
    /* 更新滤波滑动窗口数组 */
    for(i=0; i<FILTER_NUM; i++)
    { 
    	FILT_TMP[A_X] +=FILT_BUF[A_X][i];
    	FILT_TMP[A_Y] += FILT_BUF[A_Y][i];
    	FILT_TMP[A_Z] += FILT_BUF[A_Z][i];
    	FILT_TMP[G_X] += FILT_BUF[G_X][i];     
    	FILT_TMP[G_Y] += FILT_BUF[G_Y][i]；
    	FILT_TMP[G_Z] += FILT_BUF[G_Z][i];
    }
    /* 得出处理后的加速度计数据 */
    MPUManager.Acc.x=(float)(FILT_TMP[ACC_X] )/(float)FILTER_NUM;
    MPUManager.Acc.y=(float)( FILT_TMP[ACC_Y])/(float)FILTER_NUM; 
    MPUManager.Acc.z=(float)(FILT_TMP[ACC_Z] )/(float)FILTER_NUM;
    /* 得出处理后的陀螺仪数据 */
    MPUManager.Gyro.x=(float)(FILT_TMP[GROY_X] )/(float)FILTER_NUM;
    MPUManager.Gyro.y=(float)(FILT_TMP[GROY_Y] )/(float)FILTER_NUM;
    MPUManager.Gyro.z=(float)(FILT_TMP[GROY_Z] )/(float)FILTER_NUM;

[MPU6050（9255）数据获取代码][MPU6050_9255]

[静止校准代码][Link 1]

[滑动均值滤波算法C语言][C]

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center]: https://img-blog.csdnimg.cn/20200606215947446.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 1]: https://img-blog.csdnimg.cn/20200607130001373.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz_size_16_color_FFFFFF_t_70_pic_center 2]: https://img-blog.csdnimg.cn/20200607130047774.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMDQ2NzYz,size_16,color_FFFFFF,t_70#pic_center
[MPU6050_9255]: https://download.csdn.net/download/qq_43046763/12565887
[Link 1]: https://download.csdn.net/download/qq_43046763/12565898
[C]: https://download.csdn.net/download/qq_43046763/12565931