基于姿态传感器MPU6050的卡尔曼滤波应用

朱豪坤

（中北大学机电工程学院，山西太原 030051）

基于姿态传感器MPU6050的卡尔曼滤波应用

朱豪坤

（中北大学机电工程学院，山西太原 030051）

现在风靡一时的六轴姿态传感器MPU6050，单独使用三轴加速度计或者三轴陀螺仪都很难得到十分准确的姿态数据，且传感器容易受到电场、磁场等因素的干扰，使数据不稳定。本文研究利用MPU6050获取相应位置的角加速度和加速度，通过几何运算分别将陀螺仪和加速度计所获得的数据转换成相应的两个角度，介绍了卡尔曼滤波（Kalman filtering）算法对多惯性数据进行融合的实际运用［1］，在单片机平台上利用卡尔曼滤波算法整合由陀螺仪和加速度计获得的数据，过滤掉相关外在噪声，获得姿态数据的最优估计。

MPU6050模块 卡尔曼滤波 三轴加速度计 三轴陀螺仪 STM32F103ZET6单片机

随着微机电系统（MEMS）的发展，低成本的惯性传感器得到了广泛的使用，MPU6050就是典型的代表。MPU6050集成了MEMS三轴加速度计和MEMS三轴陀螺仪，是全球首例整合性6轴运动处理组件，被广泛应用于智能型手机等电子产品。在使用MPU6050时，单独通过陀螺仪和加速度计的数据都难以获得准确的角度数据，除了传感器自身的数据误差、计算误差之外，外界电场、磁场等对传感器的干扰也很大。卡尔曼滤波很适用于在计算机上运行，我们应用STM32单片机运行卡尔曼滤波算法，将陀螺仪和加速度计的相关数据输入卡尔曼滤波系统，通过系统中的线性系统状态方程，输出观测数据，对系统状态进行最优估计，从输入的存在测量噪声的数据中，估计动态系统的状态，获得最优估计的姿态数据［2］，并对现场采集的数据进行实时的更新和处理。

1 系统整体设计

图1是本文研究的系统方框图，STM32是一款32位的单片机，处理速度快、功能强大，具有丰富的外围接口，STM32单片机通过一个IIC接口与MPU6050相连，通过IIC读取MPU6050陀螺仪和加速度计的数据，同时运行卡尔曼滤波系统，单片机每隔时间t中断一次，连续从MPU6050中读出的数据输入卡尔曼滤波系统，并且将卡尔曼滤波输出后的数据通过串口传给电脑上位机，在PC端整理好上位机获得的数据，并将其导入MATLAB中，在MATLAB中绘制出坐标图，便于我们更直观的分析数据，这是我们的硬件部分的大体结构框架。

软件部分主要包括以下结构：首先，由宏定义，用于驱动MPU6050的底层驱动程序，实现卡尔曼滤波的算法程序等构成；其次，有用于STM32读取MPU6050数据的IIC通信程序，定时将MPU6050数据换算成角度的几何换算，并将角度导入卡尔曼滤波系统进行卡尔曼滤波的定时器中断程序；另外，还有将滤波后的数据传输给电脑上位机端的串口通信程序，和将整理好的数据转换成坐标图的MATLAB处理程序。

2 角度的获取

2.1 陀螺仪获取角度

如图2，通过陀螺仪获取的角度αa，通过陀螺仪可以读出三个轴的角速度Gx、Gy、Gz，角速度乘以时间就是这段时间内转过的角度，把每次计算的角度进行累加，从而得到当前所在位置的角度。如下图：以x轴为例，用x轴的角速度Gx乘以单片机读取陀螺仪数据的时间间隔t就得到了这段时间内转过的角度α1，即α1=Gx*t，把每次x轴转过的角度α1、α2…αn进行加减运算，就可以得到当前时刻的角度，即αa=α1+α2+…αn。这种计算方法存在一定的误差，而且总的角度是通过累加的方法得到，相当于一个积分过程，所以累加次数越多，误差就会越大，最终可能导致测出来的角度与实际角度相差很大。

2.2 加速度计获取角度

如图3，通过加速度计获取的的角度αb，重力加速度可以分解成x，y，z三个方向的加速度gx，gy，gz，加速度计可以测量某一时刻x，y，z三个方向的加速度值Ax，Ay，Az。这里我们暂时不考虑传感器运动的影响，假设重力加速度在三个轴方向的加速度分量等于三个轴的加速度，即Ax=gx，Ay=gy，Az=gz，利用重力在各个方向的分量的几何关系来计算出传感器大致的倾角。如图3：以x轴为例，αb为x轴与水平面的夹角，∠d为z轴与竖直方向的夹角，g为重力角速度，αb=∠d，∠d的正弦tand=Ax/Az，所以αb=∠d=arctan（Ax/ Az）。只要通过加速度计获取相应轴实时的加速度，就可获得相应轴的角度。因为物体时刻都会受到一个向下的重力加速度，而传感器在动态时，会受到其他方向的作用力，此时加速度计测出的结果是重力加速度与传感器运动加速度合成得到一个总的加速度在三个方向上的分量。所以在传感器运动的时候，通过加速度计这样测出的结果并不是非常精确。

3 卡尔曼滤波算法的应用

通过陀螺仪和加速度计获得的角度都存在一定的误差，陀螺仪获得角度是一个积分的过程，累加次数越多，误差就越大。通过加速度计获得角度，在姿态传感器非静止的时候，加速度计获取的数据是重力加速度与运动加速度的和值，获取的数据本身具有一定误差。在此我们使用卡尔曼滤波算法将陀螺仪和加速度计获取的数据融合，不断根据实时数据把协方差矩阵递归，从而估算出最优的角度值，它只保留了上一时刻的协方差矩阵，可以随不同的时刻的数据而改变卡尔曼增益系数的值，从而可以根据不同时刻的数据进行最优估计［3］。通过调节卡尔曼滤波中的系数Q，R可以调节系统对陀螺仪数据和对加速度计数据的信任度，调参数也是卡尔曼滤波中很重要的一个环节。以下是详细的实施步骤［4］：

3.1 预测当前角度值

首先预测当前角度值，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为Angle=Gyro*t，但是陀螺仪有个静态漂移Q_gyro，这个值没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值Angle。

其中等号左边Angle为此时预测的角度，等号右边Angle为上一时刻预测的角度，Gyro 为陀螺仪测的角速度的值，t是两次滤波之间的时间间隔。Q_gyro也是一个变化的量，但是就预测来说认为现在的漂移跟上一时刻是相同的即Q_gyro=Q_gyro。

3.2 预测协方差矩阵

第二步是预测方差阵的预测值，Q_Gyro是漂移的噪声，Q_Angle是角度值的噪声为系统噪声协方差矩阵，方差值D（Q_Angle）、D（Q_Gyro）为指定常数，所以系统噪声协方差矩阵是已知的，通过设置系统噪声协方差矩阵Q可以设置系统对陀螺仪的信任度。

其中等号左边的abcd为此次预测的协方差矩阵元素，等号右边abcd为上一次的预测的协方差矩阵元素。

3.3 计算卡尔曼增益系数

3.4 计算当前最优化估算值

通过卡尔曼增益对数据进行修正，其中Acc_Angle是加速度计计算得来的角度，为测量值，右边为当前的预测值，通过结合预测值和测量值，我们可以得到显著状态的最优化估计值，即左边的

3.5 更新协方差矩阵

为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们要更新协方差矩阵。

4 实验测试与结果

4.1 加速度干扰测试

在加速度干扰测试中，将MPU6050传感器水平放置，对传感器进行Y轴向的震荡作用，即对Y轴方向施加前后变化的加速度，单片机定时器采样周期为t=1ms，采样1000次，采集此时MPU6050数据［5］，将数据导入MATLAB得到图4。其中蓝色曲线为只用加速度计的情况下测得的传感器的角度变化，红色曲线为卡尔曼滤波融合加速度计数据和陀螺仪数据所得角度变化。可以看出，蓝色曲线比较粗糙，红色曲线相对光滑，且蓝色曲线振幅高红色曲线很多，说明使用卡尔曼滤波后，平移运动时动态噪声对角度的干扰减少了很多，数据更加准确。

4.2 角速度干扰测试

在角度的干扰测试中，以MPU6050的X轴为旋转中心，使传感器绕X轴进行左右往复翻转运动，单片机定时器采样周期为t=1ms，采样1000次，采集此时MPU6050的数据，导入MATLAB中，得出图5。其中绿色曲线为用陀螺仪测得的传感器的角度变化，红色曲线为卡尔曼滤波融合加速度计数据和陀螺仪数据所得角度变化。可以看出，绿色曲线的震荡幅度比红色曲线大很多，数据较为不稳定，说明使用卡尔曼滤波后，很好的减少了翻转运动时动态噪声对角度的干扰，使姿态数据更接近实际数据。

5 结语

通过上述分析和实验的测试，我们可以发现通过使用卡尔曼滤波算法对加速度传感器数据和陀螺仪数据进行融合，实时地改变测量噪声协方差的值，很好的避免了动态噪声对加速度传感器的影响，提高了MPU6050传感器姿态数据测量的精度，为MPU6050姿态数据的修正提供了很好的方法，也为MPU6050在实际应用方面解决了很多问题，提高了MPU6050在使用中的性能，提供更精确的数据，具有一定的应用价值。

［1］邱云平，伍宝玉.MPU-6050模块角度算法处理及在嵌入式中的应用［J］.江西科技学院学报，2014（2）∶26-29.

［2］王帅，魏国.卡尔曼滤波在四旋翼飞行器姿态测量中的应用［J］.兵工自动化，2011（1）∶73-74+80.

［3］张道德，王强.Kalman滤波算法在自平衡机器人中的应用.湖北工业大学学报，2012（5）∶1-5.

［4］傅忠云，朱海霞，孙金秋，刘文波.基于惯性传感器MPU6050的滤波算法研究［J］.压电与声光，2015（5）∶821-825+829.

［5］赖义汉，王凯.基于MPU6050的双轮平衡车控制系统设计［J］.河南工程学院学报，2014（1）∶53-57.