Algorithmic Foundations of Robotics XI pp 197-214

Part of the Springer Tracts in Advanced Robotics book series (STAR, volume 107) | Cite as

Fast Nearest Neighbor Search in SE(3) for Sampling-Based Motion Planning



Nearest neighborsearching is a fundamental building block of most sampling-based motion planners. We present a novel method for fast exact nearest neighbor searching in \(SE(3)\)—the 6 dimensional space that represents rotations and translations in 3 dimensions. \(SE(3)\) is commonly used when planning the motions of rigid body robots. Our approach starts by projecting a 4-dimensional cube onto the 3-sphere that is created by the unit quaternion representation of rotations in the rotational group \({ SO}(3)\). We then use 4 kd-trees to efficiently partition the projected faces (and their negatives). We propose efficient methods to handle the recursion pruning checks that arise with this kd-tree splitting approach, discuss splitting strategies that support dynamic data sets, and extend this approach to \(SE(3)\) by incorporating translations. We integrate our approach into RRT and RRT* and demonstrate the fast performance and efficient scaling of our nearest neighbor search as the tree size increases.


  1. 1.
    Choset, H., Lynch, K.M., Hutchinson, S.A., Kantor, G.A., Burgard, W., Kavraki, L.E., Thrun, S.: Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, Cambridge (2005)Google Scholar
  2. 2.
    Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 30(7), 846–894 (2011)CrossRefGoogle Scholar
  3. 3.
    Şucan, I.A., Moll, M., Kavraki, L.E.: The open motion planning library. IEEE Robot. Autom. Mag. 19(4), 72–82 (2012)CrossRefGoogle Scholar
  4. 4.
    Bentley, J.L.: Multidimensional binary search trees used for associative searching. Commun. ACM 18(9), 509–517 (1975)CrossRefMATHMathSciNetGoogle Scholar
  5. 5.
    Friedman, J.H., Bentley, J.L., Finkel, R.A.: An algorithm for finding best matches in logarithmic expected time. ACM Trans. Math. Softw. (TOMS) 3(3), 209–226 (1977)CrossRefMATHGoogle Scholar
  6. 6.
    Sproull, R.F.: Refinements to nearest-neighbor searching in k-dimensional trees. Algorithmica 6(1–6), 579–589 (1991)CrossRefMATHMathSciNetGoogle Scholar
  7. 7.
    Finkel, R.A., Bentley, J.L.: Quad trees a data structure for retrieval on composite keys. Acta Inform. 4(1), 1–9 (1974)CrossRefMATHGoogle Scholar
  8. 8.
    Yianilos, P.N.: Data structures and algorithms for nearest neighbor search in general metric spaces. In: Proceedings of the ACM-SIAM Symposium Discrete Algorithms (1993)Google Scholar
  9. 9.
    Bentley, J.L., Saxe, J.B.: Decomposable searching problems I. Static-to-dynamic transformation. J. Algorithms 1(4), 301–358 (1980)CrossRefMATHMathSciNetGoogle Scholar
  10. 10.
    Yershova, A., LaValle, S.M.: Improving motion-planning algorithms by efficient nearest-neighbor searching. IEEE Trans. Robot. 23(1), 151–157 (2007)CrossRefGoogle Scholar
  11. 11.
    Shoemake, K.: Animating rotation with quaternion curves. Proc. ACM SIGGRAPH 19(3), 245–254 (1985)CrossRefGoogle Scholar
  12. 12.
    Brin, S.: Near neighbor search in large metric spaces. In: Proceedings of the International Conference Very Large Databases (1995)Google Scholar
  13. 13.
    Beygelzimer, A., Kakade, S., Langford, J.: Cover trees for nearest neighbor. In: Proceedings of the International Conference Machine Learning, pp. 97–104. ACM (2006)Google Scholar
  14. 14.
    Ciaccia, P., Patella, M., Zezula, P.: M-tree: An efficient access method for similarity search in metric spaces. In: Proceedings of the International Conference Very Large Databases, p. 426 (1997)Google Scholar
  15. 15.
    Indyk, P.: Nearest neighbors in high-dimensional spaces. Handbook of Discrete and Computational Geometry, 2nd edn. Chapman and Hall/CRC, New York (2004)Google Scholar
  16. 16.
    Plaku, E., Kavraki, L.E.: Quantitative analysis of nearest-neighbors search in high-dimensional sampling-based motion planning. Algorithmic Foundation of Robotics VII, pp. 3–18. Springer, New York (2008)CrossRefGoogle Scholar
  17. 17.
    Arya, S., Mount, D.M., Netanyahu, N.S., Silverman, R., Wu, A.Y.: An optimal algorithm for approximate nearest neighbor searching fixed dimensions. J. ACM 45(6), 891–923 (1998)CrossRefMATHMathSciNetGoogle Scholar
  18. 18.
    Kushilevitz, E., Ostrovsky, R., Rabani, Y.: Efficient search for approximate nearest neighbor in high dimensional spaces. SIAM J. Comput. 30(2), 457–474 (2000)CrossRefMATHMathSciNetGoogle Scholar
  19. 19.
    Muja, M., Lowe, D.G.: Fast approximate nearest neighbors with automatic algorithm configuration. In: International Conference Computer Vision Theory and Application (VISSAPP), pp. 331–340. INSTICC Press (2009)Google Scholar
  20. 20.
    Mount, D.M.: ANN programming manual. Technical report Department of Computer Science, University of Maryland (1998)Google Scholar
  21. 21.
    Yershova, A., LaValle, S.M.: Deterministic sampling methods for spheres and SO(3). In: Proceedings of the IEEE International Conference Robotics and Automation, pp. 3974–3980 (2004)Google Scholar
  22. 22.
    Nowakiewicz, M.: Mst-based method for 6d of rigid body motion planning in narrow passages. In: Proceedings of the IEEE/RSJ International Conference Intelligent Robots and Systems (IROS), pp. 5380–5385. IEEE (2010)Google Scholar
  23. 23.
    Knuth, D.E.: The art of computer programming. Sorting and Searching, 2nd edn. Addison Wesley Longman Publishing Co., Inc., Redwood (1998)Google Scholar
  24. 24.
    LaValle, S.M., Kuffner, J.J.: Rapidly-exploring random trees: progress and prospects. In: Donald, B.R. (ed.) Algorithmic and Computational Robotics: New Directions, pp. 293–308. AK Peters, Natick (2001)Google Scholar

Copyright information

© Springer International Publishing Switzerland 2015

Authors and Affiliations

  1. 1.University of North Carolina at Chapel HillChapel HillUSA

Personalised recommendations