Approcci basati su scan matching

L'idea alla base di questo approccio consiste nel determinare il vettore spostamento tra due scansioni acquisite in posizioni diverse. Queste vengono prima riferite entrambe a un sistema di riferimento assoluto, confrontate e rototraslate una rispetto all'altra, in modo che le due scansioni si sovrappongano in maniera ottimale. Il vettore di spostamento così determinato, viene utilizzato quindi per la stima dello spostamento effettuato dal robot: in questo modo si mantiene piccolo l'errore odometrico e si rende più agevole il processo di costruzione della mappa.

Lu e Milios in [37] hanno proposto un algoritmo di scan matching che lavora su dati ottenuti da scansioni laser, chiamato IDC (Iterative Dual Correspondence).

Date due scansioni, $S_{new}$ e $S_{\mbox{{\em\scriptsize ref}}}$, l'algoritmo genera un allineamento approssimativo utilizzando i dati odometrici e quindi raffina l'allineamento cercando di minimizzare la distanza tra le due scansioni, definita dalla seguente funzione:

$$E_{dist}(\omega,T) = \sum_{i=1}^{n} \vert R_{\omega} P_i + T - P'_i\vert^2$$ (2.3)

dove $n$ è il numero di punti corrispondenti nelle due scansioni, $P_i$ è un punto appartenente alla prima scansione e $P'_i$ è il suo corrispondente nella seconda scansione. Infine $\omega$ e $T$ rappresentano rispettivamente la rotazione e la traslazione relative tra le due scansioni.

Per ogni punto $P_{i}$ di $S_{new}$ viene determinata, una corrispondenza con un punto $P'_{i}$ di $S_{\mbox{{\em\scriptsize ref}}}$. Tutte le corrispondenze così generate vengono utilizzate per trovare la rototraslazione relativa risolvendo un sistema con il metodo dei minimi quadrati atto a minimizzare la distanza, definita in (2.3). Il processo viene iterato più volte fino a quando non converge.

La ricerca delle corrispondenze avviene per mezzo di due regole, una che considera le relazioni di distanza e una che tiene in considerazione la rotazione relativa tra le due scansioni. La prima regola è la closet-point rule (CPR), che genera un'associazione tra i punti appartenenti alle due scansioni basandosi sulla relazione di vicinanza tra due punti. Ad ogni punto di $S_{new}$ viene associato il punto più vicino di $S_{\mbox{{\em\scriptsize ref}}}$ e viene generata la coppia. Generate tutte le coppie possibili la somma delle distanze tra i punti associati diviene la distanza da minimizzare attraverso il metodo dei minimi quadrati. Questa regola mette in evidenza le componenti relative alla traslazione delle due mappe. La seconda regola, detta matching-range-point rule (MRPR) permette di generare corrispondenze tra le due scansioni, secondo un criterio che tiene conto della rotazione relativa.

La determinazione delle coppie viene fatta in base al seguente criterio: dato un punto $P$ della scansione $S_{new}$, il suo corrispondente punto nello scan $S_{\mbox{{\em\scriptsize ref}}}$ sarà dato dalla relazione $P' = R_{\omega} P + T$ dove $R_{\omega}$ è la matrice di rotazione e $T$ il vettore di traslazione. Se si ritiene trascurabile la traslazione si ottiene che $\left\vert {P'} \right\vert \approx \left\vert P \right\vert$, poiché le due scansioni differiscono per la sola componente rotazionale.

Allo stesso modo si può assumere che per l'angolo polare $\theta$ di $P$ e $\theta'$ di $P'$ valga la seguente relazione $\theta' \approx \theta + \omega$. La ricerca della corrispondenza avviene quindi cercando il punto $P' \in S_{\mbox{{\em\scriptsize ref}}}$ il cui angolo polare $\theta'$ stia in un intervallo $\left\vert {\theta - \theta '} \right\vert \le B_\omega$ e il modulo di $P'$ sia il più vicino possibile a quello di $P$, dove $B_\omega$ indica un bound per la rotazione.

Applicando entrambe le regole si otterranno informazioni precise sia per la traslazione, che per la rotazione relative. A ogni iterazione vengono eseguiti i seguenti passi:

1. per ogni punto $P_{i} \in S_{new}$ viene determinato il corrispondente punto $P_{i}' \in S_{\mbox{{\em\scriptsize ref}}}$ con la closest-point rule e il corrispondente punto $P'' \in S_{\mbox{{\em\scriptsize ref}}}$ con la matching-range-point rule.

2. Viene calcolata la soluzione $\left( \omega_{1}, T_{1} \right)$ con il metodo dei minimi quadrati utilizzando l'insieme delle corrispondenze $\left( P_{i} , P_{i}' \right)$, ottenute con la closest-point rule.

3. Viene calcolata la soluzione $\left( \omega_{2}, T_{2} \right)$ con il metodo dei minimi quadrati utilizzando l'insieme delle corrispondenze $\left( P_{i} , P_{i}'' \right)$, ottenute con la matching-range-point rule.

4. Si utilizza la coppia $\left( \omega_{2}, T_{1} \right)$ come trasformazione relativa tra le due mappe all'iterazione successiva.

L'uso di una mappa a punti rende l'algoritmo proposto da Lu e Milios applicabile sia in ambienti poligonali, che in ambienti irregolari. Ciò che penalizza questo metodo è il processo di individuazione delle corrispondenze che influisce sulle prestazioni dell'algoritmo: il metodo appena discusso è caratterizzato da una complessità pari a $O \left( N^{2} \right)$, dove con $N$ si è indicato il numero di punti nella mappa. Esso è tuttavia efficace se il numero di punti costituenti la mappa non è eccessivamente elevato. Nella presente tesi questo metodo è stato implementato con successo e affiancato ad una procedura di clustering atta ad aumentarne le prestazioni.

Il metodo sviluppato in [63] e successivamente implementato in [62] propone invece un approccio statistico al problema del mapping. Il robot è dotato di uno scanner rotante a infrarossi, in grado restituire una mappa a punti dell'ambiente mediante una scansione di 360 gradi. A ogni movimento del robot la scansione dell'ambiente viene confrontata con la scansione ottenuta al passo precedente per determinare lo spostamento compiuto dal robot. Lo spostamento viene descritto tramite un vettore, che indica la traslazione lungo l'asse $X$ e l'asse $Y$ e un angolo $\theta$, che rappresenta la rotazione. Il metodo si basa sulla costruzione, per ognuna delle due mappe, di istogrammi statistici per ciascuna delle tre grandezze da determinare, l'angolo $\theta$ e gli spostamenti $x$ e $y$.

Il metodo sfrutta le proprietà geometriche invarianti rispetto alle rotazioni e alle traslazioni; in particolare, la distribuzione dell'angolo formato dal vettore congiungente due punti consecutivi della mappa viene utilizzata per determinare la rotazione, mentre la distribuzione della direzione dei punti lungo i due assi per il calcolo della traslazione. Nel caso della rotazione, ad esempio, per ogni mappa viene costruito il relativo istogramma che descrive, su un intervallo da $-90$ gradi a $+90$ gradi, la distribuzione degli angoli formati dal vettore che unisce due punti consecutivi della mappa. La distribuzione di questi angoli è una proprietà invariante rispetto alla rotazione: considerate due mappe identiche rototraslate tra loro, i grafici dei due istogrammi avranno lo stesso andamento, sfasato però esattamente dell'angolo $\theta$ della rotazione.

Alle due distribuzioni così ottenute viene applicata la funzione di cross-correlazione nella forma discreta, in base alla risoluzione con cui si è costruito l'istogramma. La funzione di cross-correlazione in forma discreta ha la seguente forma:

$$k\left( j \right) = \sum\limits_{i = 1}^n {f\left( i \right)g\left( {i + j} \right)}$$ (2.4)

dove $n$ è il numero di classi utilizzate per la costruzione dell'istogramma. La funzione di cross-correlazione permette di misurare la correlazione tra le due funzioni stocastiche discrete $f\left( x \right)$ e $g\left( x \right)$; il valore di tale funzione presenta un massimo in $s$ se $f\left( x \right) = g\left( x+s \right)$.

Il valore angolare associato al massimo della funzione di correlazione corrisponde all'angolo di rotazione cercato. Dal momento che la funzione potrebbe essere caratterizzata da più massimi relativi caratterizzati da valori simili, è necessario ricorrere all'informazione odometrica per disambiguare la scelta dell'angolo di rotazione. Lo stesso tipo di procedura, una volta ruotata una mappa rispetto all'altra dell'angolo $\theta$ trovato, viene applicata alle direzioni $X$ e $Y$ per determinare l'entità delle traslazioni, eventualmente ricorrendo all'odometria per filtrare i valori ottenuti dalla funzione di cross-correlazione.

Il metodo proposto da Cox [15] utilizza il robot Blanche per la navigazione autonoma all'interno di ambienti strutturati come uffici o in contesti industriali. Il sistema di localizzazione del robot si basa su tre assunzioni fondamentali. In primo luogo viene fornita una mappa a priori dell'ambiente, rappresentata come una collezione di segmenti in uno spazio bidimensionale. In secondo luogo, un algoritmo iterativo confronta la mappa a punti fornita di volta in volta dal sensore a bordo del robot, un telemetro a infrarosso, con il modello a priori, trovando la trasformazione che permette la migliore sovrapposizione tra i due modelli ambientali. Infine, la trasformazione trovata viene utilizzata per determinare la posa del robot integrandola con l'informazione proveniente dell'odometria.

Gutmann e Schlegel in [28] hanno ripreso e integrato l'algoritmo IDC con l'algoritmo proposto da Cox [15]. Utilizzando uno scanner laser hanno testato i due algoritmi e successivamente hanno introdotto per ognuno di essi alcune varianti al fine di costruire un sistema di scan matching che combini in maniera ottimale i vantaggi offerti dai due algoritmi, sia in termini di prestazioni sia in termini di tipologia di ambiente.

Per velocizzare i tempi di elaborazione, un filtro viene applicato alle due mappe prima dell'esecuzione dell'algoritmo IDC: il filtro sostituisce gruppi di punti contigui e molto vicini con un unico punto che rappresenta il centro di gravità dell'insieme di punti. Questo ha il vantaggio di ridurre la distribuzione dei punti nella mappa rendendola più uniforme senza perdita di eccessiva informazione.

Per quanto riguarda l'algoritmo di Cox, è stata introdotta una modifica per evitare la necessità di un modello a priori dell'ambiente: invece di un modello predefinito, si utilizza come riferimento per lo scan matching una delle mappe acquisite in precedenza. Questa mappa (a punti) viene passata attraverso un filtro per l'estrazione dei segmenti che andranno a comporre la mappa da usare come riferimento. La mappa corrente viene filtrata attraverso un filtro per l'estrazione di segmenti in modo da mantenere solamente i punti che possono essere approssimati da un segmento.

Sulla base di queste modifiche, i due algoritmi sono stati combinati assieme per ottenere un nuovo algoritmo per lo scan matching, chiamato CSM (Combined Scan Matcher). L'idea è quella di utilizzare uno dei due algoritmi modificati in base alla tipologia di ambiente in cui si viene a trovare il robot. In particolare in ambienti non poligonali viene utilizzato il metodo IDC mentre in ambienti poligonali, caratterizzati da pareti lunghe ben descrivibili da linee e segmenti, viene utilizzato il metodo di Cox. Il criterio di scelta tra i due metodi è determinato dal numero di punti che possono essere approssimati con dei segmenti.

Prima di processare le mappe, viene applicato un filtro di proiezione a entrambe le mappe per eliminare da ciascuna di esse i punti non visibili dal punto in cui è stata presa l'altra mappa, nella stessa maniera già sperimentata da Lu e Milios in [37]. In questo modo si ha una riduzione ulteriore del numero di punti da considerare nelle associazioni punto-punto dell'IDC o nelle associazioni punto-segmento dell'algoritmo di Cox. Per l'applicazione del filtro è necessaria l'informazione odometrica.

L'algoritmo CSM viene utilizzato soprattutto per la localizzazione del robot nell'ambiente. Nelle sperimentazioni effettuate dai due autori attraverso l'implementazione del sistema robotico AMOS (Autonomous MObile System), non viene costruita alcuna mappa globale dell'ambiente: al contrario, è necessaria una prima fase di esplorazione dell'ambiente in cui il robot naviga nell'ambiente, eventualmente tramite comandi manuali, per acquisire le mappe. Le mappe così ottenute vengono in un secondo momento processate per ottenere una mappa globale e, tramite l'algoritmo IDC, una matrice contenente, per ogni coppia di mappe, la stima della relativa posizione. L'insieme di mappe e di posizioni ottenute vengono poi trasferite nel robot come mappe di riferimento. A ogni nuova acquisizione, in base alle stime fornite dall'apparato odometrico viene scelta una mappa come riferimento per lo scan matching. Il metodo è poi stato ripreso in studi successivi in [27] e in [46]: in questi lavori viene eliminata la fase di esplorazione iniziale e vengono proposti dei metodi incrementali per la costruzione di una mappa globale contenente sia informazioni geometriche sia informazioni di carattere topologico. Viene mantenuto lo schema dell'algoritmo CSM per la stima della posizione e viene impiegato anche un filtro di Kalman per combinare la stima prodotta da CSM con l'informazione odometrica.

Zhang e Ghosh in [65] propongono un metodo incrementale attraverso cui procedere alla localizzazione del robot e alla successiva fase di costruzione della mappa globale: ad ogni rilevazione sensoriale, i nuovi dati, opportunamente filtrati e rielaborati, vengono confrontati con la mappa globale costruita fino a quel punto, individuando la correzione da apportare alle informazioni rilevate dal sistema odometrico. Una volta determinata la posizione del robot viene costruita la nuova mappa globale fondendo insieme le informazioni della mappa globale precedente e le nuove informazioni raccolte.

L'algoritmo per lo scan matching proposto dai due autori si basa sulla corrispondenza tra i segmenti delle due mappe. I segmenti della mappa locale $M_{L}$ appena acquisita vengono trasformati nel sistema di coordinate della mappa globale $M_{G}$, basandosi sulla trasformazione $t$ ottenuta attraverso il sistema odometrico. Quindi la corrispondenza tra i segmenti di $M_{L}^{t}$ e $M_{G}$ viene effettuata in base a due criteri: le direzioni di due segmenti corrispondenti, $\theta_{L}$ e $\theta_{G}$, devono essere simili e i due centri di gravità devono essere vicini. Considerando un insieme di $n$ corrispondenze trovate, la correzione per l'angolo di rotazione viene calcolata come:

$$\theta = \sum_{i = 1}^n {\left( {\theta _G^i - \theta _L^i } \right)/\left( {\sigma _{\theta G}^i - \sigma _{\theta L}^i } \right)}$$ (2.5)

Il calcolo del vettore di traslazione $\left[ t_{x}, t_{y} \right]$ viene effettuato per ogni singola corrispondenza in modo che la seguente equazione risulti verificata:

$$\left( {\left[ {\begin{array}{*{20}c} {a_G } & {b_G } \\ \end{arr... ...ight]} \right) + c_G } \right)\left( {\sigma _{cG} + \sigma _{cL} } \right) = 0$$ (2.6)

dove $\left( a_{G}, b_{G}, c_{G} \right)$ sono i coefficienti della retta utilizzando l'usuale descrizione $ax + by + c = 0$ e $R_\theta$ è la matrice di rotazione. Le varianze dei centri di gravità vengono utilizzate come pesi. Il vettore di traslazione $\left[ t_{x}, t_{y} \right]$ viene quindi calcolato con il metodo dei minimi quadrati per tutte le corrispondenze tra segmenti trovate.

Una volta localizzato il robot, i segmenti della mappa locale vengono aggiunti alla mappa globale o fusi con quelli già presenti. L'operazione di fusione prevede la sostituzione dei segmenti che si sovrappongono o si intersecano con un nuovo segmento, ottenuto a partire dalle caratteristiche dei segmenti considerati. In particolare, la direzione $\theta$ e il nuovo centro di gravità vengono determinati come media pesata degli stessi parametri che descrivono i segmenti coinvolti nella fusione (e lo stesso accade per le rispettive varianze). Gli estremi del nuovo segmento, infine, vengono determinati proiettando tutti gli estremi dei segmenti sulla retta di supporto del nuovo segmento e scegliendo quelli a distanza maggiore.

Si osservi che il metodo esaminato non può prescindere dalla conoscenza dell'informazione odometrica come prima stima della posizione della nuova mappa acquisita.