M9-T2 — Direct Methods: Shooting and Collocation

Learning objectives

After this lesson, you should be able to:

• Write an optimal control problem as a nonlinear program by single shooting, multiple shooting and trapezoidal collocation.
• State the unknowns and the constraints of each and count them.
• Explain why the sensitivity of single shooting grows with the horizon for an unstable plant.
• Run a Newton-type method on each and compare the iterations from the same guess.

The intuition

UAV M5-T12: Kinodynamic planning searches for dynamically feasible paths by sampling, and Navigation M5-T2: Smoothing improves a path by gradient steps. A direct method is the numerical-optimization counterpart for dynamics: it keeps the dynamics exact, represents the control by a finite number of parameters, and lets a general solver find the best values.

Discretize the control, then choose how to hold the dynamics

All three methods start the same way: the control $u(t)$ is replaced by $N$ values, one per interval of length $h = T/N$. They differ in what else becomes an unknown and how the dynamics enter.

• Single shooting. Only the controls are unknowns. The state is obtained by simulating the dynamics from the initial state, so the final state is a function of all the controls, and the terminal condition is the only constraint. Few unknowns, few constraints, but the final state is a long chain of nested functions.
• Multiple shooting. The horizon is split into segments. The state at the start of each segment is an extra unknown, each segment is simulated separately, and continuity constraints say that each segment ends where the next begins. More unknowns, but each simulation is short.
• Collocation. The state at every node is an unknown, and the dynamics are enforced as equality constraints between neighbouring nodes by an integration formula, here the trapezoidal rule. There is no simulation at all: the problem is large and very sparse.

Why the unstable plant makes a difference

Hopper's pitch obeys $\ddot{\theta} = 0.49\,\theta + b\,\delta$ (plus a cubic term in the example), with an unstable pole at +0.7 rad/s. A small change of the first control grows by about $e^{0.7\,t}$ before it reaches the end of the horizon. For a linear model the sensitivity of the final angle to the first control is 0.87 over a 1 s horizon, 36 over 4 s and 1130 over 8 s. Single shooting has to solve for controls whose effect on the final state spans that range: the constraint Jacobian is badly scaled, and a Newton step computed from the linearization is accurate only in a tiny neighbourhood. A segment of T/4 has a sensitivity of only 15.6 even for T = 8 s.

The same answer, three costs

When all three converge they reach the same optimum: the cost of the minimum-effort slew is 1.39×10⁻³, 2.4×10⁻⁴ and 9×10⁻⁵ for the 1, 2 and 4 s horizons, to every digit shown. The choice is about robustness and size. Single shooting is the smallest and the most fragile; collocation is the largest and the most robust, and its sparsity is what large solvers exploit.

Direct methods trade a hard problem in function space for a large but ordinary nonlinear program. Shooting methods are small and simple but carry the plant's instability across the whole horizon; multiple shooting and collocation cut that chain and converge from poor guesses.

Definitions

Transcription

The conversion of a continuous optimal control problem into a nonlinear program with finitely many unknowns.

Single shooting

Unknowns: the controls. The state is a function of them by simulation; the constraint is the terminal condition.

Multiple shooting

Unknowns: the controls and the states at the segment starts. Constraints: continuity at each junction and the terminal condition.

Collocation

Unknowns: states and controls at all nodes. Constraints: the dynamics between nodes by a quadrature rule, and the boundary conditions.

Sequential quadratic programming (SQP)

A Newton method on the optimality conditions of the nonlinear program: each step solves a quadratic program built from the linearized constraints.

Derivation: unknown counts and the growth of the sensitivity

Counting and conditioning for $N$ intervals, an $n$-state system with one control.

1. $$\text{single shooting: } N \text{ unknowns},\ n \text{ constraints}$$

   The terminal condition has n scalar equations (here 2); the controls are the only unknowns.

2. $$\text{multiple shooting with } S \text{ segments}: N + n(S - 1) \text{ unknowns},\ nS \text{ constraints}$$

   Each interior segment boundary adds n unknown states and n continuity equations; the terminal condition adds n more.

3. $$\text{collocation}: (N + 1) + nN \text{ unknowns},\ nN + n \text{ constraints}$$

   The controls at the N + 1 nodes and the states at N nodes after the initial one; the defects and the terminal condition.

The sensitivity for an unstable plant

1. $$\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}u \;\Rightarrow\; \mathbf{x}(T) = e^{\mathbf{A}T}\mathbf{x}_0 + \int_0^T e^{\mathbf{A}(T - s)}\mathbf{B}u(s)\,ds$$

   The variation of constants for a linear model: the final state is a linear function of the controls with weights $e^{\mathbf{A}(T-s)}\mathbf{B}$.

2. $$\left\|\dfrac{\partial\mathbf{x}(T)}{\partial u_0}\right\| \approx h\,\|e^{\mathbf{A}T}\mathbf{B}\| \sim e^{\lambda T}$$

   The weight on the first control is the largest. With an unstable eigenvalue $\lambda = 0.7$ it grows as $e^{\lambda T}$: 2.0 at 1 s, 16 at 4 s, 270 at 8 s (the finite-difference values are 0.87, 36 and 1130 for this plant).

3. $$\left\|\dfrac{\partial\mathbf{x}(T/S)}{\partial u_0}\right\| \sim e^{\lambda T/S}$$

   In multiple shooting each segment has its own unknown start, so a control affects only the end of its segment: the weight is that of a horizon of T/S, a tamer $e^{\lambda T/S}$.

4. $$\text{Newton steps converge quadratically only inside a neighbourhood of size} \sim \epsilon/(\text{curvature}\times\text{sensitivity}^2)$$

   The linearization is accurate for steps small compared with the inverse of the sensitivity times the curvature of the dynamics; with sensitivities in the hundreds the neighbourhood is tiny and the zero guess lies outside it.

Single shooting chains the whole horizon into one function, so its Jacobian entries grow like e^{λT}; multiple shooting and collocation add the intermediate states as unknowns, which breaks the chain into pieces whose sensitivity depends on T/S or h only.

A worked example

Minimum control effort $\int\delta^2dt$ to bring Hopper's pitch from 0.2 rad to rest at 0 in time $T$, for $\ddot\theta = 0.49\theta + 0.6\theta^3 + 19.62\,\delta$ (the cubic term is added only to make the problem nonlinear). 24 intervals, multiple shooting with 4 segments, Gauss-Newton SQP from the all-zero guess.

1. Sizes. Single shooting: 24 unknowns, 2 constraints. Multiple shooting: 30 unknowns, 8 constraints. Collocation: 25 + 48 = 73 unknowns, 50 constraints.
2. Short horizons. At T = 1 s all three need 3 iterations and reach the cost 0.00139; at T = 2 s they need 4, 3 and 3 and reach 0.00024.
3. Longer. At T = 4 s single shooting needs 11 iterations; multiple shooting and collocation need 4. All three reach the cost 0.00009.
4. Divergence. At T = 5, 6 and 8 s single shooting diverges (the simulation overflows), while multiple shooting and collocation need 5 iterations each and reach 0.00008.
5. Why. For the linear model the sensitivity of the final angle to the first control is 0.87, 4.3, 36, 214 and 1130 for T = 1, 2, 4, 6 and 8 s; one segment of T/4 has 0.19, 0.76, 3.2, 7.8 and 15.6. The sensitivity to be handled by Newton's method is smaller by 70 times at 8 s.

import math
import gnckit as g

lam, b, beta = 0.7, 19.62, 0.6          # Hopper pitch: theta'' = lam^2 theta + beta theta^3 + b delta (the cubic term is a made-up stiffening to make the problem nonlinear)
th0 = 0.2

def f(x, u):
    return [x[1], lam ** 2 * x[0] + beta * x[0] ** 3 + b * u]

def rk4(x, u, h):
    k1 = f(x, u)
    k2 = f([x[i] + 0.5 * h * k1[i] for i in range(2)], u)
    k3 = f([x[i] + 0.5 * h * k2[i] for i in range(2)], u)
    k4 = f([x[i] + h * k3[i] for i in range(2)], u)
    return [x[i] + h / 6 * (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) for i in range(2)]

def jac_fd(fun, x, eps=1e-6):
    f0 = fun(x)
    J = [[0.0] * len(x) for _ in f0]
    for j in range(len(x)):
        xp = list(x); xp[j] += eps
        fp = fun(xp)
        for i in range(len(f0)):
            J[i][j] = (fp[i] - f0[i]) / eps
    return f0, J

def sqp(cost_grad, cost_hess, cons, x0, iters=40):
    """Gauss-Newton SQP for min cost s.t. cons(x) = 0: Newton on the KKT conditions with the cost Hessian only, and step halving on a merit function."""
    x = list(x0)
    n = len(x)
    for it in range(iters):
        c, J = jac_fd(cons, x)
        gr = cost_grad(x)
        Hm = cost_hess(x)
        p = len(c)
        K = [[0.0] * (n + p) for _ in range(n + p)]
        for i in range(n):
            for j in range(n): K[i][j] = Hm[i][j] + (1e-9 if i == j else 0.0)
        for k in range(p):
            for j in range(n):
                K[n + k][j] = J[k][j]; K[j][n + k] = J[k][j]
        rhs = [-v for v in gr] + [-v for v in c]
        try:
            sol = g.mat_solve(K, rhs)
        except ValueError:
            return x, it, float('inf')
        dx = sol[:n]
        cn = math.sqrt(sum(v * v for v in c))
        if cn < 1e-9 and math.sqrt(sum(v * v for v in dx)) < 1e-9:
            return x, it, cn
        mu = 50.0
        def merit(z):
            cz = cons(z)
            return 0.0 + mu * math.sqrt(sum(v * v for v in cz))
        s = 1.0
        m0 = merit(x) + 0.5 * sum(x[i] * 0 for i in range(n))
        while s > 1e-4:
            xn = [x[i] + s * dx[i] for i in range(n)]
            if merit(xn) < m0 or cn < 1e-6:
                break
            s *= 0.5
        x = [x[i] + s * dx[i] for i in range(n)]
    c = cons(x)
    return x, iters, math.sqrt(sum(v * v for v in c))

def problem(T, N, method, segs=None):
    h = T / N
    nu = N
    if method == 'single':
        def cons(z):
            x = [th0, 0.0]
            for k in range(N): x = rk4(x, z[k], h)
            return [x[0], x[1]]
        n = nu
        def unpack_u(z): return z[:nu]
        x0 = [0.0] * n
    elif method == 'multiple':
        S = segs
        per = N // S
        def cons(z):
            u = z[:nu]; st = z[nu:]            # node states 1..S-1 (two each)
            out = []
            xs = [th0, 0.0]
            for s_ in range(S):
                x = xs
                for k in range(per): x = rk4(x, u[s_ * per + k], h)
                if s_ < S - 1:
                    nxt = st[2 * s_: 2 * s_ + 2]
                    out += [x[0] - nxt[0], x[1] - nxt[1]]
                    xs = nxt
                else:
                    out += [x[0], x[1]]
            return out
        n = nu + 2 * (S - 1)
        x0 = [0.0] * n
    else:                                     # trapezoidal collocation: states at all nodes
        def cons(z):
            u = z[:nu + 1]; st = z[nu + 1:]
            X = [[th0, 0.0]] + [st[2 * k: 2 * k + 2] for k in range(N)]
            out = []
            for k in range(N):
                f1 = f(X[k], u[k]); f2 = f(X[k + 1], u[k + 1])
                for i in range(2):
                    out.append(X[k + 1][i] - X[k][i] - 0.5 * h * (f1[i] + f2[i]))
            out += [X[N][0], X[N][1]]
            return out
        n = nu + 1 + 2 * N
        x0 = [0.0] * n
    nuc = nu + 1 if method == 'collocation' else nu
    wts = [h * (0.5 if (method == 'collocation' and k in (0, nuc - 1)) else 1.0) for k in range(nuc)]
    def cost_grad(z): return [2 * wts[k] * z[k] if k < nuc else 0.0 for k in range(len(z))]
    def cost_hess(z):
        H = [[0.0] * len(z) for _ in z]
        for k in range(nuc): H[k][k] = 2 * wts[k]
        return H
    return cons, cost_grad, cost_hess, x0, n

print('minimum control effort (integral of delta^2) to bring pitch from 0.2 rad to rest at 0 in T seconds; theta_ddot = 0.49 theta + 0.6 theta^3 + 19.62 delta')
print('Gauss-Newton SQP from the zero guess: iterations to reach |constraint| < 1e-9, and the cost')
for T in (1.0, 2.0, 4.0, 5.0, 6.0, 8.0):
    row = []
    for method, segs in (('single', None), ('multiple', 4), ('collocation', None)):
        N = 24
        cons, cg, ch, x0, n = problem(T, N, method, segs)
        try:
            x, it, cn = sqp(cg, ch, cons, x0, iters=40)
        except (OverflowError, ValueError):
            x, it, cn = x0, 40, float('inf')
        cost = sum(((0.5 if (method == 'collocation' and k in (0, N)) else 1.0) * (T / N) * x[k] ** 2) for k in range(N + 1 if method == 'collocation' else N))
        ok = cn < 1e-7
        row.append(f'{method:11s} ' + (f'{it:2d} iterations, cost {cost:.5f}' if ok else f'diverged'))
    print(f'  T = {T:3.1f} s: ' + ' | '.join(row))

print('why: the sensitivity of the final angle to the first control, by finite differences about u = 0 in the linear model (single shooting needs it over the whole horizon, multiple shooting over one segment of T/4)')
beta = 0.0
def final_angle(T, u0, steps_):
    h = T / 24
    x = [th0, 0.0]
    for k in range(steps_):
        x = rk4(x, u0 if k == 0 else 0.0, h)
    return x[0]
for T in (1.0, 2.0, 4.0, 6.0, 8.0):
    full = (final_angle(T, 1e-6, 24) - final_angle(T, 0.0, 24)) / 1e-6
    seg = (final_angle(T, 1e-6, 6) - final_angle(T, 0.0, 6)) / 1e-6
    print(f'  T = {T:3.1f} s: d theta(T) / d u0 = {abs(full):10.3g} (one segment: {abs(seg):8.3g}); exp(0.7 T) = {math.exp(0.7 * T):9.3g}')

minimum control effort (integral of delta^2) to bring pitch from 0.2 rad to rest at 0 in T seconds; theta_ddot = 0.49 theta + 0.6 theta^3 + 19.62 delta
Gauss-Newton SQP from the zero guess: iterations to reach |constraint| < 1e-9, and the cost
  T = 1.0 s: single       3 iterations, cost 0.00139 | multiple     3 iterations, cost 0.00139 | collocation  3 iterations, cost 0.00139
  T = 2.0 s: single       4 iterations, cost 0.00024 | multiple     3 iterations, cost 0.00024 | collocation  3 iterations, cost 0.00024
  T = 4.0 s: single      11 iterations, cost 0.00009 | multiple     4 iterations, cost 0.00009 | collocation  4 iterations, cost 0.00009
  T = 5.0 s: single      diverged | multiple     5 iterations, cost 0.00008 | collocation  5 iterations, cost 0.00008
  T = 6.0 s: single      diverged | multiple     5 iterations, cost 0.00008 | collocation  5 iterations, cost 0.00008
  T = 8.0 s: single      diverged | multiple     5 iterations, cost 0.00008 | collocation  5 iterations, cost 0.00008
why: the sensitivity of the final angle to the first control, by finite differences about u = 0 in the linear model (single shooting needs it over the whole horizon, multiple shooting over one segment of T/4)
  T = 1.0 s: d theta(T) / d u0 =      0.865 (one segment:    0.188); exp(0.7 T) =      2.01
  T = 2.0 s: d theta(T) / d u0 =        4.3 (one segment:    0.762); exp(0.7 T) =      4.06
  T = 4.0 s: d theta(T) / d u0 =       36.1 (one segment:     3.21); exp(0.7 T) =      16.4
  T = 6.0 s: d theta(T) / d u0 =        214 (one segment:     7.84); exp(0.7 T) =      66.7
  T = 8.0 s: d theta(T) / d u0 =   1.13e+03 (one segment:     15.6); exp(0.7 T) =       270

In robotics

Direct transcription is how most trajectory optimization is done in practice.

• Launch and descent. Production trajectory optimizers for launch vehicles, spacecraft transfers and landers use collocation or multiple shooting inside large sparse solvers (SNOPT and IPOPT are common), with tens of thousands of variables.
• Legged and aerial robots. Whole-body motion plans and aggressive quadrotor trajectories are produced by direct collocation, which handles contacts and constraints as additional equations.
• Warm starts. The sensitivity analysis above is why the first guess matters: a good one is a simpler trajectory, a previous solution, or a solution of an easier convex problem, as in the next lessons.
• Accuracy and cost. Trapezoidal collocation is second-order accurate in the step; higher-order schemes (Hermite-Simpson, pseudospectral methods) give more accuracy per node. Shooting with an adaptive integrator gives the accuracy of the integrator.
• Limits. These methods find a local optimum, not the global one, and they take seconds to minutes. For onboard use the problem is usually made convex (next lessons) or solved by a fast structured method.

The explicit guidance laws of M8-T3: Explicit Guidance to a Terminal State are the closed-form solutions of the simplest problems of this kind. The next lesson changes the problem class so that the solution is global and the solve time is bounded: M9-T3: Convex Optimization for Guidance.

Common misconceptions

Key takeaways

1. Single shooting: unknowns are the controls, the state comes from simulation. Multiple shooting adds segment states and continuity constraints. Collocation makes every node state an unknown with defect constraints.
2. All three give the same optimum; in the example the cost was 0.00139, 0.00024 and 0.00009 for 1, 2 and 4 s.
3. For an unstable plant the sensitivity of the final state to the first control grows like $e^{\lambda T}$: 0.87 at 1 s, 36 at 4 s, 1130 at 8 s.
4. From the zero guess single shooting needs 11 iterations at 4 s and diverges at 5 s; multiple shooting and collocation need 3 to 5 at every horizon.
5. The Newton step solves the KKT system $[\mathbf{H}\ \mathbf{C}^{\mathsf{T}};\ \mathbf{C}\ \mathbf{0}]$ built from the linearized constraints.
6. The result is a local optimum, found in seconds to minutes: for onboard use the problem is made convex.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• J. T. Betts, Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, 2nd edition, SIAM, 2010, the standard reference on transcription. Paid.
• M. Kelly, "An introduction to trajectory optimization: how to do your own direct collocation", SIAM Review 59(4), 2017, a tutorial with code. Free to read from the author's site and arXiv; licence stated there.
• R. Tedrake, Underactuated Robotics, the chapter on trajectory optimization , covering shooting, collocation and SQP. Free to read; licence CC BY-NC-ND 4.0.
• Z. Manchester, CMU 16-745 Optimal Control and Reinforcement Learning , with notes and notebooks on direct collocation. Free to read; no licence statement was found.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M9-T1: The Optimal Control Problem and the Minimum Principle

  State a problem by its cost, dynamics and constraints, apply Pontryagin's minimum principle, and obtain the bang-bang solution of a minimum-time and a minimum-fuel problem.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M5-T8: Model Predictive Control
• UAV M5-T9: Trajectory generation
• UAV M5-T12: Kinodynamic planning
• Navigation M5-T6: LQR and MPC for Path Tracking
• Navigation M5-T3: Curvature, Velocity and Acceleration Limits
• Navigation M5-T2: Smoothing: Shortcuts, Splines and Gradient Methods
• Robot Learning M7-T5: Model Predictive Control with Learned Models
• Robot Learning M4-T1: Markov Decision Processes

This course leaves the following to the pages that already teach them:

• Minimum-snap polynomial trajectories and differential flatness of a quadrotor
• Path smoothing and velocity profiles for ground robots
• Sampling-based planning with a learned model (random shooting, CEM)
• Reinforcement learning