Abstract
Nearest neighbor searching 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.
Access this chapter
Tax calculation will be finalised at checkout
Purchases are for personal use only
References
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)
Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 30(7), 846–894 (2011)
Şucan, I.A., Moll, M., Kavraki, L.E.: The open motion planning library. IEEE Robot. Autom. Mag. 19(4), 72–82 (2012)
Bentley, J.L.: Multidimensional binary search trees used for associative searching. Commun. ACM 18(9), 509–517 (1975)
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)
Sproull, R.F.: Refinements to nearest-neighbor searching in k-dimensional trees. Algorithmica 6(1–6), 579–589 (1991)
Finkel, R.A., Bentley, J.L.: Quad trees a data structure for retrieval on composite keys. Acta Inform. 4(1), 1–9 (1974)
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)
Bentley, J.L., Saxe, J.B.: Decomposable searching problems I. Static-to-dynamic transformation. J. Algorithms 1(4), 301–358 (1980)
Yershova, A., LaValle, S.M.: Improving motion-planning algorithms by efficient nearest-neighbor searching. IEEE Trans. Robot. 23(1), 151–157 (2007)
Shoemake, K.: Animating rotation with quaternion curves. Proc. ACM SIGGRAPH 19(3), 245–254 (1985)
Brin, S.: Near neighbor search in large metric spaces. In: Proceedings of the International Conference Very Large Databases (1995)
Beygelzimer, A., Kakade, S., Langford, J.: Cover trees for nearest neighbor. In: Proceedings of the International Conference Machine Learning, pp. 97–104. ACM (2006)
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)
Indyk, P.: Nearest neighbors in high-dimensional spaces. Handbook of Discrete and Computational Geometry, 2nd edn. Chapman and Hall/CRC, New York (2004)
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)
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)
Kushilevitz, E., Ostrovsky, R., Rabani, Y.: Efficient search for approximate nearest neighbor in high dimensional spaces. SIAM J. Comput. 30(2), 457–474 (2000)
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)
Mount, D.M.: ANN programming manual. Technical report Department of Computer Science, University of Maryland (1998)
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)
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)
Knuth, D.E.: The art of computer programming. Sorting and Searching, 2nd edn. Addison Wesley Longman Publishing Co., Inc., Redwood (1998)
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)
Acknowledgments
This research was supported in part by the National Science Foundation (NSF) through awards IIS-1117127 and IIS-1149965.
Author information
Authors and Affiliations
Corresponding author
Editor information
Editors and Affiliations
Rights and permissions
Copyright information
© 2015 Springer International Publishing Switzerland
About this chapter
Cite this chapter
Ichnowski, J., Alterovitz, R. (2015). Fast Nearest Neighbor Search in SE(3) for Sampling-Based Motion Planning. In: Akin, H., Amato, N., Isler, V., van der Stappen, A. (eds) Algorithmic Foundations of Robotics XI. Springer Tracts in Advanced Robotics, vol 107. Springer, Cham. https://doi.org/10.1007/978-3-319-16595-0_12
Download citation
DOI: https://doi.org/10.1007/978-3-319-16595-0_12
Published:
Publisher Name: Springer, Cham
Print ISBN: 978-3-319-16594-3
Online ISBN: 978-3-319-16595-0
eBook Packages: EngineeringEngineering (R0)