foo

20706a59 · Olivier Cots · 47bef088 · 20706a59
Commit 20706a59 authored 1 month ago by Olivier Cots
--- a/tp/mpc-navigation.ipynb
+++ b/tp/mpc-navigation.ipynb
@@ -40,8 +40,9 @@
    "1. Ecrire le problème de contrôle optimal sous la forme de Mayer.\n",
    "2. Donner le pseudo-hamiltonien $H(q, p, u)$, où $q = (x, y, \\theta)$ et $p = (p_x, p_y, p_\\theta)$.\n",
    "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.\n",
-    "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)).\n",
+    "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))$.\n",
-    "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."
+    "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)).\n",
+    "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: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)).
+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 singulier, c'est-à-dire celui permettant de vérifier $p_\theta(t) = 0$ sur un intervalle de temps non réduit à un singleton.
+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"
 )
 ```