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.

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

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

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

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

[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]  Jon M. Kleinberg,et al.  Two algorithms for nearest-neighbor search in high dimensions , 1997, STOC '97.

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

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

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

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

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

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

[13]  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).

[14]  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).

[15]  Nancy M. Amato,et al.  Choosing good distance metrics and local planners for probabilistic roadmap methods , 2000, IEEE Trans. Robotics Autom..

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

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