Pianificazione di traiettorie per robot mobili in modalita' distribuita con roadmap espansa nel tempo

UNIVERSITÀ DI PISA

Scuola di Ingegneria

Corso di Laurea Magistrale in Ingegneria Robotica e dell'Automazione

TESI DI LAUREA

Pianicazione di traiettorie per robot mobili in modalità

distribuita con roadmap espansa nel tempo

RELATORE

Prof.ssa Lucia PALLOTTINO

CANDIDATA

Pamela DAVID

Sommario

Nel presente lavoro è proposto un approccio al problema della pianicazione e coordinazione di un gruppo di agenti in maniera distribuita. Ogni robot pianica autonomamente un cammino in grado di portarlo dalla congurazione iniziale a quella nale senza collisioni e basandosi su un algoritmo di campionamento su uno spazio delle congurazioni ibrido (discreto-continuo) generato dall'espansione di un grafo orientato (mappa 2D) nel tempo (continuo). La coordinazione, che avviene tramite lo scambio di informazioni tra gli agenti, sfrutta anche la congurazione Treno (nata per gestire situazioni con più robot in coda). Viene poi presentata l'implementazione dell'algoritmo nell'ambiente ROS e le relative simulazioni e analisi dei dati; inne è proposto un possibile metodo per aggiungere il costo della rotazione nella scelta del cammino da compiere.

Indice

Sommario i

1 Introduzione 1

2 Stato dell'arte 2

3 MrFERT 6

3.1 Descrizione del problema . . . 6

3.2 Approccio di pianicazione del cammino per multi-robot . . . 10

3.2.1 Calcolo della traiettoria . . . 12

3.2.2 Protocollo di negoziazione . . . 14

4 Congurazione treno 15 4.1 Riconoscimento congurazione treno . . . 16

4.2 Gestione priorità . . . 18

4.3 Induzione pianicazioni . . . 19

4.3.1 Distanza di sicurezza . . . 20

4.4 Gestione tamponamenti su stesso arco . . . 21

4.5 Dimensione massima del treno . . . 21

5 Implementazione 22 5.1 Mappa . . . 23 5.2 Struttura codice . . . 24 5.2.1 Condizioni iniziali . . . 24 5.2.2 Assegnazione goal . . . 25 5.3 Pianicazione . . . 25 5.3.1 Algoritmo MrFert . . . 26

5.3.2 Condivisione delle informazioni e check collisioni . . . 27

5.3.3 Treno . . . 27

5.4 Controllo del moto . . . 28

6 Simulazioni 29 6.1 Incrocio . . . 30

6.2 Rotonda . . . 35

6.2.1 Confronto tra incrocio e rotonda . . . 38

6.3 Magazzino . . . 39

6.4 Treno . . . 41

6.4.1 Considerazioni . . . 55

7 Costo di rotazione 56 7.1 Problema . . . 57

7.2 Idea: ampliamento del grafo . . . 59

7.3 Esempio . . . 60 7.4 Applicazione su mrfert . . . 61 7.4.1 Test . . . 61 7.5 Osservazioni . . . 64 8 Conclusioni 65 A APPENDICE 66

1 Introduzione

Lo studio sulla coordinazione di multi agenti è stato inizialmente motivato da fenomeni osservati in natura (stormi di uccelli e mandrie di animali da prateria), dalla sica statistica e dal calcolo distribuito. Col tempo, e in particolare negli ultimi tre decenni, il controllo cooperativo di sistemi multi agenti ha ottenuto sempre maggiore attenzione [6].

La robotica mobile per magazzini, in particolare, sta ormai entrando nel mercato di produzione di massa con diverse aziende che costruiscono robot mobili autonomi di varie grandezze e per scopi dierenti. Per gestire un gruppo di robot mobili è necessario il calcolo della loro traiettoria ed evitare collisioni o situazioni di stallo (deadlock).

L'obiettivo del presente lavoro, che riprende e continua quanto realizzato da Ferrati in [5], è quindi quello di sviluppare, implementare e simulare un approccio di tipo distribuito in cui ogni robot pianica autonomamente il proprio cammino (che lo porti dalla posizione di partenza alla destina-zione desiderata) e dove la coordinadestina-zione avviene tramite la comunicadestina-zione tra gli agenti presenti nell'ambiente di lavoro. La pianicazione è basata sul campionamento, in cui si sfrutta la creazione di una roadmap su uno spazio di congurazione ibrido (continuo-discreto) e la minimizzazione di una funzione di costo che tiene conto della distanza e del tempo di percorrenza.

La tesi è consistita innanzitutto nella denizione e analisi del problema con particolare attenzione alla coordinazione dei robot; in relazione a questo aspetto è stata introdotta infatti la Congurazione Treno capace di gestire particolari situazioni di congestione del traco e di impedire tamponamenti tra gli agenti.

Molta importanza è stata poi riservata all'implementazione dell'algoritmo, realizzata con linguaggio C++ in ambiente ROS. Durante tale implementazione è stato possibile, e in certi casi necessario, discostarsi leggermente da quanto inizialmente teorizzato in modo da migliorare il risultato nale. In particolare gli aspetti più impegnativi sono stati: permettere agli agenti di avere velocità massime dierenti ed evitare il fallimento del calcolo della traiettoria (che si vericava in alcune situazioni speciche) andando a modicare una parte dell'algoritmo di pianicazione.

Per analizzare poi le capacità dell'approccio proposto sono state condotte una serie di simulazioni in dierenti scenari, con relativa analisi dei dati.

Inne, pensando a possibili implementazioni future dell'algoritmo per veicoli con vincoli dinamici più complessi, è stata proposta una possibile modica del grafo (su cui si basa la pianicazione) che permetta di tenere conto anche del costo di rotazione nella scelta del cammino minimo.

La tesi risulta quindi così articolata: una breve introduzione al problema della robotica mobile con uno sguardo allo stato dell'arte (Capitolo 2), la denizione e descrizione dell'algoritmo utilizzato (Capitolo 3) e l'introduzione e spiegazione della Congurazione Treno (Capitolo 4).

Nel Capitolo 5 vengono descritti gli aspetti più importanti riguardo l'implementazione, mentre nel Capitolo 6 si riportano le simulazioni realizzate con l'analisi dei dati ottenuti.

2 Stato dell'arte

Nell'ambito della robotica mobile ha acquisito sempre maggiore importanza lo studio sui sistemi multi robot mobili (MMRS) la cui ricerca è iniziata alla ne degli anni '80. Tra gli aspetti più signicativi collegati a questo ambito vanno ricordati: il meccanismo di comunicazione, la strategia di pianicazione e l'architettura decisionale.

L'obiettivo di un sistema MMRS è quello di far lavorare insieme un gruppo di robot che si muovono in un ambiente allo scopo di completare un task. La necessità di moto e interazione sica è proprio ciò che lo dierenzia da un sistema di multi-agenti (MAS) che è costituito da un sistema di calcolo distribuito (nodi stazionari).

Per capire la crescente importanza acquisita da sistemi multi robot si può fare un veloce confronto con un singolo robot. Mentre un sistema a singolo robot è spesso modellato per gestire un task da solo e quindi necessita di un numero elevato di sensori, di un sistema di controllo avanzato ed ha evidenti limiti sici nel caso in cui il task sia costituito da compiti distanziati nello spazio, un siste-ma MMRS può avere diversi vantaggi. Innanzitutto permette una distribuzione spaziale migliore, in secondo luogo ha una maggiore robustezza che deriva dal data fusion e dalla condivisione di informa-zioni oltre ad una maggiore tolleranza agli errori. Può inoltre avere costi ridotti se si utilizzano robot più semplici da programmare e più economici da costruire (a dierenza di un robot singolo comples-so e costocomples-so). In ultima analisi un sistema multi robot può rivelarsi più adabile, essibile e versatile. Parlando dei sistemi di robot mobili è interessante analizzare brevemente alcune classicazio-ni [16].

I MMRS si possono innanzitutto suddividere in:

-omogenei: quando le capacità sono identiche per ogni robot

-eterogenei: se le capacità dei robot sono dierenti per cui questi possono specializzarsi su specici compiti.

È poi importante identicare il comportamento collettivo del sistema cioè la sua risposta ad uno stimolo, si parla di comportamento cooperativo e di comportamento competitivo. La competizione entra in gioco quando più robot devono competere tra di loro per soddisfare un personale interesse, la cooperazione invece si riferisce ad una situazione in cui più robot devono interagire per riusci-re a completariusci-re un compito, essi quindi lavorano per raggiungeriusci-re un interiusci-resse comune. Perché gli agenti riescano a cooperare è necessaria la loro coordinazione che generalmente avviene sfruttando la comunicazione (come vedremo più avanti questa può avvenire tra i robot e un controllo centrale o direttamente tra gli agenti stessi). La coordinazione serve per risolvere conitti riguardo alle risorse condivise che nel caso dei robot mobili saranno principalmente gli spazi sici dell'ambiente.

Coordinazione

La coordinazione può essere statica o dinamica. Si denisce statica (detta anche coordinazione of-ine) quando avviene l'adozione di una convenzione a priori (cioè prima di iniziare il task) come ad esempio le regole per il controllo del traco (mantenere la destra, fermarsi agli incroci etc.). Si parla invece di coordinazione dinamica (o online) quando questa avviene durante l'esecuzione del task e generalmente si basa sull'analisi e sintesi delle informazioni ricevute tramite comunicazione. Dato che un sistema statico può gestire compiti più complessi ma risulta scadente nel controllo real time potrebbe essere opportuno utilizzare entrambe le modalità in maniera combinata.

Comunicazione

La comunicazione permette l'interazione tra robot, questi infatti possono sia condividere la posi-zione, lo stato dell'ambiente e i dati dei sensori, sia ricevere informazioni sulle intenzioni degli altri agenti. Più in particolare una comunicazione diretta (o esplicita) è caratterizzata da un eettivo scambio di informazioni tra i robot che può avvenire tramite l'invio intenzionale di messaggi (unicast o broadcast) e necessita di uno specico modulo di comunicazione a bordo di ogni veicolo.

sugli altri agenti attraverso l'ambiente; questo può avvenire attivamente se il robot raccoglie infor-mazioni lasciate nell'ambiente dagli altri veicoli (atteggiamento ispirato al mondo animale: api e formiche) oppure passivamente se gli agenti percepiscono dei cambiamenti nell'ambiente circostante tramite i sensori e devono poi calcolare le informazioni basandosi sull'analisi dei dati percepiti. Per quanto riguarda la comunicazione implicita un esempio ci viene fornito da Khaliq in [9] dove viene applicato come metodo di comunicazione la stigmergia (principio osservato in natura dove gli animali rilasciano e recuperano informazioni nell'ambiente). Più in particolare, invece di dotare i robot di risorse di calcolo potenti e sensori sosticati necessari all'auto-localizzazione, si utilizzano dei tag di identicazione a radiofrequenza (RFID) installati nell'ambiente in modo da formare un campo potenziale tale da permettere di raggiungere il goal seguendo il suo gradiente.

L'uso della comunicazione esplicita assicura accuratezza al sistema ma all'aumentare del numero di robot può portare ad una eccessiva quantità di informazioni e una conseguente diminuzione delle performance, dall'altra parte quella implicita non sarà molto adabile ma garantisce maggiore sta-bilità e tolleranza agli errori.

Pianicazione

La pianicazione consiste nel trovare una sequenza di azioni che permetta il raggiungimento dell'o-biettivo. La pianicazione per multi robot si suddivide in:

-pianicazione del task: risolve il problema di assegnazione del task ad un agente -pianicazione del moto: genera il cammino del robot

Nel caso di robot mobili risulta particolarmente importante la pianicazione del moto che avrà come scopo: produrre un moto continuo nella spazio a partire da una congurazione iniziale verso una congurazione nale evitando collisioni con gli ostacoli.

Si possono distinguere tre principali approcci: la decomposizione in celle, il campo potenziale e l'utilizzo della roadmap. Generalmente in tutti questi casi il problema continuo viene ridotto ad un grafo discreto.

Decomposizione in celle: Avviene una scomposizione dello spazio libero in un numero nito di regioni contigue chiamate celle. Un esempio in cui è stato utilizzato questo approccio è stato presentato da Bennewitz in [1] che utilizza una strategia basata su algoritmi di pianicazione A*. In particolare, in tale lavoro, per rappresentare l'ambiente dei robot è stato applicata una griglia di occupazione in cui ogni cella ha la stessa grandezza e a cui è associata la probabilità che sia occupata. La procedura A* tiene in considerazione simultaneamente il costo per raggiungere una cella a partire dalla posizione iniziale e il costo stimato per il raggiungimento della posizione desiderata. Il costo di attraversamento di ogni cella è proporzionale alla probabilità di occupazione.

Campo potenziale: Il cammino viene generato combinando la repulsione dagli ostacoli con l'at-trazione verso la posizione desiderata. Un esempio è stato già presentato in [9].

Roadmap: Si riduce lo spazio su cui il robot può muoversi ad un insieme di curve monodimensio-nali che connettono un gruppo di nodi (roadmap). In questo ambito un approccio tipico è l'utilizzo dei diagrammi di Voronoi: dato un insieme di ostacoli poligonali disposti nell'ambiente è infatti possibile denire un insieme di punti P lungo i lati di ogni poligono, costruire i diagrammi rispetto a tale insieme (punti equidistanti da due o più ostacoli), convertire il diagramma ottenuto in un grafo i cui vertici corrispondono a quelli di Voronoi ed inne individuare un cammino tramite un qualsiasi algoritmo di esplorazione del grafo. Un metodo di esplorazione di un ambiente sconosciuto da parte di un gruppo di robot mobili sfruttando i diagrammi di Voronoi è stato proposto da Wurm in [15]. In particolare esso si soerma sulle modalità di assegnamento delle regioni da esplorare per ogni agente in modo da minimizzare il tempo totale di lavoro.

Un altro possibile approccio è l'utilizzo di una roadmap probabilistica (PRM). Questo metodo genera in maniera random un vasto numero di congurazioni prive di collisioni e permette la pianicazione

del moto attraverso la connessione di alcune di esse. Più in dettaglio, [8], la costruzione della road-map avviene generando delle congurazione in modo casuale nello spazio C che vengono collegate tra loro tramite un pianicatore semplice e veloce, detto connettore, (ad esempio in un piano possono essere unite due congurazioni tramite un banale segmento). Il connettore lega i nodi (congurazio-ni) che si trovano ad una certa distanza l'uno dall'altra (secondo la metrica adottata nel problema particolare). Ogni connessione avvenuta con successo diventa un arco del grafo. Dopo che è stato generato un largo numero di nodi (e le loro connessioni) vengono determinate in maniera euristica le zone più problematiche per generare in tali ragioni ulteriori nodi. Inne la ricerca di un cammino dalla congurazione iniziale (a) a quella nale (b) necessita prima la connessione di tali nodi alla roadmap e in seguito una ricerca del cammino sul grafo.

Un ulteriore metodo probabilistico è il RRT (Rapidly-exploring Random Tree), [10], che aronta il problema con una ricerca su albero. Tale albero è costruito in modo incrementale connettendo ogni nuovo vertice ad una congurazione dell'albero. Rispetto al metodo PRM l'espansione RRT tende naturalmente verso le regioni meno esplorate. L'algoritmo è abbastanza semplice da implementare e l'albero tende a rimanere connesso anche con un numero minimo di archi.

PRM e RRT sono metodi per la pianicazione del moto basati sul campionamento e sono adatti a spazi geometricamente complessi e spazi di congurazione di grandi dimensioni in quanto il tempo di calcolo non cresce esponenzialmente con la dimensione di C.

Struttura decisionale

Un ultima importante distinzione riguarda l'architettura decisionale che può essere centralizzata o decentralizzata. Nel caso di una struttura centralizzata un controllo centrale possiede le informazioni globali sull'ambiente e sui robot ed è esso stesso a pianicare, coordinare e comunicare direttamente con gli agenti. Il vantaggio evidente è la visione globale del problema e la possibilità di avere solu-zioni ottime. Bisogna però tenere conto che tale metodo può essere ecace solo in presenza di pochi robot e non è robusto rispetto alla dinamica dell'ambiente e al fallimento della comunicazione. Un applicazione che permette di pianicare no ad un massimo di 100 robot (ma solo no ad un massi-mo di 10 quando si considerano situazioni da Worst Case Scenario) è proposto da Siméon in [13]. In tale lavoro viene introdotto un diagramma di coordinazione allo scopo di gestire il moto di diversi agenti lungo cammini indipendenti in modo coordinato. Per ridurre la dimensione del problema (esponenziale al crescere del numero di veicoli) viene applicata una decomposizione del cammino in pezzi elementari (linee rette e archi di circonferenza).

Una struttura decentralizzata può invece essere distribuita o gerarchica. Nel caso sia distribuita tutti i robot sono uguali in termini di controllo e completamente autonomi nelle decisioni. Diversamente una struttura gerarchica può essere vista come un ibrido tra distribuita e centralizzata: ci sono uno o più controlli che organizzano i robot in gruppi. Un approccio di questo genere viene proposto da Digani in [3] che si concentra sulla pianicazione e la coordinazione di AGV in un magazzino auto-matizzato. Per ridurre la complessità totale e semplicare il controllo vengono utilizzati due livelli gerarchici: un grafo topologico dello stabilimento realizzato tramite la suddivisione della mappa in settori (nodi del grafo) e una vera e propria mappa del cammino su cui si muove ogni veicolo. In tal modo la coordinazione è limitata ad un solo settore del primo livello. In altre parole in ogni settore il traco è gestito in una modalità decentralizzata.

Rispetto all'architettura centralizzata quella decentralizzata può rispondere meglio ad ambienti sco-nosciuti o che si modicano nel tempo. È però opportuno precisare che le soluzioni saranno spesso subottimali. Sebbene entrambi gli approcci si possano considerare praticabili a seconda del proble-ma reale, quello distribuito è considerato più promettente per la capacità di controllare un numero maggiore di veicoli.

Nel presente lavoro si è optato per una struttura distribuita che trae fortemente ispirazione da quanto proposto in [4]. In tale lavoro viene proposta una gestione del traco per un gruppo di veicoli in moto all'interno di un ambiente condiviso. L'algoritmo si basa sulla rappresentazione dello spazio-tempo in modalità discreta attraverso il TEN (Time Expanded Network) e permette la

pia-nicazione del moto evitando le collisioni. In particolare il TEN nasce dall'espansione di un grafo diretto (rappresentante lo spazio sico) nella dimensione temporale. Questa è modellata in modo discreto infatti ogni arco è pesato con il tempo necessario al raggiungimento del nodo successivo (tale valore sarà un numero intero) e vale che t ∈ T = {0, 1, ...M} con M limite superiore della dimensione temporale.

3 MrFERT

Come già accennato nel Capitolo 2 un protocollo distribuito per multi-robot è stato proposto in [4], dove sia lo spazio che il tempo sono discreti e le traiettorie, libere da collisioni, sono pianicate su un Time Expanded Network. In questo lavoro invece viene proposto un pianicatore di traiettoria di tipo distribuito con uno spazio discreto e un tempo continuo basandosi su un approccio simile. L'al-goritmo sviluppato si basa su una procedura di campionamento di tipo random in una congurazione spaziale ibrida (discreta/continua) per sfruttare la capacità del robot di regolare la propria velocità in modo da evitare collisioni, mantenendo comunque un andamento quanto più regolare possibile. Deniamo tale algoritmo come: Multi-robot Fast Expandend Roadmap Tree che abbreviato risulta MrFERT.

Traendo ispirazione da varianti degli algoritmi FTM [7] e RRT, ogni robot utilizza singolarmente un pianicatore basato sul campionamento per determinare una traiettoria (3.2.1). In un secondo momento, a seguito dello scambio di informazioni con gli agenti vicini, vengono calcolate traiettorie prive di collisioni (3.2.2).

3.1 Descrizione del problema

Sia G = (V, E) un grafo topologico che rappresenti un ambiente di lavoro n-dimensionale dove V = {v1, ..., vn}è l'insieme di nodi ed E ⊆ V x V è l'insieme degli archi (per esempio eijè l'arco che porta

dal nodo vi al nodo vj). Sia poi tale grafo orientato.

La posizione del nodo è denita come q(vi), nel caso di una dimensione dello spazio pari a due

(n = 2) avremo che q(vi) = (xi, yi) e ogni arco eij è il cammino che porta da q(vi) a q(vj).

Po-tremmo denire il peso sugli archi wij per rappresentare la lunghezza del cammino in Rntra q(vi)e

q(vj)ma per motivi che sono spiegati nel Capitolo 7 è meglio considerare, per il momento, il grafo

come non pesato e denire tale distanza come di,j. Si consideri poi un insieme R={r1, ..., rM}di M

robot mobili ordinati per priorità. Ogni robot ri si sposta sulla roadmap con una propria velocità

massima ammissibile vi,max. La posizione del robot i-esimo al tempo t sarà quindi denita come

ri(t). In seguito ci si riferirà a ri anche come Agi.

Ogni robot (agente) dovrà essere in grado di determinare una traiettoria senza collisioni con gli altri agenti sfruttando la comunicazione tra robot vicini.

Lo spazio misto discreto-continuo è caratterizzato dalle congurazioni s ∈ Rn x R+ dove Rn è un

sottoinsieme di Rn _{che contiene le pose discrete del robot. Quindi la prima componente è la posa}

dell'agente nell'ambiente e la seconda componente è il tempo (per n = 2 s = (x, y, t)). In seguito chiameremo q(s) le coordinate discrete in Rn _{e τ(s) il tempo continuo associato a s. In particolare}

vale che le congurazioni sono associate alla roadmap (q(s) ∈ G). Deniamo questo spazio di stato-tempo come una roadmap espansa nel stato-tempo (time-expanded roadmap) e lo denotiamo come Gt.

Sia Ds(safety distance) la minima distanza necessaria tra due robot per evitare collisioni. Questa

distanza deve tener conto delle dimensioni siche degli agenti e dei vincoli di moto. In questo la-voro abbiamo assunto che tutti gli agenti siano sicamente uguali (degli iRobot Roomba), quindi tale distanza minima è uguale per ogni coppia di agenti. Se invece si lavora con robot tra loro diversi si avranno delle Ds,ij per ogni (i, j) coppia di veicoli, in tal caso possiamo assumere che

Ds= max(Ds,ij).

Denizione 1 (Collisione tra robot): si aerma che una collisione tra due robot ri e rj avviene

al tempo t se vale:

kri− rjk ≤ Ds (1)

sulla topologia della roadmap e sulle posizioni desiderate dei robot.

Assunzione 1 (Topologia della roadmap): Gli archi sono cammini in uno spazio libero tali che la distanza tra due qualsiasi punti posti su due dierenti archi è maggiore di Dsogni volta che i punti

si trovano ad una distanza maggiore di un dato D da un qualsiasi nodo. Inoltre, qualsiasi coppia di nodi deve avere, ovviamente, una distanza maggiore di Dsin modo che due agenti posti su due nodi

diversi non siano in collisione. Il nodo non è quindi solo caratterizzato dalle sue coordinate nello spazio ma anche da un disco di raggio D centrato su tali coordinate (g.1).

Figura 1: Topologia della roadmap

La costruzione del grafo deve rispettare determinate condizioni. Si avrà infatti che ogni coppia di nodi (vi, vj) collegata da un arco ei,jdovrà essere distanziata di almeno una quantità pari a 2 ¯Di,j

con ¯Di,j= max{Di, Dj}.

L(ei,j)> 2 ¯Di,j (2)

Come già detto Diè il raggio del disco associato al nodo vie, come vedremo poco più avanti, dipende

dagli angoli formati da tutti gli archi del nodo in questione (eq. 5).

È necessario considerare sempre anche il problema particolare che si va ad arontare, nello specico le velocità massime degli agenti. I robot possono avere velocità massime diverse fra loro, sia allora velM AX = max{velmax,1, ..., velmax,n}la velocità massima per l'agente più veloce. Sarà necessario

che ogni arco abbia una lunghezza tale che ogni agente se necessario riesca a ripianicare almeno una volta prima di passare all'arco successivo della sua traiettoria. Quindi si deve imporre che:

L(ei,j)> TAvelM AX (3)

dove TAè il tempo di pianicazione. Se invece il problema necessità di particolari distanze tra nodi

per cui l'eq. 3 non è rispettata (deve però essere sempre rispettata l'eq. 2) sarà necessario cambiare opportunamente TA tenendo sempre conto che deve essere un intervallo temporale che permetta di

realizzare la pianicazione (maggiore sarà la grandezza del grafo più tempo sarà necessario per il calcolo della traiettoria).

Denizione 2 (Intervallo di occupazione temporale del nodo): Un nodo v ∈ V della roadmap è occupato dal robot r nell'intervallo temporale di occupazione T = [t1, t2]se ∀t ∈ T vale:

kr(t) − q(v)k < D (4) Tale tempo di occupazione T dipende dal percorso fatto dal robot e dalla sua velocità. Considerando l'assunzione 1 quindi, un robot non può occupare simultaneamente due nodi diversi. Nel caso in cui la roadmap sia costituita da segmenti (in qualità di archi) colleganti i nodi, D può essere facilmente calcolato:

D = Ds

2 sin(α₂) (5)

dove α è l'angolo minimo del nodo, cioè il più piccolo angolo tra quelli formati dagli archi del nodo. Quando due robot, spostandosi lungo due archi distinti (entranti e/o uscenti) dello stesso nodo, sono ad una distanza maggiore di tale D dal nodo, non avviene nessuna collisione (g. 2).

L'assunzione 1 generalizza tale condizione per archi non rettilinei. L'idea generale è di avere delle

Figura 2: Geometria di D

roadmap sulle quali le collisioni possono avvenire solo sul disco di occupazione o lungo lo stesso arco. Assunzione 2 (Roadmap sempre connessa): la roadmap utilizzata è un grafo fortemente con-nesso, inoltre gli agenti non ricevono mai dei compiti le cui posizioni sui nodi o sugli archi portano ad una perdita della connettività del grafo.

Dato che i robot possono scontrarsi solo ad una distanza minore di D dai singoli nodi o lungo gli archi, sarà necessario realizzare una coordinazione tra agenti che impedisca loro di occupare uno stesso nodo nello stesso intervallo temporale e di tamponarsi a vicenda.

Si introducono adesso i concetti di cammino e traiettoria. Un insieme di nodi adiacenti a z in G viene denito come adj(z) = {v ∈ V |(z, v) ∈ E}.

Denizione 3 (Cammino): un cammini P (vs, vg) = {vs, ..., vg}è una sequenza di nodi distinti in G

tale che ogni nodo è adiacente al nodo precedente e/o successivo, partendo dal nodo vs (start) no

ad arrivare al nodo vg (goal).

Denizione 4 (Traiettoria): una traiettoria σ ∈ Gtè una sequenza σ = {s1, ..., sk}di congurazioni

si associate ai nodi cioè tali che q(si) ∈ V. La sequenza di nodi {q(s1), ..., q(sk)}è un cammino dal

nodo q(s1) al nodo q(sk) e la sequenza di tempi di attraversamento dei nodi {τ(s1), ..., τ (sk)} è

positiva e monotona crescente.

Ad ogni traiettoria è possibile associare un costo di traiettoria basandosi sulle speciche del pro-blema (ad esempio distanza percorsa minima o tempo di viaggio minimo). Nel presente lavoro è proposto un costo di traiettoria che considera sia la distanza percorsa sia il tempo di viaggio (una modica a tale costo viene proposta nel Capitolo 7 per introdurre anche il costo di rotazione). Data la traiettoria σ = {s1, ..., sk}, il suo costo sarà dato da:

C(σ) = ht k X i=0 τ (si) + hc k X i=0 di,i+1 (6)

3.2 Approccio di pianicazione del cammino per multi-robot

L'idea generale è che dati M robot, ognuno con una posizione di partenza e una di arrivo (nodi sul grafo), l'algoritmo permetta ad ogni agente di determinare una traiettoria priva di collisioni tramite un controllo sia sulla velocità che sul cammino percorso in modo tale da ridurre l'ener-gia consumata da cambi di velocità. I robot comunicano solo con i loro vicini, cioè con gli agenti che si trovano ad una distanza minore del range di comunicazione Rc. Vale ovviamente che Rc> Ds.

Denizione 5 (Agenti vicini): sia N(p, t) = {i|1 ≤ i ≤ M, kri(t) − rp(t)k ≤ Rc} l'insieme degli

indici dei vicini del robot rpcioè dei robot che si trovano ad una distanza da rpminore di Rc. Come

già si evince dalla nomenclatura N(p, t) è un insieme che dipende dal tempo t in quanto gli agenti si muovono e e le loro distanze relative si modicano.

L'approccio proposto consiste quindi nell'alternare la fase di pianicazione della traiettoria con quella di negoziazione tra agenti (coordinazione). Durante la fase di pianicazione ogni robot calcola la propria traiettoria sulla roadmap espansa sul tempo Gt, tenendo conto degli ostacoli presenti

nel-l'ambiente. Tali ostacoli sono generati durante la fase di negoziazione dove si considerano possibili collisioni con gli agenti vicini (cioè entro il range di comunicazione) aventi una priorità più alta. Osservazione: In questo lavoro le priorità sono indicate con dei numeri interi i ∈ N, minore è il nu-mero, più alta sarà la priorità, quindi un agente con priorità i = 1 avrà precedenza su tutti (priorità più alta possibile).

Ogni robot rpriceve, solo dai propri vicini N(p, t), la loro ultima traiettoria pianicata σ. Sia quindi

σl= {s1, ..., sk}la traiettoria che il robot rp riceve dal robot rl, con l < p (cioè il robot l ha priorità

più alta), allora gli intervalli temporali di occupazione di ciascun nodo associato alle traiettorie ri-cevute, vengono calcolati come segue.

Per ogni arco (q(si−1), q(si))(arco ei−1,i sul grafo) in σl la velocità media è:

¯ vi−1,i=

di−1,i

τ (si) − τ (si−1) (7)

Date le velocità medie lungo gli archi si può determinare l'intervallo temporale in cui il nodo q(si)

è occupato dal robot rl:

Intervallo = [τ(si) − D ¯ vi−1,i , τ (si) + D ¯ vi,i+1 ] (8)

Con questo approccio quindi l'intervallo di occupazione temporale è calcolato tenendo conto delle velocità del robot in ingresso e in uscita dal nodo q(si) (queste possono ovviamente essere diverse

tra loro). Durante questo intervallo di tempo il nodo q(si)risulta, come già detto, essere occupato

(eq.4) dal robot rl che ha priorità maggiore di rp e quindi per quest'ultimo rappresenta un ostacolo

statico in Gt. Questo intervallo è un segmento lineare lungo la dimensione temporale.

Per descrivere meglio lo scenario è possibile rappresentarlo gracamente.

Si può vedere in g. 3 come i due robot rl(in rosso) e rp(in verde) vogliano entrambi passare per

il nodo B (il primo proviene dal nodo C, il secondo da A). Si suppone che B sia collegato anche ad altri nodi (non mostrati in gura) e che quindi gli agenti debbano solo passare sul nodo in questione e non sostarci. Si può quindi rappresentare l'ostacolo statico come un cilindro la cui base sarà la circonferenza di raggio D (diversa per ogni nodo), mentre l'altezza del cilindro altro non è che l'intervallo temporale di occupazione del nodo. Consideriamo adesso con più attenzione cosa succede sul nodo B (g. 4): si supponga innanzitutto che i due agenti siano gli unici robot nel loro range

Figura 3: Rappresentazione intervallo temporale di occupazione visto come ostacolo statico

sua massima velocità, idealmente la velocità di ingresso e di uscita sarà pressoché la stessa (quella massima) e l'intervallo temporale è simmetrico rispetto all'istante eettivo di raggiungimento del nodo in Gt(cilindro rosso su B). Invece il robot rp deve considerare l'ostacolo di rl, quindi pianica

per raggiungere il nodo B dopo che si è liberato, per fare questo deve utilizzare una velocità di ingresso minore e quindi un intervallo temporale di occupazione maggiore. Assumendo poi che la sua velocità di uscita sia maggiore di quella di ingresso, si avrà un intervallo non simmetrico.

Figura 4: Dettaglio della rappresentazione 3D di un nodo

Osservazione 1: Bisogna far notare che con questo concetto di intervallo temporale di occupa-zione del nodo è possibile gestire anche ostacoli imprevisti presenti nell'ambiente e rilevati dal robot. In particolare se l'ostacolo viene riconosciuto come statico, al nodo viene associato un intervallo di occupazione temporale innito. Sarà possibile trovare lo stesso un cammino se il grafo rimane fortemente connesso anche ignorando tale nodo.

Proposizione 1 Sia Pp=

S

i∈N (p,t),i<pσi l'insieme di traiettorie degli agenti vicini e con priorità

più alta di rp e sia σp = {si, ..., sk} la traiettoria calcolata per rp in un certo istante. Per ogni

nodo v ∈ {q(si)|i = 1, ..., k} sia Tv l'intersezione degli intervalli temporali di occupazione per ogni

i ∈ N (p, t)e i ≤ p. Se Tv è vuoto per ogni nodo v in σp, il robot rp non avrà nessuna collisione con

gli agenti ri con i < p quando si trova ad una distanza minore di D dal nodo v lungo la traiettoria

3.2.1 Calcolo della traiettoria

Durante questa fase, per ogni robot verranno calcolate traiettoria prive di collisioni rispettando la condizione imposta dalla Proposizione 1, per evitare scontri sui nodi. Le eventuali collisioni lungo gli archi verranno gestite durante la fase di negoziazione.

L'idea generale dell'approccio proposto è che ogni robot rp usi un algoritmo di pianicazione a

campionamento per determinare una traiettoria priva di collisioni che lo porti dal nodo occupato al momento iniziale (vs)no al nodo desiderato (vg). Il robot estrae i campioni s ∈ Gt sulla roadmap

espansa nel tempo con q(s) ∈ V e τ(s) ∈ [0, ∞). Per cui i nodi della roadmap G sono campionati venendo associati ad un tempo random di cammino. I campioni sono poi scartati o connessi allo scopo di far crescere l'albero T ⊂ Gtche consiste di nodi campionati nello spazio-tempo.

Di seguito è riportata la procedura per ottenere in un breve intervallo di tempo una traiettoria priva di collisioni non ottimale (Algoritmo 1). Una volta trovata una prima traiettoria, viene svolta una procedura di ottimizzazione (Algoritmo 2).

Oss: L'algoritmo 1, qui di seguito mostrato, sarà modicato in fase di implementazione (5.3.1) Algoritmo 1

1 Goal <-- (v_g,inf), Start <-- (v_s, t_s) ; % Inizializzazione 2 s <-- Start, T <-- {};

3 Q <-- {s}, V <-- {q(s)}; 4 while q(s) != q(Goal) do 5 | if Q = 0 then

6 | |_ ritorna: goal irraggiungibile 7 | w <-- campiona nodo da Q;

8 | if adj(q(w)) non appartiene a V then

9 | | v <-- campiona un nodo in adj(q(w)) \ V; 10 | | t <-- campiona un tempo;

11 | | s <-- (v,tau(w)+t); 12 | | if s è "buono"

13 | | | aggiungi q(s) a V; 14 | | | aggiungi s a Q;

15 | | | aggiungi nodo s e arco (q(w),s) a T; 16 | | else 17 | | |_ continue; 18 | else 19 | |_ elimina w da Q |_ 20 t_g = tau(s); 21 traj <-- percorrendo T da (v_s,t_s) a (v_g,t_g); 22 ritorna traj;

Analizziamo l'algoritmo: un campione può essere connesso ad un nodo dell'albero se i corrispet-tivi nodi sulla roadmap G sono connessi da un arco in E. Ogni nodo v ∈ G, associato al campione s ∈ T (cioè tale che q(s) = v), è contrassegnato come visitato e inserito in V ⊂ V (insieme di nodi visitati). Si considera poi un ulteriore insieme di nodi, Q ⊂ Gt dal quale l'albero cresce, che viene

inizializzato con la congurazione iniziale Start = (vs, ts)dove tsè il tempo attuale.

In modo simile all'algoritmo FMT* ( [7]), la procedura di campionamento estrae un nodo w dall'in-sieme crescente Q (riga 7), in seguito seleziona v ∈ V, uno tra i nodi adiacenti a q(w) sulla roadmap Gche non sono contrassegnati come visitati (riga 9) e tale che l'arco (q(w), v) ∈ E, quindi campiona

specicato il tempo campionato (¯t) deve essere non più piccolo del minimo tempo necessario al robot per raggiungere v partendo da q(w) rispettando i propri vincoli dinamici, più il tempo in cui si trova sul nodo q(w). Infatti tempi minori corrisponderebbero a traiettorie non realizzabili.

Il nuovo nodo campionato s = (v, ¯t+ τ(w)) è accettabile se l'intervallo temporale di occupazione del nodo non interseca nessun ostacolo statico in Gt. Ricordando la formula 8, per calcolare tale

intervallo servirebbero le velocità di entrata e di uscita al nodo v. Dato che la velocità di uscita è al momento sconosciuta (non è noto il nodo successivo a v), in questa fase al suo posto si utilizza sempre la velocità lungo l'arco entrante.

Se il nuovo campione s = (v, ¯t+τ(w)) è 'buono', viene aggiunto all'insieme Q (riga 13) e all'albero T con l'arco (q(w), v) (riga 14), mentre il nodo v è contrassegnato come visitato e inserito nell'insieme V (riga 12). Se invece non è buono, cioè avverrebbe una collisione, il campione viene scartato e ne viene generato uno nuovo (riga 17). I campioni w i cui corrispettivi nodi associati hanno tutti vicini già visitati vengono rimossi dall'insieme Q (riga 19). Nel caso in cui Q diventi vuoto l'algoritmo restituisce un messaggio di goal irraggiungibile (riga 6). Se invece il nodo goal (vg)viene raggiunto

l'algoritmo fornisce il tempo di cammino nale tg e la traiettoria σ (traj) che porta da (vs, ts) a

(vg, tg)sull'albero T .

Con la procedura sopra esposta si può trovare una traiettoria in un intervallo di tempo relativamen-te breve che però non sarà ottima dato che nell'algoritmo non è stato utilizzato alcun crirelativamen-terio di ottimalità.

Algoritmo 2

1 Q <-- {Start}; 2 BestCost <-- inf; 3 W <--V;

4 while non si chiama lo stop 5 | if C(traj) < BestCost then 6 | | BestCost <-- C(traj) 7 | |_ W <-- InformedPruning(BestCost); 8 | v <-- campione da W; 9 | t <-- campione da (0,inf); 10 | w <-- (v,t); 11 | if w è 'buono' then 12 | | s <-- BestNeighbor(w); 13 | | if s non è vuoto then 14 | | | aggiungi w a Q;

15 | | | ricollega Q se necessario; 16 |_ |_ |_ traj <-- aggiorna la soluzione; 17 restituisci traj;

Una volta ottenuta la traiettoria σ dall'Algoritmo 1, si utilizza un approccio di campionamento informato per calcolare una traiettoria di costo minore. Il costo C(σ) può essere utilizzato come limite superiore per il campionamento informato dei nodi in V.

In particolare dato un nodo v ∈ V, il costo di ogni traiettoria che parta da start e vada in (v, t) è maggiore di C(Start, v) = hckq(Start)−vk+htkq(Start)−vk_v

max per qualsiasi t (cioè sarà sempre maggiore

del costo corrispondente al cammino su una linea retta percorsa a velocità massima).

Ugualmente il costo di qualsiasi traiettoria che vada da (v, t) a Goal è maggiore di C(v, Goal) = hckv − q(Goal)k + htkv−q(_v Goal)k

max per qualsiasi t. Per cui ogni ogni traiettoria che vada da Start a

Goal passando per (v, t) avrà un costo maggiore di C(Start, v) + C(v, Goal). Allora se tale somma è maggiore di C(σ), il nodo v può essere trascurato dalla procedura perché non farà parte di una traiettoria ottimale; sarà quindi rimosso dall'insieme W (insieme di nodi candidati alla procedura

e inizializzato con V). Questa procedura viene ripetuta per tutti i nodi candidati presenti in W ogni volta che viene trovata una traiettoria a costo minore. Nel caso in cui venga trovata una traiettoria a costo minore, il limite superiore viene aggiornato prima di ripetere la procedura per i nodi ancora presenti in W come descritto sopra. Tale procedura, chiamata InformedPruning (potatura informata) è riportata alla riga 7 dell'Algoritmo 2. Una volta che i nodi non ottimali sono rimossi da W, ne viene campionato uno (riga 8) insieme ad un t (riga 9) per formare il nodo w = (v, t)nella roadmap Gtespansa nel tempo. Se tale nodo w ∈ Gtnon interseca nessun ostacolo,

si esegue una procedura di BestNeighbor, in maniera simile a ciò che avviene in RRT*. La procedura di BestNeighbor (riga 12) controlla tutti i campioni in Q vicini di w per trovarne, se presente, uno che possa essere connesso a w con un costo minimo tramite una traiettoria che rispetti il limite massimo di velocità. Se viene trovato un tale campione s, w viene aggiunto a Q con una traiettoria associata da s a w percorsa ad una velocità costante nel tempo τ(w) − τ(s) (riga 14). Inne una procedura di rewire (riconnessione) fornisce il cammino ottimale. Nel corso di queste procedure viene sempre rispettato il vincolo di velocità massima dell'agente. Non appena l'Algoritmo 2 viene interrotto (quando termina il tempo concesso per la pianicazione), si ottiene una traiettoria a costo minimo lungo l'albero costruito a partire dalla traiettoria iniziale ottenuta con l'Algoritmo 1. 3.2.2 Protocollo di negoziazione

Quando il robot si sposta nell'ambiente scambia informazioni con gli altri agenti presenti nel suo range di comunicazione per riuscire a coordinarsi. Per questo motivo il problema di pianicazione cambia in continuazione ed è importante che il robot possa generare nuove soluzioni che tengano conto delle ultime informazioni ricevute. Ovviamente le soluzioni devono essere generate in un tempo relativamente breve in modo che una volta pronte non siano obsolete (cosa che potrebbe portare a collisioni).

È quindi necessario determinare il range di comunicazione in modo che ogni robot abbia possa evitare le collisioni. Sia TA il tempo necessario alla pianicazione, tale tempo dovrà essere costituito da il

tempo necessario ad eseguire completamente l'Algoritmo 1 più un intervallo di tempo per iniziare ed eseguire almeno parzialmente l'Algoritmo 2 (come specicato in 3.2.1). Nel caso peggiore (Worst Case Scenario) tutti i nodi presenti nel range di comunicazione sono diretti verso lo stesso nodo e il robot a priorità minore viaggia a massima velocità (se i robot hanno velocità massima diversa signica che ha la velocità massima più alta). Bisogna inoltre tener conto che tale robot con priorità minore rivelerà eventuali collisioni per ultimo, dopo che anche il penultimo (in ordine di priorità) avrà trovato una traiettoria priva di collisioni (avendo priorità minore deve aspettare tutte le pianicazioni degli altri agenti a cui dovrà dare precedenza), cioè dovrà aspettare un tempo pari a TA(M − 1). La

massima distanza percorsa prima di una possibile collisione è quindi Dmax= TA(M − 1)vmax. Per

essere sicuri che si evitino le collisioni ogni robot deve comunicare con tutti gli agenti che si trovino entro il raggio Rc= 2Dmax.

Ogni robot invia agli agenti vicini alcune informazioni, tra cui, la propria priorità e la traiettoria dalla sua posizione attuale no al proprio goal. I robot con priorità minori controllano se le proprie traiettorie in Gt vanno a sovrapporsi a quelle ricevute dagli agenti a priorità maggiore. Nel caso

rilevino una collisione calcolano una nuova traiettoria aggiornando le proprie condizioni iniziali. Potrebbe succedere che un robot a priorità maggiore segua un robot (a priorità minore) con una velocità maggiore. Ciò potrebbe portare ad un tamponamento. Per evitare questa situazione è necessario che l'agente dietro ripianichi con particolari accorgimenti che evitino collisioni. Questi aspetti verranno esposti più dettagliatamente nel paragrafo 4.4.

4 Congurazione treno

La congurazione treno nasce dall'idea di poter gestire le code ma risulta utile anche per evitare tamponamenti lungo uno stesso arco quando l'agente in posizione arretrata ha priorità maggiore e/o velocità maggiore. L'idea iniziale è quella di cercare di ridurre i rallentamenti, che possono avvenire in prossimità di un incrocio se più agenti devono passare dallo stesso nodo, per un numero quanto più elevato possibile di agenti.

Figura 5: Incrocio con coda

Si consideri lo scenario rappresentato in g.5; in condizioni 'normali' l'ordine di passaggio all'in-crocio, basato sulle priorità dei singoli robot, sarebbe: Ag1, Ag2, Ag3, Ag4, ciò comporterebbe un rallentamento per 3 dei 4 agenti infatti l'agente 2 dovrebbe rallentare per far passare l'agente 1 e ugualmente gli agenti 3 e 4 che sarebbero in coda ad Ag1, per ipotesi con la sua stessa velocità, dovrebbero a loro volta dare precedenza ad Ag2.

Si potrebbe pensare quindi di far passare prima tutti i robot in coda così da rallentare solamente un veicolo (Ag 2) invece che 3 veicoli diversi. Bisogna quindi per prima cosa che gli agenti riconoscano la particolare situazione cioè la presenza o assenza di un coda (4.1).

Dato che l'assegnazione delle precedenze si basa sulle priorità singole dei robot bisogna quindi che gli agenti 3 e 4 abbiano una priorità maggiore dell'agente 2. Si può quindi cambiare la loro priorità, questo cambiamento però deve essere temporaneo e basato sulla situazione particolare del momento Se infatti tali agenti proseguono poi il loro cammino su una traiettoria diversa dall'agente 1 (e quindi non ne sono più in coda) devono perdere il 'diritto di precedenza', cioè devono avere ristabilita la propria eettiva priorità (4.2).

Una volta che si sono realizzate tali modiche alle priorità, anché portino a dei cambiamenti nelle precedenze, è necessario che gli agenti ripianichino la loro traiettoria (4.3). Sarà anche necessario poter regolare la lunghezza massima di un treno, in modo che Ag2 non debba aspettare troppo qualora ci fossero molti agenti in coda ad Ag1 (4.5).

4.1 Riconoscimento congurazione treno

Come già accennato in 3.2.2, gli agenti si scambiano informazioni, necessarie per il loro coordina-mento, all'interno del proprio range di comunicazione. Tra le informazioni scambiate ci sono anche le traiettorie di ogni agente ri: σi= {si,1, ..., si,k} dove ogni si,lè la combinazione di un nodo e del

relativo tempo di arrivo in tale punto per l'agente i − esimo. Si potrebbe pensare che ogni robot analizzi le traiettorie ricevute da ogni agente e le confronti con la propria per determinare se ci saranno delle situazioni di code future.

Denizione (Coda): Un insieme di 2 o più robot che condividono una parte di cammino, cioè la medesima successione di 2 o più nodi, in intervalli temporali0_vicini0_.

Questo approccio risulta però troppo oneroso in termini computazionali, in quanto è necessario tenere in considerazione anche il tempo di arrivo sui nodi oltre ai nodi stessi e che le traiettorie potrebbero essere anche molto ampie (in termini di nodi da visitare) in base al particolare problema. La via seguita invece prevede che ogni agente invii ulteriori informazioni più dettagliate sulla pro-pria posizione attuale e la posizione futura più prossima. Ogni robot infatti conosce ad ogni istante l'ultimo nodo visitato (last), il prossimo da visitare (next) e quello ancora successivo (next2 ) (g. 6). Chiameremo questi 3 nodi: nodi di posizione.

Figura 6: Nodi che descrivono la recente e prossima posizione del robot

Tali informazioni, che vengono aggiornate online ogni volta che l'agente raggiunge e supera il nodo next, vengono inviate ai robot entro il range di comunicazione. Per determinare se un agente si trova in coda rispetto ad un altro robot esso dovrà confrontare i nodi di posizione ricevuti con i propri. Più in dettaglio un robot si denirà in coda (cioè in una congurazione treno) se si verica una delle seguenti condizioni:

-robot sullo stesso arco

Come si può vedere in g. 7 quando due robot si trovano su uno stesso arco si verica: lasti=

lastk e nexti = nextk, ovviamente tali uguaglianze si riferiscono alle coordinate spaziali dei nodi, i

tempi di arrivo su tali nodi saranno diversi e tali da evitare collisioni. -robot su archi immediatamente successivi

Figura 8: Agenti su archi immediatamente successivi

In g. 8 invece i robot si trovano su archi immediatamente successivi e valgono le seguenti uguaglianze: nexti= lastk e next2i= nextk.

Quando il robot ri riceve le informazioni da rk e verica l'esistenza di una delle condizioni sopra

elencate, si riconosce come in coda all'agente k. Se invece nessuna di tali condizioni è vericata signica che il robot non è in coda. É opportuno notare che ad ogni istante si può vericare solo una delle due condizioni, cioè queste non possono coesistere.

Abbiamo quindi detto che si possono vericare tre situazioni: nessuna coda, coda su stesso arco e coda su archi dierenti, e che tali situazioni non possono mai vericarsi contemporaneamente. Una situazione però facilmente vericabile è l'oscillazione (ickering) tra queste condizioni quando uno dei due agenti supera un nodo e quindi aggiorna i suoi nodi di posizione. Se il passaggio da una congurazione di coda su stesso arco a quella su archi dierenti non risulta problematico in quanto l'agente rimane comunque sempre in coda, discorso diverso si ha quando avviene una oscillazione tra coda/non coda.

Consideriamo per esempio la g. 8 e supponiamo che rk raggiunga e passi il nodo nextk al tempo

τnextk e che ri raggiunga nexti all'istante τnexti e sia ∆T = τnexti − τnextk > 0. Durante tale

intervallo di tempo quindi si andrà a perdere la condizione di coda, dato che non saranno più valide le uguaglianze precedenti, quando poi anche l'agente riavrà superato il proprio nodo nextisi riavrà

un ristabilimento del treno. Supponiamo ora che tali agenti abbiano la stessa velocità e condividano una parte consistente della loro traiettoria (e quindi mantengano la stessa distanza uno dall'altro). Ad ogni aggiornamento dei nodi di posizione ci sarà una alternanza nella condizione di treno/non treno. Come vedremo successivamente la formazione di un treno o la sua distruzione induce una nuova pianicazione in alcuni degli agenti nel range di comunicazione (4.3) e questa oscillazione provocherebbe inutili e controproducenti ripianicazioni.

Per evitare tale ickering sarà necessario discriminare tra una possibile e una eettiva congurazione treno. Ogni volta che le uguaglianze di treno saranno rilevate si avrà una possibile congurazione, questa diverrà eettiva se tali condizioni si saranno mantenute costanti per un certo intervallo di tempo ∆Ttrain,start. Il treno sarà considerato dissolto invece se tali condizioni non saranno vericate

4.2 Gestione priorità

Si consideri nuovamente g. 5 e sia che Agi abbia priorità = i. Una volta che Ag3 determina di

essere in coda ad Ag1 assume momentaneamente la sua priorità. Allo stesso modo Ag4 in coda ad Ag3 ne assumerà la priorità (che sarà pari a quella dell'Ag1). Si avranno quindi 3 agenti con priorità 1 e l'agente 2 con priorità 2 (g.9).

Figura 9: Modica delle priorità per agenti in treno

Come si può vedere in g. 9 è opportuno che ogni agente mantenga anche la propria priorità originaria (si avrà priorità e nuova priorità). Questo è utile innanzitutto perché una volta persa la congurazione treno ogni agente deve ristabilire la propria priorità (e quindi ne deve mantenere memoria), in secondo luogo in modo da stabilire le precedenze anche all'interno del treno ed evitare conitti, se ad esempio Ag3 avesse la stessa priorità di Ag1 e avesse una velocità massima maggiore potrebbe competere per uno stesso nodo nello stesso intervallo di tempo e non ci sarebbe possibilità di risolvere il conitto.

Fino ad ora abbiamo visto il caso di un treno in cui in testa c'è l'agente con priorità maggiore, se invece tale agente si trova in un'altra posizione del convoglio (ad esempio g. 10), avviene un passaggio della priorità in avanti, cioè l'agente 1 riesce a passare la propria priorità all'agente 3 che si trova davanti (oltre ovviamente a passarla indietro all'agente 4).

Figura 10: Passaggio della priorità in avanti

Più in dettaglio avviene che l'Ag1 riconosce di essere in treno ad Ag3 e ne prende la priorità (pr1,new = 3), a questo punto la confronta con la propria (pr1 = 1), dato che pr1 < pr1,new invia

un messaggio ad Ag3 inducendolo a cambiare la propria priorità pr3,new = 1. Una volta che Ag3

modica la propria priorità lo faranno a cascata tutti gli agenti in treno.

In conclusione una volta formatosi un treno questo (cioè tutti gli agenti che lo compongono) assume la priorità maggiore presente nel convoglio, qualunque sia la sua posizione.

4.3 Induzione pianicazioni

La formazione e la dissoluzione della congurazione treno inducono delle ripianicazioni da parte di alcuni degli agenti presenti nel range di comunicazione.Tali ripianicazioni sono possibili solo se è rispettata una opportuna distanza di sicurezza.

- Formazione della congurazione treno

Supponiamo che si sia formato un treno e ci sia stato il 'passaggio' delle priorità, chi ha passato la priorità (che, come già detto, sarà l'agente con priorità originaria maggiore) non dovrà eettuare nessuna nuova pianicazione in quanto la sua priorità è rimasta invariata. Tutti gli altri agenti facenti parte del treno invece ripianicano per poter sfruttare un eventuale miglioramento di precedenze dovuto alla nuova priorità. Tutti gli altri robot nel range di comunicazione non facenti parte del treno realizzeranno una nuova pianicazione solo se quelle realizzate dagli agenti del treno hanno portato a conitti su almeno un nodo della loro traiettoria e se pri> prtreno (g.11).

Figura 11: La formazione del treno induce nuove pianicazioni - Dissoluzione della congurazione treno

Quando invece termina una congurazione treno gli agenti che ne facevano parte lo comunicano ai robot circostanti. Quando un robot riceve un messaggio di ne treno (ovviamente un treno di cui non faceva parte) realizza una nuova pianicazione se pri > prtreno e se pri< prk dove ri è il robot

che riceve il messaggio e rk è il robot che ha inviato il messaggio e che faceva parte del treno. Tali

ripianicazioni possono indurre nuove traiettorie agli agenti che facevano parte del treno se si creano dei conitti sull'occupazione di qualche nodo (avendo tali robot perso la precedenza che garantiva loro il treno, g.12).

4.3.1 Distanza di sicurezza

È opportuno notare che una nuova pianicazione, in caso di formazione o dissoluzione della congu-razione treno, può avvenire solo se sono rispettate opportune distanze di sicurezza.

(a) (b)

Figura 13: Due scenari simili in cui alla formazione del treno Ag3-Ag1 la distanza di sicurezza è rispettata (a) e non è rispettata (b)

Con riferimento alla g.13 (supponendo che il treno formato da Ag3 e Ag1 si sia appena formato) l'agente 3 potrà eettuare una nuova pianicazione solo se nessun agente non appartenente al treno (nel caso in questione Ag2) è in procinto di arrivare al suo stesso nodo in un tempo inferiore a quello necessario ad eettuare una nuova pianicazione per entrambi gli agenti. Quindi sia t2 il tempo in

cui l'Ag2 arriva al nodo in questione, la pianicazione potrà avvenire solo se:

t2− tnow > 2TA (9)

La stessa cosa vale per l'Ag2 in caso di dissoluzione della congurazione treno, in tal caso si dovrà considerare il tempo in cui Ag3 raggiunge il nodo e quindi in eq.9 bisognerà considerare t3al

4.4 Gestione tamponamenti su stesso arco

Come è stato già accennato in 3.2.2, possibili scontri possono avvenire anche sugli archi del grafo (tamponamenti). Si considerino due agenti ri e rj che si trovano su uno stesso arco che collega il

nodo A con il nodo B (g.14), avverrà un tamponamento se vale: τ(A)i > τ (A)j e τ(B)i < τ (B)j

cioè se rj passa per primo sul nodo A e ri passa per primo per il nodo B.

Figura 14

Ciò può avvenire se il robot ri ha una velocità massima maggiore di rj. Lo sfruttamento della

congurazione treno e dei nodi di posizione permette di risolvere questo problema.

In particolare per rilevare un tamponamento imminente è suciente analizzare i tempi di arrivo sui nodi di posizione (last,next,next2 ) e indurre una nuova pianicazione ritardata a ri in modo da

fargli raggiungere il nodo B dopo rj. Per ragioni di sicurezza (ovvero per agire più velocemente)

questo viene fatto non appena viene rilevata una possibile congurazione treno senza attendere la sua eettiva formazione.

È invece possibile evitare un futuro tamponamento, che avverrebbe in prossimità di nodi successivi a quelli di posizione, imponendo che una volta entrati in congurazione treno la nuova pianicazione sia realizzata con una velocità massima pari al minimo tra la velocità dell'agente e quella dei veicolo immediatamente precedente.

4.5 Dimensione massima del treno

Per evitare treni composti da un numero eccessivo di 'vagoni' è necessario poter stabilire una di-mensione massima del treno DimT rainmax. Questa sarà scelta in base al problema particolare e

risponde all'esigenza di poter limitare il tempo di attesa dell'agente non in treno.

Una DimT rainmax = 0signicherà che non è possibile creare congurazioni treno, questo può

es-sere utile in quei problemi in cui le priorità rappresentano una eettiva necessità che alcuni agenti arrivino velocemente al proprio obiettivo. Se invece le priorità sono solo un espediente per risolvere i conitti agli incroci assegnando le varie precedenze, allora la congurazione treno può essere utile come spiegato precedentemente per limitare il numero di agenti a cui è imposto un rallentamento. Nel caso in cui ci sia un limite alla dimensione del treno (o non sia ammesso alcun treno) rimane però attivo il riconoscimento della possibile congurazione in modo da poter comunque gestire eventuali tamponamenti.

5 Implementazione

Gli algoritmi sono stati implementati in C++ usando il framework OMPL per la pianicazione e ROS per il controllo dei robot. La simulazione utilizza sia Gazebo (per una visione 3D del problema, g.15) che Rviz (con cui è stato possibile ragurare gli intervalli di occupazione dei vari nodi come ostacoli cilindrici, g.16).

Figura 15: Visualizzazione in Gazebo

Figura 16: Visualizzazione in Rviz

In g.16 si può vedere anche una rappresentazione della mappa. In particolare in blu sono rappresentati gli archi del grafo del particolare problema.

5.1 Mappa

La mappa su cui si muovono gli agenti nasce da un grafo i cui archi diventano le 'strade' percorribili. Ogni grafo è stato realizzato in formato gml tramite l'editor yEd. Tale formato descrive ogni elemento del grafo come una lista di archi e nodi. Le distanze sono espresse in pixel e verranno convertite in metri all'interno del codice implementato. L'utilizzo dell'editor permette di avere una visione 2D della mappa con i suoi archi e nodi, un esempio di un grafo è riportato in g. 17 (altro non è che la mappa che si può vedere anche in g.16).

Figura 17: Esempio di grafo realizzato con yEd

Come già spiegato nel paragrafo 3.1 nella creazione della mappa è necessario prestare molta attenzione alla distanza tra i nodi. In particolare l'eq. 3 nasce dall'esigenza di permettere ad ogni agente di eettuare almeno una pianicazione ad ogni aggiornamento dei nodi di posizione. La generazione di un grafo che rispetti le proprietà descritte nel Capitolo 3 risulta quindi fondamentale per l'utilizzo della Congurazione Treno e più in generale perché l'approccio abbia successo.

5.2 Struttura codice

Il codice è suddiviso in due nodi: il pianicatore che si occupa della pianicazione della traiettoria e dello scambio di informazioni e il controllore che gestisce lo spostamento del robot e l'aggiornamento dei nodi. È bene ricordare che il problema è di tipo distribuito, ogni agente avrà il suo pianicatore e il suo controllore.

Figura 18: Struttura

Come si può vedere in g. 18 il pianicatore calcola la traiettoria basandosi anche sulle condizioni iniziali del problema. Tale traiettoria viene passata al controllore. Questo tra le altre cose aggiorna i nodi di posizioni online e li invia al pianicatore.

5.2.1 Condizioni iniziali

Le condizioni iniziali vengono impostate in un launch le. Queste includono tra le altre: • Condizioni generali per tutti i robot

 grafo

 scala della mappa (permette la conversione da pixel a metri)  massima dimensione del treno

 booleano per poter attivare la scelta interattiva del goal • Condizioni particolari per ogni robot

 nodo di partenza (start)  nodo di arrivo (goal)  priorità

5.2.2 Assegnazione goal

Come visto è possibile assegnare il nodo di arrivo in due modi. Si può assegnare oine tramite launch le oppure è possibile assegnarlo interattivamente su Rviz. Ponendo a true un booleano nel launch le la pianicazione partirà solo quando si sarà cliccato sul punto di arrivo desiderato sulla mappa (tale punto sarà convertito nel nodo più vicino). Data la natura distribuita del problema, assegnazioni non simultanee dei goal per i vari agenti non comportano alcun problema di sincronizzazione. È anche possibile, una volta che il robot è già in moto per raggiungere il suo obiettivo, assegnare un goal successivo a quello attuale, che porterà ad una nuova pianicazione una volta che l'agente avrà raggiunto il primo goal, ovviamente ciò è possibile solo se la struttura del grafo lo consente infatti il grafo è orientato, quindi perché sia possibile è necessario che il nodo di arrivo abbia anche almeno un arco uscente. Qualora durante il moto di un robot avvenissero più assegnazioni di goal successivi, al termine della traiettoria attuale, il goal scelto sarà l'ultimo indicato. In altre parole non è prevista una coda di goal successivi. Poiché infatti la scelta del goal avviene manualmente, evitare la memorizzazione di tutte le assegnazioni permette di correggere rapidamente un eventuale errore nel goal scelto andando semplicemente ad assegnare quello corretto (ovviamente prima che il robot raggiunga il suo attuale obiettivo).

5.3 Pianicazione

Per la parte di pianicazione ci si appoggia alla libreria OMPL (Open Motion Planning Library) che consiste in algoritmi di pianicazione del moto basati sul campionamento.

Questa sezione può essere vista come suddivisa su 2 livelli come si può vedere in g. 19.

5.3.1 Algoritmo MrFert

Il livello 1 riceve le condizioni iniziali dal livello 2, queste sono: la velocità massima, il nodo di partenza e quello di arrivo e TA. Determina quindi una traiettoria (se esiste) costituita da una

successione di punti nello spazio-tempo.

Innanzitutto viene trovata una prima traiettoria, non ottimale, che permetta di raggiungere l'obiet-tivo. Sia T1 il tempo necessario al calcolo di questa traiettoria iniziale (con T1 < TA), nel tempo

restante (TA− T1) l'algoritmo ricerca una traiettoria a costo minore come descritto in 3.2.1.

Facendo riferimento al procedimento per il calcolo della prima traiettoria è opportuno precisare co-me questo abbia necessitato una leggera modica rispetto a quanto teorizzato in precedenza. Si è notato che, nel caso di molti agenti intenzionati a passare per lo stesso nodo contemporaneamente, gli agenti con priorità più bassa faticano a trovare un τ di passaggio per il nodo tale che l'intervallo non intersechi quello degli altri agenti con priorità maggiore.

(a) (b) (c)

Figura 20: Ricerca di un intervallo ammissibile per Ag4 durante il calcolo della prima traiettoria Facendo riferimento alla g. 20 si vede che se l'Ag4 potesse procedere a velocità massima avrebbe una collisione con Ag2 (a). Nel caso (b) si mette in evidenza il problema sopra descritto. Si osserva che il τ trovato in maniera random si trova in un intervallo libero tra Ag2 e Ag3. Poiché però l'Ag4 deve necessariamente rallentare, la velocità di ingresso al nodo è più bassa di quella massima e ciò provoca un intervallo di occupazione del nodo più largo che lo porta ad intersecare quello dell'Ag2 (si ricordi infatti eq. 4). Ciò porta l'algoritmo a ricercare un nuovo τ. Dato che la generazione random di τ può avvenire un numero limitato di volte (es. 1000) per ovvi motivi temporali (TA) e

che il nodo può essere occupato per diversi intervalli successivi (qui ad esempio dall'Ag3), potrebbe succedere di non trovare un τ ammissibile. Poiché questo è il calcolo della prima traiettoria, un suo fallimento coincide con una pianicazione non trovata.

Per ovviare a questo problema invece di campionare τ in modo random, si sceglie quello opportuno in base alla conoscenza dei tempi di occupazione del nodo da parte degli agenti a priorità maggiore. In particolare si sceglie il primo τ buono, cioè quello minimo tale da rispettare i vincoli sulla velocità e evitare collisioni.

5.3.2 Condivisione delle informazioni e check collisioni

Ogni agente invia un messaggio in un topic condiviso con tutti i robot. Tale messaggio contiene le informazioni necessarie per il coordinamento volto ad evitare collisioni e per la congurazione treno. Tra le varie informazioni ci sono: la traiettoria, la priorità originaria e quella attuale, la velocità massima eettiva e quella attuale e vari segnali booleani (ad es. quello di dissoluzione del treno). Ogni robot riceve tali messaggi solo dagli agenti che si trovano nel range di comunicazione e con-frontando tali informazioni con le proprie condizioni valuta la presenza o meno di collisioni (check collisioni) e di treni.

Nel controllo delle collisioni l'agente valuta se le traiettorie degli agenti con priorità attuale maggiore vanno a collidere con la propria e in tal caso avviano una nuova pianicazione che avverrà tenendo conto delle nuove conoscenza sugli intervalli di occupazione dei vari nodi.

5.3.3 Treno

Si faccia riferimento alla g. 21.

Figura 21: Implementazione congurazione treno

Si consideri l'agente ri, tale robot determina se è in condizioni di treno possibile o meno basandosi

sulle informazioni ricevute dagli agenti circostanti (rj). In particolar modo avviene un confronto dei

nodi di posizione. Se ri risulta in treno 'possibile' a rj denisce quest'ultimo come suo master. Da

notare che il master non è necessariamente il veicolo con priorità maggiore ma quello immediatamente davanti cioè è una proprietà che dipende esclusivamente dalla posizione nel treno.

Se ri rileva un imminente tamponamento (che è destinato a vericarsi sugli archi che connettono i

nodi di posizione), avvia una pianicazione che lo rallenti (pianicazione ritardata) e invia un segnale per avvertire un suo eventuale slave, cioè un agente che ha riconosciuto ricome proprio master. Tale

slave, se esiste, ripianica a sua volta con lo stesso ritardo.

Se la condizione di treno passa da possibile a eettiva ri assume la priorità del master poi avvia un

confronto tra la sua nuova priorità j e quella reale i. Qualora risulti i < j invia un segnale a rj di

passaggio in aventi della priorità. Se invece i > j eettua una nuova pianicazione se la distanza di sicurezza da eventuali altri robot lo permette.

Allo stesso tempo rj riceve le informazioni da ri dalle quali potrà, nel caso, denirlo suo slave e se

presente un messaggio di passaggio in aventi insieme alla nuova priorità, la sostituisce alla sua e avvia una nuova pianicazione.

Nel caso in cui invece ri perda la sua congurazione treno, ristabilisce la sua priorità e invia un

segnale di ne treno. Sia rk un agente (non ragurato) presente nel range di comunicazione di ri e

non facente parte dello stesso treno. Se riceve un messaggio di ne treno da ri confronta le priorità

5.4 Controllo del moto

Il nodo del controllore riceve la traiettoria dal pianicatore e si adopera anché il robot raggiunga i vari nodi al tempo stabilito. In particolare, denita l la distanza tra la posizione attuale e il prossimo nodo sulla traiettoria (next target), e sia ∆ = τnext− τnow, si impone una velocità

vx,d= Kp,1

l

∆ (10)

che sarà valida quando l'agente sta procedendo dritto (g. 22).

Figura 22: Velocità robot

Quando l'agente si trova in prossimità del next target inizia la curva per permettergli di allinearsi alla nuova direzione di moto che lo porterà verso il nuovo next target. La velocità lineare sarà pari ad una media tra quella attuale e quella che dovrebbe avere nel successivo arco (tale velocità è evidenziata in rosso in gura).

vx,c=

v(x,d)1+ v(x,d)2

2 (11)

Durante la curva ci sarà anche una velocità angolare che permetta l'allineamento, sia θe= θ2− θ1,

allora:

ωz= −Kp,2sin θe (12)

Il controllore ha anche il compito di aggiornare i nodi di posizione ogni volta che avviene il passaggio da un arco al successivo e di inviare tale informazione al pianicatore.

6 Simulazioni

Allo scopo di analizzare le prestazioni di robot gestiti in maniera distribuita tramite l'algoritmo MrFert sono stati pensati 4 diversi scenari:

• Incrocio • Rotonda

• Scenario generale: magazzino • Treno

L'incrocio e la rotonda possono essere visti come due scenari agli antipodi e si possono ipotizzare quindi risultati molto dierenti, in maniera simile a quanto avviene realmente nelle strade. Lo scenario generale invece cerca di unire incroci e rotonda e ricreare un ambiente simile a quello di lavoro. Inne si osserva la dierenza che avviene in presenza di treno e senza.

L'analisi delle performance si concentrerà su il tempo necessario al robot per raggiungere il suo obiettivo e lo confronterà con il tempo ideale che impiegherebbe se non ci fossero altri agenti sulla mappa. Si osserverà anche la presenza o meno di intervalli di tempo in cui l'agente è fermo. Per come è stato realizzato l'algoritmo è facile notare che gli unici momenti un cui il robot è eettivamente fermo, cioè quando v = 0, sono sul nodo di partenza prima che si avvii la pianicazione e sul nodo di arrivo quando ha terminato il suo moto. Allora il robot si denisce fermo se sta procedendo molto lentamente rispetto alla sua velocità massima (v < 10% di vM ax). Un'altra grandezza di interesse

sarà la velocità media che l'agente manterrà durante il suo percorso. È opportuno evidenziare che dove non precisato le velocità sono sempre espresse in m/s.

Il numero degli agenti presenti sulla mappa varia da 1 no ad un massimo di 8, in questo modo è possibile vedere anche le dierenze dovute alle priorità dei robot riscontrate nelle varie simulazioni. Dove risultava signicativo fare un numero elevato si simulazioni queste sono state fatte andando a modicare in modo random le condizioni iniziali (nodo di partenza e nodo di arrivo).

6.1 Incrocio

In questo scenario tutti gli agenti si trovano su una circonferenza e devono passare per il centro per raggiungere ognuno il proprio obiettivo (situato sempre sulla circonferenza). Come si può vedere anche in g. 23 i punti di partenza e di arrivo sono ben distinti, in blu sono evidenziati i nodi di start e in rosso i nodi di goal. È opportuno inoltre specicare che oltre ai nodi posti sulla circonferenza e a quello centrale sono presenti anche dei nodi intermedi (non ragurati) lungo i vari raggi.

Figura 23: Mappa

Innanzitutto si è considerato l'agente r8, cioè quello con priorità più bassa, che deve realizzare

sempre lo stesso percorso ma con un numero di robot presenti sulla mappa via via maggiore (si passa dalla condizione in cui r8 è l'unico agente a quella in cui ci sono anche altri 7 agenti con priorità

maggiore: r1...r7).

La velocità massima imposta per ogni agente è sempre vmax= 0.4 m/s.

In g. 24 si può osservare un tipico andamento delle velocità lineari e angolari per r8 in presenza

di un altro robot. Osservando la velocità lineare si vede come sia presente un calo iniziale dovuto proprio alla presenza di un altro agente a priorità maggiore. La velocità angolare evidenzia il tipo di traiettoria seguita dal robot, questo avanza in linea retta lungo il raggio della circonferenza no ad arrivare al centro per poi modicare la sua orientazione per raggiungere il suo obiettivo.

(a) (b)

I risultati seguenti sono la media di 8 congurazioni iniziali dierenti per ogni robot (start e goal); per ogni congurazione si è aumentato il numero di robot come precedentemente descritto. Si può osservare l'incremento del tempo di percorrenza per r8in g. 25. Si passa da un tempo ideale

di 41.1404 s no a raggiungere i 126.1926 s quando l'agente è costretto a dare la precedenza a tutti gli altri 7 agenti. Si può vedere che l'incremento temporale è consistente (206%).

Figura 25: Tabella dei tempi di percorrenza

Figura 26: Rappresentazione graca dei tempi di percorrenza

In g. 26 di può osservare come l'aumento temporale sembra avere un andamento lineare. Per quanto riguarda la velocità media dell'agente in g. 27 si vede come nel caso ideale v = 0.38 m/s sia abbastanza vicina a quella massima (che ricordiamo è pari a 0.4 m/s). Si può vedere (g. 29) che la diminuzione inizialmente è lineare per poi diventare più dolce, questo è spiegabile col fatto che una volta che l'agente ha passato il nodo centrale (e ha quindi dato precedenza agli altri robot) può procedere a velocità sostenuta.

Si osserva anche che, quando l'agente deve dare precedenza a 4 o più robot, ha dei periodi in cui la sua velocità può essere equiparata a quasi fermo.

Per mostrare più in dettaglio la dierenza dell'andamento della velocità e del tempo di percor-renza nei due casi estremi (1 agente e 8 agenti), si può osservare la g. 30, questa evidenzia bene quanto detto in precedenza. Si osserva infatti la netta dierenza temporale necessaria a raggiungere