Trajectory Optimization in Continuous Time

As in the discrete-time setting, we work with three continuous-time variants that differ only in how the objective is written while sharing the same dynamics, path constraints, and bounds. The path constraints $\mathbf{g}(\mathbf{x}(t),\mathbf{u}(t))\le \mathbf{0}$ are pointwise in time, and the bounds $\mathbf{x}_{\min}\le \mathbf{x}(t)\le \mathbf{x}_{\max}$ and $\mathbf{u}_{\min}\le \mathbf{u}(t)\le \mathbf{u}_{\max}$ are understood in the same pointwise sense.

These three forms are different lenses on the same task. Bolza contains both terminal and running terms. Lagrange can be turned into Mayer by augmenting the state with an accumulator:

with the original dynamics left unchanged. Mayer is a special case of Bolza with zero running cost. We will use these equivalences freely, since a numerical method cares only about what must be evaluated and where those evaluations are taken.

With this catalog in place, we now pass from functions to finite representations.

Direct Transcription Methods¶

The discrete-time problems of the previous chapter already suggested how to proceed: we convert a continuous problem into one over finitely many numbers by deciding where to look at the trajectories and how to interpolate between those looks. We place a mesh $t_0<t_1<\cdots<t_N=t_f$ and, inside each window $[t_k,t_{k+1}]$, select a small set of interior fractions $\{\tau_j\}$ on the reference interval $[0,1]$. The running cost is additive over windows, so we write it as a sum of local integrals, map each window to $[0,1]$, and approximate each local integral by a quadrature rule with nodes $\tau_j$ and weights $w_j$. This produces

with $h_k=t_{k+1}-t_k$. The dynamics are treated in the same way by the fundamental theorem of calculus,

so the places where we “pay” running cost are the same places where we “account” for state changes. Path constraints and bounds are then enforced at the same interior times. In the infinite-horizon discounted case, the same formulas apply with an extra factor $e^{-\rho(t_k+h_k\tau_j)}$ multiplying the weights in the cost.

The values $\mathbf{x}(t_k+h_k\tau_j)$ and $\mathbf{u}(t_k+h_k\tau_j)$ do not exist a priori. We create them by a finite representation. One option is shooting: parameterize $\mathbf{u}$ on the mesh, integrate the ODE across each window with a chosen numerical step, and read interior values from that step. Another is collocation: represent $\mathbf{x}$ inside each window by a local polynomial and choose its coefficients so that the ODE holds at the interior nodes. Both constructions lead to the same structure: a nonlinear program whose objective is a composite quadrature of the running term (plus any terminal term in the Bolza case) and whose constraints are algebraic relations that encode the ODE and the pointwise inequalities at the selected nodes.

Specific choices recover familiar schemes. If we use the left endpoint as the single interior node, we obtain the forward Euler transcription. If we use both endpoints with equal weights, we obtain the trapezoidal transcription. Higher-order rules arise when we include interior nodes and richer polynomials for $\mathbf{x}$. What matters here is the unifying picture: choose nodes, translate integrals into weighted sums, and couple those evaluations to a finite trajectory representation so that cost and physics are enforced at the same places. This is the organizing idea that will guide the rest of the chapter.

Discretizing cost and dynamics together¶

In a continuous-time OCP, integrals appear twice: in the objective, which accumulates running cost over time, and implicitly in the dynamics, since state changes over any interval are the integral of the vector field. To compute, we must approximate both the integrals and the unknown functions $\mathbf{x}(t)$ and $\mathbf{u}(t)$ with finitely many numbers that an optimizer can manipulate.

A natural way to do this is to lay down a finite set of time points (a mesh) over the horizon. You can think of the mesh as a grid we overlay on the “true” trajectories that exist as mathematical objects but are not directly accessible. Our aim is to approximate those trajectories and their integrals using values and simple models tied to the mesh. Using the same mesh for both the cost and the dynamics keeps the representation coherent: we evaluate what we pay and how the state changes at consistent times.

Concretely, we begin by choosing a mesh

The running cost is additive over disjoint intervals. When the horizon $[t_0,t_f]$ is partitioned by the mesh, additivity (linearity) of the integral gives

This identity is exact: it is just the additivity (linearity) of the Lebesgue/Riemann integral over a partition. No approximation has been made yet. Approximations enter only when we later replace each window integral by a quadrature rule: a finite set of nodes and positive weights prescribing an integral approximation. This sets the table for three important ingredients that we will use throughout the chapter.

First, it turns a global object into local contributions that live on each $[t_k,t_{k+1}]$. Numerical integration is most effective when it is composite: we approximate each small interval integral and then sum the results. Doing so controls error uniformly, because the global quadrature error is the accumulation of local errors that shrink with the step size. It also allows non-uniform steps $h_k=t_{k+1}-t_k$, which we will use later for mesh refinement.

Second, the split aligns the cost with the local dynamics constraints. On each interval the ODE can be written, by the fundamental theorem of calculus, as

When we approximate this integral, we introduce interior evaluation points $t_{k,j}\in[t_k,t_{k+1}]$. Using the same points in the cost and in the dynamics ties $\mathbf{x}$ and $\mathbf{u}$ together coherently: the places where we “pay” for running cost are also the places where we enforce the ODE. This avoids a mismatch between where we approximate the objective and where we impose feasibility.

Third, the decomposition yields a nonlinear program with sparse structure. Each interval contributes a small block to the objective and constraints that depends only on variables from that interval (and its endpoints). Modern solvers exploit this banded sparsity to scale to long horizons.

With the split justified, we standardize the approximation. Map each interval to a reference domain via $t=t_k+h_k\tau$ with $\tau\in[0,1]$ and $dt=h_k\,d\tau$. A quadrature rule on $[0,1]$ is specified by evaluation points $\{\tau_j\}_{j=1}^q \subset [0,1]$ and positive weights $\{w_j\}_{j=1}^q$ such that, for a smooth $\phi$,

Applying it on each interval gives

Summing these window contributions gives a composite approximation of the integral over $[t_0,t_f]$:

The outer index $k$ indicates the window; the inner index $i$ indicates the samples within that window; the factor $h_k$ appears from the change of variables.

The dynamics admit the same treatment. By the fundamental theorem of calculus,

Replacing this integral by a quadrature rule that uses the same nodes produces the window defect relation

where $\{b_j\}$ are the weights used for the ODE. Path constraints $\mathbf{g}(\mathbf{x}(t),\mathbf{u}(t))\le 0$ are imposed at selected nodes $t_{k,j}$ in the same spirit. Using the same evaluation points for cost and dynamics keeps the representation coherent: we “pay” running cost and “account” for state changes at the same times.

On the choice of interior points¶

Once we select a mesh $t_0<\cdots<t_N$ and interior fractions $\{\tau_j\}_{j=1}^q$ per window $[t_k,t_{k+1}]$, we need $\mathbf{x}(t_{k,j})$ and $\mathbf{u}(t_{k,j})$ at the evaluation times $t_{k,j} := t_k + h_k\tau_j$. These values do not preexist. They come from one of two constructions that align with the standard quadrature taxonomy: step-function based and interpolating-function based rules.

Step-function based construction (piecewise constants; rectangle or midpoint)¶

Here we approximate the relevant time functions by step functions on each window. For controls, a common choice is piecewise-constant:

For the running cost and the vector field, the corresponding quadrature is a rectangle rule on $[t_k,t_{k+1}]$. Using the left endpoint gives

and replacing the dynamics integral by the same step-function idea yields the forward Euler relation

If we prefer the midpoint rectangle rule, we sample at $t_{k+\frac12}=t_k+\tfrac{h_k}{2}$. In practice we then generate $\mathbf{x}_{k+\frac12}$ by a half-step of the chosen integrator, and set $\mathbf{u}_{k+\frac12}=\mathbf{u}_k$ (piecewise-constant) or an average if we allow a short linear segment. Either way, interior values come from integrating forward given a step-function model for $\mathbf{u}$ and a rectangle-rule view of the integrals. This is the shooting viewpoint. Single shooting keeps only control parameters as decision variables; multiple shooting adds the window-start states and enforces step consistency.

Interpolating-function based construction (low-order polynomials; trapezoid, Simpson, Gauss/Radau/Lobatto)¶

Here we approximate time functions by polynomials on each window. If we interpolate $\mathbf{x}(t)$ linearly between endpoints, the cost naturally uses the trapezoidal rule

and the dynamics use the matched trapezoidal defect

With a quadratic interpolation that includes the midpoint, Simpson’s rule appears in the cost and the Hermite–Simpson relations tie $\mathbf{x}_{k+\frac12}$ to endpoint values and slopes. More generally, collocation chooses interior nodes on $[t_k,t_{k+1}]$ (equally spaced gives Newton–Cotes like trapezoid or Simpson; Gaussian points give Gauss, Radau, or Lobatto schemes) and enforces the ODE at those nodes:

with continuity at endpoints. The interior values $\mathbf{x}(t_{k,j})$ are evaluations of the decision polynomials; $\mathbf{u}(t_{k,j})$ follows from the chosen control interpolation (constant, linear, or quadratic). The running cost is evaluated by the same interpolatory quadrature at the same nodes, which keeps “where we pay” aligned with “where we enforce.”

Polynomial Interpolation¶

We often want to construct a function that passes through a given set of points. For example, suppose we know a function should satisfy:

These are called interpolation constraints. Our goal is to find a function $f(x)$ that satisfies all of them exactly.

To make the problem tractable, we restrict ourselves to a class of functions. In polynomial interpolation, we assume that $f(x)$ is a polynomial of degree at most $N$. That means we are trying to find coefficients $c_0, \dots, c_N$ such that

where the functions $\phi_n(x)$ form a basis for the space of polynomials. The most common choice is the monomial basis, where $\phi_n(x) = x^n$. This gives:

Other valid bases include Legendre, Chebyshev, and Lagrange polynomials, each chosen for specific numerical properties. But all span the same function space.

To find a unique solution, we need the number of unknowns (the $c_n$) to match the number of constraints. Since a degree-$N$ polynomial has $N+1$ coefficients, we need:

So if we want a function that passes through 4 points, we need a cubic polynomial ($N = 3$). Choosing a higher degree than necessary would give us infinitely many solutions; a lower degree may make the problem unsolvable.

Solving for the Coefficients (Monomial Basis)¶

If we fix the basis functions to be monomials, we can build a system of equations by plugging in each $x_i$ into $f(x)$. This gives:

This system can be written in matrix form as:

The matrix on the left is called the Vandermonde matrix. Solving this system gives the coefficients $c_n$ that define the interpolating polynomial.

Using a Different Basis¶

We don’t have to use monomials. We can pick any set of basis functions $\phi_n(x)$, such as Chebyshev or Fourier modes, and follow the same steps. The interpolating function becomes:

and each interpolation constraint becomes:

Assembling these into a system gives:

From here, we solve as before and reconstruct $f(x)$.

Derivative Constraints¶

Sometimes, instead of a value constraint $f(x_i) = y_i$, we want to impose a slope constraint $f'(x_i) = s_i$. This is common in applications like spline interpolation or collocation methods, where derivative information is available from an ODE.

Since

we can directly write the slope constraint:

To enforce this, we replace one of the interpolation equations in our system with this slope constraint. The resulting system still has $m+1$ equations and $N+1 = m+1$ unknowns.

Concretely, suppose we have $k+1$ value constraints at nodes $X=\{x_0,\ldots,x_k\}$ with values $Y=\{y_0,\ldots,y_k\}$ and $r$ slope constraints at nodes $Z=\{z_1,\ldots,z_r\}$ with slopes $S=\{s_1,\ldots,s_r\}$, with $k+1+r=N+1$. The linear system for the coefficients $\mathbf{c}=[c_0,\ldots,c_N]^\top$ is

If a value and a slope are imposed at the same node, take $z_j=x_i$ and include both the value row and the derivative row; the system remains square. In the monomial basis, the top block is the Vandermonde matrix and the derivative block has entries $n\,x^{\,n-1}$. Once solved for $\mathbf{c}$, reconstruct

Interpolating ODE Trajectories (Collocation)¶

Having established the general framework of interpolation, we now apply these concepts to the specific context of approximating trajectories governed by ordinary differential equations. The idea of applying polynomial interpolation with derivative constraints yields a method known as “collocation”. More precisely, a degree-$s$ collocation method is a way to discretize an ordinary differential equation (ODE) by approximating the solution on each time interval with a polynomial of degree $s$, and then enforcing that this polynomial satisfies the ODE exactly at $s$ carefully chosen points (the collocation nodes).

Consider a dynamical system described by the ordinary differential equation:

where $\mathbf{x}(t)$ represents the state trajectory and $\mathbf{u}(t)$ denotes the control input. Let us focus on a single mesh interval $[t_k,t_{k+1}]$ with step size $h_k := t_{k+1}-t_k$. To work with a standardized domain, we introduce the transformation $t = t_k + h_k\tau$ that maps the physical interval $[t_k,t_{k+1}]$ to the reference interval $[0,1]$. On this reference interval, we select a set of collocation nodes $\{\tau_j\}_{j=0}^K \subset [0,1]$.

Our goal is now to approximate the unknown trajectory using a polynomial of degree $K$. Using a monomial basis, we represent (parameterize) our trajectory as:

where $\mathbf{a}_n \in \mathbb{R}^d$ are coefficient vectors to be determined. Collocation enforces the differential equation at a chosen set of nodes on $[0,1]$. Depending on the node family, these nodes may be interior-only or may include one or both endpoints. With the polynomial state model, we can differentiate analytically. Using the change of variables $t=t_k+h_k\,\tau$, we obtain:

The collocation condition requires that this polynomial derivative equals the right-hand side of the ODE at each collocation node $\tau_j$:

where $\mathbf{u}_j$ represents the control value at node $\tau_j$.

Boundary Conditions and Node Families¶

import matplotlib.pyplot as plt
import numpy as np
from matplotlib.patches import Rectangle
import matplotlib.patches as mpatches
from scipy.interpolate import CubicSpline

# Set up the figure with subplots
fig = plt.figure(figsize=(16, 12))

# Create a 2x2 subplot layout
gs = fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3)

def create_collocation_plot(ax, title, node_positions, slope_nodes, endpoint_types):
    """
    Create a collocation method illustration
    
    Parameters:
    - ax: matplotlib axis
    - title: plot title
    - node_positions: list of x-positions for nodes (normalized 0-1)
    - slope_nodes: list of booleans indicating which nodes enforce slopes
    - endpoint_types: tuple of (left_type, right_type) where type is 'slope', 'eval', or 'continuity'
    """
    
    # Time interval
    t_start, t_end = 0, 1
    
    # Create a smooth trajectory curve for illustration
    x = np.linspace(t_start, t_end, 100)
    # Create an S-shaped curve to represent state trajectory
    y = 0.3 + 0.4 * np.sin(3 * np.pi * x) * np.exp(-2 * x)
    
    # Build a cubic spline of the trajectory for consistent slope evaluation
    spline = CubicSpline(x, y, bc_type='natural')
    
    # Plot the trajectory
    ax.plot(x, y, 'k-', linewidth=2, label='State trajectory x(t)')
    
    # Plot collocation points
    for i, pos in enumerate(node_positions):
        t_node = t_start + pos * (t_end - t_start)
        y_node = 0.3 + 0.4 * np.sin(3 * np.pi * t_node) * np.exp(-2 * t_node)
        
        # Endpoints are rendered in the endpoint section (as squares). Skip here.
        if np.isclose(t_node, 0.0) or np.isclose(t_node, 1.0):
            continue

        if slope_nodes[i]:
            # Blue dot for slope constraint nodes
            ax.plot(t_node, y_node, 'bo', markersize=8, markerfacecolor='blue', 
                   markeredgecolor='darkblue', linewidth=1.5)
            # Add tangent line to show slope constraint (centered on node)
            dt = 0.08  # Half-length for symmetric extension
            t_prev = max(0, t_node - dt)
            t_next = min(1, t_node + dt)
            
            # Calculate slope from the spline derivative (matches plotted curve)
            slope = spline.derivative()(t_node)
            
            # Create symmetric tangent line centered on the node
            y_prev = y_node + slope * (t_prev - t_node)
            y_next = y_node + slope * (t_next - t_node)
            ax.plot([t_prev, t_next], [y_prev, y_next], 'r--', alpha=0.8, linewidth=2)
        else:
            # Green dot for evaluation-only nodes
            ax.plot(t_node, y_node, 'go', markersize=8, markerfacecolor='lightgreen', 
                   markeredgecolor='darkgreen', linewidth=1.5)
    
    # Handle endpoints specially (always render as squares if applicable)
    endpoints = [(0, 'left'), (1, 'right')]
    for pos, side in endpoints:
        y_end = 0.3 + 0.4 * np.sin(3 * np.pi * pos) * np.exp(-2 * pos)
        end_type = endpoint_types[0] if side == 'left' else endpoint_types[1]
        
        if end_type == 'slope':
            ax.plot(pos, y_end, 'bs', markersize=10, markerfacecolor='blue', 
                   markeredgecolor='darkblue', linewidth=2)
            # Add tangent line (centered on endpoint)
            dt = 0.08  # Half-length for symmetric extension
            
            # Calculate slope from the spline derivative (matches plotted curve)
            slope = spline.derivative()(pos)
            
            t_prev = pos - dt
            t_next = pos + dt
            y_prev = y_end + slope * (t_prev - pos)
            y_next = y_end + slope * (t_next - pos)
            ax.plot([t_prev, t_next], [y_prev, y_next], 'r--', alpha=0.8, linewidth=2)
        elif end_type == 'eval':
            ax.plot(pos, y_end, 'gs', markersize=10, markerfacecolor='lightgreen', 
                   markeredgecolor='darkgreen', linewidth=2)
        elif end_type == 'continuity':
            ax.plot(pos, y_end, 'ms', markersize=10, markerfacecolor='orange', 
                   markeredgecolor='darkorange', linewidth=2)
    
    # Add time markers
    ax.axvline(x=0, color='gray', linestyle=':', alpha=0.5)
    ax.axvline(x=1, color='gray', linestyle=':', alpha=0.5)
    ax.text(0, -0.15, r'$t_k$', ha='center', va='top', fontsize=12)
    ax.text(1, -0.15, r'$t_{k+1}$', ha='center', va='top', fontsize=12)
    
    # Formatting
    ax.set_xlim(-0.1, 1.1)
    ax.set_ylim(-0.2, 0.8)
    ax.set_xlabel('Time', fontsize=12)
    ax.set_ylabel('State', fontsize=12)
    ax.set_title(title, fontsize=14, fontweight='bold')
    ax.grid(True, alpha=0.3)

# Define node positions for each method (normalized to [0,1])
# Using 4 nodes total for fair comparison

# Lobatto IIIA nodes (includes both endpoints)
lobatto_nodes = [0.0, 0.276, 0.724, 1.0]
lobatto_slopes = [True, True, True, True]
lobatto_endpoints = ('slope', 'slope')

# Radau IA nodes (includes left endpoint)
radau1_nodes = [0.0, 0.155, 0.645, 0.955]
radau1_slopes = [True, True, True, True]  # Collocation at left endpoint and interior nodes
radau1_endpoints = ('slope', 'eval')  # Left: slope, Right: evaluation-only

# Radau IIA nodes (includes right endpoint)
radau2_nodes = [0.045, 0.355, 0.845, 1.0]
radau2_slopes = [True, True, True, True]  # Collocation at interior nodes and right endpoint
radau2_endpoints = ('continuity', 'slope')  # Left: continuity, Right: slope

# Gauss nodes (no endpoints)
gauss_nodes = [0.113, 0.387, 0.613, 0.887]
gauss_slopes = [True, True, True, True]
gauss_endpoints = ('eval', 'eval')

# Create subplots
ax1 = fig.add_subplot(gs[0, 0])
create_collocation_plot(ax1, 'Lobatto IIIA Method', lobatto_nodes, lobatto_slopes, lobatto_endpoints)

ax2 = fig.add_subplot(gs[0, 1])
create_collocation_plot(ax2, 'Radau IA Method', radau1_nodes, radau1_slopes, radau1_endpoints)

ax3 = fig.add_subplot(gs[1, 0])
create_collocation_plot(ax3, 'Radau IIA Method', radau2_nodes, radau2_slopes, radau2_endpoints)

ax4 = fig.add_subplot(gs[1, 1])
create_collocation_plot(ax4, 'Gauss Method', gauss_nodes, gauss_slopes, gauss_endpoints)

# Create legend
legend_elements = [
    mpatches.Patch(color='blue', label='Slope constraint (f = dynamics)'),
    mpatches.Patch(color='lightgreen', label='Polynomial evaluation only'),
    mpatches.Patch(color='orange', label='Continuity constraint'),
    plt.Line2D([0], [0], color='red', linestyle='--', alpha=0.7, label='Tangent (slope direction)'),
    plt.Line2D([0], [0], color='black', linewidth=2, label='State trajectory')
]

fig.legend(handles=legend_elements, loc='lower center', bbox_to_anchor=(0.5, 0.03), 
           ncol=3, fontsize=12, frameon=True, fancybox=True, shadow=False)

# Add main title
fig.suptitle('Collocation Methods for Optimal Control\n(Illustration of Node Types and Constraints)', 
             fontsize=16, fontweight='bold', y=0.95)

plt.tight_layout(rect=[0.04, 0.10, 0.98, 0.93])
plt.show()

The choice of collocation nodes determines how boundary conditions are handled and affects the resulting discretization properties. Three standard families are commonly used: Labatto, Randau and and Gauss.

Let’s consider these three setup with more generality over any given basis. We start again by taking a mesh interval $[t_k,t_{k+1}]$ of length $h_k=t_{k+1}-t_k$ and reparametrize time by

We then choose to represent the (unknown) state by a degree–$K$ polynomial

where $\{\phi_n\}_{n=0}^K$ is any linearly independent basis of polynomials of degree $\le K$, and $\mathbf{a}_0,\dots,\mathbf{a}_K$ are vector coefficients to be determined.

The collocation condition mean that we require for the chosen $K$ collocation points that:

which, using $\tfrac{d}{dt}=(1/h_k)\tfrac{d}{d\tau}$, becomes

Put simply: choose some nodes inside the interval, and at each of those nodes force the slope of the polynomial approximation to match the slope prescribed by the ODE. What we mean by the expression “collocation conditions” is simply to say that we want to satisfy a set of “slope-matching equations” at the chosen nodes.

By definition of the mesh variables,

and (if you sample the control at endpoints)

With the monomial basis,

For the derivative, $\phi_n'(\tau)=n\,\tau^{n-1}$, so

When chaining intervals into a global trajectory, direct collocation enforces state continuity by construction: the variable $\mathbf{x}_{k+1}$ at the end of one interval is the same as the starting variable of the next. What is not enforced automatically is slope continuity; the derivative at the end of one interval generally does not match the derivative at the start of the next. Different collocation methods may have different slope continuity properties depending on the chosen collocation nodes.

Lobatto Nodes (endpoints included):

The family of Labotto methods correspond to any set of so-called Lobatto nodes $\{\tau_j\}_{j=0}^K$ with the specificity that we require $\tau_0=0$ and $\tau_K=1$. Let’s assume that we work with the power (monomial) basis $\phi_n(\tau)=\tau^n$, so that

Differentiating $\mathbf{x}_h$ with respect to $\tau$ gives

Since we have the chain rule $\frac{d}{dt} = \frac{1}{h_k}\frac{d}{d\tau}$ from the time transformation $t = t_k + h_k\tau$, the time derivative becomes

so the collocation equations at Lobatto nodes are

For $j=0$ and $j=K$, these conditions become:

With the monomial basis $\phi_n(\tau)=\tau^n$, we have $\phi_n'(0)=n\delta_{n,1}$ (only $\phi_1'=1$, others vanish) and $\phi_n'(1)=n$. Also, $\phi_n(0)=\delta_{n,0}$ and $\phi_n(1)=1$ for all $n$. This simplifies the endpoint conditions to:

These equations enforce that the polynomial’s slope at both endpoints matches the ODE’s prescribed slope, which is why the figure shows red tangent lines at both endpoints for Lobatto methods.

Radau Nodes (one endpoint included): Radau points include only one endpoint. Radau-I includes the left endpoint ($\tau_0 = 0$) while Radau-II includes the right endpoint ($\tau_K = 1$). This means that a radau collocation is defined by any set of collocation nodes such that $\tau_K = 1$. This translates into requireing that we match the ODE over the mesh only at the right endpoiunt in addition to the interior nodes. As a consequence, we leave the solution unconstrained to take any value on the left endpoint. When chaining up multiple intervals across a global solution, this may pose some complication as we will no longer be able to ensure continuity as the slope at one endpoitn need not match that of the next endpoint. (But could you have a situation where slopes match but the states don’t line up?)

At the included endpoint the ODE is enforced (slope shown in the figure), while at the other endpoint continuity links adjacent intervals. For Radau-I with $K+1$ points:

The endpoint $\mathbf{x}_{k+1} = \mathbf{x}_h(1) = \sum_{n=0}^K \mathbf{a}_n$ is not directly constrained by a collocation condition, requiring separate continuity enforcement between intervals.

Gauss Nodes (endpoints excluded): Gauss points exclude both endpoints, using only interior points $\tau_j \in (0,1)$ for $j = 1,\ldots,K$. The ODE is enforced only at interior nodes; both endpoints are handled through separate continuity constraints:

Origins and Selection Criteria: These node families derive from orthogonal polynomial theory. Gauss nodes correspond to roots of Legendre polynomials and provide optimal quadrature accuracy for smooth integrands. Radau nodes are roots of modified Legendre polynomials with one endpoint constraint, while Lobatto nodes include both endpoints and correspond to roots of derivatives of Legendre polynomials.

For optimal control applications, Radau-II nodes are often preferred because they provide implicit time-stepping behavior and good stability properties. Lobatto nodes simplify boundary condition handling but may require smaller time steps. Gauss nodes offer highest quadrature accuracy but complicate endpoint treatment.

Control Parameterization and Cost Integration¶

The control inputs can be handled with similar polynomial approximations. We may use piecewise-constant controls, piecewise-linear controls, or higher-order polynomial parameterizations of the form:

where $\mathbf{u}_j = \mathbf{u}_h(\tau_j)$ represents the control values at each collocation point. This polynomial framework extends to cost function evaluation, where running costs are integrated using the same quadrature nodes and weights:

A Compendium of Direct Transcription Methods in Trajectory Optimization¶

The mesh and interior nodes are the common scaffold. What distinguishes one transcription from another is how we obtain values at those nodes and how we approximate the two integrals that appear implicitly and explicitly: the integral of the running cost and the integral that carries the state forward. In other words, we now commit to two design choices that mirror the previous section: a finite representation for $\mathbf{x}(t)$ and $\mathbf{u}(t)$ over each interval $[t_i,t_{i+1}]$, and a quadrature rule whose nodes and weights are used consistently for both cost and dynamics. The result is always a sparse nonlinear program; the differences are in where we sample and how we tie samples together.

Below, each transcription should be read as “same grid, same interior points, same evaluations for cost and physics,” with only the local representation changing.

Euler Collocation¶

Work on one interval $[t_k,t_{k+1}]$ of length $h_k$ with the reparametrization $t=t_k+h_k\,\tau$, $\tau\in[0,1]$. Assume a degree 1 polynomial:

for any basis $\{\phi_0,\phi_1\}$ of linear polynomials. Endpoint conditions give

by backsubstitution and because

Furthermore, the derivative with respect to $\tau$ is:

Because from the mapping $t(\tau)=t_k+h_k\tau$ we can invert: $\tau(t)=\frac{t-t_k}{h_k}$ and differentiating gives

The collocation condition at a single Radau-II node $\tau=1$:

Because $\mathbf{x}_h$ is linear, $\mathbf{x}_h'(\tau)$ is constant in $\tau$, so $\mathbf{x}_h'(1)=\mathbf{x}_h'(\tau)$ for all $\tau$. Moreover, linear interpolation between the two endpoints gives

Substitute this into the collocation condition:

which is exactly the implicit Euler step

The overall direct transcription is then:

Note that:

• The running cost and path constraints are evaluated at the same right-endpoint where the dynamics are enforced, keeping “where we pay” aligned with “where we enforce.”

• State continuity is automatic because $\mathbf{x}_{i+1}$ is a shared variable between adjacent intervals; slope continuity is not enforced unless you add it. Here’s an updated subsection that explicitly says what collocation nodes are chosen and why the trapezoidal defect uses them the way it does.

Side remark. If you instead collocate at the left endpoint (Radau-I with $\tau=0$) with the same linear model, you obtain $\frac{1}{h_k}(\mathbf{x}_{k+1}-\mathbf{x}_k)=\mathbf{f}(\mathbf{x}_k,\mathbf{u}_k,t_k)$, i.e., the explicit Euler step. In that very precise sense, explicit Euler can be viewed as a (left-endpoint) degree-1 collocation scheme.

Trapezoidal collocation¶

In this scheme we take the two endpoints as the nodes on each interval:

We approximate $\mathbf{x}$ linearly over $[t_i,t_{i+1}]$, and we evaluate both the running cost and the dynamics at these two nodes with equal weights. Because a linear polynomial has a constant derivative, we do not try to match the ODE’s slope at both endpoints (that would overconstrain a linear function). Instead, we enforce the ODE in its integral form over the interval and approximate the integral of $\mathbf{f}$ by the trapezoid rule using those two nodes. This makes the cost quadrature and the state-update (“defect”) use the same nodes and weights.

Summary: the collocation nodes for trapezoidal are the two endpoints; the state is linear on each interval; and the dynamics are enforced via the integrated ODE with the trapezoid rule at those two nodes, yielding the familiar trapezoidal defect.

Hermite–Simpson (quadratic interpolation; midpoint included)¶

On each interval $[t_i,t_{i+1}]$ we pick three collocation nodes on the reference domain $\tau\in[0,1]$:

So we evaluate at left, midpoint, right. These are the same three nodes used by Simpson’s rule (weights $1{:}4{:}1$) for numerical quadrature. We let $\mathbf{x}_h$ be quadratic in $\tau$. Two things then happen:

1. Integral (defect) enforcement with Simpson’s rule. We enforce the ODE in integral form over the interval and approximate the integral of $\mathbf{f}$ with Simpson’s rule using the three nodes above. This yields the first constraint (the “Simpson defect”), which uses $\mathbf{f}$ evaluated at left, midpoint, and right.

2. Slope matching at the midpoint (collocation). Because a quadratic has limited shape, we don’t try to match slopes at both endpoints. Instead, we introduce midpoint variables $(\mathbf{x}_{i+\frac12},\mathbf{u}_{i+\frac12})$ and match the ODE at the midpoint. The second constraint below is exactly the midpoint collocation condition written in an equivalent Hermite form: it pins the midpoint state to the average of the endpoints plus a correction based on endpoint slopes, ensuring that the polynomial’s derivative is consistent with the ODE at $\tau=\tfrac12$.

This way, where we pay (Simpson quadrature) and where we enforce (midpoint collocation + Simpson defect) are aligned at the same three nodes, which is why the method is both accurate and well conditioned on smooth problems.

Collocation nodes recap: $\tau=0,\ \tfrac12,\ 1$.

• The midpoint is where we explicitly match the ODE slope (collocation).

• The three nodes together are used for the Simpson integral of $\mathbf{f}$ (state update) and of $c$ (cost), keeping physics and objective synchronized.

Examples¶

Compressor Surge Problem¶

A compressor is a machine that raises the pressure of a gas by squeezing it into a smaller volume. You find them in natural gas pipelines, jet engines, and factories. But compressors can run into trouble if the flow of gas becomes too small. In that case, the machine can “stall” much like an airplane wing at too high an angle. Instead of moving forward, the gas briefly pushes back, creating strong pressure oscillations that can damage the compressor and anything connected to it.

To prevent this, engineers often add a close-coupled valve (CCV) at the outlet. The valve can quickly adjust the flow to keep the compressor away from these unstable conditions. Our goal is to design a control strategy for operating this valve so that the compressor never enters surge.

Following Gravdahl & Egeland, 1997 and Grancharova & Johansen (2012), we model the compressor using a simplified second-order representation:

Here, $\mathbf{x} = [x_1, x_2]^T$ represents the state variables:

• $x_1$ is the normalized mass flow through the compressor.

• $x_2$ is the normalized pressure ratio across the compressor.

The control input $u$ denotes the normalized mass flow through a CCV. The functions $\Psi_e(x_1)$ and $\Phi(x_2)$ represent the characteristics of the compressor and valve, respectively:

The system parameters are given as $\gamma = 0.5$, $B = 1$, $H = 0.18$, $\psi_{c0} = 0.3$, and $W = 0.25$.

One possible way to pose the problem Grancharova & Johansen (2012) is by penalizing deviations from the setpoints using a quadratic penalty in the instantaneous cost function as well as in the terminal one. Furthermore, we also penalize taking large actions (which are energy hungry and potentially unsafe) within the integral term. The idea of penalizing deviations throughout is a natural way of posing the problem when solving it via single shooting. Another alternative, which we will explore below, is to set the desired setpoint as a hard terminal constraint.

The control objective is to stabilize the system and prevent surge, formulated as a continuous-time optimal control problem (COCP) in the Bolza form:

The parameters $\alpha$, $\beta$, $\kappa$, and $R$ are non-negative weights that allow the designer to prioritize different aspects of performance (e.g., tight setpoint tracking vs. smooth control actions). We also constraint the control input to be within $0 \leq u(t) \leq 0.3$ due to the physical limitations of the valve.

The authors in Grancharova & Johansen (2012) also add a soft path constraint $x_2(t) \geq 0.4$ to ensure that we maintain a minimum pressure at all time. This is implemented as a soft constraint using slack variables. The reason that we have the term $R v^2$ in the objective is to penalizes violations of the soft constraint: we allow for deviations, but don’t want to do it too much.

In the experiment below, we choose the setpoint $\mathbf{x}^* = [0.40, 0.60]^T$ as it corresponds to an unstable equilibrium point. If we were to run the system without applying any control, we would see that the system starts to oscillate.

import numpy as np
from scipy.optimize import minimize
import matplotlib.pyplot as plt

# System parameters
gamma, B, H, psi_c0, W = 0.5, 1, 0.18, 0.3, 0.25
alpha, beta, kappa, R = 1, 0, 0.08, 0
T, N = 12, 60
dt = T / N
x1_star, x2_star = 0.40, 0.60

def psi_e(x1):
    return psi_c0 + H * (1 + 1.5 * ((x1 / W) - 1) - 0.5 * ((x1 / W) - 1)**3)

def phi(x2):
    return gamma * np.sign(x2) * np.sqrt(np.abs(x2))

def system_dynamics(x, u):
    x1, x2 = x
    dx1dt = B * (psi_e(x1) - x2 - u)
    dx2dt = (1 / B) * (x1 - phi(x2))
    return np.array([dx1dt, dx2dt])

def euler_step(x, u, dt):
    return x + dt * system_dynamics(x, u)

def instantenous_cost(x, u):
    return (alpha * np.sum((x - np.array([x1_star, x2_star]))**2) + kappa * u**2)

def terminal_cost(x):
    return beta * np.sum((x - np.array([x1_star, x2_star]))**2)

def objective_and_constraints(z):
    u, v = z[:-1], z[-1]
    x = np.zeros((N+1, 2))
    x[0] = x0
    obj = 0
    cons = []
    for i in range(N):
        x[i+1] = euler_step(x[i], u[i], dt)
        obj += dt * instantenous_cost(x[i], u[i])
        cons.append(0.4 - x[i+1, 1] - v)
    obj += terminal_cost(x[-1]) + R * v**2
    return obj, np.array(cons)

def solve_trajectory_optimization(x0, u_init):
    z0 = np.zeros(N + 1)
    z0[:-1] = u_init
    bounds = [(0, 0.3)] * N + [(0, None)]
    result = minimize(
        lambda z: objective_and_constraints(z)[0],
        z0,
        method='SLSQP',
        bounds=bounds,
        constraints={'type': 'ineq', 'fun': lambda z: -objective_and_constraints(z)[1]},
        options={'disp': True, 'maxiter': 1000, 'ftol': 1e-6}
    )
    return result.x, result

def simulate_trajectory(x0, u):
    x = np.zeros((N+1, 2))
    x[0] = x0
    for i in range(N):
        x[i+1] = euler_step(x[i], u[i], dt)
    return x

# Run optimizations and simulations
x0 = np.array([0.25, 0.25])
t = np.linspace(0, T, N+1)

# Optimized control starting from zero
z_single_shooting, _ = solve_trajectory_optimization(x0, np.zeros(N))
u_opt_shoot, v_opt_shoot = z_single_shooting[:-1], z_single_shooting[-1]
x_opt_shoot = simulate_trajectory(x0, u_opt_shoot)

# Do-nothing control (u = 0)
u_nothing = np.zeros(N)
x_nothing = simulate_trajectory(x0, u_nothing)

# Plotting
plt.figure(figsize=(15, 20))

# State variables over time
plt.subplot(3, 1, 1)
plt.plot(t, x_opt_shoot[:, 0], label='x1 (opt from 0)')
plt.plot(t, x_opt_shoot[:, 1], label='x2 (opt from 0)')
plt.plot(t, x_nothing[:, 0], ':', label='x1 (do-nothing)')
plt.plot(t, x_nothing[:, 1], ':', label='x2 (do-nothing)')
plt.axhline(y=x1_star, color='r', linestyle='--', label='x1 setpoint')
plt.axhline(y=x2_star, color='g', linestyle='--', label='x2 setpoint')
plt.xlabel('Time')
plt.ylabel('State variables')
plt.title('State variables over time')
plt.legend()
plt.grid(True)

# Phase portrait
plt.subplot(3, 1, 2)
plt.plot(x_opt_shoot[:, 0], x_opt_shoot[:, 1], label='Optimized from 0')
plt.plot(x_nothing[:, 0], x_nothing[:, 1], ':', label='Do-nothing')
plt.plot(x1_star, x2_star, 'r*', markersize=10, label='Setpoint')
plt.xlabel('x1 (mass flow)')
plt.ylabel('x2 (pressure)')
plt.title('Phase portrait')
plt.legend()
plt.grid(True)

# Control inputs
plt.subplot(3, 1, 3)
plt.plot(t[:-1], u_opt_shoot, label='Optimized from 0')
plt.plot(t[:-1], u_nothing, ':', label='Do-nothing')
plt.xlabel('Time')
plt.ylabel('Control input (u)')
plt.title('Control input over time')
plt.legend()
plt.grid(True)

Solution by Trapezoidal Collocation¶

Another way to pose the problem is by imposing a terminal state constraint on the system rather than through a penalty in the integral term. In the following experiment, we use a problem formulation of the form:

We then find a control function $u(t)$ and state trajectory $x(t)$ using the trapezoidal collocation method.

import numpy as np
from scipy.optimize import minimize
from scipy.integrate import solve_ivp
from scipy.interpolate import interp1d
import matplotlib.pyplot as plt

# System parameters
gamma, B, H, psi_c0, W = 0.5, 1, 0.18, 0.3, 0.25
kappa = 0.08
T, N = 12, 20  # Number of collocation points
t = np.linspace(0, T, N)
dt = T / (N - 1)
x1_star, x2_star = 0.40, 0.60

def psi_e(x1):
    return psi_c0 + H * (1 + 1.5 * ((x1 / W) - 1) - 0.5 * ((x1 / W) - 1)**3)

def phi(x2):
    return gamma * np.sign(x2) * np.sqrt(np.abs(x2))

def system_dynamics(t, x, u_func):
    x1, x2 = x
    u = u_func(t)
    dx1dt = B * (psi_e(x1) - x2 - u)
    dx2dt = (1 / B) * (x1 - phi(x2))
    return [dx1dt, dx2dt]

def objective(z):
    x = z[:2*N].reshape((N, 2))
    u = z[2*N:]
    
    # Trapezoidal rule for the cost function
    cost = 0
    for i in range(N-1):
        cost += 0.5 * dt * (kappa * u[i]**2 + kappa * u[i+1]**2)
    
    return cost

def constraints(z):
    x = z[:2*N].reshape((N, 2))
    u = z[2*N:]
    
    cons = []
    
    # Dynamics constraints (trapezoidal rule)
    for i in range(N-1):
        f_i = system_dynamics(t[i], x[i], lambda t: u[i])
        f_ip1 = system_dynamics(t[i+1], x[i+1], lambda t: u[i+1])
        cons.extend(x[i+1] - x[i] - 0.5 * dt * (np.array(f_i) + np.array(f_ip1)))
    
    # Terminal constraint
    cons.extend([x[-1, 0] - x1_star, x[-1, 1] - x2_star])
    
    # Initial condition constraint
    cons.extend([x[0, 0] - x0[0], x[0, 1] - x0[1]])
    
    return np.array(cons)

def solve_trajectory_optimization(x0):
    # Initial guess
    x_init = np.linspace(x0, [x1_star, x2_star], N)
    u_init = np.zeros(N)
    z0 = np.concatenate([x_init.flatten(), u_init])
    
    # Bounds
    bounds = [(None, None)] * (2*N)  # State variables
    bounds += [(0, 0.3)] * N  # Control inputs
    
    # Constraints
    cons = {'type': 'eq', 'fun': constraints}
    
    result = minimize(
        objective,
        z0,
        method='SLSQP',
        bounds=bounds,
        constraints=cons,
        options={'disp': True, 'maxiter': 1000, 'ftol': 1e-6}
    )
    return result.x, result

# Run optimization
x0 = np.array([0.5, 0.5])
z_opt, result = solve_trajectory_optimization(x0)
x_opt_coll = z_opt[:2*N].reshape((N, 2))
u_opt_coll = z_opt[2*N:]

print(f"Optimization successful: {result.success}")
print(f"Final objective value: {result.fun}")
print(f"Final state: x1 = {x_opt_coll[-1, 0]:.4f}, x2 = {x_opt_coll[-1, 1]:.4f}")
print(f"Target state: x1 = {x1_star:.4f}, x2 = {x2_star:.4f}")

# Create interpolated control function
u_func = interp1d(t, u_opt_coll, kind='linear', bounds_error=False, fill_value=(u_opt_coll[0], u_opt_coll[-1]))

# Solve IVP with the optimized control
sol = solve_ivp(lambda t, x: system_dynamics(t, x, u_func), [0, T], x0, dense_output=True)

# Generate solution points
t_dense = np.linspace(0, T, 200)
x_ivp = sol.sol(t_dense).T

# Plotting
plt.figure(figsize=(15, 20))

# State variables over time
plt.subplot(3, 1, 1)
plt.plot(t, x_opt_coll[:, 0], 'bo-', label='x1 (collocation)')
plt.plot(t, x_opt_coll[:, 1], 'ro-', label='x2 (collocation)')
plt.plot(t_dense, x_ivp[:, 0], 'b--', label='x1 (integrated)')
plt.plot(t_dense, x_ivp[:, 1], 'r--', label='x2 (integrated)')
plt.axhline(y=x1_star, color='b', linestyle=':', label='x1 setpoint')
plt.axhline(y=x2_star, color='r', linestyle=':', label='x2 setpoint')
plt.xlabel('Time')
plt.ylabel('State variables')
plt.title('State variables over time')
plt.legend()
plt.grid(True)

# Phase portrait
plt.subplot(3, 1, 2)
plt.plot(x_opt_coll[:, 0], x_opt_coll[:, 1], 'go-', label='Collocation')
plt.plot(x_ivp[:, 0], x_ivp[:, 1], 'm--', label='Integrated')
plt.plot(x1_star, x2_star, 'r*', markersize=10, label='Setpoint')
plt.xlabel('x1 (mass flow)')
plt.ylabel('x2 (pressure)')
plt.title('Phase portrait')
plt.legend()
plt.grid(True)

# Control inputs
plt.subplot(3, 1, 3)
plt.step(t, u_opt_coll, 'g-', where='post', label='Collocation')
plt.plot(t_dense, u_func(t_dense), 'm--', label='Interpolated')
plt.xlabel('Time')
plt.ylabel('Control input (u)')
plt.title('Control input over time')
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.show()

You can try to vary the number of collocation points in the code and observe how the state trajectory progressively matches the ground truth (the line denoted “integrated solution”). Note that this version of the code also lacks bound constraints on the variable $x_2$ to ensure a minimum pressure, as we did earlier. Consider this a good exercise to try on your own.

System Identification as Trajectory Optimization (Compressor Surge)¶

We now turn the compressor surge model into a simple system identification task: estimate unknown parameters (here, the scalar $B$) from measured trajectories. This can be viewed as a trajectory optimization problem: choose parameters (and optionally states) to minimize reconstruction error while enforcing the dynamics.

Given time-aligned data $\{(\mathbf{u}_k,\mathbf{y}_k)\}_{k=0}^{N}$, model states $\mathbf{x}_k\in\mathbb{R}^d$, outputs $\mathbf{y}_k\approx \mathbf{h}(\mathbf{x}_k;\boldsymbol{\theta})$, step $\Delta t$, and dynamics $\mathbf{f}(\mathbf{x},\mathbf{u};\boldsymbol{\theta})$, the simultaneous (full-discretization) viewpoint is