A quadratic regulator-based heuristic for rapidly exploring state space

Kinodynamic planning algorithms like Rapidly-Exploring Randomized Trees (RRTs) hold the promise of finding feasible trajectories for rich dynamical systems with complex, nonconvex constraints. In practice, these algorithms perform very well on configuration space planning, but struggle to grow efficiently in systems with dynamics or differential constraints. This is due in part to the fact that the conventional distance metric, Euclidean distance, does not take into account system dynamics and constraints when identifying which node in the existing tree is capable of producing children closest to a given point in state space. We show that an affine quadratic regulator (AQR) design can be used to approximate the exact minimum-time distance pseudometric at a reasonable computational cost. We demonstrate improved exploration of the state spaces of the double integrator and simple pendulum when using this pseudometric within the RRT framework, but this improvement drops off as systems' nonlinearity and complexity increase. Future work includes exploring methods for approximating the exact minimum-time distance pseudometric that can reason about dynamics with higher-order terms.

[1]  J P Lasalle,et al.  TIME OPTIMAL CONTROL SYSTEMS. , 1959, Proceedings of the National Academy of Sciences of the United States of America.

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

[3]  John H. Reif,et al.  Complexity of the mover's problem and generalizations , 1979, 20th Annual Symposium on Foundations of Computer Science (sfcs 1979).

[4]  T. Başar,et al.  Dynamic Noncooperative Game Theory , 1982 .

[5]  John F. Canny,et al.  An exact algorithm for kinodynamic planning in the plane , 1991, Discret. Comput. Geom..

[6]  Frank L. Lewis,et al.  Applied Optimal Control and Estimation , 1992 .

[7]  Dimitri P. Bertsekas,et al.  Dynamic Programming and Optimal Control, Two Volume Set , 1995 .

[8]  Steven M. LaValle,et al.  A game-theoretic framework for robot motion planning , 1996 .

[9]  Nancy M. Amato,et al.  A randomized roadmap method for path and manipulation planning , 1996, Proceedings of IEEE International Conference on Robotics and Automation.

[10]  Zvi Shiller,et al.  Optimal obstacle avoidance based on the Hamilton-Jacobi-Bellman equation , 1994, IEEE Trans. Robotics Autom..

[11]  Jean-Paul Laumond,et al.  Guidelines in nonholonomic motion planning for mobile robots , 1998 .

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

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

[14]  Steven M. LaValle,et al.  Rapidly-Exploring Random Trees: Progress and Prospects , 2000 .

[15]  E. Feron,et al.  Robust hybrid control for autonomous vehicle motion planning , 2000, Proceedings of the 39th IEEE Conference on Decision and Control (Cat. No.00CH37187).

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

[17]  Steven M. LaValle,et al.  From Dynamic Programming to RRTs: Algorithmic Design of Feasible Trajectories , 2003, Control Problems in Robotics.

[18]  Antonio Bicchi,et al.  Control Problems in Robotics , 2003, Springer Tracts in Advanced Robotics.

[19]  Vijay Kumar,et al.  Design and verification of controllers for airships , 2003, Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453).

[20]  Vijay Kumar,et al.  An RRT-Based Algorithm for Testing and Validating Multi-Robot Controllers , 2005, Robotics: Science and Systems.

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

[22]  S. LaValle,et al.  Sampling-Based Motion Planning With Differential Constraints , 2005 .

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

[24]  M. Kalisiak,et al.  Toward More Efficient Motion Planning with Differential Constraints , 2008 .

[25]  Russ Tedrake,et al.  LQR-trees: Feedback motion planning on sparse randomized trees , 2009, Robotics: Science and Systems.

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

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

[28]  Dimitri P. Bertsekas,et al.  Dynamic Programming and Optimal Control 3rd Edition, Volume II , 2010 .

[29]  Wolfram Burgard,et al.  Robotics: Science and Systems XV , 2010 .