M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici SLAM Simultanea localizzazione e mappatura di veicoli autonomi.

Presentazione sul tema: "M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici SLAM Simultanea localizzazione e mappatura di veicoli autonomi."— Transcript della presentazione:

1 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici SLAM Simultanea localizzazione e mappatura di veicoli autonomi in ambiente non strutturato

2 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Strutturato: -Ingegnerizzazione dell’ambiente -Bassa flessibilità -Conoscenza a priori della mappa Non strutturato: -Non necessita di infrastrutture -Alta flessibilità -Costruzione mappa in tempo reale Passi fondamentali per interagire con ambiente non-strutturato: - navigazione e propagazione incertezza mediante sistemi di misura autocontenuti - autolocalizzazione mediante landmark naturali - fusione tra navigazione incrementale e quella riferita all’ambiente - mappatura dell’ambiente stesso Ambiente in cui il robot opera

3 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici - Veicolo a guida differenziale - Sistema di acquisizione PXI LabView RTOS ParametroIncertezzaRange distanze5 cm7-50 m angoli0.1 °0-180 ° - Laser a scansione IR [SICK PLS-101] : Tempo di volo Risoluzione angolare max: 0.5° Campo di scansione: (0°,180°) Distanza massima: 50 m Accuratezza = f(orientazione relativa,materiali) - 2 Encoder incrementali : n o :4000 n o :4000 Veicolo e strumentazione

4 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici ParametroValore Nominale µ Deviazione standard σR 89.50 mm0.14 mm RLRL 89.50 mm0.14 mm L89.50 mm0.5 mm b89.50 mm0.5 mm Equazione cinematica di ricorrenza: Equazione cinematica di ricorrenza(PLS): Covarianza della posa: Propagazione incertezza: [x,y,δ] δ PLS R ΔLS b θ LS α i,j RLRL Parametri incerti: Propagazione incertezza odometrica

5 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappatura

6 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Modello rappresentativo dell’ambiente in cui il robot stesso si trova immerso: Occupancy Grids - Suddivisione ambiente in celle disposte a griglia - Ogni cella ha probabilità di essere occupata o non-occupata - Costruzione del modello del sensore Mappatura – caso monodimensionale Caso laser: σ r = 5 cm z = misura x = distanza reale s = stato di occupazione Distribuzione probabilistica O/E: 1 Occupazione 0.5 Nessuna informazione 0 Non occupazione

7 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Modello rappresentativo dell’ambiente in cui il robot stesso si trova immerso -Occupancy Grids -Suddivisione ambiente in griglia area 10x10 m cella 5x5 cm - Ogni cella ha probabilità di essere occupata o non-occupata - Costruzione del modello del sensore Estensione al caso 2D: σ r =5cm σ θ =0.5° Ambiente realeAmbiente mappato Distribuzione O/E Mappatura – caso bidimensionale Distribuzione probabilistica O/E OPPOSTA: 0 Occupazione 0.5 Nessuna informazione 1 Non occupazione

8 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappatura – singolo raggio laser z : l’angolo misurato

9 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici La O, ovvero occupied region viene definita nell’intervallo: Linea continua blu: Mappatura – singolo raggio laser

10 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici La E, ovvero empty region viene definita nell’intervallo: Linea tratto-punto blu: Mappatura – singolo raggio laser Linea continua blu:

11 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Distribuzione probabilistica O/E: 1 Occupazione 0.5 Nessuna informazione 0 Non occupazione Rappresentazione mediante livelli di grigio Mappatura – modello bidimensionale polare

12 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici PUNTI SCANSIONATIMAPPA LOCALE MAPPA CON INCERTEZZA Filtraggio gaussiano: Mappatura Fusione Bayesiana: (ricorsiva tra le mappe date dai singoli raggi laser)

13 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappatura – Fusione Bayesiana Dove: - mappa o singola cella - posa del veicolo - misure laser dalla 1a alla t-esima - mappa data dalla t-esima scansione - mappa data dalle 1:t-1 scansioni precedenti - conoscenza a priori sulla mappa

14 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappatura – associazione mappe singoli raggi

15 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappatura – associazione mappe scansioni

16 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici

17

18 Mappatura – associazione mappe singoli raggi

19 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappatura e localizzazione – Algoritmo completo

20 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici 1. Rototraslazione - secondo misura encoder 2. Associazione - nell’intorno incertezza encoder (dominio cartesiano) si cercano features corrispondenti 3. Determinazione correzione di posa – considerando solo le features associate si minimizza la somma delle distanze pesate funzione della correzione di posa 4. Fusione tra posa odometrica e correzione ricavata Localizzazione – passi dell’algoritmo

21 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici - Estrazione segmenti - Segmentazione punti - Determino parametri e incertezza Aggiungi al cluster Nuovo cluster Localizzazione - clusterizzazione

22 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Presenza di punti occlusi - Estrazione angoli: segmenti adiacenti con differenza di orienzazione a 90° - Estrazione spigoli: estremi di segmenti non occlusi - Estrazione punti isolati (foreground point): punti che non appartengono a segmenti - Inizio di parti occluse: - Punti solo apparentemente isolati Localizzazione – estrazione Features

23 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Vantaggi:  riduzione informazioni da elaborare  considera solo le caratteristiche rilevanti Svantaggi:  l’estrazione è influenzata dalla tecnologia del sensore (clock counter per le zebrature) Localizzazione – estrazione Features Esempio di ‘zebrature’

24 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici - Stima Rototraslazione relativa - Funzione per il cambio di prospettiva - Data association: - definito un certo livello di tolleranza lateralmente di solito non si hanno associazioni Localizzazione – associazione features Weighting functions:

25 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Iterating Closest Point Localizzazione a partire da insiemi di features di Fb: Si determinano i Closest Point su Fa Si calcola R e T Si applica la trasformazione a Fb Se l ’ errore quadratico medio pesato è inferiore ad una certa soglia Localizzazione – metodo ICP

26 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici L’algoritmo ICP può essere schematizzato nel seguente modo: 1. Per ogni feature appartenente all’insieme Fb, si calcola quella più vicina della stessa categoria appartenente all’insieme Fa; 2. Con le corrispondenze calcolate nello step 1, calcola la trasformazione di rototraslazione [R t] 3. Si applica la trasformazione [R t] all’insieme Fb; 4. Se l’errore quadratico medio è minore di una certa soglia, l’algoritmo termina, altrimenti si ricomincia dallo step 1. NOTA: - ad ogni iterazione ciò che varia è il set di associazioni (in caso negativo si è giunti in un minimo locale) - L’algoritmo ICP converge sempre monotonicamente al minimo locale rispetto alla distanza quadratica media della funzione oggetto, che è quindi oggetto di valutazione e scelta accurata Localizzazione – metodo ICP

27 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici - Determinazione incertezza localizzazione - Raggio spettrale matrice Cov - Viene ad essere determinata l’incertezza a partire dal numero di features che partecipano al marching Taratura Rototraslazioni in ambienti reali noti mediante guida prismatica ed encoder - Prove eseguite con differenti ambienti reali, caratterizzati da diversi tipi e numero di punti invarianti

28 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici - Fusione Bayesiana: si combina l’informazione precedente (Encoder) con l’informazione del sensore (ICP) in modo da calcolare la distribuzione posteriore: Osservazioni: - in presenza di rotazioni repentine del veicolo gli encoder sovrastimano - si nota l’accumulo di deriva del solo metodo odometrico Fusione

29 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Sono stati fatti compiere al veicolo 2 giri del laboratorio per verificare i metodi sviluppati

30 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappa da encoderMappa da fusione Vetrate Incongruenze dovuta alla deriva

31 M. De Cecco - Sensor Fusion: teoria ed applicazione alla percezione di sistemi robotici Mappa da encoder Mappa da fusione Esempio di mappatura con forte deriva