Improving motion-planning algorithms by efficient nearest-neighbor searching

The cost of nearest-neighbor (NN) calls is one of the bottle-necks in the performance of sampling-based motion-planning algorithms. Therefore, it is crucial to develop efficient techniques for NN searching in configuration spaces arising in motion planning. In this paper, we present and implement an algorithm for performing NN queries in Cartesian products of R, S-1, and RP3, the most common topological spaces in the context A motion planning. Our approach extends the algorithm based on kd-trees, called ANN, developed by Arya and Mount for Euclidean spaces. We prove the correctness of the algorithm and illustrate substantial performance improvement over the brute-force approach and several existing NN packages developed for general metric spaces. Our experimental results demonstrate a clear advantage of using the proposed method for both probabilistic roadmaps and rapidly exploring random trees.