基于侧扫声纳的地标辅助导航方法：一种应对测量起源不确定性的贝叶斯估计框架

《IEEE Journal of Oceanic Engineering》：A Landmark-Aided Navigation Approach Using Side-Scan Sonar

本文针对自主水下航行器在GPS拒止环境中的定位难题，提出了一种创新的地标辅助导航方法。该方法结合侧扫声纳（SSS）测量与贝叶斯估计框架，通过概率数据关联（PDA）处理测量起源不确定性（MOU），利用无迹变换预测和粒子滤波更新，有效约束了导航误差。仿真和实地实验（采用Hydrone水面平台和Iver3水下航行器）表明，该方法能显著提高定位精度，为低成本海洋监测提供了可行的技术路径。

　　

在广阔的海洋中，精确导航一直是自主水下航行器（AUV）面临的核心挑战。由于GPS信号在水下无法传播，AUV通常依赖惯性测量单元或多普勒速度日志（DVL）等进行航位推算（DR）。然而，这些方法的定位误差会随时间累积，且不受限制地增长。虽然通过浮出水面接收GPS信号或部署水声应答器阵列（如长基线和超短基线定位系统）可以校正误差，但前者会减少有效作业时间，后者则成本高昂且限制了作业范围。因此，开发能够利用低成本传感器实现长时间、大范围精确水下导航的技术，对于海洋科学考察、资源勘探和环境监测等领域具有至关重要的意义。

传统的导航方式存在明显局限，而利用海底已知地标进行辅助导航则展现出了巨大潜力。侧扫声纳（SSS）作为一种常见的水下成像传感器，能够生成高分辨率的海底图像，清晰地显示海底地貌和物体，使其非常适合用于识别和利用海底地标。然而，将SSS数据用于导航面临两大挑战：一是声纳测量值与地标位置之间存在高度非线性关系；二是在复杂的海底环境中，声纳探测到的目标往往存在不确定性，即无法确定一个探测信号究竟来源于哪个已知地标（测量起源不确定性，MOU），或者是否是虚假探测（杂波）。这些因素使得开发稳健、实时的地标辅助导航算法变得异常困难。

为了解决上述问题，发表在《IEEE Journal of Oceanic Engineering》上的这项研究，提出了一种新颖的地标辅助导航方法。该方法的核心在于将SSS的每一次声脉冲（称为一个“Ping”）视为独立的测量数据，而非将多个Ping拼接成的完整图像进行处理，这简化了测量模型并提高了滤波器的效率。研究人员设计了一个基于贝叶斯估计的滤波框架，该框架巧妙地处理了MOU问题。滤波过程分为两步：预测步使用无迹变换（UT）来传播车辆状态的不确定性；更新步则采用粒子滤波（PF），并融入了概率数据关联（PDA）来处理地标探测与已知地标之间的关联不确定性。这种方法能够稳健地应对漏检和误检的情况。

为开展研究，团队运用了几个关键技术方法：首先，建立了包含车辆位置、航向和高度等状态的非线性运动模型，以及描述SSS单个Ping如何探测地标边缘斜距的测量模型。其次，引入了概率数据关联（PDA）框架，通过关联向量建模测量与地标的对应关系，并利用置信度传播（Belief Propagation）近似来降低计算复杂度。此外，还采用了选通（Gating）技术来筛选当前车辆位置附近可能被观测到的地标，进一步提升计算效率。研究数据来源包括模拟数据和实地采集数据，后者使用Hydrone水面机器人和Iver3水下机器人两个不同平台，在Mission Bay和La Jolla两个不同地点，搭载两种不同的SSS传感器收集。

运动模型和通用测量模型

研究人员建立了自主水下航行器（sAUV）的运动模型，其状态包括2D平面位置（x_k, y_k）、航向角θ_k和离地高度γ_k。控制输入为速度u_s,k和转向率u_t,k，并考虑了相应的驱动噪声。状态转移模型g(x_k-1, n_k, u_k)描述了状态随时间的演化。对于SSS测量，研究假设地标位置全局已知，并将每个地标建模为一个矩形。当SSS的Ping扫描线与地标矩形相交时，可能产生探测。测量模型h(x_k, m_d, n_k,d)预测了地标近边和远边相对于航行器的斜距，并附加了高斯噪声n_k,d。此外，测量向量还包含了带有噪声的航向和高度测量。

max显示了SSS能观测到的最远点。当地标存在时，其相应的斜距r_s可以从SSS图像中提取。这个斜距对应一个水平距离r_h。车辆离海底的高度γ是三角形的第三边，需要从r_s得到r_h。'>

SSS测量的MOU模型

测量起源不确定性（MOU）是地标辅助导航的关键挑战。研究采用联合概率数据关联（JPDA）的思想，引入关联随机向量a_k来表示每个探测可能来源于哪个地标（或是否为杂波）。模型假设地标探测是独立的伯努利试验（探测概率为P_det），杂波数量服从泊松分布，且每个地标最多产生一个探测。基于这些假设，推导了关联向量和探测数量的联合概率质量函数（PMF），以及给定关联向量后测量向量的似然函数。为了应对对所有可能关联情况进行求和所带来的计算复杂度爆炸问题，研究采用了基于置信度传播（Belief Propagation）的近似方法，将复杂的联合似然近似为每个地标关联可能性的乘积之和，从而将计算复杂度从指数级降低到与地标数量D和探测数量L_k的乘积成正比。

导航滤波器

导航滤波器旨在估计航行器的状态后验概率密度函数（PDF）。滤波器采用预测-更新的框架。预测步使用无迹变换（UT）来处理非线性运动模型：通过采样 sigma 点，将其通过状态转移模型传播，然后计算预测状态的均值和协方差。更新步则采用粒子滤波（PF）来处理高度非线性的测量似然（包含数据关联）：从预测PDF中抽取大量粒子，然后根据近似后的联合似然函数（公式15）计算每个粒子的权重，权重计算中包含了PDA对MOU的处理。最后根据加权粒子集计算更新后的状态估计和协方差矩阵。为了进一步控制计算成本，在更新步中引入了选通（Gating）技术，仅考虑落在以预测状态为中心的一定区域（验证区域）内的地标，这使得更新步的计算复杂度仅与选通区域内的地标数量D_k有关。

仿真、数据采集和处理

研究通过仿真验证算法性能，仿真模型根据测量模型生成含噪声的SSS探测和杂波。此外，还在两个不同地点使用两个不同的海洋机器人平台进行了实地实验。一是使用集成Ark Scout Mk II SSS的Hydrone水面机器人在Mission Bay（站点1）进行实验，该地点存在显著的表层流和波浪干扰。二是使用搭载EdgeTech SSS的Iver3水下机器人在Scripps海洋研究所（站点2）进行实验，环境相对平静。实验中使用由高密度聚乙烯（HDPE）制成的人工地标。地标通过手动分析SSS图像进行识别。

结果

仿真结果表明，当地标被观测到的概率较高时（例如10%），所提出的导航滤波器能够有效限制定位误差，使其保持有界；而仅依赖航位推算（DR）的误差则会持续增长。当地标观测概率降低至1%时，误差仍能保持有界，但性能有所下降；当概率降至0.1%时，误差则呈现无界增长趋势。这凸显了地标观测频率对算法性能的重要性。

实地实验结果与仿真趋势一致。在动态性较强的Mission Bay环境（Hydrone平台）中，DR方法由于表层流影响误差迅速增大（7分钟内达25米），而采用SSS地标辅助导航的方法则将误差限制在约1.8米左右。在相对平静的La Jolla环境（Iver3平台）中，DR误差增长较慢（11分钟内约7米），但SSS地标辅助导航同样表现出色，将误差限制在约1.7米，证明了其在低噪声环境下的有效性。累积频率（CF）图进一步表明，使用SSS地标时，定位误差在任何时刻基本被限制在5米以下，而DR误差则随时间推移不断增大。

计算需求

算法计算时间分析表明，更新步的计算时间与选通地标数量近似呈线性关系，而非最坏情况下的二次关系。在配备Apple M3 MAX处理器的计算机上，处理一次Iver3实验（30Hz声纳，13000个时间步）仅需约30秒，表明该方法比声纳更新速率快一个数量级，具备实时应用的潜力，但这依赖于强大的车载处理器。

结论与未来工作

本研究成功开发并验证了一种用于小型自主水下航行器（sAUV）的侧扫声纳（SSS）地标辅助导航方法。该方法通过使用单个SSS Ping作为测量值，并结合贝叶斯估计框架和概率数据关联（PDA）来处理测量起源不确定性（MOU），有效约束了导航误差。仿真和实地实验均证明，该方法能显著提高定位精度，尤其在动态海洋环境中优势明显。算法计算效率较高，具备实时应用前景。

未来研究工作将集中在几个方面：首先，实现通过嵌入式深度神经网络进行地标的自动检测，以替代当前耗时的手动或离线处理。其次，探索利用SSS图像中的纳迪尔（Nadir）区域信息来估计航行器离地高度。第三，研究在长时间未观测到地标后，如何根据新的地标探测重新初始化或调整车辆状态的提议概率密度函数（PDF），这可能需要对地标进行分类或标记。最终，目标是将此导航系统扩展至未知环境，通过同步定位与地图构建（SLAM）方法实时构建地标地图，并考虑动态地标的处理。这些进展将共同推动低成本、长航时自主水下航行器导航技术的发展。