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

1 卡尔曼滤波器设计
通过卡尔曼滤波方法对陀螺仪的随机漂移进行处理时，首先需对陀螺仪的输出信号进行预处理并使其 满足平稳随机序列，然后建立相应的时序模型，并对其检验适用性. 这类建模方法在文献[2-3]中已经给出了 说明. 另外本文综合考虑了系统实时性要求及模型适用性要求等方面因素，故采用 AR（1）模型对陀螺仪随 机噪声进行建模分析.
准 差 的 作 用 及 其 特 性 可 知 ，误 差 标 准 差 的 降 低 表 明 了 所 采 集 的 数 据 的 离 散 程 度 降 低 ，稳 定 性 有 了 很 大 的 提
升. 因此采用这样的处理方法可以实现数据的稳定处理.
3 系统测试
为了便于观察卡尔曼算法在整个系统中的滤波效果，结合系统对陀螺输出数据进行仿真. 下面主要对 静态工作和动态工作时的数据进行仿真. 3.1 静态工作数据仿真
未加滤波算法时，采集陀螺仪的 X 轴数据并将其导入 MATLAB，从而得到 X 轴原始角速度的仿真图，如 图 1 所示，角度的跳动在±2°位置左右. 理想状态下的 X 轴数据应该是过 0 水平线，在实际测量中，通过 X 轴 原始角速度仿真图可以看出，陀螺仪在静态情况下，X 轴输出信号是在 0 均值的水平线上随时间作无规律跳 动的随机变量.
参考文献：
[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.
擎，包含 3 轴陀螺仪、3 轴加速器，并可通过 I2C 端口连接其他磁力传感器、加速器等. 可处理运动感测的复杂 数据，降低了运动处理运算对操作系统的负荷，并为应用开发提供了架构化的 API.
本文采用该模块的零偏不稳定性参数对漂移误差模型的准确性和滤波器的有效性进行验证. 在仿真验 证时，首先根据 MPU6050 陀螺的参数设置 MATLAB 的卡尔曼滤波程序中的参数，分别为陀螺仪 0.0015°/s，加 速计 0.7 mg，测量噪声 0.0030°/s 和 35 mg. 在数据采集时，先将陀螺放在一个平稳的桌面上，尽量使陀螺仪 输出值是零值或者处在某个特定的常值. 采集此时陀螺仪 X 轴的数据，比较加入卡尔曼滤波算法前后的数 据稳定性，从而判定滤波的性能.
系统运行时，将 MPU6050 陀螺模块放置在平稳的桌面上，使其处在静止工作状态，真实反馈信号为零. 然后以 1000 HZ 的采样频率对 MPU6050 模块中输出的角度值进行采集，将采集数据以. txt 形式导入 MAT⁃ LAB 中生成模型，可以得到如图 3 所示的静态工作仿真图. 3.2 动态工作数据仿真
2
王小英，林志，连军政：一种稳定平台中陀螺漂移滤波算法的设计与实现
83
在陀螺仪随机漂移误差中主要包括系统中产生的有色噪声和系统运行时陀螺自身产生的测量白色噪
声 .[4] 通过 AR（1）模型描述陀螺仪的随机漂移误差后，将 AR（1）模型改为状态空间模型，然后采用状态扩增
法，得到状态空间方程.
设系统方程为
X
a k
=
éëêWX Kkùûú
（4）
并得到新的系统方程及观测方程：
éëêWX kkùûú
=
éφ ê ë
k, k
0
-
1
ΠΓ kk,,kk--11ùûú
éëêWX kk
-
1ùú 1û
+
é0ù ë1û
ξk
-
1
（5）
[ ] Zk = Hk 0 éëêWXkkùûú + Vk
（6）
式中：Vk 是当均值为零值时的白噪声序列，满足卡尔曼滤波的要求并能进行相应滤波方程的推导.
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)
系统运行时，轻轻旋转 MPU6050 陀螺模块，使陀螺输出的反馈信号不为零，然后利用同样的方法对原始 数据和滤波后的数据进行采集. 为了更明显的观察，分别对原始数据和滤波后的数据进行 MATLAB 仿真. 得到如图 4 所示的动态工作仿真图. 3.3 分析总结
通过滤波前后数据仿真图的对比可以看出，MPU6050 陀螺模块无论是处在静态工作状态还是动态工作 状态，都能有效抑制信号中的扰动现象，使陀螺输出的反馈信号趋向平稳，有效地提高了系统在运行时的工 作精度. 所以卡尔曼滤波算法是提高稳定控制系统精度的关键部分，且在实际应用中简单方便.
2015 年
HZ 采 集 数 据 改 为
1000 HZ 的 采 样 数
据进行仿真. 分别对
5° 、15° 、50° 、150° 振
幅的陀螺信号进行
数 据 仿 真 ，采 集 原 始
工作中直接建模得
到的滤波前的数据
和加入卡尔曼算法
改善后的滤波后的 数 据 ，将 二 者 采 集 的
图 1 X 轴原始角速度仿真图
收稿日期：2013-11-03 基金项目：苏州市科技计划项目“集图像采集功能的新型 3G 网络环境监测小型浮标研制”（SGZ2012064）；国家自然科学
基金项目“基于蚂蚁智能搜索行为的多细胞自动跟踪及其应用研究”（61273312）；江苏省大学生创新训练计划 项目“海洋浮标陀螺稳定控制摄像系统的研制”（201410333005Z） 通讯联系人：王小英，副教授，工学博士，研究方向：嵌入式系统、图_wang@.
2
王小英，林志，连军政：一种稳定平台中陀螺漂移滤波算法的设计与实现
85
图 3 静态工作仿真
图 4 动态工作仿真
4 结论
本文针对采用传统陀螺仪建模方式 后 [2-4] 随机误差较大的问题，通过对 MPU6050 陀螺进行试验和仿真， 采 集 其 在 静 态 和 动 态 状 态 下 的 数 据 ，利 用 卡 尔 曼 滤 波 算 法 对 数 据 进 行 处 理 ，并 对 误 差 的 均 值 和 标 准 差 进 行 了比较分析，实验结果表明陀螺仪精度可以在此方法下得到有效提高，是一种可行的简便方法.
在
实
际
的
陀
螺
随
机
漂
移
数
据
处
理
中
，X
a k
=
[ω
k
]rk T ，rk 为陀螺的随机漂移量，ωk 为陀螺的实际角速率
值，Γk,k - 1 、φk,k - 1 、Hk 均为单位矩阵，Πk,k - 1 是建立陀螺随机漂移模型的参数值 .[5]
2 滤波器性能的检验
2.1 仿真验证 本文以 MPU6050 陀螺仪模块为仿真试验对象来进行检测. MPU6050 是一个数位运动处理硬件加速引
图 2 滤波后的 X 轴角速度仿真图
数据进行比较分析，以说明其适用性.
表 1 和表 2 分别是陀螺仪输出角速率的误差均值和误差标准差的数值. 从表 1 滤波前后的数据比较来
看 ，滤 波 前 后 角 速度误差均值的 变化不是很明 显. 甚至出现在 高角度时滤波后 的误差均值大于 滤波前的情况.
但对表 2 的
15
0.6350
0.0936
50
-0.3131
-0.3398
50
0.6350
0.0936
150
-0.3131
-0.6265
150
0.6350
0.0941
角度误差标准差进行比较分析时发现加入卡尔曼滤波算法后误差标准差有了很大程度的降低，滤波前的标
准差在 0.635 左右，滤波后的标准差则稳定在 0.093 左右，滤波后的误差标准差稳定在一个较小范围内. 从标
而 加 入 卡 尔 曼 滤 波 算 法 处 理 数 据 后 ，重 新 运 行 程 序 ，以 同 样 的 方 式 采 集 陀 螺 仪 的 X 轴 数 据 将 其 导 入 MATLAB，得到滤波后的 X 轴角速度仿真图，如图 2 所示，可见其角度的跳动在±1°位置左右. 采用卡尔曼滤 波方式可以很好地抑制陀螺仪的随机漂移误差，降低陀螺的随机噪声，是一种行之有效的降低误差的方法. 2.2 适应性试验
卡 尔 曼 滤 波 器 设 计 中 ，默 认 的 系 统 状 态 仿 真 矩 阵 为 单 位 矩 阵 ，这 样 就 会 使 角 速 率 值 在 一 定 的 角 度 周 期 范围内不会随卡尔曼递推公式变化. 针对这种情况，可以通过提高采样的频率方法来解决. 将之前的 100
84
常熟理工学院学报（自然科学）
摘 要：针对稳定平台工作中陀螺仪漂移问题，采用以陀螺仪角速度传感器的输出量作为观 测 量 ，建 立 陀 螺 仪 漂 移 的 数 学 模 型 和 状 态 方 程 ，然 后 利 用 卡 尔 曼 滤 波 对 含 有 噪 声 的 测 量 数 据 进 行 滤波处理的方式改善漂移问题. 仿真结果表明，该方式可以有效抑制陀螺仪信号中的扰动，提高陀 螺仪稳定平台的工作精度.
关键词：稳定平台；陀螺漂移；卡尔曼滤波 中图分类号：TP391.9 文献标识码：A 文章编号：1008-2794（2015）02-0082-04