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

Part of the Springer Tracts in Advanced Robotics book series (STAR, volume 114)


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.


Cost Function Control Problem Optimal Control Problem Control Input Control Policy 
These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.


  1. 1.
    Bar-Shalom, Y., Li, R., Kirubarajan, T.: Estimation with Applications to Tracking and Navigation, Wiley-Interscience, New Jersey (2004)Google Scholar
  2. 2.
    Bell, B.: The iterated Kalman smoother as a Gauss-Newton method. SIAM J. Optim. 4(3), 626–636 (1994)MathSciNetCrossRefzbMATHGoogle Scholar
  3. 3.
    Betts, J.: Practical methods for optimal control and estimation using nonlinear programming, vol. 19, SIAM (2009)Google Scholar
  4. 4.
    Bertsekas, D.: Dynamic Programming and Optimal Control. Athena Scientific, Belmont (2001)Google Scholar
  5. 5.
    A. Björck. Numerical Methods for Least Squares Problems. SIAM, Philadelphia (1996)Google Scholar
  6. 6.
    Chen, M.S., Kao, C.Y.: Control of linear time-varying systems using forward Riccati equation. J. Dyn. Syst. Meas. Control 119(3), 536540 (1997)Google Scholar
  7. 7.
    Fujita, Y., Nakamura, Y., Shiller, Z.: Dual Dijkstra search for paths with different topologies. In: Proceedings of the IEEE International Conference on Robotics and Automation (2003)Google Scholar
  8. 8.
    Higham, N.: Computing a nearest symmetric positive semidefinite matrix. Linear Algebra Appl. 103, 103–118 (1988)MathSciNetCrossRefzbMATHGoogle Scholar
  9. 9.
    Jacobsen, D., Mayne, D.: Differential Dynamic Programming. Elsevier, New York (1970)Google Scholar
  10. 10.
    Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 30(7), 846–894 (2011)CrossRefzbMATHGoogle Scholar
  11. 11.
    Lavalle, S.: Planning Algorithms. Cambridge University Press, Cambridge (2006)Google Scholar
  12. 12.
    Li, W., Todorov, E.: Iterative linear-quadratic regulator design for nonlinear biological movement systems. In: Proceedings of the International Conference on Informatics in Control, Automation and Robotics (2004)Google Scholar
  13. 13.
    Nocedal, J., Wright, S.: Numerical Optimization. Springer Science+ Business Media, Germany (2006)Google Scholar
  14. 14.
    Rauch, H., Tung, F., Striebel, C.: Maximum likelihood estimates of linear dynamic systems. AIAA J. 3(8), 1445–1450 (1965)MathSciNetCrossRefGoogle Scholar
  15. 15.
    Rawlik, K., Toussaint, M., Vijayakumar, S.: An approximate inference approach to temporal optimization in optimal control. In: Proceedings of the Advances in Neural Information Processing Systems, pp. 2011–2019 (2010)Google Scholar
  16. 16.
    Rawlik, K., Toussaint, M., Vijayakumar, S.: On stochastic optimal control and reinforcement learning by approximate inference. In: Proceedings of the Robotics Science and Systems Conference (R:SS 2012), Sydney, Australia (2012)Google Scholar
  17. 17.
    Schulman, J., Ho, J., Lee, A., Awwal, I., Bradlow, H., Abbeel, P.: Finding locally optimal, collisi-on-free trajectories with sequential convex optimization. In: Robotics: Science and Systems (2013)Google Scholar
  18. 18.
    Tedrake, R., Manchester, I., Tobenkin, M., Roberts, J.: LQR-trees: Feedback motion planning via sums-of-squares verification. Int. J. Robot. Res. 29(8), 1038–1052 (2010)CrossRefGoogle Scholar
  19. 19.
    Theodorou, E., Tassa, Y., Todorov, E.: Stochastic differential dynamic programming. Proceedings of the American Control Conference (2010)Google Scholar
  20. 20.
    Todorov, E.: General duality between optimal control and estimation. In: Proceedings of the IEEE Conference on Decision and Control (2008)Google Scholar
  21. 21.
    Todorov, E., Li, W.: A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems. In: Proceedings of the American Control Conference (2005)Google Scholar
  22. 22.
    Toussaint, M.: Robot trajectory optimization using approximate inference. In: Proceedings of the International Conference on Machine Learning (2009)Google Scholar
  23. 23.
    van den Berg, J., Patil, S., Alterovitz, R.: Motion planning under uncertainty using iterative local optimization in belief space. Int. J. Robot. Res. 31(11), 1263–1278 (2012)CrossRefGoogle Scholar
  24. 24.
    Weiss, A., Kolmanovsky, I., Bernstein, D.: Forward-integration Riccati-based output-feedback control of linear time-varying systems. In: American Control Conference (2012)Google Scholar
  25. 25.
    Whittle, P.: Risk-sensitive linear/quadratic/Gaussian control. Adv. Appl. Prob. 13(4), 764–777 (1981)MathSciNetCrossRefzbMATHGoogle Scholar
  26. 26.
    Yakowitz, S.: Algorithms and computational techniques in differential dynamic programming. Control Dyn. Syst. 31, 75–91 (1989)CrossRefGoogle Scholar
  27. 27.
    Zucker, M., Ratliff, N., Dragan, A., Pivtoraiko, M., Klingensmith, M., Dellin, C., Bagnell, J., Srinivasa, S.: CHOMP: Covariant Hamiltonian optimization for motion planning. Int. J. Robot. Res. (2013)Google Scholar

Copyright information

© Springer International Publishing Switzerland 2016

Authors and Affiliations

  1. 1.School of ComputingUniversity of UtahSalt LakeUSA

Personalised recommendations