9 Numerical Methods for Optimal Control

Author

Affiliation

Enrico Bertolazzi

University of Trento, Department of Industrial Engineering

The necessary conditions developed in the preceding chapters transform an optimal control problem into differential equations, algebraic conditions, and endpoint constraints. A numerical method must still choose a finite representation of the trajectories and solve the resulting nonlinear system or optimization problem.

This chapter develops the two principal approaches:

indirect methods, which discretize the optimality system derived from Pontryagin’s principle;
direct methods, which discretize the state and control first and solve a finite-dimensional nonlinear programming problem.

Dynamic programming provides a third viewpoint through the Hamilton–Jacobi–Bellman equation. It is conceptually fundamental, but a state-space grid becomes prohibitively expensive as the dimension grows. The discussion below therefore concentrates on shooting, transcription, and collocation methods used for trajectory optimization [@betts2010practical].

Learning objectives

After studying this chapter, the reader should be able to

distinguish indirect shooting from direct transcription;
formulate single- and multiple-shooting residuals;
derive trapezoidal and Hermite–Simpson defect constraints;
explain how sensitivities and adjoints provide derivatives;
transcribe endpoint, control, path, and free-time constraints;
recognize the sparse structure of the resulting NLP;
assess mesh accuracy, feasibility, and first-order optimality.

9.1 Reference optimal control problem

Consider the Bolza problem

\begin{aligned} \operatorname*{minimize}_{\bm x,\bm u,\bm p,T}\quad &J =\Phi\big(\bm x(0),\bm x(T),\bm p,T\big) +\int_0^T L(\bm x,\bm u,\bm p,t)\,\mathrm dt,\\ \text{subject to}\quad &\bm x'(t)=\bm f(\bm x,\bm u,\bm p,t),\\ &\bm b\big(\bm x(0),\bm x(T),\bm p,T\big)=\bm0,\\ &\bm c(\bm x,\bm u,\bm p,t)\leq\bm0,\\ &\bm u_{\min}\leq\bm u(t)\leq\bm u_{\max}. \end{aligned} \tag{9.1}

The vector \bm p contains constant decision parameters and T may be fixed or free. Equality path constraints can be included by replacing selected components of \bm c with equalities.

A numerical approximation should be judged on four distinct properties:

dynamic consistency: the state approximately satisfies the ODE;
feasibility: endpoint and path constraints are satisfied;
optimality: the discrete KKT residual is small;
mesh accuracy: the result is stable under refinement.

A small NLP residual alone does not guarantee that the continuous-time OCP has been accurately approximated.

9.2 Optimize then discretize or discretize then optimize

An indirect method first derives the continuous necessary conditions and then discretizes them. This is often described as optimize then discretize. A direct method first creates a finite-dimensional approximation and then derives the KKT conditions of that approximation: discretize then optimize.

Approach	Unknowns	Equations solved	Main strength	Main difficulty
Indirect shooting	missing state/costate data, switching times, parameters	boundary residuals of the PMP system	high local accuracy and physical interpretation	sensitive initialization and switching structure
Direct single shooting	control parameters and static parameters	NLP with states eliminated by integration	few optimization variables	dense, ill-conditioned sensitivities
Multiple shooting	controls, parameters, and states at subinterval boundaries	sparse NLP with continuity constraints	improved robustness	more variables and constraints
Direct collocation	state and control values at mesh/collocation points	sparse NLP with defect constraints	robust handling of constraints	mesh design and transcription error

For a consistent discretization, the discrete adjoints of a direct method normally approach the continuous costates as the mesh is refined. On a coarse mesh, however, the two optimality systems need not coincide exactly.

9.3 Indirect methods

With the Hamiltonian convention

H=L+\bm\lambda^{\mathsf T}\bm f,

an interior normal extremal satisfies

\begin{aligned} \bm x'&=H_{\bm\lambda}=\bm f,\\ \bm\lambda'&=-H_{\bm x},\\ \bm0&=H_{\bm u}, \end{aligned} \tag{9.2}

together with endpoint and transversality conditions. Control bounds replace H_{\bm u}=\bm0 by pointwise minimization of the Hamiltonian.

After eliminating the control when possible, the state and costate equations form a two-point boundary-value problem. Shooting solves this BVP by converting the missing boundary data into finite-dimensional unknowns.

Single shooting

Assume for simplicity that \bm x(0)=\bm x_0 is fixed and that the unknown initial costate is \bm\nu=\bm\lambda(0). For every trial \bm\nu, integrate the canonical system from 0 to T and evaluate the terminal residual

\bm R(\bm\nu) =\bm r\big(\bm x(T;\bm\nu),\bm\lambda(T;\bm\nu)\big). \tag{9.3}

The shooting problem is

\bm R(\bm\nu)=\bm0.

Newton’s method requires

\bm R_{\bm\nu}(\bm\nu_k)\,\delta\bm\nu_k =-\bm R(\bm\nu_k), \qquad \bm\nu_{k+1}=\bm\nu_k+\delta\bm\nu_k.

The Jacobian may be computed by finite differences, automatic differentiation, or variational equations. If \bm z=(\bm x^{\mathsf T},\bm\lambda^{\mathsf T})^{\mathsf T} satisfies

\bm z'=\bm F(\bm z,t),

then the sensitivity matrix

\bm S(t)=\frac{\partial\bm z(t)}{\partial\bm\nu}

satisfies

\bm S'=\bm F_{\bm z}(\bm z,t)\bm S, \qquad \bm S(0)= \begin{bmatrix} \bm0\\ \bm I \end{bmatrix}. \tag{9.4}

The terminal value \bm S(T) supplies the shooting Jacobian by the chain rule.

Single shooting is compact and can be very accurate near a known extremal. It becomes fragile when unstable dynamics amplify small errors in the guessed costate, when the horizon is long, or when the switching structure is unknown.

A converged shooting root is not automatically an optimum

Shooting enforces first-order necessary conditions. The computed extremal may be a maximum, a saddle, an abnormal solution, or a member of a nonunique family. Curvature, neighboring extremals, and the original objective must still be examined.

Multiple shooting

Partition the time interval,

0=t_0<t_1<\cdots<t_N=T,

and introduce an independent initial value \bm z_i for each subinterval. Let

\bm\varphi_i(\bm z_i)

denote the numerical flow from t_i to t_{i+1}. Continuity is imposed by

\bm z_{i+1}-\bm\varphi_i(\bm z_i)=\bm0, \qquad i=0,\ldots,N-1. \tag{9.5}

Perturbations are prevented from growing across the entire horizon before a correction is made. The price is a larger nonlinear system, but its Jacobian has a sparse block-banded structure. Multiple shooting is therefore often more robust than single shooting for unstable or long-horizon problems.

Unknown switching times can be added as decision variables. Each smooth arc is integrated separately and matching, switching, and transversality conditions are imposed at the interfaces.

9.4 Direct control parameterization

The simplest direct method represents the control in a finite basis,

\bm u(t;\bm a) =\sum_{j=1}^{M}\bm a_j\psi_j(t), \tag{9.6}

and integrates the state equation for every parameter vector \bm a. The state is thereby eliminated, producing a nonlinear program in \bm a and any static parameters:

\min_{\bm a,\bm p,T}\widehat J(\bm a,\bm p,T) \quad\text{subject to}\quad \widehat{\bm b}=\bm0,\qquad \widehat{\bm c}\leq\bm0.

Piecewise-constant and piecewise-linear controls are common. The approach is also called direct single shooting.

Advantages include a small decision vector and exact dynamic feasibility up to the ODE integration tolerance. Disadvantages include dense sensitivities, poor behavior for unstable systems, and difficulty enforcing path constraints between sampled times.

Forward sensitivities

For a parameter a_j, define

\bm S_j(t)=\frac{\partial\bm x(t;\bm a)}{\partial a_j}.

Differentiating the state equation gives

\bm S_j' =\bm f_{\bm x}\bm S_j +\bm f_{\bm u}\frac{\partial\bm u}{\partial a_j}, \qquad \bm S_j(0)=\frac{\partial\bm x_0}{\partial a_j}. \tag{9.7}

Forward sensitivities are efficient when the number of decision parameters is small. Their cost grows roughly in proportion to that number.

Adjoint gradients

When the number of control parameters is large but the objective is scalar, an adjoint calculation can compute the complete objective gradient at a cost largely independent of the number of parameters. The continuous adjoint resembles the costate equation, while a discrete adjoint differentiates the actual time-stepping scheme.

For implementation and derivative verification, it is essential to state which gradient is being used:

the continuous adjoint followed by discretization;
the exact derivative of the discrete integrator;
automatic differentiation through the integrator.

The last two are consistent with the discrete NLP. A continuous adjoint may introduce a mismatch unless the state and adjoint discretizations are chosen compatibly.

9.5 Direct transcription

Direct transcription retains discrete states as optimization variables. Choose mesh points

0=t_0<t_1<\cdots<t_N=T, \qquad h_k=t_{k+1}-t_k,

and introduce

\bm X_k\approx\bm x(t_k), \qquad \bm U_k\approx\bm u(t_k).

The integral is replaced by quadrature, and the ODE is replaced by algebraic defect constraints. The resulting NLP has the schematic form

\begin{aligned} \min_{\bm X,\bm U,\bm p,T}\quad &J_h(\bm X,\bm U,\bm p,T),\\ \text{subject to}\quad &\bm D_k(\bm X_k,\bm X_{k+1},\bm U_k,\bm U_{k+1},\bm p,T)=\bm0,\\ &\bm b_h(\bm X_0,\bm X_N,\bm p,T)=\bm0,\\ &\bm c_h(\bm X,\bm U,\bm p,T)\leq\bm0. \end{aligned} \tag{9.8}

Each defect involves only neighboring mesh variables. Consequently, the Jacobian is sparse even though the number of variables may be large.

Euler transcription

The forward-Euler defect is

\bm D_k =\bm X_{k+1}-\bm X_k -h_k\bm f(\bm X_k,\bm U_k,\bm p,t_k). \tag{9.9}

This first-order formula is useful pedagogically but usually too inaccurate for serious trajectory optimization. Its discretization error can bias the computed control and objective.

Trapezoidal collocation

The trapezoidal defect is

\bm D_k =\bm X_{k+1}-\bm X_k -\frac{h_k}{2} \left[ \bm f(\bm X_k,\bm U_k,t_k) +\bm f(\bm X_{k+1},\bm U_{k+1},t_{k+1}) \right]. \tag{9.10}

The running cost is approximated consistently by

\int_{t_k}^{t_{k+1}}L\,\mathrm dt \approx \frac{h_k}{2} \left[ L(\bm X_k,\bm U_k,t_k) +L(\bm X_{k+1},\bm U_{k+1},t_{k+1}) \right].

The method is second-order accurate for sufficiently smooth solutions.

Hermite–Simpson collocation

Let t_{k+\frac12}=(t_k+t_{k+1})/2 and define

\begin{aligned} \bm f_k&=\bm f(\bm X_k,\bm U_k,t_k),\\ \bm f_{k+1}&=\bm f(\bm X_{k+1},\bm U_{k+1},t_{k+1}),\\ \bm X_{k+\frac12} &=\frac12(\bm X_k+\bm X_{k+1}) +\frac{h_k}{8}(\bm f_k-\bm f_{k+1}). \end{aligned} \tag{9.11}

With a midpoint control \bm U_{k+\frac12}, the Simpson defect is

\bm D_k =\bm X_{k+1}-\bm X_k -\frac{h_k}{6} \left( \bm f_k +4\bm f_{k+\frac12} +\bm f_{k+1} \right). \tag{9.12}

For smooth solutions this gives substantially higher accuracy than Euler or trapezoidal transcription at comparable mesh sizes. Discontinuities and switches should be placed at mesh boundaries; otherwise high-order polynomial interpolation can smear the switching structure.

General collocation and Runge–Kutta schemes

Implicit Runge–Kutta collocation introduces stage states and enforces the dynamics at collocation points. Gauss schemes provide high order, Radau schemes include one endpoint, and Lobatto schemes include both endpoints. Pseudospectral methods use global polynomial approximations and are highly accurate for smooth trajectories, but local mesh methods are usually easier to refine near switches and active path constraints.

9.6 A complete trapezoidal transcription

Consider the minimum-energy transfer

\begin{aligned} \min_{x,v,u}\quad &\frac12\int_0^1u(t)^2\,\mathrm dt,\\ \text{subject to}\quad &x'=v,\qquad v'=u,\\ &x(0)=0,\quad v(0)=0,\quad x(1)=1,\quad v(1)=0. \end{aligned} \tag{9.13}

On a uniform mesh with h=1/N, the decision variables are

\bm w =\{X_k,V_k,U_k\}_{k=0}^{N}.

The trapezoidal objective is

J_h(\bm w) =\frac{h}{4} \sum_{k=0}^{N-1}\left(U_k^2+U_{k+1}^2\right), \tag{9.14}

and the defects are

\begin{aligned} X_{k+1}-X_k-\frac h2(V_k+V_{k+1})&=0,\\ V_{k+1}-V_k-\frac h2(U_k+U_{k+1})&=0. \end{aligned} \tag{9.15}

Together with the four endpoint conditions, this is a sparse convex quadratic program. The continuous solution is

x^\star(t)=3t^2-2t^3,\qquad v^\star(t)=6t-6t^2,\qquad u^\star(t)=6-12t. \tag{9.16}

This example exposes the main ingredients of direct transcription without nonlinear complications. It also provides an exact reference for checking mesh convergence, objective error, and the signs of discrete multipliers.

9.7 Time scaling and free terminal time

For a free terminal time, introduce \tau\in[0,1] and set t=T\tau. Then

\frac{\mathrm d\bm x}{\mathrm d\tau} =T\bm f(\bm x,\bm u,\bm p,T\tau), \tag{9.17}

and

\int_0^T L\,\mathrm dt =T\int_0^1L(\bm x,\bm u,\bm p,T\tau)\,\mathrm d\tau.

The normalized mesh is fixed while T becomes an NLP variable. Bounds T_{\min}\leq T\leq T_{\max} are often useful safeguards. The derivatives with respect to T must include its effect on the dynamics, running cost, endpoint terms, and explicit time dependence.

9.8 Control, endpoint, and path constraints

Control bounds become ordinary variable bounds at control nodes. Endpoint constraints are imposed directly on \bm X_0 and \bm X_N. Path constraints are commonly imposed at every collocation point:

\bm c(\bm X_{k,j},\bm U_{k,j},\bm p,t_{k,j})\leq\bm0.

Checking constraints only at mesh nodes can miss violations between nodes. After solving, reconstruct the polynomial trajectory and evaluate the path constraints on a denser verification grid.

An active path constraint may create entry, boundary, and exit arcs. Mesh points should be added near the junctions. Complementarity information from the NLP multipliers helps identify the active intervals, but isolated multiplier spikes on a coarse mesh should not be overinterpreted.

Integral constraints can be handled either by quadrature in the NLP or by the auxiliary-state transformation developed in the preceding chapter. The auxiliary-state form is often convenient because it converts the requirement into a terminal equality and exposes an associated multiplier.

9.9 Sparse nonlinear programming structure

A direct transcription may contain thousands of variables, but each defect depends only on a small local subset. Sparse SQP and primal-dual interior-point methods exploit this structure [@wachter2006implementation].

The NLP Jacobian has a block-banded dynamics portion plus rows for endpoint and global integral constraints. The Lagrangian Hessian is also sparse for a local transcription. Eliminating all states before optimization destroys much of this sparsity, which explains why a larger multiple-shooting or collocation formulation can be faster than a smaller dense single-shooting problem.

Derivative options include:

exact analytical Jacobians;
automatic differentiation;
sparse finite differences using a known sparsity pattern;
matrix-free products for very large systems.

Finite differences are useful for verification but are often too noisy and expensive as the primary derivative source.

9.10 Scaling, initialization, and continuation

Scaling

States, controls, time, objective terms, and constraints should have comparable scaled magnitudes. A useful nondimensionalization is

\widehat x_i=\frac{x_i-x_{i,\mathrm{ref}}}{s_{x_i}}, \qquad \widehat u_j=\frac{u_j-u_{j,\mathrm{ref}}}{s_{u_j}},

with characteristic scales s_{x_i} and s_{u_j}. Poor scaling can make a well-posed OCP appear numerically singular.

Initial guess

A direct method does not require a feasible initial trajectory, but a dynamically plausible guess is valuable. Common strategies are:

interpolate endpoint states and use a neutral control;
simulate a feedback controller;
solve a relaxed or shorter-horizon problem;
warm-start from a neighboring parameter value;
solve on a coarse mesh and interpolate to a finer mesh.

Continuation

Introduce difficult features gradually. For a parameter \gamma, solve a sequence

\gamma_0,\gamma_1,\ldots,\gamma_M,

using each solution to initialize the next. Continuation can increase weights, tighten path constraints, reduce regularization, extend the horizon, or move the endpoints to their desired values.

9.12 Diagnostics and verification

The final report should include more than the solver’s success flag.

Primal feasibility

Check endpoint constraints, defect norms, control bounds, path constraints on a dense grid, and any integral constraints.

Discrete optimality

Inspect the scaled KKT residual of the NLP. For a direct method, the relevant stationarity equations are those of the discretized problem.

Dynamic verification

Reintegrate the original ODE with the computed control using tighter tolerances. Compare the resulting terminal state with the transcribed state. This test is especially important for coarse collocation meshes.

Adjoint consistency

Compare discrete NLP multipliers with the expected costate structure. After accounting for sign convention and scaling, the discrepancy should decrease under mesh refinement.

Sensitivity and initialization

Resolve the problem from several initial guesses or through different continuation paths. Different local solutions are expected for a nonconvex OCP; agreement is useful evidence but not a proof of global optimality.

Three independent tolerances

ODE integration error, mesh transcription error, and NLP termination error are different quantities. Tightening the NLP tolerance cannot repair a coarse mesh, and refining the mesh cannot compensate for inaccurate derivatives.

9.13 Choosing a numerical formulation

Problem characteristic	Recommended starting point
Smooth solution and a reliable costate guess	indirect single shooting
Unstable or long-horizon canonical dynamics	multiple shooting
Few control parameters and inexpensive simulation	direct single shooting
Path constraints or control bounds	direct collocation
Large sparse trajectory problem	sparse collocation with SQP or interior point
Bang–bang control with known switches	multiphase shooting or collocation with switch times
Unknown switching structure	coarse direct transcription followed by structure detection
High accuracy on smooth arcs	high-order Runge–Kutta or pseudospectral collocation

A productive hybrid workflow is often:

solve a coarse direct transcription from a simple initial guess;
inspect active constraints and switching structure;
refine the direct mesh;
optionally use the reconstructed costate to initialize an indirect method for high-accuracy local refinement.

9.14 Implementation checklist

State the sign convention and continuous OCP precisely.
Decide whether the solution structure is known well enough for shooting.
Choose control parameterization or collocation order.
Scale variables, objective terms, and constraints.
Supply sparse derivatives and verify them directionally.
Use a feasible or dynamically plausible initial guess when possible.
Solve on a coarse mesh before refinement.
Verify constraints between nodes.
Reintegrate the original dynamics independently.
Report mesh, feasibility, KKT, and sensitivity evidence.

9.15 Exercises

Hints: for the fundamental group, begin from the integral state equation and differentiate the shooting map; for the computational group, keep mesh and solver tolerances fixed while changing one discretization choice; for the advanced group, inspect sparsity before eliminating variables and base mesh refinement on a continuous residual.

Fundamental

Derive the single-shooting residual for the minimum-energy transfer Equation 9.13 when the initial costates are unknown.
Show that the Jacobian of the multiple-shooting matching conditions Equation 9.5 is block banded.
Derive Equation 9.10 from the integral form of the state equation.

Computational

Transcribe Equation 9.13 using forward Euler and determine the order of the terminal-state error.
Add the bound |u|\leq u_{\max} to the double-integrator problem and predict how the optimal control structure changes.
Formulate the free-time version of the double-integrator transcription using Equation 9.17.
Construct a path constraint that is satisfied at every mesh node but violated between two nodes.
Compare a forward-sensitivity gradient with a centered finite-difference directional derivative.

Advanced

Explain why eliminating all state variables makes the reduced shooting Hessian dense.
Design an h-refinement indicator based on Equation 9.18 and describe when p-refinement would be preferable.