Introduzione 1 1 Localizzazione probabilistica con filtraggio Bayesiano 4
1.1 Tassonomia dei problemi di localizzazione . . . 4
1.2 Tassonomia dei filtri bayesiani . . . 7
1.2.1 Metodi probabilistici . . . 9
1.3 Filtro di Kalman. . . 11
1.3.1 Extended Kalman Filter, EKF . . . 19
1.4 Modelli sensoriali . . . 21
1.4.1 Modelli per considerare l’incertezza geometrica. . . 22
1.4.2 SPModel . . . 24
1.4.3 Modelli a feature rettilinee . . . 28
1.4.4 Modelli a segmenti . . . 29
1.5 Associazione di dati . . . 30
2 Realizzazione del sistema 32 2.1 Nomad 200 . . . 32
2.2 Setup Laser scanner 2D SICK S3000 . . . 33
2.2.1 Configurazione Sick S3000 . . . 36
2.2.2 Protocollo RK512 . . . 38
2.2.3 Implementazione di un driver per l’S3000 . . . 40
2.3 Design della libreria Spf . . . 41
2.3.1 Design aggiornato . . . 43
ii
3 Algoritmi per feature rettilinee 46
3.1 Mapping . . . 46
3.1.1 Dati per costruzione mappe . . . 47
3.1.2 Costruzione mappe . . . 47
3.1.3 Algoritmo di riduzione della mappa . . . 48
3.1.4 Assottigliamento di mappe . . . 52
3.2 Algoritmi per la selezione di feature rettilinee in mappe . . . 55
3.2.1 Feature rettilinee . . . 56
3.2.2 Algoritmo simple select feature . . . 56
3.2.3 Algoritmo ray tracing . . . 57
4 Risultati 61 4.1 Risultati laser scanner S3000 . . . 61
4.2 Risultati sulle mappe . . . 62
4.3 Risultati di localizzazione . . . 69
5 Conclusioni 74 5.1 Conclusioni . . . 74
5.2 Sviluppi futuri . . . 75
Bibliografia 76
1.1 Tassonomia metodi bayesiani. . . 7
1.2 Rappresentazione Hessiana della retta. . . 29
2.1 Il Nomad200. . . 32
2.2 Il Nomad200 in dotazione al RimLab. . . 33
2.3 Laser Scanner Sick S3000. . . 34
2.4 Principio di funzionamento del laser scanner Sick S3000. . . 35
2.5 Campi di applicazione mobili dell’S3000. . . 37
2.6 Interfaccia di configurazione del laser scanner Sick S3000. . . 37
2.7 Diagramma della struttura generale della libreria spf. . . 43
2.8 Diagramma UML delle classi relativo al modello sensoriale. . . 44
2.9 Diagramma relativo a Plottable e Plotter. . . 45
3.1 Trasformata di Hough. . . 50
3.2 Distanza di Hausdorff. . . 51
3.3 Risultati algoritmo di thinning applicati a una mappa in uso. . . 53
3.4 Applicazione dell’algoritmo simple select feature. . . 57
3.5 Ray-tracing con 10 linee dalla posa. . . 57
3.6 Ray-tracing con 20 segmenti dalla posa. . . 59
3.7 Confronto tra le feature selezionate nei due algoritmi. . . 60
4.1 Mappa ottenuta con EKF senza alcuna procedura di fusione. . . 64
4.2 Mappe ottenute con scan matching senza alcuna procedura di fusione. 64 4.3 Mappa ottenuta con EKF con fusione. . . 65
4.4 Mappa ottenuta con scan matcher con procedura di fusione. . . 66 4.5 Mappe del laboratorio di robotica ottenute con algoritmo di scan matcher. 67
iv
4.6 Mappe assottigliate. . . 68 4.7 Localizzazione in corridoio tramite EKF. . . 69 4.8 Localizzazione in laboratorio tramite EKF. . . 71
3.1 Rappresentazione otto vicinato del punto P1. . . 54
4.1 Precisione del laser scanner. . . 61
4.2 Caratteristiche del PC utilizzato . . . 62
4.3 Errori di localizzazione presso check point in corridoio in m. . . 70
4.4 Tempi esecuzione step associazione feature in sec.. . . 70
4.5 Errori di localizzazione presso check point in laboratorio in m. . . 71
4.6 Tempi di esecuzione nella simulazione del laboratorio in sec. . . 72
vi
3.1 Algoritmo di riduzione della mappa. . . 49 3.2 Algoritmo di ray tracing. . . 59
vii
Uno dei compiti fondamentali della robotica mobile `e stimare la posizione e l’orien- tamento, cio`e la posa, di un robot. I metodi di localizzazione dipendono dal contesto operativo e dalla sensorialit`a del sistema robot-ambiente.
A volte, `e importante conoscere la variazione temporale della posa, cio`e la va- riazione relativa rispetto alla posa precedente. Accade spesso in ambito d’ufficio o industriale. `E il caso dei robot museali, tra i quali ricordiamo RHINO, una guida tu- ristica completamente autonoma installata presso il Museo di Bonn, [8]. `E in grado di condurre i visitatori da una mostra a un’altra calcolando il cammino, ricavato da una mappa del museo. In ambito industriale, `e molto conosciuto il sistema NAV 200, prodotto da Sick. Questo sistema `e composto da uno scanner laser rotante posto sulla sommit`a del veicolo. Nell’ambiente vengono posti alle pareti dei riflettori, strisce ca- tarifrangenti o riflettori cilindrici. Mappata nel computer del veicolo la posizione dei riflettori, la scansione laser consente, ad ogni rotazione, la deduzione della posizione istantanea del veicolo. Il rilevamento di tre riflettori `e sufficiente per la determinazione della posizione del veicolo.
Altre volte, `e importante conoscere la posizione del robot nel sistema di riferimen- to globale (problema di localizzazione globale). `E l’idea alla base, ad esempio, dei navigatori satellitari, o dei piloti automatici degli aerei. Un ricevitore GPS localizza, secondo il sistema di riferimento globale, il navigatore, associandolo a un punto ben definito della mappa.
Se, invece, non si conosce in alcun modo l’ambiente, si affronta un problema di SLAM (Simultaneous Localization And Mapping). Il robot costruisce una mappa del- l’ambiente che lo circonda utilizzando i sensori di cui `e dotato, e qui si localizza. `E uno dei problemi affrontati, ad esempio, dai robot che esplorano ambienti estremi, quali miniere, vulcani, ambienti spaziali, sottomarini. Un esempio, tra i pi`u conosciu-
1
ti, `e Sojourner, progettato dalla NASA, che ha esplorato per primo Marte nel 1997. `E anche il caso di robot che intervengono in situazioni di emergenza: ad esempio, dopo un crollo, in seguito a un incidente stradale, dopo un terremoto. Tutti i sistemi robotici elencati hanno affrontato, spesso con successo, il problema della localizzazione.
Nell’analisi e nella risoluzione del problema di localizzazione si devono analizzare diverse problematiche, tra le quali l’analisi dell’algoritmo risolutivo, il modello senso- riale, il processo di associazione dei dati. Tra gli algoritmi pi`u significativi utilizzati nell’ambito della localizzazione c’`e il filtro di Kalman esteso, [8,26]. Il filtro di Kal- man `e il modello ottimo per determinare l’evoluzione dello stato in sistemi lineari con termini di rumore gaussiani. Tuttavia, nello studio di applicazioni reali i modelli non sono lineari. Il filtro di Kalman esteso, in particolare, ottiene risultati migliori, rispetto al filtro di Kalman semplice, in modelli non lineari.
Esistono, in letteratura, svariati modelli di estrazione di feature da dati sensoriali:
modelli per feature puntiformi (spigoli, punti d’angolo, colonne), modelli per feature rettilinee. Spesso, per aumentare la conoscenza dell’ambiente circostante sono utiliz- zati pi`u modelli contemporaneamente. Ci`o aumenta anche la complessit`a associata. Il modello di estrazione dati considerato in questa tesi `e un modello per feature rettilinee.
Le feature rettilinee sono ottenute applicando massivamente la trasformata di Hough ai dati estratti.
I dati estratti dai sensori sono intrinsecamente associati a errori di valutazione.
Spesso si incontrano in letteratura modelli che tengono conto dell’incertezza del dato sensoriale. I modelli probabilistici, infatti, rappresentano l’incertezza della posizione di un elemento geometrico usando una probabilit`a di distribuzione, spesso Gaussiana.
Un aspetto fondamentale del processo di localizzazione `e il modello di associa- zione dei dati, che aggrega le letture sensoriali a elementi della mappa. Per poter localizzare il robot all’interno dell’ambiente, `e in genere necessario costruire mappe dell’ambiente in uso. La bont`a della costruzione di mappe dipende, ancora una volta, dalla localizzazione del robot nell’ambiente in oggetto.
L’obiettivo di questo progetto di tesi `e la realizzazione del sistema di localizzazione per un veicolo robotico, equipaggiato con un sensore laser scanner. In particolare, tale sistema deve essere in grado di operare in un ambiente non strutturato e caratterizzato dalla presenza di ingombri, ovvero un cosiddetto cluttered environment, quale pu`o essere un ufficio o un magazzino industriale. Il progetto sviluppato nella tesi avvicina,
quindi, alcune metodologie sviluppate in ambito di ricerca alle concrete applicazioni industriali.
La tesi `e organizzata nel modo seguente. Nel Capitolo 1 si discute del proble- ma della localizzazione, descrivendo brevemente gli algoritmi pi`u diffusi per la sua risoluzione; si illustrano, infine, il filtro di Kalman e il filtro di Kalman esteso. Nel Capitolo 2 si presenta il sistema robotico utilizzato e se ne descrive la capacit`a sen- soriale. Nel Capitolo 3, si illustrano gli algoritmi di associazione dei dati sensoriali e di mapping implementati. Nel Capitolo 4 si illustrano i risultati di localizzazione e di mapping. Infine, nel Capitolo 5, sono esposte le considerazioni finali e sviluppi futuri.
Localizzazione probabilistica con filtraggio Bayesiano
1.1 Tassonomia dei problemi di localizzazione
Non tutti i problemi di localizzzazione sono equivalenti. Per capire la difficolt`a del problema di localizzazione, ne proponiamo una tassonomia, come descritto in [26].
Questa tassonomia divide il problema di localizzazione attraverso le sue dimensioni, come la natura dell’ambiente e la conoscenza iniziale che il veicolo pu`o possedere.
Localizzazione globale - locale
I problemi di localizzazione sono caratterizzati dal tipo di conoscenza disponibile ini- zialmente e a run-time. Si distinguono tre tipologie di problemi di localizzazione, con un crescente grado di difficolt`a.
Il position tracking assume che la posa iniziale sia conosciuta. Si ottiene la localiz- zazione inserendo rumore nel moto del robot. L’effetto del rumore di solito `e piccolo.
Quindi, i metodi per il position tracking si basano sull’assunto che l’errore di posa sia piccolo. L’incertezza della posa `e spesso approssimata da una distribuzione unimoda- le, ad esempio Gaussiana. Il problema di position tracking `e un problema locale, dal momento che l’incertezza `e locale e confinata alla regione vicina alla posa reale del robot.
Nella localizzazione globale, la posa iniziale del robot non `e conosciuta. Il ro-
4
bot `e inizialmente disposto in una qualunque posizione dell’ambiente, ma manca della conoscienza di dove sia. Gli approcci di localizzazione globale non possono non con- siderare l’errore della posa. Distribuzioni di probabilit`a unimodali sono di solito inap- propriate. La localizzazione globale `e pi`u generale rispetto al position tracking; infatti, questo problema contiene, come caso particolare, il problema di position tracking.
Il problema kidnapped robot `e una variante del problema della localizzazione glo- bale, ma di risoluzione molto pi`u difficile. Durante l’operazione, il robot pu`o essere rapito e portato in qualche altro luogo. `E un problema pi`u difficile rispetto al pro- blema della localizzazione globale, infatti, il robot potrebbe pensare di sapere la sua posizione, ma in realt`a l’ipotesi `e completamente errata. Nella localizzazione globa- le, invece, il robot sa di non conoscere la sua posizione. Si potrebbe sostenere che il problema kidnapped robot sia un problema senza applicazione pratica. L’importanza pratica di questo problema si sviluppa dall’osservazione che la letteratura riguardo gli algoritmi di localizzazione non garantisce il non fallimento. L’abilit`a di recuperare da failure `e essenziale per robot completamente autonomi. Il testing di un algoritmo di localizzazione con kidnapping mostra l’abilit`a del robot di recuperare da failure di localizzazioni globali.
Ambienti statici - dinamici
Una seconda dimensione del problema, che ha un impatto sostanziale riguardo la difficolt`a della localizzazione `e l’ambiente. L’ambiente pu`o essere statico o dinamico.
Ambienti statici sono ambienti dove la sola quantit`a variabile `e la posa del robot.
Solo il robot si muove, mentre il resto dell’ambiente `e statico. Tutti gli altri oggetti nell’ambiente rimangono nella stessa posizione. Ambienti statici possiedono alcu- ne propriet`a matematiche che possono rendere il problema di localizzazione gestibile mediante stime probabilistiche efficienti.
Ambienti dinamici possiedono oggetti la cui posizione o configurazione cambia nel corso del tempo. I cambiamenti che non sono misurabili non sono ovviamente di alcuna rilevanza, e sono tratti spesso come rumore. Esempi di cambiamenti persistenti sono: le persone, la luce (da considerare se tra i sensori c’`e una telecamera), le porte, gli oggetti mobili. Pi`u gli ambienti reali sono dinamici, pi`u i cambiamenti di stato avvengono a range di velocit`a differenti.
Ovviamente, la localizzazione in ambiente dinamico `e pi`u difficile rispetto ad una
in ambiente statico. Esistono due approcci prinicpali per gestire la dinamicit`a. Nel primo, le entit`a dinamiche sono incluse nel vettore di stato. Come risultato, si rende valida l’ipotesi di Markov, ma tale approccio porta ad elevati oneri computazionali e di modellazione. Nel secondo approccio, i dati sensoriali sono filtrati in modo da eliminare gli effetti dannosi di dinamiche non modellate.
Approcci passivi - attivi
Una terza dimensione che caratterizza i differenti problemi di localizzazione si riferi- sce alla relazione tra l’algoritmo di localizzazione e il controllo del moto del robot. Si distinguono due casi: una localizzazione passiva e una attiva.
Nella localizzazione passiva, il modulo di localizzazione osserva solamente l’ope- rato del robot. Nella localizzazione attiva, gli algoritmi di navigazione controllano il robot per minimizzare l’errore di localizzazione. Gli approcci attivi alla localizzazione di solito danno risultati di localizzazione migliori rispetto a quelli passivi. Una limi- tazione chiave degli approcci attivi `e che essi richiedono un controllo sul robot. Nelle applicazioni pratiche, una tecnica di localizzazione attiva tende ad essere insufficien- te. Il robot, infatti, deve essere in grado di localizzarsi anche nel caso in cui compia altri task in aggiunta alla localizzazione. Alcune tecniche di localizzazione attiva so- no costruite su tecniche passive, altre combinano obiettivi di performance di task con obiettivi di localizzazione.
Robot singolo - multi-robot
Una quarta dimensione del problema `e relativa al numero di robot considerati. La localizzazione a singolo robot `e l’approccio maggiormente studiato. La localizzazione a singolo robot offre il vantaggio che tutti i dati sono raccolti sulla stessa piattaforma, e non c’`e bisogno di comunicazione.
La localizzazione multirobot si sviluppa in gruppi di robot. A un primo sguardo, ogni robot potrebbe localizzarsi individualmente, spostando il problema di localizza- zione multi-robot in molti problemi di localizzazione a singolo robot. Se i robot sono in grado di rintracciarsi, c’`e opportunit`a di operare in modo migliore. Infatti, la stima dello stato di un robot pu`o essere utilizzata per verificare la stima dello stato di un altro robot se la conoscenza della posizione relativa di entrambi i robot `e disponibile.
Figura 1.1: Tassonomia metodi bayesiani.
Il problema di una localizzazione multi-robot diventa interessante, come il problema non banale della rappresentazione delle ipotesi di stato e la natura della comunicazione tra i robot.
Queste quattro dimensioni catturano le quattro pi`u importanti caratteristiche del problema della localizzazione nella robotica mobile. Esistono un numero notevole di altre caratteristiche che descrivono la difficolt`a del problema, come le informazioni sensoriali estratte dal robot e le informazioni perdute attraverso il moto. Ancora, am- bienti simmetrici sono pi`u difficili rispetto a ambienti asimmetrici, a causa di gradi di ambiguit`a maggiori.
1.2 Tassonomia dei filtri bayesiani
In questa sezione, si vogliono brevemente elencare alcuni algoritmi di classificazione bayesiani, secondo lo schema proposto anche in [11]. Lo scopo `e definire un quadro che tenga conto dell’ampio spettro di scelte algoritmiche e progettuali possibili.
Le principali famiglie di algoritmi sono due, come descritto in Figura 1.1: quelli continui, e quelli discreti, in base al tipo di rappresentazione adottata per lo stato. La continuit`a non riguarda il dominio cui lo stato appartiene, bens`ı il fatto che questi metodi rappresentano analiticamente la densit`a di probabilit`a dello stato.
I filtri bayesiani discreti discretizzano la rappresentazione dello stato, la sua densit`a di probabilit`a, e, con modalit`a differenti, lo spazio. L’approccio topologico `e basato su rappresentazioni simboliche, strutturate a grafo, dell’ambiente. Lo spazio degli stati consiste di un insieme di posizioni discrete, localmente distinguibili, come gli spigoli.
Il vantaggio di questo approccio sta nella sua efficienza e nella forte riduzione dello spazio degli stati. Il limite risiede nell’assenza di informazioni metriche e, dunque, nella grossolanit`a della rappresentazione.
L’approccio metrico, grid based, si affida a una rappresentazione discreta della stima della posizione. Come per l’approccio topologico, questi metodi possono rap- presentare distribuzioni arbitrarie nello spazio degli stati discreto e possono risolvere il problema di localizzazione globale. In contrasto con l’approccio topologico, l’approc- cio metrico fornisce accurata stima della posizione in combinazione a un’alta robu- stezza al rumore. Uno svantaggio `e sicuramente rappresentato dalla sua elevata com- plessit`a computazionale, dovuta alla necessit`a di mantenere una griglia di probabilit`a di posizioni, tipicamente a tre dimensioni, e aggiornarla ad ogni nuova osservazione dell’ambiente. Modelli sensoriali efficienti, schemi di aggiornamento selettivi, una rappresentazione ad albero adattiva, migliorano notevolmente l’efficienza di questo metodo, rendendolo applicabile a localizzazioni in real-time. Esempio conosciuto `e RHINO, robot e guida turistica all’interno del museo di Bonn.
I filtri bayesiani continui comprendono il filtro di Kalman, [8,26], e le sue varianti.
La densit`a di probabilit`a `e sempre rappresentata da una PDF gaussiana identificabile tramite le sue statistiche del primo e del secondo ordine, cio`e vettore delle medie e la matrice di covarianza. Il filtro di Kalman `e ottimo nel caso in cui il sistema os- servato sia lineare. In pratica, purtroppo, i sistemi non sono quasi mai lineari. Il filtro di Kalman esteso, EKF, si applica a modelli di sistemi non lineari, attraverso la linearizzazione delle equazioni che rappresentano l’evoluzione. Non garantisce l’ot- timalit`a, ma spesso ottiene risultati migliori rispetto al filtro di Kalman semplice in ambiti non lineari. Uno dei vantaggi del filtro di Kalman `e la sua efficienza. Grazie alla loro bassa complessit`a computazionale, i filtri di Kalman possono essere applicati allo SLAM, dove stimano a posteriori sia le posizioni del robot sia le posizioni dei landmark, tipicamente centinaia di dimensioni.
Sia il filtro di Kalman sia l’EKF impiegano una sola distribuzione per rappresentare lo stato, sono adatti a rappresentare, quindi, un’unica ipotesi di posizione; l’incertez-
za riguarda un intorno della media della gaussiana. Non sono, questi, presupposti sufficienti a garantire una localizzazione globale.
I metodi multi-hypothesis tracking rappresentano la stima dello stato tramite una miscela di Gaussiane. Ogni ipotesi, o Gaussiana, `e tipicamente estratta da un EKF.
Grazie alla loro abilit`a di rappresentare stime multi-modali, questi approcci sono in grado di rappresentare la localizzazione globale. Non appare chiaro, tuttavia, come questi metodi possano essere applicati a sistemi fortemente non-lineari. In aggiun- ta, l’approccio multi-hypothesis richiede euristiche molto sofisticate per risolvere il problema dell’associazione dei dati e per determinare quando aggiungere o eliminare ipotesi.
Tutte le metodologie dette continue hanno il vantaggio di essere formulate con ben precise ipotesi analitiche e costituiscono un importante modello teorico di riferimento.
Di seguito si far`a una panoramica concettuale dei metodi probabilistici e si presen- ter`a il filtro di Kalman per i sistemi lineari. Poi, si descriver`a il filtro di Kalman esteso (EKF), una variante del filtro di Kalman utilizzata per i sistemi non lineari.
1.2.1 Metodi probabilistici
In questo paragrafo, si introduce il concetto di probabilistic estimation considerando il problema fondamentale della stima della localizzazione di un robot mobile. Con probabilistic localizationsi intende un algoritmo probabilistico che mantiene una di- stribuzione di probabilit`a nello spazio degli stati. La rappresentazione probabilistica esamina direttamente le incertezze che nascono dai modelli di moto e dalle letture ru- morose dei sensori. L’obiettivo `e mantenere la densit`a di probabilit`a della posizione su tutte le possibili pose del robot. Cos`ı la densit`a pu`o avere forme arbitrarie, rap- presentando vari tipi di informazione circa la posizione del robot. Ad esempio, lo stato del robot pu`o avere inizialmente una distribuzione uniforme, che rappresenta una completa incertezza riguardo la sua posizione, cio`e il robot potrebbe essere con uguale probabilit`a in una qualsivoglia altra posizione. Nei casi, frequenti, in cui il robot ha un’elevata certezza circa la sua posizione, la posizione del robot consiste di una di- stribuzione unimodale centrata intorno alla sua posizione reale. Il Filtro di Kalman, descritto nella sezione1.3, `e una forma di probabilistic estimation dove la stima `e una PDF Gaussiana (unimodale). Altri metodi di probabilistic estimation, come i metodi bayesiani e il particle filtering, possono gestire distribuzioni pi`u generali.
Si pu`o ottenere un metodo di localizzazione di un robot mobile integrando la velo- cit`a del robot da una posizione di partenza. Tale metodologia viene detta dead recko- ning. Se i comandi sono eseguiti perfettamente e la posizione del robot fosse perfetta- mente conosciuta, questo metodo darebbe una stima perfetta della sua posizione. Cer- tamente, esecuzione e conoscenza perfette sono impossibili nel mondo reale. Errori tra i comandi precedenti e attuali si accumulano nel tempo. In altre parole, man mano che il robot si muove, esso perde continuamente informazioni riguardo la sua posizione.
Pertanto, il robot accumuler`a tanto errore che la stima non avr`a alcun significato.
Con qualche estensione, il comando di integrazione pu`o essere pensato come un metodo di probabilistic estimation. Dapprima consideriamo la posizione di partenza del robot. Nel mondo reale, la posizione di partenza non pu`o essere conosciuta perfet- tamente; ha senso rappresentarla con una PDF nello spazio degli stati, ad esempio, lo spazio di tutte le posizioni del robot. Se `e disponibile una buona stima della posizione iniziale, la PDF avr`a un picco presso la posizione, e la nitidezza del picco rappresen- ter`a la certezza della stima iniziale: pi`u lo stato iniziale `e certo, pi`u alto e stretto sar`a il picco. Ora la sfida `e propagare questa distribuzione di probabilit`a mentre il robot si muove. La locazione del picco `e propagata integrando i comandi di velocit`a come descritto in precedenza. Comunque, dal momento che i comandi non sono eseguiti in modo perfetto, la stima diventa pi`u incerta man mano che scorre il tempo e il picco diviene pi`u basso e largo. Con il procedere della navigazione, il picco diventer`a cos`ı basso che la stima perder`a ogni senso.
L’obiettivo della probabilistic estimation `e prevedere una stima significativa dello stato del sistema, la posa del robot in questo caso. La criticit`a, usando il comando di integrazione, `e che si perde continuamente informazione e non se ne guadagna alcu- na. La soluzione `e iniettare nel sistema nuove informazioni utilizzando sensori che ricevono dati dall’ambiente circostante. Ad esempio, consideriamo un robot capace di misurare la direzione e la distanza da una posizione nota. Dal momento che la misura aggiunge nuove informazioni al sistema, esse possono essere usate per compensare la perdita delle informazioni dovute all’integrazione. Le nuove informazioni posso- no essere rappresentate mediante una PDF nello spazio dei sensori, solitamente, con un picco presso il valore della lettura sensoriale. Come abbiamo descritto sopra, la conoscenza della posa del robot prima della misura sensoriale `e descritta da una PDF nello spazio degli stati. Il problema cruciale della probabilistic estimation `e combinare
queste due distribuzioni in modo significativo.
Generalmente, il filtraggio bayesiano pu`o essere pensato come un processo a due fasi: predizione(prediction) e aggiornamento (update). Data una stima dello stato del sistema nella forma di PDF, la prediction propaga la PDF in accordo ai comandi del robot, insieme ad un modello di movimento per il robot. La fase di update corregge la prediction mescolando la PDF predetta con le informazioni fornite dai sensori. La nuo- va stima `e data dalla PDF aggiornata, e il processo continua ad iterare. Da notare che, in ogni iterazione, la fase di prediction tiene conto delle informazioni perdute dovute agli errori nel modello dianamico, mentre la fase di update incorpora le informazioni ricavate dai sensori.
La sezione successiva descriver`a il filtro di Kalman, che `e una specifica tecnica di probabilistic estimation. Nel filtro di Kalman si assume che il modello dinamico sia una funzione lineare delle variabili di stato e degli ingressi (comandi). Si assume che le quantit`a misurate dai sensori siano funzioni lineari delle variabili di stato. Sia nel modello dinamico, sia nel modello sensoriale gli errori sono rumori bianchi gaussiani a media nulla. Grazie a questa forma semplice, `e possibile derivare equazioni a forma chiusa per ottenere le fasi di prediction e update, facendo dell’implementazione del filtro di Kalman un processo efficace e diretto.
1.3 Filtro di Kalman
Per applicare il filtro di Kalman al problema della localizzazione di un robot, `e ne- cessario definire le equazioni che potranno essere usate dal modello dinamico e dal modello sensoriale del sistema del robot. Il vettore x `e utilizzato per denotare lo stato del sistema e la sua evoluzione attraverso il tempo. Si utilizza un modello a tempo discreto, nel quale la variazione continua dello stato del robot `e campionata a istanti discreti, intervalli di tempo regolari per creare la sequenza x(k), k ∈ 0, 1, 2, . . .. Nello specifico, se x(0) `e il valore dello stato al tempo t = t0, allora x(k) `e il valore dello stato al tempo t0 + T k, dove T `e l’intervallo di campionamento.
Si assume che l’evoluzione dello stato del robot e i valori misurati dai sensori del
robot possano essere modellati come un sistema dinamico lineare tempo-discreto:
( x(k + 1) = F (k)x(k) + G(k)u(k) + v(k)
y(k) = H(k)x(k) + w(k) (1.1)
Il vettore x(k) ∈ Rn delinea lo stato del sistema. Il vettore u(k) ∈ Rm rappre- senta gli ingressi del sistema, come i comandi di velocit`a, sterzo, o forza applicati intenzionalmente al robot. Il vettore y(k) ∈ Rp `e l’output del sistema e contiene i valori riportati dai sensori del sistema. La matrice F (k) ∈ Rn×n codifica la dinamica del sistema, e G(k) ∈ Rn×m descrive come gli input guidano la dinamica. Il vetto- re v(k) ∈ Rn `e chiamato process noise, rumore di processo, e si assume che sia un rumore bianco1 Gaussiano con media nulla e matrice di covarianza V (k). Il rumore di processo `e utilizzato per gestire i disturbi non modellati, ad esempio le ruote che slittano, che hanno effetto sul sistema dinamico. La matrice H(k) ∈ Rp×n descrive come i vettori di stato sono mappati in uscita. Il vettore w(k) ∈ Rp, measurement noi- se, rumore sulla misura, `e solitamente un rumore bianco Gaussiano con media nulla e matrice di covarianza W (k). Si assume inoltre che H(k) sia a rango massimo per tutti i k.
L’obiettivo del filtro di Kalman `e determinare la miglior stima dello stato x al tempo k data una stima precedente assieme all’ingresso u(k) e all’uscita osservata y(k). Ci sono due difficolt`a da superare per ricavare correttamente l’evoluzione del sistema. La prima `e la presenza di vettori di rumore sconosciuti e non misurabili v(k) e w(k). Un compito del filtro di Kalman `e filtrare questi disturbi non voluti. La seconda difficolt`a
`e che lo stato, in generale, non pu`o essere osservato dall’output poich`e H(k) potrebbe non essere invertibile. Questo significa che la stima dello stato deve essere ricostruita usando la storia dei tempi dei segnali conosciuti y(k) e u(k) assieme ai parametri conosciuti F (k), G(k), H(k), V (k), W (k). Un algoritmo che svolge questi compiti `e chiamato osservatore. Il filtro di Kalman `e, dunque, sia un osservatore sia un filtro.
1Il termine bianco significa che il vettore v(k) `e indipendente da v(k − 1) ∀k. Le propriet`a di una distribuzione Gaussiana, che `e interamente definita da un vettore di media e dalla matrice di covarianza, sono discusse successivamente.
Cenni sulle distribuzioni Gaussiane
Prima di proseguire oltre, si richiamano alcuni concetti sulle distribuzioni Gaussiane multivariate. Per x ∈ Rn, una distribuzione Gaussiana multivariata ha la PDF di forma
p(x) = 1
p(2π)n|P |e−12(x−¯x)TP−1(x−¯x)
dove ¯x `e un vettore in Rne P `e una matrice n×n, simmetrica, definita positiva. Risulta chiaro che p(x) `e interamente definita da ¯x e P . Inoltre, E[x] = ¯x `e il vettore di media e E[(x − ¯x)(x − ¯x)T] = P la matrice di covarianza. Il filtro di Kalman assume come stima dello stato all’istante t il valore a massima probabilit`a nello spazio degli stati:
questo valore corrisponde al valor medio della Gaussiana.
Si consideri il sistema lineare a tempo discreto descritto in (1.1). Le matrici sono come quelle descritte in (1.1). Prima di cominciare a costruire le equazioni necessarie, si deve introdurre qualche notazione. Dati due interi k1 e k2 con k1 ≥ k2, si user`a la notazione ˆx(k1|k2) per esprimere il valore di valutazione dello stato al tempo k1 dato il valore dell’uscita al tempo k2.
Si descriveranno lo step di predizione e lo step di aggiornamento. Si generer`a un vettore di stima dello stato ˆx(k|k) e una stima della matrice di covarianza P (k|k).
Di qui, il passo di predizione generer`a ˆx(k + 1|k) e P (k + 1|k), propagando la sti- ma corrente dello stato in accordo con le equazioni del sistema in (1.1). Il passo di aggiornamento generer`a la stima successiva ˆx(k + 1|k + 1) e P (k + 1|k + 1).
La predizione del vettore di stato ˆx(k + 1|k) si trova sostituendo ˆx(k|k) nell’e- quazione (1.1). Dal momento che il valore di v(k) `e zero, la predizione risultante sar`a:
ˆ
x(k + 1|k) = F (k)ˆx(k|k) + G(k)u(k) (1.2) Per calcolare la predizione della covarianza, si parte dalla definizione di matrice di covarianza:
P (k + 1|k) = E[(x(k + 1) − ˆx(k + 1|k))(x(k + 1) − ˆx(k + 1|k))T]
Sostituendo x(k + 1) dall’equazione (1.1) e ˆx(k + 1|k) dall’equazione (1.2), e molti-
plicando i termini nelle parentesi, otteniamo
P (k + 1|k) = E[F (k)(x(k) − ˆx(k|k))(x(k) − ˆx(k|k))TF (k)T +2F (k)(x(k) − ˆx(k|k))v(k)T + v(k)v(k)T] Il fatto che v(k) sia indipendente da x(k) e ˆx(k|k) implica che
E[(x(k) − ˆx(k|k))v(k)] = E[x(k) − ˆx(k|k)]E[v(k)]
che `e zero a causa della media nulla di v(k). Usando questo fatto insieme alla propriet`a di linearit`a del valore atteso, si ottiene
P (k + 1|k) = F (k)E[(x(k) − ˆx(k|k))(x(k) − ˆx(k|k))T]F (k)T +E[v(k)v(k)T]
Il primo termine nell’equazione uguaglia la definizione di matrice di covarianza P (k|k), mentre il secondo termine uguaglia la definizione di matrice di covarianza di V (k). Come risultato, si pu`o scrivere l’equazione di predizione per la matrice di covarianza,
P (k + 1|k) = F (k)P (k|k)F (k)T + V (k) (1.3) Per ottenere il passo di aggiornamento, non ci sono vincoli algebrici. Non si co- nosce esattamente quale possa essere l’uscita. Si sa solo che l’uscita ha forma di distribuzione Gaussiana in Rp con media y(k + 1) e matrice di covarianza W (k). Si procede cercando il termine di uscita pi`u probabile, y∗ dato dalla predizione (ˆx(k + 1|k), P (k + 1|k)) insieme all’uscita misurata y(k + 1). Una volta ottenuto il termine y∗, di introdurr`a il vincolo geometrico y∗ = H(k + 1)ˆx(k + 1|k + 1).
Dapprima, si rintraccia il termine y∗ proiettando la predizione nello spazio delle uscite. Usando la mappa delle uscite H(k + 1) e la definizione di covarianza, si vede che la distribuzione nello spazio degli stati con media ˆx(k+1|k) e matrice di covarianza P (k + 1|k) proietta verso una distribuzione Gaussiana nello spazio delle uscite (Rp) con media
ˆ
y(k + 1) = H(k + 1)ˆx(k + 1|k)
e matrice di covarianza
W = E[(ˆˆ y(k + 1) − y(k + 1))(ˆy(k + 1) − y(k + 1))T]
= E[H(k + 1)(ˆx(k + 1|k) − x(k + 1))(ˆx(k + 1) − x(k + 1))TH(k + 1)T]
= H(k + 1)P (k + 1|k)H(k + 1)T
L’uscita migliore y∗ `e definita come il punto migliore in tutto lo spazio delle uscite Rp, data la distribuzione Gaussiana che risulta proiettando la predizione e la distribuzione Gaussiana che risulta dalla misura. La proiezione proiettata e la distribuzione dell’u- scita hanno distribuzioni con coppie media-covarianza (ˆy, ˆW ) e (y(k + 1), W (k + 1)) rispettivamente. Dal momento che le distribuzioni sono indipendenti, y∗ sar`a il picco della funzione che risulta dal prodotto delle due distribuzioni. Fortunatamente, il pro- dotto di due Gaussiane `e ancora una Gaussiana e il risultato pu`o essere ottenuto usando una formula nota.
Teorema 1 Il prodotto di due distribuzioni Gaussiane con la coppia media-covarianza pari a (z1, C1) e (z2, C2) `e proporzionale a una terza distribuzione Gaussiana con media pari a
z3 = z1+ Q(z2− z1) e matrice di covarianza
C3 = C1 − QC1
dove
Q = C1(C1+ C2)−1
Applicando il teorema1, si ottiene
y∗ = ˆy + ˆW ( ˆW + W (k + 1))−1(ˆy − y(k + 1))
Si sceglie ˆx(k + 1|k + 1) in modo tale che sia il punto migliore in tutto l’insieme Ω∗, definito come
Ω∗ = {x ∈ Rn|H(k + 1)x = y∗}
Di qui, si cerca x ∈ Ω∗che massimizzi la distribuzione Gaussiana definita dalla coppia
(x(k + 1|k), P (k + 1|k), ad esempio
p(x) = 1
p(2π)n|P (k + 1|k)|e−12(x−ˆx(k+1|k))TP (k+1|k)−1(x−ˆx(k+1|k))
Poich`e l’esponenziale `e crescente in modo monotono, p(x) `e massimizzata quando (x − ˆx(k + 1|k))TP (k + 1|k)−1(x − ˆx(k + 1|k)) `e minimizzato. Dunque, si introduce una nuova notazione di distanza con la norma
k x k2M= xTP (k + 1|k)−1x che `e derivata dal prodotto interno su Rn,
< x1, x2 >M= xT1P (k + q|k)−1x2
Si noti che dM(x, ˆx) =k x − ˆx kM `e chiamata la distanza di Mahalanobis tra x e ˆx.
La distanza di Mahalanobis indica quanto lontano sia il punto x dalla media ˆx in unit`a di deviazione standard.
Si definisce ∆x = ˆx(k + 1|k + 1) − ˆx(k + 1|k). Si vuole trovare ˆx(k + 1|k + 1) tale che:
1. k ∆x kM sia minimizzata 2. (ˆx(k + 1|k) + ∆x) ∈ Ω∗
La prima condizione significa che il vettore ∆x deve essere ortogonale all’iperpiano Ω∗, rispettando il prodotto interno <· , · >M. Con questa nozione di distanza, scegliere ˆ
x(k + 1|k + 1) come il punto pi`u vicino a ˆx(k + 1|k) su Ω∗ `e equivalente a scegliere il punto presso il quale le ellissi equidistanti intersecano tangenzialmente Ω∗. La ∆x risultante deve essere ortogonale a Ω∗, ma la notazione di ortogonalit`a `e deviata da P (k + 1|k)−1. Ci`o significa che si deve avere
aP (k + 1|k)−1(∆x) = 0 ∀a ∈ ker(H(k + 1))
Si denoter`a H(k + 1) con H per brevit`a. Usando le ipotesi dell’algebra lineare pre- sentate precedentemente, questa espressione pu`o essere vera se ∆x ∈ column(P (k +
1|k)HT), che significa che
∆x = P (k + 1|k)HTγ
per qualche γ ∈ Rp. Ora, si cerca di dare un valore a γ. Si definisce l’errore di innovazioneν come la differenza tra l’output attuale y(k+1) e l’uscita predetta H ˆx(k+
1|k). In altre parole, ν `e la differenza tra il valore riportato dal sensore e il valore che si avrebbe se si considerasse corretta la predizione. Maggiore `e la discrepanza tra il valore attuale e il valore predetto dell’uscita, maggiore sar`a il valore di ∆x. Si far`a l’ipotesi che γ sia una funzione lineare di ν, γ = Kν per qualche K ∈ Rp×p. Allora, si ottiene
∆x = P (k + 1|k)HTKν (1.4)
Forzando (ˆx(k + 1|k) + ∆x) ∈ Ω∗, si ottiene,
H(ˆx(k + 1|k) + ∆x) = y∗ che implica che H∆x = ν. Sostituendo ∆x in1.4, accade che
HP (k + 1|k)HTKν = ν che significa che si deve avere
K = (HP (k + 1|k)HT)−1 Si ottiene, cos`ı,
∆x =P (k + 1|k)HT(HP (k + 1|k)HT)−1 (y∗− H ˆx(k + 1|k))
=P (k + 1|k)HT(HP (k + 1|k)HT + W (k))−1ν Dichiarando
R = P (k + 1|k)HT(HP (k + 1|k)HT + W (k + 1))−1
si pu`o scrivere il valore l’equazione di aggiornamento di stima dello stato come:
ˆ
x(k + 1|k + 1) = ˆx(k + 1|k) + Rν (1.5) Per rintracciare l’equazione di aggiornamento per la matrice di covarianza, si deve usare la definizione di matrice di covarianza insieme all’equazione di aggiornamento della stima dello stato. Si ottiene:
P (k + 1|k + 1) = P (k + 1|k) − RHP (k + 1|k) (1.6)
Sommario sul filtro di Kalman
Le equzioni del filtro di Kalman possono essere riassunte, nelle fasi di predizione e aggiornamento, come segue:
PREDIZIONE ˆ
x(k + 1|k) = F (k)ˆx(k|k) + G(k)u(k) (1.7) P (k + 1|k) = F (k)P (k|k)F (k)T + V (k) (1.8) AGGIORNAMENTO
ˆ
x(k + 1|k + 1) = ˆx(k + 1|k) + P ν (1.9)
P (k + 1|k + 1) = P (k + 1|k) − RH(k + 1)P (k + 1|k) (1.10)
dove
ν = y(k + 1) − H(k + 1)x(k + 1|k) (1.11)
S = H(k + 1)P (k + 1|k)H(k + 1)T + W (k + 1) (1.12)
R = P (k + 1|k)H(k + 1)TS−1 (1.13)
Queste equazioni forniscono la stima ottima di x nel senso che il valore atteso dell’errore tra x(k) e ˆx(k|k) `e minimizzato a ogni k. Si pu`o vedere R come il fattore
pesato che tiene conto della relazione tra l’accuratezza della stima predetta e il rumore di misura. Se R `e grande allora la lettura sensoriale `e pi`u credibile della predizione e il filtro di Kalman d`a molto peso alle letture sensoriali quando si calcola la stima aggiornata. Se R `e piccola, le letture sensoriali sono poco credibili, non hanno molta influenza nella fase di aggiornamento.
E importante notare che la stima e la covarianza stimate, che risultano dall’uso di` queste equazioni, sono non solo la stima migliore, ma anche la stima corretta. Se la stima all’istante k `e Gaussiana e descritta da (ˆx(k|k), P (k|k)), allora la distribuzione corretta all’istante k + 1, la distribuzione seguente, `e ancora Gaussiana ed `e descritta da (ˆx(k + 1|k + 1), P (k + 1|k + 1)).
Se i termini di rumore v(k) e w(k) avessero distribuzioni non Gaussiane, le equa- zioni proposte dal filtro di Kalman restano le migliori stime lineari, ma potrebbero esistere stimatori dello stato non lineari che danno risultati migliori.
1.3.1 Extended Kalman Filter, EKF
Il filtro di Kalman `e uno strumento molto potente per i sistemi lineari, ma molti sistemi che si incontrano nella pratica sono non lineari. Si consideri il sistema:
x(k + 1) = f (x(k), u(k), k) + v(k) y(k) = h(x(k), k) + w(k)
dove x(k) ∈ Rn, y(k) ∈ Rp, u(k) ∈ Rm, v(k) ∈ Rn, w(k) ∈ Rpe f : Rn×Rm×Z+ →
Rn e h : Rn× Z+ → Rp sono entrambe differenziabili in modo continuo in x(k).
L’unico approccio per stabilire la stima della posizione per un sistema di questo tipo
`e linearizzare le equazioni della stima corrente e applicare le equazioni del filtro di Kalman usando l’approssimazione ricavata. Questo genere di formulazione `e chiamata EKF, Filtro di Kalman Esteso.
Le equazioni dell’EKF sono:
PREDIZIONE ˆ
x(k + 1 | k) = f (ˆx(k | k), u(k), k) (1.14) P (k + 1 | k) = F (k)P (k | k)F (k)T + V (k) (1.15) dove
F (k) = ∂f
∂x
x=ˆx(k|k)
=
∂f1
∂x1
∂f1
∂x2 · · · ∂x∂f1
n
∂f2
∂x1
∂f2
∂x2 · · · ∂x∂f2 .. n
. ... . .. ...
∂fn
∂x1
∂fn
∂x2 · · · ∂x∂fn
n
x=ˆx(k|k)
(1.16)
AGGIORNAMENTO
ˆ
x(k + 1|k + 1) = ˆx(k + 1|k) + Rν (1.17)
P (k + 1|k + 1) = P (k + 1|k) − RH(k + 1)P (k + 1|k) (1.18) dove
ν = y(k + 1) − h(x(k + 1|k), k + 1) (1.19)
S = H(k + 1)P (k + 1|k)H(k + 1)T + W (k + 1) (1.20)
R = P (k + 1|k)H(k + 1)TS−1 (1.21)
e
H(k + 1) = ∂h
∂x
x=ˆx(k+1|k)
=
∂h1
∂x1
∂h1
∂x2 · · · ∂h∂x1
n
∂h2
∂x1
∂h2
∂x2 · · · ∂h∂x2 .. n
. ... . .. ...
∂hn
∂x1
∂hn
∂x2 · · · ∂h∂xn
n
x=ˆx(k+1|k)
(1.22)
1.4 Modelli sensoriali
I modelli di misura dell’ambiente, [26,6,4], descrivono il processo di formazione delle misure sensoriali dal mondo fisico. Oggi i robot usano una variet`a di sensori differenti, come sensori tattili, sensori di distanza, telecamere, o altri. La specificit`a del modello dipende dai sensori. I sensori di immagine sono meglio modellati da una geometria proiettiva, i sensori sonar sono meglio modellati descrivendo le onde sonore e la loro riflessione su superfici dell’ambiente.
La robotica probabilistica modella esplicitamente il rumore nelle misure senso- riali. I modelli tengono conto dell’incertezza nei sensori robotici. Formalmente, il modello di misura `e definito come una distribuzione di probabilit`a p(zt|xt, m), dove xt rappresenta la posa del robot, ztla misura al tempo t, m la mappa dell’ambiente.
Sebbene all’interno di questo progetto di tesi ci si indirizzer`a verso lo studio di sensori di distanza, come il laser scanner, i principi sottolineati non sono limitati a questo tipo di sensori. Il principio di base pu`o essere applicato a ogni tipo di sensori.
Pi`u accurato `e il modello sensoriale, migliori saranno i risultati. In pratica, `e spesso impossibile modellare accuratamente un sensore, a causa della complessit`a e della vastit`a dei fenomeni fisici.
Spesso, le caratteristiche di risposta di un sensore si basano su variabili che pre- feriamo non rendere esplicite nell’algoritmo probabilistico. Ad esempio, il materiale dei muri, che non `e comunemente modellato nel mapping. La robotica probabilistica aggiusta le inaccuratezze dei modelli sensoriali usando aspetti stocastici, modellando il processo di misura come una densit`a di probabilit`a condizionata p(zt|xt), anzich`e una funzione deterministica zt = f (xt). In questo modo, l’incertezza del model- lo sensoriale pu`o esprimere indirettamente gli aspetti non deterministici del modello.
Ecco, dunque, un vantaggio chiave della robotica probabilistica rispetto alla robotica classica: si possono considerare modelli molto pi`u inaccurati. Comunque, quando si concepisce un modello probabilistico, si devono comprendere le differenti tipologie di incertezza che genera una misura sensoriale.
Per esprimere il processo di generazione di misure, si deve specificare l’ambien- te nel quale una misura `e generata. Una mappa dell’ambiente `e una lista di ogget- ti e di posizioni dell’ambiente. Formalmente, una mappa m `e una lista di oggetti nell’ambiente:
m = m1, m2, · · · , mN (1.23) N rappresenta il numero totale di oggetti nell’ambiente, e ogni mn con 1 ≤ n ≤ N specifica una propriet`a. Si identificano due tipologie di mappe: feature-based e location-based. Nelle mappe feature-based, n rappresenta l’indice della feature. Il valore mncontiene, in accordo alla propriet`a della feature, la collocazione cartesiana della feature. Nelle mappe location-based, l’indice n corrisponde a una posizione specifica. Nelle mappe planari, `e comune denotare un elemento della mappa con mx,y al posto di mnper rendere esplicito che mx,y `e la propriet`a di una specifica coordinata, (x, y).
Entrambi i tipi di mappe hanno vantaggi e svantaggi. Le mappe location-based sono volumetriche, in quanto etichettano ogni posizione nel mondo. Le mappe volu- metriche contengono non solo informazioni sulla presenza di oggetti, ma anche sul- l’assenza. L’approccio `e abbastanza differente nelle mappe feature-based. Le mappe feature-based specificano la forma dell’ambiente in una specifica posizione. La rap- presentazione a feature rende pi`u facile aggiustare la posizione di un oggetto. Per questa ragione, le mappe feature-based sono popolari nel campo del mapping, dove le mappe sono costruite a partire dai dati sensoriali.
Il formalismo dei filtri di Kalman si adatta meglio a modelli feature based, che si presentano come pi`u adatti a essere parametrizzati. Una vasta letteratura attesta questa propensione [15,5,9].
Si introdurr`a nel seguito della sezione una versione a due dimensioni del Symme- tries and Perturbations Model (SPModel), [25, 3]. L’SPModel `e un metodo generale per la rappresentazione della posizione di ogni entit`a geometrica e della sua incertez- za. La posizione di un elemento `e rappresentata da un riferimento associato, mentre la sua incertezza, il vettore di perturbazione di un elemento geometrico, `e collegata a una variabile randomizzata Gaussiana a rumore bianco.
1.4.1 Modelli per considerare l’incertezza geometrica
La posizione di un elemento geometrico F rispetto a un riferimento A `e solitamente rappresentata da un vettore θAF, differente per ogni tipo di caratteristica geometrica.
Ad esempio, la posizione di un oggetto in 2D `e solitamente rappresentata da un vet- tore [x, y]T, dove x e y rappresentano le coordinate Cartesiane del punto rispetto al sistema di riferimento dell’ambiente. La posizione di una linea pu`o essere rappre- sentata da una coppia [ρ, θ]T, dove ρ rappresenta la normale della retta passante per l’origine del sistema di riferimento globale e θ l’orientamento rispetto all’asse X di riferimento. In modo alternativo, le linee del piano possono essere parametrizzate dai coefficienti [a, b]T dell’equazione della retta. La localizzazione di oggetti, caratteri- stiche geometriche complesse come gli angoli, le porte, o il robot stesso, richiedono anche l’orientamento, [x, y, θ]T.
Una discussione riguardo la rappresentazione di entit`a geometriche in due e tre dimensioni pu`o essere ritrovata in [3, 10]. Nei modelli probabilistici classici, l’infor- mazione circa la posizione dell’elemento era rappresentata da una funzione di distri- buzione di probabilit`a, assunta Gaussiana. La posizione stimata `e data dal valor atteso di θAF e l’imprecisione `e associata alla matrice di covarianza:
θˆAF = E[θAF] (1.24)
CAF = E[(θAF − ˆθAF)(θAF − ˆθAF)T] (1.25) Questo tipo di rappresentazione ha parecchie conseguenze:
• Parametri differenti sono usati per diversi componenti geometrici. Le leggi di trasformazione dei parametri tra il riferimento e le equazioni di stima del para- metro sono differenti in ogni situazione e spesso alquanto complesse. Ci`o rende difficile combinare diversi elementi geometrici in un’applicazione.
• Molte delle rappresentazioni proposte hanno singolarit`a per qualche valore dei parametri. Un esempio `e la rappresentazione di una linea in 2D usando la no- tazione θAF = [a, b]T, parametri dell’equazione lineare della retta y = ax + b.
Questa rappresentazione ha una singolarit`a per linee parallele all’asse Y . Questo forza ad avere una rappresentazione alternativa per quel tipo di linee, ad esempio, x = ay + b, introducendo di fatto una maggior complessit`a nella stima dell’e- quazione. In generale, vicino alle singolarit`a, la covarianza tende a infinito, la precisione del calcolo fatta decresce in modo drastico.
• Alcune rappresentazioni sono parametrizzate con un numero troppo elevato di parametri. Un chiaro esempio pu`o essere la rappresentazione di un muro in 3D, la cui minima rappresentazione consta di 4 parametri. Se si usa un punto e un vettore di direzione θAF = [x, y, z, ux, uy, uz]T, il punto pu`o essere preso arbi- trariamente lungo il margine, e il vettore ha modulo unitario. In pratica, questo tipo di informazione pu`o porre due problemi quando viene integrata: o il valore di qualche parametro diviene indeterminato, o il set di parametri deve verificare molti vincoli, spesso non lineari, dando una matrice di covarianza singolare.
• I valori di incertezza dipendono dalla posizione del sistema di riferimento base.
Ad esempio, se si rappresenta una linea in 2D con θAF = [ρ, θ]T, una piccola in- certezza nell’orientamento del muro fa crescere molto la varianza per la distanza perpendicolare all’origine ρ, se il muro `e abbastanza lontano dall’origine. La grandezza del valore nella matrice di covarianza non riflette l’importanza del- l’errore. Cos`ı la rappresentazione diviene poco intuitiva e diventa impossibile confrontare valori di incertezza.
Tutte queste problematiche conducono a definire un nuovo modello probabilistico che permetta di rappresentare la posizione di ogni tipo di caratteristica geometrica o un’osservazione sensoriale e la sua incertezza: l’SPModel.
1.4.2 SPModel
Questa sezione descrive l’SPModel, una rappresentazione di informazioni geometriche incerte che combina l’uso della teoria probabilistica per rappresentare l’imprecisione della posizione di un elemento geometrico, e la teoria della simmetria per rappresentare la parzialit`a a causa delle caratteristiche di ogni tipo di elemento geometrico.
Rappresentare feature geometriche
Una semplice osservazione sensoriale d`a informazioni sulla posizione nello spazio di elementi locali come vertici, piani, segmenti, linee. Tutti questi elementi saranno chiamati feature geometriche.
La determinazione di relazioni tra le rappresentazioni classiche di feature geome- triche `e complessa, come descritto nella sezione precedente. L’SPModel associa un
sistema di riferimento E a ogni elemento geometrico . Gli assi del sistema di riferi- mento sono allineati con l’elemento geometrico. Ad esempio, il sistema di riferimento relativo a un punto deve avere come origine il punto, il sistema di riferimento relativo a una linea deve avere il proprio asse-X lungo la linea.
La posizione di un elemento geometrico `e definita rispetto a un sistema di riferi- mento base W da una trasformazione tW E che `e rappresentata da un vettore di posi- zione xW E. Nel caso 3D, il vettore di posizione `e formato da tre coordinate cartesiane e tre angoli di roll-pitch-yaw.
xW E = [x, y, z, ψ, θ, φ]T (1.26)
tW E = T rans(x, y, z)Rot(Z, φ)Rot(Y, θ)Rot(X, ψ) (1.27) Nel caso a due dimensioni, il vettore di posizione si riduce a due coordinate carte- siane e a un angolo:
xW E = [x, y, φ]T (1.28)
tW E = T rans(x, y)Rot(Z, φ) (1.29)
La composizione di due trasformazioni, rappresentata dal loro vettore di posizione,
`e espressa da:
xW F = xW E ⊕ xEF (1.30)
Similmente, una trasformazione inversa si denota con:
xEW = xW E (1.31)
Per rappresentare tipi differenti di elementi geometrici, si usa il concetto di sim- metria. Le simmetrie di un elemento geometrico sono un set di trasformazioni che preservano l’elemento. Tutte le possibili simmetrie in 2D e 3D sono classificate. Nel
caso generale, le feature geometriche hanno simmetrie continue, che corrispondono a gradi di libert`a non determinati dalla posizione della feature, e simmetrie cicliche, che corrispondono a insiemi di differenti operazioni che fan corrispondere due feature. Ad esempio, le simmetrie di un segmento in 3D, espresse da un sistema di riferimento con l’asse X allineato con il segmento, sono i set di traslazioni lungo il segmento e il set di rotazioni intorno a RxTx, e le simmetrie invertite C2ycorrispondenti ai due possibili orientamenti del segmento.
Si rappresenta il set di simmetrie continue di un elemento geometrico usando la selezione di righe di una matrice di BE, denominata matrice di binding. La matrice di binding permette di esprimere il concetto geometrico fondamentale di coincidenza.
Due entit`a geometriche dello stesso tipo coincidono se la trasformazione tra i loro sistemi di riferimento A e B appartiene alla simmetria continua dell’elemento, che `e stabilito dall’equazione:
BAxAB = 0 (1.32)
dove BAdenota la matrice di binding di entrambe le entit`a geometriche.
Per esprimere la coincidenza tra tipi differenti di entit`a geometriche si usa il con- cetto di matrice di binding di un accoppiamento. Nel caso di due entit`a geometriche di diverso tipo, la cui posizione `e rappresentata rispettivamente da A e B, una delle seguenti equazioni esprime se le loro posizioni coincidono.
BABxAB = 0 (1.33)
BBAxBA = 0 (1.34)
dove AB e BA denotano la matrice di binding di un accoppiamento. L’uso di un vincolo diretto, rappresentato dalla prima equazione, o inverso, dalla seconda, dipende dal tipo di elemento geometrico considerato.
Rappresentare l’incertezza
Nei modelli probabilistici classici, l’imprecisione del sensore `e rappresentata da una variabile aleatoria Gaussiana a rumore bianco, caratterizzata da una media e da una
matrice di covarianza. Sfruttando la stessa idea, si usa una rappresentazione locale dell’incertezza.
Nel SPModel, la posizione stimata di un elemento geometrico F rispetto al sistema di riferimento base W si denota con ˆxW F, rappresentando la miglior approssimazione della posizione reale di un elemento geometrico ottenuta dal sensore. In aggiunta, la stima dell’errore `e rappresentata localmente da un vettore di posizione differenziale dF relativo al sistema di riferimento associato all’elemento F . Pertanto, la posizione vera dell’elemento `e data dalla composizione:
xW F = ˆxW F ⊕ dF (1.35)
dove dF `e espresso come
dF = [dxF, dyF, dφF]T (1.36) Si assegna un valore nullo ai componenti di dF allineati con le simmetrie dell’ele- mento perch`e non rappresenta un errore effettivo nella posizione dell’elemento. I gra- di di libert`a di un elemento geometrico F sono rimossi definendo un vettore random chiamato vettore di perturbazione pF formato dagli elementi non nulli di dF
dF = BFTpF (1.37)
pF = BFdF (1.38)
dove BF `e la matrice di binding dell’elemento geometrico.
Concludendo, l’SPModel rappresenta informazioni sulla posizione di un elemento geometrico F con una quadrupla:
LW F = [ˆxW F, ˆpF, CF, BF]T (1.39) chiamata posizione probabile dell’elemento geometrico F , dove:
xW F = ˆxW F ⊕ BFTpF (1.40) ˆ
pF = E[pF] (1.41)
CF = E[(pF − ˆpF)((pF − ˆpF)T] (1.42) La trasformazione ˆxW F `e una stima presa come base per le perturbazioni, ˆpF `e una stima del vettore di perturbazione, e CF `e la sua covarianza. Quando ˆpF = 0 si dice che la stima `e centrata.
Il grande vantaggio di questo modello `e la sua generalit`a: `e valida per ogni feature geometrica e osservazione sensoriale. Da notare che la rappresentazione dell’incertez- za usando un vettore di perturbazione non dipende dal sistema di riferimento usato, ha una chiara interpretazione, e non ha troppi parametri. In pi`u, i problemi dovuti alle singolarit`a sono completamente eliminati.
1.4.3 Modelli a feature rettilinee
In questo lavoro di tesi si analizzano piattaforme robotiche dotate di sensori range, come, ad esempio, un laser scanner. Nel caso considerato `e difficile utilizzare feature puntiformi a causa dell’elevato numero dei punti estratti dalla scansione e della confor- mazione dell’ambiente, piuttosto disordinato. Di conseguenza, risulta maggiormente efficiente e utile l’utilizzo di feature rettilinee.
Una feature rettilinea pu`o essere parametrizzata in vari modi. Per esempio, data l’e- quazione cartesiana della retta y = m × x + q la linea pu`o essere descritta dalla coppia [m, q]. Questa rappresentazione ha singolarit`a per feature parallele all’asse-X. Come descritto in [18], una retta `e una semplice feature modellata come z = [α, ρ]T, dove α e ρ rappresentano l’orientamento e la distanza del vettore che si estende dall’origine alla linea, perpendicolare alla linea, come in Figura 1.2. α definisce l’orientamento della linea z, mentre ρ definisce la distanza normale al sistema di riferimento base.
Il modello parametrico che descrive la retta `e:
x cos α + y sin α − ρ = 0 (1.43)
Nel modello considerato, che `e chiamato modello Hessiano della retta, z = [α, ρ]T, ρ `e
Figura 1.2: Rappresentazione Hessiana della retta.
sempre positiva. Esiste, comunque, una rappresentazione alternativa z = [α +π, −ρ]T, che differisce dal precedente solo per il segno di ρ. Ci`o discende dal fatto che una linea pu`o essere rintracciata da due parti.
Solitamente, l’insieme intero di feature che descrivono l’ambiente `e salvato in una rappresentazione della mappa relativa al sistema di riferimento base. Nei pro- cessi di localizzazione, a causa del matching delle feature, ciascuna linea deve essere trasformata rispetto al sistema di riferimento base.
Data una retta zW = [αW, ρW]T rispetto al sistema di riferimento base (W `e il frame di riferimento), un robot nella posa xW = [x, y, θ]T con sistema di riferimento R e un sensore con un sistema di coordinate S, volendo far coincidere il sistema di riferimento del sensore con quello del robot S = R, le equazioni di trasformazione zR= [αR, ρR]T = h(zW, xW) sono
zR =
"
αW − θ
ρW − x cos αW − y sin αW
#
(1.44)
1.4.4 Modelli a segmenti
In letteratura, a differenza delle feature rettilinee, non esiste una formulazione generale per la parametrizzazione di un segmento. `E un problema tuttora non completamente
codificato. Esistono, tuttavia, diverse euristiche adottabili per la rappresentazione di questo tipo di feature.
Spesso, la feature segmento viene rappresentata come una feature rettilinea dotata di due estremi, line segment feature, [13]. Anche in questo lavori di tesi, si rappre- senter`a la feature segmento tramite i suoi due estremi e la feature rettilinea che li congiunge.
1.5 Associazione di dati
La soluzione presentata nella sezione 1.3 glissa su un aspetto veramente importan- te della localizzazione: assume, infatti, che ogni osservazione sensoriale sia auto- maticamente associata a un’adeguata posizione nell’ambiente circostante, cio`e nella mappa.
Nella pratica, accade spesso che le feature abbiano propriet`a geometriche similari e siano spesso difficili da distinguere l’una dall’altra. Quando questo accade, ci si deve indirizzare al problema dell’associazione di dati, data association, che riguarda l’associazione dell’osservazione attuale con le feature della mappa dell’ambiente.
L’idea alla base del data association `e la seguente. Si consideri la i-esima os- servazione sensoriale yi(k + 1). Per ogni feature nella mappa, si calcola νi,j. νi,j `e la differenza tra l’osservazione attuale, yi(k + 1), e l’osservazione che ci si sarebbe aspettati se yi(k + 1) fosse corrisposta alla j-esima feature e la predizione ˆx(k + 1|k) fosse stata corretta. Questo significa che
νi,j = yi(k + 1) − hi(ˆx(k + 1|k), j)
Pi`u `e piccolo νi,j, pi`u `e buona la corrispondenza tra l’i-esimo dato e la j-esima feature.
La nozione della dimensione deve essere pesata dall’incertezza della previsione e del dato sensoriale. Fortunatamente, questa incertezza `e codificata nella matrice S del filtro di Kalman, nell’equazione di aggiornamento. S pu`o essere usata per creare la norma di Mahalanobis per νi,j che indica la dimensione dell’innovazione in unit`a di deviazione standard. La misura dell’innovazione sar`a
χ2i,j = νi,jSi,j−1νi,jT
dove
Si,j = Hi(k + 1, j)P (k + 1|k)Hi(k + 1, j)T + Wi(k + 1)
Si pu`o costruire la funzione di data association a(), fissando a(i) uguale al valore di j che minimizza χi,j.
Realizzazione del sistema
Il sistema di localizzazione realizzato in questa tesi utilizza come componenti hard- ware principali il robot mobile Nomad200 e il sensore di range laser scanner 2D Sick S3000. Le funzionalit`a software sviluppate sono state integrate all’interno della li- breria denominata spf. Tali componenti hardware e software sono descritte in questo capitolo.
2.1 Nomad 200
Il robot Nomad200, Figura2.1, `e una
Figura 2.1: Il Nomad200.
piattaforma mobile sviluppata da Noma- dic Technologies Inc. nei primi anni ’90.
Ora, il Nomad200 non `e pi`u in produ- zione. Si vuole, in queste poche pagi- ne, riassumere l’hardware installato sul- la piattaforma. Nomad200 `e una piatta- forma robusta e potente, sebbene sia sta- to progettato tempo fa. Il PC installa- to, ormai datato, riduce la possibilit`a di applicazioni complesse nel mondo reale.
Ad ora, il Nomad200 `e equipaggiato con:
• processore 1GHz Pentium III
32
• 256Mb RAM
• 40Gb HDD
• Motherboard ATX con VGA, IDE, USB integrati
• Adattatore 802.11b WiFi USB (11Mbps)
Il sistema lancia l’originale server di controllo Nomadic robotd grazie a un’appli- cazione Linux, adattata alle esigenze del robot.
(a) Struttura interna. (b) Motherboard.
Figura 2.2: Il Nomad200 in dotazione al RimLab.
La Figura 2.2 mostra una vista corrente del robot. La nuova motherboard `e po- sizionata all’incirca nella medesima locazione dell’originale, dietro i cavi flat. Molte delle vecchie ISA cards non sono pi`u richieste, soppiantate dall’abbondanza di porte di I/O della scheda installata.
La motherboard `e equipaggiata con una CPU SY-7VEM, Socket 370, processore Pentium III Coppermine 1Ghz e una memoria RAM PC133 256Mb.
