Stima di Posizione e Orientamento Mediante Elaborazione di Immagini con il Filtro di Kalman

Transcript

1 Incontro CIRA 21 Lecce, Settembre 21 Stima di Posizione e Orientamento Mediante Elaborazione di Immagini con il Filtro di Kalman Fabrizio CACCAVALE Vincenzo LIPPIELLO Bruno SICILIANO Luigi VILLANI PRISMA Lab Dipartimento di Informatica e Sistemistica Università degli Studi di Napoli Federico II Via Claudio 21, 8125 Napoli SOMMARIO Introduzione Filtro di Kalman esteso Simulazioni Conclusione

2 INTRODUZIONE Un problema fondamentale nel controllo con feedback visuale di manipolatori robotici stima di posizione e orientamento dell organo terminale o di oggetti noti in movimento nello spazio di lavoro La stima può essere ottenuta misurando la posizione di punti caratteristici fissati sull oggetto a partire da immagini fornite da una o più telecamere l uso di più telecamere garantisce maggiore robustezza della stima in caso di perdita di alcuni punti caratteristici dell oggetto durante lo svolgimento del compito Se si dispone di una sequenza temporale di immagini è possibile migliorare l accuratezza della stima utilizzando il filtro di Kalman esteso (Lee and Kay, 199; Wang and Wilson, 1992; Wilson, 1996) rumore e errori dovuti al campionamento e alla quantizzazione temporale e spaziale del segnale visivo, distorsione delle lenti, ecc.

3 Ulteriori vantaggi del filtro di Kalman implementazione ricorsiva utilizzo di misure ridondanti possibilità di operare anche in presenza di perdita di alcuni punti caratteristici possibilità di adottare tecniche di finestratura dinamica del piano immagine che consentono di ridurre sensibilmente il tempo di elaborazione In questo lavoro viene presentato un approccio sistematico per la stima di posizione e orientamento di un oggetto basato sul filtraggio di Kalman di immagini provenienti da telecamere fisse A differenza di altri approcci, la stima è calcolata direttamente nella terna base, senza necessità di ulteriori trasformazioni delle variabili di moto stimate possibilità di impiegare un numero qualsiasi di telecamere fisse possibilità di utilizzare efficacemente misure ridondanti

4 #! MODELLISTICA Terne di riferimento terna base terna oggetto terna (solidale alla) telecamera con l asse parallelo all asse ottico, il piano parallelo al piano dei sensori e l origine coincidente con il centro ottico terna immagine con gli assi e paralleli agli assi e rispettivamente e l origine (punto ottico principale) all intersezione dell asse ottico con il piano immagine Un punto di coordinate nella terna telecamera è proiettato nel punto del piano immagine di coordinate "! distanza focale effettiva trasformazione prospettica

5 ith - Camera coordinate frame x Ci Base coordinate frame x z c Ci v Ci u Ci y Ci O Ci O Ci Sensor plane Object coordinate frame o O O p j y z Ci Ci Ci ( u, v ) j j r Ci Image plane x O z O O p j Object O O y O Campionamento spaziale " " ( coordinate di (numero di pixel) ) = fattore di scala di riga (colonna)

6 La posizione e l orientamento dell oggetto possono essere espressi in termini del vettore di coordinate dell origine e della matrice di rotazione della terna oggetto rispetto alla terna base è il vettore degli angoli di Roll, Pitch e Yaw Le componenti dei vettori e sono le sei quantità incognite da stimare in termini delle misure delle coordinate degli punti caratteristici dell oggetto fornite dalle telecamere

7 ! Calcolando le coordinate dei punti caratteristici ( ) in termini di numero di pixel sul piano immagine della -esima telecamera ( ), si ottengono le equazioni che costituiscono un sistema non lineare nelle incognite e le coordinate nel piano immagine sono calcolate come è calco- il vettore di coordinate lato come dove e sono rispettivamente il vettore di posizione e la matrice di rotazione (assunti noti e costanti) della -esima telecamera espressi in terna base il vettore è calcolato come dove è il vettore (costante e noto) di coordinate di nella terna oggetto

8 FILTRO DI KALMAN ESTESO Il calcolo della soluzione è oneroso e deve essere ripetuto ad ogni istante di campionamento il filtro di Kalman fornisce una soluzione di tipo ricorsivo e può incorporare misure ridondanti Il moto dell oggetto è descritto in termini di un modello dinamico a tempo discreto nello spazio di stato dove errore di modello (rumore Gaussiano a media nulla) la velocità dell oggetto è assunta costante nell intervallo di campionamento

9 Equazione di uscita del filtro di Kalman dove!!!!!!!! rumore di misura (Gaussiano a media nulla) funzione non lineare del vettore di stato l equazione di uscita deve essere linearizzata intorno alla stima corrente dello stato (filtro di Kalman esteso)

10 Equazioni del filtro di Kalman dove matrice di covarianza del disturbo in ingresso matrice di covarianza del rumore di misura in uscita matrice di covarianza dell errore di stima matrice di guadagni di Kalman Jacobiano della equazione di uscita

11 SIMULAZIONI Sistema di visione stereo costituito da tre telecamere fisse con una frequenza di campionamento di 5 Hz Gli assi ottici delle telecamere giacciono su un piano parallelo al piano della terna base a una distanza m La distanza focale effettiva è di pixel è m mm; la dimensione di un

12 Camera 3 y 3 O 3 z 3 2.m Base coordinate system O y Observation center x 2.m Camera 1 Camera 2 z 1 z 2 O 1 y 2 y 1 O 2.75m 1.5m 1.5m Posizione e orientamento delle telecamere

13 Per riprodurre gli effetti di distorsione geometrica delle telecamere reali è stato utilizzato un modello polinomiale che include la distorsione radiale, di decentramento e prismatica (Weng, Cohen and Herniou, 1992) i valori numerici dei parametri di distorsione sono stati scelti in maniera da ottenere errori fino a % della semi-dimensione del sensore Lo stesso modello è stato considerato nell algoritmo del filtro di Kalman per la compensazione della distorsione i parametri corrispondenti sono stati scelti in modo da ottenere errori per le singole componenti compresi tra % e % per rispecchiare una tipica condizione di compensazione imperfetta errori fino a % sono stati introdotti nei parametri del modello geometrico del sistema di visione stereo per rispecchiare una tipica condizione di calibrazione imperfetta

14 x [mm] Roll [deg] y [mm] Pitch [deg] z [mm] Yaw [deg] Traiettoria di posizione (a sinistra) e orientamento (a destra) dell oggetto sull oggetto sono stati scelti quattro punti caratteristici alle misure delle proiezioni dei punti caratteristici sul piano immagine è stato aggiunto rumore gaussiano bianco indipendente la varianza del rumore è stata scelta pari a ( m è la dimensione del pixel) per simulare l errore di campionamento spaziale

15 Matrici di covarianza scelta in modo da tener conto approssimativamente dell errore dovuto alle semplificazioni introdotte nel modello (velocità costante) considerando solo disturbi di velocità calcolata sulla base delle statistiche degli errori di calibrazione

16 Primo caso di studio: condizioni operative standard x [mm] Roll [deg] y [mm] 2 2 Pitch [deg] z [mm] 2 2 Yaw [deg] Errore di posizione (a sinistra) e di orientamento (a destra) a causa dell asimmetria nella disposizione delle telecamere l errore di posizione appartiene all intervallo mm per la componente mentre appartiene all intervallo mm per le componenti e per la stessa ragione l errore di orientamento appartiene all intervallo per l angolo di Yaw e all intervallo per gli angoli di Roll e di Pitch quando l oggetto si ferma è presente un errore residuo dovuto al rumore e alle incertezze di modello

17 Secondo caso di studio: perdita casuale di punti caratteristici dovuta a una scelta errata della finestra del piano immagine da processare o a posizioni e/o orientamenti particolari dell oggetto rispetto alla telecamera La presenza di misure ridondanti consente di continuare il processo di stima purchè le misure disponibili siano sufficienti a caratterizzare posizione e orientamento dell oggetto le varianze corrispondenti alle misure mancanti (elementi diagonali della matrice di covarianza ) possono essere incrementate fino a eliminare l influenza di tali misure sulla stima quando una misura è di nuovo disponibile la corrispondente varianza viene riportata al valore normale

18 Perdita casuale di punti caratteristici orientation [deg] position [mm] time [sec] time [sec] indicati con x : istanti di tempo corrispondenti alla perdita contemporanea di un punto caratteristico per ciascuna telecamera ( % di probabilità) indicati con o : istanti di tempo corrispondenti alla perdita contemporanea di due punti caratteristici per ciascuna telecamera ( % di probabilità) La norma dell errore di posizione rimane inferiore a mm e la norma dell errore di orientamento rimane inferiore a gradi

19 Perdita completa dell immagine orientation [deg] position [mm] time [sec] time [sec] le telecamere, e perdono completamente l immagine dopo s, s e s rispettivamente; il normale funzionamento viene ripristinato dopo s per tutte le telecamere Le prestazioni rimangono soddisfacenti

20 CONCLUSIONE Stima di posizione e orientamento di un oggetto in movimento da misure visuali filtro di Kalman esteso di una sequenza di immagini stereo per il calcolo ricorsivo di una soluzione delle equazioni prospettiche la stima è calcolata direttamente in terna base e non sono richieste ulteriori trasformazioni delle variabili di moto stimate l approccio può essere applicato a qualsivoglia numero di telecamere fisse nello spazio di lavoro robustezza in presenza di rumore, distorsione geometrica delle lenti, errori di calibrazione e perdita temporanea di punti caratteristici

21 Sviluppi futuri utilizzo di tecniche di modellazione geometrica di oggetti basate su strutture BSP (Binary Space Transition) per migliorare l efficienza dell approccio basato sul filtro di Kalman sperimentazione su un sistema di visione costituito da due telecamere analogiche SONY 85CE con frame grabber e DSP MATROX Genesis sviluppo e sperimentazione di algoritmi di controllo del moto con feedback visuale sviluppo e sperimentazione di algoritmi di controllo dell interazione con feedback di forza e visuale