Optimizing Trajectories with Closed-Loop Dynamic SQP

Sumeet Singh, Jean-Jacques Slotine, Vikas Sindhwani

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 closed-loop policies (i.e., feedback gains) for constrained problems. Our key theoretical result introduces a novel dynamic programming-based 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 closed-loop 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.

Introduction

Trajectory optimization in robotics typically falls into two categories: indirect methods like Differential Dynamic Programming (DDP) and direct methods like Sequential Quadratic Programming (SQP). DDP is favored for its "closed-loop" rollouts, where feedback gains maintain numerical stability during optimization. However, it struggles with complex inequality constraints. Conversely, SQP is the standard for handling constraints (e.g., obstacle avoidance, control limits) but typically relies on "open-loop" updates.

For unstable systems like drones or balancing robots, open-loop updates are problematic. A minor control change early in a trajectory can cause exponential deviations later, forcing the optimizer to take extremely small steps. This leads to slow convergence or failure. This work introduces a "Hybrid SQP" algorithm that bridges this gap by bringing the closed-loop stability of DDP to constrained nonlinear programming.

Methodology: Dynamic Programming SQP (DP-SQP)

The proposed DP-SQP approach integrates closed-loop rollouts into a standard SQP framework through several innovations:

  1. Sensitivity-Based Interpretation: Feedback gains are re-interpreted as sensitivity-based corrections to the Newton search direction.
  2. Constraint-Set Recursion: A new theoretical framework back-propagates the set of valid constraints (the "constraint-set") alongside the standard "cost-to-go," ensuring feasibility even when sub-problems are complex.
  3. Hybrid-SQP Algorithm: Closed-loop rollouts are integrated into a standard SQP line-search. The robot uses computed feedback gains to track the intended perturbation during optimization.
  4. Parallelized Approximation (CLγ): To overcome the computational cost of exact constrained DP, a smoothed version using barrier functions (interior-point method) was developed. This allows feedback gains to be computed efficiently in parallel across all time steps.

Experimental Setup

The algorithm was evaluated on three simulated benchmarks:

  • 2D Car: Navigating a 4-state car through circular obstacles.
  • Acrobot: A classic swing-up task for a two-link underactuated robot.
  • Quad-Pendulum: An 8-state system (planar quadrotor with a hanging pendulum) requiring complex maneuvering around obstacles to swing the pendulum upright.

Results and Discussion

The proposed closed-loop methods (CL and CLγ) demonstrated superior reliability and speed compared to standard open-loop (OL) SQP.

Performance comparison on the 2D Car benchmark. OL SQP fails to converge in two out of three cases, while CL methods succeed rapidly.

As shown in the performance metrics above, OL SQP failed to converge in 2 of 3 test cases for the 2D car, whereas CL and CLγ converged in under 20 iterations. In the most challenging Quad-Pendulum task, standard SQP stalled immediately due to numerical instability. The CLγ method successfully navigated this complex landscape.

The Quad-Pendulum task: CLγ successfully finds a trajectory that swings the pendulum upright while avoiding obstacles, a task where standard SQP fails.

Key insights include:

  • Feedback as a Stabilizer: Feedback gains are powerful tools for improving the numerical behavior of the optimization process itself.
  • Smoothing Benefits: The barrier-based approximation (CLγ) often outperformed exact DP (CL) by smoothing non-differentiable constraint edges, enhancing numerical stability.
  • Efficiency: Parallelized gain computation makes the closed-loop overhead negligible relative to the total optimization time.

Conclusion

This work demonstrates that incorporating closed-loop feedback into SQP significantly improves the optimization of trajectories for unstable, constrained systems. Future work will explore applying these gains to "Full" direct methods and utilizing faster unconstrained solvers to generate initial search directions.

Summary updated

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 closed-loop policies (i.e., feedback gains) for constrained problems. Our key theoretical result introduces a novel dynamic programming-based 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 closed-loop 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.
BibTeX
@inproceedings{singh2022optimizing,
  author = {Sumeet Singh and Jean-Jacques Slotine and Vikas Sindhwani},
  title = {{Optimizing Trajectories with Closed-Loop Dynamic SQP}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2022},
  url = {https://openreview.net/forum?id=FhhUgKgXPG},
}
Figure