Source-linked AI summary
GuSTO: Guaranteed Sequential Trajectory Optimization via Sequential Convex Programming
Riccardo Bonalli, Abhishek Cauligi, Andrew Bylard, Marco Pavone
TL;DR
Trajectory optimization methods often lack rigorous guarantees and are tailored to specific optimal-control setups. GuSTO generalizes sequential convex programming for control-affine systems with drift, and the paper reports faster, more accurate solutions than recent state-of-the-art SCP solvers in numerical simulations and experiments.
Problem
Many trajectory-optimization methods lack rigorous guarantees and are tailored to specific optimal-control setups, while practical guidance for new systems and problem setups remains limited.
Source: §I · p. 1Method
GuSTO applies sequential convex programming to drift control-affine systems, supporting control and state constraints, goal sets, and fixed or free final time.
Source: §I · p. 2, A. Trajectory Optimization as an… · p. 2Results
Numerical simulations and experiments show more accurate solutions achieved faster than with recent state-of-the-art SCP solvers.
Source: §V · p. 6Takeaways & Limitations
The framework’s convergence guarantees are independent of the chosen time discretization scheme and method used to solve each SCP iteration.
Source: §I · p. 2
Abstract
from arXiv · showhide
Sequential Convex Programming (SCP) has recently seen a surge of interest as a tool for trajectory optimization. However, most available methods lack rigorous performance guarantees and they are often tailored to specific optimal control setups. In this paper, we present GuSTO (Guaranteed Sequential Trajectory Optimization), an algorithmic framework to solve trajectory optimization problems for control-affine systems with drift. GuSTO generalizes earlier SCP-based methods for trajectory optimization (by addressing, for example, goal-set constraints and problems with either fixed or free final time) and enjoys theoretical convergence guarantees in terms of convergence to, at least, a stationary point. The theoretical analysis is further leveraged to devise an accelerated implementation of GuSTO, which originally infuses ideas from indirect optimal control into an SCP context. Numerical experiments on a variety of trajectory optimization setups show that GuSTO generally outperforms current state-of-the-art approaches in terms of success rates, solution quality, and computation times.
I. INTRODUCTION
Trajectory optimization methods still lack practical guidance, rigorous guarantees, and broad applicability across systems and problem setups. GuSTO addresses these gaps with a generalized SCP framework and an optimal-control formulation that accommodates fixed or free final time and goal-set constraints.
I. INTRODUCTION
- Existing trajectory optimization methods leave a gap in practical guidance, behavioral guarantees, and exploiting optimal control theory to improve performance.The paper identifies these as needs across a broad class of robotic systems and problem setups.Source: §I · p. 1
- Earlier SCP methods have distinct limitations: TrajOpt penalizes dynamics, depends on a particular discretization, and cannot ensure hard enforcement of dynamics.Free-final-time and goal-set extensions are also unaddressed in most of these works.Source: §I · pp. 1–2
- GuSTO generalizes continuous-time SCP for drift control-affine systems, with control and state constraints, goal sets, and fixed or free final time.Its analysis establishes convergence to a Pontryagin Maximum Principle stationary point and supports initializing shooting methods with SCP dual solutions.Source: §I · p. 2
A. Trajectory Optimization as an Optimal Control Problem
- The optimal-control formulation minimizes an integral cost under drift control-affine dynamics and bounded controls, while allowing a fixed or free final time.State-constraint violations are penalized in the formulation; the algorithm can still enforce hard state constraints up to a chosen tolerance ε.Source: A. Trajectory Optimization as an… · p. 2
B. Sequential Convex Programming
The SCP scheme repeatedly linearizes the optimal-control problem around prior solutions to form convex subproblems. Its assumptions ensure subproblem solutions exist, while trust-region constraints and penalties guide the iteration.
B. Sequential Convex Programming
- At each iteration, SCP linearizes nonlinear contributions around local solutions and constructs a new Linearized Optimal Control Problem (LOCP).The new problem uses linearized dynamics and a modified cost with penalized state-constraint violations.Source: B. Sequential Convex Programming · pp. 2–3
- The LOCP uses trust-region-type state constraints and penalizes state-constraint violations through bounded radii and nondecreasing weights.The radius and weight may vary by iteration to ease finding a solution to the next LOCP.Source: B. Sequential Convex Programming · p. 3
- The initial curve should be close to feasible or optimal for the first LOCP, but need not be feasible for the original optimal-control problem.The initial problem is linearized around the chosen state-control pair.Source: B. Sequential Convex Programming · p. 3
- Under assumptions including compact convex controls, a compact goal set, and feasible LOCPs, each iteration has an optimal solution.For free-final-time problems, feasible final times must also be uniformly bounded; the paper notes virtual control can ensure LOCP feasibility.Source: B. Sequential Convex Programming · p. 3
III. GUSTO: ALGORITHM OVERVIEW AND THEORETICAL ANALYSIS
GuSTO combines a generalized SCP setup with trust-region adaptation and a stopping rule based on successive solutions. Its iteration evaluates model accuracy, adjusts the trust region and penalties, and can start from simple, even infeasible guesses.
A. Generalized SCP Algorithm
- GuSTO’s time-continuous SCP scheme targets Pontryagin Maximum Principle stationarity, enforces dynamics, accommodates free final time and goal sets, and includes a model-accuracy ratio and stopping criterion.The ratio guides trust-region adaptation and is intended to prevent cycling within algorithm loops.Source: A. Generalized SCP Algorithm · p. 3
- GuSTO rejects a solution when the convexified model’s relative error exceeds tolerance, shrinking the trust region; otherwise it accepts the solution and updates the radius.It increases the state-constraint penalty when constraints or soft trust-region conditions are not satisfied, and ends when successive solutions are identical or the penalty exceeds ωmax.Source: A. Generalized SCP Algorithm · p. 4
- GuSTO requires no particular initialization and can start from simple, even infeasible, guesses such as a straight line in state space.When initialization is far from an optimal strategy, the maximum trust-region radius may need careful selection.Source: A. Generalized SCP Algorithm · p. 4
B. Theoretical Convergence Guarantees
GuSTO’s convergence analysis guarantees convergence to at least a stationary point under stated assumptions, while identifying constraint-violation failure as a separate termination outcome.
B. Theoretical Convergence Guarantees
- The convergence analysis assumes continuous optimal controls for each linearized problem; the authors note that many trajectory-optimization systems satisfy this condition.Normality of Pontryagin extremals is sufficient under minor assumptions.Source: B. Theoretical Convergence Guarantees · p. 4
- Under (A1)–(A4), a subsequence of optimal linearized solutions converges to a feasible OCP solution and a Pontryagin extremal.For fixed final time, the limiting final time equals the fixed time.Source: B. Theoretical Convergence Guarantees · p. 4
- The algorithm either terminates with unsatisfied state constraints, returns a stationary point, or generates solutions with a subsequence converging to a stationary point.The first case is a failure to obtain a feasible optimized strategy; the latter two are success cases.Source: B. Theoretical Convergence Guarantees · p. 4
- The continuous-time guarantees hold independently of the discretization scheme, provided sufficiently small time steps keep the discrete SCP solution close to the original OCP solution.Source: B. Theoretical Convergence Guarantees · p. 5
- Unlike, GuSTO establishes strong convergence of both trajectories and Pontryagin extremals, rather than only weak convergence up to a subsequence.Source: B. Theoretical Convergence Guarantees · p. 5
C. Accelerating Convergence using Shooting Methods
GuSTO’s convergence result supports accelerating SCP by using its dual solutions to initialize shooting methods.
C. Accelerating Convergence using Shooting Methods
- Converging SCP dual solutions approximate the initial adjoint values needed to warm-start a shooting method for the original OCP.The authors describe this as a principled way to facilitate faster convergence toward a more precise solution.Source: C. Accelerating Convergence using… · p. 5
IV. NUMERICAL EXPERIMENTS AND DISCUSSION
The experiments compare GuSTO with prior SCP methods across a spacecraft batch test and airplane initialization strategies, using a common discretization and collision-checking setup.
A. Implementation Details
- Experiments used trapezoidal discretization with zero-order-hold controls, 30–40 time points, and collision-free convergence as the success criterion.Collision-avoidance constraints for Mao et al. were enforced as hard linearized inequalities in the comparison.Source: A. Implementation Details · p. 5
B. Batch Comparison using a Simple Initialization Scheme
- GuSTO and TrajOpt each succeeded in 97% of spacecraft trials, while GuSTO was faster on average and returned higher-quality solutions.Mao et al. had a high failure rate with the simple initialization, which was often deeply in collision.Source: B. Batch Comparison using a Simple… · p. 5, Figure 3 · p. 6
C. Initialization Strategies
- The airplane tests compare three initializations, from a straight line to a dynamically feasible, collision-free trajectory generated using 4D planning with model-mismatch tubes.The experiments examine sensitivity to initialization because an initial planner may be unavailable or costly to design or run.Source: C. Initialization Strategies · pp. 5–6
- The straight-line initialization failed for all three SCP algorithms; with the highest-quality start, GuSTO and TrajOpt returned feasible trajectories, while Mao et al. did not.For that highest-quality start, run times were 0.55s for GuSTO and 2.67s for TrajOpt, with cost improvements of 55% and 49%, respectively.Source: C. Initialization Strategies · p. 6
D. Shooting Method Acceleration
GuSTO dual solutions warm-start shooting methods, reducing computation time in the tested car and free-flyer models. The paper also reports hardware experiments and summarizes its theoretical and numerical contributions.
D. Shooting Method Acceleration
- 52% and 74% less computation time for the car and free-flyer, respectively, using SCP dual solutions to warm-start shooting methods.Shooting methods converged after a few SCP iterations, while completing refinement with SCP was time-consuming.Source: D. Shooting Method Acceleration · p. 6, Table II · p. 6
E. Hardware Experiments
- GuSTO was implemented on a three-DoF free-flyer robot navigating a cluttered environment to berth with a capturing spacecraft.The robot used eight thrusters and a reaction wheel on a frictionless surface.Source: E. Hardware Experiments · p. 6
V. CONCLUSIONS
- Numerical simulations and experiments showed more accurate solutions faster than recent state-of-the-art SCP solvers, alongside theoretical guarantees intended to support broad applicability.The paper presents GuSTO as a generalized sequential-convex-programming approach for trajectory optimization.Source: §V · p. 6
- Future work would study higher-order conditions, extensions to manifolds and Lie-group systems, high-DOF robots, and free-flyer hardware experiments in a full SE(3) microgravity environment.Source: §V · p. 6
• Adjoint Equations
- For a final state on a submanifold, the adjoint vector can be chosen to satisfy transversality conditions; with free final time, an additional condition applies.The Hamiltonian is defined from the dynamics and running cost, and an extremal is normal when p0 is nonzero.Source: A. Pontryagin Maximum Principle · p. 7
- The theorem’s multipliers are more informative than Lagrange multipliers because controls need not be penalized in the cost, the Hamiltonian is globally maximized, and (p, p0) are continuous.Source: A. Pontryagin Maximum Principle · p. 7
B. Pontryagin Cone Analysis
The convergence proof uses Pontryagin cones to relate local trajectory variations to necessary optimality conditions. It also distinguishes an extremal from an actual solution of the original optimal control problem.
B. Pontryagin Cone Analysis
- The proof of the convergence theorem uses Pontryagin cones, and assumes the final time is a Lebesgue point of the control unless limiting cones are used.Source: B. Pontryagin Cone Analysis · p. 8
- The Pontryagin cone is the smallest closed convex cone containing the local variation vectors generated along the trajectory.Source: B. Pontryagin Cone Analysis · p. 8
- The Pontryagin Maximum Principle supplies a nontrivial terminal multiplier pair satisfying the cone conditions, but a Pontryagin extremal is not necessarily an OCP solution.At each SCP iteration, the principle applies to an optimal solution of that iteration’s linearized problem.Source: B. Pontryagin Cone Analysis · p. 8
1) Convergence of Trajectories and Controls:
The proof extracts convergent subsequences of final times, controls, and trajectories. Under the stated assumptions, their limit is feasible for the original optimal control problem.
1) Convergence of Trajectories and Controls:
- A subsequence of final times converges, controls converge weakly in L2, and trajectories converge strongly in C0 after weak H1 convergence.Source: 1) Convergence of Trajectories and… · p. 8
- The limit trajectory-control pair is feasible for the original problem, using feasibility of each iterate and the stated assumptions and convergences.Source: 1) Convergence of Trajectories and… · p. 8
2) Convergence of Multipliers:
The multiplier-convergence argument first transfers local variations and Pontryagin cones to the limit trajectory. It then establishes that a subsequential limit of the multipliers defines a Pontryagin extremal.
2) Convergence of Multipliers:
- Under Assumption (A4), nearby Lebesgue points of successive controls can be selected so their values approach the limit control’s value.Source: 2) Convergence of Multipliers · pp. 8–9
- Every vector in the limit Pontryagin cone can be approximated by vectors in the iteration cones.Source: 2) Convergence of Multipliers · p. 9
- A subsequence of the iteration adjoints converges strongly in C0 to an adjoint that, with the limit trajectory, control, and multiplier, forms a Pontryagin extremal.Source: 2) Convergence of Multipliers · p. 9
- Convex state-dependent cost terms need not be linearized for the numerical scheme and convergence result to hold.Source: 2) Convergence of Multipliers · p. 9
B. Airplane Model
The 8D airplane model uses position, flight and orientation variables as its state, with acceleration and rotation rates as controls. Its dynamics use lift and drag forces, and the model imposes box constraints on selected states and controls.
B. Airplane Model
- The 8D state comprises position x, y, z; course angle ψ; airspeed v; flight path angle γ; roll angle φ; and angle-of-attack α.The controls are longitudinal acceleration ua, roll rate u ˙φ, and pitch rate u ˙α.Source: B. Airplane Model · p. 10
- The continuous-time dynamics are given for the airplane model, with lift and drag forces calculated using a flat-plate airfoil model.Lift is Flift(v, α) = πρAv2α and drag is Fdrag(v, α) = ρAv2(CD0 + 4πKα2).Source: B. Airplane Model · p. 10
- Box constraints apply to states ψ, v, γ, and φ and controls ua, u ˙φ, and u ˙α.Source: B. Airplane Model · p. 10
C. Dubins Car Model
The Dubins car model is a three-dimensional kinematic system with position and orientation states and steering control. Its dynamics use constant car speed and a curvature constant.
C. Dubins Car Model
- The three-dimensional kinematic model consists of positions x and y, orientation θ, and steering control u.Source: C. Dubins Car Model · p. 10
- The continuous-time dynamics are given for the Dubins car, with v as the car's constant speed and k as a curvature constant.Source: C. Dubins Car Model · p. 10
- The model treats car speed v as constant and uses k as its curvature constant.Source: C. Dubins Car Model · p. 10