A Novel Dynamic Localization Graph for Efficient Relative Localization

Relative Localization (RelLoc) is crucial for consistent decision-making in multi-robot systems. To achieve efficient RelLoc, two key problems must be solved: how to evaluate the RelLoc accuracy, and how to coordinate the robot swarm to enhance it when it does not meets the stopping criterion. Existing research mainly focuses on passively optimizing relative poses, without explicitly evaluating the uncertainty in this process or efficiently improving RelLoc accuracy in an active manner. In this paper, we propose a novel Dynamic Localization Graph (DLG) to evaluate, monitor, and actively improve inter-robot RelLoc accuracy. To address the first problem, we propose the Metric of Relative Localization Accuracy (MRLA) based on the pose covariance without requiring prior knowledge of relative poses. Specifically, graduated non-convexity factor graph optimization and uncertainty propagation is applied to calculate the transformation and update the pose covariance between the initial frames, followed by outlier rejection through hypothesis testing. Then, the DLG is constructed by assigning the MRLA between robots as the edge weight, which illustrates the status of RelLoc in the multi-robot system. To address the second problem, we further introduce an active RelLoc enhancement strategy on top of the DLG that triggers additional inter-robot measurements when needed, enabling autonomous, accuracy-cost aware RelLoc enhancement. When the MRLA between all robot pairs exceeds the threshold, the stopping criterion for RelLoc is satisfied, ensuring high-accuracy RelLoc among robots. The DLG is sensor-independent, focusing on evaluating and enhancing RelLoc accuracy. It is instantiated on a place recognition RelLoc pipeline as a representative case study. Simulations demonstrate that when the MRLA in the DLG reaches 0.98, the Absolute Trajectory Error is less than 0.1 m, and the DLG is further verified in both indoor and outdoor experiments.