A Closed-Form 4-DoF Inter-Robot Pose Estimator using Bearing-only Measurements

摘要

Bearing-odometry-based cooperative localization has attracted increasing research interest due to its minimal infrastructure requirements, low communication bandwidth and broad applicability in complex environments. However, existing 6-DoF approaches still face challenges in rapidly obtaining accurate and reliable inter-robot pose estimation, as the system is prone to observability degeneracy under specific motion patterns. To address these issues, we first propose a closed-form 4-DoF inter-robot pose estimator, which relaxes nonlinear constraints for rotations estimation and employs error projection for translations estimation. We then conduct a theoretical analysis of the system's observability, identifying degeneracy under two typical motion patterns: collinear and shape-preserving formations. The analysis further shows that the proposed 4-DoF system requires less stringent motion excitation for observability, enabling reliable estimation under a broader range of cooperative maneuvers. Furthermore, an observability test module is introduced to autonomously determine the optimal estimation instant, eliminating reliance on a predefined fixed-length sliding window. Extensive simulations and real-world experiments demonstrate that the proposed algorithm achieves higher estimation accuracy with significantly low computational cost, and the observability test module ensures estimation reliability while minimizing the data collection interval.