Self Localization and Mapping (SLAM) mit Lidar- oder Kamera

Autor: Martin Berysztak
Betreuer: Prof. Schneider

Aufgabenstellung

Inbetriebnahme Hardware an einem Lego Mindstorms NXT oder EV3
Regelung eines Fahrzeugs in der Mitte eines Ganges
Kartografierung der Umwelt
Rückfahrt auf dem schnellsten Weg
Kartografierung der Strecke
Optimierung der Fahrparameter (Geschwindigkeit, optimaler Kurs)
Dokumentieren Sie Ihre Vorgehensweise fortlaufend wissenschaftlich.

SLAM Theorie

Formulierung des SLAM Problems
Ein mobiler Roboter baut eine Karte in einem unbekannten Gebiet gleichzeitig weiß er immer wo er sich gerade befindet. Der Roboter muss zur Lösung dieses Problems über die Möglichkeit verfügen mithilfe von Sensorik Landmarken zu ermitteln.

Das Szenario wird durch folgendes Modell beschrieben:

Das Modell wird in Zeitschritten $k=1...m$ angegeben. Zu jedem Zeitpunkt k sind folgende Sachen bekannt:

$x_k$ - Roboterpose: Position und Orientierung(Blickfeld) des Roboters

$u_k$ - Steuersignal: Angestrebte Position - Orientierungsänderung beim Übergang von $k-1$ zu $k$

$m$ - Menge $m=\lbrace m_i \mid i=1=n \rbrace$ Tatsächliche Position aller Landmarken

$z_k$ - Menge $z_k=\lbrace z_{ik} \mid i=1:n \rbrace$ Zum Zeitpunkt $k$ beobachtete Position der Landmarke relativ zum Roboter, wobei $z_{ik}$ die jeweils beobachtete Position einer Landmarke $i$ ist.

Zum Zeitpunkt $k$ sind dadurch dann folgende Werte bekannt:

$x_{0:k}=\lbrace x_0, ..., x_k \rbrace$ Roboterpose
$u_{0:k}=\lbrace u_0, ..., u_k \rbrace$ Steuersignal
$z_{0:k}=\lbrace z_0, ..., z_k \rbrace$ Die Beobachtete Landmarken-Position bis zum Zeitpunkt $k$ .

Zustandsübergang: Allgemein ist alles definiert. Nun wird die Zustandsänderung also die Bewegung des Roboters beschrieben. Hierbei ist zu beachten, dass jede Bewegung Odometriefehler zur Folge hat. Odometriefehler können viele Ursachen haben, wie zum Beispiel ein Hügel oder ein Burnout der Reifen. Dadurch ist jede Odometriemessung unsicher.
Um diese Unsicherheit zu bekämpfen wird das Modell in probabilistischer Form beschrieben. Probalistisch sagt, dass eine Wahrscheinlichkeitsaussage über das bestehende Modell bzw. den nächsten Zeitpunkt $k$ und die damit verbundenen Werte wie die neue Roboterpose $x_k$ getätigt wird. Eine Wahrheitsverteilung für die Zustandsübergange wird aufgestellt und beschrieben durch.

$\Rho \left( x_k,m \mid z_{0,i:k},u_{0,i:k},x_0 \right)$

Die Position und Orientierung des Roboters $x_k$ ist verbunden mit der gesehenen Landmarken Positionen $m$ durch:

$x_0$ - Startposition des Roboters
$u_{0,i:k}$ - Die Vorgeschichte aller Steuersignale
$z_{0,i:k}$ - Die Vorgeschichte aller Landmarken Positionen relativ zum Roboter

Das Bedeutet zu jedem Zeitpunkt $x_k$ fließen die kompletten Werte aller vergangenen Zeitschritte mit der selben Gewichtung ein. Dies führt zu einem hohen Rechenaufwand und langen Rechenzeiten bei längeren Messungen. Wünschenswert wäre ein rekursives Modell.Dazu kann einen neues Beobachtungsmodell dienen, welches die Wahrscheinlichkeit, eine bestimmte Beobachtung zu machen.

$\Rho \left( z_k \mid x_k, m \right)$ Die Beobachtung der Landmarken Position hängt von der Roboterpose und der tatsächlichen Position der Landmarke ab.
$\Rho \left( x_k \mid x_{k-1}, u_k \right)$ Die Roboterpose hängt ab von der letzten Position und dem Steuersignal ab.

Um diese Wahrscheinlichkeit zu lösen wird ein zweistufiges-rekursives Verfahren benötigt:

Schritt 1: Schätzung der Verbundverteilung für die Roboterpose und der Landmarken Position unter der Nutzung des Bewegungsmodells:

$\Rho \left( x_k,m \mid z_{0:k-1},u_{0:k},x_0 \right)\;=\; \int \Rho \left( x_k \mid x_{k-1},u_k \right) \cdot \Rho \left( x_k, m \mid z_{0:k-1},u_{0:k-1},x_0 \right) \cdot d_{x-1}$

Schritt 2: $\Rho \left( x_k,m \mid z_{0:k},u_{0:k},x_0 \right)\;=\; \frac{\Rho \left( z_k \mid x_{k},m \right)\Rho \left( x_k,m \mid z_{0:k-1},u_{0:k},x_0 \right) }{\Rho \left(z_k \mid z_{0:k-1}, u_{0:k} \right)}$

Extended Kalman-Filter

Um das zuvor beschriebene Problem zu lösen, werden in der Literatur viele verschiedene Methoden genannt. In diesem Abschnitt wird der theoretische Lösungsansatz mithilfe des Erweiterten Kalman-Filters beschrieben. Auf die genaue Funktionsweise des Kalman-Filter wird nicht eingegangen.

Das einfache Kalman-Filter lässt sich nur auf lineare Systeme anwenden. Aus diesem Grund wird ein erweiteter Kalman-Filter eingesetzt.
Das Extended Kalman-Filter verwendet zum lösen des Problems die aufgenommen Odometriedaten und die gemessenen Daten der Umgebung, in diesem Fall Lidardaten. Aus der Kombination dieser zwei Daten lässt sich eine Positionsschätzung angeben. Diese Schätzung dient dazu die Unsicherheit zu verringern. Das System driftet über die Zeit immer weiter ab, dadruch wird die berechnete Position immer ungenauer. Dieses Driften wird mit dem EKF unterbunden.

Das Kalman-Filter setzt beim zuvor beschriebenen Zustandsübergang also beim bewegen des Roboters ein. Dieser Übergang teilt sich dann in zwei Schritte:

1.Vorhersageschritt: Die aktuellen Odometriedaten werden für die Positionsschätzung verwendet. Dadurch entsteht eine neue Positionsschätzung wo der Roboter sich jetzt befinden sollte. Diese Schätzung ist aufgrund der fehleranfälligen Odometriedaten.

2.Korrekturschritt: In diesem Schritt werden die Daten des Lidarsensors zur Positionsbestimmung mit ein. Die Merkmale (Landmarken) aus vergangenden Messungen werden mit der aktuellen Messung abgeglichen. Die Differenz dieser Merkmaldaten wird bestimmt. Anhand dieser Differenz wird die zuvor geschätzte Position korrigiert.

Mathematischer Hintergrund: Das Kalman-Filter schätzt folgende Parameter zum Zeitpunkt $k$ mit Hilfe des zweistufigen-rekrusiven Verfahrens, welches zuvor beschrieben wurde:

$\mu_k$ - Erwartungswert
$\sum$ _k - Kovarianz einer Normalverteilung

Weil das erweiterte Kalman-Filter ein nicht-lineares Problem linealisiert entsteht ein Genauigkeitsverlust. Unter Verwendung eines Zustandsraummodells werden hierbei Störungen und Unsicherheiten durch Gaußverteilungen approximiert. Dadurch sind folgende Dinge bekannt:

$f \left( x_{k-1},u_k \right)$ - Bewegungstransformation.
$\omega_k$ - Gauss'sche Störung

Daraus folgt, dass sich die Roboterpose wie folgt darstellen lässt:

$\Rho \left( x_k \mid x_{k-1}, u_k \right) \approx x_k = f\left(x_{k-1},u_k \right)+\omega_k$

Das Beobachtungsmodell lässt sich ebenfalls als Summe einer Koordinatentransfomation darstellen $h\left( x_k,m \right)$ . Zur der Koordinatentransformation muss eine Gauss'sche Störung $v_k$ dazu addiert werden. Die Beobachteten Landmarken Positionen lassen sich so darstellen:

$\Rho \left( z_k \mid x_k, m \right) \approx z_k = h\left(x_k,m \right)+v_k$ - Fehlerloser Fall

Mithilfe der Funktion $h\left(\right)$ lassen sich die Landmarken $m$ aus dem globalen Koordinatensystem der Karte in die Koordinaten der aktuellen Roboterpose $x_k$ übertragen.

EKF linearisieren: Wie oben bereits erwähnt handelt es sich bei diesem Modell um ein Nicht-lineares Modell. Das Modell wird beim EKF durch die Tayler Näherung erster Ordnung linearisiert.

CAS Robot Navigation Toolbox

Anforderung

Besuch der Veranstaltung Praxisseminar

Empfohlene Zusatzkurse

Matlab/Simulink

Siehe auch

Weblinks

Literatur

→ zurück zum Hauptartikel: Studentische Arbeiten