Kombination von Inertialsensoren und GPS zur Navigation

Transcription

Kombination von Inertialsensoren
und GPS zur Navigation
Diplomarbeit
von
Jonas Hoffmann
am Institut für Informatik
des Fachbereichs Mathematik und Informatik
Freie Universität Berlin
Erstgutachter:
Zweitgutachter:
Prof. Dr. Raúl Rojas
Dr. Daniel Göhring
Betreuende Mitarbeiter:
Dipl.-Inform. Tinosch Ganjineh
Dipl.-Inform. Fabian Wiesel
Ich erkläre hiermit, dass ich die vorliegende Arbeit selbständig verfasst und keine
anderen als die angegebenen Quellen und Hilfsmittel verwendet habe.
Berlin, den 17. August 2010
Anmerkungen
Alle Abbildungen wurden von mir selbst erstellt oder sind frei von Lizenzen (public
domain), sofern keine Quellenangabe in der Bildunterschrift vorhanden ist.
Begriffe, die fett gedruckt sind, werden im Glossar erläutert.
Inhaltsverzeichnis
1 Einleitung
1.1 Das Projekt Autonome Fahrzeuge
1.1.1 Spirit of Berlin . . . . . .
1.1.2 Projekt AutoNOMOS . .
1.2 Zielsetzung der Arbeit . . . . . .
1.3 Gliederung der Arbeit . . . . . .
an der FU
. . . . . .
. . . . . .
. . . . . .
. . . . . .
Berlin
. . . .
. . . .
. . . .
. . . .
.
.
.
.
.
2 Grundlagen
2.1 Navigation . . . . . . . . . . . . . . . . . . . . . . . .
2.1.1 Geschichte der Navigation . . . . . . . . . . .
2.1.2 Globale Navigationssatellitensysteme (GNSS)
2.1.3 GPS-Technik . . . . . . . . . . . . . . . . . .
2.1.4 Differential-GPS . . . . . . . . . . . . . . . .
2.1.5 Echtzeitkinematik (RTK) . . . . . . . . . . .
2.2 Hardware und Sensoren . . . . . . . . . . . . . . . .
2.2.1 Beschleunigungssensoren . . . . . . . . . . . .
2.2.2 Gyroskope . . . . . . . . . . . . . . . . . . . .
2.2.3 MEMS-Sensoren . . . . . . . . . . . . . . . .
2.3 Weltmodell und Fahrzeugdynamik . . . . . . . . . . .
2.3.1 Koordinatensysteme . . . . . . . . . . . . . .
2.3.2 Bezugssyteme . . . . . . . . . . . . . . . . . .
2.4 Quaternionen . . . . . . . . . . . . . . . . . . . . . .
2.5 Richtungskosinusmatrix . . . . . . . . . . . . . . . .
2.6 Kalman-Filter . . . . . . . . . . . . . . . . . . . . . .
2.6.1 Lineares Kalman-Filter . . . . . . . . . . . . .
2.6.2 Erweitertes Kalman-Filter . . . . . . . . . . .
2.6.3 Unscented Kalman-Filter . . . . . . . . . . . .
2.6.3.1 Vorhersage . . . . . . . . . . . . . .
2.6.3.2 Update . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3 Analyse
3.1 Problemstellung, Anforderungen und Randbedingungen
3.2 Existierende Lösungsansätze . . . . . . . . . . . . . . .
3.2.1 Existierende Arbeiten . . . . . . . . . . . . . . .
3.2.2 Zustandsraum und dynamisches Modell . . . . .
3.2.2.1 Übergangsfunktion . . . . . . . . . . .
3.2.2.2 Beobachtungsfunktion . . . . . . . . .
3.2.3 Ergebnisse und Vergleich mit weiteren Arbeiten
3.3 Schlussfolgerungen . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
1
2
3
4
4
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7
7
7
8
10
10
11
12
12
13
13
14
14
14
15
17
17
17
19
19
21
21
.
.
.
.
.
.
.
.
23
23
24
24
24
25
26
26
27
vi
Inhaltsverzeichnis
4 Hardware
4.1 Die Hardwareplattform IMU6 . . . . . . . . . . . . . . .
4.1.1 Technische Daten . . . . . . . . . . . . . . . . . .
4.1.2 Aufbau und Funktionsweise . . . . . . . . . . . .
4.2 Versuchsaufbau im autonomen Fahrzeug . . . . . . . . .
4.2.1 Einbau und Anschluss an das vorhandene System
4.3 Schlussfolgerungen . . . . . . . . . . . . . . . . . . . . .
5 Software
5.1 Firmware der Hardwareplattform .
5.1.1 Aufbau und Funktionsweise
5.1.2 Nachrichtenformat . . . . .
5.2 Kalman-Filter, Orocos-Modul . . .
5.2.1 Bestandteile . . . . . . . . .
5.2.2 Ein-/Ausgabemodul . . . .
5.2.3 Kalman-Filter-Modul . . . .
5.2.4 Übergangsfunktion . . . . .
5.2.5 Beobachtungsfunktion . . .
5.3 Zusammenfassung und Resultate .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
29
29
29
31
32
32
33
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
35
35
35
36
37
37
37
38
38
40
41
6 Evaluierung
6.1 Evaluierung der Sensorqualität . . . . . . . . . . . . . .
6.1.1 GPS-Daten . . . . . . . . . . . . . . . . . . . .
6.1.2 Gyroskopdaten . . . . . . . . . . . . . . . . . .
6.1.2.1 Gierrate . . . . . . . . . . . . . . . . .
6.1.2.2 Nick- und Rollrate (X-/Y-Gyroskope)
6.1.3 Accelerometerdaten . . . . . . . . . . . . . . . .
6.2 Ergebnisse der Filterung . . . . . . . . . . . . . . . . .
6.2.1 Teststrecke . . . . . . . . . . . . . . . . . . . .
6.2.2 Ergebnisse . . . . . . . . . . . . . . . . . . . . .
6.2.3 Ersatz der X- und Y-Gyroskope . . . . . . . . .
6.3 Fazit . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
43
43
43
45
45
46
46
48
48
48
49
51
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
7 Zusammenfassung und Ausblick
53
A Anhang: Unscented Kalman-Filter Pseudocode
A.1 Initialisierung . . . . . . . . . . . . . . . . . . . . . . . . . .
A.2 Vorhersage . . . . . . . . . . . . . . . . . . . . . . . . . . . .
A.2.1 Berechnung der Sigma-Punkte . . . . . . . . . . . . .
A.2.2 Anwendung der Übergangsfunktion auf Sigma-Punkte
A.2.3 Berechnung von Mittelwert und Kovarianz . . . . . .
A.3 Beobachtung . . . . . . . . . . . . . . . . . . . . . . . . . . .
A.3.1 Berechnung der Sigma-Punkte . . . . . . . . . . . . .
A.3.2 Anwendung der Übergangsfunktion auf Sigma-Punkte
A.3.3 Berechnung von Mittelwert und Kovarianz . . . . . .
A.3.4 Korrektur . . . . . . . . . . . . . . . . . . . . . . . .
55
55
55
55
56
56
56
56
57
57
57
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Literaturverzeichnis
59
Glossar
64
Abbildungsverzeichnis
1.1
Das autonome Fahrzeug Spirit of Berlin . . . . . . . . . . . . . . . .
2
1.2
Das Team AutoNOMOS mit autonomem Fahrzeug Made in Germany
3
2.1
Ein Sextant zur astronomischen Navigation . . . . . . . . . . . . . . .
8
2.2
Positionsbestimmung mittels Pseudoranging (2D) . . . . . . . . . . .
9
2.3
Nutzung von DGPS mit Referenzstation . . . . . . . . . . . . . . . . 11
2.4
Klassisches Kreiselinstrument (Gyroskop) . . . . . . . . . . . . . . . . 13
2.5
Aufbau und mikroskopische Struktur eines MEMS-Gyroskops . . . . . 14
2.6
Lokale und Weltkoordinatensysteme . . . . . . . . . . . . . . . . . . . 15
2.7
Fahrzeugkoordinaten, Winkel und ENU-Koordinaten . . . . . . . . . 16
2.8
Vorhersage- und Updateschritt beim Kalman-Filter . . . . . . . . . . 17
2.9
Systemmodell des Kalman-Filters . . . . . . . . . . . . . . . . . . . . 18
2.10 Funktionsprinzip des UKF (aus [VDMWa04]) . . . . . . . . . . . . . 20
4.1
Oberseite der IMU6-Platine mit Beschriftung der Komponenten . . . 30
4.2
IMU6-Aufbau im Schema . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.3
Schema des Datenflusses zwischen den Komponenten im Fahrzeug . . 32
5.1
Zustände des in der Firmware implementierten Automaten . . . . . . 36
5.2
Aufbau des Nachrichtenformats . . . . . . . . . . . . . . . . . . . . . 36
5.3
Auszug aus den vom IMU6-Modul übertragenen Nachrichten . . . . . 37
5.4
Berechnungen zum Positionsupdate . . . . . . . . . . . . . . . . . . . 40
6.1
GPS-Evaluation auf Strecke 1 . . . . . . . . . . . . . . . . . . . . . . 44
6.2
GPS-Evaluation auf Strecke 2 . . . . . . . . . . . . . . . . . . . . . . 44
6.3
Vergleich des Z-Achsen-Gyroskop der IMU6 mit Referenzsystem . . . 46
6.4
Drift der Gyroskope . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
viii
Abbildungsverzeichnis
6.5
Ausschnitt aus Sensordaten des Z-Achsen-Beschleunigungsmessers . . 47
6.6
Teststrecke für die Evaluation des Kalman-Filters . . . . . . . . . . . 48
6.7
Resultate des Filters bei der Lokalisierung . . . . . . . . . . . . . . . 49
6.8
Fehler des Filters in Position und Geschwindigkeit . . . . . . . . . . . 50
1. Einleitung
Den Wunsch oder die Notwendigkeit für Menschen sich fortzubewegen gibt es seit
Anbeginn der Menschheit. Mindestens ebenso alt ist auch das Bedürfnis nach Orientierung.
Die Grundlage für die Navigation im Allgemeinen bildet die möglichst genaue Bestimmung von Position, Geschwindigkeit und Ausrichtung eines Objekts. Dabei handelt es sich meist um Flugzeuge, Fahrzeuge, Schiffe oder Fußgänger.
Navigationsgeräte für PKW sind erst seit etwa 10 Jahren zu erschwinglichen Preisen
auf dem Markt erhältlich und dennoch bereits zu Alltagsgegenständen avanciert, die
für viele nicht mehr wegzudenken sind.
In der vorliegenden Arbeit liegt der Schwerpunkt auf der präzisen Lokalisierung mittels globaler Navigationssatellitensysteme und auf der Integration von
Inertialsensoren zur Verbesserung der Genauigkeit und Überbrückung von Empfangsausfällen, im Speziellen im Kontext der Entwicklung eines autonom fahrenden
PKW.
Autonome Fahrzeuge sind gleich aus mehreren Gründen ein besonders interessantes
Anwendungsgebiet der Informatik. Sie vereinen die verschiedenen Unterdisziplinen
der Künstlichen Intelligenz wie kaum ein anderes Forschungsgebiet. Von ComputerVision und 3D-Sensoren über 2D-Bildverarbeitung bis hin zu reaktivem Verhalten,
Wegplanung und intelligentem, energiebewusstem Fahren. Es gibt dabei Herausforderungen zu lösen, die in viele Bereiche sowohl der theoretischen und praktischen
Informatik als auch der Regelungstechnik, Elektrotechnik und Physik reichen.
Sich autonom zu bewegen bedeutet auch für ein Fahrzeug, dass es sich in einer Umgebung zurechtfindet und sich orientieren kann, die Kenntnis um die eigene Position
ist dafür essentiell.
1.1
Das Projekt Autonome Fahrzeuge an der FU
Berlin
Diese Arbeit entstand im Projekt Autonome Fahrzeuge der AG Künstliche Intelligenz an der Freien Universität Berlin.
2
1. Einleitung
Das Projekt unter Leitung von Prof. Dr. Raúl Rojas beschäftigt sich schon seit
vielen Jahren mit der Thematik und entwickelt seit 2006 das autonome Fahrzeug
Spirit Of Berlin 1 , welches im Fall dieser Arbeit zugleich als Versuchsträger und
Referenzsystem dient.
Die Hauptarbeit am Projekt fließt dabei in die Softwareentwicklung auf Basis des
Robotik-Frameworks (OROCOS2 ) ein. Dazu werden Module entwickelt, die im Zusammenspiel die Integration der umwelterfassenden Sensorik, Verhaltenssteuerung
und Aktorik steuern.
Die Beschäftigung mit autonomen Fahrzeugen entstand ursprünglich durch die Arbeit an einem thematisch verwandten Projekt. Bereits 1998 wurden am Fachbereich
Informatik der Freien Universität autonome Roboter entwickelt und gebaut, mit denen seitdem sehr erfolgreich in verschiedenen Ligen und an diversen Meisterschaften
im Roboterfußball, dem RoboCup, teilgenommen wurde.
1.1.1
Spirit of Berlin
Im Jahr 2006 wurde damit begonnen, ein autonomes Fahrzeug zu entwickeln, den
Spirit of Berlin. Mit diesem Fahrzeug wurde 2007 erfolgreich an der DARPA Urban Grand Challenge teilgenommen, seitdem wird seine Entwicklung kontinuierlich
weiter vorangetrieben.
Abbildung 1.1: Das autonome Fahrzeug Spirit of Berlin vor der Teilnahme an der
DARPA Urban Grand Challenge 2007
Der Spirit of Berlin, kurz SpoB, ist ursprünglich ein behindertengerecht umgerüsteter Dodge PKW. Die gesamte Aktorik: Pedale für Bremse und Gas, Schaltung und
Lenkstangenmotor lassen sich elektronisch ansteuern. Er besitzt außerdem diverse
1
2
http://robotics.mi.fu-berlin.de/
http://www.orocos.org/
1.1. Das Projekt Autonome Fahrzeuge an der FU Berlin
3
Lasersensoren für den Nah- und Fernbereich, verschiedene Kameras, sowie ein Applanix POS LV220 -GPS mit einer IMU (Inertial Measurement Unit: ein elektronisches
Gerät, bestehend aus Inertialsensoren) und einem Odometer. Dieses GPS dient
bei der Evaluation der in dieser Arbeit vorgestellten Plattform und Filterung als
Hochpräzisionssystem.
Alle Sensordaten laufen in einem zentralen Server zusammen, auf dem eine modifizierte Variante des OROCOS Real Time Toolkit läuft. Hierbei handelt es sich um
ein Framework, welches die Steuerung und Kommunikation von Threads in Echtzeit
ermöglicht. Die entwickelten Module für den Empfang von Sensordaten, Verarbeitung, Verhalten, Entscheidungsfindung und Aktorik laufen als unabhängige Threads,
die untereinander über Ports Daten austauschen.
1.1.2
Projekt AutoNOMOS
Die bisher erreichten Ergebnisse und Erfahrungen werden seit 2010 in dem vom
Bundesministerium für Bildung und Forschung geförderten Projekt AutoNOMOS 3
genutzt. Die finanzielle Unterstützung konnte dazu verwendet werden, ein von Grund
auf fürs autonome Fahren konzipiertes Auto zu entwickeln und zu realisieren.
Der Made in Germany ist ein VW Passat, der auf den ersten Blick nicht von einem
regulären PKW zu unterscheiden ist. In die Karosserie sind jedoch umwelterfassende Sensoren integriert. Im Kofferraum befinden sich große Teile der Technik, und
durch aufwendige Elektronik lassen sich von einem Computer aus umfangreiche Informationen über den Zustand des Fahrzeugs abfragen und nahezu alle Funktionen
steuern.
Außerdem konnten durch die Unterstützung des BMBF Stellen geschaffen werden,
die es ermöglichen, mit zusätzlicher personeller Unterstützung die Weiterentwicklung
und den Weg zu autonomen Fahrzeugen im alltäglichen Straßenverkehr voranzubringen.
Abbildung 1.2: Das Team AutoNOMOS mit autonomem Fahrzeug Made in Germany
3
http://autonomos.inf.fu-berlin.de/
4
1. Einleitung
Auch die Software wird ständig weiterentwickelt und enthält inzwischen umfangreiche Verbesserungen und Neuentwicklungen aufgrund der Erkenntnisse aus zahllosen
fahrerlosen Feldversuchen.
1.2
Zielsetzung der Arbeit
Die Aufgabe bestand in der Konzeption, Implementation und Evaluierung eines
Filters zur Integration von Sensordaten, um die aktuelle Position, Orientierung und
Geschwindigkeiten eines Fahrzeugs zu ermitteln.
Von der Firma Hella Aglaia Mobile Vision GmbH, einem Automobilzulieferer und
Anbieter von Komponenten für Fahrerassistenzsysteme, wurde eine Hardwareplattform zur Verfügung gestellt, bestehend unter anderem aus einem GPS-Modul,
Inertialsensoren und einem Mikroprozessor. In Abschnitt 4.1 wird diese Platine
genauer vorgestellt.
Das Interesse der Firma besteht hauptsächlich darin, die Nutzbarkeit dieser Plattform als Quelle für ein von ihr entwickeltes Spurhaltesystem (Lane Departure Warning - LDW) zu evaluieren. Dem Projekt wurde seitens der Firma neben der IMU6Hardware ein solches Spurhaltesystem, bestehend aus Mono-Kameras (INKA) mit
Fusionsbox und einem PC-System mit Softwareumgebung (Cassandra), zur Nutzung im Fahrzeug zur Verfügung gestellt.
Dabei werden als Eingabeparameter für das Spurhaltesystem die genaue Geschwindigkeit und die Gierrate des Fahrzeugs benötigt und zwar insbesondere auch bei
eingeschränktem oder nicht vorhandenem GPS-Empfang. Diese Daten müssten sonst
über eine teure und aufwendige Anbindungen an den CAN-Bus des Fahrzeugs bezogen werden.
Diese Hardwareplattform wurde ohne laufende Firmware ausgeliefert, es war lediglich eine Beispielsoftware, bestehend aus Treibern und einem kleinen Demoprogramm, vorhanden, durch das rudimentärer Zugriff auf die Sensoren erfolgen konnte. Ein Teil der Vorarbeit bestand demnach in der Entwicklung einer Firmware,
welche die Daten von GPS und Inertialsensoren ausliest und mit Zeitstempeln
und Prüfsummen versieht und danach in Echtzeit über die serielle Schnittstelle des
Moduls ausgibt. Genauer beschrieben wird die Funktionsweise dieser Firmware in
Kapitel 5.1.
Es stellte sich beim Programmieren der IMU6-Plattform schnell heraus, dass durch
diverse Fehler in den Treibern und auch im Hardwarelayout der Platine diese Entwicklung einen erheblichen Anteil an der Gesamtarbeit in Anspruch nehmen würde.
1.3
Gliederung der Arbeit
Zunächst wird auf die Grundlagen der Arbeit eingegangen. Dazu gehört die
Funktionsweise von globalen Navigationssatellitensystemen und die der beteiligten
Inertialsensoren. Es werden außerdem die zugrunde liegenden Koordinatensysteme und die Lagerepräsentation durch die Quaternion beschrieben. Desweiteren wird
die Theorie des Kalman-Filters und seiner nichtlinearen Varianten erklärt.
Im darauf folgenden Kapitel 3 wird die Problemstellung und Zielsetzung genauer
erörtert. Anschließend werden die bereits existierenden Lösungsansätze vorgestellt,
miteinander verglichen und der eigenen Vorgehensweise gegenübergestellt.
1.3. Gliederung der Arbeit
5
Im 4. Kapitel geht es um die verwendete Hardware sowie deren Einsatz und Aufbau
im Versuchsträger. Das Kapitel 5 beschäftigt sich mit der Konzeption und Implementierung der Softwarekomponenten. Dabei geht es einerseits um die Firmware,
die auf der verwendeten Hardwareplattform läuft, und andererseits um die Softwaremodule, die für die Integration der Daten zuständig sind und um das verwendete
Framework.
Danach werden die verschiedenen durchgeführten Tests des Systems vorgestellt, deren Ergebnisse ausgewertet und ein Fazit gezogen.
Auf dieses Kapitel folgt eine kurze Zusammenfassung und ein Ausblick auf die Fortführung des Projekts und auf weitergehende Arbeiten.
6
1. Einleitung
2. Grundlagen
In diesem Kapitel wird zunächst auf die Grundlagen der satellitenbasierten Navigation allgemein eingegangen. Danach werden speziell die Funktionsweise des Navstar-GPS erläutert und die verschiedenen Verfahren zur Verringerung der auftretenden Fehler angesprochen.
In weiteren Kapiteln werden die Inertialsensoren einzeln vorgestellt und deren
Funktion erklärt. Anschließend wird das verwendete Weltmodell1 dargestellt und im
letzten Abschnitt die Theorie und Varianten des Kalman-Filters beschrieben.
2.1
2.1.1
Navigation
Geschichte der Navigation
Schon immer haben die Menschen den Verlauf der Sonne, des Mondes und der Sterne beobachtet, um dadurch Rückschlüsse auf ihre Umwelt zu ziehen. Auch zur Bestimmung der eigenen Position und zum Abschätzen von Richtungen und Zeiten
wurden seit jeher die Bahnen der Himmelskörper herangezogen. Zum Beispiel kann
durch die Bestimmung des Winkels zwischen Polarstern und Horizont leicht auf
den Breitengrad geschlossen werden. Bedeutende Instrumente für derartige Winkelbestimmungen waren beispielsweise die eingesetzten Oktanten oder Sextanten, wie
in Abbildung 2.1 dargestellt. Besonders wichtig wurden diese Techniken durch die
stetig zunehmende Bedeutung der Seefahrt.
Bei dieser sogenannten astronomischen Navigation wurden jedoch auch unter Idealbedingungen kaum Genauigkeiten unter einigen Kilometern erreicht. Das änderte
sich erst mit der Verfügbarkeit genauer Uhren und der Einführung von Kurzwellenfunk, mit dem ein exaktes Zeitsignal auch auf Schiffen empfangen werden konnte.
Seit der griechischen Antike wird auch das Erdmagnetfeld genutzt, um Himmelsrichtungen mit Hilfe von Magnetnadeln bzw. der heutigen Kompasse zu bestimmen.
Es wurden nach und nach Tabellen, Karten und Geräte entwickelt, um die Zusammenhänge darzustellen und diese zum Navigieren zu nutzen.
1
Modell, welches die dynamischen Eigenschaften der Umgebung beschreibt
8
2. Grundlagen
Indexspiegel
verdunkelte Gläser
Horizontspiegel
Teleskop
Rahmen
verdunkelte
Gläser
14
0
0
13
0
10
12
0
Winkelskala
110
100
30
90
80
70
60
20
50
Alhidade
Lupe
Mikrometer-Trommel
Klemme
Abbildung 2.1: Ein Sextant, wie er in der Seefahrt etwa seit dem 18. Jahrhundert
eingesetzt wird (Quelle: Wikimedia Commons, bearbeitet von Arne Nordmann, Original von Joaquim Alves Gaspar)
In den letzten Jahrzenten wurden diese natürlichen Navigationsquellen in vielen Bereichen mehr und mehr durch technische Hilfsmittel ersetzt, weil die Genauigkeit der
optischen Verfahren häufig nicht mehr ausreichend war oder weil sie nicht mit den
oftmals computergestützten und automatisierten Verfahren Schritt halten konnte.
Ein Hauptgrund und die treibende Kraft für die Einführung satellitenbasierter Navigationssysteme war natürlich auch der Bedarf des Militärs an präziser Lokalisierung
und an genauen Kartendaten.
2.1.2
Globale Navigationssatellitensysteme (GNSS)
Ziel der Lokalisierung mittels Navigationssatelliten ist es, die eigene Position im dreidimensionalen Raum zu bestimmen, in der Regel dargestellt als geografische Breite,
Länge und Höhe. Desweiteren lässt sich die Eigengeschwindigkeit des Empfängers
durch den Doppler-Effekt auch direkt aus den Satellitensignalen ableiten.
Anfang der 1960er Jahre, nicht lange nachdem die ersten Satelliten überhaupt in eine
Erdumlaufbahn geschickt worden waren, entwickelte die John-Hopkins-Universität
im Auftrag der US Navy bereits das erste satellitenbasierte Navigationssystem mit
dem Namen SPUTNIK. Damals mit einer Genauigkeit von etwa 200 Metern. Hier
erfolgte auch die Lokalisierung noch durch Ausnutzung des Doppler-Effekts.
Die zur Navigation genutzten Satelliten sind mit genauen Atomuhren ausgestattet. Beispielsweise befinden sich in GPS-Satelliten Rubidium-Atomuhren mit Abweichungen von maximal 1 Sekunde in 10.000 Jahren. Die aktuelle Position der
2.1. Navigation
9
Satelliten kann berechnet werden, da sie fortwährend ihre eigenen Bahndaten zum
Empfänger senden.
Hätte der Empfänger nun auch eine genaue Uhr, könnte er die Laufzeiten von drei
Satelliten bestimmen, dadurch auf die Entfernungen schließen und somit durch klassische Triangulation die eigene Position ermitteln.
Jede Signallaufzeit zu einem Satelliten steht für seine Entfernung und beschreibt die
Oberfläche einer Kugel um den Satelliten als Mittelpunkt. Dort, wo sich die Oberflächen dreier Kugeln schneiden, befände sich die Empfängerposition. (Das Verfahren
wird auch Trisphäration genannt.) In der Regel schneiden sich diese drei Kugeln in
zwei Schnittpunkten, einer der Punkte liegt aber viele Kilometer über der Erdoberfläche und kann somit ausgeschlossen werden.
Die Uhr im Empfänger ist in der Regel keine Atomuhr, allein schon wegen der Größe
und Kosten einer solchen, sondern eine einfache Quarzuhr. Diese ist etwa 100.000mal ungenauer als eine typischerweise in Satelliten eingesetzte Atomuhr und daher
gibt es zusätzlich zu den drei Unbekannten Länge, Breite und Höhe noch eine vierte
unbekannte Variable, die Abweichung der Empfängeruhr ∆t.
Die gemessenen Laufzeiten und somit Entfernungen der Satelliten sind also um genau diese Differenz von den realen Entfernungen abweichend und werden deshalb
auch Pseudoentfernungen genannt. Eine Abweichung der Empfängeruhr von nur
0,01 Sekunden würde dabei schon einen Fehler in der Distanz in Höhe von 3.000
km bedeuten. Aus diesem Grund sind für die endgültige Bestimmung der Position
mindestens vier Satelliten erforderlich.
r1
S1
dt
r2
P
S2
r3
S3
Abbildung 2.2: Positionsbestimmung mittels Pseudoranging (2D)
In Abbildung 2.2 ist der Sachverhalt für den zweidimensionalen Fall veranschaulicht.
P ist die reale Position des Empfängers. Die gemessenen Pseudoentfernungen sind
10
2. Grundlagen
hier rot gestrichelt dargestellt. Die beiden Entfernungen zu S1 und S2 weichen um
ein unbekanntes ∆t von der realen Entfernung ab und schneiden sich deshalb hier
gar nicht. Die reale Entfernung wird hier durch grüne Kreise dargestellt.
Durch Hinzunahme eines dritten Satelliten S3 lässt sich ∆t bestimmen, geometrisch
hier durch einen roten Kreis dargestellt, der die Kreise mit r1, r2 und r3 berührt.
Der Mittelpunkt dieses Kreises ist die gesuchte Position P. Dieses Verfahren wird als
Pseudoranging oder auch Hyperbelortung bezeichnet. Analog dazu funktioniert das
Pseudoranging im dreidimensionalen Raum mit Kugeloberflächen statt Kreisen.
Das am weitesten verbreitete globale Satellitennavigationssystem ist heutzutage das
Navstar-GPS, das ursprünglich für militärische Zwecke entwickelte und weiterhin
genutzte System des US-Verteidigungsministeriums. Weitere Systeme sind das russische GLONASS und das chinesische COMPASS, welches sich ebenso wie das europäische GALILEO Programm im Aufbau befindet. Im Folgenden wird hier immer
vom Navstar-GPS ausgegangen und meist die Kurzform GPS verwendet.
2.1.3
GPS-Technik
Die jeweils mindestens 24 Satelliten des Navstar-GPS umkreisen die Erde in 6 Umlaufbahnen mit jeweils 4 Satelliten in 20.200 km Höhe und senden ihre Signale auf
den Trägerfrequenzen L1 bei 1575,42 Mhz und L2 bei 1227,6 MHz. Zusätzlich befinden sich zurzeit 4 Reservesatelliten in einer Umlaufbahn. Die übermittelten Daten
auf den Frequenzen bestehen aus dem C/A-Code (engl. Coarse Aquisition) und dem
verschlüsselten, der militärischen Nutzung vorbehaltenen P(Y)-Code [HWLW08].
Der C/A-Code wird auch PRN-Code (Pseudo Random Noise) genannt und ist eine
deterministische Folge aus 1023 Bits, die jede Millisekunde übertragen werden. Das
entspricht also einer Bitfrequenz von 1,023 Mhz. Dabei identifiziert eine Bitfolge
den jeweiligen Satelliten immer eindeutig. Die Signale der unterschiedlichen Satelliten werden auf der gleichen Frequenz mittels Codemultiplexverfahren übertragen.
Dabei kommen als Spreizcodes die sogenannten Gold-Folgen zum Einsatz, welche die
Trennung der unterschiedlichen Signale vereinfachen, weil die einzelnen Gold-Folgen
zueinander jeweils nahezu orthogonal stehen. Die gleiche Folge wird im Empfänger
erzeugt und soweit um einen Verzögerungswert verschoben, bis die beiden Signale
korrekt übereinander liegen. Man spricht dabei auch von der Synchronisation der
Signale.
Auf den C/A-Code werden die Daten mit 50 Bit/s aufmoduliert. Diese bestehen
aus den drei Teilen: Uhrzeit des Satelliten, Ephemeridendaten und Almanachdaten.
Die Almanachdaten dienen dazu, alle zu einem Zeitpunkt sichtbaren Satelliten zu
bestimmen und enthalten Informationen über die globale Konstellation des GPSSystems. Sie erleichtern somit das Finden“ von Satelliten nach einem Neustart des
”
Empfängers und werden von diesem meist intern zwischengespeichert. Die Ephemeridendaten sind erforderlich, um den aktuellen Satelliten zu verfolgen, und beinhalten
genaue Informationen über seine Umlaufbahn und Abweichungen. Die Uhrzeit des
Satelliten ist, wie bereits beschrieben, die Grundbedingung, um dessen Entfernung
und somit die eigene Position zu bestimmen.
2.1.4
Differential-GPS
Der Fehler in der ermittelten Laufzeit des Signals von einem Satelliten setzt sich aus
drei Teilen zusammen:
2.1. Navigation
11
• Fehler im Satelliten: Uhr- und Bahnabweichungen, relativistische Einflüsse
• atmosphärische Störungen: Ionosphären- und Troposphäreneinflüsse
• Fehler im Empfänger: Uhrabweichung und ungenaue Synchronisation
Durch die Laufzeitmessungen aller Satelliten an ortsfesten Referenzstationen mit
bekannter Position können die satellitenabhängigen Fehler und die atmosphärischen
Einflüsse größtenteils bestimmt werden. Die Abweichungen der gemessenen Laufzeiten (Pseudoentfernungen) von den aus der bekannten Eigenposition eigentlichen
Laufzeiten werden als Korrekturwerte an den Empfänger übermittelt (siehe Abbildung 2.3).
S3
S2
S4
S1
Δ1-Δ4
Rover
Referenzstation
Abbildung 2.3: Nutzung von DGPS mit Referenzstation
Es gibt dabei grundsätzlich die Möglichkeiten der Übertragung über Funk oder
über das Internet. Funk kann in dem Fall bedeuten, dass die Korrektursignale von
Langwellen- oder Kurzwellensendern, per GSM oder über geostationäre Satellitensysteme, sogenannte SBAS (Satellite Based Augmentation Systems), ausgestrahlt
werden. Das europäische SBAS-System heißt EGNOS und die US-amerikanische
Variante WAAS. Der Empfang für diese beiden Korrektursysteme ist in vielen
modernen GPS-Empfängern bereits implementiert. Zusätzlich gibt es kommerzielle
Anbieter von Korrekturdaten über geostationäre Satelliten, wie beispielsweise der
OmniSTAR-Dienst der niederländischen Firma Fugro. Die Korrektursignale ermöglichen eine Verbesserung der Positionsgenauigkeit von etwa 10-20 Metern bis auf
wenige Meter. Dabei hängt die erreichbare Genauigkeit direkt von der Entfernung
zwischen Referenzstation und Empfänger ab.
2.1.5
Echtzeitkinematik (RTK)
Als Echtzeitkinematik wird eine spezielle Variante des Differential-GPS bezeichnet,
welche nicht die GPS-Nachrichten, also C/A-Codes, sondern die Trägerfrequenz des
Signals zur Synchronisierung nutzt. Genau wie bei D-GPS werden hier auch ständig
die Pseudoentfernungen durch Korrekturdaten von einer oder mehreren Basisstationen verbessert. Theoretisch ist die hierbei erreichbare Genauigkeit im Vergleich
12
2. Grundlagen
zu D-GPS um denselben Faktor größer, um den auch die Trägerfrequenz größer als
die Nachrichtenfrequenz ist.
Die erreichte Positionsgenauigkeit ist bei allen GPS-Verfahren stark abhängig davon,
wie genau der Empfänger sich auf das GPS-Signal synchronisieren kann. Angenommen, der Empfänger ist in der Lage, sich mit einer Abweichung von 1% der Bitweite
auf ein Signal zu synchronisieren, dann entspräche das bei D-GPS und einer Frequenz von 1,023 Mhz (C/A-Code) einer Ungenauigkeit von 0,01 Mikrosekunden, das
sind in Entfernung ausgedrückt etwa 3 Meter.
Wenn der Empfänger, wie beim RTK-Verfahren, die L1-Trägerfrequenz 1575,42 Mhz
zur Synchronisation nutzt, ist die theoretisch erreichbare Genauigkeit aufgrund der
Wellenlänge von 19 cm bereits 1,9 mm, wenn man wieder die Abweichung von 1% der
Bitweite annimmt. Bei RTK treten jedoch Mehrdeutigkeiten in Abständen von ganzzahligen Wellenlängen auf, die bei Verwendung des Nachrichtensignals (C/A-Code)
nicht vorhanden sind, da hier die Kodierung durch die Gold-Folgen so gewählt ist,
dass sich zwei gleiche, zeitlich versetzte Signale leicht synchronisieren lassen, und da
sich die Bitfolge aufgrund der geringeren Frequenz nicht so schnell wiederholt. Diese Mehrdeutigkeiten aufzulösen, ist eine der komplexesten Aufgaben, die moderne
GPS-Empfänger zu erfüllen haben, und kann im Einzelnen in [HWLW08] nachgelesen werden.
2.2
2.2.1
Hardware und Sensoren
Beschleunigungssensoren
Beschleunigungssensoren oder Accelerometer messen Beschleunigung in Richtung einer Achse, also die zeitliche Ableitung des Geschwindigkeitsvektors. Durch einfaches
Integrieren erhält man daraus die Geschwindigkeit und durch zweifaches Integrieren
die zurückgelegte Strecke in Richtung der Messachse des Sensors, denn die Geschwindigkeit ist ja genau die zeitliche Ableitung des Ortsvektors.
Voraussetzung für die Positionsbestimmung mit Beschleunigungssensoren ist, dass
sich die Lage im Raum nicht ändert oder genau wie die Beschleunigung zu jedem
Zeitpunkt bekannt ist. Ebenfalls müssen die Startwerte für Geschwindigkeit und
Position bekannt sein, um sich absolut positionieren zu können. Hierbei ist zu beachten, dass bei diesem Verfahren selbst kleine Messfehler, bei denen es sich nicht
um normalverteiltes Rauschen handelt, sondern um systematische Abweichungen wie
Temperaturabhängigkeit, durch die zweifache Integration so bedeutende Dimensionen annehmen, dass durch simples Aufaddieren der fehlerbehafteten Messungen in
der Praxis die bestimmten Werte und die reellen Größen schnell divergieren.
Der Aufbau eines klassischen Accelerometers (Beschleunigungsmessers) besteht
meist aus einer beweglich gelagerten Masse und einem anliegenden piezoelektrischen
Element. Durch die Beschleunigung in eine Richtung wird auf das piezoelektrische
Element eine Kraft ausgeübt und somit eine messbare Spannung erzeugt, die durch
zwei Drähte an Kontakte außerhalb des Sensorgehäuses geleitet werden. Oft integriert man drei Beschleunigungssensoren, die paarweise orthogonal zueinander stehen, in einem Gehäuse und erhält dadurch einen 3-Achsen-Sensor.
2.2. Hardware und Sensoren
2.2.2
13
Gyroskope
Gyroskope oder auch Drehratensensoren messen die Drehrate um eine oder mehrere Achsen. Gemessen wird die zeitliche Ableitung des Winkels. Durch einmaliges
Integrieren erhält man daraus den Winkel, in dem sich ein Objekt im betrachteten
Zeitraum um eine Achse gedreht hat.
Rahmen
kardanische
Aufhängung
Drehachse
rotierender
Kreisel
Abbildung 2.4: Klassisches Kreiselinstrument (Gyroskop) mit Schwungrad, Drehachse, kardanischer Aufhängung und Rahmen
Ein klassisches Gyroskop, oft auch als Kreiselinstrument oder Kreiselkompass bezeichnet, besteht aus einem sich drehenden Schwungrad, welches frei beweglich in
einem kardanischen Lager aufgehängt ist (siehe Abbildung 2.4). Das rotierende Rad
behält durch die Drehimpulserhaltung seine Lage bei, auch wenn sich die Lage der
Aufhängung ändert. Diese Lagedifferenz lässt sich an den Achsen messen.
2.2.3
MEMS-Sensoren
Sowohl Beschleunigungsmesser als auch Gyroskope können mit heutiger Technik
als sogenannte MEMS (Micro Electro Mechanical System) - Sensoren hergestellt
werden. Hierbei kommen ähnliche Verfahren wie bei integrierten Schaltkreisen zum
Einsatz. Die Siliziumwafer werden dabei auf unterschiedliche Weise belichtet, neue
Schichten aufgetragen und überflüssige Teile weggeätzt, so dass am Ende die elektromechanischen Bestandteile des Sensors übrigbleiben.
Die MEMS-Technologie hat es möglich gemacht, zu geringen Preisen sehr kleine
Inertialsensoren herzustellen.
Gyroskope, die als MEMS-Sensoren hergestellt werden, bestehen meist aus sehr kleinen Massenelementen, die in einer Achse in Schwingung versetzt werden und deren
Bewegung durch Einflüsse der Corioliskraft auf kapazitive Elemente, die in einer
horizontalen Achse angeordnet sind, gemessen wird. Man spricht auch von sogenannten Vibrationskreiseln (engl. Vibrating Structure Gyroscope). Ein Beispiel für
eine Variante eines solchen, des sogenannten Tuning-Fork“-Drehratensensors, ist in
”
Abbildung 2.5 zu sehen.
An den Ausgängen der MEMS-Bausteine liegt dann entweder eine analoge Spannung
an, die äquivalent zur Drehrate ist, oder die Spannung wird intern bereits durch einen
integrierten Analog-Digital-Umsetzer (ADU) digitalisiert. In diesem Fall ist die Auflösung und Qualität des internen Wandlers von entscheidender Bedeutung. Einige
14
2. Grundlagen
Abbildung 2.5: Aufbau und mikroskopische Struktur eines MEMS-Gyroskops (Quelle: [AlAk05])
MEMS-Sensoren haben interne Temperatursensoren, welche die Temperaturabhängigkeit, die besonders bei Gyroskopen eine entscheidende Rolle spielt, ausgleichen
können.
2.3
Weltmodell und Fahrzeugdynamik
In diesem Abschnitt folgt eine kurze Vorstellung der verschiedenen verwendeten
Koordinatensysteme und der Bezugssysteme für das dynamische Modell.
2.3.1
Koordinatensysteme
Die vom GPS gelieferten Koordinaten beziehen sich auf das WGS84-Referenzkoordinatensystem, welches die Erdoberfläche als Ellipsoid mit einer großen Halbachse
1
von etwa 6.380.000 Metern und einer Abplattung von ca. 298
modelliert. Sind diese
Werte gegeben, lassen sich beliebige Koordinaten leicht in ein kartesisches Koordinatensystem mit Ursprung im Erdmittelpunkt umrechnen.
ECEF steht für Earth-Centered Earth-Fixed und ist ein solches Koordinatensystem,
bei dem die Z-Achse zum Nordpol, die X-Achse zum Schnittpunkt von Äquator und
Nullmeridian und die Y-Achse orthogonal dazu in Richtung des Schnittpunkts von
90. östlichem Längengrad und Äquator zeigt.
Vom ECEF-System lässt sich wiederum sehr einfach in ein beliebiges lokales Koordinatensystem umrechnen (siehe Abbildung 2.6). In dieser Arbeit wird das ENUSystem verwendet. Die Buchstaben stehen für East“, North“ und Up“ und be”
”
”
zeichnen die Richtungen von X-, Y- respektive Z-Achse.
2.3.2
Bezugssyteme
Bei der Modellierung der Fahrzeugbewegung spielen verschiedene Bezugssyteme eine
Rolle. Die GPS-Koordinaten beziehen sich auf das globale Bezugssytem der Erde, die
Ausgabe erfolgt üblicherweise in Geokoordinaten oder im ECEF-Koordinatensystem
mit Ursprung am Erdmittelpunkt. Dieses erdfeste Bezugssystem nennen wir auch
earth frame. Durch die Erdrotation dreht es sich um die z-Achse gegenüber den
Fixsternen (und somit gegenüber dem inertial frame).
Die Inertialsensoren im Fahrzeug messen Beschleunigungen und Drehraten stets
relativ zum Inertial-Koordinatensystem, welches seinen Ursprung ebenfalls im Erdmittelpunkt hat, jedoch nicht mit der Erde rotiert, die Drehung der Erde wird daher
2.4. Quaternionen
15
Zecef
North
Up
East
φ
Y ecef
λ
X ecef
Abbildung 2.6: Lokale und Weltkoordinatensysteme
auch von den Drehratensensoren gemessen. Diese Koordinatensystem wird auch als
inertial frame bezeichnet.
Die zurückgelegte Strecke des Fahrzeugs bezieht sich immer auf einen festen Bezugspunkt auf der Erdoberfläche, in der Regel der Startpunkt der Messung. Dieses
Referenzsystem heißt Navigations-Bezugssystem oder auch navigation frame. Hierbei kommt das ENU-Koordinatensystem zum Einsatz.
Der body frame schließlich ist fest an das Fahrzeug gebunden und hat seinen Ursprung üblicherweise im Schwerpunkt des Fahrzeugs. Dabei zeigt die x-Achse nach
vorn, die y-Achse nach links und die z-Achse nach oben (siehe Abbildung 2.7).
Durch Translation und Rotation können wir die Bewegungen aus dem body frame
in das Navigations-Bezugssystem überführen.
2.4
Quaternionen
Die Orientierung im dreidimensionalen Raum kann dargestellt werden als eine Rotation von einem Koordinatensystem in ein anderes. Beispielsweise die Drehung des
fest ans Fahrzeug gebundenen body frame in das Navigations-Bezugssystem (navigation frame). Dabei hat die Darstellung der Rotation als Quaternion gewisse Vorteile
gegenüber einer Rotationsmatrix oder den Euler-Winkeln:
• Die Rotationsachse und -winkel lassen sich leicht bestimmen
• Die Darstellung ist kompakter als bei Rotationsmatrizen (4 statt 9 Elemente)
• Mehrere Rotationen lassen sich durch Multiplikation zu einer verketten
Eine Quaternion wird dargestellt als:
q = q0 + q1 · i + q2 · j + q3 · k
16
2. Grundlagen
RPY-Winkel
& Fahrzeugkoordinaten
z
Up
Gierwinkel
(Yaw) Ψ
z
North
y
y
Rollwinkel
(Roll) Φ
x
+Ψ
East
x
NavigationsKoordinaten (ENU)
Nickwinkel
(Pitch) Θ
Abbildung 2.7: Fahrzeugkoordinaten, Winkel und ENU-Koordinaten (Quelle: bearbeitet von Jonas Hoffmann, Original von Autor: Qniemiec, Wikimedia Commons,
Lizenz: Creative Commons Attribution-ShareAlike 3.0 unported3 )
Dabei sind q0 , q1 , q2 , q3 reelle Zahlen, und die neuen Zahlen i, j, k erweitern den Zahlenraum, ähnlich wie bei komplexen Zahlen. Dabei gilt:
i2 = j2 = k2 = i · j · k = −1
Gegeben eine Rotationsachse v̂ und Winkel σ, ist die entsprechende Quaternion q,
die diese Rotation repräsentiert, bestimmt durch:
q0
q1
q2
q3
= cos(σ/2),
= v̂1 sin(σ/2),
= v̂2 sin(σ/2),
= v̂3 sin(σ/2)
Umgekehrt kann man die Rotationsachse und den Rotationswinkel aus der QuaterT
nion-Darstellung herleiten, dabei sei q̃ = q1 q2 q3 :
σ = 2 arccos(q0 )
q̃
v̂ =
kq̃k
Die Rotation eines dreidimensionalen Vektors x erfolgt als Multiplikation mit vierdimensionalen Quaternionen durch:
0
0
· q−1
=q·
x
x′
3
http://creativecommons.org/licenses/by-sa/3.0/deed.en
2.5. Richtungskosinusmatrix
2.5
17
Richtungskosinusmatrix
Für die aktuelle Orientierungs-Quaternion q = q0 + q1 · i + q2 · j + q3 · k, welche
die Orientierung des Fahrzeugs repräsentiert, ist die Richtungskosinusmatrix Cnb
definiert als:

0, 5 − q22 − q32 q1 q2 − q0 q3
q1 q3 + q0 q2
Cnb = 2  q1 q2 + q0 q3 0, 5 − q12 − q32 q2 q3 − q0 q1 
q1 q3 − q0 q2
q2 q3 + q0 q1 0, 5 − q12 − q22

(2.1)
Sie transformiert einen Vektor aus dem body frame in den navigation frame:
xn = Cnb · b
2.6
Kalman-Filter
Das Kalman-Filter ist eine Menge von mathematischen Gleichungen, die es ermöglichen, das Fortschreiten des unbekannten Zustands eines dynamischen Systems zwischen diskreten Zeitpunkten zu schätzen. Diese Gleichungen wurden nach Rudolf E.
Kálmán benannt, der sie in seinem 1960 veröffentlichten Artikel aufstellte [Ká60].
Es tritt dabei sowohl beim Zustandsübergang, als auch bei den zu einem Zeitpunkt
beobachteten Ausgangsvariablen, den Messungen, Rauschen auf. Das Kalman-Filter
geht dabei von einer probabilistischen Gauß-Verteilung des Rauschens aus.
Vorhersage
Update
Abbildung 2.8: Vorhersage- und Updateschritt beim Kalman-Filter
Die Kenntnis über die Messfunktion und die Zustandsübergangsfunktion ermöglicht
es so, durch iterative Vorhersage- und Updateschritte auf den unbekannten Zustand
zu schließen. Im Updateschritt wird dabei jeweils die gemachte Vorhersage mit den
beobachteten Messwerten verglichen, um damit die Vorhersage zu korrigieren. Es
handelt sich hier also um ein klassisches Prädiktor-Korrektor-Modell.
2.6.1
Lineares Kalman-Filter
In Abbildung 2.9 ist das zugrunde liegende Modell des Kalman-Filters dargestellt.
Matrizen sind dabei als Quadrate dargestellt, Gauß-Verteilungen durch Mittelwert
und Kovarianzmatrix in einer Ellipse. Die restlichen Variablen sind einfache Vektoren.
18
2. Grundlagen
Zeitpunkt
k -1
k
zk
Bekannte
Größen
uk -1
F
B
uk
0, R
0, Q
H
F
B
0, R
0, Q
H
v
w
xk ,Pk
Abbildung 2.9: Systemmodell des Kalman-Filters
Die Gleichungen, die für den linearen Fall den Zustandsraum modellieren, setzen
sich zusammen aus der Übergangsgleichung:
xk = Fk xk−1 + Buk + wk
(2.2)
und der Beobachtungsgleichung:
zk = Hxk + vk
(2.3)
Die beiden Variablen wk und vk sind unabhängige Zufallsvariablen und stehen für
das Zustandsübergangsrauschen respektive das Messrauschen. Diese Zufallsvariablen
seien normalverteilt:
p(w) ∼ N (0, Q)
p(v) ∼ N (0, R)
(2.4)
(2.5)
Q und R sind dabei jeweils die Kovarianzmatrizen der Wahrscheinlichkeitsverteilungen. Im Vorhersageschritt wird aus der Wahrscheinlichkeitsverteilung zum Zeitpunkt
k − 1 der Zustand zum Zeitpunkt k vorhergesagt:
x̄k = Fk x̂k−1 + Buk−1
(2.6)
Fk P̂k−1 FTk
(2.7)
P̄k =
+Q
x̄k ist hier die a-priori-Schätzung der Zustandsvariablen, basierend auf der aktuellen Normalverteilung des Zustands N (x̂k−1 , P̂k−1 ), die wir im vorherigen Schritt
berechnet haben. P̄k ist die Kovarianzmatrix der vorhergesagten Wahrscheinlichkeitsverteilung. F und B stammen aus Gleichung 2.2.
Im Updateschritt wird der sogenannte Kalman-Gain Kk eingeführt, eine Hilfsvariable, die als Gewichtungsfaktor die Differenz zwischen Vorhersage und aktuellem
Zustand minimiert.
Kk = P̄k HT (HP̄k HT + R)−1
x̂k = x̄k + Kk (zk − Hx̄k )
Pk = (I − Kk H)P̄k
(2.8)
(2.9)
(2.10)
2.6. Kalman-Filter
19
Der aktuelle Zustand x̂k ist die Summe aus der Vorhersage x̄k und der Differenz
zwischen Messung zk und dem durch die Beobachtungsfunktion H transformierten
vorhergesagten Zustand x̄k , gewichtet durch den Kalman-Gain. Im letzten Schritt
wird analog die neue (a posteriori) Kovarianzmatrix aus der Differenz der vorhergesagten Kovarianz P̄k und der mit Kk gewichteten, durch die Beobachtungsfunktion
H transformierten Vorhersage gebildet. Die Herleitung lässt sich in [Simo06] nachschlagen.
2.6.2
Erweitertes Kalman-Filter
Oft werden Kalman-Filter dazu benutzt, um Bewegungen von Objekten vorauszusagen. Da es sich hierbei nicht immer um lineare Bewegungen handelt, wurde
das Modell erweitert, um auch nichtlineare aber differenzierbare Übergangsfunktionen zuzulassen. In der Tat handelt es sich in vielen nichttrivialen Anwendungsfällen des Kalman-Filters um nichtlineare, dynamische Prozesse. Die Erweiterung des
Kalman-Filters auf nichtlineare Fälle gelingt durch die stellenweise Linearisierung
um die Mittelwerte der Wahrscheinlichkeitsverteilungen. Um die Übergangsfunktion
stellenweise zu linearisieren, werden die Jacobi-Matrizen, also die partiellen Ableitungen der Übergangsfunktionen, zur jeweils geschätzten Verteilung ausgewertet,
man erhält dadurch eine Annäherung erster Ordnung [WaVDM00].
Eine der Hauptschwächen dieses erweiterten Kalman-Filters ist die Tatsache, dass
die Zufallsvariablen nach Anwenden der nichtlinearen Übergangs- und Messfunktionen keine Normalverteilung mehr aufweisen.
2.6.3
Unscented Kalman-Filter
Die in dieser Arbeit verwendete Variante des Kalman-Filters, das sogenannte Unscented Kalman-Filter (UKF), unterscheidet sich in einigen Aspekten vom klassischen Modell. Es basiert auf der Annahme, dass es leichter ist, eine Zufallsverteilung zu approximieren als eine beliebige nichtlineare Funktion [JUIJC04]. Das UKF
verwendet dazu eine minimale Anzahl an speziell ausgewählten Testpunkten, die
um den Mittelwert der Wahrscheinlichkeitsverteilung liegen. Auf diese sogenannten Sigmapunkte wird die nichtlineare Übergangsfunktion angewendet und aus den
transformierten Punkten der neue Mittelwert und die Kovarianz berechnet.
Dieses Verfahren wird Unscented Transformation genannt. Eine n-dimensionale Zustandsvariable x ∈ Rn mit der Kovarianz Px und dem Mittelwert x̄ wird dabei durch
die folgenden 2n+1 Sigmapunkte xi und die zugehörigen Gewichte Wi approximiert:
x0 = x̄
p
(n + k)Px )i
p
xi = x̄ − ( (n + k)Px )i−n
xi = x̄ + (
für i = 1, . . . , n
(2.11)
für i = n + 1, . . . , 2n
√
Hierbei bezeichnet ( A)i die i-te Zeile der Quadratwurzel4 aus der Matrix A.
4
Die Quadratwurzel B aus einer Matrix A ist genau die Matrix, für welche gilt: A = BB.
20
2. Grundlagen
Messdaten
Linearisiert (EKF)
Sigma−Point (UKF)
Sigmapunkte
Kovarianz
Mittelwert
´
Ý
Ý
y = g(x)
´
Ü
´
Ü
gewichteter Mittelwert
und Kovarianz
Ì
È
Ö
È
Ý
´
Ö
µ
Ü
µ
´
Ü
transformierte
Sigmapunkte
µ
wahrer Mittelwert
wahre Kovarianz
µ
µ
SP Mittelwert
Ì
Ö
È
´
Ö
µ
Ü
SP Kovarianz
Abbildung 2.10: Funktionsprinzip des UKF (aus [VDMWa04])
W0m = λ/(n + λ)
W0c = W0m + (1 − α2 + β)
Wic = Wim = 1/2(n + λ) i = 1, . . . , 2n
(2.12)
α legt die Entfernung der Sigmapunkte um den Mittelwert x̄ fest. λ = α2 (n + k) − n
ist ein Skalierungsfaktor, wobei meist k = 0 gesetzt wird. β kann variiert werden,
um möglicherweise bereits bekannte Abweichungen der Wahrscheinlichkeitsverteilung von einer Normalverteilung einfließen zu lassen. Für Gauß-Verteilungen ist
β = 2 optimal [WaVDM00].
Mit dem UKF kann man die Schwächen des erweiterten Kalman-Filters umgehen und erhält eine Annäherung dritter Ordnung anstatt erster Ordnung, während man auf die Linearisierung durch Berechnung der Jacobi-Matrixen beim EKF
verzichten kann, die bei komplexen Funktionen unter Umständen sehr aufwendig
ist[WaVDM00]. Außerdem ist es möglich, dass die Fortpflanzung des Fehlers bei einem nichtlinearen System sich ebenfalls nicht durch lineare Funktionen beschreiben
lässt. In diesem Fall kann es sein, dass die geschätzten Werte beim EKF divergieren [JUIJC04]. [JuUh97] zeigt, dass diese Annäherung dem EKF auch qualitativ
überlegen ist.
Eine Möglichkeit, das Unscented Kalman-Filter zu implementieren, ist es, den Systemzustandsraum um die Rauschkomponenten zu erweitern.
x′ k = xTk wkT vkT
(2.13)
Ebenso wird die Prozesskovarianzmatrix um die Kovarianzen der Rauschkomponenten erweitert.


P 0 0
P′ k =  0 Q 0 
0 0 R
(2.14)
2.6. Kalman-Filter
2.6.3.1
21
Vorhersage
Die nichtlineare Zustandsübergangsfunktion y = g(x) wird im Vorhersageschritt auf
die Sigmapunkte aus Gleichung 2.12 angewandt und diese werden entsprechend gewichtet. Daraus werden der neue Mittelwert x̄ und die Kovarianzmatrix P̄ berechnet.
x̄ =
P̄ =
2n
X
i=0
2n
X
i=0
2.6.3.2
Wim g(xi )
(2.15)
Wic {g(xi ) − x̄}{g(xi ) − x̄}T
(2.16)
Update
Für den Updateschritt wird analog die nichtlineare Messfunktion z = h(x) auf die
Sigmapunkte angewandt und auch hier der Mittelwert z̄ und die Messungskovarianzmatrix P̄zz gebildet.
z̄ =
2n
X
Wim h(xi )
(2.17)
P̄zz =
2n
X
Wic {h(xi ) − z̄}{h(xi ) − z̄}T
(2.18)
i=0
i=0
Zusätzlich wird noch eine Kreuzkorrelationsmatrix aus Zustand und Messung gebildet, die benutzt wird, um den Kalman-Gain K zu bestimmen.
P̄xz =
2n
X
i=0
Wic {g(xi ) − x̄}{h(xi ) − z̄}T
K = Pxz P−1
zz
(2.19)
(2.20)
Der Rest ist analog zum klassischen Kalman-Filter. Zustand und Kovarianz werden
jeweils, gewichtet durch den Kalman-Gain, basierend auf der Differenz von Vorhersage und Messung, korrigiert.
x̂ = x̄ + K(z − z̄)
P = P̄ − KPzz K
T
(2.21)
(2.22)
22
2. Grundlagen
3. Analyse
In diesem Kapitel wird die zu lösende Aufgabe noch einmal genauer beschrieben
und es wird auf die Anforderungen und Randbedingungen der Problemstellung eingegangen.
Im zweiten Abschnitt werden Arbeiten vorgestellt, die sich mit einer ähnlichen Thematik beschäftigt haben, und die dabei geltenden Randbedingungen, verwendete
Methoden und erreichte Ergebnisse diskutiert.
3.1
Problemstellung, Anforderungen und Randbedingungen
Wie bereits in Kapitel 1.2 kurz erläutert, bestand die Aufgabe darin, aus den
Sensoren- und GPS-Daten der im Fahrzeug installierten IMU6-Platine möglichst
genau die Pose zu bestimmen. Das heißt, es sollen Position, Lage im Raum und die
Geschwindigkeiten in die verschiedenen Richtungen bestimmt werden.
Dem Kooperationspartner Hella Aglaia war es dabei besonders wichtig, dass die
Längsgeschwindigkeit (Vorwärtsgeschwindigkeit) und die Gierrate bzw. Lenkwinkeländerung bestimmt werden. Diese Größen sollten als Eingabe für das zur Verfügung gestellte Spurhaltesystem (LDW) dienen.
Folgende Werte wurden seitens Hella Aglaia für die gewünschte Genauigkeit spezifiziert:
• Gierrate
0,1 Grad/s Auflösung
maximaler Fehler von 0,3 Grad/s bei 0 Grad/s tatsächlicher Bewegung,
linear steigend auf 0,5 Grad/s bei 3 Grad/s
• Geschwindigkeit
±3 km/h maximaler Fehler
24
3. Analyse
Die GPS-Antenne sollte dabei letztlich im Innenraum hinter der Windschutzscheibe
zum Einsatz kommen. Für die Tests wurde die Seitenscheibe des Spirit of Berlin
gewählt, da die Frontscheibe wegen ihrer Bedampfung die Signale stark abschwächt.
Das System sollte außerdem im Idealfall in der Lage sein, kurze GPS-Ausfälle (bis
etwa 30 Sekunden) zu überbrücken und dabei die geforderte Messgenauigkeit einzuhalten.
Zusätzlich zu den Anforderungen der Kooperationsfirma gab es eigene Anforderungen innerhalb des Projekts.
• Integration der entwickelten Software in das vorhandene, auf OROCOS basierende System
Entwicklung eines Input-/Sensormoduls für die IMU6-Platine
Entwicklung eines Kalman-Filter-Moduls
• Evaluation des verbauten GPS-Moduls und Vergleich mit dem Referenzsystem
• Evaluation der Nutzbarkeit für die Lokalisierung unabhängig vom Spurhaltesystem
3.2
3.2.1
Existierende Lösungsansätze
Existierende Arbeiten
Van der Merwe und Wan stellen in ihrem Artikel Sigma-Point Kalman Filters for
Integrated Navigation [VDMWa04] eine Möglichkeit vor, GPS- und IMU-Daten zu
kombinieren. Dabei wird eine Variante eines Unscented Kalman-Filters verwendet,
das sogenannte Sigma-Point-Kalman-Filter (SPKF). Für den verwendeten Algorithmus werden zwei mögliche Implementationen angeführt, das square-root UKF
(SRUKF) und das square-root central difference Kalman filter (SRCDKF), die sich
lediglich in der Art und Weise unterscheiden, wie die Sigmapunkte ausgewählt werden.
Als Versuchsträger kam eine Experimentierplattform auf Basis eines unbemannten
Modellhelikopters zum Einsatz, der mit einer IMU, einem 10Hz-GPS, einem barometrischen Höhenmesser und einem Embedded PC ausgestattet war.
Diese Arbeit wird hier exemplarisch angeführt, weil die anderen Autoren ähnliche
Modelle und Filter benutzten. Auf die speziellen Unterschiede in weiteren Arbeiten
wird am Ende dieses Kapitels eingegangen.
3.2.2
Zustandsraum und dynamisches Modell
Es wurde von Van der Merwe und Wan ein 16-dimensionaler Zustandsvektor benutzt, bestehend aus 3D-Position, 3D-Geschwindigkeit, 4D-Lage-Quaternion, 3DBeschleunigungsfehler und 3D-Drehratenfehler. Der Index b bei den Fehlergrößen
steht dabei für Bias.
x = pT vT qT aTb ωbT
(3.1)
3.2. Existierende Lösungsansätze
3.2.2.1
25
Übergangsfunktion
Zunächst müssen die Messungen (Beschleunigungen und Drehraten) um Rauschen
und Fehler korrigiert werden:
ā = ã − ab − wa
(3.2)
ω̄ = ω̃ − ωb − Cbn ωc − wω
(3.3)
Die Matrix Cbn = (Cnb )T ist die transponierte Richtungskosinusmatrix aus Kapitel 2.5
und transformiert einen Vektor aus dem Navigation-Frame in den Body-Frame. ωc ist
hier die durch den Coriolis-Effekt gemessene Drehrate der Erde. Sie ist eigentlich eine
Funktion des aktuellen Breitengrads, kann aber auf kurzen Strecken ohne weiteres
als konstant angenommen werden.
Position und Geschwindigkeit lassen sich durch die folgenden Gleichungen in den
Folgezustand transformieren:
pt = pt−1 + ṗt−1 · dt
vt = vt−1 + v̇t−1 · dt
(3.4)
(3.5)
und haben die Ableitungen:
ṗ = v
v̇ = Cnb ā + 0 0 g
(3.6)
T
(3.7)
g ≈ 9, 81 m/s2 ist dabei der skalare Wert der lokalen Gravitation.
Auch die Fehlervektoren ab und ωb werden nach diesem Schema transformiert, die
Ableitung ist dabei eine Gauß’sche Zufallsvariable.
Die Winkeldifferenzen für Roll-, Nick- und Gierwinkel sind ein Produkt aus der
korrigierten Drehrate ω̄ und der vergangenen Zeit dt:
∆φ = ω̄roll · dt
∆θ = ω̄pitch · dt
∆ψ = ω̄yaw · dt
(3.8)
(3.9)
(3.10)
Das Quaternion-Update geschieht folgendermaßen:
1
sin(s)
qt = I(cos(s)) − Φ∆
qt−1
2
s
1p
(∆φ)2 + (∆θ)2 + (∆ψ)2
s=
2
(3.11)
(3.12)
26
3. Analyse
wobei die schiefsymmetrische Matrix Φ∆ definiert ist als:

0
∆φ
∆θ
∆ψ
 −∆φ
0
−∆ψ ∆θ 

Φ∆ = 
 −∆θ ∆ψ
0
−∆φ
−∆ψ −∆θ ∆φ
0

3.2.2.2
(3.13)
Beobachtungsfunktion
Bei der Beobachtung fließen die Messungen des GPS-Sensors und des Höhenmessers
in die Korrektur des Filters ein. Die Gleichungen für die Beobachtungsfunktion sind:
S
pGP
= pt−k + Cnb r̃gps + wp
t
(3.14)
vtGP S
(3.15)
= vt−k +
Cnb ωt−k
× r̃gps + wv
pt−k , vt−k bezeichnen die Position und Geschwindigkeit vor k Schritten, gemessen
von der um k Zeiteinheiten zeitlich verschobenen IMU-Messung. Diese zeitliche Verschiebung um k Samples kann bei GPS-Empfängern mit geringer Latenz ignoriert
werden.
r̃gps ist die Position der GPS-Antenne relativ zu den Inertialsensoren, muss also
auf die Position addiert werden. wp und wv sind wiederum normalverteilte Zufallsvariablen, die das Messrauschen repräsentieren.
3.2.3
Ergebnisse und Vergleich mit weiteren Arbeiten
Van Der Merwe und Wan haben in ihren Experimenten deutliche Vorteile der verwendeten Lösung im Vergleich zum EKF festgestellt. Der verwendete SPKF mit
Ausgleich der GPS-Verzögerung zeigte eine Verringerung des Fehlers von über 30%
bei Position, Geschwindigkeit, Rollwinkel und Nickwinkel und eine 65%-ige Verbesserung beim Gierwinkel.
Zhang, Gu, Milios und Huynh zeigen in [ZGMH05] ebenfalls eine Integration von
IMU und GPS. Es kommt derselbe 16-dimensionale Zustandsvektor zum Einsatz
(Gleichung 3.1). Hier wurde als Versuchsträger ein PKW verwendet, der mit einem 1 Hz GPS-Empfänger, mit 20 Hz getakteter IMU und digitalem Kompass
ausgestattet war. Die Sensordaten wurden hier mit einem Wavelet-Filter vorverarbeitet. Die Autoren betonen dabei, dass die IMU nur sehr eingeschränkt Ausfälle
des GPS verkraften kann, da durch die verschiedenen Fehlereinflüsse und das Rauschen der Inertialsensoren die Richtung und Position sehr schnell von der Realität
abweichen. Es wurde jedoch auf einer kurzen Strecke auf einem Parkplatz mit GPSEmpfang ein zufriedenstellendes Ergebnis erzielt, quantifiziert wurde der Fehler,
vermutlich wegen fehlender Referenz, jedoch nicht.
In Kapitel 10 von [Wend07] findet sich ebenfalls eine Anwendung für eine Integration von IMU und GPS. Bei dem Versuchsträger handelt es sich um ein unbemanntes
Flugobjekt (UAV) mit 4 Rotoren, von denen jeweils zwei auf orthogonal zueinander stehenden Achsen angebracht sind. Diese Flugobjekte sind auch bekannt als
3.3. Schlussfolgerungen
27
Quadrokopter. Interessant an diesem Anwendungsfall ist die Umschaltung bei einem GPS-Ausfall auf ein zweites Filter, welches Position und Lage ohne die externe
Quelle des GPS schätzt. Ist der GPS-Empfang vorhanden, so werden die Sensoren
kontinuierlich kalibriert. Das Modell wurde sowohl simuliert, als auch einem praktischen Test unterzogen. Hierbei wurde eine sehr gute Lageschätzung erreicht, sofern
entweder GPS-Empfang gegeben war oder die durch die Integration der Beschleunigungsmesser entstandenen Fehler in der Lage durch Messen des Gravitationsvektors
und des Erdmagnetfelds ausgeglichen wurden.
Auch El-Sheimy und Abdel-Hamid betonen in ihrem Artikel [AHAESL06] die Problematik bei der Zustandsschätzung im Falle von GPS-Ausfällen. Das Kalman-Filter
arbeitet dabei nur im Vorhersagemodus, es gibt kein Update durch externe Messungen (GPS). Die Autoren stellen im Artikel ein Fuzzy Inference System (FIS) vor,
welches die Vorhersagen des Kalman-Filters korrigiert. Das FIS ist ein mehrschichtiges Netzwerk und basiert auf gelernten Regeln und Gewichten.
3.3
Schlussfolgerungen
Mit dem Thema der Integration von GPS und Inertialsensoren haben sich bereits einige Autoren beschäftigt, meist kam in den untersuchten Arbeiten auch ein
Kalman-Filter zum Einsatz. Die gestellte Aufgabe läuft darauf hinaus, ein KalmanFilter zu entwickeln, welches idealerweise in der Lage ist, die GPS-Position zu verbessern und Ausfälle zu kompensieren. Die dabei erreichten Ergebnisse variieren, lassen
jedoch hoffen, die gestellten Anforderungen zu erfüllen, sofern die Inertialsensoren
nicht zu stark driften und das entwickelte dynamische Modell die Realität gut widerspiegelt. Für eine direkte Umsetzung eignet sich keine der untersuchten Arbeiten,
aufgrund der unterschiedlichen Randbedingungen oder der verwendeten Methoden.
28
3. Analyse
4. Hardware
In diesem Kapitel wird die verwendete Hardwareplattform IMU6 vorgestellt und
deren Bestandteile und schematischer Aufbau dargelegt. Danach wird die Integration
in das autonome Fahrzeug Spirit of Berlin beschrieben.
4.1
Die Hardwareplattform IMU6
Die IMU6 (Inertial Motion Unit 6) ist eine von der Firma Bluetechnix im Auftrag
von Analog Devices hergestellte Entwicklungsplatine, die bereits in verschiedenen
Forschungsprojekten im Bereich Navigation zum Einsatz kam.
Sie besteht aus einer Platine mit den Abmessungen 95x55 mm, auf der ein CPUModul, ein GPS-Modul, ein digitaler Kompass, ein 3-Achsen-Beschleunigungsmesser
und 2 Gyroskope (Z-Achse und kombinierte X-/Y-Achse) verbaut sind.
Die Spannungsversorgung für das Board erfolgt über eine 2-polige Stiftleiste mit
5 Volt Gleichspannung. Auf dem Board befinden sich weitere Spannungsregler, um
die einzelnen Komponenten jeweils mit den erforderlichen Spannungen zu versorgen.
4.1.1
Technische Daten
• Prozessormodul TCM-BF537
Analog Devices Blackfin Prozessor ADSP-BF537, 500MHz
32 MB SD-RAM
8 MB Flash-Speicher
Echtzeituhr (RTC)
Spannungsregelung für Kernspannung
Schnittstellen: SPI, PPI, CAN, TWI, JTAG, Ethernet, SPORT, UART
• GPS-Modul
u-blox LEA-5T (50 Kanal GPS-/GALILEO-Empfänger)
Ausgabe im Format NMEA (ASCII) und UBX (binär)
Erreichbare horizontale Genauigkeit: <2,5 m (laut Hersteller)
30
4. Hardware
J TAGPort
Spannungsversorgung
GPSAntennenanschluss
CPU-Modul ResetTaster
UARTPort
Modulverbinder
SD-CardSlot
Z-AchsenGyroskop
I OPorts
Abbildung 4.1: Oberseite der IMU6 Platine mit Beschriftung der Komponenten. Auf
der Unterseite befinden sich weitere Sensoren und das GPS-Modul (Foto: Bluetechnix Mechatronische Systeme GmbH)
• Beschleunigungssensor
ADXL340
Hersteller: Analog Devices
3-Achsen: X, Y, Z
Auflösung: 8-Bit
Messbereich: wahlweise ± 2 g oder ± 8 g
Empfindlichkeit: 0,015625 g/LSB
• Gyroskope
ADIS16100
Hersteller: Analog Devices
Empfindlichkeit: 0,2439 Grad/LSB
1-Achse: Z (Gierrate)
Auflösung: 12-Bit für interne und externe Spannungen
Temperatursensor
zwei Eingänge für analoge Spannungen der X/Y-Achse
IDG-300
Hersteller: Invensense
2-Achsen: X,Y (Roll-, Nickrate)
analoge Spannungsausgänge
• Magnetometer
PNI11096 (PNI Corp.)
4.1. Die Hardwareplattform IMU6
31
• Schnittstellen
TTL-Level UART-Port für serielle Kommunikation als 4-pol. Pin-Header
SD-Card-Slot
diverse IO-Anschlüsse auf Micromatch-Steckverbindern
RP-SMA-Antennenanschluss für das GPS-Modul
• weitere Komponenten
drei LEDs: rot, orange, grün
Reset-Taster
4.1.2
Aufbau und Funktionsweise
Wie in Abbildung 4.2 erkennbar, sind die Inertialsensoren über einen SPI-Bus
(Serial Programming Interface) und das GPS-Modul über die zweite UART-Schnittstelle an das CPU-Modul angebunden.
Die analogen Signale des kombinierten X-/Y-Gyroskop IDG-300 sind an die AnalogDigital-Umsetzer-Eingänge des Z-Achsen-Gyroskop ADIS16100 geführt und werden
von diesem mit 12-Bit-Auflösung digitalisiert.
Es befindet sich eine JTAG-Schnittstelle zur Programmierung und zum Debugging
der Software auf der Platine. Zu diesem Zweck wurde ein USB-zu-JTAG-Debugger
(ADZS-HPUSB-ICE) mitgeliefert.
Gyro
X, Y
IDG-300
X (analog)
Magnetometer
Accelerometer
X, Y, Z
Gyro Z
& ADC
PNI11096
ADXL340
ADIS16100
ISP Bus
Y (analog)
ISP Bus
RESET
TCM-BF537
CPUModul
GPSModul
UART 1
UART 0
Abbildung 4.2: IMU6-Aufbau im Schema
Relativ früh im Verlauf des Projekts wurde ein Fehler im Schaltplan deutlich. Die
RESET-Leitung ist sowohl mit dem GPS-Modul als auch mit dem Magnetometer verbunden. Das führte dazu, dass das notwendige Schalten auf Massepotential
der RESET-Leitung zum Auslesen des Sensors (etwa 100 Mal pro Sekunde) einen
Neustart des GPS-Moduls bewirkte. Dadurch war das GPS-Modul beim simultanen
32
4. Hardware
Auslesen des Sensors nicht nutzbar. Dieser Designfehler konnte jedoch am fertigen
Modul behoben werden, indem die Verbindung der RESET-Leitung zum GPS-Modul
getrennt wurde.
4.2
Versuchsaufbau im autonomen Fahrzeug
Zunächst wurde die IMU6 allein auf diversen Testfahrten ausgiebig erprobt. Dazu wurde meist ein Notebook verwendet, welches über den USB-Port sowohl die
5-Volt-Versorgungsspannung für die Platine lieferte, als auch die Daten mitloggen
konnte. Für diesen Anschluss wurde vom Verfasser dieser Arbeit ein Adapterkabel
zur Stromversorgung hergestellt. Außerdem wurde ein Wandler vom TTL-Logiklevel
des Boards auf eine RS-232-kompatible serielle Schnittstelle aus einem Bausatz zusammengebaut und in Kombination mit einem weiteren Wandler von der RS-232Schnittstelle auf einen USB-Anschluss eingesetzt. Diese Vorarbeiten waren nötig,
weil mit der Entwicklungsplatine keinerlei Anschlusskabel mitgeliefert wurden.
4.2.1
Einbau und Anschluss an das vorhandene System
IMU 6 Board
GPS
Inertialsensoren
UART (TTL)
TTL<-> RS-232
Adapter
RS-232
(Serial Port)
Comtrol Devicemaster
RS-232 <-> Ethernet Server
Ethernet
Cassandra
LDW
Velocity Input
Yaw Rate Input
Orocos RTT
BtImu6 Modul
HellaLaneDetection
Modul
Abbildung 4.3: Schema des Datenflusses zwischen den Komponenten im Fahrzeug
Um die Platine mit dem Referenzsystems im Fahrzeug vergleichen zu können, wurde
diese in den Kofferraum des Spirit of Berlin eingebaut.
Um Störeinflüsse der umfangreichen Elektronik im autonomen Fahrzeug auf die empfindlichen Sensoren der IMU auszuschließen bzw. zu minimieren, wurde ein Trägergehäuse für die Platine aus Aluminium mit Aussparungen für die Kabel konstruiert.
Auch in diesem Aufbau wurden zunächst Testfahrten ohne Anschluss an das im
Fahrzeug laufende OROCOS-System durchgeführt.
Um letztendlich die Anbindung zu bewerkstelligen, wurde anstelle des vorher benutzten RS-232-zu-USB-Wandlers der serielle Ausgang der IMU6 an einen RS-232Ethernet-Gateway (Comtrol Device-Master ) angeschlossen, der die seriellen Daten
über einen TCP-Port zur Verfügung stellt. Die Magnetantenne für das GPS wurde
auf dem Dach des Spirit of Berlin befestigt.
Der schematische Aufbau und Datenfluss ist in Abbildung 4.3 zu sehen. Im OROCOS-Modul werden die Daten vom TCP-Port eingelesen und kontinuierlich weiterverarbeitet (siehe Abschnitt 5.2).
4.3. Schlussfolgerungen
33
Die Ausgabe der aktuellen Geschwindigkeit und Gierrate für das LDW-System
von Hella Aglaia erfolgt wiederum über einen TCP-Port. Die Software des LDWSystems (Cassandra) läuft auf einem weiteren Rechner, der über Ethernet mit dem
OROCOS-Rechner verbunden ist.
4.3
Schlussfolgerungen
Die in dieser Arbeit genutzte Hardwareplattform IMU6 lässt sich über den JTAGPort frei programmieren und ermöglicht es so, die integrierten Sensoren auszulesen
und die gewonnenen Daten über einen seriellen Port auszugeben.
Die verbaute CPU ist mit dieser Aufgabe keinesfalls ausgelastet und es wäre sogar
denkbar, das in dieser Arbeit implementierte Filter auf diese Hardwareplattform
zu portieren, wenn man auf effiziente Implementierungen der benötigten mathematischen Operationen zurückgreift und eventuell Abstriche bei der Frequenz der
Sensormessungen in Kauf nimmt.
34
4. Hardware
5. Software
Die im Rahmen dieser Arbeit geschriebene Software teilt sich auf in die Firmware
für die IMU6-Plattform und die Software-Module für das Orocos-Framework des
autonomen Fahrzeugs Spirit of Berlin.
Die Firmware für den Mikroprozessor ADSP-BF537 wurde in C geschrieben und
mit der Visual DSP IDE von Analog Devices entwickelt.
Die OROCOS-Module wurden in C++ implementiert.
5.1
5.1.1
Firmware der Hardwareplattform
Aufbau und Funktionsweise
Der Aufbau des Programms lässt sich durch einen Zustandsautomaten beschreiben,
der im Ausgabemodus die Sensoren auf der Plattform nacheinander ausliest. Dieses ist der Hauptarbeitszustand des Automaten. Die ausgelesenen Daten werden
in Nachrichten verpackt und mit einem Zeitstempel versehen. Zusätzlich wird eine
Prüfsumme berechnet und ans Ende der Nachricht angehängt.
Für die Abfrage der Daten über den SPI-Bus agiert die CPU als sogenanntes SPIMaster-Device, die Sensoren sind SPI-Slaves. Die Datenkommunikation wird dabei
durch periodisches Umschalten der SCK -Leitung durch das Master Device getaktet,
über die MOSI (Master Out Slave In)-Leitung werden Daten oder Befehle gesendet
und über die MISO (Master In Slave Out)-Leitung empfangen. Ein grundlegender
Treiber für die SPI-Kommunikation wurde mit der Hardware mitgeliefert, es waren
jedoch einige Änderungen nötig, um alle Informationen der Sensoren zuverlässig mit
hoher Datenrate abzufragen.
Die Initialisierung wurde in der Art implementiert, dass im Startzustand auf einen
Satellitenfix des GPS-Moduls gewartet wird und dann, sofern dieser vorhanden ist,
ein interner Timer auf die exakten Zeitpulse des GPS-Moduls synchronisiert wird.
Das erleichtert das spätere Auswerten der Logfiles und ermöglicht es zudem, die
Daten des Referenzsystems genau mit den aufgezeichneten Logdaten zu vergleichen.
36
5. Software
(no timepulse)
(no fix)
Synchronisieren
(fix)
(timepulse)
Warte auf
GPS-Fix
Ausgabemodus
"exit"
"command"
"command"
"toggle
SENS"
Befehsmoduis
"toggle
GPS"
"pass"
"cal"
"exit"
Kalibrieren
PassThrough
Abbildung 5.1: Zustände des in der Firmware implementierten Automaten
Im laufenden Programm kann durch einen Befehl in einen Kommandomodus gewechselt werden, in dem weitere Funktionen zur Verfügung stehen. Es kann die
Ausgabe der GPS- und Sensor-Nachrichten einzeln ausgestellt werden und in einen
Kalibiriermodus gewechselt werden, der die festen Offsets der Sensoren bestimmen
und speichern kann. In einem weiteren Modus werden alle Nachrichten direkt zum
GPS-Modul an der zweiten seriellen Schnittstelle weitergeleitet und alle von dort
kommenden Nachrichten direkt ausgegeben. Dieser Modus ermöglicht die Konfiguration des GPS-Moduls über die vom Hersteller mitgelieferte Software.
Das fertige Programm ist 145 KB groß und kann entweder direkt über JTAG in den
Flash-Speicher geschrieben oder über einen auf dem System installierten Bootloader
in den Arbeitsspeicher geladen und von dort aus gestartet werden. Die zweite Methode hat den Vorteil, dass die begrenzte Lebensdauer des Flash-Speichers dadurch
nicht reduziert wird.
5.1.2
Nachrichtenformat
Das Format der Nachrichten ist dabei ähnlich aufgebaut wie das von GPSEmpfängern bekannte NMEA-Nachrichtenformat. Nachrichten fangen dabei immer
mit einem $-Zeichen an, gefolgt von einem Nachrichtentyp-Bezeichner, es folgen beliebig viele durch Kommata separierte Nachrichtenfelder und am Ende der Nachricht
ein ∗-Zeichen, gefolgt von der Prüfsumme.
Es wurde bewusst ein Klartextformat (ASCII-Zeichensatz) verwendet, um den Datenstrom direkt mitlesen und verifizieren zu können und um es zu ermöglichen, die
GPS-Daten auch ohne Konvertierungsprogramm direkt in Standardsoftware zu verarbeiten. Das Format sieht folgendermaßen aus:
[HH:MM:SS.FFF TTT] <TYPE>: $<MSGTYPE>,<FIELD>,....*<CHKSUM>
Abbildung 5.2: Aufbau des Nachrichtenformats
5.2. Kalman-Filter, Orocos-Modul
[15:06:51.892
[15:06:51.897
[15:06:51.901
[15:06:52.096
7083692]
7083697]
7083701]
7083874]
37
SENS: $SENS,23.199498,57.997995,-22.682700,0.078125,0.000000,-1.078125,14,14,0.00000,37.93*3A
GPS: $GPGLL,5233.62154,N,01326.01603,E,130650.00,A,A*69
SENS: $SENS,23.199498,57.997995,-22.194900,0.078125,0.000000,-1.062500,12,8,0.00000,37.64*2E
GPS: $UBX0103,90,b0,43,12,03,dd,00,00,d8,39,32,00,12,fd,78,00*53,9e
Abbildung 5.3: Auszug aus den vom IMU6-Modul übertragenen Nachrichten
Dabei stehen HH, MM, SS, FF für die aktuelle Zeit in Stunden, Minuten, Sekunden,
Millisekunden. TTT steht für die vergangenen Millisekunden seit Start des Moduls.
Die Nachrichten werden mit einer Bitrate von 115.200 Bit/s, 8 Datenbits, einem
Stoppbit und ohne Parität über die serielle Schnittstelle ausgegeben. In Abbildung 5.3 ist ein Auszug der Nachrichten zu sehen.
5.2
5.2.1
Kalman-Filter, Orocos-Modul
Bestandteile
Die Softwaremodule in der OROCOS-Software bestehen aus einem Ein-/Ausgabemodul, welches Nachrichten empfängt, verarbeitet und ausgibt, und dem Kalman-Filter-Modul, welches die reine Funktion des Filterns übernimmt und damit
unabhängig vom verwendeten Modell ist. Somit kann es auch wiederverwendet und
von anderen Modulen genutzt werden. Beispielweise ist der Einsatz des Filters zum
Lernen der Gewichte eines neuronalen Netzes denkbar.
Das Kalman-Filter ist als Template implementiert, welches sich durch Angeben einer Übergangsfunktion, einer Beobachtungsfunktion und den Standardparametern
instanziieren lässt.
Die Funktionen für Zustandsübergang und Beobachtung (Messung) beschreiben
schließlich das dynamische System. Sie wurden speziell für den vorliegenden Anwendungsfall der Inertialeinheit mit GPS in einem PKW implementiert.
5.2.2
Ein-/Ausgabemodul
Das Inputmodul hat die folgenden Aufgaben:
• Empfang der Nachrichten von einem TCP-Port
• Parsen der Nachrichten
• Verifizieren der Prüfsumme
• Aufruf der Vorhersage- und Update-Funktionen des Filters
• Bereitstellen der Daten über einen Ausgabe-Port in einem zum restlichen Projekt kompatiblen Format (EgoState-Port)
Der Parser arbeitet zeichenorientiert, und kann als Zustandsautomat angesehen werden, der solange Zeichen einliest, bis entweder ein Sonderzeichen (Feldtrenner, Ende/Beginn einer Nachricht, Zeilenumbruch,...) auftritt oder die maximale Anzahl an
Zeichen überschritten wird. Wenn das passiert, oder wenn die Prüfsumme nicht verifiziert werden kann, wird die Nachricht verworfen. Als Erstes wird dabei anhand des
38
5. Software
ersten Feldes der Typ der Nachricht bestimmt und dann die relevanten Felder der
Nachricht in Variablen gespeichert. Tritt irgendwo im Datenstrom ein unerwartetes
Zeichen auf, wird der bisher gelesene Teil der Nachricht ebenfalls verworfen und in
den Anfangszustand zurückgekehrt.
Wenn die Nachricht korrekt eingelesen wurde und die nötigen Daten extrahiert wurden, kann die Funktion des Filters mit den entsprechenden Eingabedaten aufgerufen
werden.
Zuletzt wird der Zustand des Filters ausgelesen und in den Ausgabeport (EgoState)
geschrieben.
5.2.3
Kalman-Filter-Modul
Das Kalman-Filter-Modul implementiert ein Unscented Kalman-Filter ähnlich zu
dem in Abschnitt 2.6.3 beschriebenen. Der Übergang läuft dabei analog zur Beobachtung ab.
Der implementierte UKF orientiert sich am Central-Difference-Kalman-Filter
(CDKF), Hauptunterschied zu dem in Abschnitt 2.6.3 vorgestellten UKF ist, dass
der Systemzustand für beide Schritte des Filters unterschiedlich erweitert wird. Für
die Vorhersage wird der Zustand x mit dem Übergangsrauschen w und für die Beobachtung mit dem Messrauschen v erweitert (siehe Gleichung 5.1). Ebenso werden die
Kovarianzmatrizen P jeweils einzeln um die Fehlerkovarianzmatrizen Q, R erweitert
(siehe Gleichung 5.2).
xw = xT w T
Pw =
T
xv = xT v T
PT 0
0 QT
Pv =
T
PT 0
0 RT
(5.1)
(5.2)
Das macht es erforderlich, die Sigmapunkte und Gewichte jeweils einmal für die
beiden Schritte zu berechnen. Die Berechnung erfolgt dabei aber wie in den Gleichungen 2.12 und 2.13 angegeben. Auch die Vorhersage und der Updateschritt sind
analog zum Algorithmus aus Kapitel 2.6.3 implementiert.
Der komplette Pseudocode des Filters findet sich in Anhang A.
Die Messungen der Inertialsensoren dienen dabei der Vorhersage des Zustands
und werden somit in der Übergangsfunktion benutzt. Die externe Positionsbestimmung durch GPS wird dann jeweils im Updateschritt genutzt, um die Vorhersage
zu korrigieren.
Weil das GPS mit einer Frequenz von 1 Hz und die Inertialsensoren mit 100 Hz
messen, werden jeweils 100 Vorhersagen zwischen den Updateschritten getroffen.
5.2.4
Übergangsfunktion
Um die Zustandsvektoren und Kovarianzmatrizen klein zu halten, wurde nach dem
anfänglichen Experimentieren mit größeren Zuständen ein vereinfachter Zustandsvektor entworfen, der aus den folgenden 8 Dimensionen besteht:
5.2. Kalman-Filter, Orocos-Modul
39
• 2D-Position im EN(U)-Koordinatensystem p
• 1D-Vorwärtsgeschwindigkeit v
• 4D-Lage-Quaternion q
• 1D-Vorwärtsbeschleunigungsfehler baccx
x = pT v qT baccx
T
(5.3)
Die dritte (Hoch-)Koordinate im ENU-Koordinatensystem soll hier außer Acht gelassen werden, da in erster Linie die Verschiebung in lateraler und longitudinaler
Richtung interessiert und außerdem angenommen werden kann, dass sich das straßengebundene Fahrzeug vorwiegend in dieser Ebene bewegt. Es wird also von einer
Ebene für die möglichen Koordinaten des Fahrzeugs ausgegangen.
Bewusst wurde dabei auch auf das Einbeziehen der Höhe und der Geschwindigkeiten
in der seitlichen Y-Achse und der nach oben zeigenden Z-Achse verzichtet, weil die
Bestimmung dieser aufgrund einiger Schwierigkeiten mit den X- und Y-Gyroskopen
ohnehin problematisch war (mehr dazu in Abschnitt 6.1.2).
Der erweiterte Zustand enthält dabei zusätzlich zu den oben angegebenen 8 Komponenten weitere 8 Rauschkomponenten.
Als Vorbedingung für den Zustandsübergang werden die folgenden Hilfsgrößen berechnet (siehe Abbildung 5.4):
• Radius der aktuellen Lenkkurve: r =
v
gyrz
• Richtungsvektor zum Zeitpunkt t-1: d = cos(Ψ) sin(Ψ)
• Winkeldifferenz: ∆Ψ = gyrz · dt
T
• Neuer Winkel zur X-Achse: Ψ′ = Ψ + ∆Ψ
T
• Richtungsvektor zum Zeitpunkt t: d′ = cos(Ψ′ ) sin(Ψ′ )
T
• Hilfsvektor, orthogonal zur Richtung: s = d(1) −d(0) · r
T
• orthogonal zur neuen Richtung: s′ = d′ (1) −d′ (0) · r
• Positionsdifferenz: ∆p = s′ − s.
Der Zustandsübergang SimplifiedCarMotionModel::transition() wurde nach
den folgenden Übergangsgleichungen implementiert:
pt = pt−1 + ∆p
x
) ∗ dt
vt = vt−1 + (accx + bacc
t
accx
accx
bt = bt−1 + wbaccx
(5.4)
(5.5)
(5.6)
40
5. Software
North
(y)
d
d'
Δp
s'
s
-s
d
ΔΨ
p
r
Δp
Ψ
ΔΨ
Ψ'
Ψ' = Ψ + ΔΨ
d' = (cos(Ψ'), sin(Ψ'))
s = (d(1), -d(0)) * r
s' = (d'(1), -d'(0)) * r
Δp = s' - s
d'
East
(x)
Abbildung 5.4: Berechnungen zum Positionsupdate
Wobei accx dabei die gemessene Beschleunigung in der X-Achse (Vorwärtsbeschleunigung) ist.
Das Update der Quaternion q verläuft nach folgendem Schema: Es wird zunächst in
Euler-Winkel umgewandelt und die neue Quaternion mit den aktualisierten Winkeln,
gegeben durch Gyroskopmessungen mal Zeitdifferenz dt, gebildet. Veranschaulicht
durch folgendes Code-Beispiel:
vnl_double_3 rpyAngles
= q.rotation_angles();
vnl_double_3 newRpyAngles = rpyAngles + gyros * dt;
vnl_quaternion<double>
qPost(newRpyAngles);
5.2.5
Beobachtungsfunktion
Die Beobachtungsfunktion ist in SimplifiedCarPositionMeasurement::observe() implementiert und beschreibt, wie sich die GPS-Messungen aus dem um
Rauschen erweiterten Zustand zusammensetzen.
Der Messvektor xobs ist 8-dimensional und enthält die folgenden Messungen vom
GPS:
• 2D-Position im EN(U)-Koordinatensystem: pobs
• 1D-Vorwärtsgeschwindigkeit: vobs
• 4D-Lage-Quaternion: qobs
x
• 1D-Vorwärtsbeschleunigungsfehler: bacc
obs
5.3. Zusammenfassung und Resultate
x
xobs = pTobs vobs qTobs bacc
obs
41
T
(5.7)
Die Beobachtungsfunktion addiert jeweils das Messrauschen w aus dem erweiterten
Zustand zu jedem Vektor:
pobs
vobs
qobs
x
bacc
obs
= p + wp
= v + wv
= q + wq
= baccx + wbaccx
(5.8)
(5.9)
(5.10)
(5.11)
Diese Funktionen beschreiben die Annahme, dass die vom GPS-Empfänger bestimmte Position, Geschwindigkeit und Ausrichtung genau dem aktuellen Zustand plus
Messrauschen entsprechen.
5.3
Zusammenfassung und Resultate
Als Teil dieser Diplomarbeit wurden Softwaremodule entwickelt. Ein Softwaremodul läuft auf der IMU6-Platine und liest dort permanent die Sensordaten aus. Diese
Sensordaten werden in ein spezielles Nachrichtenformat verpackt und über die serielle UART-Schnittstelle der Platine ausgegeben.
Der zweite Teil der Software läuft auf dem OROCOS-System des autonomen Fahrzeugs und empfängt über TCP/IP die Nachrichten der Platine. Danach erfolgt die
Filterung der Daten mit einem für diesen Zweck entwickelten Unscented KalmanFilter, bestehend aus dem eigentlichen UKF-Algorithmus und den Übergangs- und
Beobachtungsfunktionen, die das dynamische System modellieren.
Im Verlauf der Entwicklung kam es häufig dazu, dass das Filter im Test nicht zu
einem Ergebnis kam. Dabei divergierten die Werte der Kovarianzmatrix. Vermutlich
hingen diese Fehler mit der damals unvollständigen Modellierung des recht komplexen Zustandsraummodells mit bis zu 20-dimensionalen Zustandsvektoren zusammen. Nach einer Vereinfachung auf die in diesem Kapitel beschriebenen Funktionen
und Zustände, traten diese Schwierigkeiten kaum noch auf.
42
5. Software
6. Evaluierung
Die Evaluierung setzt sich zusammen aus einem ersten Teil, in dem die Sensoren und
das GPS-Modul einzeln auf ihre Qualität hin untersucht werden, und einem zweiten
Teil, der die Ergebnisse der Integration und des implementierten Kalman-Filters
enthält.
6.1
6.1.1
Evaluierung der Sensorqualität
GPS-Daten
Um die Qualität des verbauten GPS-Moduls zu bestimmen, wurden Testfahrten unter verschiedenen erschwerten Bedingungen, im Stadtverkehr zwischen hohen Häuserfronten, unter dichtem Baumbewuchs und unter nahezu Idealbedingungen auf
dem Rollfeld des stillgelegten Flughafens Tempelhof in Berlin unternommen.
Exemplarisch sind hier zwei Testfahrten auf den von der Firma Hella Aglaia vorgeschlagenen Test-Strecken vorgestellt. Dabei befand sich die Antenne des u-blox -GPS
im Innenraum des Fahrzeugs hinter einer Seitenscheibe um dem späteren Einsatzszenario als integriertes Spurhaltesystem möglichst nahezukommen.
Die Teststrecke 1 besteht größtenteils aus Autobahn und Landstraße mit wenig
Baumbewuchs. Die Qualität des empfangenen GPS-Signals ist in Abbildung 6.1
zu sehen.
Vom GPS-Modul selbst erhielten wir die folgenden Werte für die (geschätzte) Genauigkeit auf der Strecke:
Maximaler Fehler:
4,24 m/s
Minimaler Fehler:
0,23 m/s
Mittelwert:
1,3372 m/s
Varianz:
2,4210 m/s
Standardabweichung: 1,5560 m/s
Die 2. Teststrecke führte auf der Berliner Stadtautobahn A 100 aus Neukölln kommend vorbei am Flughafen Tempelhof bis zur Seestraße in Wedding. In Abbil-
44
6. Evaluierung
fix/number of satellites/accuracy [m/s]
teststrecke1-2009-12-09+001 IMU6 GPS Status
20
19
18
17
16
15
14
13
12
11
10
9
8
7
6
5
4
3
2
1
0
fix
num. satellites
speed accuracy estimate [m/s]
0
500
1000
1500
2000
2500
3000
3500
4000
4500
5000
time [s]
Abbildung 6.1: Anzahl sichtbarer Satelliten und geschätzte Ungenauigkeit der Geschwindigkeit des GPS-Moduls auf Teststrecke 1
fix/number of satellites/accuracy [m/s]
teststrecke2-2009-12-09+001 IMU6 GPS Status
20
19
18
17
16
15
14
13
12
11
10
9
8
7
6
5
4
3
2
1
0
fix
num. satellites
speed accuracy estimate [m/s]
0
200
400
600
800
1000
1200
1400
time [s]
Abbildung 6.2: Anzahl sichtbarer Satelliten und geschätzte Ungenauigkeit der Geschwindigkeit des GPS-Moduls auf Teststrecke 2
dung 6.2 ist gleich zu Anfang ein langer Ausfall des GPS-Empfangs festzustellen.
Zurückführen kann man diesen Ausfall wohl auf einen 1,7 km langen Tunnel zu
Beginn der Strecke und die Schwierigkeiten, danach wieder einen Satellitenfix zu
bekommen.
Die Statistik für die vom Modul geschätzten Fehler für die 2. Teststrecke (Abschnitte
ohne GPS-Fix ausgenommen):
6.1. Evaluierung der Sensorqualität
45
Maximaler Fehler:
5,49 m/s
Minimaler Fehler:
0,33 m/s
Mittelwert:
1,4176 m/s
Varianz:
2,9473 m/s
Standardabweichung: 1,7167 m/s
Zur besseren Einschätzung der Fehler wurden dann auf einer anderen Teststrecke
mehrere GPS-Empfänger gleichzeitig geloggt und anschließend ausgewertet. Wie
man an den Ergebnissen erkennen kann, war auf dieser Strecke der Empfang sehr
gut. Es zeigten sich dabei die folgenden Abweichungen des u-blox -GPS zum Referenzsystem Applanix POS LV 220 :
Maximale Abweichung:
0,31 m/s
Minimale Abweichung:
- 0,28 m/s
Mittelwert:
0,0151 m/s
Varianz:
0,0236 m/s
Standardabweichung:
0,0809 m/s
Das zeigt, dass die Vorgaben von Hella Aglaia bezüglich der gewünschten Geschwindigkeitsgenauigkeit, zumindest unter für den GPS-Empfang guten Bedingungen, in
der Regel eingehalten werden können.
Die Ergebnisse zeigen hier, dass man zwar häufig eine gute Positionierung mit dem
u-blox -GPS erhalten kann, teilweise aber auch Totalausfälle auftreten. Es lässt sich
jedoch sehr gut anhand der eigenen Einschätzung des Moduls auf die erzielte Genauigkeit schließen und es können so von vornherein gelieferte Werte, die unzureichend
genau sind, verworfen werden.
6.1.2
Gyroskopdaten
6.1.2.1
Gierrate
Das temperaturkorrigierte Z-Achsen-Gyroskop Analog Devices ADIS16100 zeigt ohne weitere nötige Korrekturen bereits sehr wenig Drift, eine gute Genauigkeit und
zeichnet sich durch wenig Rauschen aus.
Es wurde folgende Differenz zum Testsystem auf einer unserer Teststrecken gemessen:
Maximale Abweichung:
Minimale Abweichung:
Mittelwert:
Varianz:
Standardabweichung:
1.0112◦ /sec
-1.2823◦ /sec
0.02174◦ /sec
0.05911◦ /sec
0.2431◦ /sec
Fehler des Referenzsystems in diesem Zeitraum:
Maximaler Fehler:
Minimaler Fehler:
Mittelwert:
Varianz:
Standardabweichung:
0.05774◦ /sec
0.01088◦ /sec
0.04156◦ /sec
0.00186◦ /sec
0.04315◦ /sec
46
6. Evaluierung
strasse-a-short Vergleich Z-Achsen Gyro
0.4
IMU6 Z
Applanix Z
0.35
angular rate [rad/s]
0.3
0.25
0.2
0.15
0.1
0.05
0
210
212
214
216
218
220
time [s]
Abbildung 6.3: Vergleich des Z-Achsen-Gyroskop der IMU6 mit Referenzsystem
6.1.2.2
Nick- und Rollrate (X-/Y-Gyroskope)
Das kombinierte X-/Y-Gyroskop zeigt starke Drift. Teilweise ist diese temperaturabhängig, wie man in Abbildung 6.4 gut sehen kann. Außerdem zeigt sich, dass
der Sensor der Y-Achse zudem in einem deutlich höheren Maße driftet, als jener
in der X-Achse. Jedoch bleiben auch nach Temperaturkompensation sich mit der
Zeit ändernde Fehler übrig. Zum Teil kann das darauf zurückgeführt werden, dass
sich der verwendete Temperatursensor im Z-Achsen-Gyroskop befindet, welches sich
unterschiedlich schnell erwärmt und abkühlt. Zudem befindet er sich etwas entfernt
vom anderen Gyroskop auf der Platine.
Gyroscope Test - No Motion
Gyroscope Test - No Motion
30
Gyro X
Temperature [deg Celsius] - 30
angular rate [deg/s], temperature in z-gyro [deg Celsius]-30
angular rate [deg/s], temperature in z-gyro [deg Celsius]-30
8
7
6
5
4
3
2
1
0
-1
Gyro Y
Temperature [deg Celsius] - 30
25
20
15
10
5
0
0
100
200
300
400
500
600
700
800
900
time [s]
1000
0
100
200
300
400
500
600
700
800
900
1000
time [s]
Abbildung 6.4: Drift des X-Gyroskops (links) und des Y-Gyroskops (rechts), ohne
Glättung der Rohdaten
Es wurde daraufhin zunächst sehr aufwendig versucht, die Abhängigkeit von der
Temperatur und anderen Variablen durch Fitting mit Polynomen verschiedenen
Grades zu bestimmen, was aber keine zufriedenstellenden Ergebnisse brachte. Im
Endeffekt erschien dieser Sensor als kaum nutzbar für die Anwendung.
6.1.3
Accelerometerdaten
Die Werte der Beschleunigungsmesser machten im Prinzip einen guten Eindruck,
allerdings fehlte es hier etwas an Präzision aufgrund von zu geringer Auflösung des
6.1. Evaluierung der Sensorqualität
47
Sensors. Der Sensor besitzt eine 8-Bit-Auflösung und einen Messbereich von ± 2g,
jedes Bit entspricht folglich 0,015625 g. Sowohl die diskreten Beschleunigungswerte,
als auch das Grundrauschen kann man in Abbildung 6.5 am Beispiel des Z-Achsen
Sensors gut erkennen. Hier wurde bereits geglättet, indem jeweils der Mittelwert aus
4 aufeinander folgenden Messwerten gebildet wurde.
teststrecke1-2009-12-09+001 IMU6 Acceleration
0.2
Accel. Z IMU6
Accel. Z Applanix
0.15
0.1
acceleration [m/s2]
0.05
0
-0.05
-0.1
-0.15
-0.2
-0.25
75
76
77
78
79
80
time [s]
Abbildung 6.5: Ausschnitt aus Sensordaten des Z-Achsen-Beschleunigungsmessers,
im Vergleich zur Referenz deutlich zu sehen die diskreten Stufen und das Grundrauschen. Das Fahrzeug stand im Zeitraum von 75-78 Sekunden still.
Dass dies nicht ausreicht, um kleine Veränderungen im Nick- und Rollwinkel festzustellen, lässt sich anhand der folgenden Tabelle zeigen:
Nickwinkel in
◦
0
1
2
3
4
5
10
11
X-Achse [g]
0.0000
0.0175
0.0349
0.0523
0.0698
0.0872
0.1736
0.1908
Z-Achse [g]
1.0000
0.9998
0.9994
0.9986
0.9976
0.9962
0.9848
0.9816
(1 − Z-Achse) [g]
0.0000
0.0002
0.0006
0.0014
0.0024
0.0038
0.0152
0.0184
Ein Nickwinkel von 0◦ bedeutet hier, dass das Fahrzeug völlig waagerecht steht, und
die Z-Achse des Beschleunigungsmessers orthogonal nach unten zeigt. Die Erdbeschleunigung übt dabei in Z-Richtung eine Kraft von etwa 1 g aus. Bei größerem
Nickwinkel wird die Gravitation zum Teil in der X-Achse gemessen.
Die Tabelle zeigt, dass Nickwinkel unter 10 Grad kaum durch den 8-Bit Beschleunigungsmesser in der Z-Achse detektiert werden, sich aber deutlich in der X-Achse
bemerkbar machen und so leicht für Vorwärtsbeschleunigung gehalten werden können.
48
6.2
6.2.1
6. Evaluierung
Ergebnisse der Filterung
Teststrecke
Die Teststrecke für den Test des Kalman-Filters besteht aus einem kurzen Teil im
Stadtverkehr und einer Hin- und Rückfahrt auf der Autobahn mit langgestreckter
Kurve und Tunnel, zu sehen ist die Strecke in Abbildung 6.6.
Abbildung 6.6: Teststrecke für die Evaluation des Kalman-Filters (aus Google
Earth). In Rot der Streckenverlauf, in Grün die empfangenen GPS-Positionen. Im
mittleren Streckenabschnitt der etwa 500 Meter lange Tunnel.
Auf der Fahrt wurden die Ausgaben der IMU6-Platine mitgeloggt, sowie alle Daten
des Referenzsystems. Später wurden die Daten offline gefiltert und es wurde viel
experimentiert, um optimale Parameter zu finden. Am besten arbeitete das Filter in
diesem Fall mit den Parametern α = 1, β = 0, k = 0, und somit λ = α2 (n + k) − n =
0.
6.2.2
Ergebnisse
Die erreichte Positionsgenauigkeit und vor allem auch die Geschwindigkeit wichen zu
Beginn der Messungen sehr stark von der Realität ab. Als Ursache der Fehler konnte
ermittelt werden, dass die auftretenden Nick- und Rollwinkel nicht bestimmt werden
konnten und dadurch die Einflüsse der Gravitation auf den Beschleunigungssensor
für die Längsgeschwindigkeit nicht korrigiert werden konnten. Im Endeffekt zeigte
sich ein besseres Ergebnis ohne Einbeziehung der Nick- und Rollwinkel. Diese wurden
im Modell einfach auf Null gesetzt und die X-/Y-Raten ignoriert. Die Ergebnisse sind
in der oberen Hälfte von Abbildung 6.7 zu sehen.
6.2. Ergebnisse der Filterung
49
strasse-a-short IMU6 UKF Output XY
1600
1400
y/north ENU [m]
1200
IMU6 UKF filtered
Applanix
IMU6 GPS only
1000
800
600
400
-900
-880
-860
-840
-820
-800
-780
-760
-740
-720
-700
x/east ENU [m]
strasse-a-short IMU6 UKF Output XY
1600
1400
y/north ENU [m]
1200
IMU6 UKF filtered
Applanix
IMU6 GPS only
1000
800
600
400
-880
-860
-840
-820
-800
-780
x/east ENU [m]
-760
-740
-720
-700
Abbildung 6.7: Resultate des Filters bei der 2-dimensionalen Lokalisierung im Vergleich zum Referenzsystem Applanix, oben mit den in der IMU6-Platine verbauten
MEMS-Gyroskopen, unten nach Austausch durch Applanix-Gyroskope
6.2.3
Ersatz der X- und Y-Gyroskope
Es wurde daraufhin nach einer Möglichkeit gesucht, grundsätzliche Fehler im Filter
auszuschließen und zu zeigen, dass das Softwaremodul mit besseren Eingangsdaten
sinnvoll arbeitet. Eine Neuentwicklung der Platine oder ein Austausch der Sensoren kam nicht in Frage, einerseits mangels Zeit bis zum gewünschten Abschluss
des Projekts, andererseits weil keine passenden pinkompatiblen Gyroskopsensoren
verfügbar waren. Eine Filterung ohne die betreffenden Sensoren, allein mit den Beschleunigungswerten und der Gierrate schied auch aus, weil, wie in Abschnitt 6.1.3
beschrieben, die Auflösung dafür unzureichend war.
50
6. Evaluierung
strasse-a-short IMU6 UKF Error
-720
-720
Applanix
IMU6 GPS only
IMU6 UKF filtered
-760
-760
-780
-780
-800
-820
Applanix
IMU6 GPS only
IMU6 UKF filtered
-740
x ECEF [m]
x ECEF [m]
-740
-800
-820
-840
-840
-860
-860
-880
-880
-900
-900
100
150
200
250
300
100
150
time [s]
250
300
1600
1600
1400
1400
1200
1200
y ECEF [m]
y ECEF [m]
200
time [s]
1000
800
1000
800
600
600
Applanix
IMU6 GPS only
IMU6 UKF filtered
Applanix
IMU6 GPS only
IMU6 UKF filtered
400
400
100
150
200
time [s]
250
300
100
150
200
time [s]
250
300
80
80
70
60
60
40
velocity [km/h]
velocity [km/h]
50
30
20
40
20
10
0
0
-10
Applanix
IMU6 GPS
IMU6 UKF filtered
Applanix
IMU6 GPS
IMU6 UKF filtered
-20
-20
100
150
200
time [s]
250
300
100
150
200
time [s]
250
Abbildung 6.8: Fehler des Filters bei der Lokalisierung in Ost-West-Richtung
(oben), Nord-Süd-Richtung (mittig) und bei der Geschwindigkeitsbestimmung (unten). Links mit den verbauten MEMS-Gyroskopen, rechts nach Austausch durch
Applanix-Gyroskope
Deshalb wurde entschieden, als Eingabe für das Filter zusätzlich zu den qualitativ
ausreichend guten Sensoren die Drehratensensoren für die X- und Y-Achse der IMU
des Referenzsystems Applanix zu verwenden.
Die Ergebnisse mit den verbesserten Gyroskopen kann man in Abbildung 6.7 und
6.8 sehen.
300
6.3. Fazit
6.3
51
Fazit
Anhand der Ergebnisse wird deutlich, dass die Qualität der Sensoren einer Inertialeinheit essentiell für eine exakte Bestimmung der Positionsveränderung über die
Zeit durch Integrieren der Sensordaten ist.
Auch wenn das Kalman-Filter verrauschte Signale sehr gut filtern kann und auch
feste Offsets in den Messungen sicher bestimmt, kann es den starken nichtlinearen
Drift der Sensoren nicht vorhersagen, vermutlich auch deshalb, weil hier Messungen
fehlen, wie zum Beispiel die exakte Temperatur am Gyroskop.
Nichtsdestotrotz zeigt sich, dass das entworfene Kalman-Filter korrekt arbeitet und
sinnvoll einsetzbar ist, wie man an den Ergebnissen nach dem Austausch der Sensoren erkennen kann.
52
6. Evaluierung
7. Zusammenfassung und Ausblick
Abschließend haben sich im Rückblick auf die Arbeit und nach der Auswertung der
Ergebnisse einige Erkenntnisse ergeben. Zum einen scheint die Qualität der benutzten Sensoren eine essentielle Rolle für die Nutzbarkeit einer Inertialeinheit als Quelle
für eine Korrektur und Ergänzung von GPS-Modulen zu spielen. Wünschenswert ist
dabei eine hohe Auflösung bei digitalen Sensoren und möglichst ein Minimum an
sich kontinuierlich verändernden Messfehlern (nichtlinearer Drift).
Zum anderen ist es bei dem Einsatz eines Kalman-Filters unabdingbar, eine sinnvolle Modellierung der Umwelt zu wählen und die Beobachtungs- und Messfunktion sorgfältig zu entwerfen. Die Varianten des Unscented Kalman-Filters sind dem
erweiterten Kalman-Filter dahingehend überlegen, dass sich beliebige nichtlineare
Funktionen approximieren können und diese nicht erst durch Auswerten der JacobiMatrizen linearisiert werden müssen. Zusätzlich sind die Approximationen in der
Regel genauer als die des EKF.
Bei der Entwicklung der Firmware hat sich gezeigt, dass ein Debugging nur sehr
begrenzt möglich war und sich oft als Geduldsspiel gestaltete. Die angezeigten Fehlermeldungen führten häufig in die Irre und waren nicht sehr hilfreich zum Eingrenzen von Problemen. Oftmals half hier nur die ausführliche Ausgabe von DebugMeldungen über die serielle Schnittstelle.
In der Zukunft ist geplant, die Arbeit mit der IMU6-Hardwareplattform fortzusetzen.
Der Kooperationspartner Hella Aglaia hat in Aussicht gestellt, dass der Beschleunigungssensor auf der Platine in naher Zukunft ausgetauscht werden kann. Es lohnt
sich, damit noch einige Testfahrten zu unternehmen.
Außerdem ist eine neue Entwicklungsplatine in Planung, bei der aller Voraussicht
nach viele der Mängel im Entwurf und Layout der IMU6-Platine beseitigt werden
können. Nicht zuletzt deshalb, weil die Ergebnisse dieser Arbeit in die Entwicklung
der neuen Plattform mit einfließen konnten.
Unabhängig davon erscheint es sinnvoll, die Integration weiterer Sensoren, wie zum
Beispiel eines hochauflösenden digitalen Kompasses oder eines barometrischen Höhenmessers, in das Kalman-Filter zu erproben. Im Gesamtkontext des autonomen
54
7. Zusammenfassung und Ausblick
Fahrzeugs kommt der Lokalisierung eine bedeutende Rolle zu und daher macht es
Sinn, die Informationen aus möglichst vielen Sensoren dafür zu nutzen, und ein
Kalman-Filter scheint nach den gemachten Erfahrungen hierzu ein geeignetes Mittel zu sein.
Auf dem Weg zu einem serienmäßigen PKW mit autonomen Funktionen ist es unabdingbar, die Kostenfrage nicht außer Acht zu lassen. MEMS-Sensoren sind dabei
ein wichtiger Schritt in die richtige Richtung. Es muss aber gelingen, qualitativ höherwertige MEMS-Sensoren zu fertigen, die für diesen Einsatz dennoch preislich in
Frage kommen. Diese Arbeit hat aufgezeigt, dass es dabei auf geringe Drift und hohe
Auflösung ankommt und demonstriert, wie sich die Sensoren durch ein Kalman-Filter
mit externen Quellen fusionieren lassen, um sie für die exakte Positionsbestimmung
in autonomen Fahrzeugen sinnvoll zu nutzen.
A. Anhang: Unscented
Kalman-Filter Pseudocode
Es folgt der Pseudocode für das als Teil der Arbeit implementierte Unscented
Kalman-Filter. Die Implementierung orientiert sich an der Variante square-root central difference Kalman filter (SRCDKF) aus [VDMWa04] und basiert auf der central
difference transformation, wie sie unter anderem in [Nø00] beschrieben ist.
A.1
Initialisierung
Der Zustand x wird zunächst auf einen Initialwert x0 gesetzt. Die Diagonalelemente
der Kovarianzmatrix P′ werden mit einem kleinen Wert ǫ > 0 initialisiert.
x′ = x0 ,
A.2
A.2.1
P′ = I · ǫ
Vorhersage
Berechnung der Sigma-Punkte
Der Zustand x wird dazu mit dem Übergangsrauschen w und die Kovarianzmatrix
P′ um die Fehlerkovarianzmatrix Q erweitert.
T
x = xT w T
Pw =
P′ 0
0 0
Transitionfunction::setQ(Pw )
setQ(Pw ) ersetzt die untere rechte Teilmatrix in Pw durch eine in der Übergangsfunktion definierte Fehlerkovarianzmatrix Q.
56
Sw =
A. Anhang: Unscented Kalman-Filter Pseudocode
√
Pw ist die Matrixquadratwurzel von Pw
xΣ
0 = x
xΣ
i = x+
xΣ
i
√
n + λ (Sw )i
√
= x − n + λ (Sw )i
i = 1 . . . 2n, i ungerade
i = 1 . . . 2n, i gerade
Der Paramater λ ist ein Skalierungsfaktor und definiert als λ = α2 (n + k) − n, wobei
hier k = 0 gesetzt wird, also λ = (α2 − 1)n.
A.2.2
Anwendung der Übergangsfunktion auf Sigma-Punkte
Σ
XΣ
i = Transitionfunction::transition(xi )
A.2.3
i = 1 . . . 2n
Berechnung von Mittelwert und Kovarianz
x̂′ =
2n
X
Wim XΣ
i
i=0
δ i = XΣ
i − x̂
′
P̂ =
2n
X
i=0
i = 1 . . . 2n
Wic δi · δiT
Die Gewichte Wim , Wic werden berechnet wie in der Standardform des Unscented
Kalman-Filters:
W0m = λ/(n + λ)
W0c = W0m + (1 − α2 + β)
Wic = Wim = 1/2(n + λ) i = 1, . . . , 2n
A.3
A.3.1
Beobachtung
Berechnung der Sigma-Punkte
x̂ = x̂′T vT
P̂v =
P̂′ 0
0 0
T
Observationfunction::setR(P̂v )
A.3. Beobachtung
57
setR(P̂v ) ersetzt die untere rechte Teilmatrix in P̂v durch eine in der Beobachtungsfunktion definierte Fehlerkovarianzmatrix R
Sv =
p
Pv
zΣ
0 = x̂
A.3.2
zΣ
i = x̂ + (Sv )i
i = 1 . . . 2n, i ungerade
zΣ
i
i = 1 . . . 2n, i gerade
= x̂ − (Sv )i
Anwendung der Übergangsfunktion auf Sigma-Punkte
Σ
ZΣ
i = Observationfunction::observe(zi )
A.3.3
Berechnung von Mittelwert und Kovarianz
ẑ
=
2n
X
Wim ZΣ
i
i=0
γi
P̂zz
=
ZΣ
i − ẑ
=
2n
X
i=0
A.3.4
i = 1 . . . 2n
i = 1 . . . 2n
Wic γi · γiT
Korrektur
P̂xz
=
2n
X
i=0
Wic yiΣ − y0Σ · γiT
K
=
Pxz · P−1
zz
x
=
x̂ + K · (z − ẑ)
P
=
P̂ − KPzz KT
58
A. Anhang: Unscented Kalman-Filter Pseudocode
[AHAESL06] W. Abdel-Hamid, T. Abdelazim, N. El-Sheimy und G. Lachapelle.
Improvement of MEMS-IMU/GPS performance using fuzzy modeling.
GPS Solutions 10(1), 2006, S. 1–11.
[AlAk05] S. Alper und T. Akin. A single-crystal silicon symmetrical and decoupled MEMS gyroscope on an insulating substrate. Journal of Microelectromechanical Systems 14(4), 2005, S. 707.
[HoKM08] S. Holmes, G. Klein und D. Murray. A square root unscented Kalman filter for visual monoSLAM. In Proc Int Conf on Robotics and
Automation. Citeseer, 2008, S. 3710–3716.
[HSLS+ 07] J. Hol, T. Schön, H. Luinge, P. Slycke und F. Gustafsson. Robust realtime tracking by fusing measurements from inertial and vision sensors.
Journal of Real-Time Image Processing 2(2), 2007, S. 149–160.
[HWLW08] B. Hofmann-Wellenhof, H. Lichtenegger und E. Wasle. GNSS–Global
Navigation Satellite Systems: GPS, GLONASS, Galileo and more.
Springer. 2008.
[IgSH06] C. Igel, T. Suttorp und N. Hansen. A computational efficient covariance matrix update and a (1+ 1)-CMA for evolution strategies. In
Proceedings of the 8th annual conference on Genetic and evolutionary
computation. ACM, 2006, S. 460.
[JUIJC04] S. Julier, J. Uhlmann, I. Ind und M. Jefferson City. Unscented filtering
and nonlinear estimation. Band 92, 2004, S. 401–422.
[JuUDW95] S. Julier, J. Uhlmann und H. Durrant-Whyte. A new approach for
filtering nonlinear systems. In Proceedings of the American Control
Conference, Band 3. American Automatic Control Council, Evanston,
IL, 1995, S. 1628–1632.
[JuUh97] S. Julier und J. Uhlmann. A new extension of the Kalman filter to
nonlinear systems. In Int. Symp. Aerospace/Defense Sensing, Simul.
and Controls, Band 3. Citeseer, 1997, S. 26.
[Ká60] R. Kálmán. A new approach to linear filtering and prediction problems. Journal of basic Engineering 82(1), 1960, S. 35–45.
[KoFS09] S. Kolås, B. Foss und T. Schei. Constrained nonlinear state estimation
based on the UKF approach. Computers & Chemical Engineering
33(8), 2009, S. 1386–1401.
60
[Mayb82] P. Maybeck. Stochastic models, estimation and control, S. 1–16. Academic Press. 1982.
[Nø00] Nørgaard, M., Poulsen, N.K. und Ravn, O. New developments in
state estimation for nonlinear systems 1. Automatica 36(11), 2000,
S. 1627–1638.
[PKSR09] N. Phuong, H. Kang, Y. Suh und Y. Ro. A DCM Based Orientation
Estimation Algorithm with an Inertial Measurement Unit and a Magnetic Compass. Journal of Universal Computer Science 15(4), 2009,
S. 859–876.
[Sama08] N. Samama. Global positioning: technologies and performance. WileyInterscience. 2008.
[Seeg04] M. Seeger. Low rank updates for the Cholesky decomposition. Technical report, 2004.
[Simo06] D. Simon. Optimal state estimation: Kalman, H [infinity] and nonlinear approaches. John Wiley and Sons. 2006.
[VDMWa01] R. Van Der Merwe und E. Wan. The square-root unscented Kalman
filter for state and parameter-estimation. In IEEE International Conference on Acoustics, Speech and Signal Processing, Band 6. Citeseer,
2001, S. 3461–3464.
[VDMWa04] R. Van Der Merwe und E. Wan. Sigma-point Kalman filters for integrated navigation. In Proceedings of the 60th Annual Meeting of the
Institute of Navigation (ION). Citeseer, 2004, S. 641–654.
[WaLR03] J. Wang, H. Lee und C. Rizos. GPS/INS integration: A performance
sensitivity analysis. Wuhan University Journal of Nature Sciences
8(2B), 2003, S. 508–516.
[WaVDM00] E. Wan und R. Van Der Merwe. The unscented Kalman filter for
nonlinear estimation. In Proceedings of Symposium. Citeseer, 2000,
S. 153–158.
[WeBi95] G. Welch und G. Bishop. An introduction to the Kalman filter. University of North Carolina at Chapel Hill, Chapel Hill, NC, 1995.
[Wend07] J. Wendel. Integrierte Navigationssysteme: Sensordatenfusion, GPS
und inertiale Navigation. Oldenbourg Wissenschaftsverlag. 2007.
[Wood07] O. Woodman. An introduction to inertial navigation. University
of Cambridge, Computer Laboratory, Tech. Rep. UCAMCL-TR-696,
2007.
[ZGMH05] P. Zhang, J. Gu, E. Milios und P. Huynh.
Navigation with
IMU/GPS/digital compass with unscented Kalman filter. In 2005 IEEE International Conference Mechatronics and Automation, Band 3,
2005.
61
[ZhZh08] T. Zhu und H. Zheng. Application of Unscented Kalman Filter to Vehicle State Estimation. In ISECS International Colloquium on Computing, Communication, Control, and Management, 2008. CCCM’08,
2008, S. 135–139.
62
Glossar
Cassandra
Eine von der Hella Aglaia entwickelte Softwareplattform für visuelle Fahrerassistenzsysteme. 4, 33
DSP
Digital Signal Processing oder auch Digital Signal Processor, ein Prozessor, der (vorher digitalisierte) Signale verarbeitet und beispielsweise filtert. 41
Gierrate
Die Rate, mit der sich ein Fahr- oder Flugzeug
um die Z-Achse (senkrechte Achse) dreht. 4,
23, 30, 33, 49
siehe Satellitenfix. 44
GPS-Fix
IMU
Inertial Motion Unit: ein elektronisches Gerät, bestehend aus kombinierten
Inertialsensoren. 3, 32
IMU6
Name einer Entwicklungsplattform von Analog Devices, bestehend aus GPS-Modul und
Inertialeinheit. 4, 32, 33
Inertialeinheit siehe IMU. 37
Inertialsensor Sensor, der eine physikalische Größe in einem
Inertialsystem misst, z.B. Beschleunigungsmesser, Drehratensensor. 1, 3, 4, 7, 13, 15, 26,
27, 31, 38, 63
JTAG
Steht für Joint Test Action Group. ein Standard für eine Debug-Schnittstelle von Prozessoren, ermöglicht Programmierung, Setzen
von Breakpoints, schrittweise Ausführung eines Programms und Monitoring von Variablen. 33, 36
64
Glossary
LDW
kurz für: Lane Detection Warning, Name eines
Spurhaltesystems der Hella Aglaia GmbH, das
den Fahrer beim unbeabsichtigten Verlassen
der Fahrspur warnt. 4, 33
NMEA
Abkürzung für National Marine Electronics Association. Protokoll, welches ASCIIbasierte Datensätze definiert, die beispielsweise von GPS-Empfängern ausgegeben werden.
36
Odometer
Ein Messgerät für Radumdrehungen, in mechanischer oder elektronischer Ausführung,
bei PKW meist als Kilometerzähler integriert,
auch zusätzlich nachrüstbar zur Montage an
der Felge. 3
steht für Open Robot Control Software,
ein C++-Framework für die RobotikEntwicklung. 2, 3, 32, 33, 35, 37
OROCOS
Pseudoentfernung Scheinbare Entfernung, die vom GPSEmpfänger gemessen wird, weicht um einen
Fehlerbetrag ab, der hauptsächlich durch die
Uhrungenauigkeit des Empfängers zustande
kommt. 9, 11
Satellitenfix
SPI
Ein Satellitenfix ist gegeben, wenn der Empfänger seine Position durch den Empfang der
Signale von ausreichend vielen Satelliten bestimmen konnte. 44
Serial Peripheral Interface Bus, Synchroner
Datenbus, wird häufig bei Mikrocontrollern
und Sensoren eingesetzt. 35

Kombination von Inertialsensoren und GPS zur Navigation

Transcription

Similar documents

Häufig gestellte Fragen zum AutoNOMOS-Projekt

Ein integritätswahrendes Messsystem für - geometh-data

Sensorfusion zur Navigation eines Fahrzeugs mit low

INSIDE Frühjahr 2016 PDF 14.0 MB

Informationen zum HotelManager

Gefragt und gewusst - Navi