Bachelorarbeit. Ein online SLAM Ansatz basierend auf der Extended Kalman Filter Lokalisierung

Transkript

1 ISSN Nr. ZAI-BSC Bachelorarbeit im Studiengang Angewandte Informatik an der Georg-August-Universität Göttingen Ein online SLAM Ansatz basierend auf der Extended Kalman Filter Lokalisierung An online SLAM approach based on Extended Kalman Filter Localization angefertigt von Daniel Kalin aus Celle am III. Physikalisches Institut 10. November 2015

2

3 Georg-August-Universität Göttingen Drittes Physikalisches Institut Friedrich-Hund-Platz Göttingen Germany +49 (551) / (551) office-dpi@phys.uni-goettingen.de Erstgutachter: Zweitgutachter: Betreuer: Prof. Dr. Florentin Wörgötter Prof. Dr. Konrad Rieck Simon Reich, Jan-Matthias Braun, Michael Fauth

4

5 Ich erkläre hiermit, dass ich die vorliegende Arbeit selbständig verfasst und keine anderen als die angegebenen Quellen und Hilfsmittel verwendet habe. Göttingen, den 10. November 2015

6

7 vii Zusammenfassung Diese Arbeit beschäftigt sich mit der simultanen Lokalisierung und Kartenerstellung und präsentiert einen online SLAM Ansatz basierend auf der Extended Kalman Filter Lokalisierung und dem Sensor Forward Modell zur Kartenerstellung. Es wird der Frage nachgegangen, ob der Einsatz des entwickelte Grid2DSLAMs eine zuverlässige Erkennung der Umgebung mit nur einem Ultraschallsensor als Distanzsensor ermöglicht. Für diesen Zweck wird der mobile Roboter WheelPi konstruiert. Der entwickelte Grid2DSLAM ist für den Betrieb auf Mehrkernprozessoren konzipiert und ermöglicht somit im Vergleich zu konventionellen SLAMs eine separate Lokalisierung und Kartenerstellung, trotz der gegenseitigen Abhängigkeit voneinander. Bei der Kartenerstellung wird eine Gitterkarte der Umgebung mit Hilfe des Sensor Forward Modells erstellt. Dabei dient das Raycasting-Verfahren zur Gitterzellendetektion und der EM-Algorithmus wird zur Ermittlung der Gitterzellenbelegung genutzt. Die Kartenerstellung im Grid2DSLAM ist daher nicht wie viele andere SLAM-Verfahren Landmarken basiert. Die ermittelte Gitterkarte wird dann in der EKF-Lokalisierung zur Bestimmung der Roboterposition verwendet. Um die Funktionsweise des Grid2DSLAMs zu untersuchen, wurde die entwickelte Software auf dem WheelPi Roboter in zwei verschiedenen Umgebungen getestet. Die experimentellen Ergebnisse zeigen, dass die Darstellung einer kleinen Umgebung mit vereinzelten Hindernissen und die Lokalisierung durch den Einsatz des Grid2DSLAMs auf dem WheelPi Roboter grundsätzlich möglich ist. Aufgrund der zur Laufzeitminimierung getätigten Einschränkungen im EM-Algorithmus ist jedoch die genaue Abbildung der Umgebung in der Gitterzellenkarte derzeit noch nicht möglich. Auch eine Erkundung von weitläufigen Umgebungen ohne Hindernisse konnte durch die Arbeit nicht realisiert werden. Da dies jedoch der Verwendung von nur einem Ultraschallsensor geschuldet ist, kann zusammenfassend festgehalten werden, dass durch das in dieser Arbeit entwickelte online SLAM-Verfahren das Konzept der simultanen aber separaten Lokalisierung und Kartenerstellung in Echtzeit realisiert wurde.

8

9 INHALTSVERZEICHNIS I Inhaltsverzeichnis 1 Einleitung 1 2 Grundlagen der Lokalisierung und Kartenerstellung Lokalisierung Bewegung des Roboters Positiontracking Extended Kalman Filter Kartenerstellung Inverse Sensor Modell Forward Modell EM-Algorithmus Grid2DSLAM EKF-Lokalisierung Messfehlermodell Umsetzung des Extended Kalman Filters Kartenerstellung mit dem Forward Modell Raycasting-Verfahren zur Gitterzellendetektion EM-Implementierung WheelPi Roboter Aufbau des WheelPi Roboters MovingPi Bibliothek Realisierung der WheelPi-Steuerungssoftware Einstellungen des Grid2DSLAMs Experimentelle Ergebnisse EKF-Lokalisierung Kartenerstellung Erstelldauer der Umgebungskarte Diskussion 59 7 Fazit und Ausblick 63 A Anhang 65

10 II INHALTSVERZEICHNIS A.1 WheelPi Bauteile A.2 Vorbereitung des Dagu Arduino Mini Driver A.3 Installation und Einrichtung des WheelPi Roboters A.4 Grid2DSLAM Konfiguration des WheelPi Roboters Literatur 71

11 1 1 Einleitung Mobile Roboter werden im Alltag immer populärer. Von autonomen Rasenmähern und Staubsaugern über Paket ausliefernde Drohnen bis hin zu Servicerobotern zur Pflege von kranken und älteren Menschen werden Roboter entwickelt, die selbstständig Aufgaben erledigen, ohne dass eine manuelle Steuerung notwendig ist. Folglich ist für viele Aufgaben erforderlich, dass sich der Roboter in der Umgebung zurechtfindet und seine Position ihm stets bekannt ist. Eine weit verbreitete Technik zur Positionsbestimmung ist das GPS [1, 2]. Auch wenn die Technik in den letzten Jahren immer genauer geworden ist, eignet sie sich nur für eine grobe Ortung. Roboter, die in geschlossenen Räumen agieren, haben oft nur einen eingeschränkten oder keinen Zugang zu einem GPS-Signal. Damit Roboter auch ohne GPS zurechtkommen, werden auch andere Techniken eingesetzt, wie das Positionstracking [3]. Dabei spielt das Messen der Roboterbewegung eine große Rolle. Anhand dieser können ausgehend von einer Initialposition, die nachfolgenden Positionen des Roboters bestimmt werden. Da die Roboterbewegungen nicht fehlerfrei mit Sensoren gemessen werden können, kommt es mit der Zeit zu einem immer größeren Fehler zwischen der bestimmten Position des Roboters und der realen Position. Das kann z. B. bei einem staubsaugenden Roboter dazuführen, dass er einen Raum nicht vollständig reinigt, oder ein Serviceroboter annimmt, dass er bereits beim Patienten ist, obwohl er sich noch im Nebenraum befindet. Um diesen Fehler zu reduzieren, werden Messwerte verschiedener Sensoren kombiniert. So werden neben Bewegungssensoren, wie einem Accelerometer zum Messen der Beschleunigung und Gyroskop zum Messen der Drehgeschwindigkeit, z. B. Distanzsensoren eingesetzt. Häufig kommen dabei Ultraschallsensoren oder Laserentfernungsmesser zum Einsatz [4, 5]. Die Distanzsensoren ermöglichen dem Roboter, Entfernungen zu Hindernissen zu messen und durch Abgleich mit einer Umgebungskarte seine Position relativ zur Karte zu bestimmen. In der Umgebungskarte sind alle Hindernisse der Umgebung abgebildet, sodass die von den Sensoren erfassten Hindernisse in der Karte erkannt werden können. Solche Umgebungskarten müssten vorab erstellt und dem Roboter zur Verfügung stehen. Eine Möglichkeit ist, diese Karte manuell zu erzeugen, was bei unbekannten Umgebungen nicht möglich ist. Für den Nutzer der Roboter ist es umständlich, zunächst eine Karte des

12 2 1 EINLEITUNG Raumes zu erstellen, bevor der Roboter eingesetzt werden kann. Demzufolge wäre eine automatische Erstellung ohne Eingriff vom Anwender anwenderfreundlicher. Bei der Erstellung der Umgebungskarte durch den Roboter besteht die Schwierigkeit darin, dass die örtliche Bestimmung der Hindernisse immer nur relativ zur Position des Roboters durchgeführt werden kann, da die Sensoren auf dem Roboter montiert sind. Um die Position der Hindernisse in der Umgebung zu bestimmen und somit eine Umgebungskarte zu erstellen, muss aus dem oben genannten Grund die Position des Roboters in der Umgebung bekannt sein. Ist diese nicht gegeben muss sie, wie oben erwähnt, mit Hilfe einer Umgebungskarte bestimmt werden, d. h. zur Bestimmung der Roboterposition in der Umgebung muss bereits eine Kartenerstellung durchgeführt worden sein. Die Positionsbestimmung des Roboters, was auch als Lokalisierung bekannt ist, und die Kartenerstellung bedingen sich also gegenseitig. Die Problemstellung SLAM [6 8] (engl. simultaneous localization and mapping), die eine der größten Herausforderungen der mobilen Robotik darstellt, greift das oben beschriebene Problem auf und beschäftigt sich mit der simultanen Lokalisierung und Kartenerstellung. Die bisherigen Lösungen für das SLAM Problem, die im Folgenden als SLAM-Verfahren bezeichnet werden, lassen sich in zwei Kategorien einteilen: online SLAM und full SLAM. Beim online SLAM werden die Lokalisierung und die Kartenerstellung in Echtzeit durchgeführt, dazu werden die von den Sensoren aufgenommenen Messwerte instantan ausgewertet. Im Gegensatz dazu wird im full SLAM ein ganzer Datensatz, bestehend aus mehreren Messwerten, ausgewertet, um anhand dieser den zurückgelegten Weg des Roboters und die Umgebungskarte zu ermitteln. [7, S ] Die meisten SLAM-Verfahren setzen dabei eine Landmarken basierte Karte [9 11] ein. Landmarken sind markante Gegebenheiten der Umgebung, die aus verschiedenen Raumpositionen zuverlässig durch Distanzsensoren erkannt werden können. Beispiele dafür sind Ecken in langen Fluren, Stützpfeiler in Hallen und Möbel in Räumen. Die wohl bekanntesten online und full SLAM-Verfahren sind: der EKF SLAM [7,9,10] ist ein online SLAM-Verfahren und gehört zu den ältesten und populärsten Ansätzen zur Lösung des SLAM-Problems. Das Verfahren setzt einen Extended Kalman Filter [12] zur Vereinigung der Informationen über die Position und Landmarken ein. Die Karte wird mit Hilfe von Landmarken realisiert. Der Nachteil dieser Methode ist die Laufzeit des Kalman Filters von O(L 2 ), die von der Anzahl der Landmarken L abhängt [10].

13 3 der FastSLAM [7, 10, 13] kann als Lösung sowohl für online als auch full SLAMs verwendet werden. Mit Hilfe des Rao-Blackwellized Partikelfilters (siehe dazu [14]) wird der FastSLAM realisiert. Auch dieser SLAM verwendet eine Karte mit wiedererkennbaren Landmarken. Ein Vorteil gegenüber dem EKF SLAM ist die Laufzeit, die in der ersten Version (FastSLAM 1.0 [7, 10]) bei O(KL) und in der zweiten Version des Ansatzes (FastSLAM 2.0 [7, 13]) sogar bei O(K log L) liegt. Dabei ist K die Anzahl der Partikel und L die Anzahl der Landmarken. der GraphSLAM [7, 11] ist ein full SLAM-Verfahren. Auch hier wird die Karte anhand von Landmarken realisiert. Zur Lösung des Problems wird eine Adjazenzmatrix erstellt, die jede Position des Roboters mit den im Umfeld des Roboters erkannten Landmarken verknüpft. Adjazenzmatrizen werden auch als Graphen dargestellt, was dem SLAM seinen Namen verleiht. Die Reduktion des Graphen auf die Knoten, welche die Position repräsentieren, führt zum Lösen des full SLAM Problems. Einen Überblick über weitere SLAM-Verfahren und fertige Implementierungen in diversen Test- und Programmierumgebungen sowie Datensätze sind auf OpenSLAM.org [15] zu finden. Häufig wird bei den bekannten SLAM-Verfahren zum Erkennen der Landmarken als Distanzsensor ein Laserentfernungsmesser verwendet [4, 5, 16], da dieser eine punktgenaue Distanzmessung ermöglicht. Dadurch lassen sich Hindernisse präziser identifizieren und gesetzte Landmarken erkennen. Ultraschallsensoren hingegen können aufgrund des großen Messkegels nur die Entfernung des Roboters zu einem Hindernis liefern, aber keine präzise Auskunft über die Position des Hindernisses geben. Damit sind Laserentfernungsmesser bezüglich der Identifizierung von Landmarken aufgrund ihrer hohen Präzision gegenüber Ultraschallsensoren im Vorteil. Da sie aber deutlich teurer als Ultraschallsensoren sind, ist die weitere Untersuchung von SLAM-Verfahren, die zuverlässig mit Distanzmessungen von Ultraschallsensoren arbeiten, notwendig. In dieser Arbeit soll deshalb ein online SLAM-Verfahren entwickelt werden, das nur Distanzmessungen vom einem Ultraschallsensor benötigt. Da durch die Wahl des Distanzsensors keine präzise Erkennung von Landmarken möglich ist, wird die Lokalisierung anhand einer Belegungsgitterkarte [17], die auf den Ultraschallmesswerten basiert, durchgeführt. Die Lokalisierung und Kartenerstellung soll parallel zueinander ablaufen, wobei die Lokalisierung mit einem Extended Kalman Filter [7, 18, 19] geschieht und die Erstellung der Belegungsgitterkarte mit dem Sensor Forward Modell [7, 20] realisiert wird. In dieser Arbeit soll daher der Frage nachgegangen werden, ob der Einsatz des online SLAM-Verfahrens,

14 4 1 EINLEITUNG basierend auf der Extended Kalman Filter Lokalisierung und der Gitterkartenerstellung mit dem Sensor Forward Modell, eine zuverlässige Erkennung der Umgebung mit nur einem Ultraschallsensor ermöglicht. Für die Implementierung des Verfahrens wird im Zuge dieser Arbeit ein mobiler Roboter gebaut, der von einem Raspberry Pi 2 [21, 22] gesteuert wird. Dieser bietet einen 900 MHz Quad-Core ARM Prozessor, der eine parallele Ausführung der Lokalisierung und Kartenerstellung ermöglicht. Um Kompatibilität zu weiteren Projekten zu gewährleisten, wie dem Quadrocopter aus einer vorherigen Masterarbeit von G. Jahn [23], wird die Steuerung des Roboters auf der FlyPi Software basieren, die im Zuge der selben Masterarbeit entstand. Da FlyPi für den Einsatz auf Quadrocoptern konzipiert wurde, wird sie im Zuge dieser Arbeit weiterentwickelt. Zusammenfassend kann das Ziel dieser Arbeit wie folgt formuliert werden: Ziel ist die Entwicklung eines online SLAM-Verfahrens, basierend auf der Extended Kalman Filter Lokalisierung und dem Sensor Forward Modell zur Kartenerstellung, und die Konzipierung eines mobilen Roboters mit nur einem Ultraschallsensor für die Umsetzung des entwickelten Verfahrens. Um das SLAM-Verfahren zu verstehen, ist die Kenntnis über die Bewegung des Roboters (Abschnitt 2.1.1), das Positionstracking (Abschnitt 2.1.2) und die Theorie der Belegungsgitterkarten mit dem Forward Modell (Abschnitt 2.2.2) notwendig. Diese Grundlagen sowie eine Einführung in den Extended Kalman Filter (Abschnitt 2.1.3) werden in Kapitel 2 gegegeben. Im Kapitel 3 wird dann der entwickelte SLAM namens Grid2DSLAM vorgestellt und in Kapitel 4 der Aufbau des Roboters namens WheelPi und die Weiterentwicklung der FlyPi Software zu der Bibliothek MovingPi dargestellt. Im Anschluss daran werden in Kapitel 5 die Ergebnisse des entwickelten SLAM-Verfahrens präsentiert und in Kapitel 6 diskutiert und analysiert. Am Ende dieser Arbeit wird in Kapitel 7 das Fazit der Arbeit präsentiert und ein Ausblick über Weiterentwicklungsmöglichkeiten des entwickelten SLAM-Verfahrens gegeben.

15 5 2 Grundlagen der Lokalisierung und Kartenerstellung Die Problemstellung des SLAM-Verfahrens ist die simultane Lokalisierung und Kartenerstellung. In diesem Kapitel werden daher die Grundlagen zum Verständnis des SLAMs dargestellt. Um den Einstieg in das Thema zu erleichtern, werden die beiden Thematiken zunächst getrennt voneinander betrachtet. Auf die wechselseitigen Abhängigkeiten der Lokalisierung und Kartenerstellung wird daher nicht eingegangen. Im Abschnitt 2.1 wird zunächst die Lokalisierung thematisiert und der zweite Abschnitt 2.2 handelt von der Kartenerstellung und geht detailliert auf die Belegungsgitterkarte ein. 2.1 Lokalisierung Die Problemstellung der Lokalisierung ist die Bestimmung der Position eines mobilen Roboters relativ zu einer gegebenen Umgebungskarte. Das Positionstracking zählt zu den einfachsten Methoden der Lokalisierung eines Roboters [7]. Dabei werden ausgehend von einer bekannten Startposition die Bewegungen des Roboters gemessen und anhand dieser Messungen die Positionsänderungen kontinuierlich geschätzt. Die vorhandenen Messfehler soll das Lokalisierungsverfahren kompensieren und so den Fehler in der bestimmten Position verringern. Um die Methode des Positionstrackings zu verstehen, ist die Kenntnis über die Theorie der Roboterbewegung notwendig. Deshalb wird dies in Abschnitt thematisiert. Anschließend wird das Positiontracking beschrieben und der Extended Kalman Filter in Abschnitt eingeführt Bewegung des Roboters Die Bewegung des Roboters kann durch die Theorie der Bewegung in der Ebene mit zwei Freiheitsgraden beschrieben werden [24]. Bewegt sich eine Roboter in der Ebene, so wird seine Bewegung durch zwei Freiheitsgrade beschrieben. Die Anzahl an Freiheitsgraden gibt die voneinander unabhängigen Bewegungsmöglichkeiten eines Systems an [25]. Im Falle des Roboters reicht es aus als Freiheitsgrade die Rotation um seine eigene R z -Achse und die Translation in Richtung seiner R y -Achse zu wählen. Das Koordinatensystem des Roboters ist in Abbildung 1 dargestellt. Der Roboter kann als starrer Körper mit einem eigenen Ruhesystem betrachtet werden, wobei seine ausgeführten Bewegungen im globalen System stattfinden. Beide Systeme werden als Rechtssysteme mit mathematisch positivem Drehsinn definiert.

16 6 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG G y R y y R x θ x G x Abbildung 1: Darstellung des Roboter-Koordinatensystems R xy im globalen Koordinantensystem G xy. Dabei repräsentieren x und y die Roboterposition im globalen System und θ die Ausrichtung der R y -Achse zu der G x -Achse. Die Messwerte zur Positionsbestimmung werden durch einen Sensor, der sich auf dem Roboter befindet, ermittelt. Dadurch findet die Messung der Bewegung im Robotersystem statt. Bewegt sich der Roboter, so wird die Beschleunigung a R (t) = ( ) ax a y (2.1) und die Winkelgeschwindigkeit ω(t) seines Systems unabhängig von der Position und Ausrichtung im globalen Koordinatensystem gemessen. Um die Bewegung nun in das globale Koordinatensystem zu überführen, ist eine Rotation von a R (t) notwendig. Durch die Drehung um den Orientierungswinkel θ, der zwischen der R y - und G x -Achse liegt (siehe Abbildung 1), wird die Beschleunigung aus dem Robotersystem in das globale System transformiert. Die Drehung wird durch die Rotationsmatrix R θ = ( ) sin θ cos θ cos θ sin θ (2.2) beschrieben. [26]

17 2.1 Lokalisierung 7 Die Beschleunigung des Roboters im globalen Koordinatensystem ergibt sich zu a G (t) = R θ a R (t). (2.3) Im Folgenden wird a G (t) mit a(t) bezeichnet, um die Notation zu erleichtern. Die Rotation des Roboters, die durch die Winkelgeschwindigkeit ω(t) beschrieben wird, verursacht eine Veränderung des Orientierungswinkels θ um ω(t) dt. (2.4) Aus der Beschleunigung a(t) lässt sich die Geschwindigkeit des Roboters durch Integration bestimmen v(t) = a(t) dt. (2.5) Durch erneute Integration kann die zurückgelegte Strecke s(t) aus der Geschwindigkeit ermittelt werden s(t) = v(t) dt. (2.6) Mit Hilfe der Gleichungen 2.4 und 2.6 kann die Änderung der Position des Roboters zu einer gegebenen Startposition beschrieben werden. Dieses wird im nächsten Abschnitt genauer thematisiert Positiontracking Nach der Darstellung der Theorie im vorherigen Abschnitt wird in diesem Abschnitt die Methode des Positionstrackings [7, S , ] erläutert. Positionstracking beschäftigt sich mit der Positionsbestimmung des Roboters zum Zeitpunkt t. Die Position des Roboters im globalen System kann allgemein durch x x = y (2.7) θ

18 8 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG beschrieben werden. Dabei stellt x, y die Koordinaten und θ den Orientierungswinkel dar (siehe Abbildung 1). Zur Bestimmung der Position des Roboters ist die Kenntnis über die Startposition x 0 notwendig. Wird die Beschleunigung a(t) und Winkelgeschwindigkeit ω(t) gemessen, so wird die Position des Roboters nach den Gleichungen 2.4 und 2.6 durch vx (t) dt x = x 0 + vy (t) dt (2.8) ω(t) dt beschrieben. Diese Darstellung ist ein idealisiertes Modell. In der Realität können Beschleunigungen und Winkelgeschwindigkeiten nur diskret gemessen werden. Infolgedessen ist auch nur eine diskrete Bestimmung der Roboterposition möglich, wodurch Gleichung 2.8 zu v x,t x t = x t 1 + t v y,t (2.9) ω t wird. Dabei stellt t die Zeitdifferenz zwischen zwei Messpunkten dar und v x,t, v y,t, ω t beschreiben die Geschwindigkeiten bzw. die Winkelgeschwindigkeit zum Zeitpunkt t. Die Geschwindigkeit des Roboters v t zum Zeitpunkt t wird durch beschrieben. v t = v t 1 + t a t (2.10) Trotz der diskreten Darstellung ist das hier vorgestellte Modell zur Positionsbestimmung idealisiert und entspricht nicht den realen Beobachtungen. Tatsächlich wird die Positionsbestimmung durch Messfehler gestört, wodurch eine zunehmende Unsicherheit der berechneten Position entsteht. Um eine genauere Positionsbestimmung zu ermöglichen werden deshalb weitere Sensoren verwendet. Auch die Messdaten zusätzlicher Sensoren sind grundsätzlich fehlerbehaftet. Die Fehler sind in den meisten Fällen normalverteilt und unvorhersehbar. Aus diesem Grund wird die Positionsbestimmung durch einen stochastischen Prozess modelliert.

19 2.1 Lokalisierung 9 Zur Ermittlung einer optimalen Lösung solcher Prozesse werden stochastische Filtersysteme eingesetzt. Einer der Bekanntesten ist der Extended Kalman Filter, kurz EKF, der im nächsten Abschnitt vorgestellt wird Extended Kalman Filter Der Extended Kalman Filter (EKF) ist eine Erweiterung des von R. E. Kalman 1960 vorgestellten Kalman Filters [12]. Ursprünglich wurde der Kalman Filter zur Vorhersage von diskreten Zeit kontrollierten, linearen stochastischen Prozessen entwickelt. EKF verallgemeinert den Ansatz auf stetig differenzierbare nicht lineare Prozesse, bei denen der Fehler als normalverteilt angenommen werden kann. Mit Hilfe der Taylor-Entwicklung ersten Grades lassen sich die nicht linearen Prozesse linearisieren und auf den ursprünglichen Formalismus des Kalman Filters zurückführen. Als Hauptquelle für diesen Abschnitt dient An Introduction to the Kalman Filter [27] von Welch, G. und Bishop, G. Das Ziel des EKF ist, den Statusvektor x t R n eines zeitgesteuerten Prozesses zu bestimmen. Die Idee dabei ist, den Statusvektor, der die Eigenschaften des Systems beschreibt, für den aktuellen Zeitschritt zunächst vorherzusagen (engl. predict) und im zweiten Schritt mit Hilfe von Messwerten zu korrigieren (engl. update). Die Abbildung 2 gibt einen Überblick über den Ablauf des EKFs, der im weiteren Verlauf des Textes näher beschrieben wird. Der Prozess lässt sich durch den Statusvektor x t = f(x t 1, u t, w t 1 ) (2.11) und den beobachteten Messwert z t R m zum Zeitpunkt t durch z t = h(x t, v t ) (2.12) beschreiben. Durch die Anwendung der Prozessfunktion f auf den alten Statusvektor wird mit Hilfe eines optionalen Kontrollwertvektors u t R d eine Vorhersage über den aktuellen Statusvektor getroffen. Die Funktion h beschreibt die Messwerte ausgehend vom neuen Statusvektor. Die Vektoren w t 1 R n und v t R m enthalten zufällige voneinander unabhängige, normalverteilte Variablen, die den Prozessfehler bzw. Messfehler angeben. p(w t 1 ) N(0, Q t 1 ) (2.13) p(v t ) N(0, R t ) (2.14)

20 10 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG Da diese im Allgemeinen nicht bekannt sind, können die Gleichungen 2.11 und 2.12 wie folgt umgeschrieben werden: ˆx t = f(ˆx t 1, u t, 0) (2.15) ẑ t = h(ˆx t, 0) (2.16) Dabei wird ˆx t als a priori, ˆx t als a posteriori Schätzung des Statusvektors x t bezeichnet und ẑ t ist die Schätzung der Messung z t. Um den Fehler des Statusvektors ˆx t zu beschreiben, wird die a posteriori Kovarianzmatrix P t eingeführt. Die Herleitung der Kovarianzmatrix wird hier nicht näher beleuchtet, aber kann z. B. im Skript An Introduction to the Kalman Filter [27] von Welch, G. und Bishop, G. nachgelesen werden. Für den a priori Statusvektor ˆx t P t als ist die Kovarianzmatrix P t = A t P t 1 A T t + W t Q t 1 W T t (2.17) definiert. Dabei sind A t und W t Jacobimatritzen der Form A ti,j = f i x j (ˆx t 1, u t, 0) (2.18) W ti,j = f i w j (ˆx t 1, u t, 0). (2.19) Um den geschätzten Statusvektor ˆx t aus Gleichung 2.15 zu verbessern, wird zu diesem ein gewichtetes Residuum zwischen den tatsächlich beobachteten Messwerten z t und dem geschätzten Messwert ẑ t aus Gleichung 2.16 addiert. ˆx t = ˆx t + K t (z t ẑ t ) (2.20) Dabei ist K t der sogenannte Kalman-Gain mit K t = P t HT t (H t P t HT t + V t R t V T t ) 1, (2.21)

21 2.1 Lokalisierung 11 wobei H t und V t Jacobimatritzen der Form sind. H ti,j = h i (ˆx t, 0) (2.22) x j V ti,j = h i (ˆx t, 0) (2.23) v j Auch die Kovarianzmatrix P t wird durch P t = (I K t H t )P t (2.24) in diesem zweiten Schritt verbessert. Die Funktionsweise des EKFs ist in Abbildung 2 zusammengefasst dargestellt. ˆx t, P t u t z t Initial: ˆx 0, P 0 Predict-Schritt (1) Berechne Vorhersage Statusvektor ˆx t = f(ˆx t 1, u t, 0) (2) Berechne Vorhersage Kovarianzmatrix Pt = A t P t 1 A T t + W t Q t 1 Wt T Update-Schritt (1) Berechne die Vorhersage für die Messung ẑ t = h(ˆx t, 0) (2) Berechne das Kalman-Gain K t = Pt HT t (H t Pt HT t + V t R t Vt T ) 1 (3) Update Statusvektor ˆx t = ˆx t + K t (z t ẑ t ) (4) Update Kovarianzmatrix P t = (I K t H t )Pt ˆx t Abbildung 2: Ablaufdiagramm des Extended Kalman Filters: Der EKF ist in die zwei Schritte, Predict- und Update-Schritt, unterteilt. Am Anfang werden die Startwerte ˆx 0 und P 0 als Initialwerte gesetzt und anhand dieser die erste Vorhersage über den Statusvektor ˆx t und Kovarianzmatrix P t im Predict-Schritt getätigt. Während der Zeitschritte t = 1,... werden zum Predict-Schritt optionale Kontrollwerte u t und zum Update-Schritt Messwerte z t hinzugefügt. Die Ausgabe des Update-Schrittes wird zu einem für den nächsten Predict-Schritt verwendet zum anderen als Ausgabe zurückgegeben. ˆx t, P t Die Vorteile des EKFs liegen in der einfachen Erweiterbarkeit des Statusvektors eines Prozesses und der einfachen Implementierung. Die Laufzeit mit O(n 2 ) [10], die aufgrund der Berechnung der Kovarianzmatrix des Statusvektors für hochdimensionale Statusvektoren

22 12 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG groß ist, stellt ein Nachteil des EKFs dar. Des Weiteren leidet die Performance des EKFs bei stark nicht linearen Prozessen. Auch die Beschränkung auf normalverteilte Fehler schränkt das Einsatzgebiet des EKFs ein. Für Prozesse mit nicht normalverteilten Fehlern bestehen jedoch weitere Ansätze wie den Unscented Kalman Filter [28] oder Partikelfilter [14]. Im verwendeten Modell wird von einem normalverteilten Prozess ausgegangen. Da viel Wert auf eine einfache Gestaltung des entwickelten SLAM-Verfahrens gelegt wird, eignet sich der Extended Kalman Filter für den Lokalisierungsprozess. Da der in dieser Arbeit aufgestellte Statusvektor von niedriger Dimension ist, kann die Laufzeitschranke ignoriert werden, sodass der Nachteil des EKFs für das behandelte Problem nicht besteht. 2.2 Kartenerstellung Neben der Lokalisierung ist die Kartenerstellung ein weiterer Bestandteil jedes SLAM- Verfahrens. Bei der Betrachtung der bisherigen Verfahren fällt auf, dass zwei Kartentypen existieren. Zum einem wird die Umgebung anhand von Landmarken beschrieben, die dann in der Karte verzeichnet sind. Landmarken sind markante Umgebungsmerkmale und dienen in den SLAM-Verfahren neben der Abbildung der Umgebung auch zur Lokalisierung. Dieser Kartentyp kommt z. B. im EKF SLAM und FastSLAM, die im Kapitel 1 vorgestellt wurden, zum Einsatz. Eine Darstellung der Karte anhand von Landmarken beschreibt die Umgebung allerdings nicht vollständig, da nur markante Umgebungsmerkmale in der Karte verzeichnet werden. Um eine genauere Umgebungsbeschreibung durch die Karte zu erhalten, hat sich die Verwendung von Belegungsgitterkarten (engl. Occupancy Grid Maps) [20] durchgesetzt, was den zweiten Kartentyp darstellt. Diese wurden speziell für die Erstellung von Karten basierend auf Distanzmessungen mit Ultraschallsensoren entwickelt [17], da der breite Messkegel des Sensors eine genau Bestimmung von Hindernissen nicht zulässt. Die Belegungsgitterkarten werden aber auch zur Darstellung von Distanzmessungen, die mit anderen Sensoren aufgenommen worden sind, eingesetzt. Eine Belegungsgitterkarte (engl. Occupancy Grid Map) ist eine Projektion des realen Raumes in beliebig kleine, diskrete Gitterzellen. Üblicherweise wird nur eine Ebene des Raums (z. B. die xy-ebene), die durch Sensoren des Roboters erfasst wird, in eine zweidimensionale Gitterkarte projiziert. Das reicht aus, um eine Orientierung in ebenen Räumen zu ermöglichen. Natürlich sind auch dreidimensionale Karten möglich [29], sind aber nicht besonders verbreitet, da für die Erstellung zusätzlicher Rechenaufwand notwendig ist [20].

23 2.2 Kartenerstellung 13 Jede Gitterzelle m x,y der Belegungsgitterkarte m kann entweder belegt (engl. occupied) oder frei (engl. unoccupied) sein. Befindet sich im realen Raum ein Hindernis, so werden die Gitterzellen, auf die das Hindernis projiziert wird, als belegt markiert. Anderenfalls wird die Zelle als frei gekennzeichnet. Das Ermitteln der Belegung jeder Zelle kann auf verschiedene Weise geschehen, z. B. durch manuelle Festlegung, Projektion einer real existierenden Karte oder anhand von Sensordaten. Da in dieser Arbeit eine autonome Kartenerstellung das Ziel ist, können nur Sensordaten zur Erstellung genutzt werden. Dazu wird unter dem Öffnungswinkel des Sensors Strahlung emittiert, um die Umgebung zu untersuchen. Trifft diese auf ein Hindernis, wird der Strahl reflektiert. Der reflektierte Strahl wird dann vom Sensor detektiert und die zugehörige Gitterzelle als belegt markiert. Durch die Reflexion des Strahls kann die sich hinter dem Hindernis befindende Umgebung nicht mehr untersucht werden. Durch die Lage des ersten erkannten Hindernisses wird daher die Länge des Sensormesskegels bestimmt. Im weiteren Verlauf des Textes werden zwei Verfahren, die zur Erstellung von Belegungsgitterkarten dienen, beschrieben. Auf das Inverse Sensor Modell [7, 17, 20] wird eingegangen, da es weit verbreitet ist und oft als Standardverfahren bezeichnet wird. Dabei wird das Problem des Verfahrens erarbeitet. Anschließend wird dann im Abschnitt das Forward Modell [20] vorgestellt, das die auftretende Problematik des Inverse Sensor Modells löst. In Abschnitt wird die Umsetzung des Forward Modells mit dem Expectation- Maximization-Algorithmus [30] vorgestellt, der in dieser Arbeit zur Kartenerstellung genutzt wird. Als Hauptquelle für das Thema Kartenerstellung ist Learning occupancy grid maps with forward sensor models [20] von S. Thrun zu nennen Inverse Sensor Modell Eine gängige Methode ist das Erstellen der Karte mit Hilfe des Inverse Sensor Modells [20]. Dabei wird die Karte anhand von Messungen evaluiert. Das Verfahren beruht auf einem stochastischen Problem. Seien die Distanzmessungen z 1,..., z T und die dazugehörige Position und Ausrichtung x t = (x, y, θ) T des Roboters, gegeben, dann ist p(m z 1:T, x 1:T ) die Wahrscheinlichkeit, dass die auf den Distanzmessungen beruhende Karte m die Umgebung beschreibt. Ziel ist es, die wahrscheinlichste Karte m zu bestimmen. Dazu müssen die Wahrscheinlichkeiten aller möglichen Karten berechnet werden.

24 14 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG Das größte Problem dabei stellt die Größe der Karte m dar. Gitterkarten enthalten oft mehr als Zellen. Da jede Zelle entweder belegt oder frei sein kann, gibt es mehr als mögliche Karten. Infolgedessen ist die Bestimmung der wahrscheinlichsten Karte mit großem Rechenaufwand verbunden. Zur Lösung dieses Problems wird das mehrdimensionale Problem p(m z 1:T, x 1:T ) in einfachere eindimensionale Probleme p(m x,y z 1:T, x 1:T ), unter der Annahme, dass die Zellen unabhängig voneinander sind, zerlegt und separat berechnet. [20] Damit wird nun nicht mehr die Wahrscheinlichkeit p(m z 1:T, x 1:T ) der Karte m berechnet, sondern die Belegungswahrscheinlichkeit für jede einzelne Gitterzelle. Um den Rechenaufwand weiter zu verringern, werden die so genannten Log-Odds [31, S. 121] verwendet, anstatt p(m x,y z 1:T, x 1:T ) direkt zu berechnen. Log-Odds für jede Gitterzelle sind wie folgt definiert: l T x,y = log p(m x,y z 1:T, x 1:T ) 1 p(m x,y z 1:T, x 1:T ) (2.25) und lassen sich durch die Anwendung der Bayes schen Regel [31, S. 213] und der Annahme über eine statische Umgebung, in der die Messungen z 1... z T voneinander unabhängig sind, rekursiv durch l t x,y = log für alle t = 1... T und l 0 x,y = log ist ausführlich in [20] erläutert. p(m x,y z t, x t ) 1 p(m x,y z t, x t ) + log 1 p(m x,y) + lx,y t 1 (2.26) p(m x,y ) p(mx,y) 1 p(m x,y) berechnen. Die Herleitung der rekursiven Form Der Vorteil der Log-Odds ist die einfache Rücktransformation zu p(m x,y z 1:T, x 1:T ) mit Hilfe der Exponentialfunktion. p(m x,y z 1:T, x 1:T ) = 1 (e lt x,y) 1 (2.27) Durch die Verwendung von Log-Odds ist eine schnelle und einfach Implementierung des Inverse Sensor Modells möglich [20]. Wie an der Gleichung 2.26 zu erkennen ist, müssen nur die zwei Wahrscheinlichkeiten p(m x,y ) und p(m x,y z t, x t ) für das Inverse Sensor Modell mit Log-Odds bekannt sein.

25 2.2 Kartenerstellung 15 Die Wahl der Wahrscheinlichkeit p(m x,y ) ist von der Umgebungskomplexität abhängig und wird normalerweise zwischen 0,2 0,5 [20] gewählt. Sie gibt die allgemeine Belegungswahrscheinlichkeit an und basiert daher nicht auf Sensormesswerten. Viel wichtiger ist die Festlegung von p(m x,y z t, x t ), welche die Wahrscheinlichkeit für die Belegung der Gitterzelle m x,y unter Berücksichtigung des Messwerts z t angibt. Diese wird anhand von Sensordaten ermittelt, welche den Rückschluss auf die Umgebung und die damit verbundene Kartenerstellung zulässt [20]. Das Inverse Sensor Modell weist jedoch einen großen Nachteil auf. Es können Karten entstehen, die im Konflikt zu den beobachtenden Messungen stehen, was in Abbildung 3 zu sehen ist. Wie in Abbildung 3a schematisch dargestellt, wurden zwei Messungen der Umgebung aufgenommen. Nach der oben vorgestellten Berechnungsmethode wird, ausgehend von je einer Messung, die Belegungswahrscheinlichkeit jeder Gitterzelle im Messkegel berechnet und in einer Umgebungskarte dargestellt (siehe Abbildung 3c und 3d). Anschließend werden die beiden Karten wie in Abbildung 3e zusammengeführt. Diese Karte spiegelt jedoch nicht die reale Umgebung wieder, was durch den Vergleich der Bilder 3e und 3a deutlich wird. Die gekennzeichneten Zellen in 3e werden fälschlicherweise als belegt markiert. Dieses Problem besteht aufgrund der Vorhersage über die Belegung einer Zelle ohne die Berücksichtigung der Nachbarzellen und dem fehlenden Abgleich von Einzelmessungen, deren Messkegel sich überlagern. Die errechnete Belegungswahrscheinlichkeit der Gitterzellen unter der Annahme, dass die Gitterzellen unabhängig sind, spiegelt also nicht die reale Umgebung wieder. [20] Ziel ist es, Karten wie in Abbildung 3f zu erstellen, da diese die reale Umgebung vollständig widerspiegeln (Vgl. Abbildung 3a und 3f). Dies ist nur mit Verfahren möglich, die keine Unabhängigkeit von Gitterzellen voraussetzen. Auf Grund dessen wird im folgenden Abschnitt eine weitere Methode beschrieben Forward Modell Ein anderer Ansatz zur Erstellung von Belegungsgitterkarten, als die im vorherigen Abschnitt vorgestellte Methode, ist das Forward Modell. Dieses wurde von S. Thrun entwickelt und seine Ausarbeitung über das Forward Modell [20] dient als Hauptquelle für diesen Abschnitt.

26 16 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG Abbildung 3: Konfliktentstehung im Forward Sensor Modell aus [7, S. 300] (nachbearbeitet): (a) stellt schematisch zwei Messungen der Umgebung dar; (b) zeigt die Übertragung der Messkegel in die Gitterkarte; in (c) und (d) sind die erstellten Gitterkarten für die einzelnen Messungen dargestellt; (e) zeigt die Zusammenführung von (c) und (d), dabei entsteht ein Konflikt; (f) demonstriert die optimale Gitterkarte der Umgebung Anders als das Inverse Sensor Modell, ermöglicht die Methode die Erstellung einer Karte unter Berücksichtigung interzellularer Abhängigkeit. Damit tritt die am Ende des letzten Abschnittes geschilderte Problematik nicht auf, da im Forward Modell für die Entscheidung, ob eine Gitterzelle frei oder belegt ist, alle Distanzmessungen, welche die gleiche Gitterzelle erfasst, abgeglichen werden. Auch hier handelt es sich um ein stochastisches Problem. Allerdings ist der Kerngedanke des Forward Modells die Wahrscheinlichkeit p(z, x m) zu ermitteln und nicht wie im Inverse Sensor Modell p(m z, x). Die Wahrscheinlichkeit p(z, x m) beschreibt dabei, inwieweit die Messung z zu einer gegebenen Gitterkarte m passt. Da in der Regel die Karte m nicht bekannt ist, wird die optimal passende Karte mit der Maximum Likelihood- Schätzung [31, S. 377] anhand der Messwerte z 1,..., z T geschätzt, was das eigentliche Ziel

27 2.2 Kartenerstellung 17 darstellt. Einfachheitshalber wird im Folgenden angenommen, dass die Roboterposition und Ausrichtung x t Teil des Messwertes z t. Im Forward Modell werden zwei Arten von Messwerten unterschieden: Der zufällige Wert und der nicht zufällige Wert. Der Messwert des Sensors z t ist mit der Wahrscheinlichkeit p rand ein zufälliger Wert, und nimmt dabei Werte zwischen 0 und der maximalen Reichweite des Sensors mit gleicher Wahrscheinlichkeit an. Zufällige Werte sind alle nicht modellierten Zustände, wie z. B. spiegelnde Reflexionen und falsche Distanzinformationen. [20] Mit einer Wahrscheinlichkeit von (1 p rand ) ist der Messwert z t ein nicht zufälliger Wert. Die Ursache des Messwerts z t ist ein tatsächliches Hindernis, was durch die Entfernungsmessung wahrgenommen wurde. Der Messwert ist aufgrund der Messung fehlerbehaftet. Der Fehler ist dabei normalverteilt. Ein Hindernis reflektiert mit einer Wahrscheinlichkeit von p hit die Sensorstrahlung. Das Hindernis mit dem kleinsten Abstand zum Sensor hat die höchste Wahrscheinlichkeit detektiert zu werden, diese beträgt (1 p rand ) p hit. Ein weiter entferntes Hindernis hat somit die Wahrscheinlichkeit (1 p rand ) (1 p hit ) n p hit detektiert zu werden, wobei n die Anzahl der nicht erkannten Hindernisse im Messkegel mit kleinerem Abstand zum Sensor angibt. Ist eine Karte gegeben, wird zu jeder Messung die Menge D t = {d t,1,..., d t,kt } angelegt, die alle Distanzen d t,1 d t,kt zu belegten Gitterzellen in der Karte unter dem Sensormesskegel der Messung enthält. Dabei gibt K t die Anzahl der ermittelten Distanzen an. Zusätzlich wird eine zu D t korrespondierende Menge C t = {c t,, c t,0, c t,1,..., c t,kt } mit K t + 2 binären Variablen gebildet, wovon nur genau ein c t,k = 1 ist und die restlichen den Wert Null haben. Ist der Messwert z t zufällig, so ist c t, = 1. Anderenfalls ist z t nicht zufällig und es gilt c t,k = 1 für genau ein k, mit 0 k K t. Wenn c t,0 = 1, dann passt keine der Distanzen d t,k D t zum Distanzmesswert z t. Es wird dann angenommen, dass der Messwert z t die maximale Distanz z max, die der Sensor wahrnehmen kann, ist. Gilt c t,k = 1 für 1 k K t, so stimmt der Messwert der Messung z t mit der k-ten Distanz d t,k D t überein.

28 18 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG In den Fällen c t,k, mit 0 k K t, ist die Wahrscheinlichkeitsverteilung eine Gaußverteilung mit dem Erwartungswert d t,k D t bzw. z max und dem Fehler des Sensors als Standardabweichung σ. Die Wahrscheinlichkeitsdichten lauten p(z t m, c t,k = 1) = 1 e (z t d t,k ) 2 2σ mit 1 k K t (2.28) 2πσ 2 p(z t m, c t,0 = 1) = 1 2πσ 2 e (z t zmax)2 2σ. (2.29) Für den Fall, dass c t, = 1, ist z t ein zufälliger Wert zwischen 0 und z max. Der Wert für z t ist dann gleichverteilt mit der Wahrscheinlichkeitsdichte p(z t m, c t, = 1) = 1 z max. (2.30) Um alle drei Dichtefunktionen 2.28, 2.29 und 2.30 zu kombinieren, wird die Gleichung 2.30 in folgender Weise geschrieben: p(z t m, c t, = 1) = 1 1 2πσ 2 e 2 log z 2 max 2πσ 2. (2.31) Somit ergibt sich die gemeinsame Dichtefunktion zu p(z t m, C t ) = 1 2πσ 2 e 1 2 [ c t, log z2 max 2πσ 2 +c t,0 (z t zmax)2 σ 2 ] K + t k=1 c (z t d t,k ) 2 t,k σ 2. (2.32) Da nur ein Element der Menge C t ungleich Null ist, hat auch nur ein Summand im Argument der Exponentialfunktion einen Wert ungleich Null. Folglich stellt die Gleichung 2.32 eine verallgemeinerte Darstellung der Gleichungen 2.28, 2.29 und 2.30 dar. In der Realität ist nicht bekannt, ob ein Messwert zufällig oder nicht zufällig ist. Allerdings sind die Wahrscheinlichkeiten dafür, wie oben erwähnt, bekannt. Daher kann die Wahrscheinlichkeit, dass ein Element der Menge C t den Wert Eins hat, berechnet werden. p rand, wenn c t, = 1 p(c t ) = (1 p rand )(1 p hit ) Kt, wenn c t,0 = 1 (2.33) (1 p rand )(1 p hit ) k 1 p hit, wenn c t,k = 1, für 1 k K t

29 2.2 Kartenerstellung 19 Unter Berücksichtigung der Wahrscheinlichkeit p(c t ) wird Gleichung 2.32 zu p(z t, C t m) = p(z t m, C t ) p(c t m) = p(z t m, C t ) p(c t ). (2.34) Bisher wurde angenommen, dass eine Karte m gegeben ist, mit der die Messwerte verglichen werden können. Wenn dies nicht der Fall ist, kann mit Hilfe der Likelihood-Funktion [31, S. 376] aus allen Messungen ein Schätzer für die Karte ermittelt werden. Da alle z t voneinander unabhängig sind und die Wahrscheinlichkeitsdichte durch Gleichung 2.34 beschrieben wird, lautet die Likelihood-Funktion p(z, C m) = t p(z t, C t m) (2.35) Dabei beschreibt Z = {z 1,..., z T } die Menge aller Messungen und C = {C 1,..., C T } die Menge der Korrespondenzmengen zu den jeweiligen Messungen. Um die Berechnung der Likelihood-Funktion zu erleichtern, kann der Logarithmus der Funktion gebildet werden. Somit ergibt sich die Log-Likelihood-Funktion log p(z, C m) = t log p(z t, C t m) (2.36) Die logarithmische Transformation verändert nicht das Ergebnis (s. dazu [31, S. 377]). Um die wahrscheinlichste Karte zu erhalten, muss das Maximum der Log-Likelihood- Funktion bestimmt werden [20]. Da diese aber von den Variablen c t C t abhängt und der Wert dieser nicht bekannt ist, kann das Maximum nicht bestimmt werden. Um trotzdem eine Aussage über die wahrscheinlichste Karte treffen zu können, kann der Erwartungswert der Log-Likelihood-Funktion E[log p(z, C m) Z, m] betrachtet werden, da dann nur der Erwartungswert der Variablen bestimmt werden muss. Unter der Anwendung der Gleichungen 2.36, 2.34, 2.32 und der linearen Eigenschaft des Erwartungswertes, folgt für den Erwartungswert [20] E[log p(z, C m) Z, m] ( = t E[log p(c t z t, m)] + log + E[c t,0 z t, m] (z t z max ) 2 σ ( E[c t, z t, m] log z2 max 2πσ 2 2 2πσ 2 K t E[c t,k z t, m] (z t d t,k ) 2 )) σ 2 k=1 (2.37)

30 20 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG Ziel ist nun, den Erwartungswert der Log-Likelihood-Funktion zu maximieren, um die wahrscheinlichste Karte zu finden. Eine mögliche Methode zur Bestimmung des Maximums ist im folgenden Abschnitt beschrieben EM-Algorithmus Ein iteratives Verfahren zur Ermittlung des Maximums der Likelihood- bzw. Log-Likelihood- Funktion wurde 1977 von A. P. Dempster at al. unter dem Namen Expectation-Maximization- Algorithmus (kurz EM-Algorithmus) veröffentlicht [30]. Als Einführung in die Funktionsweise und Möglichkeiten des Verfahrens bietet sich das Buch von G. J. McLachlan und T. Krishnan The EM Algorithm and Extensions [32] an. Das Verfahren gliedert sich in drei Schritte. Nach der Initialisierung findet die Expectation (kurz E-Schritt) und die Maximization (kurz M-Schritt) statt. Wird der EM-Algorithmus auf das Forward Modell angewendet, ergeben sich die nachfolgend dargestellten Einzelschritte. Als Hauptquelle für die formale Definition der Schritte dient die Veröffentlichung Learning Occupancy Grid Maps With Forward Sensor Models von S. Thrun. 1. Initialisierung: Zunächst wird eine Karte m initialisiert. Dabei wird jede Gitterzelle m x,y zufällig als belegt oder frei markiert. Eine Angabe über die Unsicherheit der Gitterzellenbelegung findet nicht statt. 2. E-Schritt: In diesem Schritt wird eine Vorhersage über den Erwartungswert der Variablen c t,k für die gegebene Karte m und Messwert z t getätigt. Der Erwartungswert lautet für 1 k K t nach [20] e t,k = E[c t,k z t, m] = p(c t,k = 1 z t, m) = 1 p(z t m) p(z t c t,k = 1, m)p(c t,k = 1 m) 1 1 = p(z t m) e (z t d t,k ) 2 2σ (1 p rand )(1 p hit ) k 1 p hit 2πσ 2 (z t d t,k ) 2 = η(1 p rand )(1 p hit ) k 1 p hit e 2σ 2 (2.38)

31 2.2 Kartenerstellung 21 mit der Normierungskonstante η = 1 p(z t m) 2πσ 2. (2.39) Für die Korrespondenzvariablen c t,0 lautet der Erwartungswert nach [20] e t,0 = E[c t,0 z t, m] = p(c t,0 = 1 z t, m) 1 = p(z t m) p(z t c t,0 = 1, m)p(c t,0 = 1 m) 1 1 = p(z t m) e (z t zmax)2 2σ (1 p rand )(1 p hit ) Kt 2πσ 2 = η(1 p rand )(1 p hit ) Kt e (z t zmax) 2 2σ 2 (2.40) und für c t, e t, = E[c t, z t, m] = p(c t, = 1 z t, m) 1 = p(z t m) p(z t c t, = 1, m)p(c t, = 1 m) 1 1 2πσ 2 = p rand p(z t m) 2πσ 2 z max 2πσ 2 = η p rand. (2.41) z max Die Normierungskonstante kann nach S. Thrun [20] durch Summation der nicht normierten Erwartungswerte der Korrespondenzvariablen berechnet werden. η = ( p rand 2πσ 2 z max K t k=1 + (1 p rand )(1 p hit ) Kt e (zt zmax) 2 2σ2 ) 1 + (1 p rand )(1 p hit ) k 1 (z t d t,k ) 2 e 2σ 2 (2.42) 3. M-Schritt: Der Erwartungswert der Log-Likelihood-Funktion aus Gleichung 2.37 wird in diesem Schritt maximiert und so die wahrscheinlichste Karte m bestimmt.

32 22 2 GRUNDLAGEN DER LOKALISIERUNG UND KARTENERSTELLUNG Dazu werden die vorhergesagten Erwartungswerte e t,, e t,0 und e t,k aus dem E-Schritt als Konstanten genutzt, sodass sich der Erwartungswert wie folgt berechnen lässt [20]: arg max m E[log p(z, C m) Z, m] ( = t E[log p(c t z t, m)] + log + e t,0 (z t z max ) 2 σ 2 + K t k=1 1 1 ( e t, log z2 max 2πσ 2 2 2πσ 2 (z t d t,k ) 2 )) e t,k σ 2 (2.43) An dieser Gleichung ist zu erkennen, dass alle Terme bis auf t Kt k=1 e t,k (zt d t,k) 2 σ 2 bei der Ermittlung der besten Karte m vernachlässigt werden können, da sie konstant sind. Das reduziert das Optimierungsproblem zu dem folgenden Minimierungsproblem K t e t,k (z t d t,k ) 2 min (2.44) t k=1 Die Ermittlung der wahrscheinlichsten Karte wird durch Veränderung der Belegung der Gitterzellen realisiert. Dabei werden die Zustände zwischen frei und belegt geflippt, bis Gleichung 2.44 minimal ist. Der M-Schritt wird beendet, wenn ein Minimum erreicht ist. Dieses muss nach S. Thrun nicht eindeutig sein [20]. Die so ermittelte Karte m wird im E-Schritt für eine erneute Vorhersage über die Erwartungswerte genutzt und anschließend wieder im M-Schritt optimiert. Die beiden Schritte werden so lange wiederholt, bis sich die Karte nicht mehr ändert und das Verfahren somit konvergiert.

33 23 3 Grid2DSLAM Der in dieser Arbeit entwickelte SLAM wird als Grid2DSLAM bezeichnet und ist ein online SLAM-Verfahren, das auf der EKF-Lokalisierung [7, S ] [19] und dem in Abschnitt vorgestellten Forward Model zur Erstellung zweidimensionaler Belegungsgitterkarten basiert. Trotz der Namensverwandtschaft zu GridSLAM, welches von S. Thrun et al. [7, S ] entwickelt wurde, sind beide Verfahren zu unterscheiden. Der GridSLAM basiert auf dem FastSLAM und die Belegungskarte wird mit einem beliebigen Verfahren aus Abschnitt 2.2 erstellt. Der entwickelte SLAM ist für den Einsatz auf einem Mehrkernprozessor konzipiert, da die Lokalisierung und Kartenerstellung, trotz der Abhängigkeit voneinander, getrennt durchgeführt werden. Die gegenseitige Abhängigkeit wird durch den Austausch von Zwischenergebnissen gelöst. Die Lokalisierung geschieht dabei anhand der erstellten Gitterkarte. Dies grenzt das Grid2DSLAM-Verfahren von bisher entwickelten SLAM Ansätzen wie dem EKF SLAM und dem FastSLAM ab, welche die Lokalisierung mit Hilfe von Landmarken realisieren. Die Landmarken sind dabei Teil des eingesetzten stochastischen Filters (EKF bei EKF SLAM und Partikel Filter bei FastSLAM). Daher läuft die Lokalisierung und Bestimmung der Landmarken gleichzeitig in einem Filter ab. Die Darstellung der Umgebung als Gitterkarte geschieht im EKF SLAM und im FastSLAM bei Bedarf erst im Nachhinein. Im Gegensatz zu Grid2DSLAM sind die bisher entwickelten SLAMs für Einkernprozessoren konzipiert, allerdings bestehen bereits Lösungsansätze, wie der EKF SLAM auch auf Mehrkernprozessoren angewendet werden kann [33]. Der Grid2DSLAM besitzt einen modularen Aufbau, damit die Weiterentwicklung und Wartung der Implementierung des Verfahrens ohne größeren Aufwand möglich ist. Die Abbildung 4 gibt einen Überblick über den Aufbau des entwickelten SLAMs und verdeutlicht die Zusammenhänge der drei verwendeten Module. Diese werden im Folgenden kurz beschrieben: Grid2DSLAM bildet die Kommunikationsschnittstelle zwischen Grid2DEMmapper und Grid2DPoseEstimator und stellt die Benutzerschnittstelle für den Aufruf des SLAMs dar. Des Weiteren ist das Modul für den Aufruf und die Initialisierung von Grid2DPoseEstimator und Grid2DEMmapper zuständig.

34 24 3 GRID2DSLAM Grid2DPoseEstimator beinhaltet ein auf dem Extended Kalman Filter basierendes Lokalisierungsverfahren zum Positionstracking (siehe Abschnitt 2.1.2). Dafür werden die gemessenen Bewegungen des Roboters, die Umgebungskarte und die alte Roboterposition benötigt. Grid2DEMmapper ist für die Kartenerstellung nach dem in Abschnitt vorgestellten Forward Modell zuständig und benötigt damit eine Distanzmessung und die zugehörige Roboterposition. a t, z t x t, m t Grid2DSLAM initialisiert a t, f t, x t 1 x t x t 1, z t m t initialisiert Grid2DPoseEstimator Grid2DEMmapper Abbildung 4: Überblick über den Grid2DSLAM: Das Modul Grid2DSLAM erhält als Eingabewerte die aktuellen Beschleunigung- und Gyroskopmesswerte a t sowie die Distanzmesswerte z t und extrahiert anhand der vorherigen Karte m t 1 die Features f t. Zur Initialisierung wird Grid2DPoseEstimator a t, f t und die vorherige Position x t 1 übergeben und ermittelt durch die EKF-Lokalisierung die aktuelle Position x t, was den Ausgabewert des Moduls darstellt. Grid2DEMmapper wird mit x t 1 und z t initialisiert und berechnet mit dem EM-Algorithmus die aktuelle Karte m t und übergibt diese dem Modul Grid2DSLAM. Somit sind die Ausgabewerte des Grid2DSLAM Moduls die aktuelle Karte m t und Position x t. Die Kartenerstellung und die Lokalisierung läuft separat voneinander ab, was in Abbildungen 4 zu sehen ist. Das ermöglicht einen parallelen Ablauf beider Vorgänge. Während eine neue Karte m t anhand der Roboterposition x t 1 und Distanzmessung z t zum Startzeitpunkt des EM-Algorithmus vom Grid2DEMmapper erstellt wird, nutzt Grid2DPoseEstimator die vorhandene Karte zur Lokalisierung. Dafür werden belegte Gitterzellen aus der vorherigen Karte m t 1, die zu dem Messwert der aktuellen Distanzmessung z t passen, genutzt. Diese verifizierten Gitterzellen werden als Features f t bezeichnet und durch einen zweidimensionaler Vektor dargestellt: ( ) r f t =. (3.1) φ

35 25 Dabei stellt r die Distanz von der momentanen Position des Roboters zu der Mitte der Gitterzelle dar und φ bezeichnet den Winkel, den der Feature mit der y-achse des Roboters einschließt. Die Extraktion der Features geschieht in dem Modul Grid2DSLAM. Anschließend werden diese als Messwert im Update-Schritt des EKFs im Grid2DPoseEstimator verarbeitet. Anhand des Algorithmus in Listing 1 kann die Extraktion der Feature nachvollzogen werden. Listing 1: Feature Extraktion 1 Eingabe: z t Messwert der a k t u e l l e n Distanzmessung (x, y, θ) t A k t u e l l e Roboterposition, aus der gemessen wird m t 1 v o r h e r i g e G i t t e r k a r t e N 2 Maximale Anzahl der Features, mindestens 2 ϕ Öf f n u n g s w i n k e l des S e n s o r m e s s k e g e l s 6 c G i t t e r z e l l e n g r ö ß e Ausgabe: F t Menge der e x t r a h i e r t e n Features Initialisiere: 11 F t = for w {θ ϕ/2 + nα n = 0,..., N 1, α = ϕ/(n 1) : E r m i t t l e d i e G i t t e r z e l l e n k o o r d i n a t e n < g x, g y > : g x = (x + z t cos(w))/c 16 g y = (y + z t sin(w))/c i f < g x, g y > m t 1 : r = (x c (g x + 0, 5)) 2 + (y c (g y + 0.5)) 2 φ = atan2(g y, g x) θ i f (r, φ) / F t : 21 Füge (r, φ) zu F t hinzu endfor Die Hauptbestandteile des entwickelten SLAMs werden im weiteren Verlauf des Textes beschrieben. Im Abschnitt 3.1 wird die Lokalisierung mit dem Extended Kalman Filter thematisiert. Im Zuge dessen wird ein Messfehlermodell für die Messwerte des Accelerometers und des Gyroskopsensors vorgestellt und die Umsetzung des EKFs für die Lokaliserung präsentiert. Darauffolgend wird die Kartenerstellung mit dem Forward Modell in Abschnitt 3.2 dargestellt, indem das Raycasting-Verfahren und die EM-Implementierung erläutert werden.

36 26 3 GRID2DSLAM 3.1 EKF-Lokalisierung Die Positionsbestimmung mit dem Extended Kalman Filter ist sehr weit verbreitet und wird vielseitig eingesetzt, da sie sich für die Lokalisierung bewährt hat [18, 19]. Die EKF- Lokalisierung ist dabei vom EKF SLAM zu unterscheiden. Der Unterschied zwischen ihnen ist, dass im EKF SLAM zusätzlich zu der Position des Roboters auch Landmarken Teil des Statusvektors sind. Da der Grid2DSLAM auf den Einsatz von Landmarken verzichtet, reicht es in der EKF-Lokalisierung aus, nur die Position des Roboters zu betrachten. Der Aufbau der entwickelten EKF-Lokalisierung basiert auf dem Verfahren aus [7]. Die EKF-Lokalisierung gliedert sich aufgrund des EKFs in zwei Schritte: Den Predictund Update-Schritt. Die Vorhersage über die Position wird anhand von Messwerten des Accelerometer- und Gyroscopsensors getätigt, die als Kontrollwerte in den Predict-Schritt des EKFs eingehen. Im Update-Schritt wird die vorhergesagte Position mit den extrahierten Features aus der Karte korrigiert. Da die Messwerte des Accelerometers und des Gyroskopsensors fehlerbehaftet sind, muss bei der Vorhersage der Position im EKF der Messfehler berücksichtigt werden. Um diesen richtig festzulegen, wird ein Fehlermodell von Mourikis et al. [34] eingesetzt, das im nächsten Abschnitt beschrieben wird. Im Anschluss daran wird in Abschnitt die entwickelte Umsetzung des Extended Kalman Filter zur Lokalisierung vorgestellt Messfehlermodell Die Messwerte des Accelerometers und Gyroskopsensors (kurz Inertialsensor genannt) können nach Mourikis et al. [34] mit dem folgenden Modell beschrieben werden. ˆm = m + m bias + ɛ. (3.2) Der gemessene Messwert ˆm setzt sich somit aus dem tatsächlichen Wert m, dem Bias des Sensors m bias und einem normalverteilten Messfehler ɛ zusammen. Da der Bias durch äußere Faktoren wie Temperatur und elektrische Spannung beeinflusst wird, kann dieser sich während der Umgebungserfassung ändern. Der Bias des Sensors kann daher nach B. Barshan et al. [35] durch m bias (t) = C 1 (1 e t T ) + C2 (3.3)

37 3.1 EKF-Lokalisierung 27 modelliert werden. Die Ermittlung der Parameter C 1, C 2 und T kann dabei experimentell erfolgen und z. B. der Levenberg-Marquardt-Algorithmus [36 38] genutzt werden. Bei einem konstanten Messintervall T c lässt sich die Gleichung 3.3 auch in einer iterativen Form schreiben: m (k+1) bias = T m (k) bias T + T + T c (C 1 + C 2 ). (3.4) c T + T c Dabei beschreibt der Index k den k-ten Messwert und der Initialwert für k = 0 ist m (0) bias = C 2. Unter Berücksichtigung der Messfehler kann die Positionsbestimmung im EFK durchgeführt werden Umsetzung des Extended Kalman Filters Wie bereits in Abschnitt beschrieben, ist das generelle Ziel des Extended Kalman Filters, den Statusvektor x t R n eines zeitgesteuerten Prozesses zu bestimmen. Daher muss zunächst der Statusvektor für die Problemstellung definiert werden. Für die Lokalisierung eines mobilen Roboters in einer Ebene würde ein dreidimensionaler Statusvektor für den EKF ausreichen. Da die Änderung der Roboterposition anhand von Messwerten des Inertialsensors geschieht, müssen zusätzlich zu der Roboterposition auch der Bias des Accelerometers und Gyroskopsensors im Statusvektor berücksichtigt werden, damit die Messwerte möglichst fehlerfrei in den EKF einfließen. Damit ergibt sich ein sechsdimensionaler Statusvektor x t = (x, y, θ, a }{{} xbias, a ybias, ω bias ) T. (3.5) }{{} P osition Bias Die x- und y-koordinate geben dabei die Position des Roboters im globalen System an und θ stellt die Orientierung des Roboters im Raum dar (siehe dazu Abschnitt 2.1.1). Die Werte a xbias, a ybias und ω bias sind die jeweiligen Bias der Beschleunigung in x- bzw. y-richtung und der Winkelgeschwindigkeit ω im Robotersystem. Für die Vorhersage der Roboterposition im Predict-Schritt wird der Kontrollvektor u t = (v x, v y, ω) T (3.6) definiert, der die Geschwindigkeit in x- und y-richtung des Roboters sowie seine Drehgeschwindigkeit im globalen System enthält. Für die Berechnung des Kontrollvektors u t

38 28 3 GRID2DSLAM werden zunächst die Messwerte des Accelerometer- und Gyroskopsensors durch Abzug des Bias aufgearbeitet und anschließend aus dem Robotersystem in das globale System überführt. Da die Rotation und Translation nicht kommutieren, wird angenommen, dass der Roboter erst nach einer Drehung eine Translation durchführt [7, S ] [18]. Für die gemessene Winkelgeschwindigkeit ˆω(t) folgt somit: ω t = ˆω(t) ω bias (3.7) und die Geschwindigkeit v t ergibt sich mit Gleichung 2.10 und der Rotationsmatrix aus Gleichung 2.2 zu ) (ârx (t) a xbias v t = v t 1 + t R (θ+ t ωt). (3.8) â Ry (t) a ybais Die Rotationsmatrix R (θ+ t ωt) überführt dabei die im Robotersystem gemessenen Beschleunigungen â Rx (t) und â Ry (t) in das globale System und verwendet den aktuellen Orientierungswinkel des Roboters θ + t ω t [18]. Die Bias a xbias, a ybias und ω bias stammen aus dem vorherigen Statusvektor x t 1, und v t 1 ist die vorherige Robotergeschwindigkeit mit v 0 = 0. Im Predict-Schritt überführt die Prozessfunktion f den alten Statusvektor x t 1 mit Hilfe des Kontrollvektors u t in die Vorhersage des neuen Statusvektors ˆ x t : x t 1 + t v x y t 1 + t v y ˆ x t = f( x t 1, u t ) = θ t 1 + t ω. (3.9) a xbias a ybias ω bias Die drei Bias a xbias, a ybias und ω bias werden nach der Gleichung 3.4 berechnet. Zuvor müssen jedoch für alle drei Biaswerte die jeweiligen Parameter C 1, C 2 und T einmalig ermittelt werden. Die zum Statusvektor x t zugehörige Kovarianzmatrix P t wird durch ˆP t = A t P t 1 A T t + Q t (3.10)

39 3.1 EKF-Lokalisierung 29 im Predict-Schritt vorhergesagt. Dabei wird als Startwert für P 0 die Einheitsmatrix genutzt. Für die Jacobimatrix A t folgt nach Gleichung 2.18: 1 0 a 1,3 a 1,4 a 1,5 a 1,6 0 1 a 2,3 a 2,4 a 2,5 a 2, t ω bias A t = T axbias T axbias + t 0 0 T aybias T aybias + t T ωbias T ωbias + t (3.11) Wobei die Elemente a 1,3, a 1,4, a 1,5, a 1,6, a 2,3, a 2,4, a 2,5 und a 2,6 wie folgt berechnet werden: a 1,3 = t (v x cos(θ + t ω) v y sin(θ + t ω)) a 2,3 = t (v x sin(θ + t ω) + v y cos(θ + t ω)) a 1,4, a 2,4 = t (sin(θ + t ω)) a 1,5, a 2,5 = t (cos(θ + t ω)) a 1,6, a 2,6 = t 2 (v x cos(θ + t ω) v y sin(θ + t ω)) und die Kovarianzmatrix Q ergibt sich zu t σx 2 Q t = t σ 2 y t σ 2 θ t σa 2. (3.12) xbias t σa 2 ybias t σω 2 bias Die σ x, σ y und σ θ geben die Unsicherheit der vorhergesagten Position an und σ axbias, σ axbias und σ ωbias sind die Residuen, die beim Ermitteln der Parameter aus Gleichung 3.3 entstanden. Der Update-Schritt des EKF ist an [7, S. 216 ff.] angelehnt. Anhand der vorhergesagten Position, die im Statusvektor ˆ x t verzeichnet ist, wird zunächst die Entfernung zu den M belegten Gitterzellen aus der Karte m t 1 = {m (1) t 1,..., m(m) t 1 } berechnet. Die Karte stellt

40 30 3 GRID2DSLAM dabei nur den Teil der Umgebungskarte dar, der mit den letzten n Distanzmessungen wahrgenommen wurde. Die Funktion ˆf (k) t = h (k) (ˆ x t, m (k) t 1 ) = (m (k) t 1,x ˆx t) 2 + (m (k) t 1,y ŷ t) 2 atan2(m (k) t 1,y ŷ t, m (k) t 1,x ˆx t) ˆθ (3.13) t berechnet dann für alle belegten Gitterzellen k = 1,..., M ausgehend vom vorhergesagten Statusvektor die Vorhersage der Features. Dabei sind m (k) t 1,x bzw. m(k) t 1,y die Koordinaten des Mittelpunktes der Zelle relativ zum globalen Koordinatensystem und ˆx t, ŷ t und ˆθ t die vorhergesagte Position und Ausrichtung des Roboters aus ˆ x t. Die Matrix H (k) t ist die dazugehörige Jacobimatrix und lautet H (k) t = m (k) t 1,x xt (m (k) t 1,x ˆxt)2 +(m (k) t 1,y ŷt)2 m (k) t 1,y yt (m (k) t 1,x ˆxt)2 +(m (k) t 1,y ŷt)2 m (k) t 1,y yt (m (k) t 1,x ˆxt)2 +(m (k) m (k) t 1,x xt (m (k) t 1,x ˆxt)2 +(m (k) t 1,y ŷt) t 1,y ŷt)2 (3.14) Die Vorhersage über die Features wird getätigt, um den vorhergesagten Statusvektor zu verbessern. Dazu wird die Differenz der extrahierten Features f (i) t und vorhergesagten (k) Features ˆf t mit dem Kalman-Gain gewichtet. Um zu den f (i) (k) t passende Features ˆf t zu finden wird mit der Funktion j(i) = arg min 1 k M f (i) t ˆf (k) t 2 2 (3.15) der Feature ˆf (k) t mit der kleinsten quadratischen Abweichung zu f (i) t gesucht. Sind diese gefunden, wird für jeden extrahierten Feature der Kalman Gain K (i) t = ˆP t [H (j(i) t ] T (H j(i) t ˆP t [H j(i) t ] T + R t ) 1 (3.16) bestimmt. Dabei ist ˆP t wie zuvor die Kovarianzmatrix des vorhergesagten Statusvektors und R t die Kovarianzmatrix der vorhergesagten Features R t = ( σ 2 r σ 2 φ ). (3.17)

41 3.2 Kartenerstellung mit dem Forward Modell 31 Dabei gibt σ r und σ φ die Standardabweichung der berechneten Distanz und Ausrichtung zum vorhergesagten Feature an. Die anschließende Korrektur des vorhergesagten Statusvektors ˆp t und seiner Kovarianzmatrix ˆP t erfolgt durch x t = ˆ x t + i K (i) t (f (i) t ˆf j(i) t ) (3.18) P t = i (I K (i) t H j(i) t ) ˆP t. (3.19) Damit ist x t der Statusvektor für den Zeitschritt t, der die momentane Position und Ausrichtung des Roboters enthält. 3.2 Kartenerstellung mit dem Forward Modell Die Erstellung der Belegungsgitterkarte im entwickelten Grid2DSLAM wird mit dem Forward Modell realisiert. Zur Bestimmung des Maximums der Log-Likelihood-Funktion aus Gleichung 2.37 wird der EM-Algorithmus, der in Abschnitt vorgestellt wurde, verwendet. Die eingesetzte Implementierung des EM-Algorithmus für das Forward Modell ist aus S. Thruns Arbeit [20] abgeleitet. Für die Umsetzung des EM-Algorithmus ist es notwendig, dass alle Gitterzellen, die von einer Distanzmessung erfasst wurden und somit im Messkegel liegen, ermittelt werden. Dies wird mit dem Raycasting-Verfahren [39] realisert. Eine genaue Beschreibung der Methode erfolgt im nächsten Abschnitt. Im Anschluss daran wird die Implementierung des EM-Algorithmus anhand eines Pseudocodes in Abschnitt dargestellt Raycasting-Verfahren zur Gitterzellendetektion Raycasting wird oft zur Visualisierung von dreidimensionalen Objekten in verschiedenen Anwendungen wie z. B. in 3D-Programmen [39, 40] genutzt. Im Gegensatz dazu dient das Verfahren in dieser Arbeit zur Bestimmung der Gitterzellen, die im Messkegel einer Distanzmessung liegen, und wird somit für jede Distanzmessung durchgeführt. Da im EM-Algorithmus die Distanzen von der Roboterposition zum Mittelpunkt jeder Gitterzelle im Messkegel benötigt werden, geschieht die Berechnung bereits im Raycasting. Um jede Gitterzelle im Messbereich zu identifizieren, werden, ausgehend von der Roboterposition, im gesamten Messkegel unterschiedlich lange Strecken konstruiert. Dann werden die Koordinaten der Gitterzellen ermittelt, in denen die konstruierten Strecken enden. Jede

42 32 3 GRID2DSLAM so gefundene Gitterzelle wird nur einmal gezählt. Der Algorithmus in Listing 2 verdeutlicht das Verfahren. Die erfassten Zellen werden auch zur Kartenerweiterung genutzt, wenn sie noch nicht Teil der Karte sind (siehe Zeile 21). So kann die Umgebungskarte dynamisch vergrößert werden, da die Gitterzellen nur bei Bedarf initialisiert werden, was den positiven Nebeneffekt eines geringeren Speicherbedarfs hat. Listing 2: Raycasting-Verfahren zur Gitterzellendetektion Eingabe: z t Messwert der a k t u e l l e n Distanzmessung (x, y, θ) t A k t u e l l e Roboterposition, aus der gemessen wird 3 m t 1 v o r h e r i g e G i t t e r k a r t e ϕ Öf f n u n g s w i n k e l des S e n s o r m e s s k e g e l s c G i t t e r z e l l e n g r ö ß e Ausgabe: G zt Menge a l l e r G i t t e r z e l l e n mit den dazugeh ö r i g e n Distanzen 8 im Messkegel von z t ˆm t E r w e i t e r t e G i t t e r k a r t e Z <gx,g y> Menge der Distanzmessungen, zu den G i t t e r z e l l e n < g x, g y >, zu den z t h i n z u g e f ügt wurde 13 Initialisiere: G zt =, ˆm t = m t 1 for s {nc n = 0,..., n = z t/c } und w {θ ϕ/2 + nα nα ϕ, α = (c π)/(z t 180 ), n N 0} : E r m i t t l e d i e G i t t e r z e l l e n k o o r d i n a t e n < g x, g y > : 18 g x = x + s cos(w)/c g y = y + s sin(w)/c i f G i t t e r z e l l e < g x, g y > / ˆm t : Füge < g x, g y > a l s f r e i markierte G i t t e r z e l l e zu ˆm t hinzu i f G i t t e r z e l l e < g x, g y > / G zt : 23 Berechne d i e Distanz zu der Mitte der G i t t e r z e l l e : d <gx,gy> = (g x x) 2 + (g y y) 2 Füge (< g x, g y >, d <gx,gy>) zu G zt hinzu Füge z t zu Z <gx,gy> der G i t t e r z e l l e < g x, g y > hinzu endfor

43 3.2 Kartenerstellung mit dem Forward Modell EM-Implementierung Die EM-Implementierung, die in Anlehnung an die Arbeit von Thrun entstand, wird im diesem Abschnitt dargestellt. Da Thruns EM-Algorithmus die Kartenerstellung im Anschluss an die eigentliche Umgebungserfassung anhand eines kompletten Datensatzes erstellt, müssen für den Einsatz in einem online SLAM Änderungen vorgenommen werden, um die Performanz zu verbessern. Die Laufzeit des EM-Algorithmus hängt stark von der Anzahl der Distanzmessungen, die zur Kartenerstellung genutzt werden, ab. Da sowohl in dem E-Schritt als auch im M-Schritt über alle vorhandenen Distanzmessungen iteriert wird, steigt die Laufzeit des EM-Algorithmus mit jeder neu hinzugefügten Distanzmessung an. Infolgedessen nimmt auch die Erstelldauer einer Karte mit jeder Messung zu. Da die Karte im entwickelten Grid2DSLAM zur Lokalisierung genutzt wird, ist eine kurze Erstelldauer notwendig, damit möglichst schnell eine aktuelle Umgebungskarte vorliegt. Um dies zu erreichen, wird in dieser Implementierung die Anzahl der Distanzmessungen begrenzt. Somit werden für die Kartenerstellung nur jeweils n zurückliegende Distanzmessungen genutzt. Nachteilig ist, dass dadurch weit zurückliegende Distanzmessungen nicht mehr zum Abgleich der aktuellen dienen. Infolgedessen kann sich die Belegung der Gitterzellen, die vor den n zurückliegenden Distanzmessungen ermittelt wurde, fälschlicherweise ändern. Ein Vorteil von der Einschränkung auf n zurückliegende Distanzmessungen ist allerdings, dass sich dynamische Veränderungen in der Umgebung, z. B. das Öffnen oder Schließen einer Tür, schneller registrieren lassen. Nach Thrun ist es möglich, dass der EM-Algorithmus mit Hilfe von doppelt-verketteten Listen eine Konvergenzzeit von wenigen Minuten auf leistungsschwacher Hardware erreicht. Für den Einsatz in einem online SLAM muss die Konvergenzzeit noch weiter verkürzt werden. Aus diesem Grund werden weitere Einschränkungen im EM-Algorithmus getätigt. Die Erläuterung dieser erfolgt anhand des Algorithmus in Listing 3. Wie in Abschnitt beschrieben, gliedert sich der EM-Algorithmus in drei Schritte. Im ersten Schritt erfolgt die Initialisierung, wie im Pseudocode in Zeile 11 zu sehen ist. Als Initialisierungskarte für den EM-Algorithmus wird die aktuell vorliegende Karte benutzt, die bei Bedarf vom Raycasting-Verfahren dynamisch mit frei markierten Zellen erweitert wurde. Dies ist notwendig, wenn die neueste Distanzmessung z t Z n Zellen erfasst, die noch nicht Teil der Karte sind, und tritt deswegen auch am Anfang der Umgebungserfassung auf.

44 34 3 GRID2DSLAM Den Kern der EM-Algorithmus bilden die beiden while-schleifen in den Zeilen 13 und 38. In der äußeren Schleife wird in den Zeilen im E-Schritt eine Vorhersage über den Erwartungswert nach den Gleichungen 2.38, 2.40 und 2.41 getätigt. Im Anschluss daran, wird in der inneren Schleife der M-Schritt in den Zeilen durchgeführt und somit das Max-Likelihood ermittelt. Eine eins zu eins Implementierung des EM-Algorithmus aus Abschnitt würde nur die Änderung der Zellbelegung von belegt nach frei und nicht von frei nach belegt ermöglichen. Daher wird im M-Schritt ein Mini-E-Schritt [20] benötigt. Dieser wird in Zeile durchgeführt. Für jede beobachtete Zelle werden temporär beide möglichen Zustände, frei und belegt, betrachtet und die Veränderungen des Erwartungswertes über alle Messungen in Z <gx,gy> für beide Zustände berechnet. In Abhängigkeit der Größe der Differenz erhält die Gitterzelle in den Zeilen ihre Belegung. Als Abbruchkriterium im M-Schritt ist der Wechsel der Zellbelegung gewählt. Wurde bei keiner Zelle während des M-Schrittes die Belegung geändert, liegt die optimale Karte für die im E-Schritt ermittelten Erwartungswerte vor. In der äußeren Schleife wird die Summe der Erwartungswertdifferenzen aller Gitterzellen, wie an der Zeile 13 zu sehen, als Abbruchkriterium genutzt. Ändert sich diese nicht mehr, so wird auch der E-Schritt abgebrochen. Da die Konvergenzzeit des Verfahrens sehr groß sein kann und in manchen Fällen die Konvergenz nicht eintritt, können beide Schritte auch nach einer festen Anzahl an Iterationen abgebrochen werden. Dadurch wird sichergestellt, dass keine Endlosschleifen im EM-Algorithmus durch die fehlende Konvergenz entstehen. Nachteilig ist dabei, dass die Ermittlung einer optimalen Karte in manchen Fällen durch das frühzeitige Abbrechen des EM-Algorithmus nicht möglich ist. Nach dem Ablauf des EM-Algorithmus wird die gefundene Karte als die aktuelle Karte im SLAM verwendet. Die aktuelle Karte m t setzt sich aus allen Zellen < g x, g y > zusammen für die gilt Z <gx,gy>. Die belegten Zellen aus der Karte m t werden für die Lokalisierung zur Extraktion von Features genutzt.

45 3.2 Kartenerstellung mit dem Forward Modell 35 Listing 3: EM-Algorithmus zur Kartenerstellung Eingabe: ˆm t vom Raycasting e r w e i t e r t e G i t t e r k a r t e Z n Menge der n zur ü c k l i e g e n d e n Distanzmessungen z t 3 Z <gx,gy> Die Menge der Distanzmessungen z t zu j e d e r G i t t e r z e l l e mit z t Z <gx,gy> und z t Z n E Maximale Anzahl an I t e r a t i o n e n im E S c h r i t t M Maximale Anzahl an I t e r a t i o n e n im M S c h r i t t 8 Ausgabe: m t Neue Karte Initialisiere: m t = ˆm t, E c = 0, M c = 0 13 while Σ neu Σ neu = 0 E c = E c + 1 # E S c h r i t t for z t Z n : Σ alt oder E c < E : 18 I n i t i a l i s i e r e : D t = Finde a l l e Tupel (< g x, g y >, d <gx,g y>) G zt deren G i t t e r z e l l e n < g x, g y > b e l e g t s i n d. S o r t i e r e d i e s e a u f s t e i g e n d nach den dazugeh ö r i g e n Distanzen d <gx,g y> Füge d i e s e d <gx,g y> zu D t hinzu Bestimme K t = D t 23 Berechne d i e Erwarungswerte : e t, = p rand 2πσ 2 /z max e t,0 = (1 p rand )(1 p hit ) K t exp( 0, 5(z t z max) 2 /σ 2 ) Fü r 1 k K t mit d t,k = d <gx,g y> an der k ten S t e l l e i n D t : e t,k = (1 p rand )(1 p hit ) k 1 p hit exp( 0, 5(z t d t,k ) 2 /σ 2 ) 28 Berechne den Normierungsfaktor η : ] 1 η = [ e t, + K t e k=0 t,k Normiere d i e Erwartungswerte : for k {, 0,..., K t} : e t,k = ηe t,k 33 endfor endfor # M S c h r i t t M c = 0 38 while Belegung der G i t t e r z e l l e n < g x, g y > m t s i c h ä ndert oder M c < M : M c = M c + 1 for < g x, g y > m t mit Z <gx,gy> :

46 36 3 GRID2DSLAM for Belegung i {0, 1} 43 m (i) <g x,g y> = 0 for z t Z <gx,g y> : Berechne D t und K t mit e i n e r temporä ren G i t t e r z e l l e n b e l e g u n g < g x, g y >= i im Messkegel z t wie i n Z e i l e n : i f t a t s ä c h l i c h e G i t t e r z e l l e n b e l e g u n g < g x, g y >= 0 und i = 1 : 48 Berechne e t,k wie i n Z e i l e n erneut Berechne m (i) <g x,g y> mit d t,k = d <gx,gy> : m (i) <g x,g y> = m (i) <g x,g y> + e t,k (z t d t,k ) 2 endfor Berechne D i f f e r e n z <gx,gy> : 53 <gx,gy> = m (1) <g x,g y> m (0) <g x,g y> Σ neu = Σ neu + <gx,gy> i f <gx,gy> 0 : Markiere d i e G i t t e r z e l l e < g x, g y >= 0 ( a l s f r e i ) else : 58 Markiere d i e G i t t e r z e l l e < g x, g y >= 1 ( a l s b e l e g t ) endfor endfor endwhile Setze Σ alt 63 endwhile = Σ neu

47 37 4 WheelPi Roboter Das entwickelte SLAM-Verfahren soll auf einem kleinen mobilen Roboter getestet werden. Für diesen Zweck wurde der Roboter namens WheelPi konstruiert. Der Grid2DSLAM setzt voraus, dass die Lokalisierung und Kartenerstellung parallel zueinander ausgeführt werden können. Aus diesem Grund muss der Roboter mit einem Mehrkernprozessor ausgestattet sein. Der Minicomputer Raspberry Pi 2 erfüllt mit seinem 900 MHz Quad-Core ARM Prozessor diese Anforderung und eignet sich damit zum Einsatz auf dem WheelPi Roboter. Zum Steuern des Roboters wurde eine Software basierend auf der MovingPi Bibliothek geschrieben. Diese stellt eine Weiterentwicklung des FlyPi-Projektes [23] dar und ist im Vergleich zu offenen Roboter Operating Systemen wie z. B. ROS [41] weniger umfangreich und auf das Wesentliche beschränkt. Dadurch ist die Implementierung von Steuerungssoftware für verschiedene Arten von Robotern unkompliziert möglich. Der Vorteil der Nutzung der MovingPi Bibliothek ist, dass der WheelPi Roboter damit zu dem Quadrocopter FlyPi kompatibel ist. Im Unterschied zu auf dem Markt verfügbaren Robotersystemen, wie z. B. den Pioneer P3 [42], besitzt der WheelPi Roboter ein offenes Hardwaredesign und kann daher problemlos und kostengünstig nachgebaut werde. Der Aufbau des Roboters soll daher im folgenden Abschnitt 4.1 erläutert werden. Darauffolgend wird in Abschnitt 4.2 kurz das Konzept der MovingPi Bibliothek vorgestellt und die Programmbestandteile der WheelPi Steuerungssoftware in Abschnitt 4.3 beschrieben. Abschließend werden in Abschnitt 4.4 die wichtigsten Parameter des Grid2DSLAMs für den WheelPi Roboter erläutert. 4.1 Aufbau des WheelPi Roboters Als Basis für den WheelPi Roboter dient der Bausatz von Dawn Robotics [43]. Dieser wird mit einem Ultraschallsensor und einem Accelerometer- und Gyroskopsensor, auch Inertialsensor genannt, erweitert. Die Tabelle 1 zeigt die wichtigsten Komponenten des Roboters und im Anhang A.1 ist die vollständige Liste zu finden. Der Zusammenbau des Chassis erfolgt nach der mitgelieferten Anleitung zu dem Chassis Bundle mit dem Unterschied, dass die Vorderseite des Roboters über der Stützkugel ist, wie in der Abbildung 5 zu erkennen 1. Während der Montage des Chassis sollte der Inertialsensor (MPU6050) auf der untersten Ebene zwischen den Antriebsrädern angebracht 1 Das mitgelieferte Stützrad/-kugel unterscheidet sich von dem in der Abbildung 5. Die abgebildete Stützkugel wurde extra angefertigt.

48 38 4 WHEELPI ROBOTER Rahmen Steuercomputer Motorsteuerung Distanzsensor Inertialsensor Kommunikation Stromversorgung Dagu Magician 2WD Robot Chassis mit einem Pan-Tilt-Gelenk Raspberry Pi 2 Modell B Dagu Arduino Mini Driver Ultraschallsensor SRF08 MPU6050 WLAN-, Bluetooth-Adapter Lithium-Polymer-Akku (LiPo) Tabelle 1: Liste der wichtigsten Bauteile des Roboters. werden. Der Ultraschallsensor (SRF08) wird oberhalb der Raspberry Pi Kamera wie in Abbildung 5 montiert. Auf der obersten Ebene werden dann der Raspberry Pi 2, der Dagu Arduino Mini Driver und das Schwenk-Kipp-Gelenk angebracht und wie in Abbildung 6 dargestellt verbunden. Die Servomotoren des Schwenk-Kipp-Gelenks (engl. Pan-Tilt) sollten vor der Montage auf den Winkel von 90 ausgerichtet werden (siehe dazu Anhang A.2). Zur Kommunikation zwischen dem Raspberry Pi, dem Arduino und den Sensoren kommt das I2C-Protokoll zum Einsatz [44]. Als Batterie wird ein Lithium-Polymer-Akku (LiPo) benutzt. Zum Schluss werden noch der Bluetooth- und WLAN-Adapter in die USB-Eingänge des Pis gesteckt. Abbildung 5: Der fertige WheelPi Roboter.

49 4.2 MovingPi Bibliothek 39 UBEC GND 5 V LiPo + Battery 5 V GND micro USB Servo schwenken Servo neigen D3 5 V GND D2 5 V GND ML MR Dagu Arduino Mini Driver GND SCL SDA GND SCL SDA 3,3 V 5 V Raspberry Pi 2 Camera DC Motor links DC Motor rechts 5 V SDA SCL GND 3,3 V SDA SCL GND SRF08 MPU6050 Pi Kamera Abbildung 6: Schematische Darstellung der Verdrahtung des WheelPi Roboters. 4.2 MovingPi Bibliothek Die MovingPi Bibliothek stellt die Weiterentwicklung der FlyPi Software dar, die von G. Jahn 2014 im Zuge seiner Masterarbeit in C++ entwickelt wurde und bildet die Steuerungssoftware des von ihm entwickelten Raspberry Pi gesteuerten Quadrocopters [23]. Ein Ziel bei der Entwicklung der FlyPi Software war eine möglichst von Dritt-Bibliotheken unabhängige, leicht weiter zu entwickelnde Plattform zu kreieren, die über ein Netzwerkprotokoll überwacht und gesteuert werden kann. Außerdem sollten die Hardwareelemente, wie die Sensoren und die Motorkontroller, über eine I2C Schnittstelle mit dem Raspberry Pi verbunden sein. Da die FlyPi Software nur für den Quadrocopter entwickelt wurde, existiert keine Trennung zwischen generell definierten Klassen, die ohne weiteres auch in anderen Projekten verwendet werden könnten, und der expliziten Implementierung, die für den Betrieb des Quadrocopters ausgelegt ist. Um die Software auch für andere Projekte zugänglich zu machen, musste eine Umstrukturierung der FlyPi Software erfolgen. Die Abbildung 7 zeigt die Struktur der MovingPi Bibliothek. Bei der Portierung der Klassen aus der FlyPi-Software wurde viel Wert darauf gelegt, möglichst viele bereits vorhandene Klassen in die neue MovingPi Bibliothek zu übernehmen.

50 40 4 WHEELPI ROBOTER Implementierte Kernklassen Abstrakte Klassen und Interfaces I2CInterface Inputs SixAxisDefaultKeyMap PS3SixAxisJoystick KeyboardInput BasePi I2CBusBCM2835 I2CBusFile Hilfsklassen BaseArduino BaseVision BaseSensor RaspiCamera MarkerTracker Sensoren SensorADS1015 SensorAK8975 SensorMPL3115A2 SensorMPU6050 SensorMPU9150A SensorNavigatron2 SensorSRF08 BaseController BrowsableShare PID BaseInputSource Grid2DSLAM I2CBus Grid2DEMmapper Grid2DPoseEstimator I2CSlave ArduinoDevice ACP helpers jpge Browsable BrowsableProperty Rotation3D RapidMarkerTracker PerspectiveEstimator BrowsableMethod GridMap2D MeasurementDataTypes LogEntry Browsable Basisklassen Abbildung 7: Überblick über die MovingPi Bibliothek: Die vier Kategorien der Bibliothek sind in vier verschiedenen Farben dargestellt.

51 4.3 Realisierung der WheelPi-Steuerungssoftware 41 Es erfolgte eine Aufspaltung der Klassen in vier Kategorien. Die kleinste der vier ist die Kategorie Browsable Basisklassen. Die enthaltenen Klassen ermöglichen eine Browser ähnliche Programmstruktur. Die von Browsable abgeleiteten Klassen können Methoden, Attribute und Instanzen anderer Browsable Klassen global im Programm registrieren und diese durch die Verwendung der BrowsableShare Klasse über das Netzwerk bereitstellen. Dadurch ist es möglich, über eine Weboberfläche oder eine Skriptsprache wie Python die Attribute auszulesen, zu setzen oder Methoden aufzurufen. Der Zugriff darauf erfolgt dabei wie bei einem Dateipfad [23]. Die gesetzten Attributwerte werden nach dem Beenden in Konfigurationsdateien gespeichert und stehen beim erneutem Programmstart wieder zur Verfügung. Die von Browsable abgeleiteten Klassen sind sowohl in Abstrakte Klassen und Interfaces und Implementierte Kernklassen zu finden. Der Unterschied zwischen den beiden Kategorien liegt in der Definition der Klassen. Alle Klassen in Abstrakte Klassen und Interfaces dienen als ein Gerüst für eine einheitliche Basis der davon abgeleiteten Klassen. So bilden die Base- Klassen eine generelle Schnittstelle innerhalb der Bibliothek, die in einem Projekt wie WheelPi oder FlyPi den Bedürfnissen entsprechend implementiert werden müssen. Auch innerhalb der Bibliothek existieren Klassen, die eine Klasse aus Abstrakte Klassen und Interfaces erweitern und gehören zu der Implementierte Kernklassen Kategorie. So besitzen alle Sensor- Klassen ein gemeinsames Gerüst als I2CSlave. Diese Klassen ermöglichen die Kommunikation zu diversen Sensoren, die über einen I2C-Bus mit dem ausführenden Computer verbunden sind. Die Hilfsklassen bilden die vierte Kategorie. Diese sind nicht von Browsable abgeleitet und stellen weitere Funktionen und Strukturen zur Verfügung, die in den restlichen Bibliotheksklassen Anwendung finden. 4.3 Realisierung der WheelPi-Steuerungssoftware Die MovingPi Bibliothek liefert bereits eine Vielzahl an fertigen Klassen, die zum Steuern des WheelPi Roboters nützlich sind. Es müssen lediglich einige für den Roboter spezifische Klassen implementiert werden. Die Abbildung 8 zeigt den schematischen Aufbau der WheelPi-Steuerungssoftware mit den genutzten MovingPi Bibliotheksklassen und -schnittstellen. Nachfolgend sollen die für den WheelPi implementierten Klassen kurz erläutert werden.

52 42 4 WHEELPI ROBOTER WheelPi BasePi WheelPiArduino BaseArduino WheelPiArduinoDCMotors WheelPiArduinoPanTilt BaseArduinoDevice WheelPiController WheelPiAttitudeController BaseController WheelPiPanTiltController WheelPiInputs PS3SixAxisJoystick WheelPiSensor SensorSRF08 SensorMPU6050 Weitere Klassen aus der MovingPi Bibliothek, die nicht direkt von WheelPi Erweitert oder Benutzt werden. WheelPiSLAM Grid2DSLAM BaseVision I2CInterface BrowsableShare Legende nutzt Klasse erweitert Klasse Abbildung 8: Überblick über implementierte Klassen für den WheelPi Roboter (blau), und genutzten Schnittstellen und Klassen von der MovingPi Bibliothek (grün).

53 4.4 Einstellungen des Grid2DSLAMs 43 WheelPi initialisiert alle Programmkomponenten und ist für deren sequenziellen Aufruf zuständig. WheelPiArduino dient als Schnittstelle zu den Sensoren und Motoren, die an den Arduino angeschlossenen sind. Wie die Abbildung 8 zeigt, sind bisher nur zwei sogenannte ArduinoDevices implementiert. WheelPiArduinoDCMotors und Wheel- PiArduinoPanTilt dienen zur Kommunikation mit den DC- und Servomotoren des Roboters. WheelPiController verwaltet alle Controllerklassen des Roboters. Die Controller sind für die korrekte Umsetzung der Roboterbewegung zuständig und bilden die Schnittstelle zwischen der getätigten Eingabe mit z. B. einem Joystick und der Ausführung durch die Motoren. WheelPiInputs verwaltet Schnittstellen zur manuellen Steuerung des Roboters. Bisher ist es nur möglich den WheelPi Roboter mit einem Bluetooth Playstation 3 Joystick zu steuern. WheelPiSensor bildet die Schnittstelle zu den verwendeten Sensoren SRF08 und MPU6050, und stellt die Sensormesswerte anderen WheelPi Klassen zur Verfügung. Das Auslesen der Messwerte geschieht über das I2CInterface. WheelPiSLAM ist für den Aufruf des Grid2DSLAMs zuständig. BaseVision ermöglicht das Nutzen der Raspberry Pi Kamera. Das Kamerabild wird über die Netzwerkschnittstelle bereitgestellt. I2CInterface ermöglicht den Zugriff auf den I2C Bus. Es ist möglich verschiedene I2C-Treiber zur Kommunikation auszuwählen. BrowsableShare ist die Netzwerkschnittstelle des Programms und ermöglicht das Kommunizieren mit dem Roboter über das Netzwerk. 4.4 Einstellungen des Grid2DSLAMs Für den Einsatz des Grid2DSLAMs auf dem WheelPi Roboter muss der Inertialsensor charakterisiert werden. Um das Messfehlermodell aus Abschnitt umzusetzen, werden Messwerte über einen Zeitraum von zwei Stunden mit einer Intervalllänge von 0,1 s bei einer Messfrequenz von 200 Hz aufgenommen. Daraus erfolgt mit dem Levenberg- Marquardt-Algorithmus die Ermittlung der Parameter aus Gleichung 3.3 für die Biaswerte

54 44 4 WHEELPI ROBOTER der Beschleunigung in die x- und y-achse sowie die Drehgeschwindigkeit um die z-achse des Sensors. Die Tabelle 2 stellt die ermittelten Parameter dar. C 1 C 2 T σ Residuum σ a x 0,0059 m/s 2 0,0059 m/s 2 0,7520 s 0,0287 m s 1 0,0029 m/s 2 a y 0,0073 m/s 2 0,0073 m/s 2 0,4664 s 0,027 m/s 2 0,0028 m/s 2 ω 0,0003 rad s 1 0,0002 rad s 1 165,254 s 0,0011 rad s 1 0,0612 rad s 1 Tabelle 2: Mit dem Levenberg-Marquardt-Algorithmus ermittelte Parameter für das Messfehlermodell. Dabei stellt σ Residuum die Abweichung der gefitteten Funktion aus Gleichung 3.3 zu den Messwerten dar und σ beschreibt die normalverteilte Abweichung der Messwerte. Als Prozessfehler werden bei der Berechnung der Vorhersage über die x und y-position und die Orientierung θ des Roboters die Werte aus Tabelle 3 im EKF genutzt. Dabei gilt für die Berechnung von σ x und σ y σ i = σ 2 a x + σ 2 a y + σ 2 ω mit i = x, y. (4.1) Für die Unsicherheit der vorhergesagten Feature im EKF werden die Werte in Tabelle 4 verwendet. x y θ σ 0,0613 m 0,0613 m 0,0612 rad σ r φ Zellengröße 2 m 0,06 rad Tabelle 3: Prozessfehler der Position. Tabelle 4: Unsicherheit der vorhergesagten Features. Da die beiden verwendeten Sensoren, MPU6050 und SRF08, nicht mit der gleichen Messfrequenzen arbeiten können, ist die gleichzeitige Verarbeitung ihrer Messdaten im EKF erschwert. Um zuverlässigere Ergebnisse zu erhalten ist das Zusammenführen der Messdaten jedoch notwendig. Der Inertialsensor MPU6050 wird daher auf eine Messfrequenz von 200 Hz und der Ultraschallsensor SRF08 auf eine Messfrequenz von 50 Hz eingestellt. Da das Messen der Roboterbewegung mit weniger als 200 Hz zu langsam ist, muss die unterschiedliche Messfrequenz der verwendeten Sensoren vom EKF kompensiert werden. Dazu wird im EKF die Einstellung vorgenommen, dass im Predict-Schritt gleichzeitig zwei Messwertsätze des Inertialsensors, bestehend aus der Beschleunigung in x- und y-richtung und der Drehgeschwindigkeit, verarbeitet werden. Der Update-Schritt wird außerdem nur

55 4.4 Einstellungen des Grid2DSLAMs 45 in jedem zweiten Durchlauf des EKFs ausgeführt, da die Distanzmessung, die für das Extrahieren von Features notwendig ist, nur alle 0,02 s zur Verfügung steht. Damit arbeitet der EKF auf dem WheelPi Roboter mit etwa 100 Hz, was sich bei Testläufen bewährt hat. Da der Inertialsensor kontinuierlich Messwerte liefert, auch wenn der Roboter sich nicht bewegt, kann die Berücksichtigung aller Messwerte zu einer Fehlkalkulationen der Roboterposition führen. Da dies auch bei Testläufen mit dem WheelPi Roboter beobachtet wurde, werden nur die Messwerte des Inertialsensors, die bei einer tatsächlichen Bewegung des Roboters aufgenommen wurden, im EKF berücksichtigt. Bei der Kartenerstellung im Grid2DSLAM wurde beim WheelPi Roboter beobachtet, dass der EM-Algorithmus eine sehr lange Laufzeit hat. Infolgedessen ist auch die Kartenerstelldauer sehr groß. Für die Verwendung im online SLAM ist jedoch eine möglichst kurze Kartenerstelldauer notwendig, da die Lokalisierung anhand der aktuellen Karte geschieht (siehe Abschnitt 3.2.2). Daher werden die maximalen Iterationen für den E- und M-Schritt auf zehn begrenzt.

56 46 5 EXPERIMENTELLE ERGEBNISSE 5 Experimentelle Ergebnisse Das entwickelte SLAM-Verfahren wurde in zwei verschiedenen Umgebungen getestet. Um die Funktionsweise des Grid2DSLAMs mit dem WheelPi Roboter in einer Umgebung mit vereinzelten Hindernissen zu untersuchen, wurde eine quadratische Fläche in einem Büro mit für die Umgebung typischen Hindernissen gewählt. Als zweite Testumgebung wurde ein langer Flur verwendet. Dieser diente zur Untersuchung, ob das entwickelte SLAM-Verfahren auch in weitläufigen Umgebungen ohne Hindernisse funktioniert. In beiden Testumgebungen wurden Versuche mit gleichen Einstellungsparametern für das SLAM-Verfahren durchgeführt. Ein wichtiger Parameter ist dabei die Anzahl der zurückliegenden Distanzmessungen für die Erstellung der Umgebungskarte, der in einer Versuchsreihe auf 5 und in einer anderen auf 10 beschränkt wurde. Die maximale Anzahl an Iterationen im E- und M-Schritt des EM-Algorithmus wurde in allen Experimenten auf 10 gesetzt und damit sichergestellt, dass der EM-Algorithmus bei einer zu langen Suche nach einer optimalen Karte abgebrochen wird. Für die Darstellung der Gitterkarten wurde 10 cm große Gitterzellen verwendet. Im Folgenden werden zunächst die Ergebnisse des EKF-Lokalisierungsverfahrens anhand der Versuche, die zehn zurückliegende Distanzmessungen in der Kartenerstellung berücksichtigen, im Abschnitt 5.1 beschrieben. Anschließend wird anhand der gleichen Versuche die Kartenerstellung in Abschnitt 5.2 thematisiert. In Abschnitt 5.3 wird dann die Erstelldauer der Umgebungskarte anhand der Versuche mit unterschiedlicher Anzahl, der für die Kartenerstellung berücksichtigten Distanzmessungen, untersucht. 5.1 EKF-Lokalisierung Im Folgenden werden die Ergebnisse des entwickelten Lokalisierungsverfahrens aus Abschnitt 3.1 dargestellt. Aufgrund des beobachteten Drifts des Roboters während seiner Fortbewegung war es nicht möglich, den Roboter auf einer definierten Bahn fahren zu lassen. Außerdem bestand keine Möglichkeit, die Bewegung des Roboters aus dem globalen System zu messen und infolgedessen konnten keine Vergleichsdaten aufgenommen werden. Die berechneten Positionen können also nicht anhand von Messdaten mit den realen verglichen werden, jedoch können die während der Testversuche gemachten Beobachtungen genutzt werden, um zu prüfen, ob das Lokalisierungsverfahren grundsätzliche Fehler aufweist. Um weitere Aussagen über die Verarbeitung der Rohmessdaten treffen zu können, werden die

57 5.1 EKF-Lokalisierung 47 aus den Messdaten des Accelerometers und Gyroskops bestimmten Roboterpositionen, im Folgenden als Rohpositionen bezeichnet, mit den durch das EKF-Lokalisierungsverfahren berechneten Roboterpositionen (EKF-Positionen) verglichen. Der erste Test des EKF-Lokalisierungsverfahrens fand in einer etwa 4 m 3,5 m großen nahezu freien Fläche eines Büros statt. In der Testfläche wurden am Rand einige Hindernisse aufgestellt, um neben der Positionsbestimmung auch die Kartenerstellung zu testen, auf die erst im nächsten Abschnitt eingegangen wird. Die Skizze der Testfläche mit skizzierter beobachteter Fahrstrecke des Roboters ist in 9 abgebildet. Abbildung 9: Skizze der Testfläche im Büro mit Hindernissen und skizzierte beobachtete Fahrstrecke des Roboters. In Abbildung 10 sind die EKF-Positionen in blau dargestellt und die grüne Messkurve zeigt die gesamten Rohpositionen, die aus den Messungen des Sensors MPU6050 ermittelt wurden. Zusätzlich sind in rot die Rohpositionen, welche bei einer tatsächlichen Bewegung des Roboters aufgenommen wurden, in der Abbildung verzeichnet. Werden die Abbildungen 9 und 10 verglichen, fällt auf, dass die ermittelten Roboterpositionen dem skizzierten, beobachteten Weg des Roboters grundsätzlich ähneln. Der Roboter wurde in einer Weise in der Testumgebung bewegt, dass die Start- und Endposition nahezu übereinstimmt. Dies ist jedoch anhand der ermittelten Roboterpositionen nicht ersichtlich. Weder die EKF-Position noch die Rohpositionen spiegeln die reale Endposition wider. Der Unterschied zwischen gemessener und realer Endposition beträgt etwa 39 cm in x-richtung und 17 cm in y-richtung.

58 48 5 EXPERIMENTELLE ERGEBNISSE Wie in Abbildungen 10 zu sehen, weichen die EKF-Positionen (blau) und die Rohpositionen (rot und grün) während des gesamten zurückgelegten Weges um etwa 10 cm voneinander ab. Die gesamten Rohpositionen (grün) und die Rohpositionen (rot), die bei einer tatsächlichen Bewegung des Roboters aufgenommen wurden, stimmen hingegen nahezu überein. Die Einschränkung des EKF-Verfahrens auf die Messwerte einer tatsächlichen Bewegung scheint in diesem Testversuch nicht notwendig zu sein y in [m] x in [m] Abbildung 10: Darstellung der ermittelten Roboterpositionen in der Testfläche mit Hindernissen im Büro: Die EKF-Positionen sind in blau dargestellt und die grüne Messkurve zeigt die gesamten Rohpositionen, die aus den Messungen des Sensors MPU6050 ermittelt wurden. In rot sind die Rohpositionen, die bei einer tatsächlichen Bewegung des Roboters aufgenommen wurden, dargestellt. Um das Verhalten des EKF-Lokalisierungsverfahrens weiter zu untersuchen, wurde das entwickelte SLAM-Verfahren in einer zweiten Umgebung getestet. Die Testfläche war ein rechteckiger, 18 m 2 m großer, leerer Flur. Der Roboter wurde am Anfang des Flures nahe der rechten Wand platziert und in verschiedenen Bewegungsmustern entlang des Flures bewegt. Die EKF-Positionen und Rohpositionen sind in Abbildung 11 dargestellt. Anders als in Abbildung 10 ist ein deutlicher Unterschied zwischen den Rohpositionen und den EKF-Positionen erkennbar. Im ersten Teil des zurückgelegten Weges ((0 m, 0 m) bis (0 m, 8 m)) ist nur eine Verschiebung zwischen der Rohposition und EKF-Position in y-richtung zu erkennen. Die Ursache könnte sein, dass die EKF-Position die wahre Position

59 5.1 EKF-Lokalisierung y in [m] x in [m] Abbildung 11: Darstellung der ermittelten Roboterpositionen in der weitläufigen Testfläche ohne Hindernisse im Flur: Die EKF-Positionen sind in blau dargestellt und die grüne Messkurve zeigt die gesamten Rohpositionen, die aus den Messungen des Sensors MPU6050 ermittelt wurden. In rot sind die Rohpositionen, die bei einer tatsächlichen Bewegung des Roboters aufgenommen wurden, dargestellt. besser als die Rohposition beschreibt und somit der EKF korrekt funktioniert. Möglich ist jedoch auch, dass Fehler im EKF aufgetreten sind, welche zu falsch berechneten Positionen führen.

60 50 5 EXPERIMENTELLE ERGEBNISSE Der weitere Verlauf der Rohpositionen ab ( 1 m, 8,5 m) und den EKF-Positionen ab (0,5 m, 9 m) lassen jedoch einen Fehler in der vom EKF berechneten Orientierung des Roboters vermuten. Dieser Anschein entsteht, da die Rohpositionen die beobachtete Fahrstrecke des Roboters besser beschreiben als die EKF-Positionen und das Verlaufsmuster der EKF-Positionen und der Rohpositionen gleich ist. Da die Orientierung des Roboters seine Fahrtrichtung im globalen System angibt (siehe Abschnitt 2.1.1), führt ein Fehler in der Orientierung zu einer starken Abweichung der nachträglich vom EKF berechneten Roboterpositionen in x- als auch in y-richtung von den Rohpositionen. Zusammenfassend ist festzuhalten, dass die Bewegung des Roboters durch die EKF- Lokalisierung grundsätzlich abgebildet wird und eine Weiterverarbeitung der Messdaten des Accelerometer- und Gyroskopesensors vom EKF zu erkennen ist. 5.2 Kartenerstellung Nachdem die Ergebnisse des EKF-Lokalisierungsverfahrens im letzten Abschnitt erläutert wurden, sollen im Folgenden die mit Hilfe des EM-Algorithmus erstellten Karten präsentiert und näher betrachtet werden. Anhand des Testlaufes im Büro soll zunächst die Kartenerstellung demonstriert werden. Während der Bewegung des Roboters findet eine Kartenerstellung der Umgebung anhand der Ultraschalldistanzmessungen statt. Sobald der EM-Algorithmus die beste Karte zu den ihm momentan vorliegenden Distanzmessungen erstellt hat, wird, ausgehend von dieser und einer aktuellen Distanzmessung, das Erstellen einer neuen Karte begonnen. Die so erstellten Einzelkarten sind in der Abbildung 13 zu sehen. Zur Darstellung der Karte werden unterschiedlich farbige Pixel, die jeweils eine Gitterzelle darstellen, verwendet. Die weißen Pixel symbolisieren frei markierte Gitterzellen und die schwarzen belegte. Die Graufärbung bedeutet, dass dieser Bereich noch nicht vom Sensor erfasst wurde und sind streng genommen kein Teil der Umgebungskarte, die dem Roboter bekannt ist. Der rote Pixel stellt die Position des Roboters in der Umgebungskarte dar. In der Karte 13a ist die Auswertung der ersten Distanzmessung dargestellt, die aus der Startposition des Roboters mit dem Ultraschallsensor aufgenommen wurde. Wird die Karte 13a genau betrachtet, ist der Messkegel des Ultraschallsensors anhand der schwarzen und weißen Pixel zu sehen. Die Distanzmessung liefert den Abstand vom Roboter zu einem Hindernis, aber keine Informationen über den genauen Ort. Alle Gitterzellen im Messkegel, die den ermittelten Abstand vom Roboter aufweisen, werden daher als belegt markiert.

61 5.2 Kartenerstellung 51 Dadurch ergibt sich die bogenartige Anordnung der schwarzen Pixel am äußeren Rand des Messkegels in der Karte 13a. Die Karte 13b wurde, nachdem der Roboter sich bewegt hat, erstellt. Dies ist deutlich an der Verschiebung der Roboterposition (roter Pixel) zu sehen. Durch die Bewegung des Roboters können neue Bereiche der Umgebung durch den Ultraschallsensor erfasst werden, wodurch die erstellte Umgebungskarte erweitert wird. So kann iterativ die gesamte Umgebung des Roboters erfasst werden, was in den Karten 13c bis 13h zu sehen ist. Dabei stellt die Abbildung 13h die letzte erstellte Umgebungskarte dar. (a) #167 (b) Skizze der Testumgebung (c) #231 Abbildung 12: Vergleich der Skizze der Testumgebung im Büro mit den erstellten Umgebungskarten mit Kartennummer 167 und 231: Mit rot und grün sind zwei Hindernisse, die in beiden Karten abgebildet wurden, markiert. Werden die erstellten Karten 13g und 13h mit der Skizze der Umgebung in Abbildung 9 verglichen, fällt auf, dass die Karte 13g die skizzierte Umgebung besser darstellt als die Karte 13h. Der Vergleich der beiden Karten mit der Skizze der Testumgebung ist separat in Abbildung 12 dargestellt. Die grün- und rot-markierten Hindernisse in Abbildung 12b können anhand der belegten Gitterzellen im gleichen Bereich der Karte 12a wiedergefunden werden. Bei der großen freien Fläche, die in der Karte 12a unten links zu sehen ist, handelt es sich um einen Gang, den der Roboter erfasst hat. In der Skizze ist dieser nur angedeutet, da dieser Gang kein Teil der Testumgebung war. Auch in der letzten Karte 12c lassen sich die markierten Hindernisse in der Umgebung erahnen, jedoch ist die Form der Hindernisse nicht erkennbar. Im Vergleich zu der Karte 12a sind die Hindernisse außerdem deutlich verschoben und der zuvor erkannte Gang in der unteren linken Ecke der Karte wird nicht mehr als solcher erkannt. Dabei handelt es sich jedoch nicht um einen Fehler in der Kartenerstellung, sondern um die Folge der Einstellung,