采用卡尔曼滤波算法的MEMS器件姿态测量

周树道， 金永奇， 卫克晶， 刘 星(解放军理工大学 气象海洋学院，江苏 南京 211101)



采用卡尔曼滤波算法的MEMS器件姿态测量

周树道， 金永奇， 卫克晶， 刘 星
(解放军理工大学 气象海洋学院，江苏 南京 211101)

针对小型无人机上使用的MEMS惯性传感器在精度、噪声上存在的问题，采用卡尔曼滤波算法结合角度传感器、加速度传感器、磁阻传感器的传感信息，来解算姿态角最优值。采用四元素法确立了捷联矩阵，利用三轴陀螺仪传感器所得到的角速度信息建立系统状态方程，利用三轴加速度传感器和磁阻传感器信息建立了系统测量方程，进而设计了一种卡尔曼滤波器来滤除MEMS传感器存在的随机噪声，并解算出了小型无人机的姿态角。通过实验室静态测试和动态测试表明，无人机姿态角解算结果与实际值对比，误差能够控制在2°以内，可满足工程应用要求。

卡尔曼滤波； 姿态角解算； 微机电系统传感器； 四元素法

0 引 言

姿态角是飞机飞行过程中重要参数，无论在控制飞机飞行还是进行各种无人机探测时，都需要获得机体的姿态信息，因此，对飞机姿态角的准确测量具有重要意义。通常在传统的大型飞机上安装有高精度的静电陀螺仪、液浮陀螺仪等，零偏稳定性优于0.0015 °/s[1-2]，可长久地得到高精度的姿态信息，但这些器件价格昂贵，体积、重量也较大，无法在小型无人机上应用。随着MEMS技术发展，硅微机械电子陀螺仪得以应用，其体积小、重量轻、功耗低、惯性小[3-4]，成为无人机姿态测量的很好选择，但精度偏低[5]，无法单独使用。本文在提出相关数学算法基础上，融合多种传感器信息，有效提高了姿态角的测量精度。

1 相关坐标系引入及姿态角定义

1.1 相关坐标系引入

机体坐标系：以飞机重心为原点，机体纵轴指向前、横轴指向右、竖轴指向上分别为三轴建立三维直角坐标系，用Oxbybzb表示；地理坐标系：以无人机重心为原点，地理方向指向东、北和铅直向上为三轴建立另一个直角坐标系，用Oxtytzt表示[6]。

1.2 姿态角的定义

飞机姿态角是机体坐标系与地理坐标系之间的方位关系。无人机绕横轴xb转动，纵轴yb与地理坐标系Oxtyt面之间的夹角叫做无人机的俯仰角，用θ表示，从Oxtyt面算起，向上为正，向下为负，定义域为[-90°，90°]；无人机绕横纵yb转动，横轴xb与地理坐标系Oxtyt面之间的夹角叫做无人机的横滚角，用γ表示，从Oxtyt算起，右倾为正，左倾为负，定义域为(-180°，180°]；无人机绕横纵zb转动，纵轴yb与地理坐标系Oytzt面之间的夹角叫做无人机的航向角，用ψ表示，从Oytzt面算起，0°、90°、180°和270°分别指向正北、正东、正南和正西，定义域为[0°，360°)[7]。

2 捷联矩阵与四元素法

由于飞机姿态角蕴含于机体坐标系和地理坐标系之间的关系中，所以可将飞机姿态角解算问题转化到机体坐标系与地理坐标系变换计算上[8]。而计算两坐标系变换，关键是找出捷联矩阵。

2.1 捷联矩阵的确定

机体坐标系与地理坐标系间关系如图1所示[7]。

图1 两坐标关系图

捷联矩阵[9-10]的确定思路为：

第①次变换为：

第②次变换为：

第③次变换为：

则由Oxtytzt坐标系转到Oxbybzb坐标系的变换关系为：

其中，T为捷联矩阵，根据矩阵中的元素可以求得飞机的姿态角。

2.2 捷联矩阵的四元素表示

上述所得T是以三个姿态角为变量来表示的，虽然直观易懂，但在求解姿态角，当θ=90°时，将出现奇点，因而该表示法的应用有一定的局限性，不宜用在全姿态运动体的姿态解算中[11]。

三维空间中，一个向量在不同坐标系之间转化时，可以采用类似二维空间中的乘以一个复数的方法来实现，只是其中涉及到的旋转角度有3个，复数标记也需要推广到空间[12]，这样就引出了一个新的变量——四元素，其表示形式为：

Q=q0+q1i+q2j+q3k

设Q为由地理坐标系向机体坐标系变化的四元素，则用四元素表示的捷联矩阵为：

(1)

3 姿态角求解

根据四元素法表示的捷联矩阵求解姿态角为[13]：

(2)

其中，ψ和γ需要进行主值判断[14]。

又根据捷联惯导系统中四元素的相关理论，得到四元素的微分方程为：

⊗ω

(3)

式中，ω=0+wxi+wyj+wzk，wx、wy、wz分别为三轴角速度，矩阵形式为：

(4)

即：

(5)

这样，求解微分方程(5)，得到四元素的4个值，代入式(1)得捷联矩阵，再根据式(2)即可解得飞机的姿态角。

使用MEMS陀螺仪，其随机漂移达到了10-2°/s，求解微分方程得到的姿态角会随着时间推移误差积累增大，解算值将不可信，因此需要进行滤波处理。陀螺仪漂移是一种随机噪声，没有确定的频谱，无法用常规滤波器去除，但有确定的功率谱，可以看做是白噪声，可用卡尔曼滤波从量测量中去估计所需信号[15]，达到滤波的效果。

这里主要使用离散型卡尔曼滤波器。卡尔曼滤波算法的主要工作是建立系统的状态方程和测量方程：

(6)

图2 卡尔曼滤波计算更新流程图

两个方程建立思路：根据陀螺仪测量信息与姿态角的关系建立状态方程；根据加速度与磁阻传感器测量信息与重力场和地磁场关系建立测量方程。

首先进行状态方程的建立，将四元素的4个参数作为状态量，式(4)作为系统状态方程，利用比卡逼近法将其离散化，忽略高阶量，得到：

Q(k+1)=Φ(k+1/k)Q(k)+T(k)W(k)

(7)

其中：

式中：ΔT为传感器采样周期；W(k)为陀螺仪噪声。这里将陀螺仪漂移看作是一个随机游走的模型，即存在一个随机误差，故将W(k)近似为均值为零的独立高斯白噪声，且其协方差矩阵是非负定常值对角阵。

然后进行测量方程的建立。根据三轴加速度估计值与重力加速度存在的关系得：

(8)

根据三轴磁阻传感器测量信息与地球磁场存在的关系得：

(9)

综合式(8)、(9)建立系统的测量方程：

(10)

式中，V为测量噪声。

对式(10)离散化，得到离散测量方程为：

(11)

至此，建立了状态方程和测量方程，完成了卡尔曼滤波器的设计。这样按照图2计算流程利用传感器数据进行姿态角解算。

4 实验测试

为验证所设计的卡尔曼滤波器解算姿态角的实际效果，设计传感器电路测试系统进行实验测试。实验主要分静态和动态测试，静态测试主要对比卡尔曼滤波算法和陀螺仪积分算法解算姿态角的效果。动态测试主要验证卡尔曼滤波解算姿态角的准确性。

4.1 静态测试

采集测试系统传感器在静止状态下的测量值，分别使用卡尔曼滤波和角速度积分的算法，计算姿态角，结果如图3所示。

由结果可见，由于MEMS陀螺仪存在的较大漂移，使得直接积分得到的角度呈现一种发散的状况；而卡尔曼滤波算法，融合了无时间积累误差的加速度和磁阻传感器数据，有效滤除了陀螺仪漂移误差，使计算的角度曲线非常稳定，波动值在1°以内。

图3 卡尔曼滤波(上)和角速度积分(下)解算姿态角结果图

4.2 动态测试

为便于姿态解算结果与实际值进行对比，测试系统分别绕各轴作匀速转动，通过观察姿态角变化曲线各段的线性程度和计算线性斜率值，得出姿态角解算的精确程度。

控制转台分别绕x、y、z轴做正、反匀速转动，使用卡尔曼滤波算法融合各传感器数据解算的姿态角如图4所示。观察曲线可知，各角度的变化曲线呈现出较好的线性性，并具有良好的周期性。对实际数据分析得到周期为3.34 s，直接测量转台旋转周期为3.36 s，基本吻合。对几个特定位置进行直接测量，与解算结果偏差在2°以内。

(a) 航向角

(b) 俯仰角

(c) 横梁角

5 结 语

本文使用四元素法表示捷联矩阵，采用所设计的卡尔曼滤波器对实验测试数据进行了姿态解算并对结果做了对比分析。实验表明，按照本文所提供的方法，可以有效滤除MEMS陀螺仪漂移误差，将姿态角误差控制在2°以内，基本满足实际应用要求。

[1] 王寿荣. 硅微型惯性器件理论及应用[M]. 南京:东南大学出版社,2000:172-173.

[2] 刘 宇,李秋俊,刘 俊. 基于压电陀螺的快速方位测井短接[J]. 电子测量与仪器学报,2007(2): 90-94.

[3] 姜岩峰. 微纳电子器件[M]. 北京: 化学工业出版社, 2005:222-224.

[4] 钱华明,夏全喜,阙兴涛,等.基于Kalman滤波的MENS陀螺仪滤波算法[J]. 哈尔冰工程大学学报,2010,31(9): 1217-1221.

[5] 程 承,潘 泉,王坤龙,等. 基于压缩感知理论的MEMS陀螺仪信号降噪研究[J]. 仪器仪表学报，2012,33(4): 769-773.

[6] 王彦杰,周树道,朱国涛,等. 基于转动状态的气象无人机测风方法[J]. 气象科学,2011,31(5):576-581.

[7] 袁 信，郑 谔. 捷联式惯性导航原理[M]. 北京: 航空专业教材编审组出版, 1985:5-7.

[8] 周树道,王彦杰,王 敏,等. 基于气象无人机飞行角度的改进型测风模型[J].飞行力学,2011,29(1): 26-29.

[9] 关 劲,李仔冰. 旋转式捷联惯导系统精对准方法[J]. 中国惯性技术学报,2010,18(4): 396-400.

[10] 张荣辉,贾宏光,陈 涛,等. 基于四元素法的捷联式惯性导航系统的姿态解算[J].光学精密工程,2008,16(10): 1963-1970.

[11] 苑海静. 船用MEMS航姿测量系统算法研究与实现[D]. 哈尔滨: 哈尔滨工程大学, 2010：5-70.

[12] 以光衢. 陀螺理论与应用[M]. 北京: 北京航空航天大学出版社, 1990:39-40.

[13] 孙冬梅,田增山,韩令军. 捷联惯导系统中四元素法求解姿态角仿真模拟[J]. 弹箭与制导学报,2009,29(1): 51-53.

[14] 秦永元. 惯性导航[M]. 北京: 科学出版社, 2006:297-298.

[15] 秦永元. 卡尔曼滤波与组合导航原理[M]. 西安: 西北工业大学出版社, 1998:1-4.

Attitude Measurement of MEMS Devices by Using the Kalman Filter Algorithm

ZHOUShu-dao，JINYong-qi，WEIKe-jing，LIUXing
(Institute of Meteorology and Oceanography, PLA University of Science and Technology, Nanjing 211101, China)

For the problems of MEMS inertial sensors in accuracy and noise, Kalman filter algorithm is presented by fusing multi-sensor information obtained from angle sensor, acceleration sensor and magnetoresistive sensor to get optimal result of attitude angle. At first, a strapdown equation described by quaternion is established; an equation of state of the system is established by using tri-axis gyroscopes data, and a system measurement equation is established by using tri-axis accelerometer and MARG sensor data. Then a Kalman filter is designed to remove the random noise of the MEMS sensors and calculated attitude angles. Static test and dynamic test show that the error of attitude solver results can be controlled within 2°, and the design satisfies engineering requirement.

Kalman filter; solution of attitude angles; Micro-Electro-Mechanical Systems (MEMS )sensors; quaternion method

2013-12-12

周树道(1964-)，男，浙江宁波人，教授，现主要从事信号与信息处理研究。Tel.:025-80830101；E-mail:zhousd70131@sina.com

TP 274+.2

A

1006-7167(2015)02-0038-05