SLAM. Simultaneous Localization and Mapping. KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann

Transkript

1 SLAM Simultaneous Localization and Mapping KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann

2 Simultaneous Localization And Mapping SLAM Problematik SLAM Arten SLAM Methoden: (E)KF SLAM GraphSLAM Fast SLAM KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 2

3 SLAM - Problematik Ziel: Karte und Pose bestimmen Fragen: Wie sieht die Welt aus? Wo bin ich? Bekannt: Kontrolldaten, z.b. Odometriedaten (Daten des Vortriebssystems) z t Sensordaten (erfasste Landmarken) ut KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 3

4 Probleme Odometrie Positionsmessung ungenau auf Grund von: Bodenbeschaffenheit Verschleiß Ungleichmäßige Gewichtsverteilung etc KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 4

5 Probleme durch Sensordaten Ungenauigkeit der Sensoren Noise (Rauschen) Gleichartigkeit der Landmarken (Landmarken mit gleichen Signaturen) Robot pose uncertainty KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 5

6 Pfad- und Kartenfehler Pfadfehler und Kartenfehler korrelieren! KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 6

7 Statische Welt Markov Annahme (Markov Assumption) Alle Landmarken sind unbeweglich Außer dem Roboter gibt es keine weiteren bewegliche Elemente Aktueller Zustand ist eine komplette Zusammenfassung der Vergangenheit (und kann dadurch aus dieser ermittelt werden) KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 7

8 Einführung der Variablen zt ut Messung (Beobachtung) zum Zeitpunkt t Kontrolldaten für Zeitpunkt t -> alle Daten im Intervall ]t-1,t] z t1:t 2 Menge aller Messungen (Sensordaten) von Zeitpunkt t1 bis t2 u t1:t 2 Menge aller Kontrolldaten von Zeitpunkt t1 bis t2 xt m State, der Zustand des Systems zum Zeitpunkt t (durch die Annahme der statischen Welt beschränkt sich der Zustand auf die Pose des Roboters) Map, die Karte KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 8

9 Arten des SLAM-Problems Online SLAM: Schätzt aktuelle Pose und Karte p ( xt, m z1 : t, u1: t ) = p( x1: t, m z1: t, u1: t ) dx1dx2... dxt 1 Integrationen werden normal schrittweise durchgeführt Full SLAM: Schätzt den ganzen Pfad und Karte p( x : t, m z1: t, u1 : 1 t ) KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 9

10 Online SLAM: Graphisches Modell p ( xt, m z1 : t, u1: t ) = p( x1: t, m z1: t, u1: t ) dx1 dx2... dxt KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 10

11 Full SLAM: Graphisches Modell p( x : t, m z1: t, u1 : 1 t KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 11 )

12 (E)KF - SLAM Verwendet (Extended) Kalman Filter Online SLAM Verfahren Nutzt nur positive Landmarken-Sichtungen Annahme: Rauschen (Noise) ist normal verteilt KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 12

13 Kalman Filter Bayes Filter Von Swerling (1958) und Kalman (1960) entwickelt Belief wird durch den Erwartungswert μt und die Kovarianz Σt repräsentiert KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 13

14 Kalman Filter: Annahmen 1. Markov Assumption 2. Wahrscheinlichkeitsfunktion der Zustandsübergänge ist linear mit normal verteiltem Rauschen 3. Wahrscheinlichkeitsfunktion der Messungen ist ebenfalls linear mit normal verteiltem Rauschen 4. Initial belief bel(x0) ist normal verteilt KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 14

15 Komponenten des Kalman Filters A t B t C t Matrix (nxn), die beschreibt, wie sich der Zustand, ohne Kontrolldaten oder Rauschen, von t nach t-1 entwickelt. Matrix (nxm), die beschreibt, wie die Kontrolldaten u t den Zustand von t nach t-1 ändern. Matrix (kxn), die beschreibt, wie der Zustand x t auf eine Beobachtung z t abgebildet wird. ε δ t t Zufallsvariablen, die das Rauschen abbilden. Sie sind unabhängig und normalverteilt mit Covarianz R t and Q t KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 15

16 Kalman Filter Algorithmus 1. Algorithm Kalman_filter( µ t-1, Σ t-1, u t, z t ): 1. Vorhersage: 2. µ t = A t µ t 1 + Btut T 3. Σ t = A t Σ t 1At + R 4. Korrektur: T T 5. Kt = Σ tct ( Ct Σ tct + Qt ) 6. µ t = µ t + K t ( zt Ct µ t ) 7. Σ t = ( I KtCt ) Σ t t 1 Vorhersage auf Grund der Kontrolldaten Korrektur anhand der Beobachtungen (Sensordaten) 8. Return µ t, Σ t KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 16

17 Extended Kalman Filter Problem: Kalman Filter benötigt Linearität In der Realität ist diese Linearität häufig nicht gegeben. Lösung: Linearisierung Die nichtlineare Funktion g(x) wird durch ihre Tangente am Erwartungswert der Normalverteilung linearisiert KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 17

18 EKF: Linearisierung KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 18

19 Kombinierter Zustandsvektor Der kombinierte Zustandsvektor besteht aus der Pose des Roboters und der Karte mit N Landmarken. Die Pose des Roboters besteht aus x und y Koordinaten sowie der Ausrichtung des Roboters. Die N Landmarken werden durch ihre Koordinaten und ihre Signatur im Vektor repräsentiert KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 19

20 EKF-SLAM Algorithmus Teil 1: Motion Update KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 20

21 EKF-SLAM Algorithmus Teil 2 7ff: Iteration über alle Messungen Messung = Entfernung, Richtung, Signatur 9-11: Initialisierung neuer (bisher unbekannter) Landmarken KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 21

22 EKF-SLAM Algorithmus Teil 3 Berechnung der erwarteten Messung KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 22

23 EKF-SLAM Algorithmus Teil 4 17: Berechnung des Kalman-Gain 18,19: Filter-Update KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 23

24 EKF-SLAM: Beispiel Beispielfahrt eines Roboters in einer Welt mit 8 Landmarken Mit jedem Schritt steigt die Ungewissheit über die Pose des Roboters sowie der Lage der Landmarken an KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 24

25 EKF-SLAM Beispiel Erst die erneute Sichtung der ersten Landmarke führt zu einer Abnahme der Unsicherheiten (für alle Landmarken und die Pose des Roboters) KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 25

26 General EKF SLAM Korrespondenz oft unbekannt Erweiterung des Algorithmus nötig Statt einer gegebenen Korrespondenz wird die ML-Korrespondenz (maximum likelihood correspondence) verwendet Dem Algorithmus wird statt der Korrespondenz die momentane Größe der Karte übergeben KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 26

27 EKF-SLAM Beispiel KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 27

28 GraphSLAM Full SLAM Verfahren Full SLAM Problem formt einen sparse Graph (= Graph mit wenigen Kanten) Lazy SLAM (Datenerhebung und -verarbeitung werden getrennt durchgeführt) Graph wird reduziert bis er nur noch Roboterposen enthält KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 28

29 GraphSLAM KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 29

30 GraphSLAM Algorithmus: Hauptschritte 1. initialize 2. repeat 3. linearize 4. reduce 5. solve 6. until convergence Initialisierung des Posenerwartungswertvektors Erstellung von Informationsmatrix und -vektor Reduzieren der Matrix und des Vektors Update μ (auch Erwartungswerte der Features) 7. return μ KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 30

31 GraphSLAM Illustration: Erstellung der Informationsmatrix KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 31

34 GraphSLAM Illustration: Reduzieren der Informationsmatrix KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 34

35 GraphSLAM Illustration: Reduzieren der Informationsmatrix KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 35

36 GraphSLAM Illustration: Ergebnis KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 36

37 Fast SLAM Nutzt Partikelfilter Direkte Anwendung von Partikelfiltern auf das SLAM-Problem nicht möglich exponentieller Aufwand Als Online oder Full SLAM Verfahren einsetzbar KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 37

38 Fast SLAM Trick Bei gegebener Pose besitzt das Full SLAM Problem mit bekannter Korrespondenz eine bedingte Unabhängigkeit zwischen 2 disjunkten Featuremengen. Abhängigkeiten entstehen nur durch die Unsicherheit der Pose. Diese Beobachtung führt zur Möglichkeit Rao- Blackwellized particle filters einzusetzen KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 38

39 FastSLAM Trick Jeder Partikel behandelt einen ausgewählten Pfad Der Pfad wird ausgeklammert (je Partikel ein Pfad) und die Features werden behandelt als ob sie unabhängig wären Jedes Feature wird durch eine eigene Normalverteilung repräsentiert KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 39

40 FastSLAM Graph Features werden getrennt von einander betrachtet. Die Gesamtkarte m besteht aus Teilkarten m1 bis m KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 40

41 FastSLAM Partikel Menge von Kalman-Filtern für die Features KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 41

42 FastSLAM Algorithmus: Hauptschritte KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 42

43 FastSLAM Beispiel (grid-based) KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 43

44 FastSLAM Beispiel Video KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 44

45 Feature Elimiation FastSLAM kann die Abwesenheit von Features nutzen Dadurch können zweifelhafte/falsche Features (bewegliche Objekte die bei der ersten Beobachtung als Feature erkannt wurden) eliminiert werden KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 45

46 Feature Elimination Beispiel KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 46

47 Vergleich: FastSLAM & EKF KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 47

48 Quellen Literatur: Sebastian Thrun, Wolfram Burgard, Dieter Fox: Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The Mit Press, 2005, ISBN Abbildungen Website zum Buch KogSys-Sem-M2: Reading Club - SLAM - Andreas Habermann 48