Uncertainty Estimation Based on Error Propagation Law for Multi-Robot Pose Graph Merging

Abstract

In the domain of collaborative Simultaneous Localization and Mapping (CSLAM), a significant challenge is enhancing the accuracy of multi-robot trajectory merging. To the best of our knowledge, there is currently no relevant literature addressing uncertainty estimation for relative coordinate transformations under indirect data association and the difficulty of this issue stems from covariance propagation of three primary information sources (both single-robot pose and inter-robot loop closure). In this paper, we represent multi-robot trajectory in the form of pose graph and present a novel uncertainty estimation with compound pose (UECP). Initially, we develop a cost function through Lie algebra, followed by the direct differentiation of the Jacobian. We then apply the error propagation law (EPL) to estimate uncertainty, which incorporates the covariances from both single-robot pose and inter-robot loop closure. Ultimately, we propose a simplified solution by implementing a compound pose technique, which merges two successive poses into a unified estimate. Through a series of experiments, our findings indicate a substantial enhancement in both computation time and trajectory alignment accuracy. Specifically, our approach, which leverages the derived Jacobian matrix and the UECP method, achieves a computation time that is more efficient than both automatic differentiation and the EPL method. Additionally, it demonstrates a reduction in estimation error compared to state-of-the-art methods.