Efficient nearest neighbor searching for motion planning

We present and implement an efficient algorithm for performing nearest-neighbor queries in topological spaces that usually arise in the context of motion planning. Our approach extends the Kd tree-based ANN algorithm, which was developed by Arya and Mount (1993) for Euclidean spaces. We argue the correctness of the algorithm and illustrate its efficiency through computed examples. We have applied the algorithm to both probabilistic roadmaps (PRMs) and rapidly-exploring random trees (RRTs). Substantial performance improvements are shown for motion planning examples.