Riemannian-Based Pose Graph Optimization for Multiple UUV Systems
摘要
Pose estimation is essential for cooperative task execution in multiple-unmanned underwater vehicle (multi-UUV) systems, where pose accuracy usually exhibits significant fluctuations due to the harsh underwater environment. For example, in small-scale operation scenarios, optical-based pose estimation can be easily affected by limited visibility and occlusions. To address these challenges, vision-based cooperative pose estimation is formulated as a pose graph optimization problem in this paper, where observations from multiple UUVs are integrated to determine the optimal poses. Different from conventional pose graph-based methods that require local linearization and projecting poses back onto the manifold, this work proposes a Riemannian-based pose graph optimization (RPGO) algorithm, which updates poses directly on the Stiefel manifold, preserving manifold constraints and ensuring stable and accurate optimization. Finally, numerical and Gazebo-based simulations are conducted to validate the effectiveness and accuracy of the proposed RPGO algorithm.