Incremental Path Planning Algorithm via Topological Mapping with Metric Gluing

We present an incremental topology-based motion planner that, while planning paths in the configuration space, performs metric gluing on the constructed Vietoris-Rips simplicial complex of each sub-space (voxel). By incrementally capturing topological and geometric information in batches of voxel graphs, our algorithm avoids the time overhead of analyzing the properties of the entire configuration space. We theoretically prove in this paper that the simplices of all voxel graphs joined together are homotopy-equivalent to the union of the simplices in the configuration space. Experiments were carried out in seven different environments using various robots, including the articulated linkage robot, the Kuka YouBot, and the PR2 robot. In all environments, the results show that our algorithm achieves better convergence for path cost and computation time with a memory-efficient roadmap than state-of-the-art methods.