Multi-robot 6D graph SLAM connecting decoupled local reference filters

Teams of mobile robots can be deployed in search and rescue missions to explore previously unknown environments. Methods for joint localization and mapping constitute the basis for (semi-)autonomous cooperative action, in particular when navigating in GPS-denied areas. As communication losses may occur, a decentralized solution is required. With these challenges in mind, we designed a submap-based SLAM system that relies on inertial measurements and stereo-vision to create multi-robot dense 3D maps. For online pose and map estimation, we integrate the results of keyframe-based local reference filters through incremental graph SLAM. To the best of our knowledge, we are the first to combine these two methods to benefit from their particular advantages for 6D multi-robot localization and mapping: Local reference filters on each robot provide real-time, long-term stable state estimates that are required for stabilization, control and fast obstacle avoidance, whereas online graph optimization provides global multi-robot pose and map estimates needed for cooperative planning. We propose a novel graph topology for a decoupled integration of local filter estimates from multiple robots into a SLAM graph according to the filters´ uncertainty estimates and independence assumptions and evaluated its benefits on two different robots in indoor, outdoor and mixed scenarios. Further, we performed two extended experiments in a multi-robot setup to evaluate the full SLAM system, including visual robot detections and submap matches as inter-robot loop closure constraints.