Robust Adaptive Algorithm Based on SE(3) and Its Application in SINS/DVL

doi:10.16183/j.cnki.jsjtu.2022.513

Abstract

Abstract:

Aimed at the problem that SINS/DVL integrated navigation is vulnerable to interference in complex environment, a robust adaptive algorithm based on SE(3) is proposed. By introducing the Lie group/Lie algebra theory and the robust adaptive strategy into the orthogonal transformed cubature Kalman filter (TCKF), the estimated states of TCKF are made to incorporate the Special Euclidean group (SE(3)), and the state space inconsistency problem is improved. The anomaly measurement is adaptively reconstructed based on the innovation vector, by using the chi-square test and the Huber method. The experimental results of SINS/DVL show that the proposed method has a better spatial consistency and robustness than the traditional methods.

Key words: integrated navigation, Lie group/Lie algebra, Huber method, orthogonal transformed cubature Kalman filter (TCKF)

CLC Number:

TP13
U666.11

QIAN Leiyuan, QIN Fangjun, LI Kailong, ZHU Tiangao. Robust Adaptive Algorithm Based on SE(3) and Its Application in SINS/DVL[J]. Journal of Shanghai Jiao Tong University, 2024, 58(4): 498-510.

Figures/Tables 15

Fig.1

Tab.1

Fig.2

Fig.3

Fig.4

Fig.5

Fig.6

Fig.7

Tab.2

Tab.3

Tab.4

Tab.5

Tab.6

Fig.8

Tab.7

References 32

[1]	高红莲, 尤杰, 曹松银. 基于PF-UKF组合滤波的SINS/GPS组合导航系统空中对准方法[J]. 上海交通大学学报, 2022, 56(11): 1447-1452. doi: 10.16183/j.cnki.jsjtu.2022.167
	GAO Honglian, YOU Jie, CAO Songyin. In-flight alignment method of integrated SINS/GPS navigation system based on combined PF-UKF filter[J]. Journal of Shanghai Jiao Tong University, 2022, 56(11): 1447-1452.
[2]	ZHU J, LI A, QIN F, et al. A new robust adaptive filter aided by machine learning method for SINS/DVL integrated navigation system[J]. Sensors, 2022, 22(10): 3792-3813. doi: 10.3390/s22103792 URL
[3]	YAR Y, XU X, YANG D, et al. An IMM-UKF aided SINS/USBL calibration solution for underwater vehicles[J]. IEEE Transactions on Vehicular Technology, 2020, 69(4): 3740-3747. doi: 10.1109/TVT.25 URL
[4]	NORDIN M H, SHARMA S, KHAN A, et al. Collaborative unmanned vehicles for inspection, maintenance, and repairs of offshore wind turbines[J]. Drones, 2022, 6(6): 137-163. doi: 10.3390/drones6060137 URL
[5]	GAN W, ZHU D, JI D. QPSO-model predictive control-based approach to dynamic trajectory tracking control for unmanned underwater vehicles[J]. Ocean Engineering, 2018, 158: 208-220. doi: 10.1016/j.oceaneng.2018.03.078 URL
[6]	张文佳, 马辛. 深空探测器接近段自主导航的滑动窗口自适应滤波方法[J]. 上海交通大学学报, 2022, 56(11): 1461-1469. doi: 10.16183/j.cnki.jsjtu.2022.233
	ZHANG Wenjia, MA Xin. A sliding window adaptive filtering algorithm for autonomous navigation of the approach phase of deep space probe[J]. Journal of Shanghai Jiao Tong University, 2022, 56(11): 1461-1469.
[7]	姜俊豪, 陈刚. 驾驶机器人转向操纵的动态模型预测控制方法[J]. 上海交通大学学报, 2022, 56(5): 594-603. doi: 10.16183/j.cnki.jsjtu.2021.108
	JIANG Junhao, CHEN Gang. Dynamic model predictive control method for steering control of driving robot[J]. Journal of Shanghai Jiao Tong University, 2022, 56(5): 594-603.
[8]	ZHAO W, ZHAO H, LIU G, et al. ANFIS-EKF-based single-beacon localization algorithm for AUV[J]. Remote Sensing, 2022, 14(20): 5281-5301. doi: 10.3390/rs14205281 URL
[9]	AL B N, GAVRILOV A. A novel approach for aiding unscented Kalman filter for bridging GNSS outages in integrated navigation systems[J]. Navigation, 2021, 68(3): 521-539. doi: 10.1002/navi.v68.3 URL
[10]	YANG J, GAO S, LI G, et al. An adaptive cubature Kalman filtering algorithm based on variational mode decomposition for pulsar navigation[J]. IET Communications, 2022, 16(16): 1982-1992. doi: 10.1049/cmu2.v16.16 URL
[11]	秦康, 董新民, 陈勇, 等. 基于正交变换的改进CKF算法[J]. 控制与决策, 2018, 33(2): 330-336.
	QIN Kang, DONG Xinmin, CHEN Yong, et al. Modified CKF algorithm based on orthogonal transformation[J]. Control and Decision, 2018, 33(2): 330-336.
[12]	赵丽, 薛建平. 基于采样点正交变换的改进CKF算法[J]. 计算机工程与应用, 2018, 54(18): 45-51. doi: 10.3778/j.issn.1002-8331.1707-0344
	ZHAO Li, XUE Jianping. Improved CKF based on orthogonal transformation[J]. Computer Engineering and Applications, 2018, 54(18): 45-51. doi: 10.3778/j.issn.1002-8331.1707-0344
[13]	严恭敏, 严卫生, 徐德民. 基于欧拉平台误差角的SINS非线性误差模型研究[J]. 西北工业大学学报, 2009, 27(4): 511-516.
	YAN Gongmin, YAN Weisheng, XU Demin. A SINS nonlinear error model reflecting better characteristics of SINS errors[J]. Journal of Northwestern Polytechnical University, 2009, 27(4): 511-516.
[14]	王茂松, 吴文启, 何晓峰, 等. 状态变换卡尔曼滤波的进一步解释及应用[J]. 中国惯性技术学报, 2019, 27(4): 499-504.
	WANG Maosong, WU Wenqi, HE Xiaofeng, et al. Further explanation and application of state transformation extended Kalman filter[J]. Journal of Chinese Inertial Technology, 2019, 27(4): 499-504.
[15]	ISERLES A, MUNTHE K H Z, NORSETT S P, et al. Lie-group methods[J]. Acta Numerica, 2000, 9: 215-365. doi: 10.1017/S0962492900002154 URL
[16]	CUI J, WANG M, WU W, et al. Lie group based nonlinear state errors for MEMS-IMU/GNSS/magnetometer integrated navigation[J]. Journal of Navigation, 2021, 74(4): 887-900. doi: 10.1017/S037346332100014X URL
[17]	CHANG L, DI J, QIN F. Inertial based integration with transformed INS mechanization in earth frame[J]. IEEE/ASME Transactions on Mechatronics. 2021, 27: 1738-1749. doi: 10.1109/TMECH.2021.3090428 URL
[18]	QIAN L, QIN F, LI K, et al. Research on the necessity of lie group strapdown inertial integrated navigation error model based on Euler angle[J]. Sensors, 2022, 22(20): 7742-7761. doi: 10.3390/s22207742 URL
[19]	AXEL B, SILVERE B. Invariant Kalman filtering[J]. Annual Review of Control, Robotics, and Autonomous Systems, 2018, 1(1): 237-257. doi: 10.1146/control.2018.1.issue-1 URL
[20]	LUO Y, GUO C, LIU J. Equivariant filtering framework for inertial-integrated navigation[J]. Satellite Navigation. 2021, 2: 1-17. doi: 10.1186/s43020-020-00033-9
[21]	WANG M, WU W, ZHOU P, et al. State transformation extended Kalman filter GPS/SINS tightly coupled integration[J]. GPS Solution. 2018, 22: 1-12. doi: 10.1007/s10291-017-0674-x URL
[22]	DU S, HUANG Y, LIN B, et al. A lie group manifold-based nonlinear estimation algorithm and its application to low-accuracy SINS/GNSS integrated navigation[J]. IEEE Transactions on Instrumentation and Measurement. 2022, 71: 1-27.
[23]	ZHU T, LI A, LI K, et al. The quaternion based error model based on SE(3) of the INS[J]. IEEE Sensors Journal, 2022, 22(13): 13067-13077. doi: 10.1109/JSEN.2022.3174596 URL
[24]	FAN X, WANG G, HAN J, et al. Interacting multiple model based on maximum correntropy Kalman filter[J]. IEEE Transactions on Circuits and Systems II—Express Briefs, 2021, 68(8): 3017-3021.
[25]	ZHU B, CHANG L, XU J, et al. Huber-based adaptive unscented Kalman filter with non-Gaussian measurement noise[J]. Circuits, Systems, and Signal Processing, 2018, 37(9): 3842-3861. doi: 10.1007/s00034-017-0736-x
[26]	张文杰, 王世元, 冯亚丽, 等. 基于Huber的高阶容积卡尔曼跟踪算法[J]. 物理学报, 2016, 65(8): 358-366.
	ZHANG Wenjie, WANG Shiyuan, FENG Yali, et al. Huber-based high-degree cubature Kalman tracking algorithm[J]. Acta Physica Sinica, 2016, 65(8): 358-366.
[27]	黄玉, 武立华, 孙枫. 基于Huber M估计的鲁棒Cubature卡尔曼滤波算法[J]. 控制与决策, 2014, 29(3): 572-576.
	HUANG Yu, WU Lihua, SUN Feng. Robust Cubature Kalman filter based on Huber M estimator[J]. Control and Decision, 2014, 29(3): 572-576.
[28]	CANTELOBRE T, CHAHBAZIAN C, CROUX A, et al. A real-time unscented Kalman filter on manifolds for challenging AUV navigation[C]// 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems. Providence, Las Vegas, NV, USA: IEEE, 2020: 2309-2316.
[29]	LUO L, ZHANG Y, FANG T, et al. A new robust Kalman filter for SINS/DVL integrated navigation system.[J]. IEEE Access, 2019, 7: 51386-51395. doi: 10.1109/ACCESS.2019.2911110
[30]	朱天高, 刘勇, 李开龙, 等. 基于欧拉角的李群捷联惯导误差模型分析与比较研究[J]. 系统工程与电子技术, 2023, 45(10): 3265-3273. doi: 10.12305/j.issn.1001-506X.2023.10.31
	ZHU Tiangao, LIU Yong, LI Kailong, et al. Analysis and comparison of Euler angles based error model based on Lie groups of the SINS[J]. Systems Engineering and Electronics, 2023, 45(10): 3265-3273. doi: 10.12305/j.issn.1001-506X.2023.10.31
[31]	HUBER P J. Robust estimation of a location parameter[J]. The Annals of Mathematical Statistics, 1964, 35(1): 73-101. doi: 10.1214/aoms/1177703732 URL
[32]	HOU J, HE H, YANG Y, et al. A variational Bayesian and Huber-based robust square root cubature Kalman filter for lithium-ion battery state of charge estimation[J]. Energies, 2019, 12(9): 1717-1739. doi: 10.3390/en12091717 URL

传感器	误差项	误差值
IMU	加速度计漂移 (10^-6g)	≤50
IMU	陀螺仪漂移/ [(°)·h^-1]	≤0.02
DVL	位置误差/ m	≤10
DVL	速度误差/ (m·s^-1)	≤0.005

类别	姿态误差/(°)	速度误差/ (m·s^-1)	位置误差/m
SO(3)TCKF	0.439 8	0.270 1	148.147 7
LSE-TCKF	0.406 3	0.272 6	145.222 0
RSE-TCKF	0.489 8	0.270 5	148.556 2

类别	姿态误差/(°)	速度误差/ (m·s^-1)	位置误差/m
SO(3)TCKF	5.721 6	0.282 3	168.388 0
LSE-TCKF	2.886 4	0.280 0	130.768 1
RSE-TCKF	3.762 9	0.291 6	112.626 6

类别	姿态误差/(°)	速度误差/ (m·s^-1)	位置误差/m
SO(3)TCKF	13.796 0	0.825 2	267.466 8
LSE-TCKF	4.441 3	0.294 8	124.607 7
RSE-TCKF	8.349 5	0.461 9	83.175 3

类别	姿态误差/(°)	速度误差/ (m·s^-1)	位置误差/m
SO(3)TCKF	28.539 2	2.945 7	530.985 0
LSE-TCKF	8.267 5	0.368 1	111.966 1
RSE-TCKF	18.539 6	1.976 3	291.014 9