基于状态变换卡尔曼滤波的DVL/SINS组合导航算法

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

摘要/Abstract

摘要：

针对捷联惯性导航系统(strapdown inertial navigation system, SINS)误差定义存在的坐标系不一致的问题, 将速度误差统一投影在真实导航坐标系中, 基于状态变换Kalman滤波推导了更严密的SINS姿态、速度和位置误差方程。进一步地, 在新定义的速度误差和SINS误差方程的基础上, 构建了改进的多普勒计程仪(Doppler velocity logger, DVL)/SINS组合导航卡尔曼滤波状态模型和量测模型。仿真和船载实验表明, 与常规DVL/SINS卡尔曼滤波模型相比, 所提算法可以处理传统DVL/SINS误差模型中坐标系不一致问题的影响, 可在一定程度上提高组合导航系统的性能。

关键词: 捷联惯性导航系统, 多普勒计程仪, 组合导航, 误差方程, 卡尔曼滤波

Abstract:

The frame-inconsistent problem exists in conventional error equation of strapdown inertial navigation system (SINS). In this study, a more rigorous error equation of SINS attitude, velocity and position is derived by projecting the velocity error in the real navigation frame. Based on the newly defined velocity error, a modified Kalman filter model, including state model and measurement model, is constructed for Doppler velocity logger (DVL) /SINS integrated navigation. Compared with conventional Kalman model for DVL/SINS, simulations and field experiments indicate that the proposed algorithm is efficient in solving the frame-inconsistent problem in conventional DVL/SINS system, which can improve the navigation accuracy for the integrated navigation system.

Key words: strapdown inertial navigation system (SINS), Doppler velocity logger (DVL), integral navigation, error equation, Kalman filter

中图分类号:

TN967.2

靳凯迪, 柴洪洲, 宿楚涵, 向民志, 李明. 基于状态变换卡尔曼滤波的DVL/SINS组合导航算法[J]. 系统工程与电子技术, 2023, 45(11): 3624-3631.

Kaidi JIN, Hongzhou CHAI, Chuhan SU, Minzhi XIANG, Ming LI. State transformation Kalman filter for DVL/SINS integral navigation system[J]. Systems Engineering and Electronics, 2023, 45(11): 3624-3631.

图/表 15

图1

图2

表1

图3

图4

图5

图6

图7

图8

图9

图10

图11

图12

图13

表2

参考文献 30

1	PANDA J P , MITRA A , WARRIOR H V . A review on the hydrodynamic characteristics of autonomous underwater vehicles[J]. Proceedings of the Institution of Mechanical Engineers Part M-journal of Engineering for the Maritime Environment, 2021, 235 (1): 15- 29. doi: 10.1177/1475090220936896
2	FU Q W , LIU Y , LIU Z B , et al. Autonomous in-motion alignment for land vehicle strapdown inertial navigation system without the aid of external sensors[J]. Journal of Navigation, 2018, 71 (6): 1312- 1328. doi: 10.1017/S0373463318000437
3	LI W L , TANG K H , LU L Q , et al. Optimization-based INS in-motion alignment approach for underwater vehicles[J]. Optik, 2013, 124 (20): 4581- 4585. doi: 10.1016/j.ijleo.2013.01.069
4	BERNELTE P J , PAPAELIAS M , GARCIA F P . Autonomous underwater vehicles: instrumentation and measurements[J]. IEEE Instrumentation and Measurement Magazine, 2020, 23 (2): 105- 114. doi: 10.1109/MIM.2020.9062680
5	靳凯迪, 柴洪洲, 宿楚涵, 等. DVL/SINS组合导航技术发展现状及趋势[J]. 导航定位学报, 2022, 10 (2): 1-12, 92. doi: 10.16547/j.cnki.10-1096.20220201
	JIN K D , CHAI H Z , SU C H , et al. Research status and trends of DVL/SINS integrated navigation technology[J]. Journal of Navigation and Positioning, 2022, 10 (2): 1-12, 92. doi: 10.16547/j.cnki.10-1096.20220201
6	ZHANG T W , TANG J L , QIN S J , et al. Review of navigation and positioning of deep-sea manned submersibles[J]. Journal of Navigation, 2019, 72 (4): 1021- 1034. doi: 10.1017/S0373463319000080
7	RAHIMI H , NIKKHAH A , HOOSHMANDI K . A fast alignment of marine strapdown inertial navigation system based on adaptive unscented Kalman filter[J]. Trans.of the Institute of Measurement and Control, 2021, 43 (4): 749- 758. doi: 10.1177/0142331220934293
8	YOO T , KIM M , YOON S , et al. Performance enhancement for conventional tightly coupled INS/DVL navigation system using regeneration of partial DVL measurements[J]. Journal of Sensors, 2020, 5324349.
9	LIU P J , WANG B , DENG Z H , et al. INS/DVL/PS tightly coupled underwater navigation method with limited DVL measurements[J]. IEEE Sensors Journal, 2018, 18 (7): 2994- 3002. doi: 10.1109/JSEN.2018.2800165
10	LIU S D , ZHANG T , ZHANG J Y , et al. A new coupled method of SINS/DVL integrated navigation based on improved dual adaptive factors[J]. IEEE Trans.on Instrumentation and Measurement, 2021, 70, 8504211.
11	WANG D , XU X S , YAO Y Q , et al. A novel SINS/DVL tightly integrated navigation method for complex environment[J]. IEEE Trans.on Instrumentation and Measurement, 2020, 69 (7): 5183- 5196. doi: 10.1109/TIM.2019.2955187
12	JIN K D , CHAI H Z , SU C H , et al. A compensation algorithm with motion constraint in DVL/SINS tightly coupled positioning[J]. Marine Geodesy, 2022, 45 (4): 380- 406. doi: 10.1080/01490419.2022.2040662
13	ALIAN M , LUIS H , HICHEM S , et al. Model-aided navigation with sea current estimation for an autonomous underwater vehicle[J]. International Journal of Advanced Robotic Systems, 2015,
14	徐晓苏, 潘永飞, 邹海军. 基于自适应滤波的SINS/DVL组合导航系统[J]. 华中科技大学学报(自然科学版), 2015, 43 (3): 95-99, 106. doi: 10.13245/j.hust.150319
	XU X S , PAN Y F , ZOU H J . SINS/DVL integrated navigation system based on adaptive filtering[J]. Journal of Huazhong University of Science and Technology (Nature Science Edition), 2015, 43 (3): 95-99, 106. doi: 10.13245/j.hust.150319
15	SHI W C , XU J N , HE H Y , et al. Fault-tolerant SINS/HSB/DVL underwater integrated navigation system based on variational Bayesian robust adaptive Kalman filter and adaptive information sharing factor[J]. Measurement, 2022, 196 (23): 111225- 111234.
16	LI K L , CHANG L B , HU B Q , et al. A variational Bayesian-based unscented Kalman filter with both adaptivity and robustness[J]. IEEE Sensors Journal, 2016, 16 (18): 6966- 6976. doi: 10.1109/JSEN.2016.2591260
17	LI K L , LU X , LI W K , et al. Nonlinear error model based on quaternion for the INS: analysis and comparison[J]. IEEE Trans.on Vehicular Technology, 2021, 70 (1): 263- 272. doi: 10.1109/TVT.2020.3046680
18	ZHU T G , LIU Y , LI W K , et al. The quaternion-based attitude error for the nonlinear error model of the INS[J]. IEEE Sensors Journal, 2021, 21 (22): 25782- 25795. doi: 10.1109/JSEN.2021.3118039
19	ANDRLE M S , CRASSIDIS J L . Attitude estimation employing common frame error representations[J]. Journal of Gui-dance, Control, and Dynamics, 2015, 38 (9): 1614- 1624. doi: 10.2514/1.G001025
20	WANG M S , WU W Q , ZHOU P Y , et al. State transformation extended Kalman filter for GPS/SINS tightly coupled integration[J]. GPS Solutions, 2018, 22 (4): 112. doi: 10.1007/s10291-018-0773-3
21	WANG M S , WU W Q , HE X F , et al. Consistent ST-EKF for long distance land vehicle navigation based on SINS/OD integration[J]. IEEE Trans.on Vehicular Technology, 2019, 68 (11): 10525- 10534. doi: 10.1109/TVT.2019.2939679
22	WANG M S , ZHAO Q C , WU W Q , et al. State transformation multi-state constraint Kalman filter for visual-inertial integrated navigation[J]. Journal of Chinese Inertial Technology, 2021, 29 (2): 199- 207.
23	王茂松, 吴文启, 何晓峰, 等. 状态变换卡尔曼滤波的进一步解释及应用[J]. 中国惯性技术学报, 2019, 27 (4): 499-504, 509. doi: 10.13695/j.cnki.12-1222/o3.2019.04.013
	WANG M S , WU W Q , HE X F , et al. Further explanation and application of state transformation extended Kalman filter[J]. Journal of Chinese Inertial Technology, 2019, 27 (4): 499-504, 509. doi: 10.13695/j.cnki.12-1222/o3.2019.04.013
24	CHANG L B , QIN F J , JIANG S . Strapdown inertial navigation system initial alignment based on modified process model[J]. IEEE Sensors Journal, 2019, 19 (15): 6381- 6391. doi: 10.1109/JSEN.2019.2910213
25	赵仁杰, 李开龙, 胡柏青, 等. 基于改进四元数阻尼误差模型的SINS初始对准算法[J]. 系统工程与电子技术, 2021, 43 (11): 3330- 3337.
	ZHAO R J , LI K L , HU B Q , et al. SINS initial alignment algorithm based on improved quaternion damping error model[J]. Systems Engineering and Electronics, 2021, 43 (11): 3330- 3337.
26	KLEIN I , DIAMANT R . Observability analysis of heading aided INS for a maneuvering UUV[J]. Navigation, 2018, 65 (1): 73- 82. doi: 10.1002/navi.222
27	严恭敏, 邓瑀. 传统组合导航中的实用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.
28	陈建华, 朱海, 王超, 等. 水下SINS/DVL紧组合导航算法[J]. 海军工程大学学报, 2017, 29 (2): 108- 112.
	CHEN J H , ZHU H , WANG C , et al. Underwater SINS/DVL tightly integrated navigation algorithm[J]. Journal of Naval University of Engineering, 2017, 29 (2): 108- 112.
29	TAL A , KLEIN I , KATZ R . Inertial navigation system/Doppler velocity log (INS/DVL) fusion with partial DVL measurements[J]. Sensors, 2017, 17 (2): 415- 434. doi: 10.3390/s17020415
30	LI W L , ZHANG L D , SUN F P , et al. Alignment calibration of IMU and Doppler sensors for precision INS/DVL integrated navigation[J]. Optik, 2015, 126 (23): 3872- 3876. doi: 10.1016/j.ijleo.2015.07.187

运动状态	数值	数据区间/s
加速	1 m/s²	0-10、301-306、667-672
加速	-1 m/s²	286-291、652-657、795-800
匀速	10 m/s	10-163、216-286、306-376、429-529、582-652、672-742
匀速	5 m/s	291-301、657-667
横滚	0.5°/s	163-167、376-380、529-533、742-746
横滚	-0.5°/s	212-216、425-429、578-582、791-795
转向	2°/s	167-212、380-425、533-578、746-791

方案	失准角/(′)			速度误差/(m/s)			位置误差/m
方案	E	N	U	E	N	U	E	N	U
常规算法	0.025	0.015	0.823	0.037	0.038	0.010	6.525	4.454	3.795
本文算法	0.025	0.014	0.469	0.024	0.026	0.010	4.662	4.028	3.737

[1]	王希彬, 戴洪德, 全闻捷, 王瑞, 贾临生. 基于加速度补偿的惯性行人导航非零速区间姿态估计CKF算法[J]. 系统工程与电子技术, 2023, 45(9): 2894-2901.
[2]	戴洪德, 马宇峰, 戴邵武, 郑百东, 张笑宇. 航向误差非线性预测的惯性行人导航零速修正算法[J]. 系统工程与电子技术, 2023, 45(8): 2555-2561.
[3]	薛海建, 王涛, 蔡星会, 王金涛, 江英. 里程计辅助的车载SINS行进间对准方法[J]. 系统工程与电子技术, 2023, 45(6): 1805-1813.
[4]	戴礼灿, 刘欣, 张海瀛, 代翔, 王成刚. 基于卡尔曼滤波算法展开的飞行目标轨迹预测[J]. 系统工程与电子技术, 2023, 45(6): 1814-1820.
[5]	何益康, 张文瀚, 王振华, 何闻. 用于卫星应急恢复的太阳电池阵转角估计方法[J]. 系统工程与电子技术, 2023, 45(3): 797-805.
[6]	唐洪琼, 许江宁, 史文策, 何泓洋, 李方能. 基于位置观测信息的Davenport四元数DVL标定方法[J]. 系统工程与电子技术, 2023, 45(11): 3640-3648.
[7]	朱天高, 刘勇, 李开龙, 赵仁杰. 基于欧拉角的李群捷联惯导误差模型分析与比较研究[J]. 系统工程与电子技术, 2023, 45(10): 3265-3273.
[8]	高利春, 高铭阳, 陈晓芳, 饶建兵, 费泽松, 倪少杰. 基于SINS/GBAS组合导航的高精度进近着陆导航技术[J]. 系统工程与电子技术, 2023, 45(1): 210-220.
[9]	穆静, 严东升, 蔡远利, 王长元. 基于Masreliez-Martin的鲁棒分数阶容积卡尔曼滤波算法及应用[J]. 系统工程与电子技术, 2023, 45(1): 234-240.
[10]	姜颖颖, 潘树国, 叶飞, 高旺, 马春, 王浩. 基于抗差估计和改进AIME的缓变故障检测方法[J]. 系统工程与电子技术, 2022, 44(9): 2894-2902.
[11]	徐庚, 何永旭, 张勇刚. 基于罗德里格斯参数的惯性系传递对准算法[J]. 系统工程与电子技术, 2022, 44(9): 2903-2913.
[12]	史浩然, 卢发兴, 祁江鑫, 杨光. 基于辅助信标的无人机协同目标跟踪[J]. 系统工程与电子技术, 2022, 44(7): 2302-2310.
[13]	翟光, 王妍欣, 孙一勇. 基于低轨星网的多目标协同跟踪滤波技术[J]. 系统工程与电子技术, 2022, 44(6): 1957-1967.
[14]	董一平, 刘宁, 苏中, 王靖骁, 白宏阳. 基于AEKF的高速自旋飞行体组合导航方法[J]. 系统工程与电子技术, 2022, 44(6): 1977-1983.
[15]	李文华, 汪立新, 沈强, 李灿, 吴宗收. 基于鲁棒EKF的MEMS-INS/GNSS/VO组合导航方法[J]. 系统工程与电子技术, 2022, 44(6): 1994-2000.