Simulation-Based MPC

cheapdear

the cheapest plan

dropped: breaks a constraint

t = 0.0 s

Plans

horizon N Δt [s]

segments: turn rates held in turn

first segment [s]

turn rates per segment

Cost

λ what turning costs [m²/(deg/s)²]

Constraints

r_max turn rate [deg/s]

a kayak crossing the first leg

d_safe kept from the kayak [m]

The Otter

U speed [m/s]

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

the steps of the cheapest plan

Off the path y_e [m]: measured, and planned over the horizon

Turn rate r [deg/s]: carried out, and planned

Cost of each plan, J(r_a, r_b)

The cheapest plan, now

Things to try

Press Pause, then Step Δt a few times. The Otter carries out only the first step of the amber plan, then the whole fan is predicted again from where it really is: a receding horizon. In the charts, the planned curves slide to the left with time, and each new plan starts from the measured value.
Watch the Otter reach a bend: it starts to turn before the corner, because the plans that follow the next leg early are cheaper. Shorten the horizon to 3 s and it turns only at the corner, and overshoots.
Set λ to 0: turning is free, so the commanded turn rate changes more often and now and then hits the limit (watch the turn-rate chart). Raise λ to 1 or more: the Otter turns gently and takes longer to reach the path.
Change the turn rates per segment and the segments, and read the number of plans: it is the rates to the power of the segments, 3 segments of 17 rates is 4913 plans. Trying every plan only works when there are relatively few, which is why other types of MPC use numerical optimization.
Pause and drag the kayak along its line so it crosses just before the Otter, then just after: the plans that come within d_safe of the kayak's predicted positions turn red, and the Otter passes behind or ahead. Hover a red plan to see where MPC predicted the kayak when that plan came too close.
Raise the turn lag τ to 3 s. The model does not know about it, so the predictions are wrong, yet the Otter still follows the path: each new plan corrects the errors of the last.

How it works

The model (kinematic, no current and no sideslip), one step ahead by forward Euler, with η = [xⁿ, yⁿ, ψ]^⊤ the position north and east and the heading:

xⁿ_k+1 = xⁿ_k + Δt U cos ψ_k
yⁿ_k+1 = yⁿ_k + Δt U sin ψ_k
ψ_k+1 = ψ_k + Δt r_k

A plan is a turn rate r_k for each step k = 0, …, N − 1 up to the horizon NΔt, here held constant in each segment (r_a, then r_b, then r_c), each one of an evenly spaced set from −r_max to r_max.

The cost adds up the squared distance from the path, y_e,k, and the squared turn rates. The distance is measured to the path ahead of the Otter, from the point nearest it on, so a plan that turns round and sails the loop backwards does not score well:

J = Σ_k=1^N y_e,k² + λ Σ_k=0^N−1 r_k²

The constraints: |r_k| ≤ r_max (no plan asks for more), ‖p_k − p_k^kayak‖ ≥ d_safe with the kayak predicted straight on at its present velocity, and 2.2 m from each buoy. A plan that breaks one is dropped. When every plan is dropped, the Otter keeps its last turn rate.

The Otter, a 2 m USV made by Maritime Robotics, is simulated here as the same kinematic model, every 20 ms, except that its turn rate follows the commanded one with a first-order lag τ, which the model leaves out. The numbers are those of the video: U = 1.5 m/s, Δt = 0.5 s, a horizon of 15 s (N = 30), r_a for 4 s and r_b for 11 s, 9 rates from −8 to 8 deg/s, λ = 0.05 and d_safe = 8 m. The path is a closed loop of straight legs; y_e is positive to starboard of the direction it is sailed.

Simulation-based MPC tries every plan in a small set and keeps the cheapest. Optimization-based MPC searches for the best plan with a numerical solver instead. A well-known simulation-based MPC for ships is SB-MPC (Johansen, Perez and Cristofaro, 2016): it simulates the ship for a small set of course and speed offsets, each held over the horizon, and keeps the one with the lowest cost, with collision risk and COLREGs compliance in the cost. The MPC here differs in its details (turn-rate plans in segments, following a path), but the idea is the same.

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.