基于Kalman滤波算法的姿态传感器运动补偿方法研究

贾培刚，李明哲

JIA Pei-gang, LI Ming-zhe

（西安工业大学 机电工程学院，西安 710021）

0 引言

随着微电子机械系统（MEMS）技术在微电子、材料、力学、化学、机械学诸多学科领域获得越来越广泛的普及，从而基于MEMS技术的传感器得到越来越多的应用[1]。由于此类传感器具有体积小、精度高、可靠性优良等优点，在材料工程、电子信息产品、机械制造等领域得到广泛的应用，但由于姿态传感器在运动过程中的误差对其造成误差干扰降低其精度，因此对姿态传感器运动补偿方法的研究变得非常重要[2]。其中基于MEMS姿态传感器主要用于对载体运动过程中倾斜角变化的测量与稳定，但是由于倾斜角角度和运动速度的改变，就会导致其输出值及测量结果的改变[3]，所以必须采取相应方法消除或抑制在运动过程中传感器产生的倾斜角误差以及速度误差，对姿态传感器进行运动误差补偿。

姿态传感器的运动补偿可以通过硬件补偿和软件补偿两种方法实现[4]。硬件补偿的方法主要通过改变电子元器件选型以及布板，但这种方法会导致系统复杂化，并提高其成本。软件补偿方法主要有Allan方差补偿算法、自相关分析算法、Kalman滤波算法等[5]。本文从运动误差补偿精确性以及实用性，因此采用Kalman滤波算法进行传感器运动补偿。

1 姿态传感器的运动补偿原理

本文采用亚德诺半导体技术公司（Analog Device Inc.，ADI）公司生产的ADXL345加速度传感器。ADXL345是一款惯性测量三轴加速度传感器，该芯片具有高分辨率、高敏感度、高精度等性能，可以随时检测出传感器的角度及加速度情况。在运动过程中，由于传感器会产生不确定性以及数据不稳定，因此传感器输出检测信号的角度值与理论角度输出值会出现一定的误差，称之为角度误差[6]。为了将这种角度误差消除或者减少，利用Kalman滤波算法对传感器运动过程中进行误差补偿，从而抑制或减小运动误差。

在传感器运动过程中，传感器通过输入检测模块采集数据并发送至姿态检测模块和加速度传感器模块，检测到的信号数据经过加速度补偿模块进行相应的运动补偿，最后通过输出检测信号模块对传感器做出相应姿态角调整。姿态传感器的运动补偿原理框图如图1所示。

图1 运动补偿原理框图

2 姿态传感器的运动补偿方法

由于传感器采集的数据通过输入和检测过程中得到的信号不仅包含有用信号，而且包含各类噪声和干扰信号。所以需要对其误差进行滤波以减少或抑制误差。Kalman滤波的作用就是利用传感器在动态情况下的测量检测中，从一系列包含噪声的测量结果中抑制噪声的影响，得到传感器的有用信号，从而保证传感器的精确性和稳定性。

Kalman滤波包括两个阶段：预测与校正。在预测阶段，系统通过对上一时刻状态的估计值作为估计当前状态的输入值。在校正阶段，系统通过获得当前状态输出值，从而作为对预测值的判断并对预测阶段获得的数值进行处理，得到更精确的输出值。滤波算法用X(k|k)表示在k时刻的对于当前系统状态的估计，用P(k|k)的协方差X(k|k)表示k时刻对系统预测值的精确估计。

1）预测阶段

假设当前系统所处时刻为k，根据滤波器系统模型，可以通过对上一时刻状态的估计值而做出对当前状态的预测：

式中，X(k-1|k-1)是通过上一时刻的状态从而得到的当前状态，X(k|k-1)是上一状态优化后的估计值，U(k) 为当前时刻的系统的控制量，若系统无控制量，它可以为0。

除了对系统状态的估计，预测阶段还包含对系统估测值进行精确的估计。用系统状态的协方差来表示这个估计值：

式中，P(k|k-1)是X(k|k-1)的协方差，P(k-1|k-1)是X(k-1|k-1)的协方差，通过上式可以表示对滤波前对当前系统的预测。

2）校正阶段

通过上述对当前状态预测值的获得，并结合对当前状态而得到的预测值，可以得到对当前状态最优化的估计值：

式中，Kg为Kalman增益。

通过上述预测阶段和校正阶段，可以得到k时刻系统的最优估计值X(k|k)，但是为了要让Kalman滤波器不断的运行下去，还要对当前状态下X(k|k) 的协方差：

式中，I为单位矩阵，当系统状态进入k+1时，P(k|k)替换上式中的P(k-1|k-1)，这样算法就可以循环运算下去。

3 实验验证及数据处理

3.1 数据采集与分析

旋转运动状态下数据采集。将姿态传感器水平放置于二维转台内，与基准面重合，Z轴方向与桌面垂直，与地球重力方向重合，保持相对静止，以100Hz的频率对姿态传感器进行数据采集，随机选择其中5组数据，如表1所示。

表1 旋转运动状态下数据采集

2）变速运动状态下数据采集。因为姿态传感器在运动过程中沿X、Y、Z三个方向的运动情况类似，因此只对传感器沿X方向运动的数据进行采集处理。将传感器沿X轴方向移动，进行数据采集，随机选择其中5组数据，如表2所示。

表2 变速运动状态下数据采集

3.2 Kalman滤波分析

分别对姿态传感器在旋转运动状态和变速运动状态下取得的加速度数据进行Kalman滤波。如图2和图3所示。

图2 旋转运动状态下的加速度输出值比较图

图3 变速运动状态下的加速度输出值比较图

从图2中可以看出，在旋转运动状态下，本文所采用的Kalman滤波算法有效地减少在运动过程中姿态传感器在测量姿态角以及运动时所引起的误差。换言之，在传感器旋转运动状态下，Kalman滤波算法可以对旋转过程中产生的运动误差做出相应补偿，提高姿态角的测量精度。从图3中可以看出，在变速运动状态下，随着传感器的加速运动和减速运动，姿态传感器的加速度在相应增加，变速运动状态下输出的加速度经过Kalman滤波之后变得更为平滑，更加符合实际情况。

3.3 实验数据验证

本文是采用MEMS姿态传感器实现对物体运动误差的补偿，对姿态传感器的加速度输出值采用Kalman滤波算法消除抑制误差，再通过二维转台和变速运动对补偿后的姿态传感器进行数据采集。表3为在旋转状态下采用Kalman滤波算法对传感器加速度输出值的数据采集，表4为在变速运动下对传感器加速度输出值的数据采集。

表3 补偿后旋转运动状态下数据采集

表3、表4的结果显示，Kalman滤波算法能实现对姿态传感器的运动补偿，对姿态传感器的运动误差起到有效明显的消除和抑制，为今后姿态传感器的实验与实际使用提供了可靠的保障。

表4 补偿后变速运动状态下数据采集

4 结论

本文通过对MEMS姿态传感器的运动特性进行分析研究，通过Kalman滤波算法对其运动误差进行补偿。在对旋转运动和变速运动的分析中，验证Kalman滤波算法对传感器的加速度输出值的变化，并按具体的实验进行了验证。由此说明，Kalman滤波能够有效滤除和抑制姿态传感器的运动误差，更好的达到预期效果。因此这种方法对姿态传感器的运动补偿具有一定的实际应用价值。

[1]陈航科,张东升,盛晓超,王凯.基于Kalman滤波算法的姿态传感器信号融合技术研究[J].传感器与微系统,2013,12(1):82-85.

[2]刘宇,鞠文斌,刘羽熙.MEMS加速度传感器计量检测技术的研究进展[J].计测技术,2010,30(4):5-8.

[3]胡恩伟.基于MEMS多传感器数据融合的惯性组合导航系统算法设计与实现[C].2013.

[4]谷庆红.微机械陀螺仪的研制现状[J].中国惯性技术学报,2003,11(5):67-72.

[5]陈萍,陈黎黎.一种压力传感器温度补偿新方法[J].计算技术与自动化,2009,28(4):49-52.

[6]Bar-Shalom,Birmiwal K.Variable Dimension Filter for Maneuvering Target Tracking[J].IEEE Transactions on Aerospace and Electronic Systems,1982,AES-18(5):621-629.

[7]Akeila E,Salcic Z,Swain A.A self-resetting method for reducing error accumulation in INS-based tracking[C].2010.13(1):418-427.

[8]宋丽君.基于MEMS器件的航向姿态测量系统的研究[D].西北工业大学,2007.

[9]秦永元,张洪钺,汪叔华.卡尔曼滤波与组合导航原理[M].西北工业大学出版社.1998:33-37.

[10]耿延睿,崔中兴.组合导航系统卡尔曼滤波衰减因子自适应估计算法研究[J].中国惯性技术学报,2001,9(4):8-10.