Simulation-Based MPC in 3D

cheapdear

the cheapest plan

dropped: breaks a constraint

the other drone, d_safe

t = 0.0 s

Plans

horizon N Δt [s]

segments: accelerations held in turn, then a coast

each segment [s]

accelerations per axis, north and east

accelerations up and down

a_max on each axis [m/s²]

Cost

λ what accelerating costs [s⁴]

U speed along the path [m/s]

Constraints

v_max speed limit [m/s]

another drone crossing the first leg

d_safe kept from it [m]

its speed [m/s]

d_clear kept from obstacles [m]

The drone

acceleration lag τ [s] (not in the model)

wind from the west [m/s²] (not in the model)

the steps of the cheapest plan the dropped plans lines down to the ground

Off the path [m]: across (+ to the right) and above, measured and planned

Speed |v| [m/s]: measured, and planned

Acceleration [m/s²]: carried out, and planned

The cheapest plan, now

Cost of the cheapest plan for each first acceleration

Things to try

Watch the first leg: the other drone crosses it just as the drone gets there. The plans that would come within d_safe of where MPC predicts it turn red, and the drone brakes and lets it pass. Unlike the boat in the 2D app, it can wait. Press Pause and Step Δt through it, and hover a red plan to see where the other drone was predicted to be when that plan came too close.
Click the other drone and drag it along its line, so it crosses earlier or later, or drag it across to move its line up, down or sideways. Raise d_safe and the drone climbs over it or swerves round it instead of waiting.
Press Balloon ahead or Pillar ahead and watch the plans split round it. Drag a balloon onto the drone's path while it flies, and the next plans already go round where it is now.
Set the accelerations up and down to 1 (level): the plans stay in a plane, so the drone can no longer climb with the path or go over anything. It goes round or waits.
Count the plans: per segment there are the accelerations north × east × up, to the power of the segments. 3 segments of 27 is 19 683 plans. Read the model steps per replan too: plans that start the same way share that part of the prediction, so there are far fewer steps than plans × N.
Shorten the horizon to 2 s: the drone sees the other drone late, brakes hard, and now and then no plan keeps every constraint. Lengthen it and it reacts early and smoothly.
Raise the acceleration lag τ to 0.8 s, or the wind. The model knows about neither, so its predictions are wrong, and the drone may come closer than d_safe: replanning corrects much, but not everything.
Set λ to 0: accelerating is free and the drone jerks about. Raise it to 2: it moves gently, and takes longer to reach the path.

How it works

The model is a point mass driven by its acceleration, with p = [xⁿ, yⁿ, zⁿ]^⊤ the position north, east and down (the altitude is h = −zⁿ), v the velocity and a the acceleration, held over each step (exactly, for a constant a):

p_k+1 = p_k + Δt v_k + ½ Δt² a_k
v_k+1 = v_k + Δt a_k

A quadrotor's attitude loop is fast: it tilts its thrust to get the acceleration it is asked for, so its acceleration is what a plan can choose (the drone drawn here tilts by it).

A plan holds an acceleration for each of one, two or three segments, each one of a lattice: an evenly spaced set of values from −a_max to a_max on each axis, every combination of north, east and up. After the last segment it coasts (a = 0) to the horizon NΔt.

The cost adds up the squared distance from a reference point r_k, which sets off from the point on the path nearest the drone and moves along it at the speed U (so the cost asks for both the path and the speed on it), and the squared accelerations:

J = Σ_k=1^N |p_k − r_k|² + λ Σ_k=0^N−1 |a_k|²

The constraints: |a| ≤ a_max on each axis (no plan asks for more), |p_k − q_k| ≥ d_safe with the other drone predicted straight on at its present velocity, d_clear from the surface of each obstacle, an altitude of at least 1 m, and |v_k| ≤ v_max. A plan that breaks one is dropped, from the step where it does. When every plan is dropped, the drone carries out the one that breaks a constraint latest.

Plans that start the same way share that part of the prediction. The kernel walks the plans as a tree: it predicts each first segment once, each second segment once for each first, and so on, and when a first segment already breaks a constraint it drops every plan that starts with it at once.

The drone is simulated as the same model, every 20 ms, except that its acceleration follows the commanded one with a first-order lag τ, and the wind pushes it east; the model leaves both out. The other drone flies back and forth across the first leg, climbing as it goes east.

The kernel, sbmpc/ in this repository, is a small library with no dependencies that runs in a browser or Node: give it a model, the controls each segment may pick from, cost terms and constraints, and it returns every plan's prediction and cost and the cheapest one. Given the 2D app's model, plans, cost and constraints, it scores every plan as the 2D app does (its tests check this).

T. A. Johansen, T. Perez and A. Cristofaro, "Ship collision avoidance and COLREGS compliance using simulation-based control behavior selection with predictive hazard assessment," IEEE Transactions on Intelligent Transportation Systems, 17(12), 3407–3422, 2016. S. Liu et al., "Search-based motion planning for quadrotors using linear quadratic minimum time control," IROS, 2017 (acceleration motion primitives).