Distributed 3-D Multi-Robot Cooperative Localization: An Efficient and Consistent Approach

This paper studies the problem of Cooperative Localization (CL) for multi-robot systems in 3-D environments, where a group of mobile robots jointly localize themselves by using measurements from onboard sensors and shared information from other robots. To ensure the efficiency of information fusion and observability consistency in a distributed CL system, we propose a distributed multi-robot CL method based on Lie groups, well-suited for 3-D scenarios with full 3-D rotational dynamics and generic nonlinear inter-robot measurement models. Unlike most existing distributed CL algorithms that operate in vector space and are only applicable to simple 2-D environments, the proposed algorithm performs distributed information fusion directly on the manifold that inherently accounts for the non-Euclidean nature of 3-D rotations and translations. By leveraging the nice property of invariant errors, we analytically prove that the proposed algorithm naturally preserves the observability consistency of the CL system. This ensures that the system maintains the correct structure of unobservable directions throughout the estimation process. The effectiveness of the proposed algorithm is validated by several numerical experiments conducted to rigorously investigate the effects of relative information fusion in the distributed CL system.