Concurrent Nearest-Neighbor Searching for Parallel Sampling-Based Motion Planning in SO(3), SE(3), and Euclidean Spaces

This paper presents a fast exact nearest neighbor searching data structure and method that is designed to operate under highly-concurrent parallel operation on modern multi-core processors. Based on a kd-tree, the proposed method is fast, supports metric spaces common to robot motion planning, and supports nearest, k-nearest, and radius-based queries. But unlike traditional approaches using kd-trees, our approach supports simultaneous queries and insertions under concurrency, supports wait-free queries, and provides asymptotically diminishing expected wait-times for random concurrent inserts. We provide proofs of correctness under concurrency, and we demonstrate the proposed method’s performance in a parallelized asymptotically-optimal sampling-based motion planner.

[1]  Lydia E. Kavraki,et al.  The Open Motion Planning Library , 2012, IEEE Robotics & Automation Magazine.

[2]  Ron Alterovitz,et al.  Scalable Multicore Motion Planning Using Lock-Free Concurrency , 2014, IEEE Transactions on Robotics.

[3]  Jon Louis Bentley,et al.  Multidimensional binary search trees used for associative searching , 1975, CACM.

[4]  Steven M. LaValle,et al.  Improving Motion-Planning Algorithms by Efficient Nearest-Neighbor Searching , 2007, IEEE Transactions on Robotics.

[5]  Richard I. Hartley,et al.  Optimised KD-trees for fast image descriptor matching , 2008, 2008 IEEE Conference on Computer Vision and Pattern Recognition.

[6]  David G. Lowe,et al.  Shape indexing using approximate nearest-neighbour search in high-dimensional spaces , 1997, Proceedings of IEEE Computer Society Conference on Computer Vision and Pattern Recognition.

[7]  Ron Alterovitz,et al.  Fast Nearest Neighbor Search in SE(3) for Sampling-Based Motion Planning , 2014, WAFR.

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

[9]  Alexandr Andoni,et al.  Near-Optimal Hashing Algorithms for Approximate Nearest Neighbor in High Dimensions , 2006, 2006 47th Annual IEEE Symposium on Foundations of Computer Science (FOCS'06).

[10]  Emilio Frazzoli,et al.  Sampling-based algorithms for optimal motion planning , 2011, Int. J. Robotics Res..

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

[12]  J. Kuipers.  Quaternions and Rotation Sequences , 1998 .

[13]  Andrew Golightly,et al.  Adaptive, Delayed-Acceptance MCMC for Targets With Expensive Likelihoods , 2015, 1509.00172.

[14]  H. T. Kung,et al.  Concurrent manipulation of binary search trees , 1980, TODS.

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

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

[17]  Nancy M. Amato,et al.  Probabilistic roadmap methods are embarrassingly parallel , 1999, Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C).

[18]  Sergey Brin,et al.  Near Neighbor Search in Large Metric Spaces , 1995, VLDB.

[19]  Howie Choset,et al.  Principles of Robot Motion: Theory, Algorithms, and Implementation ERRATA!!!! 1 , 2007 .