Direct Transcription

For instance, let us start by writing both $\bu[\cdot]$ and $\bx[\cdot]$ as decision variables. Then we can write: \begin{align*} \min_{\bx[\cdot],\bu[\cdot]} \quad & \ell_f(\bx[N]) + \sum_{n=0}^{N-1} \ell(\bx[n],\bu[n]) \\ \subjto \quad & \bx[n+1] = {\bf A}\bx[n] + {\bf B}\bu[n], \quad \forall n\in[0, N-1] \\ & \bx[0] = \bx_0 \\ & + \text{additional constraints}. \end{align*} We call this modeling choice -- adding $\bx[\cdot]$ as decision variables and modeling the discrete dynamics as explicit constraints -- the "direct transcription". Importantly, for linear systems, the dynamics constraints are linear constraints in these decision variables. As a result, if we can restrict our additional constraints to linear inequality constraints and our objective function to being linear/quadratic in $\bx$ and $\bu$, then the resulting trajectory optimization is a convex optimization (specifically a linear program or quadratic program depending on the objective). We can reliably solve these problems to global optimality at quite large scale; these days it is common to solve these optimization online inside a high-rate feedback controller.

If you have not studied convex optimization before, you might be surprised by the modeling power of even of this framework. Consider, for instance, an objective of the form $$\ell(\bx,\bu) = |\bx| + |\bu|.$$ This can be formulated as a linear program. To do it, add additional decision variables ${\bf s}_x[\cdot]$ and ${\bf s}_u[\cdot]$ -- these are commonly referred to as slack variables -- and write $$\min_{\bx,\bu,{\bf s}_x,{\bf s}_u} \sum_n^{N-1} {\bf s}_x[n] + {\bf s}_u[n], \quad \text{s.t.} \quad {\bf s}_x[n] \ge \bx[n], \quad {\bf s}_x[n] \ge -\bx[n], \quad ...$$ The field of convex optimization is replete with tricks like this. Knowing and recognizing them are skills of the (optimization) trade. But there are also many relevant constraints which cannot be recast into convex constraints (in the original coordinates) with any amount of skill. An important example is obstacle avoidance. Imagine a vehicle that must decide if it should go left or right around an obstacle. This represents a fundamentally non-convex constraint in $\bx$; we'll discuss the implications of using non-convex optimization for trajectory optimization below.

Direct Shooting

The savvy reader might have noticed that adding $\bx[\cdot]$ as decision variables was not strictly necessary. If we know $\bx[0]$ and we know $\bu[\cdot]$, then we should be able to solve for $\bx[n]$ using forward simulation. For our discrete-time linear systems, this is particularly nice: \begin{align*} \bx[1] =& \bA\bx[0] + \bB\bu[0] \\ \bx[2] =& \bA(\bA\bx[0] + \bB\bu[0]) + \bB\bu[1] \\ \bx[n] =& \bA^n\bx[0] + \sum_{k=0}^{n-1} \bA^{n-1-k}\bB\bu[k]. \end{align*} What's more, the solution is still linear in $\bu[\cdot]$. This is amazing... we can get rid of a bunch of decision variables, and turn a constrained optimization problem into an unconstrained optimization problem (assuming we don't have any other constraints). This approach -- using $\bu[\cdot]$ but not $\bx[\cdot]$ as decision variables and using forward simulation to obtain $\bx[n]$ -- is called the direct shooting transcription. For linear systems with linear/quadratic objectives in $\bx$, and $\bu$, it is still a convex optimization, and has less decision variables and constraints than the direct transcription.

Computational Considerations

Continuous Time

If we wish to solve the continuous-time version of the problem, then we can discretize time and use the formulations above. The most important decision is the discretization / numerical integration scheme. For linear systems, if we assume that the control inputs are held constant for each time step (aka zero-order hold), then we can integrate the dynamics perfectly: $$\bx[n+1] = \bx[n] + \int_{t_n}^{t_n + h} \left[ {\bf A} \bx(t) + {\bf B}\bu[n] \right]dt = e^{{\bf A}h}\bx[n] + {\bf A}^{-1}(e^{{\bf A}h} - {\bf I}){\bf B}\bu[n],$$ is the simple case (when ${\bf A}$ is invertible). Section 2.1 of these MIT 2.14 notes has a nice presentation of the derivation.

Direct Transcription and Direct Shooting

The formulations that we wrote for direct transcription and direct shooting above are still valid when the dynamics are nonlinear, it's just that the resulting problem is nonconvex. For instance, the direct transcription for discrete-time systems becomes the more general: \begin{align*} \min_{\bx[\cdot],\bu[\cdot]} \quad & \ell_f(\bx[N]) + \sum_{n=0}^{N-1} \ell(\bx[n],\bu[n]) \\ \subjto \quad & \bx[n+1] = {\bf f}(\bx[n], \bu[n]), \quad \forall n\in[0, N-1] \\ & \bx[0] = \bx_0 \\ & + \text{additional constraints}. \end{align*} Direct shooting still works, too, since on each iteration of the algorithm we can compute $\bx[n]$ given $\bx[0]$ and $\bu[\cdot]$ by forward simulation. But things get a bit more interesting when we consider continuous-time systems.

For nonlinear dynamics, we have many choices for how to approximate the discrete dynamics $$\bx[n+1] = \bx[n] + \int_{t[n]}^{t[n+1]} f(\bx(t), \bu(t)) dt, \quad \bx(t[n]) = \bx[n].$$ For instance, in we have an entire suite of numerical integrators that achieve different levels of simulation speed and/or accuracy, both of which can be highly dependent on the details of ${\bf f}(\bx,\bu)$.

One very important idea in numerical integration of differential equations is the use of variable-step integration as a means for controlling integration error. Runge-Kutta-Fehlberg, also known as "RK45", is one of the most famous variable-step integrators. We typically avoid using variable steps inside a constraint (it can lead to discontinuous gradients), but it is possible to accomplish something similar in trajectory optimization by allowing the sample times, $t[\cdot]$, themselves to be decision variables. This allows the optimizer to stretch or shrink the time intervals in order to solve the problem, and is particularly useful if you do not know apriori what the total duration of the trajectory should be. Adding some constraints to these time variables is essential in order to avoid trivial solutions (like collapsing to a trajectory of zero duration). One could potentially even add constraints to bound the integration error.

Direct Collocation

It is very satisfying to have a suite of numerical integration routines available for our direct transcription. But numerical integrators are designed to solve forward in time, and this represents a design constraint that we don't actually have in our direct transcription formulation. If our goal is to obtain an accurate solution to the differential equation with a small number of function evaluations / decision variables / constraints, then some new formulations are possible that take advantage of the constrained optimization formulation. These include the so-called collocation methods.

In direct collocation (c.f., Hargraves87), both the input trajectory and the state trajectory are represented explicitly as piecewise-polynomial functions. In particular, the sweet spot for this algorithm is taking $\bu(t)$ to be a first-order polynomial and $\bx(t)$ to be a cubic polynomial.

It turns out that in this sweet spot, the only decision variables we need in our optimization are the sample values $\bu(t)$ and $\bx(t)$ at the so called "break" points of the spline. You might think that you would need the coefficients of the cubic spline parameters, but you do not. For the first-order interpolation on $\bu(t)$, given $\bu(t_k)$ and $\bu(t_{k+1})$, we can solve for every value $\bu(t)$ over the interval $t \in [t_k, t_{k+1}]$. But we also have everything that we need for the cubic spline: given $\bx(t_k)$ and $\bu(t_k),$ we can compute $\dot\bx(t_k) = f (\bx(t_k), \bu(t_k))$; and the four values $\bx(t_k), \bx(t_{k+1}), \dot\bx (t_k), \dot\bx(t_{k+1})$ completely define all of the parameters of the cubic spline over the interval $t\in[t_k, t_{k+1}]$. This is very convenient, because it is very natural to write constraints on $\bu$ and $\bx$ at the sample points (and would have been relatively harder to have to convert every constraint into constraints on the spline coefficients).

Given the dynamics and the values of the decision variables ($\bu(t_k)$ and $\bx(t_k)$ at the break points), the spline is fully specified, but we need dim$(\bx)$ additional constraints to constrain that $\bx(t_{k+1})$ is (approximately) $\bx(t_k) + \int_{t_k}^{t_{k+1}} f(\bx(t),\bu(t)) dt.$ In direct collocation, we impose this with derivative constraints at the so-called collocation points. In particular, if we choose the collocation times to be the midpoints of the spline, then we have that \begin{gather*} t_{c,k} = \frac{1}{2}\left(t_k + t_{k+1}\right), \qquad h_k = t_{k+1} - t_k, \\ \bu(t_{c,k}) = \frac{1}{2}\left(\bu(t_k) + \bu(t_{k+1})\right), \\ \bx(t_{c,k}) = \frac{1}{2}\left(\bx(t_k) + \bx(t_{k+1})\right) + \frac{h}{8} \left(\dot\bx(t_k) - \dot\bx(t_{k+1})\right), \\ \dot\bx(t_{c,k}) = -\frac{3}{2h}\left(\bx(t_k) - \bx(t_{k+1})\right) - \frac{1}{4} \left(\dot\bx(t_k) + \dot\bx(t_{k+1})\right). \end{gather*} These equations come directly from the equations that fit the cubic spline to the end points/derivatives then interpolate them at the midpoint. They give us precisely what we need to add the dynamics constraint to our optimization at the collocation times:\begin{align*} \min_{\bx[\cdot],\bu[\cdot]} \quad & \ell_f(\bx[N]) + \sum_{n=0}^{N-1} h_n \ell(\bx[n],\bu[n]) \\ \subjto \quad & \dot\bx(t_{c,n}) = f(\bx(t_{c,n}), \bu(t_{c,n})), & \forall n \in [0,N-1] \\ & \bx[0] = \bx_0 \\ & + \text{additional constraints}. \end{align*} I hope this notation is clear -- I'm using $\bx[k] = \bx(t_k)$ as the decision variables, and the collocation constraint at $t_{c,k}$ depends on the decision variables: $\bx[k], \bx[k+1], \bu[k], \bu[k+1]$. The actual equations of motion get evaluated at both the break points, $t_k$, and the collocation times, $t_{c,k}$, because we need to evaluate the dynamics at the break points in order to write $\dot{\bx}(t_{c,k}).$

Once again, direct collocation effectively integrates the equations of motion by satisfying the constraints of the optimization -- this time producing an integration of the dynamics that is accurate to third order with effectively two evaluations of the plant dynamics per segment (since we use $\dot\bx(t_k)$ for two intervals). Hargraves87 claims, without proof, that as the break points are brought closer together, the trajectory will converge to a true solution of the differential equation. Once again it is very natural to add additional terms to the cost function or additional input/state constraints, and very easy to calculate the gradients of the objective and constraints. I personally find it very nice to explicitly account for the parametric encoding of the trajectory in the solution technique.

Direct Collocation for the Pendulum, Acrobot, and Cart-Pole

Direct collocation also easily solves the swing-up problem for the pendulum, Acrobot, and cart-pole system. Try it for yourself:

As always, make sure you take a look at the code!

Pseudo-spectral Methods

The direct collocation method of Hargraves87 was our first example of explicitly representing the solution of the optimal control problem as a parameterized trajectory, and adding constraints to the derivatives at a series of collocation times. In the algorithm above, the representation of choice was piecewise-polynomials, e.g. cubic spines, and the trajectory values at the break points were the decision variables. A closely related approach, often called "pseudo-spectral" optimal control, uses the same collocation idea, but represents the trajectories instead using a linear combination of global, polynomial basis functions. These methods use typically much higher-degree polynomials, but can leverage clever parameterizations to write sparse collocation objectives and to select the collocation times Garg11+Ross12a. Interestingly, the continuously-differentiable nature of the representation of these methods has led to comparatively more theorems and analysis than we have seen for other direct trajectory optimization methods Ross12a -- but despite some of the language used in these articles please remember they are still local optimization methods trying to solve a nonconvex optimization problem. While the direct collocation method above might be expected to converge to the true optimal solution by adding more segments to the piecewise polynomial (and having each segment represent a smaller interval of time), here we expect convergence to happen as we increase the degree of the polynomials.

The pseudo-spectral methods are also sometimes knowns as "orthogonal collocation" because the $N$ basis polynomials, $\phi_j(t)$, are chosen so that at the $j$th collocation time $t_j$, we have $$\phi_i(t_j) = \begin{cases} 1 & i=j, \\ 0 & \text{otherwise.}\end{cases}$$ This can be accomplished by using the Lagrange polynomials, $$\phi_i(t) = \prod_{j=0, j\ne i}^{N} \frac{t-t_j}{t_i - t_j}.$$ Note that for both numerical reasons and for analysis, time is traditionally rescaled from the interval $[t_0, t_f]$ to $[-1, 1]$. Collocation times are chosen based on small variations of Gaussian quadrature, known as the "Gauss-Lobatto" which includes collocation times at $t=-1$ and $t=1$.

Interestingly, a number of papers have also considered infinite-horizon pseudo-spectral optimization by the nonlinear rescaling of the time interval $t\in[0, \infty)$ to the half-open interval $\tau\in[-1, 1)$ via $\tau = \frac{t-1}{t+1}$ Garg11+Ross12a. In this case, one chooses the collocation times so that they include $\tau = -1$ but do not include $\tau=1$, using the so-called "Gauss-Radau" points Garg11.

Efficiently computing gradients

Providing gradients of the objectives and constraints to the solver is not strictly required -- most solvers will obtain them from finite differences if they are not provided -- but I feel strongly that for smooth problems the solvers are faster and more robust when exact gradients are provided (systems that make and break contact might be a different storySuh22b). Providing the gradients for the direct transcription methods is very straight-forward -- we simply provide the gradients for each constraint individually. But in the direct shooting approach, where we have removed the $\bx$ decision variables from the program but still write objectives and constraints in terms of $\bx$, it would become very inefficient to compute the gradients of each objective/constraint independently. We need to leverage the chain rule.

To be concise (and slightly more general), let us define $\bx[n+1]=f_d(\bx[n],\bu[n])$ as the discrete-time approximation of the continuous dynamics; for example, the forward Euler integration scheme used above would give $f_d(\bx[n],\bu[n]) = \bx[n]+f(\bx[n],\bu[n])dt.$ Then we have \[\pd{J}{\bu_k} = \pd{\ell_f(\bx[N])}{\bu_k} + \sum_{n=0}^{N-1} \left(\pd{\ell(\bx[n],\bu[n])}{\bx[n]} \pd{\bx[n]}{\bu_k} + \pd{\ell(\bx[n],\bu[n])}{\bu_k} \right), \] where the gradient of the state with respect to the inputs can be computed during the "forward simulation", \[ \pd{\bx[n+1]}{\bu_k} = \pd{f_d(\bx[n],\bu[n])}{\bx[n]} \pd{\bx[n]}{\bu_k} + \pd{f_d(\bx[n],\bu[n])}{\bu_k}. \] These simulation gradients can also be used in the chain rule to provide the gradients of any constraints. Note that there are a lot of terms to keep around here, on the order of (state dim) $\times$ (control dim) $\times$ (number of time steps). Ouch. Note also that many of these terms are zero; for instance with the Euler integration scheme above $\pd{\bu[n]}{\bu_k} = 0$ if $k\ne n$. (If this looks like I'm mixing two notations here, recall that I'm using $\bu_k$ to represent the decision variable and $\bu[n]$ to represent the input used in the $n$th step of the simulation.)

The special case of direct shooting without state constraints

By solving for $\bx(\cdot)$ ourselves, we've removed a large number of constraints from the optimization. If no additional state constraints are present, and the only gradients we need to compute are the gradients of the objective, then a surprisingly efficient algorithm emerges. I'll give the steps here without derivation, but will derive it in the Pontryagin section below:

1. Simulate forward: $$\bx[n+1] = f_d(\bx[n],\bu_n),$$ from $\bx[0] = \bx_0$.
2. Calculate backwards: $$\lambda[n-1] = \pd{\ell(\bx[n],\bu[n])}{\bx[n]}^T + \pd{f(\bx[n],\bu[n])}{\bx[n]}^T \lambda[n],$$ from $\lambda[N-1]=\pd{\ell_f(\bx[N])}{\bx[N]}$.
3. Extract the gradients: $$\pd{J}{\bu[n]} = \pd{\ell(\bx[n],\bu[n])}{\bu[n]} + \lambda[n]^T \pd{f(\bx[n],\bu[n])}{\bu[n]},$$ with $\pd{J}{\bu_k} = \sum_n \pd{J}{\bu[n]}\pd{\bu[n]}{\bu_k}$.

Here $\lambda[n]$ is a vector the same size as $\bx[n]$ which has an interpretation as $\lambda[n]=\pd{J}{\bx[n+1]}^T$. The equation governing $\lambda$ is known as the adjoint equation, and it represents a dramatic efficiency improvement over calculating the huge number of simulation gradients described above. In case you are interested, yes the adjoint equation is exactly the backpropagation algorithm that is famous in the neural networks literature, or more generally a bespoke version of reverse-mode automatic differentiation. (The previous section would correspond to forward-mode automatic differentiation.)

Penalty methods and the Augmented Lagrangian

In fact, in recent years we have seen a surge in popularity in robotics for doing trajectory optimization using (often special-purpose) solvers for unconstrained trajectory optimization, where the constrained problems are transformed into unconstrained problem via penalty methods.

Manually adding penalty terms to the cost function with user-specified cost weighting can be nasty business. Often this generates a significant amount of effort in "cost function tuning"; setting the weights too large can cause the problems to become numerically ill-conditioned, but setting them too small can allow unwanted constraint violations. The augmented Lagrangian approach offers a principled solution to this problem, by automatically scheduling the weights in order to achieve "essentially exact" satisfaction of the constraints with finite penalties Bertsekas82, Nocedal06.

Methods based on the augmented Lagrangian are particularly popular for trajectory optimization these days Lin91+Toussaint14.

Zero-order optimization

Cross-entropy method (CEM) Kobilarov12 and Model-predictive path-integral control (MPPI) Williams17.

In the last few years, we've seen trajectory optimization playing a major role in "model-based reinforcement learning", especially when the model is represented using a deep neural network (e.g. Li18+Li18a). Despite the fact that neural network toolboxes can provide efficient gradients, the gradient-free trajectory optimization methods described here normally the tool of choice. There are a few possible explanations for this. The trajectory optimization problem has local minima, and the zero-order optimizers do provide some aspect of "global optimization" (at least they tend to explore multiple possible minima before converging). Neural network models fit with limited data may even have more local minima than if one used the original physics model -- the standard regression loss could be small even if the neural representation of the function is locally bumpy/non-smooth. But I think the biggest reason for the popularity of the zero-order methods is that they are trivially parallelizable, and very nicely compatible with the existing GPU neural-network workflows. When rolling out 10,000 samples in parallel costs approximately the same as rolling out 1, then this certainly makes the zero-order methods more appealing.

Getting good solutions... in practice.

As you begin to play with these algorithms on your own problems, you might feel like you're on an emotional roller-coaster. You will have moments of incredible happiness -- the solver may find very impressive solutions to highly nontrivial problems. But you will also have moments of frustration, where the solver returns an awful solution, or simply refuses to return a solution (saying "infeasible"). The frustrating thing is, you cannot distinguish between a problem that is actually infeasible, vs. the case where the solver was simply stuck in a local minima.

So the next phase of your journey is to start trying to "help" the solver along. There are two common approaches.

The first is tuning your cost function -- some people spend a lot of time adding new elements to the objective or adjusting the relative weight of the different components of the objective. This is a slippery slope, and I tend to try to avoid it (possibly to a fault; other groups tend to put out more compelling videos!).

The second approach is to give a better initial guess to your solver to put it in the vicinity of the "right" local minimal. I find this approach more satisfying, because for most problems I think there really is a "correct" formulation for the objective and constraints, and we should just aim to find the optimal solution. Once again, we do see a difference here between the direct shooting algorithms and the direct transcription / collocation algorithms. For shooting, we can only provide the solver with an initial guess for $\bu(\cdot)$, whereas the other methods allow us to also specify an initial guess for $\bx(\cdot)$ directly. I find that this can help substantially, even with very simple initializations. In the direct collocation examples for the swing-up problem of the Acrobot and cart-pole, simply providing the initial guess for $\bx(\cdot)$ as a straight line trajectory between the start and the goal was enough to help the solver find a good solution; in fact it was necessary.

I'm old enough now, though, that I've tired of playing games with finicky solvers that require hand-holding. In recent work, my group has been working on more robust "global" solutions to the motion planning problem, based on an idea call Graphs of Convex Sets (GCS) Marcucci21. The initial transcriptions of motion planning into GCS were focused on kinematic trajectory optimization (as needed in robotic manipulation) Marcucci23a, but we are growing the capability towards dynamic (underactuated) trajectory optimization. You can read more about here.

Finite-horizon LQR

We have already developed one approach for trajectory stabilization in the LQR chapter. This is one of my favorite approaches to trajectory feedback, because it provides a (numerically) closed-form solution for the controller, $\bK(t),$ and even comes with a time-varying quadratic cost-to-go function, $S(t),$ that can be used for Lyapunov analysis.

The basic procedure is to create a time-varying linearization along the trajectory in the error coordinates: $\bar\bx(t) = \bx(t) - \bx_0(t)$, $\bar\bu(t) = \bu(t)-\bu_0(t)$, and $\dot{\bar{\bx}}(t) = {\bf A}(t)\bar\bx(t) + {\bf B}(t)\bar\bu(t).$ This linearization uses all of the same gradients of the dynamics that we have been using in our trajectory optimization algorithms. Once we have the time-varying linearization, then we can apply finite-horizon LQR (see the LQR chapter for the details).

A major virtue of this approach is that we can proceed immediately to verifying the performance of the closed-loop system under the LQR policy. Specifically, we can apply the finite-time reachability analysis to obtain "funnels" that certify a desired notion of invariance -- guaranteeing that trajectories which start near the planned trajectory will stay near the planned trajectory. Please see the finite-time reachability analysis section for those details. We will put all of these ideas together in the perching case-study below.

Model-Predictive Control

The maturity, robustness, and speed of solving trajectory optimization using convex optimization leads to a beautiful idea: if we can optimize trajectories quickly enough, then we can use our trajectory optimization as a feedback policy. The recipe is simple: (1) measure the current state, (2) optimize a trajectory from the current state, (3) execute the first action from the optimized trajectory, (4) let the dynamics evolve for one step and repeat. This recipe is known as model-predictive control (MPC).

Despite the very computational nature of this controller (there is no closed-form representation of this policy; it is represented only implicitly as the solution of the optimization), there is a bounty of theoretical and algorithmic results on MPC Garcia89+Camacho13. The theoretical and practical aspects of Linear MPC are so well understood today that it is considered the de-facto generalization of LQR for controlling a linear system subject to (linear) constraints. And there are a few core ideas that practitioners should really understand.

The Flat-Plate Glider Model

In our experiments, we found the dynamics of our aircraft Cory08 were captured very well by the so-called "flat plate model" Hoerner85. In flat plate theory lift and drag forces of a wing are summarized by a single force at the center-of-pressure of the wing acting normal to the wing, with magnitude: $$f_n(S, {\bf n}, \bv) = \rho S \sin\alpha |\bv|^2 = -\rho S ({\bf n} \cdot \bv) |\bv|,$$ where $\rho$ is the density of the air, $S$ is the area of the wing, $\alpha$ is the angle of attack of the surface, ${\bf n}$ is the normal vector of the lifting surface, and $\bv$ is the velocity of the center of pressure relative to the air. This corresponds to having lift and drag coefficients $$c_{\text{lift}} = 2\sin\alpha\cos\alpha, \quad c_{\text{drag}} = 2\sin^2\alpha.$$ In our glider model, we summarize all of the aerodynamic forces by contributions of two lifting surfaces, the wing (including some contribution from the horizontal fuselage) denoted by subscript $w$ and the elevator denoted by subscript $e$, with centers at $\bp_w = [x_w, z_w]^T$ and $\bp_e = [x_e, z_e]^T$ given by the kinematics: $$\bp_w = \bp - l_w\begin{bmatrix} c_\theta \\ -s_\theta \end{bmatrix},\quad \bp_e = \bp - l_h \begin{bmatrix} c_\theta \\ -s_\theta \end{bmatrix} - l_e \begin{bmatrix} c_{\theta+\phi} \\ -s_{\theta+\phi} \end{bmatrix},$$ where the origin of our vehicle coordinate system, $\bp = [x,z]^T$, is chosen to be the center of mass. We assume still air, so $\bv = \dot{\bp}$ for both the wing and the elevator. We assume that the elevator is massless, and the actuator controls velocity directly (note that this breaks our "control affine" structure, but is more realistic for the tiny hobby servos we were dealing with). This gives us a total of 7 state variables $\bx = [x, z, \theta, \phi, \dot{x}, \dot{z}, \dot\theta]^T$ and one control input $\bu = \dot\phi.$ The resulting equations of motion are: \begin{gather*} {\bf n}_w = \begin{bmatrix} s_\theta \\ c_\theta \end{bmatrix}, \quad {\bf n}_e = \begin{bmatrix} s_{\theta+\phi} \\ c_{\theta+\phi} \end{bmatrix}, \\ f_w = f_n(S_w, {\bf n}_w, \dot\bp_w), \quad f_e = f_n(S_e, {\bf n}_e, \dot\bp_e), \\ \ddot{x} = \frac{1}{m} \left(f_w s_\theta + f_e s_{\theta+\phi} \right), \\ \ddot{z} = \frac{1}{m} \left(f_w c_\theta + f_e c_{\theta+\phi} \right) - g, \\ \ddot\theta = \frac{1}{I} \left( l_w f_w + (l_h c_\phi + l_e) f_e \right). \end{gather*}

Trajectory optimization

Trajectory stabilization

Trajectory funnels

Beyond a single trajectory

The linear controller around a nominal trajectory was surprisingly effective, but it's not enough. We'll return to this example again when we talk about "feedback motion planning", in order to discuss how to find a controller that can work for many more initial conditions -- ideally all of the initial conditions of interest for which the aircraft is capable of getting to the goal.

Lagrange multiplier derivation of the adjoint equations

Let us use Lagrange multipliers to derive the necessary conditions for our constrained trajectory optimization problem in discrete time \begin{align*} \min_{\bx[\cdot],\bu[\cdot]} & \ell_f(\bx[N]) + \sum_{n=0}^{N-1} \ell(\bx[n],\bu[n]),\\ \subjto \quad & \bx[n+1] = f_d(\bx[n],\bu[n]). \end{align*} Formulate the Lagrangian, \[L(\bx[\cdot],\bu[\cdot],\lambda[\cdot]) = \ell_f(\bx[N]) + \sum_{n=0}^{N-1} \ell(\bx[n],\bu[n]) + \sum_{n=0}^{N-1} \lambda^T[n] \left(f_d(\bx[n],\bu[n]) - \bx[n+1]\right), \] and set the derivatives to zero to obtain the adjoint equation method described for the shooting algorithm above: \begin{gather*} \forall n\in[0,N-1], \pd{L}{\lambda[n]} = f_d(\bx[n],\bu[n]) - \bx[n+1] = 0 \Rightarrow \bx[n+1] = f(\bx[n],\bu[n]) \\ \forall n\in[0,N-1], \pd{L}{\bx[n]} = \pd{\ell(\bx[n],\bu[n])}{\bx} + \lambda^T[n] \pd{f_d(\bx[n],\bu[n])}{\bx} - \lambda^T[n-1] = 0 \\ \quad \Rightarrow \lambda[n-1] = \pd{\ell(\bx[n],\bu[n])}{\bx}^T + \pd{f_d(\bx[n],\bu[n])}{\bx}^T \lambda[n]. \\ \pd{L}{\bx[N]} = \pd{\ell_f}{\bx}^T - \lambda^T[N-1] = 0 \Rightarrow \lambda[N-1] = \pd{\ell_f}{\bx} \\ \forall n\in[0,N-1], \pd{L}{\bu[n]} = \pd{\ell(\bx[n],\bu[n])}{\bu} + \lambda^T[n] \pd{f_d(\bx[n],\bu[n])}{\bu} = 0. \end{gather*} Therefore, if we are given an initial condition $\bx_0$ and an input trajectory $\bu[\cdot]$, we can verify that it satisfies the necessary conditions for optimality by simulating the system forward in time to solve for $\bx[\cdot]$, solving the adjoint equation backwards in time to solve for $\lambda[\cdot]$, and verifying that $\pd{L}{\bu[n]} = 0$ for all $n$. The fact that $\pd{J}{\bu} = \pd{L}{\bu}$ when $\pd{L}{\bx} = 0$ and $\pd{L}{\lambda} = 0$ follows from some basic results in the calculus of variations.

Necessary conditions for optimality in continuous time

You won't be surprised to hear that these necessary conditions have an analogue in continuous time. I'll state it here without derivation. Given the initial conditions, $\bx_0$, a continuous dynamics, $\dot\bx = f(\bx,\bu)$, and the instantaneous cost $\ell(\bx,\bu)$, for a trajectory $\bx(\cdot),\bu(\cdot)$ defined over $t\in[t_0,t_f]$ to be optimal it must satisfy the conditions that \begin{align*} \forall t\in[t_0,t_f],\quad & \dot{\bx} = f(\bx,\bu), \quad &\bx(0)=\bx_0\\ \forall t\in[t_0,t_f],\quad & -\dot\lambda = \pd{\ell}{\bx}^T + \pd{f}{\bx}^T \lambda, \quad &\lambda(T) = \pd{\ell_f}{\bx}^T \\ \forall t\in[t_0,t_f],\quad & \pd{\ell}{\bu} + \lambda^T\pd{f}{\bu} = 0.& \end{align*}

In fact the statement can be generalized even beyond this to the case where $\bu$ has constraints. The result is known as Pontryagin's minimum principle -- giving necessary conditions for a trajectory to be optimal.

Note that the terms which are minimized in the final line of the theorem are commonly referred to as the Hamiltonian of the optimal control problem, $$H(\bx,\bu,\lambda,t) = \ell(\bx,\bu) + \lambda^T f(\bx,\bu).$$ It is distinct from, but inspired by, the Hamiltonian of classical mechanics. Remembering that $\lambda$ has an interpretation as $\pd{J}{\bx}^T$, you should also recognize it from the HJB.

Differential Flatness

There are some very important cases where nonconvex trajectory optimization can be turned back into convex trajectory optimization based on a clever change of variables. One of the key ideas in this space is the notion of "differential flatness", which is philosophically quite close to the idea of partial feedback linearization that we discussed for acrobots and cart-pole systems. But perhaps the most famous application of differential flatness, which we will explore here, is actually for quadrotors.

One of the most important lessons from partial feedback linearization, is the idea that if you have $m$ actuators, then you basically get to control exactly $m$ quantities of your system. Differential flatness exploits this same idea (choosing $m$ outputs), but in the opposite direction. The idea is that, for some systems, if you give me a trajectory in those $m$ coordinates, it may in fact dictate what all of the states/actuators must have been doing. The fact that you can only execute a subset of possible trajectories can, in this case, make trajectory planning much easier!

Let's start with an example...

Differential flatness for the Planar Quadrotor

The planar quadrotor model described earlier has 3 degrees of freedom ($x,y,\theta$) but only two actuators (one for each propeller). My claim is that, if you give me a trajectory for just the location of the center of mass: $x(t), y(t), \forall t \in [t_0, t_f],$ then I will be able to infer what $\theta(t)$ must be over that same interval in order to be feasible. Furthermore, I can even infer $\bu(t)$. There is one technical condition required: the trajectory you give me for $x(t)$ and $y(t)$ needs to be continuously differentiable (four times).

To see this, recall the equations of motion for this system were given by: \begin{gather} m \ddot{x} = -(u_1 + u_2)\sin\theta, \label{eq:quad_x}\\ m \ddot{y} = (u_1 + u_2)\cos\theta - mg, \label{eq:quad_y}\\ I \ddot\theta = r (u_1 - u_2) \label{eq:quad_theta} \end{gather} Observe that from ($\ref{eq:quad_x}$) and ($\ref{eq:quad_y}$) we have $$\frac{-m \ddot{x}}{ m \ddot{y} + mg} = \frac{(u_1 + u_2)\sin\theta}{(u_1+u_2)\cos\theta} = \tan\theta.$$ In words, given $\ddot{x}(t)$ and $\ddot{y}(t)$, I can solve for $\theta$(t). I can differentiate this relationship (in time) twice more to obtain $\ddot\theta$. Using ($\ref{eq:quad_theta}$) with ($\ref{eq:quad_x}$) or ($\ref{eq:quad_y}$) give us two linear equations for $u_1$ and $u_2$ which are easily solved.

Now you can see why we need the original trajectories to be smooth -- the solution to $\bu(t)$ depends on $\ddot\theta(t)$ which depends on $\frac{d^4 x(t)}{dt^4}$ and $\frac{d^4 y(t)}{dt^4}$; we need those derivatives to exist along the entire trajectory.

What's more -- if we ignore input limits for a moment -- any sufficiently smooth trajectory of $x(t), y(t)$ is feasible, so if I can simply find one that avoids the obstacles, then I have designed my state trajectory. As we will see, optimizing even high-degree piecewise-polynomials is actually an easy problem (it works out to be a quadratic program), assuming the constraints are convex. In practice, this means that once you have determined whether to go left or right around the obstacles, trajectory design is easy and fast.

I've coded up a simple example of that for you here:

The example above demonstrates that the planar quadrotor system is differentially flat in the outputs $x(t)$, $y(t)$. More generally, if we have a dynamical system $$\dot{\bx} = f(\bx, \bu),$$ and we design some output coordinates (essentially "task-space"): $$\bz(t) = h\left(\bx, \bu, \frac{d\bu}{dt}, ..., \frac{d^k\bu}{dt^k}\right),$$ such that we can write the $\bx$ and $\bu$ purely as a function of the output and it's time derivatives, \begin{gather*} \bx (t) = \bx\left(\bz, \frac{d\bz}{dt}, ..., \frac{d^k\bz}{dt^k}\right), \\ \bu (t) = \bu\left(\bz, \frac{d\bz}{dt}, .., \frac{d^k\bz}{dt^k}\right),\end{gather*} then we say that the system $f$ is differentially flat in the outputs $\bz$ Murray95. And the requirement for showing that a system is differentially flat in those outputs is to write the function which solves for $\bx(t)$ and $\bu(t)$ as a function of only $\bz(t)$ and its time derivatives.

I'm not aware of any numerical recipes for showing that a system is differentially flat nor for finding potential flat outputs, but I admit I haven't worked on it nor looked for those recipes. That would be interesting! I think of differential flatness as a property that one must find for your system -- typically via a little mechanical intuition and a lot of algebra. Once found, it can be very useful.

Iterative LQR and Differential Dynamic Programming

There is another approach to trajectory optimization (at least for initial-value problems) that has gotten quite popular lately. Iterative LQR (iLQR)Li04 also known as Sequential Linear Quadratic optimal control) Sideris05. The idea is simple enough: given an initial guess at the input and state trajectory, make a linear approximation of the dynamics and a quadratic approximation of the cost function. Then compute and simulate the time-varying LQR controller to find a new input and state trajectory. Repeat until convergence.

The motivation behind iterative LQR is quite appealing -- it leverages the surprising structure of the Riccati equations to come up with a second-order update to the trajectory after a single backward pass of the Riccati equation followed by a forward simulation. Anecdotally, the convergence is fast and robust. Numerous applications...Farshidian17+Tassa12.

One key limitation of iLQR is that it only supports unconstrained trajectory optimization -- at least via the Riccati solution. The natural extension for constrained optimization would be to replace the Riccati solution with an iterative linear model-predictive control (MPC) optimization; this would result in a quadratic program and is very close to what is happening in SQP. But a more common approach in the current robotics literature is to add any constraints into the objective function using penalty methods, especially using Augmented Lagrangian.

Differential Dynamic Programming (DDP) Jacobson70 is inspired by the idea of solving locally quadratic approximations of the HJB equations. The resulting algorithm is almost identical to iLQR; the only difference is that DDP uses a second-order approximation of the plant dynamics instead of a first-order (linear) one. The conventional wisdom is that taking these extra plant derivatives is probably not worth the extra cost of computing them; in most cases iLQR converges just as quickly as DDP (though the details will be problem dependent). Unfortunately, despite the distinction between these algorithms being unambiguous and uncontroversial, you will find many papers that incorrectly write DDP when they are actually using iLQR.

Leveraging combinatorial optimization

Many of the nonconvex costs and constraints that we have been dealing with (or avoiding!) can naturally be represented as "disjunctive" constraints (e.g. $x \in A$ or $x \in B$). There has been an important series of work showing how mixed-integer programming can be used, for instance, to perform trajectory optimziation around obstacles Richards02+Mellinger12+Deits15+Landry15b. These methods nicely distinguish separate the discrete/combinatorial aspects of the optimization problem (e.g. do I fly left or right around the tree) from the continuous aspects (e.g. what smooth path/trajectory should I take, assuming I decide to fly left around the tree?), and leverage mostly mixed-integer programming solvers based on "branch and bound" to solve the transcribed problem to global optimality. However, these transcriptions were not particularly efficient, leading to potentially very long solve times in the branch-and-bound solver.

Recently this approach to trajectory optimization has gotten dramatically more effective using a new framework call Graphs of Convex Sets" (GCS). GCS provides a much more efficient transcription for many of the discrete+continuous trajectory optimization problems. In many cases, the convex relaxation of the mixed-integer problem is tight enough that one only needs to solve the convex relaxation (followed by a little "rounding") to solve the problem to global optimality.

Minimum-time linear optimal control

Recall our first example of the chapter was doing trajectory optimization for the double integrator. Optimizing convex objectives over a fixed horizon is naturally transcribed into convex optimization problem. But in order to solve the minimum-time problem (where the horizon is unknown), we had to solve multiple optimization problems to find the correct horizon.

This search over horizon can naturally be encoded as a mixed-integer problem, and GCS provides a natural and efficient framework for doing it. First, let's observe that the original fixed-horizon problem can naturally be described as a GCS: take the sets to be the domain of $x$ (to make these domains bounded, one can simply pick a conservative bounding box). Our graph will simply be a serial chain with one copy of this set for each timestep, and edge constraints that enforce the dynamics: $$\bx[n+1] - \bA \bx[n] \in \bB {\bf U},\qquad {\bf U} = \left\{ \begin{bmatrix}u\end{bmatrix} \, | \, u \in [-1, 1] \right\}.$$ Awesomely, once we implement all of the code optimizations that we are currently working on for Drake's GCS implementation, formulating the problem in this way will give you back exactly the same linear program that we solved above.

Now we can make the time duration optional by defining edges, with constraints, from each timestep directly to the goal:

Note that for this example, the GCS convex relaxation is not tight (yet; I still have some ideas), but the convex_relaxation = False version solves beautifully, if you have an MIP solver available.

The minimum-time double integrator problem is too simple to warrant the use of GCS. Afterall, the line search we recommended above can work efficiently enough. But this is a nice simple illustration of how one might formulate trajectory optimization as a GCS, and it will scale to much more complicated formulations.

Motion planning around obstacles

Let's consider the simple problem of planning a path (even without any dynamics) for a point robot moving around an obstacle. This is our canonical example of a non-convex constraint that we might encounter in our trajectory optimization formulations.

In the notebook below, I've implemented a handful of different ways to transcribe this into an optimization problem.

Fill this out with all of the details from the slides in e.g. my columbia GCS seminar.

Minimum-snap trajectories for quadrotors flying around obstacles

Mellinger11 showed that minimum-snap is convex (as long as each time step of the plan is confined to a single convex region) using differential flatness. Mellinger12 used an older MIP formulation to allow the assignment of time steps to convex regions to be chosen by the optimizer. But in both of these formulations, the total duration of the trajectory had to be fixed. Requiring a fixed duration, in my opinion, is a very unfortunate limitation.

Marcucci23a showed that the quadrotor motion planning problem could be transcribed using GCS. Moreover, this paper considered a richer version of the problem where the duration in each convex set is not pre-specified, but is jointly optimized. Unfortunately, constraints on acceleration, jerk, and snap are nonconvex, and so did not fit neatly into this transcription.

More recently, Wrangel24 showed that we can effectively handle the non-convexity of the derivative constraints (with time-scaling) by using convex approximations of the constraints in the (global) GCS search, and then using the exact nonconvex version of these constraints in the final rounding step.

Try it out for yourself:

You can check out the trajectories in the meshcat animation.

Explicit model-predictive control

We do actually understand what the optimal policy of the inequality-constrained LQR problem looks like, thanks to work on "explicit MPC" Alessio09 -- the optimal policy is now piecewise-linear (though still continuous), with each piece described by a polytope, and the optimal cost-to-go is piecewise-quadratic on the same polytopes. Unfortunately, the number of pieces grows exponentially with the number of constraints and the horizon of the problem, making it impractical to compute for all but very small problems. There are, however, a number of promising approaches to approximate explicit MPC (c.f. Marcucci17).