SLAM. Navigationskonzepte für autonome Fahrzeuge

Inhaltsverzeichnis

Kurzfassung

1 Einleitung
1.1 Problemstellung

2 Überblick über SLAM
2.1 Gemeinsame Begriffe
2.1.1 Inertialnavigation (Dead Reckoning)
2.1.2 Landmarken
2.1.3 Kalman Filter
2.2 Landmarkenfindung
2.2.1 Pattern Matching
2.2.2 SPIKES
2.2.3 RANSAC - Random Sample Consensus
2.3 EKF - SLAM
2.4 FAST SLAM
2.5 Weitere Entwicklungen

3 EKF - SLAM auf AICC
3.1 Landmarkenfindung
3.1.1 RANSAC zur Wanderkennung
3.1.2 Ermittlung der Eckpunkte
3.2 Extended Kalman Filter
3.2.1 Aufbau und Inhalt der Filtermatrizen
3.2.2 Vorhersage (state prediction)
3.2.3 Datenassoziation
3.2.4 Korrektur (state correction)
3.2.5 Hinzufügen neuer Landmarken

4 Erkenntnisse aus der Implementierung

Abbildungsverzeichnis

Literatur- und Quellenverzeichnis

Erkl¨arung

Kurzfassung / Summary

SLAM – Navigationskonzepte für autonome Fahrzeuge

Die Arbeit widmet sich mit dem ”Synchronous Localization and Map-ping“(kurz SLAM) einer grundlegenden Problematik der autonomen Robo-ter – Navigation. Neben der Problematik selbst werden einige Konzepte zur L¨osung des Problems vorgestellt. Das grundlegende Standardkonzept des EKF - SLAMs wird detaillierter behandelt. Diese genauere Analyse zielt auf eine Implementierung auf dem ”Artificial Intelligence Concept Car“ (kurz AICC) Projektprototypen der im Projektjahr aus einer vollstandigen Uber-arbeitung des TTcars entstanden ist. Die Implementierung dieses Navigati-ons - Konzepts wird in dieser Arbeit ebenso dokumentiert, wie auch die Er-kenntnisse und etwaige Weiterführungs- und Verbesserungsvorschlage für weiterführende Projekte.

Suchbegriffe: Robotik, Navigation, SLAM, Kalman Filter, Extendet Kalman Filter, RANSAC, AICC

SLAM – Navigation - Concepts For Autonomous Vehicles

Synchronous Localization and Mapping (SLAM) is a central problem in autonomous navigation. There is a wide range of methods to deal with this problem. After reviewing various possible solutions, this thesis discusses the choice of the method EKF – SLAM for the development of the Artificial Intelligence Concept Car (AICC) which was built in the Bachelor’s project. The discussion includes the implementation of this navigation concept and the evaluation of the results. Some suggestions for improvements and future projects are outlined.

Key words: Robotic, Navigation, SLAM, Kalman Filter, Extendet Kal­man Filter, RANSAC, AICC

1 Einleitung

Ferngesteuerte Fahrzeuge und Maschinen sind heutzutage keine Seltenheit mehr, ebenso wenig exotisch sind Maschinen, die einem genau vordefinier-tem Programm folgen und damit auch Fahrzeuge die einem gewissen ge-nau abgesteckten Pfad folgen. Ferngesteuerte Systeme benotigen eine zu-verl¨assige Verbindung und die Aufmerksamkeit eines Benutzers, und Skript - gesteuerte Fahrzeuge sind sehr unflexibel gegen Veränderungen. In beiden Fällen wird das Problem: ”Wie komme ich von Punkt A nach Punkt B“ nicht vom System selbst gelost, sondern im Vorhinein durch den, der den Losungsweg beschrieben hat (Skript) oder durch denjenigen der die Fern-steuerung bedient. Ein geskriptetes Verhalten scheidet also dann aus, wenn das Umfeld in dem sich das Fahrzeug nicht genau bekannt bzw. ständigen ¨Anderungen unterworfen ist und eine Fernsteuerung dann, wenn eine Re­mote - Verbindung zu teuer bzw. schwierig zu realisieren (z.B. in Bergwer-ken, unter Wasser) oder zu langsam(z.B. auf dem Mars) ist oder wenn es nicht zweckmäßig ist, jede Aktion von einem Benutzer abhängig zu ma-chen(z.B. Staubsauger - Roboter).

Die notige Flexibilität erhält man erst dann, wenn das System in der La-ge ist, das Problem: ”Wie komme ich von Punkt A nach Punkt B“ selbst zu losen. Dieses Problem enthält zwei verschiedene Problemstellungen. Ein Problem ist das finden moglicher Pfade (engl. pathfinding), die Routenpla-nung, die mit diversen Wegfindungsalgorithmen (z.B. A* und D* Algorith-mus) behandelt werden kann.

Die zweite Problemstellung ergibt sich aus den Voraussetzungen dieser Algorithmen, da sie ein konsistentes Bild der Umgebung benotigen, die eigene Position darin und die Position des Zielorts. Erschwerend kommt hinzu, dass das sonst omnipräsente GPS (Global Position System) nur bei freier Sichtverbindung zu genugend Satelliten verwendbar ist. Das ist z.B. in Gebäuden nicht der Fall und bedeutet, dass die Positionierung oft auch ohne dieses System auskommen muss.

Ein autonomes mobiles Robotersystem muss also, um navigieren zu kon-nen, uber eine Karte seines Umfelds verfugen und seine Position darauf be-stimmen konnen. Sollte keine Karte (oder eine veraltete, fehlerhafte Karte) vorhanden sein, muss ein wirklich autonomes System in der Lage sein, Kar-tenmaterial selbst zu erstellen und zu aktualisieren. Um jedoch mit den Informationen aus der Sensorauswertung brauchbare Karten zeichnen zu konnen muss das System wissen an welche Stelle der Karte zum Beispiel das neu erfasste Hindernis einzutragen ist. Somit muss dass System auch während des Erstellens und Wartens der Karte immer seine aktuelle Posi­tion kennen. Diese Problemstellung wird in Fachkreisen meist mit SLAM ( S imultaneous L ocalization A nd M apping) bezeichnet.

1.1 Problemstellung

Das Projektfahrzeug ”Artificial Intelligence Concept Car“ (kurz AICC) soll, um seinem Namen einigermaßen gerecht zu werden, in der Lage sein sich selbstständig in Gebäuden zurecht zu finden, Wege zu gegebenen Zielen finden und ein Abbild seiner Umgebung zu erzeugen. Da für diesen Robo-ter gilt, dass kein GPS zur Verfügung steht, ist eine L¨osung des SLAM - Problems ein sehr wichtiger Schritt zur Umsetzung der vorgegebenen Ziele.

Abbildung in dieser Leseprobe nicht enthalten

Abbildung 1.1: Artificial Intelligence Concept Car

2 ¨Uberblick über SLAM

Zuerst nochmal der Hinweis, dass SLAM keinesfalls einen speziellen Algo-rithmus betitelt, sondern die Problemstellung an sich. Weiters haben Pro-bleme meist an sich, dass es mehr als eine mogliche Losung gibt. Es gibt ei-ne Vielzahl von Konzepten mit unterschiedlichsten Implementierungen, die sich durch die eingesetzte Sensorik und das jeweilige Umfeld unterscheiden, in dem der Roboter navigieren soll. Um sich nicht in Sensorik - Konzepten zu verlieren werden nur einige Beispiele für verschiedene Moglichkeiten, bezüglich der Zusammenführung und Verarbeitung der Sensorinformation genannt.

2.1 Gemeinsame Begriffe

Die verschiedene Navigationskonzepte bauen oft auf den selben Grundbe-griffen auf, deshalb ist es zielführend diese Begriffe zu Beginn zu erläutern. Dieselben Begriffe werden auch in der Literatur [Mon] [Blas] beschrieben. Auf die folgenden Begriffe wird auch bei der Beschreibung der Konzepte zurückgegriffen.

2.1.1 Inertialnavigation (Dead Reckoning)

Der Begriff der Inertialnavigation(engl. dead reckoning), oder auch Koppel-navigation, beschreibt die Positionsbestimmung relativ zu einer zuvor be-stimmten Position(engl. fix) anhand des Kurses, der gefahrenen Geschwin-digkeit und der Fahrzeit. Dieses Verfahren wurde vor der Einführung des GPS in der Schifffahrt und in Flugzeugen eingesetzt, wobei der Kurs an-hand eines Kompasses bestimmt wurde und für die Geschwindigkeitser-mittlung anhand eines Fahrtmessers mussten auch Wind und Stromung berücksichtigt werden. Heute wird es in der Raumfahrt eingesetzt, dort je-doch werden Beschleunigungssensoren ausgewertet. Weitere Anwendungen finden sich Flugzeugen, die auf diese Weise kurzzeitig die Position zwi-schen GPS - Stützstellen bestimmen und bei fortschrittlicheren Navigati-ons - Systemen die so einen kurzzeitigen Ausfall oder Genauigkeitsverlust durch Tunnel oder Gebäude überbrücken. Diese Positionsbestimmung an-hand der Bewegung von einem vorher bestimmten Punkt ist ein wichtiger Bestandteil in vielen SLAM -Konzepten. In der Robotik werden fallweise elektronische Kompasse, Drehraten - Sensoren oder auch eine Kombination aus beidem eingesetzt um die Bewegungsrichtung zu bestimmen. Wegstre-cke oder Geschwindigkeit werden meist aus den Inkrementalgebern der An-triebsmotoren ermittelt. Auf diese Weise l¨asst sich mit ausreichend genauer Sensorik bereits für gewisse Anwendungen eine genügend genaue Naviga­tion realisieren.

Die gesamte Methodik beinhaltet jedoch einen grof3en Nachteil, da sich die Fehler der einzelnen Messungen aufsummieren. Ein Fehler der Inkre-mentalgeber - Auswertung von beispielsweise 1% der gefahrenen Wegstre-cke kann bereits zur folge haben, dass ein Roboter, der sich 10m von einer Wand weg bewegt und dann wieder zurück, 20cm hinter der Wand ste-hen bleiben würde. Kompasse sind naturgemäf3 anfällig gegen magnetische Störeinflüsse, während Drehraten - Sensoren sogar eine zeitabhängige Ab-weichung haben. So würde ein Roboter sogar am Stand mit der Zeit jegli-che Kenntniss über die Ausrichtung verlieren. Um also noch einmal auf die Schiffsnavigation zurückzukommen, dort stellte man dafür an gefährlichen Küsten Leuchttürme auf. Die Leuchttürme dienten als Anhaltspunkt um präzise zwischen gefährlichen Riffen hindurch schiffen zu können. Das Prin-zip des Leuchtturms führt geradewegs zum nächsten wichtigen Begriff in der Roboter - Navigation.

2.1.2 Landmarken

Ein Leuchtturm ist tatsächlich ein gutes Beispiel für eine Landmarke, denn unter Landmarken versteht man Orientierungspunkte in der Umgebung die verwendet werden, um eine genauere Positionsbestimmung zu realisieren. Anhand der Idee des Leuchtturms können die Voraussetzungen erläutert werden, die eine Landmarke erfüllen muss, um für SLAM einsetzbar zu sein. Man stelle sich vor, dass ein Schiff sich einem gefährlichen Riff nähert und dieses durch Orientierung am Leuchtturm umschiffen will, doch der Leuchtturm befindet sich plötzlich an einer anderen Stelle. Der Navigator geht davon aus, dass dieser Leuchtturm an der richtigen Stelle steht und kann sich auch nicht vom Gegenteil überzeugen, weil er auf3er dem Licht in der Ferne nichts sieht(siehe Abbildung (2.1)). So wird diese Fahrt wohl ein schreckliches Ende haben, weil das Schiff möglicherweise zielsicher auf das Riff zusteuert. Ebenso könnte es ausgehen, wenn das Licht des Leuchtturms erlöschen ist und der Navigator keinen Anhaltspunkt hat.

Der Versuch zwei Leuchttürme aufzustellen, falls das Licht eines Turms erlischt, könnte den Navigator jedoch in die nächste Zwickmühle führen. Angenommen einer der beiden Leuchttürme fällt aus, dann kann der Navi­gator möglicherweise nicht feststellen, welchen der beiden er noch sieht und navigiert ins Unheil, weil er die falschen Position verwendet. Diese Türme müssten weit genug voneinander entfernt sein, dass er sie mit Hilfe seiner

Abbildung in dieser Leseprobe nicht enthalten

Abbildung 2.1: Falsche Landmarkenpositionen bewirken Positionsfehler

vorherigen Kursberechnungen unterscheiden kann.

Anhand der Rolle des Leuchtturms in der Seefahrt erarbeitet muss eine

brauchbare Landmarke also folgende Vorgaben erfüllen:

- Die Position der Landmarke muss genau genug bekannt sein und darf sich nicht zufällig verändern, sie sollte stationär sein
- Es müssen ausreichend Landmarken auffindbar sein um die Position genau genug ermitteln zu konnen
- Die Landmarke muss eindeutig und ohne Verwechslungen wieder-erkennbar und wiederauffindbar sein

An sich ist es egal und von Implementierung zu Implementierung ver-schieden, was genau als Landmarke verwendet wird, solange die oben ge-nannten Bedingungen erfüllt werden. Es kann sich je nach Anwendung und eingesetzter Sensorik um Wände, Ecken, bestimmte Muster in Kamera-bildern, Funkfeuer (z.B. auch umliegende WLAN - Access Points) oder bei Nostalgikern sogar tatsächlich um Leuchttürme handeln.

2.1.3 Kalman Filter

Folgt man dem roten Faden, dann stellt sich die Frage, wie Informationen aus dem Dead Reckoning und aus den Landmarken zusammengeführt wer-den sollen, um die jeweiligen Stärken nutzen zu konnen. Zu diesem Zweck werden aufwändige Filteralgorithmen verwendet, die meist in irgendeiner Form auf dem sogenannten Kalman Filter aufbauen. Hierbei handelt es sich um einen adaptiven Filteralgorithmus der bereits 1960 von Rudolf Emil Kálmán in [Kal] publiziert wurde. Dieses Filter kann zur Zustandser-mittung (engl. state estimation) linearer Systeme verwendet werden, die durch starkes gausssches weißes Rauschen nur schwer beobachtbar sind. Das Kalman Filter lässt sich gut rekursiv implementieren und benotigt im Gegensatz zum Wiener Filter, der alle vorherigen Messwerte benotigt, nur die Informationen des vorherigen Zustands (engl. state). So wächst die Berechnungszeit nicht mit der Anzahl der Iterationen.

Grundlegend lasst sich jede Kalman Iteration in zwei Schritte untertei-len. Der erste Schritt beinhaltet eine Voraussage (engl. state prediction) des aktuellen Zustandes anhand der bereits gewonnenen Informationen über das System ohne Messung des aktuellen Zustandes (a priori). Ist die Vor-hersage über den aktuellen Zustand getroffen wird im zweiten Schritt der Messwert aufgenommen und die Abweichung von der Vorhersage (engl. in­novation) ermittelt. Wie stark diese innovation der aktuellen Messung in die Zustandsbestimmung eingeht, hangt anfangs vom Initialisierungswert ab, danach davon wie lange das System bereits beobachtet wird bzw. wie genau dem Filter das Systemverhalten bereits bekannt ist. Dieser zweite Schritt beinhaltet also die Korrektur (engl. state correction) der Zustands-voraussage mit der neu erhaltenen Information aus der Messung. Eine gute Einführung in diese Thematik bietet [Wel]. Dem Kalman Filter widmet auch [Hay] ein Kapitel, wahrend [Blas] und das Kapitel [3.2] dieser Arbeit den Extended Kalman Filter für die spezielle Anwendung im EKF - SLAM behandeln.

2.2 Landmarkenfindung

2.2.1 Pattern Matching

Beim Pattern Matching wird versucht Muster aus einer Messung deckungs-gleich über Muster in vorherigen Messungen zu legen. Aus den dafür notigen Verschiebungen und Drehungen lasst sich dann die Verschiebung und Dre-hung der Roboters zwischen den beiden Messungen ermitteln. So einfach sich diese Methode auch anhort, so schwierig kann die Implementierung sein. Die erste Schwierigkeit ergibt sich schon aus der Natur der Sensorik, die immer einen gewissen Messfehler mit sich bringt. Dieser Messfehler be-wirkt, dass es praktisch keinen Sinn macht nach 100% ¨Ubereinstimmung zu suchen, da dies in den seltensten Fallen eintreten wird. So muss immer ermittelt werden wie grof3 die ¨Ubereinstimmung ist. Es muss eine Mindest - ¨Ubereinstimmung erarbeitet werden, ab der davon ausgegangen werden kann, das selbe Muster wiedergefunden zu haben. Jedoch darf dieses Mi­nimum nicht zu klein angesetzt werden, denn dann besteht Gefahr zu Ver-wechslungen. Auch nicht zu vernachl¨assigen ist der Resourcen - Bedarf die-ser Methode, da einerseits komplette Messungen gepuffert werden müssen, mit denen verglichen werden kann. Andererseits müssen die Muster aus der neuen Messung mit denen in mehreren gespeicherten Messungen ver-glichen werden, um eine grof3ere Robustheit und Genauigkeit zu erzielen. Zu alledem sollten die verwendeten Muster selbst aussagekraftig und nicht zu leicht verwechselbar sein und sich nicht zufallig bewegen, um den Krite-rien einer Landmarke zu entsprechen. Die Auswahl passender Muster durch das System setzt naturgemaf3 die Fahigkeit voraus Muster in der Messung richtig interpretiern zu konnen.

Es ist ist hilfreich die Auflösung der Messung zu reduzieren indem man ein gröberes Raster verwendet (z.B. 10cm * 10cm Quadrate) und den Inhalt jedes Rasterelements mit frei von Hindernissen oder blockiert beschreibt (engl. occupancy grid). So verliert man zwar Informationen über feinere Strukturen die Sensorik aufnehmen kann, reduziert jedoch beträchtlich den Aufwand für weitere Berechnungen. Ein geringer Informationsverlust stellt meist kein Problem dar, da bei geeigneter Rasterauflösung die Information über die Gröf3e und Grundrissform eines Raums erhalten bleibt und nur Detailinformationen über die genaue Form der Objekte und damit über kleine passierbare Bereiche und enge Fugen verloren gehen. Es ist nahelie-gend die Karte im selben Rasterformat anzulegen und die Ausschnitte aus den gerasterten Messungen vereinfacht gesagt in Puzzlemanier an die pas-sende Stelle zu setzen. Ein Vorteil dieser Methode, die meist als grid based SLAM bezeichnet wird, ist, dass mit diesem occupancy grid ein mit raster-basierten Wegfindungsalgorithen gut handhabbares Kartenformat vorliegt. Als Beispiel für diese Alternative zu Landmarken - basierten Konzepten sei [Grz] genannt.

2.2.2 SPIKES

Im Gegensatz zum Pattern Matching trifft man bei Verwendung des SPIKES - Algorithmus eine Auswahl von Punkten, die zur Orientierung verwen-det werden. Dieser Algorithmus sucht nach zum Roboter ragenden Spitzen (engl. spikes). Bei Verwendung eines horizontal scannenden LIDAR - Scan­ners wie am AICC liefert dieser Algorithmus in einem leeren Raum kaum Punkte, während es in einem Raum mit Möbeln und anderen Gegenstände oder bei einem U - Boot am schroffen Meeresboden nur so vor potentiellen Landmarken wimmelt. Der Rechenaufwand für SPIKES ist recht gering, da es sich nur um eine Ermittlung von Extremwerten handelt, doch die gelie-ferten Punkte sind mit Vorsicht zu genief3en, da der SPIKES eine Vorliebe für Türen, Stühle und vor allem Personen hat, die durch den Scan spazie-ren. Die in Abschnitt [2.1.2] aufgelisteten Vorgaben erfüllt ein Möbelstück nur solange es nicht verrückt wird und ein Passant nur, wenn er starr wie eine Salzsäule in der Gegend herumsteht.

2.2.3 RANSAC - Random Sample Consensus

Der Random Sample Consensus - Algorithmus wird unter anderem in der Bildverarbeitung verwendet, um automatisiert Modelle durch Ermittlung der Parameter über Datensätze zu fitten, die Ausreif3er beinhalten. Diese Aufgabe wird gelöst indem zuerst zufällig Messwerte (engl. random samp­les) ausgewählt werden. Dann wird das Modell auf diesen kleinen Aus-schnitt angepasst und diese Hypothese wird geprüft. Der gesamte Daten-satz wird nun durchlaufen und es wird geprüft wie viele Messwerte im Datensatz das Modell unterstützen (engl. inlier), indem untersucht wird, ob der Abstand des jeweiligen Wertes zum Modell nicht grof3er ist als eine zuvor angegebene Toleranz. Wenn das untersuchte Modell von ausreichend vielen Messwerten unterstützt wird, also der consensus set ausreichend grof3 ist, wird angenommen ein gutes Modell gefunden zu haben. Erreicht der consensus set nicht diese ebenfalls zuvor schon festzulegende Grof3e ist das Modell ungeeignet um den Datensatz zu beschreiben und wird verwor-fen. Die beschriebenen Schritte sollten mehrmals ausgeführt werden, um zu Ergebnissen zu kommen. Eine Moglichkeit ist es, solange nach einem consensus set zu suchen bis eine gewisse Anzahl von Modellen gefunden wurde, aber damit riskiert man, dass sich bei einem schlecht strukturier-ten Datensatz eine Endlosschleife bildet. Die sicherere Variante ist es, die Abarbeitung in eine for – Schleife zu platzieren und eine fixe Anzahl von RANSAC – Durchläufen (engl. iterations) anzugeben. Wie viele Iteratio-nen ausreichend sind ist von Applikation zu Applikation verschieden und kann empirisch ermittelt oder mit Kenntnis der Grof3e des Datensatzes und dem Ausreif3eranteil berechnet werden (siehe [Hart]). Weiters sollte noch erwähnt sein, dass der RANSAC ein schlechtes Laufzeitverhalten hat, da sowohl die Berechnungszeit für eine Iteration als auch die Anzahl der not-wendigen Iterationen, bei gleichem relativem Ausreif3eranteil, linear mit der Grof3e des Datensatzes zunehmen. Unterm Strich ergibt das ein quadrati-sches Laufzeitverhalten.

Unter Verwendung des wohl einfachsten Modells, der Gerade, erweist sich der RANSAC laut [Blas] als gute Wahl für den Einsatz in Gebäuden. Man hat die Moglichkeit aus Datensätzen von LIDAR – Scannern Wände zu ermitteln, die sich gut als Bezugspunkte für SLAM – Implementierungen eignen. Die Robustheit des Algorithmus gegen Ausreisser bewirkt, dass ei-ne Wand auch erkannt wird, wenn sie abschnittsweise verdeckt oder durch offene Türen und Gänge unterbrochen ist. Weiters ist es komplett unerheb-lich die Wand durch ein stillstehendes Objekt teilweise verdeckt ist, oder durch eine Person, die vorbeigeht. Also bringt der RANSAC im Gegensatz zu SPIKES bereits eine gewisse Robustheit gegenüber Storungen durch bewegte Objekte mit sich.

[...]