Lokalisierung Mobiler Agenten

Behandelter Stoff

Übersicht

Statische Lokalisierung

Slides: 20160517-lma-1

Hessische Normalform (Hesse normal form)

An equation describing a plane: $$x \cdot n - d = 0$$ where $x$ is a point, $n$ is the unit normal vector of the plane and $d$ is the distance of the plane to the origin.

Methode der kleinsten Quadrate (Least Squares Methode)

Bei der Lokalisierung mobiler Agenten in einem Raum mit 4 Wänden genügen zwei nicht-parallele Abstandsmessungen. Hat man mehr Messungen, so kann man mit der Methode kleinster Quadrate die beste Position bestimmen.

Der Fehler ist dann $e = y - H x$.

Damit definiert man das Gütemaß $$G(x) = (y- Hx)^T W^{-1} (y-Hx)$$ wobei $W^{-1}$ eine symmetrische, positiv definite Gewichtungsmatrix ist.

Rekursive Methode kleinster Quadrate

Wenn bereits einmal die beste Schätzung bestimmt wurde, will man nicht wieder alles neu berechnen, wenn ein neues Sensorergebnis hinzukommt. Bei uns mit vektorwertigen Sensoren, nicht wie im Skript mit Skalaren.

Slides: 6. Vorlesung

Sherman-Morrison-Woodbury-Formel

Für zwei $n\times k$-Matrizen $U, V$ gilt:
Die $k\times k$-Matrix $E-V^T A^{-1}U$ sei regulär, dann gilt $$(A-UV^T)^{-1} = A^{-1}+A^{-1}U(E-V^TA^{-1}U)^{-1}V^T A^{-1}$$

Dynamische Lokalisierung

Kalman-Filter

Siehe Kalman-Filter-Artikel.

SLAM

SLAM (Simultaneous Localization and Mapping)

SLAM ist ein Algorithmus, welcher zugleich eine Karte erstellt und einen mobilen Agenten auf der Karte lokalisiert. Eine Karte sind Positionen von Landmarken.

Ansätze:

• Filterung (rekursiv): z.B. Kalman-Filter; EKF, UKF (sigma-Punkte / samples; einfach zu implementieren und liefert häufig bessere Ergebnisse als EKF), Partikelfilter (Wahrscheinlichkeitsdichten werden durch Partikel / samples repräsentiert)
  $\Rightarrow$ Schätzen für Zeitschritt
• Smoothing / Glättung (batch): Nachteile (Rechenaufwendig, erst nach batch ist Ergebnis da, man muss Daten speichern) und Vorteile (komplette Trajektorie, optimale Schätzung selbiger); z.B. Graph-based SLAM

Loop Closure: Wenn der Agent wieder an einen Punkt kommt, an dem er bereits war, kann ein Kreis geschlossen werden. Daher können die vorherigen Landmarken im Kreis neu geschätzt werden. Allerdings kann ein falsches Loop Closure zur Divergenz des Filters führen.

Herausforderungen:

• Datenassoziation: Erkennen, ob Landmarken die gleichen sind.
• Komplexität (große Karte)
• Veränderung der Umgebung: z.B. Auto als Landmarke erkannt; neue Brücke kommt hinzu oder wurde abgerissen
• Nichtlinearitäten des Problems

Landmarke

Ein gut sichtbares / wiedererkennbares Objekt, welches zur Abstandsmessung benutzt werden kann.

EKF SLAM

EKF SLAM basiert auf dem Extended Kalman Filter. Vereinfachungen:

• Problem in 2D
• Feste Zahl von punktförmigen Landmarken
• Assoziationen bekannt: Landmarken werden zuverlässig erkannt und nicht verwechselt.

Zustand $\mathbf{x}_k = [\underbrace{x_k, y_k, \theta_k}_{\text{Pose des Roboters}}, \underbrace{m_{1, x}, m_{1, y}}_{\text{LM 1}}, \dots, \underbrace{m_{M, x}, m_{M, y}}_{\text{LM } M}]$
Dimension von $\mathbf{x}_k$: $3 + 2 M$.
Kovarianz hat $(3+2M)^2$ Elemente, d.h. der Speicheraufwand ist quadratisch in der Zahl der Landmarken.

Systemgleichung: $x_{k+1} = a_k (x_k, M_k) + w_k$
$x_{k+1} \approx A_k x_k + w_k,$ wobei $A_k$ Jacobi-Matrix von $a_k(x_k, u_k)$ für festes $u_k$ an der Stelle $\hat{x}_k^e$.

Messgleichung: $y_k = h(x_k) + v_k$
In der Regel sind nicht alle Landmarken zugleich sichtbar. In diesem Fall werden Zeilen in der Messabbildung weggelassen. $$y_k = H_k(x_k) + v_k,$$ wobei $H_k$ die Jacobi-Matrix von $h(x_k)$ an der Stelle $\hat{x}_k^P$ ist.

Particle Filter (Partikelfilter, Sequential Monte Carlo, SMC)

Idee: Schätze Zustand über Partikel. Ansatz: Sample aus Transitionsdichte $$f(x_{k+1} | x_k = p_k^1, u_k$$ Implementierung:

• Rauschen $w_k$ samples $\rightarrow v_k^1, \dots, v_k^N$
• Propagiere durch $x_{k+1} = a_N(x_N, u_N) + w_N$, d.h. $p^1_{k+1} = a_k (p_k^1, u_k) + v_k^1$
• Gewichte bleiben gleich

Problem: Viele Partikel $p_k^i$ haben geringes Gewicht $w_k^i$
Lösung: Resampling mit sequential importance resampling (SIR: ziehe $N$ neue Partikel mit Gewichten $\frac{1}{n}$ aus Menge der alten Partikel, wobei die Wahrscheinlichkeit, das Partikel $p_k^i$ zu ziehen, gerade $w_k^i$ ist.)

Material und Links

Die Vorlesung wurde gestreamt und ist unter mml-streamdb01.ira.uka.de verfügbar.

• Vorlesungswebsite

Literatur und Links:

• SLAM-Vorlesung der Uni Freiburg (Material)

Vorlesungsempfehlungen

Folgende Vorlesungen sind ähnlich:

• Analysetechniken großer Datenbestände
• Informationsfusion
• Machine Learning 1
• Machine Learning 2
• Mustererkennung
• Neuronale Netze
• Lokalisierung Mobiler Agenten
• Probabilistische Planung

Termine und Klausurablauf

Es ist eine mündliche Prüfung.