Abstract
Nearest-neighbor search dominates the asymptotic complexity of sampling-based motion planning algorithms and is often addressed with k-d tree data structures. While it is generally believed that the expected complexity of nearest-neighbor queries is \(O(\text {log}(N))\) in the size of the tree, this paper reveals that when a classic k-d tree approach is used with sub-Riemannian metrics, the expected query complexity is in fact \(\varTheta (N^{p} \text {log}(N))\) for a number \(p\in \) 2 [0, 1) determined by the degree of nonholonomy of the system. These metrics arise naturally in nonholonomic mechanical systems, including classic wheeled robot models. To address this negative result, we propose novel k-d tree build and query strategies tailored to sub-Riemannian metrics and demonstrate significant improvements in the running time of nearest-neighbor search queries.
This is a preview of subscription content, log in via an institution to check access.
Access this chapter
Tax calculation will be finalised at checkout
Purchases are for personal use only
Preview
Unable to display preview. Download preview PDF.
Similar content being viewed by others
References
J. L. Bentley. Multidimensional binary search trees used for associative searching. Communications of the ACM, 18(9):509–517, 1975.
M. Bharatheesha, W. Caarls, W. J. Wolfslag, and M. Wisse. Distance metric approximation for state-space RRTs using supervised learning. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 252–257, 2014.
B. Burns and O. Brock. Single-query motion planning with utility-guided random trees. In Proceedings 2007 IEEE International Conference on Robotics and Automation, pages 3307–3312, 2007.
J. H. Friedman, J. L. Bentley, and R. A. Finkel. An algorithm for finding best matches in logarithmic expected time. ACM Transactions on Mathematical Software(TOMS), 3(3):209–226, 1977.
E. Glassman and R. Tedrake. A quadratic regulator-based heuristic for rapidly exploring state space. In Robotics and Automation (ICRA), 2010 IEEE International Conference on, pages 5021–5028, 2010.
J. Ichnowski and R. Alterovitz. Fast nearest neighbor search in SE(3) for samplingbased motion planning. In Algorithmic Foundations of Robotics XI, pages 197–214. Springer, 2015.
S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846–894, 2011.
S. Karaman and E. Frazzoli. Sampling-based optimal motion planning for nonholonomic dynamical systems. In Robotics and Automation (ICRA), 2013 IEEE International Conference on, pages 5041–5047, 2013.
L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE transactions on Robotics and Automation, 12(4):566–580, 1996.
J. J. Kuffner. Effective sampling and distance metrics for 3D rigid body path planning. In Robotics and Automation, 2004. Proceedings. ICRA’04. 2004 IEEE International Conference on, volume 4, pages 3993–3998, 2004.
S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning. Citeseer, 1998.
S. M. LaValle and J. J. Kuffner. Randomized kinodynamic planning. The International Journal of Robotics Research, 20(5):378–400, 2001.
R. Montgomery. A tour of subriemannian geometries, their geodesics and applications. Number 91. American Mathematical Soc., 2006.
M. Muja and D. G. Lowe. Fast approximate nearest neighbors with automatic algorithm configuration. In International Conference on Computer Vision Theory and Application VISSAPP ’09) pages 331–340. INSTICC Press, 2009.
L. Palmieri and K. Arras. Distance metric learning for RRT-based motion planning for wheeled mobile robots. In 2014 IROS Machine Learning in Planning and Control of Robot Motion Workshop, 2014.
J. Reeds and L. Shepp. Optimal paths for a car that goes both forwards and backwards. Pacific journal of mathematics, 145(2):367–393, 1990.
A. Shkolnik, M.Walter, and R. Tedrake. Reachability-guided sampling for planning under differential constraints. In Robotics and Automation, 2009. ICRA’09. IEEE International Conference on, pages 2859–2865, 2009.
. I. A. Şucan, M. Moll, and L. E. Kavraki. The Open Motion Planning Library. IEEE Robotics & Automation Magazine, (4):72–82, December.
A. Yershova and S. M. LaValle. Improving motion-planning algorithms by efficient nearest-neighbor searching. IEEE Transactions on Robotics, 23(1):151–157, 2007.
Author information
Authors and Affiliations
Corresponding author
Editor information
Editors and Affiliations
Rights and permissions
Copyright information
© 2020 Springer Nature Switzerland AG
About this chapter
Cite this chapter
Varricchio, V., Paden, B., Yershov, D., Frazzoli, E. (2020). Effcient Nearest-Neighbor Search for Dynamical Systems with Nonholonomic Constraints. In: Goldberg, K., Abbeel, P., Bekris, K., Miller, L. (eds) Algorithmic Foundations of Robotics XII. Springer Proceedings in Advanced Robotics, vol 13. Springer, Cham. https://doi.org/10.1007/978-3-030-43089-4_39
Download citation
DOI: https://doi.org/10.1007/978-3-030-43089-4_39
Published:
Publisher Name: Springer, Cham
Print ISBN: 978-3-030-43088-7
Online ISBN: 978-3-030-43089-4
eBook Packages: Intelligent Technologies and RoboticsIntelligent Technologies and Robotics (R0)