卡尔曼滤波在MEMS惯性姿态测量中的应用

张彤　孙玉国

摘要：由于测控成本和有效载荷的限制，一般采用微机电系统（MEMS）惯性传感器来测量小型无人机的飞行姿态。在MC9S12XS128单片机上通过嵌入式软件编程实现了卡尔曼滤波算法，并在JZJ-1型自准直仪转台上对MEMS加速度计和陀螺仪的输出信号进行了数据融合试验，较好地解决了MEMS惯性测量系统的零漂和机械振动干扰问题。

关键词：姿态测量； 微机电系统； 惯性传感器； 卡尔曼滤波

中图分类号： TP 275 文献标志码： A doi： 10.3969/j.issn.1005-5630.2015.01.007

Abstract：Micro-electronic mechanical system （MEMS） inertial sensors are usually used to measure the attitude of the small unmanned aerial vehicle （UAV）， in order to reduce the controlling cost and save the effective payload. In this paper， the Kalman filtering algorithm was implemented on MCU MC9S12XS128 by embedded software. The Data fusion of the MEMS accelerometer and gyroscope output signals was executed by Kalman filter. The experimental results show that the zero drift and mechanical vibration interference problem of the MEMS inertial sensors could be reduced dramatically by the Kalman filter.

Keywords：attitude determination； MEMS； inertial sensor； Kalman filter

引 言

卡尔曼滤波器是Kalman于1960年提出的一种应用于离散线性滤波的迭代算法[1]，其目的是尽可能地减少测量噪声的影响，并从含有噪声的测量值中得到系统状态的最优估计。在小型无人机自动驾驶仪的设计中，受有效载荷重量和安装体积的限制，业界多采用微机电系统（MEMS）惯性测量单元来获取无人机的飞行姿态参数[2-4]。同时，受MEMS传感器制造工艺的限制，由MEMS加速度计和陀螺仪组合而成的惯性测量系统不可避免地存在零漂误差，且测试数据极易受到机体振动的干扰。为此，本文着重对小型四轴飞行器纵摇和横摇姿态（分别对应俯仰角和横滚角）测量进行研究。

1 卡尔曼滤波器的构造

2 卡尔曼滤波实验与结果分析

如图1所示，采用三轴MEMS陀螺仪L3G4200D，三轴MEMS加速度计MMA8451Q和16位Freescale单片机MC9S12XS128构成惯性姿态测量单元。其中，加速度计和陀螺仪通过IIC总线与单片机GPIO口的E2、E3引脚相连，经卡尔曼滤波后的数据通过Tx、Rx引脚发送到PC机。

软件流程图如图2所示，流程包括传感器初始化、IIC数据采集、卡尔曼数据融合和串口数据发送等4个主要步骤。

首先将JZJ-1转台的俯仰角保持在22°附近，采样时间为50 s左右。在此期间用橡皮锤对转台底座施加机械冲击载荷，测试曲线如图3所示。由图3可见，MEMS陀螺仪输出的俯仰角度值随着时间的推移明显偏离实际值，这是典型的零漂现象。在机械冲击载荷作用的时刻，由加速度值换算得到的角度值会明显偏离实际值。而经卡尔曼滤波后输出的角度值零漂现象不明显，且对外界冲击载荷的抗干扰能力较强。

当转台从-5°附近开始产生动态俯仰运动，在测试过程中对底座施加1次锤击，测试曲线如图4所示。由图可见，在动态工况下，卡尔曼滤波后的动态角度曲线比单纯依靠加速度计测得的角度曲线更平滑且无偏移。此时，卡尔曼滤波器同样对机械冲击载荷有较好的抑制作用。

3 结 论

本文利用卡尔曼滤波器对MEMS陀螺仪和角速度计的输出信号进行了数据融合，较好地解决了零漂和机械振动干扰问题。其基本原理是利用MEMS陀螺仪的信号作为预测值，利用加速度计输出信号对其校正。所设计的卡尔曼滤波器可运行在16位飞思卡尔单片机上，受单片机浮点运算能力的限制，采样时间间隔设置为0.1 s。如何在32位ARM芯片上实现多通道卡尔曼滤波，并提高采样频率将是今后研究工作的重点。

参考文献：

[1] 秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].西安：西北工业大学出版社.1998：110-135.

[2] 付勇杰，丁艳红，梁义维，等.动态倾角传感器及其传递特性的研究[J].仪表技术与传感器，2012（9）：6-8.

[3] 周升良，孙玉国，任强.基于MEMS的摇摆振动测试方法研究[J].信息技术，2014（1）：15-16.

[4] 任强，周升良，孙玉国.基于MEMS的角位置无线随动控制系统设计[J].传感器与微系统，2013，32（9）：122-124.

（编辑：刘铁英）