Iterative methods for efficient sampling-based optimal motion planning of nonlinear systems

Abstract This paper extends the RRT* algorithm, a recently developed but widely used sampling based optimal motion planner, in order to effectively handle nonlinear kinodynamic constraints. Nonlinearity in kinodynamic differential constraints often leads to difficulties in choosing an appropriate distance metric and in computing optimized trajectory segments in tree construction. To tackle these two difficulties, this work adopts the affine quadratic regulator-based pseudo-metric as the distance measure and utilizes iterative two-point boundary value problem solvers to compute the optimized segments. The proposed extension then preserves the inherent asymptotic optimality of the RRT* framework, while efficiently handling a variety of kinodynamic constraints. Three numerical case studies validate the applicability of the proposed method.

[1]  Jacek Blaszczyk,et al.  Optimization-based approach to path planning for closed chain robot systems , 2011, Int. J. Appl. Math. Comput. Sci..

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

[3]  Michal Kvasnica,et al.  An optimal path planning problem for heterogeneous multi-vehicle systems , 2016, Int. J. Appl. Math. Comput. Sci..

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

[5]  H. Yuan Z. Qu.  Optimal real-time collision-free motion planning for autonomous underwater vehicles in a 3 D underwater space , 2010 .

[6]  Emilio Frazzoli,et al.  Optimal sampling-based Feedback Motion Trees among obstacles for controllable linear systems with linear constraints , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[7]  Han-Lim Choi,et al.  A successive approximation-based approach for optimal kinodynamic motion planning with nonlinear differential constraints , 2013, 52nd IEEE Conference on Decision and Control.

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

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

[10]  César Alejandro Chavez-Olivares,et al.  Saturating Stiffness Control of Robot Manipulators with Bounded Inputs , 2017, Int. J. Appl. Math. Comput. Sci..

[11]  Emilio Frazzoli,et al.  Incremental Sampling-Based Algorithms for a Class of Pursuit-Evasion Games , 2010, WAFR.

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

[13]  Steven M. LaValle,et al.  Motion Planning : The Essentials , 2011 .

[14]  Russ Tedrake,et al.  Underactuated Robotics: Learning, Planning, and Control for Ecient and Agile Machines Course Notes for MIT 6.832 , 2009 .

[15]  Eric Walter,et al.  Reliable Robust Path Planning with Application to Mobile Robots , 2009, Int. J. Appl. Math. Comput. Sci..

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

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

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

[19]  Emilio Frazzoli,et al.  A martingale approach and time-consistent sampling-based algorithms for risk management in stochastic optimal control , 2013, 53rd IEEE Conference on Decision and Control.

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

[21]  Emilio Frazzoli,et al.  An incremental sampling-based algorithm for stochastic optimal control , 2012, 2012 IEEE International Conference on Robotics and Automation.

[22]  RODOLFO GARCÍA-RODRÍGUEZ,et al.  Dynamic optimal grasping of a circular object with gravity using robotic soft-fingertips , 2016, Int. J. Appl. Math. Comput. Sci..

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

[24]  Donald E. Kirk,et al.  Optimal control theory : an introduction , 1970 .

[25]  Panagiotis Tsiotras,et al.  Use of relaxation methods in sampling-based algorithms for optimal motion planning , 2013, 2013 IEEE International Conference on Robotics and Automation.

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

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

[28]  Robert Platt,et al.  Optimal sampling-based planning for linear-quadratic kinodynamic systems , 2013, 2013 IEEE International Conference on Robotics and Automation.

[29]  Wojciech Blajer,et al.  Motion planning and control of gantry cranes in cluttered work environment , 2007 .

[30]  Woojin Chung,et al.  Practical motion planning for car-parking control in narrow environment , 2010 .

[31]  Zhihua Qu,et al.  Optimal real-time collision-free motion planning for autonomous underwater vehicles in a 3D underwater space , 2009 .

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