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

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

摘要/Abstract

摘要：

针对传统惯导/卫导组合导航在复杂环境下易受干扰，观测量异常从而影响导航性能的问题，提出了基于鲁棒扩展卡尔曼滤波(extended Kalman filter, EKF)的组合导航方法。设计了基于微惯性导航系统(micro-electro-mechanical system-inertial navigation system, MEMS-INS)、全球导航卫星系统(global navigation satellite system, GNSS)及视觉里程计(visual odometry, VO)的融合框架，给出了在GNSS信号失效情形下的导航滤波模型，并将EKF与Huber方法结合，克服观测量受噪声干扰时对导航性能的影响，以提升系统鲁棒性。经仿真和KITTI数据集验证，MEMS-INS/GNSS/VO组合导航方法在GNSS信号失效时仍能输出较高精度导航结果，且可以较好克服异常观测值对系统的影响，具有较高可靠性和鲁棒性。

关键词: 组合导航, 扩展卡尔曼滤波, Huber方法, 视觉里程计, KITTI数据集

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

中图分类号:

U666.1

李文华, 汪立新, 沈强, 李灿, 吴宗收. 基于鲁棒EKF的MEMS-INS/GNSS/VO组合导航方法[J]. 系统工程与电子技术, 2022, 44(6): 1994-2000.

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.

图/表 12

图1

图2

图3

图4

表1

图5

图6

图7

图8

图9

图10

表2

参考文献 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]	姜颖颖, 潘树国, 叶飞, 高旺, 马春, 王浩. 基于抗差估计和改进AIME的缓变故障检测方法[J]. 系统工程与电子技术, 2022, 44(9): 2894-2902.
[2]	董一平, 刘宁, 苏中, 王靖骁, 白宏阳. 基于AEKF的高速自旋飞行体组合导航方法[J]. 系统工程与电子技术, 2022, 44(6): 1977-1983.
[3]	王琪, 廖志忠, 燕飞. 基于概率数据关联的雷达导引头抗速度拖引干扰算法[J]. 系统工程与电子技术, 2022, 44(2): 448-454.
[4]	刘艺, 周晓雄, 程广俊. 高动态跳频载波跟踪技术[J]. 系统工程与电子技术, 2022, 44(2): 677-683.
[5]	孙照强, 王志贵, 孟飞, 李陆雨, 于中, 陈燕. 基于EKF及弹道方程的弹道目标跟踪滤波器设计[J]. 系统工程与电子技术, 2022, 44(10): 3207-3212.
[6]	孙淑光, 温启新. 基于高程异常补偿的飞机终端区组合导航高度优化算法[J]. 系统工程与电子技术, 2021, 43(9): 2612-2619.
[7]	文者, 卞鸿巍, 马恒, 臧涛. 中低纬度下惯导极区性能模拟测试方法[J]. 系统工程与电子技术, 2021, 43(9): 2620-2627.
[8]	赵仁杰, 胡柏青, 吕旭, 田佳玉. 基于双欧拉角的UKF组合导航滤波算法[J]. 系统工程与电子技术, 2021, 43(7): 1912-1920.
[9]	蔡保杰, 邵雷. 三段判别域与最小二乘拟合的抗差滤波算法[J]. 系统工程与电子技术, 2021, 43(5): 1346-1353.
[10]	苏敬, 何华锋, 何耀民, 王依繁, 韩晓斐. 考虑SAR量测特性的弹载SINS/SAR组合导航滤波算法[J]. 系统工程与电子技术, 2021, 43(4): 1044-1049.
[11]	廖志忠, 王琪. 雷达导引头指向误差对导弹制导的影响与对策[J]. 系统工程与电子技术, 2021, 43(2): 519-525.
[12]	吕旭, 胡柏青, 戴永彬, 赵仁杰. 基于PSO的USQUE在组合导航姿态估计中的应用[J]. 系统工程与电子技术, 2020, 42(6): 1366-1371.
[13]	罗凯鑫, 吴美平, 范颖. 基于最大熵方法的鲁棒自适应滤波及其应用[J]. 系统工程与电子技术, 2020, 42(3): 667-673.
[14]	何康辉, 董朝阳. 基于正交变换的五阶容积卡尔曼滤波导航算法[J]. 系统工程与电子技术, 2020, 42(3): 680-685.
[15]	王晓龙, 刘海颖, 王景琪. 基于分层SLAM的空地多智能体协同导航[J]. 系统工程与电子技术, 2020, 42(1): 166-171.