Reliability-Adaptive Multimodal Relative Pose Tracking for Underwater Leader-Follower Robots
Authors: Mingyang Yang, Zeyu Sha, Xiaorui Wang, Jun Wang, David Chen Wang, Feitian Zhang
Organizations: Department of Robotics and Manufacturing Engineering, College of Advanced Manufacturing and Robotics, and the State Key Laboratory of Turbulence and Complex Systems, Peking University, Beijing, 100871, China
Underwater leader-follower formation control requires continuous relative-pose estimation despite degraded sensing conditions and limited onboard computation. This paper presents a reliability-adaptive multimodal perception framework for six-degree-of-freedom relative-pose tracking between two moving underwater robots. The framework integrates complementary visual, acoustic-ranging, and hydrodynamic measurements while accounting for their distinct physical characteristics and time-varying reliability. It adaptively weights modality contributions and models temporal dependencies with linear computational complexity, enabling efficient pose estimation on embedded hardware. Extensive experiments were conducted using a custom-built AUV platform, with 648 pool trials spanning two leader vehicle types, three illumination conditions, and nine trajectory patterns. Evaluation yields an average translational error of 3.8 cm and an average orientation error of 2.4°. On an onboard NVIDIA Jetson AGX Orin, the fusion network achieves an inference latency of 17.62 ms, providing a 2.69-fold speedup over the evaluated Transformer-based fusion baseline. Closed-loop experiments further demonstrate stable formation maintenance with a moving leader under onboard computational constraints. A multimodal dataset containing measurements from all 648 trials will be made publicly available.
Figures & tables
Fig. 1: Schematic of the proposed multimodal fusion framework for underwater leader-follower formation control. The follower AUV obtains complementary information about the leader through visual observations, active ultrasonic ranging measurements, and hydrodynamic wake measurements. These measurements are acquired using a dedicated sensor suite comprising a monocular camera, ultrasonic sensors, and a pressure sensor array, respectively. The fusion framework processes these inputs to estimate the leader’s relative pose and support real-time formation control.
Fig. 2: Overview of the reliability-adaptive multimodal perception and control loop for the leader-follower system. The system combines information-bottleneck visual encoding via IB-VE, physics-aware acoustic temporal encoding via PA-GATE, and hydrodynamic wake encoding with distributional regularization via KLD-WE to estimate the leader’s relative pose. The modality-specific encoders incorporate relevant physical and statistical structure, while the fusion module adjusts modality contributions according to their estimated reliability. The fused relative-pose estimate is provided to a sliding mode controller, supporting real-time formation control on resource-constrained AUV hardware.
Fig. 3: Hardware configuration of the follower AUV platform. The top view and side view of the follower OpenAUV are shown in (a) and (b), respectively. The follower OpenAUV consists of a rear main body and a forward sensing module, connected by two carbon-fiber plates. The main body is based on the same platform design as the leader OpenAUV, with minor modifications to its mechanical configuration. The front view of the sensing module in (c) shows the three sensing modalities. A monocular camera, three acoustic ranging sensors, and eight pressure sensors are outlined by green, blue, and orange contours, respectively.
Fig. 4: Experimental pool and underwater motion-capture system. The system comprises eight high-resolution underwater optical cameras, with four positioned along each long side of the pool.
Fig. 5: Overview of the proposed perception framework. The perception framework incorporates three encoder modules, one adaptive fusion module and one regression head. The encoder, fusion, and regression stages are indicated by blue, orange, and green backgrounds, respectively. A YOLO-based front end first detects and segments the leader, from which a task-specific five-channel feature map is constructed. The Information Bottleneck Vision Encoder (IB-VE) then extracts a compact visual representation. The Physics-Aware Gated Acoustic Temporal Encoder (PA-GATE) processes static sensor-geometry descriptors and time sequences of acoustic range measurements to obtain the acoustic representation. The KL-Divergence Wake Encoder (KLD-WE) encodes pressure sequences acquired by the distributed artificial lateral line using lightweight convolutional and BiLSTM layers to obtain the hydrodynamic representation. The features from the three sensing modalities are adaptively fused by the Adaptive Cross-Modal Fusion Module, which combines a Mamba-based selective state-space model with the Adaptive Weight Generator (AWG). The prediction head maps the fused representation to the leader’s 6-DoF pose relative to the follower.
Stage
Layer
Output Size
Operator
#Out
Input
Input Map
128 × 128
YOLO-SEG
5
CoordConv
128 × 128
Concat (x,y)
7
Encoder
Conv 1
64 × 64
Conv(3,3)
16
Conv 2
32 × 32
Conv(3,3)
32
Conv 3
16 × 16
Conv(3,3)
64
Conv 4
8 × 8
Conv(3,3)
128
TABLE I: Architecture of IB-VE
Stage
Output Size
Operator
#Out
1
64 × 1
Conv(3,1)
128
64 × 1
Conv(3,1)
128
2
64 × 1
Conv(3,1)
64
64 × 1
Conv(3,1)
64
3
1 × 1
BiLSTM(128,3)
256
4
1 × 1
FC
64
TABLE II: Architecture of KLD-WE
Fig. 6: Illustrative images of illumination conditions. Fig. 6(a) , Fig. 6(b) , and Fig. 6(c) illustrate the moderate, dim, and extremely dim light conditions during data collection, respectively. The illumination lights are located above the water surface; the blue lights visible in the images originate from the MoCap system cameras.
Variable
Description
Leader Vehicle Type
1. OpenAUV
2. BlueROV
Illumination Condition
1. Moderate Light
2. Dim Light
3. Extremely Dim Light
Leader Trajectory
1. Move Forward
TABLE III: Dataset Collection Settings
Fig. 7: Training sample statistics. (a) Instance counts show a balanced class distribution. (b) Overlapping bounding boxes. (c) Bounding box size distribution relative to image dimensions. (d) Bounding box center position distribution relative to the image. The relative bounding box size distribution corresponds to the physical sizes of the OpenAUV and BlueROV, while the relative center positions indicate where the target typically appears within the camera’s field of view.
Train
Box
Mask
Class
Distribution Focal
Loss
0.7216
1.3525
1.0641
0.9653
Validation
Box
Mask
Class
Distribution Focal
Loss
0.6298
1.1745
1.0854
0.8908
Test
Precision
Recall
mAP50
mAP50-95
(Box)
0.9984
1.0000
0.9950
0.9488
Test
Precision
Recall
mAP50
mAP50-95
TABLE IV: Stage One Model Evaluation
Fig. 8: Constructed feature maps. (a) Sample image of the leader OpenAUV. (b) Five-channel feature map constructed from the first-stage results. The top row shows the weighted RGB, the rough segmentation mask, and the global position along the x -axis feature maps. The bottom row shows the global position along the y -axis, the confidence feature map, and the normalized intensity bar for the last three feature maps.
Illumination
Δx
Δy
Δz
Δϕ
Δθ
Δψ
MAE(m)
MAPE(%)
MAE(m)
MAPE(%)
MAE(m)
MAPE(%)
MAE( ∘ )
MAPE(%)
MAE( ∘ )
MAPE(%)
MAE( ∘ )
MAPE(%)
Moderate
0.057
3.8
0.038
3.8
0.013
3.3
0.537
5.4
0.779
3.9
5.119
4.3
Dim
0.060
4.0
0.042
4.2
0.011
2.8
0.503
5.0
0.837
4.2
5.786
4.8
Extremely Dim
0.067
4.5
0.044
4.4
0.014
3.5
0.652
6.5
1.153
5.8
6.338
5.3
TABLE V: Accuracy under Different Illumination (MAE and MAPE)
Illumination
Δx (m)
Δy (m)
Δz (m)
Δϕ ( ∘ )
Δθ ( ∘ )
Δψ ( ∘ )
Moderate
0.044
0.032
0.006
0.414
0.607
4.738
Dim
0.051
0.025
0.008
0.457
0.637
3.082
Extremely Dim
0.101
0.033
0.015
0.582
1.071
5.615
TABLE VI: Robustness under Different Illumination (SD)
Structure
Δx
Δy
Δz
Δϕ
Δθ
Δψ
MAE(m)
MAPE(%)
MAE(m)
MAPE(%)
MAE(m)
MAPE(%)
MAE( ∘ )
MAPE(%)
MAE( ∘ )
MAPE(%)
MAE( ∘ )
MAPE(%)
w/o PA-GATE
0.074
4.9
0.044
4.4
0.013
3.3
0.567
5.7
0.987
4.9
5.895
4.9
w/o KLD-WE
0.075
5.0
0.047
4.7
0.015
3.8
0.608
6.1
1.013
5.5
6.638
5.5
Complete
0.068
4.5
0.040
4.0
0.012
3.0
0.531
5.3
0.887
4.4
5.613
4.7
TABLE VII: Accuracy in the Ablation Study (MAE and MAPE)
Structure
Δx (m)
Δy (m)
Δz (m)
Δϕ ( ∘ )
Δθ ( ∘ )
Δψ ( ∘ )
w/o PA-GATE
0.111
0.066
0.018
0.769
1.441
9.877
w/o KLD-WE
0.109
0.071
0.022
0.840
1.477
10.194
Baseline 1
0.116
0.078
0.022
0.909
1.745
11.855
Baseline 2
0.102
0.060
0.021
0.790
1.383
8.930
Ours
0.097
0.057
0.018
0.743
1.340
9.108
TABLE VIII: Robustness in the Comparison and Ablation Studies (SD)
Structure
Δx
Δy
Δz
Δϕ
Δθ
Δψ
MAE(m)
MAPE(%)
MAE(m)
MAPE(%)
MAE(m)
MAPE(%)
MAE( ∘ )
MAPE(%)
MAE( ∘ )
MAPE(%)
MAE( ∘ )
MAPE(%)
Baseline 1
0.086
5.7
0.059
5.9
0.016
4.0
0.683
6.8
1.209
6.0
8.250
6.9
Baseline 2
0.069
4.6
0.045
4.5
0.014
3.5
0.581
5.8
0.935
4.7
6.041
5.0
Ours
0.068
4.5
0.040
4.0
0.012
3.0
0.531
5.3
0.887
4.4
5.613
4.7
TABLE IX: Accuracy in the Comparison Study (MAE and MAPE)
Latency / Information
Baseline 1
Baseline 2
Ours
Latency on RTX 4090 (ms)
0.69
1.44
1.38
Latency on Jetson AGX Orin (ms)
6.54
47.48
17.62
Params (M)
0.45
1.67
1.07
FLOPs (M)
519.17
521.59
520.57
TABLE X: Inference Latency and Model Information
Fig. 9: Follower trajectory in the leader-attached frame during the same segment. The leader is fixed at the origin; the star marks the desired relative pose (2.10,0,0)m . The follower remains clustered around the desired pose throughout the maneuver.
Fig. 10: Closed-loop relative-pose and attitude tracking over a representative 30s segment with the leader in motion. Left column: relative position xrel , yrel , zrel and relative distance ∥prel∥ (solid) against their references (dashed). Right column: relative roll, pitch, yaw, and the leader speed. All measured quantities were obtained from the motion capture system.
Reliable relative pose estimation between autonomous underwater vehicles (AUVs) is critical for cooperative ocean exploration, sampling, and multi-robot coordination. However, achieving robust vision-based relative localization in underwater environments remains challenging due to severe optical degradation, including turbidity, illumination variations, reflections, and intermittent feature occlusions. This paper presents AMR-Pose, an active LED marker-based relative pose estimation framework for cooperative AUVs. A compact marker module consisting of one red central LED and three blue peripheral LEDs is developed and integrated onto the leader AUV to provide distinctive visual features under complex underwater conditions. Building upon the detected marker observations, a probabilistic switching Perspective-n-Point estimator (PSwPnP) is developed by combining Lie-group pose propagation on SE(3), probabilistic marker association, and visibility-adaptive measurement fusion for robust six-degree-of-freedom relative pose estimation. The proposed framework dynamically adapts the estimation process according to marker visibility, maintaining geometric consistency and temporal stability during partial observations and visibility transitions. Extensive water-tank experiments with motion-capture ground truth validate that AMR-Pose achieves accurate, smooth, and robust relative pose estimation under challenging underwater conditions. Closed-loop leader-follower experiments further demonstrate its feasibility for real-time relative pose feedback in cooperative underwater robotics.
Zeyu Sha, Xiaorui Wang, Mingyang Yang +1
Robotics and Control Laboratory, School of Advanced Manufacturing and Robotics, and the State Key Laboratory of Turbulence and Complex Systems, Peking University, Beijing, 100871, China
Classical leader-follower formation control suffers from single points of failure and error propagation, and relies on absolute localization sensors that are ill-suited for GPS-denied environments. We present a learned, vision-only estimator that maps each robot's monocular image, together with messages exchanged over a communication graph, directly to its 6-DoF relative pose. Its key ingredient is the implicit virtual leader (IVL): a non-physical reference frame at the team centroid, implicitly learned inside a Transformer-based graph neural network, so that estimation has no privileged node and needs no absolute localization. The estimator additionally reports well-calibrated aleatoric (heteroscedastic GNLL) uncertainty alongside epistemic (MC~Dropout) uncertainty, compared systematically across simulation and real-world test sets. Trained only in simulation, the estimator generalizes to unseen scenes, to larger unseen team sizes, and to an external real-world benchmark. It exhibits no single point of failure: removing any one robot costs at most 1.24× the median removal, and removing 71% of the communication links costs 1.77× in position error without retraining. Trained on real-robot data from a single platform, it transfers without modification to a heterogeneous team, estimating relative pose to 0.22,m and 1.6∘ on physical robots, where it drives closed-loop formation control.
Underwater robots rely on complementary sensors whose reliability changes abruptly with water visibility and vehicle motion. We introduce AquaJEPA, a sensor-configurable family of action-conditioned joint-embedding predictive models spanning full multimodal, camera-only, sonar-only, and sensor-dropout configurations. Its members share a latent objective and receding-horizon control interface that predict future representations and physical dynamics from camera, forward-looking sonar, proprioception, and thruster commands. Trained from scratch on one hour of action-labelled data, the family is evaluated in Stonefish on 120 fresh paired scenarios spanning unseen layouts, visibility changes, dynamics shifts, and scheduled DVL loss. AquaJEPA-base achieves the strongest aggregate closed-loop performance, improving success over state-only by 12.5 percentage points and reducing final error by 0.189 m; both paired 95% intervals exclude zero. In a separate three-seed evaluation, it reduces paired final error relative to AquaJEPA-S by 0.118 m, with the same direction for every seed. AquaJEPA-robust more than halves prediction error during camera and camera-DVL blackouts. These results show that full multimodal prediction improves over state-only control and the sonar-only family member in this benchmark, while sensor-dropout training provides robustness under sensor loss.