Relative Navigation and Dynamic Target Tracking for Autonomous Underwater Proximity Operations

2025-08-23

AI Summary

In underwater close-range operations, sparse, noisy, and incomplete relative measurements—such as those from ultra-short baseline (USBL) systems—lead to inaccurate six-degree-of-freedom (6-DOF) target motion estimation. To address this, we propose a factor graph optimization framework formulated directly on the Lie group SE(3). Our key contribution is a generalized constant-twist motion prior defined in the tangent space, implemented via a closed-form Jacobian-computed ternary factor that intrinsically couples translation and rotation on the SE(3) manifold, enabling spatiotemporally consistent trajectory constraints across representations (SE(3) and 3D position). The framework natively supports heterogeneous sensor inputs, significantly enhancing pose estimation robustness. In real-world dynamic docking experiments using only USBL and optical relative measurements, our method achieves stable, high-precision relative trajectory estimation—substantially outperforming raw sensor observations in accuracy.

Technology Category

• Intelligent Robots: State Estimation
• Search and Optimization: Sampling/Simulation-based Search
• Computer Vision: Motion & Tracking

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Search and Retrieval-Augmented AI: Web learning to rank, online learning, and counterfactual learning for ranking
• Systems and Infrastructure for Web, Mobile and WoT: Experiences and lessons learnt from Web-based algorithms and system deployments

Abstract

Estimating a target's 6-DoF motion in underwater proximity operations is difficult because the chaser lacks target-side proprioception and the available relative observations are sparse, noisy, and often partial (e.g., Ultra-Short Baseline (USBL) positions). Without a motion prior, factor-graph maximum a posteriori estimation is underconstrained: consecutive target states are weakly linked and orientation can drift. We propose a generalized constant-twist motion prior defined on the tangent space of Lie groups that enforces temporally consistent trajectories across all degrees of freedom; in SE(3) it couples translation and rotation in the body frame. We present a ternary factor and derive its closed-form Jacobians based on standard Lie group operations, enabling drop-in use for trajectories on arbitrary Lie groups. We evaluate two deployment modes: (A) an SE(3)-only representation that regularizes orientation even when only position is measured, and (B) a mode with boundary factors that switches the target representation between SE(3) and 3D position while applying the same generalized constant-twist prior across representation changes. Validation on a real-world dynamic docking scenario dataset shows consistent ego-target trajectory estimation through USBL-only and optical relative measurement segments with an improved relative tracking accuracy compared to the noisy measurements to the target. Because the construction relies on standard Lie group primitives, it is portable across state manifolds and sensing modalities.

Problem

• Estimating 6-DoF motion in underwater proximity operations with sparse observations
• Addressing underconstrained factor-graph estimation without motion prior
• Maintaining temporal consistency across degrees of freedom during representation changes

Innovation

• Generalized constant-twist motion prior on Lie groups
• Ternary factor with closed-form Jacobians derivation
• Dual deployment modes with SE(3) representation switching