Asymptotically-Optimal Topological Nearest-Neighbor Filtering

Nearest-neighbor finding is a major bottleneck for sampling-based motion planning algorithms. The cost of finding nearest neighbors grows with the size of the roadmap, leading to a significant computational bottleneck for problems which require many configurations to find a solution. In this letter, we develop a method of mapping configurations of a jointed robot to neighborhoods in the workspace that supports fast search for configurations in nearby neighborhoods. This expedites nearest-neighbor search by locating a small set of the most likely candidates for connecting to the query with a local plan. We show that this filtering technique can preserve asymptotically-optimal guarantees with modest requirements on the distance metric. We demonstrate the method's efficacy in planning problems for rigid bodies and both fixed and mobile-base manipulators.