Autonomous Robots

, Volume 43, Issue 2, pp 375–387 | Cite as

Robust direct trajectory optimization using approximate invariant funnels

  • Zachary ManchesterEmail author
  • Scott Kuindersma
Part of the following topical collections:
  1. Special Issue on Robotics: Science and Systems


Many critical robotics applications require robustness to disturbances arising from unplanned forces, state uncertainty, and model errors. Motion planning algorithms that explicitly reason about robustness require a coupling of trajectory optimization and feedback design, where the system’s closed-loop response to disturbances is optimized. Due to the often-heavy computational demands of solving such problems, the practical application of robust trajectory optimization in robotics has so far been limited. Motivated by recent work on sums-of-squares verification methods for nonlinear systems, we derive a scalable robust trajectory optimization algorithm that optimizes approximate invariant funnels along the trajectory while planning. For the case of ellipsoidal disturbance sets and LQR feedback controllers, the state and input deviations along a nominal trajectory can be computed locally in closed form, permitting fast evaluation of robust cost and constraint functions and their derivatives. The resulting algorithm is a scalable extension of classical direct transcription that demonstrably improves tracking performance over non-robust formulations while incurring only a modest increase in computational cost. We evaluate the algorithm in several simulated robot control tasks.


Trajectory optimization Motion planning Robustness Robust trajectory optimization Robust control 



This work was supported by an Internal Research and Development grant from Draper. The authors would like to thank the reviewers and the members of the Harvard Agile Robotics Laboratory for their valuable feedback.


  1. Betts, J. T. (1998). Survey of numerical methods for trajectory optimization. Journal of Guidance, Control, and Dynamics, 21(2), 193–207.CrossRefzbMATHGoogle Scholar
  2. Betts, J. T. (2001). Practical methods for optimal control using nonlinear programming, volume 3 of advances in design and control. Philadelphia, PA: Society for Industrial and Applied Mathematics (SIAM).Google Scholar
  3. Dai, H., & Tedrake, R. (2012). Optimizing robust limit cycles for legged locomotion on unknown terrain (pp. 1207–1213).Google Scholar
  4. Dai, H., & Tedrake, R. (2013). L2-gain optimization for robust bipedal walking on unknown terrain. In Proceedings of the IEEE international conference on robotics and automation (ICRA).Google Scholar
  5. Deisenroth, M. P., & Rasmussen, C. E. (2011). PILCO: A model-based and data-efficient approach to policy search. In Proceedings of the 28th international conference on machine learning, Bellevue, WA.Google Scholar
  6. Desaraju, V., Spitzer, A., & Michael, N. (2017). Experience-driven predictive control with robust constraint satisfaction under time-varying state uncertainty. In Robotics: Science and systems (RSS). ISBN 978-0-9923747-3-0.
  7. Fahroo, F., & Ross, I. M. (2002). Direct trajectory optimization by a Chebyshev pseudospectral method. Journal of Guidance, Control, and Dynamics, 25(1), 160–166. ISSN 0731-5090.
  8. Farshidian, F., & Buchli, J. (2015). Risk sensitive, nonlinear optimal control: Iterative linear exponential-quadratic optimal control with Gaussian noise. arXiv:1512.07173 [cs].
  9. Gill, P. E., Murray, W., & Saunders, M. A. (2005). SNOPT: An SQP algorithm for large-scale constrained optimization. SIAM Review, 47(1), 99–131.MathSciNetCrossRefzbMATHGoogle Scholar
  10. Griffin, B., & Grizzle, J. (2015). Walking gait optimization for accommodation of unknown terrain height variations. In 2015 American control conference (ACC) (pp. 4810–4817).
  11. Hargraves, C. R., & Paris, S. W. (1987). Direct trajectory optimization using nonlinear programming and collocation. Journal of Guidance, 10(4), 338–342.CrossRefzbMATHGoogle Scholar
  12. Jacobson, D. (1968). Differential dynamic programming methods for solving bang–bang control problems. IEEE Transactions on Automatic Control, 13(6), 661–675. Scholar
  13. Jacobson, D. H., & Mayne, D. Q. (1970). Differential dynamic programming. Amsterdam: Elsevier.zbMATHGoogle Scholar
  14. Johnson, A. M., King, J., & Srinivasa, S. (2016). Convergent planning. IEEE Robotics and Automation Letters, 1(2), 1044–1051. ISSN 2377-3766.
  15. Julier, S. J., & Uhlmann, J. K. (2004). Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 92(3), 401–422.CrossRefGoogle Scholar
  16. Julius, A. A., & Pappas, G. J. (2009). Trajectory based verification using local finite-time invariance. In Hybrid systems: Computation and control, lecture notes in computer science (pp. 223–236). Berlin: Springer. ISBN 978-3-642-00601-2, 978-3-642-00602-9.
  17. Kavraki, L. E., Svestka, P., Latombe, J. C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4), 566–580.CrossRefGoogle Scholar
  18. Kothare, M. V., Balakrishnan, V., & Morari, M. (1996). Robust constrained model predictive control using linear matrix inequalities. Automatica, 32(10), 1361–1379. Scholar
  19. Kuffner Jr, J. J., & LaValle, S. M. (2000). RRT-connect: An efficient approach to single-query path planning. In Proceedings of the IEEE international conference on robotics and automation.Google Scholar
  20. Kuindersma, S., Grupen, R., & Barto, A. (2013). Variable risk control via stochastic optimization. International Journal of Robotics Research, 32(7), 806–825.CrossRefGoogle Scholar
  21. Lin, T. C., & Arora, J. S. (1991). Differential dynamic programming technique for constrained optimal control. Computational Mechanics, 9(1), 27–40. ISSN 0178-7675, 1432-0924.
  22. Lin, W., & Byrnes, C. I. (1996). H infinty-control of discrete-time nonlinear systems. IEEE Transactions on Automatic Control, 41(4), 494–510. ISSN 0018-9286.
  23. Lou, J., & Hauser, K. (2015). Robust trajectory optimization under frictional contact with iterative learning. In Robotics science and systems (RSS).Google Scholar
  24. Magni, L., De Nicolao, G., Scattolini, R., & Allgöwer, F. (2003). Robust model predictive control for nonlinear discrete-time systems. International Journal of Robust and Nonlinear Control, 13(3–4), 229–246. ISSN 1099-1239.
  25. Majumdar, A., & Tedrake, R. (2013). Robust online motion planning with regions of finite time invariance. In Algorithmic foundations of robotics X (pp. 543–558). Berlin: Springer.Google Scholar
  26. Majumdar, A., & Tedrake, R. (2016). Funnel libraries for real-time robust feedback motion planning. arXiv:1601.04037 [cs, math].
  27. Manchester, Z., & Kuindersma, S. (2016). Derivative-free trajectory optimization with unscented dynamic programming. In Proceedings of the 55th conference on decision and control (CDC), Las Vegas, NV.Google Scholar
  28. Mayne, D. Q., & Kerrigan, E. C. (2007). Tube-based robust nonlinear model predictive control. In Proceedings of the 7th IFAC symposium on nonlinear control systems, Pretoria (pp. 110–115).Google Scholar
  29. Mellinger, D., Michael, N., & Kumar, V. (2012). Trajectory generation and control for precise aggressive maneuvers with quadrotors. The International Journal of Robotics Research, 31(5), 664–674. ISSN 0278-3649, 1741-3176.
  30. Moore, J., Cory, R., & Tedrake, R. (2014). Robust post-stall perching with a simple fixed-wing glider using LQR-trees. Bioinspiration & Biomimetics, 9(2), 025013. ISSN 1748-3182, 1748-3190.
  31. Moore, J., & Tedrake, R. (2014). Adaptive control design for underactuated systems using sums-of-squares optimization. In Proceedings of the 2014 American control conference (ACC).Google Scholar
  32. Mordatch, I., Lowrey, K., & Todorov, E. (2015). Ensemble-CIO: Full-body dynamic motion planning that transfers to physical humanoids. In Proceedings of the international conference on robotics and automation (ICRA).Google Scholar
  33. Morimoto, J., Zeglin, G., & Atkeson, C. G. (2003). Minimax differential dynamic programming: Application to a biped walking robot. In Proceedings of the 2003 IEEE/RSJ international conference on intelligent robots and systems.Google Scholar
  34. Nocedal, J., & Wright, S. J. (2006). Numerical optimization (2nd ed.). Berlin: Springer.zbMATHGoogle Scholar
  35. Pan, Y., Theodorou, E., & Bakshi, K. (2015). Robust trajectory optimization: A cooperative stochastic game theoretic approach. In Proceedings of robotics: Science and systems, Rome.Google Scholar
  36. Parillo, P. (2000). Structured semidefinite programs and semialgebraic geometry methods in robustness and optimization. Ph.D. thesis, California Institute of Technology.Google Scholar
  37. Plancher, B., Manchester, Z., & Kuindersma, S. (2017). Constrained unscented dynamic programming. In Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (IROS), Vancouver, BC.Google Scholar
  38. Platt, R., Kaelbling, L., Lozano-Perez, T., & Tedrake, R. (2012). Non-Gaussian belief space planning: Correctness and complexity. In Proceedings of the international conference on robotics and automation (ICRA), St. Paul, MN.Google Scholar
  39. Posa, M., Cantu, C., & Tedrake, R. (2014). A direct method for trajectory optimization of rigid bodies through contact. International Journal of Robotics Research, 33(1), 69–81.CrossRefGoogle Scholar
  40. Posa, M., Kuindersma, S., & Tedrake, R. (2016). Optimization and stabilization of trajectories for constrained dynamical systems. In Proceedings of the international conference on robotics and automation (ICRA) (pp. 1366–1373), Stockholm. IEEE.Google Scholar
  41. Ratliff, N., Zucker, M., Bagnell, J. A., & Srinivasa, S. (2009). CHOMP: Gradient optimization techniques for efficient motion planning. In Proceedings of the international conference on robotics and automation (ICRA).Google Scholar
  42. Schulman, J., Duan, Y., Ho, J., Lee, A., Awwal, I., Bradlow, H., et al. (2014). Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research, 33(9), 1251–1270. ISSN 0278-3649, 1741-3176.
  43. Tassa, Y., Erez, T., & Todorov, E. (2012). Synthesis and stabilization of complex behaviors through online trajectory optimization. In IEEE/RSJ international conference on intelligent robots and systems.Google Scholar
  44. Tedrake, R., Manchester, I. R., Tobenkin, M. M., & Roberts, J. W. (2010). LQR-Trees: Feedback motion planning via sums of squares verification. International Journal of Robotics Research, 29, 1038–1052.CrossRefGoogle Scholar
  45. Tobenkin, M., Manchester, I., & Tedrake, R. (2011). Invariant funnels around trajectories using sums-of-squares programming. In Proceedings of the 18th IFAC World Congress, Milan.Google Scholar
  46. van den Berg, J., Abbeel, P., & Goldberg, K. (2011). LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information. The International Journal of Robotics Research, 30(7), 895–913. ISSN 0278-3649.
  47. van den Broek, B., Wiegerinck, W., & Kappen, B. (2010). Risk sensitive path integral control. In Proceedings of the 26th conference on uncertainty in artificial intelligence (UAI) (pp. 615–622).Google Scholar
  48. Whittle, P. (1981). Risk-sensitive linear/quadratic/Gaussian control. Advances in Applied Probability, 13, 764–777.MathSciNetCrossRefzbMATHGoogle Scholar
  49. Yeon, J. S., & Park, J. H. (2008). Practical robust control for flexible joint robot manipulators. In 2008 IEEE international conference on robotics and automation (pp. 3377–3382).
  50. Zhou, K. (1996). Robust and optimal control. Upper Saddle River, NJ: Prentice Hall. ISBN 978-0-13-456567-5.Google Scholar

Copyright information

© Springer Science+Business Media, LLC, part of Springer Nature 2018

Authors and Affiliations

  1. 1.School of Engineering and Applied SciencesHarvard UniversityCambridgeUSA

Personalised recommendations