A successive approximation-based approach for optimal kinodynamic motion planning with nonlinear differential constraints

This paper presents an extension to RRT* [1], a sampling-based motion planning with asymptotic optimality guarantee, in order to incorporate nonlinear differential equations in motion dynamics. The main challenge due to nonlinear differential constraints is the computational complexity of solving a two-point boundary-value problem that arises in the tree expansion process to optimally connect two given states. This work adapts the successive approximation method that transforms a nonlinear optimal control problem into a sequence of linear-quadratic-like problems to solve these TPBVPs. The resulting algorithm, termed SA-RRT*, is demonstrated to create more realistic plans compared to existing kinodynamic extensions of RRT*, while preserving asymptotic optimality.

[1]  Emilio Frazzoli,et al.  Anytime computation of time-optimal off-road vehicle maneuvers using the RRT* , 2011, IEEE Conference on Decision and Control and European Control Conference.

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

[3]  Thierry Siméon,et al.  Dynamic-Domain RRTs: Efficient Exploration by Controlling the Sampling Domain , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[4]  Gong-You Tang,et al.  Approximation design of optimal controllers for nonlinear systems with sinusoidal disturbances , 2007 .

[5]  Frank L. Lewis,et al.  Optimal Control , 1986 .

[6]  Emilio Frazzoli,et al.  Anytime Motion Planning using the RRT* , 2011, 2011 IEEE International Conference on Robotics and Automation.

[7]  Steven M. LaValle,et al.  Resolution complete rapidly-exploring random trees , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[8]  S. LaValle.  Rapidly-exploring random trees : a new tool for path planning , 1998 .

[9]  Steven M. LaValle,et al.  Reducing metric sensitivity in randomized trajectory design , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[10]  Steven M. LaValle,et al.  Motion Planning Part I: The Essentials , 2011 .

[11]  Russ Tedrake,et al.  A quadratic regulator-based heuristic for rapidly exploring state space , 2010, 2010 IEEE International Conference on Robotics and Automation.

[12]  Steven M. LaValle,et al.  RRT-connect: An efficient approach to single-query path planning , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[13]  Lydia E. Kavraki,et al.  Probabilistic Roadmaps for Robot Path Planning , 1998 .

[14]  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.

[15]  Matthew R. Walter,et al.  Reachability-guided sampling for planning under differential constraints , 2009, 2009 IEEE International Conference on Robotics and Automation.

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

[17]  Russ Tedrake,et al.  Path planning in 1000+ dimensions using a task-space Voronoi bias , 2009, 2009 IEEE International Conference on Robotics and Automation.

[18]  Steven M. LaValle,et al.  Current Issues in Sampling-Based Motion Planning , 2005, ISRR.

[19]  Gong-You Tang,et al.  Suboptimal control for nonlinear systems: a successive approximation approach , 2005, Syst. Control. Lett..

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

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