Controllo Ottimo per Veicoli Robotici da Corsa

Universit`a di Pisa

Facolt`

a di Ingegneria

Corso

di

Laurea

Specialistica

in

Ingegneria

dell’Automazione

Tesi di laurea specialistica

Controllo Ottimo in Tempo Reale

per Veicoli Robotici da Corsa

Candidato:

Alessio Salvatore Coppola

Matricola 407295

Relatore:

Prof.ssa Lucia Pallottino Correlatore:

Sommario

Nella presente tesi viene discusso il problema di controllo ottimo in tempo reale per una macchina robotizzata da corsa, con la finalità di riuscire a percorrere un tracciato nel minor tempo possibile.

In primo luogo, vengono identificate la sequenza di manovre ottime che permettono alla macchina di completare ogni settore del percorso con tempo minimo: inizialmente vengono studiate le manovre elementari che soddisfano le condizioni di ottimalità del Principio del Minimo di Pontryagin, sviluppando un alfabeto dei casi possibili; in se-condo luogo, si cerca la sequenza di manovre elementari che permetta di attraversare ogni porzione di circuito nel minor tempo possibile, al variare della posizione assunta dalla macchina all’interno del tracciato. In secondo fase, affrontiamo il problema della pianificazione globale della traiettoria congiungendo tutte le manovre ammissibili che attraversano ogni settore del circuito, attraverso la costruzione di un grafo orientato che può essere interrogato durante la guida della macchina, in tempo reale, per scegliere i comandi di controllo coerenti con le manovre ottime studiate. A supporto di quan-to esposquan-to analiticamente, vengono forniti risultati numerici tramite il software per la risoluzione di problemi di controllo ottimo GPOPS−II , su Matlab , per rettilinei e curve generiche, e dopo anche su un circuito definito nella tesi stessa.

Infine, i risultati numerici ottenuti con GPOPS−II sul circuito completo sono utilizzati per controllare una macchina del simulatore di guida TORCS in tempo reale.

In this work we discussed the problem of optimal control in real time for robotic racing car, with the aim of being able to complete a track in the shortest possible time.

Abstract

First, we identified optimal maneuvers sequence that allow the car to complete each section of the course with minimum time: initially we studied elementary maneuvers that satisfy the Pontryagin Minimum, developing an alphabet of possible cases; sec-ondly, we search the sequence of elementary maneuvers that allows to cross any portion of the circuit in the shortest possible time, to vary the position assumed the car inside the track. In the second phase, we address the problem of the global planning of the trajectory, joining all feasible maneuvers that cross each sector of the circuit, through the construction of a directed graph that can be queried during driving the car, in real time, to choose control commands consistent with the optimal maneuvers studied. To support of the above, we provided numerical results using a software for solving opti-mal control problems GPOPS-II, on Matlab, for generic straight and curves, and then also on a circuit described in the thesis.

Finally, we used the numerical results obtained with GPOPS-II on full circuit to control a car on the driving simulator TORCS, in real time.

Sintesi tesi di laurea Coppola

Il presente studio riguarda il controllo ottimo applicato alla guida di una macchina robotica da corsa. Partendo dal lavoro in [1] e [2], abbiamo applicato il controllo ottimo per ricavare le manovre elementari che una vettura può effettuare per produrre una traiettoria che minimizza il tempo di percorrenza. Inizialmente, abbiamo sviluppato uno studio locale dei settori che comprendono il circuito e successivamente abbiamo raccolto le informazioni su un grafo, in modo da poter effettuare la guida in maniera semplice ed efficace, con tempi di risposta che permettano un controllo real-time sui comandi della macchina.

Inizialmente è stato introdotto il modello cinematico della macchina funzionale al no-stro scopo, e che comprenda gli effetti dinamici più rilevanti [3]. Così viene ricavato il modello cinematico di un veicolo car-like da un modello dinamico generale, pre-vedendo: variabili di stato posizione, direzione e velocità longitudinale della vettura; variabili di controllo un angolo di sterzata ed un’accelerazione di comando.

 ˙ x = vxcosϑ − vysinϑ ˙ y = vxsinϑ + vycosϑ      ˙

vx=Fxr+Fxfcosϕ−Fm yfsinϕ+ ˙ϑ vy−Faerom

˙ vy=Fyr+Fxfsinmϕ+Fyfcosϕ− ˙ϑ vx ¨ϑ =lf(Fxfsinϕ+Fyfcosϕ)−lrFyr Iz      ˙ x = v cos ϑ ˙ y = v sin ϑ ˙ϑ =v Ltanϕ ˙ v = av− b v

Gli ingressi di controllo sono limitati. Inoltre, viene tenuto conto di un vincolo sul-l’accelerazione laterale massima che limita la velocità ed il raggio di curvatura du-rante la fase di sterzata. Il vincolo sull’accelerazione laterale massima impedisce il ribaltamento del veicolo ed è definito nella relazione seguente:

v2tanϕ 6 alL

In generale, per macchine normalial= µsg, ma nel caso di auto da corsa

l’accele-razione laterale può raggiungere valori maggiori perché viene utilizzato l’effetto-suolo aerodinamico che produce una forza di deportanza in grado di aumentare l’aderenza con il terreno: così è possibile definire l’accelerazione laterale in relazione di questa forza,al= µmax

 g +FL

m



; da ciò è possibile ottenere una velocità in curva maggiore (v 6√alR), migliorando le prestazioni dell’intero tracciato [4].

Infine, vengono fornite anche alcune possibilità di scelta dei parametri che regolano la dinamica della velocità in modo da raggiungere determinate prestazioni in gara. Nel problema di controllo ottimo si tiene conto dei vincoli sulle variabili di stato e controllo, ricercando le manovre elementari che soddisfino le condizioni di ottimalità del Principio del Minimo di Pontryagin e fornendo alla fine un alfabeto delle manovre identificate. Si utilizza la funzione Hamiltoniana per ricavare gli estremali di ottimalità che dipendono dall’attivazione dei vincoli considerati (vedi Tabella 1), e che forniscono l’alfabeto delle manovre elementari (vedi Figura 1), e le tipologie di manovre previste. Il problema di controllo ottimo per la pianificazione locale della traiettoria è:

min av(t),ϕ(t) ZT 0 dt, soggetto a av(t) ∈ [amin,amax] ϕ(t) ∈ [−ϕmax,ϕmax]  v2_{(t) tan ϕ}  6a_lL

Tabella 1. Casi estratti dalla risoluzione del problema di controllo ottimo TRATTOT ˙ v 6= 0 av= b v + altan (ϑ − γ) ϕ = tan−1 alL v2 R = v2 al TRATTOT ˙ v 6= 0 av= amax(o amin) ϕ = tan−1 alL v2 R = v2 al TRATTOC ˙ v = 0 av= b v ϕ = ±ϕmax R =_tanϕL = Rmin TRATTOS ˙ v = 0 av= b v ϕ = 0 R = ∞ TRATTOS ˙ v 6= 0 av= amax(o amin) ϕ = 0 R = ∞

La manovra ottima è dunque una sequenza di:

• rettilinei, percorsi con accelerazione (o decelerazione) di comando massima, o con velocità costante;

• archi di cerchio, attraversati con raggio di curvatura costante, pari a quello mini-mo, e accelerazione laterale massima;

• raccordi di transizione, percorsi con accelerazione laterale massima, o accelera-zione (o deceleraaccelera-zione) di comando massima.

Figura 2. Rappresentazione di settori adiacenti

L’approccio seguito prevede il passaggio dalla pianificazione locale del moto a quella glo-bale attraverso l’utilizzo di una metodologia di discretizzazione della pista e la costruzione di un grafo orientato. Il circuito viene diviso in settori, i quali prevedo-no alla frontiera una discretizza-zione delle posizioni e velocità che la macchina può assumere come stato iniziale o fi-nale sul tratto locale: ogni posizione fifi-nale del settore corrente è la posizione fifi-nale del settore successivo; le posizioni iniziali del settore corrente invece, sono identiche alle posizioni finali del settore precedente; così facendo, ogni settore prevede che il proprio spazio di inizio e fine sia partizionato in un numero finito di celle, come mostrato in Figura 2, che associamo ai nodi di un grafo.

All’interno del circuito, due nodi sono tra di loro connessi se nel tracciato si trovano su settori adiacenti, e se esiste una sequenza di manovre ammissibile, generata dal-l’alfabeto di Figura 1, in modo tale che il veicolo sia in grado di muoversi dalla cella iniziale a quella finale. Ogni arco del grafo prevede un costo che indica il tempo neces-sario alla macchina per passare da una cella all’altra seguendo la sequenza di manovre ammissibile. Il percorso a costo minimo che congiunge una cella iniziale con un’altra finale sull’intero tracciato rappresenta la soluzione del problema di pianificazione glo-bale della traiettoria: la cella iniziale è la posizione che la macchina possiede all’inizio (assieme alla velocità), mentre il nodo finale del grafo rappresenta la posizione e la velocità che il veicolo raggiunge seguendo la sequenza di manovre ottime che permet-tono di terminare il giro; infine, il costo che congiunge il nodo iniziale a quello finale

Figura 1. Diagramma di flusso decisionale delle soluzioni ottime

indica il tempo speso per completare il giro del circuito. Un approccio di questo tipo, ottenuto discretizzando i casi di entrata ed uscita da ogni settore in un numero finito di casi e sviluppando un grafo che connette tutte le posizioni che prevedono una sequen-za di manovre ammissibile, permette di eseguire un controllo ottimo della traiettoria sulla macchina in tempo reale: infatti, una volta determinate le soluzioni ottime che esistono tra le celle del circuito in maniera off-line, è sufficiente costruire il grafo che, tramite una ricerca della posizione ottima più vicina a quella che la macchina assume in ogni istante, fornisca in tempo reale la sequenza di manovre ottime che permetto-no di completare la gara nel mipermetto-nor tempo possibile; la pianificazione globale diventa un semplice problema di cammino minimo, ed il grafo può essere interrogato con i tradizionali algoritmi di ricerca noti in letteratura (es: Dijkstra e Bellman-Ford).

Figura 3. Curva a raggio

massimo costante

Dopo una breve introduzione alla libreria GPOPS−II [5], si comincia a fornire alcune prove preliminari che mo-strano l’attivazione dei vincoli durante la percorrenza di una generica curva, per cominciare a produrre le prime considerazioni sulla pianificazione locale su una curva. Successivamente, viene sviluppato uno studio prelimina-re sullo spazio di percorprelimina-renza minimo per una generica curva introducendo il concetto di curva a raggio massi-mo costante, e determinando la lunghezza di un generico tratto rettilineo da affiancare all’entrata e all’uscita della curva per ottenere la velocità di percorrenza massima co-stante: in altre parole, il raggio di curvatura massimo che permette alla macchina di percorrere la curva con

veloci-tà massima costante, passando per il punto di tangenza al bordo interno della curva, partendo e terminando vicino al bordo esterno della porzione di circuito considerato, fornisce la distanza dalla curva necessaria al veicolo per raggiungere la velocità massi-ma costante. Così vengono modificati i settori curvilinei in curve speciali, composte da un “breve” rettilineo iniziale e finale in relazione alla velocità massima permessa nella curva considerata e all’insieme finito di possibili velocità di ingresso e di uscita defi-nito. A fine capitolo, vengono proposti alcune prove numeriche su un tratto di strada

composto da una curva compresa tra due lunghi rettilinei: il tracciato viene diviso in tre settori, cambiando il punto di inizio e fine della curva, vengono studiate le diverse ti-pologie di curve speciali, chiamate da noi curve candidate, attraverso la comparazione dei risultati ottenuti in termini di numero di soluzioni ammissibili.

0 33 66 99 132165 9 11.8 14.6 17.4 20.2 23 Coordinata X [m] 0 33 66 99 132165 −135 203.8 542.6 881.4 1220.2 1559 Coordinata Y [m] 0 33 66 99 132165 89 89.4 89.8 90.2 90.6 91 Orientazione Θ [gradi] 0 33 66 99 132165 0 77.2 154.4 231.6 308.8 386 Velocità [km/h] 0 33 66 99 132165 −26 −15.6 −5.2 5.2 15.6 26 Accelerazione [m/s 2] 0 33 66 99 132165 −5 −3 −1 1 3 5

Angolo di Sterzo [gradi]

0 33 66 99 132165 0 10 20 30 40 50

vincolo di non ribaltamento

9 11.814.617.420.223 −135 203.8 542.6 881.4 1220.2 1559 Coordinata X Coordinata Y

Figura 4. Andamento delle variabili di stato e controllo nel tempo, perly= ∆ymin

Il passaggio successivo nella pianificazione locale prevede lo studio delle tipologie di manovre elementari che producono una traiettoria ammissibile in un tratto di strada rettilineo. Dapprima sono studiate le manovre eseguite a velocità costante, identifi-cando i parametri che permettono di spostarsi dalla posizione iniziale a quella finale in termini di rotazione e traslazione. In secondo luogo viene fatta la stessa analisi per una macchina che si muove a velocità variabile, tenendo conto della legge oraria che definisce il moto relativo alla dinamica della velocità definita nel Capitolo 1. Infine, vengono presentati dei risultati numerici ottenuti in ambiente Matlab con il software GPOPS_{−II , in merito alle traiettorie ammissibili, variando la lunghezza dei rettilinei,} e quindi la distanza tra la posizione iniziale e quella finale, per le diverse velocità previ-ste: qui, il numero di soluzioni ammissibili diminuisce quando il rapporto di manovra β (definito all’interno del capitolo) scende sotto la soglia critica βmin= ∆y_∆xmin

min; in

generale, la sequenza di manovre per un veicolo che comincia e termina la corsa a ve-locità diversa da quella massima, prevede un raccordo di transizione con accelerazione (o decelerazione) di comando e laterale massima sia all’inizio che alla fine, con in mez-zo un lungo tratto rettilineo percorso con accelerazione (o decelerazione) di comando massima; quando la velocità del veicolo si mantiene costante i raccordi di transizio-ne tendono ad archi di cerchio percorsi con raggio di curvatura costante, e i rettilitransizio-nei invece vengono percorsi con accelerazione di comando quasi massima. Nel caso di distanza longitudinale delle due posizioni iniziale e finalely= ∆ymintutte le

confi-gurazioni tra le celle definite producono una soluzione che raggiunge lavmax e che

arriva al punto finale con la velocità desiderata. In Figura 4 viene mostrata la soluzione trovata.

Nel Capitolo 5 vengono utilizzate le equazioni che regolano il moto del modello cine-matico descritto nel Capitolo 1 per fornire un’espressione analitica al tempo di percor-renza necessario per percorrere un generico settore del circuito, sia esso un rettilineo che una curva: sono esaminati tutti i casi possibili di entrata ed uscita dal settore e vengono forniti quindi i tempi minimi che servono per percorrere una certa distanza nel tratto di strada considerato, con qualsiasi tipo di velocità iniziale e finale.

Una volta che sono stati forniti alcuni strumenti utili alla pianificazione locale per sem-plici tratti di stada, si comincia ad applicare le considerazioni fatte nelle pagine

denti su un tracciato intero, completando lo studio con la pianificazione globale:preso come riferimento il circuito Sepang del Gran Premio della Malesia di Formula 1, ab-biamo costruito un circuito composto dalla sua metà superiore e lo abab-biamo chiamato Sepang-like. −700 −600 −500 −400 −300 −200 −100 0 100 200 300 400 500 100 200 300 400 500 600 700 SEPANG−LIKE CIRCUIT START = END

Figura 5. Circuito Sepang-like

Il tracciato viene diviso in settori rettilinei e archi di cerchio a cui viene applicata la pianificazione locale della traiettoria ricavando una prima pianificazione globale.

Figura 6. Diagramma di flusso dell’approccio seguito per ricavare la soluzione finale

Successivamente, vengono ricercate le curve candidate più adatte a sostituire le cur-ve tradizionali con lo scopo di migliorare le prestazioni in gara, e cur-vengono quindi prodotti i risultati numerici su alcuni test effettuati: così facendo il numero di settori diminuisce rispetto quello iniziale perchè i rettilinei molto corti compresi tra due curve risulta conveniente aggiungerli alle curve stesse, secondo i risultati ottenuti dalle prove effettuate.

I dati relativi alle prove effettuate per differenti scelte di curve candidate sono mostrati nella Tabella 2.

Tabella 2. Informazioni sui tempi di percorrenza per giro trovati nei test effettuati con il circuito Sepang-like adattato C∗ 6 C ∗ 8 C ∗ 11 Tempo speso nel giro 1 [secondi] Tempo speso nel giro 2 [secondi] Tempo speso nel giro 3 [secondi] (3 fy, 3fy) (2 fy, 0) (0, 3 fy) 48.2912 46.2201 46.2177 (3 fy, 3fy) (fy, 0) (0, 3 fy) 48.3699 46.2988 46.2964 (3 fy,fy) (3 fy, 0) (0, 3 fy) 49.1394 47.0682 47.0658 (3 fy, 2fy) (3 fy, 0) (0, 3 fy) 48.9879 46.9168 46.9144 (3 fy, 3fy) (2+fy, 0) (0, 3 fy) 48.2641 46.1929 46.1906 (3 fy, 3 fy) (2+fy, 0) (0+, 2 fy) 47.4485 45.3773 45.3750

−700 −600 −500 −400 −300 −200 −100 0 100 200 300 400 500 100 200 300 400 500 600 700 Coordinata X Coordinata Y

Figura 7. Percorso nel tracciato partendo al centro della pista

0 1 2 3 4 5 6 7 8 9 10 0 5 10 15 20 25 30 35 40 45 50 SETTORE

TEMPO DI PERCORRENZA [sec]

GIRO 1 GIRO 2 GIRO 3 0 1 2 3 4 5 6 7 8 9 10 50 100 150 200 250 300 350 SETTORE VELOCITA‘ [km/h] GIRO 1 GIRO 2 GIRO 3

Figura 8. Confronto tra le i tempi di percorrenza e le velocità nei primi tre giri

I dati forniti da GPOPS−II sono utilizzati per fornire le sequenze di manovre che identificano le traiettorie ammissibili di ogni settore e della soluzione a tempo minimo per l’intero circuito.

Nelle prove numeriche effettuate è stato impostoamax= b pvvmax(conpv> 1) e

quindi viene introdotto un estremo superiore alla macchina in maniera diretta come non è stato fatto invece nel Capitolo 2 allo scopo di avere la possibilità di raggiungere la velocità massima in un tempo finito: di ottiene il diagramma decisionale mostrato in Figura 9.

Raccogliendo le considerazioni fatte sulle sequenze ottenute dalle manovre relative alle prove effettuate, si ottiene lo schema mostrato nella Tabella 3: nel riassumere i dati relativi alle sequenze ottenute, non sono state fatte distinzioni tra casi maiuscoli e minuscoli, ovvero non si è tenuto conto se il veicolo si muove con accelerazione laterale massima oppure no.

Figura 9. Diagramma di flusso decisionale

Tabella 3. Sequenze di manovre ottime più frequenti

Settore Velocità bassa Velocità crescente

Velocità alta Velocità

massima 1 {Ti,S, Ti}1 {Ti,S, Ti}1 {Ti,S, Ti}1 {Ci,S, Ci}1 2 {TR} {TR} - -3 {CL,TL,CL} - - -4 {TR,S, TR,S, TR} {TR,S, TR,S, TR} {TR,TL} -5 {Ti,S, Ti}1 {Ti,S, Ti}1 {Ti,S, Ti}1 {Ci,S, Ci}1 6 {TL,S, TR,S, TL} {TL,TR,TL} - -7 {Tj,S, TL,S, Tj}2 {S, TL} o {TR,TL} - -8 {TL,S, TR,TL} {TR} o {TR,TL} - -9 {Tj,S, TR,S, Tj}2 {TR,S, TL} - -10 {Ti,S, Ti}1 {Ti,S, Ti}1 {Ti,S, Ti}1 {Ci,S, Ci}1

Infine, sono utilizzati i dati forniti da GPOPS−II sulla soluzione ritenuta migliore per eseguire un controllo in tempo reale di una macchina nell’ambiente di simulazione 3D TORCS [6] affinché possa essere interrogato il grafo rappresentante le traiettorie possibili per eseguire la pianificazione globale della traiettoria su tutto il tracciato e fornire quindi i comandi necessari alla macchina per inseguire la traiettoria ottima più vicina alla posizione nel circuito.

Vengono sviluppate le leggi di controllo del moto che, tramite lo studio di una candidata Lyapunov, permettano l’inseguimento della traiettoria desiderata: le leggi dedotte sono state provate in ambiente Matlab e poi applicate a TORCS con le dovute modifiche per ottenere dei risultati soddisfacenti nella guida di una vettura realistica che insegue la traiettoria desiderata.

Gli ingressi di controllo sono:

av= av,d+  b −Kv hv  ve ϕ = tan−1Lhy hϑρ sinγ γ + L cosγ ρ + LKhϑϑ γ v  (1)

1_{i ∈ {R, L} in base a dove si trova la posizione finale rispetto quella iniziale}

Figura 10. Diagramma di Flusso sulla scelta della traiettoria ottima con TORCS

Nella Figura 11 viene mostrato l’inseguimento della traiettoria ottima da parte della macchina. −800 −600 −400 −200 0 200 400 600 100 200 300 400 500 600 700 X Y

Figura 11. Inseguimento della traiettoria ottima, con legge di controllo 1

Su TORCS, utilizziamo il controllo sul moto seguente:

av= av,d+  b −Kv hv  ve ϕ = tan−1yd−y xd−x  − ϑ (2)

I risultati ottenuti sono mostrati in Figura 12, con il tempo speso nei primi dieci giri indicato nella Tabella 4.

Tabella 4. Informazioni sui tempi di percorrenza per giro trovati nella prova effettuata con TORCS

Giro Tempo speso Velocità massima [km/h]

1 49′′ 408 307.6 2 46′′ 010 307.6 3 45′′ 928 307.8 4 45′′_{896 307.9} 5 45′′_{894 308.2} 6 45′′_{978 308.2} 7 45′′ 880 308.4 8 45′′ 860 308.4 9 45′′ 856 308.4 10 45′′ 930 308.4

Ricapitolando, in questa tesi ci siamo concentrato sullo sviluppo di una procedura di pianificazione ottima globale da applicare ad una macchina durante la fase di guida:

−50 50 150 250 350 450 550 650 750 850 950 1050 1150 −50 50 150 250 350 450 550

Figura 12. Inseguimento della traiettoria ottima, su TORCS

si comincia con la pianificazione locale dei singoli settori del circuito, si sviluppa una struttura a grafo e viene applicata la pianificazione globale della sequenza di manovre ottime che permette di completare un giro nel minor tempo possibile. La memorizza-zione delle informazioni su un grafo permette di trovare il cammino minimo in maniera semplice ed efficace, consentendo una guida ottima in tempo reale.

L’approccio seguito può essere applicato anche a due o più veicoli presenti nel traccia-to, inserendo opportune strategie di sorpasso che possono ampliare il lavoro già svolto: lo scenario competitivo non è l’unico a cui questa strategia può essere applicata, ma più in generale, la pianificazione ottima globale sviluppata in questa tesi può essere appli-cata anche nel controllo in tempo reale di robot che si muovono su un generico percorso noto, dove è possibile introdurre anche un’opportuna strategia di elusione degli ostacoli presenti nell’ambiente, realizzando una pianificazione generale più completa.

Bibliografia

[1] T. Rizano, D. Fontanelli, L. Palopoli, L. Pallottino, and P. Salaris. Local motion planning for robotic race cars. In in Conference on Decision and Control (CDC), 2013.

[2] T. Rizano, D. Fontanelli, L. Palopoli, L. Pallottino, and P. Salaris. Global path planning for competitive robotic cars. In IEEE Conference on Decision and Control, Florence, Italy, 2013.

[3] G. M. Hoffmann, C. J. Tomlin, D. Montemerlo, and S. Thrun. Autonomous automobile trajectory trac-king for off-road driving: Controller design, experimental validation and racing. In American Control Conference, 2007. ACC ’07, pages 2296–2301, 2007.

[4] The Influence of Aerodynamics on the Design of Formula One Racing Cars, volume 3. International Journal of Vehicle Design, 1982.

[5] M. Patterson and A. V. Rao. GPOPS-II: A matlab software for solving multiple-phase optimal

control problems using hp-adaptive gaussian quadrature collocation methods and sparse nonlinear programming. ACM Transactions on Mathematical Software, 39(3):575 – 582, July 2013.

[6] B. Wymann, E. Espié, C. Guionneau, C. Dimitrakakis, R. Coulom, and A. Sumner. TORCS, the open