Simultane Lokalisierung und Kartenerstellung eines autonomen Roboters in einer dynamischen Umgebung bei Anwesenheit von Personen

Transkript

1 Simultane Lokalisierung und Kartenerstellung eines autonomen Roboters in einer dynamischen Umgebung bei Anwesenheit von Personen Diplomarbeit im Studiengang Angewandte Informatik Fakultät für Informatik Technische Universität Dortmund vorgelegt von Colin Doert Gutachter: Prof. Dr.-Ing. Gernot A. Fink Dipl.-Ing. Jan Richarz

2

3 Erklärung Ich erkläre hiermit, dass ich die vorliegende Arbeit selbständig verfasst, keine ausser den angegebenen Quellen und Hilfsmitteln verwendet sowie alle Zitate gekennzeichnet habe. Dortmund, Colin Doert

4

5 Inhaltsverzeichnis 1 Einleitung Motivation und Zielsetzung Motivation Zielsetzung Aufbau der Arbeit Grundlagen Robotermodell Bewegungsmodell Sensormodell Kartenrepräsentation Merkmalsbasierte Karten Positionsbasierte Karten Lokalisierung Markov Lokalisierung Monte Carlo Lokalisation Simultane Lokalisierung und Kartenerstellung Historische Entwicklung Mathematische Fundierung des Grundproblems Lösungsansätze für das SLAM Problem EKF SLAM Rao-Blackwellized Partikelfilter Verbesserter rasterbasierter Rao-Blackwellized Particle Filter Betrachtung dynamischer Umgebungen Herausforderungen durch dynamische Umgebungen Klassifikation der dynamischen Änderungen Zeitliche Beobachtbarkeit der Änderungen Einfluß auf die Lokalisierung Variation des Beobachtungsmodells Variation der Kartenrepräsentation Klassifikation der Sensormessungen Segmentierung Detektion geradliniger Strukturen Detektion dynamischer Objekte I

6 Inhaltsverzeichnis 5 Anwesenheit von Personen Überblick über Verfahren zur Beindetektion Verwendetes Verfahren zur Beindetektion Verfahrensüberblick Durchmesser Varianz der inneren Winkel Hintergrundabstand Position des Segmentmittelpunktes Segmentierung von zwei Beinen Kumulierter Gütewert Beinpaardetektion Implementierung Technische Rahmenbedingungen Hardware des Roboters Verwendete Softwarekomponenten und Bibliotheken Komponenten der Implementierung Sensordatenerfassung Segmentierung & Klassifikation der Sensormessungen Partikelfilter Evaluation SLAM für statische Umgebungen Darstellung und Interpretation der Ergebnisse Experiment Experiment Experiment Experiment Evaluation der Erweiterungen für dynamische Umgebungen Simulation einer dynamischen Umgebung Experimente Klassifikation der Laserabstandsmessungen Kantenerkennung Beindetektion Fazit Zusammenfassung und Ausblick 105 Literaturverzeichnis 107 II

7 1 Einleitung Der erste Abschnitt dieses Kapitels soll zunächst grundlegend in das Thema dieser Arbeit sowie deren Zielsetzung einführen. Der zweite Abschnitt gibt anschließend einen Überblick über den Aufbau der gesamten Arbeit. 1.1 Motivation und Zielsetzung Motivation In den vergangenen Jahren konnte eine Vielzahl an Forschungserfolgen im Bereich der autonom agierenden Roboter beobachtet werden. Es zeichnen sich bereits unterschiedliche Einsatzmöglichkeiten sowohl im industriellen Umfeld als auch im täglichen Leben ab. Dabei sollen die Roboter möglichst intuitiv mit den Menschen in ihrer Umgebung interagieren und beispielsweise Aufgaben als Museumsführer oder Nachtwächter übernehmen. Auch Anwendungen im häuslichen Bereich sowie in der Alten- und Krankenpflege geraten zunehmend in den Blickpunkt. Eine grundlegende Erfordernis dieser Aufgaben ist die Fähigkeit des Roboters sich in einer fremden Umgebung zu orientieren und dort anschließend sicher zu navigieren. Diese Grundanforderung an autonome Roboter lässt sich auf die Probleme der Kartenerstellung und der Lokalisierung zurückführen. Es ist offensichtlich, dass diese beiden Probleme aufgrund ihrer Interdependenz nicht getrennt voneinander betrachtet werden können, sondern mit Hilfe eines simultanen Prozesses bewältigt werden müssen Zielsetzung Im Rahmen dieser Arbeit soll die Umsetzung einer simultanen Lokalisierung und Kartenerstellung auf einer mobilen Roboterplattform vom Typ SCITOS G5 erfolgen. Ausgehend von den aktuellen Forschungsansätzen soll unter Verwendung der Daten eines Laserscanners und der Odometrie eine probabilistische Repräsentation der Umgebung sowie der Position des Roboters gewonnen werden. Es soll dabei möglich sein, Beschreibungen verschiedener Umgebungen in Form von Karten zu speichern und diese bei erneutem Aufenthalt in der selben Umgebung mit aktuell erzeugten Karten zu fusionieren. Ein geplanter Einsatzbereich des Roboters ist ein weitläufiges Labor mit einer Unterteilung in verschiedene Sektionen. Dieser Bereich stellt eine Vielzahl von Herausforderungen an die geplante Implementierung. So gibt es zahlreiche Hindernisse wie Tischbeine, Stühle oder Personen, deren Positionen sich im Zeitverlauf ändern können. 1

8 1 Einleitung 1.2 Aufbau der Arbeit In Kapitel 2 werden zunächst die theoretischen Grundlagen der probabilistischen Robotik vorgestellt. Diese bilden die Basis für das in Kapitel 3 eingeführte Problem der simultanen Lokalisierung und Kartenerstellung. Neben der geschichtlichen Entwicklung und einer mathematischen Fundierung dieses Problems werden allgemeine Lösungsansätze sowie das im weiteren verwendete Verfahren präsentiert. In Kapitel 4 erfolgt eine Betrachtung der Eigenschaften von dynamisch veränderlichen Umgebungen sowie der damit verbundenen Schwierigkeiten für die simultane Lokalisierung und Kartenerstellung. Weiterhin wird ein Lösungsansatz präsentiert, um die dynamischen Aspekte in das gewählte Verfahren integrieren zu können. Da die Betrachtung der von anwesenden Personen hervorgerufenen Veränderungen der Umgebung einen Schwerpunkt der vorliegenden Arbeit darstellen, erfolgt in Kapitel 5 eine Beschreibung, wie diese erkannt und behandelt werden sollen. Im darauf folgenden Kapitel wird der gegebene Rahmen für die Umsetzung einer Lösung vorgestellt. Anschließend werden die einzelnen Komponenten der entwickelten Lösung sowie die vollständige Funktionsweise der Implementierung beschrieben. Kapitel 7 stellt die durchgeführten Experimente und deren Ergebnisse dar. Hierbei wird insbesondere ein Vergleich zwischen den Resultaten der Kartenerstellung und Lokalisierung mit und ohne Berücksichtigung der dynamischen Umgebungsveränderungen gezogen. Zum Abschluss gibt Kapitel 8 eine Zusammenfassung der geleisteten Arbeit und stellt Ideen und Ansätze für weitere Untersuchungen vor, welche im vorliegenden Zeitrahmen nicht mehr umgesetzt werden konnten. 2

9 2 Grundlagen 2.1 Robotermodell Um die Interaktion eines Roboters mit seiner Umgebung zu beschreiben, werden in den folgenden Abschnitten zwei mathematische Modelle vorgestellt. Diese bilden eine Grundlage für die in den weiteren Kapiteln dargestellten probabilistischen Verfahren Bewegungsmodell Dieser Abschnitt befasst sich in erster Linie mit der Kinematik eines Roboters. Der Schwerpunkt der Darstellung wird dabei auf einem probabilistischen Bewegungsmodell liegen, wie es von Thrun ausführlich diskutiert wurde (vgl. [40]). Unter dem Begriff Kinematik versteht man die Lehre der Bewegung von Punkten im Raum, welche Größen wie Weg, Geschwindigkeit und Beschleunigung betrachtet, aber die wirkenden Kräfte außer Acht lässt. Aufgabe der Roboterkinematik ist es den Einfluss von Steuersignalen auf den Zustand bzw. die Pose (Position und Orientierung) des Roboters zu beschreiben. Diese wird durch einen Vektor repräsentiert, dessen Dimension sowohl von der betrachteten Umgebung als auch von den Freiheitsgraden der Roboterbewegung abhängen. Ein Zustandsvektor, der eine freie Positionierung in einem dreidimensionalen Raum beschreiben soll, benötigt beispielsweise 6 Dimensionen. Die x-, y- und z-komponente eines Punktes im Raum sowie eine dreidimensionale Repräsentation der Orientierung (z.b. durch eulersche Winkel oder Yaw-Pitch-Roll). Die in dieser Arbeit vorgestellte Anwendung wird für eine Roboterplattform (vgl ) entwickelt, die sich lediglich auf einer zweidimensionalen Ebene bewegen kann. Die Pose des Roboters zu einem Zeitpunkt t kann damit durch einen Vektor der Form x x t = y (2.1) θ dargestellt werden. x und y entsprechen hierbei den Koordinaten in der Ebene und θ der Orientierung gemessen im Bogenmaß. Unter einer Orientierung von θ = 0 sei die Fahrtrichtung des Roboters dabei entlang der x-achse ausgerichtet (siehe Abbildung 2.1). Geschwindigkeits-Bewegungsmodell Abhängig von der vorliegenden Art der Steuersignale kann die Bestimmung der Posenänderung mit verschiedenen Methoden durchgeführt werden. Wird das Steuersignal 3

10 2 Grundlagen y y t ɵ t 0 x t x Abbildung 2.1: Pose des Roboters ausgedrückt durch seine Position (x t, y t ) in globalen Koordinaten sowie seine Orientierung θ t bezüglich der x-achse. u t zu einem Zeitpunkt t durch die Geschwindigkeit der Translationsbewegung v t und der Rotationsbewegung ω t ausgedrückt, so kann die Roboterbewegung mit Hilfe eines Geschwindigkeits-Bewegungs-Modells bestimmt werden. Die Geschwindigkeit zum Zeitpunkt t ausgedrückt in Weltkoordinaten lässt sich dann berechnen durch: x cos(θ(t)) 0 ( ) y = f(θ 0, v, ω, t) = sin(θ(t)) 0 v θ ω 0 1 mit θ(t) = θ 0 + ωt (2.2) wobei θ 0 der Ausgangsorientierung zum Zeitpunkt t = 0 entspricht. Die zeitpunktabhängige Position ( x(t) y(t) ) T lässt sich durch Integration der jeweiligen Komponente von f(.) bestimmen. Geschindigkeitsvektoren werden in der Regel als Steuersignale verwendet, wenn eine Pfadplanung durchgeführt werden soll. Diese lassen sich leicht in Rotationsgeschwindigkeiten der einzelnen Räder und somit in Signale zur Ansteuerung der jeweiligen Motoren überführen. Odometrie-Bewegungsmodell Für Anwendungen wie Kartenerstellung oder Lokalisierung und somit auch für das vorliegende Szenario sind Steuergrößen zur Planung der Bewegung von untergeordnetem Interesse. In erster Linie soll bestimmt werden welche Pose x t ausgehend von der vorherigen Pose x t 1 nach Ausführung des Steuersignals u t tatsächlich erreicht wurde. Zu diesem Zweck kann in der Regel auf die interne Odometrie-Sensorik des Roboters zurückgegriffen werden. Diese ermöglicht die Bestimmung der zurückgelegten Strecke 4

11 2.1 Robotermodell mit Hilfe von Rotationssenoren an den angetriebenen Rädern. Die zum Zeitpunkt t erreichte Pose wird dabei in der Form ˆx t = (ˆx ŷ ˆθ) T bereitgestellt. Die Koordinaten sind dabei in den meisten Fällen relativ zur Startposition des Roboters. Auch wenn es sich dabei strenggenommen um Messwerte handelt, werden diese für die weiteren Betrachtungen als Steuersignale der Form betrachtet. ˆx t ˆx t 1 u t = ŷ t ŷ t 1 (2.3) ˆθ t ˆθ t 1 Probabilistisches Bewegungsmodell Insbesondere die Lokalisierung wäre offensichtlich kein Forschungsfeld der Robotik, wenn tatsächlich zu jedem Zeitpunkt mit Hilfe der Odometrie die Pose des Roboters ermittelt werden könnte. In der Realität sind die Messungen der Odometriesensorik fehlerbehaftet. Ursache für diese Messfehler sind vor allem der von der Oberflächenbeschaffenheit abhängige Schlupf der Räder sowie Unebenheiten des Untergrundes. Die Messung der Radrotation hingegen erfolgt in der Regel mit einer sehr hohen Auflösung (bspw. 450 Messpunkte/Rotation), so dass dort auftretende Messfehler eher unbedeutend sind. Da die Posenbestimmung mit Hilfe der Odometrie ein inkrementeller Prozess ist, wächst der potentielle Odometriefehler kontinuierlich. Sei x T die tatsächliche und ˆx T die aufgrund der Odometrie vermutete Pose des Roboters zum Zeitpunkt T sowie ɛ t der in Schritt t aufgetretene Odometriefehler, so gilt für den Gesamtfehler der Posenschätzung offensichtlich: T x T ˆx T = ɛ t (2.4) Die Charakteristik des kumulierten Messfehlers ist sowohl von der Hardware des Roboters als auch vom zurückgelegten Pfad abhängig. Sie kann in Form des probabilistischen Bewegungsmodells P (x t u t, x t 1 ) (2.5) beschrieben werden. Es wird angenommen, dass es sich bei der Bewegung um einen Markov-Prozess handelt, die erreichte Pose x t also nur von der letzten bekannten Pose x t 1 und dem ausgeführten Steuersignal u t abhängig ist. Der Rest des zurückgelegten Pfades (x 0,..., x t 2 ) muss in diesem Fall nicht betrachtet werden. Abbildung 2.2 stellt in zwei Fällen exemplarisch die Verteilung der erwarteten Position (ohne Betrachtung der Orientierung) nach dem Abfahren des rot markierten Pfades dar. Die Unsicherheit über die erreichte Pose steigt hier mit der Komplexität des zurückgelegten Weges. Eine Modellierung des Positionsfehlers lässt sich sowohl für ein Odometrie- als auch für ein Geschwindigkeits-Bewegungsmodell durchführen. Da letzteres für die folgenden Betrachtungen nicht relevant ist, wird für eine Beschreibung der Fehlerabschätzung auf Thrun verwiesen [40]. t=1 5

12 2 Grundlagen Abbildung 2.2: Odometriefehler dargestellt als a posteriori Verteilung der Roboterposition nach Ausführung der durch den roten Pfad repräsentierten Steuersignalfolge. Dunkle Grauwerte entsprechen einer hohen Wahrscheinlichkeitsdichte. [40] rot,2 trans rot,1 Abbildung 2.3: Modellierung des Odometriefehlers durch Zerlegung einer Bewegung des Roboters in zwei Rotations- (δ rot,1 und δ rot,2 ) sowie eine Translationsbewegung (δ trans ). (Nach [40]) 6

13 2.1 Robotermodell Das auf Odometrieinformationen basierende Fehlermodell nach den Ausführungen von Thrun (siehe [40]) soll im weiteren erläutert werden. Die Pose zum Zeitpunkt t wird hierbei durch eine Zufallsvariable x t modelliert. In diskreten Zeitintervallen (bspw. t 80ms) stellt das Robotersystem Odometriemessungen zur Abschätzung dieser Zufallsvariablen zur Verfügung. Für hinreichend kleine Zeitintervalle kann angenommen werden, dass der Übergang zwischen zwei Posen auf direktem Weg erfolgt. Um den Messfehler in verschiedene Komponenten zu zerlegen, soll die gemessene Posenänderung für das Zeitintervall (t 1, t] (von Pose x t 1 zu x t ) vereinfacht durch drei Teilbewegungen (wie in Abbildung 2.3 dargestellt) angenähert werden: 1. Rotation um δ rot1 auf die Verbindungsachse zwischen den Positionen der Posen x t 1 und x t 2. Translationsbewegung der Länge δ trans bis zur Position der Pose x t 3. Rotation um δ rot2 um die Orientierung der Pose x t einzunehmen Die einzelnen Komponenten lassen sich folgendermaßen berechnen: δ rot1 = atan2(y t y t 1, x t x t 1 ) θ t 1 (2.6) δ trans = (x t x t 1 ) 2 + (y t y t 1 ) 2 (2.7) δ rot2 = θ t 1 θ t δ rot1 (2.8) Jede dieser Teilbewegungen kann fehlerbehaftet sein. Im vorliegenden Modell trifft Thrun dabei die Annahme eines normalverteilten Fehlers ɛ b mit Erwartungswert 0 und Varianz b, der jeweils von dem tatsächlichen Wert der Rotations- und Translationsbewegung abgezogen wird. ˆδ rot1 = δ rot1 ɛ α1 δ rot1 +α 2 δ rot2 (2.9) ˆδ trans = δ trans ɛ α3 δ trans +α 4 δ rot1 +δ rot2 (2.10) ˆδ rot2 = δ rot2 ɛ α1 δ rot2 +α 2 δ trans (2.11) Die Varianz der jeweiligen Fehlergröße ist dabei von den Beträgen der verschiedenen Teilbewegungen abhängig, die mit den roboterspezifischen Fehlerparametern α 1 bis α 4 gewichtet werden. Später soll das Bewegungsmodell genutzt werden, um die Wahrscheinlichkeit zu bestimmen, mit der x t der tatsächlichen Pose entspricht, obwohl die Odometriemessungen ˆx t als gegenwärtige Pose angeben. Hierfür werden sowohl die Bewegung von x t 1 nach ˆx t als auch nach x t wie oben beschrieben in ihre drei Komponenten zerlegt. Anschließend können die Fehler, die hätten auftreten müssen, um bei Vorliegen der Odometriemessung ˆx t zur Pose x t zu gelangen, entlang jeder Teilbewegung bestimmt werden: δ rot1 = δ rot1 ˆδ rot1 (2.12) δ trans = δ trans ˆδ trans (2.13) 7

14 2 Grundlagen δ rot2 = δ rot2 ˆδ rot2 (2.14) Sei N b (x) die Dichtefunktion einer Normalverteilung mit Erwartungswert 0 und einer Varianz von b, so lassen sich die Wahrscheinlichkeiten für die Fehler in den Komponenten folgendermaßen berechnen: p rot1 = N α1 δ rot1 +α 2 δ rot2 ( δ rot1 ) (2.15) p trans = N α3 δ trans +α 4 δ rot1 +δ rot2 ( δ trans ) (2.16) p rot2 = N α1 δ rot2 +α 2 δ trans ( δ rot2 ) (2.17) Unter der weiteren Annahme, dass die Fehlerkomponenten der Teilbewegungen unabhängig sind, lässt sich die Wahrscheinlichkeit die Pose x t erreicht zu haben ausdrücken durch: P (x t x t 1, u t ) = p rot1 p trans p rot2 mit u t = ˆx t x t 1 (2.18) Sensormodell Ein autonomer Roboter nutzt Sensoren, um Informationen über seine Umgebung zu gewinnen. Gegenwärtig gibt es eine ganze Reihe verschiedener Sensorarten, die in diesem Umfeld zum Einsatz kommen. Das Spektrum reicht von eher einfachen Berührungssensoren über ultraschall- und laser-basierte Abstandsensoren bis hin zu Stereokamera- Systemen. Die Art des verwendeten Sensormodells ist dabei in erster Linie von der Technik des Sensors abhängig. Die folgenden Betrachtungen beziehen sich auf Abstandssensoren und dabei insbesondere auf Laserabstandssensoren (engl. laser range finder), da diese im Kontext dieser Arbeit Verwendung finden. Laser-Abstandssensoren Die Bestimmung des Abstandes eines Objektes kann prinzipiell nach zwei Methoden erfolgen. Eine Klasse der Abstandssensoren verwendet die Methode der Triangulation, bei der die Winkel zwischen zwei Referenzpunkten und dem Messpunkt ermittelt werden. Unter Kenntnis des Abstandes der Referenzpunkte lassen sich daraus die Seitenlängen des mit dem Messpunkt gebildeten Dreiecks und somit der Abstand des Messpunktes bestimmen. Eine weitere Klasse der Abstandssensoren sendet Signale aus und bestimmt Abstände anhand der von den Objekten zurückgeworfenen Reflektionen dieser Signale. Ein derartiges Vorgehen wird aufgrund des ausgesandten Signals auch als aktive Abstandsmessung bezeichnet. Als Signal können dabei Schall- sowie elektromagnetische Wellen (z.b. Radar) oder Licht- bzw. Laserstrahlen zum Einsatz kommen. Zur Ermittlung der Entfernung aus dem reflektierten Signal existieren verschiedene Verfahren. Je nach Beschaffenheit des Signals (Wellenlänge, Ausbreitungsgeschwindigkeit) können Phasenverschiebungen zwischen dem ausgesandten und dem empfangenen Signal verwendet werden um Abstände zu bestimmen (vgl. [35]). In den meisten Fällen wird allerdings die Laufzeit gemessen, die zwischen dem Aussenden und der Detektion des reflektierten Signals 8

15 2.1 Robotermodell verstreicht. Abhängig von der Ausbreitungsgeschwindigkeit des Signals im vorliegenden Medium c ergibt sich die Entfernung d bei einer gemessenen Laufzeit t zu: d = c t (2.19) 2 Der im Rahmen dieser Arbeit verwendete Laser-Abstandssensor arbeitet nach dem Prinzip der Pulslaufzeitmessung, d.h. der Laserstrahl wird in diskreten Pulsen emittiert, so dass die Abstandsmessungen mit einer hohen Frequenz erfolgen können. Der Sensor enthält einen rotierenden Spiegel, der eine Aussendung des Laserstrahls unter variablen Winkeln ermöglicht (siehe Abb. 2.4). Die Reflektion des Strahls wird mit Hilfe des Spiegels auf ein Detektorelement umgelenkt. Da die Rotationsgeschwindigkeit des Spiegels im Verhältnis zur Ausbreitungsgeschwindigkeit des Lichtes sehr gering ist, erfolgt eine Messung, bevor sich der Winkel des Spiegels zu sehr geändert hat. reflektierter Strahl rotierender Spiegel emittierter Strahl reflektierter Strahl LED/Laser Detektor Abbildung 2.4: Aufbau eines Laser-Abstandssensors: Der durch den Laser emittierte Strahl wird nach der Reflexion an einem Hindernis vom Detektor erfasst. Die Abstandsbestimmung erfolg mit Hilfe einer Laufzeitmessung. Der rotierende Spiegel ermöglicht Messungen unter variierendem Messwinkel. (Nach [35]) Probabilistisches Sensormodell Die Beobachtung eines Laser-Abstandssensors z t zum Zeitpunkt t entspricht einer Menge individueller Abstandsmessungen unter verschiedenen Messwinkeln: z t = {z 1 t,..., z K t } (2.20) 9

16 2 Grundlagen Bei einem minimalen Messwinkel von θ min und einer Winkelauflösung von δ (z.b. 0,5 ) ergeben sich die zur Abstandsmessung z k t gehörenden Messwinkel δ k = δ min + (k 1) δ. (2.21) Das probabilistische Sensormodell kann (wie von Thrun in [40] ausführlich erläutert) in Form der bedingten Wahrscheinlichkeitsverteilung P (z t x t, m) modelliert werden. Diese beschreibt die Wahrscheinlichkeit, mit der eine Beobachtung z t unter Kenntnis der Karte m und der zum Zeitpunkt t eingenommenen Pose x t gemacht werden kann. Zur Bestimmung dieser Wahrscheinlichkeit werden die einzelnen Komponenten der Messung betrachtet. P (zt k x t, m) ist hierbei die Wahrscheinlichkeit, die Abstandsmessungen zt k bei einem Messwinkel δ k sowie gegebener Karte und Pose zu erhalten. Trifft man die idealisierte Annahme, dass die Messfehler der einzelnen Komponenten unabhängig verteilt sind, so erfolgt die Berechnung der Messungswahrscheinlichkeit durch: p(z t x t, m) = K p(zt k x t, m) (2.22) k=1 Die Verteilung der Abstandskomponenten kann nach Thrun durch ein Strahlenmodell dargestellt werden. Er unterscheidet vier Bestandteile der Verteilung (vgl. Abb. 2.5): 1. Korrekter Abstand mit lokalem Messfehler: Eine Gaußverteilung, die ihren Erwartungswert an der tatsächlichen Entfernung z k t des nächsten unter dem Winkel θ k beobachtbaren Hindernisses hat, repräsentiert den idealen Fall, in dem die Abstandsmessung nur durch einen geringen lokalen Messfehler mit Standardabweichung σ korrekt verrauscht wird. Dieser Fehler ergibt sich in der Regel durch die begrenzte Auflösung des Sensors. Setzt man voraus, dass der Sensor weder negative Abstandswerte noch Werte oberhalb der maximalen Messreichweite z max liefert, so gilt für die Verteilung: P korrekt (z k t x t, m) = { ηn (z k t ; z k t, σ 2 korrekt ) falls 0 zk t z max 0 sonst (2.23) Dabei ist N (zt k ; zt k, σkorrekt 2 ) die Dichtefunktion einer Normalverteilung mit Erwartungswert zt k und Varianz σkorrekt 2. Der Normalisierungsfaktor η = ( zmax 0 N (z k t ; z k t, σ 2 korrekt )dzk t ) 1 (2.24) ist an dieser Stelle notwendig, damit P korrekt (z k t x t, m) eine gültige Dichtefunktion einer Wahrscheinlichkeitsverteilung darstellt. 2. Unerwartete Objekte: Die Wahrscheinlichkeit, Objekte zu beobachten, die bisher nicht in der Karte verzeichnet sind, lässt sich nach Thrun auf dem Intervall [0; z k t ] durch eine Exponentialverteilung beschreiben. Die Wahl der Verteilung 10

17 2.1 Robotermodell Gauss-Verteilung P z t k x t,m P korrekt Exponential-Verteilung P z t k x t,m P unerw z k t z max z z k t z max z Gleichverteilung P max Gleichverteilung P zufall P z t k x t,m P z t k x t,m z k t z max z z k t z max z Abbildung 2.5: Komponenten des probabilistischen (Sensor-)Strahlenmodells nach Thrun: P korrekt repräsentiert die Verteilung von Messungen mit korrektem Abstand (entspr. d. Karte) und lokalem Messfehler, P unerw die Wahrscheinlichkeit für die Messung eines unerwarteten Objektes, P max die Wahrscheinlichkeit für das Auftreten von Abstandsmessungen maximaler Länge. P zufall spiegelt die Verteilung zufälliger Messungen wider. (Nach [40]) 11

18 2 Grundlagen kann aus der Annahme begründet werden, dass zufällige Objekte in jedem Abstand mit gleicher Wahrscheinlichkeit auftreten, der Lasersensor aber jeweils nur den Abstand des nächsten Objektes ermittelt. Außerhalb dieses Intervalls ist die Wahrscheinlichkeit ein unerwartetes Objekt zu beobachten 0, da zuvor das Objekt mit Abstand zt k hätte beobachtet werden müssen. Für unerwartete Objekte ergibt sich folgende Verteilung: P unerw (z k t x t, m) = { ηλunerw e λunerwzk t falls 0 z k t z k t 0 sonst (2.25) Hierbei ist λ unerw der Parameter der Exponentialverteilung und η ein Normalisierungsfaktor, der sicherstellt, dass es sich bei P unerw (zt k x t, m) um eine Dichtefunktion handelt. 3. Messungsausfall: Es ist möglich, dass eine Entfernungsmessung bei bestimmten Oberflächenbeschaffenheiten vollständig ausfällt. Raue schwarze Flächen könnten den Laserstrahl beispielsweise vollständig absorbieren. Glas hingegen könnte den Strahl möglicherweise gar nicht reflektieren sondern vollständig durchlassen. In diesen Fällen entspricht der gemessene Abstandswert zt K nicht dem tatsächlichen Objektabstand zt k, sondern dem maximal messbaren Abstand z max. In der daraus resultierenden Verteilung ist die gesamte Wahrscheinlichkeitsmasse dementsprechend auf den Punkt z max konzentriert: P max (z K t x t, m) = { 1 falls z k t = z max 0 sonst (2.26) 4. Zufällige Messungen: Zur Erklärung von gelegentlich auftretenden, völlig zufällig erscheinenden Messungen schlägt Thrun vor, eine Gleichverteilung über den gesamten Messbereich [0; z max ] zu verwenden: P zufall (z k t x t, m) = { 1 z max falls 0 zt k z max 0 sonst (2.27) Da das Phänomen vollig zufällig erscheinender Messungen bei Verwendung von Laser-Abstandssensoren (im Gegensatz beispielsweise zu Ultraschall-Abstandssensoren) kaum zu beobachten ist, sollte der Einfluss dieser Verteilungskomponente auf die kombinierte Verteilung gering gewählt werden. Die kombinierte Verteilung einer Messung kann als gewichteter Durchschnittswert aus den vier Komponenten bestimmt werden. T z korrekt P korrekt (z k t x t, m) P (zt k x t, m) = z unerw z max P unerw (zt k x t, m) P max (zt k x t, m) (2.28) z zufall P zufall (zt k x t, m) Für die Gewichtungsparameter z korrekt,z unerw,z max,z zufall gilt dabei z korrekt + z unerw + z max + z zufall = 1. 12

19 2.2 Kartenrepräsentation 2.2 Kartenrepräsentation Ein Schwerpunkt der vorliegenden Arbeit ist die Kartierung einer unbekannten Umgebung. Als Informationen stehen dem Roboter hierfür lediglich die Sensormessungen (üblicherweise Abstandsmessungen eines Laser Range Finders oder Ultraschall Sensors) zur Verfügung, welche in geeigneter Weise in die Karte eingetragen werden müssen. Für die Wahl einer geeigneten Kartenrepräsentation sind in Anlehnung an Siegwart und Nourbakhsh ([35]) zunächst drei fundamentale Beziehungen zwischen der Anwendung und der Darstellung der Karte zu beachten: 1. Die Auflösung der Karte wird durch die Präzision, die der Roboter zur Erfüllung seiner Aufgaben benötigt, nach unten begrenzt. 2. Die Art der repräsentierten Merkmale ist abhängig von den verfügbaren Sensordaten. Die Sensorauflösung stellt eine obere Begrenzung der Kartenauflösung dar. 3. Die Komplexität der Kartendarstellung hat direkten Einfluss auf den im Rahmen der Aufgaben Kartengenerierung, Lokalisierung und Navigation benötigten Rechenaufwand. Unter Kenntnis dieser Zusammenhänge und der Gegebenheiten des Anwendungsszenarios kann eine Abwägung der Vor- und Nachteile möglicher Kartenrepräsentationen, von denen zwei Grundlegende im folgenden erläutert werden, erfolgen. Nach Thrun ([40]) kann eine Karte formal als Liste von Objekten der Umgebung aufgefasst werden: m = {m 1, m 2,..., m N } (2.29) Für jedes Objekt m i werden charakteristische Eigenschaften gespeichert, die von der Art der Kartenrepräsentation abhängig sind. Üblicherweise erfolgt die Differenzierung der wahrgenommenen Objekte entweder mit einem merkmalsbasierten oder mit einem positionsbasierten Ansatz Merkmalsbasierte Karten Merkmalsbasierte Verfahren segmentieren die Sensormessungen und detektieren Landmarken gemäß des zugrunde liegenden Merkmals. Die Kartenobjekte m i entsprechen in diesem Fall den gefundenen Landmarken. Als Landmarken kommen in der Regel passive, statische Objekte in Frage, deren Position anhand der Sensordaten mit einer hohen Genauigkeit bestimmt werden kann. Die verwendeten Landmarken sind in erster Linie von der Art der Sensordaten abhängig. Kommt eine Kamera zum Einsatz, so können beispielsweise Deckenlampen oder markante Farbstrukturen als Landmarken gewählt werden. Stehen, wie im vorliegenden Fall, Abstandsinformationen zur Verfügung, so müssen charakteristische Merkmale in diesen gefunden werden (z.b. gradlinige Wände, Ecken, Türen usw.). Die Landmarken lassen sich dabei einteilen in natürlich in der Umgebung vorhandene (z.b. Türen) und künstlich für die Roboternavigation eingerichtete Landmarken (z.b. eindeutige farbige Markierungen). Landmarken des letzten Typs können eine Detektion und Identifikation und somit 13

20 2 Grundlagen eine genaue Lokalisierung wesentlich vereinfachen. Sie entsprechen allerdings nicht dem Bestreben, den Einsatz von autonomen Robotersystemen in beliebigen Umgebungen zu ermöglichen. Die Wahl des verwendeten Merkmals bzw. der verwendeten Merkmale erfolgt üblicherweise unter Beachtung des Szenarios bereits während der Entwicklung. Es gibt aber auch Ansätze, die mit Hilfe maschinellen Lernens (z.b. neuronaler Netze) eine dynamische Auswahl der verwendeten Merkmale/Landmarken durchführen (vgl. Thrun [38]). Im Rahmen der Kartenrepräsentation werden für jede Landmarke neben der Position in kartesischen Koordinaten Informationen zur Merkmalsausprägung, die eine Identifikation der Landmarke ermöglichen, abgespeichert. Die Anzahl der Kartenobjekte N ist nicht nur von der Größe des kartierten Bereichs sondern vor allem von der Wahl der Merkmalsart sowie von der Häufigkeit, mit der Objekte dieses Merkmals in der untersuchten Umgebung auftreten, abhängig. Die Beziehungen zwischen verschiedenen Landmarken werden bei Karten dieses Typs in der Regel in Form eines Graphen gespeichtert. Aus diesem Grund werden derartige Kartenrepräsentationen auch als topologische Karten bezeichnet. Das Abstraktionsniveau der Karten kann dabei variieren und ist abhängig von den verwendeten Landmarken. Die Knoten im Graphen können beispielsweise einzelne beobachtete Merkmale (z.b. Ecken) darstellen, wobei die Kanten des Graphen dann den Entfernungen zwischen den Merkmalen entsprächen. Es sind aber auch topologische Karten höheren Abstraktionsgrades möglich, bei denen Knoten beispielsweise Räume repräsentieren, die anhand mehrerer Merkmale identifiziert werden können. Die Kanten in einer solchen Darstellung könnten passierbaren Wegen zwischen den Räumen entsprechen. Da insbesondere bei maschinell (durch autonome Roboter) erzeugten Karten eine Zahl von Fehlerquellen die Identifikation und Positionsbestimmung der Landmarken beeinflussen kann, werden oftmals probabilistische Ansätze verwendet um die approximierten Landmarkenpositionen zu ermitteln. Die Schätzung der verschiedenen Landmarkenpositionen m i ( x i y i ) T können dabei als stochastisch abhängige Zufallsvariablen betrachtet werden. Die Unsicherheiten der Karte können somit durch die Kovarianzmatrix Σ ausgedrückt werden. Eine derartige probabilistische Karte kommt unter anderem bei dem in Kapitel beschriebenen Verfahren zum Einsatz Positionsbasierte Karten Bei positionsbasierten oder metrischen Kartenrepräsentationen entsprechen die Kartenobjekte m i nicht Landmarken sondern Positionen im zu kartierenden Raum/ in der zu kartierenden Ebene. In einem zweidimensionalen Kontext kann somit die Indizierung der Kartenobjekte entsprechend ihrer kartesischen Koordinaten vorgenommen werden. Element m x,y der Karte m umfasst somit die Eigenschaften des Punktes (x, y) der beschriebenen Umgebung. Offensichtlich muss an dieser Stelle eine Diskretisierung der Ebene erfolgen. Die Wahl der Auflösung ist in der Regel abhängig von den zur Verfügung stehen den Ressourcen wie Speicherkapazität und Rechenleistung sowie von der Präzision der Sensormessungen. Sie hat direkten Einfluss auf die Anzahl vorhandener Kartenobjekte N und somit auf die Qualität und den Informationsgehalt der resultierenden 14

21 2.3 Lokalisierung Karte. Trägt jedes Objekt m x,y der Karte (bei positionsbasierten Karten auch als Kartenzelle bezeichnet) Information über die Existenz von Hindernissen an der Stelle (x, y), so spricht man von einer Belegungsrasterkarte (engl. occupancy grid map) wie sie von Moravec vorgestellt wurde (vgl. [30]). Die Kartenzellen repräsentieren in diesem Fall binäre Zufallsvariablen. Gilt für eine Zelle m x,y = 1 so enthält sie ein Hindernis. Gilt m x,y = 0 so ist die zu dieser Zelle gehörende Fläche frei von Hindernissen. Wie in Abschnitt 2.1 erläutert wird, sind sowohl die Positionsschätzungen eines autonomen Roboters als auch seine Sensormessungen in der Regel fehlerbehaftet. Da diese Unsicherheiten die Beobachtung der tatsächlichen Ausprägung der Zufallsvariablen m x,y verhindern, kann nur der Wert für P (m x,y = 1) bzw. P (m x,y ) ermittelt werden, der aufgrund der vorherigen Beobachtungen beschreibt, mit welcher Wahrscheinlichkeit die Zelle belegt ist. Die hier beschriebene Kartenrepräsentation speichert diese Information für jede Zelle der Karte (siehe Abb. 2.6). In einer unbekannten Umgebung sind die Wahrscheinlichkeiten für die Anwesenheit eines Hindernises für alle Zellen zunächst gleich groß. Ist bekannt, wie groß das Verhältnis von belegten zu freien Flächenstücken in der Umgebung ist, so kann dies als Grundlage für einen initialen Wahrscheinlichkeitswert verwendet werden. Üblicherweise wird ein Wert P (m x,y ) = 0, 5 für alle Zellen gewählt, um auszudrücken, dass die Anwesenheit und Abwesenheit eines Hindernisses in einer Zelle gleich wahrscheinlich sind. Bewegt sich der Roboter durch die zu kartierende Umgebung, so ändern sich die Wahrscheinlichkeiten für Hindernisse entsprechend der Sensorbeobachtungen dieser Zellen. Eine detailierte Beschreibung eines solchen Verfahrens wird in Kapitel gegeben. Abbildung 2.6: Ausschnitt einer occupancy grid map. Repräsentation der Hinderniswahrscheinlichkeit durch Grauwerte. 2.3 Lokalisierung Unter dem Begriff Lokalisierung versteht man das Problem, bei Kenntnis der Karte m die gegenwärtige Pose x t des Roboters relativ zur Karte zu ermitteln. Die dem Roboter hierbei zur Verfügung stehenden Informationen sind die Sensormessungen z t sowie die 15

22 2 Grundlagen Odometriedaten ˆx t. Folgt man der Argumentation von Thrun ([40]), so lässt sich das Problem auch als Ermittlung einer zum Zeitpunkt t gültigen Koordinatentransformation f t (.) zwischen den Odometriekoordinaten ˆx t und den globalen Koordinaten der Karte x t auffassen: x t = f t (ˆx t ) (2.30) Diese ist leider in der Regel nicht eindeutig bestimmbar. Versteht man den initialen Schritt der Lokalisation zunächst als Suche nach der Pose x t in der Karte, die durch die aktuelle Beobachtung z t am stärksten gestützt wird, erkennt man, dass neben den in den Sensormessungen impliziten Unsicherheiten vor allem Mehrdeutigkeiten der Karte (vgl. Abb. 2.7) ein Problem darstellen. Derartige Mehrdeutigkeiten können bei fortlaufender Lokalisierung während der Bewegung des Roboters möglicherweise beseitigt werden. Um diese Zusammenhänge geeignet zu modellieren verwenden die im folgenden beschriebenen Ansätze eine probabilisitsche Form der Darstellung. Abbildung 2.7: Beispiel einer mehrdeutigen Umgebung bzw. Karte. Nur aufgrund der Sensorinformationen kann nicht zwischen der linken und der rechten Pose des Roboters unterschieden werden. Das Problem der Roboterlokalisierung existiert in unterschiedlichen Ausprägungen. Diese lassen sich nach Thrun ([40]) anhand verschiedener Dimensionen klassifizieren. Bezogen auf die Unkenntnis über die Pose unterscheidet man zwischen: 1. Lokale Lokalisierung: Bei diesem auch unter dem Begriff der Positionsverfolgung (engl. position tracking) bekannten Problem gibt es eine Schätzung der gegenwärtigen Pose, die sich in der Regel aus einer bekannten Startposition sowie der Odometrie des Roboters ergibt. Die Unsicherheit bezüglich dieser Schätzung resultiert aus dem Bewegungsmodell des Roboters (vgl ) und kann in der Regel unimodal beispielsweise durch eine gauss sche Verteilung über dem Posenraum dargestellt werden. 2. Globale Lokalisierung: Bei einem Problem dieser Art ist die Pose des Roboters bezüglich der Karte vollkommen unbekannt. Somit ist die Posenschätzung über dem Posenraum zunächst gleichverteilt. Gestüzt durch die Sensorbeobachtungen entwickelt die Verteilung über den Zeitverlauf lokale Maxima, welche den wahrscheinlichsten Posen entsprechen. 16

23 2.3 Lokalisierung Betrachtet man die Umgebung, in der die Lokalisierung erfolgen soll, so lassen sich grob folgende Ausprägungen unterscheiden: 1. Lokalisierung in statischer Umgebung: Bei dieser Form der Lokalisierung ist die Umgebung, in der sich der Roboter bewegt, bezogen auf seine Sensorwahrnehmung unveränderlich. Die Pose x t des Roboters ist somit die einzige Variable in diesem System. Diese Restriktion ermöglicht eine effiziente probabilistische Modellierung der Pose. Sie ist in vielen Umgebungen insbesondere bei Anwesenheit von Personen in der Regel nicht erfüllt. 2. Lokalisierung in dynamischer Umgebung: Sind in der Umgebung nicht alle Kartenelemente statisch, so spricht man von Lokalisierung in einer dynamischen Umgebung. Hierbei kann unterschieden werden zwischen zeitweiligen Veränderung (z.b. der kurzzeitigen Sichtung einer Person) und andauernden Veränderungen (z.b. dem Verschieben von Mobiliar). Vor allem letztere stellen eine große Herausforderung dar, weil hierdurch die Informationen der Karte fehlerhaft werden und gegebenenfalls aktualisiert werden müssen. In den folgenden beiden Abschnitten sollen zwei bekannte Lokalisierungsansätze kurz vorgestellt werden Markov Lokalisierung Die Markov Lokalisierung ist ein probabilistisches Verfahren, das erfolgreich in verschiedenen Arbeiten zum Einsatz kam (vgl. bspw. Cassandra et al. [7], Burgard et al. [6] oder Fox et al. [19]). Es dient der Bestimmung einer Wahrscheinlichkeitsverteilung (im folgenden als Belief bezeichnet) über alle möglichen Roboterposen P (x t ) zum Zeitpunkt t. Die Berechnung dieser Verteilung erfolgt rekursiv auf Grundlage des Beliefs des vorherigen Zeitpunktes P (x t 1 ). Dabei werden die Beliefs jeweils unter Verwendung des entsprechenden Modells aktualisiert, sobald eine Bewegung oder eine Sensormessung durchgeführt wurde. Bewegungsmodell Wie in Kapitel beschrieben, kann die Bewegung des Roboters in Form der bedingten Wahrscheinlichkeitsverteilung P (x t u t, x t 1 ) ausgedrückt werden. Sie entspricht der Wahrscheinlichkeit, mit der eine Pose x t erreicht wird, wenn ausgehend von der vorherigen Pose x t 1 das Steuersignal u t ausgeführt wird. Der Belief jeder möglichen Pose P (x t ) kann wie folgt berechnet werden: P (x t ) = P (x t x t 1, u t )P (x t 1 )dx t 1 (2.31) Die Beliefs jeder potentiellen vorherigen Pose x t 1 werden dabei entsprechend der Wahrscheinlichkeit, mit der die untersuchte Pose x t von dort aus erreicht wird, gewichtet und kumuliert. 17

24 2 Grundlagen Beobachtungsmodell Das Beobachtungsmodell P (z t x t, m) (vgl. Kapitel 2.1.2) beschreibt die Wahrscheinlichkeit, mit der eine Beobachtung z t gemacht werden kann, wenn die Karte m bekannt ist und der Roboter sich an der Pose x t befindet. Ausgehend davon können die Beliefs für jede potentielle Pose P (x t ) aktualisiert werden durch: P (x t ) = ηp (z t x t 1, m)p (x t 1 ) (2.32) η ist dabei ein Normalisierungsfaktor, der sicher stellt, dass das Integral über alle Beliefs 1 ergibt. Illustration Das klassische Beispiel für die Markov Lokalisierung im eindimensionalen Fall zeigt Abbildung 2.8. Hierbei entspricht die Darstellung der Wand mit den Türen der bekannten Karte. Der Belief für die gegenwärtige Pose (in diesem Fall nur die x-komponente) ist in kontinuierlicher Form dargestellt. Zu Beginn der Sequenz (Abb. a) herrscht völlige Unsicherheit über die Position des Roboters. Dies wird ausgedrückt durch eine Gleichverteilung der Beliefs. Nach Verarbeitung der ersten Sensorinformationen (in Abbildung b rot dargestellt) gibt es drei Hypothesen für die gegenwärtige Position, die sich in den Maxima der Beliefs widerspiegeln. Die vor Übergang zu Abbildung c durchgeführte Bewegung des Roboters führt zu einer erhöhten Unsicherheit in den Positionshypothesen. Eine weitere Beobachtung, die in Abbildung d hinzu kommt, unterstützt insbesondere eine Hypothese wodurch der Belief an dieser Stelle sein absolutes Maximum annimmt. Mit Aufnahme der Bewegung (Abbildung e) steigt wiederum die Unsicherheit dieser Hypothese. Umsetzung Um dieses Verfahren in der Praxis einsetzen zu können, wird üblicherweise von der kontinuierlichen Darstellung der Beliefs abgewichen und eine Diskretisierung der Kartenpositionen in Form eines Rasters eingeführt. Die oben vorgestellte Aktualisierung der Beliefs nach einer Bewegung (Formel 2.31) wird dann zu: P (x t ) = x t 1 P (x t x t 1, u t )P (x t 1 ). (2.33) Um eine globale Lokalisierung durchzuführen bekommen alle potentiellen Posen, d.h. 1 Zellen im diskretisierten Posenraum, einen identischen initialen Belief von N xn yn θ, wobei N x, N y, N θ der Anzahl der Kartenzellen entlang der jeweiligen Dimension entspricht. Zur Durchführung einer Positionsverfolgung bei bekannter initialer Pose x 0 bekommt diese den initialen Belief P (x 0 ) = 1. Besteht lediglich eine Schätzung der initialen Pose so kann diese durch Auswertung einer multivariaten Gauß-Verteilung für die entsprechenden Zellen der Beliefs angenähert werden. 18

25 2.3 Lokalisierung a) P x t b) x t P x t x t P z t x t c) x t P x t d) x t P x t x t P z t x t x t e) P x t x t Abbildung 2.8: Darstellung einer eindimensionalen Markov Lokalisierung. Für jeden Schritt ist der Positionsbelief P (x t ) dargestellt. In Bild b) und d) wird weiterhin die Beobachtungswahrscheinlichkeit für eine Tür entlang des Korridors P (z t x t ) abgebildet. (Abb. analog zu [40]) 19

26 2 Grundlagen Monte Carlo Lokalisation Ein weiteres zur Roboterlokalisierung unter Kenntnis der Karte eingesetztes Verfahren ist die Monte Carlo Lokalisierung, wie sie von Fox et al. vorgestellt wurde (vgl. [18]). Die grundlegende Idee des Verfahrens ist die Approximation der Wahrscheinlichkeitsverteilung der gegenwärtigen Pose P (x) (Belief) durch einen Partikelfilter. Bei diesem Verfahren werden die Beliefs für einzelne Posen x i durch eine gewichtete Partikelmenge S = {s i i = 1..N} dargestellt. Ein Partikel s i = (x i, w i ) enthält dabei neben seiner Pose x i eine positive Gewichtung w i welche die Wahrscheinlichkeit beschreibt, mit der dieser Partikel die tatsächliche Pose repräsentiert. Analog zu den Schritten der Markov Lokalisierung erfolgt die Aktualisierung der Beliefs nach einer Bewegung mit Hilfe eines Bewegungsmodells sowie nach einer Sensorbeobachtung unter Verwendung eines Sensormodells. Aktualisierung der Beliefs nach einer Bewegung Nachdem der Roboter eine Bewegung u t durchgeführt hat, werden aus der Partikelmenge S t 1 des vorherigen Zeitschrittes N Partikel jeweils mit der Wahrscheinlichkeit gezogen, die durch ihre Gewichtung repräsentiert wird. Die neue Pose x t jedes dieser Partikel wird durch Ziehen aus der Verteilung P (x t x t 1, u t ) des Bewegungsmodells betimmt, wobei x t 1 die Pose des jeweiligen Partikels aus dem vorherigen Zeitschritt ist. Wenn von einer bekannten Startposition ausgegangen wird und somit alle N Partikel zum Zeitpunkt t = 0 eine identische Pose haben, kann man beobachten, dass die Streuung der Partikelposen aufgrund der Bewegungsunsicherheit in jedem Zeitschritt zunimmt. Aktualisierung der Beliefs durch eine Sensormessung Die durch eine Sensormessung z t gewonnenen Informationen werden ebenfalls zur Aktualisierung der Beliefs verwendet. Entsprechend des Sensormodells werden die Partikelgewichte neu berechnet: w i = ηp (z t x t, m) (2.34) Der Normalisierungsfaktor η wird so gewählt, dass i w i = 1 gilt. Eigenschaften des Verfahrens Fox et al. erwähnen, dass die Ergebnisse des Verfahrens robuster werden, wenn nach jedem Schätzungsschritt einige über der Karte gleichverteilte Partikel gezogen werden. Diese ermöglichen eine Relokalisierung in dem seltenen Fall, dass die Verfolgung der Roboterposition zuvor misslungen ist und die Partikel somit an einer falschen Position konvergiert sind. Durch das gleichverteilte Sampling befinden sich in der Nähe der tatsächlichen Position zumindest ein paar Partikel, so dass diese Hypothese aufgrund der Sensorinformationen erneut erstarken kann. Ein großer Vorteil dieses Verfahrens gegenüber der Markov Lokalisierung ist, dass die Beliefs über die Positionen bei vergleichbarem Rechenaufwand mit einer wesentlich höheren Genauigkeit approximiert werden können. Durch die beliebige Positionierung 20

27 2.3 Lokalisierung der Punkte, an denen die Wahrscheinlichkeitsverteilung P (x) ausgewertet wird, kann der Informationsgehalt in der Umgebung guter Positionshypothesen wesentlich höher ausfallen, als es bei dem festgelegten Raster der Markov Lokalisierung der Fall ist. 21

28 2 Grundlagen 22

29 3 Simultane Lokalisierung und Kartenerstellung Im folgenden Kapitel soll zunächst eine kurze historische Zusammenfassung über die Entwicklung der simultanen Lokalisierung und Kartenerstellung gegeben werden. Im Anschluss daran wird eine mathematische Formulierung des Grundproblems vorgestellt. Darauf aufbauend geben die folgenden Abschnitte einen Überblick über die zwei meist verwendeten Lösungsansätze. Zum Abschluss des Kapitels wird die Erweiterung eines der Lösungsansätze vorgestellt, welche die Grundlage für die in Kapitel 6 beschriebene Implementierung darstellt. 3.1 Historische Entwicklung Die Entwicklungsgeschichte der simultanen Lokalisierung und Kartenerstellung angelehnt an die Ausführungen von Durrant-Whyte und Bailey [16] soll im weiteren kurz vorgestellt werden. In der Mitte der 1980er Jahre begann die Betrachtung des gegenwärtig unter dem Akronym SLAM (simultaneous localization and mapping) bekannten Aufgabenfeldes als probabilistisches Problem. Im Rahmen der IEEE Robotics and Automation Conference (1986) in San Francisco fiel zunächst der Fokus auf konsistente, probabilisitsche Kartenerstellung als ein Kernproblem der Robotik, woraufhin in den folgenden Jahren grundlegende statistische Modelle zur Schätzung von Landmarkenpositionen und dabei insbesondere der Korrelation zwischen diesen entwickelt wurden (vgl. [37], [15]). Neben der Kartenerstellung fanden auch Verfahren zur Roboternavigation unter Verwendung von Kalmanfiltern verstärkt Beachtung (bspw. bei Crowley [9] und Chatila & Laumond [8] ). Die Vereinigung dieser zwei Disziplinen wurde von Smith et al. in deren Arbeit zur Schätzung von räumlichen Unsicherheiten in der Robotik erreicht[36]. Erstmals wurde mit dieser Arbeit der Zusammenhang zwischen der fehlerbehafteten Schätzung der Roboterposition und der Korrelation zwischen den Landmarkenpositionen gezogen. Hieraus ergab sich die Folgerung, dass eine konsistente Lösung des SLAM Problems mit Hilfe einer Zustandsschätzung einen Zustandsvektor verwenden muss, der sowohl die Roboterposition als auch sämtliche Landmarkenpositionen umfasst, wodurch die hierbei verwendete Kovarianzmatrix (und somit auch die Laufzeit) quadratisch in der Zahl der Landmarken wachsen muss. Entscheidende Fortschritte ergaben sich in den folgenden Jahren vor allem durch die Erkenntnis, dass eine Lösung des SLAM Problems als kombiniertes Schätzungsproblem über die Roboter- und Landmarkenpositionen konvergiert. Die Korrelation zwischen den Landmarkenpositionen, welche zuvor möglichst minimiert wurde, stellte sich als Schlüssel für die Konvergenz des Problems heraus (vgl. Durrant-Whyte et. al. [17] und Csorba [11],[10]). In der Folgezeit nahmen die Forschungs- 23

30 3 Simultane Lokalisierung und Kartenerstellung aktivitäten in diesem Bereich zu. Auf dem International Symposium on Robotics Research (1999) kamen dann zu den kalmanfilter-basierten Methoden die von Thrun et al. [39] entwickelten probabilistischen Verfahren zur Lokalisierung und Kartengenerierung. Dieser stellte auch 2002 mit Montemerlo und weiteren das FastSLAM Verfahren [27] auf der Basis eines Partikelfilter vor, welches in Kapitel erläutert wird und eine Grundlage für die im Rahmen dieser Arbeit entwickelte Implementierung darstellt. 3.2 Mathematische Fundierung des Grundproblems In diesem Abschnitt soll die mathematische Formulierung des SLAM Problems nach Durrant-Whyte [16] eingeführt werden. Aufbauend auf dieser Terminologie können dann in den anschließenden Abschnitten Lösungsansätze für das SLAM Problem vorgestellt werden. Grundlage für die folgenden Betrachtungen ist ein mobiler, autonomer Roboter, der in der Lage ist, die (zu seiner Pose relativen) Positionen verschiedener Landmarken mit Hilfe seiner Sensoren zu ermitteln. Das Modell verwendet diskrete Zeitschritte k = 1..m. Zum Zeitpunkt k wird das System durch folgende Größen beschrieben: Der Zustandsvektor x k repräsentiert die Pose (Position und Orientierung) des Roboters. Das Steuersignal u k entspricht der angestrebten Positions- und Orientierungsänderung beim Übergang vom Zeitpunkt k 1 zum Zeitpunkt k. Die Menge m = {m i i = 1..n} umfasst die tatsächlichen Positionen m i aller n Landmarken. Die zu einem Zeitpunkt k beobachteten Positonen der Landmarken relativ zur Roboter Pose werden duch die Menge z k = {z i,k i = 1 : n} repräsentiert, wobei z i,k die jeweils beobachtete Position einer Landmarke i beschreibt. Die Historie der bisher beobachteten zeitpunktbezogenen Größen ist definiert durch X 0:k = {x 0,..., x k } für die Roboterposen (Trajektorie), U 0:k = {u 0,..., u k } für die Steuersignale und Z 0:k = {z 0,..., z k } für alle bis zum Zeitpunkt k beobachteten Landmarkenpositionen. Betrachtet man nun die Zustandsübergänge im Modell, so sollte zunächst zur Kenntnis genommen werden, dass eine Zustandsänderung in Form einer Bewegung des Roboters stets mit Unsicherheit behaftet ist. Diese resultiert aus den Odometriefehlern, welche beispielsweise durch den Schlupf der Räder, Unebenheiten des Untergrundes sowie Ungenauigkeiten der Rotationssensorik hervorgerufen werden. Um dieser Unsicherheit gerecht zu werden, beschreibt das Modell den Zusammenhang in einer probabilisitischen Form. Hierbei werden die fehlerbehafteten Zustandsübergänge durch Wahrscheinlichkeitsverteilungen repräsentiert. Eine Lösung des Problems der simultanen Kartenerstellung und Lokalisierung entspricht somit der Bestimmung der Wahrscheinlichkeitsverteilung P (x k, m Z 0:k, U 0:k, x 0 ) (3.1) zu jedem Zeitpunkt k. Die tatsächliche Verbundverteilung von Roboterpose x k und Landmarkenpositionen m bei gegebener Startposition x 0 und bekannter Vorgeschichte der bisher beobachteten Landmarkenpositionen Z 0:k sowie der zuvor angewandten Steuersignale U 0:k entspricht der höchsten Genauigkeit, die bei einer fehlerbehafteten Positions- und Kartenbestimmung zum Zeitpunkt k erreicht werden kann. Auffällig ist, dass zu jedem 24

31 3.2 Mathematische Fundierung des Grundproblems Zeitpunkt k die gesamte Historie von Beobachtungen und Steuersignalen in die Bestimmung der Verteilung mit einfließt. Dieser Zusammenhang würde eine Approximation der Verteilung erschweren, da mit zunehmender Fahrzeit k des Roboters der Rechenaufwand stetig zunehmen würde. Aus diesem Grund ist eine rekursive Repräsentation des Modells wünschenswert. Hierfür wird zunächst ein Beobachtungsmodell aufgestellt, welches die Wahrscheinlichkeit, eine bestimmte Beobachtung zu machen, wenn sowohl die gegenwärtige Pose als auch die tatsächlichen Landmarkenpositionen bekannt sind, in Abhängigkeit der Sensorgenauigkeit beschreibt (siehe auch Abschnitt 2.1.2). Es entspricht somit der Wahrscheinlichkeitsverteilung: P (z k x k, m). (3.2) Weiterhin beschreibt das Bewegungsmodell die Wahrscheinlichkeit, mit der ein Zustandsvektor x k zum Zeitpunkt k erreicht werden kann, wenn der vorherige Zustand x k 1 sowie das ausgeführte Steuersignal u k bekannt sind. Unter der Annahmen, dass es sich bei diesem Zustandsübergang um einen Markov Prozess handelt, ist der gegenwärtige Zustand nur von seinem direkten Vorgänger und dem letzten Steuersignal abhängig. Sowohl die tatsächliche Karte als auch die beobachteten Landmarkenpositionen haben keinen Einfluss auf den Übergang. Somit ergibt sich die Übergangswahrscheinlichkeit aus der Verteilung: P (x k x k 1, u k ). (3.3) Um nun die zur Lösung des SLAM Problems gesuchte Verteilung (Formel 3.1) zu berechnen, wird ein zweistufig-rekursives Verfahren verwendet. Es handelt sich dabei um einen rekursiven bayes schen Filter. Im ersten Schritt wird eine Schätzung der a priori Verbundverteilung für Pose und Landmarkenpositionen unter Nutzung des Bewegungsmodells (Zeit-Update) in der folgenden Form (unter Verwendung des Gesetzes der totalen Wahrscheinlichkeiten) ermittelt: P (x k, m Z 0:k 1, U 0:k, x 0 ) = P (x k x k 1, u k ) P (x k 1, m Z 0:k 1, U 0:k 1, x 0 )dx k 1 (3.4) Nachdem die Bewegungsinformationen in Form des Steuersignals u k zur Bestimmung der a priori Verteilung verwendet wurde, finden die Beobachtungsinformationen z k im folgenden Schritt Einfluss auf die Berechnung der Verbundverteilung. Mit Hilfe des Beobachtungsmodells (Formel 3.2) lässt sich die a posteriori Verteilung folgendermaßen bestimmen: P (x k, m Z 0:k, U 0:k, x 0 ) = P (z k x k, m)p (x k, m Z 0:k 1, U 0:k, x 0 ) P (z k Z 0:k 1, U 0:k ) (3.5) Diese Vorgehensweise, welche auch unter dem Namen Prädiktor-Korrektor-Methode bekannt ist, stellt die mathematische Grundlage für alle nachfolgend betrachteten SLAM Lösungsansätze dar. 25

32 3 Simultane Lokalisierung und Kartenerstellung 3.3 Lösungsansätze für das SLAM Problem Nachdem der mathematische Unterbau des SLAM Problems im vorherigen Abschnitt erläutert wurde, sollen an dieser Stelle die grundlegenden Lösungsansätze für dieses Problem vorgestellt werden. Dabei konzentrieren sich die Ausführungen auf jene zwei Verfahren, die vielfach in der Literatur Erwähnung finden. Einen Überblick über weitere, weniger bekannte, Ansätze gibt [2]. Die im Weiteren vorgestellten Lösungsansätze für das SLAM Problem verwenden jeweils die dargestellte rekursive Form der Berechnung des bayes schen Filters, nutzen aber unterschiedliche Verfahren um Beobachtungs- und Bewegungsmodell sowie die gesuchte Wahrscheinlichkeitsverteilung zu approximieren EKF SLAM Verfahren, die auf dem erweiterten Kalmanfilter (EKF) basieren, sind in der einschlägigen Literatur sehr verbreitet. Die Grundlage hierfür bildet der als Kalmanfilter bezeichnete und von Kalman entwickelte Zustandsschätzer für lineare normalverteilte Systeme [25]. Der diskrete Kalmanfilter-Algorithmus schätzt die Paramenter Erwartungswert µ t und Kovarianz Σ t einer Normalverteilung zu einem Zeitpunkt t in einem zweistufig-rekursiven Verfahren. Die ursprüngliche Variante des Kalmanfilters kann nur auf Systeme mit linearen Prozessmodellen der Form x = Ax + Bu + ɛ (3.6) z = Hx + ω (3.7) angewendet werden, wobei A, B und H lineare Matrizen sind und ɛ und ω gauss sche Fehlergrößen darstellen. Da diese Bedingung in der Praxis oftmals nicht erfüllt ist, kommt im folgenden die abgewandelte Variante des erweiterten Kalmanfilters zum Einsatz. Diese unterliegt nicht der Linearitätsanforderung. Allerdings ist durch die implizite Linearisierung der Übergangsfunktionen mit einem Genauigkeitsverlust zu rechnen. Unter Verwendung eines Zustandsraummodells werden hierbei Störungen und Unsicherheiten ebenfalls durch (multivariate) Gaußverteilungen approximiert. Das Bewegungsmodell lässt sich dementsprechend mit Hilfe einer deterministischen (nicht notwendigerweise linearen) Bewegungstransformation f(x k 1, u k ) sowie einer additiven gauss schen Störung w k darstellen: P (x k, x k 1, u k ) x k = f(x k 1, u k ) + w k (3.8) Die zu w k gehörige Kovarianzmatrix soll als Q k bezeichnet werden. Analog lässt sich das Beobachtungsmodell als Summe einer deterministischen Koordinatentransformation h(x k, m) und einer unkorrelierten gauss sche Störung v k mit Erwartungswert 0 und Kovarianzmatrix R k verstehen: P (z k x k, m) z k = h(x k, m) + v k (3.9) h(.) überführt dabei die in der bisherigen Karte m verzeichneten Landmarkenpositionen aus dem globalen Koordinatensystem der Karte in zur derzeitigen Pose x k relative 26

33 3.3 Lösungsansätze für das SLAM Problem Koordinaten entsprechend der Sensorbeobachtungen. Sie beschreibt also, wie die Beobachtungen im fehlerlosen Fall aussehen müssten. Die Linearisierung des EKF erfolgt durch eine Taylor-Approximation erster Ordnung, wobei die Matrizen A und B des klassischen Kalmanfilters durch die Jacobimatrix f sowie H durch die Jacobimatrix h ersetzt werden. Um die gesuchte a posteriori Verteilung (Formel 3.1) mit Hilfe einer multivariaten Gaußverteilung zu approximieren, müssen die notwendigen Parameter in Form eines Erwartungswertvektors [ˆx ] T k k ˆm k und einer Kovarianzmatrix Pk k bestimmt werden: [ ] Pxx P P k k = xm P T xm P mm [ˆxk k k k ˆm k ] [ ] xk = E m Z 0:k [ (xk ) ( ) T ˆx = E k xk ˆx k Z m ˆm k m ˆm 0:k k ] (3.10) (3.11) Bei einem Zustandsübergang wird zunächst eine Prädiktion auf Basis des verwendeten Steuersignals in Form eines Zeit-Updates durchgeführt: ˆx k k 1 = f(ˆx k 1 k 1, u k ) (3.12) P xx,k k 1 = fp xx,k 1 k 1 f T + Q k (3.13) Offensichtlich werden an dieser Stelle nur die zur Pose gehörenden Teile des Erwartungswertvektors und der Kovarianzmatrix aktualisiert. Die Sensorinformationen finden anschließend im Rahmen des Beobachtungsupdates Einfluss auf die aktuelle Schätzung. Man verwendet die Jacobimatrix h der Funktion h an der Stelle x k k 1 und m k 1, um die Matrizen S k und W k entsprechend S k = hp k k 1 h T + R k (3.14) W k = P k k 1 h T S 1 k (3.15) zu bestimmen. Sie beinhalten Informationen, welche den Messfehler an der vermuteten Pose ˆx k k 1 unter Verwendung der angenommenen Karte ˆm k 1 quantifizieren. Ausgehend davon kann der Erwartungswertvektor um die entsprechend des Fehlermaßes W k gewichtete Differenz zwischen tatsächlicher Messung z k und der erwarteten Messung h(ˆx k k 1, ˆm k 1 ) korrigiert werden. ] = ˆm k [ˆxk k [ˆxk k 1 ˆm k 1 ] + W k [z k h(ˆx k k 1, ˆm k 1 )] (3.16) Weiterhin kann die Kovarianzmatrix P k k 1 unter Kenntnis der aus der Messung zu erwartenden Unsicherheiten, dargestellt durch W k und S k, folgendermaßen aktualisiert werden: P k k = P k k 1 W k S k W T k (3.17) 27

34 3 Simultane Lokalisierung und Kartenerstellung Im Rahmen einer simultanen Kartenerstellung und Lokalisierung nach dem oben beschriebenen Verfahren konvergiert typischer Weise die Determinante der Kovarianzmatrix P mm,k sowie die jeder Kovarianzmatrix eines Landmarkenpaares gegen 0 (nach [12]). Durch die Aktualisierung der vollständigen Kovarianzmatrix während jedes Beobachtungsschrittes haben derartige Verfahren allerdings einen Rechenaufwand, der quadratisch in der Zahl der Landmarken ist. Ein bisher nicht angesprochenes Problem, welches kritischen Einfluss auf die Leistungsfähigkeit eines SLAM Verfahrens hat, ist das Problem der Datenzuordnung (im angelsächsischen Sprachraum unter dem Begriff data association bekannt). Es tritt auf, da mit Hilfe der Messungen nicht nur die (möglicherweise fehlerbehaftete) Position der Landmarken ermittelt werden muss, sondern diese Landmarken jeweils auch identifiziert und (mit der Ausnahme von erstmalig gesehenen Landmarken) einer zuvor beobachteten zugeordnet werden müssen. Verfahren, die nach dem EKF Prinzip arbeiten, sind gegenüber in diesem Schritt auftretenden Fehlern sehr empfindlich. Auch die bei EKF-SLAM verwendete Linearisierung von nicht-linearen Bewegungsund Beobachtungsmodellen ist mit Nachteilen verbunden, welche nach Ausführungen von Julier et al. [24] zu inkonsistenten Lösungen führen können Rao-Blackwellized Partikelfilter Ein weiterer zur simultanen Kartenerstellung und Lokalisierung vielfach verwendeter Ansatz, der auch eine Grundlage für die im folgenden präsentierte Methode darstellt, entspricht dem von Montemerlo et al. vorgestellten FastSLAM Algorithmus [16]. Dieses Verfahren basiert auf dem Prinzip des Partikelfilters. Hierbei werden Wahrscheinlichkeitsverteilungen mit Hilfe einer gewichteten Partikelmenge approximiert. Die Partikel erhalten jeweils eine Position im Partikelraum x (i) sowie eine Partikelgewichtung w (i). Für die Summe der Partikelgewichte gilt: N i=1 w(i) = 1. Im Beispiel einer eindimensionalen Verteilung P (x) (vgl. Abb. 3.1) kann der approximierte Erwartungswert folgendermaßen aus der Partikelmenge bestimmt werden: E(x) = + xp (x)dx = N w (i) x (i) (3.18) i=1 Ein Vorteil des Partikelfilter-Ansatzes gegenüber dem EKF Verfahren ist die Möglichkeit nicht nur gauss sche sondern beliebige Wahrscheinlichkeitsverteilungen darstellen zu können. Da der gegenwärtige Zustand einer SLAM Lösung sowohl Informationen zur aktuellen Pose als auch zu sämtlichen Landmarkenpositionen beinhalten muss, zeichnet sich der Zustandsraum durch eine hohe Dimensionalität aus. Um die a posteriori Verteilung eines solchen Zustandes vollständig mit Hilfe eines Partikelfilters zu approximieren, müssten die Partikel über sämtliche Dimensionen des Zustandsraum gesamplet werden. Offensichtlich ist für eine ausreichende Repräsentation eine große Anzahl von Partikeln erforderlich. Diese wächst exponentiell mit der Anzahl der Landmarken. Somit scheint eine direkte Anwendung eines Partikelfilters zur Lösung des SLAM Problems nicht praktikabel. 28

35 3.3 Lösungsansätze für das SLAM Problem p x x x Abbildung 3.1: Darstellung einer Verteilung p durch Partikelmengen: Repräsentation der Wahrscheinlichkeitsdichte durch die Partikeldichte (mittlere Abb.) sowie durch eine Partikelgewichtung (untere Abb.). Die Zahl der Zustandsraumdimensionen lässt sich allerdings durch Anwendung einer sogenannten Rao-Blackwellization wesentlich reduzieren. Unter Rao-Blackwellization versteht man das Verfahren, eine Verbundverteilung P (x 1, x 2 ) gemäß der Produktregel zu zerlegen: P (x 1, x 2 ) = P (x 2 x 1 ) P (x 1 ) (3.19) Dieses Vorgehen bietet sich an, falls die bedingte Wahrscheinlichkeitsverteilung P (x 2 x 1 ) in analytischer Form beschrieben werden kann. Zur Approximation der Verbundverteilung durch einen Partikelfilter wäre in diesem Fall nur noch ein Sampling der Verteilung x (i) 1 P (x 1 ) notwendig. Die gesuchte Verteilung wird dann durch die Menge {x (i) 1, P (x 2 x (i) 1 )}N i dargestellt. Zur Reduzierung der Zustandsraumdimensionen des SLAM Problems bietet sich die Zerlegung der Verbundverteilung in eine Bewegungs- und eine Kartenkomponente an: P (X 0:k, m Z 0:k, U 0:k, x 0 ) = P (X 0:k Z 0:k, U 0:k, x 0 )P (m X 0:k, Z 0:k ) (3.20) Die gewichtete Partikelmenge S k = {w (i) k, X(i) 0:k, P (m X(i) 0:k, Z 0:k)} N i (3.21) beschreibt die gesuchte Posenverteilung. Jeder Partikel der Menge enthält eine eigene Repräsentation der Verteilung der Landmarkenpositionen. Diese werden als unabhängige Gaußsche Verteilung in Form eines zweidimensionalen Kalmanfilters für jede Landmarke approximiert. Bezeichnet m j die Position von Landmarke j, so kann aufgrund der Unabhängigkeitsannahme die Verteilung folgendermaßen zerlegt werden: P (m X (i) 0:k, Z 0:k) = M j=1 P (m j X (i) 0:k, Z 0:k). (3.22) Ein EKF Update der Landmarkenposition erfolgt nur, wenn die jeweilige Landmarke im entsprechenden Zeitschritt beobachtet wurde. 29

36 3 Simultane Lokalisierung und Kartenerstellung Die Berechnung der Partikelmenge S k zum Zeitpunkt k erfolgt rekursiv nach dem als sequential importance sampling with resampling (SIR) bekannten Verfahren [13]. Aufbauend auf der Partikelmenge S k 1 des vorangegangenen Zeitschrittes werden die folgenden Schritte durchgeführt: 1. Ziehung (sampling): Eine Entwurfsverteilung π wird beispielsweise durch Auswertung des Bewegungsmodells P (x k x k 1, u k ) unter Beachtung des zuletzt ausgeführten Steuersignals u k bestimmt. Sie wird ebenfalls gesamplet d.h. in Form der Partikelmenge S t dargestellt: x (i) k π(x k X (i) 0:k 1, Z 0:k, u k ). (3.23) (In der zuletzt vorgestellten Variante des Verfahrens FastSLAM 2.0 [28] verwenden Montemerlo et al. eine geänderte Entwurfsverteilung. Diese wird in Abschnitt kurz erläutert.) 2. Gewichtung (importance weighting): Jeder Partikel i aus S t bekommt eine Gewichtung w (i) k zugewiesen. Diese Entspricht dem Quotienten w (i) k = gesuchte Verteilung Entwurfsverteilung = P (x k Z 0:k, U 0:k, x 0 ) P (x k Z 0:k 1, U 0:k, x 0 ) (3.24) (Eine rekursive From der Berechnung der Partikelgewichte wird im Verlauf des Kapitels vorgestellt.) 3. Neuziehung (resampling): Die Partikelmenge S t wird durch Ziehung der Partikel aus S t jeweils mit der Wahrscheinlichkeit ihrer Gewichtung w (i) k erzeugt. Die Partikelgewichte der Menge S t werden auf den einheitlichen Wert w (i) k = 1 N gesetzt, wobei N der Anzahl der Partikel in der Menge entspricht. 4. Kartenaktualisierung (map update): Für jeden Partikel wird eine Kartenaktualisierung durchgeführt. Im Fall von FastSLAM wird hierbei für jede beobachtete Landmarkenposition ein EKF Update vollzogen. Berechnung der Entwurfsverteilung Das oben vorgestellte Verfahren zur Berechnung einer Entwurfsverteilung π, bei der ausschließlich die Bewegungsinformationen in Form des Bewegungsmodells genutzt werden, entspricht im Wesentlichen einem Bewegungsupdate, wie es beispielsweise auch bei der Monte-Carlo-Lokalisation zum Einsatz kommt (vgl ). Montemerlo et al. legen dar, dass insbesondere, falls die Streuung des Bewegungsmodells im Verhältnis zu der des Sensormodells sehr groß ist, die zusätzliche Beachtung der Sensorinformationen an dieser Stelle einen Effizienzgewinn erzielen kann. (Eine ausführliche Beschreibung der unter Verwendung der klassischen Variante auftretenden Probleme wird in 3.4 gegeben. Dementsprechend ist die entscheidende Neuerung in der zweiten Fassung des FastSLAM 30

37 3.4 Verbesserter rasterbasierter Rao-Blackwellized Particle Filter Verfahrens [27] eine geänderte Entwurfsverteilung, welche die gegenwärtige Sensormessung z k beachtet: x (i) k P (x k X (i) 0:k 1, u k, z k ) (3.25) Da die Bestimmung dieser Verteilung für den allgemeinen Fall nicht in einer geschlossenen Form möglich ist, kann diese nicht problemlos durch ein Sampling dargestellt werden. Montemerlo et al. führen deshalb eine Restriktion des Beobachtungsmodells ein: Die Abbildung g(m j, x (i) k ), welche die erwartete Sensorbeobachtung bei gegebener Position m j für eine Landmarke j darstellt, wird durch eine lineare Funktion approximiert. In diesem Fall kann die Verteilung (Gleichung 3.25) nach Art eines EKFs für jeden Partikel durch eine Gaußverteilung dargestellt werden. Mit Hilfe dieser Menge von Gaußverteilungen können die Partikel gezogen und somit die Entwurfsverteilung gesamplet werden. Berechnung der Partikelgewichte Die Berechnungsweise der Partikelgewichte ist abhängig von der gewählten Entwurfsverteilung (vgl. vorherigen Abschnitt). In der klassischen Variante mit der aus dem Bewegungsmodell bestimmten Entwurfsverteilung erfolgt die Aktualisierung der Partikelgewichte rekursiv anhand des marginalisierten Beobachtungsmodells: w (i) k = w (i) k 1 P (z k X (i) 0:k, Z 0:k 1) (3.26) Dabei wird für die Auswertung des Beobachtungsmodells die gesamte Trajektorie X (i) 0:k des Partikels sowie die vollständige Historie der Sensorbeobachtungen vorausgesetzt um entsprechend der Rao-Blackwellization die Unabhängigkeit gegenüber der Karte sicher zu stellen. Obwohl in der zweiten Version des FastSLAM Verfahrens das Beobachtungsmodell bereits in der Entwurfsverteilung berücksichtigt wurde, stellt die Partikelmenge noch nicht die gesuchten a posteriori Verteilung dar. Auch hier erfolgt die Korrektur der Approximation durch eine Aktualisierung der Partikelgewichte. Entsprechend dem Verhältnis zwischen der verwendeten Entwurfsverteilung und der gesuchten a posteriori Verteilung P (x k X (i) 0:k, Z 0:k) = 1 C P (z k x k, X (i) 0:k 1, Z 0:k 1)P (x k x (i) k 1, u k) (3.27) wird die Normalisierungskonstante C als Faktor für das Update der Gewichte w (i) k genutzt. 3.4 Verbesserter rasterbasierter Rao-Blackwellized Particle Filter = w (i) k 1 C Der SLAM Lösungsansatz, der dieser Arbeit zu Grunde liegt, entspricht zu großen Teilen dem von Grisetti, Stachniss und Burgard vorgestellten Verfahren [21]. Es handelt sich dabei um eine verbesserte Variante des im vorhergegangenen Kapitel vorgestellten raoblackwellized Partikelfilter Ansatzes. Diese generiert eine rasterbasierte probabilistische 31

38 3 Simultane Lokalisierung und Kartenerstellung Umgebungskarte. Wie auch in den zuvor beschriebenen Verfahren wird im folgenden Ansatz eine Approximation der a posteriori Verbundverteilung P (x 1:t, m Z 1:t, U 1:t ) über die Trajektorie der Posen x 1:t und die Karte m gesucht, welche die Lösung des SLAM Problems darstellt. Die Sensorbeobachtungen Z 1:t sowie die ausgeführten Steuersignale U 1:t sind dabei bekannte Größen. Mit Hilfe des Rao-Blackwellizing kann die Verbundverteilung in eine Kartenkomponente P (m X 1:t, Z 1:t ) und eine Posenkomponente P (X 1:t Z 1:t, U 1:t ) zerlegt werden (vgl. Formel 3.20). Zur Bestimmung der Kartenkomponente darf so neben den Sensorbeobachtungen Z 1:t auch die Trajektorie X 1:t als bekannt angenommen werden. Dadurch wird das Problem auf eine Kartenerstellung mit bekannten Posen reduziert, wie sie bspw. von Moravec [31] beschrieben wird. Die Verteilung der Trajektorie soll, wie bereits oben erläutert, mit Hilfe eines Partikelfilters approximiert werden. Jeder Partikel beschreibt somit eine potentiell zurückgelegte Trajektorie und hält die zugehörige Karte vor. Zur Aktualisierung der Partikelmenge nach einem Zeitschritt wird ebenfalls der unter dem Namen sequential importance sampling with resampling (SIR) Filter bekannte Algorithmus verwendet (vgl. Abschnitt 3.3.2). Im vorliegenden Kontext muss weiterhin eine Aktualisierung der Karte jedes Partikels erfolgen. Ein wesentlicher Angriffspunkt zur Verbesserung der Effizienz eines rao-blackwellized Partikelfilters ist nach Doucet et al. [13] die Qualität der Entwurfsverteilung. Stellt diese bereits eine gute Näherung der gesuchten a posteriori Verteilung dar, so schlagen sie als weitere Verbesserung vor, das Resampling nicht in jedem Zeitschritt, sondern adaptiv in Abhängigkeit von einem Gütemaß durchzuführen. Üblicherweise wird die Entwurfsverteilung aus dem Bewegungsmodell generiert. Mit Hilfe des Beobachtungsmodells können dann die Partikelgewichte aktualisiert werden. Dies ist laut Grisetti et al. insbesondere dann problematisch, wenn das Sensormodell wesentlich genauere Informationen bereitstellt und somit die Verteilung P (z t m t 1, x t ) eine wesentlich kleinere Varianz aufweist als die Entwurfsverteilung. Die anhand des Sensormodells bestimmten neuen Partikelgewichte werden dadurch im Bereich, der von den Sensormessungen unterstützt wird (vgl. Abb. 3.2 a), wesentlich größer ausfallen als in den ausserhalb liegenden Bereichen. Diese hohe Varianz der Partikelgewichte bewirkt, dass viele der Partikel, die vom Sensormodell nur sehr kleine Wahrscheinlichkeiten zugewiesen bekommen, bei einer Neuziehung (Resampling) verloren gehen. Die neu gezogene Partikelmenge umfasst somit zum großen Teil Kopien der Partikel, die vom Sensormodell eine hohe Wahrscheinlichkeit zugewiesen bekommen haben (vgl. Abb. 3.2 c). Damit der Partikelfilter eine gute Näherung der gesuchten a posteriori Verteilung erreichen kann, muss die Partikeldichte unter dem Sensormodell schon in der Entwurfsverteilung ausreichend hoch sein. Dementsprechend ist die Zahl der benötigten Partikel stark vom Varianzunterschied zwischen Entwurfsverteilung und der aus der Sensorbeobachtung hervorgegangenen Verteilung abhängig. Um eine größere Diversität der Vorgängerpartikel für die a posteriori Verteilung sicherzustellen, wird oft eine Glättung der Sensorwahrscheinlichkeitsverteilung durchgeführt. An diesem Ansatz kritisieren Grisetti et al., dass hierbei Sensorinformationen verworfen werden, wodurch die Qualität der erzeugten Karten reduziert wird. Sie schlagen hinge- 32

39 3.4 Verbesserter rasterbasierter Rao-Blackwellized Particle Filter a) b) c) P x t x t 1,u t P z t x t P x t x t 1,u t P z t x t P x t x t 1, u t x x x x x x Abbildung 3.2: Darstellung eines eindimensionalen (SIR) Partikelfilters. Sampling der Entwurfsverteilung (a) durch gleichgewichtete Partikel entsprechend des Bewegungsmodells P (x t x t 1, u t ). Aktualisierung der Partikelgewichte (b) unter Verwendung des Beobachtungsmodells P (z t x t ). Neuziehung der Partikel auf Basis der geänderten Gewichte (c). Duplizierte Partikel werden dabei gestapelt dargestellt. gen vor, (ähnlich der Entwurfsverteilung von FastSLAM 2.0 [28]) die zuletzt gewonnenen Sensorinformationen z t mit in die Bestimmung der Entwurfsverteilung einfließen zu lassen. Die von ihnen verwendete Entwurfsverteilung P (x t m (i) t 1, x(i) t 1, z t, u t 1 ) = P (z t m (i) t 1, x t)p (x t x (i) t 1, u t 1) P (z t m (i) t 1, x(i) t 1, u t 1) (3.28) ist nach Doucet [13] optimal bezogen auf die Varianz der Partikelgewichte. Durch Einsetzen der Entwurfsverteilung in w (i) t = P (X(i) 0:t Z 0:t, U 0:t 1 ) π(x (i) 0:t Z 0:t, U 0:t 1 ) ergibt sich die Berechnung der Partikelgewichte zu: w (i) t = w (i) ηp (z t m (i) t 1 t 1, x(i) t )P (x (i) t x (i) t 1, u t 1) P (x t m (i) t 1, x(i) t 1, z t, u t 1 ) (3.29) (3.30) w (i) t 1 = w (i) t 1 P (z t m (i) )P (x (i) x (i) t 1, u t 1) t 1, x(i) t t P (z t m (i) t 1,xt)P (xt x(i) t 1,u t 1) = w (i) t 1 P (z t (i) P (z t m (i) t 1,x(i) t 1,u t 1) t 1, x(i) (3.31) t 1, u t 1) (3.32) P (z t x )P (x x (i) t 1, u t 1)dx. (3.33) 33

40 3 Simultane Lokalisierung und Kartenerstellung Die verbesserte Entwurfsverteilung ermöglicht die Umsetzung des zweiten Verbesserungsvorschlags nach Doucet. Partikel, die in einem Zeitschritt eine geringe Gewichtung zugewiesen bekommen, gehen möglicherweise bei Durchführung des Resamplings verloren. Neben der Hypothese über die zurückgelegte Trajektorie, die von dem Partikel repräsentiert wird, verwirft man ebenfalls die dem Partikel zugeordnete Karte. Falls durch diesen Schritt auch Partikel verloren gehen, deren Hypothesen ansonsten bei einer der folgenden Beobachtungen eine höhere Wahrscheinlichkeit zugewiesen bekommen hätten, spricht man von Partikelverarmung (engl. particle depletion). Grisetti et al. versuchen den Einfluss dieses Phänomens zu verringern, indem sie den Resampling Schritt nur durchführen, falls die Repräsentation der gesuchten Verteilung durch die gewichtete Partikelmenge nicht mehr in hinreichender Qualität gewährleistet ist. Als Indikator dafür bestimmen sie die von Doucet et al. formulierte effektive Samplegröße: 1 N eff = N i=1 ( (3.34) w(i) ) 2 Hierbei entspricht w(i) dem normalisierten Gewicht von Partikel i. Nach der Durchführung eines Resamplings bekommen alle Partikel identische Gewichte zugewiesen. N eff entspricht in diesem Fall der Anzahl der verwendeten Partikel. Wächst die Varianz der Partikelgewichte, da manche Partikel eine verhältnismäßig hohe und andere eine dementsprechend niedrige Wahrscheinlichkeit zugewiesen bekommen, so mindert dies den Wert von N eff. Als Bedingung für ein Resampling verwenden Grisetti et al. den durch Experimente ermittelten Schwellwert N/2 für eine Partikelmenge mit N Partikeln. Als Kartenrepräsentation kommt eine raster-basierte probabilistische Karte zum Einsatz. Jede Zelle des Rasters beinhaltet dabei die Information, mit welcher Wahrscheinlichkeit die durch die Zelle repräsentierte Fläche von einem Hindernis belegt ist. Zur Bestimmung der Wahrscheinlichkeitsverteilung P (z t m (i), x (i) ) eines Partikels i wird zunächst der Punkt mit der höchsten Beobachtungswahrscheinlichkeit für jeden Partikel bestimmt: ˆx (i) t = argmax x P (x m (i) t 1, z t, x t (i) ). (3.35) Hierbei ist x t (i) die initiale Schätzung, welche sich aus der vorherigen Partikelpose x (i) t 1 und dem ausgeführten Steuersignal u t ergibt. Die Bestimmung des Maximums geschieht beispielsweise mit Hilfe einer Gradienten-Aufstiegs-Suche auf einer begrenzten Region um die initiale Schätzung. Das Verfahren zur Bestimmung der Wahrscheinlichkeit P (z t m, x) ist von der verwendeten Sensortechnologie abhängig. Bei Grisetti et al. kommt (wie auch bei dem im Rahmen dieser Arbeit untersuchten Szenario) ein Laserabstandssenor zum Einsatz. Die Auswertung der gesuchten Wahrscheinlichkeit an jedem untersuchten Punkt x erfolgt mit Hilfe eines Strahl-Endpunkt Modells, wie beispielsweise in Abschnitt vorgestellt wurde. Hierfür wird die Annahme getroffen, dass die Abstandsmessungen der einzelnen Strahlen des laser range finders unabhängig sind. Die kumulierte Wahrscheinlichkeit eines Punktes ergibt sich damit aus dem Produkt der Wahrscheinlichkeiten der einzelnen Strahlen der Messung L P (z t m, x) = P (z j t m, x) (3.36) j=1 34

41 3.4 Verbesserter rasterbasierter Rao-Blackwellized Particle Filter wobei z j t die Abstandsmessung des j-ten Strahls repräsentiert. Die Wahrscheinlichkeit P (z j t m, x) wird ermittelt, indem ausgehend vom Endpunkt des Messstrahls der minimale Abstand zu einem Hindernis bestimmt wird. Dieser Wert wird entsprechend der Annahme eines sensorabhängigen gauss schen Messfehlers und weiterer Fehlerkomponenten in eine Wahrscheinlichkeit überführt (vgl ). Zur Bestimmung der aktualisierten Partikelpose werden zunächst in einer Region um x (i) t K zufällige Punkte entsprechend der Entwurfsverteilung P (z t m (i) t 1, x j)p (x j x (i) t 1, u t 1) ausgewertet. Die Posenwahrscheinlichkeit P (x j x (i) t 1, u t 1) wird dabei anhand des von Thrun in [40] beschrieben Odometrie-Bewegungsmodells berechnet (vgl ). Die neue Partikelpose x (i) t wird anschließend aus einer durch die ausgewerteten Punkte approximierten multivariaten Gaussverteilung N (µ (i) t, Σ (i) t ) gezogen. Im nächsten Schritt erfolgt eine Aktualisierung der Partikelgewichte, wobei Grisetti et al. einen Aktualisierungsfaktor η (i) während der Auswertung der Wahrscheinlichkeiten der Umgebungspunkte folgendermaßen bestimmen: η (i) = K l=1 P (z t m (i) t 1, x l)p (x l x (i) t 1, u t 1). (3.37) Die neuen Partikelgewichte werden hiermit dann rekursiv bestimmt: w (i) t = w (i) t 1 η(i). Die Karte m (i) jedes Partikels wird entsprechend der gezogenen Pose x (i) t und der letzten Beobachtung z t aktualisert. Der Resampling-Schritt wird an dieser Stelle gemäß der obigen Ausführungen nur durchgeführt, falls der Wert von N eff den gesetzten Schwellwert unterschreitet. 35

42 3 Simultane Lokalisierung und Kartenerstellung 36

43 4 Betrachtung dynamischer Umgebungen Ziel der vorliegenden Arbeit ist es ein Verfahren zu entwickeln, welche eine Kartenerstellung und Lokalisierung in dynamischen Umgebungen und insbesondere bei Anwesenheit von Personen ermöglicht. Im folgenden Kapitel soll zunächst kurz darauf eingegangen werden worin die besonderen Schwierigkeiten für die Anwendung von SLAM Verfahren in dynamischen Umgebungen liegen. Weiterhin soll beschrieben werden, welche Arten dynamischer Objekte in solchen Umgebungen vorkommen und welchen Einfluss diese auf die Sensormessungen des Roboters haben. 4.1 Herausforderungen durch dynamische Umgebungen Wie bereits im Kapitel 2.3 kurz angedeutet, stellen veränderliche Komponenten in der Umgebung eine große Herausforderung für die Posenbestimmung und somit auch für die weitere Kartenerstellung dar. Betrachtet man nur den Fall der Lokalisierung mit einer bekannten Karte, so wird deutlich, dass ab einem bestimmten Grad der Veränderung in der Umgebung die Wahrscheinlichkeitsverteilung über der Pose nicht mehr mit Hilfe der Sensormessungen verbessert werden kann. Es sei im folgenden x t die gegenwärtige Pose und z t die zuletzt gemachte Sensormessung. Eine globale Lokalisierung versagt, sobald die Beobachtungswahrscheinlichkeit der tatsächlichen Pose x t kleiner wird als die Beobachtungswahrscheinlichkeit beliebiger anderer Posen x t x t : P (z t x t, m) < P (z t x t, m). (4.1) Im Fall einer lokalen Posenbestimmung oder Posenverfolgung, wie sie auch im SLAM Kontext durchgeführt wird, versagt das Verfahren, sobald das Ausmaß der Kartenändegen regelmäßig verhindert, dass die lokale Maximumssuche auf P (z t x t, m) in der Nähe von x t konvergiert. Eine fehlerhafte Posenbestimmung führt bei SLAM Verfahren, falls dieser Fehler nicht erkannt wird, zu falschen Einträgen in die Kartenrepräsentation und damit zu einer inkonsistenten Karte. Bei der Lokalisierung mit einer gegebenen Karte kann mit Hilfe der Odometriehypothesen eine genaue Posenbestimmung wieder gelingen, sobald ein weniger stark veränderter Bereich der Karte erreicht wird. Es ist offensichtlich, dass SLAM Verfahren nicht so robust gegenüber unerkannten Fehlern sind, da ab dem Zeitpunkt der fälschlichen Posenbestimmung eine fehlerhafte Karte gezeichnet wird und diese gleichzeitig als Grundlage für die weitere Lokalisierung dient. Für Verfahren zur simultanen Lokalisierung und Kartenerstellung kann deshalb als kritischer Punkt beim Einsatz in dynamischen Umgebungen formuliert werden, dass fehlerhafte Positionsbestimmungen frühzeitig erkannt werden müssen und die Kartenerstellung bis zur nächsten erfolgreichen Lokalisierung ausgesetzt werden muss. 37

44 4 Betrachtung dynamischer Umgebungen 4.2 Klassifikation der dynamischen Änderungen Die möglichen Veränderungen in einer dynamischen Umgebung lassen sich auf verschiedene Arten einteilen. Zum einen kann die zeitliche Beobachtbarkeit betrachtet werden, mit der eine Veränderung der Umgebung stattfindet. Zum anderen kann unterschieden werden, in welcher Art die veränderte Umgebung die Lokalisation anhand der Karte beeinflusst Zeitliche Beobachtbarkeit der Änderungen Betrachtet man die verschiedenen Komponenten der vom Roboter bei der Kartenerstellung wahrgenommenen Umgebung, so können diese eingeteilt werden in 1. statische Objekte, die sich während der gesamten Zeit der Kartenerstellung nicht verändern. Hierzu zählen insbesondere Grundstrukturen des umgebenden Gebäudes wie Wände, Pfeiler, Durchgänge usw., aber auch nicht bewegtes Mobiliar, z.b. Schränke und Tische. 2. zeitweise statische Objekte, die zunächst als unbewegt wahrgenommen werden, später aber die Position verändern. Dies können zum Beispiel Türen und Stühle sein, aber auch stehende Personen, die sich irgendwann entfernen. 3. dynamische Objekte, die durch den Roboter bereits als sich bewegend wahrgenommen werden. In der Regel handelt es sich in Indoor-Umgebungen dabei um laufende Personen. Es könnten aber auch andere autonome Fahrzeuge bzw. Roboter sein. Während die statischen Objekte für die Erstellung einer dauerhaften Karte von größter Wichtigkeit sind, fällt es schwer diese von zeitweise statischen Objekten zu unterscheiden, die zum Zeitpunkt der Beobachtung still stehen. Finden diese an ihrer ursprünglichen Position Eintrag in die Karte, so entstehen Inkonsistenzen, welche die Lokalisierung abhängig vom Ausmaß der Änderung erschweren. Dynamische (sich bewegende) Objekte können bereits in wenigen aufeinander folgenden Sensormessungen als solche erkannt werden und sollten daher nicht oder nur mit einer geringen Gewichtung der Karte hinzugefügt werden Einfluß auf die Lokalisierung Einen Einfluß auf die Leistungsfähigkeit der Lokalisierungsverfahren üben nur solche Änderungen in der Umgebung aus, die zu Inkonsistenz zwischen der Karte und der wahrgenommenen Umgebung führen. Derartige Veränderungen werden in der Regel beim Auftreten zeitweise statischer Objekte beobachtet. Betrachtet man die Art dieser Inkonsistenz, so lassen sich diese in verschiedene Klassen einteilen (vgl. Abbildungen 4.1, 4.2 und 4.3): 1. Geringfügige Änderungen: Diese Art der Variation in der Umgebung beeinflusst die Lokalisierung nicht negativ. Das Maximum der Beobachtungswahrscheinlichkeit 38

45 4.3 Variation des Beobachtungsmodells P (z t x t, m) wird weiterhin an der selben Pose in der Karte gefunden. Der Fall tritt auf, wenn relativ kleine bzw. weit entfernte Objekte bewegt werden, deren Beobachtung nur einen kleinen Anteil der Sensormessungen ausmacht. 2. Wesentliche Änderungen: Hierbei gibt es kein deutliches Maximum der Beobachtungswahrscheinlichkeit in der Nähe der tatsächlichen Pose x t. Eine lokale Maximumssuche konvergiert somit gar nicht. Im Gegensatz zum vorherigen Punkt treten diese Veränderungen der Beobachtungen gegenüber der Karte auf, wenn die sich bewegenden Objekte sehr groß bzw. sehr nah bezüglich der gegenwärtigen Position sind und ihre Beobachtung damit einen wesentlichen Anteil an der Sensorbeoachtung z t hat. 3. Wesentliche Änderungen mit Verschiebungseffekt: Es existiert ein Maximum der Beobachtungswahrscheinlichkeit, welches gegenüber der tatsächlichen Pose verschoben ist. Dieser eher seltene Fall kann auftreten, wenn in einer sehr gleichförmigen Umgebung (z.b. einem langen Korridor) das sich bewegende Objekt den entscheidenden Anhaltspunkt für die Lokalisierung darstellt. Wird es beispielsweise verschoben, verschiebt sich in diesem Bereich die Verteilung der Sensorwahrscheinlichkeiten in gleicher Weise. Offensichtlich gibt es Fälle, in denen die beobachtete Umgebung so großen Veränderungen unterworfen ist, dass eine auf Sensorbeobachtungen gestützte Lokalisierung aussichtslos wird. Ein Beispiel hierfür ist die Situation, in der der Roboter von einer großen Zahl Menschen umringt ist und somit die Sensormessungen überwiegend Beinpaare darstellen. Zwischen diesem Fall und dem entgegengesetzten Extrem einer völlig statischen Umgebung gibt es beliebige Abstufungen. Die Schwelle zwischen den oben als geringfügig und wesentlich bezeichneten Veränderungen der Umgebung ist erreicht, wenn das Maximum der Beobachtungswahrscheinlichkeit nicht mehr nahe der tatsächlichen Pose gefunden wird. Ziel von Verfahren, die speziell auf den Einsatz in dynamischen Umgebungen zugeschnitten wurden, sollte daher eine Verbesserung des Beobachtungsmodells P (z t x t, m) sein. Im folgenden werden Maßnahmen diskutiert, um robuste Maxima der Beobachtungswahrscheinlichkeit zu erhalten. 4.3 Variation des Beobachtungsmodells Mit Blick auf das in Kapitel vorgestellte Sensormodell soll an dieser Stelle untersucht werden, wie die Beobachtungswahrscheinlichkeit auch nach einer Änderung der Umgebung gegenüber der bekannten Karte dort einen hohen Wert (möglichst ein Maximum) erhalten kann. Wie bereits beschrieben, wird die Gesamtwahrscheinlichkeit, an Pose x t eine bestimmte Messung z t = {z 1 t,..., z K t } zu erhalten, als Produkt über die Wahrscheinlichkeiten für die Abstandsmessungen der verschiedenen Winkel bestimmt: p(z t x t, m) = K p(zt k x t, m). (4.2) k=1 39

46 4 Betrachtung dynamischer Umgebungen Abbildung 4.1: Beispiel einer geringfügigen Umgebungsänderung. Die Bewegung des Objekts beeinflusst nur eine geringe Zahl der Abstandsmessungen (grün markiert). Die Lokalisierung wird davon nicht beeinträchtigt. Abbildung 4.2: Beispiel für eine wesentliche Veränderung der Umgebung. Die Anwesenheit einer Vielzahl von Personen bzw. Beinpaaren (symbolisiert durch grüne Kreise) beeinflusst in diesem Beispiel nahezu alle Abstandsmessungen (grün markiert). Eine Lokalisierung anhand der Karte kann unter derartigen Umständen nicht funktionieren. 40

47 4.3 Variation des Beobachtungsmodells Abbildung 4.3: Beispiel für eine wesentliche Umgebungsveränderung mit Verschiebungseffekt. Situation vor der Verschiebung (obere Abb.). Situation nach der Verschiebung (mittlere Abb.) Durch den Roboter anhand der Karte vermutete Pose (untere Abb.) 41

48 4 Betrachtung dynamischer Umgebungen Gelingt es die Menge aller Abstandsmessungen zt k zu identifizieren, die das veränderte Objekt erfassen, und sei diese durch die Indexmenge V beschrieben, so können diese Messungen bei der Berechnung der Posenwahrscheinlichkeit außer Acht gelassen werden: p(z t x t, m) = K k=1 k / V p(z k t x t, m). (4.3) Die Identifizierung der veränderlichen Objekte und der zugehörigen Abstandsmessungen kann nur aufgrund ihrer Kontur in den Sensorbeobachtungen erfolgen. Da einerseits nicht alle dynamischen Objekte der Umgebung bekannt sind und andererseits die Abstandsmessungen oft nur einen kleinen Ausschnitt der Kontur des Objektes umfassen, kann die Trennung zwischen statischen und dynamischen Objekten nur begrenzt funktionieren. Das im Weiteren beschriebene Verfahren geht deshalb über zu einer heuristischen Abschätzung der Beobachtungswahrscheinlichkeit. Dabei fließen die Messungen zt k mit unterschiedlichen Gewichtungen in das Ergebnis ein. Es wird also nicht, wie oben vorgeschlagen, ein Teil der Abstandsmessungen gänzlich von der Bewertung der Pose ausgeschlossen. Das heuristische Verfahren bricht mit der exakten wahrscheinlichkeitstheoretischen Beschreibung der Sensorbeobachtungen p(z t x t, m) und führt stattdessen ein Gütemaß für die Übereinstimmung der Sensormessung z t mit der Karte m an der Pose x k ein: K H(z t x t, m) = gt k P (zt k x t, m). (4.4) k=1 gt k ist dabei eine Gewichtung der Messung z t, die anhand einer heuristischen Klassifikation zwischen statischen und dynamischen Objekten ermittelt wird. Zu den statischen Objekten werden dabei alle Beobachtungen gerechnet, die geradlinige Konturen aufweisen. Dem zugrunde liegt die Annahme, dass vor allem die beobachtbaren Wände keiner Veränderung unterliegen. Eine grobe Approximation der Messungen durch geradlinige Kanten soll allerdings nur der Klassifikation dieser Messungen dienen. Für die Lokalisierung und Kartenerstellung hingegen bieten die indiviuellen Messungen zt k aufgrund feiner Strukturen einen höheren Informationsgehalt. Zu den dynamischen Objekten gezählt werden können insbesondere Personen, die anhand ihrer Beinkonturen in den Sensormessungen detektiert werden sollen. Dies soll gemäß der Aufgabenstellung auch der Schwerpunkt der Klassifikation sein. Eine Erläuterung der hierbei verwendeten Heuristiken gibt der folgende Abschnitt Variation der Kartenrepräsentation Als Kartenrepräsentation soll im weiteren eine occupancy grid map wie in Kapitel beschrieben verwendet werden. Diese raster-basierte Kartendarstellung umfasst für jede Zelle ein Maß für die Wahrscheinlichkeit, mit der diese durch ein Hindernis belegt ist. Es wird dabei angenommen, dass diese Wahrscheinlichkeit steigt, je häufiger ein Hindernis in dieser Zelle beobachtet werden kann. Für bestimmte Objekte ist allerdings zu erwarten, dass diese mit geringerer Wahrscheinlichkeit wieder als Hindernis an dieser Stelle 42

49 4.4 Klassifikation der Sensormessungen beobachtet werden. Dies soll im weiteren insbesondere für Personen zutreffen. Werden sie wahrgenommen, wächst durch diese Beobachtung offensichtlich die Wahrscheinlichkeit für ein Hindernis an der Position in geringerem Maße an als bei der Beobachtung eines statischen Objektes. Vor dem Eintrag in die Karte wird daher für jede Abstandsmessung z t,i ein Gewichtungsfaktor g t,i bestimmt. Abhängig von diesem kann dann die Wahrscheinlichkeit für ein Hindernis an einer Position in der jeweiligen Zelle erhöht werden. Die detaillierte Umsetzung wird in Kapitel beschrieben. 4.4 Klassifikation der Sensormessungen Die Sensormessungen z t eines Laserabstandssensors, die jeweils eine Abstandsmessung für jeden betrachten Winkel enthalten, werden vor der Verwendung im Rahmen des in Kapitel 3.4 beschriebenen SLAM Verfahrens zunächst gemäß der oben vorgeschlagenen Heuristik verarbeitet. Im folgenden soll eine einzelne Abstandsmessung z i t durch den von ihr (bezüglich der Roboterbasis) beschriebenen Punkt (r t,i, θ t,i ) in Polarkoordinaten beschrieben werden. Eine Übersicht der im folgenden vorgestellten Verarbeitungsschritte gibt die Abbildung Segmentierung Aufgabe der Segmentierung ist eine Einteilung der K Messwerte {zt 1,..., zt K } in zusammenhängende Segmente s t (i, j) = {zt, i..., z j t } mit 1 i < j K. Hierbei wird für jeweils zwei benachbarte Messwerte r t,i und r t,i+1 untersucht, ob diese ein Abstandskriterium d(r t,i, r t,i+1 ) < d threshold erfüllen und somit noch dem gleichen Segment zuzurechnen sind. Ist dies nicht der Fall, so beginnt mit der Messung r t,i+1 ein neues Segment. Ein einfaches Abstandskriterium ist der Längenunterschied zwischen den Messungen: d(r t,i, r t,i+1 ) = r t,i r t,i+1. Wird der Grenzwert hierbei groß genug gewählt, lassen sich damit dem Roboter nahestehende Vordergrund-Objekte von den Objekten des Hintergrundes unterscheiden. Fehlerhafte Segmentgrenzen könnten allerdings entstehen, wenn 1. keine eindeutige Trennung zwischen Vorder- und Hintergrund der Objekte zu erkennen ist. Dies könnte Beispielsweise auftreten, wenn Personen an einer Wand stehen. 2. weit entfernte geradlinige Strukturen im spitzen Winkel zur Messrichtung stehen. Die Entfernungen der Messpunkte werden dabei sehr groß und liegen damit möglicherweise über dem Schwellwert d threshold. 3. benachbarte Vordergrund-Objekte, beispielsweise Beinpaare, sich berühren. Die Objekte werden dann als ein Segment wahrgenommen. Versuche, derartige Fehler durch ein komplexeres Abstandsmaß zu verhindern, z.b. durch Verwendung der Messpunktsabstände d i,i+1 = ri 2 + r2 i+1 2r ir i+1 cos(θ i+1 θ i ) (4.5) 43

50 4 Betrachtung dynamischer Umgebungen Abstandsmessung z t Segmentierung Segmentmenge {s t l,m 1 l m K } Liniendetektion Geradlinige Segmente Nicht geradlinige Segmente Beindetektion Nicht beinförmige Segmente (unbekannte Kontur) Beinförmige Segmente Beinpaarzuordnung Einzelne Beinsegmente Potentielle Beinpaare Abbildung 4.4: Ablaufdiagramm der Klassifikation der Sensormessungen. Rechteckige Blöcke stellen Prozesse dar. Blöcke mit abgerundeten Ecken repräsentieren bereitgestellte Informationen. 44

51 4.4 Klassifikation der Sensormessungen sowie einer Normierung des Schwellwertes entsprechend des durchschnittlichen Abstandes d threshold = d threshold r i+r i+1 2, führten zu keinen besseren Ergebnissen. Da in den folgenden Schritten weitere Segmentgrenzen eingefügt werden können, wird in diesem Schritt lediglich eine grobe Vorder- und Hintergrundsegmentierung durchgeführt Detektion geradliniger Strukturen Wie bereits erwähnt dient als Anhaltspunkt für die statische Natur eines Objekte die Form seiner Kontur. Insbesondere geradlinige Wände können als unveränderliche Objekte der Umgebung gut identifiziert werden. Hierfür müssen die Segmente oder Teilsegmente der Sensormessung z t bestimmt werden, deren Punkte annähernd auf einer Geraden liegen. Diese Punkte (x, y) T müssen die Geradengleichung: x cos(α) + y sin(α) = r + ɛ (4.6) erfüllen. Für jedes geradlinige Segment müssen somit die Parameter α und r bestimmt werden. Dabei entspricht α dem Winkel zwischen der X-Achse und dem Normalvektor der Geraden und r deren Abstand vom Ursprung. Der maximal zulässige Fehler ɛ beeinflußt, wie ähnlich die untersuchten Segmente einer idealen Geraden sein müssen. Die Darstellung der Geraden in Polarkoordinaten wird gewählt, damit auch Parallelen zur Y-Achse hinreichend beschrieben werden können. Zur Ermittlung der Segmente, die diese Eigenschaften erfüllen, wird der häufig erwähnte Iterative-End-Point-Fit Algorithmus verwendet (vgl. [4, 14, 34]). Es handelt sich dabei um ein rekursives Verfahren, welches zunächst jeweils die Gerade bestimmt, die durch die beiden Endpunkte des zu untersuchenden Segmentes markiert wird. Anschließend wird der Abstand jedes Segmentpunktes zur Geraden d i bestimmt. Liegen alle Abstände unter einem Schwellwert d t, so ist die Approximation der Kontur hinreichend genau. Gibt es Punkte, die einen größeren Abstand haben, so wird an dem Punkt mit dem maximalen Abstand das Segment geteilt. Das Verfahren wird dann auf beiden resultierenden Segmenten erneut ausgeführt. Eine minimale Wandlänge l min wird als Abbruchkriterium für die Rekursion definiert. Unterschreitet die Segmentlänge l = (x end x start ) 2 + (y end y start ) 2 diese unterere Schranke, so wird das Segment als nicht linienförmig verworfen. Eine Darstellung diese Vorgangs zeigt Abbildung 4.5. Segmente, die als linienähnlich erkannt wurden, korrespondieren annahmegemäß mit statischen Objekten der Umgebung. Die zugehörigen Sensormessungen z t,i bekommen daher eine hohe Gewichtung g t,i zugewiesen Detektion dynamischer Objekte Die einzige eindeutig bekannte Art dynamischer Objekte in der Umgebung sind Personen. In den Messungen des Laserabstandssensor können diese allein anhand der Kontur ihrer Beine erkannt werden. Da die Untersuchung dieses Aspekts einen Schwerpunkt dieser Arbeit darstellt, wird für eine ausführliche Behandlung der möglichen Verfahren zur Beindetektion sowie des verwendeten Ansatzes auf das Kapitel 5 verwiesen. An dieser 45

52 4 Betrachtung dynamischer Umgebungen a) d max b) d max c) Abbildung 4.5: Darstellung von drei Schritten des Iterative-End-Point-Fit Algorithmus: Bildung der Geraden durch die Endpunkte des Segments und Bestimmung des Punktes mit maximal Abstand zu dieser (a). Unterteilung der Geraden, falls Abstandsgrenzwert überschritten wird sowie erneute Ausführung des Algorithmus auf den Teilsegmenten (b). Approximationsergebnis (c). 46

53 4.4 Klassifikation der Sensormessungen Stelle soll nur erwähnt werden, dass die verbleibenden Segmente, die bei der Liniendetektion verworfen wurden, auf Beinkonturen untersucht werden (siehe Kapitel 5.1). Dabei können Segmente nochmals unterteilt werden, falls sie der Kontur von zwei Beinen ähneln. Bei einer positiven Detektion wird den Sensormessungen des Segments eine geringe Gewichtung g t,i für die Auswertung des Gütekriteriums H(.) zugewiesen. Gibt es in der Nachbarschaft eines Segments weitere Segmente, die einer Beinkontur ähneln, so wird das wahrscheinlichste Beinpaar ermittelt (vgl. Kapitel ). Bilden zwei Segmente ein potentielles Beinpaar, so steigt die Wahrscheinlichkeit, dass die Detektion korrekt ist. Die Gewichtung der beiden Segmente für die Verarbeitung im Rahmen des Beobachtungsmodells kann daher weiter verringert werden. Dynamische sowie zeitweise statische Objekte, bei denen es sich nicht um Personen handelt (z.b. regelmäßig bewegte Stühle), können vielfältige Konturen aufweisen. Da diese nicht vollständig bekannt sind, wird im weiteren keine Klassifikation anhand ihrer Kontur durchgeführt. Ihrer veränderlichen Natur wird lediglich durch die in der Kartenrepräsentation vermerkte Beobachtungshäufigkeit Rechnung getragen. Gemäß der Aufgabenstellung dieser Arbeit liegt der Schwerpunkt somit in der Betrachtung von anwesenden Personen. 47

54 4 Betrachtung dynamischer Umgebungen 48

55 5 Anwesenheit von Personen Das im Rahmen dieser Arbeit entwickelte Verfahren soll eine simultane Kartenerstellung und Lokalisierung auch in Umgebungen ermöglichen, in denen Personen natürlich agieren. Wie im vorangegangenen Kapitel erläutert, entstehen hierdurch sowohl Probleme für die Kartenerstellung, bei der Personen möglichst gar nicht in der Karte verzeichnet werden sollen, als auch bei der Lokalisierung, da Personen Bereiche der bekannten Karte verdecken können und somit die Auswertung der gegenwärtigen Pose anhand der Karte erschweren. Um diesen beiden Problemen zu begegnen, wurde in Kapitel 4.3 die Segmentierung und Klassifizierung der Sensormessungen vorgeschlagen. Eine Gewichtung der Segmente der Laser-Abstandsmessung ermöglicht sowohl bei der Lokalisierung als auch bei der Aktualisierung der Karte die Berücksichtigung dynamischer und statischer Elemente. In diesem Kapitel soll im Detail erläutert werden, in welcher Weise menschliche Beine aufgrund ihrer Kontur in den Sensormessungen erkannt werden. Dafür gibt der folgende Abschnitt zunächst einen Überblick über die in der Literatur zur Beindetektion verwendeten Verfahren. Im Anschluss wird das in dieser Arbeit verwendete Verfahren zur Detektion einzelner Beine im Detail vorgestellt, bevor im letzten Abschnitt beschrieben wird, wie die Zuordnung potentieller Beinpaare erfolgt. 5.1 Überblick über Verfahren zur Beindetektion Die Aufgabe, Beine oder Beinpaare auf Basis der Messdaten eines Laser-Abstandssensors zu ermitteln, wird vielfach in der Literatur erwähnt. Grundlegend unterscheiden lassen sich dabei zwei Herangehensweisen. Ein Gruppe von Verfahren bestimmt Beine durch einen Abgleich der Messungen gegenüber einem Hintergundbild bzw. vorangegangener Messungen. Die andere Gruppe der Verfahren verwendet Annahmen über die geometrischen Eigenschaften von Beinen. Eine einfache Herangehensweise der ersten Art wählen beispielsweise Ishihara et al. ([23]). Sie verwenden eine Histogrammdarstellung der Umgebung und detektieren Personen bzw. ihre Beinkonturen darin. Hierbei wird überprüft, ob der Zähler der Zelle, in der das wahrgenommene Objekt sich befindet, unter einem Grenzwert liegt. Ist dies der Fall, wird angenommen, dass es sich um ein Bein handelt. Blanco et al. ([3]) verwenden ein ähnliches Verfahren als Vorstufe für ihre Gesichtsdetektion. Das Hintergrundhistogramm wird dabei erzeugt, sobald ihr mobiler Roboter stehen bleibt. Schulz et al. ([33]) bestimmen lokale Minima in den Abstandsmessungen. Durch Vergleich dieser in aufeinander folgenden Messungen bestimmen sie die Wahrscheinlichkeit, mit der es sich dabei um ein statisches Objekt oder um ein menschliches Bein handelt. Unter den Vertretern der zweiten Verfahrensgruppe ist Lang zu nennen [26]. Dieser schlägt eine Klassifikation von Beinen mit Hilfe einer einfachen Heuristik vor. Er 49

56 5 Anwesenheit von Personen überprüft dabei, ob die Eigenschaften eines Vordergrundobjektes wie der Abstand zum Hintergrund, die Breite des Objektes und der durchschnittliche Abstandswert benachbarter Messpunkte jeweils in vorgegebenen Intervallen liegen. Die potentiellen Beine gruppiert er anschließend zu Beinpaaren. Schraft et al. erwähnen in ihrer Arbeit die Verwendung einer auf geometrischen Merkmalen wie Durchmesser und Form basierenden fuzzy logic zur Detektion von Beinen ohne genauere Details auszuführen [32]. Nach der Durchführung einer Hintergrundsegmentierung versuchen Brooks und Williams die Kontur der Vordergrundobjekte durch Halbkreise zu approximieren [5]. Als Mensch detektiert werden dabei jeweils zwei Vordergrundobjekte, wenn deren Approximation durch Halbkreise hinreichend gut ist. Xavier et al. bestimmen in dem als internal angle variance bezeichneten Verfahren für jeden Punkt eines Vordergrundsegmentes den Winkel, den dieser mit den Randpunkten des Segmentes bildet [41]. Liegen diese Winkel und deren Varianz innerhalb eines bestimmten Intervalls, so klassifizieren sie dieses Segment als Bein. 5.2 Verwendetes Verfahren zur Beindetektion Das in der vorliegenden Arbeit verwendete Verfahren zur Detektion von Beinen untersucht die geometrischen Eigenschaften einzelner Segmente der Laser-Abstandsmessungen, welche zuvor wie in Kapitel beschrieben unterteilt wurden. Hierbei werden nur die Segmente betrachtet, die keine gradlinige Kontur aufweisen. Die Detektion der Beine stützt sich allein auf deren Form und ist somit bewegungsunabhängig möglich. Der folgende Abschnitt gibt zunächst einen kurzen Überblick über die einzelnen Schritte des Verfahrens. Hierbei handelt es sich im wesentlichen um eine Kombination der Ansätzen verschiedener Autoren. Details der Berechnungen werden in den weiteren Abschnitten dargestellt Verfahrensüberblick Aus der Menge aller Segmente einer Abstandsmessung z t werden diejenigen weiter betrachtet, die weder zuvor als geradlinig klassifiziert wurden, noch weniger als n min Messpunkte umfassen. Segmente, welche die Mindestanzahl von Messpunkten n min unterschreiten, enthalten annahmegemäß nicht genügend Informationen, um anhand ihrer Kontur auf ein Bein zu schließen. Auf den verbleibenden Segmenten werden unterschiedliche Merkmale untersucht und zu einem gemeinsamen Gütekriterium verrechnet. Dabei werden für alle Merkmale zulässige Intervalle definiert, die einen unscharfen Randbereich aufweisen. Die Bewertung ob ein Merkmal erfüllt ist, geschieht dann in jedem Fall mit Hilfe einer Funktion, die gemäß der fuzzy logic auf das Intervall [0; 1] abbildet. Im ersten Schritt wird überprüft, ob die Breite des Segments zulässt, dass dieses ein bzw. zwei Beine enthält. Weiterhin wird getestet, ob anhand der Varianz der inneren Winkel auf eine Kreisbogenform geschlossen werden kann. Ist letzteres nicht erfüllt und enthält das Segment mit hoher Wahrscheinlichkeit zwei Beine, so wird dies gegebenenfalls aufgeteilt und die Prozedur wird auf beiden Segmenten erneut gestartet. Weitere 50

57 5.2 Verwendetes Verfahren zur Beindetektion untersuchte Merkmale sind der Abstand der Segmentränder zum Hintergrund sowie die Position des Segmentmittelpunktes bezüglich der Randpunkte. Die Klassifikation ob es sich bei einem Segment um eine Bein handelt, geschieht mit Hilfe eines kumulierten Gütewertes aller Merkmale. Im Anschluss an diese Klassifikation wird untersucht, welche dieser potentiellen Beine Beinpaare darstellen. Einen Überblick über den gesamten Ablauf gibt Algorithmus 1. Algorithm 1 Algorithmische Darstellung der Beinklassifikation. for all s i Segment(z) do if not istgerade(s i ) and #Messungen(s i )> n min then if prüfeaufbein(s i ) then S möglbeine = S möglbeine s i end if end if end for for all s i S möglbeine do if existiert(s i 1 ) and zulässigespaar((s i 1, s i )) then S möglp aare = S möglp aare (s i 1, s i ) end if if existiert(s i+1 ) and zulässigespaar((s i, s i+1 )) then S möglp aare = S möglp aare (s i, s i+1 ) end if end for while S mögl.p aare do (s i, s j )=argmin sk,s l S mögl.p aare Q((s k,s l )) S P aare = S P aare (s i, s j ) S möglp aare = S möglp aare \{(s i, s j ), (s i 1, s i ), (s j, s j+1 )} end while Durchmesser Ein offensichtliches Merkmal anhand dessen Segmente, die kein Bein darstellen, zurückgewiesen werden können, ist die Breite des Segments. Diese sollte, falls es sich bei der Beobachtung um ein Bein handelt und der Abstand zwischen Segment und Sensor hinreichend groß ist, in etwa dem Durchmesser eines Beins entsprechen. Bezeichnen z r = (r r, α r ) und z l = (r l, α l ) die erste und letzte Abstandsmessung des Segments s(r, l) so hat dieses eine Breite von: b r,l = rr 2 + rl 2 2r rr l cos(α r α l ) (5.1) Die an dieser Stelle untersuchten Segmente können sowohl ein einzelnes Bein als auch ein zusammenhängendes Beinpaar enthalten. Deshalb erfolgt anhand der Breite b r,l des Segments zunächst die Untersuchung, ob eine weitere Aufteilung des Segmentes durchgeführt werden muss. Die Bewertung der Wahrscheinlichkeiten, mit der ein Segment 51

58 5 Anwesenheit von Personen Algorithm 2 Prüfung eines Segments auf eine beinähnliche Kontur. function prüfeaufbein(s) if G 2B (s) > G 2B,min and G IAV (s) < G IAV,min then if aufteilbar(s) then (s, s ) s prüfeaufbein(s ) prüfeaufbein(s ) else return false end if end if if G 1B (s) G IAV (s) G BG (s) G MP (s) > G min then return true else return false end if end function dieser Breite ein bzw. zwei Beine enthalten kann, erfolgt mit Hilfe einer fuzzy logic (siehe Abbildung 5.1). Bezeichne b min die minimal und b max die maximal erwartete Breite eines Beins. Unter Verwendung eines zusätzlichen Unschärfebereichs der Länge q b wird die Wahrscheinlichkeit, mit der es sich aufgrund der Breite um ein Bein handelt, durch den Gütewert G 1B (b) = 0 falls b b min q b b b min +q b q b falls b min q b < b b min 1 falls b min < b b max 1 b bmax q b falls b max < b b max + q b 0 falls b max + q b b (5.2) approximiert. Ebenso wird mit Hilfe der Funktion G 2B (b) = 0 falls b 2b min 2q b b 2b min +2q b 2q b falls 2b min 2q b < b 2b min 1 falls 2b min < b 2b max 1 b 2bmax 2q b falls 2b max < b 2b max + 2q b 0 falls 2b max + 2q b b (5.3) ausgewertet, wie wahrscheinlich es ist, dass ein Segment dieser Breite zwei Beine enthält Varianz der inneren Winkel Dieses Merkmal basiert auf dem von Xavier et al. vorgeschlagenen Verfahren zur Detektion von Geraden, Kreisbögen sowie Beinen in Laser-Abstandsmessungen ([41]). Beine werden dabei in den Abstandsmessungen als entartete Kreisbögen betrachtet. 52

59 5.2 Verwendetes Verfahren zur Beindetektion G 1B 1 0 b min b max b Abbildung 5.1: Darstellung des unscharfen Gütewerts G 1B in Abhängikeit von der Segmentbreite b. Beschreibt ein Maß für die Wahrscheinlichkeit mit der es sich bei dem gegenwärtigen Segment, bezogen auf dessen Breite, um ein Bein handeln kann. Abbildung 5.2: Darstellung der inneren Winkel eines Kreisbogens. Der Winkel zwischen den Randpunkten eines idealen Kreisbogens und jedem beliebigen Punkt auf diesem Kreisbogen identisch ist. 53

60 5 Anwesenheit von Personen G IAV, G IAV, max 2 0 min max Abbildung 5.3: Komponenten G IAV,σ 2 und G IAV,β des untersuchten Kriteriums G IAV zur Bewertung der Wahrscheinlichkeit, dass es sich bei einem Segment aufgrund der kreisbogenähnlichen Kontur um ein Bein handelt. Für einen vollkommenen Kreisbogen gilt, dass der Winkel β i zwischen jedem Punkt i auf dem Kreisbogen und den Randpunkten des Bogens identisch ist (vgl. Abbildung 5.2). Der Winkel gleicht somit auch dem durchschnittlichen inneren Winkel: i : β i = 1 n n β j = β av (5.4) j=1 Da beobachtete Beine in der Regel einem Kreisbogen ähneln, ist zu erwarten, dass die Abweichung eines Winkel β i vom durchschnittlichen inneren Winkel β av begrenzt ist. Diese Abweichung, ausgedrückt durch die Varianz σβ 2 = 1 n n i=1 (β av β i ) 2, muss daher unter einem Schwellwert σmax 2 liegen. Unter Verwendung eines zusätzlichen Unsicherheitsbereichs der Breite q σ 2 stellt die Funktion 1 falls σ 2 σmax 2 G IAV,σ 2(σ 2 ) = 1 σ2 σmax 2 q falls σ 2 σ 2 max < σ 2 σmax 2 + q σ 2 (5.5) 0 falls σmax 2 + q σ 2 σ 2 die varianzabhängige Komponente der Wahrscheinlichkeit, dass es sich bei dem Segment um ein Bein handelt, dar (vgl. Abbildung 5.3(a)). Damit die Ähnlichkeit zu einem Kreisbogen gewährleistet ist, muss auch der durchschnittliche Winkel β av gewisse Annahmen erfüllen. Analog zur Bewertung der Beinbreite wird auch an dieser Stelle der minimale sowie maximale erwartete Wert für β av festgelegt. Die von diesem abhängige Wahrscheinlichkeitskomponente wird durch die Funktion G IAV,β (β av ) = 0 falls β av β av,min q βav β β av,min +q βav q βav falls β av,min q βav < β β av,min 1 falls β av,min < β β av,max 1 β βav,max q βav falls β av,max < β β av,max + q βav 0 falls β av,max + q βav β (5.6) 54

61 5.2 Verwendetes Verfahren zur Beindetektion Abbildung 5.4: Beispiel für das Kriterium des Hintergrundabstands potentieller Beinsegmente. Der Hintergrundabstand d l = r l+1 r l bzw. d r = r r 1 r r nimmt i. d. linken Abbildung für beide Objekte stets große positive Werte an. Sind zwei Beinsegmente direkt benachbart (rechte Abb.), so resultiert ein betragsmäßig kleiner negativer Werte für ein Segment. bestimmt. q βav ist dabei wiederum die Breite des unscharfen Randbereichs des Intervalls. Die kombinierte Gütefunktion wird als Produkt über die beiden Komponenten gebildet: G IAV (σ 2, β) = G IAV,σ 2(σ 2 )G IAV,β (β). (5.7) Hintergrundabstand Lang schlägt vor auszunutzen, dass Menschen in der Regel einen gewissen Abstand zu anderen Objekten einhalten. Dieses Kriterium lässt sich anhand der Abstandsmessungen am Übergang zwischen den Segmenten auswerten. Seien r l bzw. r r die Abstandsmessungen des linken bzw. des rechten Randpunktes des Segmentes, so entspricht der Abstand zu den Nachbarsegmenten d l = r l+1 r l bzw. d r = r r 1 r r. Handelt es sich bei den benachbarten Segmenten um Hintergrundobjekte, so sind die Abstände stets positiv (siehe Abbildung 5.4(a)). Tritt allerdings der Fall auf, dass ein benachbartes Segment ebenfalls ein Bein des Beinpaares ist, kann auf einer Seite der Abstand auch einen geringen negativen Wert annehmen (vgl. Abbildung 5.4(b)). Deshalb wird bei der Auswertung dieses Merkmals lediglich überprüft, ob der Maximalwert von d l und d r oberhalb eines Schwellwertes d min liegt. Dies geschieht erneut mittels einer Funktion mit einem unscharfem Übergangsbereich der Breite q d : G BG (d l, d r ) = 0 falls max(d l, d r ) d min q d max(d l,d r) d min +q d q d falls d min q d < max(d l, d r ) d min 1 falls d min < max(d l, d r ). (5.8) 55

62 5 Anwesenheit von Personen Position des Segmentmittelpunktes Ähnlich einer Idee von Xavier et al. ([41]) soll ein weiteres einfaches Merkmal von Kreisbögen, das auch für die Kontur von Beinen gültig ist, überprüft werden. Der Abstand zwischen dem mittleren Punkt des Segments und der Geraden, die durch die beiden Randpunkte des Segmentes definiert wird, muss hierbei erwartungsgemäß für beinähnliche Strukturen in einem bestimmten Intervall liegen (wie in Abbildung 5.5 dargestellt). G d MP 1 0 d min d max d MP Abbildung 5.5: Gütefunktion zur Bewertung der Position des Segmentmittelpunktes. Segmente, die Beine repräsentieren, sollen der Kontur eines Kreisbogens ähneln. Als einfaches Kriterium hierfür kann die Position des Segmentmittelpunktes bezogen auf die Randpunkte bewertet werden. Die Gütefunktion G dmp bildet ein unscharfes Intervall auf das Intervall [0; 1] ab. Zur Berechnung werden die Randpunkte (r l, α l ) und (r r, α r ) in kartesische Koordinaten (x l, y l ) bzw. (x r, y r ) bezüglich der Roboterbasis überführt. Für die Abstandsbestimmung stellt man die gesuchte Gerade in der hesseschen Normalenform dar: d(x, y) = (( x y ) ( xl y l )) n 0. (5.9) Ist n 0 der auf Länge 1 normierte Normalvektor der Geraden ( ) 1 yr y n 0 = l (xl x r ) 2 + (y l y r ) 2 x l x r, (5.10) so kann hierdurch für jeden beliebigen Punkt ( x y ) T der Abstand d(x, y) ermittelt werden. 56

63 5.2 Verwendetes Verfahren zur Beindetektion Für jedes untersuchte Segment soll geprüft werden, ob der Abstand des Segmentmittelpunktes: d mp (x l+r, y l+r ) (5.11) 2 2 innerhalb eines Intervalls zwischen d min und d max mit einem unscharfen Rand der Breite q d liegt. Dies wird ausgedrückt durch: G dmp (d) = 0 falls d d min q d d d min +q d q d falls d min q d < d d min 1 falls d min < d d max 1 d dmax q d falls d max < d d max + q d 0 falls d max + q d d. (5.12) Segmentierung von zwei Beinen Besteht aufgrund der Breite eines Segmentes die Möglichkeit, dass dieses zwei Beine enthält (G 2B > G 2B,min ), und entspricht die Kontur nach Auswertung der Varianz der innneren Winkel nicht der eines einzelnen Beins (G IAV < G IAV,min ), so wird geprüft, ob eine weitere Segmentierung möglich ist. Hierfür wird nach einer Sprungstelle im Gradienten der Abstandsmessungen r i gesucht. Enthält das Segment nur ein Bein, so ist anzunehmen, dass r i r i 1 < r i+1 r i für alle Messungen i des Segments gilt. Falls die Kontur eines weiteren Beins zu diesem Segment gehört, ist diese Bedingung üblicherweise nicht überall erfüllt. Gibt es somit eine Stelle an der r i r i 1 > r i+1 r i gilt, so wird das Segment an dieser aufgeteilt. Auf beiden resultierenden Segmenten wird das Verfahren zur Beindetektion erneut ausgeführt Kumulierter Gütewert Der kumulierte Gütewert, anhand dessen ein Segment als Bein oder nicht-bein klassifiziert wird, soll als Produkt der zuvor bestimmten Gütewerte der einzelnen Merkmale bestimmt werden: G(b, σ 2, β av, d l, d r, d mp ) = G 1B (b) G IAV (σ 2, β av ) G BG (d l, d r ) G MP (d MP ). (5.13) Das ebenfalls bestimmte Maß für die Wahrscheinlichkeit, dass es sich um zwei Beine handelt (G 2B (b)), fließt an dieser Stelle nicht ein. Es wurde lediglich für die Entscheidung, ob eine weitere Segmentierung erfolgen soll, benötigt. Ein eindeutig als Bein klassifiziertes Segment erhält den Wert G(.) = 1. Für eindeutig zurückgewiesene Segmente hingegen gilt: G(.) = 0. Die unscharf abgegrenzten zulässigen Bereiche der verschiedenen Merkmale bewirken, dass auch Segmente berücksichtigt werden, die nicht alle Kriterien vollständig erfüllen. Für deren Bewertung gilt demzufolge 0 < G(.) < 1. Die Wahl eines geeigneten Schwellwertes G min (0; 1) sowie der weiteren notwendigen Parameter soll anhand der Experimentdaten erfolgen (vgl. Kapitel 7). 57

64 5 Anwesenheit von Personen 5.3 Beinpaardetektion Die zuvor untersuchte Kontur eines Segments besteht, abhängig von der Entfernung des Objekts, teilweise nur aus wenigen Messpunkten. Der Informationsgehalt kann dementsprechend sehr gering sein. Wird jedoch ein weiteres potentielles Bein in der unmittelbaren Nachbarschaft des Segments detektiert und besteht die Möglichkeit, dass es sich bei beiden Segmenten um ein Beinpaar handelt, so erhöht dies die Sicherheit, in beiden Fällen tatsächlich ein Bein detektiert zu haben. Um mögliche Beinpaare zu ermitteln, werden unter den als Bein klassifizierten Segmenten jeweils die benachbarten betrachtet. Für das potentielle Bein, ausgedrückt durch Segment S i, sei v(i) der Index des vorangegangenen und n(i) der Index des nachfolgenden als Bein klassifizierten Segments. Für alle Segmente i wird ermittelt, welche möglichen Paarungen (S v(i), S i ) bzw. (S i, S n(i) ) existieren und zulässige Beinpaare darstellen. Als Kriterium hierfür wird der Abstand der Segmentmittelpunkte D(S j, S k ) betrachtet, welcher unterhalb eines Grenzwertes liegen muss. Daraufhin erfolgt eine Bewertung der zulässigen Paare abhängig vom Abstand der Mittelpunkte sowie von der relativen Abweichung der Segmentbreiten b(s j ). Für jedes Segmentpaar (S j, S k ) wird dies durch die Funktion Q(S j, S k ) = v D(S j, S k ) + (1 v) 2 b(s j) b(s k ) D erw b(s j ) + b(s k ) (5.14) ausgedrückt. Hierbei beschreibt D erw den durchschnittlich erwarteten Abstand zweier Beine und v den Anteil, mit dem die Abweichung des Abstandes gewichtet werden soll. Aus der Menge aller zulässigen Paare wird das Paar (S j, S k ) mit dem minimalen Wert Q(S j, S k ) entnommen und als Beinpaar markiert. Gibt es weitere Paare, die S j oder S k enthalten, so werden diese entfernt. Dieser Schritt wird auf der verbliebenen Menge wiederholt, bis diese keine Paare mehr enthält und somit alle sich nicht ausschließenden Beinpaare minimaler Bewertung markiert sind. 58

65 6 Implementierung Die im Kapitel 7 beschriebenen Untersuchungen und Ergebnisse entstanden unter Verwendung einer C++ Implementierung des SLAM Verfahrens. Als Grundlage diente hierfür das in Kapitel 3.4 erläuterte Verfahren nach Grisetti et al. [21]. Dieses wurde eigenständig implementiert und um Komponenten entsprechend der Ansätze aus Kapitel 4 und 5 erweitert. Im folgenden Kapitel sollen die technischen Rahmenbedingungen sowie der Ablauf des Verfahrens in algorithmischer Form und die wesentlichen Komponenten der Implementierung vorgestellt werden. 6.1 Technische Rahmenbedingungen Hardware des Roboters Für die Durchführung dieser Arbeit stand eine autonome, mobile Roboterplattform des Typs Scitos G5, revision B zur Verfügung. Dieses System des Herstellers MetraLabs verfügt über zwei von getrennten Elektromotoren angetriebene Räder sowie ein stabilisierendes Schlepprad. Die Steuerung des Roboters erfolgt somit nach dem differential drive Prinzip. Weiterhin ist das System mit einem Industrie PC ausgestattet. Dieser verwendet einen Intel Core Duo 1,6Ghz Prozessor und ist mit 2.048MB Arbeitsspeicher sowie einer 100GB Festplatte bestückt. Als Betriebssystem kommt hierauf Linux (Fedora Core 6) zum Einsatz. Die Messung der Odometrie erfolgt mit Hilfe von in den Motoren integrierten Inkrementalgebern (engl. incremental encoder). Diese erfassen Rotationen mit einer Genauigkeit von 460 Messpunkten pro Radumdrehung. Wie in Abbildung 6.1 zu sehen ist, besitzt der Roboter einen Berührungssensor (Bumper), 24 rundum angebrachte Ultraschall-Entfernungssensoren, eine Kamera sowie einen Laser-Abstandssensor des Typs SICK S 300. Zur Gewinnung der von dem vorgeschlagenen Verfahren benötigten Entfernungsinformationen könnten sowohl der Ultraschall- als auch der Laserabstandssensor zum Einsatz kommen. Aus dem Bild einer einzelnen Kamera hingegen lassen sich keine verwendbaren Abstandsinformationen ermitteln. Vergleicht man die Eigenschaften der beiden Abstandssensoren, so verfügt der Laser- gegenüber dem Ultraschall- Abstandssensor sowohl über eine wesentlich höhere Auflösung als auch eine geringere Störanfälligkeit. Für die Datenerfassung im Rahmen dieser Arbeit wird daher ausschließlich der Laser-Abstandssensor des Typs SICK S 300 verwendet. Entsprechend seiner Konfiguration erzeugt dieser 361 Abstandsmessungen mit einer Winkelauflösung von 0,5. Das Blickfeld umfasst somit den Bereich von -90 bis 90, wobei 0 der Fahrtrichtung entspricht. Die vom Hersteller spezifizierte maximale Reichweite des Lasers beträgt 30m. Hierbei unterstützt dieser Auflösungen von 30, 40, 50 und 70mm bei einem erwarteten systematischen Fehler von maximal +/ 20mm sowie einem statistischen Fehler 59

66 6 Implementierung Abbildung 6.1: Darstellung der Sensorik des verwendeten Scitos G5 Roboters. (1σ) von 28mm Verwendete Softwarekomponenten und Bibliotheken Bereits auf dem Roboter vorhanden ist eine Installation des Player Servers (Version 2.1.1) aus dem Player Stage Framework [20]. Dieses Programm kann als Schnittstelle zwischen verschiedenen Anwendungen und der Roboterhardware fungieren. Mit Hilfe einer TCP Server/Client Architektur ko nnen Anwendungen Zugriff auf Sensor- und Positionsinformationen des Roboters erlangen. Ebenso ko nnen Kontrollbefehle ausgefu hrt und angeschlossene Gera te konfiguriert werden. Als großer Vorteil dieses Frameworks ist die Simulationsumgebung mit dem Namen Stage zu nennen, welche u ber eine identische TCP Schnittstelle verwendet werden kann. Es ist somit ohne großen Aufwand mo glich, Anwendungen vor dem Einsatz auf dem Roboter zuna chst in einer simulierten Umgebung zu testen. 60

67 6.2 Komponenten der Implementierung Die Wahl der für die Implementierung verwendeten Programmiersprache wird durch das Player Stage Framework kaum eingeschränkt, denn es stehen unter anderem Client- Bibliotheken für die Sprachen C++, Tcl, Java, und Python zur Verfügung. Aufgrund der erwarteten hohen Rechenanforderungen, die mit der Lösung der Aufgabenstellung verbunden sind, fällt die Entscheidung zugunsten der Programmiersprache C++. Diese zeichnet sich im Vergleich zu den anderen genannten Sprachen in der Regel durch eine höhere Performanz aus. Der begrenzte Funktionsumfang der Standardbibliothek der Sprache C++, gemessen beispielsweise an Java oder Python, soll durch Verwendung der Qt 4 Bibliothek [1] der Firma Trolltech ausgeglichen werden. Diese plattformunabhängige Bibliothek bietet einen breiten Funktionsumfang und soll im folgenden insbesondere für die Entwicklung einer graphischen Oberfläche zum Einsatz kommen. 6.2 Komponenten der Implementierung In diesem Abschnitt werden die wesentlichen Komponenten der Implementierung kurz vorgestellt. Dabei soll neben einer Zusammenfassung ihrer Funktionalität gegebenenfalls auch auf Herausforderungen bei ihrer Umsetzung eingegangen werden. Zur besseren Übersicht ist in Abbildung 6.2 zunächst der Informationsfluss zwischen den Komponenten dargestellt. Die Gliederung der folgenden Abschnitte entspricht den dort aufgeführten Komponenten. Die zwei vorgelagerten Module Sensordatenerfassung und Segmentierung & Klassifikation der Sensormessungen stellen Daten bereit bzw. modifizieren diese für die Verwendung durch die Hauptkomponente SLAM Partikelfilter. Letztere stellt den wesentlichen Kern der SLAM Implementierung dar und findet deswegen im Folgenden besondere Beachtung Sensordatenerfassung Vom Datenerfassungsmodul werden die Positions- und Sensorinformationen bereitgestellt. Neben der Möglichkeit diese online vom Player Server (vgl. Abschnitt 6.1.2) zu beziehen, können sie auch in Form von Logdateien vorliegen. Hierbei werden abgesehen von dem aktuellen Player Logfile Format (0.3.0) auch das ältere Format (0.2.0) sowie die Logdateien des ebenfalls häufig verwendeten Carnegie Mellon Robot Navigation Toolkits (siehe [29]) unterstützt. Somit können auch Referenzdaten anderer Forschungsgruppen verarbeitet werden. Da im weiteren Ablauf der Implementierung die Sensor- sowie Odometriedaten für einen Zeitpunkt t stets als Paar benötigt werden, sollen diese durch das Datenerfassungsmodul zusammen bereit gestellt werden. Der Player Server ermöglicht dabei die Abfrage der jeweils aktuellsten Datensätze jedes Typs, so dass diese für einen Zeitpunkt t paarweise zur Verfügung stehen. Unterschiedliche Aktualisierungsfrequenzen der verschiedenen Sensoren fallen an dieser Stelle nicht ins Gewicht, weil die Verarbeitungszeit eines Messungspaars in der Regel ein Vielfaches der Messfrequenz beträgt. Da die Auswertung von Logdaten nicht in Echtzeit erfolgen muss, werden hierbei möglichst alle Sensormessungen verwendet. Die vorliegenden Logdateien beinhalten eine größere 61

68 6 Implementierung Sensordatenerfassung Player-Server Logdatei Sensormessungen z t ={z t k 1 k 361} Odometriedaten x t, y t, t T vorgeschlagene Erweiterungen Segmentierung & Klassifikation der Sensormessungen Segmente Gewichtung Klassifizierte Sensormessungen S t ={s t j, k 1 j k 361} Gerade Linie Bein Sonstiges Beinpaar g t s j, k {g gerade, g Bein, g Beinpaar, g sonst } SLAM Partikelfilter Basisverfahren nach Grisetti et al. Bedingtes Resampling Partikel i Pose x i t, y i t, i t T Karte m t i Partikelgewicht w t i Update der Pose aktualisierte Pose Update der Karte Update der Partikelgewichte Effektive Partikelanzahl N eff Abbildung 6.2: Informationsfluß zwischen den Komponenten der Implementierung. Rechtecke repräsentieren Komponenten oder Prozesse, Blöcke mit runden Kanten stellen übermittelte Informationen dar. 62

69 6.2 Komponenten der Implementierung Zahl an Abstandsmessungen als an Odometrieinformationen. Für jeden Odometriedatensatz wird daher die Abstandsmessung mit der kleinsten zeitlichen Differenz gewählt. Um die Kartenerstellung mit weniger Informationen auch anhand der Logdateien untersuchen zu können, bietet das Datenerfassungsmodul weiterhin die Möglichkeit, nur jeden n-ten Datensatz zu verwenden Segmentierung & Klassifikation der Sensormessungen Dieses Modul stellt eine Erweiterung des von Grisetti et al. [21] beschriebenen SLAM Verfahrens dar. Hierdurch sollen zusätzliche Informationen bezüglich dynamischer Objekte der Umgebung in den Prozess der Kartenerstellung einfließen. Die Details der Implementierung des vorliegenden Moduls ergeben sich im Wesentlichen aus den Ausführungen der Kapitel 4 und 5. An dieser Stelle wird aus Gründen der Übersicht nur eine kurze Zusammenfassung gegeben. Die durch das zuvor beschriebene Modul bereitgestellten Sensormessungen (unter Verwendung der vorliegenden Hardware in der Regel 361 Abstandsmessungen) werden zunächst entsprechend dem in Kapitel erläuterten Verfahren segmentiert. Die resultierenden Vorder- und Hintergrundsegmente werden jeweils, wie in Abschnitt dargelegt, auf geradlinige Konturen untersucht und gegebenfalls weiter unterteilt. Alle als nicht geradlinig klassifizierten Segmente werden auf die Erkennungsmerkmale von Beinen untersucht (vgl. Kapitel 5.2). Dabei erfolgt möglicherweise eine weitere Aufteilung der Segmente, falls diese zwei Beine enthalten. Aus den als beinähnlich erkannten Segmenten werden anschließend mögliche Beinpaare ermittelt. Somit stellt dieses Modul für die weitere Verarbeitung eine Einteilung der Segmente in vier Klassen bereit: 1. Geradlinige Segmente 2. Einzelne Beine 3. Beine eines Beinpaares 4. Sonstige Segmente Den Abstandsmessungen der jeweiligen Segmente werden entsprechend ihrer Klassifizierung Gewichte g i {g gerade, g Bein, g Beinpaar, g sonst } zugewiesen. Die Höhe der Gewichte spiegelt dabei wider, mit welcher Wahrscheinlichkeit diese Messung durch ein statisches Objekt hervorgerufen wurde. (In informellen Experimenten erzielten Werte von g gerade = 1.0, g sonst = 0.7, g Bein = 0.5, g Beinpaar = 0.3 gute Ergebnisse.) Soll zur Untersuchung des grundlegenden SLAM Verfahrens diese Erweiterung deaktiviert werden, so kann dies ohne Änderungen an der weiteren Implementierung durch eine identische Gewichtung sämtlicher Abstandsmessungen erfolgen Partikelfilter Wie in Kapitel 3.2 ausgeführt, stellt die a posteriori Verteilung P (x t, m z 0:t, u 0:t, x 0 ) (6.1) 63

70 6 Implementierung die Lösung des SLAM Problems dar. In der vorliegenden Implementierung wird eine Approximation dieser im SLAM Partikelfilter Modul berechnet. Eine algorithmische Übersicht über die im Folgenden erläuterte Implementierung des Partikelfilters gibt Algorithmus 3. Partikel Eine Partikelmenge mit N Partikeln repräsentiert ein Sampling der gesuchten Verteilung. Jeder Partikel entspricht einer potentiell zurückgelegten Trajektorie. Er beinhaltet jeweils eine Posenschätzung sowie eine Gewichtung, welche die Güte der durch diesen Partikel repräsentierten Annahme widerspiegelt. Weiterhin enthält jeder Partikel eine Repräsentation der Karte für die entsprechende Trajektorie. Karte Zur Darstellung der Karte wird eine occupancy grid map verwendet, wie sie in Kapitel kurz vorgestellt wurde. Hierbei werden Flächenabschnitte zu Zellen zusammengefasst. Es handelt sich somit um eine diskrete Repräsentation der Umgebung. Jede Zelle speichert ein Maß für die Wahrscheinlichkeit, dass die zugehörige Fläche ein Hindernis enthält. Diese ist sowohl abhängig von der Art des in dieser Zelle beobachteten Objekts, als auch von der Häufigkeit mit der es dort vorgefunden wird. In der Implementierung wird dieses Maß durch einen gesättigten Zähler c(x, y) umgesetzt. Gleicht der Wert der Zelle dem minimal annehmbaren Wert c min, so gilt für die Wahrscheinlichkeit, dass dort ein Hindernis existiert: P ( Hindernis in Zelle c(x, y) ) = 0. Nimmt sie hingegen den maximalen Wert c max an, so wird von einem Hindernis in der Zelle ausgegangen. Für alle Werte c min < c(x, y) < c max sei: Update der Pose P ( Hindernis in Zelle c(x, y) ) = c(x, y) + c min c max c min. (6.2) Stehen neue Sensormessungen sowie Odometriedaten bereit, wird für jeden Partikel ein Update der Pose durchgeführt. Die Odometriedaten beschreiben jeweils eine absolute Pose relativ zur Startpose des Roboters. Da für die Posenannahme jedes Partikels nur die im letzten Zeitschritt durchgeführte Bewegung zu berücksichtigen ist, wird die Differenz u t = (ˆx t, ŷ t, ˆθ t ) T (ˆx t 1, ŷ t 1, ˆθ t 1 ) T ermittelt. Ausgehend von der zuvor angenommenen Pose x t 1 wird für den Partikel zunächst die aufgrund der Odometrieänderung erwartete Pose x t = x t 1 + u t bestimmt. Anschließend wird unter Verwendung der Sensormessungen z t sowie der bisherigen Karte m t 1 die Pose mit der höchsten Beobachtungswahrscheinlichkeit x t = argmax x P (x m t 1, z t, x t) bestimmt. Dies geschieht durch ein Scanmatching. Im Rahmen einer lokalen Suche in der Umgebung von x t wird die Beobachtungswahrscheinlichkeit für einzelne Posen mit Hilfe eines Strahlenendpunktmodells ausgewertet. Hierbei wird im Posenraum nach dem Gradientenaufstiegs- bzw. hill climbing-prinzip stets die Richtung der größten Verbesserung verfolgt. Die Schrittweite wird dabei jeweils halbiert, sobald in keine Richtung eine Verbesserung festzustellen 64

71 6.2 Komponenten der Implementierung ist. Die Suche wird als erfolglos abgebrochen, falls sie nicht gegen eine Pose x t in einer begrenzten Umgebung um x t konvergiert (d.h. ( x t x t )2 + (ỹ t x y) 2 > d max bzw. θ t θ t > θ max ) oder das Maß für die Beobachtungswahrscheinlichkeit des gefundenen Maximums einen Schwellwert unterschreitet (H(z t x t, m) < H min ). Erfolgreich beendet wird die Suche, wenn die Schrittweite einen Minimalwert erreicht und die Bewertung der gegenwärtigen Pose H min überschreitet. Die minimale Schrittweite begrenzt dabei neben der Auflösung des Kartenrasters die Genauigkeit des Scanmatchings. Zur Bewertung der Pose in einem Suchschritt wird für jede Abstandsmessung zt k zunächst ausgehend von der zu untersuchenden Pose der erwartete Endpunkt bestimmt. Dieser sollte bei korrekter Lokalisierung mit einem Hindernis in der Karte an dieser Stelle korrespondieren. Falls in der erwarteten Zelle der Karte kein Hindernis vorliegt, wird der Abstand zum nächstliegenden Hindernis bestimmt. Da dieser Schritt sehr häufig ausgeführt wird (#Partikel x #Abstandsmessungen x #Suchschritte mal für jeden Zeitpunkt t), wurde an dieser Stelle auf eine effiziente Implementierung der Suche nach dem nächstliegenden Hindernis geachtet. Aufgrund des ermittelten Abstands zwischen Strahlenendpunkt und dem Hindernis in der Karte bekommt jeder Strahl ein Maß für die Wahrscheinlichkeit zugewiesen, mit der die durch ihn repräsentierte Beobachtung zt k bei Kenntnis von Karte und Pose gemacht werden kann. Wie bereits in Abschnitt 4.3 erläutert, wird hierbei eine heuristische Approximation der kumulierten Beobachtungswahrscheinlichkeit K H(z t x t, m) = gt k P (zt k x t, m) k=1 verwendet. Diese ermöglicht, im Gegensatz zu dem von Thrun vorgeschlagenen Beobachtungsmodell (vgl ), eine zusätzliche Gewichtung der einzelnen Abstandsmessungen entsprechend der Klassifizierung des zugehörigen Segments. Weiterhin kann hierdurch ein Problem vermieden werden, welches sich in informellen Experimenten mit dem multiplikativen Ansatz von Thrun gezeigt hat: Veränderungen in der Umgebung, die zwischen dem Sensor und einem in der Karte verzeichneten Hindernis stattfinden, (beispielsweise das Auftreten von Personen) führen zu Abstandsmessungen, die kürzer sind als erwartet. Diese werden im Modell von Thrun durch P unerw (zt k x t, m) modelliert. Fällt die Gewichtung dieser Komponente des Beobachtungsmodells zu gering aus, so wird sowohl den verkürzten Abstandsmessungen, die durch die Umgebungsveränderung hervorgerufen wurden, als auch der gesamte Sensorbeobachtung aufgrund des multiplikativen Zusammenhangs nur eine geringe Wahrscheinlichkeit zugewiesen. Selbst wenn in einem solchen Fall die Beobachtungswahrscheinlichkeit der korrekten Pose weiterhin das Maximum darstellt, fällt es schwer, anhand des zugewiesenen Wertes zu erkennen, ob die Maximumssuche erfolgreich war. Erhält die Wahrscheinlichkeit für unerwartete Objekte im Beobachtungsmodell der Sensormessungen eine hohe Gewichtung, so reduziert dies den Einfluss der bezogen auf die Karte korrekten Beobachtungen und verschlechtert somit wiederum die Ergebnisse der lokalen Maximumssuche. Durch den additiven Zusammenhang der Sensorbeobachtungen bei Verwendung der vorgeschlagenen Heuristik verringern einzelne Messungen mit sehr kleiner Beobachtungswahrscheinlichkeit die 65

72 6 Implementierung Gesamtbewertung einer Pose nur geringfügig. Dies ermöglicht es weiterhin das Beobachtungsmodell auf die wesentliche, der von Thrun vorgeschlagenen Komponenten zu reduzieren. Aus Effizienzgründen wird somit für jede Abstandsmessung nur die Wahrscheinlichkeit P korrekt (zt k x t, m) ausgewertet, mit der diese Messung durch ein Objekt in der Karte unter Annahme eines lokalen, normalverteilten Messfehlers hervorgerufen werden könnte. Die Ermittlung der Pose x (i) t des Partikels i gemäß des beschriebenen Suchverfahrens ist ein deterministischer Prozess. Damit die Partikel, welche zu Beginn der Lokalisierung und Kartenerstellung alle mit identischen Posen- und Karteninformationen versehen werden, unterschiedliche Hypothesen vertreten und dabei insbesondere eine Approximation der gesuchten a posteriori Verteilung darstellen, wird die tatsächliche Pose jedes Partikels aus einer Verteilung um x (i) t gezogen. Hierfür wird an K zufällig gezogenen Posen x j aus einer Umgebung um x (i) t das Bewegungs- und das Beobachtungsmodell ausgewertet. Letzteres entspricht dabei dem Modell, wie es von Thrun (vgl. Kapitel 2.1.2) vorgeschlagen wurde. Das Produkt aus beiden Modellen repräsentiert einen Wert der gesuchten a posteriori Verteilung (vgl. Rao-Blackwellizing bspw. Formel 3.20). Dieses für jeden Partikel ermittelte lokale Sampling der gesuchten Verteilung wird jeweils durch eine multivariate Normalverteilung approximiert. Dabei werden zunächst der Erwartungswert µ (i) t sowie ein Normalisierungswert η (i) t wie folgt bestimmt: µ (i) t = η (i) t = K j=1 K j=1 x j P (z t x j, m (i) t 1 ) P (x j x t 1, u t ) (6.4) P (z t x j, m (i) t 1 ) P (x j x t 1, u t ). (6.5) Anschließend kann die zugehörige Kovarianzmatrix Σ t berechnet werden durch: Σ (i) t = 1 η (i) t K j=1 (x j µ (i) t Die aktualisierte Pose x (i) t des Partikels wird aus der hierdurch bestimmten Normal-, Σ (i) t )). verteilung gezogen (x (i) t N (µ (i) t Update der Partikelgewichte )(x j µ (i) t ) T P (z t x j, m (i) t 1 ) P (x j x t 1, u t ). (6.6) Entsprechend dem von Grisetti et al. vorgeschlagenen Verfahren (vgl. Gleichung 3.30) kann die Neuberechnung der Partikelgewichte rekursiv erfolgen: w (i) t = w (i) t 1 P (z t x )P (x x (i) t 1, u t 1)dx. (6.7) 66

73 6.2 Komponenten der Implementierung Da sowohl Beobachtungs- als auch Bewegungsmodell mit wachsendem Abstand zur gegenwärtigen Pose x t erwartungsgemäß gegen 0 konvergieren, kann das hierbei verwendete Integral durch eine in der Umgebung von x t gesamplete Summe approximiert werden. Diese wurde bereits zur Bestimmung der Normalverteilung im vorangegangenen Abschnitt in Form von η berechnet. Die Aktualisierung der Partikelgewichte ergibt sich somit zu: w (i) t Update der Karte w (i) t 1 K j=1 P (z t m (i) t 1, x j) P (x j x (i) t 1, u t) = w (i) t 1 η(i) t. (6.8) Im Anschluss an die Lokalisierung bzw. das Update der Pose x t kann die Karte jedes Partikels aktualisiert werden. Unter Verwendung der Sensormessungen z t sowie der aus der Klassifikation hervorgegangenen Gewichtung g t werden die gegenwärtigen Beobachtungen in die Karte eingetragen. Jede Messung z k t, deren Wert den maximal messbaren Abstand durch den Sensor unterschreitet, repräsentiert dabei zwei Annahmen: 1. Alle Kartenzellen, die der Strahl durchquert ohne reflektiert zu werden, sind frei von Hindernissen. 2. In der Kartenzelle, in der der Stahl endet, befindet sich ein Hindernis. Messungen, die dem maximalen Messwert entsprechen, sind nicht so einfach zu deuten. Sie können einerseits auftreten, wenn sich kein Hindernis innerhalb der Sensorreichweite befindet, andererseits sind sie zu beobachten, wenn das Material des Hindernisses den Messstrahl nicht ausreichend reflektiert (z.b. Glas) oder diesen vollständig absorbiert. Aus diesem Grund dürfen derartige Messungen bei der Aktualisierung der Karte nicht berücksichtigt werden. Für die verwendeten Messungen zt k < z max wird jeweils der Endpunkt des Messstrahles ermittelt. Befindet sich der Laserabstandssensor (der Ausgangspunkt der Messstrahlen) nicht im Ursprung des Roboterkoordinatensystems sondern ist von diesem um ( ) T l x l y verschoben, so muss hierfür zunächst die Position des Sensors x l t in Weltkoordinaten aus der gegenwärtigen Pose ( x t y t ) T θ t bestimmt werden: x l t = x t + l x cos(θ t ) l y sin(θ t ) (6.9) y l t = y t + l x sin(θ t ) + l y cos(θ t ). (6.10) Mit Hilfe von x l t, dem gemessenen Abstand zt k sowie dem Messwinkel bezogen auf die Fahrtrichtung des Roboters δ k kann der Endpunkt des Messstrahls ( ( ) T x ep y ep ) wie folgt berechnet werden: x ep = x l t + zt k sin(θ t + δ k ) (6.11) y ep = y l t + z k t cos(θ t + δ k ). (6.12) Der Zähler der zugehörigen Kartenzelle c(x ep, y ep ) wird anschließend entsprechend der aus der Klassifikation hervorgegangenen Gewichtung des Messstrahls g t (z k t ) erhöht. 67

74 6 Implementierung Somit sind geradlinige Strukturen bereits nach wenigen Beobachtungen durch hohe Hinderniswahrscheinlichkeiten in den Zellen verzeichnet, wo hingegen beinähnliche Objekte mehr als doppelt so oft an der gleichen Stelle beobachtet werden müssen, um mit identischem Wert in der Karte verzeichnet zu sein. Für den geänderten Wert des Zellenzählers gilt: { c c(xep, y (x ep, y ep ) = ep ) + g t (zt k ) falls c(x ep, y ep ) + g t (zt k ) < c max sonst. c max (6.13) Die Bestimmung der von dem Messstrahl durchquerten Zellen erfolgt abhängig von dessen Steigung ϕ = yep yl t entlang der x- oder y-achse des Koordinatensystems der Karte. x ep x l t Sei b die Seitenlänge einer Kartenzelle, so wird im Fall ϕ 1 die Geradengleichung des Messstrahls an J = xep xt b 1 Punkten ausgewertet. Bezeichnen x t,j = x t + j b (6.14) y t,j = y t + ϕ j b (6.15) für j {1,..., J} jeweils einen Punkt in der gegenwärtig betrachteten Kartenzelle, so entspricht der aktualisierte Wert dieser Zelle { c c (x t,j, y t,j ) = min c (x t,j, y t,j ) g sonst falls c (x t,j, y t,j ) g sonst < c min sonst. (6.16) g sonst entspricht hierbei der Gewichtung einer weder als geradlinig noch als beinförmig klassifizierten Sensormessung. Eine bisher nicht angesprochene Herausforderung bei der Implementierung des Karten Updates ergibt sich durch die zu Beginn der Kartengenerierung unbekannte Größe der Umgebung und demzufolge der resultierenden Karte. Werden Hindernisse beobachtet, die außerhalb des bisher kartierten Bereichs liegen, so muss die Kartenrepräsentation in geeigneter Weise wachsen. In der vorliegenden Implementierung wurde deshalb besonderer Wert auf eine effiziente Speicherverwaltung gelegt, um ein schnelles Wachstum der Karte in jede Richtung zu ermöglichen. Zur weiteren Reduzierung der hierbei notwendigen Kopieroperationen von Speicherblöcken wird bei der Erweiterung der Karte in eine Richtung ein zusätzlicher Reservebereich angefügt. Bedingtes Resampling Wie bereits unter 3.4 erläutert, wird ein Resampling nur durchgeführt, wenn die Approximation der gesuchten a posteriori Verteilung durch die gegenwärtige Partikelmenge nicht mehr hinreichend gut ist. Als Maßstab hierfür wird die effektive Partikelanzahl N eff auf Basis der Partikelgewichte in jedem Zeitschritt berechnet. Gilt N eff = 1 N i=1 ( w(i) ) 2 < N 2. (6.17) 68

75 6.2 Komponenten der Implementierung so wird eine neue Partikelmenge gezogen. Die Wahrscheinlichkeit, mit der ein Partikel i gezogen wird, ist dabei proportional zu seiner Gewichtung w (i) t. Da Partikel mit hoher Gewichtung in der Regel mehrfach gezogen werden, müssen jeweils Kopien der Kartenrepräsentationen des Ursprungspartikels angelegt werden. Die resultierende Partikelmenge ist gleichgewichtet. 69

76 6 Implementierung Algorithm 3 Update der Partikelmenge. Analog zu dem Algorithmus aus [21]. S t = {} //neue Partikelmenge g t = classsensorscan(z t ) //Klassifikation der Sensormessungen for all s (i) t 1 S t 1 do x (i) t 1, w(i) t 1, m(i) t 1 = s(i) t 1 x t (i) = x (i) t 1 u t 1 // Bestimmung der Pose mit maximaler Beobachtungswahrscheinlichkeit ˆx (i) 70 t = scanmatch(x t (i), z t, g t ) if ˆx (i) t =failure then //fehlgeschlagenes Scan-matching x (i) t w (i) t p(x t x (i) t 1, u t 1) = w (i) t 1 p(z t m (i) t 1, x(i) t ) else // Sampling um das Maximum der Beobachtungswahrscheinlichkeit for k = 1,..., K do x k {x j x j ˆx (i) < } end for // Berechnung der Normal-Verteilung = (0, 0, 0) T µ (i) t η (i) = 0 for all x j {x 1,..., x K } do µ (i) t = µ (i) t + x j p(z t m (i) t 1, x j) p(x j x (i) t 1, u t 1) η (i) = η (i) + p(z t m (i) end for µ (i) t = µ (i) t η (i) Σ (i) t = 0 for all x j {x 1,..., x K } do Σ (i) t end for = Σ (i) t t 1, x j) p(x j x (i) t 1, u t 1) + (x j µ (i) )(x j µ (i) ) T p(z t m (i) t 1, x j) p(x j x (i) t 1, u t 1) Σ (i) t = Σ (i) t /η (i) // Ziehung der neuen Pose des Partikels x (i) t N (µ (i) t, Σ (i) t ) // Update der Partikelgewichtung w (i) t = w (i) t 1 η(i) end if // Update der Karte m (i) t = integratescan(m (i) t 1, x(i) t, z t, g t ) // Aktualisierung der Partikelmenge S t = S t { x (i) t, w (i) t, m (i) t } end for 1 N eff = P N i=1 ( w(i) ) 2 if N eff < T then S t = resample(s t ) end if

77 7 Evaluation Im Rahmen dieses Kapitels sollen die Ergebnisse beschrieben werden, die unter Anwendung der zuvor erläuterten Verfahren erzielt wurden. Dabei wird zunächst die SLAM Implementierung unabhängig von den vorgenommenen Erweiterungen untersucht. Weiterhin wird der Einfluss der in den Kapiteln 4 und 5 vorgeschlagenen Änderungen auf die Kartenerstellung gegenüber dem reinen SLAM Verfahren anhand einer simulierten dynamischen Umgebung untersucht werden. Abschließend erfolgt eine kurze Betrachtung der Beindetektion sowie der Erkennung von geradlinigen Strukturen. 7.1 SLAM für statische Umgebungen Eine quantitative Evaluierung automatisch generierter Karten ist in der Regel nicht möglich, da hierfür eine hinreichend detaillierte Karte der Umgebung als ground truth vorliegen müsste. Selbst Baupläne und Grundrisse umfassen nicht alle Details, wie Tische, Stühle, Schränke und ähnliches, welche aber im Rahmen der Kartenerstellung in dieser verzeichnet werden. Aus diesem Grund soll im folgenden eine qualitative Untersuchung der erzeugten Karten erfolgen. Hierfür werden vier Experimente vorgestellt. Die ersten beiden zeigen eine problemlose Kartengenerierung und geben somit Aufschluss über die Leistungsfähigkeit des Verfahrens. Experiment 3 und 4 vermitteln einen Eindruck von den Grenzen einer Kartengenerierung bei Einsatz dieses Verfahrens. Die Erzeugung der Karte wurde für alle Experimente auf Basis von Sensorlogs durchgeführt. Die Datensätze der ersten drei Experimente wurden jeweils durch eine manuell gesteuerte Fahrt des Scitos G5 Roboters (siehe Abschnitt 6.1.1) im Labor des Instituts für Roboterforschung der Technischen Universität Dortmund aufgezeichnet. Das letzte Experiment basiert auf den Daten von Gerkey [22]. Es handelt sich dabei um Sensoraufzeichnungen des Gates Buildings der Universität Stanford, welche mit Hilfe eines Pioneer 2DX Roboters und eines SICK LMS 200 Laser-Abstandssensor erzeugt wurden. Zur Generierung der Karte wurde in jedem der folgenden vier Experimente die zuvor beschriebene SLAM Implementierung ohne Klassifikation der Sensormessungen verwendet. Sämtliche Messungen des Abstandssensors werden somit mit identischem Gewicht sowohl für die Lokalisierung als auch beim Eintrag in der Karte verwendet. Die im Folgenden betrachteten Karten haben eine Auflösung von 5cm pro Zelle. Zur Repräsentation verschiedener Trajektorienhypothesen wurde ein Partikelfilter mit lediglich 10 Partikeln eingesetzt. Die weiteren Parameter des Partikelfilters wurden wie in Tabelle 7.1 aufgeführt gewählt. 71

78 7 Evaluation Parameter Wert Beschreibung N 10 Anzahl Partikel K 10 Anzahl gesampelter Posen 40mm max. Abstand d. gesampelten Posen θ 0.1 max. Orientierungsdifferenz d. gesampelten Posen Tabelle 7.1: In den Experimenten verwendete Parameter des Partikelfilters Darstellung und Interpretation der Ergebnisse In allen in diesem Kapitel abgebildeten Karten repräsentiert jeder Pixel eine Zelle der Kartenrepräsentation und entspricht somit einer Fläche von 5cm x 5cm. Die zu Beginn des Verfahrens allen Kartenzellen zugewiesene Hinderniswahrscheinlichkeit von 0, 5 korrespondiert in den Abbildungen mit einem mittleren Grauwert (RGB: 128,128,128). Dieser ist vor allem in Bereichen der Karte vorzufinden, in denen noch keine Beobachtungen gemacht wurden. Schwarze Bildpunkte in der Karte repräsentieren Zellen, deren Zähler den oberen Sättigungswert angenommen hat, was einer Hinderniswahrscheinlichkeit von 1 entspricht. Im Gegensatz dazu stehen weiße Pixel stellvertretend für Zellen, die mit dem unteren Sättigungsgrad markiert sind. Sie repräsentieren demnach eine Hinderniswahrscheinlichkeit von 0. Werden Zwischenergebnisse bzw. der letzte Schritt der Kartengenerierung präsentiert um das Verfahren zu untersuchen, so ist die gegenwärtige Positionsschätzung des Roboters durch eine rote Markierung veranschaulicht. Die Sensormessungen des selben Zeitpunktes werden strahlartig von diesem Punkt ausgehend dargestellt. Im Rahmen der Experimentbeschreibung in den folgenden Abschnitten werden hervorhebenswerte Punkte sowie die Probleme und Herausforderungen der Kartenerstellung angesprochen und erläutert Experiment 1 In diesem Experiment wird die Karte eines Korridors von ca. 35m Länge sowie der beidseitig anschließenden Räume erzeugt. Anhand von Zwischenergebnissen und dem Endresultat der Kartengenerierung sollen die Funktionalität des Verfahrens nachgewiesen und interessante Details erläutert werden. Die Abbildungen 7.1(a) und 7.1(b) zeigen den zu Beginn der Kartenerstellung zunächst kleinen untersuchten Bereich von ca. 18m x 15m. Die von dem Roboter durchgeführte Drehung um ca. 270 stellte während der Entwicklung des Verfahrens aufgrund des sich schnell ändernden Abstandsprofils eine Herausforderung für die Lokalisierung dar. Die oben genannten Abbildungen belegen die Robustheit des Verfahrens gegenüber derartigen Bewegungen. Der in Abbildung 7.2 zu erkennende Korridor zeichnet sich in der Karte durch eine geradlinige Begrenzung aus. Dies ist ebenfalls ein Qualitätsmerkmal der Lokalisierung. Wird eine Karte unter Verwendung fehlerbehafteter Positionsinformationen (beispielsweise bei ausschließlicher Verwendung der Odometrie) erstellt, so weisen gerade Strukturen oftmals eine durch Odometriefehler bedingte Krümmung auf. Auffällig sind 72

79 7.1 SLAM für statische Umgebungen die in dieser Abbildung in der Mitte des Korridors befindlichen Artefakte. Diese Bereiche mit leicht erhöhter Hinderniswahrscheinlichkeit resultieren aus den Abstandsmessungen, welche dem Wert der Sensorreichweite z max entsprechen. Derartige Messungen treten einerseits auf, wenn kein Hindernis innerhalb der Sensorreichweite vorgefunden wird, andererseits aber auch, wenn der Laserstrahl von einem Objekt nicht hinreichend reflektiert wird. Derart fehlerhafte Messungen sind in einigen der folgenden Abbildungen als Strahlen zu erkennen, die weit über die eigentliche Umgebungskontur hinausragen. Diese stellen auch den Hauptgrund für den Übergang zu einer kumulativen Wahrscheinlichkeitsheuristik (wie in Abschnitt 4.3 ausgeführt) dar. Aufgrund dieser Zweideutigkeit der Abstandsmessungen maximaler Länge werden sie nicht für die Aktualisierung der Karte verwendet. Insbesondere bei Räumen, deren Länge die Reichweite des Lasers (für die vorliegende Hardware 30m) überschreiten, treten diese in Fahrtrichtung des Roboters auf und führen somit zu den beobachteten Artefakten in der Karte. Eine Korrektur der Karte erfolgt in der Regel nach einer Beobachtung der entsprechenden Bereiche aus einer anderen Perspektive. Ihre Entstehung kann durch eine weniger geradlinige Bewegung des Roboters vermieden werden. In den in Abbildung 7.2 dargestellten Abstandsmessungen erkennt man weiterhin die Kontur eines Beinpaares. Betrachtet man dieselbe Position in einer späteren Karte (Abbildung 7.3), so fällt auf, dass dieses Beinpaar aufgrund der kurzen Beobachtungsdauer nur mit geringer Wahrscheinlichkeit in dieser verzeichnet wurde. Insbesondere in dem Bereich der Abbildung, welcher den an den Korridor anschließenden Raum (rechts oben) zeigt, erkennt man unscharfe Strukturen in der Nähe der Wände. Diese werden durch Verschattungen, beispielsweise von Stühlen, hervorgerufen und kennzeichnen somit Bereiche, die während der Fahrt des Roboters nicht oder nicht hinreichend lange beobachtet wurden. Die Zähler der zugehörigen Kartenzellen haben somit noch keinen Sättigungswert angenommen. Dies gilt auch für die Bereiche der angrenzenden Räume, die durch wenige Beobachtungen des vorbeifahrenden Roboters lediglich schemenhaft in der Karte erfasst wurden. Abbildung 7.1: Beginn der Kartenerstellung im Rahmen von Experiment 1. Erfolgreiche Lokalisierung während der Durchführung einer Drehung. 73

80 7 Evaluation Abbildung 7.2: Teil der in Experiment 1 erzeugten Karte mit erkennbaren Artefakten innerhalb des dargestellten Korridors. Durch den Laser-Abstandssensor erfasste Beinkontur. 74

81 7.1 SLAM für statische Umgebungen Abbildung 7.3: Überblick über den gesamten im Rahmen von Experiment 1 kartierten Bereich. 75

82 7 Evaluation Experiment 2 Der in diesem Experiment kartierte Bereich entspricht im wesentlichen dem aus Experiment 1. Allerdings durchfährt der Roboter den Korridor in umgekehrter Richtung. Erwartungsgemäß unterscheiden sich die Karten (wie beispielsweise in Abbildung 7.4 zu sehen) aufgrund der veränderten Beobachtungsperspektive in erster Linie in den verschatteten Bereichen. Auch die in der Darstellung des Korridors beobachtbaren Artefakte sind in geringerem Ausmaß vorhanden. Dies lässt sich auf die in diesem Experiment gefahrene leicht gebogene Trajektorie des Roboters zurückführen. Hierdurch werden Bereiche, die zunächst aufgrund der unzureichenden Informationen aus den maximalen Abstandsmessungen nicht aktualisiert werden konnten, seitlich vom Roboter passiert und entsprechend der so gemachten Beobachtungen in der Karte verzeichnet. Eine weitere Veränderung bezogen auf das vorangegangene Experiment ist die Kartierung eines an den Korridor angeschlossenen Raums. In diesem ca. 8x9m großen Raum befinden sich viele Tische, Stühle sowie auf dem Boden stehende Computer, die als Hindernisse in der Karte verzeichnet werden. Aufgrund der Vielzahl kleiner Strukturen ist es, wie in Abbildung 7.5 zu erkennen, nur durch mehrmaliges Auf- und Abfahren möglich, eine hinreichende Kartierung des Raumgrundrisses zu erhalten. Es ist erkennbar, dass die Lokalisierung des Roboters trotz der sich gegenseitig verdeckenden Objekte, durch welche das Profil der Abstandsmessungen bereits nach kurzen Bewegungen erheblich variiert, hinreichend gut funktioniert. Abbildung 7.6 belegt weiterhin, dass auch nach dem Verlassen des Raumes durch den Roboter eine korrekte Lokalisierung in dem bereits zuvor in der Karte verzeichneten Korridor erfolgt. Abbildung 7.4: Anfangsphase der Kartenerstellung im Rahmen von Experiment 2. 76

83 7.1 SLAM für statische Umgebungen Abbildung 7.5: Beispiel für die Kartenerstellung in einem Raum mit einer Vielzahl kleiner Hindernisse wie Tisch- und Stuhlbeine usw. (Experiment 2). Abbildung 7.6: Erfolgreiche Lokalisierung in einem bereits kartierten Bereich nach dem Verlassen eines abgetrennten Raums (Experiment 2). 77

84 7 Evaluation Experiment 3 Das dritte Experiment erfolgt ebenfalls auf Grundlage aufgezeichneter Sensordaten einer Fahrt des Roboters durch das Institut für Roboterforschung. In diesem Fall wird jedoch ein größerer Teil des Gebäudes kartiert. Einige Punkte, die bereits in der Beschreibung der vorherigen Experimente erläutert worden sind, können anhand der Ergebnisse dieser Untersuchung noch einmal dargelegt werden. So ist erkennbar, dass die in Abbildung 7.7 zu sehenden Artefakte nach erneutem Durchqueren dieses Bereichs nahezu vollständig beseitigt wurden (vgl. Abb. 7.8). Weiterhin kann an diesen Ergebnissen gezeigt werden, dass Räume, die durch große Fenster bzw. Glasfronten begrenzt werden, aufgrund der Eigenschaften des Lasersensors nicht vollständig abbildbar sind. So resultiert die unscharfe Abgrenzung des Raumes, in dem sich der Roboter in Abbildung 7.8 befindet, aus der rechtsseitig verlaufenden Fensterfront. Um derartige Strukturen zu erfassen, könnte zusätzlich ein Sensor anderen Typs (beispielsweise der Ultraschall-Abstandssensor) zum Einsatz kommen. Ebenfalls in Abbildung 7.8 erkennbar ist ein Phänomen, dass unter der Bezeichnung loop closure Problem bekannt ist. Der Versuch, einen bereits in der Karte verzeichneten Bereich der Umgebung nicht auf einem zuvor befahrenen Pfad, sondern durch eine noch nicht kartierte direkte Verbindung zu erreichen, stellt eine schwer zu bewältigende Herausforderung an Kartenerstellungsverfahren dar. Bei Betrachtung der Abbildungen 7.8 und 7.9 werden die damit verbundenen Probleme deutlich. Bereits sehr geringe Lokalisierungsfehler, welche schon durch die begrenzte Auflösung der Karte kaum zu vermeiden sind, akkumulieren sich über die Länge des Pfades und führen somit zu einem Versatz am Übergang zu dem bereits in der Karte verzeichneten Bereich. Unter Verwendung einer rasterbasierten Kartendarstellung mit unveränderlicher Zellgröße, wie sie auch in der vorliegenden Implementierung zum Einsatz kommt, kann dieses Problem bei der Kartengenerierung zwei Situationen hervorrufen: Gelingt beim Übergang zum bereits kartierten Bereich eine Lokalisierung anhand dieser Karte, so besteht zwar eine Inkonsistenz der Karte an dieser Stelle, die Untersuchung weiterer Bereiche kann aber davon unbeeinflusst erfolgen. Wird beim Übergang hingegen die Kartenerstellung auf Basis der Lokalisierung des zuletzt durchquerten Abschnittes fortgeführt, so erzeugt dies Fehler größeren Ausmaßes, da zuvor in der Karte verzeichnete Hindernisse entfernt bzw. verschoben eingetragen werden. Dies ist auch im vorliegenden Experiment der Fall. Wie in Abbildung 7.8 zu sehen, ist die Pose des Roboters bzgl. der vor ihm liegenden Wand teilweise korrigiert worden. Dies ist auch an der leicht gebogenen Form des dahinter liegenden Ganges zu erkennen. Allerdings erfolgte die Lokalisierung aufgrund der gleichförmigen Kontur der Wand, welche einen großen Teil der Abstandsmessungen umfasst, leicht verschoben. Das Ausmaß der Verschiebung beträgt dabei lediglich 35cm bei einem zurückgelegten Pfad von ca. 80m Länge, führt aber dennoch dazu, dass die Kartenerstellung an dieser Stelle abgebrochen werden muss Experiment 4 Das abschließende Experiment zur Untersuchung des SLAM Verfahrens ohne die vorgeschlagene Klassifizierung der Sensormessungen findet auf den Referenzdaten von Gerkey 78

85 7.1 SLAM für statische Umgebungen Abbildung 7.7: Ausschnitt der in Experiment 3 erzeugten Karte mit erkennbaren Artefakten im Messbereich des Sensors. 79

86 7 Evaluation Abbildung 7.8: Die im Rahmen von Experiment 3 erzeugte Karte unmittelbar vor dem versuchten Kreisschluss. 80

87 7.1 SLAM für statische Umgebungen Abbildung 7.9: In Experiment 3 erzeugte Karte nach dem Auftreten des loop closure Problems. 81

88 7 Evaluation statt [20]. Wie oben bereits erwähnt, handelt es sich dabei um Sensoraufzeichnungen eines SICK LMS 200 Laser-Abstandssensors welcher auf einem Pioneer 2DX Roboter zum Einsatz kommt. Der verwendete Datensatz ist bei einer Fahrt des Roboters durch das weitläufige Gates Building der Stanford University entstanden. Im vorliegenden Experiment konnte aufgrund des schon zuvor erläuterten loop closure Problems nur ein Teil der Daten verwendet und somit nur die Karte eines ca. 20m x 40m großen Bereichs des Gebäudes erstellt werden. Blickt man zunächst auf Abbildung 7.10 so erkennt man, nach einer fehlerfrei erscheinenden Kartierung des Ausgangsbereichs (unten), dass beim Durchfahren des ersten längeren Korridors bereits ein kleiner Lokalisierungsfehler auftritt. So befindet sich neben der linken Wand des (in der Karte diagonal verlaufenden) Korridors ein zuvor als frei gekennzeichneter Streifen. Ein ähnlicher Fehler ist auch im folgenden längeren Korridor zu erkennen (vgl. Abbildung 7.11). Eine mögliche Ursache für diese im Vergleich zu den vorherigen Experimenten verstärkt wahrnehmbaren Fehler liegt in der schlechteren Qualität der Odometriedaten des Pioneer 2DX gegenüber dem Scitos G5. Unter Kenntnis der somit bestehenden leichten Verzerrung der Karte erklärt sich auch das erneute Auftreten des loop closure Problems, welches in Abbildung 7.11 und 7.12 zu erkennen ist. Nachdem der Roboter einen Pfad von ca. 50m Länge zurückgelegt hat, besteht an der Nahtstelle zwischen dem zuvor und zuletzt kartierten Bereich ein Versatz von ca. 45cm. 7.2 Evaluation der Erweiterungen für dynamische Umgebungen Simulation einer dynamischen Umgebung Um die am SLAM Verfahren vorgenommenen Änderungen in einer reproduzierbaren dynamischen Umgebung zu untersuchen, werden die Experimentdaten im Rahmen einer Simulation erzeugt. Hierfür wird die bereits erwähnte Stage Komponente des Player Stage Frameworks verwendet. Als Grundlage für die simulierte Umgebung dient eine Karte, deren Abmessungen und Konturen zu großen Teilen dem späteren Einsatzumfeld des Roboters entsprechen. Sie wurde manuell auf Basis der zuvor unter statischen Bedingungen erzeugten Karten des Instituts für Roboterforschung erstellt. Dynamische Änderungen in der Simulation werden dabei durch sich bewegende Objekte hervorgerufen, deren Konturen in den Messungen eines Laserabstandssensors denen von Beinen ähneln. In der Karte befinden sich insgesamt 12 Objekte dieser Art, wovon jeweils zwei so angeordnet sind, dass sie ein Beinpaar darstellen. In den verschiedenen Durchläufen des Experiments soll der Grad der dynamischen Änderungen variiert werden. Dafür werden alle Objektpaare zunächst unbewegt belassen, um eine statische Umgebung (mit stehenden Personen) zu simulieren. In weiteren Durchläufen wird die Zahl der sich bewegenden Beinpaare schrittweise erhöht. Ein großer Vorteil der Simulation besteht in der Reproduzierbarkeit von Bewegungen sowohl der dynamischen Objekte als auch des Roboters. Soll ein derartiges Szenario mehrfach unter annähernd gleichen Bedingungen mit Hilfe eines Roboters durchgeführt werden, so ist vor allem das wiederholte Abfahren einer nahzu identischen Trajektorie eine große Herausforderung. Aufgrund der sich akkumulierenden Odometriefehler führt eine reine Wiederholung der Steuersignale in der Regel nicht zu 82

89 7.2 Evaluation der Erweiterungen für dynamische Umgebungen Abbildung 7.10: Ausschnitt der während des Experiments 4 erzeugten Karte des Gates Buildings der Stanford University mit erkennbarem Fehler in der Kontur des diagonal verlaufenden Korridors. 83

90 7 Evaluation Abbildung 7.11: Karte des Gates Buildings vor dem Versuch eines loop closures (Experiment 4). 84

91 7.2 Evaluation der Erweiterungen für dynamische Umgebungen Abbildung 7.12: Fehlerhafter Kreisschluss mit einem Kartenversatz von 45cm bei einem zurückgelegten Weg des Roboters von ca. 50m. 85

92 7 Evaluation den gewünschten Ergebnissen. Zum Abfahren eines definierten Pfades ist vielmehr eine korrekte Lokalisierung erforderlich, welche jedoch an dieser Stelle zunächst evaluiert werden soll. Unter Verwendung der Stage Simulation kann die fehlerfreie Position in Form von Odometriedaten bei entsprechender Konfiguration ermittelt werden, so dass mit Hilfe eines Pfadverfolgungsprogramms, welches als Client des Player Stage Servers realisiert ist, eine durch Wegpunkte definierte Trajektorie gefahren werden kann. Auch die Wiederholung der Bewegungen von Testpersonen ist unter realen Bedingungen mit einem hohen Koordinationsaufwand verbunden. Neben den Pfaden und Schrittgrößen müssen auch die Anfangszeiten der Bewegung präzise abgestimmt sein. Im Rahmen der Simulation hingegen besteht die Schwierigkeit vor allem in einer naturgetreuen Nachbildung der menschlichen Bewegungen wie sie im folgenden Abschnitt beschrieben wird. Simulation von Personen Um die Anwesenheit von Personen zu modellieren, muss zunächst erwähnt werden, dass der Laserabstandssensor lediglich die Konturen der Beine, in der Regel in Höhe des Schienbeins wahrnehmen kann. Diese Kontur soll im weiteren als ellipsenförmig angenommen werden. Ihr Bewegungsmuster ergibt sich aus dem menschlichen Gang. Um dieses genauer zu untersuchen, wurden in einer (aus seitlicher Perspektive aufgezeichneten) Videosequenz einer laufenden Person die Hüfte, Knie und Schienbeine manuell annotiert. Die auf dieser Grundlage ausgewertete Auslenkung von Punkten auf dem Schienbein relativ zur Hüfte entspricht erwartungsgemäß zwei phasenverschobenen Schwingungen (vgl. Abbildung 7.18). Ene derartige Bewegung zweier Objekte kann im Player Stage Framework mit Hilfe zweier aufeinander abgestimmt fahrender Agenten erzeugt werden. Da für diese lediglich die Geschwindigkeit und nicht die Position direkt gesteuert werden kann, wurden aus der Kenntnis der beobachtbaren Beinauslenkung folgende Geschwindigkeitskomponenten abgeleitet: v links = v basis + v Bein sin(γ π 2 ) (7.1) v rechts = v basis + v Bein sin(γ + π 2 ) (7.2) Der Winkel γ repräsentiert dabei den gegenwärtigen Zustand des Beinpaares, v basis entspricht der Geschwindigkeit der hypothetischen Körpermitte und v Bein der Geschwindigkeit, mit der sich das Bein relativ zu dieser maximal bewegt. Unter der Annahme, dass die absolute Geschwindigkeit eines Schienbeinpunktes (v links bzw. v rechts ) vor Beginn eines neuen Schritts mit diesem Bein den Wert 0 annimmt, soll v basis = v Bein gelten. Eine Einschränkung der Simulation besteht in der unveränderlichen Kontur jedes Objekts. Während die Kontur eines Schienbeins in realen Aufzeichnungen in Abhängigkeit vom Beobachtungswinkel variiert, kann im Rahmen der Simulation nur eine durchschnittliche Kontur verwendet werden. Die Simulation von Personen erfolgt ähnlich der Steuerung des Roboters durch einen hierfür entwickelten Client des Player Stage Servers, der für jedes Beinpaar mit entsprechenden Parametern gestarteten gestartet wird. 86

93 7.2 Evaluation der Erweiterungen für dynamische Umgebungen (a) Auswertung der Schienbeinposition in Höhe der roten Linie. (b) Abstand der Schienbeine zum Körpermittelpunkt (entlang der x-achse). Abbildung 7.13: Auswertung des menschlichen Gangs anhand einer manuell annotierten Videosequenz. Annotierte Punkte an der Hüfte, den Knien und Fußgelenken Experimente Bereits bei den ersten Untersuchungen im Rahmen der Simulation stellte sich heraus, dass die Qualität der Lokalisierung und damit auch der erzeugten Karten wesentlich geringer ist, als bei der Verwendung realer Daten. Eine wahrscheinliche Ursache hierfür liegt in der begrenzten Auflösung der simulierten Umgebung, die mit einer Rasterbreite von 5cm derjenigen der zu erzeugenden Karte entspricht. Da die simulierte Umgebung somit weniger feine Details umfasst, treten Lokalisierungsprobleme insbesondere in wenig strukturierten Bereichen (beispielsweise geraden Korridoren) auf. Um diesem Problem zu begegnen wurde lediglich ein Ausschnitt der gesamten Karte untersucht, auf dem die simulationsbedingten Schwierigkeiten zu vernachlässigen sind. In der Simulation durchfährt der Roboter einen offenen Bereich der Umgebung, welcher in einen Korridor mündet. Er passiert einen Teil des Korridors bevor er umkehrt und auf einem leicht verschobenen Pfad in den Ursprungsbereich zurückkehrt (vgl. Abbildung 7.14). Wie in der Abbildung zu erkennen, befinden sich im beobachtbaren Bereich 6 (im Weiteren entsprechend der Beschriftung als P1 - P6 bezeichnete) simulierte Beinpaare, wobei die Zahl der tatsächlich bewegten Beinpaare in den einzelnen Experimenten jeweils erhöht wurde. Auf den hierbei protokollierten Daten wurde jeweils eine Kartenerstellung mit und ohne die beschriebenen Erweiterungen für dynamische Umgebungen durchgeführt. 87

94 7 Evaluation Abbildung 7.14: Darstellung der durch das Stage Programm simulierten Umgebung. Rot markiert sind die Pfade der dynamischen Objekte (Beinpaare) P1-P6. Die blauen Markierung repräsentieren die Trajektorie des simulierten Roboters. 88

95 7.2 Evaluation der Erweiterungen für dynamische Umgebungen Experiment 1 Im ersten Experiment wird zunächst der statische Fall ohne eine Bewegung der Beinpaare untersucht. Die paarweise angeordneten Objekte repräsentieren somit stehende Personen. Mit dieser Untersuchung soll gezeigt werden, dass eine Kartengenerierung in diesem Szenario mit beiden Varianten des Verfahrens erfolgreich ist, bevor in den weiteren Experimenten der Grad der dynamischen Änderungen erhöht wird. Erwartungsgemäß erzeugen beide Varianten des Verfahrens eine annähernd identische Karte (vgl. Abbildungen 7.15 und 7.15). Während der Kartengenerierung treten in keinem der beiden Fälle die üblicherweise durch nicht erfolgreiches Scanmatching hervorgerufenen Lokalisierungsprobleme auf. Abbildung 7.17 stellt ein Beispiel für die Beindetektion (markiert durch grüne Messstrahlen) des erweiterten Verfahrens dar. Aufgrund des geringen Informationsgehalts entfernter Messungen (z.b. des ca. 9,5m entfernten Beinpaares links unten) werden die zugehörigen Hindernisse in der Regel nicht als Beinpaar erkannt, sondern als nicht klassifiziertes Objekt in der Karte verzeichnet. Bessere Ergebnisse können für näher liegende Beine (z.b. das seitlich beobachtete Bein in einer Entfernung von ca. 3m, unterhalb des Roboters) erzielt werden. Abbildung 7.15: Im Rahmen des Experiments 1 durch das erweiterte Verfahren erzeugte Karte der statischen Umgebung. Experiment 2 In diesem Experiment führen die Objektpaare P1 und P3 einen simulierten Gang durch. Dabei legt P1 von seiner Startposition ausgehend den direkten Weg (s. rote Pfeilmarkierung in Abb. 7.14) zur gegenüberliegenden Wand zurück. Anschließend erfolgt die 89

96 7 Evaluation Abbildung 7.16: Durch das Basisverfahren auf Grundlage der Simulationsdaten einer statischen Umgebung erzeugte Karte (Experiment 1). Abbildung 7.17: Erfolgreiche Beindetektion an den beinähnlichen Objekten der Simulationsumgebung (grüne Sensorstrahlen). Nicht erfolgte Detektion des weiter entfernten Beinpaares. 90

97 7.2 Evaluation der Erweiterungen für dynamische Umgebungen Rückkehr zur Startposition. Diese Bewegungsfolge wird während der gesamten Simulationsdauer wiederholt. Das Objektpaar P3 führt ebenfalls während der gesamten Simulation eine Bewegung entlang des unterhalb liegenden Wandabschnittes aus. Betrachtet man zunächst die Ergebnisse des Kartengenerierungsverfahrens in der Grundversion, so ist bereits nach wenigen Zeitschritten erkennbar, dass die Objektpaare aufgrund ihrer Bewegung nur in geringem Maße in der Karte verzeichnet werden. Aus Abbildung 7.18(a) und Abbildung 7.18(b) erkennt man, wie P1 zunächst an der beobachteten Position als Hindernis Eintrag in die Karte findet. Da allerdings P1 in späteren Beobachtungen nicht mehr an dieser Position vorgefunden wird, reduziert dies die Wahrscheinlichkeit für ein dort vorliegendes Hindernis. Nachdem der Roboter ca. 80% des vorgegebenen Pfades zurückgelegt hat, tritt ein dauerhaftes Lokalisierungsproblem auf, welches als solches erkannt wird und somit weitere Aufzeichnungen der Karte (unter Verwendung einer fehlerhaften Pose) verhindert. Das Problem resultiert aus einem Scanmatching, bei dem das gefundene lokale Maximum der Beobachtungswahrscheinlichkeiten den Minimalwert für eine korrekte Pose unterschreitet. Führt man eine Kartengenerierung auf Grundlage identischer Logdaten unter Verwendung des Verfahrens mit den vorgeschlagenen Erweiterungen durch, so erfolgt diese ohne Lokalisierungsfehler. Wie in Abbildung 7.19 zu sehen, bewirkt eine erfolgreiche Beindetektion (hier beispielsweise beim Passieren von P3), dass diese Objekte nur mit verminderter Wahrscheinlichkeit für ein Hindernis in der Karte verzeichnet werden. Diese Wahrscheinlichkeit sinkt in den darauffolgenden Beobachtungen dieser Position (nachdem P3 sich weiter bewegt hat) schnell gegen Null, so dass in der Karte keine diesbezüglichen Informationen verbleiben. Gegen Ende des vorgegebenen Pfades führt ein geringer, unentdeckter Fehler in der Lokalisation des Roboters zu einer Verschiebung in der Karte (vgl. Abb. 7.20). Experiment 3 In diesem Experiment bewegen sich die Objektpaare P1 und P3 ebenfalls wie oben beschrieben. Weiterhin führen auch die Objektpaare P4 und P5 eine wiederkehrende, dem menschlichen Gang nachempfundene Bewegung aus. Wie in Abb durch rote Pfeile angedeutet, bewegt sich P4 parallel zur oberhalb liegenden Wand, während P5 eine Auf- und Abwärtsbewegung im sich darunter anschließenden Gang ausführt. Mit Blick auf die Ergebnisse des Basisverfahrens scheinen die sich bewegenden Objekte zunächst keinen negativen Einfluss auf die Lokalisierung zu haben. Wie im vorherigen Experiment sind sie in der Karte auch nur geringfügig bis gar nicht als Hindernis verzeichnet (vgl. Abb. 7.21). Allerdings kommt es, nachdem der Roboter umgekehrt ist und einen bereits kartierten Bereich der Umgebung durchfährt, wie in Abbildung 7.22 zu sehen, zu einer fehlerhaften Lokalisierung. Dieser Fehler wird als solcher erkannt, was eine Aussetzung der Kartenaktualisierung zur Folge hat. Wenige Zeitschritte später gelingt eine Relokalisierung (Abb. 7.23) und die Kartenaktualisierung wird fortgesetzt. Bei Anwendung des erweiterten Verfahrens gelingt im vorliegenden Szenario eine durchgehend fehlerfreie Lokalisierung. Die hierbei erzeugte Karte weist dementsprechend auch keine wesentlichen Fehler auf. 91

98 7 Evaluation Abbildung 7.18: Sensorerfassung von dynamischen Objekten bei Verwendung des SLAM Verfahrens ohne Erweiterungen. Simuliertes Beinpaar P1 wird als Hindernis erfasst und in der Karte verzeichnet (links). Aufgrund der kurzen Verweildauer an einer Position hinterlässt das simulierte Beinpaar P1 kaum Spuren in der Karte. Abbildung 7.19: Erfolgreiche Beindetektion beim Passieren des simulierten Beinpaares P3 (grüne Sensorstrahlen). 92

99 7.2 Evaluation der Erweiterungen für dynamische Umgebungen Abbildung 7.20: Ein nicht erkannter Lokalisierungsfehler führt zu Verschiebungen in der Karte. Vergleicht man die resultierenden Karten beider Verfahren (Abb und Abb. 7.25), so stellt man nur geringfügige Unterschiede fest. Wie in den Abbildungen hervorgehoben, sind die von den beobachteten Objekten verursachten Artefakte in der Karte des Basisverfahrens stärker vorhanden als unter Verwendung des erweiterten Verfahrens. Allerdings ist die durch diese Artefakte repräsentierte Hinderniswahrscheinlichkeit bei beiden Verfahren sehr gering. Im ersten Fall beträgt sie 10-20%, im zweiten beläuft sie sich auf maximal 5%. Experiment 4 Im letzten Experiment führen sämtliche Objektpaare schrittähnliche Bewegungen durch. Die Bewegungsmuster von P1, P3, P4 und P5 entsprechen denen der vorangegangenen Experimente. Das bisher statische Objektpaar P2 bewegt sich parallel zu der in der Karte vertikalen Wand, während P6 eine Bewegung entlang des Korridors ausführt. Auch die weiter erhöhte Zahl dynamischer Objekte in der Umgebung scheint die Lokalisierung unter Verwendung des Basisverfahrens nicht stärker zu beeinflussen. Die erzeugte Karte stimmt mit den Karten der vorherigen Untersuchung im Wesentlichen überein. Auch sind nahezu keine Artefakte der beweglichen Objekte in der Karte zu erkennen. Lediglich ein kleiner, nicht als solcher erkannter, Lokalisierungsfehler führt zu einer leichten Verschiebung in der Karte (vgl. Abb. 7.26). Betrachtet man die Ergebnisse unter Verwendung der SLAM Variante mit den vorgeschlagenen Erweiterungen, so erkennt man, dass dort bereits nach wenigen Zeitschritten ein unerkannter Lokalisierungsfehler zu Inkonsistenzen in der Karte geführt hat (vgl. 93

100 7 Evaluation Abbildung 7.21: Im Rahmen von Experiment 3 erzeugte Karte. Dynamische Objekte erhöhen die Hinderniswahrscheinlichkeit der Kartenzellen kaum. Abbildung 7.22: Unter Verwendung des Basisverfahrens in Experiment 3 auftretender (erkannter) Lokalisierungsfehler des Roboters in einem bereits kartierten Bereich der Umgebung. Die Aktualisierung der Karte wird ausgesetzt. 94

101 7.2 Evaluation der Erweiterungen für dynamische Umgebungen Abbildung 7.23: Erfolgreiche Relokalisierung des Roboters unter Verwendung des Basisverfahrens. Anschließende Wiederaufnahme der Kartenaktualisierung (Experiment 3). Abbildung 7.24: Aus Experiment 3 resultierende Karte bei Verwendung des Verfahrens ohne Erweiterungen für dynamische Umgebungen. Die Vergrößerungen zeigen geringfügige von P3 und P4 in ihren Aufenthaltsbereichen hervorgerufene Hinderniswahrscheinlichkeiten. 95

102 7 Evaluation Abbildung 7.25: Aus Experiment 3 resultierende Karte unter Verwendung des erweiterten Verfahrens. In den Aufenthaltsbereichen von P3 und P4 (siehe Vergrößerungen) wurden nahezu keine Hinderniswahrscheinlichkeiten verzeichnet. Abb. 7.27). Dieser Fehler ist nicht in allen durch die Partikel des Verfahrens verfolgten Posenhypothesen aufgetreten. Wie Abbildung 7.28 zeigt, existiert zu einem späteren Zeitpunkt mindestens ein Partikel mit einer korrekten Karte. Allerdings sinkt die Gewichtung dieses Partikels im Verlauf des Verfahrens erneut unter die des Partikels mit der fehlerhaften Karte. Dieser Fall kann auftreten, wenn der Fehler in der Karte die Gewichtung des zugehörigen Partikels nicht so weit reduziert, dass dieser beim Resampling aus der Partikelmenge entfernt wird. Falls der weiterhin kartierte Bereich dieses Partikels anschließend eine hohe Qualität aufweist, kann seine Gewichtung unabhängig vom zuvor verzeichneten Fehler wachsen. In der endgültigen Karte, die sich aus der Karte des Partikels mit der höchsten Gewichtung ergibt, zeigt sich der zu Beginn gemachte Fehler in Form einer gekrümmten Wand. Die restliche Umgebung wurde korrekt abgebildet. 7.3 Klassifikation der Laserabstandsmessungen In Anbetracht der Erkenntnis des vorangegangenen Abschnitts, dass die Klassifikation der Laserabstandsmessungen nur einen geringen Einfluß auf die Kartenerstellung und Lokalisierung hat, verliert diese in Bezug auf die Zielsetzung der Arbeit an Relevanz. Somit soll im Folgenden lediglich ein kurzer Überblick über die erzielten Resultate der in den Kapitel 4 und 5 vorgestellten Komponenten gegeben werden. 96

103 7.3 Klassifikation der Laserabstandsmessungen Abbildung 7.26: Im Rahmen von Experiment 4 unter Verwendung des Basisverfahrens erzeugte Karte. Ein kleiner nicht erkannter Lokalisierungsfehler führt zu einer geringen Verschiebung der Roboterpose sowie einer doppelt verzeichneten Wand in der Karte Kantenerkennung Offensichtlich ist die Zahl und Qualität der gefundenen Kanten für eine gegebene Segmentierung der Sensormessungen nur von den gewählten Parameterwerten abhängig. Diese sind der maximalen Punktabstand von der Geraden d t sowie die minimale Länge einer Kante l min. Da die detektierten Kanten nicht direkt im Rahmen der Kartenerstellung verwendet werden, sondern die Abstandsmessungen eines als Kante klassifizierten Segmentes lediglich mit einer höheren Gewichtung in das Verfahren einfließen, werden die für die Lokalisierung wichtigen kleinen Strukturen nicht verworfen. Um die Funktionsfähigkeit des Verfahrens zu überprüfen und einen Eindruck davon zu bekommen, welche Segmente im Rahmen der Kartengenerierung unter verschiedenen Parametern als Kante klassifiziert werden, wurden für die folgenden Abbildungen allein die Kantenapproximationen in einer Karte verzeichnet. Um die Kantenlängen einzelner Segmente besser zu erkennen, wurden diese im Wechsel rot und grün dargestellt. Die drei Abbildungen 7.29(a), 7.29(b) und 7.29(c) zeigen eine Kartierung des bereits in den vorangegangenen Untersuchungen erwähnten Korridors des Instituts für Roboterforschung. Entsprechend der gewählten Parameter werden in Abbildung 7.29(a) nur Kanten zu Segmenten eingezeichnet, deren Länge mindestens 150cm beträgt wobei kein Punkt des Segmentes einen Abstand von mehr als 10cm hat. Die für die Erzeugung von Abbildung 7.29(b) verwendeten Parameter entsprechen denen aller vorherigen Experimente. Die Mindestlänge für Kantensegmente beträgt dabei 60cm. Als maximaler Punktabstand wurden 4cm gewählt. 97

104 7 Evaluation Abbildung 7.27: In Experiment 4 durch das erweiterte Verfahren erzeugte Karte. Ein bereits frühzeitig in der Simulation erfolgter Lokalisierungsfehler führt zu Inkonsistenzen in der Karte. 98

105 7.3 Klassifikation der Laserabstandsmessungen Abbildung 7.28: Durch das erweiterte Verfahren in Experiment 4 erzeugte Karte ohne die zuvor beobachtete Inkonsistenz (vgl. Abb. 7.27). 99