M9-T3 — Convex Optimization for Guidance

Learning objectives

After this lesson, you should be able to:

• Define convex sets and functions and recognize linear, quadratic and second-order cone constraints.
• Explain what a barrier solver returns and why the duality gap is a certificate.
• Pose a bounded-fuel guidance problem as a linear or second-order cone program.
• Detect infeasibility with the solver and say what it means.

The intuition

UAV M5-T8: Model Predictive Control and Navigation M5-T6: LQR and MPC for Path Tracking solve a small quadratic program at every step as a recipe. This lesson explains why that is safe to do on board: the class of problem has a theory that nonlinear programs do not.

What convex means

A set is convex if the straight line between any two of its points lies inside it. A function is convex if its graph lies below any chord. A convex problem minimizes a convex function over a convex set. The picture is a bowl: there is one lowest point, and any method that goes downhill finds it. The thrust of a rocket engine with an upper limit is a convex set (a disk in the plane): any blend of two allowed thrusts is allowed. With a lower limit as well it is a ring, and the chord between two points passes through the hole: not convex. The lower bound of a throttle-limited engine is the one non-convex ingredient of powered descent, and the next lesson shows how to remove it.

Three classes that cover guidance

A linear program has a linear cost and linear constraints; a quadratic program has a convex quadratic cost; a second-order cone program adds constraints of the form $\|\mathbf{A}\mathbf{x} + \mathbf{b}\| \le \mathbf{c}^{\mathsf{T}}\mathbf{x} + d$, which say that a vector is no longer than a number, such as the magnitude of a thrust vector. Fuel as the sum of thrust magnitudes, a thrust cone, a glide-slope cone, a pointing constraint are all of this form. Each class contains the one before it.

A solver with a proof

A barrier method replaces each constraint by a logarithmic penalty and follows the minimizers of the penalized cost as the penalty is relaxed. At each stage it knows that the cost is within $m/t$ of the optimum, where $m$ is the number of constraints and $t$ the weight of the cost: the duality gap. The solver stops when the gap is below a tolerance, and the result is a certificate. For Hopper's minimum-fuel slew posed as a linear program with 80 variables it takes 9 stages and ends with a gap below 10⁻⁸. If the constraints cannot be met, the first phase of the solver says so: with only 6 s of descent time and a thrust limit of 15 m/s² it reports the planar problem infeasible.

Convex problems have one optimum, a solver that proves how close it is, and an iteration count that grows slowly with size. Powered descent is nearly convex: the cost and dynamics are linear or conic, and only the thrust floor is not.

Definitions

Convex set

A set $C$ such that $\theta\mathbf{x} + (1 - \theta)\mathbf{y} \in C$ for all $\mathbf{x}, \mathbf{y} \in C$ and $\theta \in [0, 1]$.

Convex function

A function with $f(\theta\mathbf{x} + (1-\theta)\mathbf{y}) \le \theta f(\mathbf{x}) + (1-\theta)f(\mathbf{y})$.

LP, QP, SOCP

Linear, quadratic and second-order cone programs: cost linear, convex quadratic, or linear; constraints linear, linear, or linear plus $\|\mathbf{A}\mathbf{x} + \mathbf{b}\| \le \mathbf{c}^{\mathsf{T}}\mathbf{x} + d$.

Duality gap

An upper bound on the difference between the cost of the current solution and the optimum.

Infeasible

No point satisfies all constraints; a convex solver can prove it.

Derivation: the certificate, and the conic form of a thrust bound

Why the barrier solution is within $m/t$ of the optimum, for a problem with inequalities $\phi_i(\mathbf{x}) \le 0$.

1. $$\mathbf{x}^*(t) = \arg\min\ t f_0(\mathbf{x}) + \phi(\mathbf{x}),\quad \phi = -\sum_i\log(-\phi_i)$$

   The central point: the minimizer of the barrier problem for weight t (equality constraints are kept exactly).

2. $$t\nabla f_0(\mathbf{x}^*) + \sum_i\dfrac{\nabla\phi_i(\mathbf{x}^*)}{-\phi_i(\mathbf{x}^*)} = 0$$

   The stationarity condition at the central point.

3. $$\lambda_i = \dfrac{-1}{t\,\phi_i(\mathbf{x}^*)}\ge 0 \;\Rightarrow\; \nabla f_0 + \sum_i\lambda_i\nabla\phi_i = 0$$

   Divide by t and name the coefficients: they are feasible dual variables, because the Lagrangian's gradient vanishes at $\mathbf{x}^*$.

4. $$g(\boldsymbol\lambda) = f_0(\mathbf{x}^*) + \sum_i\lambda_i\phi_i(\mathbf{x}^*) = f_0(\mathbf{x}^*) - m/t \le p^*$$

   The dual function at these multipliers is a lower bound on the optimum (weak duality) and equals the cost minus $\sum\lambda_i(-\phi_i) = m/t$.

5. $$f_0(\mathbf{x}^*(t)) - p^* \le m/t$$

   The cost of the central point exceeds the optimum by at most m/t: the certificate. It needs convexity, because only then is the dual function a bound valid at a minimizer of the Lagrangian.

The cone form of a thrust magnitude

1. $$\|\mathbf{u}_k\| \le \Gamma_k$$

   The thrust acceleration vector at step k is no longer than the slack variable $\Gamma_k$: this is a second-order cone constraint with $\mathbf{F}_i$ selecting the two components and $\mathbf{q}_i$ selecting $\Gamma_k$.

2. $$\min\ \sum_k\Gamma_k h$$

   The fuel, taken as the integral of the thrust magnitude, is linear in the slacks. At the optimum the constraint holds with equality (otherwise a smaller $\Gamma_k$ would lower the cost), so $\Gamma_k = \|\mathbf{u}_k\|$.

3. $$\Gamma_{\min} \le \Gamma_k \le \Gamma_{\max}$$

   With a slack variable the upper bound is linear in $\Gamma_k$ and convex. The lower bound is also linear in $\Gamma_k$ but, with $\Gamma_k = \|\mathbf{u}_k\|$, it means $\|\mathbf{u}_k\| \ge \Gamma_{\min}$, the exterior of a disk, which is not convex: the topic of the next lesson.

Convexity makes the Lagrangian dual a certified lower bound, so a barrier solver can stop with proof. Thrust magnitudes, glide-slope cones and fuel are second-order cone constraints and linear costs; only a lower bound on thrust magnitude is not convex.

A worked example

Three solver runs on Hopper problems, with the course solver (a log-barrier interior-point method).

1. A linear program. Hopper's pitch slew of 0.2 rad in 1.5 s, minimum $\int|u|\,dt$ with $|u| \le 2.943$ (split into positive and negative parts, 80 variables). The solver stops after 9 barrier stages with a gap below 10⁻⁸. The fuel is 0.2764 rad/s against 0.2752 from the minimum principle: the difference is the piecewise-constant control over 0.0375 s.
2. The same structure. Of 40 controls, 2 are at the limit and 36 are zero: bang-off-bang, with the burst split over a step and a partial step, found by the solver from the cost and constraints alone.
3. A second-order cone program. Planar descent from (−120, 80) m at (8, −6) m/s to rest at the origin in 20 s, thrust acceleration at most 15 m/s², 20 steps, 60 variables, 20 cone constraints. 9 stages, gap below 10⁻⁷, minimum fuel 202.44 m/s.
4. Check. 12 of the 20 steps are at the thrust limit; the largest violation of the cone constraint is −2.7×10⁻⁹ (inside); replaying the optimal controls ends at (3.4×10⁻⁹, −2.9×10⁻¹¹) m with velocity (−1.2×10⁻⁹, −1.7×10⁻⁸) m/s.
5. Infeasibility. With a descent time of 6 s the same thrust limit cannot bring the vehicle to rest at the target: the first phase of the solver finds no strictly feasible point and reports the problem infeasible.

import math
import gnckit as g

# (1) a linear program: minimum-fuel pitch slew, the bang-off-bang problem of the previous lesson, now by a solver
amax, th0, T, N = 2.943, 0.2, 1.5, 40
h = T / N
Ap, bp = [], []
# u = up - um, both in [0, amax]; terminal rate and angle are linear in the controls
col_v = [h] * N
col_p = [h * (T - (k + 0.5) * h) for k in range(N)]
A = [col_v + [-v for v in col_v], col_p + [-v for v in col_p]]
b = [0.0, -th0]
G, hv = [], []
for j in range(2 * N):
    row = [0.0] * (2 * N); row[j] = -1.0; G.append(row); hv.append(0.0)
    row = [0.0] * (2 * N); row[j] = 1.0; G.append(row); hv.append(amax)
c = [h] * (2 * N)
x, info = g.solve_convex(c, A=A, b=b, G=G, h=hv)
u = [x[k] - x[N + k] for k in range(N)]
t1 = (T - math.sqrt(T * T - 4 * th0 / amax)) / 2
print(f'LP, {2 * N} variables: status {info["status"]}, barrier stages {info["iterations"]}, duality gap below {info["gap"]:.1e}')
print(f'  minimum fuel {info["cost"]:.4f} rad/s; the minimum principle gives {2 * amax * t1:.4f} (the controls are piecewise constant over {h:.4f} s, so the optimum is slightly different)')
print('  first four controls (rad/s2): ' + ', '.join(f'{v:+.3f}' for v in u[:4]) + f' ... {sum(1 for v in u if abs(abs(v) - amax) < 1e-3)} of {N} steps at the limit, {sum(1 for v in u if abs(v) < 1e-3)} at zero')

# (2) a second-order cone program: minimum-fuel planar descent without a thrust floor, ||u|| <= umax
N2, T2 = 20, 20.0
h2 = T2 / N2
gx, gz = 0.0, -9.81
r0, v0 = (-120.0, 80.0), (8.0, -6.0)
umax = 15.0
# variables: u_x[0..N-1], u_z[0..N-1], s[0..N-1]
nv = 3 * N2
def w_row(k):
    return h2 * (T2 - (k + 0.5) * h2)
Aeq, beq = [], []
for ax in (0, 1):
    pos = [0.0] * nv; vel = [0.0] * nv
    for k in range(N2):
        pos[ax * N2 + k] = w_row(k); vel[ax * N2 + k] = h2
    gg = (gx, gz)[ax]
    Aeq.append(vel); beq.append(0.0 - v0[ax] - gg * T2)
    Aeq.append(pos); beq.append(0.0 - r0[ax] - v0[ax] * T2 - 0.5 * gg * T2 * T2)
socs = []
for k in range(N2):
    F = [[0.0] * nv, [0.0] * nv]; F[0][k] = 1.0; F[1][N2 + k] = 1.0
    q = [0.0] * nv; q[2 * N2 + k] = 1.0
    socs.append((F, [0.0, 0.0], q, 0.0))                 # ||(ux, uz)|| <= s_k
Glin, hlin = [], []
for k in range(N2):
    row = [0.0] * nv; row[2 * N2 + k] = 1.0; Glin.append(row); hlin.append(umax)
cc = [0.0] * (2 * N2) + [h2] * N2
x2, info2 = g.solve_convex(cc, A=Aeq, b=beq, G=Glin, h=hlin, socs=socs)
ux, uz, s = x2[:N2], x2[N2:2 * N2], x2[2 * N2:]
print(f'SOCP, {nv} variables, {N2} cone constraints: status {info2["status"]}, barrier stages {info2["iterations"]}, gap below {info2["gap"]:.1e}')
print(f'  minimum fuel (integral of |u|) {info2["cost"]:.2f} m/s; thrust acceleration at the first step {math.hypot(ux[0], uz[0]):.2f} m/s2, at the last {math.hypot(ux[-1], uz[-1]):.2f}; limit {umax}')
print(f'  how many steps are at the thrust limit: {sum(1 for k in range(N2) if math.hypot(ux[k], uz[k]) > umax - 0.05)}; largest violation of ||u|| <= s: {max(math.hypot(ux[k], uz[k]) - s[k] for k in range(N2)):.1e}')
# check the terminal state by simulation of the optimal controls
r = [r0[0], r0[1]]; v = [v0[0], v0[1]]
for k in range(N2):
    a = (ux[k] + gx, uz[k] + gz)
    r = [r[0] + v[0] * h2 + 0.5 * a[0] * h2 * h2, r[1] + v[1] * h2 + 0.5 * a[1] * h2 * h2]
    v = [v[0] + a[0] * h2, v[1] + a[1] * h2]
print(f'  replaying the controls: final position ({r[0]:+.2e}, {r[1]:+.2e}) m, final velocity ({v[0]:+.2e}, {v[1]:+.2e}) m/s')

# (3) the certificate of infeasibility: the same problem with too little time
T3 = 6.0
h3 = T3 / N2
Aeq3 = []
for ax in (0, 1):
    pos = [0.0] * nv; vel = [0.0] * nv
    for k in range(N2):
        pos[ax * N2 + k] = h3 * (T3 - (k + 0.5) * h3); vel[ax * N2 + k] = h3
    gg = (gx, gz)[ax]
    Aeq3.append(vel); Aeq3.append(pos)
beq3 = [0.0 - v0[0] - gx * T3, 0.0 - r0[0] - v0[0] * T3 - 0.5 * gx * T3 * T3, 0.0 - v0[1] - gz * T3, 0.0 - r0[1] - v0[1] * T3 - 0.5 * gz * T3 * T3]
# reorder to (x vel, x pos, z vel, z pos)
x3, info3 = g.solve_convex([0.0] * (2 * N2) + [h3] * N2, A=Aeq3, b=beq3, G=Glin, h=hlin, socs=socs)
print(f'with only {T3:.0f} s: status {info3["status"]} (the thrust limit of {umax} m/s2 cannot bring the vehicle to rest at the target in that time)')

LP, 80 variables: status optimal, barrier stages 9, duality gap below 1.0e-08
  minimum fuel 0.2764 rad/s; the minimum principle gives 0.2752 (the controls are piecewise constant over 0.0375 s, so the optimum is slightly different)
  first four controls (rad/s2): -2.943, -0.742, -0.000, -0.000 ... 2 of 40 steps at the limit, 36 at zero
SOCP, 60 variables, 20 cone constraints: status optimal, barrier stages 9, gap below 9.3e-08
  minimum fuel (integral of |u|) 202.44 m/s; thrust acceleration at the first step 15.00 m/s2, at the last 15.00; limit 15.0
  how many steps are at the thrust limit: 12; largest violation of ||u|| <= s: -2.7e-09
  replaying the controls: final position (+3.43e-09, -2.93e-11) m, final velocity (-1.16e-09, -1.67e-08) m/s
with only 6 s: status infeasible (the thrust limit of 15.0 m/s2 cannot bring the vehicle to rest at the target in that time)

In robotics

Convex optimization is what makes trajectory optimization feasible on board.

• Guidance with a guarantee. A convex guidance problem has a single answer and a solver that terminates with a bounded number of iterations, which is the property a flight certification needs. This is why convexification is used for landing rockets: the problem is arranged so that the solver cannot get stuck in a local optimum or fail to converge.
• Embedded solvers. Code-generated interior-point and active-set solvers for QP and SOCP run in microseconds to milliseconds for the sizes of MPC and guidance, in fixed-size memory with no dynamic allocation.
• Recognizing convexity. The standard rules (sums, maxima, compositions with affine maps) let a problem be checked by inspection. Thrust magnitude, fuel, glide slope, quadratic costs on state and control, and linear dynamics are all fine; products of variables, trigonometric functions of an angle variable and lower bounds on a norm are not.
• Infeasibility as information. The solver says when a requested manoeuvre cannot be done within the limits, which is a result a guidance system can act on, by relaxing a constraint or increasing the time.
• Limits. A convex model is an approximation: here a point-mass with constant gravity and a fixed mass profile. Its optimum is global for that model, not for the vehicle.

The next lesson uses these tools for Hopper's landing: M9-T4: Powered-Descent Guidance by Convexification, and the lab M9-L2: Solve the Landing as a Convex Program solves it. The receding-horizon version is the last lesson of the module.

Common misconceptions

Key takeaways

1. Convex problems have one optimum; LP, QP and SOCP cover linear and quadratic costs and constraints on vector lengths.
2. A barrier solver returns a point with a duality gap $m/t$ that proves it is close to the optimum.
3. Hopper's minimum-fuel slew as an LP (80 variables, 9 stages, gap 10⁻⁸) gives 0.2764 against the minimum-principle 0.2752 and the same bang-off-bang structure.
4. The planar descent as an SOCP (60 variables, 20 cones) reaches the target to 10⁻⁹ m with fuel 202.44 m/s.
5. An infeasible problem is detected and reported, here for a descent time of 6 s.
6. A thrust lower bound makes the problem nonconvex; the next lesson removes it by relaxation.

References and further reading

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

• S. Boyd and L. Vandenberghe, Convex Optimization, Cambridge University Press, 2004, the standard text. Free to read as the authors' PDF; the book is copyrighted.
• M. S. Lobo, L. Vandenberghe, S. Boyd and H. Lebret, "Applications of second-order cone programming", Linear Algebra and its Applications 284, 1998. Paid (Elsevier).
• Stanford EE364A Convex Optimization I (lecture slides and videos) , the course that goes with the book. Free to read; licence stated on the site.
• CVXPY documentation, tutorial , a modelling language for convex problems with examples. Free to read; licence Apache 2.0.

Knowledge check

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

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M9-T2: Direct Methods: Shooting and Collocation

  Transcribe an optimal control problem to a nonlinear program by single shooting, multiple shooting and trapezoidal collocation, and compare their sensitivity to the initial guess.

• Lab
  M9-L2: Solve the Landing as a Convex Program

  Pose a planar divert-and-land as a quadratic program with thrust and glide-slope limits, solve it with the course solver, and re-solve it in a receding horizon when a gust displaces the vehicle.

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

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

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

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