Trajectory Optimization: Indirect and Direct Methods

This started as a course project for BLM599, Optimization Theory. The course covered the classical solvers: Newton–Raphson, Levenberg–Marquardt, and why those iterations converge in the first place. I wanted to try them on something with more structure than a scalar test function. So I picked trajectory optimization, where you are not looking for a point that minimises a function but for a whole trajectory: a state history together with the control input that produces it.

I went at it from both directions. First the indirect route: derive the conditions an optimal trajectory has to satisfy, then go looking for a trajectory that satisfies them. You end up with a solution you can defend analytically, but only for systems simple enough to get through the derivation. Then the direct route: chop the trajectory into pieces and hand the whole thing to a nonlinear program. Much easier to set up, and much less you can say about the answer that comes back.

Both examples are in MATLAB, and every animation on this page comes straight out of those scripts.

Dynamic systems, and what “optimal” means here

A continuous-time dynamic system is a differential equation driven by an input:

$$\dot x(t) = f\big(x(t),\, u(t),\, t\big), \qquad x(0) = x_0, \qquad x(T) = x_f$$

where $x$ is the state vector, $u$ the control input and $f$ the dynamics. Trajectory optimization asks for the best sequence of states and controls that takes the system to its goal while minimising a cost and respecting its constraints:

$$\begin{aligned} \min_{x(t),\,u(t),\,T} \quad & J = \int_0^T \mathcal{L}\big(x(\tau),\, u(\tau),\, \tau\big)\, d\tau \\[4pt] \text{s.t.} \quad & \dot x(t) = f\big(x(t),\, u(t),\, t\big) \\ & \psi\big(x(T),\, T\big) = 0 \\ & h\big(x(t),\, u(t),\, t\big) \le 0 \end{aligned}$$

We are minimising over a function here, not over a vector of numbers, so as written the problem is infinite-dimensional. Both methods below are, at bottom, ways of dealing with that.

Two ways to attack it

There are two families of methods, and they trade off against each other almost point for point:

	Indirect methods	Direct methods
Idea	Formulate the necessary conditions for optimality (Pontryagin’s Maximum Principle)	Discretize the trajectory and convert it to a constrained parameter optimization problem
Then	Solve the resulting two-point boundary value problem by shooting or collocation	Solve the resulting parameter optimization with a nonlinear program solver
Effort	Hard to formulate, easy to solve	Easy to formulate, hard to solve
Guarantee	The necessary conditions guarantee optimality	No guarantee of optimality

The rest of this post is one worked example from each column, starting with the theory behind the first one.

Indirect methods: where the conditions come from

Take the initial time to be zero and drop the terminal cost, just to keep the algebra readable. The dynamics have to hold at every instant, so we adjoin them to the cost with a costate $\lambda(t)$, a Lagrange multiplier that is itself a function of time, one multiplier per instant:

$$J = \int_0^T \Big[\, \mathcal{L} + \lambda^\top\big(f - \dot x\big) \Big]\, d\tau$$

The $\dot x$ inside the integral is awkward, since it couples the multiplier to the derivative of the thing we are varying. Integration by parts moves the derivative off $x$ and onto $\lambda$:

$$\int_0^T \lambda^\top \dot x \, d\tau = \lambda^\top(T)\,x(T) - \lambda^\top(0)\,x(0) - \int_0^T \dot\lambda^\top x \, d\tau$$

which turns the augmented cost into

$$J = \lambda^\top(0)\,x(0) - \lambda^\top(T)\,x(T) + \int_0^T \Big[\, \mathcal{L} + \lambda^\top f + \dot\lambda^\top x \,\Big]\, d\tau$$

The two terms that don’t involve $\dot\lambda$ show up often enough to deserve a name of their own. That combination is the Hamiltonian:

$$H(x,\, u,\, \lambda) = \mathcal{L}(x,\, u) + \lambda^\top f(x,\, u)$$ $$J = \lambda^\top(0)\,x(0) - \lambda^\top(T)\,x(T) + \int_0^T \Big[\, H + \dot\lambda^\top x \,\Big]\, d\tau$$

A detour through the calculus of variations

Differential calculus asks how a function responds to a change in its argument; the calculus of variations asks how a functional responds to a change in an entire function. The bookkeeping is the same. Write each quantity as the optimal one plus a small increment,

$$x = x^\star + \delta x, \qquad u = u^\star + \delta u, \qquad J = J^\star + \delta J$$

and require the first-order change to vanish. If the trajectory is optimal then $\delta J$ has to be zero, the same way a derivative vanishes at the minimum of an ordinary function. The endpoints are pinned by the boundary conditions, so $\delta x(0) = \delta x(T) = 0$ and the two boundary terms drop out, leaving

$$\delta J = \int_0^T \Big[ \Big( \frac{\partial H}{\partial x} + \dot\lambda^\top \Big)\, \delta x + \frac{\partial H}{\partial u}\, \delta u \Big]\, d\tau = 0$$

The next step is the one that matters. The variations $\delta x$ and $\delta u$ are arbitrary, so the integral has to vanish for every perturbation we could apply. That only happens if each bracket vanishes on its own, which gives us the necessary conditions:

$$\dot x = \frac{\partial H}{\partial \lambda} \qquad \text{(state equation)}$$ $$\dot \lambda = -\frac{\partial H}{\partial x} \qquad \text{(costate equation)}$$ $$\frac{\partial H}{\partial u} = 0 \qquad \text{(stationarity condition)}$$

That is Pontryagin’s Maximum Principle for the case where the input is unconstrained and $H$ is smooth in $u$. The general statement is that the optimal $u$ minimises the Hamiltonian pointwise; with nothing constraining $u$, minimising it comes down to setting the derivative to zero.

Worked example: a mass-spring-damper

The equations of motion, with position $z_1$ and velocity $z_2$:

$$\dot z_{1} = z_2, \qquad \dot z_2 = \frac{1}{m}\big(F - k z_1 - b z_2\big)$$

We want to get the mass from rest at the origin to rest at one metre, spending as little force as possible along the way:

$$\begin{aligned} \min_{F(t)} \quad & J = \int_0^T F^2(\tau)\, d\tau \\[4pt] \text{s.t.} \quad & \dot z(t) = f\big(z(t),\, F(t),\, t\big) \\ & z_1(0) = 0, \quad z_1(T) = 1 \\ & z_2(0) = 0, \quad z_2(T) = 0 \end{aligned}$$

Building the Hamiltonian is mechanical — cost plus costate times dynamics:

$$H = F^2 + \lambda_1 z_2 + \frac{\lambda_2}{m}\big(F - k z_1 - b z_2\big)$$

and the stationarity condition immediately gives the optimal force:

$$\frac{\partial H}{\partial F} = 2F + \frac{\lambda_2}{m} = 0 \quad\Longrightarrow\quad F = -\frac{\lambda_2}{2m}$$

This is what makes the algebra worth it. The optimal control isn’t something we search for numerically, it is an algebraic function of the costate. If we knew $\lambda_2(t)$ we would have the answer outright.

The costate equations tell us how $\lambda$ evolves:

$$\dot\lambda_1 = -\frac{\partial H}{\partial z_1} = \frac{k}{m}\lambda_2, \qquad \dot\lambda_2 = -\frac{\partial H}{\partial z_2} = -\lambda_1 + \frac{b}{m}\lambda_2$$

Substituting $F$ back into the dynamics leaves states and costates as a single autonomous system of four ODEs:

$$\begin{bmatrix} \dot z_1 \\[4pt] \dot z_2 \\[4pt] \dot\lambda_1 \\[4pt] \dot\lambda_2 \end{bmatrix} = \begin{bmatrix} z_2 \\[4pt] \dfrac{1}{m}\Big(-\dfrac{1}{2m}\lambda_2 - k z_1 - b z_2\Big) \\[4pt] \dfrac{k}{m}\lambda_2 \\[4pt] -\lambda_1 + \dfrac{b}{m}\lambda_2 \end{bmatrix}$$

Shooting: turning a boundary value problem into a search

We know the state at both ends of the interval and the costate at neither. That is a two-point boundary value problem, and it isn’t something we can just integrate. Shooting turns it into an initial value problem we can integrate: guess $\lambda(0)$, integrate the four ODEs forward to $T$, and look at where the state ended up. Take the residual to be the miss distance,

$$R(\lambda_0) = z_T - \hat z_T(\lambda_0)$$

and the optimal control problem has collapsed into a minimisation over two variables:

$$\min_{\lambda_0} \; \lVert R \rVert = \sqrt{R^\top R}$$

which is exactly the kind of problem the course was about:

$$\lambda_0^{k+1} = \lambda_0^k + \Delta\lambda_0^k = \lambda_0^k - \alpha\, \nabla_{\lambda_0} J = \lambda_0^k - \alpha \left( \frac{\partial}{\partial \lambda_0} \sqrt{R^\top R} \right)$$

The gradient is the hard part. $R$ depends on $\lambda_0$ only through the solution of a system of ODEs, so writing $\partial R / \partial \lambda_0$ in closed form is tedious. This system is linear, so an analytical solution does exist, but that stops being true as soon as there is a nonlinearity anywhere, which is why numerical gradients are usually the better bet. Central differences:

$$\frac{\partial R}{\partial \lambda_1(0)} \approx \frac{R\big(\lambda_1(0) + \Delta\lambda_1,\; \lambda_2(0)\big) - R\big(\lambda_1(0) - \Delta\lambda_1,\; \lambda_2(0)\big)}{2\,\Delta\lambda_1}$$

In MATLAB the shooting step is only a few lines:

function residuals = shooting(lambda0, z0, zT, T, p)
    % Integrate the combined state-costate system forward from a guessed
    % initial costate, then measure how far the final state missed.
    [~, z] = ode45(@(t, z) MSD_Dynamics(t, z, p), [0 T], [z0; lambda0]);

    zhatT = z(end, 1:2)';
    residuals = norm(zT - zhatT);
end

and the outer loop is a finite-difference gradient and a fixed step:

delta = 1e-9;
r1plus  = shooting(lambda + [delta; 0], z0, zT, T, p);
r1minus = shooting(lambda - [delta; 0], z0, zT, T, p);
r2plus  = shooting(lambda + [0; delta], z0, zT, T, p);
r2minus = shooting(lambda - [0; delta], z0, zT, T, p);

gradR = [(r1plus - r1minus) / (2*delta);
         (r2plus - r2minus) / (2*delta)];

lambda = lambda - stepSize * gradR;

Results

With $m = 1$ kg, $b = 0.5$ N·s/m, $k = 2$ N/m over a horizon of $T = 5$ s, starting from $\lambda(0) = [2,\, 2]^\top$ with a step size of $\alpha = 0.01$:

The contour plot is worth a second look. The plant is linear, so the combined state–costate system is linear too and the terminal state $\hat z_T$ depends affinely on $\lambda_0$. That makes $\lVert R \rVert$ convex in the variables we are searching over: the bowl in the plot isn’t one basin among several, it is the only one there is. Whatever minimum the iteration walks into is the global one, and that follows from the structure of the problem rather than from how the plot happens to look.

That is the appeal of the indirect route, and also its limit: every step above needed me to differentiate the Hamiltonian by hand.

Direct collocation

For anything with real nonlinearity in it, deriving and solving the necessary conditions stops being practical. Direct methods drop the analytical guarantee and get tractability in return. The idea is transcription: represent the continuous trajectory by its values at a finite set of collocation points and approximate everything in between.

Applying the trapezoidal rule to the cost integral turns it into a finite sum:

$$\int_0^T \mathcal{L}\, d\tau \;\approx\; \sum_{k=0}^{N-1} \frac{h_k}{2}\big(\mathcal{L}_k + \mathcal{L}_{k+1}\big)$$

Applying the same rule to the dynamics gives the constraint that makes the discrete states physically consistent, since each state has to be reachable from the previous one under the system’s own dynamics:

$$x_{k+1} = x_k + \frac{h_k}{2}\big(f_{k+1} + f_k\big)$$

Put together, the infinite-dimensional problem becomes an ordinary constrained parameter optimization over the values at the knot points:

$$\begin{aligned} \min_{\substack{x_0,\, \dots,\, x_N \\ u_0,\, \dots,\, u_N}} \quad & J = \sum_{k=0}^{N-1} \frac{h_k}{2}\big(\mathcal{L}_k + \mathcal{L}_{k+1}\big) \\[4pt] \text{s.t.} \quad & x_{k+1} = x_k + \frac{h_k}{2}\big(f_{k+1} + f_k\big) \\ & x_0 = x(t_0), \qquad x_N = x(t_N) \\ & x_{k,\min} \le x_k \le x_{k,\max} \\ & u_{k,\min} \le u_k \le u_{k,\max} \end{aligned}$$

Notice what is missing: no Hamiltonian, no costate, no differentiating anything by hand. Bounds on the states and inputs, genuinely awkward in the indirect formulation, are now just extra rows in the constraint list. The catch is the one already in the table above: the program is nonconvex, so the solver returns a local minimum, and which one you get depends on the initial guess and the mesh.

Worked example: a double-link cart pendulum

Six states (cart position, two link angles and their velocities) and a single control, the horizontal force on the cart:

$$x = \begin{bmatrix} x_c & \theta_1 & \theta_2 & \dot x_c & \dot\theta_1 & \dot\theta_2 \end{bmatrix}^\top, \qquad u = F_{\text{cart}}$$

Three degrees of freedom driven by a single actuator, so the system is underactuated by two, and the linearisation about the inverted equilibrium is unstable. Writing the answer down by hand is out of the question. The equations of motion come from Bogdanov’s derivation and are implemented in DCP_Dynamics.m; the transcription and solve are handled by Matthew Kelly’s OptimTraj, using the trapezoidal method described above.

Parameter	Value	Parameter	Value
$m_c$	1.095 kg	$L_1,\, L_2$	0.330, 0.310 m
$m_1$	0.096 kg	$\ell_1,\, \ell_2$	0.160, 0.150 m
$m_2$	0.120 kg	$J_1,\, J_2$	0.022962, 0.002972 kg·m²

The task is a swing-up combined with a translation: start hanging at rest half a metre to the left, finish balanced inverted at rest half a metre to the right, over a fixed horizon of eight seconds, with the cart confined to $\pm 1$ m and the force to $\pm 5$ N.

$$\text{start } \begin{bmatrix} -0.5 & 0 & 0 \end{bmatrix}^\top, \qquad \text{finish } \begin{bmatrix} 0.5 & \pi & \pi \end{bmatrix}^\top, \qquad \text{both at zero velocity}$$

Setting it up is almost all bookkeeping, which is the point:

problem.func.dynamics = @(t,x,u)( DCP_Dynamics(x, u, DCP) );

problem.bounds.initialState.low = [-0.5;  0;  0; 0; 0; 0];
problem.bounds.initialState.upp = [-0.5;  0;  0; 0; 0; 0];
problem.bounds.finalState.low   = [ 0.5; pi; pi; 0; 0; 0];
problem.bounds.finalState.upp   = [ 0.5; pi; pi; 0; 0; 0];

problem.bounds.control.low = -maxForce;
problem.bounds.control.upp =  maxForce;

problem.options.method = 'trapezoid';
soln = optimTraj(problem);

Three different notions of “best”

Since the objective is just a function handle now, asking a different question of the same system costs one line. Three worth comparing:

switch ObjectiveSelect
    case "force"
        problem.func.pathObj = @(t,x,u)( R*u.^2 );
    case "error"
        problem.func.pathObj = @(t,x,u)( ...
            Q(1,1)*(targetCartPos    - x(1,:)).^2 + ...
            Q(2,2)*(targetPend1Angle - x(2,:)).^2 + ...
            Q(3,3)*(targetPend2Angle - x(3,:)).^2 + ...
            Q(4,4)*x(4,:).^2 + Q(5,5)*x(5,:).^2 + Q(6,6)*x(6,:).^2 + R*u.^2 );
    case "energy"
        problem.func.pathObj = @(t,x,u)( E_kin(t,x,u) - E_pot(t,x,u) + R*u.^2 );
end

Written out over the knot points, with the trapezoidal weights from the transcription left implicit, the three are as follows.

The first penalises control effort and nothing else:

$$J_1 = \sum_{k=0}^{N-1} R\, u_k^2$$

The second is the usual quadratic tracking cost, penalising distance from the goal at every instant rather than only at the end, with $e_k = x_k - x_{\text{ref}}$ the error against the inverted target:

$$J_2 = \sum_{k=0}^{N-1} \Big( e_k^\top Q\, e_k + R\, u_k^2 \Big)$$

The third adds kinetic minus potential energy, which is the system’s own Lagrangian, so the running cost is the action along the trajectory:

$$J_3 = \sum_{k=0}^{N-1} \Big( E_{\text{kin},k} - E_{\text{pot},k} + R\, u_k^2 \Big)$$

All three reach the same terminal state, since that is a hard constraint. What changes is everything in between, and none of the three is more “correct” than the others. They are answers to three different questions.

Tuning the weights

Nothing below is a new objective. These are the same cost functions again with $Q$ and $R$ changed: the first two are $J_2$, the third is $J_3$. The trajectories still come out this different.

Within $J_2$, the entries of $Q$ decide which errors the solver is willing to tolerate. Two runs, both with the control penalty dropped to $R = 1$:

$J_3$ is just as sensitive to $R$, the only weight it has:

This is what working with direct methods actually feels like. The formulation took minutes; the real work is deciding what to ask for, and the weights are where that decision gets made.

What I took away

The two families really do sit at opposite ends of a trade-off. The indirect route gave me something I could reason about — a control law in closed form, a convex residual, a global optimum I could point to instead of hope for — but the derivation only stayed manageable because the system was a mass on a spring. The direct route swung up an underactuated double pendulum with input saturation and state bounds from a formulation that fits on one screen, and told me nothing about whether a better trajectory exists somewhere else in the space.

For anything you would actually build, the guarantee is the first thing to go. What the indirect derivation is really worth is not the answer it produces but knowing why the answer has the shape it does, and that is what tells you whether the trajectory your NLP solver just handed back is plausible.

References

1. M. Kelly, “An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation,” SIAM Review, vol. 59, no. 4, pp. 849–904, 2017. doi:10.1137/16M1062569
2. M. P. Kelly, OptimTraj — a trajectory optimization library for MATLAB.
3. A. Bogdanov, “Optimal Control of a Double Inverted Pendulum on a Cart,” Technical Report CSE-04-006, OGI School of Science and Engineering, 2004.
4. D. E. Kirk, Optimal Control Theory: An Introduction. Dover Publications, 1970.
5. A. E. Bryson and Y.-C. Ho, Applied Optimal Control: Optimization, Estimation and Control. Taylor & Francis, 1975.
6. J. T. Betts, Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, 2nd ed. SIAM, 2009.