Erfassung von 3D Orientierung und Translation mittels eines 9DOF Sensors und eines Barometers

Inhaltsverzeichnis

1. Setup

2. Auslesen des Sensors

3. Funktionsdefinitionen

4. Hinweise zur Notation

5. Algorithmus

6. Grafische Visualisierung

7. Vor- und Nachteile des Algorithmus

8. Quellen

9. Anhänge

1. Setup

Die verwendeten Senoren sind ein LSM9DS0 (3D Gyroskop, 3D Accelerometer, 3D Magnetometer sowie ein BMP180 (Barometer). Die Messdaten werden über den Bus I²C ausgelesen. Die I²C-Schnittstelle erlaubt das Ansteuern von 112 Slave-Geräten, diese werden in Reihe geschaltet. Die notwendigen Pull-Up-Widerstände für Daten- und Clock-Leitung sind bereits auf der Platine des Raspberry Pi 3 vorhanden. Rechts ist die Beschaltung skizziert und unser Aufbau abgebildet.

Als Programmiersprache wird Java mit der Erweiterung pi4j verwendet, da Java die Möglichkeit einer grafischen Darstellung in Echtzeit bietet.

Abbildung in dieser Leseprobe nicht enthalten

Abbildung 1: Schema der I²C-Schnittstelle und Aufbau der Schaltung auf einem Steckbrett

2. Auslesen des Sensors

Zu Beginn werden die Sensoren durch Beschreiben und Auslesen der Konfigurationsregister konfiguriert. Im Datenblatt der Sensoren ist genau beschrieben, welche 1 oder 0 in welchem Register was bedeutet. Die Einstellungen sind:

Abbildung in dieser Leseprobe nicht enthalten

Des Weiteren wurden einige sensorinterne Filter sowie dreifaches Oversampling beim Barometer eingeschaltet.

Die Rohdaten der Messwerte werden aus je 2 Bytes zusammengesetzt. Es folgt die Kalibrierung der Sensoren. Dazu wird der maximale Messbereich jeder Achse erfasst und anschließend jede Achse mit einer Konstanten zentriert und mit einer weiteren Konstanten so skaliert, dass alle Achsen dieselben Messbereiche liefern. Das Barometer führt zunächst 100 Messungen durch, um die aktuelle absolute Höhe mit großer Genauigkeit zu mitteln.

3. Funktionsdefinitionen

Zunächst müssen einige Vektor- und Matrixoperationen definiert werden, die die Formulierung des Hauptalgorithmus stark vereinfachen werden.

Rotation eines Vektors durch eine Rotationsmatrix um einen festen Punkt:

Abbildung in dieser Leseprobe nicht enthalten

Rotation eines Vektors durch eine Rotationsmatrix um den Ursprung:

Abbildung in dieser Leseprobe nicht enthalten

Erstellen einer Rotationsmatrix, die die Rotation um einen (normierten) Vektor v um den Winkel r (im Bogenmaß) beschreibt:

Abbildung in dieser Leseprobe nicht enthalten

Multiplikation zweier 3x3-Matrizen:

Abbildung in dieser Leseprobe nicht enthalten

Normierung eines Vektors (auf die Länge 1):

Abbildung in dieser Leseprobe nicht enthalten

Normierung eines Vektors (auf die Länge s):

Abbildung in dieser Leseprobe nicht enthalten

Kreuzprodukt zweier Vektoren:

Abbildung in dieser Leseprobe nicht enthalten

es Vektors um den Faktor s:

Abbildung in dieser Leseprobe nicht enthalten

Berechnung des Winkels (im Bogenmaß) zwischen zwei Vektoren:

Abbildung in dieser Leseprobe nicht enthalten

Skalarprodukt zweier Vektoren:

Abbildung in dieser Leseprobe nicht enthalten

Länge eines Vektors:

Abbildung in dieser Leseprobe nicht enthalten

Addieren zweier Vektoren:

Abbildung in dieser Leseprobe nicht enthalten

Subtrahieren zweier Vektoren:

Abbildung in dieser Leseprobe nicht enthalten

Quadrat einer Zahl:

Abbildung in dieser Leseprobe nicht enthalten

4. Hinweise zur Notation

Abbildung in dieser Leseprobe nicht enthalten

5. Algorithmus

Abbildung in dieser Leseprobe nicht enthalten

Als Konstanten werden zunächst ein Einheits-Koordinatensystem, der Gravitations-Vektor sowie Inklination und Rektaszension des Erdmagnetfeldes definiert:

Gemessen werden der Drehgeschwindigkeits-Vektor, der Beschleunigungs-Vektor, der Magnetfeld-Vektor, die relative Höhe zur Startposition sowie die Update-Frequenz des Algorithmus:

Abbildung in dieser Leseprobe nicht enthalten

Zunächst wird ein Maß dafür erfasst, wie ruhig der Sensor gerade ist. Dazu dient die Funktion still(), die einen Wert zwischen 0 (bewegt) und 1 (ruhig) anhand verschiedener Kriterien liefert, unter anderem der Übereinstimmung vom Winkel zwischen Beschleunigungs- und Magnetfeld-Vektor mit der tatsächlichen Inklination des Erdmagnetfeldes sowie die Abweichung der Gesamtbeschleunigung von der Erdbeschleunigung. Der Funktionswert muss noch mit der Aktualisierungsrate des Algorithmus verrechnet werden.

Abbildung in dieser Leseprobe nicht enthalten

Die Rotation wird zunächst vorläufig aus den Messdaten des Gyroskops berechnet. Dazu wird der Vektor der Drehgeschwindigkeit mittels Trapez-Integration zum Drehvektor aufintegriert. Aus dem Drehvektor wird eine Rotationsmatrix um dessen Achse berechnet und von vorne an die bestehende Rotationsmatrix multipliziert.

Abbildung in dieser Leseprobe nicht enthalten

Die Vektoren der Messungen von Beschleunigung und Magnetfeld werden durch Rotation um die vorläufige Rotationsmatrix R ins gedrehte Koordinatensystem des Sensors gebracht. Da der Beschleunigungs-Vektor von der Erdbeschleunigung beim Bewegen des Sensors stark abweichen kann, während der Magnetfeld-Vektor wesentlich genauer bleibt, wird die Beschleunigung bei diesem Schritt mithilfe des Magnetfeldes so korrigiert, dass Beschleunigungs- und Magnetfeld-Vektor stets genau den Winkel inc der Inklination des Erdmagnetfeldes einschließen.

Abbildung in dieser Leseprobe nicht enthalten

Aus diesen beiden Vektoren wird das rotierte Koordinatensystem des Sensors rekonstruiert.

Abbildung in dieser Leseprobe nicht enthalten

Das rekonstruierte Koordinatensystem des Sensors kann vom tatsächlichen Koordinatensystem der drei Einheitsvektoren mit der Zeit abdriften. Zur Korrektur muss es, wenn der Sensor ruhig ist, dem Koordinatensystem der drei Einheitsvektoren durch zwei zusätzliche Rotationen langsam angepasst werden.

Abbildung in dieser Leseprobe nicht enthalten

Die aktuelle Rotation des Sensors ist jetzt mit großer Genauigkeit bekannt und in der Rotationsmatrix R gespeichert. Zur Berechnung der Translation des Sensors wird die gemessene Beschleunigung in das gedrehte Koordinatensystem des Sensors gebracht, sodass der Erdbeschleunigungs-Vektor davon subtrahiert werden kann, sodass nur noch die reine Beschleunigung übrig bleibt. Diese wird dann zweimal mittels Trapez-Integration zu Geschwindigkeit und Ort aufintegriert.

Abbildung in dieser Leseprobe nicht enthalten

Weil sich bei Geschwindigkeit und Ort sehr schnell eine Drift aufbaut, müssen beide abgedämpft werden, wenn der Sensor in Ruhe ist. Die z-Komponente wird via Low-pass-Filter mit der vom Barometer gemessenen Höhe verrechnet.

Abbildung in dieser Leseprobe nicht enthalten

[...]