Centralized and distributed state estimation for robot localization

The localization of a robot in a 2D space amounts to the estimation of its state variables, namely its x and y coordinates and its orientation. A robot localization via range-only-measurements system is therefore a suitable nonlinear framework to test different estimators. The extended Kalman filter is the most widely used estimator for nonlinear systems, which has shown its effectiveness in several applications. For many physical systems some additional prior knowledge may be available, in the form of either state or noise constraints, not explicitly taken into account by the model. Typical constraints in robot localization are spatial limitations to the robot trajectory. However, the extended Kalman filter is not able to incorporate constraints in estimation. On the contrary, moving horizon estimation is an optimization-based strategy which not only can deal with nonlinearities, but it can account for constraints, as well. Simulation results show that the inclusion of constraints in robot localization with moving horizon approach leads to tangible improvements in estimates accuracy. In a dynamic system the measurements are usually accomplished by several sensors. In the classic centralized approach to state estimation all the observations are gathered in a single central computing unit, in which they are processed by a proper state estimator. If such a central unit failed, the estimates would be no longer achievable. The distributed approach overcomes this drawback by distributing computations among the sensing units. In this case the sensors are not just simple sensing devices, rather they have computational and communication capabilities so that they can compute estimates and communicate to each other. Each sensor performs its own estimate of the entire system state. The aim of a distributed estimator is to make all the sensors obtain, at least asymptotically, a common state estimate, which should be as close as possible to the one attained by centralized estimation. This means that a sensor needs to correct its individual estimate by comparison with the estimates of the other sensors. As a result, the main challenge is to find a methodology which guarantees that all the sensors asymptotically reach a common reliable estimate i.e., their local estimates reach a consensus. Hence, every distributed estimator embodies a consensus algorithm in order to merge the local estimates of the sensors. Distributed versions of both the extended Kalman filter and the moving horizon estimator are employed with satisfactory results in a robot localization problem.

La conoscenza dello stato è essenziale nello studio di sistemi dinamici di vario genere, poiché incorpora le informazioni passate necessarie per la comprensione del comportamento futuro del sistema. Tuttavia, lo stato di un sistema è raramente accessibile in maniera diretta dalle misure disponibili. Si rende perciò necessario trovare un modo per dedurlo dalle informazioni a disposizione: le misure, in genere affette da rumore, e un modello che descrive il comportamento del sistema. Questa inferenza dello stato va sotto il nome di stima dello stato ed il procedimento, espresso tramite un algoritmo, che ne esplica lo svolgimento viene detto stimatore. Il contributo più significativo alla teoria della stima è stato dato dal lavoro di Kalman, nel 1960, con l'introduzione di uno stimatore con caratteristiche di ottimalità, il filtro di Kalman (KF), che è diventato la scelta più comune nella stima dello stato di sistemi lineari non soggetti a vincoli e con disturbi gaussiani. Uno dei motivi della popolarità di questo stimatore è che possiede numerose importanti proprietà, quale ad esempio la stabilità. Ciononostante, il filtro di Kalman ha un grosso limite dovuto alla sua applicablità ai soli sistemi lineari, quando nella realtà la maggioranza dei sistemi fisici reali presenta dinamiche ed equazioni di misura non lineari. A tali sistemi non è possibile applicare il filtro di Kalman nella sua formulazione originale. Il filtro di Kalman esteso (EKF) nasce come sviluppo del filtro di Kalman classico, proprio con l'intento di affrontare il problema della stima dello stato di sistemi con dinamiche ed equazioni di misura non lineari. Del KF mantiene l'impostazione di base, composta dai due passi di predizione ed aggiornamento della stima e della relativa matrice di covarianza. La stima dello stato è propagata da un istante al successivo mediante le funzioni non lineari che descrivono le equazioni di stato e misura, mentre la matrice di covarianza viene propagata attraverso modelli lineari, ottenuti dalla linearizzazione a ogni istante del sistema non lineare di partenza. Questo stimatore ha conosciuto e conosce tuttora una vasta diffusione in campo industriale, grazie alla relativa semplicità e all'efficacia nell'operare in presenza di non linearità, nonostante non esista una trattazione esaustiva sulle sue proprietà di stabilità e convergenza. Recentemente l'attenzione della ricerca si sta sempre più spostando su metodi di stima che possiedono un'importante caratteristica che i filtri di Kalman, incluso l'EKF, non hanno: la capacità di considerare la presenza di vincoli nel processo di stima. Molti sistemi fisici infatti, presentano variabili di stato o variabili disturbo che sono soggette a vincoli. Tali vincoli rappresentano dell'informazione aggiuntiva, nota a priori, che non è esplicitamente inclusa nel modello che descrive il sistema. Ad esempio, in applicazioni industriali, alcune grandezze come temperature, pressioni o concentrazioni sono positive; la presenza di saturatori in un processo forza alcune variabili ad assumere valori solo all'interno di un intervallo definito. Questi sono casi in cui la conoscenza del sistema dovuta al modello e alle misure viene ampliata da informazioni in più che né il modello né le misure riuscirebbero a rendere da sé. Dal punto di vista matematico questa conoscenza si traduce in vincoli, tipicamente relazioni di disuguaglianza, cui possono essere sottoposte le variabili del sistema. E' quindi legittimo pensare che la qualità della stima dello stato possa essere migliorata inglobando nel processo di stima queste conoscenze in più a disposizione. In effetti ciò corrisponde al vero, anche se non tutti i metodi di stima sono in grado di farlo. Tra quelli che non hanno questa caratteristica ci sono proprio il KF e tutte le sue varianti ed estensioni. Ovviamente, questo non è ostativo all'impiego del KF o dell'EKF per la stima dello stato di sistemi soggetti a vincoli; semplicemente significa che le loro stime non trarranno alcun giovamento dalla presenza di tali vincoli. Questa è la ragione che ha spinto i ricercatori a volgere l'attenzione verso altri metodi, in particolare i metodi ad ottimizzazione, mediante i quali il problema di stima viene formulato come un opportuno problema di ottimizzazione nel tempo (minimizzazione di un funzionale di costo), che consente appunto l'inclusione di eventuali vincoli sulle variabili di stato e/o sui disturbi. In particolare, lo stimatore ad orizzonte mobile (MHE) è un metodo appartenente a questa classe di stimatori, il quale può essere applicato a sistemi sia lineari sia non lineari, e che perciò risulta essere molto versatile. L'impiego di metodi di ottimizzazione, nel loro approccio più generale, determina un aumento del carico computazionale richiesto per lo svolgimento della stima. Questo perché l'ottimizzazione ad un certo istante temporale avviene considerando tutte le misure del sistema presenti nell'orizzonte temporale di stima, fino all'istante corrente. All'istante successivo nuove misure vanno ad aggiungersi alle precedenti e prendono parte all'ottimizzazione. In questo modo, il carico computazionale aumenta indefinitamente con il passare del tempo, dato che lo stimatore colleziona ed elabora sempre un maggior numero di dati. Il problema diviene quindi di difficile gestione sotto l'aspetto computazionale. Lo stimatore ad orizzonte mobile fornisce un'elegante ed efficace soluzione a questo inconveniente limitando la quantità di dati considerati a ogni istante per mezzo di una finestra mobile, la cui dimensione (fissa) corrisponde alla quantità di dati usati ad ogni istante per effettuare la stima. La finestra si sposta di un passo ad ogni istante, in modo che le misure pervenute al nuovo istante sono aggiunte a quelle già considerate dalla finestra, mentre quelle riferite all'istante più lontano nella finestra temporale sono scartate, mantenendo così fissa la dimensione del problema. Le vecchie misure che vengono scartate ad ogni istante devono comunque essere incluse nel processo di stima, seppur non esplicitamente, anche negli istanti in cui esse non appartengono più alla finestra di stima. Questo per mantenere la corrispondenza con il metodo originario, nonché per preservare la stabilità dello stimatore. Il modo in cui tali misure vengono sintetizzate rappresenta uno dei punti cruciali del metodo. Il costrutto che consente la corretta sintesi dei dati passati prende il nome di costo di arrivo e, purtroppo, possiede un'espressione esplicita solo per sistemi lineari non vincolati, nel cui caso lo stimatore MHE può essere ricondotto al classico KF. Il modo più semplice di costruire il costo d'arrivo è mediante le equazioni di aggiornamento della covarianza del filtro di Kalman. Tuttavia, dato che la ragion d'essere del MHE è proprio la sua capacità di sfruttare i vincoli, è d'obbligo cercare un'approssimante del costo d'arrivo che possa essere impiegata anche per sistemi vincolati e non lineari. Per i sistemi non lineari l'idea è quella di usare le equazioni di aggiornamento del filtro di Kalman esteso, mentre in caso di presenza di vincoli si possono semplicemente impiegare le approssimazioni per sistemi non vincolati, a patto di verificare a posteriori la bontà della scelta. Si è visto come la stima dello stato di un sistema sia di fondamentale importanza in molte discipline. Fra i diversi ambiti in cui trova applicazione, quello della localizzazione incontra la nostra esperienza quotidiana quando utilizziamo gli ormai diffusissimi dispositivi GPS o quando cerchiamo un luogo su una semplice cartina, in quanto consiste nel trovare la posizione di un oggetto rispetto a dei precisi punti di riferimento. Un robot completamente automatico, come quelli che si stanno affacciando sul mercato e che si diffonderanno sempre più in tutte le case per lo svolgimento delle comuni mansioni domestiche, necessita di un corretto rilevamento della propria posizione all'interno della sua area di lavoro, per poter operare in modo realmente autonomo. Un'adeguata localizzazione pone infatti il robot nelle condizioni di poter prendere una decisione circa il proseguimento delle sue operazioni. La localizzazione di un robot che si muove su un piano è definita dalla sua posizione nel piano, in termini di ascissa ed ordinata, e dal suo orientamento. Queste sono le sue tre variabili di stato. Esistono numerosi modi con i quali questa localizzazione può essere realizzata in un ambiente come quello domestico. L'uso di un certo numero di sensori che effettuano misure di distanza fra se stessi e il robot è una soluzione semplice, economica e di facile implementazione. La localizzazione mediante misure di distanza consiste nel determinare lo stato, vale a dire posizione ed orientamento di un oggetto, in questo caso un robot, sulla base proprio di misure di distanza effettuate da un certo numero di sensori fissi nello spazio. Si tratta di un problema di stima dello stato di un sistema non lineare sia nella dinamica sia nell'equazione di misura. In virtù di questa non linearità, la localizzazione di un robot mediante misure di distanza appare un buon banco di prova per metodi di stima di sistemi non lineari, quali, tra quelli qui presi in considerazione, il filtro di Kalman esteso e lo stimatore ad orizzonte mobile. Quest'ultimo consente anche l'aggiunta di vincoli al problema, che nello specifico si traducono in limiti spaziali alla traiettoria percorribile dal robot. Le simulazioni dimostrano l'efficacia di entrambi i metodi di stima in un problema di localizzazione di robot con sei sensori di distanza. L'EKF si dimostra un buono stimatore pur non avvalendosi dell'aggiunta di vincoli, al livello del MHE che però ha l'inconveniente di avere un carico computazionale decisamente più elevato, dovendo effettuare ad ogni istante una minimizzazione di un funzionale di costo non lineare. Il punto di forza del MHE sta proprio nello sfruttare l'aggiunta di vincoli al problema, nel qual caso si ha un netto miglioramento della qualità della stima del MHE rispetto a quella dell'EKF, soprattutto nei tratti di traiettoria in cui il robot è molto vicino ai limiti spaziali. Le misure di un sistema dinamico sono solitamente effettuate da diversi sensori che possono, e in taluni casi devono necessariamente, essere dislocati in diverse posizioni. Nel classico approccio centralizzato alla stima le misure di tutti i sensori vengono raccolte da un'unica unità centrale di computazione, preposta all'elaborazione di tali misure e al calcolo della stima dello stato mediante uno stimatore. Il solo dispositivo dotato di capacità computazionali è, in questo caso, l'unità centrale. E' evidente come questa strategia abbia un grosso punto debole: se venisse meno, per qualsiasi ragione, la capacità dell'unità centrale di assolvere il proprio compito, non sarebbe più possibile ottenere una stima dello stato. C'è quindi una tolleranza ai guasti notevolmente limitata. Si potrebbe pensare di predisporre delle unità di riserva, oppure si potrebbe optare per un diverso approccio al processo di stima. L'approccio distribuito al processo di stima distribuisce il carico computazionale fra i sensori, piuttosto che fare affidamento su un solo dispositivo di calcolo. In questo caso i sensori non sono più dei meri dispositivi di misura, bensì dispongono di capacità computazionali, per eseguire la stima, e di comunicazione, per lo scambio di dati tra sensori. Un insieme di tali dispositivi prende il nome di rete di sensori. Questa strategia distribuita non solo aumenta la tolleranza ai guasti del sistema, ma implica anche una maggior robustezza contro la perdita di dati, poiché non c'è più un solo dispositivo che si basa sui dati di tutti i sensori. I sensori di una rete sono connessi fra loro secondo una determinata configurazione, che può variare a seconda delle esigenze di ogni specifica applicazione, il che significa che non sempre ogni sensore può comunicare con tutti gli altri sensori della rete. Al contrario, questa situazione rappresenta l'eccezione piuttosto che la regola nell'ambito della stima distribuita. Ogni sensore calcola autonomamente la stima dell'intero stato sulla base di misure locali, cioè solo del sensore stesso, o di misure regionali, cioè di sensori direttamente connessi nella rete. Lo scopo di uno stimatore distribuito è fare in modo che tutti i sensori pervengano, almeno asintoticamente, ad una stima condivisa dello stato. L'obiettivo è raggiunto imponendo che i sensori correggano la propria stima locale, ad ogni istante, comparandola alle stime dei sensori attigui. Questo è il punto cruciale di un metodo distribuito di stima: fare in modo che i sensori raggiungano un consenso sulle stime locali. Per questo motivo, ogni stimatore distribuito comprende un algoritmo di consenso avente proprio la funzione di mediare le diverse stime dei sensori della rete. Le prime implementazioni di filtri di Kalman distribuiti (DKF) risalgono ad un paio di decenni fa, il che implica che ormai teoria e pratica applicativa in questo ambito siano assodate. Non così invece per stimatori non lineari distribuiti, come il filtro di Kalman esteso o lo stimatore ad orizzonte mobile, che è un risultato piuttosto recente. In particolare, il primo MHE distribuito (DMHE) per sistemi lineari è stato proposto solo qualche anno fa, mentre la distribuzione di MHE per sistemi non lineari sta ancora muovendo i primi passi. Il problema della localizzazione di un robot mediante misure di distanza può essere affrontato in maniera distribuita, per la sua non linearità, con DEKF e DMHE. Quest'ultimo, proprio come il metodo centralizzato da cui deriva, può incorporare vincoli nel processo di stima. La distribuzione del MHE necessita inoltre di un particolare accorgimento nell'approssimazione del costo di arrivo nei problemi di ottimizzazione locale di ogni sensore, al fine di garantire la stabilità della stima. Nella Tesi, tramite simulazioni si dimostra l'efficacia degli stimatori distribuiti DEKF e DMHE nella stima dello stato del robot, con e senza la presenza di vincoli. Si nota inoltre come differenti configurazioni della rete di sensori, cioè differenti gradi di connettività fra i sensori della rete, influiscano notevolmente sul risultato. Allo stesso modo, si mette in rilievo l'importanza che la fase di consenso, e il numero di volte cui viene iterata in ogni ricorsione dell'algoritmo di stima, ha sulla convergenza delle stime locali ad una stima globale condivisa. La conclusione che si può trarre è certamente l'adeguatezza della scelta di EKF e MHE come stimatori dello stato del robot nel sistema di localizzazione in esame, tanto nelle versioni centralizzate quanto in quelle distribuite, pur con marcate differenze fra i due metodi circa l'onerosità computazionale e l'utilizzo della conoscenza di vincoli.