一种稳定平台中陀螺漂移滤波算法的设计与实现

王小英，林　志，连军政

（常熟理工学院 电气与自动化工程学院，江苏 常熟 215500）

一种稳定平台中陀螺漂移滤波算法的设计与实现

王小英，林志，连军政

（常熟理工学院 电气与自动化工程学院，江苏 常熟 215500）

针对稳定平台工作中陀螺仪漂移问题，采用以陀螺仪角速度传感器的输出量作为观测量，建立陀螺仪漂移的数学模型和状态方程，然后利用卡尔曼滤波对含有噪声的测量数据进行滤波处理的方式改善漂移问题.仿真结果表明，该方式可以有效抑制陀螺仪信号中的扰动，提高陀螺仪稳定平台的工作精度.

稳定平台；陀螺漂移；卡尔曼滤波

在各种需要稳定环境工作的系统中，为保持系统整体的稳定运行，通常采用陀螺仪稳定平台去除姿态扰动以实现稳定控制.陀螺仪作为稳定平台的核心测量元件，其本身的误差对平台性能有着很大影响，比如陀螺仪常值漂移会造成平台漂移随时间的增长而变大，使得所采集的位置数据产生较大的误差.当陀螺稳定平台应用于图像采集系统中时，这样的误差最终所带来的是图像的扰动，使得采集清晰图像的工作难以实现.自身误差所带来的不稳定因素也限制了陀螺稳定平台的应用领域，因此对陀螺仪误差进行补偿的必要性也凸现出来.研究表明，对陀螺仪误差的补偿，可以通过对其进行模型辨识和滤波处理的方式，一般使用小波分析、神经网络等方法对陀螺仪的随机误差进行建模分析.但在实际操作中，这些方法得到的模型一般具有较高的阶数，进而不适合低成本系统的实时在线估计［1］.因此在实际应用中更多的是采用时序分析法，对陀螺仪随机误差建立AR模型对误差进行补偿，并利用卡尔曼滤波进行处理，这在实际的低成本应用中更加适合.本文以稳定平台中的陀螺仪作为研究对象，分析了陀螺仪漂移误差及其算法模型，然后在算法模型的基础上对卡尔曼滤波器进行设计验证，最后结合系统在MATLAB中对陀螺仪输出数据进行仿真分析，取得了较好的效果.

1　卡尔曼滤波器设计

通过卡尔曼滤波方法对陀螺仪的随机漂移进行处理时，首先需对陀螺仪的输出信号进行预处理并使其满足平稳随机序列，然后建立相应的时序模型，并对其检验适用性.这类建模方法在文献［2-3］中已经给出了说明.另外本文综合考虑了系统实时性要求及模型适用性要求等方面因素，故采用AR（1）模型对陀螺仪随机噪声进行建模分析.

在陀螺仪随机漂移误差中主要包括系统中产生的有色噪声和系统运行时陀螺自身产生的测量白色噪声［4］.通过AR（1）模型描述陀螺仪的随机漂移误差后，将AR（1）模型改为状态空间模型，然后采用状态扩增法，得到状态空间方程.

设系统方程为

设测量方程为

上式中，Wk-1是系统噪声，为有色噪声，Vk是零均值白噪声序列的观测噪声，其中Wk-1满足方程

通过状态扩增法对卡尔曼滤波方程进行状态推导，所得状态方程为

并得到新的系统方程及观测方程：

式中：Vk是当均值为零值时的白噪声序列，满足卡尔曼滤波的要求并能进行相应滤波方程的推导.

在实际的陀螺随机漂移数据处理中，为陀螺的随机漂移量，ωk为陀螺的实际角速率值，Γk,k-1、φk,k-1、Hk均为单位矩阵，Πk,k-1是建立陀螺随机漂移模型的参数值［5］.

2　滤波器性能的检验

2.1仿真验证

本文以MPU6050陀螺仪模块为仿真试验对象来进行检测.MPU6050是一个数位运动处理硬件加速引擎，包含3轴陀螺仪、3轴加速器，并可通过I2C端口连接其他磁力传感器、加速器等.可处理运动感测的复杂数据，降低了运动处理运算对操作系统的负荷，并为应用开发提供了架构化的API.

本文采用该模块的零偏不稳定性参数对漂移误差模型的准确性和滤波器的有效性进行验证.在仿真验证时，首先根据MPU6050陀螺的参数设置MATLAB的卡尔曼滤波程序中的参数，分别为陀螺仪0.0015°/s，加速计0.7 mg，测量噪声0.0030°/s和35 mg.在数据采集时，先将陀螺放在一个平稳的桌面上，尽量使陀螺仪输出值是零值或者处在某个特定的常值.采集此时陀螺仪X轴的数据，比较加入卡尔曼滤波算法前后的数据稳定性，从而判定滤波的性能.

未加滤波算法时，采集陀螺仪的X轴数据并将其导入MATLAB，从而得到X轴原始角速度的仿真图，如图1所示，角度的跳动在±2°位置左右.理想状态下的X轴数据应该是过0水平线，在实际测量中，通过X轴原始角速度仿真图可以看出，陀螺仪在静态情况下，X轴输出信号是在0均值的水平线上随时间作无规律跳动的随机变量.

而加入卡尔曼滤波算法处理数据后，重新运行程序，以同样的方式采集陀螺仪的X轴数据将其导入MATLAB，得到滤波后的X轴角速度仿真图，如图2所示，可见其角度的跳动在±1°位置左右.采用卡尔曼滤波方式可以很好地抑制陀螺仪的随机漂移误差，降低陀螺的随机噪声，是一种行之有效的降低误差的方法.

2.2适应性试验

卡尔曼滤波器设计中，默认的系统状态仿真矩阵为单位矩阵，这样就会使角速率值在一定的角度周期范围内不会随卡尔曼递推公式变化.针对这种情况，可以通过提高采样的频率方法来解决.将之前的100HZ采集数据改为1000 HZ的采样数据进行仿真.分别对5°、15°、50°、150°振幅的陀螺信号进行数据仿真，采集原始工作中直接建模得到的滤波前的数据和加入卡尔曼算法改善后的滤波后的数据，将二者采集的数据进行比较分析，以说明其适用性.

图1　X轴原始角速度仿真图

图2　滤波后的X轴角速度仿真图

表1和表2分别是陀螺仪输出角速率的误差均值和误差标准差的数值.从表1滤波前后的数据比较来看，滤波前后角速度误差均值的变化不是很明显.甚至出现在高角度时滤波后的误差均值大于滤波前的情况.

表1　适应性摇摆试验角速率误差均值（1000 Hz）

表2　适应性摇摆试验角速率误差标准差（1000 Hz）

但对表2的角度误差标准差进行比较分析时发现加入卡尔曼滤波算法后误差标准差有了很大程度的降低，滤波前的标准差在0.635左右，滤波后的标准差则稳定在0.093左右，滤波后的误差标准差稳定在一个较小范围内.从标准差的作用及其特性可知，误差标准差的降低表明了所采集的数据的离散程度降低，稳定性有了很大的提升.因此采用这样的处理方法可以实现数据的稳定处理.

3　系统测试

为了便于观察卡尔曼算法在整个系统中的滤波效果，结合系统对陀螺输出数据进行仿真.下面主要对静态工作和动态工作时的数据进行仿真.

3.1静态工作数据仿真

系统运行时，将MPU6050陀螺模块放置在平稳的桌面上，使其处在静止工作状态，真实反馈信号为零.然后以1000 HZ的采样频率对MPU6050模块中输出的角度值进行采集，将采集数据以.txt形式导入MAT⁃LAB中生成模型，可以得到如图3所示的静态工作仿真图.

3.2动态工作数据仿真

系统运行时，轻轻旋转MPU6050陀螺模块，使陀螺输出的反馈信号不为零，然后利用同样的方法对原始数据和滤波后的数据进行采集.为了更明显的观察，分别对原始数据和滤波后的数据进行MATLAB仿真.得到如图4所示的动态工作仿真图.

3.3分析总结

通过滤波前后数据仿真图的对比可以看出，MPU6050陀螺模块无论是处在静态工作状态还是动态工作状态，都能有效抑制信号中的扰动现象，使陀螺输出的反馈信号趋向平稳，有效地提高了系统在运行时的工作精度.所以卡尔曼滤波算法是提高稳定控制系统精度的关键部分，且在实际应用中简单方便.

图3　静态工作仿真

图4　动态工作仿真

4　结论

本文针对采用传统陀螺仪建模方式［2-4］后随机误差较大的问题，通过对MPU6050陀螺进行试验和仿真，采集其在静态和动态状态下的数据，利用卡尔曼滤波算法对数据进行处理，并对误差的均值和标准差进行了比较分析，实验结果表明陀螺仪精度可以在此方法下得到有效提高，是一种可行的简便方法.

［1］袁广民，李晓莹，常洪龙，等.MEMS陀螺随机误差补偿在提高姿态参照系统精度中的应用［J］.西北工业大学学报，2008，26 （6）：777-781.

［2］吉训生，王寿荣.MEMS陀螺仪随机漂移误差研究［J］.宇航学报，2006，27（4）：640-642.

［3］杨叔子，吴雅.时间序列分析的工程应用［M］.武汉：华中理工大学出版社，2007：87-93.

［4］吴富梅，杨元喜.基于高阶AR模型的陀螺随机漂移模型［J］.测绘学报，2007，36（4）：389-394.

［5］戴洪德，周绍磊.卡尔曼滤波及其实时应用［J］.应用数学，2010（4）：42-91.

Design and Implementation of a Gyro
Drift Filtering Algorithm of Stabilized Platform

WANG Xiao-ying，LIN Zhi，LIAN Jun-zheng
（School of Electrical and Automation Engineering,Changshu Institute of Technology,Changshu 215500,China）

For the gyro drift problem of stabilized platform,this paper established a gyro drift mathematic model and state equation based on angular velocity output of gyro sensor and removed the noises data through kalman filter.Simulation results show that kalman filtering can effectively restrain the disturbance of gyro signal and im⁃prove the working precision of the gyro stabilized platform.

stable platform；gyro drift；kalman filter

TP391.9

A

1008-2794（2015）02-0082-04

2013-11-03

苏州市科技计划项目“集图像采集功能的新型3G网络环境监测小型浮标研制”（SGZ2012064）；国家自然科学基金项目“基于蚂蚁智能搜索行为的多细胞自动跟踪及其应用研究”（61273312）；江苏省大学生创新训练计划项目“海洋浮标陀螺稳定控制摄像系统的研制”（201410333005Z）

通讯联系人：王小英，副教授，工学博士，研究方向：嵌入式系统、图形图像处理、计算机网络等，E-mail：xiaoying_wang@cslg.cn.