Extended LQR: Locally-Optimal Feedback Control for Systems with Non-Linear Dynamics and Non-Quadratic Cost

We present Extended LQR, a novel approach for locally-optimal control for robots with non-linear dynamics and non-quadratic cost functions. Our formulation is conceptually different from existing approaches, and is based on the novel concept of LQR-smoothing, which is an LQR-analogue of Kalman smoothing. Our approach iteratively performs both a backward Extended LQR pass, which computes approximate cost-to-go functions, and a forward Extended LQR pass, which computes approximate cost-to-come functions. The states at which the sum of these functions is minimal provide an approximately optimal sequence of states for the control problem, and we use these points to linearize the dynamics and quadratize the cost functions in the subsequent iteration. Our results indicate that Extended LQR converges quickly and reliably to a locally-optimal solution of the non-linear, non-quadratic optimal control problem. In addition, we show that our approach is easily extended to include temporal optimization, in which the duration of a trajectory is optimized as part of the control problem. We demonstrate the potential of our approach on two illustrative non-linear control problems involving simulated and physical differential-drive robots and simulated quadrotor helicopters.

[1]  C. Striebel,et al.  On the maximum likelihood estimates for linear dynamic systems , 1965 .

[2]  David Q. Mayne,et al.  Differential dynamic programming , 1972, The Mathematical Gazette.

[3]  P. Whittle.  Risk-sensitive linear/quadratic/gaussian control , 1981, Advances in Applied Probability.

[4]  Anil V. Rao,et al.  Practical Methods for Optimal Control Using Nonlinear Programming , 1987 .

[5]  N. Higham.  COMPUTING A NEAREST SYMMETRIC POSITIVE SEMIDEFINITE MATRIX , 1988 .

[6]  Sidney Yakowitz,et al.  Algorithms and Computational Techniques in Differential Dynamic Programming , 1989 .

[7]  Max Donath,et al.  American Control Conference , 1993 .

[8]  Bradley M. Bell,et al.  The Iterated Kalman Smoother as a Gauss-Newton Method , 1994, SIAM J. Optim..

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

[10]  J. Navarro-Pedreño.  Numerical Methods for Least Squares Problems , 1996 .

[11]  Chung-Yao Kao,et al.  Control of Linear Time-Varying Systems Using Forward Riccati Equation , 1997 .

[12]  Stephen J. Wright,et al.  Numerical Optimization , 2018, Fundamental Statistical Inference.

[13]  Thiagalingam Kirubarajan,et al.  Estimation with Applications to Tracking and Navigation , 2001 .

[14]  Zvi Shiller,et al.  Dual Dijkstra Search for paths with different topologies , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[15]  Emanuel Todorov,et al.  Iterative Linear Quadratic Regulator Design for Nonlinear Biological Movement Systems , 2004, ICINCO.

[16]  E. Todorov,et al.  A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems , 2005, Proceedings of the 2005, American Control Conference, 2005..

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

[18]  Emanuel Todorov,et al.  General duality between optimal control and estimation , 2008, 2008 47th IEEE Conference on Decision and Control.

[19]  John T. Betts,et al.  Practical Methods for Optimal Control and Estimation Using Nonlinear Programming , 2009 .

[20]  Marc Toussaint,et al.  Robot trajectory optimization using approximate inference , 2009, ICML '09.

[21]  Ian R. Manchester,et al.  LQR-trees: Feedback Motion Planning via Sums-of-Squares Verification , 2010, Int. J. Robotics Res..

[22]  Marc Toussaint,et al.  An Approximate Inference Approach to Temporal Optimization in Optimal Control , 2010, NIPS.

[23]  Yuval Tassa,et al.  Stochastic Differential Dynamic Programming , 2010, Proceedings of the 2010 American Control Conference.

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

[25]  Ron Alterovitz,et al.  Motion planning under uncertainty using iterative local optimization in belief space , 2012, Int. J. Robotics Res..

[26]  Dennis S. Bernstein,et al.  Forward-integration Riccati-based feedback control of magnetically actuated spacecraft , 2012 .

[27]  Marc Toussaint,et al.  On Stochastic Optimal Control and Reinforcement Learning by Approximate Inference , 2012, Robotics: Science and Systems.

[28]  Dennis S. Bernstein,et al.  Forward-integration Riccati-based output-feedback control of linear time-varying systems , 2012, 2012 American Control Conference (ACC).

[29]  Pieter Abbeel,et al.  Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization , 2013, Robotics: Science and Systems.

[30]  Siddhartha S. Srinivasa,et al.  CHOMP: Covariant Hamiltonian optimization for motion planning , 2013, Int. J. Robotics Res..