Simultaneous Localisation And Mapping SLAM

Transkript

1 Simultaneous Localisation And Mapping SLAM

2

3

4 Sebastian Thrun, Gewinner der DARPA Grand Challenge 2005

5 SLAM Problemdefinition Ein mobiler Roboter erkundet eine unbekannte Umgebung von einer bekannten Startposition Die Bewegungen sind ungenau, d.h. Positionsabschätzung wird mit der Zeit immer ungenauer Der Roboter hat eine Sensorik mit der die Umgebung wahrgenommen werden kann Aufgabe: Erstellen einer Karte bei gleichzeitiger Lokalisierung in der Karte Einer der wichtigsten (teilweise noch ungelösten) Aufgaben in der Robotik

6 SLAM Mathematische Beschreibung des Problems Notation: Zeit t Posture x t =(x, y, ) Pfad x 1:t = {x 0, x 1,...,x t } Initialposture x 0 Odometrie u t, Bewegung von t 1! t Relative Bewegung u 1:t = {u 1, u 2,...,u t } wäre ausreichend, wenn es kein Rauschen gäbe True map m (Weltwissen) Annahme: Ein Messung pro Zeiteinheit Sensordaten z 1:t = {z 1, z 2,...,z t } x t 1 x t x t+1 u t 1 u t u t+1 z t 1 z t z t+1 m

7 SLAM Unterschiedliche Arten Full-SLAM (Offline-SLAM) Batch process, d.h. man berechnet die Karte und Trajektorie anschliessend, basierend auf allen Daten: Online-SLAM p( x 1:t, m {z } not observerd z 1:t, u {z 1:t ) } observerd Inkrementelle Berechnung der aktuellen Posture (wird auch Filter bezeichnet) p(x t, m z 1:t, u 1:t ) Benötigt 2 Modelle: p(x t x t 1, u t ) Odometriemodell p(z t x t 1, m) Sensormodell

8 Beispiel: SLAM mit Landmarken Annahme: Die Welt besteht aus N punktförmigen Landmarken, d.h. die Welt läßt sich vollständig durch 2N Werte beschreiben (2D) Der Roboter kann die relative Entfernung, den relativen Winkel und die Identität des Objekts bestimmen Definitionen: Exaktes (rauschfreies) Sensormodell Verrauschtes Sensormodell Bewegungsmodell ẑ t = h(x t, m) p(z t x t, m) N (h(x t, m), Q t ) ˆx t = g(x t 1, u t ) N (µ t, t )= N (µ t, t )= 1 p 2 2 exp 1 2 (x µ) p (2 )k det exp 1 2 (x µ)t 1 (x µ)

9 Beispiel: SLAM mit Landmarken Annahme: Die Welt besteht aus N punktförmigen Landmarken, d.h. die Welt läßt sich vollständig durch 2N Werte beschreiben (2D) Der Roboter kann die relative Entfernung, den relativen Winkel und die Identität des Objekts bestimmen Definitionen: Exaktes (rauschfreies) Sensormodell Verrauschtes Sensormodell Bewegungsmodell Verrauschtes Bewegungsmodell ẑ t = h(x t, m) p(z t x t, m) N (h(x t, m), Q t ) ˆx t = g(x t 1, u t ) p(x t x t 1, u t ) N (g(x t+1, u t ), R t ) N (µ t, t )= N (µ t, t )= 1 p 2 2 exp 1 2 (x µ) p (2 )k det exp 1 2 (x µ)t 1 (x µ)

10 Kategorisierungen in SLAM Annahmen und Methoden sollten explizit angeben werden (z.b. in einer Publikation, Buch, Implementierung, ) Volumetric vs. Feature-Based Volumenbasiert: erlaubt fotorealistische Rekonstruktionen der Umgebung Feature-based: z.b. Landmarken, Liniensegmente Topological vs. Metric Topologisch: Qualitative der Objektbeziehungen (Obj. A hinter Obj. B) Metrisch: Abstandsinformation Known vs. Unknown Correspondence Zuordnung von Sensordaten zu Objekten. Data Association Problem ist eines der schwierigsten Probleme im SLAM

11 Kategorisierungen in SLAM Static vs. Dynamic Statisch: Umgebung bleibt unverändert mit der Zeit Dynamisch: Veränderungen sind erlaubt. Aufwendiger, aber auch robuster Small vs. Large Uncertainty Algorithmen unterscheiden sich darin, wie gut sie Positionsungenauigkeiten vertragen Loop-closing Problem, wenn der Pfad des Roboters sich selbst kreuzt Active vs. Passive Passive: Roboter wird ferngesteuert Aktiv: Roboter exploriert eigenständig Single- vs. Multi-Robot Multi-Robots noch selten, aber bekommen mehr Aufmerksamkeit

12 Hauptansätze in SLAM Extende Kalman-Filter Historisches erstes Verfahren Sehr hoher Rechenaufwand Wird als eigenständiges Verfahren unwichtiger Iterative Closest Point Eines der ersten Verfahren (für 3D-Punktmengen) Graph-basierte Verfahren Hauptmethode für Full-SLAM Particle Filter Nonparametric statistical filtering FastSLAM scheint in diesem Bereich gerade zu dominieren Neuer Ansatz für das Data Association Problem

13 Iterative Closest Point (ICP) Andreas Nüchter

14 Iterative Closest Point (ICP) Gegeben sind zwei Punktmengen X = {x 1, x 2,...,x n }, x i 2 R n Y = {y 1, y 2,...,y n }, x i 2 R n Gesucht wird die Rotation R und die Translation t, die den Fehler E minimieren E(R, t) = 1 NX kx i Ry i tk 2 N i=1 wobei xi und yi korrespondierende Punkte sind.

15 . direkt Iterative Closest Point (ICP) Wenn die korrespondierenden Punkte bekannt sind, läßt sich die Lösung für x, y 2 R 3 berechnen: 1. Massenschwerpunkte der Punktmengen berechnen: µ x = 1 N NX x i und µ y = 1 N i=1 X 0 = {x i µ x } = {x 0 i } und Y 0 = {y i µ y } = {y 0 i } NX i=1 y i 2. Singulärwertzerlegung: W = NX i=1 xi 0 yi 0 W = U V T 3. Optimale Lösung: R = UV T, t = µ x Rµ y

16 Iterative Closest Point (ICP) Idee: Zugehörigkeiten (correspondences) iterative finden Varianten von ICP 1. Selection Verteilung der Punkte 2. Matching Auswahl der Zugehörigkeiten 3. Weighting Gewichtung der Zugehörigkeiten 4. Rejecting Auswahl / Ignorieren von Punkten 5. Metric Abstandsberechnung Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

17 Iterative Closest Point (ICP) Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

18 Iterative Closest Point (ICP) Selection 1. Alle verfügbaren Punkte 2. Uniformes sampling 3. Random sampling 4. Gradientenbasierte Auswahl (z.b. Farbe) 5. Auswahl basierende auf der Flächennormale Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

19 Iterative Closest Point (ICP) Selection Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

20 Random sampling Normal-space sampling Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

21 Iterative Closest Point (ICP) Matching 1. Punkte mit dem geringsten Abstand 2. Normal shooting 3. View point projection 4. Closest compatible point (z.b. Normalen, Farbe, Curvature, ) Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

22 Iterative Closest Point (ICP) Selection Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

23 Iterative Closest Point (ICP) Weighting 1. Konstantes Gewicht 2. Abstandsabhängiges Gewicht w 12 =1 Dist(p 1, p 2 ) max Dist(p i,p j ) 3. Basierend auf den Normalen w 12 = n 1 n 2 4. Rauschbasierte Gewichtung Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

24 Iterative Closest Point (ICP) Selection Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

25 Iterative Closest Point (ICP) Rejecting 1. Festgelegter Maximalabstand 2. Ablehnung der n% schlechtesten Matches (z.b. 10% der am weitesten entfernten Punkten) 3. Abstand kleiner als ein Vielfaches der Standardabweichung (z.b. 2.5σ) 4. Ablehnung, wenn benachbarte Punkte sich zu stark unterscheiden (x 1, y 1 ), (x 2, y 2 ): Dist(x 1, x 2 ) Dist(y 1, y 2 ) > max(dist(x i, x j ), Dist(y k, y l )), = Keine Punkte, die am Rand der Meshes liegen Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

26 Iterative Closest Point (ICP) Selection Efficient Variants of the ICP Algorithm, Rusinkiewicz, 2001

27 Iterative Closest Point (ICP) Metric 1. Summe der quadratischen Abstände zusammengehöriger Punkte (SVD) Point-to-point 2. Summe der Abstände, geometrisch und Farben 3. Summe der quadratischen Abstände zusammengehöriger Punkte, gewichtet mit der Flächennormalen Point-to-plane

28 SLAM Extended Kalman Filter (EKF SLAM) Metric, feature-based Verfahren Ein Vektor um den Zustand des Roboters und die Umgebung zu repräsentieren Wenn neue Features entdeckt werden, wird der Vektor erweitert Problem: Die Covariance-Matrix Q wächst quadratisch Repräsentation der Unsicherheit des Zustands erfolgt durch eine multivariate Gauss-Verteilung ŷ k = Ay k ˆP k = AP k 1 + Bu k H Abbildung der Sensordaten auf den Zustand A Modell des Systems B Modell der Kontrolle Q Covariance des Sensorfehlers R Rauschen der Sensoren konstant P Schätzung des Fehlers y Zustand u Steuerbefehle x Messung 1 A T + Q K = ˆP k H T (H ˆP k H T + R) 1 y k =ŷ k + K(z k Hŷ k ) P k =(1 KH)ˆP k aktualisiert Linearisierung des Modells A mit First-order Taylor Expansion (Jacobian Matrix) f (x) = 1X n=0 f (n) (a) n! (x a) n

29 EKF-SLAM p(x t, m z 1:t, u 1:t ) N (s t ; µ t, t ) µ t 2 R 3+2N Pose des Roboters + Position aller Objekte t 2 R (3+2N) (3+2N) Unsicherheit für jede Koordinate s t = {x t, x m1, y m1, x m2, y m2,...,x mn, y mn } µ t = {µ xt, µ m1,t, µ m2,t,...,µ mn,t} 2 xt xt m 1,t xt m 2,t xt m N,t m1 x t m1,t m1 m 2,t m1 m N,t t = mn x t mn,t 3 7 5

30 EKF-SLAM

31 EKF-SLAM s t = {x t, x m1, y m1 } µ t = {µ xt, µ m1,t} t = apple xt x t 1,t m1 x t m1,t

32 EKF-SLAM s t = {x t, x m1, y m1, x m2, y m2 } µ t = {µ xt, µ m1,t, µ m2,t} t = apple xt xt m 1,t xt m 2,t m1 x t m1,t m1 m 2,t

33 EKF-SLAM s t = {x t, x m1, y m1, x m2, y m2,...,x m7, y m7 } µ t = {µ xt, µ m1,t, µ m2,t,...,µ m7,t} 2 xt xt m 1,t xt m 2,t xt m 7,t m1 x t m1,t m1 m 2,t m1 m 7,t t = m7 x t m7,t 3 7 5

34 EKF-SLAM s t = {x t, x m1, y m1, x m2, y m2,...,x m7, y m7 } µ t = {µ xt, µ m1,t, µ m2,t,...,µ m7,t} 2 xt xt m 1,t xt m 2,t xt m 7,t m1 x t m1,t m1 m 2,t m1 m 7,t t = m7 x t m7,t 3 7 5

35 EKF-SLAM s t = {x t, x m1, y m1, x m2, y m2,...,x m7, y m7 } µ t = {µ xt, µ m1,t, µ m2,t,...,µ m7,t} 2 xt xt m 1,t xt m 2,t xt m 7,t m1 x t m1,t m1 m 2,t m1 m 7,t t = m7 x t m7,t 3 7 5

36 Montemerlo, Thrun, FastSLAM, 2007, Springer

37 Fast-SLAM Nutzt die strukturellen Eigenschaften aus, die in EKF-SLAM nicht beachtet werden Die Landmarken sind erst dann vollständig korreliert, wenn deren Positionen gesichert sind EKF macht davon keinen Gebrauch, da keine Annahmen über die Umgebung gemacht werden

38 FastSLAM SLAM Posterior: p(x t, m z 1:t, u 1:t, n 1:t ) FastSLAM Posterior: p(x 1:t, m z 1:t, u 1:t, n 1:t ) Erlaubt die Faktorisierung des Posteriors in einfachere Terme m1 z1 z3 x1 x2 x3 xt u1 u2 ut z2 zt m2 Die Position der Landmarken sind unabhängig voreinander, gegeben des Roboterpfads

39 Faktorisierung der Landmarken s t = {x t, x m1, y m1, x m2, y m2,...,x m7, y m7 } µ t = {µ xt, µ m1,t, µ m2,t,...,µ m7,t} t = { xt, m1,t, xt m 2,t,... m7,t}

40 Faktorisierung der Landmarken s t = {x t, x m1, y m1, x m2, y m2,...,x m7, y m7 } µ t = {µ xt, µ m1,t, µ m2,t,...,µ m7,t} t = { xt, m1,t, xt m 2,t,... m7,t} p(x 1:t, m z 1:t, u 1:t, n 1:t )=p(x 1:t z 1:t, u 1:t, n 1:t ) NY n=1 p(m n x 1:t, z 1:t, u 1:t, n 1:t )

41 FastSLAM Herleitung p(x 1:t, m z 1:t, u 1:t, n 1:t )=p(x 1:t z 1:t, u 1:t, n 1:t ) NY n=1 p(m n x 1:t, z 1:t, u 1:t, n 1:t ) Es gilt: p(x 1:t, m z 1:t, u 1:t, n 1:t )=p(x 1:t z 1:t, u 1:t, n 1:t )p(m x 1:t, z 1:t, u 1:t, n 1:t ) Zu zeigen: p(m x 1:t, z 1:t, u 1:t, n 1:t )= Zwischenschritt 1: NY n=1 p(m n x 1:t, z 1:t, u 1:t, n 1:t ) p(m nt x 1:t, z 1:t, u 1:t, n 1:t ) Bayes = p(z t m nt, x 1:t, z 1:t 1, u 1:t, n 1:t ) p(m nt x 1:t, z 1:t 1, u 1:t, n 1:t ) p(z t x 1:t, z 1:t 1, u 1:t, n 1:t ) p(m nt x 1:t, z 1:t, u 1:t, n 1:t )= p(z t m nt, x 1:t, n 1:t ) p(z t x 1:t, z 1:t 1, u 1:t, n 1:t ) p(m n t x 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 ) p(m nt x 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 )= p(z t x 1:t, z 1:t 1, u 1:t, n 1:t ) p(z t m nt, x 1:t, n 1:t ) Zwischenschritt 2: p(m nt x 1:t, z 1:t, u 1:t, n 1:t ) p(m n6=nt x 1:t, z 1:t, u 1:t, n 1:t )=p(m n6=nt x 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 )

42 FastSLAM Beweis per Induktion Induktionshypothese für t-1: p(m x 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 )= NY n=1 p(m n x 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 ) Trivial für t = 1: Da keine Beobachtungen gemacht wurden ist die Faktorisierung richtig. Für t > 1: p(m x 1:t, z 1:t, u 1:t, n 1:t )= p(z t m, x 1:t, z 1:t 1, u 1:t, n 1:t ) p(z t x 1:t, z 1:t 1, u 1:t, n 1:t ) p(m x 1:t, z 1:t 1, u 1:t, n 1:t ) [Markov] [Induktionshypothese] [Zwischenergebnisse] = = p(z t m nt, x t, n t ) p(z t ) x 1:t, z 1:t 1, u 1:t, n 1:t ) p(m x 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 ) p(z t m nt, x t, n t ) p(z t ) x 1:t, z 1:t 1, u 1:t, n 1:t ) = p(m nt x 1:t, z 1:t, u 1:t, n 1:t ) = NY NY n=1 NY n6=n t p(m n x 1:t, z 1:t, u 1:t, n 1:t ) p(m n x 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 ) n6=n t p(m n x 1:t, z 1:t, u 1:t, n 1:t )

43 FastSLAM Faktorisierung Erlaubt die Schätzung der Position und Umgebung, ohne die Korrelationen zwischen den Landmarken explizit zu beachten Es werden nur N + 1 Filter benötigt, jeder mit einer erheblichen geringeren Komplexität (Dimension) Erlaubt die Anwendung des Particle Filters für SLAM Jedes Particle enthält den Pfad des Roboters und ein EKF für jede Landmarke

44 FastSLAM Particles & Algorithmus Particle 1: x, y, θ μ1, Σ1 μ2, Σ2 μn, ΣN Particle 2: x, y, θ μ1, Σ1 μ2, Σ2 μn, ΣN Particle M: x, y, θ μ1, Σ1 μ2, Σ2 μn, ΣN 1. Bestimme (sample) neue Roboterpose für jedes Partikel gemäß der Kontrolle u 2. Updaten der Landmarken-EKFs für die aktuellen Sensordaten z 3. Berechnen der Importance Weights für jedes Particle 4. Updaten der Partikel aufgrund der Importance Weights

45 FastSLAM Neue Roboterpose Importance Sampling: Verwenden einer bekannte Verteilung zum Generieren von neuen Particles (Hypothesen), weil wir die eigentliche Verteilung nicht kennen Gewichten die neuen Partikels nach der Aussagekraft, die sie haben Proposal Distribution: x [m] t p(x t u t, x [m] t 1 ) Bewegungsmodell kann jede Form (z.b. nicht-linear) annehmen Üblicherweise: v 0 t N (v t, 1 v t + 2 )! 0 t N (! t, 3! t + 4 )

46 FastSLAM Landmarken Update Landmarkenschätzungen EKF repräsentiert p(m n x 1:t, z 1:t, u 1:t, n 1:t ) werden als low-dimensional p(m n x 1:t, z 1:t, u 1:t, n 1:t )= p(z t m nt, x 1:t, z 1:t 1, u 1:t, n 1:t )p(m nt x 1:t, z 1:t 1, u 1:t, n 1:t ) (1) p(z t m nt, x t, z t, n t ) h(x t, m nt ) = p(z t m nt, x t, z t, n t ) {z } (1) G mn t p(m nt x 1:t 1, z 1:t 1, u 1:t {z 1, n 1:t 1 ) } (2) ẑ t = h(x [m] t, µ nt,t 1) = r h(x mn t t, m nt ) xt =x [m] t ;m n t =µ[m] nt,t 1 h(x t, m nt ) ẑ t + G m (m nt µ [m] n t,t 1 ) p(z t m i, x t, n t ) N (z t,ẑ t + G m (m nt µ [m] n t,t 1 ), R t)

47 FastSLAM Landmarken Update (2) p(m i s 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 ) N (m nt ; µ [m] n t,t G mn t ẑ t = h(x [m] t, µ nt,t 1) = r h(x mn t t, m nt ) xt =x [m] t ;m n t =µ[m] nt,t 1 Z n,t = G mn t [m] n t,t 1 G T m t + R t K t = [m] n t,t 1 G m T Z 1 n t n,t µ [m] n t,t = µ [m] n t,t 1 + K(z t ẑ t ) [m] n t,t =(I K t G mn t ) [m] n t,t 1 1, [m] n t,t 1 ) ŷ k = Ay k ˆP k = AP k 1 + Bu k 1 A T + Q K = ˆP k H T (H ˆP k H T + R) 1 y k =ŷ k + K(z k Hŷ k ) P k =(1 KH)ˆP k h(x t, m nt )= apple r(xt, m nt ) (x t, m nt ) = " p (mnt,y x t,x ) 2 +(m nt,y x t,y ) 2 tan 1 mn t,y m n t,x x t,y x t,x x t,m # G mn t = " mn t,x x t,x p q m n t,y x t,y p q m n t,y q x t,y m n t,x q x t,x q =(m nt,x x t,x ) 2 +(m nt,y x t,y ) 2 #

48 FastSLAM Importance Weights Proposal Distribution: Desired Posterior: p(x [m] 1:t z 1:t 1, u 1:t, n 1:t 1 ) p(x [m] 1:t z 1:t, u 1:t, n 1:t )! [m] t = target distribution proposal distribution = [m] p(x 1:t z 1:t, u 1:t, n 1:t ) p(x [m] 1:t z 1:t 1, u 1:t, n 1:t 1 ) Zähler mit Bayes entwickeln (Normalisierung erfolgt beim Resampling)! [m] t = p(z t x [m] 1:t, z 1:t 1, u 1:t, n 1:t )p(x [m] 1:t z 1:t 1, u 1:t, n 1:t ) p(x [m] 1:t z 1:t 1, u 1:t, n 1:t 1 ) = p(z t x [m] 1:t, z 1:t 1, u 1:t, n 1:t )p(x [m] 1:t z 1:t 1, u 1:t, n 1:t 1 ) p(x [m] 1:t z 1:t 1, u 1:t, n 1:t 1 ) = p(z t x [m] 1:t, z 1:t 1, u 1:t, n 1:t )

49 FastSLAM Importance Weights Berechnung der Importance Weights! [m] t = 1 1 p 2 Znt,t exp 2 (z t ẑ nt,t) T (Z nt,t) 1 (z t ẑ nt,t) x 1:t Abhängigkeit vom Roboterpfad kann ignoriert werden, da alle Funktionen für das Update der Landmarken nur von xt abhängen Data Association Problem: ˆn t =argmax n t p(z t n t,ˆn 1:t 1, x 1:t, z 1:t 1, u 1:t )

50 input: X t 1,z t,r t,u t X t = X aux = {} for m =1toM do retrieve m-th particle hx [m] t from X t 1 draw x [m] t p(x t s [m] t 1,u t) for n =1toN [m] t 1 do ẑ t = h(x [m] t,µ nt,t 1) G mnt 1,N[m] t 1,µ[m] 1,t 1, [m] = r mnt h(x t,m nt ) xt =x [m] t ;m nt =µ [m] n t,t 1 Z n,t = G mnt [m] n t,t 1,t 1,...,µ[m] N [m] t 1 1 GT m t + R t p [m] 1 n,t = p exp 1 2 Znt,t 2 (z t end for ẑ nt,t) T (Z nt,t) 1 (z t ẑ nt,t) p [m] N [m] t 1 +1,t 0 ˆn t = arg max n p [m] n,t or draw ˆn t with prob. p [m] n,t for m =1toM do draw random particle from X aux with prob.! [m] t add new particle to X t end for,t 1, [m] N [m] t 1,t 1i t n n,t t if ˆn t = N [m] t N [m] t µ [m] 1 +1then = N [m] t 1 +1 ˆn t,t = h 1 (x [m] t, ẑˆnt,t) [m] ˆn t,t =(GT m nt R 1 G mnt ) 1 else N [m] t = N [m] t 1 Kˆnt,t = [m] ˆn t,t 1 GT m nt Zn,t 1 µ [m] ˆn t,t = µ[m] ˆn t,t 1 + Kˆn t,t(z t ẑˆnt,t) [m] ˆn t,t =(I end if for n =1toN [m] t if n 6= ˆn t then µ [m] ˆn t,t = µ[m] ˆn t,t 1 [m] ˆn t,t = [m] ˆn t,t 1 end if end for! [m] t = p [m] ˆn t,t Kˆn t,tg mnt ) [m] ˆn t,t do add new particle to X aux end for

51 FastSLAM

52 FastSLAM

53 Graph-based SLAM Offline, Full-SLAM, feature-based Algorithmus Idee Position und Landmarken sind Knoten in einem Graph Jeder Übergang von xt-1 nach xt (Roboterposition) wird durch Kanten in dem Graphen repräsentiert Jede Messung eines Objekts mi wird ebenfalls mit dem Zustand xt durch eine Kante verknüpft, wenn mi zum Zeitpunkt t beobachtet wurde

54 Graph-based SLAM m1 x1 m1 x1 x1 m1

55 Graph-based SLAM m1 x1 x2 x3 x4 m1 m2 m3 m4 x1 x1 x2 x2 x3 x4 m1 m2 m3 m4

56 Graph-based SLAM m1 m2 x1 x2 m1 m2 x1 x1 x2 x2 m1 m2

57 Graph-based SLAM m1 m2 x1 x2 x3 x4 m1 m2 m3 m4 x1 x1 x2 x3 x2 x3 x4 x4 m1 m3 m2 m4 m3 m4

58 Graph-based SLAM Stellen uns den Graphen als ein Feder-Massen-System vor Lösung des SLAM Problems entspricht dem Gleichgewichtszustand (minimale Energie des Systems) Graph korrespondiert zu dem Logarithmus der Posterior-Verteilung: log p(x 1:t, m z 1:t, u 1:t ) Ohne Herleitung: log p(x 1:t, m z 1:t, u 1:t ) = const [Kante im Graph: Bewegung des Roboters] + X t log t p(x t x t 1, u t ) [Kante im Graph: Sensormessung] + X t log t p(z t x t, m) SLAM: Maximierung der Gleichung x 1:t, m =argmax x 1:t,m log p(x 1:t, m z 1:t, u 1:t )

59 Graph-based SLAM log p(x 1:t, m z 1:t, u 1:t ) = const + X t [x t g(x t 1, u t )] T R 1 t [x t g(x t 1, u t )] + X t [z t h(x t, m)] T Q 1 t [z t h(x t, m)] Lösungen kann durch Standardmethoden gefunden werden, z.b. Gradientenverfahren, Simulated Annealing

60 ATLAS Hybrides SLAM Verfahren Graphenbasiertes, metrischen Verfahren Funktioniert mit Scan-matching und Feature-based SLAM-Verfahren Explicit Loop-closing via effizientes Map-Matching Verfahren Grundlegende Idee: Knoten sind lokale Koordinatensysteme Kanten beschreiben die Translationen zwischen den Knoten und die Unsicherheiten eines Knoten in Bezug des anderen Simultaneous Localization and Map Building in Large-Scale Cyclic Environments Using the Atlas Framework, Bosse, Newman, Leonard, Teller, 2004

61 ATLAS Algorithmus In jeder Iteration des Algorithmus müssen 4 Schritte ausgeführt werden 1. Local Map Iteration Lokales SLAM-Verfahren um Daten zu sammeln 2. Hypothesis State Transition Generieren und testen von Hypothesen über den aktuellen Zustand 3. Map Projection Iteration Aktualisieren der lokalen Map-unsicherheiten und -positionen 4. Map Matching Loop-closing / Reduction

62 ATLAS Komponenten 1. Uncertainty Projection 2. Competing Hypotheses 3. Generation of new maps 4. Loop closing 5. Instantiation and evaluation of new maps

63 ATLAS Uncertainty Projection In einem zyklischen Graph existieren mehrere Pfade zwischen Knoten Für die Berechnung der Unsicherheiten wird der kürzeste Pfad genommen, d.h. der Pfad mit der geringsten projizierten Unsicherheiten B D A C

64 ATLAS Genesis Jede lokale Map hat eine maximale Komplexität In den Anwendungen sind das maximal 15 Positionen oder Features Der Graph muß daher durch neue Maps erweitert werden 1. Aktuelle Position des Roboters definiert den Ursprung des neuen Koordinatensystems 2. Posture wird auf Null gesetzt 3. Unsicherheit wird auf Null gesetzt 4. Unsicherheit des neuen Koordinatensystems wird relativ zur Source angeben

65 ATLAS Map Matching / Loop-closing In zyklischen Umgebungen, werden Bereiche mehrfach besucht Map-matching bedeutet, dass man diese Bereiche automatisch identifiziert 3 Schritte: 1. Durch die Uncertainty Projection erhalten wir eine Liste von möglichen Matches 2. Für jede der möglichen Matches, wird die Ähnlichkeit bestimmt 3. Update des Graphen

66 ATLAS Feature-based Map Matching Erstellen einer Signatur, die invariant gegenüber Transformationen ist d 1 d 1 d 2 d 3 1 A 0 d 1 d 2 d 3 d 4 1 C A Self-Matching: Entfernen von repetitive Features (z.b. Türen in einem Flur) Vergleich & Zuordnung: Jeder Match beschreibt eine mögliche Transformation zwischen den beiden Maps Jede Transformation wird evaluiert, nach der Anzahl der Features, die in Einklang gebracht werden Die besten n Transformationen ergeben die Gesamttransformation & Covariance Matrix ij = E [( X i µ i )(X j µ j )]

67 ATLAS Cycle Verification Zwei mögliche Probleme beim Schliessen von Zyklen 1. False Positives führt zu falschen Loops 2. False Negatives führt zu Loops, die nicht geschlossen werden Ansatz: Entscheidung eine Loop zu schliessen wird erst dann getroffen, wenn genug Informationen vorhanden sind Schliessen von kleinen Loops wird bevorzugt

68 ATLAS Cycle Verification

69 ATLAS Traversal with Competing Hypotheses Wenn der Roboter eine Map verlässt, muss entweder einer anderen, angrenzenden Map zugeordnet werden oder es muss eine neue Map erzeugt werden Die Entscheidung geschieht aufgrund von Hypothesen, die mit den Sensordaten verglichen werden Es wird zu jedem Zeitpunkt eine Menge von Hypothesen verwaltet, um die Position in der Map zu bestimmen Es gibt vier Arten von Hypothese, Dominant, Mature, Juvenile, Retired

70 ATLAS Hypotheses states Mature Map: Kann den Status angrenzender Maps auf Juvenile setzen Kann neue Maps erzeugen Dominant Map: Mature Map mit der besten Metric (gegeben der Sensordaten) Ausgabe des ATLAS Frameworks Juvenile Map: Testet wie gut eine angrenzende Map die aktuellen Messungen erklären kann Wird nach einer festen Zeit (z.b. 2 Sekunden) gelöscht Retired Map: Mature Maps, die die aktuellen Daten nicht erklären können Können als Juvenile Map reaktiviert werden Markieren inaktive Bereiche der Karte Wenn es nur eine Mature Map gibt, die aber die Daten nicht erklären kann, wird eine neue Map erzeugt Juvenile Dominant Mature Retired

71 ATLAS Traversal Generation Prior Map-Matching Prior Posterior Posterior Hypothesenauswahl Prior Loop-closing Prior Posterior Posterior

72 ATLAS Erstellen einer globalen Karte Simultaneous Localization and Map Building in Large-Scale Cyclic Environments Using the Atlas Framework, Bosse, Newman, Leonard, Teller, 2004

73 ATLAS Erstellen einer globalen Karte Simultaneous Localization and Map Building in Large- Scale Cyclic Environments Using the Atlas Framework, Bosse, Newman, Leonard, Teller, 2004

74 ATLAS Erstellen einer globalen Karte Simultaneous Localization and Map Building in Large- Scale Cyclic Environments Using the Atlas Framework, Bosse, Newman, Leonard, Teller, 2004

75

76 SLAM Offene Probleme Dynamische Umgebungen Multi-robot SLAM Standardimplementierungen auf den Niveau industrieller Anwendungen. Beispiel OpenCV Long-term SLAM (Tage, Wochen, Monate, Jahre)