A Multi-UUV Cooperative Navigation and Formation Control Method Based on Robust FGO with Mixed Correntropy
-
摘要: 无人水下航行器(UUV)编队协同作业是海洋勘探与水下目标监视等任务执行的重要技术途径, 但水下环境中普遍存在的非高斯厚尾噪声、水声通信延迟与量测中断等问题严重制约着协同定位精度与编队控制稳定性。文中提出一种基于混合相关熵抗差因子图优化(FGO)的多UUV协同导航与编队控制方法, 用FGO滑动窗口批量处理机制, 引入双带宽高斯核凸组合构造混合相关熵代价函数、动态协方差缩放和自适应质量评估机制, 通过导航估计与编队控制的双向信息交互形成闭环协同。仿真结果表明, 该方法能有效提升多UUV编队在复杂水下环境下的定位精度与编队保持精度。Abstract: Cooperative operations of unmanned underwater vehicles (UUVs) are an important technical approach for ocean exploration and underwater target surveillance. However, non-Gaussian heavy-tailed noise, underwater acoustic communication delays, and measurement interruptions are common in underwater environments. These problems severely degrade cooperative localization accuracy and formation control stability. To address these issues, this paper proposes a multi-UUV cooperative navigation and formation control method based on robust factor graph optimization (FGO) with mixed correntropy. The method employs the sliding-window batch processing mechanism of FGO. A convex combination of dual-bandwidth Gaussian kernels is introduced to formulate a mixed correntropy cost function. Dynamic covariance scaling and adaptive quality assessment are further incorporated with the mixed correntropy cost function to form a three-layer robust framework, while marginalization is implemented within the sliding-window optimization. Through bidirectional information exchange between navigation estimation and formation control, closed-loop cooperation is established. Simulation results show that the proposed method effectively improves localization accuracy and formation-keeping accuracy for multi-UUV formations in complex underwater environments.
-
表 1 蒙特卡洛统计结果(N=200, 均值±95%置信半宽)
Table 1. Monte Carlo statistical results (N=200, mean ± 95% confidence half-width)
m 场景 指标 EKF UKF FGO-Std FGO-Huber FGO-MCC FGO-Robust 正常场景 定位RMSE 4.215±0.157 2.712±0.084 19.078±0.683 1.809±0.043 1.203±0.026 0.678±0.013 编队RMSE 5.103±0.178 3.280±0.090 19.821±0.711 1.984±0.043 1.331±0.026 0.743±0.013 通信延迟 定位RMSE 3.945±0.142 2.622±0.084 18.574±0.678 1.728±0.038 1.148±0.022 0.674±0.013 编队RMSE 4.783±0.161 3.171±0.089 19.296±0.706 1.899±0.038 1.272±0.022 0.739±0.013 量测中断 定位RMSE 3.193±0.124 2.275±0.087 13.801±0.614 1.469±0.035 0.965±0.019 0.635±0.013 编队RMSE 3.829±0.138 2.703±0.093 14.358±0.639 1.623±0.035 1.081±0.018 0.718±0.013 噪声异常 定位RMSE 24.739±3.865 17.871±3.636 28.860±1.096 3.822±0.155 4.550±0.238 0.694±0.011 编队RMSE 29.364±4.589 20.991±4.287 29.965±1.443 4.040±0.158 4.774±0.246 0.763±0.011 表 2 抗差机制消融实验结果(定位平面RMSE)
Table 2. Ablation experiment results of the robust mechanism (horizontal positioning RMSE)
m 配置 正常场景 通信延迟 量测中断 噪声异常 MCC 1.188 1.402 1.040 2.357 DCS 0.774 0.933 0.929 0.863 AW 15.479 12.127 18.299 18.585 MCC+DCS 0.610 0.782 0.752 0.677 MCC+DCS+AW 0.602 0.781 0.729 0.627 -
[1] 袁蕊霞. 基于因子图的多AUV协同定位算法研究[D]. 秦皇岛: 燕山大学, 2024: 1-4. [2] 栾正琨. 基于因子图的多UUV协同定位方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2024: 1-7. [3] 张继文, 徐博, 王储岩, 等. 基于神经动力学模型预测的多AUV编队自适应控制方案[J]. 中国舰船研究, 2025, 20(1): 326-339. doi: 10.19693/j.issn.1673-3185.04045Zhang J W, Xu B, Wang C Y, et al. A neural dynamics model prediction-based adaptive control system for AUV formation control[J]. Chinese Journal of Ship Research, 2025, 20(1): 326-339. doi: 10.19693/j.issn.1673-3185.04045 [4] Xu B, Zuo Y B, Wang Z Y, et al. Event-driven metrology-communication joint framework based on LMPC multi-AUV formation control[J]. Chinese Journal of Ship Research, 2026, 21(2): 89-100. [5] 宁君, 王二月, 李铁山, 等. 带有事件触发和输入量化的无人船轨迹跟踪控制[J]. 控制理论与应用, 2026, 43(3): 530-540. doi: 10.7641/CTA.2024.30488Ning J, Wang E Y, Li T S, et al. Unmanned surface vehicle trajectory tracking control with event-triggered and input quantization[J]. Control Theory & Applications, 2026, 43(3): 530-540. doi: 10.7641/CTA.2024.30488 [6] 宁君, 彭周华, 李铁山, 等. 带有输入量化的分布式多无人船舶自适应模糊编队控制[J]. 控制与决策, 2024, 39(8): 2588-2596. doi: 10.13195/j.kzyjc.2023.0007Ning J, Peng Z H, Li T S, et al. Fuzzy-adaptive distributed formation control of unmanned surface vehicles with input quantization[J]. Control and Decision, 2024, 39(8): 2588-2596. doi: 10.13195/j.kzyjc.2023.0007 [7] Wibisono A, Piran M J, Song H K, et al. A survey on unmanned underwater vehicles: Challenges, enabling technologies, and future research directions[J]. Sensors, 2023, 23(17): 7321. doi: 10.3390/s23177321 [8] 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 [9] Liu W, Pokharel P P, Principe J C. Correntropy: Properties and applications in non-Gaussian signal processing[J]. IEEE Transactions on Signal Processing, 2007, 55(11): 5286-5298. doi: 10.1109/TSP.2007.896065 [10] Li P, Sun X, Chen Z, et al. A robust and adaptive AUV integrated navigation algorithm based on a maximum correntropy criterion[J]. Electronics, 2024, 13(13): 2426. doi: 10.3390/electronics13132426 [11] Ma X S, Li C L, Liu X X. Research on multi-source information fusion based on sliding window-factor graph for autonomous underwater vehicle[J]. Transactions of the Institute of Measurement and Control, 2025, 47(14): 2930-2939. doi: 10.1177/01423312241274573 [12] 涂豫, 李国胜, 覃羡烘. 基于因子图结合卡方检测的多AUV协同定位方法[J]. 数据采集与处理, 2021, 36(5): 978-985. doi: 10.16337/j.1004-9037.2021.05.013Tu Y, Li G S, Qin X H. Multi-AUV cooperative localization method based on factor graph combined with Chi-Square detection[J]. Journal of Data Acquisition and Processing, 2021, 36(5): 978-985. doi: 10.16337/j.1004-9037.2021.05.013 [13] Agarwal P, Tipaldi G D, Spinello L, et al. Robust map optimization using dynamic covariance scaling[C]//2013 IEEE International Conference on Robotics and Automation (ICRA), 2013: 62-69. [14] 杨旭升, 夏晓翠, 金宇强, 等. 基于广义最大相关熵准则的几何滤波方法[J]. 自动化学报, 2025, 51(7): 1612-1625. doi: 10.16383/j.aas.c240497Yang X S, Xia X C, Jin Y Q, et al. Geometric filtering method based on generalized maximum correntropy criterion[J]. Acta Automatica Sinica, 2025, 51(7): 1612-1625. doi: 10.16383/j.aas.c240497 [15] Chen B, Wang X, Lu N, et al. Mixture correntropy for robust learning[J]. Pattern Recognition, 2018, 79: 318-327. doi: 10.1016/j.patcog.2018.02.010 [16] Dellaert F, Kaess M. Factor graphs for robot perception[J]. Foundations and Trends in Robotics, 2017, 6(1-2): 1-145. doi: 10.1561/9781680833270 [17] Kaess M, Johannsson H, Roberts R, et al. iSAM2: Incremental smoothing and mapping using the Bayes tree[J]. The International Journal of Robotics Research, 2012, 31(2): 216-235. doi: 10.1177/0278364911430419 -

下载: