Broadcast Your Weaknesses: Cooperative Active Pose-Graph SLAM for Multiple Robots

Abstract

In this letter, we propose a low-cost, high-efficiency framework for cooperative active pose-graph simultaneous localization and mapping (SLAM) for multiple robots in three-dimensional (3D) environments based on graph topology. Based on the selection of weak connections in pose graphs, this method aims to find the best trajectories for optimal information exchange to repair these weaknesses opportunistically when robots move near them. Based on tree-connectivity, which is greatly related to the D-optimality metric of the Fisher information matrix (FIM), we explore the relationship between measurement (edge) selection and pose-measurement (node-edge) selection, which often occurs in active SLAM, in terms of information increment. The measurement selection problem is formulated as a submodular optimization problem and solved by an exhaustive method using rank-1 updates. We decide which robot takes the selected measurements through a bidding framework where each robot computes its predicted cost. Finally, based on a novel continuous trajectory optimization method, these additional measurements collected by the winning robot are sent to the requesting robot to strengthen its pose graph. In simulations and experiments, we validate our approach by comparing against existing methods. Further, we demonstrate online communication based on offline planning results using two unmanned aerial vehicles (UAVs).