本文提出并验证了一种基于因子图（factor graph）的水下自主航行器（AUV）近距操作（proximity operations）中同步轨迹估计与相对导航（simultaneous trajectory estimation and relative navigation, STERN）框架，旨在为不同类型的水下接近任务提供统一的建模与估计方法。研究由aldo terán espinoza、antonio terán espinoza、john folkesson、clemens deutsch、niklas rolleberg、peter sigray和jakob kuttenkeuler共同完成，作者分别来自瑞典皇家理工学院（kth royal institute of technology）和麻省理工学院（massachusetts institute of technology）。该研究得到了瑞典战略研究基金会（stiftelsen för strategisk forskning, ssf）通过瑞典海事机器人中心（swedish maritime robotics centre, smarc）的资助，论文发表于ieee journal of oceanic engineering，在线版本可于https://doi.org/10.1109/joe.2025.3624470获取。
研究的主要科学领域为水下机器人导航与状态估计。随着auv在海洋观测、水下设施检测和干预任务中的广泛应用，其在能源、续航和自主性方面的限制日益凸显。水下对接（underwater docking）被认为是提升auv续航与任务能力的关键手段之一。然而，传统对接研究多聚焦于静态目标或特定传感器配置，缺乏对不同任务场景进行统一描述和比较的通用框架。为此，作者借鉴航天器近距操作的概念，将水下场景中追逐者（chaser）与目标（target）之间的接近任务划分为四个阶段：远程阶段（remote phase）、远距离阶段（long distance phase）、终端阶段（terminal phase）和联合阶段（joint phase）。在此基础上，作者提出利用因子图对任意近距操作场景中的信息融合问题进行建模，以应对追逐者与目标之间的相对导航和轨迹估计需求。
所提出的STERN框架由两条并行变量节点链构成：一条表示追逐者（通常为auv）的状态历史，另一条表示目标的状态历史。两条链分别通过里程计因子或运动模型因子在时间上传播。当追逐者从远程阶段进入远距离阶段并开始获得相对测量时，两条链之间通过传感器观测因子（如声学超短基线，usbl因子）建立约束。进入终端阶段后，随着传感器精度的提高，可以在图中引入更多相对约束，并将目标的表示从三维点地标提升为六自由度位姿或包含速度的导航状态。该框架能够自然地表达从静态目标定位到动态目标同步估计的不同复杂度场景：静态目标可退化为单变量节点，而动态目标则需要完整的变量节点链和运动模型因子。
研究的具体实验场景为一个资源受约束的auv在远距离阶段通过声学相对测量追踪一艘动态移动的水面母船（mothership）。追逐者搭载低等级惯性导航设备，包括航姿参考系统（ahrs）、多普勒速度计（dvl）和压力传感器，并通过声学通信调制解调器接收母船转发的usbl相对位置测量。目标的动态模型假设为恒定速度与恒定航向，且深度保持恒定。在因子图模型中，追逐者状态由位姿和速度构成，目标状态为三维位置，同时引入速度大小v和航向角θ作为待估计的恒定参数。目标状态之间的约束由运动模型因子（motion model factor）实现，该因子利用v和θ预测相邻时刻的目标位置。声学测量经坐标变换后转化为距离与方位因子（range and bearing factor），并在二维流形上计算误差。惯性测量通过预积分imu因子（preintegrated imu factor）约束相邻追逐者位姿，深度与速度测量则以先验因子的形式加入图中。通过选择不同的窗口长度n对状态历史进行边缘化（marginalization），同一个因子图后端可以灵活实现滤波、固定滞后平滑（fixed-lag smoothing）或全平滑（full smoothing）估计。
在实现与验证方面，作者基于机器人操作系统（ros）和georgia tech smoothing and mapping（gtsam）库开发了完整的前端与后端系统。前端负责传感器数据预处理，包括基于随机采样一致性（random sampling consensus, ransac）的二维直线一致性检查，以剔除异常usbl测量。后端利用iSAM2增量求解器对因子图进行在线优化。系统初始化时，利用前n个usbl测量拟合出目标轨迹的直线代理，进而估计初始航向和速度，并为目标状态设置先验因子。
模拟实验使用stonefish模拟器设计了两种场景：垂直交叉轨迹和平行追赶轨迹。实验在不同usbl噪声水平（0至5级）和不同运动模型约束紧密度（0至4级）下进行了批量测试，共30种组合，每种组合分别采用滤波、窗口大小为10的固定滞后平滑、窗口大小为20的固定滞后平滑以及全平滑四种方法处理。结果显示，随着usbl噪声增大，估计精度整体下降；运动模型约束过松或过紧均会降低一致性，最佳结果通常出现在中等偏紧的约束水平。滤波方法由于能够分段更新恒定航向假设，对目标的振荡航向具有更好的跟踪能力，但全平滑方法在位置估计上表现更稳定。模拟结果验证了因子图框架的灵活性以及不同后端求解策略在同一图结构上的可替换性。
在实地实验中，追逐者为smarc自主研制的auv lolo，目标为装有顶部usbl单元和差分gnss航姿参考系统的服务船。实验在斯德哥尔摩附近的baggensfjärden海域进行，共完成七次部署并生成九个有效数据集。由于usbl系统缺乏可视化界面且服务船低速操控受限，实际目标轨迹偏离了恒定航向和速度的假设，呈现对抗性动态。结果表明，STERN估计器即使在实际目标动力学明显违反运动模型假设的情况下，仍能比基于航位推算（dead-reckoning）的原始usbl反投影估计更平滑且更接近gnss真值。具体而言，全平滑估计在位置误差上一致优于实时估计和航位推算估计；但在速度和航向方面，部分数据集中航位推算估计因能直接利用原始测量波动而表现出更小的平均误差，但总体标准差较大，一致性较差。异常值检测的二维限制、初始化阶段对噪声敏感、以及usbl噪声模型过于简化，是导致实时估计精度受限的主要原因。尽管如此，在所有九个数据集上，STERN方法对目标位置的估计均表现出较好的连续性和相对一致性。
在计算时间方面，处理全部九个数据集时，基于intel i7-10510u处理器的系统平均每关键帧处理时间为8.724毫秒，上限为53.726毫秒，而usbl测量速率为约0.5 hz，对应2000毫秒的帧间预算，证明了全平滑在线求解在计算上是可行的，且具有较大裕量。
本研究的主要价值在于：第一，提供了一种统一的因子图建模方法，能够涵盖从静态目标定位到动态目标同步估计的广泛水下近距操作场景；第二，通过在同一因子图上切换估计窗口长度，展示了滤波、固定滞后平滑与全平滑估计之间的无缝转换；第三，针对实际auv恢复任务中声学远距离归航问题，给出了从建模、实现到模拟与实地验证的完整路线图；第四，通过实验表明，即使在不完全符合恒定动力学假设的现实条件下，基于因子图的相对导航估计仍能提供平滑且较为一致的目标状态估计，为后续控制和对接阶段提供了可靠支持。未来工作可集中于三维空间中的异常值检测、更精细的usbl噪声建模、延迟补偿以及更灵活的目标动力学模型。