Design of Admissible Heuristics for Kinodynamic Motion Planning via Sum-of-Squares Programming

How does one obtain an admissible heuristic for a kinodynamic motion planning problem? This paper develops the analytical tools and techniques to answer this question. A sufficient condition for the admissibility of a heuristic is presented which can be checked directly from the problem data. This condition is also used to formulate a concave program to optimize an admissible heuristic. This optimization is then approximated and solved in polynomial time using sum-of-squares programming techniques. A number of examples are provided to demonstrate these concepts.

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

[2]  R. Vinter.  Convex duality and nonlinear optimal control , 1993 .

[3]  Kim-Chuan Toh,et al.  SDPT3 -- A Matlab Software Package for Semidefinite Programming , 1996 .

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

[5]  Pablo A. Parrilo,et al.  Introducing SOSTOOLS: a general purpose sum of squares programming solver , 2002, Proceedings of the 41st IEEE Conference on Decision and Control, 2002..

[6]  Amir Ali Ahmadi,et al.  DSOS and SDSOS optimization: LP and SOCP-based alternatives to sum of squares optimization , 2014, 2014 48th Annual Conference on Information Sciences and Systems (CISS).

[7]  P. Parrilo.  Structured semidefinite programs and semialgebraic geometry methods in robustness and optimization , 2000 .

[8]  Brian Paden,et al.  The Generalized Label Correcting Method for Optimal Kinodynamic Motion Planning , 2016, WAFR.

[9]  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).

[10]  Nils J. Nilsson,et al.  A Formal Basis for the Heuristic Determination of Minimum Cost Paths , 1968, IEEE Trans. Syst. Sci. Cybern..

[11]  Jos F. Sturm,et al.  A Matlab toolbox for optimization over symmetric cones , 1999 .

[12]  Wendell H. Fleming,et al.  Generalized Solutions in the Optimal Control of Diffusions , 1988 .

[13]  Siddhartha S. Srinivasa,et al.  Informed RRT*: Optimal Incremental Path Planning Focused through an Admissible Ellipsoidal Heuristic , 2014, ArXiv.

[14]  J. Lofberg,et al.  YALMIP : a toolbox for modeling and optimization in MATLAB , 2004, 2004 IEEE International Conference on Robotics and Automation (IEEE Cat. No.04CH37508).