Topological belief space planning for active SLAM with pairwise Gaussian potentials and performance guarantees

Abstract

Determining a globally optimal solution of belief space planning (BSP) in high-dimensional state spaces directly is computationally expensive, as it involves belief propagation and objective function evaluation for each candidate action. However, many problems of interest, such as active SLAM, exhibit structure that can be harnessed to expedite planning. Also, in order to choose an optimal action, an exact value of the objective function is not required as long as the actions can be sorted in the same way. In this paper we leverage these two key aspects and present the topological belief space planning (t-bsp) concept that uses topological signatures to perform this ranking for information-theoretic cost functions, considering only topologies of factor graphs that correspond to future posterior beliefs. In particular, we propose a highly efficient topological signature based on the von Neumann graph entropy that is a function of graph node degrees and supports an incremental update. We analyze it in the context of active pose SLAM and derive error bounds between the proposed topological signature and the original information-theoretic cost function. These bounds are then used to provide performance guarantees for t-bsp with respect to a given solver of the original information-theoretic BSP problem. Realistic and synthetic simulations demonstrate drastic speed-up of the proposed method with respect to the state-of-the-art methods while retaining the ability to select a near-optimal solution. A proof of concept of t-bsp is given in a small-scale real-world active SLAM experiment.