Sviluppo di un sistema di controllo per veicoli autonomi da competizione

(1)

Sviluppo di un Sistema di Controllo per

Veicoli Autonomi da Competizione

Universit`

a di Pisa

Francesco Amerotti

21 Febbraio 2019

(2)

Sommario

In questo lavoro di tesi si descrivono lo sviluppo, la prototipazione e la verifica sperimentale di un sistema di controllo di sterzo e forza (di trazione e di frenatura) per la guida autonoma di livello 5 di vei-coli da competizione. Lo scenario considerato è quello delle piste da corsa, allinterno del quale è possibile sperimentare in sicurezza sistemi innovativi di controllo, come quello proposto, in condizioni limiti di aderenza per la ricerca delle prestazioni massime in fase di gara. La tesi illustra il contributo offerto dal laureando per lo sviluppo e l’imple-mentazione di un pilota autonomo nell’ambito del progetto Roborace, che ha coinvolto un Team dell’Università di Pisa nato allinterno del Centro di Ricerca E. Piaggio come primo in Italia e fra i primi al mondo a realizzare questo tipo di sperimentazioni nelle condizioni che saranno descritte. Il lavoro presenta la descrizione del modello del veicolo monotraccia, ritenuto il più appropriato per essere utilizzato come modello di riferimento, nell’ottica dell’applicazione di un con-trollo model-based. Si propone inoltre una procedura che ha come obiettivo la ricerca di una traccia di riferimento, o racing line, in un tracciato noto; la procedura basata sulla costruzione di un grafo to-pologico della pista ed volta a trovare il percorso clotoidale in grado di ottimizzare il profilo di curvatura, in relazione alla velocit massima sostenibile dalla vettura, allo scopo di minimizzare il tempo sul giro. Si introduce il problema della localizzazione e si illustra il progetto di uno stimatore, basato su un filtro di Kalman esteso, che integri le in-formazioni dei sensori inerziali e non inerziali per stimare la posizione della vettura in pista. Si giunge infine alla parte centrale del lavoro di tesi che ha riguardato lo sviluppo, l’implementazione ed il test di un sistema di controllo predittivo basato su modello, adatto allutilizzo in tempo reale. Il sistema, basato su una procedura di ottimizzazione non lineare, valuta un’azione combinata di sterzo e forza longitudinale per condurre il veicolo lungo una traiettoria di riferimento preceden-temente calcolata. Il lavoro si conclude con una descrizione dei risul-tati sperimentali ottenuti durante le sessioni di test e uno sguardo ai possibili sviluppi futuri.

(3)

Indice

1 Introduzione 4

2 Modello Single-Track 7

2.1 Modello cinematico . . . 7

2.2 Modello dinamico . . . 9

3 Pianificazione del moto 15 3.1 Algoritmo di pianificazione . . . 15

3.1.1 Definizione della Road . . . 17

3.1.2 Definzione di Cfree . . . 19

3.1.3 Costruzione del grafo topologico . . . 20

3.1.4 Ricerca del cammino minimo . . . 21

3.2 Ricerca euristica: uso iterativo dell’algoritmo di pianificazione 23 3.3 Valutazione del profilo di velocit`a e generazione di una traiet-toria funzione del tempo . . . 31

4 Localizzazione 34 4.1 La localizzazione in Tazio . . . 34

4.1.1 Struttura del layer di localizzazione . . . 35

4.2 Filtro di Kalman esteso (EKF) per la stima dello stato della vettura . . . 36 4.2.1 Implementazione dell’EKF . . . 39 4.2.2 Tuning dell’EKF . . . 40 4.2.3 Prestazioni dell’EKF . . . 40 5 Controllo 44 5.1 Il controllo in Tazio . . . 44

5.1.1 Struttura del layer di controllo . . . 47

5.2 Controllo disaccoppiato (Pure Pursuit ) . . . 49

5.3 Controllo predittivo model-based . . . 51

5.3.1 Real-Time MPC in Tazio . . . 53

5.3.2 Implementazione dell’MPC non lineare . . . 55

6 Risultati sperimentali 58 6.1 Pure Pursuit vs. MPC . . . 58

(4)

1 Introduzione

L’automazione dei mezzi di trasporto è diventata nell’ultima decade una del-le sfide più importanti per la comunità scientifica. Se in alcuni campi (linee di trasporto metropolitane, applicazioni nautiche e aeronautiche) le tecno-logie possono ritenersi già mature, in ambito automotive le grandi industrie automobilistiche non hanno ancora sviluppato sistemi pronti per essere resi accessibili al mercato di massa [1]. Per quanto l’auto che si guida da sola sia già presente nell’immaginario collettivo, in realtà la maturità dei sistemi di guida autonoma passa necessariamente dallo sviluppo di sistemi di sicurezza che garantiscano la coesistenza di questa tecnologia in ambiente urbano. Uno dei campi su cui si focalizza la ricerca è quello dello sviluppo di un’in-tellgenza artificiale che, per mezzo di telecamere, sensori LIDAR e Radar possa usufruire di quegli input normalmente a disposizione di un guidatore umano cos`ı da avere le informazioni necessarie per riconoscere le situazioni di pericolo e prendere le giuste decisioni controllando il veicolo in sicurezza [2].

L’attrattivit`a che questo tipo di tecnologie riscuote nei confronti dell’opinione pubblica, ha suscitato l’interesse degli investitori nell’ambito delle competi-zioni automobilistiche l`ı dove la tendenza all’innovazione e la disponibilit`a delle risorse consentono di sopperire agli elevati costi da sostenere per la sen-soristica e per i test sul campo.

In questo scenario si inserisce Roborace, un’azienda con sede nel Regno Unito che si propone di realizzare vetture full-electric da competizione (Figura 1) dotate di un sistema di intelligenza artificiale (guida autonoma di livello 5 [3]) nell’ottica di farle prender parte ad un Driverless Car Championship 1_.

(5)

Figura 1: Roborace Robocar.

Data l’ambizione del progetto ed il grande contenuto di innovazione ne-cessario, Roborace ha aperto le porte al mondo accademico iniziando una collaborazione con il Centro di Ricerca E. Piaggio dellUniversit`a di Pisa 2_.

Nell’ambito di questa partership `e nato il Team Roborace Italia3_{, un gruppo}

di studenti, dottorandi e ricercatori che hanno avuto accesso all’utilizzo della Devbot (Figura 2), un prototipo full-electric di derivazione LMP3 spinto da 4 motori elettrici (per un totale di 550hp) in grado di supportare guida in piena modalit`a autonoma e dotato dell’hardware pi`u avanzato oggi a disposizione nel campo automotive per l’implementazione di sistemi driverless.

2

https://www.youtube.com/watch?v=S8PPaRVT7FA&feature=share

3

(6)

Figura 2: Roborace Devbot.

La Devbot è equipaggiata con una GPU Nvidia PX2 ed una unità di calcolo real-time, ed integra telecamere, sensori LIDAR, Radar, GPS, ed una unità IMU.

L’obiettivo del Team Roborace Italia `e lo sviluppo di un software di guida auotonoma che, utizzando le misure dei sensori disponibili, sia in grado di condurre la vettura in pista senza alcun ausilio esterno.

Il software `e stato chiamato Tazio in onore al pilota italiano Tazio Nuvola-ri. Ad oggi Tazio `e in grado costruire una mappa di un circuito, pianificare off-line una traiettoria, e guidare la Devbot lungo la traiettoria pianificata in sicurezza.

Il lavoro svolto dal team nel corso dell’ultimo anno `e stato presentato alla 4th International Forum on Research and Technologies for Society and Industry (RTSI) tenutasi a Palermo a settembre 2018 [4] e sar`a presentato a mag-gio in occasione della International Conference on Robotics and Automation (ICRA) a Montreal [5].

Questo lavoro di tesi presenta il contributo offerto dal laureando al Team per lo sviluppo e l’implementazione di Tazio, incentrato sulla parte di controllo del veicolo.

(7)

2 Modello Single-Track

Per simulare la dinamica di un autoveicolo è necessario adottare un approc-cio scientifico e affidarsi a modelli matematici sufficientemente complessi per descriverne il comportamento in situazioni critiche quali sottosterzo e sovra-sterzo. In tal senso è importante introdurre nel modello una caratterizzazione seppur approssimativa degli pneumatici. Il modello single-track è in grado di riprodurre quelle dinamiche che si manifestano negli autoveicoli alle al-te velocità e al contempo presenta pochi parametri fisici: risulta dunque appropriato in problemi di identificazione e controllo [6].

2.1 Modello cinematico

Il modello monotraccia (single-track) cinematico, anche detto biciclo, consiste in un veicolo planare con due ruote. La ruota anteriore (nella direzione del senso di marcia) pu`o ruotare attorno all’asse perpendicolare al piano e passante per il suo punto di contatto.

δ(t) xmap y_map m rear wheel front wheel xa ya xm(t) ym(t) xr yr Lr La ur(t) ψ(t)

Figura 3: Modello Single-Track cinematico. Il modello `e caratterizzato da 3 parametri:

• interasse o passo: L = La+ Lr

• distanza tra baricentro e asse anteriore: La

(8)

Per descrivere lo stato del biciclo sono sufficienti 4 variabili: le coordinate di un punto m lungo l’asse longitudinale del veicolo, l’orientazione del veicolo nel frame inerziale e l’angolo di sterzo alla ruota anteriore.

q =       xm ym ψ δ       (1)

La geometria del veicolo impone i seguenti vincoli strutturali. ( xr= xm− Lrcos ψ yr = ym− Lrsin ψ ( xa= xm+ Lacos ψ ya = ym+ Lasin ψ (2)

Ai precedenti si aggiungono i vincoli di rotolamento senza strisciamento sulle due ruote.

(

˙xrsin ψ − ˙yrcos ψ = 0

˙xasin(ψ + δ) − ˙yacos(ψ + δ) = 0

(3) Derivando le (2) e sostituendole nelle (3) si ottiene la matrice dei vincoli.

(

( ˙xm+ Lrψ sin ψ) sin ψ − ( ˙˙ ym− Lrψ cos ψ) cos ψ = 0˙

( ˙xm− Laψ sin ψ) sin(ψ + δ) − ( ˙˙ ym+ Laψ cos ψ) cos(ψ + δ) = 0˙

=⇒

"

sin ψ − cos ψ Lr 0

sin(ψ + δ) − cos(ψ + δ) −Lacos δ 0

#       ˙xm ˙ ym ˙ ψ ˙δ       = 0 (4)

La matrice ottenuta `e in forma Pfaffiana Γ(q) ˙q = 0. La matrice Γ(q) ha rango 2. `E dunque possibile ricondurre il modello ad uno equivalente con due sole variabili di stato scegliendo una base del kernel della matrice Γ(q).

      ˙xm ˙ ym ˙ ψ ˙δ       =      

Lacos δ cos ψ + Lrcos(ψ + δ) 0

Lacos δ sin ψ + Lrsin(ψ + δ) 0

sin δ 0 0 1       " λ1 λ2 #

Scegliendo il punto m sull’asse posteriore e effettuando un’opportuna scelta della base del kernel di Γ(q) si ottengono le equazioni del modello monotraccia

(9)

a trazione posteriore.       ˙xr ˙ yr ˙ ψ ˙δ       =        cos ψ 0 sin ψ 0 tan δ L 0 0 1        " ur ω # (5)

Nelle (5) le variabili di stato del sistema assumono un significato fisico preciso: • ur `e la velocit`a di avanzamento dell’asse posteriore;

• ω `e la variazione dell’angolo di sterzo alla ruota anteriore.

2.2 Modello dinamico

Il modello single-track dinamico permette di dare una descrizione verosimi-le della dinamica lateraverosimi-le di un veicolo a quattro ruote. Si considerano verosimi-le seguenti assunzioni:

• strada perfettamente piatta;

• assenza di movimenti di rollio e beccheggio; • piccoli angoli di sterzo;

• velocit`a longitudinale costante; • bassa resistenza aerodinamica.

Le prime due assunzioni consentono di ridurre il modello ad un problema piano con soli tre gradi di libert`a. Le ultime due consentono di semplificare la dinamica longitudinale, focalizzando l’attenzione sullo slittamento laterale degli pneumatici.

(10)

y_car x_car δ(t) xmap y_map xG(t) yG(t) Lr La T u(t) r(t) v(t) ψ(t) Vcar

Figura 4: Modello del veicolo a 4 ruote. Si introducono i due sistemi di riferimento:

• sistema di riferimento inerziale (xmap, ymap): il sistema `e solidale alla

strada e ha orientazione e origine arbitrarie (per convenzione si orienta l’asse x verso ovest e l’asse y verso nord);

• sistema di riferimento locale (xcar, ycar): il sistema `e solidale al veicolo

con asse x diretto lungo l’asse longitudinale, positivo lungo la direzione di marcia e con origine coincidente col baricentro del veicolo (xG, yG); il

frame si muove con velocit`a Vcar ed `e ruotato rispetto al frame inerziale

di un angolo ψ (angolo di imbardata).

Il modello in esame presenta i seguenti parametri strutturali:

• La distanza lungo l’asse longitudinale tra braicentro e assale anteriore

(semipasso anteriore);

• Lr distanza lungo l’asse longitudinale tra braicentro e assale posteriore

(semipasso posteriore);

• T distanza lungo gli assali tra ruota e asse longitudinale, assunta uguale per assale anteriore e posteriore (semicarreggiata).

Si assume inoltre che le ruote sterzanti siano solo quelle anteriori con angolo di sterzo alla ruota pari a δ(t).

Il moto del veicolo nel piano è completamente descritto dalla velocità traslazionale del suo baricentro rispetto al sistema inerziale e dalla velocità

(11)

angolare (velocità di imbardata) rispetto al sistema inzerziale. È conveniente esprimere il modello in funzione delle velocità misurate nel frame locale u, v, r.      ˙xG = u cos ψ − v sin ψ ˙ yG= u sin ψ + v cos ψ ˙ ψ = r (6) Nella (6)

• u: rappresenta la velocit`a di avanzamento longitudinale; • v: rappresenta la velocit`a laterale;

• r: la velocit`a rotazionale di imbardata.

Per risalire alle forze esercitate dagli pneumatici sul veicolo `e necessario inve-stigare la velocit`a del punto di contatto ruota-strada di ciascuno dei quattro pneumatici: V11= (u − rT ) i + (v + rLa) j V12= (u + rT ) i + (v + rLa) j V21= (u − rT ) i + (v − rLr) j V22= (u + rT ) i + (v + rLr) j (7)

Sotto le ipotesi di piccoli angoli di sterzo 0 ≤ δ(t) < 30◦ è lecito assumere u rT . Ciò implica che in pratica le ruote appartenenti allo stesso assale si muovono alla stessa velocità. Da questa osservazione nasce l’idea alla base del modello Single-Track ovvero quella di considerare una ruota per assale e assumere che le forze longitudinali e laterali applicate sul veicolo siano imputabili a soli due pneumatici posizionati lungo l’asse longitudinale del veicolo.

V11≈ V21 ≈ V1 = u i + (v + rLa) j

(12)

δ(t) α1 α2 xmap y_map y_car x_car xG(t) yG(t) V1 V2 F_1l F_1t F_2l F_2t Lr La u(t) r(t) v(t)

Figura 5: Modello Single-Track dinamico.

Si definisce angolo di deriva (slip angle) dello pneumatico l’angolo tra la direzione che punta lo pneumatico (orientazione del suo asse longitudinale) e la direzione verso cui sta effetivamente viaggiando (direzione della velocit`a del suo punto di contatto). Gli angoli di deriva per ciascuno pneumatico sono indicati in Figura 5 con α1 e α2 e possono esprimersi come segue.

α1 = δ − arctan v + rLa u α2 = − arctan v − rLr u (8)

Le equazioni costitutive del modello sono rappresentate dalle caratteristi-che degli pneumatici. Nel caso di solo slittamento laterale (in condizioni di perfetta aderenza) la forza laterale degli pneumatici pu`o essere espressa in funzione del solo angolo di deriva.

F1t= F1t(α1)

F2t= F2t(α2)

(9)

Fit= Di sin {Ci arctan [Biαi− Ei(Biαi− arctan(Biαi))]} (10)

Sotto le ipotesi precedentemente assunte la forza longitudinale imposta dagli pneumatici `e direttamente riconducibile alla forza di trazione trasmessa dal motore alla ruota. Assumendo una ripartizione della coppia motrice (FT)

(13)

(FB) pari a Bb si ottengono le seguenti espressioni valide nelle due fasi di accelerazione e frenata. F1l =        − Tb 1 + Tb FT in accelerazione Bb 1 + Bb FB in frenata F2l =        − 1 1 + Tb FT in accelerazione 1 1 + Bb FB in frenata (11)

Infine si impone l’equilibrio traslazionale e rotazionale del centro di massa del veicolo.

m( ˙u − vr) = −F1tsin(δ) − F1lcos(δ) − F2l

m( ˙v + ur) = F1tcos(δ) − F1lsin(δ) + F2t

Jz˙r = La[F1tcos(δ) − F1lsin(δ)] − LrF2t

(12)

Riassumendo, le equazioni che descrivono il modello Single-Track dinami-co sono le seguenti, ottenute mettendo insieme le (6), (8), (9), (11), (12). Il modello ha sei variabili di stato e due variabili di ingresso e richiede di “swit-chare” tra due set di equazioni a seconda che si richieda forza di trazione o forza di frenata. x =h u v r xG yG ψ iT uacc/bra = h δ FT/FB iT

(14)

Sistema in accelerazione: x = Φ˙ acc(x, uacc) Φacc:                                                        ˙ u = vr −F1t(α1(u, v, r, δ)) m sin(δ) + Tb m(1 + Tb) FT cos(δ) + 1 m(1 + Tb) FT ˙v = −ur + F1t(α1(u, v, r, δ)) m cos(δ) + Tb m(1 + Tb) FT sin(δ) + F2t(α2(u, v, r)) m ˙r =LaF1t(α1(u, v, r, δ)) Jz cos(δ) + LaTb Jz(1 + Tb) FT sin(δ) − LrF2t(α2(u, v, r)) Jz ˙ xG = u cos ψ − v sin ψ ˙ yG= u sin ψ + v cos ψ ˙ ψ = r (13) Sistema in frenata: x = Φ˙ bra(x, ubra)

Φbra :                                                        ˙ u = vr −F1t(α1(u, v, r, δ)) m sin(δ) − Bb m(1 + Bb) FBcos(δ) − 1 m(1 + Bb) FB ˙v = −ur + F1t(α1(u, v, r, δ)) m cos(δ) − Bb m(1 + Bb) FBsin(δ) + F2t(α2(u, v, r)) m ˙r = LaF1t(α1(u, v, r, δ)) Jz cos(δ) − LaBb Jz(1 + Bb) FBsin(δ) − LrF2t(α2(u, v, r)) Jz ˙ xG= u cos ψ − v sin ψ ˙ yG= u sin ψ + v cos ψ ˙ ψ = r (14) Il modello single-track `e stato in grado di simulare con una buona preci-sione la dinamica della Devbot. L’analisi della dinamica e l’identificazione parametrica del single-track `e stata descritta in [7].

(15)

3 Pianificazione del moto

In questa sezione `e trattato il problema della pianificazione della traccia di riferimento per il controllo del veicolo su un tracciato predefinito. Gli obiettivi della pianificazione sono:

• pianificare una traccia che sia feasible, considerando i limiti del veicolo (massimo angolo di sterzo, massima forza di trazione e massima forza frenante) e i limiti degli pneumatici (limiti di aderenza);

• pianificare una traccia che rispetti i vincoli dettati dai limiti di pista compresi eventuali margini di sicurezza;

• minimizzare il tempo sul giro.

La minimizzazione del tempo di percorrenza di un giro in pista passa necessa-riamente per due criteri: minimizzare lo spazio percorso, ovvero la lunghezza della traccia; o massimizzare la velocità con cui il veicolo può viaggiare sul-la traiettoria. Il secondo criterio coincide con sul-la minizzazione del profilo di curvatura della traccia: tanto più è larga una curva (più grande il raggio di curvatura) tanto più velocemente può essere percorsa senza che gli pneuma-tici superino i limiti di aderenza.

In generale il quadrato della velocit`a massima percorribile in curva `e diretta-mente proporzionale alla massima forza laterale sostenibile dagli pneumatici Ftmax ed inversamente proporzionale al valore di curvatura k,

V_max2 ∝ Ftmax

k .

La miminizzazione del profilo di curvatura è il criterio più utilizzato in lette-ratura nonchè quello che più verosimilmente adottano i piloti umani in pista. In seguito sarà descritto un metodo per il calcolo della traccia che fa uso della teoria dei grafi. Partendo dalla conoscenza di margine destro e sinistro della pista il metodo crea un grafo topologico i cui vertici corrispondono a punti accessibili sul circuito e cerca su di esso il cammino che minimizzi una oppor-tuna funzione costo. I vertici sono connessi per mezzo di clotoidi4_{, dunque}

la traccia generata `e clotoidale.

3.1 Algoritmo di pianificazione

L’algoritmo prende in input margine destro e sinistro della pista. Tali margi-ni sono ottenuti con precisione in un opportuno sistema di riferimento (map

4_Def. _{Clotoide: curva il cui profilo di curvatura varia linearmente lungo la sua}

(16)

frame) per mezzo di dispositivi di localizzazione GPS e forniti da Roborace ai team prima di ogni evento. Ciascun margine corrisponde ad una serie ordinata di punti equispaziati (a passo di 0.5m).

Tali margini delimitano lo spazio accessibile (Cfree): il baricentro della

vet-tura deve restare entro questi limiti per evitare impatti con i muri esterni. Questo vincolo è da considerarsi un hard task. A questi margini si aggiungo-no ulteriori margini di sicurezza, la cui entità dipende da diversi fattori quali precisione del sistema di localizzazione, garanzie di stabilità del sistema di controllo, aderenza dell’asfalto, condizioni metereologiche. In ogni caso la di-stanza di sicurezza dai margini è imposta da Roborace il giorno stesso della sessione di test o di gara e solitamente rientra nel range 1 − 3m.

Figura 6: Porzione del tracciato di Berlino e margini di sicurezza (1.5m) Si `e reso necessario nel corso degli eventi preparare preventivamente di-verse tracce, ciascuna associata ad un diverso margine di sicurezza.

L’algoritmo deve fornire una traccia che:

• sia sempre compresa tra margine sinistro e destro della pista, tenendo conto del margine di sicurezza fornito in input;

(17)

• abbia un profilo di curvatura di ampiezza limitata e la cui velocit`a di variazione sia limitata, entro limiti prefissati forniti in input;

• sia soluzione di un problema di ottimizzione del tipo: p∗(s) = arg min p(s) J , (15) J = s=Smax−2 X s=1 alkp(s + 1) − p(s)k₂ + ak|ψ(s + 1) − ψ(s)| , ψ(s) = arctan py(s + 1) − py(s) px(s + 1) − px(s) heading angle

La funzione costo J tiene conto della lunghezza della traccia pesata con un coefficiente ale della curvatura totale della traccia pesata con un coefficiente

ak. Si noti che la curvatura `e definita come la variazione dell’angolo di

direzione della traccia.

L’algoritmo consta di 4 passi: 1. Definizione della Road ; 2. Definzione di Cfree;

3. Costruzione del grafo topologico; 4. Ricerca del cammino minimo. Ne segue una breve descrizione. 3.1.1 Definizione della Road

La classe Road `e definita come una serie ordinata di segmenti quasi paralleli di lunghezza variabile apparentemente simili a traversine ferroviarie, come illustrato in Figura 7.

(18)

Figura 7: Esempio di Road.

In pratica `e definito un insieme ordinato di coppie di punti: ogni coppia costituisce una traversina e definisce la larghezza della carreggiata accessibile della pista. Lo scopo `e ottenere due insiemi ordinati di punti (insieme dei limiti destri delle traversine ed insieme dei limiti sinistri) in corrispondenza biunivoca che avanzino coerentemente lungo la direzione di percorrenza della pista.

Per ottenere ci`o si sceglie uno dei margini destro o sinistro forniti in input come margine pivot e l’altro come margine fringe: ad ogni punto del pivot si associa un punto del fringe in modo tale che la traversina ottenuta sia il pi`u possibile ortogonale ad entrambe le curve definite dai margini, come illustrato in Figura 8.

(19)

Figura 8: Il punto p del margine pivot è associato a quel punto q∗del margine fringe che rende la traversina il più possibile ortogonale ad entrambi i margini. Con riferimento alla Figura 9, alla fine della procedura è possibile ottenere facilmente la linea media della carreggiata e i limiti imposti dal margine di sicurezza fornito in input.

Figura 9: Esempio della definizione della Road e della seguente valutazione della linea media e dei margini di sicurezza.

3.1.2 Definzione di Cfree

L’insieme Cfree`e il risultato di un campionamento della carreggiata

accessibi-le come previsto dal margine di sicurezza. La procedura definisce un insieme di punti equispaziati ad alta risoluzione (circa 10cm) che rappresentano lo spazio accessibile della pista, come mostrato in Figura 10. I punti sono scelti campionando uniformemente le porzioni delle traversine comprese tra i mar-gini destro e sinistro. A ciascun punto si associa un angolo di direzione scelto

(20)

in modo da puntare la direzione ortogonale alla traversina di appartenenza: `

e la direzione di riferimento per la traiettoria che in prima approssimazione pu`o considerarsi la direzione tenuta dalla linea media.

Figura 10: Esempio di Cfree.

3.1.3 Costruzione del grafo topologico

Si crea un grafo che ha per vertici coppie (p, ψ), con p ∈ Cfreee ψ ∈ B(ψ0, α),

dove B(ψ0, α) `e l’intorno di ψ0 di raggio α. Ad ogni vertice corrisponde un

punto in Cfreeassociato ad una direzione scelta in un intorno della direzione di

riferimento. Pi`u vertici possono corrispondere ad uno stesso punto in Cfreema

con direzioni differenti, come visibile in Figura 11. Tali vertici costituiscono gli estremi di clotoidi possibili candidate a far parte della soluzione finale. La scelta dei punti in Cfreeche prendono parte al grafo `e fatta secondo criteri

di spaziatura longitudinale e trasversale scelti dall’utente. Allo stesso modo l’utente può definire l’insieme di scostamenti direzionali dalla direzione di riferimento ψ0 per ciascun punto. La cardinalità del grafo è tanto maggiore

quanto maggiore è la risoluzione con cui si sceglie di mappare la pista e quanto maggiore è l’insieme degli scostamenti direzionali. Una maggiore cardinalità del grafo va a beneficio dell’ottimalità della soluzione, a scapito del tempo computazionale dell’algoritmo.

(21)

Figura 11: Esempio di vertici di un grafo topologico. 3.1.4 Ricerca del cammino minimo

Dati due vertici del grafo, ciascuno rappresentato da un punto ed una dire-zione, esiste una sola clotoide che li connette. I vertici sulla linea di partenza sono contrassegnati con costo nullo. Con riferimento alla Figura 12, gli ar-chi sono tracciati seguendo la direzione di percorrenza del circuito. Ad ogni vertice è associato un vertice-genitore tra quelli che lo precedono e distano al più un certo orizzonte spaziale definito dall’utente. Nel momento in cui ad un vertice è associato un genitore gli si assegna un costo pari al costo del genitore più il costo della clotoide che li connette. Il genitore assegnato ad un vertice è quello che consente di raggiungere il vertice stesso a costo minimo per mezzo di una clotoide giacente in Cfree.

(22)

Figura 12: Al vertice V `e assegnato come genitore il vertice P nell’orizzonte che consente di raggiungere V a costo minimo.

Maggiore è l’orizzonte spaziale, maggiore è l’insieme delle clotoidi da trac-ciare per l’assegnamento del genitore a ciascun vertice, e tanto maggiore è il tempo computazionale; tuttavia un orizzonte maggiore consente di esplorare un maggior numero di cammini a beneficio dell’ottimalità della soluzione. La funzione costo è la (15) ed include un test di accettabilità della clotoide: se la clotoide tracciata non rispetta i vincoli sulla curvatura previsti o non giace in Cfree (si sviluppa fin oltre i margini di sicurezza) è restituito costo infinito

(23)

Figura 13: In alto un esempio di una clotoide scartata perch`e v`ıola i vincoli di curvatura; in basso un esempio di una colotoide scartata perch`e supera i margini di sicurezza.

Quando la procedura si conclude `e creato un albero: tutti i vertici sulla linea di arrivo sono connessi alla linea di partenza per mezzo di un solo cammino e contrassegnati con un valore costo. La traccia fornita in output `

e quella che connette la linea di partenza al vertice sulla linea di arrivo contrassegnato dal costo minimo.

3.2 Ricerca euristica: uso iterativo dell’algoritmo di

pianificazione

Il codice che implementa il metodo esposto consente di espandere a propria discrezione lo spazio delle tracce, inteso come l’insieme delle combinazio-ni di possibili clotoidi. I criteri di spaziatura longitudinale e laterale e gli scostamenti direzionali valutati per ciascun punto definiscono il numero di vertici del grafo e dunque il numero di combinazioni di traiettorie clotoidali esplorate. Il tempo computazionale aumenta esponenzialmente

(24)

all’aumenta-re della cardinalit`a del grafo: la scelta dipende principalmente dal tempo a disposizione per la valutazione della traiettoria. Si riporta un esempio dell’al-goritmo eseguito sul tracciato di Berlino. L’output dell’aldell’al-goritmo `e riportato in Figura 14.

Track: Berlin_2018 Margin value: 1.5

Starting optimization loop.

Graph has been created according to the following criteria: - longitudinal resolution: maximum distance between lines 9m - lateral resolution: mean distance between points 2.5m - heading resolution: all heading angles in the range

(heading_ref + [-1.5:1.5:1.5]) There are 3741 nodes in the graph.

Parents node are searched within an horizon of 200m. Completing first lap... 100%

Completing second (closing) lap... 25% Best trajectory cost: 3.2868

Best trajectory length: 2323.3227 Elapsed time is 105 minutes.

(25)

Figura 15: Soluzione ottima sul tracciato di Berlino: in alto i vertici del grafo, in basso la traiettoria ottima fornita.

I criteri scelti in input hanno portato alla creazione di un grafo con circa 4000 vertici; il tempo computazionale `e stimato di 105 minuti.

`

E possibile ottenere comunque un buon risultato utilizzando un metodo di ricerca euristico che costruisca grafi con cardinalità inferiori campionando Cfree in modo più approssimativo ma mirato: iterando l’algoritmo è possibile

ad ogni passo utilizzare criteri di spaziatura che sfruttino le informazioni ottenute al passo precedente.

Si noti che i vertici scelti dall’algoritmo per la traccia finale sono spesso sui limiti della carreggiata accessibile. Per la ricerca di una soluzione di primo tentativo `e opportuno dunque focalizzare la ricerca sui limiti nella carreggiata accessibile, come mostrato in Figura 16.

(26)

Figura 16: Soluzione sub-ottima di primo tentativo: in alto i vertici del grafo, in basso la traiettoria ottima fornita.

In seconda battuta, avendo ottenuto i punti di corda, si pu`o estendere la ricerca verso punti interni della carreggiata aumentando la risoluzione laterale. In pratica, con riferimento a Figura 17, si crea un nuovo grafo utilizzando i vertici del cammino minimo ottenuti nella prima ricerca e in aggiunta i vertici vicini sulle stesse traversine.

(27)

Figura 17: Soluzione sub-ottima di secondo tentativo: in alto i vertici del grafo, in basso la traiettoria ottima fornita.

Infine per raffinare la soluzione si esplorano i vertici con scostamenti di-rezionali rispetto alla direzione di riferimento tenuta dalla linea media, come mostrato in Figura 18.

(28)

Figura 18: Soluzione sub-ottima di terzo tentativo: in alto i vertici del grafo, in basso la traiettoria ottima fornita.

La terza soluzione `e sub-ottima ma molto vicina alla soluzione ottima ottenuta con l’onerosa ricerca esaustiva.

Si riporta in Figura 19 un confronto delle due soluzioni e in Figura 20 l’output del metodo iterativo eseguito su Matlab.

(29)

(30)

Track: Berlin_2018 Margin value: 1.5

Starting first optimization loop.(1/3)

Graph has been created according to the following criteria: - longitudinal resolution: maximum distance between lines 9m - lateral resolution: 2 points per line

- heading resolution: reference heading There are 558 nodes in the graph.

Parent nodes are searched within an horizon of 200m. Completing first lap... 100%

Starting second optimization loop.(2/3)

Graph has been created according to the following criteria: - longitudinal resolution: hot lines

- lateral resolution: mean distance between points 2.5m - heading resolution: reference heading

There are 212 nodes in the graph.

Starting last optimization loop.(3/3)

Graph has been created according to the following criteria: - longitudinal resolution: hot lines

- lateral resolution: hot points

- heading resolution: all heading angles in the range (heading_ref + [-1.5:1.5:1.5]) There are 513 nodes in the graph.

Figura 20: Output del metodo iterativo eseguito in Matlab.

Si riportano in Tabella 1 e Tabella 2 la schematizzazione dei risultati ot-tenuti rispettivamente con metodo non iterativo (ricerca esaustiva) e metodo di ricerca euristico iterativo (ricerca progressiva).

Berlin

Iter.

Resolution Criteria (Long/Lat/Head)

Path Cost Path Length Time

1 9m / 2.5m/ 3 3.2868 2323.3227 105min

(31)

Berlin

Iter.

Resolution Criteria (Long/Lat/Head)

Path Cost Path Length Time

1 9m/ 2 per line/ 1 3.307 2326.6484 2min

2 from prev. sol./ 2.5m/ 1 3.3057 2326.3366 1min 3 from prev. sol./ from prev. sol./ 3 3.2877 2325.9201 1min

Tabella 2: Risultati ottenuti per ricerca progressiva.

Il metodo iterativo consente di ottenere una traccia di costo peggiore dello 0.03% con un risparmio del tempo computazionale del 96% (il tempo stimato di calcolo passa da 105 a 4 minuti).

Il metodo di pianificazione della traccia proposto `e tuttora in fase di svilup-po: `e neccessario ridurre ulteriormente il tempo computazionale per pensare di applicarlo per una pianificazione on-line.

La maggior parte del carico computazionale è impiegato per la generazione delle clotoidi: si può pensare di costruire off-line un insieme di clotoidi va-rianti in lunghezza e direzione da cui attingere on-line per risparmiare tempo di calcolo. Inoltre in previsione di una pianificazione che tenga conto della presenza di ulteriori veicoli in pista, è necessario introdurre ulteriori vincoli che evitino l’impatto con ostacoli in movimento nel mezzo della carreggiata.

3.3 Valutazione del profilo di velocit`

a e generazione di

una traiettoria funzione del tempo

Data una traccia di riferimento, è necessario pianificare la velocità di percor-renza ideale del veicolo. Il profilo di velocità dipende necessariamente dalla dinamica longitudinale e laterale del veicolo e deve tener conto di:

• limiti di aderenza degli pneumatici sull’asfalto; • limiti di potenza erogata dai motori elettrici; • limiti dell’impianto frenante.

Nella pratica il calcolo del profilo di velocità è affidato ad una procedura di ottimizzazione vincolata [7] in cui la funzione costo pesa il tempo di per-correnza del giro e i vincoli sono imposti sulle accelerazioni longitudinale e laterale che devono sostenere gli pneumatici nel corso del giro. Un punto materiale viaggiante lungo la traiettoria p(s) con velocità v(s) è soggetto

(32)

alle seguenti accelerazioni: aL(s) = v(s + 1) − v(s) kp(s + 1) − p(s)k aT(s) = v(s) k2(s) = v(s) ψ(s + 1) − ψ(s) kp(s + 1) − p(s)k 2

Il modulo dell’accelerazione longitudinale e laterale deve essere inferiore ai limiti di aderenza longitudinale e laterale degli pneumatici. Le accelerazioni massime sostenibili sono funzioni delle condizioni dell’asfalto e degli pneu-matici, della velocit`a per via dei profili aerodinamici, del peso della vettura. Si rivela necessaria in questa fase l’identificazione di una mappa delle perfor-mance raggiungibili [8], ovvero di leggi del tipo aLmax(u), aT max(u) funzioni

della velocit`a longitudinale. Tali mappe consentono di porre i vincoli sulle accelerazioni ed ottenere un profilo di velocit`a che la vettura possa effetti-vamente sostenere in sicurezza. A questi si aggiungono i vincoli derivanti dalla forza massima di trazione erogata dai motori Facc(u) e dalla forza

mas-sima consentita dall’impianto frenante Fbra(u) da imporre all’accelerazione

longitudinale. kaL(s)k < aLmax(v(s)) kaT(s)k < aT max(v(s)) aL(s) aLmax(v(s)) 2 + aT(s) aT max(v(s)) 2 < 1 −Fbra(u) m < aL(s) < Facc(u) m

Avendo ottenuto un profilo di velocità v(s) su una traccia p(s) è possibile ottenere una legge oraria p(τ ). Il controllore predittivo necessita infatti di un riferimento che si aggiorni lungo l’orizzonte temporale: per far questo è necessario che la traiettoria fornita come riferimento sia funzione del tempo. Questo approccio consente di valutare allo stesso tempo e con un unico con-trollore gli input di sterzo e forza longitudinale (di trazione o di freno) da fornire al veicolo.

Il calcolo della legge oraria è il risultato di una interpolazione. A partire dalla velocità istantanea in un punto della traiettoria si calcola il tempo neces-sario per giungere al successivo ipotizzando che la velocità resti costante nel

(33)

tratto di traiettoria compreso tra i due. L’approssimazione è accettabile se la traiettoria è fornita ad alta risoluzione: in caso contrario è necessario effet-tuare un up-sampling. La procedura illustrata sotto forma di pseudo-codice fornisce la legge oraria della traiettoria di riferimento a passo temporale Ts.

Il passo temporale `e quello con cui il controllore predittivo aggiorna la legge di controllo. Siano dati: p(s), v(s), s = 1, . . . N Upsampling (0.1m) δS(s) = {0, kp(2) − p(1)k₂, . . . kp(i) − p(i − 1)k₂, . . . kp(N ) − p(N − 1)k₂} ∆S(s) = {δS(0), δS(1) + δS(0), . . . δS(s) + δS(s − 1), . . . δS(N ) + δS(N − 1)} ∆Sres(ˆs) = {0, 0.1, . . . ˆs 0.1, . . . M 0.1 u ∆S(N )} , s = 1, . . . Mˆ

p_res(ˆs) = resample( ∆S(s), p(s), ∆Sres(ˆs) ), s = 1, . . . Mˆ

vres(ˆs) = resample( ∆S(s), v(s), ∆Sres(ˆs) ), s = 1, . . . Mˆ

Trajectory generation (Ts) δT (ˆs) = 0, 0.1 vres(1) , . . . 0.1 vres(ˆs) , . . . 0.1 vres(M ) ∆T (ˆs) = {δT (0), δT (1) + δT (0), . . . δT (ˆs) + δT (ˆs − 1), . . . δT (M ) + δT (M − 1)} ∆Tres(τ ) = {0, Ts, . . . τ Ts, . . . K Tsu ∆T (M )} , τ = 1, . . . K p_ref(τ ) = resample( ∆T (ˆs), p_res(ˆs), ∆Tres(τ ) ), τ = 1, . . . K

(34)

4 Localizzazione

Il problema della localizzazione è di cruciale importanza nell’ottica del con-trollo e della sicurezza. È necessario capire dove è situata la vettura e come `

e orientata in pista per poterla controllare sulla traiettoria di riferimento. Il sistema GPS di cui è dotata la Devbot consente di localizzarsi con un’ac-curatezza dell’ordine dei 10cm: questo significa che quando la ricezione del segnale GPS è buona non occorrono altri sistemi di localizzazione. Tutta-via in alcune piste cittadine la presenza di superfici riflettenti ed alberi a bordo pista possono causare problemi di ricezione a scapito dell’accuratezza della stima della posizione della vettura. Una localizzazione imprecisa com-promette il controllo e aumenta il rischio d’impatto l`ı dove la traiettoria di riferimento è vicina ai muri a bordo pista. Per ragioni di sicurezza è dunque necessario garantire una buona localizzazione anche in assenza del segnale GPS, e ciò è possibile sfruttando tutte le informazioni provenienti dai sensori (sensore ottico di velocità, IMU, Lidars).

4.1 La localizzazione in Tazio

Lo sviluppo del sistema di localizzazione si concentra sul come utilizzare al meglio tutte le informazioni disponibili dalla sensoristica a disposizione sul Devbot. Escludendo il sistema GPS, `e possibile localizzarsi utizzando due tecniche.

• Odometria

Il sensore ottico e la IMU consentono di ottenere informazioni iner-ziali: velocit`a lineari ed angolari ed accelerazioni lineari. Integrando tali segnali si ottiene una stima della posizione della vettura che ine-vitabilmente deriva nel tempo: sebbene consenta di essere precisi nel breve periodo ha bisogno di correzioni periodiche che annullino l’errore derivato dall’integrazione del rumore cui sono affette tutte le misure inerziali.

• SLAM

I LIDAR consentono di localizzare la vettura relativamente ai muri ed oggetti rilevati a bordo pista. Software con tecnologia SLAM (Simulta-neous Localization and Mapping) come Google Cartographer permet-tono di localizzarsi su una mappa creata a partire dalle informazioni dei sensori Lidars e dalle informazioni odometriche. La localizzazione otte-nuta con l’ausilio di questi software `e rumorosa e fornita con una bassa frequenza di aggiornamento, ma pu`o costituire una buon feedback per la correzione del drift della localizzazione ottenuta da odometria.

(35)

L’approccio scelto per realizzare il software di localizzazione odometrica è basato sul filtro di Kalman. Avendo a disposizione un buon modello della vettura, il filtro di Kalman è in grado di integrare al meglio le informazioni provenienti dai sensori di velocità e accelerazione assieme ai controlli di sterzo e coppia forniti alla stessa vettura per stimare l’evoluzione dello stato della vettura compresa la sua posizione e orientazione. Infatti, nelle ipotesi che le misure fornite dai sensori siano affette da rumore bianco, il filtro di Kalman `

e uno stimatore che minimizza la varianza dell’errore di stima.

Dal momento che il modello della vettura introdotto `e altamente non lineare, si utilizza un filtro di Kalman esteso.

4.1.1 Struttura del layer di localizzazione

Il sistema di localizzazione pensato per Tazio presenta la seguente architet-tura. Lidar Optical sensor Unit`a IMU SLAM Slam localization EKF uacc/bra(t) ˆ x(t)

Figura 21: Layer di localizzazione del software Tazio. La localizzazione SLAM utilizza le informazioni dei sensori Lidar, sensore ottico e unit`a IMU per ottenere una prima localizzazione approssimativa della vettura. Questa `

e utilizzata assieme alle misure del sensore ottico e dell’unit`a IMU e ai con-trolli imposti al passo precedente uacc/bra(t) dal filtro di Kalman esteso per

ottenere una stima del vettore di stato completo al passo corrente.

La localizzazione SLAM è affidata a Cartographer ed è in grado di fornire una stima approssimativa della posizione della vettura in pista con una bassa frequenza di aggiornamento. Tuttavia tale stima è fondamentale per correg-gere il drift cui è affetta la localizzazione stimata dal filtro di Kalman. I due filtri funzionano dunque in modo complementare, con il filtro SLAM che fornisce informazioni significative di steady-state ed il filtro EKF che invece ricostruisce le dinamiche in alta frequenza.

(36)

4.2 Filtro di Kalman esteso (EKF) per la stima dello

stato della vettura

Il filtro di Kalman esteso utilizza il modello non lineare (16) descritto dalle (13) e (14).

˙ x =

(

Φacc(x, uacc) se si richiede trazione

Φbra(x, ubra) se si richiede frenatura

(16)

Le uscite del modello sono definite in base alle misure a disposizione per valutare la stima dello stato del sistema. Si progetta un filtro che utilizza le misure dei sensori come descritto nella (17).

Localizzazione SLAM :      EastSLAM → xG NordSLAM → yG YawSLAM → ψG Sensore ottico : ( V longitudinal_OS → u V lateralOS → v (17) Unit`a IMU :     

Yaw rateIMU → r

A longitudinalIMU → ax

A lateralIMU → ay

Per le prime 6 misure l’assegnamento `e banale: sono misure attribuibili di-rettamente alle variabili di stato del sistema. Per le ultime due invece si fa riferimento alle (12) per il calcolo delle accelerazioni nel frame locale. Le uscite del sistema sono poste come mostrato nella (18).

y = (

Γacc(x, uacc) se si richiede trazione

Γbra(x, ubra) se si richiede frenatura

(37)

Nella (18) le funzioni non lineari Γacc e Γbra sono descritte rispettivamente dalle (19) e (20). Γacc :                                                                        xG= xG yG = yG ψG = ψG u = u v = v r = r ax = −F1t(α1(u, v, r, δ)) sin(δ) + Tb 1 + Tb FT cos(δ) + 1 1 + Tb FT ay = F1t(α1(u, v, r, δ)) cos(δ) + Tb 1 + Tb FT sin(δ) + F2t(α2(u, v, r)) (19)

(38)

Γbra :                                                                        xG= xG yG = yG ψG = ψG u = u v = v r = r ax = −F1t(α1(u, v, r, δ)) sin(δ) − Bb 1 + Bb FBcos(δ) − 1 1 + Bb FB ay = F1t(α1(u, v, r, δ)) cos(δ) − Bb 1 + Bb FBsin(δ) + F2t(α2(u, v, r)) (20)

Il filtro di Kalman esteso valuta ad ogni passo il sistema linearizzato nel-l’intorno dello stato corrente e degli ingressi correnti. Il sistema linearizzato `

e descritto dalle matrici A, C descritte rispettivamente nelle (21), (22).

A =            Aacc(x, uacc) = ∂Φacc(x, uacc) ∂x se si richiede trazione Abra(x, ubra) = ∂Φbra(x, ubra) ∂x se si richiede frenatura (21) C =            Cacc(x, uacc) = ∂Γacc(x, uacc) ∂x se si richiede trazione Cbra(x, ubra) = ∂Γbra(x, ubra) ∂x se si richiede frenatura (22)

(39)

4.2.1 Implementazione dell’EKF

L’implementazione del filtro `e ricorsiva. Al passo iniziale sono noti: • ˜x0 stima dello stato iniziale del sistema;

• P0 matrice di covarianza associata all’errore di stima dello stato inziale

(nulla se lo stato iniziale `e noto); • uacc/bra

0 controllo imposto all’istante inziale;

• ymeas

0 uscita del sistema misurata all’istante iniziale.

Supponendo di aggiornare la stima con passo temporale T , al passo k sono eseguite le seguenti istruzioni.

• Valutazione della matrice C del sistema linearizzato

Ck=       

Cacc(˜xk, uacck ) se si richiede trazione

Cbra(˜xk, ubrak ) se si richiede frenatura

• Correzione della stima dello stato e della matrice di covarianza in base alle misure correnti degli output

˜ y_k =       

Γacc(˜xk, uacck ) se si richiede trazione

Γbra(˜xk, ubrak ) se si richiede frenatura

Kk = PkCTk CkPkCTk + R −1 ˆ xk= ˜xk+ Kk(ymeask − ˜yk) ˜ Pk = Pk− PkCkT CkPkCTk + R −1 CkPk

• Valutazione della matrice A del sistema linearizzato e discretizzato

Ak=       

Aacc(ˆxk, uacck ) se si richiede trazione

(40)

Fk= I + AkT

• Predizione dello stato e della matrice di covarianza ˜ xk+1 =        ˆ

xk+ Φacc(ˆxk, uacck ) T se si richiede trazione

ˆ

xk+ Φbra(ˆxk, ubrak ) T se si richiede frenatura

Pk+1 = FkP˜kFTk + Q

4.2.2 Tuning dell’EKF

Il buon funzionamento dell’EKF dipende dalla scelta delle matrici peso Q e R. Da un punto di vista fisico Q rappresenta la matrice di covarianza del disturbo sul processo. Tale disturbo modella le imprecisioni del modello della vettura, e i disturbi esterni quali vento, variazione del grip dell’asfalto, effetto scia, che causano variazioni non previste della dinamica della vettura. La matrice R rappresenta invece la covarianza degli errori di misura da cui sono affette le informazioni dei sensori. La scelta delle matrici peso è fatta principalmente in base all’esperienza del controllista, ma un buon feedback per la valutazione del corretto funzionamento del filtro è dato dal segnale GPS. Il tuning è dunque possibile in quelle situazioni in cui il segnale GPS ha buona ricezione: le matrici peso sono quindi scelte in modo che la localiz-zazione fornita dal filtro di Kalman si avvicini il più possibile a quella fornita dal GPS.

4.2.3 Prestazioni dell’EKF

Le prestazioni del filtro sono valutate in simulazione sui dati registrati da sensore ottico di velocit`a, unit`a IMU e GPS nel corso delle prove effettuate sul tracciato di Berlino a Maggio 2018 in occasione del Gran Premio di For-mula E.

I dati sono campionati a passo Ts= 0.004s, lo stesso passo utilizzato per

l’ag-giornamento della stima. La localizzazione SLAM `e simulata aggiungendo rumore bianco di covarianza unitaria alle misure GPS. Per rendere realistica la simulazione si suppone che le misure da SLAM siano aggiornate a passo Ts= 0.04s.

In un primo momento si valuta l’errore di localizzazione del filtro di Kalman fornendogli in input solo misure inerziali, dunque solo velocità e accelerazio-ni. In Figura 22 è riportato l’errore tra la stima della localizzazione ottenu-ta utilizzando l’EKF e la misura del GPS; l’entità dell’errore è evidenziata graficamente in per mezzo di una colorbar tarata in metri.

(41)

Figura 22: Localizzazione valutata con L’EKF senza feedback da SLAM Le prestazioni del filtro deteriorano rapidamente fin dai primi secondi della simulazione a causa della deriva di cui soffre la localizzazione da odo-metria. Nella Figura 23 `e riportato l’andamento dell’errore lungo il giro di pista.

Figura 23: Errore di localizzazione dell’EKF senza feedback da SLAM Seppur affetta da errore di deriva, la stima odometrica alla fine di un giro su un tracciato lungo circa 2km localizza la vettura in un punto distante 8m dalla posizione reale della vettura.

(42)

Si valutano dunque le prestazioni dell’EKF fornendogli le misure non inerzia-li. La localizzazione SLAM fornita `e affetta da errore di covarianza unitaria e valor medio superiore a 1m, come riportato in Figura 24.

Figura 24: Errore della localizzazione SLAM fornita all’EKF

Il filtro EKF fornisce in simulazione i risultati riportati nella Figura 25.

Figura 25: Localizzazione valutata con L’EKF con feedback SLAM Il filtro di Kalman riesce a migliorare la localizzazione SLAM utilizzando le informazioni del modello della vettura e le misure provenienti dai

(43)

senso-ri inerziali. L’errore medio di localizzazione senso-rispetto alla misura del GPS migliora del 70%, come mostrato nella Figura 26.

Figura 26: Errore di localizzazione dell’EKF con feedback da SLAM Inoltre si passa da una stima di localizzazione, quella SLAM, aggiornata a passo 0.04s, ad una stima aggiornata a passo 0.004s. L’aumento della banda del segnale di stima di localizzazione va a vantaggio dell’azione di control-lo, che usa appunto la posizione e l’orientazione del veicolo come principali segnali di feedback.

(44)

5 Controllo

Il problema del controllo in ambito automotive è stato affrontato in lettera-tura spesso separando il controllo di sterzo da quello di velocità. Si parla di Lane assist e di Cruise control : il primo è il sistema che corregge lo sterzo quando la vettura si accinge a superare i limiti della corsia di percorrenza, il secondo invece regola la velocità della vettura ad un valore preimpostato. I sistemi di Lane assist si basano perlopiù su sistemi di controllo che valuta-no l’angolo di sterzo a partire da considerazioni geometriche e regolavaluta-no per mezzo di controllore PID l’errore di direzione rispetto ad un look ahead point [9] localizzato sulla traiettoria di riferimento o lo scostamento laterale del baricentro della vettura dalla traiettoria di riferimento [10], [11].

I sistemi di Cruise control pi`u moderni consentono di regolare la velocit`a del veicolo e allo stesso tempo la distanza dal veicolo che precede: si parla di ACC (Adaptive Cruise Control) [12]. Da qualche anno sono in commercio sistemi di controllo predittivo che basandosi sul modello del punto materiale valutano l’accelerazione o decelerazione richiesta per minimizzare il tracking distance error vincolando accelerazione e jerk della vettura in range che ga-rantiscano un buon confort di guida.

La sfida `e progettare controlli che tengano conto delle dinamiche pi`u comples-se della vettura e siano in grado di gestire situazioni di sottosterzo e sovra-sterzo valutando un’azione combinata di sovra-sterzo e accelerazione/decelerazione per condurre il veicolo su traiettorie veloci o controllarlo in sicurezza nel caso in cui si debbano evitare ostacoli improvvisi o fronteggiare imprevisti (asfalto bagnato, pneumatici usurati, fino eventualmente a considerare urti).

Nel seguito saranno presentate una tecnica di controllo classica che disaccop-pia regolazione di sterzo e di velocit`a ed una tecnica di controllo predittivo model-based basata sul modello monotraccia descritto dalle (13), (14) che valuta contemporaneamente angolo di sterzo e richiesta di forza longitudi-nale. Entrambe le tecniche sono state implementate e testate sulla unit`a di calcolo real-time disponibile sulla vettura.

5.1 Il controllo in Tazio

La parte del controllo di Tazio si occupa di fornire alla vettura gli input di controllo pi`u opportuni per far s`ı che la vettura insegua la traiettoria di riferimento prestabilita con legge oraria p_ref(τ ) e profilo di velocit`a vref(τ )

(45)

p_ref(τ ) = (

xref(τ ) ascissa di riferimento nel frame mappa

yref(τ ) ordinata di riferimento nel frame mappa

. (23) xmap y_map p_ref(τ ) xG(t) yG(t) ψ(t) p_car(t) xref(ˆτ ) yref(ˆτ ) p_ref(ˆτ )

Figura 27: Traiettoria di riferimento.

Gli ingressi a disposizione dei Team per il controllo della vettura sono due, u (t) = ( δreq(t) FT req(t) (24) dove nella (24):

• δreq `e l’angolo di sterzo richiesto alle ruote anteriori;

• FT req `e la forza longitudinale richiesta; se positiva `e da intendersi

co-me forza di spinta richiesta ai motori elettrici, se negativa coco-me forza frenante richiesta all’impianto frenante.

Il secondo ingresso controlla dunque sia i motori che i freni della vettura e impone che ad ogni istante sia richiesto l’intervento o della power unit o dell’impianto frenante con impossibilit`a dell’uso di entrambe le azioni con-temporanemente. La configurazione degli ingressi facilita l’implementazione del controllo model-based: se FT req > 0 il modello di riferimento `e il (13), se

(46)

˙ x = f (x, u) =       

Φacc(x, uacc) , uacc= δreq FT req

T

, se FT req> 0

Φbra(x, ubra) , ubra = δreq |FT req|

T

, se FT req < 0

(25) Nella (25) il vettore di stato x, come definito nella sottosezione 2.2, include 6 variabili di stato: x(t) =                    p_car(t) = (

xG(t) ascissa del baricentro nel frame inerziale

yG(t) ordinata del baricentro nel frame inerziale

ψ(t) angolo di imbardata

u(t) velocit`a di avanzamento longitudinale nel frame locale

v(t) velocit`a laterale nel frame locale

r(t) velocit`a rotazionale di imbardata

. (26)

Supponendo di voler controllare la posizione della vettura e la sua velocit`a di avanzamento longitudinale, si pone il vettore delle variabili di uscita come:

y (t) = g (x(t)) = " p_car(t) u(t) # . (27)

In Figura 28 si mostrano graficamente le variabili di stato del modello di controllo. y_car xcar δ(t) xmap y_map p_ref(τ ) xG(t) yG(t) u(t) r(t) v(t) ψ(t) Vcar

(47)

5.1.1 Struttura del layer di controllo

Il layer del controllo ha una struttura di base che resta invariata a prescindere dalla tecnica di controllo utilizzata, illustrata in Figura 29.

RHG Control EKF ymeas_(t) _ˆ_{x (t)} p_ref(τ ) vref(τ ) [phor(ˆτ ) vhor(ˆτ )] δreq(t) FT req(t) u(t)

Figura 29: Layer di controllo del software Tazio. Le misure ymeas_{(t) assieme}

ai controlli valutati al passo precedente sono usati dall’EKF per ottenere lo stato stimato del sistema ˆx(t). Questo `e usato sia dall’RHG assieme alla p_ref(τ ) per valutare la traiettoria desiderata lungo l’orizzonte di predizione p_hor(ˆτ ), che dal controllore assieme alla stessa p_hor(ˆτ ) per valutare i controlli futuri u(t).

Ad ogni passo il controllore ha bisogno di un riferimento da seguire: nel caso di un controllore che insegue un look-ahead point il riferimento è sem-plicemente un punto in avanti sulla traiettoria ed un riferimento di velocità; nel caso di un controllore predittivo è necessaria una legge oraria che fornisca informazioni sulla posizione e sulla velocità desiderata nel prossimo orizzonte temporale. Questo è necessario dal momento in cui non è garantito che la vettura insegua la legge oraria prestabilita: nel caso in cui la vettura si ritrovi più in avanti o più indietro del previsto a causa di disturbi esterni o dinami-che non modellate è necessario aggiornare il look-ahead point o rischedulare il riferimento nell’orizzonte temporale.

Il modulo che valuta il riferimento da fornire al controllo `e il Reference Ho-rizon Generator (RHG).

(48)

xmap y_map p_ref(τ0) p_ref(τend) p_ref(τ ) ˆ p_car(t)

Figura 30: Porzione di traiettoria fornita al controllore dal RHG Il modulo RHG cerca sulla traiettoria p_ref(τ ) il punto pi`u vicino alla posizione stimata corrente ˆp_car(t): in Figura 30 `e il punto p_ref(τ0).

Le istruzioni eseguite nell’RHG hanno dunque come obiettivo la ricerca della coordinata τ0 sulla traiettoria tale per cui:

τ0 = arg min τ

p_ref(τ ) − ˆp_car(t)

2 . (28)

Il punto p_ref(τ0) `e il punto in cui si troverebbe la vettura se all’istante

cor-rente non si commettesse errore di tracking, e costituisce il punto di partenza per i riferimenti futuri. Scegliendo un orrizzonte temporale Thor, la porzione

di traiettoria che si desidera far seguire alla vettura a partire dal passo cor-rente `e p_hor(ˆτ ): τend = τ0+ Thor ( p_hor(ˆτ ) = p_ref(τ − τ0) vhor(ˆτ ) = vref(τ − τ0) , τ ∈ [τ0, τend] ˆ τ ∈ [0, Thor] . (29)

Il modulo di controllo utilizza lo stato stimato dall’EKF e la traiettoria de-siderata al passo corrente [p_hor(ˆτ ) vhor(ˆτ )] per valutare gli ingressi correnti

(49)

5.2 Controllo disaccoppiato (Pure Pursuit )

La prima strategia di controllo implementata è quella proposta in [9] sotto il nome di Pure Pursuit. La legge di sterzo è una legge geometrica che porta la vettura dal allinearsi lungo la direzione che punta un look-ahead point scelto sulla traiettoria di riferimento a distanza opportuna. L’angolo di sterzo desiderato è determinato usando l’angolo α (Figura 31) compreso tra l’angolo di imbardata del veicolo e l’angolo del vettore look-ahead (vettore che unisce il centro dell’asse posteriore del veicolo al look-ahead point). La legge di sterzo usata è descritta dalla (30).

δreq(t) = arctan

2 La sin(α)

kp_hor(Tahead) − ˆpcar(t)k

α = arctan yhor(Tahead) − ˆyG(t) xhor(Tahead) − ˆxG(t)

− ˆψ(t)

(30)

Figura 31: Geometria Pure Pursuit.

La distanza del look-ahead point `e un importante parametro progettuale. Se si sceglie piccola il controllo Pure Pursuit forza il veicolo a seguire la

(50)

traiettoria con più precisione e aumenta il valore massimo dell’angolo di sterzo richiesto; tuttavia un look-ahead point troppo vicino al veicolo può essere causa di oscillazioni ed instabilità. Se si aumenta la distanza del look-ahead point invece, la richiesta di sterzo perde la sua caratteristica oscillatoria e la vettura inizia a sterzare prima che raggiunga la curva seguendo traiettorie più dolci a scapito dell’accuratezza del tracking. Allontanando troppo look-ahead point si verifica infatti il fenomeno del cutting corner come mostrato in Figura 32.

Figura 32: Il fenomeno del cutting corner nel Pure Pursuit.

In [9] il look-ahead point `e scelto a distanza variabile con la velocit`a come descritto nella (31).

dahead = 0.5 Vcar[km/h] = 1.8 Vcar[m/s] (31)

Ovvero, se consideriamo il look-ahead point come un punto viaggiante con velocit`a Vcar, questo anticipa la vettura di 1.8s. Ci`o corrisponde a

sceglie-re come look-ahead point il punto p_hor(1.8) della traiettoria fornita dal RHG. Per il calcolo dell’input FT req si impiega un controllore PD che regola

l’errore corrente di velocit`a (32).

FT req(t) = KP(ev(t)) + KD de_v(t) dt ev(t) = vhor(0) − ˆu(t) (32)

(51)

Per la valutazione dell’errore corrente ev(t) si utilizza vhor(0), ovvero il

pri-mo valore del profilo di velocit`a desiderato nell’orizzonte di predizione che corrisponde appunto al valore desiderato al passo corrente.

5.3 Controllo predittivo model-based

La seconda strategia di controllo implementa un controllore predittivo (MPC, Model Predictive Control ). Il controllo predittivo `e basato su un concetto semplice: usare il modello del sistema per predirne la dinamica lungo un orizzonte temporale e valutare la sequenza di controlli lungo l’orizzonte che minimizzi lo scarto tra le uscite predette e le uscite desiderate (Figura 33).

Figura 33: Il controllo predittivo [13].

In generale, sia dato il sistema discreto non lineare descritto dalle funzioni f (·), g(·):

xk+1 = f (xk, uk)

y_k = g(xk, uk)

. Il controllore MPC ad ogni istante t:

(52)

• utilizza le misure correnti del sistema y0 = ymeas(t) per ottenere una

stima dello stato corrente x0 = ˆx(t);

• risolve il seguente problema di ottimizzazione vincolato rispetto alla sequenza di controlli U = {u0, u1, . . . , uM −1}: U∗ = arg min U J, J = N −1 X k=1 kWy(y_k− r(t))k2₂+ N −1 X k=0 kWu ukk2₂ Vincoli:    xk+1 = f (xk, uk) y_k = g(xk, uk) con uk k > M −1 = uM −1    umin < uk < umax y_min < y_k < y_max x0 = ˆx(t)

• fornisce in input al sistema il primo valore della sequenza ottima U∗

, u(t) = u∗₀, scartando gli altri elementi della sequenza (Receding Hori-zon).

Il parametro P fissa l’orizzonte di predizione e indica il numero di uscite future valutate dal controllore nella procedura di ottimizzazione. Riducendo P aumenta l’aggressivit`a dell’azione di controllo.

Il parametro M fissa l’orizzonte di controllo, ovvero il numero di variabili libere di controllo. Se si aumenta M aumentano esponenzialmente le pos-sibili combinazioni di sequenze di controlli U tra cui cercare quella ottima. Un aumento di M va a beneficio dell’ottimalit`a dell’azione di controllo ma al prezzo di un aumento del costo computazionale. Tipicamente M `e scelto nel range 1 ÷ 5.

La matrice peso Wy _{pesa gli errori dei valori di uscita predetti rispetto ai}

valori desiderati, mentre la matrice Wu _{pesa le azioni del controllo: tanto}

pi`u i valori di Wy sono grandi rispetto ai valori di Wu tanto pi`u aggressiva `

e l’azione del controllo.

Un altro fattore importante in fase di progetto `e la scelta del modello: il modello del sistema da controllare deve essere sufficientemente complesso da descriverne le dinamiche pi`u significative, ma al contempo semplice abba-stanza da poter essere adoperato in real-time data la potenza computazionale dell’hardware a disposizione.

(53)

5.3.1 Real-Time MPC in Tazio

Il controllo predittivo utilizzato in Tazio richiama ad ogni passo Ts un

al-goritmo di ottimizzazione non lineare che ricerca la sequenza di M controlli U = {u1, u2, . . . uM} in grado di minimizzare gli scarti tra la predizione

delle uscite nell’orizzonte ed i riferimenti.

L’onerosit`a computazionale dell’algoritmo di ottimizzazione e la complessit`a del modello hanno reso impossibile per l’utilizzo del controllore in real-time utilizzare come passo temporale per la predizione delle uscite lo stesso passo Ts= 0.004s con cui sono aggiornate le variabili di misura e con cui si richiede

siano aggiornate le variabili di controllo. Un aumento del passo Ts avrebbe

causato perdita di informazioni dei sensori e una minore “prontezza” dell’a-zione di controllo; di contro se si fosse utilizzato Ts come passo di predizione

sarebbe stato necessario porre P = 125 per estendere l’orizzonte temporale a 0.5s, che si è verificato empiricamente essere il limite accettabile per avere garanzie di stabilità. Si è scelto quindi di iterare la procedura di controllo a passo Ts, ma di utilizzare come passo di predizione TMPC = 0.04s. La

scelta ha consentito di porre P = 16 per estendere l’orizzonte di predizione a Thor = 0.64s al prezzo di un aumento dell’errore di predizione della dinamica

di sistema.

Per quanto di mia conoscenza, rispetto ai lavori trovati in letteratura [14] [15] [16], l’originalità del metodo proposto circa l’implementazione dell’MPC è la collocazione lungo l’orizzonte di predizione delle variabili libere di controllo. Si parla di orizzonte di controllo, con riferimento alla ??, in quanto le varia-bili libere sono collocate in corrispondenza dei primi M passi (dove M < P ) appena successivi al passo corrente, lasciando le successive P − M variabili di controllo bloccate al valore assunto dall’ultima variabile libera. La stra-tegia è opportuna se i riferimenti restano perlopiù costanti lungo l’orizzonte di predizione, e si vuole portare il sistema in una condizione di regime; nel caso di Tazio è importante che la legge di controllo abbia libertà di azione lungo tutto l’orizzonte per valutare l’istante esatto del punto di staccata o l’istante di sterzata in una chicane. Allo scopo di raggiungere tale obiettivo si è scelto di localizzare la variabili libere di controllo equidistanti lungo tutto l’orizzonte di predizione, come illustrati in Figura 34. Data una scelta M del numero di variabili libere, si sceglie Tcontrol = P TMPC/(M − 1), e fissando la

prima al controllo corrente si distribuiscono le restanti M − 1 variabili libere a passo Tcontrol.

(54)

Figura 34: Esempio di localizzazione delle variabili libere nell’orizzonte di predizione nel caso P = 16, M = 3, TMPC = 1. In A: i riferimenti. In

B: le 3 variabili libere di controllo sono distribuite lungo l’orizzonte a passo TControl= P TMPC/(M − 1) = 8. In C: la sequenza di 16 controlli uk necessari

per valutare la predizione `e ottenuta per interpolazione lineare. In D: le uscite del sistema lungo l’orizzonte sono predette simulando il modello a partire dallo stato corrente x0 e imponendo lungo l’orizzonte i controlli uk.

La sequenza di variabili di controllo ˆU = {uk} lungo l’orizzonte di

predi-zione, necessaria per predire la dinamica del sistema, `e ricostruita per mezzo di una interpolazione lineare.

Gli algoritmi di ottimizzazione non lineare non garantiscono in generale la convergenza al minimo globale: la soluzione restituita dipende fortemente dalla sequenza di controlli di partenza U0. Si `e ritenuto quindi opportuno

esplorare in un primo momento lo spazio delle sequenze di controllo a valori costanti U per valutare una U0 adeguata (warm start solution). Lo spazio

U disponibile, ovvero quello che ha per valori i controlli compresi tra umin

e umax, `e campionato uniformemente e il funzionale di costo J `e valutato

su tutti i campioni. Il campione U∗ che minimizza il funzionale `e utilizzato come sequenza di controlli di partenza. La procedura, che differisce dalla

(55)

me-todologia standard (che utilizza come sequenza di partenza per il problema di ottimizzazione parte della sequenza ottima ottenuta al passo precendente) `

e pensata per risolvere il problema dello stallo in partenza e per controllare la vettura in situazioni in cui `e necessaria una brusca variazione dei controlli a causa di improvvisa perdita di aderenza degli pneumatici.

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

y_k = 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:

p_hor(ˆτ )|_ˆ_{τ =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.

(56)

Ricampionamento dei riferimenti

I riferimenti forniti dal RHG a passo Ts sono ricampionati a passo TMPC.

rdes_k =     p_hor(ˆτ )|_{τ =kT}_ˆ MPC vhor(ˆτ )|_ˆ_{τ =kT}_MPC     , ˆ τ = {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:

(57)

• 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 = { ûk} = { û0, û1, . . . ûP −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 rdes_k − g (xk) 2 2

Al termine della procedura, il controllore restituisce in uscita il primo valore della sequenza ottima U∗, u(t) = u∗₀.