GitHub

Example: Path planning problem solved by Symbolically guided Model Predictive Control (SgMPC).

This example was adapted from the numerical experiments in [1, Sec. 5]. This is a control problem for a discrete-time nonlinear system.

Let us consider the 3-dimensional state space control system of the form

\[x_{t+1} = f(x_t, u_t)\]

with $f: \mathbb{R}^3 × \mathbb{R}^2 \to \mathbb{R}^3$ given by

\[f(x, (u_1, u_2)) = \begin{bmatrix} x_1 + u_1 \cos(x_3) \\ x_2 + u_1 \sin(x_3) \\ x_3 + u_2 \ (\text{mod} \ 2\pi) \end{bmatrix}\]

and with state and control constraints given by:

\[X = \left\{ (x_1, x_2)^T \in \mathbb{R}^2 \ | \ x_1^2 - x_2^2 \leq 4, \ 4x_2^2 - x_1^2 \leq 16 \right\}\]

\[U = [0.2, 2] \times [-1, 1]\]

Here, $(x_1, x_2)$ represents the 2D Cartesian coordinates and $x_3$ is the angular orientation of a mobile cart. The control inputs $u_1$ and $u_2$ are the linear and angular velocities. The control objective is to drive the mobile cart to a desired reference position $x_r$.

In order to validate the proposed Symbolically guided Model Predictive Control (SgMPC) strategy, the authors consider the following setup:

The optimization problem is set with a prediction horizon $N = 20$ and the stage cost:

\[\ell(x, u) = 100 \|(x_1, x_2)^T - x_r\|^2 + \|u\|^2\]

The terminal cost is:

\[L(x) = 100 \|(x_1, x_2)^T - x_r\|^2\]

Simulations are performed for two reference positions:

  • \[x_r = (0.5, 0.5)\]

  • \[x_r = (\sqrt{32}/3, \sqrt{20}/3)\]

First, let us import StaticArrays and Plots.

using StaticArrays, Plots

At this point, we import Dionysos.

using Dionysos
const DI = Dionysos
const UT = DI.Utils
const DO = DI.Domain
const ST = DI.System
const SY = DI.Symbolic
const PR = DI.Problem
const OP = DI.Optim
const AB = OP.Abstraction
Dionysos.Optim.Abstraction

And the file defining the hybrid system for this problem

include(joinpath(dirname(dirname(pathof(Dionysos))), "problems", "path_planning_sgmpc.jl"))
Main.PathPlanningSgMPC

Definition of the problem

Defining the initial and target sets for the state-space of the system:

initial = SVector(1.0, -1.7, 0.0)
#initial = SVector(0.0, 0.0, 0.0)
#target = SVector(0.5, 0.5, -pi)
target = SVector(-0.5, 0.5, -pi)
#target = SVector(2.6, 2.0, -pi)
#target = SVector(-2.6, 2.0, -pi)
#target = SVector(sqrt(32.0 / 3.0), sqrt(20.0 / 3.0), -pi)
3-element StaticArraysCore.SVector{3, Float64} with indices SOneTo(3):
 -0.5
  0.5
 -3.141592653589793

Now we instantiate the problem using the function provided by PathPlanning.jl

concrete_problem =
    PathPlanningSgMPC.problem(; sgmpc = true, initial = initial, target = target)
concrete_system = concrete_problem.system;

Definition of the abstraction

Definition of the grid of the state-space on which the abstraction is based (origin x0 and state-space discretization h):

x0 = SVector(0.0, 0.0, 0.0);
h = SVector(0.1, 0.1, 0.2);
state_grid = DO.GridFree(x0, h);

Definition of the grid of the input-space on which the abstraction is based (origin u0 and input-space discretization h):

u0 = SVector(1.1, 0.0);
h = SVector(0.3, 0.3);
input_grid = DO.GridFree(u0, h);

We now solve the optimal control problem with the Abstraction.UniformGridAbstraction.Optimizer.

using JuMP
optimizer = MOI.instantiate(AB.UniformGridAbstraction.Optimizer)
MOI.set(optimizer, MOI.RawOptimizerAttribute("concrete_problem"), concrete_problem)
MOI.set(optimizer, MOI.RawOptimizerAttribute("state_grid"), state_grid)
MOI.set(optimizer, MOI.RawOptimizerAttribute("input_grid"), input_grid)
MOI.optimize!(optimizer)
compute_symmodel_from_controlsystem! started
compute_symmodel_from_controlsystem! terminated with success: 21204363 transitions created
  1.749028 seconds (80 allocations: 686.292 MiB, 5.23% gc time)
compute_controller_reach! started

compute_controller_reach! terminated with success

Get the results

abstract_system = MOI.get(optimizer, MOI.RawOptimizerAttribute("abstract_system"))
abstract_problem = MOI.get(optimizer, MOI.RawOptimizerAttribute("abstract_problem"))
abstract_controller = MOI.get(optimizer, MOI.RawOptimizerAttribute("abstract_controller"))
concrete_controller = MOI.get(optimizer, MOI.RawOptimizerAttribute("concrete_controller"))
(::Dionysos.Optim.Abstraction.UniformGridAbstraction.var"#concrete_controller#10"{Dionysos.Optim.Abstraction.UniformGridAbstraction.var"#concrete_controller#9#11"{Dionysos.Symbolic.SymbolicModelList{3, 2, Dionysos.Domain.DomainList{3, Float64, Dionysos.Domain.GridFree{3, Float64}}, Dionysos.Domain.DomainList{2, Float64, Dionysos.Domain.GridFree{2, Float64}}, Dionysos.Symbolic.AutomatonList{Dionysos.Utils.SortedTupleSet{3, Tuple{Int64, Int64, Int64}}}}, Dionysos.Utils.SortedTupleSet{2, Tuple{Int64, Int64}}}}) (generic function with 1 method)

Trajectory display

We choose a stopping criterion reached and the maximal number of steps nsteps for the sampled system, i.e. the total elapsed time: nstep*tstep as well as the true initial state x0 which is contained in the initial state-space _I_ defined previously.

nstep = 100
function reached(x)
    if x ∈ concrete_problem.target_set
        return true
    else
        return false
    end
end
x0 = initial #SVector(1.1, -1.6, 0.0)
control_trajectory = ST.get_closed_loop_trajectory(
    concrete_system.f,
    concrete_controller,
    x0,
    nstep;
    stopping = reached,
)
Dionysos.System.Control_trajectory{StaticArraysCore.SVector{3, Float64}, Any}(Dionysos.System.Trajectory{StaticArraysCore.SVector{3, Float64}}(StaticArraysCore.SVector{3, Float64}[[1.0, -1.7, 0.0], [1.2000000000000002, -1.7, 0.8999999999999999], [1.3243219936541333, -1.5433346180745031, 1.7999999999999998], [1.1425603178996637, -0.7642565133719469, 2.6999999999999997], [-0.6655839661344582, 0.09050324709571345, 2.6999999999999997], [-1.1176200371429887, 0.30419318721262856, 2.0999999999999996], [-1.2185892580629603, 0.4768350605424035, 2.0999999999999996], [-1.3195584789829318, 0.6494769338721784, 1.1999999999999997], [-1.247086928087597, 0.8358847510656239, 0.2999999999999998], [-1.0560196302624756, 0.8949887923978919, 1.1999999999999997]  …  [-0.5775832837120202, -0.2506379710883688, 1.2000000000000006], [-0.5051117328166855, -0.06423015389492334, 2.1000000000000005], [-1.2118962792564867, 1.1442629594134996, 1.2000000000000006], [-1.139424728361152, 1.3306707766069452, 0.3000000000000007], [-0.9483574305360307, 1.3897748179392133, -0.5999999999999992], [-0.7832903075540948, 1.2768463232602063, -0.2999999999999992], [-0.5922230097289733, 1.2177422819279384, -1.199999999999999], [-0.5197514588336384, 1.031334464734493, -1.199999999999999], [-0.4472799079383034, 0.8449266475410477, -1.4999999999999991], [-0.43313246760476265, 0.6454276502202367, -2.099999999999999]]), Dionysos.System.Trajectory{Any}(Any[[0.20000000000000018, 0.8999999999999999], [0.20000000000000018, 0.8999999999999999], [0.8, 0.8999999999999999], [2.0, 0.0], [0.5000000000000001, -0.6], [0.20000000000000018, 0.0], [0.20000000000000018, -0.8999999999999999], [0.20000000000000018, -0.8999999999999999], [0.20000000000000018, 0.8999999999999999], [0.20000000000000018, 0.8999999999999999]  …  [0.20000000000000018, 0.8999999999999999], [0.20000000000000018, 0.8999999999999999], [1.4000000000000001, -0.8999999999999999], [0.20000000000000018, -0.8999999999999999], [0.20000000000000018, -0.8999999999999999], [0.20000000000000018, 0.3], [0.20000000000000018, -0.8999999999999999], [0.20000000000000018, 0.0], [0.20000000000000018, -0.3], [0.20000000000000018, -0.6]]))

Here we display the coordinate projection on the two first components of the state space along the trajectory.

fig = plot(; aspect_ratio = :equal);

We display the concrete domain

plot!(concrete_system.X; color = :yellow, opacity = 0.5);

We display the abstract domain

plot!(abstract_system.Xdom; color = :blue, opacity = 0.5);

We display the concrete specifications

plot!(concrete_problem.initial_set; color = :green, opacity = 0.2);
plot!(concrete_problem.target_set; dims = [1, 2], color = :red, opacity = 0.2);

We display the abstract specifications

plot!(
    SY.get_domain_from_symbols(abstract_system, abstract_problem.initial_set);
    color = :green,
);
plot!(
    SY.get_domain_from_symbols(abstract_system, abstract_problem.target_set);
    color = :red,
);

display(control_trajectory)

We display the concrete trajectory

plot!(control_trajectory; ms = 0.5)
Example block output

References

  1. Z. Azaki, A. Girard and S. Olaru, "Predictive and Symbolic Control: Performance and Safety for Non-linear Systems," in IFAC-PapersOnLine, 2022, vol. 55, no 16, pp. 290-295..

This page was generated using Literate.jl.