` DEGLI STUDI DI PADOVA UNIVERSITA

Facolt` a di Ingegneria Corso di Laurea in Ingegneria dellAutomazione L.S.

Corso di Progettazione dei sistemi di Controllo a.a. 2008/09

Navigazione autonoma di robot in ambiente sconosciuto

Pietro Salvagnini, Francesco Simmini, Michele Stoppa Matricola: 586929 586206 586204 pietro.salvagnini@gmail.com; francesco.simmini@gmail.com; stoppa.michele@gmail.com Padova, 16 Febbraio 2009

Navigazione autonoma di robot in ambiente sconosciuto

Francesco Simmini, 586206; Pietro Salvagnini, 586929; Michele Stoppa, 586204

` la navigazione autonoma AbstractIl problema affrontato e di robot in ambiente sconosciuto. Ci si propone di utilizzare N robot esploratori dotati di una piccola telecamera, ed un attore cieco che conosce solo la sua posizione. Si deve individuare un percorso dalla posizione iniziale dellattore ad un obbiettivo noto, attraverso la costruzione di una mappa dellambiente. A tal ` ideata una soluzione effettivamente implementabile, e scopo si e stato predisposto un originale ` testata sperimentalmente. E la si e sistema di controllo che gestisce in tempo reale le complicate operazioni richieste dal problema. Si propone una soluzione al problema del map-building, legata al particolare sensore di visione; quindi si presenta un nuovo algoritmo di esplorazione, ottimo secondo un ragionevole criterio di selezione delle zone ` adottato un algoritmo di ricerca dei cammini da esplorare. Si e minimi per il problema del path-planning offerto dalla letteratura. ` utilizzato un sistema GPS virtuale ottenuto applicando un Si e algoritmo di visione alle immagini tratte da una telecamera ssata sopra al piano di lavoro. I test sperimentali hanno vericato la validit` a del sistema proposto per due robot esploratori ed un attore cieco.

Appendice A: Aspetti tecnici ed implementativi A-A Impostazione dellarea di ripresa della telecamera . . . . . . . . . . . . . . . . A-B Mapping . . . . . . . . . . . . . . . . . A-C Path-Planning . . . . . . . . . . . . . . A-D Rilocalizzazione . . . . . . . . . . . . . A-E Tecniche per la simulazione del Mapping Appendice B: Apparato Strumentale Riferimenti bibliograci I. I NTRODUZIONE

16 16 16 17 17 17 17 19

I NDICE I II III IV Introduzione Panoramica Generale Il Sistema di Controllo Mapping IV-A Stato dellarte . . . . . . . . . . . . IV-B Tipologia e dimensione della mappa IV-C Rilevamento degli ostacoli . . . . . IV-D Costruzione della mappa . . . . . . IV-E Aggiornamento della mappa . . . . Esplorazione V-A Stato dellarte . . . . . . . . V-B Algoritmo di esplorazione . . V-C Estensione Multirobot . . . . V-D Terminazione dellalgoritmo 1 2 3 4 4 5 6 6 7 8 8 9 10 11 11 11 13 13 14 14 15

. . . . .

. . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

VI

Lalgoritmo di ricerca dei cammini VI-A Lalgoritmo D* (D-star) . . . . . . . . . Controllo della traiettoria e rilocalizzazione VII-A Controllo della traiettoria . . . . . . . . VII-B Rotazione del robot . . . . . . . . . . . VII-C Rilocalizzazione . . . . . . . . . . . . .

VII

VIII Risultati e conclusioni

` la navigazione autonoma di robot in ambiente L problema e sconosciuto. Si hanno a disposizione N robot dotati di sensori dai quali si pu` o trarre informazione dellambiente circostante, ovvero essenzialmente la presenza o meno di ostacoli, e un robot privo di sensori (cieco) che si vuole far viaggiare no ad un punto obbiettivo di coordinate note. Lidea ` quella di costruire tramite i sensori una mappa del territorio, e e di pianicare su questa il percorso per lattore cieco. ` adottata una losoa progettuale Per ottenere lo scopo si e che si pu` o riassumere in tre punti fondamentali. Realismo: utilizzare tutti gli strumenti a disposizione per mantenere il progetto il pi` u generale possibile, limitando linserimento di sensori simulati; Funzionalit` a: creare un sistema effettivamente funzionante; Semplicit` a: cominciare da tecniche semplici, implementabili praticamente, da rafnare solo dopo averne vericato il funzionamento per ottenere risultati migliori. Questi criteri hanno indirizzato ogni scelta, in particolare lutilizzo della telecamera montata sui robot come sensore di visione, piuttosto che simulare altri sensori non presenti. Il problema affrontato risulta piuttosto particolare, con problematiche relative alla particolare strumentazione utilizzata. Il sistema globale di controllo e larchitettura generale sono state quindi ideate ad hoc, mentre per quanto riguarda la soluzioni ` potuti confrontare con una vasta delle singole tematiche ci si e letteratura, per maggiori riferimenti a riguardo si rimanda alle singole sezioni. Il raggiungimento dello scopo richiede la soluzione dei seguenti problemi fondamentali. Il problema del mapping consiste sostanzialmente nel trovare una trasformazione dallimmagine presa dalla telecamera del robot al piano di lavoro visto dallalto, cio` e nella costruzione di unusuale mappa del territorio. Per la rappresentazione ` afdati a tecniche ben conosciute in della mappa ci si e letteratura mentre lutilizzo della telecamera ha richiesto un contributo originale per una precisa e veloce ricostruzione dellambiente.

Lexploration si propone di trovare un metodo per decidere ` la zona pi` qual e u interessante da esplorare, cercando una soluzione possibilmente ottima secondo qualche criterio, e considerando lo scopo di trovare una strada che congiunga la posizione dellattore cieco allobbiettivo. Lalgoritmo di ` stato elaborato a partire da alcune proposte esplorazione e gi` a implementate, adattandolo alla particolare tecnica di costruzione della mappa ed alla focalizzazione verso la zona dellobiettivo a cui condurre lattore cieco. Il path-planning vuole identicare una traiettoria tra due punti in modo da evitare gli ostacoli che possono trovarsi in mezzo, e generare un segnale di riferimento di posizione da ` sfruttato far inseguire al robot per spostarsi. Per fare ci` o si e un noto algoritmo di ricerca di cammini minimi presente in letteratura. ` Il problema della rilocalizzazione tramite GPS virtuale e sostanzialmente un problema di visione computazionale: individuare in unimmagine il robot, e trasformarne le cordinate in metri per stimarne la posizione sulla mappa. Inne, un problema noto nei controlli automatici di sistemi ` il trajectory-tracking: controllare in retroameccanici che e zione il robot in modo da inseguire un riferimento dato, nel nostro caso in posizione. Si presenta ora la soluzione nella sua globalit` a, nelle sezioni successive si presentano invece le soluzione speciche per i problemi elencati. II. PANORAMICA G ENERALE Si devono coordinare N + 1 robot, N nella loro azione di ` il moto verso esplorazione, e uno la cui azione principale e lobbiettivo. Gli N robot esploratori verranno denominati Explorer, laltro Actor. Tutti i robot sono dotati di sensori di posizione, cio` e di encoder posti sugli alberi dei due motori dai quali si pu` o trarre facilmente informazione sulla posizione e sulla direzione dei robot a partire da una condizione iniziale nota. Questi sono gli unici sensori a disposizione dellActor, mentre gli Explorer hanno un sensore visivo, una piccola telecamera, dalla quale si traggono le informazioni per ricostruire lambiente circostante. I robot non hanno la capacit` a di comunicare direttamente tra loro, ma comunicano con ununit` a di calcolo centralizzata che raccoglie le informazioni provenienti dai sensori, le elaborara, ` e trasmette nuovi ordini ai robot. Il sistema di controllo e quindi di tipo centralizzato. Si suppone tutti i robot siano posizionati inizialmente in ` semplice. Ogni unarea priva di ostacoli (ovviamente). Lidea e singolo robot Explorer, dalla posizione di partenza in cui si trova dovrebbe: 1) fare una foto e aggiornare la mappa (Mapping); 2) eseguire lalgoritmo di esplorazione, che porger` a una nuova destinazione e orientamento verso cui esplorare (Exploration); 3) eseguire lalgoritmo di Path-Planning; 4) eseguire il moto controllato in retroazione sugli encoder (Moto); 5) correggere la propria posizione grazie al GPS virtuale (Rilocalizzare);

6) girare nella direzione individuata al punto 3) (Girare); 7) ricominciare dal punto 1). e proseguire cos` nch` e non viene avvistato lobbiettivo e ci ` accertati che e ` raggiungibile. Lactor intanto dovrebbe si e 1) aspettare aggiornamenti della mappa (Wait); 2) calcolare la strada pi` u breve con linformazione nota a questa a punto, supponendo lo spazio ancora ignoto privo di ostacoli, e decidere se partire (Path-Planning) ` conosciuta 3) se si parte, muoversi no a dove la mappa e e tornare al punto 1) (Moto) e proseguire nch` e non si giunge allobbiettivo. Per Mapping si intende la costruzione della mappa attra` scelto di verso le successive misure raccolte dai robot. Si e rappresentare la mappa mediante le occupancy grid cell, si ` costituita da una matrice in cui ogni veda [1], cio` e la mappa e cella corrisponde ad unarea di 1 cm2 ed il valore corrisponde alla probabilit` a che una cella sia occupata. Le misure sono ottenute elaborando linformazione della telecamera dei robot: nellimmagine si individuano gli spigoli orizzontali degli ostacoli tramite un opportuno algoritmo di visione. Dalla loro posizione nel piano immagine si risale alla loro posizione nel piano reale attraverso alcune nozioni di geometria proiettiva. Intuitivamente pi` u il bordo si trova in alto nellimmagine, pi` u lostacolo sar` a distante dal robot. Dalla colonna dellimmagine in cui compare lostacolo si possono ricavare informazioni sulla sua direzione rispetto al robot. Banalmente, un ostacolo che si trova sulla destra dellimmagine sar` a sulla parte destra del cono di visione del robot. La fase di Exploration consiste nellassegnare ad ogni robot la successiva posizione da cui effettuare la fotograa. In primo luogo si individuano sulla mappa le frontier cells, cio` e quelle celle esplorate che si trovano vicine alla regione inesplorata. Queste celle vengono ordinate in base ad un indice che tiene conto della loro vicinanza allobiettivo e della distanza dalla posizione attuale. Inoltre nel caso di pi` u robot si va ad aggiungere un termine che mantenga i robot ad una distanza minima tra di loro. Ad ogni robot viene quindi assegnato come punto di esplorazione successivo il punto che minimizza questindice. Il Path-Planning serve ad individuare la traiettoria che ogni robot deve percorrere per andare da un punto ad un altro, evitando gli ostacoli e passando solo per le regioni esplorate. ` implementato lalgoritmo D*, un algoritmo di ricerca Si e di cammini minimi su un grafo. Esso risulta molto efcace anche per lindividuazione della traiettoria che porti lActor allobiettivo, in quando aggiorna di volta in volta il percorso in base all nuove osservazioni degli Explorer. Inne la parte di Rilocalizzazione serve a correggere la posizione del robot. Tramite un algoritmo di blob-detection si va ad individuare il robot cercato sullimmagine ottenuta dal GPS virtuale. Quindi con una veloce normalizzazione se ne ottengono le coordinate nel sistema di riferimento assoluto. Nella prossima sezione sar` a illustrato il sistema di controllo globale che gestisce i vari robot ed assegna loro le operazioni da compiere. In quelle successive saranno affrontati nel dettaglio i problemi relativi alle singole operazione sopra presentate.

III. I L S ISTEMA DI C ONTROLLO

Le azioni computazionalmente onerose che gli Explorer devono compiere sono sostanzialmente quattro: la costruzione della mappa, la scelta del punto di esplorazione successivo (e della direzione verso cui guardare), la pianicazione del percorso dalla posizione attuale a quella nuova, laggiornamento della posizione dei robot tramite GPS virtuale. Questo si rivela necessario perch` e lodometria ricavata dagli encoder dei motori soffre del problema della deriva: piccoli errori nei sensori portano dopo tempi lunghi (ma non tanto) ad un inevitabile errore di posizione. Nel caso dei nostri robot, gli e-puck, tale errore si aggira intorno ai 5 cm su un moto di 40-50 cm. A questo si potrebbe rimediare sfruttando le informazioni sullambiente esterno date dai sensori visivi, ma ` utile aggiungere ` un problema noto e complicato, che si trova in A questi e questo e letteratura sotto il nome di Slam (Simulataneous Localization 0. WAIT: stato di attesa. And Mapping). Qui invece si suppone la posizione dei robot ` solo quella essere nota, cio` e si nge un sistema tipo GPS attraverso una L azione principale che lActor deve compiere e di pianicare il proprio percorso dalla posizione attuale a telecamera montata sopra lambiente di lavoro, dalla quale si quella del goal e decidere quando partire. Tutto ci` o pu` o ricava la posizione dei robot. Oltre a queste quattro azioni, essere inserito in uno stato simile al 3 dellelenco sopra, con ogni robot deve ovviamente potersi muovere, ma questa azione ` comodo la differenza che la traiettoria va pianicata no al punto non richiede tempi di calcolo impegnativi. Inoltre, e introdurre una distinzione tra lazione di moto vero e proprio obbiettivo, e laggiunta di una decisione su quando partire. e quella del girarsi soltanto attorno al proprio asse verticale, Non essendo chiaramente possibile denire una condizione o essere nota a priori mantenendo la stessa posizione, perch` e questi due tipi di moto ottima di partenza, in quanto non pu` ` compiuta, si tutta la strada se non quando lesplorazione si e verranno gestiti in maniera diversa. ` e deciso di far partire l Actor quando la strada nota supera il Lunica azione computazionalmente onerosa che deve comdoppio della strada minima mancante per raggiungere il goal. ` quella della ricerca del cammino minimo dalla piere lactor e ` Questa informazione e facilmente ricavabile dalla natura della sua posizione attuale a quella dellobbiettivo, oltre naturalmensoluzione al problema di Path-Planning utilizzata. L Actor te allaggiornamento anche della sua posizone tramite GPS deve poi saper rilocalizzare la propria posizione, girarsi, e virtuale. ` dotato del software M ATLAB, che e ` muoversi. Ne consegue che i suoi stati possono essere ridotti Il computer centrale e il responsabile di ogni tipo di calcolo, e attraverso il quale agli ultimi quattro dellelenco, con lo stato 3 modicato come si sono implementati i diversi algoritmi. Grazie al pacchetto detto. ` stata in fatta in modo da avere La suddivisione degli stati e ` possibile ePic2, disponibile sul sito ufciale degli e-puck 1 , e u o meno uniforme, comunicare con i robot attraverso M ATLAB, acquisendo le un tempo di esecuzione di ogni stato pi` informazioni dai sensori, dando i segnali di comando ai motori e la scelta del tempo di campionamento T deve soddisfare ` il tempo massimo di esecuzione e via dicendo. Il problema serio che si pone subito sono i T > Tex /(N +1), dove Tex e importante mantenere una suddivisione u lento. E tempi di calcolo e di trasmissione tra i robot e il computer. dello stato pi` ` certo uniforme del tempo nel caso ci sia almeno un robot che si sta Infatti, M ATLAB, seppur molto comodo e intuitivo, non e ` il muovendo, in modo da poter acquisire e comandare il robot a famoso per la sua rapidit` a di esecuzione, e altrettanto lento e chiaro invece che da un punto di vista protocollo bluetooth utilizzato per la trasmissione. Queste len- intervalli regolari. E tezze comportano inevitabilmente un tempo di campionamento logico gli stati 1, 2 e 3 potrebbero essere inglobati in un unico stato, ma il suo tempo di esecuzione risulterebbe troppo molto alto. ` organizzato il sistema in modo alto. Un tempo di campionamento piccolo implica migliori Per questo motivo, si e a di movimento dei robot, in che ad ogni passo ogni robot compia una sola tra le azioni prestazioni in termini di velocit` descritte pocanzi. Si assegna uno stato ai robot. Ad ogni stato quanto si ha un maggiore controllo, e dunque si vorrebbe u basso possibile. La suddivisione dei calcoli in corrisponde unazione, e ad ogni passo si controllano gli stati mantenerlo il pi` ` una soluzione a questa problematica. Per lutilizzo di u stati e dei singoli robot e si fa eseguire loro la rispettiva azione. In pi` questo modo, ad ogni passo si dovranno compiere N azioni due Explorer ed un Actor il tempo di campionamento minimo ` pari a 2 secondi. denite dagli stati degli N Explorer, pi` u una denita dallo e stato dellActor. Ogni stato comporta ovviamente delle problematiche da Di seguito, le denizione degli stati degli Explrer, con una risolvere, che sono oggetto di studio delle sezioni successive. descrizione dettagliata delle operazioni da eseguire in ognuno Supponiamo ora di saper risolvere tutte queste problematiche e presentiamo un algoritmo di controllo che esegua lo scopo di essi. introdotto precedentemente: esplorare il territorio no a trovare 1 http://www.e-puck.org un percorso privo di ostacoli dalla posizione dellActor ad una

1. M APPING : il robot deve fare una foto, elaborarla, mandarla al pc, il pc costruisce la mappa. 2. E XPLORATION : il pc deve scegliere un nuovo punto da esplorare. 3. PATH -P LANNING : il pc deve individuare una strada dal punto dove si trova il robot, al punto di osservazione denito da 2.E XPLORATION. 4. G IRARE : il pc calcola i segnali da mandare al robot per farlo girare e li trasmette, il robot gira. 5. R ILOCALIZZARE : si fa una foto dellambiente, il pc la elabora e trasmette la nuova posizione al robot. 6. M OTO : il pc calcola i segnali da mandare al robot per fargli seguire la traiettoria denita in 3.PATH -P LANNING e li trasmette.

posizione obbiettivo le cui coordinate sono note a priori, e contemporaneamente farvi giungere lActor. Allidea principale esposta in sezione II si aggiungono una serie di aspetti tecnici che complicano leggermente questo schema. Primo fra tutti: la visuale della telecamera degli ` troppo stretta. Si rende necessario eseguire almeno Explorer e una sequenza di 3 foto per coprire un area ragionevole di territorio. Ci sono poi alcune operazioni pi` u delicate di altre. Come ` importante un sincronismo gi` a accennato, durante il moto e del tempo al quale si accede alle informazioni degli encoder e si calcola il comando da dare al robot. Infatti, il sistema calcola tale comando supponendo un tempo di campionamento regolare, T , durante il quale il segnale di controllo viene mantenuto costante. Naturalmente, eventuali imprecisioni sulla posizione allistante successivo verranno corrette dalla retroazione, ma rimane importante che lintervallo sia circa quello di campionamento. Per questo motivo, si sceglie di servire i robot ad ogni passo a partire da quello che ha stato maggiore, che corrisponde allo stato di moto, in modo da non risentire dei ritardi causati dai calcoli necessari agli altri stati. Per semplicare il moto, inoltre, si fa in modo che il robot, prima di partire, si giri nella direzione giusta, cio` e quella individuata dai primi passi del percorso. Poi, gli stati dellActor includono azioni leggermente diver` se da quelli degli Explorer, e il loro ordine di esecuzione e pure diverso. Li si distingue mantenendogli lo stesso numero intero, ma cambiato di segno. Ad esempio, lo stato PATH P LANNING dellActor verr` a contrassegnato con -3. Anche per ` necessaria una rilocalizzazione tramite GPS virtuale, lActor e afnch` e la posizione presunta del robot non sia errata a tal punto da farlo scontrare contro un ostacolo (o un altro robot). Il moto viene quindi interrotto a intervalli regolari per correggere la posizione del robot, a cui segue ovviamente una nuova pianicazione. Inoltre, i tempi di calcolo del cammino minimo da parte dellActor si sono rivelati decisamente troppo lunghi. ` fatto in modo che il calcolo venga fatto soltanto in Allora, si e una situazione in cui tutti i robot sono fermi, introducendo un ulteriore stato di attesa del momento opportuno per lActor. ` che cambi molto le cose, diciamo che al peggio il Questo non e robot scopre leggermente in ritardo la condizione di partenza ` pi` postagli, cio` e, ricordiamo, che la strada nota e u di met` a della strada minima mancante. Inne, manca una gestione di una situazione di emergenza ` fatto lalin cui due robot si scontrano. In verit` a, per come e goritmo di esplorazione, non pu` o accadere che due Explorer si scontrino, perch` e esso provvede a dividere larea di ricerca tra i due robot, che si mantengono dunque distanti. Invece, in una situazione nale in cui tutti i robot possono trovarsi vicino ` pi` al goal, e u probabile uno scontro tra Explorer e Actor. Per evitare questo, si fa in modo che una volta visto lobbiettivo, e constatata la presenza di una effettiva traiettoria possibile per raggiungerlo, si sposta lExplorer in questione in una direzione opposta a quella di partenza dellActor, cio` e dietro lobiettivo, in modo che lActor, dovendo necessariamente essere pi` u indietro, non si scontri. Per` o, non sempre esiste un punto conosciuto tanto distante da essere certi di evitare ` posto rimedio occupando nella lo scontro. A questo, si e

mappa unarea corrispondente a quella del robot, proprio come fosse un ostacolo, in modo che lalgoritmo di pianicazione lo eviti, essendo certi che questo non si sposter` a pi` u in quanto ha concluso il suo compito. ` tratto il seguente algoritDa tutte queste considerazioni, si e mo di controllo, leggibile in coppia con la tabella I, che illustra uno schema di transizione tra lo stato corrente e il successivo elencando le condizioni su quale stato futuro scegliere. ` giunto a destinazione Finch` e lActor non e 1. ordina decrescentemente i robot secondo il loro stato (in modulo); 2. Per ogni robot 3. Esegui le azioni del suo corrispondente stato; 4. Aggiorna lo stato del robot per la successiva iterazione secondo lo schema di tabella I; Fine
Algoritmo 1: Algoritmo di controllo

IV. M APPING In questa parte si tratteranno le problematiche e le soluzioni proposte riguardo il problema del mapping, cio` e della realizzazione della mappa di un ambiente attraverso misure successive da parte di uno o pi` u sensori, posizionati sui diversi robot a disposizione. Nel nostro caso la consegna iniziale del progetto prevedeva lutilizzo di un sensore virtuale realizzato a partire dallimmagine ottenuta dalla telecamera posta sopra la pedana del NAVLAB. Dallimmagine totale si ricava un cerchio od un settore circolare centrato sulle-puck, simulando cos` un radar. Visto che i robot a nostra disposizione sono per` o dotati di fotocamera digitale abbiamo deciso di utilizzare questo come sensore. Gli stessi robot dispongono anche di 8 sensori di prossimit` a che potrebbero essere utili allo scopo. In seguito a qualche prova sperimentale le loro misure si sono dimostrate molto rumorose e soprattutto con un raggio ` pertanto ritenuto massimo di sensibilit` a inferiore ai 5 cm. Si e di utilizzare come unico sensore la telecamera di cui ogni e` dotato. Lutilizzo della telecamera ha comportato per` puck e o una notevole mole lavoro per essere sfruttata al meglio secondo i nostri scopi. Le misure successive ricavate dai diversi robot nelle varie posizioni devono essere poi rielaborate e fuse insieme per poter andare a costruire una mappa pi` u precisa possibile della zona esplorata. A. Stato dellarte ` affrontato in numerosi testi Il problema del mapping e presenti in letteratura. In tutti il problema viene affrontato prima in maniera generale, ma poi ci si restringe alle particolari condizioni di lavoro ed agli strumenti utilizzati. In particolare in [2] sono presentati alcuni casi di map-building e navigazione risolti. Nessuno presenta tutte le caratteristiche del nostro problema; luso di una telecamera come sensore ` molto diffuso per il mapping 2D, per` non e o si sono trovati spunti utili per decidere il tipo di approccio da usare.

Tabella I: Schema di transizione tra stati S TATO ATTUALE S TATO S UCCESSIVO Per gli Explorer 2.E XPLORATION 1. M APPING 3.PATH -P LANNING 4.G IRARE 3.PATH -P LANNING 2.E XPLORATION 4.G IRARE 3.PATH -P LANNING 3.PATH -P LANNING 4.G IRARE 6.M OTO 1.M APPING 4.G IRARE 4.G IRARE 6.M OTO 4.G IRARE 5.R ILOCALIZZARE 0.WAIT 6.M OTO 6.M OTO 5.R ILOCALIZZARE 0.WAIT 0.WAIT Per lActor -1.M AP WAIT -1.M AP WAIT -2.P LANNING WAIT -2.P LANNING WAIT -2.P LANINNG WAIT -3.PATH -P LANNING -1.M AP WAIT -3.PATH -P LANINNG -6.M OTO -4.G IRARE -4.G IRARE -6.M OTO -5.R ILOCALIZZARE -2.P LANNING WAIT -6.M OTO -6.M OTO -5.R ILOCALIZZARE Condizione di scelta stato successivo

se si sono fatte le 3 foto ` in vista, per essere sicuri che esiste una strada se lobbiettivo e se si devono fare ancora foto per completare la tripletta ` diverso dalla posizione corrente se il nuovo punto di esplorazione e ` lo stesso, cambia solo la direzione se il punto e ` nita lesplorazione, per pianicare di spostarsi se si e ` diversa da quella corrente se la direzione iniziale del moto e ` gi` se non serve girarsi perch` e si e a dritti se non ho nito la tripla oppure ho nito il moto e la localizzazione se non avevo nito di girare in un passo se devo partire verso un nuovo punto in generale, se non ho nito, devo fare una foto se ho nito: ho visto, so che si pu` o andare, mi sono spostato se non sono ancora arrivato nito il moto stato di nullafacenza se non ho nuove informazioni sulla mappa se ho nuove informazioni sulla mappa ` in stato 6.M OTO se qualche Explorer e ` in stato 6.M OTO se nessun Explorer e se non so abbastanza percorso se so abbastanza percorso se non avevo nito di girare in un passo se devo partire verso lobbiettivo sempre se non sono ancora arrivato nito il moto

a) Tipologia e dimensione della mappa: La prima scelta dove con p(m|x) si indica la probabilit` a di generare una certa da effettuarsi riguarda la modalit` a di rappresentazione della misura m sotto lipotesi x, p(x) costituisce la probabilit` a di 1 ` riscontrato in [1] un utilissimo riepilogo delle occupazione della cella a priori, in genere pari ad 2 ` mappa. Si e , mentre e possibili tecniche, dei loro vantaggi e svantaggi. In linea con una fattore di normalizzazione che rende p(x|m) effettivamen` cercato innanzitutto te una densit` la losoa progettuale gi` a presentata si e a di probabilit` a. Questo aproccio probabilistico una rappresentazione che fosse leggera da un posto di vista sviluppabile poi in diverse forme serve sostanzialmente ad computazionale e facilmente implementabile in M ATLAB. Le irrobustire la costruzione della mappa agli errori di posizione e ` pertanto necessario problemi quali lo SLAM, ipotesi del problema prevedono la conoscenza della posizione di misura, ed e ` in cui la posizione e ` incognita. Nel nostro caso per` e dellorientamento del robot ad ogni istante, pertanto non e o viene richiesto luso di stimatori come il ltro di Kalman. Le possibi- garantita la conoscenza della posizione con errore minimo e ` concentrati, con buoni risultati nel rendere il pi` lit` a sono essenzialmente due: o si utilizza una rappresentazioni ci si e u preciso con nodi e gra o si utilizza una rappresentazione occupancy possibile il sensore utilizzato. grid cells cio` e una griglia in cui il valore delle singole celle indica la probabilit` a che essa sia occupata. La prima tecnica B. Tipologia e dimensione della mappa risulterebbe decisamente pi` u pratica per la successiva fase di Per cercare di semplicare il problema e giungere rapiesplorazione e pianicazione del moto in quanto consente di ` damente ad una implementazione completa del sistema si e individuare facilmente percorsi liberi e transitabili da parte del deciso di iniziare assegnando alle singole celle di occupazione robot. Inoltre risultando pi` u sintetica comporta una minore solo tre valori: occupazione della memoria. Daltro canto essa risulta per` o 0: nel caso una cella sia ritenuta libera ` scelto complessa da un punto di vista implementativo. Si e 1: nel caso una cella sia ritenuta occupata quindi di adottare la tecnica della occupancy grid cells, adatta : nel caso nessuna misurazione abbia riguardato quella al linguaggio M ATLAB che useremo, visto che si tratta in cella sostanza di una matrice, e pi` u intuitiva. Come detto in [1] A prima vista questa scelta pu` o apparire rischiosa e poco vengono anche suggerite le diverse tecniche applicabili per afdabile, mentre in fase di simulazione e di sperimentazione lassegnazione delle probabilit` a di occupazione alle singole ` in N AVLAB si e dimostrata efcace e sufcientemente robusta. celle. Tipicamente, considerando ambienti statici, la probabi` Questo e dovuto in particolar modo alla tecnica decisamente lit` a di occupazione di una cella x data una misura m, p(x|m), conservativa con cui vengono analizzate le misure (cio` e le viene calcolata tramite la regola di Bayes: foto) delle telecamere dei robot. E chiaro che tale scelta rappresenta solamente un primo approccio al problema, essendo p(x|m) = p(m|x)p(x)

` mai rivelata, al per` o che nel corso del lavoro essa non si e ` stata mantenuta contrario di altri aspetti, un punto critico e invariata, ed anzi la sua semplicit` a ha garantito una velocit` a di calcolo determinante nellapplicazione real-time con M ATLAB. Si tratta ora di dimensionare adeguatamente la matrice che andr` a a rappresentare la mappa. Dopo alcune veriche sulla precisione della telecamera dei robot, del controllo del loro e ` delle dimensioni degli ostacoli che si intendeva utilizzare si e deciso di utilizzare una griglia con celle quadrate di lato 1 cm. In denitiva quindi la mappa che useremo sar` a costituita da una matrice di dimensioni 192 256, in cui ogni elemento individua unarea di un centimetro quadrato. C. Rilevamento degli ostacoli Poich` e si vuole ricostruire una mappa 2D dellambiente sufciente ricavare dalle foto effettuate dalla telecamera gli spigoli orizzantali degli ostacoli adiacenti al pavimento. Gli algoritmi di edge detection sono numerosissimi in letteratura, si veda per esempio Canny o Sobel, le cui funzioni sono gi` a implementate anche in M ATLAB. Per ridurre il tempo di trasmissione dellimmagine abbiamo pensato di fare in modo che leventuale bordo venisse individuato sfruttando le ` dotato capacit` a computazionali del microprocessore di cui e le-puck, cos` che lo stesso trasmetta solamente un vettore che indichi per ogni colonna dellimmagine la posizione del ` dovuto dunque implementare lalgoritmo diretbordo. Si e tamte sulle-puck. Viste le sue limitate capacit` a di calcolo e le difcolt` a implementative che si sarebbero incontrate si ` deciso di individuare il bordo degli ostacoli in maniera e pi` u semplice, sfruttando qualche informazione aggiuntiva sugli ostacoli utilizzati. Si sono utilizzati ostacoli neri di modo da avere un chiaro contrasto con il pavimento bianco. Quindi si va a scorrere ogni colonna dal basso verso lalto e non appena si individuano due pixel consecutivi di intensit` a inferiore ad una certa soglia si segnala quello come bordo dellostacolo. Con questa tecnica quindi si riduce di 1/40 la quantit` a di byte tasmessa dalle-puck al computer in seguito ad ogni fotograa. Limplementazione dellalgoritmo in C non ha presentato particolari problemi, al di l` a del fatto di inserirla correttamente allinterno del codice gi` a presente. Una taratura della soglia adeguata alla luminosit` a esterna garantisce il funzionamento dellalgoritmo come mostrato in gura 1. D. Costruzione della mappa Sostanzialmente luso della telecamera come sensore per ricostruire la mappa richiede di denire una trasformazione che associ ai punti (pixel) dellimmagine ripresa dalla telecamera i punti del piano su cui si muove il robot: : Z2 R2 , o pi` u precisamente nel nostro caso : [1, Xp ] [1, Yp ] R2 , avendo a che fare con unimmagine di risoluzione Xp Yp pixel. In una prima fase si andr` a a considerare un sistema di riferimento del piano solidale alla camera, e quindi alle-puck stesso, conoscendo poi la posizione e lorientamento del robot si ottiene facilmente una corrispondenza con il piano denito nel sistema di riferimento assoluto. Se si considera il modello della fotocamera pin hole riportato in gura 2 si ricava la

(a) Case I

(b) Case II

Figura 1: Funzionamento dellalgoritmo di individuazione del bordo implementato direttamente sul microprocessore delle-puck. In effetti la ` il solo vettore rappresentato in rosso. quantit` a trasmessa dal robot e

Figura 2: Geometria della camera

seguente relazione:
cos d f cos( d ) = h yp cos

(1)

` indicato con f il fuoco della camera, con h la sua dove si e altezza rispetto al piano, con d la distanza dallostacolo, con y lordinata del punto del piano immagine in cui viene visto il bordo inferiore dellostacolo, con langolo che lasse ottico forma con il piano ed inne = arctan h d . Nel caso in esame si ha 0 2 , e dunque si pu` o riscrivere la (1) come: d df = h yp che esplicitando rispetto a d diventa: d= fh . h yp (3) (2)

Se si orientano gli assi del piano cui appartiene la pedana ` dunque denita una corrisponcome mostrato in gura 3 si e denza tra le ordinate sul piano immagine, yp , e le ordinate yr = d su questo piano. Poich` e ad ogni colonna dellimmagine corrisponde un particolare settore angolare sul piano reale risulta conveniente utilizzare le coordinate polari, in quanto cos` facendo risulter` a immediato associare ad ogni colonna dellimmagine il corrispondente settore circolare e quindi la distanza a cui si trova
2 In quanto la telecamera montata sugli e-puck ha lasse ottico circa orizzontale

Figura 4: Passaggio al sistema di riferimento assoluto

Figura 3: Geometria della camera

lostacolo. Detto langolo di visione delle-puck, xp lascissa del piano immagine di cui si vuole ricavare la fase , si ha: X p 2 xp = arctan (4) tan Xp 2
2

lorientazione delle-puck, i nuovi punti avranno modulo = + . Detta (x0 , y0 ) la posizione del = e fase centro delle-puck al punto (xp , yp ) sul piano immagine si andr` a a far corrispondere il punto (x, y ), sul piano solidale al pavimento e rispetto al sistema di riferimento assoluto, cos` denito: x = x0 + cos y = y0 + sin .

(6)

` possibile Dunque ad ogni punto del piano immagine (xp , yp ) e associare il punto (, ) del piano reale con sistema di riferi` calcolato secondo la mento solidale alla telecamera, dove e d ` (4) mentre = cos , dove d e ricavato dalla (3). Chiaramente inne tale sistema di riferimento, solidale alle-puck stesso, dovr` a essere centrato non sulla telecamera ma sul centro del robot, visto che di questo sono note le coordinate, e la fase corrisponder` a con lo sfasamento rispetto allorientazione del robot stesso, cio` e i punti di fase nulla sono quelli che si trovano lungo lasse ottico della telecamera3 . Si vogliono quindi calcolare le coordinate polari in seguito alla traslazione del centro del sistema di riferimento. Sia l la distanza della camera dal centro del robot, allora come si vede in gura 3, al punto (, ), corrisponde il punto (, ) secondo la relazione: sin cos + l cos + l = cos = arctan

E. Aggiornamento della mappa Il problema dellaggiornamento della mappa data una misura si pu` o formulare come segue: si vuole stabilire un operatore che data la k -esima misura m(k ) e la mappa aggiornata alla misura (k 1)-esima M (k 1), restituisca la mappa globale aggiornata M (k ): : (M (k 1), m(k )) M (k ) ` univoca, Ovviamente la denizione di questo funzione non e essendo essa intrinsecamente legata al tipo di misura m(k ), al sensore utilizzato ed al rumore di misura, allambiente che si vuole mappare ed allo scopo che si intende raggiungere; per ulteriori dettagli si rimanda nuovamente a [1]. Nel nostro ` evitato lapproccio probabilistico, caso come gi` a ricordato si e pertanto data la M (k 1) e la m(k ), si tratta di decidere che scelta adottare nel caso di celle (x, y ) con valori differenti ` quella di assegnare ad sulle due mappe. Una prima scelta e M (k ) i valori di M (k 1), e quindi i valori di m(k ) per le celle che in M (k 1) valevano . Rimane la scelta per le celle che hanno valore non e diverso in M (k 1) ed m(k ), celle cio` e che risultano libere in una delle due mappe ed occupate nellaltra. Esse vengono impostate come libere, cio` e con probabilit` a di occupazione pari a 0. Questa decisione, che ` in realt` a prima vista pu` o apparire rischiosa, e a giusticata, ed in qualche modo richiesta, dalla tecnica di costruzione della misura. Infatti come detto in precedenza in presenza di ostacoli sulla fotograa essi vengono posti nel piano reale al minimo

(5)

: ` cos` Riassumendo si e denita la trasformazione 2 [1, Xp ] [1, Yp ] R , (xp , yp ) (, ), lultimo passaggio consiste nel riferirsi al sistema di riferimento assoluto conoscendo la posizione e lorientazione del robot, si tratta cio` e di applicare un semplice cambio di coordinate. Questa semplice viene implementata in maniera un po particolare nel nostro caso, per diminuire i tempi di calcolo ed adattarsi meglio alla tipologia di mappa scelta. Per prima cosa si esprimono i punti rispetto ad un sistema di riferimento centrato nello stesso punto ma orientato secondo il sistema di riferimento assoluto. Sia
3 Pi` u

precisamente sulla proiezione dellasse ottico sulla pedana.

(a) I Passo

(b) II Passo

5 10 15 20 25 30 35 40 45 50 5 10 15 20 25 30 35 40 45 50

(c) III Passo

(d) IV Passo

Figura 5: Esempio di costruzione di una misura a partire da una foto effettuata dal robot

della distanza a cui corrisponde ciascun pixel4 . Poich` e quando gli ostacoli visti sono lontani si ha una maggiore incertezza sulla misura, accadr` a quasi sempre che essi sono posti troppo vicini al robot rispetto alla loro reale collocazione, le misure successive in cui il robot si avviciner` a ad essi andranno a correggere facilmente questo errore. Dunque a questa scelta ` solo conservativa, in cui lerrore che si pu` o commettere e quello di vedere ostacoli dove non ci sono, si fa seguire questo tipo di aggiornamento, per cui una volta che una misura rivela ` da ritenersi libera a tutti gli effetti, una cella libera essa e anche in seguito a misure successive che invece la ritengano occupata. V. E SPLORAZIONE A. Stato dellarte Con esplorazione si intende il problema di pianicare la navigazione dei robot al ne di acquisire pi` u informazioni possibili dallambiente. Durante lesplorazione di un ambiente
4 Ad ogni pixel quadrato del piano immagine corrisponde un quadrilatero sul piano reale, come si vede in gura 5, nella costruzione della mappa relativa ad una certa misura si posiziona lostacolo sul lato del quadrilatero pi` u vicino al robot

` cercare di ottimizzare le risorse necessarie ignoto lobiettivo e per la sua completa esplorazione e per la creazione della mappa: tempo impiegato, spazio percorso e, nel caso di sistemi multiagente, numero di robot necessari. Gli approcci classici ( [3], [4]) si basano sul concetto di Frontier-Based Exploration: guidare il robot verso le regioni di frontiera (frontier cells) tra larea esplorata e quella ancora da esplorare. Ad esempio in [5] viene denita una funzione di: costo per raggiungere una frontier cell benecio ottenuto dalla frontier cell Lalgoritmo iterativamente aggiorna la mappa del territorio e sceglie target points per i singoli robot in modo da raggiungere un compromesso tra costi e beneci: i costi sono legati alla possibilit` a di passare per celle con una probabilit` a di occupazione diversa da zero; i beneci sono determinati da quanta informazione si pu` o ottenere dal raggiungimento di ` una particolare frontier cell. Un altro approccio interessante e quello adottato in [6]. In questo articolo viene implementato un algoritmo randomizzato chiamato SRT(Sensor-based Random Tree); qui si assume che il robot utilizzi sensori di prossimit` a a 360 con un raggio R di rilevamento elevato. Lalgoritmo di Stentz in [7] vuole trovare ad ogni iterazione il cammino ` provato a minimo (migliore) data la mappa del territorio. Si e

` potuti proseguire, data implementare questultimo, ma non si e lonerosit` a dellalgoritmo. Inoltre questo faceva nascere parecchi problemi nel caso si utilizzassero diversi robot esploratori. ` stata scelta solo per la pianicazione Questultima soluzione e della traiettoria dellActor. B. Algoritmo di esplorazione ` mirata ad Rispetto alla trattazione classica lesplorazione e individuare un percorso libero, afnch` e le-puck cieco (Actor) riesca a raggiungere un punto di destinazione d. Grande ` stata quella di gestire una visuale molto stretta difcolt` a e dei robot e non particolarmente lunga (angolo di visibilit` a di 28 e raggio di circa 40 cm). Per ovviare a questo problema ` stato deciso di scattare sempre tre foto prima di richiamare e lalgoritmo di esplorazione. In tal modo si pu` o lavorare con una visuale minima di 60 . Lalgoritmo di esplorazione ha come ingresso la mappa del territorio (le celle contengono il valore numerico 1, 0 o a seconda che la cella corrispondente sia rispettivamente occupata, libera o sconosciuta), la posizione del robot e il punto d di destinazione dellActor. Ha come uscita il punto di esplorazione successivo e langolo verso cui guardare. Per semplicit` a ora verr` a descritto lalgoritmo di esplorazione implementato per un robot. Lestensione al caso multirobot avverr` a successivamente. Lalgoritmo di ` descritto in Algoritmo 2. Innanzi tutto vengono esplorazione e determinate le celle di frontiera; per ogni cella viene calcolato lindice di costo J . Lindice tiene conto della distanza dalla destinazione e dalla posizione attuale. In specico il costo J `: per ogni cella di frontiera fi e J = J1 +J2 = c1 ( fi d min fj d )+c2 ( fi p dott )
fj F

Input: p = posizione attuale del robot; = direzione attuale del robot; M = mappa del territorio; d = destinazione del cieco; Output: pesp = punto di esplorazione successivo; esp = angolo verso cui scattare la foto successiva; 1) Determinare linsieme delle celle di frontiera F M 2) Per ogni cella calcolare lindice di costo J 3) Determinare c = argmin(J ) 4) Se c p > dmin pesp = c; B = {(i, j ) S (pesp , R) : M (i, j ) = } se B = (m, n) = argmin (i, j ) (d(1), d(2)) esp = arctan altrimenti esp = arctan
(i,j )B npesp (2) mpesp (1) d(2)pesp (2) d(1)pesp (1) fi F

` esplorata completamente altrimenti se la zona attorno non e pesp = p; esp = + (k ); altrimenti A ={fi F : fi p > dmin } pesp = argmin(J )

B = {(i, j ) S (pesp , R) : M (i, j ) = } se B = (m, n) = argmin (i, j ) (d(1), d(2)) esp = arctan altrimenti esp = arctan
(i,j )B npesp (2) mpesp (1) d(2)pesp (2) d(1)pesp (1)

fi A

Fine Algoritmo 2: Algoritmo di esplorazione

` la somma di un costo dovuto alla In parole pi` u semplici J e distanza dalla destinazione di fi , J1 , e di un costo dovuto alla distanza di fi dalla posizione attuale del robot, J2 . Il primo ` nullo se fi e ` il punto della frontiera pi` costo e u vicino alla destinazione. Tutti gli altri punti hanno un costo positivo come si pu` o notare dal graco in alto in gura 6. ` Il secondo costo si annulla se la distanza tra fi e p e uguale a dott : si vuole che il robot si muova a una certa distanza dalla posizione corrente senza allontanarsi troppo in quanto si desidera che tenga una sua traiettoria e una sua contiguit` a nel movimento. Simulazioni e prove sperimentali ` hanno fatto ssare dott = 25 cm. La rappresentazione di J 2 e nel graco in basso in gura 6. I pesi ci sono stati variati in molte simulazione, ottenendo risultati signicativamente ` stato posto di due ordini di grandezza diversi. Mediamente c1 e ` importante che il robot si pi u grande rispetto a c2 , in quanto e avvicini alla destinazione d . Una volta determinato il punto c che minimizza lindice J vengono valutati diversi casi: ` maggiore di dmin allora il se la distanza tra c e p e ` la distanza minima di robot si muove verso c. dmin e ` stata posta uguale a 15 cm. Ora il problema moto ed e ` quello di determinare langolo verso cui guardare. Si e desidera che il robot guardi verso ambienti inesplorati, cercando inoltre di puntare possibilmente verso d. Per far ci` o viene calcolata la distanza tra d e tutti i punti inesplorati appartenenti ad un intorno sferico di c di

` raggio R = 15 cm (ammesso che esistano) come e indicato nellalgoritmo; esp viene calcolata in modo che il robot guardi verso il punto che minimizza questa distanza. Se non esistono punti inesplorati nellintorno, allora esp punta alla destinazione. ` minore di dmin il robot preferisce se la distanza tra c e p e girarsi e fare unaltra foto in una diversa direzione. (k ) ` una funzione della iterazione k-esima in quanto e ` e necessario che il robot riesca a esplorare tutta la zona circostante, girandosi con angoli opportuni a sinistra e a destra. nel caso in cui il robot dopo aver fatto un giro su se stesso e aver esplorato la zona circostante non trovi punti ottimi abbastanza distanti, si muove verso un punto sufcientemente distante da p che permetta al robot di trovare strade alternative. Per chiarezza vengono fatti di seguito alcuni esempi. Si osservi la gura 7. Il robot deve riuscire ad aggirare lostacolo e ` rappresenraggiungere la destinazione. In gura 8 (in alto) e tata la mappa in ingresso alla funzione di esplorazione in un certo istante, mentre il robot giallo rappresenter` a la soluzione ` la zona libera, quella verde e ` dellalgoritmo 2. La zona blu e ` la zona inesplorata. quella occupata da un ostacolo, la rossa e

10

Figura 6: Indice J1 con c1 = 1 (sopra) e indice J2 con c2 = 0.01 (sotto).ddest (fi ) indica la distanza del punto fi dalla destinazione; dpos indica la distanza tra fi e p

Figura 8: Mappa e soluzione dellalgoritmo

Figura 9: Robot circondato da ostacoli

assegnato ad uno degli N robot in base ad una funzione di costo: ogni robot andr` a verso il punto di frontiera piu vicino. Figura 7: Mappa reale ` simile allAlgoritmo 2; inoltre Lalgorimo implementato e ` stata tratta lidea da [5] di dividere la zona di frontiera e in N zone, ciascuna assegnata a un preciso robot, in base La determinazione delle celle di frontiera avviene su una al criterio di minima distanza. Per semplicit` a viene descritto mappa orlata, cio` e una mappa in cui lostacolo viene un il metodo utilizzato con due robot esploratori. Lapproccio po ingrandito. Questo tiene conto del diametro delle-puck e ` facilmente estendibile al caso di pi` u esploratori. Ciascun in quanto in questo algoritmo si suppone di utilizzare robot robot si muove nel punto della sua frontiera minimizzante puntiformi. Le celle di frontiera sono visibili in arancione in lindice J . Ora lindice tiene conto anche della distanza dei gura 8 (in basso) e sono state ottenute tramite un ltraggio dei punti ammissibili dalla posizione di osservazione dellaltro dati dellimmagine. Con lalgoritmo proposto le-puck riesce robot: per ottenere ci` ` stato introdotto un costo relativo alla oe ad uscire da situazioni difcili; il procedimento fornisce una vicinanza degli e-puck. Se i due robot sono pi` u lontani di soluzione anche nel caso il robot si trovi circondato da ostacoli. d = 40 cm questo costo si annulla; si vuole dunque che gli o ` signicativo da questo punto di vista. e-puck mantengano una certa distanza nellesplorazione. A J Lesempio successivo e ` la viene aggiunto dunque il costo quadratico: Si osservi la gura 9; la soluzione che elabora lalgoritmo e migliore possibile data linformazione che si ha sulla mappa. J3 = c3 ( fi po do )2 1 ( fi po + do ) C. Estensione Multirobot La situazione diventa pi` u articolata e pi` u complicata quando si introducono nella mappa diversi robot esploratori. Una tec` descritta in [5]. Ad ogni iterazione vengono nica interessante e determinati N punti di frontiera che saranno i prossimi punti di osservazione. In seguito ciascun punto di frontiera viene ` la posizione dellaltro robot e 1 indica la funzione Dove po e gradino. Sono stati valutati diversi valori del peso c3 . Il valore ` stato di un ordine di grandezza pi` medio di simulazione e u ` rappresentato il graco di J3 . piccolo di c1 . In gura 10 e Si veda lesempio con due robot esploratori in gura 11. ` rappresentato il robot considerato. Laltro robot In rosso e ` quello azzurro. La zona di frontiera e ` rappresentata in e

11

` valida ai nostri scopi, ove lo spazio supposizione non e operativo viene scoperto gradualmente dai robot esploratori. ` scelto per la ricerca di cammini minimi Lalgoritmo che si e ` dunque quello descritto in [7]: e A. Lalgoritmo D* (D-star) un algob) Un po di storia e motivazioni della scelta: E ritmo di ricerca di cammini minimi su un grafo, tipo il famoso Dijkstra (risalente allanno 1959, [12]). Lidea alla base di ` quella di cercare un cammino su un questultimo algoritmo e grafo da un nodo di partenza a un nodo obbiettivo a partire dalla ne, e risalendo a ritroso scegliendo di volta in volta larco meno costoso no a raggiungere il nodo di partenza. E ` computazionalmente facile intuire che questo procedimento e molto oneroso, basti pensare che in realt` a si trova un percorso non solo dal nodo che interessa, ma da tutti i nodi che distano circa quanto esso dal target. Esiste un miglioramento in termini di efcienza di questo algoritmo, laltrettanto noto A* (A-star6 , [13]), che introduce una stima euristica della distanza tra la partenza e larrivo in modo da limitare il campo di ricerca del cammino su meno nodi. Nonostante leuristica, tale algoritmo presenta delle bellissime propriet` a di ottimalit` a e completezza che ora non indaghiamo, ma che si trovano tutte formalmente dimostrate, sempre in [13]. Tali propriet` a vengono trasferite anche allalgoritmo in questione, suo fratello D*, che in effetti prende il nome proprio da questo, ma sostituisce la A con una D, che sta per Dynamic. Infatti prevede la possibilit` a di aggiornare il cammino non appena si verichino dei cambiamenti di costo negli archi, individuando una strada alternativa se questi divengono proibitivi, e migliorando la strada attuale se si apre un percorso di costo minore. questo lalgoritmo che cercavamo per il nostro Actor: E dopo una fase di inizializzazione del cammino, la strada tra lActor e il goal viene mantenuta aggiornata man mano che gli esploratori avanzano nel loro lavoro di costruzione della mappa. Prima di passare ad una breve descrizione dellalgo` meglio introdurre il tema della costruzione del grafo ritmo, e a partire dalla mappa, giusto per avere in mente un esempio concreto di applicazione. ` costituito da un c) Costruzione del Grafo: Un grafo e inseme di N nodi connessi tra di loro da archi pesati. Esistono svariate tecniche che permettono di costruire un grafo a partire da una mappa bidimensionale del territorio. Queste tecniche sono state ampio oggetto di studio7 . ` stato utilizzato per scegliere una di queste Il criterio che e ` quello della semplicit` tecniche e a: data la mappa come griglia ` collegato ai suoi otto vicini come di occupazione, ogni nodo e in gura 12. Il costo dellattraversamento di un arco libero si o scegliere pari a 1 nel caso di spostamenti laterali, alla pu` 2 per uno spostamento in diagonale. Ma per semplicit` a di calcolo si sono scelti costi interi, in particolare un costo pari a 10 per vicini laterali, a 14 per quelli diagonali. Il costo di
6 Non pervenuto il signicato di tale lettera, ma la stellina sta a signicare un qualche concetto di ottimalit` a 7 anche nelle tesine del Corso di Progettazione dei Sistemi di Controllo degli anni precedenti, ad esempio quella di Ricciato, Siego, Tamino, Pianicazione del Moto e Controllo di un Uniciclo

Figura 10: Indice J3 con c3 = 0.1. In ascissa c` e dr = fi po

Figura 11: Due robot esploratori

arancione: quella tratteggiata viene assegnata al robot rosso; lAlgoritmo restituisce la posizione di osservazione successiva (robot giallo). Alla successiva chiamata della funzione di esplorazone sar` a il robot azzurro che, in base alla posizione dellaltro robot (ora in giallo) decider` a dove andare e cos` via di seguito. D. Terminazione dellalgoritmo ` omessa la parte Per ragioni di ordine in algoritmo 2 si e di terminazione. In realt` a quando viene avvistato lobiettivo il le di esecuzione non richiama pi` u lalgoritmo di esplorazione. LAlgoritmo 2 gestisce inoltre delle situazioni degeneri: lobiettivo pu` o essere irraggiungibile, per esempio a causa di passaggi troppo stretti. In tal caso lalgoritmo termina quando non ci sono pi` u punti ammissibili, dopo lesplorazione di tutto il territorio raggiungibile per i robot. VI. L ALGORITMO DI RICERCA DEI CAMMINI La necessit` a di muoversi in un ambiente ostacolato comporta delle difcolt` a nella fase di generazione delle traiettorie. ` stato approfonditamente trattato in letteratura. Largomento e Esistono varie tecniche per fare s` che lalgoritmo di pianicazione sia in grado di evitare gli ostacoli. Un buon riassunto si pu` o trovare in [8]5 . In genere, tali algoritmi si riducono alla ricerca di cammini minimi su un grafo, con algoritmi tipo Dijkstra, ben analizzati in [9], o alla minimizzazione di indici che penalizzano la distanza, lenergia spesa, il tempo di esposizione al pericolo e via dicendo [10], o ancora alla creazione di potenziali attrattivi verso lobiettivo e repulsivi dagli ostacoli [11]. Tuttavia, tali tecniche suppongono perlopi` u che lambiente ovvio che tale ove si muove il robot sia noto a priori. E
5 Consultabile

anche in rete grazie a Google Book

12

Figura 12: Dalla mappa al grafo

attraversamento per una cella occupata si pu` o per il momento pensare innito. d) Descrizione dellalgoritmo: Lalgoritmo utilizza puntatori che tengono traccia dei percorsi ottimi nel grafo. Si indichino con X , Y , due nodi generici del grafo, e con b(X ) = Y si intenda che X punta ad Y . Come A*, D* mantiene una lista aperta (L) di nodi che devono essere elaborati. Per comodit` a, etichettiamo i nodi con t(X ) = N EW se il nodo X non ` mai stato in L, con t(X ) = OP EN se X L, con e ` uscito da L . Ad ogni nodo si t(X ) = CLOSED se X e associano due funzioni di costo. Denotando con G il nodo obbiettivo (goal), la funzione h(X ) tiene traccia della somma dei costi di un cammino da X a G, mentre la funzione k (X ), ` denita come il minimo tra la h(X ) detta funzione chiave, e attuale e tutti i valori da essa precedentemente assunti da ` stato introdotto in L. Pi` quando X e u chiaramente, indicando con un pedice i una generica iterazione dellalgoritmo, k (x) = min hi (X )
i

(7)

Si indichi poi con cXY il costo di attraversamento dellarAlgoritmo 3: Algoritmo di ricerca dei cammini minimi D* co che congiunge X e Y . In Algoritmo 3 si descrive il procedimento principale, si veda anche [7]. Lalgoritmo fa in modo che alla strada individuata dai puntatori corrisponda una h(X ) ottima, controllando passo Data la denizione (7), e ` chiaro che per i nodi RAISE non per passo se i nodi vicini porgono un cammino migliore pu` o essere ancora presente un cammino ottimo, mentre i nodi a partire dal goal e procedendo a ritroso verso il nodo di LOWER sono ottimi, perch` e la loro funzione attuale di costo partenza, S , proprio come lalgoritmo di Djkstra. Ma permette h(X ), nel momento in cui vengono elaborati, cio` e quando la un aggiornamento rapido nel caso cambino alcuni costi negli loro funzione k (X ) e ` la minima presente nella lista, e ` pari a archi che compongono il grafo. Laggiornamento avviene per kmin . Questo signica che seguendo i puntatori a partire da mezzo di ripetuti inserimenti ed eliminazioni di nodi dalla lista un nodo LOWER no a G, il cammino risultante e ` minimo. ` quella di mantenere D* usa i nodi RAISE per propagare i rialzamenti di costo del aperta. Lidea principale dellalgoritmo, e ottimi i nodi pi` u vicini al Goal, e tramite questi propagare poi cammino dovuti alla scoperta di un arco di costo crescente, i i cambiamenti di costo, in modo che il cammino si mantenga nodi LOWER per propagare le riduzioni. ottimo. I nodi presenti nella lista vengono dunque elaborati Seguendo lesempio di gura 13, immaginiamo di partire da ` minore, cio` a partire da quello la cui funzione k (X ) e e da una situazione di piano libero come nel caso (a), e che a un ` chiaro che quello pi` u vicino al goal. Cos` operando, e certo punto venga individuato un ostacolo (che occupa i nodi kmin = min k (X ) (8) 2, 6 e 10 in gura). Questi nodi, il cui cammino era ottimo, X L dunque sono LOWER, vengono inseriti nella lista aperta, e la loro elaborazione causer` a linserimento di tutti i nodi che li ` pari al costo rappresenta un valore importante, perch` e esso e puntavano (passo 11. dellalgoritmo, nodi 1, 5, 9, 13 in gura). minimo possibile, quello ottimo, esistente tra i nodi della lista Questi sono ora nodi RAISE . Prima di tutto, si cerca tra tutti i aperta. La funzione k (x) discrimina i nodi della lista in due vicini uno che porga un cammino di costo minore (passo 5.). tipologie: Poi, ad ogni vicino si esegue una delle seguenti operazioni: 1) nodi RAISE: se k (X ) < h(X ) ` 2) nodi LOWER: se k (X ) = h(X ) si propaga il cambiamento di costo se il vicino e

Prima chiamata: si inserisca il nodo G in L, e si proceda con i passi 1-11 nch` e t(S ) = CLOSED. ` Chiamate successive: si inseriscano i nodi il cui arco e cambiato di costo in L Finch` e kmin < h(S ) 1. X = arg minZ L , kmin = minX L k (X ) 2. Se il passo 1. non porge X , FINE: non esiste un percorso 3. Eliminare X da L; t(X ) = CLOSED ` un nodo RAISE 4. Se kmin < h(x), cio` eX e 5. Per ogni Y vicino di X : Se h(Y ) kmin e h(X ) > h(Y ) + cXY , allora b(X ) = Y ; h(X ) = h(Y ) + cXY 6. Per ogni Y vicino di X : 7. Se t(Y ) = N EW o {b(Y ) = X e h(Y ) = h(X ) + cY X } allora b(Y ) = X ; Inserire (Y, h(X ) + cY X ) 8. Altrimenti 9. Se b(Y ) = X e h(Y ) > h(X ) + cY X allora Inserire (X, h(X )) 10. Altrimenti se b(Y ) = X e h(X ) > h(Y )+cXY e t(Y ) = CLOSED e h(Y ) > kmin allora Inserire (Y, h(Y )) ` un nodo LOWER) 11. Altrimenti (kmin = h(x), cio` eX e Per ogni Y vicino di X : se t(Y ) = N EW o {b(Y ) = X e h(Y ) = h(X ) + cY X } o {b(y ) = X e h(Y ) > h(X ) + cY X } allora b(Y ) = X ;Inserire (Y, h(X ) + cY X ) Funzione Inserire (X, hnew ) 1. Se t(x) = N EW , allora k (X ) = hnew 2. Se t(x) = OP EN , allora k (X ) = min(k (X ), hnew ) 3. Se t(x) = CLOSED allora k (X ) = min(h(X ), hnew ) 4. h(X ) = hnew ; t(X ) = CLOSED

13

10

11

12

13

14

15

16

(a)

(b) Figura 13: Esempio di cammini generati da D*

(c)

successore (passo 7.) si controlla se il vicino pu` o essere ulteriormente miglio` il nodo stesso che va inserito in L rato. In questo caso e (passo 9.) si controlla se il vicino pu` o migliorare la strada (passo 10.). Il risultato di queste operazioni, attuate sui nodi in questione, ` stato individuato il porta alla situazione (b) della gura, in cui e nuovo percorso. Se poi uno dei nodi ostruiti torna nuovamente libero (diviene LOWER), ad esempio il numero 2, un solo passo dellalgoritmo (il 5.) porta alla soluzione di gura (c).

la velocit` a tangenziale v e quella angolare delluniciclo. Si considera allora la modellizzazione delluniciclo: x = v cos() y = v sin() = (9)

VII. C ONTROLLO DELLA TRAIETTORIA E

RILOCALIZZAZIONE

dove con x e y si intendono le coordinate del centro del robot. ` possibile calcolare v e tali In effetti, per questo sistema non e che il percorso tracciato dal centro del robot sia non smooth, ` anolonomo, cio` perch` e il sistema e e non pu` o mai avere una velocit` a laterale rispetto allorientamento del veicolo9 . Ma se si denisce un punto B fuori dallasse delle ruote, pi` u avanti del centro del veicolo di una auqntit` a b, di coordinate xB = x + b cos(), yB = y + b sin() (10)

A. Controllo della traiettoria Si propone ora una soluzione semplice al problema del trajectory-tracking, cio` e di far seguire al robot una traiettoria di riferimento. La letteratura propone diversi metodi, ma quello ` il Dynamic-Feedback Linearization. che va per la maggiore e Una trattazione approfondita del metodo, completa anche di prove sperimentali, si trova in [14]. Purtroppo tale tecnica impensabile non risulta soddisfacente per i nostri scopi. E infatti, o perlomeno alquanto insoddisfacente, controllare il robot soltanto attraverso una retroazione, in quanto si deve in ogni caso aspettare di avere un errore in posizione. Unac` necessaria per aumentare le curata azione di Feedforward e prestazioni del sistema. Per calcolarla, per` o, la tecnica menzionata pretende traiettorie sufcientemente smooth, cio` e perlomeno derivabili due volte, e calcola lazione in base ai valori di riferimento dellaccelerazione. Il nostro algoritmo di pianicazione, invece, porge traiettorie ovviamente continue, ma formate da un susseguirsi di spezzate, la cui derivata quindi ` costante a tratti, e laccelerazione risulta nulla quasi ovunque. e ` avvalsi allora della seguente tecnica, che risolve questo Ci si e problema in maniera assai semplice ma brillante, e soprattutto perfettamente funzionante8 . ` basata su una rimodellizzazione del sistema. La tecnica e ` equivalente a quello Il modello del robot a due ruote e delluniciclo, nel senso che esiste una relazione biunivoca tra i due segnali di ingresso l e r delle due ruote del robot con
8 tratta

` possibile controllare il moto del robot in modo da eseguire e con tale punto anche cammini con discontinuit` a avendo velocit` a sempre diversa da zero. Con tale cambio di coordinate, il sistema (9) diventa: x B = v cos() b sin() = vdx y B = v sin() + b cos() = vdy =
d d

(11)

dove con vdx e vdy si indicano le velocit` a desiderate, e la d sopra luguale questo concetto di desiderio. Nelle prime due ` invertibile, e si pu` equazioni la dipendenza dagli ingressi e o denire la legge di controllo statica in funzione dei due nuovi ingressi [vdx vdy ] : v = cos() sin() b sin() b cos()
1

vdx vdy

(12) dove la matrice inversa esiste perch` e il suo determinante vale ` ottenuto un sistema lineare e b > 0. In questo modo, si e disaccopiato con ingressi vdx e vdy . In sostanza, il riferimento si prende non pi` u per il centro ` scelto pi` del robot, ma per il punto B , che si e u avanti di 5 cm, si calcolano le velocit` a desiderate, anche non continue, derivando il riferimento, e si calcolano gli ingressi v e
9 si

vdx cos() + vdy sin() 1 b (vy cos( ) vdx sin( ))

da unidea del Prof. A. De Luca, Univerist` a di Roma Sapienza, [15]

veda ancora [14] per maggiori dettagli

14

Figura 14: Il punto pi` u avanti compie anche angoli retti

` opportuno corrispondenti tramite la (12). Naturalmente, e introdurre un termine di correzione in retroazione. A tal ne ` sufciente progettare un banale regolatore, per esempio e ` visto che e ` sufciente un PID, per il sistema (9). Si e banale controllore proporzionale allerrore di posizione, ossia sostituire gli ingressi desiderati con vdx vdy vdx + kx (rx (t) xB ) vdy + ky (ry (t) yB ) (13)

che nel caso degli encoder, sebbene scorrelato ad ogni istante, ` ritenuto di adottare questa tecnica di localizzazione solo si e ` necessario conoscere pi` quando e u precisamente possibile la ` posizione del robot. Inne per poter ricostruire la mappa e necessario conoscere lorientazione del robot, informazione ` possibile ricavare con molta precisione da una che non e singola immagine della telecamera10 GPS. Come illustreremo invece questa informazione pu` o essere ricavata dallanalisi del moto del robot su un certo intervallo di tempo, ed anche questa operazione viene effettuata in questa fase di rilocalizzazione. Siano qi,old (t) = (xi,old (t), yi,old (t), i,old (t)) le coordinate, espresse in pixel, del robot i-esimo al tempo t, I0 limmagine iniziale della pedana priva di robot, It , limmagine della pedana al tempo t, si vuole denire un operatore (qi,old (t), It ): : (qi,old (t), It qi,new (t) (15)

La gura 14 illustra quanto detto. B. Rotazione del robot Per girare i robot su s` e stessi al di fuori del moto non ` attuato un vero e proprio controllo, ma si e ` calcoato si e = , e ` semplicemente un comando in feedforward. Dato che facile calcolare lingresso che serve. Infatti, detto Tg il tempo in cui si vuole che il robot giri, si ha (esattamente) (k + 1) = (k ) + Tg , (14)

dove qi,new (t) sono le coordinate corrette del robot i-esimo. ` adottato si basa fondamentalmente su un Lalgoritmo che si e algoritmo di blob-detection, tramite ltraggio lineare. Disponendo gi` a delleffettivo background I0 , si ottiene unimmagine It in cui sono presenti i soli robot: It = I0 It , dove ` dovuto al fatto che lo sfondo e ` pi` lordine e u chiaro dei robot. ` considerata solo Essendo la posizione del robot circa nota si e una regione parziale di It , allinterno della quale cercare il robot: It (h, k ) = It (h, k ) h = xi,old (t) X . . . xi,old (t) + X, k = yi,old (t) Y . . . yi,old (t) + Y (16) con X ed Y sufcienti da garantire che il robot i-esimo sia contenuto in It . Poich` e gli oggetti da identicare sono solo gli ` costruito potuto costruire un modello K adeguato e-puck si e da utilizzare per il ltraggio. Esso costituisce loggetto che si andr` a ad individuare nellimmagine It tramite correlazione: Ft (u, v ) =
u,v

da cui si calcola la necessaria per compiere una rotazione di = (k + 1) (k ). Verrebbe naturale prendere Tg pari ` precisi nel al tempo di campionamento, ma siccome non si e momento in cui il comando viene effettivamente dato al robot ` scelto di creare un piccolo sottoallinterno di un passo, si e ciclo di periodo Tg che si usa per la fase di giramento. Cio` e, si d` a al robot il comando , si aspetta senza fare niente per ` stato scelto Tg secondi, e poi si ferma il robot. Il tempo Tg e pari a 0.35 secondi perch` e cos` in un tempo massimo di 2Tg si compie la rotazione massima richiesta, ovvero radianti. C. Rilocalizzazione Come detto, durante il movimento la posizione dei robot viene fornita dagli encoder posti sui due motori degli e-puck. ` molto piccolo ed e ` dovuto Lerrore di una singola misura e essenzialmente a piccoli slittamenti delle ruote, molto sensibili alle minime imperfezioni del pavimento della pedana. Durante il moto si effettuano pi` u misure successive e, poich` e lerrore ad ` scorrelato da quello allistante precedente, ogni istante non e per tempi lunghi si ottengono errori di posizionamento non ` scelto di utilizzare trascurabili. Per rimediare a questo si e una telecamera ssata sopra la pedana, a simulare un GPS virtuale. Poich` e lacquisizione e lelaborazione dellimmagine richiedono un tempo signicativo rispetto al tempo di campionamento, ed inoltre si ha un rumore di misura molto maggiore

It (h, k )K (h u, k v )

(17)

Inne si vanno a cercare il massimi di Ft (u, v ): (uM , vM ) = arg max Ft (u, v ).

(u,v )

(18)

A questo punto i (xi,new (t), yi,new (t)) si ricavano facilmente dagli (uM , vM ) ponendo: xi,new (t) = uM + xi,old (t) X ; yi,new (t) = vM + yi,old (t) Y. (19)

` quello in cui limmagine It (h, k ) Un caso particolare e contiene pi` u robot e quindi la Ft (u, v ) presenta pi` u massimi. Per risolvere questo caso si va ogni volta ad azzerare la zona ` stato trovato le-puck ed a vericare se vi di Ft (u, v ) in cui e siano altri massimi dello stesso ordine di grandezza. Alla ne si sceglie il massimo pi` u vicino alla posizione originaria.
10 Si e ` stimata unapprossimazione di circa 6 gradi, almeno con i metodi di elaborazione dellimmagine da noi adottati

15

VIII. R ISULTATI E CONCLUSIONI Per testare gli algoritmi e lo schema di controllo generale ` sviluppato parallelamente alle esperienze pratiche un si e ` fedele ambiente simulativo in M ATLAB. In particolare si e rafnato e calibrato lalgoritmo di esplorazione, scegliendo opportunamente i pesi dellindice denito in Algoritmo 2. I test in laboratorio si sono rivelati ovviamente pi` u critici, nel senso che i problemi del mondo reale sono molti e spesso imprevedibili. Il primo di questi riguarda la connessione bluetooth tra il ` rivelata computer centrale e i vari robot e-puck. Essa si e alquanto lenta ed instabile. Si pensi che il tempo medio per connettere un robot si aggira intorno ai due minuti. Purtroppo, una soluzione a questo problema non esiste, e lunica cosa da ` armarsi di pazienza e dedicarci molto tempo. fare e Altro problema che non era stato previsto in fase di progetta` la calibrazione delle telecamere dei robot. I produttori zione e svizzeri degli e-puck, infatti, non hanno pensato di ssare in alcun modo la telecamera al telaio, attaccata solamente ` dovuto costruire tramite il cavo di trasmissione dei dati. Si e un supporto artigianale, composto da frammenti di plastica ` riuscito a creare uniti da scotch di carta. Ovviamente, non si e ` dovuta compiere una un sostegno standardizzato, dunque si e calibrazione11 della telecamera su misura per ogni robot, con ` uguale per tutti. conseguenza che il cono visivo non e Un altro problema legato allinstabilit` a della connessione ` la difcolta di denire il tempo richiesto dalla bluetooth e comunicazione, fondamentale per la precisione nelloperazione di rotazione del robot. In effetti nella soluzione da noi ` proposta questa avviene in catena aperta, e la precisione e dovuta esclusivamente ad una corretta temporizzazione, resa impossibile dal protocollo bluetooth. Questo non comporta un errore sulla misura dellorientazione del robot ma fa si che essa non sempre corrisponda a quanto calcolato nella ` fase di Exploration. Se ad esempio il tempo di rotazione e eccessivo pu` o capitare che si creino alcune zone inesplorate molto sottili tra due coni visivi, cos` che le celle esplorate ad esse vicine sono ritenute erroneamente frontiers cells portando a peggioramenti nellefcienza dellalgoritmo di esplorazione ` e quindi dellintero sistema. Per diminuire questeffetto si e deciso di applicare ltraggio a nucleo costante12 sulla mappa, applicando poi una soglia adeguata, cos` che poche celle inesplorate allinterno di una zona esplorata vengano in effetti considerate esplorate. Questa operazione computazionalmente onerosa per mappe di grandi dimensioni in realt` a sfrutta il ltraggio compiuto per inspessire gli ostacoli, con un costo praticamente nullo. Una volta risolte tutte le complicazioni del caso, i test sperimentali hanno mostrato il buon funzionamento del sistema progettato. In primo luogo si sono effettuate alcuni test di prova con solamente uno o due explorer, per vericare la ` introdotto lactor per costruzione della mappa, quindi si e alcuni test denitivi. I risultati sono stati positivi, per diverse mappe, e sono riportati nella gura 16. In particolare la gura
consiste nel determinare i valori di h ed f ` cio` nucleo e e costituito da una matrice quadrata p p in cui ogni 1 elemento ha valore p 2
12 Il 11 che

Figura 15: Prima del moto si conosce la posizione del robot, grazie al GPS ` possibile che vi sia un errore sulla misura della virtuale, mentre e ` indicato le-puck, mentre in rosso posizione del robot: in nero e ` segnata la sua posizione misurata. Dopo il movimento lerrore e sullorientazione iniziale causa anche un errore sulla posizione del robot (sempre in rosso), che viene corretto tramite una nuova rilocalizzazione (in nero). Langolo tra le due posizioni nali, segnato in verde coincide con lerrore sulla misura dellorientazione del robot, che pertanto potr` a essere corretta.

K Nel nostro caso il blob K stato ottenuto come: K = max K = dove K i=1 N ki . I ki rappresentano singole immagini quadrate di dimensione adeguate, che contengono le-puck in posizione centrale. Essi sono stati ricavati manualmente effettuando pi` u foto del robot in diverse posizioni della pedana e con diverse orientazioni, tramite il GPS virtuale. Effettuando due rilocalizzazioni successive, intervallate da ` possibile anche coruna fase di spostamento del robot e reggerne lorientazione con un semplice procedimento, illustrato in gura 15. Sia qi (t0 ) = (xi (t0 ), yi (t0 ), i (t0 )) la posizione del robot i prima del moto. Tramite una prima rilocalizzazione si ottiene una misura precisa della sua posizione, mentre lorientazione non pu` o essere corretta: q i (t0 ) = i (t0 )) (xi (t0 ), yi (t0 ), i (t0 )). Dopo il moto, ( xi (t0 ), y i (t0 ), al tempo t1 , lerrore sullorientazione comporta un errore anche sulla posizione per cui si ha il robot in posizione qi (t1 ) = (xi (t1 ), yi (t1 ), i (t1 )), mentre la sua misura data dali (t1 )). lodometria del robot risulta q i (t1 ) = ( xi (t1 ), y i (t1 ), Si effettua nuovamente la rilocalizzazione del robot ottenendo una misura, supponiamo esatta, della nuova posizione del i (t1 )). Lo sfasamento i i = robot:q i (t1 ) = (xi (t1 ), yi (t1 ), = (t0 ) = (t1 ) tra lorientazione del robot e la sua misura si pu` o perci` o ottenere come:

= arctan(

yi (t1 ) yi (t0 ) y i (t1 ) yi (t0 ) ) arctan( ). xi (t1 ) xi (t0 ) x i (t1 ) xi (t0 )

Le considerazioni sopra ipotizzano che lerrore di orientazione del robot sia esclusivamente dato una misura iniziale non corretta e che poi si mantenga costante durante il moto. Questo evidentemente non accade nella realt` a, dove lerrore sullorientazione si genera durante il moto del robot. Si pu` o facilmente vericare che in tal caso la tecnica adottata non consente di annullare, nemmeno teoricamente, lerrore sullorientazione ma contribuisce comunque ad una correzione almeno parziale.

16

(a) Mondo reale

(b) Mappa misurata Figura 16: Risultati: ricostruzione ambiente

(c) Mappa ltrata

16c mostra la mappa utilizzata per la fase di Exploration nella quale sono state eliminate le frontiers cells isolate come spiegato sopra. A PPENDICE A A SPETTI TECNICI ED IMPLEMENTATIVI A. Impostazione dellarea di ripresa della telecamera ` provvisto le-puck non e ` in grado di Il DSP di cui e memorizzare ed elaborare limmagine che viene prodotta dalla ` dotato, essa pertanto viene ridimentelecamera VGA di cui e sionata prima di passare al DSP. Lutente, tramite la libreria ePicKernel, pu` o gestire parzialmente il ridimensionamento impostando i parametri di campionamento dellimmagine o le sue dimensioni, ottenendo per` o sempre una porzione centrale dellimmagine di partenza. Per la nostra applicazione risulta utile allo scopo solo la parte dellimmagine sotto lorizzonte, visto che si vuole andare a costruire una mappa bidimensionale, ed anzi, risulta indispensabile ottenere unimmagine che comprenda il pi` u possibile la parte prossima alle-puck. Per fare ci` o e quindi poter utilizzare effettivamente la telecamera ` stato necessario andare a modicare il codice eseguibile carie ` dovuto modicare cato sul microprocessore di ogni e-puck. si e il codice C da cui era stato assemblato il le presente sullepuck e quindi compilarlo e ricavarne un eseguibile tramite il software MPLAB ed il compilatore C30 della GNU. B. Mapping Limplementazione della trasformazione descritta in prece` costituita fondamentalmente di due denza in M ATLAB si e parti. La prima ha implementato in uno script le equazioni (4) e (5) in modo da ricavare per ogni pixel del piano immagine il corrispondente raggio (xp , yp ) e la fase (xp , yp ). Questi valori sono raccolti in due matrici, che chiameremo modulo(xp , yp ) e fase(xp , yp ), di dimensione Xp Yp , cio` e 40 40. La denizione di tali valori richiede ovviamente di conoscere i valori del fuoco f e dellaltezza h della telecamera. ` risultata decisamente La stima di questi due parametri e complicata e causa di notevole dispendio di tempo poich` e le

telecamere montate sugli e-puck sono mobili, e la struttura degli stessi rende complicato ogni tentativo di ssarle adeguatamente. Pertanto prima di ogni utilizzo delle stesse si rendeva necessaria una fase di calibrazione per stimare tali parametri, effettuando alcune misure di ostacoli a distanza nota, durante la quale si procedeva sia a ripristinare la telecamera in una posizione standard, sia ad aggiornare i valori di questi parametri. Nel caso di utilizzo di pi` u robot esploratori si sono adoperati parametri diversi per ognuno di essi. Valori medi riscontrati in seguito a pi` u simulazioni sono f = 50 mm ed h = 44 espresso in pixel, cio` e in unit` a di dimensione ` di un pixel del piano immagine. Realizzata questa parte si e implementata una funzione che riceve in ingresso la posizione e lorienamento del robot ed il bordo e restituisce una matrice di dimensioni pari alla mappa globale, che rappresenta la misura fatta dal robot. Tale misura sar` a costituita ovviamente da celle di valore 0 ad indicare le celle che il robot ha visto libere, di valore 1 ad indicare le celle che il robot ha visto ` occupate e di valore per le celle che larea che non e stata interessata dalla misura del robot. La funzione crea una griglia quadrata di dimensioni ridotte pari al doppio della visuale massima del robot, in cui ogni cella corrisponde ad un centimetro quadrato. Si scorrono uno ad uno i valori del bordo(i), rendendo libero un settore circolare centrato nel centro del quadrato, di modulo modulo(i,bordo(i)) e per un intervallo fase centrato in fase(i,bordo(i))+ e la cui ` anchessa contenuta in una matrice analoga a ampiezza e queste due, facilmente ricavabile in base alle considerazioni sin qui esposte, che non si riporta perch` e costituerebbe una mera ripetizione dei concetti gi` a esposti13 . Occorre ricordare che questo approccio comporta una piccola approssimazione nella rappresentazione del cono visivo del robot, come mostrato ` come se si centrasse il facendo infatti e in gura 17. Cos` cono sul centro del robot e non pi` u sulla sua telecamera. Questa scelta, che come vedremo comporta unapprossimazione minima ci consente di semplicare la trasformazione da applicare, in quanto ci si riferisce automaticamente ai dati noti, cio` e a posizione e lorientazione del robot, ed in secondo
13 In ogni caso nel codice M ATLAB allegato alla tesi si potr` a trovare anche questo calcolo

17

Figura 17: Approsimazione sul cono visivo

luogo consente di ritenere esplorato lintera area circostante al robot tramite un numero sufciente i rotazioni succesive nella ` stessa posizione. Come detto il valore di approssimazione e ` molto piccola, minimo poich` e lampiezza del cono visivo e ` inferiore al la distanza della camera dal centro del robot e valore f del fuoco della stessa, ed inoltre larea che si ritiene esplorata in pi` u, viene spesso compresa nelle osservazioni successive. Inne poich` e il robot effettua le sue misure solo da aree gi` a esplorate in precedenza il fatto di ritenere esplorata la zona intorno alla camera non porta ad alcun rischio di mancata visione di ostacoli. Si sottolinea inne come questo non comporti approssimazioni ulteriori (oltre a quelle legate alla discretizzazione della mappa e del piano immagine) sul corretto posizionamento degli ostacoli. In caso sia stato individuato un ostacolo esso viene segnato D. Rilocalizzazione sulla mappa per uno spessore di circa 3 cm, mentre nel caso ` stato implementato esattamente In M ATLAB lalgoritmo e bordo(i)=0, cio` e in cui la colonna i del piano immagine non come indicato. Per la costruzione del blob K si sono utilizzate visualizzi ostacoli, lampiezza libera sar` a pari a modulo(i,40), 15 immagini, 10 per costruire effettivamente limmagine ed e non sar` a aggiunto alcun ostacolo. Essendo il tempo di calcolo altre 5 per vericarne il funzionamento, ottenendo che il ` pensato massimo trovato differiva dal centro del robot di al pi` un parametro critico del nostro lavoro, in realt` a si e u 2-3 di lavorare riportandosi al primo quadrante, e cio` e la griglia pixel, pari a circa 0.5 cm. quadrata su cui si lavora a dimensioni pari al massimo raggio visuale del robot. Dopodich` e ci si riconduce alla corretta E. Tecniche per la simulazione del Mapping orientazione tramite alcune operazioni di traslzione o capoPer la verica del sistema creato in ambiente simulativo si volgimento della matrice stessa. Lultimo banale passaggio consiste nel collocare questa mappa locale cos` calcolata sono utilizzate mappe virtuali realizzate tramite programmi di ` stata effettuata nella maallinterno di una mappa di dimensioni pari alla mappa globale, graca. La simulazione del sensore e u fedele possibile,imitando lampiezza e la lunghezza secondo la posizione attuale delle-puck che ha effettuato la niera pi` ` documentato in gura 19. del cono visivo, come e rilevazione. Per quanto riguarda laggiornamento della mappa inve` banale: ce la funzione che implementa loperatore e A PPENDICE B M (k ) = min(M (k 1), m(k )), cio` e Mij (k ) = min(Mij (k A PPARATO S TRUMENTALE 1), mij (k )). a) Ambiente per la sperimentazione: Come area di la` utilizzata una pedana appoggiata sul pavimento, voro si e C. Path-Planning su cui si possono muovere gli e-puck. La pedana misura ` pensato di utilizzare Una naturale implementazione dellalgoritmo D* impliche- 3.20m2.40m. In un primo momento si e ` rebbe lutilizzo di strutture dati tipo liste concatenate o altre il computer sso presente in laboratorio. In seguito si e a strutture dati. Purtroppo, M ATLAB offre soltanto i vettori. Si preferito utilizzare un computer portatile per maggiori capacit` ` provvisto di rete bluetooth utilizzata per la ` dunque organizzato il tutto secondo questa struttura. Si di calcolo. Il pc e e sono deniti quattro vettori, t, b, h, k che rappresentano connessione con gli e-puck. rispettivamente il tag (etichetta), il nodo puntato, la funzione b) E-puck: Per sperimentare gli algoritmi ideati in questo ` utilizzato un veicolo h(X ) e la funzione k (X ), e i nodi sono rappresentati dagli progetto in un ambiente reale, si e ` molto indici di tali vettori. Con unopportuna funzione si trasformano mobile denominato e-puck. La meccanica di tale robot e ` basata su un corpo unico i due indici della mappa (le due cordinate), attraverso i quali semplice ed elegante: la struttura e verrebbe pi` u naturale identicare i nodi, in un unico indice del diametro di 70 mm a cui sono ssati il supporto del motore, ` anchessa il circuito e la batteria. La batteria, allocata al di sotto del lineare, come si vede in gura 12. La lista aperta e

rappresentata da un vettore, anzi, da una matrice la cui prima colonna contiene i numeri che individuano i nodi ad essa appertenenti, la seconda il valore della rispettiva funzione k . Questo per facilitarne loridinamento. Con queste denizioni, ` poi stato facile implementare lalgoritmo 3. e Nonostante la capacit` a di aggiornarsi iterativamente, lalgoritmo rimane di complessit` a abbastanza elevata, e naturalmente ` applicato crescente con il numero dei nodi. Allora D* non si e alla mappa completa, ma ad una mappa sotto-campionata ` ridotta prendendo un pixel ogni due. In questo modo, si e la complessit` a di un fattore 4. ` trovato supponendo il robot puntiforInoltre, il cammino e ` tale. Allora si e ` utilizzata una mappa me. Ma esso non e modicata in cui le celle occupate vengono orlate, cio` e ampliate in ogni direzione della dimensione pari a (un po pi` u del) raggio del robot, in modo da essere certi che esso vi passi. Ci sono poi alcune considerazioni da fare su come generare un effettivo riferimento in posizione per i robot. Dato il cammino, si devono denire gli istanti di tempo cui il robot ` interpolata la traiettoria deve attraversare i nodi. Per farlo, si e in modo che ad ogni istante di campionamento la strada che deve percorrere il robot sia sufcientemente corta da far s` che venga percorsa tutta.

18

200

200

200

400

400

400

600

600

600

800

800

800

1000

1000

1000

1200

200

400

600

800

1000

1200

1400

1600

1200

200

400

600

800

1000

1200

1400

1600

1200

200

400

600

800

1000

1200

1400

1600

(a) I0

(b) It

(c) It

10

50

50

50

20 100 30 150 40 200 200 200 150 150 100 100

50

60

250

250

250

70 10 20 30 40 50 60 70

300 50 100 150 200 250 300

300 50 100 150 200 250 300

300 50 100 150 200 250 300

(d) K ,blob

(e) It ,robot in alto

(f)

massimo

(g)

massimo

Figura 18: Immagini utilizzate nella fase di rilocalizzazione dei robot. In particolare nella gura 18f si osservano i due massimi dovuti a due robot. Quello pi` u ` leggermente maggiore e viene individuao per primo, poi viene cancelatoe si ottiene la gura 18g da cui si ricava il secondo massimo. in basso e Tra i due viene scelto questultimo poich` e pi` u vicino alla posizione prevista per il robot, che corrisponde al centro delle gure 18e, 18f e 18g.

un secondo. La comunicazione avviene via bluetooth tramite un chip bluetooth , LM X 9820A, potendo cos` comodamente accedere al robot come se fosse connesso ad una porta seriale. Lunica differenza consiste nel fatto che il LM X 9820A manda comandi particolari per la sua connessione e sconnessione (intervallo di comunicazione: min : 15cm, max : 5m). Il robot e-puck possiede inoltre tre microfoni (massima velocit` a di acquisizione: 33kHz ), un accelerometro 3D (strumento che misura la forte micro-accelerazione e micro-decelerazione che possiede il robot quando si muove), 8 sensori di prossimit` a IR (Infrared proximity sensors), una telecamera (risoluzione: 640(h) 480(l) pixel, a colori) e 8 led colorati (rossi) , tutti controllati utilizzando 24 segnali (Pulse Width Modulated) separati, generati da un microcontrollore posizionato internameno essere controllato te all e-puck (P IC 18F 6722). Ogni led pu` singolarmente, per una maggiore essibilit` a, oppure in maniera sincronizzata, semplicemente settando pochi parametri (come la massima luminosit` a,il periodo di intermittenza etc.). c) Telecamera: Per visualizzare i robot e acquisire la ` utilizzata una telecamera loro posizione in tempo reale si e Logitech QuickCam (risoluzione 1200 1600) ssata al softto perpendicolarmente al piano di lavoro. ` stata La telecamera utilizzata (Logitech QuickCam ) e ` dotata infatti di scelta poich` e considerata molto efciente: e un trigger esterno asincrono, che permette di acquisire le immagini in modo istantaneo, senza ritardi signicativi. Tale strumento inoltre d` a la possibilit` a di scandire limmagine dellintera pedana di lavoro permettendo di sfruttare tutto lo spazio disponibile per le misurazioni. Per lacquisizione e ` utilizzalutilizzo dellimmagine nellambiente M ATLAB si e

Figura 19: Costruzone della mappa in ambiente simulativo

` collegata al circuito con due contatti verticali e pu` motore, e o facilmente essere estratta per essere ricaricata. Il motore a passo con riduttore di marcia realizza 20 passi per rivoluzione e la marcia possiede una riduzione di 50 : 1. Le due ruote hanno un diametro di circa 41 mm, e possiedono un sottile pneumatico di color verde, sono distanti circa 53 mm ` di 100 passi e la massima velocit` a che possono raggiungere e al secondo, che corrisponde ad un giro completo effettuato in

19

[10] C. Lindsay, Automatic planning of safe and efcient robot paths using octree representation of conguration space, in IEEE Transactions on Robotics and Automation, 1987. [11] Y. K. Hwang and N. Ahuja, A potential eld approach to path planning, IEEE Transactions on Robotics and Automation, vol. 8, pp. 2332, Feb. 1992. [12] E. W. Dijkstra, A note on two problems in connexion with graphs, Numerische Mathematik, vol. 1, pp. 269271, 1959. [13] P. E. Hart, N. J. Nilsson, and B. Raphael, A formal basis for the heuristic determination of minimum cost paths, IEEE Transactions on Systems Science and Cybernetics, vol. SSC-4(2), pp. 100107, 1968. [14] A. D. L. G. Oriolo and M. Vendittelli, Wmr control via dynamic feedback linearization: Design, implementation, and experimental validation, IEEE Transactions on Systems Technology, vol. 10, no. 6, pp. 835852, 2002. [15] A. D. Luca, Diapositive del corso di robotica 1, universit` a di roma sapienza.

Figura 20: Epuck

Figura 21: Logitech QuickCam

to il pacchetto imaqtool, in modo da ottenere la matrice dellimmagine corrente tramite il comando getsnapshot. R IFERIMENTI BIBLIOGRAFICI
[1] S. Thrun, Robotic mapping: A survey, in Exploring Articial Intelligence in the New Millenium. Morgan Kaufmann, 2002. [2] Various, Articial Intelligence and Mobile Robots: Case Studies of Succesful Robot Systems, R. M. David Kortenkamp, R. Peter Bonasso, Ed. AAAI Press, 1998. [3] B. Yamauchi, A. C. Schultz, and W. Adams, Mobile robot exploration and map-building with continuous localization, in ICRA, 1998, pp. 37153720. [4] B. Yamauchi, A frontier-based approach for autonomous exploration, Oct. 30 1997. [Online]. Available: http://citeseer.ist.psu.edu/178266.html;http://www.aic.nrl.navy.mil/ yamauchi/papers/compressed/cira97.ps.gz [5] W. Burgard, M. Moors, C. Stachniss, and F. Schneider, Coordinated multi-robot exploration, IEEE Transactions on Robotics, vol. 21, pp. 376386, 2005. [6] A. Franchi, L. Freda, G. Oriolo, and M. Vendittelli, A randomized strategy for cooperative robot exploration, in ICRA. IEEE, 2007, pp. 768774. [Online]. Available: http://dx.doi.org/10.1109/ROBOT.2007. 363079 [7] A. Stentz, Optimal and efcient path planning for partially-known environments, in In Proceedings of the IEEE International Conference on Robotics and Automation, 1994, pp. 33103317. [8] J. C. Latombe, Robot Motion Planning. Boston, MA: Kluver Academic Publishers, 1991. [9] A. R. Soltani, H. Tawk, J. Y. Goulermas, and T. Fernando, Path planning in construction sites: performance evaluation of the dijkstra, A , and GA search algorithms, Advanced Engineering Informatics, vol. 16, no. 4, pp. 291303, 2002. [Online]. Available: http://dx.doi.org/10.1016/S1474-0346(03)00018-1