基于鲁棒EKF的MEMS-INS/GNSS/VO组合导航方法

doi:10.12305/j.issn.1001-506X.2022.06.27

Abstract

Abstract:

Aiming at the problem that traditional inertial navigation system/global navigation satellite system (INS/GNSS) integrated navigation is easy to be disturbed in complex environment and the observation is abnormal, which affects the navigation performance, an micro-electro-mechanical system (MEMS)-INS/GNSS/visual odometry (VO) integrated navigation method based on robust extended Kalman filter (EKF) is proposed. The fusion framework based on inertial navigation system (INS), global navigation satellite system (GNSS), and visual odometry (VO) is designed, and the navigation filtering model in the case of GNSS signal failure is given. The EKF and Huber method are combined to overcome the influence of noise interference on the navigation performance, so as to improve the robustness of the system. The simulation and the KITTI dataset verify that the integrated navigation method can still output high-precision navigation results when the GNSS signal fails, and can better overcome the influence of abnormal observations on the system, and has high reliability and robustness.

Key words: integrated navigation, extended Kalman filter (EKF), Huber method, visual odometry (VO), KITTI dataset

CLC Number:

U666.1

Wenhua LI, Lixin WANG, Qiang SHEN, Can LI, Zongshou WU. MEMS-INS/GNSS/VO integrated navigation method based on robust EKF[J]. Systems Engineering and Electronics, 2022, 44(6): 1994-2000.

Figures/Tables 12

Fig.1

Fig.2

Fig.3

Fig.4

Table 1

Fig.5

Fig.6

Fig.7

Fig.8

Fig.9

Fig.10

Table 2

References 30

1	严恭敏, 邓瑀. 传统组合导航中的实用Kalman滤波技术评述[J]. 导航定位与授时, 2020, 7 (2): 50- 64.
	YAN G M , DENG Y . Review on practical Kalman filtering techniques in traditional integrated navigation system[J]. Navigation Positioning and Timing, 2020, 7 (2): 50- 64.
2	李灿, 沈强, 秦伟伟, 等. 基于平滑变结构-卡尔曼滤波的MIMU/BDS组合导航技术[J]. 航空兵器, 2021, 28 (3): 51- 58.
	LI C , SHEN Q , QIN W W , et al. MIMU/BDS integrated navigation technology based on smooth variable structure-adaptive Kalman filter[J]. Aero Weaponry, 2021, 28 (3): 51- 58.
3	龙腾, 穆荣军, 苏炳志, 等. 双天线GPS/MEMS-INS深组合导航方法研究[J]. 宇航学报, 2021, 42 (1): 92- 102.
	LONG T , MU R J , SU B Z , et al. Research on dual-antenna GPS/MEMS-INS deeply integrated navigation approach[J]. Journal of Astronautics, 2021, 42 (1): 92- 102.
4	LI Z K , ZAN L , LONG Z . Improved robust Kalman filter for state model errors in GNSS-PPP/MEMS-IMU double state integrated navigation[J]. Advances in Space Research, 2021, 67 (10): 3156- 3168. doi: 10.1016/j.asr.2021.02.010
5	张礼廉, 屈豪, 毛军, 等. 视觉/惯性组合导航技术发展综述[J]. 导航定位与授时, 2020, 7 (4): 50- 63.
	ZHANG L L , QU H , MAO J , et al. A survey of visual-inertial integrated navigation technology[J]. Navigation Positioning and Timing, 2020, 7 (4): 50- 63.
6	LIN X , LI W , LI S D , et al. Combined adaptive robust Kalman filter algorithm[J]. Measurement Science and Technology, 2021, 32 (7): 075015. doi: 10.1088/1361-6501/abf57c
7	苏敬, 何华锋, 何耀民, 等. 考虑SAR量测特性的弹载SINS/SAR组合导航滤波算法[J]. 系统工程与电子技术, 2021, 43 (4): 1044- 1049.
	SU J , HE H F , HE Y M , et al. Filtering algorithm of missile-borne SINS/SAR integrated navigation considering SAR mea-surement characteristics[J]. Systems Engineering and Electro-nics, 2021, 43 (4): 1044- 1049.
8	严恭敏, 翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.
	YAN G M , WENG J . Strapdown inertial navigation algorithm and integrated navigation principle[M]. Xi'an: Northwestern Polytechnical University Press, 2019.
9	CHEN K , CHANG G B , CHEN C , et al. An improved TDCP-GNSS/INS integration scheme considering small cycle slip for low-cost land vehicular applications[J]. Measurement Science and Technology, 2021, 32 (5): 055006. doi: 10.1088/1361-6501/abd96c
10	HRUSTIC E , ABDALLAH R B , VILAVALLS J , et al. Robust linearly constrained extended Kalman filter for mismatched nonlinear systems[J]. International Journal of Robust and Nonlinear Control, 2020, 31 (3): 787- 805.
11	ABDALLAH R B , VILÀ-VALLS J , PAGES G , et al. Robust LCEKF for mismatched nonlinear systems with non-additive noise/inputs and its application to robust vehicle navigation[J]. Sensors, 2021, 21 (6): 2086. doi: 10.3390/s21062086
12	HUBER P . Robust estimation of a location parameter[J]. Annals of Mathematical Statistics, 1964, 35 (2): 73- 101.
13	朱兵, 李星, 刘强, 等. 鲁棒Kalman滤波及其在水下组合导航中的应用[J]. 导航定位与授时, 2021, 8 (1): 96- 103.
	ZHU B , LI X , LIU Q , et al. Robust Kalman filter and its application in underwater intergrated navigation[J]. Navigation Positioning and Timing, 2021, 8 (1): 96- 103.
14	高敬东, 李开龙, 常路宾. 基于Huber的改进鲁棒滤波算法[J]. 系统仿真学报, 2014, 26 (8): 1769- 1774.
	GAO J D , LI K L , CHANG L B . Huber-based modified robust filter algorithm[J]. Journal of System Simulation, 2014, 26 (8): 1769- 1774.
15	ZHU B , CHANG L B , XU J N , et al. Huber-based adaptive unscented Kalman filter with non-Gaussian measurement noise[J]. Circuits Systems and Signal Process, 2018, 37 (9): 3842- 3861. doi: 10.1007/s00034-017-0736-x
16	WANG J Y , XU T H , WANG Z J . Adaptive robust unscen-ted Kalman filter for AUV acoustic navigation[J]. Sensors, 2020, 20 (1): 60.
17	PENG D D , ZHOU T , FOLKESSON J , et al. Robust particle filter based on Huber function for underwater terrain-aided navi-gation[J]. IET Radar, Sonar & Navigation, 2019, 13 (11): 1867- 1875.
18	ROMMEL T , HUBER S , YOUNIS M , et al. Matrix pencil method for topography-adaptive digital beam-forming in synthetic aperture radar[J]. IET Radar, Sonar & Navigation, 2021, 15 (10): 1195- 1208.
19	MU R J , SU B Z , CHEN J Y , et al. Multiple-step randomly delayed adaptive robust filter with application to INS/VNS integrated navigation on asteroid missions[J]. IEEE Access, 2020, 8, 118853- 118868. doi: 10.1109/ACCESS.2020.3004575
20	XU B , RAZZAQI A A , LIU Y L . Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation[J]. The Journal of Navigation, 2019, 72 (4): 875- 893. doi: 10.1017/S0373463319000018
21	PENG D , ZHOU T , FOLKESSON J , et al. Robust particle filter based on Huber function for underwater terrain-aided navi-gation[J]. IET Radar, Sonar & Navigation, 2019, 13 (11): 1867- 1875.
22	LI Y D , HOU L H , YANG Y , et al. Huber's M-estimation-based cubature Kalman filter for an INS/DVL integrated system[J]. Mathematical Problems in Engineering, 2020, 8, 1- 12.
23	文坤. 多状态多视图约束视觉/惯性组合导航算法研究[D]. 长沙: 国防科学技术大学, 2016.
	WEN K. Research on multi-state and multi-view constraint algorithms for vision/inertial integrated navigation[D]. Changsha: National University of Defense Technology, 2016.
24	TARDIF J P, GEORGE M, LAVERNE M, et al. A new approach to vision-aided inertial navigation[C]//Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2010.
25	SOLAJ, VIDAL-CALLEJA T A, DEVY M. Undelayed initia-lization of line segments in monocular SLAM[C]//Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009.
26	SIRTKAYA S, SEYMEN B, ALATAN A A. loosely coupled Kalman filtering for fusion of visual odometry and inertial navigation[C]//Proc. of the International Conference on Information Fusion, 2013.
27	CHANG G B . Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion[J]. Journal of Geodesy, 2014, 88 (4): 391- 401. doi: 10.1007/s00190-013-0690-8
28	ZHU B , WU M , XU J N , et al. Robust adaptive unscented Kalman filter and its application in initial alignment for body frame velocity aided strapdown inertial navigation system[J]. Review of Entific Instruments, 2018, 89 (11): 115102. doi: 10.1063/1.5046760
29	ARASARATNAM I , HAYKIN S , ELLIOTT R J . Discrete-time nonlinear filtering algorithms using Gauss-Hermite quadrature[J]. Proceedings of the IEEE, 2007, 95 (5): 953- 977. doi: 10.1109/JPROC.2007.894705
30	GEIGER A , LENZ P , STILLER C , et al. Vision meets robo-tics: the KITTI dataset[J]. The International Journal of Robo-tics Research, 2013, 32 (11): 1231- 1237. doi: 10.1177/0278364913491297

导航参数	全时段			混合高斯噪声			4倍初始误差
导航参数	AKF	EKF	HREKF	AKF	EKF	HREKF	AKF	EKF	HREKF
俯仰角/(′)	22.601 2	11.149 3	9.132 0	3.908 3	4.032 9	3.035 7	5.028 8	5.243 4	2.870 2
横滚角/(′)	10.772 0	9.836 7	9.514 8	9.870 8	6.408 9	4.936 5	4.607 1	4.807 2	4.188 5
偏航角/(′)	141.239 9	120.701 6	118.026 9	32.033 1	33.150 9	28.870 6	41.239 0	28.126 9	10.707 1
东速/(m·s^-1)	0.252 0	0.212 3	0.199 6	0.403 7	0.318 6	0.095 8	0.302 8	0.305 6	0.136 5
北速/(m·s^-1)	0.332 9	0.283 7	0.162 7	0.287 0	0.275 8	0.093 5	0.561 4	0.320 7	0.084 6
天速/(m·s^-1)	0.059 5	0.065 0	0.047 9	0.026 2	0.028 8	0.010 1	0.075 9	0.037 2	0.017 4
纬度/(°)	4.847 7	2.920 6	2.332 4	4.669 6	4.208 8	1.045 9	5.829 9	2.344 1	2.908 4
经度/(°)	4.329 3	3.442 5	3.146 8	7.994 8	3.791 3	2.571 6	4.856 5	5.077 7	4.072 3
高度/m	2.336 2	2.278 1	1.365 5	1.540 5	1.443 4	0.327 6	2.285 7	3.054 6	0.581 1

导航参数	AKF	EKF	HREKF
东速/(m·s^-1)	2.057 8	1.974 1	1.966 0
北速/(m·s^-1)	2.505 0	2.639 5	1.699 0
纬度/(°)	0.924 5	0.932 0	0.902 0
经度/(°)	1.760 9	1.730 9	1.715 3

[1]	Yingying JIANG, Shuguo PAN, Fei YE, Wang GAO, Chun MA, Hao WANG. Approach for detection of slowly growing fault based on robust estimation and improved AIME [J]. Systems Engineering and Electronics, 2022, 44(9): 2894-2902.
[2]	Yiping DONG, Ning LIU, Zhong SU, Jingxiao WANG, Hongyang BAI. Integrated navigation method of high-speed spinning flying bodybased on AEKF [J]. Systems Engineering and Electronics, 2022, 44(6): 1977-1983.
[3]	Zhaoqiang SUN, Zhigui WANG, Fei MENG, Luyu LI, Zhong YU, Yan CHEN. Ballistic target tracking filter design based on EKF and ballistic equations [J]. Systems Engineering and Electronics, 2022, 44(10): 3207-3212.
[4]	Shuguang SUN, Qixin WEN. Aircraft height optimization algorithm of integrated navigation in terminal area based on height anomaly compensation [J]. Systems Engineering and Electronics, 2021, 43(9): 2612-2619.
[5]	Zhe WEN, Hongwei BIAN, Heng MA, Tao ZANG. Simulation test method of inertial navigation polar performance at middle and low latitudes [J]. Systems Engineering and Electronics, 2021, 43(9): 2620-2627.
[6]	Renjie ZHAO, Baiqing HU, Xu LYU, Jiayu TIAN. Filtering algorithm of UKF integrated navigation based on dual-Euler angles [J]. Systems Engineering and Electronics, 2021, 43(7): 1912-1920.
[7]	Baojie CAI, Lei SHAO. Robust filtering algorithm based on three discriminant domain and least squares fitting [J]. Systems Engineering and Electronics, 2021, 43(5): 1346-1353.
[8]	Jing SU, Huafeng HE, Yaomin HE, Yifan WANG, Xiaofei HAN. Filtering algorithm of missile-borne SINS/SAR integrated navigation considering SAR measurement characteristics [J]. Systems Engineering and Electronics, 2021, 43(4): 1044-1049.
[9]	Zhizhong LIAO, Qi WANG. Influence and countermeasures of radar seeker pointing error on missile guidance [J]. Systems Engineering and Electronics, 2021, 43(2): 519-525.
[10]	Xu LYU, Baiqing HU, Yongbin DAI, Renjie ZHAO. Application of USQUE based on PSO in attitude estimation of integrated navigation [J]. Systems Engineering and Electronics, 2020, 42(6): 1366-1371.
[11]	Kaixin LUO, Meiping WU, Ying FAN. Robust adaptive filtering based on maximum entropy method and its application [J]. Systems Engineering and Electronics, 2020, 42(3): 667-673.
[12]	Kanghui HE, Chaoyang DONG. Orthogonal transformed fifth-order cubature Kalman filter for navigation [J]. Systems Engineering and Electronics, 2020, 42(3): 680-685.
[13]	Xiaolong WANG, Haiying LIU, Jingqi WANG. Collaborative navigation of air-ground multi-agent based on hierarchical SLAM [J]. Systems Engineering and Electronics, 2020, 42(1): 166-171.
[14]	ZHANG Chuang, GUO Chen, ZHANG Daheng. Adaptive iterative particle filter and its application for ship integrated navigation [J]. Systems Engineering and Electronics, 2019, 41(4): 883-888.
[15]	GE Baoshuang, ZHANG Hai, JIN Yanqiong. Redundant measurement based method for online mitigation of GNSS multipath errors [J]. Systems Engineering and Electronics, 2019, 41(11): 2581-2587.

MEMS-INS/GNSS/VO integrated navigation method based on robust EKF

RichHTML

PDF (PC)

Knowledge

Abstract

Cite this article

share this article

Figures/Tables 12

References 30

Related Articles 15

Recommended Articles

Metrics

Comments