一种适应性强的鲁棒因子图优化方法，结合延迟边缘化技术，用于集成惯性导航系统（INS）、垂直速度计（DVL）和定位系统（PS）的导航应用

《Ocean Engineering》：An adaptive and robust factor graph optimization with delayed marginalization for INS/DVL/PS integrated navigation

　　•AB-DM-FGO 共同改善了 DVL 加权和先验保护。•剪切 EMA 可以在导航框架中识别出可疑的 DVL 因素。•自适应 Barron 加权可连续调整 DVL 剩余衰减。•延迟边缘化可以阻止 DVL 异常污染先验。•海试显示，与 Huber-FGO 相比，RMSE 减少了 20%以上。

引言
自主水下航行器（AUV）由于其在科学、商业和军事领域的广泛应用，长期以来一直是研究的重点。然而，由于水下环境的复杂性，AUV 的导航仍然面临着重大挑战（Zhang 等，2023）。由于水下环境会导致电磁波的显著衰减，AUV 在潜入水中时无法依赖外部全球卫星导航系统。惯性导航系统（INS）因其完全自主性和能够在短时间间隔内提供高精度导航数据而成为水下定位的首选方法（Qin 等，2023）。然而，由于其工作原理，INS 的长时间独立运行会导致不可接受的累积误差（Tal 等，2017；Wang 等，2023）。因此，有必要结合多普勒速度计（DVL）系统为 AUV 提供速度信息，从而减轻 INS 误差的累积。此外，压力传感器通常用作 AUV 的深度测量工具，提供外部约束，进一步提高定位的可靠性（Liu 等，2018）。
目前，大多数涉及 INS/DVL 集成导航系统的研究和应用都基于滤波方法。卡尔曼滤波器（KF）及其衍生物（EKF、UKF）在假设线性高斯过程的背景下具有理论上的最优估计性能。Wang 等（2020）明确指出，在实际海洋环境中，温度和洋流等因素会导致传感器观测噪声表现出显著的重尾特性；传统 KF 的高斯假设会导致滤波精度严重下降甚至发散。Xia 等（2024）进一步指出，在复杂的水下声学定位系统中，严重的多路径效应和传播延迟会导致观测数据中频繁出现异常值，传统的固定参数模型无法有效捕捉这些时变干扰。为了提高系统在复杂环境中的鲁棒性，采用了变分贝叶斯方法来估计未知噪声（Huang 等，2021），并使用 M-估计来执行测量信息的加权重构（Sun 等，2023）。此外，Bao 等（2025b）提出了一种结合 Huber 和 Agarwal 核的方法，以平衡数值稳定性和鲁棒性。尽管这些鲁棒滤波器在一定程度上提高了定位精度，但其递归结构仍然限制了在新观测数据可用后对历史状态的重新调整。当 DVL 干扰持续多个连续周期时，由此产生的速度偏差可能会通过导航递归传播，并逐渐表现为累积的位置偏移（Wen 等，2019）。
在机器人技术领域，因子图优化（FGO）已被证明比基于滤波的方法更适合解决全局状态估计问题（Shan 等，2020；Dellaert，2021）。与基于马尔可夫假设的顺序递归不同，FGO 在每次迭代中对历史状态进行批量重新线性化；因此，它更适合纠正由当前异常观测引起的历史状态估计偏差，从而实现更准确的轨迹平滑。近年来，FGO 在水下集成导航中也展示了显著的潜力（Zhang 等，2022）。为了在优化精度和计算复杂性之间取得平衡，研究人员通常使用滑动窗口因子图优化（SW-FGO），通过边缘化旧状态节点来控制解决方案的规模（Cao 等，2022）。然而，边缘化操作也会改变测量误差对后续估计的影响方式。一旦状态节点离开活动窗口，与该节点相关的信息会被缩合并成为后续优化的先验因子（Boche 等，2025）。如果相应的 DVL 观测值包含由于底部锁定不稳定、海底反射变化或声学退化引起的短期严重误差，部分有偏信息可能会被吸收到边缘化的先验中。与仍然可以在当前窗口内重新加权的普通测量残差不同，这种受污染的先验可能会继续影响后续窗口并产生持续的位置偏移。本文将这种效应称为先验污染。
为了解决水下集成导航中的测量异常影响，最近的研究将异常检测、鲁棒剩余加权和学习辅助补偿引入了因子图优化。Dong 等（2024）引入了基于马氏距离的异常检测来识别异常观测值，而 Du 等（2024a）在边缘化之前加入了惯性导航迭代以纠正估计误差。基于学习的模型也被用来增强动态系统估计的适应性。例如，物理信息神经网络和硬约束学习模型在动态响应预测和物理约束状态建模方面表现出很强的能力（Qi 等，2025；Du 等，2026a；Du 等，2026b）。在水下导航中，Hou 等（2025）进一步将异常检测节点和 LSTM 预测模块嵌入到 INS/DVL/PS 因子图中。这个框架很有价值，因为 LSTM 模块可以利用历史导航信息来处理异常观测值并定期纠正位置误差。然而，其性能取决于代表性训练数据的可用性、训练后的网络结构以及训练场景与实际水下环境之间的一致性。此外，基于 LSTM 的补偿主要是提供数据驱动的预测因子，而可疑 DVL 相关的历史信息如何被压缩到边缘化先验中的机制仍没有得到明确保护。
鲁棒核设计是另一个重要的方向，用于在非高斯干扰下改进因子图优化。Huber 和 Cauchy 核已被广泛用于减少异常测量噪声下的大残差的影响（Wang 等，2025；Geng 等，2023），而 Barron 损失提供了一种通过形状参数统一多个经典鲁棒核的连续公式（Zhang 等，2024）。基于最大互信息准则（MCC）的方法也被引入到鲁棒因子图优化中，以抑制异常值和重尾测量噪声（Li 等，2023）。最近的基于互信息的 FGO 研究进一步改进了这一方向。自适应相关熵 FGO 结合了基于熵的剩余加权和滑动窗口优化，用于 INS/USBL/DVL 导航（Cheng 等，2026b），而深度学习增强的自适应相关熵 FGO 则利用时间特征提取来提高在复杂水下航行条件下的鲁棒加权的适应性（Cheng 等，2026a）。这些方法增强了测量级的鲁棒性并提高了剩余加权的适应性。尽管如此，它们大多数仍然主要作用于当前的测量残差。一旦可疑的 DVL 相关状态达到边缘化边界，异常信息仍可能被压缩成固定的先验因子并影响后续的优化窗口。
因此，尽管现有的具有异常检测功能、学习辅助和鲁棒核的 FGO 方法提高了检测异常观测值、抑制大残差或补偿退化测量的能力，但对于水下滑动窗口 FGO 来说，仍有三个重要的限制。首先，当 DVL 测量值从正常波动逐渐变为部分退化时，固定的鲁棒核可能无法平滑适应。其次，如 LSTM-FGO 这类学习辅助方法通常需要额外的训练数据和训练好的预测模块，它们对不同水下声学条件的泛化能力并不保证。第三，大多数现有方法关注当前残差或预测因子，而由于边缘化可疑 DVL 相关状态引起的先验污染效应尚未得到明确解决。这些观察结果促使提出了 AB-DM-FGO 框架，该框架结合了自适应 Barron 剩余加权和 DVL 异常感知的延迟边缘化，以实现测量级干扰抑制和先验级保护。
1. 在滑动窗口因子图优化中引入了 DVL 异常感知的延迟边缘化。通过使用剪切 EMA 方案监测导航框架中 DVL 速度的一致性，在异常 DVL 相关因素被吸收到边缘化先验之前识别出这些因素。然后选择性地将相应的历史状态保留在活动窗口中，从而降低短期 DVL 干扰成为持续先验约束的风险。
2. 开发了一种自适应 Barron 鲁棒加权方案，用于 DVL 因素。通过将 Barron 形状和尺度参数与估计的 DVL 异常严重程度联系起来，该方案根据测量可靠性连续调整 DVL 剩余的影响，从而在可靠条件下保留有用的速度约束，并在 DVL 退化时增强剩余衰减。
3. 通过在测量优化和先验构建之间共享 DVL 健康信息，建立了一个统一的 AB-DM-FGO 框架。测量级模块限制了异常 DVL 剩余对当前状态更新的影响，而先验级模块保护后续窗口免受可疑历史信息的影响。使用受控仿真、蒙特卡洛测试和真实 AUV 海试实验来验证所提出框架的有效性。

部分片段
坐标框架定义
根据惯性导航和水下集成导航中常用的标准导航框架和机体框架约定（Tal 等，2017；Liu 等，2018），在构建因子图模型之前阐明了所提出的 INS/DVL/PS 系统中采用的坐标框架。它们的物理定义在表 1 中进行了总结。

INS 预积分和测量因子
IMU 预积分通常用于基于因子图的导航中，以总结两个相邻关键状态之间的高频率惯性测量。

滑动窗口因子图优化中的边缘化
为了在长续航 AUV 导航中保持计算复杂度的可控性，滑动窗口优化仅保留活动图中的最新状态，并将旧状态边缘化为先验因子（Cao 等，2022）。设第 k 时代的活动状态集表示为 Wk=xk?Nk+1,xk?Nk+2,…,xk，其中 Nk 是当前窗口大小。在正常情况下，窗口大小保持在一个预定义的值 N0 左右。当状态数量超过 N0 时，最旧的状态被选为...

在非高斯 DVL 干扰下的鲁棒图优化
在前一节中，介绍了剪切 EMA 监测器和延迟边缘化策略，以防止可疑的 DVL 信息污染先验。这种机制主要在先验构建阶段起作用。然而，在可疑的 DVL 因素达到边缘化边界之前，它仍然参与当前窗口的优化。如果其残差受到二次成本的处理，则单个退化的 DVL 测量仍可能对其产生过大的影响。

实验验证
为了评估所提出的 AB-DM-FGO 方法，进行了一次仿真测试和两次真实 AUV 海试实验。受控仿真和蒙特卡洛测试旨在检验不同方法在短期 DVL 噪声干扰下的鲁棒性。在这些测试中，比较了标准 SW-FGO、Huber-FGO、MCC-FGO、Student’s-t-FGO、DM-FGO、AB-FGO 和 AB-DM-FGO，以评估固定鲁棒核基线、自适应 Barron 加权的独立效果和延迟...

结论
本文提出了一种适用于 AUV INS/DVL 集成导航的自适应鲁棒因子图优化框架，该框架具有延迟边缘化功能。该方法结合了自适应 Barron 鲁棒加权和边缘化控制，以便在测量优化期间抑制不可靠的 DVL 信息并防止其污染边缘化先验。通过仿真测试和两次真实海试实验对所提出的方法进行了评估。结果表明，AB-DM-FGO...

作者贡献声明
郭帅帅：撰写——原始草稿、可视化、验证、软件、方法学、正式分析、数据整理、概念化。
姚莉：撰写——审阅与编辑、监督、资金获取。
李品池：撰写——审阅与编辑、监督。
冯希阳：调查、正式分析。
胡晨宇：调查、数据整理。
王青：调查、数据整理。
朱乐鹏：验证。
何波：监督、项目管理、资金获取。

利益冲突声明
作者声明他们没有已知的可能会影响本文工作的竞争性财务利益或个人关系。

致谢
本研究由中国国家重点研发计划资助，项目编号为 2016YFC0301404。