基于MEMS航姿参考系统的角度估计算法研究

王庆辉， 陈芳萍， 魏立峰

(沈阳化工大学 信息工程学院， 辽宁 沈阳 110142)

基于MEMS航姿参考系统的角度估计算法研究

王庆辉， 陈芳萍， 魏立峰

(沈阳化工大学 信息工程学院， 辽宁 沈阳 110142)

为解决行人导航系统中，由于采用的基于微机械加工技术(MEMS)的惯性传感器精度较低，计算误差会随时间积累的问题，在基于微机电系统(MEMS)的航姿参考系统(AHRS)基础上，提出了一种动态过程修正算法，通过利用静止条件判断，进行角度校准，减少角度累积计算误差.在双轴转台进行实际测试结果表明，角度计算精度得到了提高.

MEMS； AHRS； 角度

随着时代的发展，智能终端的普及，人们在生活和工作中越来越多地应用到位置服务，而这一切的前提是定位，最原始的定位装置主要依靠的是对自然界信息的测量，再后来，人类在生产实践中，逐步掌握了运用惯性装置(如陀螺仪)进行定位的技术.GPS的问世是定位行业里程碑式的事件，在大多数环境下能做到全天时较高精度的定位，但在诸如商场、室内、隧道等复杂环境中，GPS由于信号易受到遮挡，有时无法提供定位信息.近几年，个人室内定位技术获得了很大的市场需求，如商场内基于个人位置的精确营销、需要跟踪值班人员行动路线的巡更系统等，目前大部分智能手机都开始安装基于MEMS的惯性器件，来提供个人位置辅助定位，但要想获得相对精确的定位，准确的姿态信息是非常重要的.惯性器件包括轴陀螺仪，加速度计和磁强计等.姿态与航向参考系统是基于惯性测量器件的姿态解算方法，能够提供航向角，横滚角和俯仰角信息，应用于飞行器导航和卫星姿态控制[1].国内低成本航姿器的研究和开发也是基于MEMS惯性传感器，其中，中航618所的朱少华等人提出了基于MEMS器件的大气航姿系统的设计思路和算法，样机进行了车载试验的验证[2].空军工程大学工程学院的黄国荣等人提出了基于MEMS器件的军用航姿系统的设计思路和算法，姿态误差在1°之内[3].Sebastian O.H，Madgwick提出了基于四元素的梯度下降算法，利用惯性传感器和三轴磁强计的测量值计算出姿态欧拉角，实现了对载体姿态的确定[4].由此可见，AHRS的发展趋势主要体现在低成本化，高精度化，高度集成化，应用范围不断扩大：从目前的军用市场将扩展到民用市场[5].

本文根据姿态与航向参考系统及互补滤波算法，提出一种AHRS过程修正算法，能够在个人定位时动态修正角度的测量值.在获取姿态角度时，通过滑动窗口滤波的方法加入判断人静止的条件，在人静止时重新对AHRS进行初始化修正，提高了角度的计算精度.

1 系统的总体设计

系统硬件部分采用导航处理器模块、惯性传感器模块和磁强计模块组成系统的总体框架.导航处理器模块主要是采集各个传感器的信息，并对信息进行解算，最后再将解算出的数据传输给上位机.该系统选择ST公司生产的基于ARM Cortex-M3内核的32位处理器STM32F103RBT6作为导航处理器[6].惯性传感器的性能主要体现在精度、量程和稳定性等方面.该系统选择QFN24封装的MPU-6050型号传感器，它整合了三轴陀螺仪和三轴加速度计[7]，在系统工作中，主要作用是感知系统水平方向的倾斜，并用于修正陀螺在俯仰和滚动方向的漂移.三轴磁阻型磁强计测量三维地磁强度，用于提供方向角的初始对准以及修正航向角漂移，利用磁强计的误差不随时间积累的特点，修正陀螺随时间推移的累计误差.该系统选择广泛应用于磁场检测领域的低成本罗盘——Honeywell的HMC5883L表面贴装高集成模块[8].使用惯性传感器和三轴磁强计输出的数据，应用AHRS过程修正算法校正陀螺仪的姿态角得到载体的姿态，在TinyCapture上位机上可以很好地显示横滚角、俯仰角和偏航角.系统结构框图如图1所示.

图1 系统的总体结构

2 AHRS互补滤波算法

该系统对姿态的求解应用的是基于四元数的AHRS互补滤波算法，使用的惯性传感器包括陀螺仪、加速度计和磁强计.利用三轴加速度计、三轴陀螺仪分别测量载体坐标系3个轴上的线加速度及角速率，利用三轴磁强计测量地磁场在载体坐标系3个轴上的分量.通过对传感器测量得到的数据应用互补滤波算法，即引入积分反馈，利用陀螺补偿角计算出陀螺测量误差的四元数微分形式，进而求出估计的四元数，根据公式转化成姿态欧拉角，实现对载体姿态的确定.

2.1 航姿参考系统的初始对准

横滚角和仰俯角由加速度计测量得到的重力在载体坐标系的分量求得，而偏航角由地磁场在载体坐标系的分量计算得到.系统启动后，首先根据加速度计和磁强计的输出值解算出载体的横滚角、仰俯角和偏航角.

γ=arctan(ay/az)

(1)

θ=arcsin(ax/-g)

(2)

φ=arctan[(mz×sinγ-my×cosγ)/

(mx×cosγ+my×sinγ+

mz×sinθ×cosγ)]

(3)

其中:γ表示横滚角，θ表示仰俯角，φ表示偏航角；g为重力加速度；ax，ay，az分别表示加速度计在载体坐标系的测量值；mx，my，mz分别表示磁强计在载体坐标系的测量值.

由(4)～(7)式可求出四元数的初始值：

(4)

(5)

(6)

(7)

将四元数转换成姿态欧拉角：

Euler_γ=

(8)

Euler_θ=arcsin(-2(q0×q2-q1×q3));

(9)

Euler_φ=

(10)

至此，AHRS的初始对准结束.

2.2 航姿参考系统的更新算法

首先利用三轴加速度计和三轴磁强计分别测量载体坐标系3个轴上的线加速度和地磁场在载体坐标系3个轴上的分量，然后对其进行归一化处理.公式(11)～(13)为加速度归一化测量值；公式(14)～(16)为罗盘归一化测量值.

(1) 归一化加速度和罗盘的测量值

(11)

(12)

(13)

(14)

(15)

(16)

(2) 磁失真补偿

令

(17)

(18)

磁强只在水平面有作用，从三轴变成两轴就是消除倾斜影响之后测量磁场方向.

(3) 计算姿态误差

(19)

(20)

(4) 引入积分反馈

(21)

gx，gy和gz为陀螺补偿角，因为陀螺的零点偏移将随着时间而变化，所以直接采用姿态误差的积分反馈对陀螺零偏进行补偿；twoKi为引入的反馈因子，该因子的引入使整个算法的精度得到很大提高；Δt是采样周期.

(5) 由陀螺补偿角计算出四元数的导数：

bg=[0 gxgygz]

(23)

(24)

(25)

2.3 过程修正算法

由于MEMS角速率陀螺仪存在严重的零点漂移和随机误差，在捷联惯性导航解算中会产生积分误差，难以达到应用精度.由此，系统在初始时刻所测角速率精度是比较高的，即为AHRS初始对准时所得的姿态欧拉角是比较准确的，所以在此提出AHRS的过程修正算法，即在获取姿态角度的过程中采用滑动窗口滤波的方法，时刻判断人是否处于静止状态，当人静止时重新对AHRS进行初始对准，这样就减少了陀螺随时间的推移而累积的误差，从而提高了角度的测量精度.由于加速度的采样率是100 Hz，即10 ms采样一次，所以2 s内加速度采样200次，设定窗口长度为200，对这2 s内的采样值取均值，并不断更新滑动窗口，由下一时刻的采样值代替最初时刻的采样值，此时若均值在阈值之内则认为人处于静止状态.由于人在静止时只有重力加速度，对三轴加速度计在载体坐标系3个轴上的线加速度求平方根，其值认为是采样一次重力加速度的值.该系统是利用转台的转动来模拟人的运动，当人佩带传感器时，传感器是处于垂直状态的，但由于静止时重力加速度是有波动的，所以对程序进行在线调试，同时利用转台测控软件设置传感器处于垂直状态且转台静止，当所求均值g的值在8 182.035 LSB/g和8 238.521 LSB/g之间时，认为人处于静止状态.系统工作流程如图2所示.

图2 系统工作流程

3 测试及结果

角度的测量通过转台来完成，将传感器连接到转台上，通过转台测控软件可设定主轴和俯仰轴转过的角度，在TinyCapture的上位机上来观测传感器横滚和俯仰所转过的实际角度.通过这样的方式可检测出所设定角度与传感器变化角度的差值.系统使用的双轴转台机械台体采用UO形铝合金框架结构，由内环横滚轴框架和外环俯仰轴框架组成相互垂直的两维旋转坐标系，其角位置综合测量精度为：±0.08°.

由于陀螺存在随时间的积累误差会累积的特点，所以设定转台在连续工作的情况下，通过测量多组数据，来得到传感器实际转过的俯仰角和横滚角.以60°为例，将转台测控软件的俯仰轴和主轴设置成60°，让转台在连续工作1 h的情况下测出传感器实际转过的角度.表1和表2的左侧是算法改进前通过观测TinyCapture上位机测得的角度.右侧是算法改进后测得的角度.

表1 算法改进前后传感器俯仰轴角度

表2 算法改进前后传感器横滚轴角度

由以上数据分析可知:算法改进后俯仰轴测量角精度平均提高了1.77 %；横滚轴测量角精度平均提高了1.07 %.在利用转台的转动模拟人的运动时，在转台静止也就是人静止时重新对AHRS进行初始对准，减少了陀螺的测量误差，即改进的算法可行.

4 结 论

本系统实现了对俯仰角与横滚角的精度测量，可以应用到个人定位时对人在行走和静止时的姿态检测.系统由于在对AHRS进行初始化时用到磁强计的测量值，很容易受到来自环境和平台的磁干扰，获取准确的角度测量值颇具挑战性.所以研究更加有效的动态修正算法，减小角度测量误差，提高定位精度是很有必要的.

[1] 张丽杰，常佶.微小型航姿测量系统及其数据融合方法[J].中国惯性技术学报，2011，19(3):307-311.

[2] 朱少华，汪芳，郭志想，等.一种适用于高动态制导飞行的大气航姿系统算法[J].弹箭与制导学报，2011，31(4):14-16.

[3] 杜继永，黄国荣，张凤鸣，等.基于低成本 MEMS 器件的捷联航姿系统设计[J].传感技术学报，2010，23(11):1662-1666.

[4] Madgwick S O H.An Efficient Orientation Filter for Inertial and Inertial/magnetic Sensor Arrays[EB/OL].(2010-04-30)[2013-10-10].http://sharenet-wii-motion-trac.googlecode.com/files/An_efficient_oriention_filter_for_inertial_and_inertialmagnetic_sensor_arrays.pdf.

[5] 刘智平，谭芳.国外航姿器的技术现状分析[J].四川兵工学报，2012，33(2):11-13.

[6] ST公司.STM32参考手册英文版[EB/OL].(2009-12-05)[2013-09-11].http://wenku.baidu.com/view/2901b526ed 630b1c59eeb5f3.html.

[7] InvenSense公司.MPU6050 产品说明书[EB/OL].(2011-11-16)[2013-09-11].http://wenku.baidu.com/view/d7f8dd080740be1e640e9a01.html.

[8] Honeywell公司.HMC5883L中文数据手册[EB/OL].(2011-03-05)[2013-09-11].http://wenku.baidu.com/view/91bf2c1d964bcf84b9d57bfd.

Angle Estimation Algorithm Based on MEMS Attitude and Heading Reference System

WANG Qing-hui， CHEN Fang-ping， WEI Li-feng

(Shenyang University of Chemical Technology， Shenyang 110142， China)

In the Pedestrian Navigation System,the micro machining technology based on the inertial sensors(MEMS) is low precision,calculation error will be accumulated over time.Based on MEMS Attitude and Heading Reference System,a dynamic process correction algorithm has been proposed,which could carry through the angle calibration and reduce the calculation error of angle by using static condition.The test results,which was realised on two-axis turntable,show that the measurement precision of angle has been improved.

MEMS； AHRS； angle

2013-09-10

沈阳市科技攻关项目(F11-009-2-00)

王庆辉(1972-)，男，黑龙江齐齐哈尔人，副教授，博士，主要从事无线自组网技术，嵌入式系统等方面的研究.

2095-2198(2015)01-0054-05

10.3969/j.issn.2095-2198.2015.01.012

TP212.9

A