When introducing the idea of receding-horizon control, we took for granted that the optimization problem solved at each time step is feasible, that is, that it has a solution. However, this is not guaranteed in general. We illustrate it with the following example.
Example 1 (Recursive feasibility in MPC lost when the input constraints are too restrictive) We consider the standard discrete-time LTI system with state and input constraints, and a quadratic cost, for which the matrix parameters are given as follows:
using LinearAlgebra # Needed for Diagonal.
# System dynamics x(k+1) = A*x(k) + B*u(k):
A = [1.0 1.0; 0 1.0]
B = [0.4, 1.0]
x_init = [-1.0, 1.0] # Initial state.
# Quadratic cost function weights:
Q = Diagonal([1.0, 1.0])
R = 10.0
# State constraints:
x_min = [-1.0, -1.0]
x_max = [1.0, 1.0]
# Input constraints:
u_max = 0.3
u_min = -u_maxWe set the prediction horizon to N=3:
N = 3 # Prediction horizon length.
N_sim = N+10 # Total simulation length.
h = 1.0 # Sampling time period (just for generality).and simulate the response of the closed-loop system under the receding-horizon control for a total of 10 time steps. The results are shown in Fig. 1. Apparently, the optimization problem solved at each time step is feasible. If we extend the simulation time even further, the feasibility is not lost.
using OSQP
using JuMP
rhocp = Model(OSQP.Optimizer) # Defining the receding-horizon (RH) optimal control problem (OCP).
set_silent(rhocp)
n = size(A,1) # Order of the system.
# Building the optimization model for the receding-horizon optimal control problem RH-OCP:
@variable(rhocp, x[1:n, 1:N+1]) # State trajectory over the prediction horizon.
@variable(rhocp, u_min <= u[1:N] <= u_max) # Control trajectory over the prediction horizon.
@constraint(rhocp, [i=1:N+1], x_min .<= x[:,i] .<= x_max) # State constraints.
@constraint(rhocp, [i=1:N], x[:,i+1] == A*x[:,i] + B*u[i])
fix(x[1,1], x_init[1])
fix(x[2,1], x_init[2])
@objective(rhocp, Min, sum(x[:,i]'*Q*x[:,i] + R*u[i]^2 for i=1:N) + x[:,N+1]'*Q*x[:,N+1])
# Function that solves the RH OCP for the current state xₖ at time step k:
function mpc_regulation(rhocp, xₖ, k, N)
fix(x[1,1], xₖ[1])
fix(x[2,1], xₖ[2])
optimize!(rhocp)
return value.(u), value.(x)
end
# Receding-horizon simulation:
x_opt_full = [x_init,]
u_opt_full = Float64[]
x_pred_full = Vector{Matrix{Float64}}()
u_pred_full = Vector{Vector{Float64}}()
for k in 1:(N_sim-N) # Receding-horizon simulation loop.
xₖ = x_opt_full[end] # Current state.
u_pred, x_pred = mpc_regulation(rhocp, xₖ, k, N) # Optimal control sequence over the prediction horizon.
uₖ = u_pred[1] # Apply only the first control input.
x⁺ = x_pred[:,2] # Next state from the predicted trajectory.
push!(x_opt_full, x⁺)
push!(u_opt_full, uₖ)
push!(x_pred_full, x_pred)
push!(u_pred_full, u_pred)
end
x_opt = hcat(x_opt_full...)
# Plotting the results:
using Plots
x = [x_min[1], x_max[1], x_max[1], x_min[1], x_min[1]]
y = [x_min[2], x_min[2], x_max[2], x_max[2], x_min[2]]
p1 = plot(x, y, color=:red, alpha=0.2, linewidth=2, legend=false, aspect_ratio=:equal)
plot!(p1,x_opt[1,:], x_opt[2,:], color=:blue,label="x(k)", xlabel="x₁", ylabel="x₂", markershape=:circle, markersize=2, lw=2, aspect_ratio=:equal)
for x_pred in x_pred_full
plot!(p1, x_pred[1,:], x_pred[2,:], label="", markershape=:diamond, markersize=1, lw=1, linecolor=:gray, alpha=0.5)
end
display(p1)
Now, we make the control input constraints a bit more restrictive. With anticipation of problems, we also shorten the simulation time to just two time steps. The results are shown in Fig. 2.
# Input constraints:
u_max = 0.25
u_min = -u_max
# Simulation horizon shortened to just two time steps:
N_sim = N+2using OSQP
using JuMP
rhocp = Model(OSQP.Optimizer) # Defining the receding-horizon (RH) optimal control problem (OCP).
#set_silent(rhocp)
n = size(A,1) # Order of the system.
# Building the optimization model for the receding-horizon optimal control problem RH-OCP:
@variable(rhocp, x[1:n, 1:N+1]) # State trajectory over the prediction horizon.
@variable(rhocp, u_min <= u[1:N] <= u_max) # Control trajectory over the prediction horizon.
@constraint(rhocp, [i=1:N+1], x_min .<= x[:,i] .<= x_max) # State constraints.
@constraint(rhocp, [i=1:N], x[:,i+1] == A*x[:,i] + B*u[i])
fix(x[1,1], x_init[1])
fix(x[2,1], x_init[2])
@objective(rhocp, Min, sum(x[:,i]'*Q*x[:,i] + R*u[i]^2 for i=1:N) + x[:,N+1]'*Q*x[:,N+1])
# Function that solves the RH OCP for the current state xₖ at time step k:
function mpc_regulation(rhocp, xₖ, k, N)
fix(x[1,1], xₖ[1])
fix(x[2,1], xₖ[2])
optimize!(rhocp)
return value.(u), value.(x)
end
# Receding-horizon simulation:
x_opt_full = [x_init,]
u_opt_full = Float64[]
x_pred_full = Vector{Matrix{Float64}}()
u_pred_full = Vector{Vector{Float64}}()
for k in 1:(N_sim-N) # Receding-horizon simulation loop.
xₖ = x_opt_full[end] # Current state.
u_pred, x_pred = mpc_regulation(rhocp, xₖ, k, N) # Optimal control sequence over the prediction horizon.
uₖ = u_pred[1] # Apply only the first control input.
x⁺ = x_pred[:,2] # Next state from the predicted trajectory.
push!(x_opt_full, x⁺)
push!(u_opt_full, uₖ)
push!(x_pred_full, x_pred)
push!(u_pred_full, u_pred)
end
x_opt = hcat(x_opt_full...)
# Plotting the results:
using Plots
x = [x_min[1], x_max[1], x_max[1], x_min[1], x_min[1]]
y = [x_min[2], x_min[2], x_max[2], x_max[2], x_min[2]]
p1 = plot(x, y, color=:red, alpha=0.2, linewidth=2, legend=false, aspect_ratio=:equal)
plot!(p1,x_opt[1,:], x_opt[2,:], color=:blue,label="x(k)", xlabel="x₁", ylabel="x₂", markershape=:circle, markersize=2, lw=2, aspect_ratio=:equal)
for x_pred in x_pred_full
plot!(p1, x_pred[1,:], x_pred[2,:], label="", markershape=:diamond, markersize=1, lw=1, linecolor=:gray, alpha=0.5)
end
display(p1)
We can see in the figure that although two time steps of the simulation were expected, plotted is only the outcome of the first one. The reason is that after the first optimization, the horizon moved forward and the optimization on the new horizon became infeasible, and the outcome of the simulation are just NaN values:
x_opt_full3-element Vector{Vector{Float64}}:
[-1.0, 1.0]
[-0.10014731554947641, 0.7496325221975825]
[NaN, NaN]Losing the feasibility of the optimization problem solved at each time step is a major problem. Just imagine that the MPC controller is a component of an ABS or an ESP system in a car, and that the optimization becomes infeasible at some point.
In this section we study if we can guarantee that the optimization problem solved at each time step is feasible. The property that we are interested in is called recursive feasibility.
The crucial concept to ensure recursive feasibility is the control invariant set.
First, let’s denote the set of admissible (allowed) states as \mathcal{X} and the set of admissible control inputs as \mathcal{U}. In other words, the sets \mathcal{X} and \mathcal{U} represent the contstraints in our optimization.
In the following, we restrict ourselves to linear systems modelled by \bm x^+ = \mathbf A\bm x + \mathbf B\bm u, but the concepts we are now going to introduce hold for general systems.
We start with the definition of a precursor set, also called one-step backward reachable set.
Now, we are even more interested in the restriction of this set to the admissible states, that is, the set \text{Pre}(\mathcal{S}) \cap \mathcal{X}.
In other words, the the-one step controllable set contains only the states from which the system can be controlled to the set \mathcal{S} in one time step, while satisfying the state and input constraints.
Instead of some arbitrary set \mathcal{S}, we are now interested in the one-step controllable set of the feasible set of states \mathcal X, that is, \text{Pre}(\mathcal{X})\cap \mathcal{X}.
How can we compute it? Assume that the feasible set of states \mathcal{X} and control inputs \mathcal{U} are polyhedral sets, that is, they can be described by a finite number of linear inequalities \mathcal{X} = \{\bm x\in\mathbb R^n \mid \mathbf H_x \bm x \leq \mathbf h_x\}, \quad \mathcal{U} = \{\bm u\in\mathbb R^m \mid \mathbf H_u \bm u \leq \mathbf h_u\}.
Then the one-step controllable set \text{Pre}(\mathcal{X})\cap \mathcal{X} can be written as \boxed{ \text{Pre}(\mathcal{X})\cap \mathcal{X} = \left\{\bm x\in\mathbb R^n \mid \exists \bm u\in\mathbb R^m: \mathbf H_x \bm x \leq \mathbf h_x, \mathbf H_u \bm u \leq \mathbf h_u, \mathbf H_x(\mathbf A\bm x+\mathbf B\bm u) \leq \mathbf h_x\right\}. }
For convenience, we write the inequalities explicitly here: \begin{bmatrix} \mathbf H_x & \mathbf 0 \\ \mathbf 0 & \mathbf H_u \\ \mathbf H_x \mathbf A & \mathbf H_x \mathbf B \end{bmatrix} \begin{bmatrix} \bm x \\ \bm u \end{bmatrix} \leq \begin{bmatrix} \mathbf h_x \\ \mathbf h_u \\ \mathbf H_x \mathbf h_x \end{bmatrix}.
The inequalities define a polyhedron in the (\bm x, \bm u)-space and the one-step controllable set \text{Pre}(\mathcal{X})\cap \mathcal{X} is the projection of this polyhedron onto the \bm x-space.
Example 2 (One-step controllable set)
A = [1.5 0.0; 1.0 -1.5]
B = [1.0, 0.0]
umin, umax = -5.0, 5.0
xmin, xmax = -10.0, 10.0In the code below we use a high-level library Polyhedra.jl for manipulation with polyhedra, but it was mainly for convenience when finally plotting the polyhedra, the computational task consists in nothing else than just composing the corresponding linear inequalities.
using Polyhedra
using CDDLib
using LinearAlgebra
using Plots
"""
precursor_polyhedron(S, U, A, B, X=nothing; restrict_to_X=false)
Compute Pre(S) = { x | ∃u∈U : A*x + B*u ∈ S } for polyhedra S and U.
If restrict_to_X=true, computes Pre(S) ∩ X, where X must be provided.
S, U (and X if provided) must be Polyhedra polyhedra in H-rep (or convertible).
Returns a Polyhedra polyhedron in x-space.
"""
function precursor_polyhedron(S, U, A, B, X=nothing; restrict_to_X=false)
n = size(A, 1)
m = size(B, 2)
# Extract Hs, hs (for constraints Hs * (Ax + Bu) ≤ hs)
Hs = reduce(vcat, (hs.a' for hs in Polyhedra.halfspaces(S))) # each hs.a is a vector
hs = [hs.β for hs in Polyhedra.halfspaces(S)]
# Extract Hu, hu (for constraints Hu * u ≤ hu)
Hu = reduce(vcat, (hs.a' for hs in Polyhedra.halfspaces(U)))
hu = [hs.β for hs in Polyhedra.halfspaces(U)]
# Variables are y = [x; u] ∈ R^(n+m)
# Constraint 1: Hu*u ≤ hu -> [0 Hu] * [x;u] ≤ hu
C1 = hcat(zeros(size(Hu, 1), n), Hu)
d1 = hu
# Constraint 2: Hs*(A*x + B*u) ≤ hs -> [Hs*A Hs*B] * [x;u] ≤ hs
C2 = hcat(Hs * A, Hs * B)
d2 = hs
# If restriction x ∈ X: Hx*x ≤ hx -> [Hx 0] * [x;u] ≤ hx
if restrict_to_X
@assert X !== nothing "X must be provided when restrict_to_X=true"
Hx = reduce(vcat, (hs.a' for hs in Polyhedra.halfspaces(X)))
hx = [hs.β for hs in Polyhedra.halfspaces(X)]
C3 = hcat(Hx, zeros(size(Hx, 1), m))
d3 = hx
C = vcat(C1, C2, C3)
d = vcat(d1, d2, d3)
else
C = vcat(C1, C2)
d = vcat(d1, d2)
end
# Build lifted polyhedron in (x,u)
P_lift = polyhedron(hrep(C, d), lib)
# Project onto x-coordinates (eliminate u variables at indices n+1:n+m)
PreS = eliminate(P_lift, (n+1):(n+m),FourierMotzkin())
return PreS
end
# H-representation of interval [umin, umax]
# For u ∈ [umin, umax], we need:
# u ≤ umax (equivalently: [1]*u ≤ umax)
# u ≥ umin (equivalently: [-1]*u ≤ -umin)
lib = CDDLib.Library()
U = polyhedron(hrep([HalfSpace([1.0], umax), HalfSpace([-1.0], -umin)]), lib)
# H-representation of square [xmin, xmax]²
# For x ∈ [xmin, xmax]², we need:
# x₁ ≤ xmax → [1, 0]'x ≤ xmax
# x₁ ≥ xmin → [-1, 0]'x ≤ -xmin
# x₂ ≤ xmax → [0, 1]'x ≤ xmax
# x₂ ≥ xmin → [0, -1]'x ≤ -xmin
X = polyhedron(hrep([
HalfSpace([1.0, 0.0], xmax),
HalfSpace([-1.0, 0.0], -xmin),
HalfSpace([0.0, 1.0], xmax),
HalfSpace([0.0, -1.0], -xmin)
]), lib)
PreX = precursor_polyhedron(X, U, A, B) # Pre(X)
CPreX = precursor_polyhedron(X, U, A, B, X; restrict_to_X=true) # Pre(X) ∩ X
plot(X, ratio=:equal, label="X", alpha=0.3, xlabel="x₁", ylabel="x₂")
plot!(PreX, ratio=:equal, label="Pre(X)", alpha=0.3)
plot!(CPreX, ratio=:equal, label="Pre(X) ∩ X", alpha=0.3)
Finally we are ready to define the control invariant set.
There is not just one control invariant set. Among many possible control invariant sets, we may be interested in the maximal control invariant set.
We can compute the maximal control invariant set by the following iteration \boxed{ \mathcal C_{i+1} = \text{Pre}(\mathcal C_i) \cap \mathcal{C}_i, \quad i=0,1,2,\ldots} starting with \mathcal C_0 = \mathcal{X}, and stopping when the change in volume of the set is sufficiently small, that is, when \text{vol}(\mathcal C_{i+1}) - \text{vol}(\mathcal C_i) < \varepsilon for some small \varepsilon > 0.
Example 3 Here we demonstrate computation of the maximal control invariant set for the data of Example 2.
"""
control_invariant_set(X, U, A, B; max_iterations=100, tol=1e-6)
Compute the maximal control invariant set. A set X_inv is control invariant
if for all x ∈ X_inv, there exists u ∈ U such that A*x + B*u ∈ X_inv.
The algorithm iterates Xᵢ₊₁ = Pre(Xᵢ) ∩ Xᵢ until convergence.
"""
function control_invariant_set(X, U, A, B; max_iterations=30, tol=1e-5)
lib = CDDLib.Library()
X_inv = X
for iteration in 1:max_iterations
# Compute Pre(X_inv) ∩ X_inv
X_new = precursor_polyhedron(X_inv, U, A, B, X_inv; restrict_to_X=true)
# Remove redundant constraints to speed up subsequent iterations
removehredundancy!(X_new)
# Check convergence by comparing volumes
vol_old = volume(X_inv)
vol_new = volume(X_new)
println("Iteration $iteration: volume = $vol_new (change: $(abs(vol_new - vol_old)))")
if abs(vol_new - vol_old) < tol * (vol_old + 1e-10)
println("Converged after $iteration iterations")
return X_new
end
X_inv = X_new
end
println("Warning: did not converge after $max_iterations iterations")
return X_inv
end
# Compute the maximal control invariant set
X_invariant = control_invariant_set(X, U, A, B; max_iterations=100, tol=1e-8)Iteration 1: volume = 250.0 (change: 150.0)
Iteration 2: volume = 200.0 (change: 50.0)
Iteration 3: volume = 148.14814814814815 (change: 51.85185185185185)
Iteration 4: volume = 125.10288065843625 (change: 23.045267489711904)
Iteration 5: volume = 101.69181527206223 (change: 23.41106538637402)
Iteration 6: volume = 91.28689732256262 (change: 10.404917949499605)
Iteration 7: volume = 80.80972299841376 (change: 10.477174324148862)
Iteration 8: volume = 76.15320107656983 (change: 4.656521921843932)
Iteration 9: volume = 71.48240629059761 (change: 4.67079478597222)
Iteration 10: volume = 69.40649749683222 (change: 2.075908793765393)
Iteration 11: volume = 67.32776937188096 (change: 2.0787281249512546)
Iteration 12: volume = 66.40389020523594 (change: 0.9238791666450226)
Iteration 13: volume = 65.47945413366533 (change: 0.9244360715706108)
Iteration 14: volume = 65.06859365741177 (change: 0.4108604762535606)
Iteration 15: volume = 64.65762317524695 (change: 0.4109704821648137)
Iteration 16: volume = 64.47496962761812 (change: 0.18265354762883135)
Iteration 17: volume = 64.29229435042656 (change: 0.18267527719156362)
Iteration 18: volume = 64.21110533834145 (change: 0.08118901208510465)
Iteration 19: volume = 64.12991203399703 (change: 0.08119330434442418)
Iteration 20: volume = 64.09382612095507 (change: 0.036085913041958406)
Iteration 21: volume = 64.05773936005937 (change: 0.0360867608956994)
Iteration 22: volume = 64.04170079966134 (change: 0.0160385603980302)
Iteration 23: volume = 64.02566207178597 (change: 0.016038727875368863)
Iteration 24: volume = 64.01853374828582 (change: 0.007128323500154465)
Iteration 25: volume = 64.01140539170375 (change: 0.007128356582072115)
Iteration 26: volume = 64.00823723322281 (change: 0.003168158480931993)
Iteration 27: volume = 64.00506906820719 (change: 0.0031681650156230035)
Iteration 28: volume = 64.00366099486695 (change: 0.0014080733402437318)
Iteration 29: volume = 64.00225292023589 (change: 0.0014080746310582981)
Iteration 30: volume = 64.00162710928872 (change: 0.00062581094717018)
Iteration 31: volume = 64.00100129808659 (change: 0.0006258112021271245)
Iteration 32: volume = 64.00072315977454 (change: 0.0002781383120549208)
Iteration 33: volume = 64.00044502141209 (change: 0.0002781383624466116)
Iteration 34: volume = 64.00032140436214 (change: 0.00012361704995100808)
Iteration 35: volume = 64.0001977873022 (change: 0.00012361705994123895)
Iteration 36: volume = 64.00014284638671 (change: 5.4940915489964937e-5)
Iteration 37: volume = 64.00008790546923 (change: 5.49409174794846e-5)
Iteration 38: volume = 64.00006348728371 (change: 2.441818551801589e-5)
Iteration 39: volume = 64.00003906909772 (change: 2.4418185986974095e-5)
Iteration 40: volume = 64.00002821657066 (change: 1.0852527069005191e-5)
Iteration 41: volume = 64.00001736404349 (change: 1.0852527168481174e-5)
Iteration 42: volume = 64.00001254069807 (change: 4.823345420845726e-6)
Iteration 43: volume = 64.00000771735266 (change: 4.823345406634871e-6)
Iteration 44: volume = 64.00000557364356 (change: 2.143709096458224e-6)
Iteration 45: volume = 64.00000342993451 (change: 2.14370905382566e-6)
Iteration 46: volume = 64.0000029535547 (change: 4.7637981026582565e-7)
Converged after 46 iterationsPolyhedron CDDLib.Polyhedron{Float64}:
6-element iterator of Polyhedra.HalfSpace{Float64, Vector{Float64}}:
HalfSpace([1.0, -1.5], 4.000000107185453)
HalfSpace([-1.0, 1.5], 4.000000107185453)
HalfSpace([1.0, 0.0], 10.0)
HalfSpace([0.0, -1.0], 4.000000107185453)
HalfSpace([0.0, 1.0], 4.000000047637979)
HalfSpace([-1.0, 0.0], 10.0):
6-element iterator of Vector{Float64}:
[10.0, 4.000000047637979]
[10.0, 3.9999999285430308]
[-2.0000000535927267, -4.000000107185453]
[1.999999964271515, 4.000000047637979]
[-10.0, -4.000000107185453]
[-10.0, -3.9999999285430308]plot(X, ratio=:equal, label="X", alpha=0.3)
plot!(PreX, ratio=:equal, label="Pre(X)", alpha=0.3)
plot!(CPreX, ratio=:equal, label="Pre(X) ∩ X", alpha=0.3)
plot!(X_invariant, ratio=:equal, label="Control Invariant Set", linewidth=2)
Example 4 (Control invariant set for the example system of the recursive feasibility example) Let’s now do the same computation for the motivating Example 1 from the beginning of this section.
A = [1.0 1.0; 0 1.0]
B = [0.4, 1.0]
# State constraints:
xmin, xmax = -1.0, 1.0
# Input constraints:
umin, umax = -0.25, 0.25# H-representation of interval [umin, umax]
# For u ∈ [umin, umax], we need:
# u ≤ umax (equivalently: [1]*u ≤ umax)
# u ≥ umin (equivalently: [-1]*u ≤ -umin)
lib = CDDLib.Library()
U = polyhedron(hrep([HalfSpace([1.0], umax), HalfSpace([-1.0], -umin)]), lib)
# H-representation of square [xmin, xmax]²
# For x ∈ [xmin, xmax]², we need:
# x₁ ≤ xmax → [1, 0]'x ≤ xmax
# x₁ ≥ xmin → [-1, 0]'x ≤ -xmin
# x₂ ≤ xmax → [0, 1]'x ≤ xmax
# x₂ ≥ xmin → [0, -1]'x ≤ -xmin
X = polyhedron(hrep([
HalfSpace([1.0, 0.0], xmax),
HalfSpace([-1.0, 0.0], -xmin),
HalfSpace([0.0, 1.0], xmax),
HalfSpace([0.0, -1.0], -xmin)
]), lib)
PreX = precursor_polyhedron(X, U, A, B; restrict_to_X=false) # Pre(X)
CPreX = precursor_polyhedron(X, U, A, B, X; restrict_to_X=true) # Pre(X) ∩ X
# Compute the maximal control invariant set
X_invariant = control_invariant_set(X, U, A, B; max_iterations=100, tol=1e-8)
# Plot the regions
plot(X, ratio=:equal, label="X", alpha=0.3, xlabel="x₁", ylabel="x₂")
plot!(PreX, ratio=:equal, label="Pre(X)", alpha=0.3)
plot!(CPreX, ratio=:equal, label="Pre(X) ∩ X", alpha=0.3)
plot!(X_invariant, ratio=:equal, label="Control Invariant Set", linewidth=2)Iteration 1: volume = 3.1900000000000004 (change: 0.8099999999999996)
Iteration 2: volume = 2.7675000000000005 (change: 0.4224999999999999)
Iteration 3: volume = 2.6075 (change: 0.1600000000000006)
Iteration 4: volume = 2.5875 (change: 0.020000000000000018)
Iteration 5: volume = 2.5875000000000004 (change: 4.440892098500626e-16)
Converged after 5 iterations
X_invariantPolyhedron CDDLib.Polyhedron{Float64}:
10-element iterator of Polyhedra.HalfSpace{Float64, Vector{Float64}}:
HalfSpace([1.0, 0.0], 1.0)
HalfSpace([-1.0, 0.0], 1.0)
HalfSpace([1.0, 2.0], 1.4500000000000002)
HalfSpace([-1.0, -1.0], 1.1)
HalfSpace([1.0, 3.0], 2.0500000000000003)
HalfSpace([1.0, 4.0], 2.9000000000000004)
HalfSpace([-1.0, -4.0], 2.9000000000000004)
HalfSpace([1.0, 1.0], 1.1)
HalfSpace([-1.0, -2.0], 1.4500000000000002)
HalfSpace([-1.0, -3.0], 2.0500000000000003):
10-element iterator of Vector{Float64}:
[-0.49999999999999933, 0.8499999999999999]
[-1.0, 0.9750000000000001]
[0.24999999999999983, 0.6000000000000001]
[0.7499999999999999, 0.35000000000000003]
[1.0, 0.10000000000000009]
[-1.0, -0.10000000000000009]
[-0.7499999999999999, -0.35000000000000003]
[-0.24999999999999983, -0.6000000000000001]
[1.0, -0.9750000000000001]
[0.49999999999999933, -0.8499999999999999]It is worth emphasizing that the control invariant set depends on the system dynamics and the constraints. For the same system, if we change the constraints, we get a different control invariant set. In this particular example, you can run the code after changing the input constraints to u \in [-0.3, 0.3] and see how the control invariant set changes.
Now that we know the concept of a control invariant set and we even know how to find the set of inequelities that decribe it, we can use this set to ensure the recursive feasibility of MPC. The immediate idea is to impose that the state at every time step within the prediction horizon belongs to the control invariant set. In this way, we ensure that the state never leaves the control invariant set, and hence that the optimization problem solved at each time step is feasible.
Example 5 (Recursive feasibility of MPC by imposing the control invariance at every time step) We continue with the Example 1. We use the setting for which the alowed set of initial states (the square) was not recursively feasible. Namely, we consider the control input constraints u \in [-0.25, 0.25]. For this we computed the control invariant set in the previous Example 4. Apparently, this set does not contain the originally used initial state x_\text{init} = [-1.0, 1.0] anymore and therefore we change the initial state to, say, x_\text{init} = [-1.0, 0.975] that does belongs to the control invariant set, while still being at its boundary (actually at a vertex). The resulting simulated state trajectory is in Fig. 6 below.
using OSQP
using JuMP
using LinearAlgebra
rhocp = Model(OSQP.Optimizer) # Defining the receding-horizon (RH) optimal control problem (OCP).
set_silent(rhocp)
# System dynamics x(k+1) = A*x(k) + B*u(k):
A = [1.0 1.0; 0 1.0]
B = [0.4, 1.0]
N = 3 # Prediction horizon length.
N_full = N+10 # Total simulation length.
h = 1.0 # Sampling time.
x_init = [-1.0, 0.975] # Initial state.
# Quadratic cost function weights:
Q = Diagonal([1.0, 1.0])
R = 10.0
# State constraints:
x_min = [-1.0, -1.0]
x_max = [1.0, 1.0]
# Input constraints:
u_max = 0.25 # Recursive feasibility for 0.3, for 0.25 not recursively feasible
u_min = -u_max
n = size(A,1) # Order of the system.
# Building the optimization model for the receding-horizon optimal control problem RH-OCP:
@variable(rhocp, x[1:n, 1:N+1]) # State trajectory over the prediction horizon.
@variable(rhocp, u_min <= u[1:N] <= u_max) # Control trajectory over the prediction horizon.
#@constraint(rhocp, [i=1:N+1], x_min .<= x[:,i] .<= x_max) # State constraints.
@constraint(rhocp, [i=1:N], x[:,i+1] == A*x[:,i] + B*u[i])
@constraint(rhocp, [i=1:N+1], x[:,i] in X_invariant) # State constrained to the control invariant set.
fix(x[1,1], x_init[1])
fix(x[2,1], x_init[2])
@objective(rhocp, Min, sum(x[:,i]'*Q*x[:,i] + R*u[i]^2 for i=1:N) + x[:,N+1]'*Q*x[:,N+1])
# Function that solves the RH OCP for the current state xₖ at time step k:
function mpc_regulation(rhocp, xₖ, k, N)
fix(x[1,1], xₖ[1])
fix(x[2,1], xₖ[2])
optimize!(rhocp)
return value.(u), value.(x)
end
# Receding-horizon simulation:
x_opt_full = [x_init,]
u_opt_full = Float64[]
x_pred_full = Vector{Matrix{Float64}}()
u_pred_full = Vector{Vector{Float64}}()
for k in 1:(N_full-N) # Receding-horizon simulation loop.
xₖ = x_opt_full[end] # Current state.
u_pred, x_pred = mpc_regulation(rhocp, xₖ, k, N) # Optimal control sequence over the prediction horizon.
uₖ = u_pred[1] # Apply only the first control input.
x⁺ = x_pred[:,2] # Next state from the predicted trajectory.
#x⁺ = x_pred[:,2] + randn(2)*0.1 # True next state differs from the predicted trajectory due to the disturbance.
push!(x_opt_full, x⁺)
push!(u_opt_full, uₖ)
push!(x_pred_full, x_pred)
push!(u_pred_full, u_pred)
end
# Plotting the results:
using Plots
x_opt = hcat(x_opt_full...)
p1 = plot(X, ratio=:equal, label="X", alpha=0.3, xlabel="x₁", ylabel="x₂")
plot!(p1,X_invariant, ratio=:equal, label="Control Invariant Set", alpha=0.3)
plot!(p1,x_opt[1,:], x_opt[2,:], label="x(k)", xlabel="x₁", ylabel="x₂", markershape=:circle, markersize=2, lw=2, aspect_ratio=:equal)
for x_pred in x_pred_full
plot!(p1, x_pred[1,:], x_pred[2,:], label="", markershape=:diamond, markersize=1, lw=1, linecolor=:gray, alpha=0.5)
end
display(p1)
We highlight that the only change in the MPC optimization problem definition is the new line @constraint(rhocp, [i=1:N+1], x[:,i] in X_invariant) that enforces the control invariance constraint at every time step. Here we are taking advantage of the high-level syntax of JuMP but it represents nothing else than adding the corresponding linear inequalities defining the control invariant set (polyhedron) to the optimization problem definition.
Clearly, the predicted trajectories at every time step remain in the control invariant set. You can even verify (by running the code by yourself) that this is the case when the prediction horizon is made even shorter, say N=2.
Another way to ensure the recursive feasibility of MPC is to impose the constraint that the state at the end of the prediction horizon is in the (maximal) control invariant set. This is a less restrictive condition than the previously discussed one, in which we imposed such constraint at every time step during the prediction horizon, and hence it allows for a larger set of initial states from which the system can be controlled to the origin. We will shortly discuss why this works, but we first illustrate the functionality with an example.