Robotica Probabilistica Fast SLAM SLAM con particle filters

Robotica Probabilistica Fast. SLAM

SLAM con particle filters § § SLAM con particle filters § Si può utilizzare però una fattorizzazione: filtri Rao. Blackwellized: se un oracolo ci dice il path, la stima della locazione delle features è indipendente. § Fast. SLAM: Particelle per la posa, Gaussiane per le features § Mantiene più ipotesi sulle associazioni di dati (più robusto); non linearità; full SLAM & online SLAM trattati in modo uniforme. Problema della dimensione: molte partices e l’algoritmo scala in modo esponenziale 2

Il problema dello SLAM Robot in ambiente, statico, sconosciuto Dato: § Il controllo dei robot § Osservazione delle features vicine Stima: § Mappa delle features § Path del robot 3

Why is SLAM a hard problem? SLAM: robot path and map are both unknown! Robot path error correlates errors in the map 4

Why is SLAM a hard problem? Robot pose uncertainty § In the real world, the mapping between observations and landmarks is unknown § Picking wrong data associations can have catastrophic consequences § Pose error correlates data associations 5

Data Association Problem § A data association is an assignment of observations to landmarks § In general there are more than (n observations, m landmarks) possible associations § Also called “assignment problem” 6

Filtri Particellari § § Rappresentano belief con random samples § Principio di Sampling Importance Resampling (SIR): Stima di processi non-Gaussiani, nonlinear § Estrai la nuova generazione di particelle § Assegna un peso di importanza ad ogni particle § Resampling § Applicazioni tipiche sono: tracking, localization, etc. 7

Localizzazione vs. SLAM § Un particle filter può essere usato per risolvere entrambi i problemi § Localizzazione: spazio di stato < x, y, > § SLAM: spazio di stato < x, y, , map> § per mappe di landmark = < l 1, l 2, …, lm> § Per mappe a grid = < c 11, c 12, …, c 1 n, c 21, …, cnm> § Problema: il numero di particelle necessario per rappresentare il posterior cresce esponenzialmente con la dimensione dello spazio di stato 8

Dipendenze § Esiste una dipendenza probabilistica tra le dimensioni dello spazio di stato (posa, mappa)? § Se è così, possiamo usare le dipendenze per risolvere il problema in modo più efficiente? 9

Dipendenze § Esiste una dipendenza tra le dimensioni dello spazio di stato? § Se è così, possiamo usare le dependenze per risolvere il problema più efficienza? § Nel contesto dello SLAM: § La mappa dipende dalle pose del robot; § Noi sappiamo come costruire una mappa data la posizione nota del sensore. 10

Posteriore Fattorizzato (Landmark) pose mappe osservazioni & movimenti SLAM posterior Robot path posterior landmark posizioni Aiuta a risolvere il problema? Introdotta da Murphy nel 1999 11

Mapping con Landmarks l 1 Landmark 1 z 1 osservazioni Robot pose controllo x 1 x 0 u 0 z 3 x 2 x 3 u 1 . . . xt ut-1 z 2 Landmark 2 zt l 2 La conoscenza del percorso reale rende le posizioni dei landmark cond. indipendenti 12

Posterior Fattorizzato Robot path posterior (problema localizzazione) Cond. indipendente posizione landmark 13

Rao-Blackwellization § Questa fattorizzazione anche chiamata Rao. Blackwellization § Dato che il secondo termine può essere calcolato efficientemente il particle filtering diventa possibile 14

Fast. SLAM § Rao-Blackwellized particle filtering basato su landmarks [Montemerlo et al. , 2002] § Ogni landmark rappresentato da un 2 x 2 Extended Kalman Filter (EKF) § Ogni particella deve mantenere M EKFs x, y, Landmark 1 Landmark 2 … Landmark M Particle #2 x, y, Landmark 1 Landmark 2 … Landmark M … Particle #1 Particle N 15

Fast SLAM § Ogni particella mantiene N EKF § Per M volte: § Retrieval: prendi una posa x § Predizione: campiona una posa da p(x | x , u ) § Update di misura: per ogni misura i, trova la corrispondenza j, e aggiorna il corrispondente EKF § Peso: pesa la nuova particella wk k, t-1 t § Ricampionamento: ricampiona le M particelle 16

Action Update § Per ogni posa, campionamento dal modello probabilistico di moto: § Esempio, dal modello in velocità: § Particella aggiunta in un insieme di particelle 17

Fast. SLAM – Action Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3 18

Fast. SLAM – Sensor Update Corrispondenze note n 1, … , nt Se feature n osservata, allora update Se feature n non osservata, stessa probabilità Aggiornamento come EKF 19

Fast. SLAM – Sensor Update Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3 20

Fast. SLAM - Resampling • Peso delle particelle e ricampionamento: le particelle non sono distribuite secondo il posterior, non è inclusa la misura • Occorre il peso (considerando le corrispondenze): measurement covariance Innovazione: valore osservato – valore stimato 21

Fast. SLAM – Sensor Update Peso per la verosomiglianza Particle #1 Weight = 0. 8 Particle #2 Weight = 0. 4 Particle #3 Weight = 0. 1 24

Fast. SLAM 25

Fast. SLAM Courtesy: Mike Montemerlo 26

Fast. SLAM Complessità (Naive) § Update particelle del robot basate su controllo ut-1 § Incorporare osservazioni zt nei filtri di Kalman § Resampling del particle set N = Numero di particelle M = Numero di features O(N) Constant time per particle O(N • M) M per particle O(N • M) Log time per particle O(N • M) M per particle 27

Fast. SLAM Complessità § Update particelle del robot basate su controllo ut-1 § Incorporare osservazioni zt nei filtri di Kalman § Resampling del particle set N = Numero di particelle M = Numero di features O(N) Constant time per particle O(N • log(M)) Log time per particle 28

Fast. SLAM Complessità § l passi di update e di peso sono lineari nel numero delle particelle § Il passo di resampling se naive allora lineare nel numero di feature § Se ogni particella organizzata come albero binario di stimatori di feature allora log N § Fino a 1000000 landmark 29

Fast. SLAM Complessità § l passi di update e di peso sono lineari nel numero delle particelle 30

Fast. SLAM Complessità § Memory Complexity 31

Problema di Data Association § Quali osservazioni appartengono a quale landmark? § Un SLAM robusto deve considerare possibli associazioni di dati § Le possibili associazioni di dati dipendono anche dalla posa del robot 32

Data Association multi-ipotesi § Se la data association è basata su particelle; § Ogni particella ha la sua data association; § Prima si calcola la posa, quindi l’associazione; § Errori di posa del robot sono fattorizzati fuori dalle decisioni di associazioni data. 33

Associazione dati per particella Osservazione generata dal landmark blu o rosso? P(observation|red) = 0. 3 P(observation|blue) = 0. 7 § Due opzioni per l’associazione per-particle: § Prendi la più probabile § Prendi un’associazione random pesata con il likelihood di osservazione § Se la probabilità è bassa, genera un nuovo landmark 34

Risultati – Victoria Park § Traversata di 4 km § < 5 m RMS (Root Mean Square) errore di posizione § 100 particelle Blue = GPS Yellow = Fast. SLAM Dataset courtesy of University of Sydney 35

Risultati – Victoria Park Dataset courtesy of University of Sydney 36

Risultati – Data Association 37

Risultati – Accuratezza 38

Fast. SLAM 2. 0 § Fast. SLAM 1. 0 usa il motion model come proposal; § Fast. SLAM 2. 0 usa anche le misure per aggiustare § Es. Scan Match [Montemerlo et al. 2003] § Meno particelle, più accurate 39

Fast. SLAM Riassunto § Fast. SLAM fattorizza il posterior SLAM in problemi di stima a bassa dimensione; § Scala problemi con più di 1 milione di feature; § Fast. SLAM fattorizza l’incertezza della posa del robot separandola dal problema del data association; § Robusto a significative ambiguità nell’associazione dati; § Permette di ritardare decisioni di data association finchè non si ottiene evidenza non ambigua; § Complessità di O(N log. M). 40

SLAM Grid-based § Possiamo risolvere il problema SLAM senza landmark predefiniti? § Possiamo usare le idee di Fast. SLAM per costruire grid map? § Come con i landmark, la mappa dipende dalle pose del robot durante l’acquisizione dati § Se le pose sono note, grid-based mapping è facile (visto in “mapping con pose note”) 41

Mapping usando Raw Odometry 42

Mapping con pose note § Mapping con pose note usando dati di laser range 43

Rao-Blackwellization pose mappa osservazioni & movimenti Introdotte da Murphy nel 1999 44

Rao-Blackwellization pose mappa osservazioni & movimenti SLAM posterior Robot path posterior Mapping con pose note Introdotta da Murphy nel 1999 45

Rao-Blackwellization Localizzazione, usa MCLoc Usa la stima di posa dalla MCLoc e applica il mapping con pose note 46

Un modello grafico del Mapping Rao-Blackwellized u 0 x 0 u 1 ut-1 x 2 z 1 z 2 . . . xt m zt 47

Mapping Rao-Blackwellized § Ogni particella rappresenta una possibile traiettoria del robot § Ogni particella § mantiene la sua propria mappa e § aggiorna con “mapping con pose note” § Ogni particella sopravvive con una probabilità proporzionale al likelihood di osservazione relativa alla sua propria mappa 48

Esempio Particle Filter 3 particelle mappa particella 1 mappa particella 2 mappa particella 3 49

Problema § Ogni mappa è grande (grid maps) § Poichè ogni particella mantiene la sua mappa § Il numero di particelle va mantenuto piccolo (poche ma buone) § Soluzione: Calcolo di migliori distribuzioni § Idea: Migliorare la stima di posa prima applicare il particle filter 50

Correzione di posa con Scan Matching Massimizza il likelihood della i-esima posa e mappa relativa alla (i-1)-esima posa e mappa misura corrente Movimento robotico mappa costruita fino ad ora 51

Fast. SLAM con Odometria Migliorata § Scan-matching fornisce una correzione di posa localmente consistente § Sequenze odometriche pre-corrette usando scan-matching e uso come input per Fast. SLAM § Minori particelle sono necessarie perchè l’errore di input è ridotto [Haehnel et al. , 2003] 52

Mapping con Scan Matching 53

Modello Grafico per Mapping con Odometria Migliorata u 0. . . uk-1 z 1. . . z k-1 x 0 uk. . . u 2 k-1 zk+1. . . z 2 k-1 . . . un·k. . . u(n+1)·k-1 z n·k+1. . . z(n+1)·k-1 u'2 . . . u'n xk x 2 k . . . x n·k zk z 2 k . . . zn·k m 54

Fast. SLAM con Scan-Matching 55

Fast. SLAM con Scan-Matching Loop Closure 56

Map: Intel Research Lab Seattle Fast. SLAM con Scan-Matching 57

Confronto con Fast. SLAM Standard § Stesso modello per le osservazioni § Odometria invece di scan matching come input § Numero di particelle variano da 500 a 2. 000 § Risultato tipico: 58

Fast. SLAM con Scan-Matching 59

Fast. SLAM con Scan-Matching 60

Ulteriori Miglioramenti § Distribuzione proposata migliorata porta ad una mappa più accurata; § Ottenuta adattando la distribuzione proposta considerando le osservazioni più recenti; § Passi di re-sampling flessibile possono ulteriormente migliorare l’accuratezza. 61

Proposta Migliorata § La proposta si adatta alla struttura dell’ambiente 62

Re-sampling selettivo § Re-sampling è pericoloso, dato che campioni importanti possono andare perduti (particle depletion problem) § Nel caso di proposte sub-ottime il resampling della distribuzione è necessario per arrivare alla convergenza. § Questione chiave: quando fare il resampling? 63

Numero di Particelle Effettive § Misure empiriche di quanto la distribuzione obiettivo è approssimata da campioni estratti dalla proposta; § neff descrive “la varianza del peso delle particelle”: peggiore è l’approssimazione maggiore è la varianza; § neff con pesi uguali massima: la distribuzione è vicina a quella proposta 64

Resampling con Neff § Re-sampling ogni volta che neff va sotto una soglia (n/2) § Vedi [Doucet, ’ 98; Arulampalam, ’ 01] 65

Evoluzione tipica di neff Visita di nuove aree chiusura del primo loop Visita di area note Seconda chiusura loop 66

Intel Lab § 15 particelle § Quattro volte più veloce del realtime P 4, 2. 8 GHz § 5 cm di risoluzione durante lo scan matching § 1 cm di risoluzione nella mappa finale 67

Intel Lab § 15 particelle § Confrontato a Fast. SLAM con Scan-Matching, le particelle sono propagate più vicine alla vera distribuzione 68

Outdoor Campus Map particles § 30 particelle § 250 x 250 m 2 § 1. 75 1. 088 km miglia (odometry) (odometria) resolution § 20 cm risolutzione during scan durante lo scan matching resolution § 30 cm risoluzione in final map nella mappa finale 69

MIT Killian Court § “infinite-corridor-dataset” al MIT 70

MIT Killian Court 71

Conclusioni § Le idee di Fast. SLAM possono essere applicate alle grid maps § Usando sensori accurati per le osservazioni porta a buone proposte e filtri altamente efficienti § Simile allo scan-matching su base per-particle § Il numero di particelle necessarie e passi di re-sampling può essere seriamente ridotto § Versioni migliorate di Fast. SLAM grid-based può gestire ambienti più larghi delle implementazioni naïve in “real time” perché richiedono un ordine di grandezza minore di campioni 72

Riferimenti su Fast. SLAM § M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. Fast. SLAM: A factored solution to simultaneous localization and mapping, AAAI 02 § D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient Fast. SLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS 03 § M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. Fast. SLAM 2. 0: An Improved particle filtering algorithm for simultaneous localization and mapping that provably converges. IJCAI-2003 § G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, ICRA 05 § A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI 03 73

Robotica Probabilistica Information Gain-Based Exploration SA-1

Task dei Robot Mobili 75

Esplorazione e SLAM 76

Filtri Particellari 77

Fattorizzazione 78

Filtri Particellari 79

Fast. SLAM 80

Esplorazione 81

Esplorazione 82

Esplorazione vs SLAM La strategia di esplorazione determina la qualità della mappa 83

Approccio Decision-Teoretico 84

Incertezza Posterior 85

Calcolo Entropia 86

Calcolo incertezza della Mappa e della Posa Data la rappresentazione approssimata Incertezza percorso + incertezza mappa 87

Incertezza del posterior nelle Grid Map Ogni cella è una variabile aleatoria binaria 88

Entropia della Mappa 89

Incertezza sulla traiettoria § Ogni posa dipende dalle pose precedenti 0: t-1 § Approssimazione con incertezza media sul percorso: H(p(x 1: t|dt)) § Posterior su traiettoria rappresentato come gaussiana 90

Incertezza sulla traiettoria 91

Approssimazione incertezza sulla traiettoria 92

Guadagno di informazione con l’esecuzione delle azioni 93

Guadagno di informazione atteso 94

Sequenze di Misure 95

Sequenze di Misura 96

Utilità 97

Azioni specifiche 98

Doppia Rappresentazione per la rilevazione di cicli 99

Esempio 100

Possibili Target 101

Valutazione dei Target 102

Spostamento sul Target 103

Valutazione dei Target 104

Movimento Robot 105

Evoluzione dell’entropia 106

Confronto 107

Esplorazione 108

Esplorazione Corridoio Torna indietro per rilocalizzarsi 109

Riassunto 110

Robotica Probabilistica Navigazione SA-1

Navigazione § Evitare ostacoli (fissi, mobili) § Pianificare traiettoria § Esplorare 112

Ostacoli: Minkowski Sum § § Come rappresentare robot e ostacoli Forma ostacolo + forma del robot Robot approssimato da punto materiale Ostacolo aumentato § Somma A + B = {a+b | a in A, b in B} con A, B spazi vettoriali 113

Percorsi tra Ostacoli: Metodi di Voroni § Percorsi equidistanti tra i due punti-oggetto più vicini (diagrammi di Voronoi); § Roadmap a partire dalla mappa: § Raggiungere il path vicino, § Path verso il goal, § Raggiungere il goal. 114

Percorsi tra ostacoli: Metodi Bug § Metodi Voroni richiedono mappa intera, percorsi equidistanti e lunghi. § Metodo Bug 1: § Vai da S a T in linea retta; § Se c’è ostacolo circumnavigalo finché non trovi punto di uscita. § Non sempre un buon piano, ma garantito B se raggiungibile. Path-Planning Strategies for a Point Mobile Automaton Moving Amidst Unknown Obstacles of Arbitrary Shape, Lumelsky Stepanov, 1987 115

Percorsi tra ostacoli: Metodi Bug § Metodi voroni richiedono mappa intera, percorsi equidistanti e lunghi. § Metodo Bug 2: § Vai da A a B in linea retta; § Se c’e’ ostacolo circumnavigalo finchè non incontri/vedi la linea AB. § Continua a seguire AB § Migliore piano, nei casi semplici Path-Planning Strategies for a Point Mobile Automaton Moving Amidst Unknown Obstacles of Arbitrary Shape, Lumelsky Stepanov, 1987 116

Percorsi tra Ostacoli: Metodi Potenziali § Goal basso potenziale, ostacoli alto potenziale; § Ad ogni punto potenziale: § Ad ogni punto forza: § Quale potenziale? 117

Percorsi tra Ostacoli: Metodi Potenziali § Funzioni tipiche: § Tra voroni e bug per vicinanza ad ostacoli § Metodo locale, quindi problema minimi locali 118

Path Planning § Traiettorie prive di collisioni § Il robot dovrebbe raggiungere la locazione prima possibile 119

Path planning e Ambiente Dinamico § Come reagire ad ostacoli imprevisti? § Efficacia § Affidabilità § Approcci: 120

Obiettivi in ambiente dinamico § Calcolo del path ottimo considerando incertezze potenziali nelle azioni § Generare azioni rapidamente in caso di ostacoli imprevisti 121

Approccio Classico 122

Path Planning su Grid. Map 123

Alberi di Ricerca § Metodo: § Esplorazione dello spazione degli stati generando I successor degli stati già esplorati (a. k. a. ~expanding states). § Ogni stato valutato: è uno stato goal? 124

Best-first search § Idea: usare una evaluation function f(n) per ogni nodo § f(n) fornisce una stima del costo totale à Espandi il nodo n con più piccolo f(n). § Implementazione: Ordina i nodi in ordine di costo crescente.

A* search § Idea: avoid expanding paths that are already expensive § Evaluation function f(n) = g(n) + h(n) § g(n) = cost so far to reach n § h(n) = estimated cost from n to goal § f(n) = estimated total cost of path through n to goal § Best First search has f(n)=h(n) § Uniform Cost search has f(n)=g(n)

Admissible heuristics § A heuristic h(n) is admissible if for every node n, h(n) ≤ h*(n), where h*(n) is the true cost to reach the goal state from n. § An admissible heuristic never overestimates the cost to reach the goal, i. e. , it is optimistic § Example: h. SLD(n) (never overestimates the actual road distance) § Theorem: If h(n) is admissible, A* using TREE-SEARCH is optimal

Dominance § If h 2(n) ≥ h 1(n) for all n (both admissible) § then h 2 dominates h 1 § h 2 is better for search: it is guaranteed to expand less or equal nr of nodes.

Relaxed problems § A problem with fewer restrictions on the actions is called a relaxed problem § The cost of an optimal solution to a relaxed problem is an admissible heuristic for the original problem § If the rules of the 8 -puzzle are relaxed so that a tile can move anywhere, then h 1(n) gives the shortest solution § If the rules are relaxed so that a tile can move to any adjacent square, then h 2(n) gives the shortest solution

Consistent heuristics § A heuristic is consistent if for every node n, every successor n' of n generated by any action a, h(n) ≤ c(n, a, n') + h(n') § If h is consistent, we have f(n’) = = ≥ ≥ g(n’) + h(n’) g(n) + c(n, a, n') + h(n’) g(n) + h(n) = f(n) (by def. ) (g(n’)=g(n)+c(n. a. n’)) (consistency) § i. e. , f(n) is non-decreasing along any path. It’s the triangle inequality ! § Theorem: If h(n) is consistent, A* using GRAPH-SEARCH optimal keeps all is checked nodes in memory to avoid repeated states

Properties of A* § Complete? Yes (unless there are infinitely many nodes with f ≤ f(G) , i. e. step-cost > ε) § Time/Space? Exponential: except if: § Optimal? Yes § Optimally Efficient: Yes (no algorithm with the

Path Planning nella grid map L’algoritmo di A* può essere utilizzato nella grid map (occupancy grid) 133

Value Iteration 134

Assunzioni tipiche in A* path planning 135

Problemi 136

Possibile Soluzione: convoluzione della grid map 137

Esempio 138

Convoluzione 139

Applicazione di A* 140

Costal Navigation § Non il path più breve, ma quello che minimizza la probabilità di perdersi § Considerare: lunghezza path e infromazione persa (es. Minerva Robot) § Entropy Map: 141

Dynamic Windows 142

Dynamic Windows Regolazione della velocità in funzione degli ostacoli 143

Dynamic Windows 144

Dynamic Windows 145

Dynamic Windows Approach § Brevi tempi di reazione (dipendenti da parametri) § Veloce in ambienti popolati 146

Dynamic Windows 147

Problema con DWA 148

Problema con DWA 149

Problema con DWA 150

Problema con DWA 151

Problema con DWA 152

Problema con DWA 153

5 D Path-planning § Alternativa all’approccio a due livelli 154

Spazio di Ricerca 155

Spazio di Ricerca 156

Passi dell’algoritmo 157

Aggiornamento della grid-map 158

Percorso nella mappa 2 D 159

Restringi lo spazio di ricerca 160

Spazio di Ricerca 161

Trova il percorso in 5 D 162

Esempio 163

Timeout 164

Generazione di Comandi 165

Timeout avoidance 166

Esempio 167

Confronto con DWA 168

Confronto con DWA 169

Confronto con l’ottimo 170

Riassunto 171

Path Planning Probabilistico 172

Probabilistic Roadmap 173

Fase di Learning Configurazione random nello spazio libero, poi connessa da archi collegando tutti i k vicini a distanza minore di n 174

Fase di Query Nella fase di Query si connette goal e posizione iniziale e si cerca lo shortest path (A*, Dijkstra) E’ probabilisticamente completo: all’aumentare dei punti, la probabilità di non trovare il percorso va a zero 175

Rapidly Exploring Random Trees 176

RRT: Algoritmo 177

RRT: Esempio (inizio) 178

RRT: Esempio (Esplorazione) 179

RRT: Esempio (Ricerca) 180

RRT: Esempio (Ricerca) 181

RRT: Algoritmo Algorithm Build. RRT Input: Initial configuration qinit, number of vertices in RRT K, incremental distance Δq) Output: RRT graph G G. init(qinit) for k = 1 to K qrand ← RAND_CONF() qnear ← NEAREST_VERTEX(qrand, G) qnew ← NEW_CONF(qnear, Δq) G. add_vertex(qnew) G. add_edge(qnear, qnew) return G 182

RRT: Ostacoli 183

RRT per Planning 184

RRT Orientato al Gol 185

RRT per Planning 186

RRT 187

RRT: Algoritmo 188

RRT: Planning and Replanning 189

RRT: planning and replanning 190

ERRT: RRT esteso 191

ERRT: replanning 192

ERRT: replanning con waypoint 193

Prestazioni 194

RRT* § Neighbor search and rewiring tree operations. § Search: Best parent node for the new node before its insertion in tree (within the area of a ball with radius r). § Rewiring: rebuilds the tree within this radius of area k to maintain the tree with minimal cost between tree connections 195