Asymptotically Optimal Sampling-Based Algorithms for Topological Motion Planning

Topological motion planning is a planning problem embedding topological concept of trajectories. In this work, we propose two asymptotically optimal sampling-based algorithms for topological motion planning: (a) a batch processing-based planner, termed Fast Marching Homology-embedded Tree star (FMHT*); and (b) an incremental anytime algorithm, termed Rapidly-exploring Random Homology-embedded Tree star (RRHT*). The methods commonly expand a graph directly in the configuration space and project the associated tree onto the topological concept-augmented space; the computational cost in edge computation and collision checking is significantly reduced by allowing trajectories with different topology to share the edge and collision information. Illustrative numerical examples are presented to demonstrate the validity and applicability of the proposed approach.