Skip to content
Snippets Groups Projects
Commit 20706a59 authored by Olivier Cots's avatar Olivier Cots
Browse files

foo

parent 47bef088
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 3 additions and 2 deletions
tp/mpc-navigation.ipynb
+ 3
− 2
View file @ 20706a59
%% Cell type:markdown id:660275fc tags:
<div style="display:flex">
<img
src="https://gitlab.irit.fr/toc/etu-n7/controle-optimal/-/raw/master/ressources/Logo-toulouse-inp-N7.png"
height="100"
>
<img
src="https://gitlab.irit.fr/toc/etu-n7/controle-optimal/-/raw/master/ressources/ship.jpg"
style="display: block;
margin-left: auto;
margin-right: auto;
width: 50%;"
>
</div>
# Problème de navigation, une approche [MPC](https://en.wikipedia.org/wiki/Model_predictive_control)
- Date : 2025
- Durée approximative : inconnue
On considère un navire dans un courant constant $w=(w_x,w_y)$, $\|w\| \lt 1$.
[L'angle de cap](https://fr.wikipedia.org/wiki/Cap_(navigation)) est contrôlé, amenant aux équations différentielles suivantes :
$$ \begin{array}{rcl}
\dot{x}(t) &=& w_x+\cos\theta(t),\quad t \in [0,t_f]\\
\dot{y}(t) &=& w_y+\sin\theta(t),\\
\dot{\theta}(t) &=& u(t).
\end{array} $$
La vitesse angulaire est limitée et normalisée : $\|u(t)\| \leq 1$. Il y a des conditions aux limites au temps initial $t = 0$ et au temps final $t = t_f$, sur la position $(x,y)$ et sur l'angle $\theta$. L'objectif est de minimiser le temps final. Ce sujet est inspiré de ce [TP](https://github.com/pns-mam/commande/tree/main/tp3) dont le problème vient d'une collaboration entre l'Université Côte d'Azur et l'entreprise française [CGG](https://www.cgg.com) qui s'intéresse aux manoeuvres optimales de très gros navires pour la prospection marine.
✏️ **Exercice 1.** On supposera $p^0 = -1$, on se place dans le cas normal.
1. Ecrire le problème de contrôle optimal sous la forme de Mayer.
2. Donner le pseudo-hamiltonien $H(q, p, u)$, où $q = (x, y, \theta)$ et $p = (p_x, p_y, p_\theta)$.
3. Calculer l'équation adjointe, c'est-à-dire vérifiée par le vecteur adjoint $p$, donnée par le principe du maximum de Pontryagin.
4. Calculer le contrôle maximisant en fonction de $p_\theta$ (on pourra l'écrire comme une [fonction multivaluée](https://fr.wikipedia.org/wiki/Fonction_multivaluée)).
5. Calculer le contrôle singulier, c'est-à-dire celui permettant de vérifier $p_\theta(t) = 0$ sur un intervalle de temps non réduit à un singleton.
4. Puisque le temps final $t_f$ est libre, nous avons une condition sur le pseudo-hamiltonien à ce temps-ci. Donner la condition vérifiée par $H(q(t_f), p(t_f), u(t_f))$.
5. Calculer le contrôle maximisant en fonction de $p_\theta$ (on pourra l'écrire comme une [fonction multivaluée](https://fr.wikipedia.org/wiki/Fonction_multivaluée)).
6. Calculer le contrôle singulier, c'est-à-dire celui permettant de vérifier $p_\theta(t) = 0$ sur un intervalle de temps non réduit à un singleton. Utiliser le fait que le pseudo-hamiltonien est constant le long de l'extrémale pour conclure.
%% Cell type:markdown id:a68c170f tags:
## Données du problème
%% Cell type:code id:8dd6f718 tags:
``` julia
using OptimalControl, NLPModelsIpopt, Plots, OrdinaryDiffEq, LinearAlgebra, Plots.PlotMeasures
t0 = 0.
x0 = 0.
y0 = 0.
θ0 = π/7
xf = 4.
yf = 7.
θf = -π/2
function current(x, y) # current as a function of position
ε = 1e-1
w = [ 0.6, 0.4 ]
δw = ε * [ y, -x ] / sqrt(1+x^2+y^2)
w = w + δw
if (w[1]^2 + w[2]^2 >= 1)
error("|w| >= 1")
end
return w
end
#
function plot_state!(plt, x, y, θ; color=1)
plot!(plt, [x], [y], marker=:circle, legend=false, color=color, markerstrokecolor=color, markersize=5, z_order=:front)
quiver!(plt, [x], [y], quiver=([cos(θ)], [sin(θ)]), color=color, linewidth=2, z_order=:front)
return plt
end
function plot_current!(plt; current=current, N=10, scaling=1)
for x ∈ range(xlims(plt)..., N)
for y ∈ range(ylims(plt)..., N)
w = scaling*current(x, y)
quiver!(plt, [x], [y], quiver=([w[1]], [w[2]]), color=:black, linewidth=0.5, z_order=:back)
end
end
return plt
end
# on affiche dans le plan de phase augmenté les conditions aux limites et le courant
plt = plot(
xlims=(-2, 6),
ylims=(-1, 8),
size=(600, 600),
aspect_ratio=1,
xlabel="x",
ylabel="y",
title="Conditions aux limites",
leftmargin=5mm,
bottommargin=5mm,
)
plot_state!(plt, x0, y0, θ0; color=2)
plot_state!(plt, xf, yf, θf; color=2)
plot_current!(plt)
```
%% Cell type:code id:8b4c8930 tags:
``` julia
function plot_trajectory!(plt, t, x, y, θ; N=5) # N : nombre de points où l'on va afficher θ
# trajectoire
plot!(plt, x.(t), y.(t), legend=false, color=1, linewidth=2, z_order=:front)
if N > 0
# longueur du trajet
s = 0
for i ∈ 2:length(t)
s += norm([x(t[i]), y(t[i])] - [x(t[i-1]), y(t[i-1])])
end
# intervalle de longueur
Δs = s/(N+1)
tis = []
s = 0
for i ∈ 2:length(t)
s += norm([x(t[i]), y(t[i])] - [x(t[i-1]), y(t[i-1])])
if s > Δs && length(tis) < N
push!(tis, t[i])
s = 0
end
end
# affichage des points intermédiaires
for ti ∈ tis
plot_state!(plt, x(ti), y(ti), θ(ti); color=1)
end
end
return plt
end;
```
%% Cell type:markdown id:0403331e tags:
## Solveur (OptimalControl)
✏️ **Exercice 2.** Coder le problème de contrôle optimal ci-dessous.
On pourra s'insiprer du problème de la [rame de métro à temps minimal](https://control-toolbox.org/OptimalControl.jl/stable/tutorial-double-integrator-time.html) décrit dans la documentation du package OptimalControl pour définir notre problème de manoeuvre de navire ci-après.
%% Cell type:code id:a1933e2c tags:
``` julia
function solve(t0, x0, y0, θ0, xf, yf, θf, w;
grid_size=300, tol=1e-8, max_iter=500, print_level=4, display=true)
# ---------------------------------------
# Définition du problème : TO UPDATE
ocp = @def begin
end
# ---------------------------------------
# Initialisation
tf_init = 1.5*norm([xf, yf]-[x0, y0])
x_init(t) = [ x0, y0, θ0 ] * (tf_init-t)/(tf_init-t0) + [xf, yf, θf] * (t-t0)/(tf_init-t0)
u_init = (θf - θ0) / (tf_init-t0)
init = (state=x_init, control=u_init, variable=tf_init)
# Résolution
sol = OptimalControl.solve(ocp;
init=init,
grid_size=grid_size,
tol=tol,
max_iter=max_iter,
print_level=print_level,
display=display,
disc_method=:euler,
)
# Récupération des données utiles
t = time_grid(sol)
q = state(sol)
x = t -> q(t)[1]
y = t -> q(t)[2]
θ = t -> q(t)[3]
u = control(sol)
tf = variable(sol)
return t, x, y, θ, u, tf, iterations(sol), sol.solver_infos.constraints_violation
end;
```
%% Cell type:markdown id:cc8547d2 tags:
## Première résolution
On considère ici un vent constant et on résout une première fois le problème.
%% Cell type:code id:ffea082a tags:
``` julia
# -----------------------------------------------------------------------------------------------------------
t, x, y, θ, u, tf, iter, cons = solve(t0, x0, y0, θ0, xf, yf, θf, current(x0, y0); display=false);
println("Iterations : ", iter)
println("Constraints violation : ", cons)
println("tf : ", tf)
# -----------------------------------------------------------------------------------------------------------
# affichage
# trajectoire
plt_q = plot(xlims=(-2, 6), ylims=(-1, 8), aspect_ratio=1, xlabel="x", ylabel="y")
plot_state!(plt_q, x0, y0, θ0; color=2)
plot_state!(plt_q, xf, yf, θf; color=2)
plot_current!(plt_q; current=(x, y) -> current(x0, y0))
plot_trajectory!(plt_q, t, x, y, θ)
# contrôle
plt_u = plot(t, u; color=1, legend=false, linewidth=2, xlabel="t", ylabel="u")
#
plot(plt_q, plt_u;
layout=(1, 2),
size=(1200, 600),
leftmargin=5mm,
bottommargin=5mm,
plot_title="Simulation courant constant"
)
```
%% Cell type:markdown id:827b18d5 tags:
## Simulation du système réel
Dans la simulation précédente, nous faisons l'hypothèse que le courant est constant. Cependant, d'un point de vue pratique le courant dépend de la position $(x, y)$. Etant donné un modèle de courant, donné par la fonction `current`, nous pouvons simuler la trajectoire réelle du navire, pourvu que l'on ait la condition initiale et le contrôle au cours du temps.
%% Cell type:code id:25422e2e tags:
``` julia
function realistic_trajectory(tf, t0, x0, y0, θ0, u, current; abstol=1e-12, reltol=1e-12, saveat=[])
function rhs!(dq, q, dummy, t)
x, y, θ = q
w = current(x, y)
dq[1] = w[1]+cos(θ)
dq[2] = w[2]+sin(θ)
dq[3] = u(t)
end
q0 = [ x0, y0, θ0 ]
tspan = (t0, tf)
ode = ODEProblem(rhs!, q0, tspan)
sol = OrdinaryDiffEq.solve(ode, Tsit5(), abstol=abstol, reltol=reltol, saveat=saveat)
t = sol.t
x = t -> sol(t)[1]
y = t -> sol(t)[2]
θ = t -> sol(t)[3]
return t, x, y, θ
end;
```
%% Cell type:code id:da61110e tags:
``` julia
# trajectoire réaliste
t, x, y, θ = realistic_trajectory(tf, t0, x0, y0, θ0, u, current)
# -----------------------------------------------------------------------------------------------------------
# affichage
# trajectoire
plt_q = plot(xlims=(-2, 6), ylims=(-1, 8), aspect_ratio=1, xlabel="x", ylabel="y")
plot_state!(plt_q, x0, y0, θ0; color=2)
plot_state!(plt_q, xf, yf, θf; color=2)
plot_current!(plt_q; current=current)
plot_trajectory!(plt_q, t, x, y, θ)
plot_state!(plt_q, x(tf), y(tf), θ(tf); color=3)
# contrôle
plt_u = plot(t, u; color=1, legend=false, linewidth=2, xlabel="t", ylabel="u")
#
plot(plt_q, plt_u;
layout=(1, 2),
size=(1200, 600),
leftmargin=5mm,
bottommargin=5mm,
plot_title="Simulation avec modèle de courant"
)
```
%% Cell type:markdown id:503bd360 tags:
## Approche MPC
En pratique, nous n'avons pas à l'avance les données réelles du courant sur l'ensemble du trajet, c'est pourquoi nous allons recalculer régulièrement le contrôle optimal. L'idée est de mettre à jour le contrôle optimal à intervalle de temps régulier en prenant en compte le courant à la position où le navire se trouve. On est donc amener à résoudre un certain nombre de problème à courant constant, avec celui-ci mis réguilièremet à jour. Ceci est une introduction aux méthodes dites MPC, pour "Model Predictive Control" en anglais.
%% Cell type:code id:563c73fd tags:
``` julia
Nmax = 20 # nombre d'itérations max de la méthode MPC
ε = 1e-1 # rayon sur la condition finale pour stopper les calculs
Δt = 1.0 # pas de temps fixe de la méthode MPC
P = 300 # nombre de points de discrétisation pour le solveur
t1 = t0
x1 = x0
y1 = y0
θ1 = θ0
data = []
N = 1
stop = false
while !stop
# on récupère le courant à la position actuelle
w = current(x1, y1)
# on résout le problème
t, x, y, θ, u, tf, iter, cons = solve(t1, x1, y1, θ1, xf, yf, θf, w; grid_size=P, display=false);
# calcul du temps suivant
if (t1+Δt < tf)
t2 = t1+Δt
else
t2 = tf
println("t2=tf: ", t2)
stop = true
end
# on stocke les données: le temps initial courant, le temps suivant, le contrôle
push!(data, (t2, t1, x(t1), y(t1), θ(t1), u, tf))
# on met à jour les paramètres de la méthode MPC: on simule la réalité
t, x, y, θ = realistic_trajectory(t2, t1, x1, y1, θ1, u, current)
t1 = t2
x1 = x(t1)
y1 = y(t1)
θ1 = θ(t1)
# on calcule la distance à la position cible
distance = norm([ x1, y1, θ1 ] - [ xf, yf, θf ])
println("N: ", N, "\t distance: ", distance, "\t iterations: ", iter, "\t constraints: ", cons, "\t tf: ", tf)
if !((distance > ε) && (N < Nmax))
stop = true
end
#
N += 1
end
```
%% Cell type:markdown id:edd803b2 tags:
## Affichage
%% Cell type:code id:d0292a49 tags:
``` julia
# trajectoire
plt_q = plot(xlims=(-2, 6), ylims=(-1, 8), aspect_ratio=1, xlabel="x", ylabel="y")
# condition finale
plot_state!(plt_q, xf, yf, θf; color=2)
# courant
plot_current!(plt_q; current=current)
# contrôle
plt_u = plot(xlabel="t", ylabel="u")
N = 1
for d ∈ data
#
t2, t1, x1, y1, θ1, u, tf = d
# calcule de la trajectoire réelle
t, x, y, θ = realistic_trajectory(t2, t1, x1, y1, θ1, u, current)
# trajectoire
plot_state!(plt_q, x1, y1, θ1; color=2)
plot_trajectory!(plt_q, t, x, y, θ; N=0)
# contrôle
plot!(plt_u, t, u; color=1, legend=false, linewidth=2)
N += 1
end
plot_state!(plt_q, x(tf), y(tf), θ(tf); color=3)
#
plot(plt_q, plt_u;
layout=(1, 2),
size=(1200, 600),
leftmargin=5mm,
bottommargin=5mm,
plot_title="Simulation avec modèle de courant"
)
```
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment