Refined Analysis of Asymptotically-Optimal Kinodynamic Planning in the State-Cost Space

We present a novel analysis of AO-RRT: a tree-based planner for motion planning with kinodynamic constraints, originally described by Hauser and Zhou (AO-X, 2016). AO-RRT explores the state-cost space and has been shown to efficiently obtain high-quality solutions in practice without relying on the availability of a computationally-intensive two-point boundary-value solver. Our main contribution is an optimality proof for the single-tree version of the algorithm—a variant that was not analyzed before. Our proof only requires a mild and easily-verifiable set of assumptions on the problem and system: Lipschitz-continuity of the cost function and the dynamics. In particular, we prove that for any system satisfying these assumptions, any trajectory having a piecewise-constant control function and positive clearance from the obstacles can be approximated arbitrarily well by a trajectory found by AORRT. We also discuss practical aspects of AORRT and present experimental comparisons of variants of the algorithm.

[1]  Lydia E. Kavraki,et al.  Resolution Independent Density Estimation for motion planning in high-dimensional spaces , 2013, 2013 IEEE International Conference on Robotics and Automation.

[2]  Kostas E. Bekris,et al.  Sparse Methods for Efficient Asymptotically Optimal Kinodynamic Planning , 2014, WAFR.

[3]  S. LaValle,et al.  Motion Planning , 2008, Springer Handbook of Robotics.

[4]  A. Dmitruk.  On the development of Pontryagin's Maximum Principle in the works of A.Ya. Dubovitskii and A.A. Milyutin , 2009 .

[5]  Kostas E. Bekris,et al.  Asymptotically optimal sampling-based kinodynamic planning , 2014, Int. J. Robotics Res..

[6]  Jur P. van den Berg,et al.  Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics , 2013, 2013 IEEE International Conference on Robotics and Automation.

[7]  Steven M. LaValle,et al.  Planning algorithms , 2006 .

[8]  Dan Halperin,et al.  Sampling-Based Bottleneck Pathfinding with Applications to Fréchet Matching , 2016, ESA.

[9]  Maxim Likhachev,et al.  Multi-Heuristic A* , 2014, Int. J. Robotics Res..

[10]  Leslie Pack Kaelbling,et al.  LQR-RRT*: Optimal sampling-based motion planning with automatically derived extension heuristics , 2012, 2012 IEEE International Conference on Robotics and Automation.

[11]  Emmanuel Trélat,et al.  Convergence Results for Smooth Regularizations of Hybrid Nonlinear Optimal Control Problems , 2011, SIAM J. Control. Optim..

[12]  Emilio Frazzoli,et al.  Sampling-based optimal motion planning for non-holonomic dynamical systems , 2013, 2013 IEEE International Conference on Robotics and Automation.

[13]  Yoshihiko Nakamura,et al.  Completeness of randomized kinodynamic planners with state-based steering , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[14]  Mike Stilman,et al.  Kinodynamic RRTs with Fixed Time Step and Best-Input Extension Are Not Probabilistically Complete , 2014, WAFR.

[15]  Siddhartha S. Srinivasa,et al.  Batch Informed Trees (BIT*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs , 2014, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[16]  Marco Pavone,et al.  Optimal sampling-based motion planning under differential constraints: The driftless case , 2014, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[17]  Emilio Frazzoli,et al.  Optimal kinodynamic motion planning using incremental sampling-based methods , 2010, 49th IEEE Conference on Decision and Control (CDC).

[18]  Pieter Abbeel,et al.  Toward asymptotically optimal motion planning for kinodynamic systems using a two-point boundary value problem solver , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[19]  Nicholas M. Patrikalakis,et al.  Analysis of Asymptotically Optimal Sampling-based Motion Planning Algorithms for Lipschitz Continuous Dynamical Systems , 2014, ArXiv.

[20]  Marco Pavone,et al.  Optimal sampling-based motion planning under differential constraints: The drift case with linear affine dynamics , 2014, 2015 54th IEEE Conference on Decision and Control (CDC).

[21]  Dinesh Manocha,et al.  Collision and Proximity Queries , 2004, Handbook of Discrete and Computational Geometry, 2nd Ed..

[22]  Kris Hauser,et al.  Asymptotically Optimal Planning by Feasible Kinodynamic Planning in a State–Cost Space , 2015, IEEE Transactions on Robotics.

[23]  John M. Lee.  Introduction to Smooth Manifolds , 2002 .

[24]  Marco Pavone,et al.  Deterministic Sampling-Based Motion Planning: Optimality, Complexity, and Performance , 2015, ISRR.

[25]  Kostas E. Bekris,et al.  Probabilistic Completeness of RRT for Geometric and Kinodynamic Planning With Forward Propagation , 2018, IEEE Robotics and Automation Letters.

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

[27]  Steven M. LaValle,et al.  Randomized Kinodynamic Planning , 1999, Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C).

[28]  Simon Parsons,et al.  Principles of Robot Motion: Theory, Algorithms and Implementations by Howie Choset, Kevin M. Lynch, Seth Hutchinson, George Kantor, Wolfram Burgard, Lydia E. Kavraki and Sebastian Thrun, 603 pp., $60.00, ISBN 0-262-033275 , 2007, The Knowledge Engineering Review.

[29]  Dan Halperin,et al.  Asymptotically-optimal Motion Planning using lower bounds on cost , 2014, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[30]  L. S. Pontryagin,et al.  Mathematical Theory of Optimal Processes , 1962 .

[31]  Riccardo Bonalli,et al.  Continuity of Pontryagin Extremals with Respect to Delays in Nonlinear Optimal Control , 2018, SIAM J. Control. Optim..

[32]  Jean-Claude Latombe,et al.  Randomized Kinodynamic Motion Planning with Moving Obstacles , 2002, Int. J. Robotics Res..

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

[34]  E B Lee,et al.  Foundations of optimal control theory , 1967 .

[35]  Lydia E. Kavraki,et al.  Motion Planning , 2011, Springer Handbook of Robotics, 2nd Ed..

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

[37]  Marco Pavone,et al.  Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions , 2013, ISRR.

[38]  H. Brezis.  Functional Analysis, Sobolev Spaces and Partial Differential Equations , 2010 .

[39]  Emmanuel Trélat,et al.  Some Properties of the Value Function and Its Level Sets for Affine Control Systems with Quadratic Cost , 2000, math/0607424.