Unsere Smartphones wissen, wo oben und unten ist. Aber woher wissen sie das? Und wie genau kennen Smartphones ihre Orientierung und die Bewegung im Raum?
In dieser Arbeit wird ein Algorithmus entwickelt, welcher aus den Daten von mehreren Sensoren, wie sie in typischen Smartphones zu finden sind, die Orientierung und die Position im Raum berechnet. Mit zahlreichen mathematischen Tricks gelingt es, die Daten der einzelnen Sensoren in Echtzeit so umzurechnen, dass sogar Gesten und kurze Bewegungen erfasst werden können.
Anwendungen des Algorithmus sind nicht nur Mobiltelefone, sondern auch beispielsweise Quadrocopter oder Satelliten.
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
[...]
- Quote paper
- Moritz Lehmann (Author), 2017, Erfassung von 3D Orientierung und Translation mittels eines 9DOF Sensors und eines Barometers, Munich, GRIN Verlag, https://www.hausarbeiten.de/document/378112
