Fakultät für Elektrotechnik und Informationstechnik. Professur für Prozessautomatisierung. Diplomarbeit

Transkript

1 Fakultät für Elektrotechnik und Informationstechnik Professur für Prozessautomatisierung Diplomarbeit Implementierung eines Mono-Kamera-SLAM Verfahrens zur visuell gestützten Navigation und Steuerung eines autonomen Luftschiffes Sven Lange Chemnitz, den 9. Dezember 27 Prüfer: Betreuer: Prof. Dr. Peter Protzel Dipl.-Inf. Niko Sünderhauf

2 Lange, Sven Implementierung eines Mono-Kamera-SLAM Verfahrens zur visuell gestützten Navigation und Steuerung eines autonomen Luftschiffes Diplomarbeit, Fakultät für Elektrotechnik und Informationstechnik Technische Universität Chemnitz, Dezember 27

3 Danksagung Ich möchte mich bei Herr Prof. Dr. Peter Protzel für die Betreuung meiner Arbeit bedanken. Weiterhin danke ich ihm für die Möglichkeit der Teilnahme an der IAV Konferenz 27 in Toulouse und dem SSRR Workshop 27 in Rom. Besonderer Dank richtet sich an Niko Sünderhauf, für die intensive Betreuung und die zahlreichen nützlichen Diskussionen und Ratschläge.

4

5 Inhaltsverzeichnis Abbildungsverzeichnis v 1 Einleitung Motivation Ausblick Das Luftschiff Fritz Koordinatensysteme Erdkoordinatensysteme Welt- und Roboterkoordinatensysteme Koordinatensystemtransformationen Winkeldarstellungen Euler-Winkel Rotationsvektor Quaternionen Sensor- und Systemmodelle Kamera Das Lochkameramodell Linsenverzerrung Projektion auf Pixelkoordinaten Rückprojektion von Bildpunkten Stereokamera Kalibrierung Messbereich Rückprojektion Inertial Measurement Unit (IMU) Beschleunigungssensor Gyroskop Global Positioning System (GPS) Sensorfusion Navigation mittels Stereokamera Prinzipieller Ablauf Harris-Ecken-Detektor i

6 Inhaltsverzeichnis SURF-Punkte Bestimmung der Bewegung Implementierung Performance Gauß-Filter Kalman Filter (KF) Extended Kalman Filter (EKF) Sigma-Punkt Kalman Filter Unscented Transformation Scaled Unscented Transformation Unscented Kalman Filter (UKF) Square-Root Unscented Kalman Filter (SRUKF) Vergleich Mono-Kamera-SLAM Das SLAM Problem Warum Mono-Kamera-SLAM? Landmarken Landmarkenrepräsentation Inverse Depth Parametrization Simulation Vorbetrachtungen Bewegung und Vehikelmodell Umgebungs- und Sensormodelle GPS Kamera IMU Landmarken Implementierung Zustandsschätzung Filterablauf Statusvektor Prozessmodell Messmodell Ablauf und Implementierung des EKF Ableitung des Prozessmodells Ableitung des Messmodells Landmarkeninitialisierung ii

7 Inhaltsverzeichnis 7.6 Ablauf und Implementierung des UKF Landmarkeninitialisierung Landmarken Definitionsbereich Ergebnisse Simulationsablauf IMU GPS & IMU GPS, IMU & Kamera IMU & Kamera Orientierungsrepräsentation innerhalb des SRUKF Sensorkonfigurationen im Überblick Aufgetretene Probleme Zusammenfassung Ausblick Glossar 94 Literaturverzeichnis 95 iii

8 iv

9 Abbildungsverzeichnis 1.1 Das Luftschiff Fritz bei einem Testflug im Sommer Darstellung wichtiger Erdkoordinatensysteme für die Navigation durch GPS- Messungen. Nach [Crassidis, 25] Darstellung wichtiger Koordinatensysteme Darstellung eines Punktes in zwei verschiedenen Koordinatensystemen Das Lochkameramodell mit umgeklappter Bildebene Unverzerrtes (links) und verzerrtes Bild (rechts) der Guppy F-46 Kamera mit einem 2,8mm Weitwinkel Objektiv Korrelation zwischen unverzerrten (links) und verzerrten (rechts) Bildkoordinaten Korrelation zwischen verzerrten (links) und unverzerrten (rechts) Bildkoordinaten Näherung der Verzerrungsfunktion (links) und der Näherungsfehler in Pixelangaben (rechts) Transformation von metrischen- zu Pixelkoordinaten Achsparallele und konvergente Stereosystem Anordnung Matlab Calibration Toolbox Drei Beispiele aus der Bilderserie zum Kalibrieren einer Kamera Small Vision System Calibration Schematische Darstellung eines Stereokamerasystems Die STH-DCSG Stereokamera von Videre Design mit einer Basisweite von 9 cm und einer Brennweite von 2,8 mm. Die Auflösung beider Kameras beträgt 64 x 48 Pixel Die selbst konstruierte Stereokamera mit zwei Guppy F-46 Kameras von Allied Vision Technologies und einer Basisweite von 5 cm. Die Auflösung beider Kameras beträgt 78 x 582 Pixel Vergleich der Disparität von zwei Stereokamerasystemen. Kamera 1 besitzt eine Basisweite von 9 cm und Kamera 2 eine Basisweite von 5 cm Beispiel des Harris-Ecken-Detektors auf dem Universitätsgelände Erste Aufnahme mit Harris Punkt und Suchschablone (links) und zweite Aufnahme (rechts) mit Suchfenster (rot), Übereinstimmung (blau) und gefundenem Harris Punkt (rot) v

10 Abbildungsverzeichnis 3.3 Ergebnis der ungefilterten Korrelationssuche (links) und das RANSAC gefilterte Ergebnis (rechts) Beispiel für die Suche nach Übereinstimmungen in zwei Bildern. Zur Verdeutlichung sind beide Quellbilder übereinander geblendet Beispiel für die Suche nach Übereinstimmungen in zwei Bildern mit dem SURF-Algorithmus. Zur Verdeutlichung sind beide Quellbilder übereinander geblendet Berechnung der 3-D-Information aus dem linken und rechten Kamerabild Matlab GUI für die erstellten Funktionen Performancevergleich bei verschiedenen Anwendungszwecken Vergleich zwischen rektifizierten und unveränderten Quellbildern bezüglich der Anzahl der berechneten SURF-Punkte (oben) und der gefundenen Übereinstimmungen zwischen zwei aufeinander folgenden Bildern Vergleich zwischen gefundenen Übereinstimmungen des SURF-Algorithmus unter Verwendung rektifizierter (links) und unveränderter Quellbilder (rechts) der gleichen Szene Kalman Filter als Blockdiagramm Lineare und nichtlineare Transformation einer normalverteilten Zufallsvariable Die linearisierte Transformation einer Normalverteilung führt zu der schwarz dargestellten Gauß-Glocke. Vergleichend ist die aus der realen Verteilung genäherte Gauß-Glocke in rot visualisiert Unscented Transformation einer Normalverteilung. Wie zuvor wurde die Näherung aus der realen Verteilung rot dargestellt. Die über die Unscented Transformation entstehende genäherte Gauß-Verteilung ist in grün visualisiert Vergleich der 3σ Unsicherheiten und des Schätzwertes eines einfachen Updateschrittes mit einem Monte-Carlo-Verfahren (gelbe Punkte und grüne Ellipse), dem Extended Kalman Filter (rot gestrichelte Ellipse) und dem Unscented Kalman Filter (blau gestrichelte Ellipse). Der schwarze Kreis zeigt die Unsicherheit in der Startposition Ein kompletter Filterdurchlauf beispielhaft dargestellt. 1) Initialisierung neuer Landmarken. 2) Vorhersage der neuen Position des Roboters mit erhöhter Unsicherheit. 3) Messung der bereits kartografisierten Landmarken aus der unsichereren Roboterposition heraus. 4) Korrektur der Roboterposition, sowie der Landmarkenposition. Damit verbunden ist ebenfalls eine Reduktion der Unsicherheiten (hier als rote und blaue Ellipsen dargestellt). Nach [Solà, 27] Qualitative Verfolgung einer Landmarke über drei Zeitschritte hinweg vi

11 Abbildungsverzeichnis 5.3 Qualitative Verfolgung einer Landmarke über drei Zeitschritte hinweg. Die Ellipsen stellen die Hypothesen des FIS Algorithmus dar. Der Grauwert deutet die Gewichtung der Annahmen an Inverse Depth Parametrization Die Konfiguration einer Stereokamera (links) mit einer Unsicherheit in der Orientierung von σ =, 5, führt zu unsicheren Landmarkenschätzungen. Diese sind in kartesischer Form (Mitte) und in als inverse Tiefe (rechts) dargestellt. Eine hinzugefügte 2σ Fehlerellipse verdeutlicht die Qualität der Gauß- Annäherung Kamerabild (rechts) der simulierten Kamera in einer dreidimensionalen Umgebung (links) Simulation einer Inertial Measurement Unit. Der abgefahrene Pfad (links oben) resultiert in den Beschleunigungsmessungen (links unten) und den Winkelgeschwindigkeitsmessungen (rechts unten). Die dabei aufgetretene Geschwindigkeit ist betragsmäßig im rechten oberen Bild dargestellt Erste Auswahlmöglichkeit bei Start der Implementierten Matlab GUI Erste Auswahlmöglichkeit bei Start der Implementierten Matlab GUI Erste Auswahlmöglichkeit bei Start der Implementierten Matlab GUI Einige Ausschnitte aus der Simulation des EKF Algorithmus mit der Sensorkonfiguration IMU, GPS und Kamera Ablaufschema des Filters Entstehen von negativen, nicht definierten inversen Entfernungen in den Sigma- Punkten. Dargestellt sind die Sigma-Punkte und die Fehlerellipse (3σ-Grenze) Zwei Lösungsmöglichkeiten, um negative Entfernungen in den Sigma-Punkten zu vermeiden. In grün sind die unveränderten Sigma-Punkte und die entsprechende Fehlerellipse dargestellt. Die rotierten Sigma-Punkte und Fehlerellipse ist links in rot zu sehen. Rechts dagegen unrotiert, dafür mit einem geringeren Streuungswert Simulationseinstellungen des abzufahrenden Pfades (oben links), der Geschwindigkeit (oben rechts) und der IMU (unten links und rechts) Positionsfehler und 3σ Grenze unter Verwendung des IMU-Sensors Positionsfehler und 3σ Grenze unter Verwendung der Kombination aus IMU und GPS-Sensors Positionsfehler und 3σ Grenze unter Verwendung der Kombination aus IMU, GPS und Kamera Positionsfehler und 3σ Grenze unter Verwendung der Kombination aus IMU- Sensor und Kamera vii

12 Abbildungsverzeichnis 8.6 Fehler in den geschätzten Landmarkenpositionen. Links die Ergebnisse des EKF und rechts die Ergebnisse des SRUKF Orientierungsfehler unter Verwendung der Kombination aus IMU-Sensor und Kamera Orientierungsfehler unter Verwendung der Kombination aus IMU-Sensor und Kamera Orientierungsfehler unter Verwendung der Kombination aus IMU-Sensor und Kamera Positionsfehler und 3σ Grenze des EKF bei Verwendung verschiedener Sensorkonfigurationen Positionsfehler und 3σ Grenze des EKF bei Verwendung verschiedener Sensorkonfigurationen Trajektorie, Kamerasichtfeld und Landmarken mit 3σ Grenze. Drei geschätzte Landmarken (Sternchen) liegen hinter der Kamera viii

13 1 Einleitung Menschen können sich im Gegensatz zu Robotern mit ihren fünf Sinnen ganz intuitiv durch die Welt bewegen. Für diese intuitive Bewegung stehen dem Roboter theoretisch verschiedene Sensoren zur Verfügung, die so gut wie möglich die Sinne des Menschen nachbilden bzw. verbessern sollen. In der Praxis ist dies allerdings nicht so einfach, da zum Verwenden verschiedener Sensoren eine gewisse Intelligenz nötig ist. In der Vergangenheit und auch noch heute wird versucht die visuelle räumliche Wahrnehmung des Menschen durch seine Augen mit Hilfe von Laserscannern oder Stereokameras nachzubilden. Beide Lösungen haben allerdings den Nachteil einer begrenzten Reichweite. [Solà, 27] hat in seiner Arbeit eine treffende Analogie für dieses Problem gefunden. Er vergleicht die eingeschränkte Sichtweite mit der Bewegung des Menschen durch dichten Nebel. Alle Objekte in der unmittelbaren Umgebung können wahrgenommen werden, doch weiter entfernte Objekte werden unscharf bzw. sind gar nicht mehr zu sehen. Nach diesem Prinzip können sich die Messungen einer Stereokamera vorgestellt werden. Ein möglicher Ausweg aus der Problematik wird mit der Verwendung eines Mono- Kamera-SLAM Verfahrens angestrebt. Vor dem Einstieg in dieses Thema sollen jedoch weitere allgemeine Fakten zur Navigation eines Roboters in der realen Welt folgen. Bei der Navigation eines mobilen Roboters ist es von essentieller Bedeutung, dass die aktuelle Position und Geschwindigkeit mit hoher Genauigkeit gemessen werden kann. Auf diesen Daten muss bspw. die Positionsregelung aufbauen und für die autonome Navigation muss es anhand dieser Messungen möglich sein, eine Pfadplanung vorzunehmen. Im Bereich der radgetriebenen Bodenroboter gibt es verschiedene Möglichkeiten den zurückgelegten Weg des Roboters zu bestimmen. So existieren bspw. Odometriesensoren an den Rädern um die zurückgelegte Wegstrecke oder die Orientierung näherungsweise zu bestimmen. Ebenso existiert bereits eine mechanische Grenze in den Bewegungsfreiheiten. 1.1 Motivation Im Gegensatz zu den eingeschränkten Bewegungsmöglichkeiten eines Bodenroboters, gibt es bei einem Luftschiff keine Einschränkung der Freiheitsgrade und die große Fläche der Außenhülle bietet eine ideale Angriffsfläche für den Wind und sämtliche Luftverwirbelungen, die sich als Störungen auf das System auswirken. Auch wenn 1

14 1 Einleitung die Antriebe in eine spezielle Richtung steuern, gibt dies noch keine Garantie, dass das Luftschiff wirklich diese Richtung einschlägt. Es ist leicht vorstellbar, dass die genauen Positionen und Geschwindigkeiten nur schwer zu messen sind. Die Kombination eines GPS-Sensors und eines Inertialen Messsystems (IMU) wäre eine mögliche Lösung für die angesprochene Messaufgabe, jedoch bringt diese Sensorfusion einen entscheidenden Nachteil mit sich. Die Navigation des Luftschiffes ist fast ausschließlich von den GPS-Messungen abhängig, da ein Interiales Messsystem keine absoluten Positionen misst und schon über kurze Distanzen eine große Positionsungenauigkeit aufakkumuliert. Wie allerdings allgemein bekannt ist, sind die GPS-Messungen nicht überall verfügbar und teilweise mit sehr großer Ungenauigkeit behaftet. So werden die GPS-Messungen in der Nähe von Gebäuden durch Reflexion der Signale an den Fassaden beeinträchtigt und sehr stark gestört. In Anbetracht der Tatsache, dass ein verlässliches System entwickelt werden soll, muss die Navigation auch mit kurzzeitigem Ausfall des GPS-Moduls zuverlässig funktionieren. Mögliche und häufig verwendete Sensoren um die Navigation zu verbessern sind Laserscanner oder Stereokameras, wobei beide Systeme den entscheidenden Nachteil haben, dass die Sensorreichweite begrenzt ist. Der oft verwendete SICK LMS2 Laserscanner hat bspw. eine Reichweite von 8 m und ist somit neben seinem recht hohen Gewicht nicht für den Einsatz auf dem Zeppelin geeignet. Auf die Problematik der Stereokamera wird später eingegangen. Eine geeignete und bereits implementierte Lösung für die Navigation eines Luftschiffes mit zeitweisem Ausfall der GPS-Messungen ließen sich nach ausgiebiger Recherchearbeit nicht finden. Eine viel versprechende Lösung bietet allerdings der Einsatz eines Mono-Kamera-Verfahrens, wie bspw. von [Solà, 27] erläutert. Hier wird die räumliche Information der Umgebung anhand nur einer Kamera und der Kenntnis der zurückgelegten Wegstrecke gewonnen. Dies bietet im Vergleich zu einer Stereokamera den Vorteil der Gewichts- und Kostenreduzierung. 1.2 Ausblick Wie das angesprochene Mono-Kamera-Verfahren umgesetzt werden kann und was die Vorteile gegenüber einer Stereokamera sind, soll im Verlauf der Arbeit aufgezeigt werden. Hierzu wird in Kapitel 2 unter anderem die Funktionsweise einer konventionellen Kamera, sowie einer Stereokameraanordnung erläutert. Anschließend wird in Kapitel 3 aufgezeigt, warum eine Stereokameraanordnung nicht die Lösung der Navigationsverbesserung auf einem Luftschiff sein kann. Darauf folgend wird die Umsetzung eines Mono-Kamera-Verfahrens ausgiebig beschrieben. Begonnen wird mit der Vorstellung verschiedener Filtermöglichkeiten um aus einer Sensorfusion von Kamera, IMU- und optionalem GPS-Sensor die Positions- und Orientierungsangaben des Luftschiffes zu verbessern. Neben dem konventionellen Ex- 2

15 1.3 Das Luftschiff Fritz tended Kalman Filter werden auch die neuen Formen des Unscented Kalman Filters beschrieben und später im Vergleich mit dem Extended Kalman Filter zum Bestimmen der Position verwendet. Dabei wird sich zeigen, ob die auf der Unscented Transformation beruhenden Filteralgorithmen im vorliegenden Problemfall wirklich besser geeignet sind. Um den Einsatz des Kalman Filters in Verbindung mit nur einer Kamera zu ermöglichen wird es sich in Kapitel 5 um die nötige Landmarkenrepräsentation drehen. Im Speziellen wird auf die Inverse Depth Parametrization eingegangen. Die Vorteile dieser Darstellung in Bezug auf den Kalman Filter werden erläutert und anschließend wird ausführlich dargelegt, wie die Inverse Depth Parametrization in den Kalman Filter integriert werden kann. Den Abschluss der Arbeit bilden die Zusammenfassung der gewonnenen Erkenntnisse und ein Ausblick auf zukünftige Arbeiten. 1.3 Das Luftschiff Fritz Seit 26 existiert an der Professur für Prozessautomatisierung das 1,5 m lange und 2,2 m breite Luftschiff Fritz. Das Forschungsprojekt zielt auf den Einsatz eines teilautonomen Luftschiffes bei Feuerwehreinsätzen, THW oder polizeilicher Aufklärung hin. Bei Feuerwehreinsätzen ist es bspw. in einigen Situationen besonders wichtig, Luftaufnahmen von einem brennenden Areal oder Gebäude zu erhalten. Momentan müssen für diesen Zweck Hubschrauber angefordert werden, welche besonders teuer und nicht immer verfügbar sind. Hier bietet das kurzfristig einsatzbereite Luftschiff den entscheidenden Vorteil der schnellen Verfügbarkeit und guten Transportmöglichkeit. Ein weiterer Vorteil eines Luftschiffes sind mögliche Schadstoffmessungen in der Nähe des Brandherdes. Hubschrauber würden solche Messungen aufgrund der hohen Luftverdrängung der Rotoren sehr stark verfälschen. Abbildung 1.1: Das Luftschiff Fritz bei einem Testflug im Sommer 27. Momentan arbeitet das Luftschiff mit Hilfe von GPS-Messungen um seine Position 3

16 1 Einleitung zu bestimmen. Um die Navigation weiter zu verbessern und robuster zu gestalten, soll in dieser Arbeit ein Algorithmus entwickelt werden, der mit Hilfe einer Kamera und Beschleunigungs- bzw. Winkelgeschwindigkeitssensoren die Position des Luftschiffes auch im Falle von nicht vorhandenen GPS-Messungen möglichst genau bestimmt. Für diesen Zweck wird das Luftschiff um eine Guppy F-46 Kamera erweitert. In Verbindung mit der Inertial Measurement Unit IMU 3 von Crossbow kann der zu entwickelnde Algorithmus auf dem OnBoard PC des Luftschiffes arbeiten und verbesserte Positionsangaben bereitstellen. 1.4 Koordinatensysteme Bei allen in dieser Arbeit verwendeten Koordinatensystemen handelt es sich um kartesische, rechtshändige Koordinatensysteme. Eine gern verwendete Eselsbrücke für die Konstruktion eines rechtshändigen Koordinatensystems bildet die Rechte-Hand- Regel. Dabei ergeben sich die Achsen X, Y, Z aus Daumen, Zeigefinger und Mittelfinger. Im Laufe der Arbeit ist es notwendig verschiedene Koordinatensysteme einzuführen. So wird es u. a. ein Roboter- oder ein Kamerakoordinatensystem geben. Punkte im 3-D-Raum können in einem beliebig definierten Koordinatensystem ausgedrückt werden, somit ist die Angabe des verwendeten Systems notwendig. Dies wird, wie allgemein üblich, über einen hochgestellten Großbuchstaben realisiert. Wird der 3- D-Punkt p bspw. im Kamerakoordinatensystem C angegeben, so folgt daraus die Notation p C, um die Eindeutigkeit des Punktes zu garantieren. Jeder so angegebene Punkt kann durch eine entsprechende Rotation und Translation in ein anderes Koordinatensystem überführt werden Erdkoordinatensysteme Da in dieser Arbeit unter anderem mit GPS-Messungen gearbeitet wird, sollen im Folgenden die üblichen Koordinatensystemdefinitionen erläutert werden. Ein kurzer Überblick über die vorgestellten Koordinatensysteme ist bspw. in [Crassidis, 25] zu finden. Ausführlichere Informationen können in [Grewal et al., 27] nachgelesen werden. Das ECI (Earth-Centered-Inertial) Koordinatensystem ist ein nicht rotierendes Koordinatensystem, d.h. es steht still bezüglich der Sterne und hat im Erdzentrum seinen Ursprungspunkt. In Abbildung 1.2 ist das ECI mit [ î 1 î 2 î 3 ] bezeichnet, wobei die î 3 Achse Richtung Nordpol und die î 1 Achse Richtung Sonne zeigt. Die î 1 Achse wird genau im Zeitpunkt der Frühlingstagundnachtgleiche (vernal equinox) definiert. Mit dem Begriff equinox wird die Zeit im Jahr bezeichnet, in der die Länge des Tages gleich der Länge der Nacht ist. D.h. die Sonne muss direkt über dem Äquator stehen, 4

17 1.4 Koordinatensysteme Abbildung 1.2: Darstellung wichtiger Erdkoordinatensysteme für die Navigation durch GPS- Messungen. Nach [Crassidis, 25] was zweimal im Jahr der Fall ist. Darum wird mit vernal der Zeitraum zum Frühlingsbeginn festgelegt. Mit der Achse î 2 wird das rechtshändige Koordinatensystem komplettiert. Ein erdfestes Koordinatensystem wird mit dem ECEF (Earth-Centered-Earth-Fixed) Koordinatensystem (Bezeichnet mit [ ê 1 ê 2 ê 3 ] ) beschrieben. Wie auch beim ECI System liegt der Koordinatenursprung im Erdmittelpunkt und die dritte Achse (ê 3 ) in der Richtung des Nordpols. Die Achse ê 1 zeigt in die Richtung des Nullmeridians und wie auch das ECI Koordinatensystem bildet das ECEF Koordinatensystem ein Rechtssystem, aus welchem sich die zweite Achse ê 2 ergibt. Die Rotation des ECEF Systems im ECI Koordinatensystem wird in Abbildung 1.2 mit dem Winkel Θ bezeichnet. Das dritte in Abbildung 1.2 dargestellte Koordinatensystem bildet das NED (North- 5

18 1 Einleitung East-Down) Koordinatensystem mit der Bezeichnung [ˆn ê ˆd]. Es entsteht durch das Anlegen einer Tangential Ebene in einem selbst definierten Punkt auf der Erdkugel. Die ˆn Achse zeigt dabei in Richtung Norden und die ê Achse in Richtung Osten. Komplettiert wird das System durch die Bedingung eines rechtshändigen Koordinatensystems, womit die ˆd Achse in Richtung Erdzentrum zeigt Welt- und Roboterkoordinatensysteme Für die Beschreibung der Roboterposition und Orientierung wird das zuvor beschriebene NED Koordinatensystem mit der Notation W verwendet. In diesem Koordinatensystem wird der später vorgestellte SLAM Algorithmus arbeiten. Weitere Koordinatensysteme ergeben sich dann hierarchisch und können wiederum alle im Weltsystem W ausgedrückt werden. Abbildung 1.3 zeigt die Lage des Body- und Kamerakoordinatensystems in Bezug zum Weltsystem. Abbildung 1.3: Darstellung wichtiger Koordinatensysteme Koordinatensystemtransformationen Eine freie Bewegung des Roboters in allen 6 Freiheitsgraden kann als Bewegung des Bodykoordinatensystems im stationären Weltkoordinatensystem aufgefasst werden. Um die momentane Lage, bestehend aus Translation und Rotation, des Roboters im Weltsystem zu beschreiben, wird im Allgemeinen ein Ortsvektor und eine Winkelbeschreibung bestehend aus Euler-Winkeln oder Quaternionen verwendet. Beide Winkelbeschreibungen haben ihre Vor- und Nachteile und werden im Unterabschnitt respektive Unterabschnitt näher erläutert. 6

19 1.4 Koordinatensysteme Im Folgenden soll es darum gehen, Raumpunkte aus einem, in das andere Koordinatensystem zu überführen. Nötig wird dies beispielsweise, um Sensorinformationen auszuwerten. So wird die Kameramessung in einem separaten Koordinatensystem stattfinden, welches wiederum im Roboterkoordinatensystem liegt, welches wiederum im Weltkoordinatensystem eingebunden ist. Die allgemeine Form um einen Punkt p C im Koordinatensystem C in einen Punkt p W im Weltkoordinatensystem W zu transformieren lautet: p W = R W C p C + t W (1.1) Wobei R W C eine Rotationsmatrix ist, welche einen Vektor aus dem Koordinatensystem C in das Weltkoordinatensystem W rotiert. Diese Rotation würde ausreichen, wenn die beiden Koordinatensysteme den gleichen Ursprungspunkt haben. Ist dies nicht der Fall, so muss zusätzlich eine Verschiebung erfolgen. Diese Verschiebung wird mit einem Vektor im Weltsystem t W beschrieben. Die beschriebene Transformation ist in Abbildung 1.4 veranschaulicht. Abbildung 1.4: Darstellung eines Punktes in zwei verschiedenen Koordinatensystemen. Analog zu der oben beschriebenen Transformation kann ein Weltpunkt in das Koordinatensystem C nach der folgenden Gleichung überführt werden: p C = ( R W C) T p W ( R W C) T t W (1.2) p C = R CW p W R CW t W (1.3) Erwähnenswert ist hierbei, dass die Inverse der Rotationsmatrix gleich der transponierten Rotationsmatrix ist (R 1 = R T ), da es sich bei der Rotationsmatrix um eine orthonormale Matrix handelt. 7

20 1 Einleitung Nach dem oben genannten Prinzip können auch mehrere Koordinatensystemtransformationen aneinander gereiht werden. Um bspw. von einem Vektor im Kamerakoordinatensystem C in das Weltkoordinatensystem W zu gelangen, ist der Weg über das Bodykoordinatensystem B notwendig: p W = R W C p C + t W (1.4) p W = R W B R BC p C + R W B t B + t W (1.5) An diesem Beispiel ist leicht zu sehen, dass komplexere Rotationen durch Multiplikation der einzelnen Rotationsmatrizen erfolgen können. 1.5 Winkeldarstellungen Euler-Winkel Im vorangegangenen Kapitel wurden Rotationen durch eine Rotationsmatrix beschrieben. Dies ist allerdings nicht die einzige Möglichkeit. Für bessere Lesbarkeit werden gern die Euler-Winkel verwendet. Sie beschreiben eine Rotation mit Hilfe von drei hintereinander durchgeführten Einzelrotationen um drei verschiedene Achsen. Die Reihenfolge in welcher die Einzelrotationen um die Achsen X, Y und Z ausgeführt werden, muss festgelegt werden, da es dafür keine einheitliche Regelung gibt. In dieser Arbeit wird die in der Luftfahrtnorm (DIN 93-1) festgelegte Regelung verwendet. Nach dieser Norm wird die erste Rotation um die Z Achse vorgenommen, die Folgende um die gedrehte Y Achse und die Dritte um die zweimal gedrehte X Achse. In manchen Quellen wird diese Rotation auch als ZY X Euler-Winkel Konvention bezeichnet. Der aus den Rotationen resultierende Lagewinkel setzt sich aus den drei Einzelwinkeln Azimut (ψ, Drehung um die Z Achse), Längsneigung (θ, Drehung um die Y Achse) und Hängewinkel (φ, Drehung um die X Achse) zusammen. Im Englischen werden die Winkel ψ, θ und φ auch als yaw, pitch und roll bezeichnet. Offen bleibt noch die Definition der Drehrichtungen. Diese werden alle folgend der rechten Hand Regel definiert. Die genannten Euler-Winkel können in die allgemeinere Form der Rotationsmatrix überführt werden, um Rotationen eines Vektors einfach berechnen zu können. Soll ein Punkt im Roboterkoordinatensystem B zu einem Punkt im Weltsystem W transformiert werden, so wird die Rotationsmatrix R W B benötigt. Diese wird wie folgt 8

21 1.5 Winkeldarstellungen aus den einzelnen Anteilen der Euler-Winkel Darstellung berechnet: R W B = rotz {ψ} roty {θ} rotx {φ} (1.6) c ψ s ψ c θ s θ 1 R W B = s ψ c ψ 1 c φ s φ (1.7) 1 s θ c θ s φ c φ c ψ c θ s ψ c φ + c ψ s θ s φ s ψ s φ + c ψ s θ c φ R W B = s ψ c θ c ψ c φ + s ψ s θ s φ c ψ s φ + s ψ s θ c φ (1.8) s θ c θ s φ c θ c φ Die Nutzung der Euler-Winkel als Winkelrepräsentation unterliegt allerdings einer Einschränkung. Es existiert eine Diskontinuität, wenn die X Achse nach oben ausgerichtet ist. In diesem Fall führen schon geringe Änderungen in der Längsneigung oder dem Hängewinkel zu Richtungsänderungen von ±18. Ein weiterer Nachteil ist, dass der Zusammenhang zwischen gemessenen Winkelgeschwindigkeiten (z.b. durch die IMU) und den Winkelgeschwindigkeiten in Euler- Winkeln mathematisch aufwändig ist (siehe Abschnitt 7.3). [Grewal et al., 27] Rotationsvektor Eher der Vollständigkeit halber sei an dieser Stelle die Darstellung einer Rotation als Rotationsvektor erwähnt. Ähnlich den Euler-Winkeln besteht der Rotationsvektor aus drei Elementen, welche jedoch eine komplett andere Bedeutung haben. Die drei Vektorelemente bilden eine Achse im Raum, welche als Drehachse definiert ist. Die Stärke der Drehung ergibt sich aus dem Betrag des Vektors. Im Gegensatz zu den Euler-Winkeln treten beim Rotationsvektor keine Diskontinuitäten auf und es kann jede beliebige Drehung im Raum mit einem Rotationsvektor ausgedrückt werden. Nachteilig wirkt sich diese Darstellung allerdings auf aneinander gereihte Rotationen aus, da die Kombination zweier Rotationen zu einer neuen Rotation mathematisch aufwändig ist. [Grewal et al., 27] Quaternionen Eine weitere elegante Lösung, eine Rotation zu beschreiben, ist die Verwendung von Quaternionen. Im Gegensatz zu den Euler-Winkeln wird hier die Rotation nicht mit drei, sondern mit vier Vektorelementen beschrieben, wobei der Vorteil in der einfachen Aneinanderreihung von Rotationen durch simple Multiplikation liegt: mit q = [ cos ( θ 2 q W C = q W B q BC (1.9) ) u1 sin ( θ 2 ) u2 sin ( ) θ 2 u3 sin ( θ T 2)] (1.1) 9

22 1 Einleitung wobei u = [ ] T u 1 u 2 u 3 ein Einheitsvektor ist, welcher die Rotationsachse angibt und mit dem Winkel θ die Stärke der Drehung definiert wird. Analog zur Rotation eines Vektors v mit Hilfe der Rotationsmatrix kann dazu auch die Quaternionendarstellung verwendet werden um auf den rotierten Vektor v zukommen : v 1 v 2 v 3 = q v 1 v 2 v 3 q 1 (1.11) wobei q 1 = [ ] T q q 1 q 2 q 3 die Inverse des Quaternions q darstellt. Analog zu den Euler-Winkeln kann ein Quaternion ebenfalls in eine Rotationsmatrix umgewandelt werden und vice versa. So gestaltet sich die Umrechnung eines Quaternions in eine Rotationsmatrix noch als recht übersichtlich mit: q 2 + q2 1 q2 2 q2 3 2 (q 1 q 2 + q q 3 ) 2 (q 1 q 3 + q q 2 ) R (q) = 2 (q 1 q 2 q q 3 ) q 2 q2 1 + q2 2 q2 3 2 (q 2 q 3 + q q 1 ) (1.12) 2 (q 1 q 3 q q 2 ) 2 (q 2 q 3 q q 1 ) q 2 q2 1 q2 2 + q2 3 Für den umgedrehten Fall sind allerdings etwas mehr Rechenschritte notwendig. Der Algorithmus hierfür und ebenso für die direkte Umrechnung zwischen Euler- Winkeln und Quaternionen kann bspw. in [Solà, 26] nachgelesen werden. Dieses Kapitel sollte nur als kurzer Überblick zum Umgang mit Quaternionen angesehen werden. Genaue Definitionen und weitere Rechenregeln sind in der einschlägigen Fachliteratur zu finden. 1

23 2 Sensor- und Systemmodelle In den folgenden Abschnitten soll ein knapper Überblick über die verwendeten Sensoren, deren Sensormodelle und physikalischen Grundlagen gegeben werden. 2.1 Kamera Die Kamera ist in der heutigen Welt eine Selbstverständlichkeit geworden. Sie bildet die 3 dimensionale Welt auf eine zweidimensionale Projektion selbiger ab. Dieses Prinzip war schon im Mittelalter bekannt und wurde unter der Bezeichnung Camera Obscura im Jahre 1544 das erste mal veröffentlicht. [Schreer, 25] Seit dem ist die Entwicklung natürlich nicht stehen geblieben und mittlerweile existiert eine unendliche Vielfalt an verschiedenen Kameramodellen. Im Allgemeinen lassen sich diese Kameras aber alle auf das einfache mathematische Modell der Lochkamera herunter brechen, worum es in diesem Kapitel gehen soll. Natürlich kann man auch zu diesem Thema ganze Bücher füllen, darum sollen die folgenden Inhalte nur als Überblick zum Verstehen der restlichen Arbeit angesehen werden. Weiterführende Informationen sind z.b. in [Jähne, 22], [Schreer, 25] oder [Hartley and Zisserman, 24] zu finden Das Lochkameramodell Abbildung 2.1: Das Lochkameramodell mit umgeklappter Bildebene. Das Lochkameramodell besteht aus einem infinitesimal kleinem Loch auf der optischen Achse Z (optical axis) und führt somit zu einer idealen Projektion auf der 11

24 2 Sensor- und Systemmodelle Abbildungsebene (image plane) (Abbildung 2.1). Den Punkt C bezeichnet man als Brennpunkt (optical center ) und die Achse orthogonal zur optischen Achse ist die Brennebene (focal plane). Der Schnittpunkt der optischen Achse mit der Bildebene ist der Kamerahauptpunkt p (principal point). Die Projektion eines 3-D-Punktes (X, Y, Z) auf die Bildebene ergibt sich aus: f X x = (2.1) y Z Y wobei (x, y) die Koordinaten des 2D Punktes auf der Bildebene darstellen Linsenverzerrung Die Linsenverzerrung ist ein Effekt der bei realen Linsen auftritt. Abha ngig von der verwendeten Linse ist dieser Effekt mehr oder weniger deutlich erkennbar. Bei der hier verwendeten Guppy F-46 Kamera mit einem 2,8 mm Weitwinkelobjektiv ist der Effekt sehr deutlich sichtbar. (siehe Abbildung 2.2) Abbildung 2.2: Unverzerrtes (links) und verzerrtes Bild (rechts) der Guppy F-46 Kamera mit einem 2,8mm Weitwinkel Objektiv. Die Linsenverzerrung wird in der normalisierten Bildebene Z = 1 dargestellt und beschreibt den Effekt, dass ein Weltpunkt nicht mehr u ber eine gerade Linie durch den Brennpunkt mit der Bildebene verbunden ist. D.h. das Lochkameramodell ist nicht mehr direkt anwendbar. Um weiterhin mit dem einfachen mathematischen Modell der Lochkamera rechnen zu ko nnen, muss das aufgenommene Bild erst entzerrt (rektifiziert) werden. Eine Mo glichkeit die Linsenverzerrung mathematisch zu beschreiben, ist das radiale Verzerrungsmodell (radial distortion model ): p x xd = 1 + κ1 r2 + κ2 r mit r = x2 + y 2 (2.2) yd y 12

25 2.1 Kamera Diese Beschreibungsmöglichkeit wird auch von den später angesprochenen Kalibrierungswerkzeugen verwendet und bildet die unverzerrten Koordinaten (x, y) auf verzerrte Bildkoordinaten (x d, y d ) ab. Die enthaltenen Parameter κ 1 bis κ n sind linsenbzw. kameraspezifisch und werden in der Regel über ein geeignetes Kalibrierungsverfahren bestimmt. Ein Beispiel für die Verzerrung eines Bildes ist in Abbildung 2.3 aufgezeigt. Es beschreibt die Umwandlung eines unverzerrten Bildes über das Verzerrungsmodell zu einem verzerrten Bild. Als Parameter wurden hier die mit der Matlab Calibration Toolbox ermittelten Kameraparameter für die Guppy Kamera F-46 verwendet Abbildung 2.3: Korrelation zwischen unverzerrten (links) und verzerrten (rechts) Bildkoordinaten. Der Zusammenhang zwischen der nachgebildeten Verzerrung und den realen Bildern in Abbildung 2.2 ist deutlich erkennbar. Im Normalfall ist es allerdings nicht das Ziel ein verzerrtes Bild zu erstellen, sondern wie bereits angesprochen, das Bild zu rektifizieren. Hierzu wird in der Regel eine Lookup Table erstellt, um die verzerrten Bildkoordinaten auf die entzerrten Koordinaten abzubilden. Um ein solches Pixelmapping zu erstellen muss für jeden Punkt die entsprechende verzerrte Bildkoordinate berechnet werden, was zu einem großen Overhead führt, wenn bspw. nur wenige interessante Punkte aus dem Bild berechnet werden sollen. Dieses Problem hat [Solà, 27] aufgegriffen und vorgeschlagen, eine neue Gruppe von Kameraparametern anzunähern, um die Entzerrung direkt zu berechnen. Wie auch schon in Gleichung 2.2 berechnen sich die entzerrten Koordinaten über ein Polynom n-ten Grades, doch diesmal mit den angenäherten neuen Parametern γ: [ x y ] = ( 1 + γ 1 r 2 d + γ 2r 4 d +...) [ x d y d ] mit r d = x 2 d + y2 d (2.3) Die Vorgehensweise zum Erhalt der neuen Parameter über eine Minimierung der Fehlerquadrate, ist in [Solà, 27] beschrieben. In Abbildung 2.4 ist das Ergebnis 13

26 2 Sensor- und Systemmodelle Abbildung 2.4: Korrelation zwischen verzerrten (links) und unverzerrten (rechts) Bildkoordinaten. der Näherung für die hier eingesetzte Guppy F-46 berechnet. Abbildung 2.5 zeigt die Näherungskurve und den entstandenen Näherungsfehler, welcher in den äußeren Bildbereichen deutlich zunimmt, aber mit maximal,25 Pixel noch im Rahmen bleibt. 1 Näherung.3 Näherungsfehler.8.2 r d.6.4 Fehler [pix].1.2 Verzerrungsfunktion Entzerrungsfunktion r r Abbildung 2.5: Näherung der Verzerrungsfunktion (links) und der Näherungsfehler in Pixelangaben (rechts) Projektion auf Pixelkoordinaten Nachdem eine einfache Kameraprojektion und die Linsenverzerrung beschrieben wurde, soll in diesem Unterabschnitt der komplette Weg dargelegt werden, wie sich ein Weltpunkt zu Pixelkoordinaten umrechnen lässt. Hierzu muss ein Weltpunkt p W über eine homogene Matrix H CW, die extrinsic matrix, in einen Punkt im Kamera- 14

27 2.1 Kamera koordinatensystem p C transformiert werden (siehe auch Unterabschnitt 1.4.3), p C = R CW p W R CW t W (2.4) p C = H CW p W (2.5) [ p C R = CW R CW ] X W C Y W 1 Z W (2.6) 1 wobei C die Koordinaten des Kamerazentrums im Weltkoordinatensystem darstellen und p W der Weltpunkt in Form eines homogenen Vektors ist. Anschließend wird der transformierte Punkt p C = [ X C Y C Z C] T in die Bildebene projiziert: [ ] x x = = 1 [ ] X C y Z C Y C (2.7) Nach der Projektion muss die in Unterabschnitt besprochene Linsenverzerrung eingebunden werden: [ ] xd = ( 1 + κ 1 r 2 + κ 2 r ) [ ] x mit r = x y 2 + y 2 (2.8) y d Abbildung 2.6: Transformation von metrischen- zu Pixelkoordinaten In einem letzten Schritt müssen die metrischen Koordinaten [ x y ] T in Pixelkoordinaten [ u v ] T umgerechnet werden (siehe Abbildung 2.6). Zu diesem Zweck wird eine intrinsic matrix K definiert: α u α θ u fs u fs θ u K = α v v = fs v v (2.9)

28 2 Sensor- und Systemmodelle Die vorgestellte Matrix der internen Kameraparameter beinhaltet Informationen über die Brennweite f und die Größe der Pixelelemente des Kamerachips. Diese sind in den Variablen s u und s v mit der Einheit Pixel pro Längeneinheit definiert. Für das Beispiel der verwendeten Guppy F-46 Kamera ergeben sich die Parameter zu: s u = s v = 1pix/.83mm. Weiterhin ist neben den Parametern u und v für den Kamerahauptpunkt (principal point) ein Parameter für die Schiefe der Pixelelemente α θ = fs θ in der intrinsischen Matrix enthalten. Schließlich ergeben sich die Pixelkoordinaten aus: u s v = K 1 x d y d 1 oder : Rückprojektion von Bildpunkten [ ] u = v [ ] αu x d + α θ y d + u α v y d + v (2.1) Bei der Nutzung der Kamera als Sensor für die Wahrnehmung der Umgebung eines mobilen Roboters ist prinzipiell nicht die Projektion eines Weltpunktes von Interesse, sondern genau der umgedrehte Fall. Interessante Punkte aus einem Bild sollen der realen Welt zugeordnet werden um somit die Navigation zu verbessern und um bspw. Hinternisse zu erkennen. Leider ist durch die Projektion in das Kamerabild eine Dimension verloren gegangen, was zur Folge hat, dass der Weltpunkt aus einem einzelnen Kamerabild nicht wieder hergestellt werden kann. [ X Y Z ] T = s [ x y 1 ] T (2.11) Gleichung 2.11 für die Rückprojektion zeigt das Problem auf. Ohne die Kenntnis der Tiefe des Weltpunktes bezüglich der Kamera, kann nur die Gerade rekonstruiert werden, auf der sich der Weltpunkt befinden muss. Um den fehlenden Skalierungsfaktor s zu berechnen bedarf es weiterer Informationen, wie z.b. ein weiteres Kamerabild mit bekannter Position und Orientierung bezüglich des ersten Kamerabildes. Die Stereokamera nutzt genau diese zusätzlichen Informationen aus, um die Rauminformationen zurückzugewinnen. 2.2 Stereokamera Eine Stereokamera besteht im Allgemeinen aus zwei baugleichen Einzelkameras, welche durch eine bekannte Anordnung, ähnlich dem menschlichen Auge, zum dreidimensionalen Sehen verwendet werden können. Bei der Anordnung der beiden Kameras kann man prinzipiell zwischen einer achsparallelen (Abbildung 2.7, links) und einer konvergenten Anordnung (Abbildung 2.7, rechts) unterscheiden, wobei der achsparallele Fall für die mathematischen Berechnungen interessant ist und das konvergente 16

29 2.2 Stereokamera System eher in der Praxis vorkommt [Schreer, 25]. Konvergente Systeme können über die so genannte Rektifikation in achsparallele Systeme transformiert werden. Abbildung 2.7: Achsparallele und konvergente Stereosystem Anordnung Ist ein Stereokamerasystem nicht achsparallel angeordnet, so spricht man von der allgemeinen Stereogeometrie oder auch Epipolargeometrie. In diesem allgemeinen Fall können die Kameras untereinander verdreht und ebenfalls verschoben sein. Dieser Zusammenhang kann mit P C 2 = R C 2C1 P C 1 + t (2.12) ausgedrückt werden. R C 2C 1 bildet hierbei eine Rotationsmatrix und t einen Verschiebungsvektor. Ist t und R bekannt, so kann eine mathematische Beziehung zwischen korrespondierenden Bildpunkten der einen und der anderen Kamera aufgestellt werden. x T Ex = E = [t] R (2.13) Die Gleichung 2.13 wird auch als Epipolargleichung bezeichnet und die Matrix E erhält den Namen Essential-Matrix. Zu beachten ist hierbei, dass x und x normalisierte Bildkoordinaten sind und nach dem idealen Lochkameramodell (d.h. die Kameramatrix ist eine Einheitsmatrix) berechnet wurden. Im Normalfall müssen diese Bildkoordinaten aus der bereits bekannten intrinsischen Matrix berechnet werden, weshalb man hier vom kalibrierten Fall spricht. Im unkalibrierten Fall wird anstatt der Essential-Matrix die Fundamental-Matrix verwendet. Im Gegensatz zur Essential- Matrix hat diese eine höhere Zahl an Freiheitsgraden und beschreibt die Beziehung der korrespondierenden Bildpunkte nicht in Sensorkoordinaten, sondern in Bildkoordinaten. Für nähere Informationen über die allgemeine Stereogeometrie sei auf [Hartley and Zisserman, 24] und [Schreer, 25] verwiesen. 17

30 2 Sensor- und Systemmodelle Kalibrierung Wie bereits im vorangegangen Kapitel zu erkennen war, müssen einige Parameter des Stereokamerasystems bekannt sein, um einen dreidimensionalen Punkt aus den korrespondierenden Bildpunkten zu berechnen. Es wird sowohl eine intrinsische als auch eine extrinsische Kalibrierungsmatrix benötigt. Die intrinsische Matrix beschreibt wie bereits erwähnt, die internen Parameter einer einzelnen Kamera. Ergänzend dazu werden über die extrinsische Matrix die Positionen der Kameras im Weltsystem beschrieben. Hieraus lässt sich die benötigte Verschiebung und Orientierung der Kameras untereinander herleiten. Abbildung 2.8: Matlab Calibration Toolbox. Mit der Kalibrierung eines Stereokamerasystems beschreibt man im Allgemeinen die Schätzung der Kameraparameter unter Zuhilfenahme eines geeigneten Kalibrierungskörpers. Der exakte mathematische Weg zur Berechnung einer Parameterschätzung soll allerdings nicht Gegenstand dieser Arbeit sein, da hierzu schon fertige Softwarelösungen existieren. Beispielsweise bietet die Matlab Calibration Toolbox [Bouguet, 24] (Abbildung 2.8) eine Möglichkeit zur Kalibrierung der Kameraparameter in Matlab. Um eine Kalibrierung durchführen zu können, muss ein Kalibrierungskörper in Form eines Schachbrettmusters aus möglichst vielen unterschiedlichen Perspektiven aufgenommen werden (siehe Abbildung 2.9). Anschließend können anhand dieser Bilderserie und der Größe der Quadrate sowie der Größe eines Pixels auf dem Kamerasensor sowohl die intrinsischen, als auch die extrinsischen Parameter der Kamera geschätzt werden. Abbildung 2.9: Drei Beispiele aus der Bilderserie zum Kalibrieren einer Kamera. Die Nutzung der Matlab Calibration Toolbox stellt sich allerdings als recht zeitraubend heraus, da für jedes Bild ein Schätzwert für die radiale Verzerrung angegeben 18

31 2.2 Stereokamera werden muss. Zusätzlich müssen die Positionen der äußeren Ecken des Schachbrettmusters in Form eines Mausklicks übergeben werden. Abbildung 2.1: Small Vision System Calibration. Verglichen zu der Matlab Calibration Toolbox erweist sich die mit der Kamera STH- DCSG gelieferte Kalibrierungssoftware Small Vision System Calibration von Videre Design [Konolige and Beymer, 24] (Abbildung 2.1) als sehr zeitsparend. Nach Aufnahme der Einzelbilder werden diese in das Tool eingelesen und durch die Angabe der Anzahl und Größe der Schachbrettquadrate und der Größe eines Pixelsensors ergänzt. Anschließend können die Kalibrierungsparameter berechnet und in einer Konfigurationsdatei abgelegt werden Messbereich Abbildung 2.11: Schematische Darstellung eines Stereokamerasystems Zur Veranschaulichung des Messbereichs eines Stereokamerasystems soll Abbildung 2.11 dienen. Dargestellt ist ein Weltpunkt, sowie dessen korrespondierende Bild- 19

32 2 Sensor- und Systemmodelle punkte in den Bildebenen der beiden Kameras. Aus den Absta nden der Bildpunkte zum Bildzentrum ergibt sich die Disparita t δ [pix] mit: δ = ul ur (2.14) Durch Anwendung des Strahlensatzes la sst sich die Entfernung d zum Zielpunkt P wie folgt berechnen: d= Wobei du h mm pix i f b δ du (2.15) die Gro ße eines Pixels ist, b die Basisweite und f die Brennweite. Abbildung 2.12: Die STH-DCSG Stereokamera von Videre Design mit einer Basisweite von 9 cm und einer Brennweite von 2,8 mm. Die Auflo sung beider Kameras betra gt 64 x 48 Pixel. Es ist leicht nachvollziehbar, dass die Basisweite ausschlaggebend fu r die Entfernungsmessung ist. Je la nger die Basisweite ist, desto weitere Entfernungen ko nnen aufgelo st werden. Nachteilig verha lt sich allerdings die Fehleranfa lligkeit eines Stereokamerasystems mit einer gro ßeren Basisweite. Die Konstruktion darf keine Verwindung der Kameras zulassen, damit die Messung eine akzeptable Messgenauigkeit beha lt. Eine solche Konstruktion kann sich aufwa ndig und kostenintensiv gestalten. Fu r den Einsatz auf dem Luftschiff spielt zusa tzlich das Gewicht eine entscheidende Rolle, somit fallen aufwa ndige und damit schwere Konstruktionen aus. Um den Einfluss der Basisweite auf die Auflo sung zu verdeutlichen, wurden zwei Stereokamerasysteme gegenu bergestellt. Auf der einen Seite die STH-DCSG Stereokamera von Videre Design mit einer Basisweite von 9 cm und auf der anderen Seite eine Eigenkonstruktion, bestehend aus zwei Guppy F-46 Kameras von Allied Vision Technologies, mit einer Basisweite von 5 cm. Um die Reichweite beider Kameras zu verdeutlichen ist in Abbildung 2.14 die Disparita t beider Kameras abha ngig von 2

33 2.2 Stereokamera Abbildung 2.13: Die selbst konstruierte Stereokamera mit zwei Guppy F-46 Kameras von Allied Vision Technologies und einer Basisweite von 5 cm. Die Auflo sung beider Kameras betra gt 78 x 582 Pixel der Entfernung eines Objektes aufgetragen. Die Parameter beider Kameras wurden mit der Kalibrierungssoftware Small Vision System Calibration bestimmt und fu r die Erstellung der Graphen genutzt. 1 1 Kamera 1 Kamera Kamera 1 Kamera 2 8 Disparität [pix] Disparität [pix] Distanz [m] Distanz [m] 1 12 Abbildung 2.14: Vergleich der Disparita t von zwei Stereokamerasystemen. Kamera 1 besitzt eine Basisweite von 9 cm und Kamera 2 eine Basisweite von 5 cm. Aus den beiden Teilbildern in Abbildung 2.14 geht hervor, dass das Stereokamerasystem mit der Basisweite von 5 cm auch noch bei Distanzen von 8 m eine Disparita t von rund zwei Pixeln aufweist, wa hrenddessen bei der anderen Kamera weniger als ein Pixel Unterschied zwischen den beiden Einzelbildern herrscht Ru ckprojektion Die Ru ckprojektion zweier Bildpunkte eines Stereokamerasystems kann bspw. u ber ein homogenes Verfahren erfolgen. (siehe [Schreer, 25]) Hierzu wird eine Matrix B 21

34 2 Sensor- und Systemmodelle folgendermaßen erstellt: u L P L (3,1:4) PL (1,1:4) v L P L (3,1:4) B = PL (2,1:4) u R P R (3,1:4) PR (1,1:4) (2.16) v R P R (3,1:4) PR (2,1:4) Die nötigen Parameter sind die Pixelkoordinaten (u, v) der linken und rechten Kamera und die Projektionsmatrizen P L und P R der linken und rechten Kamera. Mit Hilfe einer SVD-Zerlegung der Matrix kann eine Lösung des gesuchten 3-D-Punktes in Form eines homogenen Vektors gefunden werden. 2.3 Inertial Measurement Unit (IMU) Eine Inertial Measurement Unit ist ein Sensor mit dessen Hilfe eine Beschleunigung und eine Winkelgeschwindigkeit in alle drei Raumrichtungen gemessen werden kann, d.h. es werden alle 6 Freiheitsgrade berücksichtigt. Die IMU ist im Grunde ein Sensorcluster, bestehend aus drei Beschleunigungssensoren und drei Gyroskopen, welche entsprechend räumlich angeordnet sind, um die jeweiligen drei Freiheitsgrade zu erfassen. In Kombination mit einem (oder mehreren) Navigationscomputer(n) wird die IMU auch als inertial navigation system (INS) bezeichnet. Anhand der IMU-Daten erfolgt hier zusätzlich eine Schätzung der Position und eine Berechnung der Gravitationsbeschleunigung. Die verschiedenen Bauformen eines INS können in die zwei Kategorien gimbaled und strapdown eingeteilt werden. Für nähere Informationen sei hier auf [Grewal et al., 27] verwiesen. Verglichen mit einem GPS-Sensor bietet die IMU den Vorteil der Unabhängigkeit. D.h. es sind keine weiteren externen Hilfsmittel, wie z.b. Satelliten, nötig, um eine Messung bereitzustellen. Somit liefert die IMU bspw. ebenfalls noch Daten, wenn sich das Trägerobjekt in einem Tunnel oder einer anderen Umgebung befindet, worin keine GPS-Daten empfangen werden können. Nachteilig wirken sich allerdings die Messfehler der IMU aus. Fehler in der Beschleunigung und Winkelgeschwindigkeit integrieren sich im Laufe der Zeit, weshalb eine Positionsbestimmung auf Grundlage der reinen IMU-Daten nicht möglich ist. Für diesen Zweck liegt eine Kombination aus GPS und IMU nahe. Im Folgenden soll kurz auf die Komponenten der IMU, deren Messmodelle sowie Messfehler eingegangen werden. 22

35 2.3 Inertial Measurement Unit (IMU) Beschleunigungssensor Ein Beschleunigungssensor arbeitet nach dem zweiten Newton schen Gesetz, wonach sich eine Kraft aus Masse mal Beschleunigung zusammensetzt (F = m a). Die auf eine Probemasse m wirkende Kraft F kann auf verschiedene Weisen gemessen werden, um die gewünschte Größe der Beschleunigung zu erhalten. Auf die genaue mechanische Umsetzung soll an dieser Stelle nicht weiter eingegangen werden. Nähere Informationen zum Aufbau eines Beschleunigungssensors sind bspw. in [Grewal et al., 27] zu finden. Im Folgenden soll das Messmodell des Sensorarrays bestehend aus drei Beschleunigungssensoren erläutert werden. z a = diag {α} z a,ideal + b a + n a (2.17) Der Parameter α dient der Korrektur des Skalierungsfaktors, welcher einer zeitlichen Änderung unterliegt. Ein so genanntes random walk model α = n α dient zu dessen Modellierung. Ein Gauß sches Rauschen n α mit dem Mittelwert Null wird der Skalierung in jedem Zeitschritt hinzugefügt. Nach dem gleichen Prinzip wird auch der Bias b a modelliert (ḃa = n ba ). Die (ideale) Messung z a,ideal eines Beschleunigungssensors setzt sich aus zwei Komponenten zusammen: z a,ideal = z a,cg + z a,offset (2.18) Hierin entspricht z a,cg der realen Beschleunigung im Schwerpunkt des Körperkoordinatensystems und z a,offset dem Beschleunigungsanteil, welcher aus einer Hebelwirkung resultiert. Als Hebel wirkt der Abstand r des Sensors bezüglich des Schwerpunktes des Körperkoordinatensystems. Zusammengesetzt aus Tangential- und Zentripetalbeschleunigung ergibt sich dieser Beschleunigungsoffset zu: Gyroskop z a,offset = ω r + ω (ω r) (2.19) Ein Gyroskop (auch als Kreisel bezeichnet) wird für die Messung der Lage verwendet. Dabei können der Messung verschiedene physikalische Modelle zugrunde liegen. Bspw. existieren mechanische Ausführungen, die mit einer rotierenden Masse arbeiten, oder Ring Laser Gyroskope, die mit Hilfe von Laufzeitmessungen die Orientierungsänderung registrieren. Ein guter Überblick über die verschiedenen Ausführungen von Gyroskopen ist in [Grewal et al., 27] zu finden. Die hier eingesetzte IMU 3 von Crossbow verwendet für die Erfassung der Lageänderung Micro Electro-Mechanical Sensors (MEMS). Unter Nutzung der Corioliskraft bestimmen diese Sensoren die Winkelgeschwindigkeit. Eine genauere Be- schreibung ist auf der Internetseite von Crossbow zu finden ( [Crossbow, 27]). 23

36 2 Sensor- und Systemmodelle Das Messmodell des Gyroskopes setzt sich aus dem tatsächlichen Wert der Winkelgeschwindigkeit ω, einem Bias b ω und einem Gauß schen Rauschen n ω mit dem Mittelwert Null zusammen. z ω = ω + b ω + n ω (2.2) Der Bias b ω wird nicht als konstant angenommen, sondern als sich ändernde Zufallsvariable ḃω = n bω. Wobei n bω ein Gauß sches Rauschen mit einem Mittelwert von Null ist. Optional kann noch ein Skalierungsfaktor in das Messmodell aufgenommen werden, worauf in dieser Arbeit für bessere Nachvollziehbarkeit verzichtet wird. 2.4 Global Positioning System (GPS) Das Global Positioning System, kurz GPS, ist eines von mehreren Satellitennavigationssystemen. Aufgrund der weltweiten Verfügbarkeit und der kommerziell vertriebenen Empfangseinheiten hat es die größte Bedeutung und Verbreitung. Andere Satellitennavigationssysteme, wie das russische GLONASS oder das chinesische System Beidou erfreuen sich keiner großen Verbreitung und das europäische Projekt Galileo befindet sich erst im Aufbau. Mit einem GPS-Empfänger kann die Position etwa auf eine Genauigkeit von etwa 15 m gemessen werden, wobei die Höhe eine größere Ungenauigkeit als die Längenund Breitenangabe hat. Um die Genauigkeit zu verbessern können Systeme wie das Differential-GPS (DGPS) eingesetzt werden. Dieses System reduziert die Messfehler, wie bspw. die Ionosphärenfehler, anhand einer Referenzstation, dessen Position genau bekannt ist. Abhängig von der Entfernung der Referenzstation zum Empfänger kann die Position damit bis auf einige Dezimeter genau bestimmt werden. Neuerdings existiert mit EGNOS (European Geostationary Navigation Overlay Service) auch ein satellitengestütztes DGPS, was dann auch als SBAS (Satellite Based Augmentation System) bezeichnet wird und eine Genauigkeit von etwa 1 bis 3 Meter liefert. Auf dem Luftschiff Fritz kommt ein solcher satellitengestützter Differential-GPS- Empfänger GPSlim236 von HOLUX zum Einsatz. Wie schon zu Beginn erwähnt, sind GPS-Messungen fehlerbehaftet und teilweise gar nicht verfügbar, weshalb sich diese Arbeit damit beschäftigt, bei teilweisem Ausfall der Messungen, die Position immer noch hinreichend genau bestimmen zu können. Besonders stark können sich in städtischen Einsatzszenarien die Fehler durch Mehrwegausbreitung (multipath) bemerkbar machen. Hierbei wird das Satellitensignal teilweise an Gebäuden oder anderen Objekten wie Autos oder der Erdoberfläche reflektiert und gelangt somit über Umwege eventuell mit dem eigentlichen Signal überlagert zum GPS-Empfänger. Aus diesen Fehlmessungen können je nach verwendeter Empfangstechnologie bis zu 7 m Fehler in der Positionsmessung entstehen. [Wendel, 27] 24

37 2.5 Sensorfusion Weitere Informationen zur GPS-Technologie können bspw. [Wendel, 27] oder [Grewal et al., 27] entnommen werden. 2.5 Sensorfusion Jedes der in diesem Kapitel vorgestellten Sensorsysteme hat Vor- und Nachteile für die Nutzung im Zusammenhang einer Navigationslösung. So bietet bspw. die GPS- Messung eine absolute Position, welche allerdings recht ungenau bzw. nicht immer verfügbar ist. Im Gegensatz dazu bietet die IMU eine zuverlässige Messung über kurze Distanzen, doch driftet diese mit der Zeit immer mehr von der realen Position ab. Eine Kombination der beiden Messmethoden kann genutzt werden, um diese Schwachstelle zu überbrücken. Durch eine Filterung der Messwerte beider Sensoren entstehen Positionswerte mit höherer Genauigkeit. Diese sogenannte Sensorfusion kann mit Hilfe verschiedener Filtermethoden durchgeführt werden. Prominente Beispiele sind der Partikelfilter oder die verschiedenen Formen des Kalman Filters, um welchen es sich in dieser Arbeit drehen soll. Die angesprochene Fusion von GPS und Inertialsystem wurde schon in verschiedenen Arbeiten und Büchern aufgegriffen und ausführlich erläutert, wie z.b. in: [Grewal et al., 27], [Wendel, 27], [Titterton and Weston, 24] oder [Crassidis, 25]. Ebenso gibt es schon kommerzielle Sensorlösungen, welche die Sensorfusion hardwareseitig mit Hilfe eines Kalman Filters vornehmen. Ein Beispiel hierfür ist der MTi-G Sensor der Firma Xsens Technologies. Bei der Kombination von GPS und IMU bleibt jedoch weiterhin das Problem von hohen Messungenauigkeiten bei temporärem Ausfall des GPS-Empfängers. Dies soll in der weiteren Arbeit durch die zusätzliche Integration von Kameradaten behoben werden. Als erste Möglichkeit wird die Integration von 3-D-Messungen mit Hilfe einer Stereokamera besprochen. 25

38 3 Navigation mittels Stereokamera Ein Ansatz um die Navigation eines Roboters zu verbessern ist der Einsatz einer Stereokamera. Dabei wird versucht das räumliche Sehen des Menschen zu imitieren, wie es schon in Abschnitt 2.2 vorgestellt wurde. Zur Integration eines solchen Systems in eine Navigationslösung existieren bereits verschiedene Ansätze. Im Hauptfokus dieser Arbeit liegt die Implementierung eines Mono-Kamera-SLAM Verfahrens, daher werden in den folgenden Abschnitten nur einige Grundlagen erläutert, die für den weiteren Verlauf der Arbeit notwendig sind. Für tiefgründigere Informationen zum Thema der Stereo Odometry sei auf [Sünderhauf, 26] verwiesen. 3.1 Prinzipieller Ablauf Die Daten der Stereokamera können verwendet werden, um die Bewegung des Roboters zu schätzen. Hierzu wird zuerst das linke und rechte Kamerabild ausgewertet, um eine dreidimensionale Punktwolke zu erhalten. Die Berechnung hierzu wird von der vormals erwähnten SVS-Software erledigt. Nach diesem ersten Schritt existieren die momentanen 3-D-Informationen, aber es kann noch keine Aussage über die zurückgelegte Bewegung getroffen werden. Hierzu werden markante Punkte in einem Kamerabild, bspw. aus dem linken Kamerabild, gespeichert. Nach der Bewegung der Kamera wird der gleiche Vorgang wiederholt und die Bewegungsinformation kann aus den wieder gefundenen markanten Punkten und den zugehörigen Rauminformationen berechnet werden. Nähere Informationen zur Rückgewinnung der Bewegungsinformationen aus den 3-D-Punktwolken können bspw. [Sünderhauf, 26] entnommen werden. Für das Finden und Zuordnen zu den vormals gefundenen markanten Punkten im Bild existieren verschiedene Verfahren, auf welche im Folgenden kurz eingegangen werden soll Harris-Ecken-Detektor Das wohl bekannteste und momentan weit verbreitete Verfahren zum Ermitteln von gleichen Bildpunkten beruht auf dem Harris-Ecken-Detektor und einem anschließenden Korrelationsverfahren. Es werden Bildpunkte gefunden, bei denen in jede Richtung eine hohe Änderungsrate in den Grauwerten auftritt. Stellt man sich bspw. ein 26

39 3.1 Prinzipieller Ablauf weißes Bild mit einem schwarzen Quadrat in der Mitte vor, so ist die größte Änderung in allen Richtungen genau an den Ecken des Quadrates. Innerhalb des Quadrates treten keine Änderungen im Grauwert auf und ebenso außerhalb nicht. Nur an den Kanten gibt es noch eine Änderung, welche allerdings nur in eine Richtung stattfindet. Nähere Informationen zum mathematischen Hintergrund können in [Hartley and Zisserman, 24] nachgeschlagen werden. Abbildung 3.1: Beispiel des Harris-Ecken-Detektors auf dem Universitätsgelände Abbildung 3.1 zeigt ein Beispiel für eine Ecken-Detektion, welche mit der Matlab- Implementation des Harris-Ecken-Detektors von [Kovesi, 27] durchgeführt wurde. Aus den detektierten Punkten zweier Bilder können Übereinstimmungen gefunden werden, um die Odometrieinformationen zu berechnen. Hierzu wird eine Korrelationsfunktion auf beide Bilder angewandt. Als Schablone für die Korrelationssuche dient der Bildausschnitt unter einem zu suchenden Harris-Punkte. Die Suche des Bildausschnittes aus der ersten Aufnahme bezüglich des zweiten Bildes kann je nach Anwendung eingeschränkt werden, um Rechenzeit und Fehldetektionen zu minimieren. Beispielsweise kann davon ausgegangen werden, dass sich der zu suchende Punkt bei einer kleinen Bewegung der Kamera auch in der Nähe der vorherigen Position befindet, womit die Fenstergröße der Suche eingeschränkt werden kann. Eine weitere Möglichkeit besteht in der Verwendung einer Positionsschätzung mittels eines Filteralgorithmus. Die Korrelationssuche ist in Abbildung 3.2 verdeutlicht. Zuerst wird ein vordefinierter Rahmen um einen Harris-Punkt gelegt (Abbildung 3.2, links). Dessen Inhalt dient als Schablone für die Suche in der zweiten Aufnahme. Abbildung 3.2 (rechts) zeigt diese mit dem definierten Suchfenster als rotes Quadrat. Die Kreuzkorrelation 27

40 3 Navigation mittels Stereokamera Abbildung 3.2: Erste Aufnahme mit Harris Punkt und Suchschablone (links) und zweite Aufnahme (rechts) mit Suchfenster (rot), Übereinstimmung (blau) und gefundenem Harris Punkt (rot). der Schablone mit dem Suchfenster ergibt das blaue Quadrat, dessen Mittelpunkt als Stern gekennzeichnet ist. Um diesen Mittelpunkt wird der nächstgelegene Harris- Punkt gesucht und entsprechend als gefundene Übereinstimmung gehandhabt, wenn dieser Punkt innerhalb eines definierten Radius zu finden ist. In Bereichen des Bildes mit wiederkehrenden ähnlichen Strukturen, ist die Korrelation stark fehlerbehaftet, wie in Abbildung 3.4 (rechts) deutlich zu erkennen ist. Zur Reduktion dieser Fehldetektionen, oder auch outliers genannt, kann der RANSAC (RANdom SAmple Consensus) Algorithmus von [Fischler and Bolles, 1981] verwendet werden. Abbildung 3.3: Ergebnis der ungefilterten Korrelationssuche (links) und das RANSAC gefilterte Ergebnis (rechts). Zur Veranschaulichung soll Abbildung 3.4 dienen. Die Fehldetektionen im linken Bild wurden durch den RANSAC Algorithmus von [Kovesi, 27] eliminiert. Als Modellgrundlage für den RANSAC Algorithmus wurde die Fundamentalmatrix verwendet. In Abbildung 3.4 ist das Endergebnis des Bildvergleiches dargestellt. Es ist deutlich zu sehen, dass der Harris-Ecken-Detektor nur in gut strukturierten Bereichen funktioniert. Auf homogenen Flächen, wie dem Asphaltboden oder dem Rasen können 28

41 3.1 Prinzipieller Ablauf Abbildung 3.4: Beispiel für die Suche nach Übereinstimmungen in zwei Bildern. Zur Verdeutlichung sind beide Quellbilder übereinander geblendet. keine Übereinstimmungen gefunden werden. Da der Vergleich mit einem Bildpaar von einer Stereokamera erstellt wurde, ist auch sehr deutlich die in Unterabschnitt beschriebene Abnahme der Disparität mit steigender Entfernung erkennbar. Zeigen die Übereinstimmungen im Vordergrund, wie bspw. im Fenster auf der Rechten oberen Seite des Bildes, noch eine deutliche Disparität, so ist diese bei weit entfernten Objekten kaum noch ersichtlich. Wie bereits auf den Beispielaufnahmen zu sehen war, funktioniert das erläuterte Verfahren nur auf gut strukturierten Umgebungen. Ein weiterer Nachteil ist die Empfindlichkeit gegenüber Rotation oder Skalierung. Die Verlässlichkeit des Verfahrens sinkt bei Zunahme von Rotation oder Skalierung, daher tendiert die aktuelle Entwicklung zur Verwendung von weiterentwickelten Algorithmen, wie SIFT [Lowe, 24] oder SURF, welche weniger empfindlich auf diese Bildveränderungen reagieren. Das folgende Kapitel soll einen kurzen Einblick in den SURF-Algorithmus geben. Für einen Vergleich dieser Algorithmen sei auf [Bauer et al., 27] verwiesen SURF-Punkte Der SURF- (Speeded Up Robust Features) Algorithmus [Bay et al., 26a] ist eine weitere Möglichkeit zum Finden von interessanten Bildpunkten. Er ist speziell dafür ausgelegt, Übereinstimmungen zwischen zwei Bildern ausfindig zu machen. Hierzu werden zuerst markante Punkte mit einem neu entwickelten Fast Hessian Detektor gefunden und anschließend in einen 64-dimensionalen Vektor überführt. Dieser Vek- 29

42 3 Navigation mittels Stereokamera tor wird auch als Deskriptor bezeichnet. Er beschreibt jeweils einen Punkt und kann über die Berechnung der Mahalanobis- oder Euklidischen Distanz mit einem anderen Deskriptor verglichen werden. Je nach Anzahl der gefundenen ähnlichen Punkte wird ein Punktpaar als Übereinstimmung gehandhabt. Der entscheidende Vorteil gegenüber dem Harris-Ecken-Detector ist die Invarianz des SURF-Algorithmus gegenüber Skalierungen oder Rotationen. Somit können auch skalierte oder rotierte Übereinstimmungen zwischen zwei Bildern gefunden werden. Dieser Vorteil resultiert allerdings in einer längeren Rechenzeit, verglichen mit dem Finden von Harris-Punkten mit anschließender Korrelation. Abbildung 3.5: Beispiel für die Suche nach Übereinstimmungen in zwei Bildern mit dem SURF-Algorithmus. Zur Verdeutlichung sind beide Quellbilder übereinander geblendet. In Abbildung 3.5 ist ein Beispiel für das Finden von Übereinstimmungen mit dem SURF-Algorithmus verdeutlicht. Die dargestellten Kreise sind die gefundenen SURF- Punkte aus dem ersten (cyan) und dem zweiten Bild (rot), wobei die Größe der Kreise von der Stärke des SURF-Punktes abhängig ist. Um Rechenzeit zu sparen, wurden nur starke Punkte ab einem vordefinierten Schwellwert verwendet Bestimmung der Bewegung Um die Navigation des Luftschiffes mittels einer Stereokamera zu verbessern, muss eine Bewegungsinformation zwischen zwei zeitlich aufeinander folgenden Punktwolken extrahiert werden. Die prinzipielle Vorgehensweise wurde bereits zu Anfang des Kapitels skizziert, ohne jedoch auf die Nachteile dieses Verfahrens einzugehen. 3

43 3.1 Prinzipieller Ablauf Das erste Problem wurde bereits in Abschnitt 2.2 angesprochen. Mit steigender Entfernung der Raumpunkte, nimmt die Disparität zwischen den Pixeln beider Einzelbilder ab, womit die Auflösung sinkt. Eine kleinere Disparität hat weiterhin zur Folge, dass die Stereokameraanordnung anfälliger gegenüber mechanischen Störungen ist. Schon leichte Winkelveränderungen der Kameras untereinander führen zu sehr großen Fehlern in der Entfernungsschätzung der gefundenen Raumpunkte. Um diese Fehler zu vermeiden, muss eine stabile Konstruktion geschaffen werden, die jegliches mechanisches Spiel zwischen den beiden verwendeten Kameras verhindert. Eine solche Konstruktion würde allerdings die Gewichtsanforderungen an die Stereokameraanordnung, für den Einsatz auf dem Luftschiff, verletzen. Ein weiteres Problem ist, dass bereits die Objektive der Kamera ein mechanisches Spiel besitzen und bei Erschütterungen das Bild verändern. Um trotz der Probleme eine genaue Bewegungsschätzung zu erhalten, müsste die Kamera während des Betriebes automatisch kalibriert werden. Zu diesem Zweck wurde eine Matlab-Simulation erstellt, um über die Essential-Matrix, die Orientierung der Kameras zueinander zu bestimmen und zusätzlich die Translation zu berechnen. Bei der Bestimmung der Translation wurde allerdings davon ausgegangen, dass die Basisweite als konstant gegeben ist und sich nicht ändert. Ohne diese Festlegung ist es nicht möglich die Translation mit dem richtigen Skalierungsfaktor zu bestimmen. Die Simulation beruht auf den Punktübereistimmungen der linken und rechten Kameraaufnahme. Aus diesen wird über eine Singular Value Decomposition (SVD), die Projektionsmatrix der zweiten Kamera bezüglich der ersten Kamera aufgestellt (siehe [Horn, 199] und [Hartley and Zisserman, 24]). Hieraus können sich vier mögliche Lösungen ergeben, welche durch ein sog. cheirality constraint [Nistér, 24] zu einer Lösung reduziert werden können. Das cheirality constraint stellt sicher, dass die gefundene Lösung einem Punkt zugeordnet ist, der vor beide Kameras projiziert wird. Bei 6 simulierten Bildpunkten und einem Pixelrauschen von,5 Pixeln in der zweiten Kamera ergaben sich allerdings schon Abweichungen von bis zu 25 cm in der berechneten Translation und bis zu 1 in den Rotationsachsen. Dieser einfache Test beweist, dass sich die angesprochene Methode ohne weiteres nicht für eine Selbstkalibrierung der Kameraanordnung eignet und eine andere Lösung gefunden werden muss, wovon in dieser Arbeit allerdings abgesehen wurde. Das Hauptthema soll sich mit der Implementation eines Mono-Kamera-SLAM Verfahrens beschäftigen. Für eine Mögliche Implementierung einer Selbstkalibrierung mittels eines Kalman Filters sei auf [Dang et al., 26] verwiesen. 31

44 3 Navigation mittels Stereokamera 3.2 Implementierung Fu r das Auslesen und Ansteuern der eingesetzten FireWire-Kameras konnte keine bereits fertige Klasse gefunden werden, daher wurde mit Hilfe der Bibliothek libdc1394 [libdc1394, 27] eine C++ Klasse erstellt. Weiterhin wurde die Bibliothek LTI-Lib [ltilib, 25] verwendet, um bspw. die Anzeige der Bilddaten zu realisieren. Abbildung 3.6: Berechnung der 3-D-Information aus dem linken und rechten Kamerabild. Mit Hilfe der SVS-API ko nnen die Bilddaten rektifiziert werden, bzw. die 3-DInformationen aus der Stereokameraanordnung berechnet werden. Abbildung 3.6 (rechts) zeigt ein Stereobild, welches mit der Guppy Kamera Anordnung mit der Basisweite von 5 cm erstellt wurde, wobei nur die Entfernungskomponente der 3-D-Punkte als Grauwert dargestellt wurde. Auf der linken Seite sind die beiden verwendeten Einzelbilder. Es wird deutlich, dass in nicht strukturierten Bildbereichen keine Rauminformation berechnet werden konnte. Abbildung 3.7: Matlab GUI fu r die erstellten Funktionen. Weiterhin wurde die offizielle Closed-Source-Bibliothek von [Bay et al., 26b] genutzt, um SURF-Punkte in den Bildern zu finden und miteinander zu vergleichen. 32

45 3.3 Performance Ein Beispiel hierfür war bereits in Abbildung 3.5 zu sehen. In Zusammenarbeit mit den C++ Implementationen wurden auch Matlab Funktionen erstellt, um die Korrelation zwischen den Harris-Punkten zu berechnen, oder die Rotation zwischen zwei Kameras einer Stereokameraanordnung zu schätzen. Die Funktionen greifen auf die von der C++ Klasse erzeugten Bilder bzw. berechneten SURF-Punkte zurück und können von der beiliegenden CD über eine Matlab GUI ( sl start odometry.m ) gestartet werden. Die GUI ist in Abbildung 3.7 dargestellt und ist zum Großteil selbsterklärend. Für nähere Informationen können die Inhalte der Matlab Skripte herangezogen werden. 3.3 Performance Abhängig vom Anwendungszweck, werden verschiedene Informationen aus den Kamerabildern benötigt. Für ein Stereo Odomety Verfahren werden bspw. die Rauminformationen und zusätzlich übereinstimmende SURF-Punkte benötigt. Im Gegensatz dazu ist für ein Mono-Kamera-SLAM Verfahren einzig das Finden von übereinstimmenden SURF-Punkten von Bedeutung. Aufgrund der verschiedenen Anforderungen wurden die Laufzeiten verschiedener Programmeinstellungen in Abbildung 3.8 gegenübergestellt. Verwendet wurde dafür ein Rechner mit einer 3 GHz Intel Pentium 4 CPU..55 Zeitvergleich ( 3D / SURF rect, 3D / SURF / SURF rect ) Zeit [s] Bildnummer Abbildung 3.8: Performancevergleich bei verschiedenen Anwendungszwecken. 33

46 3 Navigation mittels Stereokamera 1 Berechnete Punkte ( SURF auf rectifizierten Bildern / SURF auf verzerrten Bildern ) 8 Anzahl Bildnummer 5 Übereinstimmungen ( SURF auf rectifizierten Bildern / SURF auf verzerrten Bildern ) 4 Anzahl Bildnummer Abbildung 3.9: Vergleich zwischen rektifizierten und unveränderten Quellbildern bezüglich der Anzahl der berechneten SURF-Punkte (oben) und der gefundenen Übereinstimmungen zwischen zwei aufeinander folgenden Bildern. Eine konstante Zeit von rund,2 Sekunden wird benötigt, um die Rauminformationen aus den Quellbildern zu berechnen. Ein zusätzlicher Zeitaufwand wird für das Berechnen der SURF-Deskriptoren und dem Finden von Übereinstimmungen benötigt (grüner Graph). Diese Zeit ist von der Anzahl der gefundenen starken SURF-Punkte abhängig (siehe Abbildung 3.9, oben). Entsprechend weniger Rechenzeit wird benötigt, wenn nur die SURF-Informationen berechnet werden müssen. Hier kann zwischen rektifizierten (schwarz) und nicht rektifizierten Quellbildern (blau) unterschieden werden. Die Zeitdifferenz zwischen diesen Kurven bestimmt in etwa die Zeit, welche für das Rektifizieren der Quellbilder benötigt wird. Abhängig davon, ob die Quellbilder rektifiziert wurden, ergeben sich unterschiedliche Ergebnisse beim Finden von SURF-Punkten und Übereinstimmungen (siehe Abbildung 3.9). Die Anzahl der Punkte ist im nicht rektifizierten Bild höher, da keine Bildteile verloren gehen, wie dies bei den entzerrten Bildern der Fall ist. Für die Berechnung in Betracht gezogen wurden allerdings nur SURF-Punkte mit einem Stärkewert größer als 2. Die darunter liegende Grafik zeigt die Anzahl der gefundenen Übereinstimmungen an. Dabei ist zu beachten, dass dies keine Aussage über die Qualität der gefundenen Übereinstimmungen zulässt. Hierzu könnte bspw. ein RANSAC-Verfahren genutzt 34

47 3.3 Performance Abbildung 3.1: Vergleich zwischen gefundenen Übereinstimmungen des SURF-Algorithmus unter Verwendung rektifizierter (links) und unveränderter Quellbilder (rechts) der gleichen Szene. werden. Abbildung 3.1 zeigt ein Beispiel der gefundenen Übereinstimmungen im Vergleich zwischen rektifizierten und nicht rektifizierten Quellbildern der gleichen Szene. 35

48 4 Gauß-Filter In vielen Bereichen der Technik muss ein dynamischer Systemzustand geschätzt werden. Hierzu stehen im Allgemeinen Messungen und ein Vorwissen über das zu schätzende System zur Verfügung. Um beide Informationen zu vereinen, kann ein Filter benutzt werden. Viele solcher Filterprozesse sind auch aus dem täglichen Leben bekannt und werden wie selbstverständlich durch logische Schlussfolgerung gelöst. Ein anschauliches Beispiel hierzu wurde in [Bohg, 25] aufgezeigt. Herr Mustermann ist mit seinem Auto in einem eng bebauten Wohngebiet unterwegs und gelangt an eine gleichrangige Kreuzung. Seine Sicht auf die einmündende Straße ist durch parkende Autos versperrt. Schmale Sichtfenster zwischen den Autos ermöglichen allerdings kurze Einblicke auf den Verkehr der einmündenden Straße. Unter der Annahme konstanter Geschwindigkeit eines Autos kann Herr Mustermann schätzen, ob er die Straße ungefährdet überqueren kann. Dies ist nur ein Beispiel aus dem täglichen Leben, die einen Filterprozess schildert. 4.1 Kalman Filter (KF) Für eine mathematische Lösung eines Filterproblems, kann ein Kalman Filter verwendet werden. Ausgehend von einem linearen dynamischen System schätzt dieser Algorithmus, unter Zuhilfenahme eines Optimierungsverfahrens, den momentanen Zustand des Systems. Der aktuelle Systemzustand des linearen Systems wird rekursiv anhand eines Prozessmodells (Gleichung 4.1) und eines Messmodells (Gleichung 4.2) geschätzt. x k = A k x k 1 + B k u k + G k v k (4.1) y k = C k x k 1 + D k n k (4.2) Das Prozessmodell enthält die Systemmatrix A, die Eingangsmatrix B sowie den Zustandsvektor x, den Eingangsvektor u und das System- oder Prozessrauschen v. Das Systemrauschen ist ein mittelwertfreies, normalverteiltes, weißes Rauschen mit einem Erwartungswert von E [ v k vk T ] [ ] = Rvk. Des Weiteren ist das Rauschen nicht korreliert, d.h. E v i vj T = für i j. Durch das Systemrauschen werden Modellunsicherheiten bezüglich des realen Systems und rauschbehaftete Eingangsgrößen modelliert. 36

49 4.1 Kalman Filter (KF) Das Messmodell enthält die Ausgangsmatrix (oder auch Messmatrix) C k sowie ein Messrauschen n k mit den selben Eigenschaften wie das Systemrauschen, wobei mit E [ n k n T k ] = Rnk diesmal die Kovarianzmatrix der Messgrößen beschrieben wird. Die Rauschgrößen v k und n k müssen für die Anwendung des Kalman Filters unkorreliert sein, d.h. E [ v k n T k ] =. Ausgehend vom beschriebenen Systemmodell lässt sich der Kalman Filter über zwei verschiedene Wege herleiten. Einerseits über die Minimierung einer Kostenfunktion oder andererseits über normalverteilte Zufallsvektoren. Beide Wege sind u. a. in [Wendel, 27] beschrieben und werden an dieser Stelle ausgelassen. Algorithmus 4.1 (Kalman Filter) Initialisierung: ˆx = E [ ] x P x = E [(x ] ˆx ) (x ˆx ) T [ ] R v = E (v v) (v v) T R n = E [(n ] n) (n n) T Vorhersageschritt: Korrekturschritt: ˆx k = A kˆx k 1 + B k u k + G k v (4.3) P k = A kp xk A T k + G kr v G T k (4.4) K k = P x k C T ( k Ck P x k C T k + D kr n D T ) 1 k (4.5) ˆx k = ˆx k + K [ k yk C kˆx k D k n ] (4.6) P xk = (I K k C k ) P x k (4.7) Die kompletten Filtergleichungen können in Algorithmus 4.1 eingesehen werden. Sie beschreiben die beiden Hauptschritte des Kalman Filters. Die Vorhersage und die Korrektur der Vorhersage über die Messwerte. Ebenso können die Gleichungen in einem Blockschaltbild (siehe Abbildung 4.1) verdeutlicht werden. Die aufgezeigte Methode des Kalman Filters eignet sich für lineare System- und Messmodelle. Sollten nichtlineare Modelle vorliegen, so kann für geringe Nichtlinearitäten der erweiterte Kalman Filter (EKF) eingesetzt werden. 37

50 4 Gauß-Filter Abbildung 4.1: Kalman Filter als Blockdiagramm 4.2 Extended Kalman Filter (EKF) In der Praxis werden lineare Systeme eine Ausnahme sein und die Anwendung des einfachen Kalman Filters auf ein nichtlineares System ist nicht möglich, da nur bei einem linearen System die Annahme gilt, dass eine transformierte Gauß sche Variable wieder in einer Gauß-Verteilung resultiert. Für den eindimensionalen Fall lässt sich dies anschaulich darstellen, siehe Abbildung 4.2(a). Die Zufallsvariable in Form einer Gauß-Verteilung aus dem unteren rechten Bildteil wird über eine lineare Funktion projiziert und resultiert in einer neuen Gauß-Verteilung mit einem transformierten Mittelwert und einer neuen Standardabweichung. Für das nichtlineare Systemmodell wird die folgende Form angenommen: x k = f (x k 1, u k ) + v k (4.8) y k = h (x k ) + n k (4.9) Die Systemmatrix und die Eingangsmatrix des linearen Systemmodells wird durch eine Funktion f (.) generalisiert. Sinngemäß ersetzt die Funktion h (.) die Messmatrix des linearen Modells. Legt man nun das nichtlineare System als Modell zugrunde, so resultiert die Transformation der Normalverteilung nicht mehr in einer neuen Gauß-Verteilung, sondern in einer komplett anderen Zufallsverteilung. Analog zur linearen Transformation ist dieser Fall in Abbildung 4.2(b) veranschaulicht. Der Grad der Deformation der ursprünglichen Gauß-Glocke ist offensichtlich vom Grad der Nichtlinearität abhängig. Annäherungsweise kann aus der neuen Zufallsverteilung, die über eine sog. Monte- Carlo Methode berechnet wurde, eine Gauß-Verteilung erstellt werden. Dies führt zu einem neuen Mittelwert und einer neuen Standardabweichung. 38

51 4.2 Extended Kalman Filter (EKF) f (x) y p (y ) x p (x) (a) Lineare Transformation einer Normalverteilung f (x) y p (y ) x p (x) (b) Nichtlineare Transformation einer Normalverteilung resultiert in einer neuen Zufallsverteilung. Wird diese Verteilung einer Normalverteilung angenähert, so ergibt sich die rot dargestellte Glockenkurve. Abbildung 4.2: Lineare und nichtlineare Transformation einer normalverteilten Zufallsvariable. 39

52 4 Gauß-Filter Die Hauptidee hinter dem Extended Kalman Filter liegt in der Annäherung der Nichtlinearitäten. Beide Funktionen f (.) und h (.) werden im Vorhersagepunkt ˆx k 1 bzw. ˆx k linearisiert. Diese Linearisierung wird über eine Taylor-Reihen-Entwicklung erster Ordnung (Abbruch nach dem linearen Glied) berechnet. Veranschaulicht ist dies in Abbildung 4.3. Zu sehen ist hier die ursprüngliche Annahme des Systemzustandes in Form der Normalverteilung im rechten unteren Bildausschnitt. Diese Annahme wird durch die nichtlineare Funktion f (.) transformiert, was in einer neuen Zufallsverteilung p (y) und der schon in Abbildung 4.2(b) gezeigten Gauß-Näherung resultiert. Wird nun eine lineare Taylor Entwicklung im Arbeitspunkt ausgeführt, so entsteht die schwarz eingezeichnete Tangente. An dieser linearisierten Funktion kann nun die Projektion (schwarze Gauß-Kurve) im Sinne des einfachen Kalman Filters ausgeführt werden. Über diesen Weg entsteht eine einfache Näherung des neuen Systemzustandes. Verglichen mit der Näherung über die nicht normalverteilte Projektion ist ein Fehler entstanden. f (x) y p (y ) x p (x) Abbildung 4.3: Die linearisierte Transformation einer Normalverteilung führt zu der schwarz dargestellten Gauß-Glocke. Vergleichend ist die aus der realen Verteilung genäherte Gauß- Glocke in rot visualisiert. Aufgrund der einfachen Anwendung und der Recheneffizienz des Extended Kalman Filters, genießt er eine große Popularität beim Einsatz für Zustandsschätzungen in der Robotik. Momentan beschäftigen sich allerdings immer mehr aktuelle Forschungsarbeiten mit der Klasse der sog. Sigma-Punkt Kalman Filter. 4

53 4.2 Extended Kalman Filter (EKF) Algorithmus 4.2 (Extended Kalman Filter) Initialisierung: ˆx = E [ ] x P x = E [(x ] ˆx ) (x ˆx ) T [ ] R v = E (v v) (v v) T R n = E [(n ] n) (n n) T Jacobi Matrizen berechnen (Prozessmodell): F xk = f (x, v, u k) x G v = f (ˆx k 1, v, u k ) x=ˆxk 1 v (4.1) v= v Vorhersageschritt: Jacobi Matrizen berechnen (Messmodell): Korrekturschritt: H xk = ˆx k = f (ˆx k 1, u k ) (4.11) P k = F x k P xk F T x k + G v R v G T v (4.12) h (x, n) x x=ˆx k D n = h (ˆx k, n) n (4.13) n= n K k = P x k H T ( x Hxk k P x k H T x k + D n R n D T 1 n) (4.14) ˆx k = ˆx k + K [ k yk h (ˆx k, n)] (4.15) P xk = (I K k H xk ) P x k (4.16) 41

54 4 Gauß-Filter 4.3 Sigma-Punkt Kalman Filter Die Taylor-Reihen-Entwicklung, angewandt durch den EKF, ist nur ein möglicher Weg zur angenäherten Transformation einer Gauß-Verteilung. Ein weiterer, oft überlegener, Ansatz ist der Unscented Kalman Filter (UKF). Ausgehend von den Original- Veröffentlichungen von [Julier and Uhlmann, 1997], hat [van der Merwe, 24] eine ganze Klasse von Filtern vorgeschlagen und untersucht, welche alle auf dem Prinzip der Sigma-Punkt Filter basieren. Die Hauptidee dieser Klasse von Filtern liegt darin, den normalverteilten Zufallsvektor, beschrieben durch Mittelwert und Varianz, durch eine Menge deterministisch gewählter Sigma-Punkte zu beschreiben Unscented Transformation Die Grundidee hinter den Sigma-Punkt Filtern liegt in der Unscented Transformation. Die Unscented Transformation ist eine Methode für das Berechnen des Mittelwertes und der Varianz einer Zufallsvariable, die durch eine nichtlineare Projektion transformiert wurde. Das dahinter stehende Prinzip besagt, dass es einfacher ist eine Zufallsverteilung anzunähern, als eine beliebige nichtlineare Funktion. Angenommen, es soll eine L-Dimensionale Zufallsvariable x über die Funktion y = g (x) transformiert werden. Der Mittelwert der Zufallsvariable soll als x und die Kovarianz als P x bezeichnet werden. Um die ersten zwei Momente von y über die Unscented Transformation zu berechnen, wird als erstes eine gewichtete Menge von 2L + 1 Punkten, den sog. Sigma-Points, gewählt. Die Sigma-Punkte werden so bestimmt, dass sie den Mittelwert und die Kovarianz von x exakt beschreiben. Hierzu wird das folgende Auswahlschema verwendet: X = x w = κ L + κ ( ) 1 X i = x + (L + κ) Px w i = i 2 (L + κ) ( ) 1 X i = x (L + κ) Px w i = i 2 (L + κ) i = (4.17) i = 1,..., L (4.18) i = L + 1,..., 2L (4.19) Hierin steht die Variable w i für das Gewicht des i-ten Sigma-Punktes und κ für einen Skalierungsfaktor. Die Summe aller Gewichte ergibt sich zu eins. Weiterhin gibt (L + κ) Px die Quadratwurzel der Matrix der gewichteten Kovarianzmatrix an. Für die Berechnung des i-ten Sigma-Punktes wird jeweils die i-te Spalte der berechneten Kovarianzmatrix verwendet. Empfehlenswert für die Berechnung der Matrixwurzel, ist die Verwendung der effizienten Cholesky Faktorisierung. Diese ist bspw. bereits in Matlab integriert. Nachdem die Sigma-Punkte erstellt wurden, werden diese durch die nichtlineare 42

55 4.3 Sigma-Punkt Kalman Filter Funktion projiziert und ergeben eine neue Menge von Punkten Y: Y i = g (X i ) i =,..., 2L (4.2) Mittelwert, Kovarianz und Kreuz-Kovarianz ergeben sich anschließend wie folgt: ȳ = P y = P xy = 2L i= 2L i= 2L i= w i Y i (4.21) w i (Y i ȳ) (Y i ȳ) T (4.22) w i (X i x) (Y i ȳ) T (4.23) Die Genauigkeit der Annäherung des Mittelwertes und der Kovarianz entspricht einer Taylor-Reihen-Entwicklung der nichtlinearen Funktion mit Abbruch nach dem zweiten Glied, wohingegen beim EKF nur eine Entwicklung erster Ordnung durchgeführt wurde (Ein Nachweis der Genauigkeit der Unscented Transformation ist in [van der Merwe, 24] zu finden). Weiterhin ist anzumerken, dass die Unscented Transformation bei linearen Funktionen, die exakten Werte liefert, wie auch der einfache Kalman Filter. f (x) y p (y ) x p (x) Abbildung 4.4: Unscented Transformation einer Normalverteilung. Wie zuvor wurde die Näherung aus der realen Verteilung rot dargestellt. Die über die Unscented Transformation entstehende genäherte Gauß-Verteilung ist in grün visualisiert. Analog zum EKF kann die Unscented Transformation anhand einer eindimensionalen Zufallsvariable veranschaulicht werden. In Abbildung 4.4 wurde eine Gauß- 43

56 4 Gauß-Filter Verteilung über die schon zuvor verwendete nichtlineare Funktion transformiert. Das Resultat der Unscented Transformation ist die in grün dargestellte Normalverteilung. Im Gegensatz zum EKF ist der Fehler zur real angenäherten Gauß-Verteilung in rot, kleiner ausgefallen. Abhängig von der verwendeten Streuung der Sigma-Punkte kann der Fehler allerdings auch größer oder kleiner werden. Die für die Transformation verwendeten Sigma-Punkte sind durch die grün eingefärbten, gestrichelten Linien verdeutlicht. Die eben beschriebene Methode zum Bestimmen von Sigma-Punkten hängt von der Dimension L der Zufallsvariable ab. Steigt die Dimension, so vergrößert sich der Radius der Kugel, die alle Sigma-Punkte einschließt. Stark lokale Nichtlinearitäten werden dann mit steigender Dimension schlechter geschätzt. Um diesem Effekt entgegenzuwirken, kann die Distanz X i x der Sigma-Punkte zum Mittelwert, über den Parameter κ, skaliert werden. Die Distanz ist dabei proportional zu L + κ, wobei κ < eine Skalierung zum Mittelwert hin und κ > eine Skalierung vom Mittelwert weg bedeuten. Der Spezialfall κ = 3 L hebt den durch die Dimension entstandenen Skalierungseffekt komplett auf, jedoch resultiert dies in einem negativen Gewichtungswert w, wenn κ = 3 L < gewählt wird. Dies kann zu ungültigen, nicht positiv semidefiniten, Kovarianzmatrizen führen. [van der Merwe, 24] Scaled Unscented Transformation Mit der Scaled Unscented Transformation (vorgestellt in [Julier and Uhlmann, 22]) wird ein weiterer Freiheitsgrad α in der Wahl der Sigma-Punkte eingeführt um die Skalierung der Sigma-Punkte zu manipulieren, ohne das dies in einer nicht positiv definiten Kovarianzmatrix resultiert. Die originalen Sigma-Punkte werden nach dem folgenden Schema ersetzt: X i = X + α (X i X ) i =,..., 2L (4.24) Die Erstellung der Sigma-Punkte lässt sich somit zu ( ) ( ) X = x X i = x + (L + λ) Px X i = x (L + λ) Px i i (4.25) i = i = 1,..., L i = L + 1,..., 2L (4.26) umschreiben und die Gewichtungen ergeben sich aus: w (m) = λ L+λ w (c) = λ L+λ + ( 1 α 2 + β ) w (m) i = w (c) i = 1 2(L+λ) (4.27) i = i = i = 1,..., 2L (4.28) Um Berechnungen einzusparen, wurde ein Parameter λ eingeführt, der wie folgt definiert ist: λ = α 2 (L + κ) L (4.29) 44

57 4.3 Sigma-Punkt Kalman Filter Mit Hilfe des neu hinzugekommenen Parameters β lässt sich Vorwissen über die Zufallsverteilung vor der Transformation in die Gewichtungen einbringen. Genauere Informationen können in [Julier and Uhlmann, 22] nachgelesen werden. Im weiteren Verlauf der Arbeit wird angenommen, dass es sich bei den Zufallsverteilungen um Gauß-Verteilungen handelt und somit wird β = 2 gesetzt. Wie schon zuvor angesprochen, handelt es sich bei dem Parameter α um einen Skalierungsfaktor. Je größer der Wert für α gewählt wird, desto größer wird auch der Umfang der Sigma-Punkte, womit lokale Nichtlinearitäten schlechter geschätzt werden könnten. Im Allgemeinen liegt der Zahlenwert für α zwischen und 1 ( α 1). Mit der Wahl der Variable κ kann garantiert werden, dass die neu berechnete Kovarianzmatrix positiv semidefinit ist. Die Berechnung des neuen Mittelwertes und der neuen Kovarianzmatrix verhält sich analog zur Unscented Transformation. Die erstellten Sigma-Punkte werden über eine nichtlineare Funktion transformiert: Y i = g (X i ) i =,..., 2L (4.3) und anschließend auf einen Mittelwert und eine Kovarianzmatrix bzw. eine Kreuzkovarianzmatrix zurückgerechnet. ȳ = P y = P xy = 2L i= 2L i= 2L i= w (m) i Y i (4.31) w (c) i (Y i ȳ) (Y i ȳ) T (4.32) w (c) i (X i x) (Y i ȳ) T (4.33) Ein allgemein gültiges Auswahlverfahren für die Wahl er Parameter κ, α und β existiert bisher noch nicht. Die Parameter müssen jeweils problemspezifisch angepasst werden. [van der Merwe, 24] Unscented Kalman Filter (UKF) Der Unscented Kalman Filter nutzt die beschriebene Scaled Unscented Transformation um einen neuen Zustandsvektor ˆx und eine neue Kovarianzmatrix P x rekursiv zu schätzen. Eine Zusammenfassung des UKF Ablaufs ist in Algorithmus 4.3 dargestellt. Algorithmus 4.3 (Unscented Kalman Filter) 45

58 4 Gauß-Filter Initialisierung: ˆx = E [ ] x P x = E [(x ] ˆx ) (x ˆx ) T ˆx a = E [ x a] P a = E [(x ] a ˆxa ) (xa ˆxa )T ˆx a = E [ x ] P a = diag {[ ]} P x R v R n Parameter: x a = [ x T v T n T ] T X a = [(X ] x ) T (X v ) T (X n ) T T Sigma-Punkte und Wichtungen erstellen: X a k 1 = [ˆx a k 1 [( W (m) = λ W (c) = ˆx a k 1 + γ ( ) P a k 1 i ) ) ( λ (L+λ) 2(L+λ) i [( λ (L+λ) + 1 α2 + β ( λ 2(L+λ) ) ( λ 2(L+λ) ˆx a k 1 γ ( ) P a ] k 1 i ) ] i ) i ( ) ] λ 2(L+λ) i (4.34) (4.35) (4.36) mit γ = L + λ, λ = α 2 (L + κ) L und i = 1...L, wobei L der Dimension des Statusvektors x a entspricht. Die Parameter {κ R, κ }, {α R, α 1} und {β R, β } bilden eine Einstellungsmöglichkeit für die Erstellung der Sigma-Punkte. Für wachsende Werte von κ kann garantiert werden, dass eine positiv semidefinite Kovarianzmatrix erzeugt wird. (Standard: κ = ) Der Parameter α gibt die Streuung der Sigma-Punkte an und mit β können Vorkenntnisse über höhere Momente der Verteilung eingearbeitet werden. (Standard für Gauß-Verteilungen: β = 2) Vorhersageschritt: X x k k 1 = f ( X x k 1, X v k 1, u ) k 2L+1 ˆx k = P x k = i=1 2L+1 i=1 (4.37) W (m) i X x i,k k 1 (4.38) ( ) ( ) W (c) T i X x i,k k 1 ˆx k X x i,k k 1 ˆx k (4.39) 46

59 4.3 Sigma-Punkt Kalman Filter Korrekturschritt: Y k k 1 = h 2L+1 ŷ k = Pỹk = P xk y k = ( ) X x k k 1, X n k 1 i=1 2L+1 i=1 2L+1 i=1 (4.4) W (m) i Y i,k k 1 (4.41) W (c) ( i Yi,k k 1 ŷ ) ( ) T k Yi,k k 1 ŷ k (4.42) ( ) W (c) (Yi,k k 1 i X x i,k k 1 ) T ˆx k ŷ k (4.43) K k = P xk y k P 1 ỹ k (4.44) ˆx k = ˆx k + K ( k yk ŷ ) k (4.45) P xk = P x k K k Pỹk K T k (4.46) Im Gegensatz zum Kalman Filter wird der Zustandsvektor beim UKF in jedem Filterdurchlauf erneut mit einem Mittelwert des erwarteten Rauschens erweitert. Der erweiterte Zustandsvektor wird mit x a bezeichnet, wobei der obere Index a für augmeted steht. x x k x k x a k = x v k = v k x n k n k ˆx k ˆx a k = E [xa k ] = v k (4.47) n k Damit lassen sich Vorkenntnisse in Form von mittelwertbehafteten Rauschvariablen in den Schätzvorgang einbringen. Für ein ideales mittelwertfreies, normalverteiltes, weißes Rauschen, ergibt sich jeweils ein Erwartungswert von für das Prozessrauschen v sowie das Messrauschen n. Durch den erweiterten Zustandsvektor erhöht sich auch die Dimension L um die Dimension der Rauschvariablen. Die Gesamtdimension ergibt sich aus: L = L x + L v + L n, wobei L v die Dimension des Prozessrauschens und L n die Dimension des Messrauschens darstellt. Analog der Erweiterung des Zustandsvektors wird auch die Kovarianzmatrix um die Varianzen des Prozessrauschens und des Messrauschens erweitert: P x P a = R v (4.48) R n 47

60 4 Gauß-Filter Square-Root Unscented Kalman Filter (SRUKF) Innerhalb des UKF Algorithmus wird in jedem Schritt die Matrixwurzel der Kovarianzmatrix berechnet. Um diese überflüssige Rechenzeit einzusparen wurde der SRUKF entwickelt. Anstatt die komplette Cholesky Faktorisierung in jedem Schritt durchzuführen, arbeitet der Vorhersage- und Korrekturschritt des Filters direkt mit der Cholesky Faktorisierten Matrix. Dies wird mit drei verschiedenen Verfahren realisiert: QR decomposition, Cholesky factor updating und efficient pivot-based least squares. Damit reduziert sich nicht nur die Rechenzeit, sondern es wird durch diese Methoden zusätzlich sichergestellt, dass die Kovarianzmatrix positiv semidefinit bleibt. Eine ausführlichere Beschreibung der Verwendung der angesprochenen Methoden kann in [van der Merwe, 24] nachgelesen werden. Algorithmus 4.4 (Square-Root Unscented Kalman Filter) Initialisierung: ˆx = E [ x ] { S x = chol E [(x ]} ˆx ) (x ˆx ) T ]} [(x a ˆxa ) (xa ˆxa )T ˆx a = E [ x a] { S a = chol E ˆx a = E [ x ] S a = diag {[ ]} S x S v S n Parameter: x a = [ x T v T n T ] T X a = [(X ] x ) T (X v ) T (X n ) T T Sigma-Punkte und Wichtungen erstellen: X a k 1 = [ˆx a k 1 ˆx a k 1 + γ (S a ) k 1 [( ) ( ) W (m) = λ λ (L+λ) 2(L+λ) W (c) = [( λ (L+λ) + 1 α2 + β i i ( λ 2(L+λ) ) ( λ 2(L+λ) ˆx a k 1 γ (S a ) k 1 i ) ] i ) i ] ( ) ] λ 2(L+λ) i (4.49) (4.5) (4.51) mit γ = L + λ, λ = α 2 (L + κ) L und i = 1...L, wobei L der Dimension des Statusvektors x a entspricht. Die Parameter {κ R, κ }, {α R, α 1} und {β R, β } bilden eine Einstellungsmöglichkeit für die Erstellung der Sigma-Punkte. Für wachsende Werte von κ kann garantiert werden, dass eine positiv semidefinite Kovarianzmatrix erzeugt wird. (Standard: κ = ) Der Parameter α gibt die Streuung der Sigma-Punkte an und mit β können Vorkenntnisse über höhere Momente der Verteilung eingearbeitet werden. (Standard für Gauß-Verteilungen: β = 2) 48

61 4.4 Vergleich Vorhersageschritt: X x k k 1 = f ( X x k 1, X v k 1, u ) k 2L+1 ˆx k = i=1 (4.52) W (m) i X x i,k k 1 (4.53) ]} {[ ( S x k = qr W (c) 2 ) X x 2:(2L+1),k k 1 ˆx k (4.54) { } S x k = cholupdate S x k, X x 1,k k 1 ˆx k, W (c) 1 (4.55) Korrekturschritt: Y k k 1 = h 2L+1 ŷ k = ( ) X x k k 1, X n k 1 i=1 (4.56) W (m) i Y i,k k 1 (4.57) {[ Sỹk = qr W (c) 2 (Y 2:(2L+1),k k 1 ŷ ) ]} k (4.58) { } Sỹk = cholupdate Sỹk, Y 1,k k 1 ŷ k, W (c) 1 (4.59) P xk y k = 2L+1 i=1 ( ) W (c) (Yi,k k 1 i X x i,k k 1 ) T ˆx k ŷ k (4.6) K k = ( P xk y k /S T ) ỹ /Sỹk k (4.61) ˆx k = ˆx k + K ( k yk ŷ ) k (4.62) U = K k Sỹk (4.63) S xk = cholupdate { S x k, U, 1 } (4.64) Bei der Verwendung der Matlab Funktionen für die Cholesky Faktorisierung oder die QR Zerlegung sollte besonderer Wert auf die Definition des Rückgabewertes gelegt werden, denn für die Erstellung der Sigma-Punkte im SRUKF wird eine untere Dreiecksmatrix der Unsicherheiten benötigt. 4.4 Vergleich Um die Performance der vorgestellten Filter zu demonstrieren wurde ein leicht zu überschauendes zweidimensionales Beispiel aus [Langelaan, 26] übernommen. Ein 49

62 4 Gauß-Filter einfaches Statusmodell x k+1 x k + u t cos ψ y k+1 = y k + u t sin ψ (4.65) ψ k+1 ψ k + ψ t wird verwendet, um die Wirksamkeit der Filter bei stark nichtlinearen Winkelfunktionen darzustellen. Ausgehend von einer unsicheren Startposition mit einer Standardabweichung von 1cm in beiden Koordinaten und einer Winkelunsicherheit von 1 wurde ein als sicher angenommener Eingangsvektor von u t = 1m und ψ t = definiert Abbildung 4.5: Vergleich der 3σ Unsicherheiten und des Schätzwertes eines einfachen Updateschrittes mit einem Monte-Carlo-Verfahren (gelbe Punkte und grüne Ellipse), dem Extended Kalman Filter (rot gestrichelte Ellipse) und dem Unscented Kalman Filter (blau gestrichelte Ellipse). Der schwarze Kreis zeigt die Unsicherheit in der Startposition. In Abbildung 4.5 sind die Ergebnisse des Vergleiches dargestellt. Das Monte-Carlo- Verfahren besteht in diesem Fall aus simulierten 1 Punkten mit der oben genannten Standardabweichung. Aus dieser Punktmenge ergibt sich die grüne 3σ Fehlerellipse. Sehr gut wurde diese Fehlerellipse auch mit dem Unscented Kalman Filter angenähert. Nur eine leichte Differenz ist in den Fehlerellipsen sichtbar. Durch eine linearisierte Schätzung, wie sie durch den Extended Kalman Filter mit der roten Fehlerellipse dargestellt wird, zeigt sich ein deutlich schlechteres Ergebnis. Neben dem entstandenen Bias in der vorausgesagten Position ergibt sich zusätzlich eine signifikante Überschätzung der Unsicherheit im nächsten Zeitschritt. 5

63 5 Mono-Kamera-SLAM Im ersten Teil dieses Kapitels soll eine kurze Einführung in das SLAM Problem gegeben werden. Anschließend wird die Bedeutung von Landmarken und deren Darstellungsweise erläutert. 5.1 Das SLAM Problem Eines der grundlegenden Probleme in der mobilen Robotik wird als das SLAM Problem bezeichnet. SLAM stammt aus dem englischen und lautet ausgeschrieben simultaneous localisation and mapping. Im übertragenen Sinne ist SLAM kein Problem, welches erst mit dem Aufkommen der Mobilen Robotik ins Leben gerufen wurde. Schon zu beginn der Menschheit bestand die Aufgabe unbekannte Umgebungen zu erkunden und sich in diesen zu orientieren. Ein Mensch merkt sich die aktuelle Umwelt und gewinnt so einen Überblick über die Gesamtsituation. Ähnlich soll dies auch in der Robotik funktionieren. Ohne Vorwissen erkundet ein Roboter seine Umgebung und muss anhand von Sensordaten eine Karte erstellen. Dies kann auch als mapping bezeichnet werden. Eine genaue Karte kann allerdings von einem Roboter nur erstellt werden, wenn er sich über seine eigene Position in dieser Umgebung bewusst ist. Das Erkennen der eigenen Position wird als self localisation bezeichnet. In der Literatur wird die SLAM Aufgabe deshalb auch gern als Henne und Ei Problem referenziert, denn für den Aufbau einer Karte ist eine möglichst genaue Positionsangabe des Roboters in der Karte nötig und für diese Positionsschätzung ist wiederum eine genaue Karte von Bedeutung. Zwei Anforderungen, die nicht unabhängig voneinander gelöst werden können. In dieser Arbeit wird versucht das SLAM Problem mittels eines online Verfahrens zu lösen, d.h. die Karte und Position wird nur anhand von der vorangegangenen Position, Karte und den aktuellen Messwerten geschätzt. Im Gegensatz zu dieser Herangehensweise beschreibt bspw. [Thrun et al., 25] ein offline Verfahren, welches als full SLAM problem bezeichnet wird. Eine Grundlegende Untergliederung des SLAM Problems hat [Solà, 27] mit den folgenden Punkten beschrieben (Beispielhaft in Abbildung 5.1 dargestellt): Die Schätzung von Position und Landmarken im Sinne von Schätzung und Korrektur der Schätzung mit Hilfe von Sensordaten. 51

64 5 Mono-Kamera-SLAM Die Wahrnehmung zum Finden, Wiedererkennen und Messen von Landmarken. Die Entscheidung mit wichtigen Fragen wie bspw.: Wie viele Landmarken beobachte ich? oder Wann und wo initialisiere ich eine neue Landmarke?. Abbildung 5.1: Ein kompletter Filterdurchlauf beispielhaft dargestellt. 1) Initialisierung neuer Landmarken. 2) Vorhersage der neuen Position des Roboters mit erhöhter Unsicherheit. 3) Messung der bereits kartografisierten Landmarken aus der unsichereren Roboterposition heraus. 4) Korrektur der Roboterposition, sowie der Landmarkenposition. Damit verbunden ist ebenfalls eine Reduktion der Unsicherheiten (hier als rote und blaue Ellipsen dargestellt). Nach [Solà, 27] Als die am weitesten verbreitete Methode zur Lösung des Schätzproblems wird der Extended Kalman Filter angesehen, doch es existieren ebenfalls neuere Ansätze wie bspw. der FastSLAM 2. [Montemerlo et al., 23] oder der immer populärer werdende Unscented Kalman Filter. Welche Methode letztendlich die beste Performance zeigt, hängt sicherlich sehr stark vom vorliegenden Problem und den eingesetzten Sensoren ab. In dieser Arbeit wurde der Einsatz des Inverse Depth Verfahrens in Verbindung mit einem Kalman Filter, vorzugsweise dem UKF, vorgegeben. Andere Methoden und Vorgehensweisen können in der Literatur und aktuellen Veröffentlichungen gefunden werden. 5.2 Warum Mono-Kamera-SLAM? Wie bereits zu Anfang erwähnt, gibt es bei der Verwendung einer Stereokamera für die Bestimmung der Rauminformationen den Nachteil der eingeschränkten Reichweite der Messung. Zudem werden Stereokamerakonfigurationen mit einer großen 52

65 5.3 Landmarken Basisweite schwerer und störanfälliger gegenüber mechanischen Einwirkungen, wie Stößen oder Schwingungen. Wird nur eine Kamera als Sensor verwendet, entfallen die genannten Nachteile. Die Verwendung nur einer Kamera resultiert in einer robusten, weniger fehleranfälligen und kostengünstigeren Lösung. Dennoch kann mit einer Kamera keine Rauminformation wahrgenommen werden. Hierfür muss erst ein intelligenter Algorithmus entwickelt werden. Dieser Algorithmus wird dann als Mono-Kamera-SLAM oder Mono-Vision-SLAM bezeichnet. Mit Hilfe von detektierten Landmarken aus dem Kamerabild und der relativen Bewegung können über einen solchen Algorithmus die Entfernungen und Positionen der Landmarken geschätzt werden. Eine Erweiterung des Mono-Kamera-SLAM Verfahrens wäre mit einer weiteren Kamera möglich, um auch die Vorteile eines Stereokamerasystems in den intelligenten Algorithmus einzubeziehen. Diese Kombination beschreibt [Solà, 27] mit dem Bi-Kamera-SLAM Verfahren. Dies soll allerdings nur zu Vollständigkeit erwähnt und nicht Gegenstand dieser Arbeit sein. 5.3 Landmarken Mit dem Begriff Landmarken werden Punkte in der realen Welt beschrieben, die durch ein spezielles Merkmal leicht aus verschiedenen Perspektiven wiedererkannt werden können. Entsprechend wird der Begriff auch im Bereich der Bildverarbeitung verwendet. Eine Landmarke beschreibt in diesem Fall einen Bildpunkt, bzw. Bildbereich, der über eine Bildfolge hinweg erkannt bzw. verfolgt werden kann. Dabei kann dieser Punkt durch einen Merkmalsvektor, wie im Kapitel 3.1.2, definiert sein, oder durch Korrelation, wie im Kapitel beschrieben, wiedererkannt werden. Beide Verfahren sind nur ein Beispiel für eine Vielzahl von Ansätzen zum Wiedererkennen von Merkmalen in verschiedenen Aufnahmen. Die Fähigkeit, eine Landmarke zu finden und über mehrere Kamerabilder hinweg zu verfolgen, ermöglicht die räumliche Wahrnehmung dieser Landmarke. [Solà, 27] verdeutlicht dies anhand einer Analogie zum Menschen, der ebenfalls aus einer Folge von zweidimensionalen Bildern, die Räumlichkeit und Entfernungen im Bild abschätzen kann. Beispielhaft erwähnt er dabei Autorennspiele. Wie diese Entfernungsschätzung abläuft, soll qualitativ in Abbildung 5.2 aufgezeigt werden. Gezeigt ist eine Kamerabewegung in drei verschiedenen Zeitschritten, wobei im ersten Zeitschritt die Initialisierung einer Landmarke (mit einem Plus gekennzeichnet) erfolgt. In jedem Schritt wird eine Hypothese über die Lage der Landmarke aufgestellt, welche als orange Fläche dargestellt ist. Die Größe dieser Fläche ist von der Unsicherheit in der Kameraposition und Orientierung und der angenommenen Standardabweichung eines gefundenen Punktes im Kamerabild abhängig. In einem zweiten Schritt hat sich die Kamera sehr nahe entlang der vorherigen 53

66 5 Mono-Kamera-SLAM Abbildung 5.2: Qualitative Verfolgung einer Landmarke über drei Zeitschritte hinweg. Sichtlinie von Kamera zur Landmarke hin bewegt, d.h. zwischen den beiden Sichtlinien besteht nur eine sehr kleine Winkeländerung, was zu wenig neuen Informationen über die Landmarkenposition führt. Der Bereich, in dem die Landmarke angenommen wird, verringert sich folglich nur sehr wenig. Eine exaktere Positionsannahme entsteht erst durch eine größere Winkeländerung, wie im dritten Teilbild dargestellt ist Landmarkenrepräsentation Im vorhergehenden Kapitel, wurde das Prinzip der Landmarkenerkennung und Schätzung erklärt. Hieran anschließend muss eine mögliche Beschreibung einer Landmarke gefunden werden, die den Anforderungen eines Kalman Filters genügt, d.h. die Positionsannahme muss in Form einer Gauß-Verteilung beschrieben werden. Um dieses Problem zu verdeutlichen, sollen im Folgenden verschiedene Darstellungsformen vorgestellt werden. Die einfachste vorstellbare Darstellungsmöglichkeit bildet ein Punkt, beschrieben mit drei Koordinaten im Weltsystem. In Verbindung mit einer Kovarianzmatrix wird damit ein Unsicherheitsellipsoid im Raum beschrieben. Nachteilig wirkt sich diese Beschreibung allerdings für große und teilweise gleichverteilte Unsicherheitsbereiche, wie in Abbildung 5.2 dargestellt ist, aus. Eine andere Darstellungsform, die die Eigenschaften der Kamera als Winkelsensor besser in Betracht zieht, ist die Darstellung anhand eines Bezugspunktes im Raum, einer Richtung und einer Entfernung der Landmarke zu diesem Bezugspunkt. Bedauerlicherweise ist die mögliche Entfernung zur Landmarke dann eine Gleichverteilung. Erst mit steigender Sicherheit über die Entfernung der Landmarke zum Initialisierungspunkt kann eine für den Kalman Filter nötige Gauß-Verteilung modelliert werden. Dieses Problem kann prinzipiell über zwei verschiedene Wege gelöst werden. Eine Möglichkeit ist die verzögerte Initialisierung (delayed initialization) der ge- 54

67 5.3 Landmarken fundenen Landmarke, um die Entfernung außerhalb des Kalman Filters so lange zu schätzen, bis die Position der Landmarke mit Hilfe einer Gauß-Verteilung hinreichend genau beschrieben werden kann. Ist die benötigte Positionsgenauigkeit der Landmarke erreicht, kann sie in den Kalman Filter integriert werden und somit zur Positionsbestimmung des Roboters genutzt werden. Leider hat die verzögerte Initialisierung einen entscheidenden Nachteil. Bewegt sich der Roboter geradeaus, dann können die Landmarken in der Mitte des Kamerabildes nie hinreichend genau geschätzt werden, um sie in den Kalman Filter zu übernehmen. Der Roboter ist quasi blind bezüglich aller Landmarken oder Gegenstände, die sich frontal vor ihm befinden. Abbildung 5.3: Qualitative Verfolgung einer Landmarke über drei Zeitschritte hinweg. Die Ellipsen stellen die Hypothesen des FIS Algorithmus dar. Der Grauwert deutet die Gewichtung der Annahmen an. Die zweite Möglichkeit ist die Verwendung des Gaussian Sum Filters (GSM), um eine Landmarke unverzögert (undelayed initialization) in den Kalman Filter zu integrieren. Aufgrund der hohen Rechenzeit hat [Solà, 27] eine Methode, basierend auf einer Näherung des GSM, namens Federated Information Sharing (FIS), vorgeschlagen. Prinzipiell wird dabei die gleichverteilte Entfernung in mehrere Bereiche aufgeteilt. Jeder Bereich wird durch eine Normalverteilung und einem Gewicht beschrieben. Sobald eine Bewegung der Kamera einsetzt, werden die Bereiche neu bewertet und werden nach und nach als ungültig eingestuft, bis nur noch ein Bereich übrig bleibt. (siehe Abbildung 5.3) Der FIS Algorithmus ist insgesamt sehr komplex und daher nur mit einem recht hohen Aufwand zu implementieren. Durch die Einführung von sehr vielen Parametern, steigt außerdem die Wahrscheinlichkeit des Auftretens von Inkonsistenzen im Kalman Filter. Eine weitere Lösung der unverzögerten Initialisierung von Landmarken hat [Montiel et al., 26] mit dem Verfahren zur Inverse Depth Parametrization 55

68 5 Mono-Kamera-SLAM aufgezeigt Inverse Depth Parametrization Bei der Inverse Depth Parametrization wird prinzipiell die zuletzt erläuterte Darstellungsmöglichkeit verwendet, doch wird anstatt der Entfernung zu einer Landmarke die inverse Entfernung in den Statusvektor eingebunden. Dies führt dazu, dass Entfernungen in einem Bereich von einigen Metern, bis Unendlich, mit einer Normalverteilung angenähert werden können. Abbildung 5.4: Inverse Depth Parametrization. Die vollständige Beschreibung einer Landmarke soll anhand von Abbildung 5.4 erklärt werden. Es sind zwei verschiedene Kamerapositionen dargestellt. Die Erste grau dargestellte, ist die Initialposition r W init. Sie beschreibt die geschätzte Weltposition des Beobachters im Raum, in welcher die Landmarke das erste Mal erkannt wurde. Ausgehend von dieser Position wird ein Einheitsvektor m (θ, φ) in Richtung der Landmarke mit den Winkelparametern θ (Azimuth) und φ (Elevation) beschrieben. Der letzte nötige Parameter für die vollständige Beschreibung der Landmarkenposition, ist die Entfernung d, welche in diesem Fall invers als ρ = 1 d dargestellt wird. Der komplette Status einer Landmarke i kann also wie folgt beschrieben werden: x mi = ( r W init i θ i φ i ρ i ) T = ( xi y i z i θ i φ i ρ i ) T (5.1) 56

69 5.3 Landmarken Die zweite, in grün, dargestellte Kameraposition r W beschreibt die Landmarke mit einem Vektor h C im Kamerakoordinatensystem, zu einem späteren Zeitpunkt, nachdem die Initialisierung bereits erfolgt ist. Die Landmarkenposition lässt sich dann im Kamerakoordinatensystem und im Weltkoordinatensystem wie folgt angeben: h C i = r W init i + 1 m (θ i, φ i ) (5.2) ρ ( i h W i = R CW r W init i + 1 ) m (θ i, φ i ) r W (5.3) ρ i h W i = R CW ( ρ i ( r W initi r W ) + m (θ i, φ i ) ) (5.4) Wie schon zu Anfang erwähnt, ist der große Vorteil dieser Darstellungsweise die gute Beschreibung mittels einer Normalverteilung. Dies soll anhand von Abbildung 5.5 erläutert werden. Als Beispiel dient eine Stereokamerakonfiguration mit einer Basisweite von b =, 5m und einer beobachteten Landmarke in einer Entfernung von 1m. Beide Kameras sind im Mittel parallel zur Y Achse ausgerichtet, besitzen jedoch jeweils eine Unsicherheit in der Rotation um die Z Achse von σ =, 5. Dies hat zur Folge, dass die Rückprojektion der in beiden Bildern gefundenen Landmarke mit einer Unsicherheit behaftet ist, wie in den beiden rechten Teilbildern der Abbildung 5.5 zu sehen ist Y Y [m] ] ρ [ 1 m Z Y c c X c Z Y c c X c 1 1 X [m] X [m] θ [d eg] Abbildung 5.5: Die Konfiguration einer Stereokamera (links) mit einer Unsicherheit in der Orientierung von σ =, 5, führt zu unsicheren Landmarkenschätzungen. Diese sind in kartesischer Form (Mitte) und in als inverse Tiefe (rechts) dargestellt. Eine hinzugefügte 2σ Fehlerellipse verdeutlicht die Qualität der Gauß-Annäherung. In der mittleren Abbildung sind die angenommenen, zurückprojizierten Punkte 57

70 5 Mono-Kamera-SLAM in einem kartesischen Koordinatensystem abgetragen. Die Annäherung der entstandenen Zufallsverteilung an eine Gauß-Verteilung stellt sich als ungenügend heraus. Beschreibt man die gleiche Situation allerdings mit dem vorgeschlagenen Verfahren der inversen Tiefe, so führt dies zur rechten Verteilung. Die hieraus angenäherte Kovarianzmatrix ist wieder in einer rot dargestellten Fehlerellipse verdeutlicht und zeigt die deutlich ausgebildete Gauß-Verteilung in den Zufallswerten. 58

71 6 Simulation In diesem Kapitel soll eine Übersicht über die verwendete Simulationsumgebung gegeben werden. 6.1 Vorbetrachtungen Eine Simulationsumgebung dient der Validierung von Sensor- und Systemmodellen, sowie der Prüfung von implementierten Algorithmen. Natürlich kann eine solche Überprüfung auch direkt mit der Hardware vorgenommen werden, doch dies ist meist umständlich und führt zu unzuverlässigen Ergebnissen, wenn die Korrektheit eines mathematischen Ablaufes geprüft werden soll. Für die Überprüfung der später erläuterten Positionsschätzung ist bspw. eine genaue Positionsmessung über ein anderes Messverfahren als das im Algorithmus verwendete nötig, um die berechneten Positionsschätzungen zu validieren. In einer Simulation hingegen ist bereits vorher bekannt, zu welchen Ergebnissen der zu testende Algorithmus führen muss. Ein weiterer Vorteil liegt auf der Hand. Bei komplizierten Testaufbauten muss ein hoher Zeitanteil allein für die Durchführung eines Testlaufes aufgebracht werden, bevor eine Auswertung dann die Richtigkeit des eingesetzten Algorithmus ans Tageslicht bringt. Je nach Aufwand eines realen Testlaufes kann der Einsatz einer Simulation von hohem Nutzen sein. Die Entwicklung der Simulation fand komplett in Matlab statt. Die Skriptsprache hat gegenüber einer Programmiersprache wie bspw. C++ den entscheidenden Vorteil, dass eine Vielzahl der benötigten mathematischen Funktionen bereits vorhanden ist. Somit können die benötigten Berechnungen und Ausgaben schnell und einfach implementiert werden. Dies wird u. a. auch als Rapid Prototyping bezeichnet. In den nachfolgenden Kapiteln soll kurz erläutert werden, welche System- und Sensormodelle implementiert wurden. 6.2 Bewegung und Vehikelmodell Bevor die eigentlich wichtigen Sensormodelle in die Simulation eingebracht werden können, muss ein Bewegungsmodell des eingesetzten Roboters aufgestellt werden. Dies variiert je nach eingesetztem Robotertyp. In diesem Fall soll ein vereinfachtes Modell eines Luftschiffes verwendet werden. Vereinfacht kann angenommen werden, dass das Luftschiff eine Punktmasse ist, die sich in alle 6 Freiheitsgrade bewegen kann. 59

72 6 Simulation Somit bietet sich die Verwendung des 6DoF (Euler Angles) Blockes aus der Matlab Aerospace Toolbox an. Dieses System kann als Regelstrecke mit den Ausgängen Orientierung, Position, Geschwindigkeit, Beschleunigung und Winkelgeschwindigkeit betrachtet werden. Als Eingangswerte in die Regelstrecke werden die Kräfte und Drehmomente in jeweils drei Raumrichtungen verwendet. Das Vorgeben von Kräften und Momenten ist für eine Simulation eher unintuitiv, daher wird eine Lageregelung, bestehend aus zwei kaskadisierten P T 1 Gliedern, vorgeschaltet. Somit können bestimmte Raumpunkte mit zeitlichen Vorgaben angeflogen werden. Weiterhin wurden nichtlineare Begrenzungsglieder eingebaut, welche verhindern, dass eine maximale Beschleunigung oder Geschwindigkeit überschritten wird. 6.3 Umgebungs- und Sensormodelle Neben der Bewegung besteht das wichtigste Simulationskriterium in der Implementation exakter Sensormodelle, die eine nachgebildete Umgebung wahrnehmen können. Als Umgebung wird es ausreichen, einige Punkte (Landmarken) definiert oder zufällig im Raum zu platzieren. Wesentlich komplizierter wird es bei der Nachbildung der verwendeten Sensoren. Die verwendeten Sensormodelle sollen nachfolgend erläutert werden GPS Die Nachbildung eines GPS-Sensors ist keinesfalls trivial. Die Genauigkeit hängt u. a. von den sichtbaren Satelliten, der Umgebung und den momentanen Ionosphärenfehlern ab. Bei einer Suche im Internet ließen sich keine frei verwendbaren Matlab Skripte für die Nachbildung eines GPS-Sensors finden. Da die Nachbildung eines solchen Systems in Matlab eine zusätzliche Diplomarbeit ausfüllen würde, wird in dieser Arbeit auf die genaue Nachbildung eines GPS-Sensors verzichtet und eine sehr einfache Nachbildung verwendet. Die Messung des GPS-Sensors wird aus der aktuellen Position im Inertialkoordinatensystem mit hinzugefügtem normalverteilten Rauschen bestehen Kamera Die Kamera ist ein weiterer wichtiger Sensor, den es nachzubilden gilt. Hierzu existieren schon fertige Lösungen wie z.b. die Epipolar Geometry Toolbox [Mariottini and Prattichizzo, 25], welche einige Funktionen zur Darstellung und Nachbildung von Kamerasystemen mit Hilfe von Matlab bereitstellt. Dennoch wurden die gebotenen Funktionen hauptsächlich zum Validieren eigener Funktionen verwendet und nur wenige aktiv verwendet. 6

73 6.3 Umgebungs- und Sensormodelle Prinzipiell besteht der Sensor Kamera, aus der Implementierung der in Unterabschnitt vorgestellten Abfolge. Beginnend mit einer Rotation und Transformation werden die Punkte in die Kamera projiziert. Anschließend finden die Linsenverzerrung und die Umrechnung zu Pixelkoordinaten statt. Mit diesem einfachen Prinzip können Weltpunkte zu einem Bild in Pixelkoordinaten umgerechnet werden, wie in Abbildung 6.1 dargestellt Z v [pixel] Y c Z c X c u [pixel] Abbildung 6.1: Kamerabild (rechts) der simulierten Kamera in einer dreidimensionalen Umgebung 1 (links) Eine weitere kameraspezifische Funktion, die eher unter die Rubrik Simulation 2 Y X 5 einer Stereokamera fallen würde, ist die in Unterabschnitt vorgestellte Stereotriangulation. Mit dieser Funktion können die 3-D-Punkte aus zwei Kamerabildern 3 zurückberechnet werden IMU Die Inertial Measurement Unit ist wie auch das GPS, ein Sensor, der sich nicht als frei verfügbare Implementierung in Form eines Matlab Skriptes finden lies. Eine Möglichkeit der Simulation wäre die Verwendung des IMU-Blockes aus der Matlab Aerospace Toolbox, allerdings ist dieser nicht sehr ausführlich dokumentiert und unterstützt keine Simulation eines laufenden Offset-Fehlers einer IMU. Eine einfache Möglichkeit zur Simulation lies sich in [Crassidis, 25] finden. Das hierin vorgestellte und ausführlich hergeleitete Modell einer IMU simuliert das Messrauschen, sowie den laufenden Bias, der typisch für eine IMU ist: ω k+1 = ω k σ 2 [b 2 k+1 + b k ] + v t σ2 u t N v (6.1) b k+1 = b k + σ u t Nu (6.2) 61

74 6 Simulation Das dargestellte Messmodell wurde für das Gyroskop aufgestellt und verhält sich äquivalent für die Messungen des Beschleunigungssensors. In jedem Zeitschritt t wird die tatsächliche Winkelgeschwindigkeit ω durch einen laufenden Bias b und ein normalverteiltes Rauschen modifiziert, um die Messung ω zu simulieren. Dabei stellen die Variablen N v und N u eine generierte normalverteilte und auf eins normierte Zufallsgröße für die Simulation des Rauschens bzw. des laufenden Bias dar. Als weitere Faktoren wirken die Standardabweichungen σ u und σ v. Sie stehen für die Standardabweichung des laufenden Bias bzw. für die des Messrauschens. Es ist anzumerken, dass in diesem Modell auf die Nachbildung eines weiteren Fehlereinflusses durch das Driften des Skalierungsfaktors, verzichtet wurde v [km/h] a x [m/s 2 ] a y [m/s 2 ] a z [m/s 2 ].1.5 IMU Accelerometer Measurements ( with Noise / without Noise) ω x [ /s] ω y [ /s] ω z [ /s] IMU Gyro Measurements ( with Noise / without Noise) Abbildung 6.2: Simulation einer Inertial Measurement Unit. Der abgefahrene Pfad (links oben) resultiert in den Beschleunigungsmessungen (links unten) und den Winkelgeschwindigkeitsmessungen (rechts unten). Die dabei aufgetretene Geschwindigkeit ist betragsmäßig im rechten oberen Bild dargestellt. Zum Veranschaulichen des Modells und um einen Eindruck für die Größenverhältnisse des Rauschens zu erhalten, sind in Abbildung 6.2 die Ergebnisse eines Simulationslaufes über 5 Minuten zu sehen. In diesem einfachen Beispiel kann die dargestellte Kamera als das Luftschiff angesehen werden, welches sich durch den Raum bewegt. Die Bewegung ist im Ersten der vier Bilder dargestellt, wobei die Kamera in äqui- 62

75 6.3 Umgebungs- und Sensormodelle distanten Simulationsschritten entlang des Pfades eingeblendet ist. Hauptsächlich findet eine Bewegung in der X-Y-Ebene statt, allerdings ist auch eine kleine Veränderung in der Höhe enthalten, welche nicht sehr deutlich in der Pfaddarstellung zu erkennen ist. In der nebenstehenden Grafik wurde der Verlauf des Betrages der Geschwindigkeit des Luftschiffes über die Gesamtstrecke von 15m ausgewertet. Mit maximal 4 km/h ist diese sehr gering. Die eigentlich interessante IMU-Messung wird in den unteren beiden Bildern gezeigt. Hierfür wurde die Standardabweichung für das Rauschen des Gyroskopes und des Beschleunigungssensors aus dem Datenblatt der IMU MTi-G von Xsens entnommen (Gyroskop: σ gv = 1, rad/s 1/2, Beschleunigungssensor: σ av = m/s 3/2 ). Weiterhin wurde ein nicht im Datenblatt enthaltener Bias angenommen: σ gu = rad/s 3/2 und σ au = m/s 5/2. Diese Werte entsprechen nicht der Realität, sondern wurden für eine bessere Anschaulichkeit gewählt. Da die IMU nicht im Welt-, sondern im Bodykoordinatensystem misst, wurden die angreifenden Beschleunigungen in das Bodykoordinatensystem transformiert. Dabei zeigt die X Achse des Bodykoordinatensystems in Bewegungsrichtung und die Z Achse nach oben Landmarken Die Landmarken gehören nicht mehr zu den Sensormodellen, doch stellen sie einen wichtigen Bestandteil der Simulation dar. Sie bestehen aus einfachen Punkten in der 3-D-Welt, die je nach Verwendungszweck angeordnet werden können. Um eine möglichst realitätsnahe Umgebung aufzubauen, könnte bspw. ein digital elevation model (DEM) benutzt werden, um das Terrain einer realen Umgebung nachzubilden. Eine solch hohe Realitätstreue ist für das Testen von den in dieser Arbeit verwendeten Algorithmen jedoch nicht nötig. Einfache Modelle zur Anordnung von Landmarken reichen vollkommen aus. Dazu zählt eine zufällige Anordnung von N Landmarken auf einer Kugel mit variierendem Radius. Damit kann angegeben werden, in welchem Abstand sich die Landmarken in etwa befinden. Für die Simulationen wurden Landmarken im Abstand von 1 m bis 1 m vom Beobachter gewählt. An dieser Stelle ist zu bemerken, dass Landmarken in der Realität um einiges komplexer zu behandeln sind. Die Kapitel und haben bereits einen Einblick in das Finden und Zuordnen von Landmarken gegeben. Es ist keinesfalls trivial, eine Landmarke zu finden und über mehrere Bilder zu verfolgen. Dieser Tatsache konnte in dieser einfachen Simulation nicht Rechnung getragen werden. Die Landmarkenübereinstimmungen waren immer 1-prozentig und wurden nicht über einen der vorgestellten Algorithmen zum Finden interessanter Punkte bestimmt. 63

76 6 Simulation 6.4 Implementierung Im Laufe der Arbeit entstand eine Vielzahl von Matlab Funktionen, um die in den einzelnen Abschnitten vorgestellten Algorithmen, Darstellungen und Berechnungen umzusetzen. Aufgrund der Vielfalt können die Funktionen im Einzelnen nicht vorgestellt und beschrieben werden. Dennoch sind die Funktionen jeweils kommentiert und besitzen zum Großteil eine Zusammenfassung zu Beginn des Skriptes. Um die Übersichtlichkeit über die Funktionen zu wahren, wurde eine Matlab GUI erstellt, die durch die jeweils nötigen Schritte führt und einfach zu bedienen ist. Auf der beiliegenden CD kann dieses GUI über sl start.m gestartet werden. Abbildung 6.3: Erste Auswahlmöglichkeit bei Start der Implementierten Matlab GUI. Innerhalb des erscheinenden Auswahlfensters (Abbildung 6.3) besteht die Möglichkeit zwischen den Algorithmen zur Stereokamera und dem Mono-Kamera-SLAM Verfahren zu wählen. Mit der Bestätigung des unteren Auswahlknopfes öffnet sich ein neues Fenster (Abbildung 6.4) mit den entsprechenden Funktionen zum Mono- Kamera-SLAM Verfahren. Die jeweiligen Knöpfe sind nach der Funktion benannt, welche im Hintergrund ausgeführt wird, sobald der Knopf betätigt wurde. Die Funktionen für das Mono-Kamera-SLAM Verfahren sind entsprechend chronologisch von oben nach unten angeordnet. Zuerst kann die Einstellung der Simulationsparameter (Abbildung 6.5) erfolgen. Hier kann bspw. die Wahl der Sensorkonfiguration oder der Anzahl der simulierten Simulationsschritte getroffen werden. Ebenso kann eingestellt werden, ob die Simulation der Sensormessungen mit oder ohne Messfehler ( Rauschen (ein/aus) ) durchgeführt werden soll. Fällt die Auswahl auf das Simulieren von Sensorrauschen, so kann in einer weiteren Instanz eingestellt werden, ob für das Rauschen bei jeder Simulation die selben Zufallswerte verwendet werden sollen, oder bei jeder durchgeführten Simulation neue Zufallswerte erzeugt werden. Die Option konstante Zufallswerte (ein/aus) sollte also für den Vergleich von verschiedenen Sensorkonfigurationen oder Filteralgorithmen eingeschaltet sein. Nachdem alle Einstellungen vorgenommen wurden, kann die Simulation des einfachen Luftschiffmodells mit dem Aufruf des Knopfes sl simuflightdata.m durchgeführt werden. Im Hintergrund greift diese Funktion auf das erstellte Simulink Mo- 64

77 6.4 Implementierung Abbildung 6.4: Erste Auswahlmöglichkeit bei Start der Implementierten Matlab GUI. Abbildung 6.5: Erste Auswahlmöglichkeit bei Start der Implementierten Matlab GUI. dell zurück und übergibt dieser eine Liste von Wegpunkten, die durch das Luft- 65