CoLRIO: LiDAR-Ranging-Inertial Centralized State Estimation for Robotic Swarms

TL;DR

CoLRIO tackles anchor-free, GPS-denied swarm SLAM by pairing per-robot LiDAR-Inertial odometry with a centralized backend that fuses inter-robot loop closures and UWB distance data. The framework uses a front-end based on LiDAR direct registration with IMU preintegration in a fixed-lag smoother, and a back-end that performs robust global optimization using PCM and GNC to reject outliers. Key contributions include the online centralized cooperative LiDAR-ranging-inertial state estimation, the integration of Scan-Context++ for inter-robot loop closures, and extensive evaluation on public and campus datasets with up to $10$ robots, demonstrating improved accuracy and real-time performance. The approach reduces per-robot computation and communication bottlenecks while maintaining global consistency, making anchor-free swarm SLAM practical in GPS-denied environments and scalable to larger teams in the future.

Abstract

Collaborative state estimation using different heterogeneous sensors is a fundamental prerequisite for robotic swarms operating in GPS-denied environments, posing a significant research challenge. In this paper, we introduce a centralized system to facilitate collaborative LiDAR-ranging-inertial state estimation, enabling robotic swarms to operate without the need for anchor deployment. The system efficiently distributes computationally intensive tasks to a central server, thereby reducing the computational burden on individual robots for local odometry calculations. The server back-end establishes a global reference by leveraging shared data and refining joint pose graph optimization through place recognition, global optimization techniques, and removal of outlier data to ensure precise and robust collaborative state estimation. Extensive evaluations of our system, utilizing both publicly available datasets and our custom datasets, demonstrate significant enhancements in the accuracy of collaborative SLAM estimates. Moreover, our system exhibits remarkable proficiency in large-scale missions, seamlessly enabling ten robots to collaborate effectively in performing SLAM tasks. In order to contribute to the research community, we will make our code open-source and accessible at \url{https://github.com/PengYu-team/Co-LRIO}.

Figures (6)

• Figure 1: Simulation results of CoLRIO estimate from 10 drones. The total trajectory length exceeds 1000 m; the black dotted line indicates the ground truth, and the loop and distance constraints are denoted by red lines.
• Figure 2: The system structure of CoLRIO. In the front end, the preintegration and direct scan-to-map matching odometry are further jointly optimized with fixed-lag smoothness. Keyframes generated by LIO, which include UWB measurements, are transmitted to the server. The server collects all measurements, executing robust joint optimization to achieve swarm state estimations. Finally, the estimates are returned to the front end for further processing.
• Figure 3: The structure of front-end odometry, fusing LiDAR and IMU data.
• Figure 4: The structure of the CoLRIO back-end. LiDAR odometry, range, and loop closure factors are introduced to global optimization.
• Figure 5: Different methods' trajectory estimates on various dataset sequences compared with ground truth.