Barrier certificates for collision avoidance

Adapted from: Example 2 of "Engineering applications of SOS polynomials" by Georgina Hall, from the 2019 AMS Short Course on Sum of Squares: Theory and Applications Implementation by: Hugo Tadashi Kussaba

using DynamicPolynomials
@polyvar x[1:2]

using SumOfSquares
using CSDP

We need to pick an SDP solver, see here for a list of the available choices. We use SOSModel instead of Model to be able to use the >= syntax for Sum-of-Squares constraints.

solver = optimizer_with_attributes(CSDP.Optimizer, MOI.Silent() => true)
model = SOSModel(solver);

We define below the vector field $\text{d}x/\text{d}t = f$

f = [ x[2],
     -x[1] + (1/3)*x[1]^3 - x[2]]

2-element Vector{DynamicPolynomials.Polynomial{DynamicPolynomials.Commutative{DynamicPolynomials.CreationOrder}, MultivariatePolynomials.Graded{MultivariatePolynomials.LexOrder}, Float64}}:
 x₂
 -x₂ - x₁ + 0.3333333333333333x₁³

Semi-algebraic function describing the unsafe set 𝒳ᵤ

g₁ = -(x[1]+1)^2 - (x[2]+1)^2 + 0.16  # 𝒳ᵤ = {x ∈ R²: g₁(x) ≥ 0}

\[ -1.84 - 2.0x_{2} - 2.0x_{1} - x_{2}^{2} - x_{1}^{2} \]

Semi-algebraic function describing the initial set 𝒳₀

h₁ = -(x[1]-1.5)^2 - x[2]^2 + 0.25    # 𝒳₀ = {x ∈ R²: h₁(x) ≥ 0}

\[ -2.0 + 3.0x_{1} - x_{2}^{2} - x_{1}^{2} \]

Define SOS barrier function B

monos = monomials(x, 0:4)
@variable(model, B, Poly(monos))

\[ ({\_}_{1}) + ({\_}_{2})x_{2} + ({\_}_{3})x_{1} + ({\_}_{4})x_{2}^{2} + ({\_}_{5})x_{1}x_{2} + ({\_}_{6})x_{1}^{2} + ({\_}_{7})x_{2}^{3} + ({\_}_{8})x_{1}x_{2}^{2} + ({\_}_{9})x_{1}^{2}x_{2} + ({\_}_{10})x_{1}^{3} + ({\_}_{11})x_{2}^{4} + ({\_}_{12})x_{1}x_{2}^{3} + ({\_}_{13})x_{1}^{2}x_{2}^{2} + ({\_}_{14})x_{1}^{3}x_{2} + ({\_}_{15})x_{1}^{4} \]

Define barrier certificate constraints using SOS relaxation

B(x) > 0 for all x ∈ 𝒳ᵤ

ε = 0.001
@constraint(model, B >= ε, domain = @set(g₁ >= 0))

\[ ({\_}_{1} - 0.001) + ({\_}_{2})x_{2} + ({\_}_{3})x_{1} + ({\_}_{4})x_{2}^{2} + ({\_}_{5})x_{1}x_{2} + ({\_}_{6})x_{1}^{2} + ({\_}_{7})x_{2}^{3} + ({\_}_{8})x_{1}x_{2}^{2} + ({\_}_{9})x_{1}^{2}x_{2} + ({\_}_{10})x_{1}^{3} + ({\_}_{11})x_{2}^{4} + ({\_}_{12})x_{1}x_{2}^{3} + ({\_}_{13})x_{1}^{2}x_{2}^{2} + ({\_}_{14})x_{1}^{3}x_{2} + ({\_}_{15})x_{1}^{4} \text{ is SOS} \]

B(x) ≤ 0 for all x ∈ 𝒳₀

@constraint(model, B <= 0, domain = @set(h₁ >= 0))

\[ (-{\_}_{1}) + (-{\_}_{2})x_{2} + (-{\_}_{3})x_{1} + (-{\_}_{4})x_{2}^{2} + (-{\_}_{5})x_{1}x_{2} + (-{\_}_{6})x_{1}^{2} + (-{\_}_{7})x_{2}^{3} + (-{\_}_{8})x_{1}x_{2}^{2} + (-{\_}_{9})x_{1}^{2}x_{2} + (-{\_}_{10})x_{1}^{3} + (-{\_}_{11})x_{2}^{4} + (-{\_}_{12})x_{1}x_{2}^{3} + (-{\_}_{13})x_{1}^{2}x_{2}^{2} + (-{\_}_{14})x_{1}^{3}x_{2} + (-{\_}_{15})x_{1}^{4} \text{ is SOS} \]

Ḃ(x) ≤ 0 for all x ∈ R²

using LinearAlgebra # Needed for `dot`
dBdt = dot(differentiate(B, x), f)
@constraint(model, -dBdt >= 0)

\[ ({\_}_{2} - {\_}_{3})x_{2} + ({\_}_{2})x_{1} + (2 {\_}_{4} - {\_}_{5})x_{2}^{2} + (2 {\_}_{4} + {\_}_{5} - 2 {\_}_{6})x_{1}x_{2} + ({\_}_{5})x_{1}^{2} + (3 {\_}_{7} - {\_}_{8})x_{2}^{3} + (3 {\_}_{7} + 2 {\_}_{8} - 2 {\_}_{9})x_{1}x_{2}^{2} + (2 {\_}_{8} + {\_}_{9} - 3 {\_}_{10})x_{1}^{2}x_{2} + (-0.3333333333333333 {\_}_{2} + {\_}_{9})x_{1}^{3} + (4 {\_}_{11} - {\_}_{12})x_{2}^{4} + (4 {\_}_{11} + 3 {\_}_{12} - 2 {\_}_{13})x_{1}x_{2}^{3} + (3 {\_}_{12} + 2 {\_}_{13} - 3 {\_}_{14})x_{1}^{2}x_{2}^{2} + (-0.6666666666666666 {\_}_{4} + 2 {\_}_{13} + {\_}_{14} - 4 {\_}_{15})x_{1}^{3}x_{2} + (-0.3333333333333333 {\_}_{5} + {\_}_{14})x_{1}^{4} + (-{\_}_{7})x_{1}^{3}x_{2}^{2} + (-0.6666666666666666 {\_}_{8})x_{1}^{4}x_{2} + (-0.3333333333333333 {\_}_{9})x_{1}^{5} + (-1.3333333333333333 {\_}_{11})x_{1}^{3}x_{2}^{3} + (-{\_}_{12})x_{1}^{4}x_{2}^{2} + (-0.6666666666666666 {\_}_{13})x_{1}^{5}x_{2} + (-0.3333333333333333 {\_}_{14})x_{1}^{6} \text{ is SOS} \]

The model is ready to be optimized by the solver:

JuMP.optimize!(model)

We verify that the solver has found a feasible solution:

JuMP.primal_status(model)

FEASIBLE_POINT::ResultStatusCode = 1

Plot the phase plot with the 0-level set of the barrier function, and the boundary of the initial and unsafe sets

import DifferentialEquations, Plots, ImplicitPlots
function phase_plot(f, B, g₁, h₁, quiver_scaling, Δt, X0, solver = DifferentialEquations.Tsit5())
    X₀plot = ImplicitPlots.implicit_plot(h₁; xlims=(-2, 3), ylims=(-2.5, 2.5), resolution = 1000, label="X₀", linecolor=:blue)
    Xᵤplot = ImplicitPlots.implicit_plot!(g₁; xlims=(-2, 3), ylims=(-2.5, 2.5), resolution = 1000, label="Xᵤ", linecolor=:teal)
    Bplot  = ImplicitPlots.implicit_plot!(B; xlims=(-2, 3), ylims=(-2.5, 2.5), resolution = 1000, label="B = 0", linecolor=:red)
    Plots.plot(X₀plot)
    Plots.plot!(Xᵤplot)
    Plots.plot!(Bplot)
    ∇(vx, vy) = [fi(x[1] => vx, x[2] => vy) for fi in f]
    ∇pt(v, p, t) = ∇(v[1], v[2])
    function traj(v0)
        tspan = (0.0, Δt)
        prob = DifferentialEquations.ODEProblem(∇pt, v0, tspan)
        return DifferentialEquations.solve(prob, solver, reltol=1e-8, abstol=1e-8)
    end
    ticks = -5:0.5:5
    X = repeat(ticks, 1, length(ticks))
    Y = X'
    Plots.quiver!(X, Y, quiver = (x, y) -> ∇(x, y) / quiver_scaling, linewidth=0.5)
    for x0 in X0
        Plots.plot!(traj(x0), vars=(1, 2), label = nothing)
    end
    Plots.plot!(xlims = (-2, 3), ylims = (-2.5, 2.5))
end

phase_plot(f, value(B), g₁, h₁, 10, 30.0, [[x1, x2] for x1 in 1.2:0.2:1.7, x2 in -0.35:0.1:0.35])

This page was generated using Literate.jl.