Improving Motion-Planning Algorithms by Efficient Nearest-Neighbor Searching

The cost of nearest-neighbor (NN) calls is one of the bottlenecks 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, S1, and RP3, the most common topological spaces in the context of 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

[1]  K. Clarkson.  Nearest-Neighbor Searching and Metric Space Dimensions , 2005 .

[2]  Jan van Leeuwen,et al.  Dynamic multi-dimensional data structures based on quad- and k—d trees , 1982, Acta Informatica.

[3]  D. Mount.  ANN Programming Manual , 1998 .

[4]  Jon Louis Bentley,et al.  Decomposable Searching Problems I: Static-to-Dynamic Transformation , 1980, J. Algorithms.

[5]  Nancy M. Amato,et al.  A randomized roadmap method for path and manipulation planning , 1996, Proceedings of IEEE International Conference on Robotics and Automation.

[6]  E. Feron,et al.  Robust hybrid control for autonomous vehicle motion planning , 2000, Proceedings of the 39th IEEE Conference on Decision and Control (Cat. No.00CH37187).

[7]  Sunil Arya,et al.  Algorithms for fast vector quantization , 1993, [Proceedings] DCC `93: Data Compression Conference.

[8]  James J. Kuffner,et al.  Effective sampling and distance metrics for 3D rigid body path planning , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[9]  Robert F. Sproull,et al.  Refinements to nearest-neighbor searching ink-dimensional trees , 1991, Algorithmica.

[10]  Piotr Indyk,et al.  Nearest Neighbors in High-Dimensional Spaces , 2004, Handbook of Discrete and Computational Geometry, 2nd Ed..

[11]  Steven M. LaValle,et al.  Rapidly-Exploring Random Trees: Progress and Prospects , 2000 .

[12]  Trevor Darrell,et al.  Nearest-Neighbor Searching and Metric Space Dimensions , 2006 .

[13]  Mark de Berg,et al.  Computational geometry: algorithms and applications , 1997 .

[14]  Robert Krauthgamer,et al.  Navigating nets: simple algorithms for proximity search , 2004, SODA '04.

[15]  B. Faverjon,et al.  Probabilistic Roadmaps for Path Planning in High-Dimensional Con(cid:12)guration Spaces , 1996 .

[16]  Steven M. LaValle,et al.  Planning algorithms , 2006 .

[17]  Gert Vegter,et al.  In handbook of discrete and computational geometry , 1997 .

[18]  S. LaValle.  Rapidly-exploring random trees : a new tool for path planning , 1998 .

[19]  Jon Louis Bentley,et al.  An Algorithm for Finding Best Matches in Logarithmic Expected Time , 1977, TOMS.

[20]  Jon M. Kleinberg,et al.  Two algorithms for nearest-neighbor search in high dimensions , 1997, STOC '97.

[21]  S. LaValle,et al.  Randomized Kinodynamic Planning , 2001 .

[22]  K. Clarkson.  Nearest Neighbor Searching in Metric Spaces : Experimental Results for sb ( S ) , 2002 .

[23]  John Langford,et al.  Cover trees for nearest neighbor , 2006, ICML.

[24]  Kenneth L. Clarkson,et al.  Nearest Neighbor Queries in Metric Spaces , 1997, STOC '97.

[25]  Hanan Samet,et al.  Index-driven similarity search in metric spaces (Survey Article) , 2003, TODS.

[26]  Sunil Arya,et al.  An optimal algorithm for approximate nearest neighbor searching fixed dimensions , 1998, JACM.

[27]  Sunil Arya,et al.  Approximate nearest neighbor queries in fixed dimensions , 1993, SODA '93.

[28]  Steven M. LaValle,et al.  RRT-connect: An efficient approach to single-query path planning , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[29]  Leonidas J. Guibas,et al.  Algorithmic issues in modeling motion , 2002, CSUR.

[30]  Piotr Indyk,et al.  Approximate nearest neighbors: towards removing the curse of dimensionality , 1998, STOC '98.

[31]  Nicole Immorlica,et al.  Locality-sensitive hashing scheme based on p-stable distributions , 2004, SCG '04.