Constrained Differential Dynamic Programming Revisited

I. INTRODUCTION

• Constrained trajectory optimization is common in robotics and controls, but high-dimensional nonlinear optimal-control methods and direct methods each face important scalability or feasibility costs.Applications include robotic manipulation and orbital fuel minimization, while direct methods must explicitly impose dynamics feasibility.Source: §I · p. 1
• DDP decomposes trajectory optimization using Bellman’s principle and can achieve locally quadratic convergence under mild cost and dynamics assumptions.iLQR uses Gauss–Newton cost Hessians and first-order dynamics expansions as a numerically advantageous variation.Source: §I · p. 1
• Unlike unconstrained DDP, a broadly established constrained version remains unavailable despite practical state and control limits.Prior approaches include quadratic programs, projection, KKT-based methods, and Augmented Lagrangian combinations, but constrained solvers remain widely used.Source: §I · p. 1
• The paper develops discrete-time DDP methods for state and control constraints using slack variables in Bellman’s principle and smooth penalty-Lagrangian functions.The two methodologies can be used separately or combined for improved numerical performance.Source: §I · p. 1
• The paper evaluates constrained methods through comparisons across varied simulated scenarios and presents the study as an extensive assessment of constrained trajectory optimization.The introduction identifies this breadth of experimental comparison as lacking from previous DDP-related schemes.Source: §I · p. 2

A. Unconstrained Differential Dynamic Programming

• The constrained algorithms retain DDP’s backward pass over local quadratic approximations and forward-pass rollout until convergence criteria are satisfied.The method regularizes Q_uu when needed and uses first-order dynamics expansions for lower computational cost and improved numerical stability.Source: A. Unconstrained Differential Dynamic… · p. 2

B. Constrained optimization theory

• The constrained formulation considers inequality constraints, while noting that the results can be extended to equality constraints.The KKT discussion assumes differentiability and linear independence of active-constraint gradients.Source: B. Constrained optimization theory · p. 2, 1) KKT conditions · p. 3, §III · p. 3

1) KKT conditions:

• KKT conditions provide necessary optimality conditions for local solutions of the constrained problem under differentiability and active-gradient independence assumptions.The associated Lagrangian uses multipliers for the constraint vector.Source: 1) KKT conditions · pp. 2–3

2) Augmented Lagrangian:

• The Augmented Lagrangian combines multipliers, penalty parameters, and an inequality penalty function, with minimization yielding a solution under mild assumptions when the penalty has suitable properties.Its optimization is organized into an inner local-minimization loop and an outer multiplier-update loop.Source: 2) Augmented Lagrangian · p. 3
• Penalty parameters increase monotonically when constraint improvement is unsatisfactory.The outer loop updates multipliers through the derivative of the penalty function.Source: 2) Augmented Lagrangian · p. 3
• The standard Powell-Hestenes-Rockafellar penalty is not twice differentiable, which can cause numerical instability inside second-order algorithms.This smoothness limitation motivates penalty functions tailored to preserve the differentiability required by DDP convergence analysis.Source: 2) Augmented Lagrangian · p. 3

A. Backward Pass

• The constrained backward pass operates on quadratic approximations of the Q functions about nominal rollouts and uses slack variables satisfying si+gi = 0 and si ≥0, with perturbations of slack variables and Lagrange multipliers.Source: A. Backward Pass · p. 3
• The KKT system is solved with a primal-dual interior-point method that maintains nonnegative slacks and multipliers.Initialization uses positive multipliers and a small positive slack offset for numerical stability.Source: A. Backward Pass · pp. 3–4
• A nonzero complementarity target makes Newton directions less aggressive, allowing larger steps while driving the duality measure toward zero.The centering parameter satisfies 0 < σ < 1, and the duality measure is reduced to a threshold of 0.01 in the implementation.Source: A. Backward Pass · pp. 4–5
• The reformulated system avoids singularity issues because ΛS approaches µ_kσ_k rather than zero.This change addresses near-singularity when slack variables approach constraint boundaries.Source: A. Backward Pass · p. 4
• After the constrained solve, feedforward and feedback gains are computed and used to update the value functions within the backward recursion.The algorithm repeats KKT solves, step-size selection, nominal updates, and duality updates before storing Q derivatives.Source: A. Backward Pass · pp. 4–5
• Constraint influence is propagated one or two time steps forward so controls can affect velocity and position constraints at the appropriate future states.Position constraints are propagated two steps, while velocity constraints are propagated one step.Source: A. Backward Pass · p. 5

B. Forward Pass

• The forward pass solves a quadratic program for each control update subject to linearized constraints, then propagates the resulting state and control trajectory.The updated state is used to form deviations from the nominal trajectory during the rollout.Source: B. Forward Pass · pp. 5–6
• The feasibility check marks the forward-pass rollout unsuccessful when any terminal constraint g(xtemp,N) exceeds zero, setting flag = False, reducing ∆ by η∆, and breaking.Source: B. Forward Pass · p. 6
• The forward-pass algorithm accepts a candidate when its total cost improves, then decreases regularization parameters for subsequent iterations.The candidate nominal state and control sequences replace the previous trajectory after acceptance.Source: B. Forward Pass · p. 6
• The final control step uses box constraints because the horizon lacks the future position constraints needed for two-step propagation.This terminal handling accounts for the fact that the required future state is unavailable at the end of the horizon.Source: B. Forward Pass · p. 6
• When a candidate is infeasible, the adaptive trust region is reduced repeatedly until the QP solution becomes feasible.Shrinking the region keeps the new trajectory closer to the previously feasible trajectory.Source: B. Forward Pass · p. 6

C. Regularization

• Regularization affects DDP convergence and follows the scheme and scheduling technique proposed in.Source: C. Regularization · p. 6
• The regularized derivatives calculate gains k and K and update the value function during the backward pass.Source: C. Regularization · p. 6
• ν1 keeps the new trajectory closer to the previous one, while ν2 makes the control step conservative and Quu positive definite.Source: C. Regularization · p. 6

A. AL-DDP and penalty function

• AL-DDP penalizes inequality state constraints while DDP implicitly satisfies the dynamics constraints.Source: A. AL-DDP and penalty function · p. 6
• AL-DDP uses unconstrained DDP to optimize the transformed problem, then updates Lagrange multipliers and penalty parameters.Source: A. AL-DDP and penalty function · p. 6
• The selected penalty function is twice differentiable, satisfying DDP’s smoothness requirement and approximating the Powell-Hestenes-Rockafellar method.Source: A. AL-DDP and penalty function · p. 6

B. Combination of AL-DDP and KKT frameworks

• The combined method starts with AL-DDP, switches to KKT after specified cost and constraint precision, and can switch back if improvement is insufficient.Source: B. Combination of AL-DDP and KKT… · p. 7
• AL initialization helps the KKT method handle state and control constraints that conflict when the initial state is far from the goal.Source: B. Combination of AL-DDP and KKT… · p. 7

V. RESULTS

• The experiments compare constrained DDP methods using trajectories from multiple initial points and initial trajectories.Source: §V · p. 7, A. S-KKT · p. 7, Figure 1 · p. 8

1) 2D car:

• The 2D-car task reaches xg = [3, 3, π/2, 0] while avoiding three obstacles under steering and velocity controls.Source: 1) 2D car · p. 7
• S-KKT handles slightly infeasible initial trajectories, with experiments confirming successful initialization violations up to 0.3 in the 2D-car setting.Source: 1) 2D car · p. 7
• Introducing slack variables produces smoother trajectories, the lowest converged cost, and recovery from prohibited regions in the 2D-car experiments.Source: 1) 2D car · p. 7

2) Quadroter:

• S-KKT has the best performance among former KKT, active-set, and S-KKT on the quadrotor obstacle-avoidance task.Source: 2) Quadroter · pp. 7–8

1) Control constraints:

• AL-DDP is fast initially but can oscillate near constraint boundaries, whereas S-KKT is slower per iteration but maintains feasibility.Source: 1) Control constraints · p. 8
• The combination initializes S-KKT with an AL-DDP trajectory, matching S-KKT's strength in refining near-optimal trajectories while reducing failures caused by large required controls.Source: 1) Control constraints · p. 8
• When its forward-pass QP is infeasible, the method contracts box control constraints by multiplying them by 0 < η < 1 and resolves the problem.Source: 1) Control constraints · p. 8

2) 2D car:

• The combined algorithm successfully handles both state and control constraints in the 2D-car experiment.Source: 2) 2D car · p. 8
• Under tight steering limits, the car changes its obstacle-avoidance route and may fail to reach the goal despite applying maximum steering.Source: 2) 2D car · p. 8

C. Performance Analysis

• The performance study compares SQP, S-KKT, AL-DDP, AL-DDP with SQP, and AL-DDP with S-KKT using cost, time, and feasibility across cart-pole, 2D-car, and quadrotor systems.Source: C. Performance Analysis · p. 9
• Feasibility is evaluated separately for constraint-function violations and dynamics, with DDP-based methods satisfying dynamics implicitly during optimization.Source: C. Performance Analysis · p. 9

1) Exit criteria:

• The experiments use constraint satisfaction and optimality-based stopping criteria, with distinct settings for DDP-based methods, SQP, and the two augmented-Lagrangian combinations.Source: 1) Exit criteria · pp. 9–10, Table I · p. 10

2) Cart Pole:

• AL-S-KKT converges faster than S-KKT at H = 100 with nearly equal cost, and achieves lower cost at H = 200 despite slightly longer runtime.Compared with AL alone, AL-S-KKT takes longer but satisfies feasibility.Source: 2) Cart Pole · p. 11
• Longer horizons reduce SQP speed and constraint satisfaction, whereas AL-S-KKT is less affected in the cart pole experiments.S-KKT can retain small constraint violations from linearization error, especially when stopped before convergence.Source: 2) Cart Pole · p. 11

3) 2D car:

• Across 2D car experiments, combination methods clarify their speed advantage, with performance averaged over six initializations and evaluated both to convergence and under time budgets.The experiments use dt = 0.02 s, with time budgets of 3 s for H = 100 and 6 s for H = 200.Source: 3) 2D car · p. 12

4) Quadroter:

• In quadroter experiments, SQP becomes prohibitively slow as dimension increases, while S-KKT and AL-S-KKT retain stability, feasibility, and low cost in short time.AL failed to converge for H = 300, and under time budgets AL-S-KKT lost speed because AL consumed most of the available budget.Source: 4) Quadroter · p. 12

VI. CONCLUSION

• The conclusion reports that slack variables lower cost relative to active-set alternatives, while S-KKT handles state and control constraints in small feasibility sets.S-KKT is also reported as more robust to initial state trajectories.Source: §VI · p. 12
• AL is fast during its first iterations but slows near constraints and can violate them, whereas S-KKT is slower per iteration but preserves feasibility; combining them can offset these weaknesses.The paper identifies uncertainty representations, learning, and chance-constrained trajectory optimization as future directions.Source: §VI · p. 12

• The penalty derivative P′(y, λ, µ) must be continuous for all y ∈ R, λ, µ ∈ R++.Source: Appendix A · p. 12
• The derivative must satisfy P′(y, λ, µ) ≥ 0.Source: Appendix A · p. 12
• As µ diverges and λ approaches a positive limit, P′ must diverge; when λ approaches a negative limit, P′ must approach zero.Source: Appendix A · pp. 12–13