Voxplan: A 3D Global Planner using Signed Distance Function Submaps

Abstract

The ability to safely navigate through complex and cluttered environments is required for a wide range of robotics applications. This paper introduces a framework to compute safe global paths in maps represented as collections of 3D Signed Distance Function (SDF) submaps. Such maps are able to maintain global consistency in spite of odometry drift. However, computationally efficient global path planning in this context remains a challenging problem. We present a planning approach based on pre-computed local graphs, computed in each submap, that are linked to form a global path at planning time. To ensure globally safe paths, planning algorithms make frequent queries to the submap collection, which grows over time as the agent collects observational data. We present an efficient algorithm for performing these queries, through the use of a spatial hash table. We analyze the performance of our proposal extensively in simulation and real-world environments, and compare our approach to state- of-the-art planning approaches designed for monolithic maps, extended to submap-based maps. We show the efficacy of our method at adapting to global map deformations, while significantly reducing the planning time to an average of ~1.2 seconds, a reduction by 90 % compared to classical monolithic approaches.