5.3 Controllo predittivo model-based
5.3.2 Implementazione dell’MPC non lineare
L’MPC simula la dinamica del sistema a passo TMPC, utilizzando il discre-
tizzato fd(·), g(·) del modello non lineare descritto dall’ equazione (25), che
pu`o essere riscritta come:
xk+1 = fd(xk, uk) = xk+ f (xk, uk) TMPC
yk = g (xk)
.
All’istante t il controllore ha a disposizione, con riferimento alla Figura 29: • stato stimato del sistema al passo corrente:
x0 = ˆx(t), (dall’ EKF)
• porzione di traiettoria desiderata lungo l’orizzonte temporale in forma di legge oraria, campionata a passo Ts:
phor(ˆτ )|ˆτ =hT
s, τ = {0, Tˆ s, . . . , N Ts = Thor} (dal RHG)
• porzione del profilo di velocit`a desiderato lungo l’orizzonte temporale in forma di legge oraria, campionato a passo Ts:
vhor(ˆτ )|τ =hTˆ s, τ = {0, Tˆ s, . . . , N Ts = Thor} (dal RHG)
La procedura di controllo iterativa consta di 3 passi: 1. Ricampionamento dei riferimenti;
2. Valutazione della warm start solution per l’algoritmo di ottimizzazione; 3. Algoritmo di ottimizzazione.
Ricampionamento dei riferimenti
I riferimenti forniti dal RHG a passo Ts sono ricampionati a passo TMPC.
rdesk = phor(ˆτ )|τ =kTˆ MPC vhor(ˆτ )|ˆτ =kTMPC , ˆ τ = {TMPC, . . . , P TMPC} k = {1, . . . , P }
Valutazione della warm start solution per l’algoritmo di ottimizza- zione
Si ricerca una warm start solution esplorando lo spazio delle sequenze di controllo a valore costante U .
uj|j=1:N = linspace(umin, umax, N )
U = {u1, u2, . . . uM} tali che ui = uj ∀ i = 1 : M j = 1 : N U∗ = arg min U J La soluzione U∗ `e valutata per ricerca esaustiva. Algoritmo di ottimizzazione
Si avvia la procedura di ottimizzazione che ricerca nello spazio delle sequenze U = {u1, u2, . . . uM} quella ottima U∗ in grado di miminimizzare la
funzione costo J ,
U∗ = arg min
U J , (33)
partendo dalla warm start solution U∗. L’algoritmo di ottimizzazione scelto usa il metodo del simplesso.
La funzione costo J prende in ingresso x0, rdesk e la generica sequenza U ,
e restituisce l’errore di tracking eseguendo le seguenti istruzioni, descritte in pseudo codice:
• Verifica del rispetto dei vincoli U = {u1, u2, . . . uM} check that: umin < um < umax |um− um−1| < ˙umaxTcontrol ∀m = {1, . . . , M }
otherwise return J = inf
• Ricostruzione della sequenza di controlli nell’orizzonte di predizione a passo TMPC
U = {u1, u2, . . . uM}
∆Tcontrol= {0, Tcontrol, . . . (M − 1)Tcontrol}
∆TMPC = {0, TMPC, . . . (P − 1)TMPC}
ˆ
U = interpolate ( ∆Tcontrol , U , ∆TMPC )
ˆ
U = { ˆuk} = { ˆu0, ˆu1, . . . ˆuP −1}
• Predizione del modello nell’orizzonte di predizione xk= x0+
k−1
X
j=0
fd(xj, ˆuj), k = {1, . . . , P }
• Valutazione dell’errore di tracking nell’orizzonte di predizione return J = P X k=1 Wr rdesk − g (xk) 2 2
Al termine della procedura, il controllore restituisce in uscita il primo valore della sequenza ottima U∗, u(t) = u∗0.
6
Risultati sperimentali
Le prove in pista sono iniziate nel Febbrario 2018 con 3 sessioni di test. per ogni sessione Roborace che ha messo a disposizione del team pisano la vet- tura Devbot, una squadra di tecnici e ingegneri per la gestione e la messa a punto della vettura, ed un piazzale di circa 150.000m2 situato nell’ex aero-
porto militare di Upper Heyford.
Ogni sessione di test era preceduta da un’intensa attivit`a di simulazione pres- so la sede aziendale, che dispone di un simulatore Hardware-in-the-Loop. Il tempo in pista `e una risorsa limitata: `e necessario arrivare pronti e possi- bilmente risolvere in simulazione tutti i bug rilevabili nell’architettura del sofware, nei driver, nelle comunicazioni tra GPU e unit`a di calcolo Real- Time.
I primi test sono serviti per l’identificazione. Si `e trattato perlopi`u di test in anello aperto, i cui dati sono serviti per affinare il modello Single-track di riferimento. A questi sono seguiti dei test che hanno previsto l’esecuzione di semplici task (affrontare una curva, accelerare e frenare lungo un rettilineo, seguire una traccia di tipo skidpad) la cui traiettoria era prestabilita a partire da posizione e orientazione iniziale, dunque senza necessit`a di localizzazione. Nei primi test in anello chiuso, lungo un tracciato delimitato da coni, per la localizzazione ci si `e affidati al GPS, per il controllo sono stati testati sia il Pure Pursuit che il controllore MPC.
6.1
Pure Pursuit vs. MPC
Entrambi i controllori sono stati messi a punto in simulazione. Per il con- trollore Pure Pursuit `e stato necessario determinare la giusta distanza del look-ahead point e i guadagni del controllore PD sull’errore di velocit`a. Per l’MPC sono stati scelti i parametri P = 16, M = 3, il passo di predizione TMPC = 0.04s, ed `e stata scelta una risoluzione con cui grigliare lo spazio
delle sequenze di controllo a valori costanti per la valutazione della warm start solution. I parametri utilizzati sono quelli limite consentiti dalla CPU dell’unit`a di calcolo real-time: un aumento di uno qualunque dei parametri non ha dato garanzie di funzionamento real-time, ovvero non garantiva che la macchina computasse la sequenza di istruzioni entro il tempo di aggiorna- mento dei valori di controllo di Ts = 0.004s. In Figura 35 e Figura 36 sono
Figura 35: Traiettoria seguita da Tazio con controllore Pure Pursuit
Figura 37: Errore offtrack con controllore Pure Pursuit
Figura 38: Errore offtrack con controllore MPC
L’errore offtrack `e lo scostamento laterale dalla traiettoria di riferimento. Mantenere basso l’errore offtrack `e necessario per la sicurezza dei veicolo, ed `
e il primo criterio utilizzato per valutare la bont`a dell’azione di controllo. Inoltre un basso offtrack consente di essere pi`u aggressivi in fase di piani- ficazione della traiettoria, abbassare i margini di sicurezza e lasciare che la
traiettoria desiderata si avvicini al bordo pista massimizzando il profilo di curvatura.
La traiettoria seguita col controllo Pure Pursuit non soffre del fenomeno del cutting-corner, segno di una corretta collocazione del look-ahead point. Tut- tavia la traiettoria seguita col controllo predittivo risulta pi`u fedele a quella di riferimento, come `e possibile esaminare nelle Figura 37 e Figura 38. L’er- rore offtrack massimo `e elevato per entrambi i sistemi di controllo, l’errore medio per l’MPC `e inferiore di 30cm.
Gli errori elevati sono da attribuire in parte ad una localizzazione approssi- mativa, in parte ad una certa acerbit`a dell’architettura del software, ed in parte al tracciato i cui tornanti obbligavano lo stesso pilota umano a rag- giungere i limiti di sterzo.
Figura 40: Errore di inseguimento di velocit`a con controllore MPC Il secondo criterio di valutazione `e l’inseguimento del profilo di velocit`a. Il controllore PD che regola la trazione nel primo sistema di controllo segue bene (Figura 39) il profilo di velocit`a della traiettoria pianificata, ma `e affetto da un ritardo che ne deteriora le prestazioni. Il controllore MPC (Figura 40) consente di migliorare l’errore medio assoluto, ma in alcuni istanti, l`ı dove ha difficolt`a nell’inseguire il riferimento `e lasciato libero di rimodulare il profilo di velocit`a per correggere la traiettoria e recuperare il margine perso.
Per completare un giro seguendo la medesima traiettoria, il sistema di con- trollo predittivo infatti impiega circa 20s in meno rispetto al sistema di con- trollo disaccoppiato.
I buoni risultati ottenuti con il controllo MPC hanno spinto il team a foca- lizzare l’attenzione sull’implementazione di questa strategia di controllo.