Relative Localization and Dynamic Tracking of Underwater Robots Based on 3D-AprilTag

摘要

This paper presents a visual localization system for underwater robots, aimed at achieving high-precision relative positioning and dynamic target tracking. A 3D AprilTag reference structure is constructed using a cubic configuration, and a high-resolution camera module is integrated into the AUV for real-time tag detection and pose decoding. By combining multi-face marker geometry with a fused state estimation strategy, the proposed method improves pose continuity and robustness during multi-tag transitions. To address pose estimation discontinuities caused by viewpoint changes and tag switching, we introduce a fusion-based observation-switching Kalman filter, which performs weighted integration of multiple tag observations based on relative distance, viewing angle, and detection confidence, ensuring smooth pose updates during tag transitions. The experimental results demonstrate that the system maintains stable pose estimation and trajectory continuity even under rapid viewpoint changes and frequent tag switches. These results validate the feasibility and applicability of the proposed method for underwater relative localization and tracking tasks.