Skip to content

Optimizing Trajectories with Closed-Loop Dynamic SQP

Authors: Sumeet Singh, Jean-Jacques Slotine, Vikas Sindhwani

Published: 2021 (Conference Paper)

Source: International Conference on Robotics and Automation (ICRA)

Algorithm: CL-SQP

arXiv: 2109.07081

DOI: 10.1109/ICRA46639.2022.9811562

Summary

Introduces closed-loop dynamic SQP, which integrates feedback gain computation within the SQP iteration structure, improving robustness and convergence for trajectory optimization compared to open-loop sequential approaches.

Abstract

Indirect trajectory optimization methods such as Differential Dynamic Programming (DDP) have found considerable success when only planning under dynamic feasibility constraints. Meanwhile, nonlinear programming (NLP) has been the state-of-the-art approach when faced with additional constraints (e.g., control bounds, obstacle avoidance). However, a naïve implementation of NLP algorithms, e.g., shooting-based sequential quadratic programming (SQP), may suffer from slow convergence - caused from natural instabilities of the underlying system manifesting as poor numerical stability within the optimization. Re-interpreting the DDP closed-loop rollout policy as a sensitivity-based correction to a second-order search direction, we demonstrate how to compute analogous closedloop policies (i.e., feedback gains) for constrained problems. Our key theoretical result introduces a novel dynamic programmingbased constraint-set recursion that augments the canonical “cost-to-go” backward pass. On the algorithmic front, we develop a hybrid-SQP algorithm incorporating DDP-style closedloop rollouts, enabled via efficient parallelized computation of the feedback gains. Finally, we validate our theoretical and algorithmic contributions on a set of increasingly challenging benchmarks, demonstrating significant improvements in convergence speed over standard open-loop SQP.

Primary

Paper arxiv.org

Standard

arXiv Abstract 2109.07081 arXiv PDF 2109.07081 arXiv HTML 2109.07081 DOI 10.1109/ICRA46639.2022.9811562

Alternate

ieeexplore.ieee.org ieeexplore.ieee.org

Tags

  • Trajectory optimization

  • Sequential quadratic programming

  • Closed-loop

  • Nonlinear optimization

  • Robot control