Kombination von Inertialsensoren

und GPS zur Navigation

Diplomarbeit

von

Jonas Hoffmann

am Institut fur Informatik

des Fachbereichs Mathematik und Informatik

Freie Universitat Berlin

Erstgutachter: Prof. Dr. Raul Rojas

Zweitgutachter: Dr. Daniel Gohring

Betreuende Mitarbeiter: Dipl.-Inform. Tinosch Ganjineh

Dipl.-Inform. Fabian Wiesel

Ich erklare hiermit, dass ich die vorliegende Arbeit selbstandig verfasst und keineanderen 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 (publicdomain), sofern keine Quellenangabe in der Bildunterschrift vorhanden ist.Begriffe, die fett gedruckt sind, werden im Glossar erlautert.

Inhaltsverzeichnis

1 Einleitung 11.1 Das Projekt Autonome Fahrzeuge an der FU Berlin . . . . . . . . . . 1

1.1.1 Spirit of Berlin . . . . . . . . . . . . . . . . . . . . . . . . . . 21.1.2 Projekt AutoNOMOS . . . . . . . . . . . . . . . . . . . . . . 3

1.2 Zielsetzung der Arbeit . . . . . . . . . . . . . . . . . . . . . . . . . . 41.3 Gliederung der Arbeit . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Grundlagen 72.1 Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.1.1 Geschichte der Navigation . . . . . . . . . . . . . . . . . . . . 72.1.2 Globale Navigationssatellitensysteme (GNSS) . . . . . . . . . 82.1.3 GPS-Technik . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.1.4 Differential-GPS . . . . . . . . . . . . . . . . . . . . . . . . . 102.1.5 Echtzeitkinematik (RTK) . . . . . . . . . . . . . . . . . . . . 11

2.2 Hardware und Sensoren . . . . . . . . . . . . . . . . . . . . . . . . . 122.2.1 Beschleunigungssensoren . . . . . . . . . . . . . . . . . . . . . 122.2.2 Gyroskope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.2.3 MEMS-Sensoren . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.3 Weltmodell und Fahrzeugdynamik . . . . . . . . . . . . . . . . . . . . 142.3.1 Koordinatensysteme . . . . . . . . . . . . . . . . . . . . . . . 142.3.2 Bezugssyteme . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

2.4 Quaternionen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.5 Richtungskosinusmatrix . . . . . . . . . . . . . . . . . . . . . . . . . 172.6 Kalman-Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.6.1 Lineares Kalman-Filter . . . . . . . . . . . . . . . . . . . . . . 172.6.2 Erweitertes Kalman-Filter . . . . . . . . . . . . . . . . . . . . 192.6.3 Unscented Kalman-Filter . . . . . . . . . . . . . . . . . . . . . 19

2.6.3.1 Vorhersage . . . . . . . . . . . . . . . . . . . . . . . 212.6.3.2 Update . . . . . . . . . . . . . . . . . . . . . . . . . 21

3 Analyse 233.1 Problemstellung, Anforderungen und Randbedingungen . . . . . . . . 233.2 Existierende Losungsansatze . . . . . . . . . . . . . . . . . . . . . . . 24

3.2.1 Existierende Arbeiten . . . . . . . . . . . . . . . . . . . . . . . 243.2.2 Zustandsraum und dynamisches Modell . . . . . . . . . . . . . 24

3.2.2.1 Ubergangsfunktion . . . . . . . . . . . . . . . . . . . 253.2.2.2 Beobachtungsfunktion . . . . . . . . . . . . . . . . . 26

3.2.3 Ergebnisse und Vergleich mit weiteren Arbeiten . . . . . . . . 263.3 Schlussfolgerungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

vi Inhaltsverzeichnis

4 Hardware 294.1 Die Hardwareplattform IMU6 . . . . . . . . . . . . . . . . . . . . . . 29

4.1.1 Technische Daten . . . . . . . . . . . . . . . . . . . . . . . . . 294.1.2 Aufbau und Funktionsweise . . . . . . . . . . . . . . . . . . . 31

4.2 Versuchsaufbau im autonomen Fahrzeug . . . . . . . . . . . . . . . . 324.2.1 Einbau und Anschluss an das vorhandene System . . . . . . . 32

4.3 Schlussfolgerungen . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

5 Software 355.1 Firmware der Hardwareplattform . . . . . . . . . . . . . . . . . . . . 35

5.1.1 Aufbau und Funktionsweise . . . . . . . . . . . . . . . . . . . 355.1.2 Nachrichtenformat . . . . . . . . . . . . . . . . . . . . . . . . 36

5.2 Kalman-Filter, Orocos-Modul . . . . . . . . . . . . . . . . . . . . . . 375.2.1 Bestandteile . . . . . . . . . . . . . . . . . . . . . . . . . . . . 375.2.2 Ein-/Ausgabemodul . . . . . . . . . . . . . . . . . . . . . . . 375.2.3 Kalman-Filter-Modul . . . . . . . . . . . . . . . . . . . . . . . 385.2.4 Ubergangsfunktion . . . . . . . . . . . . . . . . . . . . . . . . 385.2.5 Beobachtungsfunktion . . . . . . . . . . . . . . . . . . . . . . 40

5.3 Zusammenfassung und Resultate . . . . . . . . . . . . . . . . . . . . 41

6 Evaluierung 436.1 Evaluierung der Sensorqualitat . . . . . . . . . . . . . . . . . . . . . . 43

6.1.1 GPS-Daten . . . . . . . . . . . . . . . . . . . . . . . . . . . . 436.1.2 Gyroskopdaten . . . . . . . . . . . . . . . . . . . . . . . . . . 45

6.1.2.1 Gierrate . . . . . . . . . . . . . . . . . . . . . . . . . 456.1.2.2 Nick- und Rollrate (X-/Y-Gyroskope) . . . . . . . . 46

6.1.3 Accelerometerdaten . . . . . . . . . . . . . . . . . . . . . . . . 466.2 Ergebnisse der Filterung . . . . . . . . . . . . . . . . . . . . . . . . . 48

6.2.1 Teststrecke . . . . . . . . . . . . . . . . . . . . . . . . . . . . 486.2.2 Ergebnisse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 486.2.3 Ersatz der X- und Y-Gyroskope . . . . . . . . . . . . . . . . . 49

6.3 Fazit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

7 Zusammenfassung und Ausblick 53

A Anhang: Unscented Kalman-Filter Pseudocode 55A.1 Initialisierung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55A.2 Vorhersage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

A.2.1 Berechnung der Sigma-Punkte . . . . . . . . . . . . . . . . . . 55A.2.2 Anwendung der Ubergangsfunktion auf Sigma-Punkte . . . . . 56A.2.3 Berechnung von Mittelwert und Kovarianz . . . . . . . . . . . 56

A.3 Beobachtung . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56A.3.1 Berechnung der Sigma-Punkte . . . . . . . . . . . . . . . . . . 56A.3.2 Anwendung der Ubergangsfunktion auf Sigma-Punkte . . . . . 57A.3.3 Berechnung von Mittelwert und Kovarianz . . . . . . . . . . . 57A.3.4 Korrektur . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 Zustande des in der Firmware implementierten Automaten . . . . . . 36

5.2 Aufbau des Nachrichtenformats . . . . . . . . . . . . . . . . . . . . . 36

5.3 Auszug aus den vom IMU6-Modul ubertragenen 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 fur 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 fur Menschen sich fortzubewegen gibt es seitAnbeginn der Menschheit. Mindestens ebenso alt ist auch das Bedurfnis nach Ori-entierung.

Die Grundlage fur die Navigation im Allgemeinen bildet die moglichst genaue Be-stimmung von Position, Geschwindigkeit und Ausrichtung eines Objekts. Dabei han-delt es sich meist um Flugzeuge, Fahrzeuge, Schiffe oder Fußganger.

Navigationsgerate fur PKW sind erst seit etwa 10 Jahren zu erschwinglichen Preisenauf dem Markt erhaltlich und dennoch bereits zu Alltagsgegenstanden avanciert, diefur viele nicht mehr wegzudenken sind.

In der vorliegenden Arbeit liegt der Schwerpunkt auf der prazisen Lokalisie-rung mittels globaler Navigationssatellitensysteme und auf der Integration vonInertialsensoren zur Verbesserung der Genauigkeit und Uberbruckung von Emp-fangsausfallen, im Speziellen im Kontext der Entwicklung eines autonom fahrendenPKW.

Autonome Fahrzeuge sind gleich aus mehreren Grunden ein besonders interessantesAnwendungsgebiet der Informatik. Sie vereinen die verschiedenen Unterdisziplinender Kunstlichen Intelligenz wie kaum ein anderes Forschungsgebiet. Von Computer-Vision und 3D-Sensoren uber 2D-Bildverarbeitung bis hin zu reaktivem Verhalten,Wegplanung und intelligentem, energiebewusstem Fahren. Es gibt dabei Herausfor-derungen zu losen, die in viele Bereiche sowohl der theoretischen und praktischenInformatik als auch der Regelungstechnik, Elektrotechnik und Physik reichen.

Sich autonom zu bewegen bedeutet auch fur ein Fahrzeug, dass es sich in einer Um-gebung zurechtfindet und sich orientieren kann, die Kenntnis um die eigene Positionist dafur essentiell.

1.1 Das Projekt Autonome Fahrzeuge an der FU

Berlin

Diese Arbeit entstand im Projekt Autonome Fahrzeuge der AG Kunstliche Intelli-genz an der Freien Universitat Berlin.

2 1. Einleitung

Das Projekt unter Leitung von Prof. Dr. Raul Rojas beschaftigt sich schon seitvielen Jahren mit der Thematik und entwickelt seit 2006 das autonome FahrzeugSpirit Of Berlin1, welches im Fall dieser Arbeit zugleich als Versuchstrager undReferenzsystem dient.

Die Hauptarbeit am Projekt fließt dabei in die Softwareentwicklung auf Basis desRobotik-Frameworks (OROCOS2) ein. Dazu werden Module entwickelt, die im Zu-sammenspiel die Integration der umwelterfassenden Sensorik, Verhaltenssteuerungund Aktorik steuern.

Die Beschaftigung mit autonomen Fahrzeugen entstand ursprunglich durch die Ar-beit an einem thematisch verwandten Projekt. Bereits 1998 wurden am FachbereichInformatik der Freien Universitat autonome Roboter entwickelt und gebaut, mit de-nen seitdem sehr erfolgreich in verschiedenen Ligen und an diversen Meisterschaftenim Roboterfußball, dem RoboCup, teilgenommen wurde.

1.1.1 Spirit of Berlin

Im Jahr 2006 wurde damit begonnen, ein autonomes Fahrzeug zu entwickeln, denSpirit of Berlin. Mit diesem Fahrzeug wurde 2007 erfolgreich an der DARPA Ur-ban Grand Challenge teilgenommen, seitdem wird seine Entwicklung kontinuierlichweiter vorangetrieben.

Abbildung 1.1: Das autonome Fahrzeug Spirit of Berlin vor der Teilnahme an derDARPA Urban Grand Challenge 2007

Der Spirit of Berlin, kurz SpoB, ist ursprunglich ein behindertengerecht umgeruste-ter Dodge PKW. Die gesamte Aktorik: Pedale fur Bremse und Gas, Schaltung undLenkstangenmotor lassen sich elektronisch ansteuern. Er besitzt außerdem diverse

1http://robotics.mi.fu-berlin.de/2http://www.orocos.org/

1.1. Das Projekt Autonome Fahrzeuge an der FU Berlin 3

Lasersensoren fur den Nah- und Fernbereich, verschiedene Kameras, sowie ein Appla-nix POS LV220 -GPS mit einer IMU (Inertial Measurement Unit: ein elektronischesGerat, bestehend aus Inertialsensoren) und einem Odometer. Dieses GPS dientbei der Evaluation der in dieser Arbeit vorgestellten Plattform und Filterung alsHochprazisionssystem.

Alle Sensordaten laufen in einem zentralen Server zusammen, auf dem eine modifi-zierte Variante des OROCOS Real Time Toolkit lauft. Hierbei handelt es sich umein Framework, welches die Steuerung und Kommunikation von Threads in Echtzeitermoglicht. Die entwickelten Module fur den Empfang von Sensordaten, Verarbei-tung, Verhalten, Entscheidungsfindung und Aktorik laufen als unabhangige Threads,die untereinander uber Ports Daten austauschen.

1.1.2 Projekt AutoNOMOS

Die bisher erreichten Ergebnisse und Erfahrungen werden seit 2010 in dem vomBundesministerium fur Bildung und Forschung geforderten Projekt AutoNOMOS 3

genutzt. Die finanzielle Unterstutzung konnte dazu verwendet werden, ein von Grundauf furs autonome Fahren konzipiertes Auto zu entwickeln und zu realisieren.

Der Made in Germany ist ein VW Passat, der auf den ersten Blick nicht von einemregularen PKW zu unterscheiden ist. In die Karosserie sind jedoch umwelterfassen-de Sensoren integriert. Im Kofferraum befinden sich große Teile der Technik, unddurch aufwendige Elektronik lassen sich von einem Computer aus umfangreiche In-formationen uber den Zustand des Fahrzeugs abfragen und nahezu alle Funktionensteuern.

Außerdem konnten durch die Unterstutzung des BMBF Stellen geschaffen werden,die es ermoglichen, mit zusatzlicher personeller Unterstutzung die Weiterentwicklungund den Weg zu autonomen Fahrzeugen im alltaglichen Straßenverkehr voranzubrin-gen.

Abbildung 1.2: Das Team AutoNOMOS mit autonomem Fahrzeug Made in Germa-ny

3http://autonomos.inf.fu-berlin.de/

4 1. Einleitung

Auch die Software wird standig weiterentwickelt und enthalt inzwischen umfangrei-che Verbesserungen und Neuentwicklungen aufgrund der Erkenntnisse aus zahllosenfahrerlosen Feldversuchen.

1.2 Zielsetzung der Arbeit

Die Aufgabe bestand in der Konzeption, Implementation und Evaluierung einesFilters zur Integration von Sensordaten, um die aktuelle Position, Orientierung undGeschwindigkeiten eines Fahrzeugs zu ermitteln.

Von der Firma Hella Aglaia Mobile Vision GmbH, einem Automobilzulieferer undAnbieter von Komponenten fur Fahrerassistenzsysteme, wurde eine Hardwareplatt-form zur Verfugung gestellt, bestehend unter anderem aus einem GPS-Modul,Inertialsensoren und einem Mikroprozessor. In Abschnitt 4.1 wird diese Platinegenauer vorgestellt.

Das Interesse der Firma besteht hauptsachlich darin, die Nutzbarkeit dieser Platt-form als Quelle fur ein von ihr entwickeltes Spurhaltesystem (Lane Departure War-ning - LDW) zu evaluieren. Dem Projekt wurde seitens der Firma neben der IMU6-Hardware ein solches Spurhaltesystem, bestehend aus Mono-Kameras (INKA) mitFusionsbox und einem PC-System mit Softwareumgebung (Cassandra), zur Nut-zung im Fahrzeug zur Verfugung gestellt.

Dabei werden als Eingabeparameter fur das Spurhaltesystem die genaue Geschwin-digkeit und die Gierrate des Fahrzeugs benotigt und zwar insbesondere auch beieingeschranktem oder nicht vorhandenem GPS-Empfang. Diese Daten mussten sonstuber eine teure und aufwendige Anbindungen an den CAN-Bus des Fahrzeugs be-zogen werden.

Diese Hardwareplattform wurde ohne laufende Firmware ausgeliefert, es war le-diglich eine Beispielsoftware, bestehend aus Treibern und einem kleinen Demopro-gramm, vorhanden, durch das rudimentarer Zugriff auf die Sensoren erfolgen konn-te. Ein Teil der Vorarbeit bestand demnach in der Entwicklung einer Firmware,welche die Daten von GPS und Inertialsensoren ausliest und mit Zeitstempelnund Prufsummen versieht und danach in Echtzeit uber die serielle Schnittstelle desModuls ausgibt. Genauer beschrieben wird die Funktionsweise dieser Firmware inKapitel 5.1.

Es stellte sich beim Programmieren der IMU6-Plattform schnell heraus, dass durchdiverse Fehler in den Treibern und auch im Hardwarelayout der Platine diese Ent-wicklung einen erheblichen Anteil an der Gesamtarbeit in Anspruch nehmen wurde.

1.3 Gliederung der Arbeit

Zunachst wird auf die Grundlagen der Arbeit eingegangen. Dazu gehort dieFunktionsweise von globalen Navigationssatellitensystemen und die der beteiligtenInertialsensoren. Es werden außerdem die zugrunde liegenden Koordinatensyste-me und die Lagereprasentation durch die Quaternion beschrieben. Desweiteren wirddie Theorie des Kalman-Filters und seiner nichtlinearen Varianten erklart.

Im darauf folgenden Kapitel 3 wird die Problemstellung und Zielsetzung genauererortert. Anschließend werden die bereits existierenden Losungsansatze vorgestellt,miteinander verglichen und der eigenen Vorgehensweise gegenubergestellt.

1.3. Gliederung der Arbeit 5

Im 4. Kapitel geht es um die verwendete Hardware sowie deren Einsatz und Aufbauim Versuchstrager. Das Kapitel 5 beschaftigt sich mit der Konzeption und Imple-mentierung der Softwarekomponenten. Dabei geht es einerseits um die Firmware,die auf der verwendeten Hardwareplattform lauft, und andererseits um die Softwa-remodule, die fur die Integration der Daten zustandig sind und um das verwendeteFramework.

Danach werden die verschiedenen durchgefuhrten Tests des Systems vorgestellt, de-ren Ergebnisse ausgewertet und ein Fazit gezogen.

Auf dieses Kapitel folgt eine kurze Zusammenfassung und ein Ausblick auf die Fort-fuhrung des Projekts und auf weitergehende Arbeiten.

6 1. Einleitung

2. Grundlagen

In diesem Kapitel wird zunachst auf die Grundlagen der satellitenbasierten Navi-gation allgemein eingegangen. Danach werden speziell die Funktionsweise des Nav-star-GPS erlautert und die verschiedenen Verfahren zur Verringerung der auftreten-den Fehler angesprochen.

In weiteren Kapiteln werden die Inertialsensoren einzeln vorgestellt und derenFunktion erklart. Anschließend wird das verwendete Weltmodell1 dargestellt und imletzten Abschnitt die Theorie und Varianten des Kalman-Filters beschrieben.

2.1 Navigation

2.1.1 Geschichte der Navigation

Schon immer haben die Menschen den Verlauf der Sonne, des Mondes und der Ster-ne beobachtet, um dadurch Ruckschlusse auf ihre Umwelt zu ziehen. Auch zur Be-stimmung der eigenen Position und zum Abschatzen von Richtungen und Zeitenwurden seit jeher die Bahnen der Himmelskorper herangezogen. Zum Beispiel kanndurch die Bestimmung des Winkels zwischen Polarstern und Horizont leicht aufden Breitengrad geschlossen werden. Bedeutende Instrumente fur derartige Winkel-bestimmungen waren beispielsweise die eingesetzten Oktanten oder Sextanten, wiein Abbildung 2.1 dargestellt. Besonders wichtig wurden diese Techniken durch diestetig zunehmende Bedeutung der Seefahrt.

Bei dieser sogenannten astronomischen Navigation wurden jedoch auch unter Ideal-bedingungen kaum Genauigkeiten unter einigen Kilometern erreicht. Das andertesich erst mit der Verfugbarkeit genauer Uhren und der Einfuhrung von Kurzwellen-funk, mit dem ein exaktes Zeitsignal auch auf Schiffen empfangen werden konnte.

Seit der griechischen Antike wird auch das Erdmagnetfeld genutzt, um Himmels-richtungen mit Hilfe von Magnetnadeln bzw. der heutigen Kompasse zu bestimmen.Es wurden nach und nach Tabellen, Karten und Gerate entwickelt, um die Zusam-menhange darzustellen und diese zum Navigieren zu nutzen.

1Modell, welches die dynamischen Eigenschaften der Umgebung beschreibt

8 2. Grundlagen

6080

Indexspiegel

Mikrometer-Trommel Klemme

Lupe

Rahmen

Winkelskala

Horizontspiegel

verdunkelte Gläser

Teleskop

140130

120110

10090 50

70

3020

10

0

verdunkelte

Gläser

Alhidade

Abbildung 2.1: Ein Sextant, wie er in der Seefahrt etwa seit dem 18. Jahrhunderteingesetzt wird (Quelle: Wikimedia Commons, bearbeitet von Arne Nordmann, Ori-ginal von Joaquim Alves Gaspar)

In den letzten Jahrzenten wurden diese naturlichen Navigationsquellen in vielen Be-reichen mehr und mehr durch technische Hilfsmittel ersetzt, weil die Genauigkeit deroptischen Verfahren haufig nicht mehr ausreichend war oder weil sie nicht mit denoftmals computergestutzten und automatisierten Verfahren Schritt halten konnte.Ein Hauptgrund und die treibende Kraft fur die Einfuhrung satellitenbasierter Navi-gationssysteme war naturlich auch der Bedarf des Militars an praziser Lokalisierungund an genauen Kartendaten.

2.1.2 Globale Navigationssatellitensysteme (GNSS)

Ziel der Lokalisierung mittels Navigationssatelliten ist es, die eigene Position im drei-dimensionalen Raum zu bestimmen, in der Regel dargestellt als geografische Breite,Lange und Hohe. Desweiteren lasst sich die Eigengeschwindigkeit des Empfangersdurch den Doppler-Effekt auch direkt aus den Satellitensignalen ableiten.

Anfang der 1960er Jahre, nicht lange nachdem die ersten Satelliten uberhaupt in eineErdumlaufbahn geschickt worden waren, entwickelte die John-Hopkins-Universitatim Auftrag der US Navy bereits das erste satellitenbasierte Navigationssystem mitdem Namen SPUTNIK. Damals mit einer Genauigkeit von etwa 200 Metern. Hiererfolgte auch die Lokalisierung noch durch Ausnutzung des Doppler-Effekts.

Die zur Navigation genutzten Satelliten sind mit genauen Atomuhren ausgestat-tet. Beispielsweise befinden sich in GPS-Satelliten Rubidium-Atomuhren mit Ab-weichungen von maximal 1 Sekunde in 10.000 Jahren. Die aktuelle Position der

2.1. Navigation 9

Satelliten kann berechnet werden, da sie fortwahrend ihre eigenen Bahndaten zumEmpfanger senden.

Hatte der Empfanger nun auch eine genaue Uhr, konnte er die Laufzeiten von dreiSatelliten bestimmen, dadurch auf die Entfernungen schließen und somit durch klas-sische Triangulation die eigene Position ermitteln.

Jede Signallaufzeit zu einem Satelliten steht fur seine Entfernung und beschreibt dieOberflache einer Kugel um den Satelliten als Mittelpunkt. Dort, wo sich die Oberfla-chen dreier Kugeln schneiden, befande sich die Empfangerposition. (Das Verfahrenwird auch Trispharation genannt.) In der Regel schneiden sich diese drei Kugeln inzwei Schnittpunkten, einer der Punkte liegt aber viele Kilometer uber der Erdober-flache und kann somit ausgeschlossen werden.

Die Uhr im Empfanger ist in der Regel keine Atomuhr, allein schon wegen der Großeund Kosten einer solchen, sondern eine einfache Quarzuhr. Diese ist etwa 100.000-mal ungenauer als eine typischerweise in Satelliten eingesetzte Atomuhr und dahergibt es zusatzlich zu den drei Unbekannten Lange, Breite und Hohe noch eine vierteunbekannte Variable, die Abweichung der Empfangeruhr ∆t.

Die gemessenen Laufzeiten und somit Entfernungen der Satelliten sind also um ge-nau diese Differenz von den realen Entfernungen abweichend und werden deshalbauch Pseudoentfernungen genannt. Eine Abweichung der Empfangeruhr von nur0,01 Sekunden wurde dabei schon einen Fehler in der Distanz in Hohe von 3.000km bedeuten. Aus diesem Grund sind fur die endgultige Bestimmung der Positionmindestens vier Satelliten erforderlich.

dtS1

S2

S3

r1

r2

r3

P

Abbildung 2.2: Positionsbestimmung mittels Pseudoranging (2D)

In Abbildung 2.2 ist der Sachverhalt fur den zweidimensionalen Fall veranschaulicht.P ist die reale Position des Empfangers. Die gemessenen Pseudoentfernungen sind

10 2. Grundlagen

hier rot gestrichelt dargestellt. Die beiden Entfernungen zu S1 und S2 weichen umein unbekanntes ∆t von der realen Entfernung ab und schneiden sich deshalb hiergar nicht. Die reale Entfernung wird hier durch grune Kreise dargestellt.

Durch Hinzunahme eines dritten Satelliten S3 lasst sich ∆t bestimmen, geometrischhier durch einen roten Kreis dargestellt, der die Kreise mit r1, r2 und r3 beruhrt.Der Mittelpunkt dieses Kreises ist die gesuchte Position P. Dieses Verfahren wird alsPseudoranging oder auch Hyperbelortung bezeichnet. Analog dazu funktioniert dasPseudoranging im dreidimensionalen Raum mit Kugeloberflachen statt Kreisen.

Das am weitesten verbreitete globale Satellitennavigationssystem ist heutzutage dasNavstar-GPS, das ursprunglich fur militarische Zwecke entwickelte und weiterhingenutzte System des US-Verteidigungsministeriums. Weitere Systeme sind das rus-sische GLONASS und das chinesische COMPASS, welches sich ebenso wie das eu-ropaische GALILEO Programm im Aufbau befindet. Im Folgenden wird hier immervom 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 Um-laufbahnen mit jeweils 4 Satelliten in 20.200 km Hohe und senden ihre Signale aufden Tragerfrequenzen L1 bei 1575,42 Mhz und L2 bei 1227,6 MHz. Zusatzlich befin-den sich zurzeit 4 Reservesatelliten in einer Umlaufbahn. Die ubermittelten Datenauf den Frequenzen bestehen aus dem C/A-Code (engl. Coarse Aquisition) und demverschlusselten, der militarischen Nutzung vorbehaltenen P(Y)-Code [HWLW08].

Der C/A-Code wird auch PRN-Code (Pseudo Random Noise) genannt und ist einedeterministische Folge aus 1023 Bits, die jede Millisekunde ubertragen werden. Dasentspricht also einer Bitfrequenz von 1,023 Mhz. Dabei identifiziert eine Bitfolgeden jeweiligen Satelliten immer eindeutig. Die Signale der unterschiedlichen Satel-liten werden auf der gleichen Frequenz mittels Codemultiplexverfahren ubertragen.Dabei kommen als Spreizcodes die sogenannten Gold-Folgen zum Einsatz, welche dieTrennung der unterschiedlichen Signale vereinfachen, weil die einzelnen Gold-Folgenzueinander jeweils nahezu orthogonal stehen. Die gleiche Folge wird im Empfangererzeugt und soweit um einen Verzogerungswert verschoben, bis die beiden Signalekorrekt ubereinander liegen. Man spricht dabei auch von der Synchronisation derSignale.

Auf den C/A-Code werden die Daten mit 50 Bit/s aufmoduliert. Diese bestehenaus den drei Teilen: Uhrzeit des Satelliten, Ephemeridendaten und Almanachdaten.Die Almanachdaten dienen dazu, alle zu einem Zeitpunkt sichtbaren Satelliten zubestimmen und enthalten Informationen uber die globale Konstellation des GPS-Systems. Sie erleichtern somit das

”Finden“ von Satelliten nach einem Neustart des

Empfangers und werden von diesem meist intern zwischengespeichert. Die Ephemeri-dendaten sind erforderlich, um den aktuellen Satelliten zu verfolgen, und beinhaltengenaue Informationen uber seine Umlaufbahn und Abweichungen. Die Uhrzeit desSatelliten ist, wie bereits beschrieben, die Grundbedingung, um dessen Entfernungund somit die eigene Position zu bestimmen.

2.1.4 Differential-GPS

Der Fehler in der ermittelten Laufzeit des Signals von einem Satelliten setzt sich ausdrei Teilen zusammen:

2.1. Navigation 11

• Fehler im Satelliten: Uhr- und Bahnabweichungen, relativistische Einflusse

• atmospharische Storungen: Ionospharen- und Troposphareneinflusse

• Fehler im Empfanger: Uhrabweichung und ungenaue Synchronisation

Durch die Laufzeitmessungen aller Satelliten an ortsfesten Referenzstationen mitbekannter Position konnen die satellitenabhangigen Fehler und die atmospharischenEinflusse großtenteils bestimmt werden. Die Abweichungen der gemessenen Laufzei-ten (Pseudoentfernungen) von den aus der bekannten Eigenposition eigentlichenLaufzeiten werden als Korrekturwerte an den Empfanger ubermittelt (siehe Abbil-dung 2.3).

Rover Referenz-

station

S1S2S3

S4

Δ1-Δ4

Abbildung 2.3: Nutzung von DGPS mit Referenzstation

Es gibt dabei grundsatzlich die Moglichkeiten der Ubertragung uber Funk oderuber das Internet. Funk kann in dem Fall bedeuten, dass die Korrektursignale vonLangwellen- oder Kurzwellensendern, per GSM oder uber geostationare Satelliten-systeme, sogenannte SBAS (Satellite Based Augmentation Systems), ausgestrahltwerden. Das europaische SBAS-System heißt EGNOS und die US-amerikanischeVariante WAAS. Der Empfang fur diese beiden Korrektursysteme ist in vielenmodernen GPS-Empfangern bereits implementiert. Zusatzlich gibt es kommerzielleAnbieter von Korrekturdaten uber geostationare Satelliten, wie beispielsweise derOmniSTAR-Dienst der niederlandischen Firma Fugro. Die Korrektursignale ermog-lichen eine Verbesserung der Positionsgenauigkeit von etwa 10-20 Metern bis aufwenige Meter. Dabei hangt die erreichbare Genauigkeit direkt von der Entfernungzwischen Referenzstation und Empfanger 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 Tragerfrequenz desSignals zur Synchronisierung nutzt. Genau wie bei D-GPS werden hier auch standigdie Pseudoentfernungen durch Korrekturdaten von einer oder mehreren Basissta-tionen verbessert. Theoretisch ist die hierbei erreichbare Genauigkeit im Vergleich

12 2. Grundlagen

zu D-GPS um denselben Faktor großer, um den auch die Tragerfrequenz großer alsdie Nachrichtenfrequenz ist.

Die erreichte Positionsgenauigkeit ist bei allen GPS-Verfahren stark abhangig davon,wie genau der Empfanger sich auf das GPS-Signal synchronisieren kann. Angenom-men, der Empfanger ist in der Lage, sich mit einer Abweichung von 1% der Bitweiteauf ein Signal zu synchronisieren, dann entsprache das bei D-GPS und einer Fre-quenz von 1,023 Mhz (C/A-Code) einer Ungenauigkeit von 0,01 Mikrosekunden, dassind in Entfernung ausgedruckt etwa 3 Meter.

Wenn der Empfanger, wie beim RTK-Verfahren, die L1-Tragerfrequenz 1575,42 Mhzzur Synchronisation nutzt, ist die theoretisch erreichbare Genauigkeit aufgrund derWellenlange von 19 cm bereits 1,9 mm, wenn man wieder die Abweichung von 1% derBitweite annimmt. Bei RTK treten jedoch Mehrdeutigkeiten in Abstanden von ganz-zahligen Wellenlangen auf, die bei Verwendung des Nachrichtensignals (C/A-Code)nicht vorhanden sind, da hier die Kodierung durch die Gold-Folgen so gewahlt ist,dass sich zwei gleiche, zeitlich versetzte Signale leicht synchronisieren lassen, und dasich die Bitfolge aufgrund der geringeren Frequenz nicht so schnell wiederholt. Die-se Mehrdeutigkeiten aufzulosen, ist eine der komplexesten Aufgaben, die moderneGPS-Empfanger zu erfullen haben, und kann im Einzelnen in [HWLW08] nachgele-sen werden.

2.2 Hardware und Sensoren

2.2.1 Beschleunigungssensoren

Beschleunigungssensoren oder Accelerometer messen Beschleunigung in Richtung ei-ner Achse, also die zeitliche Ableitung des Geschwindigkeitsvektors. Durch einfachesIntegrieren erhalt man daraus die Geschwindigkeit und durch zweifaches Integrierendie zuruckgelegte Strecke in Richtung der Messachse des Sensors, denn die Geschwin-digkeit ist ja genau die zeitliche Ableitung des Ortsvektors.

Voraussetzung fur die Positionsbestimmung mit Beschleunigungssensoren ist, dasssich die Lage im Raum nicht andert oder genau wie die Beschleunigung zu jedemZeitpunkt bekannt ist. Ebenfalls mussen die Startwerte fur Geschwindigkeit undPosition bekannt sein, um sich absolut positionieren zu konnen. Hierbei ist zu be-achten, dass bei diesem Verfahren selbst kleine Messfehler, bei denen es sich nichtum normalverteiltes Rauschen handelt, sondern um systematische Abweichungen wieTemperaturabhangigkeit, durch die zweifache Integration so bedeutende Dimensio-nen annehmen, dass durch simples Aufaddieren der fehlerbehafteten Messungen inder Praxis die bestimmten Werte und die reellen Großen schnell divergieren.

Der Aufbau eines klassischen Accelerometers (Beschleunigungsmessers) bestehtmeist aus einer beweglich gelagerten Masse und einem anliegenden piezoelektrischenElement. Durch die Beschleunigung in eine Richtung wird auf das piezoelektrischeElement eine Kraft ausgeubt und somit eine messbare Spannung erzeugt, die durchzwei Drahte an Kontakte außerhalb des Sensorgehauses geleitet werden. Oft inte-griert man drei Beschleunigungssensoren, die paarweise orthogonal zueinander ste-hen, in einem Gehause und erhalt dadurch einen 3-Achsen-Sensor.

2.2. Hardware und Sensoren 13

2.2.2 Gyroskope

Gyroskope oder auch Drehratensensoren messen die Drehrate um eine oder meh-rere Achsen. Gemessen wird die zeitliche Ableitung des Winkels. Durch einmaligesIntegrieren erhalt man daraus den Winkel, in dem sich ein Objekt im betrachtetenZeitraum um eine Achse gedreht hat.

kardanische

Aufhängungrotierender

Kreisel

DrehachseRahmen

Abbildung 2.4: Klassisches Kreiselinstrument (Gyroskop) mit Schwungrad, Dreh-achse, kardanischer Aufhangung und Rahmen

Ein klassisches Gyroskop, oft auch als Kreiselinstrument oder Kreiselkompass be-zeichnet, besteht aus einem sich drehenden Schwungrad, welches frei beweglich ineinem kardanischen Lager aufgehangt ist (siehe Abbildung 2.4). Das rotierende Radbehalt durch die Drehimpulserhaltung seine Lage bei, auch wenn sich die Lage derAufhangung andert. Diese Lagedifferenz lasst sich an den Achsen messen.

2.2.3 MEMS-Sensoren

Sowohl Beschleunigungsmesser als auch Gyroskope konnen mit heutiger Technikals sogenannte MEMS (Micro Electro Mechanical System) - Sensoren hergestelltwerden. Hierbei kommen ahnliche Verfahren wie bei integrierten Schaltkreisen zumEinsatz. Die Siliziumwafer werden dabei auf unterschiedliche Weise belichtet, neueSchichten aufgetragen und uberflussige Teile weggeatzt, so dass am Ende die elek-tromechanischen Bestandteile des Sensors ubrigbleiben.

Die MEMS-Technologie hat es moglich gemacht, zu geringen Preisen sehr kleineInertialsensoren herzustellen.

Gyroskope, die als MEMS-Sensoren hergestellt werden, bestehen meist aus sehr klei-nen Massenelementen, die in einer Achse in Schwingung versetzt werden und derenBewegung durch Einflusse der Corioliskraft auf kapazitive Elemente, die in einerhorizontalen Achse angeordnet sind, gemessen wird. Man spricht auch von soge-nannten Vibrationskreiseln (engl. Vibrating Structure Gyroscope). Ein Beispiel fureine Variante eines solchen, des sogenannten

”Tuning-Fork“-Drehratensensors, ist in

Abbildung 2.5 zu sehen.

An den Ausgangen der MEMS-Bausteine liegt dann entweder eine analoge Spannungan, die aquivalent zur Drehrate ist, oder die Spannung wird intern bereits durch einenintegrierten Analog-Digital-Umsetzer (ADU) digitalisiert. In diesem Fall ist die Auf-losung und Qualitat des internen Wandlers von entscheidender Bedeutung. Einige

14 2. Grundlagen

Abbildung 2.5: Aufbau und mikroskopische Struktur eines MEMS-Gyroskops (Quel-le: [AlAk05])

MEMS-Sensoren haben interne Temperatursensoren, welche die Temperaturabhan-gigkeit, die besonders bei Gyroskopen eine entscheidende Rolle spielt, ausgleichenkonnen.

2.3 Weltmodell und Fahrzeugdynamik

In diesem Abschnitt folgt eine kurze Vorstellung der verschiedenen verwendetenKoordinatensysteme und der Bezugssysteme fur das dynamische Modell.

2.3.1 Koordinatensysteme

Die vom GPS gelieferten Koordinaten beziehen sich auf das WGS84-Referenzkoor-dinatensystem, welches die Erdoberflache als Ellipsoid mit einer großen Halbachsevon etwa 6.380.000 Metern und einer Abplattung von ca. 1

298modelliert. Sind diese

Werte gegeben, lassen sich beliebige Koordinaten leicht in ein kartesisches Koordi-natensystem mit Ursprung im Erdmittelpunkt umrechnen.

ECEF steht fur Earth-Centered Earth-Fixed und ist ein solches Koordinatensystem,bei dem die Z-Achse zum Nordpol, die X-Achse zum Schnittpunkt von Aquator undNullmeridian und die Y-Achse orthogonal dazu in Richtung des Schnittpunkts von90. ostlichem Langengrad und Aquator zeigt.

Vom ECEF-System lasst sich wiederum sehr einfach in ein beliebiges lokales Koor-dinatensystem umrechnen (siehe Abbildung 2.6). In dieser Arbeit wird das ENU-System verwendet. Die Buchstaben stehen fur

”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 eineRolle. Die GPS-Koordinaten beziehen sich auf das globale Bezugssytem der Erde, dieAusgabe erfolgt ublicherweise in Geokoordinaten oder im ECEF-Koordinatensystemmit Ursprung am Erdmittelpunkt. Dieses erdfeste Bezugssystem nennen wir auchearth frame. Durch die Erdrotation dreht es sich um die z-Achse gegenuber denFixsternen (und somit gegenuber dem inertial frame).

Die Inertialsensoren im Fahrzeug messen Beschleunigungen und Drehraten stetsrelativ zum Inertial-Koordinatensystem, welches seinen Ursprung ebenfalls im Erd-mittelpunkt hat, jedoch nicht mit der Erde rotiert, die Drehung der Erde wird daher

2.4. Quaternionen 15

Z

Y

X

North

East

Up

ecef

ecef

ecef

φ

λ

Abbildung 2.6: Lokale und Weltkoordinatensysteme

auch von den Drehratensensoren gemessen. Diese Koordinatensystem wird auch alsinertial frame bezeichnet.

Die zuruckgelegte Strecke des Fahrzeugs bezieht sich immer auf einen festen Be-zugspunkt auf der Erdoberflache, in der Regel der Startpunkt der Messung. DiesesReferenzsystem heißt Navigations-Bezugssystem oder auch navigation frame. Hier-bei kommt das ENU-Koordinatensystem zum Einsatz.

Der body frame schließlich ist fest an das Fahrzeug gebunden und hat seinen Ur-sprung ublicherweise im Schwerpunkt des Fahrzeugs. Dabei zeigt die x-Achse nachvorn, die y-Achse nach links und die z-Achse nach oben (siehe Abbildung 2.7).

Durch Translation und Rotation konnen wir die Bewegungen aus dem body framein das Navigations-Bezugssystem uberfuhren.

2.4 Quaternionen

Die Orientierung im dreidimensionalen Raum kann dargestellt werden als eine Ro-tation von einem Koordinatensystem in ein anderes. Beispielsweise die Drehung desfest ans Fahrzeug gebundenen body frame in das Navigations-Bezugssystem (naviga-tion frame). Dabei hat die Darstellung der Rotation als Quaternion gewisse Vorteilegegenuber 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

Gierwinkel

(Yaw) ΨRPY-Winkel

& Fahrzeug-

koordinaten

Nickwinkel

(Pitch) Θ

Rollwinkel

(Roll) Φ

x

z

y

x

yz

UpNorth

East+Ψ

Navigations-

Koordinaten (ENU)

Abbildung 2.7: Fahrzeugkoordinaten, Winkel und ENU-Koordinaten (Quelle: bear-beitet 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 Zah-lenraum, ahnlich 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 reprasentiert, bestimmt durch:

q0 = cos(σ/2),

q1 = v1 sin(σ/2),

q2 = v2 sin(σ/2),

q3 = v3 sin(σ/2)

Umgekehrt kann man die Rotationsachse und den Rotationswinkel aus der Quater-

nion-Darstellung herleiten, dabei sei q =(

q1 q2 q3)T

:

σ = 2arccos(q0)

v =q

‖q‖

Die Rotation eines dreidimensionalen Vektors x erfolgt als Multiplikation mit vier-dimensionalen Quaternionen durch:

(

0x′

)

= q ·(

0x

)

· q−1

3http://creativecommons.org/licenses/by-sa/3.0/deed.en

2.5. Richtungskosinusmatrix 17

2.5 Richtungskosinusmatrix

Fur die aktuelle Orientierungs-Quaternion q = q0 + q1 · i + q2 · j + q3 · k, welchedie Orientierung des Fahrzeugs reprasentiert, ist die Richtungskosinusmatrix Cn

b

definiert als:

Cnb = 2

0, 5− q22 − q23 q1q2 − q0q3 q1q3 + q0q2q1q2 + q0q3 0, 5− q21 − q23 q2q3 − q0q1q1q3 − q0q2 q2q3 + q0q1 0, 5− q21 − 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 ermogli-chen, das Fortschreiten des unbekannten Zustands eines dynamischen Systems zwi-schen diskreten Zeitpunkten zu schatzen. Diese Gleichungen wurden nach Rudolf E.Kalman benannt, der sie in seinem 1960 veroffentlichten Artikel aufstellte [Ka60].

Es tritt dabei sowohl beim Zustandsubergang, als auch bei den zu einem Zeitpunktbeobachteten Ausgangsvariablen, den Messungen, Rauschen auf. Das Kalman-Filtergeht dabei von einer probabilistischen Gauß-Verteilung des Rauschens aus.

Vorhersage Update

Abbildung 2.8: Vorhersage- und Updateschritt beim Kalman-Filter

Die Kenntnis uber die Messfunktion und die Zustandsubergangsfunktion ermoglichtes so, durch iterative Vorhersage- und Updateschritte auf den unbekannten Zustandzu schließen. Im Updateschritt wird dabei jeweils die gemachte Vorhersage mit denbeobachteten Messwerten verglichen, um damit die Vorhersage zu korrigieren. Eshandelt sich hier also um ein klassisches Pradiktor-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 Mittelwertund Kovarianzmatrix in einer Ellipse. Die restlichen Variablen sind einfache Vekto-ren.

18 2. Grundlagen

xk

F B H

uk

zk

v

,Pk

0, R

0, Q

w

BekannteGrößen

Zeitpunkt k k-1

F B H

uk

0, R

0, Q

-1

Abbildung 2.9: Systemmodell des Kalman-Filters

Die Gleichungen, die fur den linearen Fall den Zustandsraum modellieren, setzensich zusammen aus der Ubergangsgleichung:

xk = Fkxk−1 +Buk +wk (2.2)

und der Beobachtungsgleichung:

zk = Hxk + vk (2.3)

Die beiden Variablen wk und vk sind unabhangige Zufallsvariablen und stehen furdas Zustandsubergangsrauschen respektive das Messrauschen. Diese Zufallsvariablenseien normalverteilt:

p(w) ∼ N(0,Q) (2.4)

p(v) ∼ N(0,R) (2.5)

Q und R sind dabei jeweils die Kovarianzmatrizen der Wahrscheinlichkeitsverteilun-gen. Im Vorhersageschritt wird aus der Wahrscheinlichkeitsverteilung zum Zeitpunktk − 1 der Zustand zum Zeitpunkt k vorhergesagt:

xk = Fkxk−1 +Buk−1 (2.6)

Pk = FkPk−1FTk +Q (2.7)

xk ist hier die a-priori-Schatzung der Zustandsvariablen, basierend auf der aktu-ellen Normalverteilung des Zustands N(xk−1, Pk−1), die wir im vorherigen Schrittberechnet haben. Pk ist die Kovarianzmatrix der vorhergesagten Wahrscheinlich-keitsverteilung. F und B stammen aus Gleichung 2.2.

Im Updateschritt wird der sogenannte Kalman-Gain Kk eingefuhrt, eine Hilfsva-riable, die als Gewichtungsfaktor die Differenz zwischen Vorhersage und aktuellemZustand minimiert.

Kk = PkHT (HPkH

T +R)−1 (2.8)

xk = xk +Kk(zk −Hxk) (2.9)

Pk = (I−KkH)Pk (2.10)

2.6. Kalman-Filter 19

Der aktuelle Zustand xk ist die Summe aus der Vorhersage xk und der Differenzzwischen Messung zk und dem durch die Beobachtungsfunktion H transformiertenvorhergesagten Zustand xk, gewichtet durch den Kalman-Gain. Im letzten Schrittwird analog die neue (a posteriori) Kovarianzmatrix aus der Differenz der vorherge-sagten Kovarianz Pk und der mit Kk gewichteten, durch die BeobachtungsfunktionH transformierten Vorhersage gebildet. Die Herleitung lasst sich in [Simo06] nach-schlagen.

2.6.2 Erweitertes Kalman-Filter

Oft werden Kalman-Filter dazu benutzt, um Bewegungen von Objekten voraus-zusagen. Da es sich hierbei nicht immer um lineare Bewegungen handelt, wurdedas Modell erweitert, um auch nichtlineare aber differenzierbare Ubergangsfunktio-nen zuzulassen. In der Tat handelt es sich in vielen nichttrivialen Anwendungsfal-len des Kalman-Filters um nichtlineare, dynamische Prozesse. Die Erweiterung desKalman-Filters auf nichtlineare Falle gelingt durch die stellenweise Linearisierungum die Mittelwerte der Wahrscheinlichkeitsverteilungen. Um die Ubergangsfunktionstellenweise zu linearisieren, werden die Jacobi-Matrizen, also die partiellen Ablei-tungen der Ubergangsfunktionen, zur jeweils geschatzten Verteilung ausgewertet,man erhalt dadurch eine Annaherung erster Ordnung [WaVDM00].

Eine der Hauptschwachen dieses erweiterten Kalman-Filters ist die Tatsache, dassdie Zufallsvariablen nach Anwenden der nichtlinearen Ubergangs- und Messfunktio-nen keine Normalverteilung mehr aufweisen.

2.6.3 Unscented Kalman-Filter

Die in dieser Arbeit verwendete Variante des Kalman-Filters, das sogenannte Un-scented Kalman-Filter (UKF), unterscheidet sich in einigen Aspekten vom klassi-schen Modell. Es basiert auf der Annahme, dass es leichter ist, eine Zufallsvertei-lung zu approximieren als eine beliebige nichtlineare Funktion [JUIJC04]. Das UKFverwendet dazu eine minimale Anzahl an speziell ausgewahlten Testpunkten, dieum den Mittelwert der Wahrscheinlichkeitsverteilung liegen. Auf diese sogenann-ten Sigmapunkte wird die nichtlineare Ubergangsfunktion angewendet und aus dentransformierten Punkten der neue Mittelwert und die Kovarianz berechnet.

Dieses Verfahren wird Unscented Transformation genannt. Eine n-dimensionale Zu-standsvariable x ∈ R

n mit der Kovarianz Px und dem Mittelwert x wird dabei durchdie folgenden 2n+1 Sigmapunkte xi und die zugehorigen GewichteWi approximiert:

x0 = x

xi = x+ (√

(n+ k)Px)i fur i = 1, . . . , n (2.11)

xi = x− (√

(n+ k)Px)i−n fur i = n+ 1, . . . , 2n

Hierbei bezeichnet (√A)i die i-te Zeile der Quadratwurzel4 aus der Matrix A.

4Die Quadratwurzel B aus einer Matrix A ist genau die Matrix, fur welche gilt: A = BB.

20 2. Grundlagen

Ý ´ Ü µ

Ý ´ Ü µ

È

Ý

Ö È

Ü

´ Ö µ

Ì

´ Ü µ

Ö È

Ü

´ Ö µ

Ì

´ µ

Messdaten Linearisiert (EKF)

SP Mittelwert

gewichteter Mittelwert

und Kovarianz

wahre Kovarianz

Sigma−Point (UKF)

Sigmapunkte

Sigmapunkte

transformierte

Kovarianz

SP Kovarianz

Mittelwert

wahrer Mittelwert

y = g(x)

Abbildung 2.10: Funktionsprinzip des UKF (aus [VDMWa04])

Wm0 = λ/(n+ λ)

W c0 = Wm

0 + (1− α2 + β) (2.12)

W ci = Wm

i = 1/2(n+ λ) i = 1, . . . , 2n

α legt die Entfernung der Sigmapunkte um den Mittelwert x fest. λ = α2(n+k)−nist ein Skalierungsfaktor, wobei meist k = 0 gesetzt wird. β kann variiert werden,um moglicherweise bereits bekannte Abweichungen der Wahrscheinlichkeitsvertei-lung von einer Normalverteilung einfließen zu lassen. Fur Gauß-Verteilungen istβ = 2 optimal [WaVDM00].

Mit dem UKF kann man die Schwachen des erweiterten Kalman-Filters umge-hen und erhalt eine Annaherung dritter Ordnung anstatt erster Ordnung, wah-rend man auf die Linearisierung durch Berechnung der Jacobi-Matrixen beim EKFverzichten kann, die bei komplexen Funktionen unter Umstanden sehr aufwendigist[WaVDM00]. Außerdem ist es moglich, dass die Fortpflanzung des Fehlers bei ei-nem nichtlinearen System sich ebenfalls nicht durch lineare Funktionen beschreibenlasst. In diesem Fall kann es sein, dass die geschatzten Werte beim EKF divergie-ren [JUIJC04]. [JuUh97] zeigt, dass diese Annaherung dem EKF auch qualitativuberlegen ist.

Eine Moglichkeit, das Unscented Kalman-Filter zu implementieren, ist es, den Sys-temzustandsraum um die Rauschkomponenten zu erweitern.

x′

k =(

xTk wT

k vTk

)

(2.13)

Ebenso wird die Prozesskovarianzmatrix um die Kovarianzen der Rauschkomponen-ten erweitert.

P′

k =

P 0 00 Q 00 0 R

(2.14)

2.6. Kalman-Filter 21

2.6.3.1 Vorhersage

Die nichtlineare Zustandsubergangsfunktion y = g(x) wird im Vorhersageschritt aufdie Sigmapunkte aus Gleichung 2.12 angewandt und diese werden entsprechend ge-wichtet. Daraus werden der neue Mittelwert x und die Kovarianzmatrix P berechnet.

x =2n∑

i=0

Wmi g(xi) (2.15)

P =2n∑

i=0

W ci {g(xi)− x}{g(xi)− x}T (2.16)

2.6.3.2 Update

Fur den Updateschritt wird analog die nichtlineare Messfunktion z = h(x) auf dieSigmapunkte angewandt und auch hier der Mittelwert z und die Messungskovari-anzmatrix Pzz gebildet.

z =2n∑

i=0

Wmi h(xi) (2.17)

Pzz =2n∑

i=0

W ci {h(xi)− z}{h(xi)− z}T (2.18)

Zusatzlich wird noch eine Kreuzkorrelationsmatrix aus Zustand und Messung gebil-det, die benutzt wird, um den Kalman-Gain K zu bestimmen.

Pxz =2n∑

i=0

W ci {g(xi)− x}{h(xi)− z}T (2.19)

K = PxzP−1zz (2.20)

Der Rest ist analog zum klassischen Kalman-Filter. Zustand und Kovarianz werdenjeweils, gewichtet durch den Kalman-Gain, basierend auf der Differenz von Vorher-sage und Messung, korrigiert.

x = x+K(z− z) (2.21)

P = P−KPzzKT (2.22)

22 2. Grundlagen

3. Analyse

In diesem Kapitel wird die zu losende Aufgabe noch einmal genauer beschriebenund es wird auf die Anforderungen und Randbedingungen der Problemstellung ein-gegangen.

Im zweiten Abschnitt werden Arbeiten vorgestellt, die sich mit einer ahnlichen The-matik beschaftigt haben, und die dabei geltenden Randbedingungen, verwendeteMethoden und erreichte Ergebnisse diskutiert.

3.1 Problemstellung, Anforderungen und Rand-

bedingungen

Wie bereits in Kapitel 1.2 kurz erlautert, bestand die Aufgabe darin, aus denSensoren- und GPS-Daten der im Fahrzeug installierten IMU6-Platine moglichstgenau die Pose zu bestimmen. Das heißt, es sollen Position, Lage im Raum und dieGeschwindigkeiten in die verschiedenen Richtungen bestimmt werden.

Dem Kooperationspartner Hella Aglaia war es dabei besonders wichtig, dass dieLangsgeschwindigkeit (Vorwartsgeschwindigkeit) und die Gierrate bzw. Lenkwin-kelanderung bestimmt werden. Diese Großen sollten als Eingabe fur das zur Verfu-gung gestellte Spurhaltesystem (LDW) dienen.

Folgende Werte wurden seitens Hella Aglaia fur die gewunschte Genauigkeit spezi-fiziert:

• Gierrate

0,1 Grad/s Auflosung

maximaler Fehler von 0,3 Grad/s bei 0 Grad/s tatsachlicher 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 Windschutzscheibezum Einsatz kommen. Fur die Tests wurde die Seitenscheibe des Spirit of Berlingewahlt, da die Frontscheibe wegen ihrer Bedampfung die Signale stark abschwacht.Das System sollte außerdem im Idealfall in der Lage sein, kurze GPS-Ausfalle (bisetwa 30 Sekunden) zu uberbrucken und dabei die geforderte Messgenauigkeit einzu-halten.

Zusatzlich zu den Anforderungen der Kooperationsfirma gab es eigene Anforderun-gen innerhalb des Projekts.

• Integration der entwickelten Software in das vorhandene, auf OROCOS basie-rende System

Entwicklung eines Input-/Sensormoduls fur die IMU6-Platine

Entwicklung eines Kalman-Filter-Moduls

• Evaluation des verbauten GPS-Moduls und Vergleich mit dem Referenzsystem

• Evaluation der Nutzbarkeit fur die Lokalisierung unabhangig vom Spurhalte-system

3.2 Existierende Losungsansatze

3.2.1 Existierende Arbeiten

Van der Merwe und Wan stellen in ihrem Artikel Sigma-Point Kalman Filters forIntegrated Navigation [VDMWa04] eine Moglichkeit vor, GPS- und IMU-Daten zukombinieren. Dabei wird eine Variante eines Unscented Kalman-Filters verwendet,das sogenannte Sigma-Point-Kalman-Filter (SPKF). Fur den verwendeten Algo-rithmus werden zwei mogliche Implementationen angefuhrt, das square-root UKF(SRUKF) und das square-root central difference Kalman filter (SRCDKF), die sichlediglich in der Art und Weise unterscheiden, wie die Sigmapunkte ausgewahlt wer-den.

Als Versuchstrager kam eine Experimentierplattform auf Basis eines unbemanntenModellhelikopters zum Einsatz, der mit einer IMU, einem 10Hz-GPS, einem baro-metrischen Hohenmesser und einem Embedded PC ausgestattet war.

Diese Arbeit wird hier exemplarisch angefuhrt, weil die anderen Autoren ahnlicheModelle und Filter benutzten. Auf die speziellen Unterschiede in weiteren Arbeitenwird am Ende dieses Kapitels eingegangen.

3.2.2 Zustandsraum und dynamisches Modell

Es wurde von Van der Merwe und Wan ein 16-dimensionaler Zustandsvektor be-nutzt, bestehend aus 3D-Position, 3D-Geschwindigkeit, 4D-Lage-Quaternion, 3D-Beschleunigungsfehler und 3D-Drehratenfehler. Der Index b bei den Fehlergroßensteht dabei fur Bias.

x =(

pT vT qT aTb ωT

b

)

(3.1)

3.2. Existierende Losungsansatze 25

3.2.2.1 Ubergangsfunktion

Zunachst mussen die Messungen (Beschleunigungen und Drehraten) um Rauschenund Fehler korrigiert werden:

a = a− ab −wa (3.2)

ω = ω − ωb −Cbnωc −wω (3.3)

Die MatrixCbn = (Cn

b )T ist die transponierte Richtungskosinusmatrix aus Kapitel 2.5

und transformiert einen Vektor aus dem Navigation-Frame in den Body-Frame. ωc isthier die durch den Coriolis-Effekt gemessene Drehrate der Erde. Sie ist eigentlich eineFunktion des aktuellen Breitengrads, kann aber auf kurzen Strecken ohne weiteresals konstant angenommen werden.

Position und Geschwindigkeit lassen sich durch die folgenden Gleichungen in denFolgezustand transformieren:

pt = pt−1 + pt−1 · dt (3.4)

vt = vt−1 + vt−1 · dt (3.5)

und haben die Ableitungen:

p = v (3.6)

v = Cnb a+

(

0 0 g)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, dieAbleitung ist dabei eine Gauß’sche Zufallsvariable.

Die Winkeldifferenzen fur Roll-, Nick- und Gierwinkel sind ein Produkt aus derkorrigierten Drehrate ω und der vergangenen Zeit dt:

∆φ = ωroll · dt (3.8)

∆θ = ωpitch · dt (3.9)

∆ψ = ωyaw · dt (3.10)

Das Quaternion-Update geschieht folgendermaßen:

qt =

[

I(cos(s))− 1

2Φ∆

sin(s)

s

]

qt−1 (3.11)

s =1

2

√

(∆φ)2 + (∆θ)2 + (∆ψ)2 (3.12)

26 3. Analyse

wobei die schiefsymmetrische Matrix Φ∆ definiert ist als:

Φ∆ =

0 ∆φ ∆θ ∆ψ−∆φ 0 −∆ψ ∆θ−∆θ ∆ψ 0 −∆φ−∆ψ −∆θ ∆φ 0

(3.13)

3.2.2.2 Beobachtungsfunktion

Bei der Beobachtung fließen die Messungen des GPS-Sensors und des Hohenmessersin die Korrektur des Filters ein. Die Gleichungen fur die Beobachtungsfunktion sind:

pGPSt = pt−k +Cn

b rgps +wp (3.14)

vGPSt = vt−k +Cn

bωt−k × rgps +wv (3.15)

pt−k,vt−k bezeichnen die Position und Geschwindigkeit vor k Schritten, gemessenvon der um k Zeiteinheiten zeitlich verschobenen IMU-Messung. Diese zeitliche Ver-schiebung um k Samples kann bei GPS-Empfangern mit geringer Latenz ignoriertwerden.

rgps ist die Position der GPS-Antenne relativ zu den Inertialsensoren, muss alsoauf die Position addiert werden. wp und wv sind wiederum normalverteilte Zufalls-variablen, die das Messrauschen reprasentieren.

3.2.3 Ergebnisse und Vergleich mit weiteren Arbeiten

Van Der Merwe und Wan haben in ihren Experimenten deutliche Vorteile der ver-wendeten Losung im Vergleich zum EKF festgestellt. Der verwendete SPKF mitAusgleich der GPS-Verzogerung zeigte eine Verringerung des Fehlers von uber 30%bei Position, Geschwindigkeit, Rollwinkel und Nickwinkel und eine 65%-ige Verbes-serung beim Gierwinkel.

Zhang, Gu, Milios und Huynh zeigen in [ZGMH05] ebenfalls eine Integration vonIMU und GPS. Es kommt derselbe 16-dimensionale Zustandsvektor zum Einsatz(Gleichung 3.1). Hier wurde als Versuchstrager ein PKW verwendet, der mit ei-nem 1 Hz GPS-Empfanger, mit 20 Hz getakteter IMU und digitalem Kompassausgestattet war. Die Sensordaten wurden hier mit einem Wavelet-Filter vorver-arbeitet. Die Autoren betonen dabei, dass die IMU nur sehr eingeschrankt Ausfalledes GPS verkraften kann, da durch die verschiedenen Fehlereinflusse und das Rau-schen der Inertialsensoren die Richtung und Position sehr schnell von der Realitatabweichen. Es wurde jedoch auf einer kurzen Strecke auf einem Parkplatz mit GPS-Empfang ein zufriedenstellendes Ergebnis erzielt, quantifiziert wurde der Fehler,vermutlich wegen fehlender Referenz, jedoch nicht.

In Kapitel 10 von [Wend07] findet sich ebenfalls eine Anwendung fur eine Integrati-on von IMU und GPS. Bei dem Versuchstrager handelt es sich um ein unbemanntesFlugobjekt (UAV) mit 4 Rotoren, von denen jeweils zwei auf orthogonal zueinan-der stehenden Achsen angebracht sind. Diese Flugobjekte sind auch bekannt als

3.3. Schlussfolgerungen 27

Quadrokopter. Interessant an diesem Anwendungsfall ist die Umschaltung bei ei-nem GPS-Ausfall auf ein zweites Filter, welches Position und Lage ohne die externeQuelle des GPS schatzt. Ist der GPS-Empfang vorhanden, so werden die Sensorenkontinuierlich kalibriert. Das Modell wurde sowohl simuliert, als auch einem prakti-schen Test unterzogen. Hierbei wurde eine sehr gute Lageschatzung erreicht, sofernentweder GPS-Empfang gegeben war oder die durch die Integration der Beschleuni-gungsmesser entstandenen Fehler in der Lage durch Messen des Gravitationsvektorsund des Erdmagnetfelds ausgeglichen wurden.

Auch El-Sheimy und Abdel-Hamid betonen in ihrem Artikel [AHAESL06] die Pro-blematik bei der Zustandsschatzung im Falle von GPS-Ausfallen. Das Kalman-Filterarbeitet dabei nur im Vorhersagemodus, es gibt kein Update durch externe Messun-gen (GPS). Die Autoren stellen im Artikel ein Fuzzy Inference System (FIS) vor,welches die Vorhersagen des Kalman-Filters korrigiert. Das FIS ist ein mehrschich-tiges Netzwerk und basiert auf gelernten Regeln und Gewichten.

3.3 Schlussfolgerungen

Mit dem Thema der Integration von GPS und Inertialsensoren haben sich be-reits einige Autoren beschaftigt, meist kam in den untersuchten Arbeiten auch einKalman-Filter zum Einsatz. Die gestellte Aufgabe lauft darauf hinaus, ein Kalman-Filter zu entwickeln, welches idealerweise in der Lage ist, die GPS-Position zu verbes-sern und Ausfalle zu kompensieren. Die dabei erreichten Ergebnisse variieren, lassenjedoch hoffen, die gestellten Anforderungen zu erfullen, sofern die Inertialsensorennicht zu stark driften und das entwickelte dynamische Modell die Realitat gut wi-derspiegelt. Fur 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 undderen Bestandteile und schematischer Aufbau dargelegt. Danach wird die Integrationin 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 Auftragvon Analog Devices hergestellte Entwicklungsplatine, die bereits in verschiedenenForschungsprojekten im Bereich Navigation zum Einsatz kam.

Sie besteht aus einer Platine mit den Abmessungen 95x55 mm, auf der ein CPU-Modul, ein GPS-Modul, ein digitaler Kompass, ein 3-Achsen-Beschleunigungsmesserund 2 Gyroskope (Z-Achse und kombinierte X-/Y-Achse) verbaut sind.

Die Spannungsversorgung fur das Board erfolgt uber eine 2-polige Stiftleiste mit5 Volt Gleichspannung. Auf dem Board befinden sich weitere Spannungsregler, umdie 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 fur Kernspannung

Schnittstellen: SPI, PPI, CAN, TWI, JTAG, Ethernet, SPORT, UART

• GPS-Modul

u-blox LEA-5T (50 Kanal GPS-/GALILEO-Empfanger)

Ausgabe im Format NMEA (ASCII) und UBX (binar)

Erreichbare horizontale Genauigkeit: <2,5 m (laut Hersteller)

30 4. Hardware

GPS-

Antennen-

anschluss

CPU-ModulJ TAG-

Port

UART-

Port

SD-Card-

Slot

Modul-

verbinderIO-

PortsZ-Achsen-

Gyroskop

Reset-

Taster

Spannungs-

versorgung

Abbildung 4.1: Oberseite der IMU6 Platine mit Beschriftung der Komponenten. Aufder Unterseite befinden sich weitere Sensoren und das GPS-Modul (Foto: Bluetech-nix Mechatronische Systeme GmbH)

• Beschleunigungssensor

ADXL340

Hersteller: Analog Devices

3-Achsen: X, Y, Z

Auflosung: 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)

Auflosung: 12-Bit fur interne und externe Spannungen

Temperatursensor

zwei Eingange fur analoge Spannungen der X/Y-Achse

IDG-300

Hersteller: Invensense

2-Achsen: X,Y (Roll-, Nickrate)

analoge Spannungsausgange

• Magnetometer

PNI11096 (PNI Corp.)

4.1. Die Hardwareplattform IMU6 31

• Schnittstellen

TTL-Level UART-Port fur serielle Kommunikation als 4-pol. Pin-Header

SD-Card-Slot

diverse IO-Anschlusse auf Micromatch-Steckverbindern

RP-SMA-Antennenanschluss fur das GPS-Modul

• weitere Komponenten

drei LEDs: rot, orange, grun

Reset-Taster

4.1.2 Aufbau und Funktionsweise

Wie in Abbildung 4.2 erkennbar, sind die Inertialsensoren uber einen SPI-Bus(Serial Programming Interface) und das GPS-Modul uber die zweite UART-Schnitt-stelle an das CPU-Modul angebunden.

Die analogen Signale des kombinierten X-/Y-Gyroskop IDG-300 sind an die Analog-Digital-Umsetzer-Eingange des Z-Achsen-Gyroskop ADIS16100 gefuhrt und werdenvon diesem mit 12-Bit-Auflosung digitalisiert.

Es befindet sich eine JTAG-Schnittstelle zur Programmierung und zum Debuggingder Software auf der Platine. Zu diesem Zweck wurde ein USB-zu-JTAG-Debugger(ADZS-HPUSB-ICE) mitgeliefert.

Gyro

X, Y

Gyro Z

& ADC

Accele-

rometer

X, Y, Z

GPS-

Modul

ISP BusISP Bus

X (analog) Y (analog)

UART 1

Magne-

tometer

TCM-BF537

CPU-

Modul

IDG-300

ADIS16100ADXL340PNI11096

RESET

UART 0

Abbildung 4.2: IMU6-Aufbau im Schema

Relativ fruh im Verlauf des Projekts wurde ein Fehler im Schaltplan deutlich. DieRESET-Leitung ist sowohl mit dem GPS-Modul als auch mit dem Magnetome-ter verbunden. Das fuhrte dazu, dass das notwendige Schalten auf Massepotentialder RESET-Leitung zum Auslesen des Sensors (etwa 100 Mal pro Sekunde) einenNeustart des GPS-Moduls bewirkte. Dadurch war das GPS-Modul beim simultanen

32 4. Hardware

Auslesen des Sensors nicht nutzbar. Dieser Designfehler konnte jedoch am fertigenModul behoben werden, indem die Verbindung der RESET-Leitung zum GPS-Modulgetrennt wurde.

4.2 Versuchsaufbau im autonomen Fahrzeug

Zunachst wurde die IMU6 allein auf diversen Testfahrten ausgiebig erprobt. Da-zu wurde meist ein Notebook verwendet, welches uber den USB-Port sowohl die5-Volt-Versorgungsspannung fur die Platine lieferte, als auch die Daten mitloggenkonnte. Fur diesen Anschluss wurde vom Verfasser dieser Arbeit ein Adapterkabelzur Stromversorgung hergestellt. Außerdem wurde ein Wandler vom TTL-Logikleveldes Boards auf eine RS-232-kompatible serielle Schnittstelle aus einem Bausatz zu-sammengebaut und in Kombination mit einem weiteren Wandler von der RS-232-Schnittstelle auf einen USB-Anschluss eingesetzt. Diese Vorarbeiten waren notig,weil mit der Entwicklungsplatine keinerlei Anschlusskabel mitgeliefert wurden.

4.2.1 Einbau und Anschluss an das vorhandene System

IMU 6 Board

TTL<-> RS-232

Adapter

Comtrol Devicemaster

RS-232 <-> Ethernet Server

Orocos RTT

BtImu6 Modul

UART (TTL) RS-232

(Serial Port)

Ethernet

Cassandra

LDW

GPS

Inertialsensoren

Velocity Input

Yaw Rate Input HellaLaneDetection

Modul

Abbildung 4.3: Schema des Datenflusses zwischen den Komponenten im Fahrzeug

Um die Platine mit dem Referenzsystems im Fahrzeug vergleichen zu konnen, wurdediese in den Kofferraum des Spirit of Berlin eingebaut.

Um Storeinflusse der umfangreichen Elektronik im autonomen Fahrzeug auf die emp-findlichen Sensoren der IMU auszuschließen bzw. zu minimieren, wurde ein Trager-gehause fur die Platine aus Aluminium mit Aussparungen fur die Kabel konstruiert.

Auch in diesem Aufbau wurden zunachst Testfahrten ohne Anschluss an das imFahrzeug laufende OROCOS-System durchgefuhrt.

Um letztendlich die Anbindung zu bewerkstelligen, wurde anstelle des vorher be-nutzten RS-232-zu-USB-Wandlers der serielle Ausgang der IMU6 an einen RS-232-Ethernet-Gateway (Comtrol Device-Master) angeschlossen, der die seriellen Datenuber einen TCP-Port zur Verfugung stellt. Die Magnetantenne fur das GPS wurdeauf dem Dach des Spirit of Berlin befestigt.

Der schematische Aufbau und Datenfluss ist in Abbildung 4.3 zu sehen. Im ORO-COS-Modul werden die Daten vom TCP-Port eingelesen und kontinuierlich weiter-verarbeitet (siehe Abschnitt 5.2).

4.3. Schlussfolgerungen 33

Die Ausgabe der aktuellen Geschwindigkeit und Gierrate fur das LDW-Systemvon Hella Aglaia erfolgt wiederum uber einen TCP-Port. Die Software des LDW-Systems (Cassandra) lauft auf einem weiteren Rechner, der uber Ethernet mit demOROCOS-Rechner verbunden ist.

4.3 Schlussfolgerungen

Die in dieser Arbeit genutzte Hardwareplattform IMU6 lasst sich uber den JTAG-Port frei programmieren und ermoglicht es so, die integrierten Sensoren auszulesenund die gewonnenen Daten uber einen seriellen Port auszugeben.

Die verbaute CPU ist mit dieser Aufgabe keinesfalls ausgelastet und es ware sogardenkbar, das in dieser Arbeit implementierte Filter auf diese Hardwareplattformzu portieren, wenn man auf effiziente Implementierungen der benotigten mathe-matischen Operationen zuruckgreift und eventuell Abstriche bei der Frequenz derSensormessungen in Kauf nimmt.

34 4. Hardware

5. Software

Die im Rahmen dieser Arbeit geschriebene Software teilt sich auf in die Firmwarefur die IMU6-Plattform und die Software-Module fur das Orocos-Framework desautonomen Fahrzeugs Spirit of Berlin.

Die Firmware fur den Mikroprozessor ADSP-BF537 wurde in C geschrieben undmit der Visual DSP IDE von Analog Devices entwickelt.

Die OROCOS-Module wurden in C++ implementiert.

5.1 Firmware der Hardwareplattform

5.1.1 Aufbau und Funktionsweise

Der Aufbau des Programms lasst sich durch einen Zustandsautomaten beschreiben,der im Ausgabemodus die Sensoren auf der Plattform nacheinander ausliest. Die-ses ist der Hauptarbeitszustand des Automaten. Die ausgelesenen Daten werdenin Nachrichten verpackt und mit einem Zeitstempel versehen. Zusatzlich wird einePrufsumme berechnet und ans Ende der Nachricht angehangt.

Fur die Abfrage der Daten uber den SPI-Bus agiert die CPU als sogenanntes SPI-Master-Device, die Sensoren sind SPI-Slaves. Die Datenkommunikation wird dabeidurch periodisches Umschalten der SCK -Leitung durch das Master Device getaktet,uber die MOSI (Master Out Slave In)-Leitung werden Daten oder Befehle gesendetund uber die MISO (Master In Slave Out)-Leitung empfangen. Ein grundlegenderTreiber fur die SPI-Kommunikation wurde mit der Hardware mitgeliefert, es warenjedoch einige Anderungen notig, um alle Informationen der Sensoren zuverlassig mithoher Datenrate abzufragen.

Die Initialisierung wurde in der Art implementiert, dass im Startzustand auf einenSatellitenfix 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 spatere Auswerten der Logfiles und ermoglicht es zudem, dieDaten des Referenzsystems genau mit den aufgezeichneten Logdaten zu vergleichen.

36 5. Software

Befehs-

moduis

Ausgabe-

modus

Warte auf

GPS-Fix

Synchro-

nisieren

KalibrierenPass-

Through

"toggle

GPS""toggle

SENS"

"cal"

"exit"

"pass"

"command"

"command"

"exit"

(no fix)

(no timepulse)

(fix) (timepulse)

Abbildung 5.1: Zustande des in der Firmware implementierten Automaten

Im laufenden Programm kann durch einen Befehl in einen Kommandomodus ge-wechselt werden, in dem weitere Funktionen zur Verfugung stehen. Es kann dieAusgabe der GPS- und Sensor-Nachrichten einzeln ausgestellt werden und in einenKalibiriermodus gewechselt werden, der die festen Offsets der Sensoren bestimmenund speichern kann. In einem weiteren Modus werden alle Nachrichten direkt zumGPS-Modul an der zweiten seriellen Schnittstelle weitergeleitet und alle von dortkommenden Nachrichten direkt ausgegeben. Dieser Modus ermoglicht die Konfigu-ration des GPS-Moduls uber die vom Hersteller mitgelieferte Software.

Das fertige Programm ist 145 KB groß und kann entweder direkt uber JTAG in denFlash-Speicher geschrieben oder uber einen auf dem System installierten Bootloaderin den Arbeitsspeicher geladen und von dort aus gestartet werden. Die zweite Me-thode hat den Vorteil, dass die begrenzte Lebensdauer des Flash-Speichers dadurchnicht reduziert wird.

5.1.2 Nachrichtenformat

Das Format der Nachrichten ist dabei ahnlich aufgebaut wie das von GPS-Empfangern bekannte NMEA-Nachrichtenformat. Nachrichten fangen dabei immermit einem $-Zeichen an, gefolgt von einem Nachrichtentyp-Bezeichner, es folgen be-liebig viele durch Kommata separierte Nachrichtenfelder und am Ende der Nachrichtein ∗-Zeichen, gefolgt von der Prufsumme.

Es wurde bewusst ein Klartextformat (ASCII-Zeichensatz) verwendet, um den Da-tenstrom direkt mitlesen und verifizieren zu konnen und um es zu ermoglichen, dieGPS-Daten auch ohne Konvertierungsprogramm direkt in Standardsoftware zu ver-arbeiten. 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 37

[15:06:51.892 7083692] SENS: $SENS,23.199498,57.997995,-22.682700,0.078125,0.000000,-1.078125,14,14,0.00000,37.93*3A

[15:06:51.897 7083697] GPS: $GPGLL,5233.62154,N,01326.01603,E,130650.00,A,A*69

[15:06:51.901 7083701] SENS: $SENS,23.199498,57.997995,-22.194900,0.078125,0.000000,-1.062500,12,8,0.00000,37.64*2E

[15:06:52.096 7083874] 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 ubertragenen Nachrichten

Dabei stehen HH, MM, SS, FF fur die aktuelle Zeit in Stunden, Minuten, Sekunden,Millisekunden. TTT steht fur die vergangenen Millisekunden seit Start des Moduls.

Die Nachrichten werden mit einer Bitrate von 115.200 Bit/s, 8 Datenbits, einemStoppbit und ohne Paritat uber die serielle Schnittstelle ausgegeben. In Abbil-dung 5.3 ist ein Auszug der Nachrichten zu sehen.

5.2 Kalman-Filter, Orocos-Modul

5.2.1 Bestandteile

Die Softwaremodule in der OROCOS-Software bestehen aus einem Ein-/Ausga-bemodul, welches Nachrichten empfangt, verarbeitet und ausgibt, und dem Kal-man-Filter-Modul, welches die reine Funktion des Filterns ubernimmt und damitunabhangig vom verwendeten Modell ist. Somit kann es auch wiederverwendet undvon anderen Modulen genutzt werden. Beispielweise ist der Einsatz des Filters zumLernen der Gewichte eines neuronalen Netzes denkbar.

Das Kalman-Filter ist als Template implementiert, welches sich durch Angeben ei-ner Ubergangsfunktion, einer Beobachtungsfunktion und den Standardparameterninstanziieren lasst.

Die Funktionen fur Zustandsubergang und Beobachtung (Messung) beschreibenschließlich das dynamische System. Sie wurden speziell fur den vorliegenden An-wendungsfall 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 Prufsumme

• Aufruf der Vorhersage- und Update-Funktionen des Filters

• Bereitstellen der Daten uber einen Ausgabe-Port in einem zum restlichen Pro-jekt kompatiblen Format (EgoState-Port)

Der Parser arbeitet zeichenorientiert, und kann als Zustandsautomat angesehen wer-den, der solange Zeichen einliest, bis entweder ein Sonderzeichen (Feldtrenner, En-de/Beginn einer Nachricht, Zeilenumbruch,...) auftritt oder die maximale Anzahl anZeichen uberschritten wird. Wenn das passiert, oder wenn die Prufsumme nicht veri-fiziert 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 derNachricht in Variablen gespeichert. Tritt irgendwo im Datenstrom ein unerwartetesZeichen auf, wird der bisher gelesene Teil der Nachricht ebenfalls verworfen und inden Anfangszustand zuruckgekehrt.

Wenn die Nachricht korrekt eingelesen wurde und die notigen Daten extrahiert wur-den, kann die Funktion des Filters mit den entsprechenden Eingabedaten aufgerufenwerden.

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 ahnlich zudem in Abschnitt 2.6.3 beschriebenen. Der Ubergang lauft dabei analog zur Beob-achtung ab.

Der implementierte UKF orientiert sich am Central-Difference-Kalman-Filter(CDKF), Hauptunterschied zu dem in Abschnitt 2.6.3 vorgestellten UKF ist, dassder Systemzustand fur beide Schritte des Filters unterschiedlich erweitert wird. Furdie Vorhersage wird der Zustand x mit dem Ubergangsrauschen w und fur die Beob-achtung mit dem Messrauschen v erweitert (siehe Gleichung 5.1). Ebenso werden dieKovarianzmatrizen P jeweils einzeln um die Fehlerkovarianzmatrizen Q,R erweitert(siehe Gleichung 5.2).

xw =(

xT wT)T

xv =(

xT vT)T

(5.1)

Pw =

(

PT 00 QT

)

Pv =

(

PT 00 RT

)

(5.2)

Das macht es erforderlich, die Sigmapunkte und Gewichte jeweils einmal fur diebeiden Schritte zu berechnen. Die Berechnung erfolgt dabei aber wie in den Glei-chungen 2.12 und 2.13 angegeben. Auch die Vorhersage und der Updateschritt sindanalog 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 Zustandsund werden somit in der Ubergangsfunktion benutzt. Die externe Positionsbestim-mung durch GPS wird dann jeweils im Updateschritt genutzt, um die Vorhersagezu korrigieren.

Weil das GPS mit einer Frequenz von 1 Hz und die Inertialsensoren mit 100 Hzmessen, werden jeweils 100 Vorhersagen zwischen den Updateschritten getroffen.

5.2.4 Ubergangsfunktion

Um die Zustandsvektoren und Kovarianzmatrizen klein zu halten, wurde nach demanfanglichen Experimentieren mit großeren Zustanden ein vereinfachter Zustands-vektor entworfen, der aus den folgenden 8 Dimensionen besteht:

5.2. Kalman-Filter, Orocos-Modul 39

• 2D-Position im EN(U)-Koordinatensystem p

• 1D-Vorwartsgeschwindigkeit v

• 4D-Lage-Quaternion q

• 1D-Vorwartsbeschleunigungsfehler baccx

x =(

pT v qT baccx)T

(5.3)

Die dritte (Hoch-)Koordinate im ENU-Koordinatensystem soll hier außer Acht ge-lassen werden, da in erster Linie die Verschiebung in lateraler und longitudinalerRichtung interessiert und außerdem angenommen werden kann, dass sich das stra-ßengebundene Fahrzeug vorwiegend in dieser Ebene bewegt. Es wird also von einerEbene fur die moglichen Koordinaten des Fahrzeugs ausgegangen.

Bewusst wurde dabei auch auf das Einbeziehen der Hohe und der Geschwindigkeitenin der seitlichen Y-Achse und der nach oben zeigenden Z-Achse verzichtet, weil dieBestimmung dieser aufgrund einiger Schwierigkeiten mit den X- und Y-Gyroskopenohnehin problematisch war (mehr dazu in Abschnitt 6.1.2).

Der erweiterte Zustand enthalt dabei zusatzlich zu den oben angegebenen 8 Kom-ponenten weitere 8 Rauschkomponenten.

Als Vorbedingung fur den Zustandsubergang werden die folgenden Hilfsgroßen be-rechnet (siehe Abbildung 5.4):

• Radius der aktuellen Lenkkurve: r = vgyr

z

• Richtungsvektor zum Zeitpunkt t-1: d =(

cos(Ψ) sin(Ψ))T

• Winkeldifferenz: ∆Ψ = gyrz · dt

• Neuer Winkel zur X-Achse: Ψ′ = Ψ+∆Ψ

• Richtungsvektor zum Zeitpunkt t: d′ =(

cos(Ψ′) sin(Ψ′))T

• Hilfsvektor, orthogonal zur Richtung: s =(

d(1) −d(0))T · r

• orthogonal zur neuen Richtung: s′ =(

d′(1) −d′(0))T · r

• Positionsdifferenz: ∆p = s′ − s.

Der Zustandsubergang SimplifiedCarMotionModel::transition() wurde nachden folgenden Ubergangsgleichungen implementiert:

pt = pt−1 +∆p (5.4)

vt = vt−1 + (accx + baccxt ) ∗ dt (5.5)

baccxt = baccxt−1 + wbaccx (5.6)

40 5. Software

p

d'Δp

r

d

ΔΨΨ

d

d'

ΔΨΨ'

Δp

s

s'

Ψ' = Ψ + ΔΨ

d' = (cos(Ψ'), sin(Ψ'))

s = (d(1), -d(0)) * r

s' = (d'(1), -d'(0)) * r

Δp = s' - s

-s

North

(y)

East

(x)

Abbildung 5.4: Berechnungen zum Positionsupdate

Wobei accx dabei die gemessene Beschleunigung in der X-Achse (Vorwartsbeschleu-nigung) ist.

Das Update der Quaternion q verlauft nach folgendem Schema: Es wird zunachst inEuler-Winkel umgewandelt und die neue Quaternion mit den aktualisiertenWinkeln,gegeben durch Gyroskopmessungen mal Zeitdifferenz dt, gebildet. Veranschaulichtdurch 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::ob-

serve() implementiert und beschreibt, wie sich die GPS-Messungen aus dem umRauschen erweiterten Zustand zusammensetzen.

Der Messvektor xobs ist 8-dimensional und enthalt die folgenden Messungen vomGPS:

• 2D-Position im EN(U)-Koordinatensystem: pobs

• 1D-Vorwartsgeschwindigkeit: vobs

• 4D-Lage-Quaternion: qobs

• 1D-Vorwartsbeschleunigungsfehler: baccxobs

5.3. Zusammenfassung und Resultate 41

xobs =(

pTobs vobs qT

obs baccxobs

)T(5.7)

Die Beobachtungsfunktion addiert jeweils das Messrauschen w aus dem erweitertenZustand zu jedem Vektor:

pobs = p+wp (5.8)

vobs = v + wv (5.9)

qobs = q+wq (5.10)

baccxobs = baccx + wbaccx(5.11)

Diese Funktionen beschreiben die Annahme, dass die vom GPS-Empfanger bestimm-te Position, Geschwindigkeit und Ausrichtung genau dem aktuellen Zustand plusMessrauschen entsprechen.

5.3 Zusammenfassung und Resultate

Als Teil dieser Diplomarbeit wurden Softwaremodule entwickelt. Ein Softwaremo-dul lauft auf der IMU6-Platine und liest dort permanent die Sensordaten aus. DieseSensordaten werden in ein spezielles Nachrichtenformat verpackt und uber die seri-elle UART-Schnittstelle der Platine ausgegeben.

Der zweite Teil der Software lauft auf dem OROCOS-System des autonomen Fahr-zeugs und empfangt uber TCP/IP die Nachrichten der Platine. Danach erfolgt dieFilterung der Daten mit einem fur diesen Zweck entwickelten Unscented Kalman-Filter, bestehend aus dem eigentlichen UKF-Algorithmus und den Ubergangs- undBeobachtungsfunktionen, die das dynamische System modellieren.

Im Verlauf der Entwicklung kam es haufig dazu, dass das Filter im Test nicht zueinem Ergebnis kam. Dabei divergierten die Werte der Kovarianzmatrix. Vermutlichhingen diese Fehler mit der damals unvollstandigen Modellierung des recht kom-plexen Zustandsraummodells mit bis zu 20-dimensionalen Zustandsvektoren zusam-men. Nach einer Vereinfachung auf die in diesem Kapitel beschriebenen Funktionenund Zustande, traten diese Schwierigkeiten kaum noch auf.

42 5. Software

6. Evaluierung

Die Evaluierung setzt sich zusammen aus einem ersten Teil, in dem die Sensoren unddas GPS-Modul einzeln auf ihre Qualitat hin untersucht werden, und einem zweitenTeil, der die Ergebnisse der Integration und des implementierten Kalman-Filtersenthalt.

6.1 Evaluierung der Sensorqualitat

6.1.1 GPS-Daten

Um die Qualitat des verbauten GPS-Moduls zu bestimmen, wurden Testfahrten un-ter verschiedenen erschwerten Bedingungen, im Stadtverkehr zwischen hohen Hau-serfronten, unter dichtem Baumbewuchs und unter nahezu Idealbedingungen aufdem Rollfeld des stillgelegten Flughafens Tempelhof in Berlin unternommen.

Exemplarisch sind hier zwei Testfahrten auf den von der Firma Hella Aglaia vorge-schlagenen Test-Strecken vorgestellt. Dabei befand sich die Antenne des u-blox -GPSim Innenraum des Fahrzeugs hinter einer Seitenscheibe um dem spateren Einsatz-szenario als integriertes Spurhaltesystem moglichst nahezukommen.

Die Teststrecke 1 besteht großtenteils aus Autobahn und Landstraße mit wenigBaumbewuchs. Die Qualitat des empfangenen GPS-Signals ist in Abbildung 6.1zu sehen.

Vom GPS-Modul selbst erhielten wir die folgenden Werte fur die (geschatzte) Ge-nauigkeit auf der Strecke:

Maximaler Fehler: 4,24 m/sMinimaler Fehler: 0,23 m/sMittelwert: 1,3372 m/sVarianz: 2,4210 m/sStandardabweichung: 1,5560 m/s

Die 2. Teststrecke fuhrte auf der Berliner Stadtautobahn A 100 aus Neukolln kom-mend vorbei am Flughafen Tempelhof bis zur Seestraße in Wedding. In Abbil-

44 6. Evaluierung

0

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

0 500 1000 1500 2000 2500 3000 3500 4000 4500 5000

fix/n

um

ber of sate

llite

s/a

ccura

cy [m

/s]

time [s]

teststrecke1-2009-12-09+001 IMU6 GPS Status

fixnum. satellites

speed accuracy estimate [m/s]

Abbildung 6.1: Anzahl sichtbarer Satelliten und geschatzte Ungenauigkeit der Ge-schwindigkeit des GPS-Moduls auf Teststrecke 1

0

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

0 200 400 600 800 1000 1200 1400

fix/n

um

ber of sate

llite

s/a

ccura

cy [m

/s]

time [s]

teststrecke2-2009-12-09+001 IMU6 GPS Status

fixnum. satellites

speed accuracy estimate [m/s]

Abbildung 6.2: Anzahl sichtbarer Satelliten und geschatzte Ungenauigkeit der Ge-schwindigkeit des GPS-Moduls auf Teststrecke 2

dung 6.2 ist gleich zu Anfang ein langer Ausfall des GPS-Empfangs festzustellen.Zuruckfuhren kann man diesen Ausfall wohl auf einen 1,7 km langen Tunnel zuBeginn der Strecke und die Schwierigkeiten, danach wieder einen Satellitenfix zubekommen.

Die Statistik fur die vom Modul geschatzten Fehler fur die 2. Teststrecke (Abschnitteohne GPS-Fix ausgenommen):

6.1. Evaluierung der Sensorqualitat 45

Maximaler Fehler: 5,49 m/sMinimaler Fehler: 0,33 m/sMittelwert: 1,4176 m/sVarianz: 2,9473 m/sStandardabweichung: 1,7167 m/s

Zur besseren Einschatzung der Fehler wurden dann auf einer anderen Teststreckemehrere GPS-Empfanger gleichzeitig geloggt und anschließend ausgewertet. Wieman an den Ergebnissen erkennen kann, war auf dieser Strecke der Empfang sehrgut. Es zeigten sich dabei die folgenden Abweichungen des u-blox -GPS zum Refe-renzsystem Applanix POS LV 220 :

Maximale Abweichung: 0,31 m/sMinimale Abweichung: - 0,28 m/sMittelwert: 0,0151 m/sVarianz: 0,0236 m/sStandardabweichung: 0,0809 m/s

Das zeigt, dass die Vorgaben von Hella Aglaia bezuglich der gewunschten Geschwin-digkeitsgenauigkeit, zumindest unter fur den GPS-Empfang guten Bedingungen, inder Regel eingehalten werden konnen.

Die Ergebnisse zeigen hier, dass man zwar haufig eine gute Positionierung mit demu-blox -GPS erhalten kann, teilweise aber auch Totalausfalle auftreten. Es lasst sichjedoch sehr gut anhand der eigenen Einschatzung des Moduls auf die erzielte Genau-igkeit schließen und es konnen so von vornherein gelieferte Werte, die unzureichendgenau sind, verworfen werden.

6.1.2 Gyroskopdaten

6.1.2.1 Gierrate

Das temperaturkorrigierte Z-Achsen-Gyroskop Analog Devices ADIS16100 zeigt oh-ne weitere notige Korrekturen bereits sehr wenig Drift, eine gute Genauigkeit undzeichnet sich durch wenig Rauschen aus.

Es wurde folgende Differenz zum Testsystem auf einer unserer Teststrecken gemes-sen:

Maximale Abweichung: 1.0112◦/secMinimale Abweichung: -1.2823◦/secMittelwert: 0.02174◦/secVarianz: 0.05911◦/secStandardabweichung: 0.2431◦/sec

Fehler des Referenzsystems in diesem Zeitraum:

Maximaler Fehler: 0.05774◦/secMinimaler Fehler: 0.01088◦/secMittelwert: 0.04156◦/secVarianz: 0.00186◦/secStandardabweichung: 0.04315◦/sec

46 6. Evaluierung

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

210 212 214 216 218 220

angu

lar

rate

[rad

/s]

time [s]

strasse-a-short Vergleich Z-Achsen Gyro

IMU6 ZApplanix Z

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 temperatur-abhangig, wie man in Abbildung 6.4 gut sehen kann. Außerdem zeigt sich, dassder Sensor der Y-Achse zudem in einem deutlich hoheren Maße driftet, als jenerin der X-Achse. Jedoch bleiben auch nach Temperaturkompensation sich mit derZeit andernde Fehler ubrig. Zum Teil kann das darauf zuruckgefuhrt werden, dasssich der verwendete Temperatursensor im Z-Achsen-Gyroskop befindet, welches sichunterschiedlich schnell erwarmt und abkuhlt. Zudem befindet er sich etwas entferntvom anderen Gyroskop auf der Platine.

-1

0

1

2

3

4

5

6

7

8

0 100 200 300 400 500 600 700 800 900 1000

angu

lar

rate

[deg

/s],

tem

pera

ture

in z

-gyr

o [d

eg C

elsi

us]-

30

time [s]

Gyroscope Test - No Motion

Gyro XTemperature [deg Celsius] - 30

0

5

10

15

20

25

30

0 100 200 300 400 500 600 700 800 900 1000

angu

lar

rate

[deg

/s],

tem

pera

ture

in z

-gyr

o [d

eg C

elsi

us]-

30

time [s]

Gyroscope Test - No Motion

Gyro YTemperature [deg Celsius] - 30

Abbildung 6.4: Drift des X-Gyroskops (links) und des Y-Gyroskops (rechts), ohneGlattung der Rohdaten

Es wurde daraufhin zunachst sehr aufwendig versucht, die Abhangigkeit von derTemperatur und anderen Variablen durch Fitting mit Polynomen verschiedenenGrades zu bestimmen, was aber keine zufriedenstellenden Ergebnisse brachte. ImEndeffekt erschien dieser Sensor als kaum nutzbar fur die Anwendung.

6.1.3 Accelerometerdaten

Die Werte der Beschleunigungsmesser machten im Prinzip einen guten Eindruck,allerdings fehlte es hier etwas an Prazision aufgrund von zu geringer Auflosung des

6.1. Evaluierung der Sensorqualitat 47

Sensors. Der Sensor besitzt eine 8-Bit-Auflosung 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-AchsenSensors gut erkennen. Hier wurde bereits geglattet, indem jeweils der Mittelwert aus4 aufeinander folgenden Messwerten gebildet wurde.

-0.25

-0.2

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

75 76 77 78 79 80

acce

lera

tion

[m/s

2 ]

time [s]

teststrecke1-2009-12-09+001 IMU6 Acceleration

Accel. Z IMU6Accel. Z Applanix

Abbildung 6.5: Ausschnitt aus Sensordaten des Z-Achsen-Beschleunigungsmessers,im Vergleich zur Referenz deutlich zu sehen die diskreten Stufen und das Grundrau-schen. Das Fahrzeug stand im Zeitraum von 75-78 Sekunden still.

Dass dies nicht ausreicht, um kleine Veranderungen im Nick- und Rollwinkel festzu-stellen, lasst sich anhand der folgenden Tabelle zeigen:

Nickwinkel in ◦ X-Achse [g] Z-Achse [g] (1− Z-Achse) [g]0 0.0000 1.0000 0.00001 0.0175 0.9998 0.00022 0.0349 0.9994 0.00063 0.0523 0.9986 0.00144 0.0698 0.9976 0.00245 0.0872 0.9962 0.003810 0.1736 0.9848 0.015211 0.1908 0.9816 0.0184

Ein Nickwinkel von 0◦ bedeutet hier, dass das Fahrzeug vollig waagerecht steht, unddie Z-Achse des Beschleunigungsmessers orthogonal nach unten zeigt. Die Erdbe-schleunigung ubt dabei in Z-Richtung eine Kraft von etwa 1 g aus. Bei großeremNickwinkel wird die Gravitation zum Teil in der X-Achse gemessen.

Die Tabelle zeigt, dass Nickwinkel unter 10 Grad kaum durch den 8-Bit Beschleu-nigungsmesser in der Z-Achse detektiert werden, sich aber deutlich in der X-Achsebemerkbar machen und so leicht fur Vorwartsbeschleunigung gehalten werden kon-nen.

48 6. Evaluierung

6.2 Ergebnisse der Filterung

6.2.1 Teststrecke

Die Teststrecke fur den Test des Kalman-Filters besteht aus einem kurzen Teil imStadtverkehr und einer Hin- und Ruckfahrt auf der Autobahn mit langgestreckterKurve und Tunnel, zu sehen ist die Strecke in Abbildung 6.6.

Abbildung 6.6: Teststrecke fur die Evaluation des Kalman-Filters (aus GoogleEarth). In Rot der Streckenverlauf, in Grun die empfangenen GPS-Positionen. Immittleren Streckenabschnitt der etwa 500 Meter lange Tunnel.

Auf der Fahrt wurden die Ausgaben der IMU6-Platine mitgeloggt, sowie alle Datendes Referenzsystems. Spater wurden die Daten offline gefiltert und es wurde vielexperimentiert, um optimale Parameter zu finden. Am besten arbeitete das Filter indiesem 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 zuBeginn der Messungen sehr stark von der Realitat ab. Als Ursache der Fehler konnteermittelt werden, dass die auftretenden Nick- und Rollwinkel nicht bestimmt werdenkonnten und dadurch die Einflusse der Gravitation auf den Beschleunigungssensorfur die Langsgeschwindigkeit nicht korrigiert werden konnten. Im Endeffekt zeigtesich ein besseres Ergebnis ohne Einbeziehung der Nick- und Rollwinkel. Diese wurdenim Modell einfach auf Null gesetzt und die X-/Y-Raten ignoriert. Die Ergebnisse sindin der oberen Halfte von Abbildung 6.7 zu sehen.

6.2. Ergebnisse der Filterung 49

400

600

800

1000

1200

1400

1600

-900 -880 -860 -840 -820 -800 -780 -760 -740 -720 -700

y/no

rth

EN

U [m

]

x/east ENU [m]

strasse-a-short IMU6 UKF Output XY

IMU6 UKF filteredApplanix

IMU6 GPS only

400

600

800

1000

1200

1400

1600

-880 -860 -840 -820 -800 -780 -760 -740 -720 -700

y/no

rth

EN

U [m

]

x/east ENU [m]

strasse-a-short IMU6 UKF Output XY

IMU6 UKF filteredApplanix

IMU6 GPS only

Abbildung 6.7: Resultate des Filters bei der 2-dimensionalen Lokalisierung im Ver-gleich zum Referenzsystem Applanix, oben mit den in der IMU6-Platine verbautenMEMS-Gyroskopen, unten nach Austausch durch Applanix-Gyroskope

6.2.3 Ersatz der X- und Y-Gyroskope

Es wurde daraufhin nach einer Moglichkeit gesucht, grundsatzliche Fehler im Filterauszuschließen und zu zeigen, dass das Softwaremodul mit besseren Eingangsdatensinnvoll arbeitet. Eine Neuentwicklung der Platine oder ein Austausch der Senso-ren kam nicht in Frage, einerseits mangels Zeit bis zum gewunschten Abschlussdes Projekts, andererseits weil keine passenden pinkompatiblen Gyroskopsensorenverfugbar waren. Eine Filterung ohne die betreffenden Sensoren, allein mit den Be-schleunigungswerten und der Gierrate schied auch aus, weil, wie in Abschnitt 6.1.3beschrieben, die Auflosung dafur unzureichend war.

50 6. Evaluierung

-900

-880

-860

-840

-820

-800

-780

-760

-740

-720

100 150 200 250 300

x E

CE

F [m

]

time [s]

strasse-a-short IMU6 UKF Error

ApplanixIMU6 GPS only

IMU6 UKF filtered

-900

-880

-860

-840

-820

-800

-780

-760

-740

-720

100 150 200 250 300

x E

CE

F [m

]

time [s]

strasse-a-short IMU6 UKF Error

ApplanixIMU6 GPS only

IMU6 UKF filtered

400

600

800

1000

1200

1400

1600

100 150 200 250 300

y E

CE

F [m

]

time [s]

strasse-a-short IMU6 UKF Error

ApplanixIMU6 GPS only

IMU6 UKF filtered 400

600

800

1000

1200

1400

1600

100 150 200 250 300

y E

CE

F [m

]

time [s]

strasse-a-short IMU6 UKF Error

ApplanixIMU6 GPS only

IMU6 UKF filtered

-20

-10

0

10

20

30

40

50

60

70

80

100 150 200 250 300

velo

city

[km

/h]

time [s]

strasse-a-short IMU6 UKF Error

ApplanixIMU6 GPS

IMU6 UKF filtered-20

0

20

40

60

80

100 150 200 250 300

velo

city

[km

/h]

time [s]

strasse-a-short IMU6 UKF Error

ApplanixIMU6 GPS

IMU6 UKF filtered

Abbildung 6.8: Fehler des Filters bei der Lokalisierung in Ost-West-Richtung(oben), Nord-Sud-Richtung (mittig) und bei der Geschwindigkeitsbestimmung (un-ten). Links mit den verbauten MEMS-Gyroskopen, rechts nach Austausch durchApplanix-Gyroskope

Deshalb wurde entschieden, als Eingabe fur das Filter zusatzlich zu den qualitativausreichend guten Sensoren die Drehratensensoren fur die X- und Y-Achse der IMUdes Referenzsystems Applanix zu verwenden.

Die Ergebnisse mit den verbesserten Gyroskopen kann man in Abbildung 6.7 und6.8 sehen.

6.3. Fazit 51

6.3 Fazit

Anhand der Ergebnisse wird deutlich, dass die Qualitat der Sensoren einer Inerti-aleinheit essentiell fur eine exakte Bestimmung der Positionsveranderung uber dieZeit durch Integrieren der Sensordaten ist.

Auch wenn das Kalman-Filter verrauschte Signale sehr gut filtern kann und auchfeste Offsets in den Messungen sicher bestimmt, kann es den starken nichtlinearenDrift der Sensoren nicht vorhersagen, vermutlich auch deshalb, weil hier Messungenfehlen, wie zum Beispiel die exakte Temperatur am Gyroskop.

Nichtsdestotrotz zeigt sich, dass das entworfene Kalman-Filter korrekt arbeitet undsinnvoll einsetzbar ist, wie man an den Ergebnissen nach dem Austausch der Senso-ren erkennen kann.

52 6. Evaluierung

7. Zusammenfassung und Ausblick

Abschließend haben sich im Ruckblick auf die Arbeit und nach der Auswertung derErgebnisse einige Erkenntnisse ergeben. Zum einen scheint die Qualitat der benutz-ten Sensoren eine essentielle Rolle fur die Nutzbarkeit einer Inertialeinheit als Quellefur eine Korrektur und Erganzung von GPS-Modulen zu spielen. Wunschenswert istdabei eine hohe Auflosung bei digitalen Sensoren und moglichst ein Minimum ansich kontinuierlich verandernden Messfehlern (nichtlinearer Drift).

Zum anderen ist es bei dem Einsatz eines Kalman-Filters unabdingbar, eine sinn-volle Modellierung der Umwelt zu wahlen und die Beobachtungs- und Messfunkti-on sorgfaltig zu entwerfen. Die Varianten des Unscented Kalman-Filters sind demerweiterten Kalman-Filter dahingehend uberlegen, dass sich beliebige nichtlineareFunktionen approximieren konnen und diese nicht erst durch Auswerten der Jacobi-Matrizen linearisiert werden mussen. Zusatzlich sind die Approximationen in derRegel genauer als die des EKF.

Bei der Entwicklung der Firmware hat sich gezeigt, dass ein Debugging nur sehrbegrenzt moglich war und sich oft als Geduldsspiel gestaltete. Die angezeigten Feh-lermeldungen fuhrten haufig in die Irre und waren nicht sehr hilfreich zum Ein-grenzen von Problemen. Oftmals half hier nur die ausfuhrliche Ausgabe von Debug-Meldungen uber 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 Beschleuni-gungssensor auf der Platine in naher Zukunft ausgetauscht werden kann. Es lohntsich, damit noch einige Testfahrten zu unternehmen.

Außerdem ist eine neue Entwicklungsplatine in Planung, bei der aller Voraussichtnach viele der Mangel im Entwurf und Layout der IMU6-Platine beseitigt werdenkonnen. Nicht zuletzt deshalb, weil die Ergebnisse dieser Arbeit in die Entwicklungder neuen Plattform mit einfließen konnten.

Unabhangig davon erscheint es sinnvoll, die Integration weiterer Sensoren, wie zumBeispiel eines hochauflosenden 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 esSinn, die Informationen aus moglichst vielen Sensoren dafur zu nutzen, und einKalman-Filter scheint nach den gemachten Erfahrungen hierzu ein geeignetes Mit-tel zu sein.

Auf dem Weg zu einem serienmaßigen PKW mit autonomen Funktionen ist es un-abdingbar, die Kostenfrage nicht außer Acht zu lassen. MEMS-Sensoren sind dabeiein wichtiger Schritt in die richtige Richtung. Es muss aber gelingen, qualitativ ho-herwertige MEMS-Sensoren zu fertigen, die fur diesen Einsatz dennoch preislich inFrage kommen. Diese Arbeit hat aufgezeigt, dass es dabei auf geringe Drift und hoheAuflosung ankommt und demonstriert, wie sich die Sensoren durch ein Kalman-Filtermit externen Quellen fusionieren lassen, um sie fur die exakte Positionsbestimmungin autonomen Fahrzeugen sinnvoll zu nutzen.

A. Anhang: UnscentedKalman-Filter Pseudocode

Es folgt der Pseudocode fur das als Teil der Arbeit implementierte UnscentedKalman-Filter. Die Implementierung orientiert sich an der Variante square-root cen-tral difference Kalman filter (SRCDKF) aus [VDMWa04] und basiert auf der centraldifference transformation, wie sie unter anderem in [Nø00] beschrieben ist.

A.1 Initialisierung

Der Zustand x wird zunachst auf einen Initialwert x0 gesetzt. Die Diagonalelementeder Kovarianzmatrix P′ werden mit einem kleinen Wert ǫ > 0 initialisiert.

x′ = x0, P′ = I · ǫ

A.2 Vorhersage

A.2.1 Berechnung der Sigma-Punkte

Der Zustand x wird dazu mit dem Ubergangsrauschen w und die KovarianzmatrixP′ um die Fehlerkovarianzmatrix Q erweitert.

x =(

xT wT)T

Pw =

(

P′ 00 0

)

Transitionfunction::setQ(Pw)

setQ(Pw) ersetzt die untere rechte Teilmatrix in Pw durch eine in der Ubergangs-funktion definierte Fehlerkovarianzmatrix Q.

56 A. Anhang: Unscented Kalman-Filter Pseudocode

Sw =√Pw ist die Matrixquadratwurzel von Pw

xΣ0 = x

xΣi = x+

√n+ λ (Sw)i i = 1 . . . 2n, i ungerade

xΣi = x−

√n+ λ (Sw)i i = 1 . . . 2n, i gerade

Der Paramater λ ist ein Skalierungsfaktor und definiert als λ = α2(n+k)−n, wobeihier k = 0 gesetzt wird, also λ = (α2 − 1)n.

A.2.2 Anwendung der Ubergangsfunktion auf Sigma-Punkte

XΣi = Transitionfunction::transition(xΣ

i ) i = 1 . . . 2n

A.2.3 Berechnung von Mittelwert und Kovarianz

x′ =2n∑

i=0

Wmi XΣ

i

δi = XΣi − x i = 1 . . . 2n

P′ =2n∑

i=0

W ci δi · δTi

Die Gewichte Wmi ,W

ci werden berechnet wie in der Standardform des Unscented

Kalman-Filters:

Wm0 = λ/(n+ λ)

W c0 = Wm

0 + (1− α2 + β)

W ci = Wm

i = 1/2(n+ λ) i = 1, . . . , 2n

A.3 Beobachtung

A.3.1 Berechnung der Sigma-Punkte

x =(

x′T vT)T

Pv =

(

P′ 00 0

)

Observationfunction::setR(Pv)

A.3. Beobachtung 57

setR(Pv) ersetzt die untere rechte Teilmatrix in Pv durch eine in der Beobachtungs-funktion definierte Fehlerkovarianzmatrix R

Sv =√

Pv

zΣ0 = x

zΣi = x+ (Sv)i i = 1 . . . 2n, i ungerade

zΣi = x− (Sv)i i = 1 . . . 2n, i gerade

A.3.2 Anwendung der Ubergangsfunktion auf Sigma-Punkte

ZΣi = Observationfunction::observe(zΣi ) i = 1 . . . 2n

A.3.3 Berechnung von Mittelwert und Kovarianz

z =2n∑

i=0

Wmi ZΣ

i

γi = ZΣi − z i = 1 . . . 2n

Pzz =2n∑

i=0

W ci γi · γTi

A.3.4 Korrektur

Pxz =2n∑

i=0

W ci

[

yΣi − yΣ0]

· γTi

K = Pxz ·P−1zz

x = x+K · (z− z)

P = P−KPzzKT

58 A. Anhang: Unscented Kalman-Filter Pseudocode

Literaturverzeichnis

[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 decou-pled MEMS gyroscope on an insulating substrate. Journal of Micro-electromechanical Systems 14(4), 2005, S. 707.

[HoKM08] S. Holmes, G. Klein und D. Murray. A square root unscented Kal-man filter for visual monoSLAM. In Proc Int Conf on Robotics andAutomation. Citeseer, 2008, S. 3710–3716.

[HSLS+07] J. Hol, T. Schon, H. Luinge, P. Slycke und F. Gustafsson. Robust real-time 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–GlobalNavigation Satellite Systems: GPS, GLONASS, Galileo and more.Springer. 2008.

[IgSH06] C. Igel, T. Suttorp und N. Hansen. A computational efficient cova-riance matrix update and a (1+ 1)-CMA for evolution strategies. InProceedings of the 8th annual conference on Genetic and evolutionarycomputation. ACM, 2006, S. 460.

[JUIJC04] S. Julier, J. Uhlmann, I. Ind und M. Jefferson City. Unscented filteringand nonlinear estimation. Band 92, 2004, S. 401–422.

[JuUDW95] S. Julier, J. Uhlmann und H. Durrant-Whyte. A new approach forfiltering nonlinear systems. In Proceedings of the American ControlConference, 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 tononlinear systems. In Int. Symp. Aerospace/Defense Sensing, Simul.and Controls, Band 3. Citeseer, 1997, S. 26.

[Ka60] R. Kalman. A new approach to linear filtering and prediction pro-blems. Journal of basic Engineering 82(1), 1960, S. 35–45.

[KoFS09] S. Kolas, B. Foss und T. Schei. Constrained nonlinear state estimationbased on the UKF approach. Computers & Chemical Engineering33(8), 2009, S. 1386–1401.

60 Literaturverzeichnis

[Mayb82] P. Maybeck. Stochastic models, estimation and control, S. 1–16. Aca-demic Press. 1982.

[Nø00] Nørgaard, M., Poulsen, N.K. und Ravn, O. New developments instate 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 OrientationEstimation Algorithm with an Inertial Measurement Unit and a Ma-gnetic Compass. Journal of Universal Computer Science 15(4), 2009,S. 859–876.

[Sama08] N. Samama. Global positioning: technologies and performance. Wiley-Interscience. 2008.

[Seeg04] M. Seeger. Low rank updates for the Cholesky decomposition. Tech-nical report, 2004.

[Simo06] D. Simon. Optimal state estimation: Kalman, H [infinity] and nonli-near approaches. John Wiley and Sons. 2006.

[VDMWa01] R. Van Der Merwe und E. Wan. The square-root unscented Kalmanfilter for state and parameter-estimation. In IEEE International Con-ference 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 inte-grated navigation. In Proceedings of the 60th Annual Meeting of theInstitute of Navigation (ION). Citeseer, 2004, S. 641–654.

[WaLR03] J. Wang, H. Lee und C. Rizos. GPS/INS integration: A performancesensitivity analysis. Wuhan University Journal of Nature Sciences8(2B), 2003, S. 508–516.

[WaVDM00] E. Wan und R. Van Der Merwe. The unscented Kalman filter fornonlinear estimation. In Proceedings of Symposium. Citeseer, 2000,S. 153–158.

[WeBi95] G. Welch und G. Bishop. An introduction to the Kalman filter. Uni-versity of North Carolina at Chapel Hill, Chapel Hill, NC, 1995.

[Wend07] J. Wendel. Integrierte Navigationssysteme: Sensordatenfusion, GPSund inertiale Navigation. Oldenbourg Wissenschaftsverlag. 2007.

[Wood07] O. Woodman. An introduction to inertial navigation. Universityof Cambridge, Computer Laboratory, Tech. Rep. UCAMCL-TR-696,2007.

[ZGMH05] P. Zhang, J. Gu, E. Milios und P. Huynh. Navigation withIMU/GPS/digital compass with unscented Kalman filter. In 2005 IE-EE International Conference Mechatronics and Automation, Band 3,2005.

Literaturverzeichnis 61

[ZhZh08] T. Zhu und H. Zheng. Application of Unscented Kalman Filter to Ve-hicle State Estimation. In ISECS International Colloquium on Com-puting, Communication, Control, and Management, 2008. CCCM’08,2008, S. 135–139.

62 Literaturverzeichnis

Glossar

Cassandra Eine von der Hella Aglaia entwickelte Softwa-replattform fur visuelle Fahrerassistenzsyste-me. 4, 33

DSP Digital Signal Processing oder auch Digital Si-gnal Processor, ein Prozessor, der (vorher di-gitalisierte) Signale verarbeitet und beispiels-weise filtert. 41

Gierrate Die Rate, mit der sich ein Fahr- oder Flugzeugum die Z-Achse (senkrechte Achse) dreht. 4,23, 30, 33, 49

GPS-Fix siehe Satellitenfix. 44

IMU Inertial Motion Unit: ein elektroni-sches Gerat, bestehend aus kombiniertenInertialsensoren. 3, 32

IMU6 Name einer Entwicklungsplattform von Ana-log Devices, bestehend aus GPS-Modul undInertialeinheit. 4, 32, 33

Inertialeinheit siehe IMU. 37Inertialsensor Sensor, der eine physikalische Große in einem

Inertialsystem misst, z.B. Beschleunigungs-messer, Drehratensensor. 1, 3, 4, 7, 13, 15, 26,27, 31, 38, 63

JTAG Steht fur Joint Test Action Group. ein Stan-dard fur eine Debug-Schnittstelle von Pro-zessoren, ermoglicht Programmierung, Setzenvon Breakpoints, schrittweise Ausfuhrung ei-nes Programms und Monitoring von Varia-blen. 33, 36

64 Glossary

LDW kurz fur: Lane Detection Warning, Name einesSpurhaltesystems der Hella Aglaia GmbH, dasden Fahrer beim unbeabsichtigten Verlassender Fahrspur warnt. 4, 33

NMEA Abkurzung fur National Marine Electro-nics Association. Protokoll, welches ASCII-basierte Datensatze definiert, die beispielswei-se von GPS-Empfangern ausgegeben werden.36

Odometer Ein Messgerat fur Radumdrehungen, in me-chanischer oder elektronischer Ausfuhrung,bei PKW meist als Kilometerzahler integriert,auch zusatzlich nachrustbar zur Montage ander Felge. 3

OROCOS steht fur Open Robot Control Software,ein C++-Framework fur die Robotik-Entwicklung. 2, 3, 32, 33, 35, 37

Pseudoentfernung Scheinbare Entfernung, die vom GPS-Empfanger gemessen wird, weicht um einenFehlerbetrag ab, der hauptsachlich durch dieUhrungenauigkeit des Empfangers zustandekommt. 9, 11

Satellitenfix Ein Satellitenfix ist gegeben, wenn der Emp-fanger seine Position durch den Empfang derSignale von ausreichend vielen Satelliten be-stimmen konnte. 44

SPI Serial Peripheral Interface Bus, SynchronerDatenbus, wird haufig bei Mikrocontrollernund Sensoren eingesetzt. 35