附加运动约束的MEMS惯性传感器融合定姿算法
|
摘要
针对MEMS惯性传感器因精度低、误差随时间累积导致无法满足长时间姿态测量要求的问题,提出了一种附加运动约束的姿态估计方法,即在以陀螺仪解算的姿态信息作为系统预测、以加速度计与磁强计解算的姿态信息作为系统量测的基础上,将载体运动约束作为虚拟观测量输入滤波器。同时,针对传统EKF算法精度不高的问题,提出了一种新的滤波融合算法,即迭代更新扩展卡尔曼滤波(iterated update extended Kalman filter,IU-EKF)。新算法通过将当前量测信息逐步引入量测更新过程实现后验状态估计,从而达到减弱观测模型非线性、提高滤波估计精度的目的。数值仿真结果表明:本文算法的姿态估计精度较传统的"双矢量法+EKF"模式有大幅提升。
To solve the problem of the MEMS inertial sensors' failure to meet the requirement of long time attitude measurement for its low precision and errors accumulating with time,an attitude estimation method with additional motion constraints is proposed. The new method is carried out by treating the attitude information solved by gyroscope as system prediction,treating the attitude information solved by accelerometer and magnetometer as system measurements and treating the carrier's motion constraints as a virtual measurement. Moreover,to solve the problem of low accuracy of traditional EKF algorithm,a new nonlinear filtering method,namely,the iterated update extended Kalman filter( IU-EKF),is proposed. The new algorithm is carried out by introducing the current time measurement information gradually to the measurement update process in pseudo-time,so as to achieve the purpose of weakening the observation model nonlinearity and improving the accuracy of filtering estimation. The numerical simulation results show that the proposed algorithm improves the accuracy of attitude estimation compared with the traditional " double vector method + EKF" mode.
To solve the problem of the MEMS inertial sensors' failure to meet the requirement of long time attitude measurement for its low precision and errors accumulating with time,an attitude estimation method with additional motion constraints is proposed. The new method is carried out by treating the attitude information solved by gyroscope as system prediction,treating the attitude information solved by accelerometer and magnetometer as system measurements and treating the carrier's motion constraints as a virtual measurement. Moreover,to solve the problem of low accuracy of traditional EKF algorithm,a new nonlinear filtering method,namely,the iterated update extended Kalman filter( IU-EKF),is proposed. The new algorithm is carried out by introducing the current time measurement information gradually to the measurement update process in pseudo-time,so as to achieve the purpose of weakening the observation model nonlinearity and improving the accuracy of filtering estimation. The numerical simulation results show that the proposed algorithm improves the accuracy of attitude estimation compared with the traditional " double vector method + EKF" mode.
引文
[1] KENDOUL F. Survey of advances in guidance,navigation,and control of unmanned rotorcraft systems[J].Journal of Field Robotics,2012,29(2):315-378.
[2]彭孝东,张铁民,李继宇,等.基于传感器校正与融合的农用小型无人机姿态估计算法[J].自动化学报,2015,41(4):854-860.PENG Xiao-dong,ZHANG Tie-min,LI Ji-yu,et al.Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica,2015,41(4):854-860.(in Chinese)
[3] BARBOUR N,SCHMIDT G. Inertial sensor technology trends[J]. IEEE Sensors Journal,2002,1(4):332-339.
[4] YUN X,BACHMANN E R,MOORE H,et al. Selfcontained position tracking of human movement using small inertial/magnetic sensor modules[C]//IEEE International Conference on Robotics and Automation.Roma:IEEE,2007.
[5] SABATINI A M. Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing[J]. IEEE Transactions on Biomedical Engineering,2006,53(7):1346-1356.
[6] MADGWICK S O H,HARRISON A J L,VAIDYANATHAN R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//IEEE International Conference on Rehabilitation Robotics(ICORR). Zurich,Switzerland:IEEE,2011.
[7] MAHONY R,HAMEL T,PFLIMLIN J M. Nonlinear complementary filters on the special orthogonal group[J]. IEEE Transactions on Automatic Control,2008,53(5):1203-1218.
[8]辛琪,史忠科.基于多源信息的飞行姿态估计方法[J].飞行力学,2012,30(6):527-531.XIN Qi,SHI Zhong-ke. Flight attitude determination base on multiple measurements[J]. Flight Dynamics,2012,30(6):527-531.(in Chinese)
[9] LU X,LIU Z,ZHANG H X. Adaptive Kalman filter for guided rolling projectile attitude estimation[C]//2016 IEEE Chinese Guidance,Navigation and Control Conference(CGNCC). Nanjing:IEEE,2016.
[2]彭孝东,张铁民,李继宇,等.基于传感器校正与融合的农用小型无人机姿态估计算法[J].自动化学报,2015,41(4):854-860.PENG Xiao-dong,ZHANG Tie-min,LI Ji-yu,et al.Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica,2015,41(4):854-860.(in Chinese)
[3] BARBOUR N,SCHMIDT G. Inertial sensor technology trends[J]. IEEE Sensors Journal,2002,1(4):332-339.
[4] YUN X,BACHMANN E R,MOORE H,et al. Selfcontained position tracking of human movement using small inertial/magnetic sensor modules[C]//IEEE International Conference on Robotics and Automation.Roma:IEEE,2007.
[5] SABATINI A M. Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing[J]. IEEE Transactions on Biomedical Engineering,2006,53(7):1346-1356.
[6] MADGWICK S O H,HARRISON A J L,VAIDYANATHAN R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//IEEE International Conference on Rehabilitation Robotics(ICORR). Zurich,Switzerland:IEEE,2011.
[7] MAHONY R,HAMEL T,PFLIMLIN J M. Nonlinear complementary filters on the special orthogonal group[J]. IEEE Transactions on Automatic Control,2008,53(5):1203-1218.
[8]辛琪,史忠科.基于多源信息的飞行姿态估计方法[J].飞行力学,2012,30(6):527-531.XIN Qi,SHI Zhong-ke. Flight attitude determination base on multiple measurements[J]. Flight Dynamics,2012,30(6):527-531.(in Chinese)
[9] LU X,LIU Z,ZHANG H X. Adaptive Kalman filter for guided rolling projectile attitude estimation[C]//2016 IEEE Chinese Guidance,Navigation and Control Conference(CGNCC). Nanjing:IEEE,2016.