作者：杜思远, 李杰, 郑涛, 杨雁宇
作者单位：中北大学 仪器科学与动态测试教育部重点实验室, 山西 太原 030051
关键词：惯性测量;低成本MEMS惯性器件;三子样算法;扩展卡尔曼滤波;姿态四元数
摘要：
针对低成本MEMS惯性器件精度较低、噪声较大、存在误差随时间积累而无法满足长时间载体姿态测量的问题，提出一种基于三子样姿态更新的扩展卡尔曼姿态估计算法。该算法结合三子样构造姿态更新四元数作为扩展卡尔曼滤波的状态量，利用当地导航系下重力场、磁场中相关数据通过比例积分控制器完成对角速率误差的一次补偿，同时作为扩展卡尔曼观测信息，完成对姿态四元数的实时校正，以此降低误差随时间积累的影响。为验证算法精度，设计转台静态及车载动态试验，试验结果表明在测试约为400 s的时间内，航向角误差约为5，俯仰角及滚转角误差低于2，且无发散问题，满足长时姿态测量要求。
Attitude estimation algorithm for low cost MEMS based on updating tri-sample of EKF
DU Siyuan, LI Jie, ZHENG Tao, YANG Yanyu
Key Laboratory of Instrumentation Science & Dynamic Measurement, Ministry of Education, North University of China, Taiyuan 030051, China
Abstract: The low-cost micro-electro mechanical systems(MEMS) inertial device has problems of low precision and high noise, and can't meet the long-time carrier attitude measurement with error accumulated over time. Therefore, an attitude estimation algorithm based on updating tri-sample of extended Kalman filter(EKF) is proposed. The algorithm combines the tri-sample algorithm to construct the attitude updating quaternion as EKF filter state, using the relevant data of gravity field and magnetic field under navigation to compensate rate error with proportional integral controller, meanwhile, as EKF observation information to complete the real-time correction of the attitude quaternion, so as to reduce the influence of the error accumulated over time. In order to test the accuracy of the algorithm, the turntable static and vehicle dynamic test are designed. The result shows that heading angle error is about 5°, and pitch and roll angle errors are less than 2° in 400 s test, without divergence problem. Long time attitude measurement requirement is met.
Keywords: inertial measurement;low cost MEMS inertial device;tri-sample algorithm;extended Kalman filter(EKF);attitude quaternion
2018, 44(3): 38-43 收稿日期: 2017-08-23;收到修改稿日期: 2017-10-19
基金项目: 国家自然科学基金（51575500）
作者简介: 杜思远(1992-),男,黑龙江佳木斯市人,硕士研究生,专业方向为微惯性系统集成及测量方向。
参考文献
[1] 刘俊,石云波,李杰. 微惯性技术[M]. 北京:电子工业出版社,2005:19-27.
[2] 王梓函,姚敏立,闫志翔,等. 载体高精度惯导辅助动中通传递算法[J]. 中国测试,2017,43(1):106-111.
[3] 张承岫,李铁鹰,王耀力. 基于MPU6050和互补滤波的四旋翼飞控系统设计[J]. 传感技术学报,2016,29(7):1011-1015.
[4] 陈孟元,谢义建,陈跃东. 基于四元数改进型互补滤波的MEMS姿态解算[J]. 电子测量与仪器学报,2015,29(9):1391-1397.
[5] 贺晶晶,姜平,冯晓荣. 基于UWB的无人运输车的导航定位算法研究[J]. 电子测量与仪器学报,2016,30(11):1743-1749.
[6] 张剑锋. 基于松耦合方式的低成本GPS/SINS组合导航系统的设计与实现[D]. 成都:电子科技大学,2014.
[7] 秦永元. 惯性导航[M]. 北京:科学出版社,2015:243-244.
[8] 李海涛,曹咏弘,祖静,等. 效旋转矢量法在旋转弹姿态解算中的应用[J]. 测试技术学报,2011,25(4):287-291.
[9] 刘震,王雪梅,倪文波. 基于MEMS传感器的高精度姿态角测量研究[J]. 中国测试,2017,43(2):6-12.
[10] 赵欣,王仕成,杨东方,等. 一种改进的高动态捷联惯导解算算法[J]. 中国惯性技术学报,2011,19(2):163-169.
[11] 闫林波,贾维敏,姚敏立,等. 基于扩展卡尔曼滤波的动中通低成本姿态估计[J]. 电光与控制,2013,20(7):53-57.
[12] 秦永元,张洪钺,汪叔华. 卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社,2015:316-320.
[13] SABATELLI S, GALGANI M, FANUCCI L, et al. A double-stage kalman filter for orientation tracking with an integrated processor in 9-D IMU[J]. IEEE Transactions on Instrumentation & Measurement,2013,62(3):590-598.
[14] 陈国彬. 弹载三轴磁测系统设计与误差标定补偿技术研究[D]. 太原:中北大学,2012.
