基于鲁棒自适应滤波器的在线平滑技术，用于减少USBL/SINS系统中的异常值和信号中断问题

《Ocean Engineering》：Online smoothing based on robust adaptive filter for mitigating outliers and signal outages in USBL/SINS system

定位精度提升19.8%的USBL/SINS集成导航系统鲁棒自适应算法研究。

　　在复杂的海洋环境中，自主水下航行器（AUV）的定位和导航任务面临着诸多挑战，如USBL测量中的异常值、时间变化的噪声统计特性以及测量中断等问题，这些因素通常会显著降低定位精度。为了解决这些问题，研究人员提出了一种新的鲁棒自适应卡尔曼滤波算法，结合了基于IGGIII的鲁棒估计方法与改进的Sage-Husa自适应噪声协方差估计器，从而在鲁棒性和自适应性方面取得了显著提升。此外，为了应对测量中断问题，还引入了一种高效的在线Rauch-Tung-Striebel（RTS）平滑策略，大大提高了信号中断期间的定位精度。同时，对新算法引入的额外计算复杂性、运行时间和内存开销进行了分析，证明其在AUV平台上的实际部署可行性。经过湖试和海试的验证，该鲁棒自适应在线平滑算法在真实海洋环境中实现了比传统卡尔曼滤波19.8%的定位精度提升。

自主水下航行器在海洋应用中扮演着不可或缺的角色，包括海底测绘、海洋资源勘探以及国防相关的任务，如目标检测、反潜作战和水下信息操作。AUV具备独立的动力系统、较大的航程和较长的续航能力，使其能够执行一些危险、繁琐、枯燥且具有挑战性的任务。高精度的导航状态，包括姿态、速度和位置，是AUV成功执行关键任务的基础。然而，水下环境的特殊性带来了独特的导航挑战，例如电磁波在水中的快速衰减，使得基于无线电的导航系统（如全球导航卫星系统GNSS）在水下应用中无效。此外，复杂的动态海洋环境使得长时间、高精度的定位变得极具挑战性。

Strapdown惯性导航系统（SINS）因其强抗干扰能力、自主运行、被动特性、连续输出和全面的导航信息，成为AUV水下定位系统的核心传感器。然而，SINS存在固有局限性，如初始对齐误差、传感器不完美以及死 reckoning 导航的误差累积特性，这些都会导致定位误差随时间持续增长，使得其无法独立维持长期的高精度定位。为了解决这些局限性，通常将SINS与互补传感器集成，以抑制或校正其误差累积。与电磁波相比，声波在水中的传播衰减显著降低，这一独特的物理特性使得水下声学定位成为AUV导航系统的重要使能技术。SINS与声学定位技术的集成已成为克服水下惯性导航基本局限性的标准方法。多普勒速度计（DVL）基于声纳多普勒效应测量车辆相对于海底或水层的三维速度，有效校正SINS中的累积速度误差。然而，DVL无法观测车辆的绝对位置，导致在长时间任务中，DVL/SINS集成系统中位置误差持续累积。在底部跟踪模式下，DVL的性能容易受到各种环境因素的影响，包括海洋生物、高吸收性的海底基质和复杂的地形，这些因素可能导致数据丢失。此外，当前的DVL系统通常有最大有效范围，深海操作中必须采用水下跟踪模式时，速度测量精度会显著下降，从而直接影响导航性能。

水下声学定位系统（UAPS）能够提供绝对位置测量，避免误差累积。基于水下水听器阵列的基线长度，UAPS可以分为三类：长基线（LBL）、短基线（SBL）和超短基线（USBL）系统。其中，LBL系统需要在海底部署多个声学应答器，提供最高的定位精度。SBL系统通过在水面船舶的不同位置安装应答器，实现了介于LBL和USBL之间的中等精度。USBL系统则将所有换能器集成在一个紧凑的单元中，安装在主机船舶上，大大简化了部署过程。尽管LBL系统具有优越的精度，但其实际应用受到复杂部署程序和有限覆盖区域的限制。SBL系统虽然提供中等精度，但其实施面临严格的船舶要求（包括多个穿透式安装）和需要干坞校准，这阻碍了其广泛应用。相比之下，USBL系统采用集成换能器阵列和声学信标，显著提高了操作的实用性。因此，USBL/SINS集成系统已成为AUV水下导航的主流解决方案。

然而，由于复杂的水下环境，USBL定位系统仍然面临若干技术挑战。主要问题包括声速变化和由水面及海底反射引起的多路径效应，这些因素通常会导致USBL测量中的俯仰角和方位角偏差，进而导致性能不稳定和偶尔出现异常测量。为了检测和处理这些异常测量，传统方法包括基于卡方检验（C2T）的异常识别，以及基于Huber估计器和IGGIII方案的鲁棒估计方法。针对USBL系统的异常测量，研究人员开发了多种鲁棒滤波解决方案，如基于Huber M-估计的鲁棒滤波方法、自适应神经模糊推理系统（ANFIS）、基于混合分布模型的鲁棒卡尔曼滤波、基于马哈拉诺比斯距离的鲁棒卡尔曼滤波，以及基于统计相似性度量（SSM）的多重自适应异常鲁棒状态估计器（MAORSE）。

此外，USBL系统的测量噪声具有时间变化的特性。使用固定测量噪声协方差矩阵可能会影响滤波器的收敛性，而与实际噪声条件的显著偏差甚至可能导致滤波器发散。因此，实时估计准确的测量噪声协方差至关重要。自适应Sage-Husa滤波器是一种经典的自适应方法，通过窗口平滑创新序列或残差序列来估计噪声统计特性。这些方法分别称为基于创新的自适应估计（IAE）和基于残差的自适应估计（RAE）。此外，基于混合分布模型的鲁棒卡尔曼滤波器已被广泛应用于大地测量领域，通过自适应因子调整预测状态误差协方差。对于USBL应用，近期的研究进展包括条件自适应增益扩展卡尔曼滤波器（CAEKF）和无偏自适应两阶段信息滤波器（ATSIF）。为了解决非高斯和重尾噪声，研究人员开发了基于最大相关熵准则（MCC）的变体，如自适应核带宽和Student’s t核，作为传统最小均方误差准则的鲁棒替代方案。

在典型操作中，USBL系统以固定间隔发送信号。然而，在深海环境中，声学链路的固有延迟以及复杂的洋流效应往往导致信号丢失或传输周期延长。当USBL经历信号中断且更新频率较低时，连续的测量丢失会导致USBL/SINS集成系统中误差迅速累积。针对测量中断问题，后处理平滑技术可以显著提高中断期间的定位精度，但目前尚缺乏可接受的实时解决方案。标准卡尔曼滤波器（KF）仅能以单向方式处理数据，其估计精度高度依赖于测量质量。在测量中断期间，KF处于预测模式，预测误差随时间迅速增长。现有研究表明，最优平滑算法理论上优于预测和滤波方法。作为一种后处理估计器，平滑算法可以利用更全面的观测数据，包括所有可用的过去、当前和未来测量值。因此，当后处理可行时，实施最优平滑算法是提高导航精度的推荐做法。

根据测量值选择和估计间隔的不同，平滑算法可分为三种类型：固定点平滑、固定间隔平滑和固定滞后平滑。在GNSS/INS集成系统中，需要对每个epoch进行平滑估计导航状态，因此通常采用固定间隔平滑。两种常见的固定间隔平滑算法是Rauch-Tung-Striebel平滑器（RTS）和前向-后向组合（FBC）。在RTS框架中，首先执行标准KF的前向处理，记录误差状态向量、误差状态转移矩阵和误差协方差矩阵。完成前向滤波后，再基于最大似然估计进行后向平滑。FBC也被称为双滤波平滑（TFS），它通过加权结合独立前向卡尔曼滤波（FKF）和后向卡尔曼滤波（BKF）的结果来实现。许多研究人员在GNSS/SINS集成系统中实施RTS和FBC平滑算法，用于城市环境下的陆地车辆导航，显著提高了导航参数在完全和部分GNSS中断期间的精度。类似的应用也扩展到了空中平台用于空中地球观测以及海洋系统，包括无人水面航行器（USV）和自主水下航行器（AUV）。然而，这些研究主要集中在后处理应用上，对RTS和FBC平滑算法的在线实现潜力探索有限。FBC方法在资源受限的导航系统中存在显著的计算挑战，需要存储原始观测值，并涉及计算密集的矩阵求逆操作。相比之下，RTS平滑方法在实时实现中具有显著优势，因为它仅需要存储FKF过程中的误差状态向量、误差状态转移矩阵和误差协方差矩阵，同时具备优化的计算过程。

最近，分段平滑技术的进展提高了处理效率，同时不牺牲精度，但这些方法在处理随机测量中断时仍存在不足。本文设计了一种高效的在线平滑架构，以便在状态预测过程中存储误差状态转移矩阵和预测误差状态协方差矩阵，而在测量更新过程中，同时存储估计的误差状态向量及其协方差矩阵，并激活在线RTS平滑器，从当前epoch向后处理至最近的测量更新epoch。此外，当某个epoch没有测量更新时，误差状态向量和协方差矩阵为零。完成当前周期的平滑处理后，平滑结果要么存储，要么输入到输出流中，同时动态数组准备存储下一周期的预测信息。这种在线平滑策略几乎与滤波过程并行运行，提供了更大的灵活性，同时减少了内存和计算资源需求，而保持了与后处理平滑方法相当的精度。通过结合新的鲁棒自适应卡尔曼滤波方法和在线RTS平滑算法，我们提出了基于鲁棒自适应平滑的USBL/SINS水下定位方法，其完整的计算流程如图5所示。

为了评估该算法在AUV平台上的实际部署可行性，我们分析了其引入的额外计算复杂性、运行时间和内存开销。算法的计算复杂性主要由矩阵运算决定。标准卡尔曼滤波器的测量更新步骤需要8次矩阵乘法和1次矩阵求逆。相比之下，所采用的RTS平滑器在完成前向滤波后，每个epoch在后向平滑过程中仅需额外的5次矩阵乘法和1次矩阵求逆。在实际应用中，每个操作步骤的执行时间取决于处理器配置、使用的编程语言和实现的优化程度。

我们使用C++编写程序，在配备Intel Core i9-13900HX 5.40 GHz CPU的系统上测试了代码执行时间。在1000个epoch中计算平均运行时间，得到以下结果：SINS机械实现：0.03 ms；卡尔曼滤波器时间更新：0.05 ms；卡尔曼滤波器测量更新：0.05 ms；完整的卡尔曼滤波周期：0.11 ms；完整的USBL/SINS集成导航解：0.22 ms；单个RTS平滑步骤：0.03 ms；与标准卡尔曼滤波器相比，RAKF的额外计算时间<0.01 ms（可忽略不计）；在线和离线RTS平滑所需的计算时间相同。因此，所提出的基于RAKF的在线RTS平滑算法引入的额外计算时间是可控的。每个epoch的总额外计算负载约为0.04 ms，相较于基础集成导航过程所需的0.22 ms，增加幅度非常小。

值得注意的是，我们的现场试验是在配备双核ARM Cortex-A72 @ 1.8 GHz处理器和8 GB RAM的AUV上进行的。尽管在该嵌入式平台上的绝对执行时间会更长，但我们的实验中USBL的更新频率为1 Hz，每个epoch提供1000 ms的预算。所提出的算法在每个epoch中仅增加0.04 ms的计算负载，相当于CPU时间的0.004%，这充分验证了该算法在目标平台上的实时计算开销可以忽略不计，因此完全具备实际部署的可行性。假设一个任务需要在1 Hz的更新频率下对1小时的测量数据进行平滑，那么在线RTS平滑算法需要0.03 × 3600 / 1000 = 0.108秒来完成。这表明在线平滑过程可以在下一个测量更新到来之前完成。

最后，我们分析了所提出算法带来的额外存储负载。RTS平滑器需要存储矩阵X^f,k+1、ΔX^f,k+1、P^f,k、P^f,k+1?和Φk,k+1。其中，矩阵X^f,k+1和P^f,k在平滑未进行时就已经存储。因此，需要额外存储的矩阵仅限于ΔX^f,k+1、P^f,k+1?和Φk,k+1。以一个15维状态向量为例，考虑双精度浮点数（float64）占用8字节的内存，每个epoch的额外内存消耗计算为(15 + 15×15 + 15×15) × 8 / 1024 ≈ 3.63 KB。因此，平滑一个1小时周期的总额外内存需求为3.63 × 3600 / 1024 ≈ 12.76 MB。鉴于海洋试验平台配备有1 TB的存储容量，由平滑过程引入的额外存储负载是完全可控的。

我们的理论分析和实际时间测试均表明，基于RAKF的在线RTS平滑算法引入的计算开销和存储需求微乎其微。与现代资源受限的AUV平台相比，这些额外成本可以忽略不计，因此充分证明了该算法的实际部署可行性。

为了进一步验证所提出算法在真实海洋环境中的有效性，我们于2024年1月28日在海南省三亚市亚龙湾附近的水域进行了USV和AUV的协同定位测试。USV和AUV的导航轨迹如图17所示，总任务持续时间为1.6小时。在测试区域附近设置了一个参考站，确保与移动站之间的基线长度在15公里以内。USV在水面按照割草机模式轨迹运行，而AUV最初在水面运行，随后在GPS秒25153时下潜，在GPS秒29063时再次上浮，执行水下割草机模式的移动。USV通过其USBL系统与AUV进行水下定位和通信。图18展示了AUV的3D轨迹，其中黑色点代表高精度参考位置（水面误差<1米，水下误差<5米），蓝色向上的三角形表示水面运行期间的GNSS位置，红色菱形表示USBL测量的位置。图19展示了USBL定位的误差序列，显示测量中存在一个显著的异常值。我们将其归因于AUV在海面附近运行，导致信标接近USBL阵列的运行范围极限。这种场景可能导致显著的声学定位偏差。在该数据集中，此类极端异常值仅发生一次。此外，USBL在东、北、上方向的RMS定位误差分别为14.72米、9.62米和15.44米。

为验证USBL/SINS集成的四种滤波方案，我们实施了常规卡尔曼滤波（KF）、所提出的RAKF、对KF结果进行在线平滑（KF-RTS）以及对RAKF结果进行在线平滑（RAKF-RTS）。图20展示了这四种方案的3D定位误差序列，图21比较了它们处理后的轨迹与高精度参考AUV位置。结果清晰表明，所提出的RAKF-RTS实现了最高的定位精度，其次是KF-RTS和RAKF，而常规KF表现最差。统计结果显示，AUV水下导航阶段的RMS定位误差分别为46.61米（KF）、31.59米（VBAKF）、30.30米（RAKF）和24.56米（RAKF-RTS）。与VBAKF相比，RAKF和RAKF-RTS在3D定位精度上分别提高了4.1%和22.25%。这些结果证实了所提出方法的优越性和先进性。

在湖试中，对于包含不匹配测量协方差和人工异常值的数据集4，所提出的RAKF相比KF提升了3D定位精度10.5%。对于每300秒进行一次位置校正的数据，RAKF-RTS将KF的最大3D定位误差从59.65米降至15.51米，提升了74%。在海试中，对于没有人工引入异常值且中断时间小于30秒的USBL数据，所提出的RAKF-RTS相比KF提升了19.8%的定位精度。对于包含随机中断（100秒至400秒）和人工引入的30米至60米异常值的USBL数据，RAKF相比VBAKF提升了4.1%的定位精度，而RAKF-RTS相比KF提升了47.3%。所提出的RAKF-RTS的主要优势在于其集成设计，能够同时处理测量异常和时间变化的噪声，这是VBAKF等方法所不具备的。

通过湖试和海试的统计结果，我们验证了所提出的鲁棒自适应在线平滑算法的有效性和技术先进性，证明其显著提升了定位精度。尽管复杂的海洋环境对实现长时间、高精度的AUV水下定位提出了挑战，但所提出的算法在减轻测量中断和异常值影响方面展现出良好的潜力，从而增强了系统的鲁棒性，并为AUV的水下操作提供了可靠的高精度定位支持。

综上所述，本研究探讨了一种新型的鲁棒自适应在线平滑算法，用于AUV水下定位的USBL/SINS集成导航系统。所提出的算法结合了基于IGGIII的鲁棒方法和改进的Sage-Husa自适应方法，以减轻测量异常和时间变化噪声对滤波性能的影响。此外，开发了一种高效的在线平滑方案，显著提高了测量中断期间的定位精度。通过湖试和海试验证了所提出算法的有效性，结果表明其在真实海洋环境中比传统卡尔曼滤波提升了19.8%的定位精度。尽管该算法设计用于实时操作，但目前的测试使用的是离线AUV数据。未来的工作将集中在将该算法集成到AUV的机载导航模块中，并进行现场水下测试，以进一步优化其满足实时导航需求的能力。