Range-Aided LiDAR Cooperative Localization and Mapping

The localization and mapping of the robotics team in unknown environments, especially GNSS denied environments, is a prerequisite for the perception and exploration of unknown environments by the robotics team. In this paper, we propose an algorithm based on sliding window graph optimization, which incorporates UWB range information and LiDAR odometry information for real-time localization and mapping. The cooperative localization mapping system mentioned in this paper incorporates a non-line-of-sight error (NLOS) discrimination method based on triangulation and statistical fusion. Based on Fisher Information Matrix, we investigate the number and arrangement of UWB ranging sensors to compensate for the drift of the LiDAR odometry. Finally, experiments on real scenes confirm the precision of our proposed co-localization system and the collaborative mapping of real scenes. The effectiveness of the fusion of UWB ranging information with LiDAR odometry information and the ability of ranging information to correct for LiDAR Odometry drift are demonstrated. Through cooperative localization tests in real-world scenarios, we demonstrate the improvement in precision and adaptability of our method compared to filtering methods and single sensor measurement information.