Trägheitsnavigation

Bei der Trägheitsnavigation kommen Inertialsensoren zum Einsatz, die auf Basis der Massenträgheit drei zueinander orthogonale translatorische und drei rotatorische Anteile einer Objektbewegung messen, um auf die Bewegung zurückzuschließen. Wie gut das mit günstigen Sensoren funktioniert, wie sie in modernen Handys verbaut sind oder zum Beispiel auf dem STM32 F3 Discovery Dev-Board zu finden sind, gilt heraus zu finden.

Video vom 23.02.2013

Sensoren

Google Tech Talk zum Thema: https://www.youtube.com/watch?v=C7JQ7Rpwn2k

Gyroskop

Ein 3-Achsen MEMS Gyroskop liefert einem eine Drehgeschwindigkeit je Achse, typischerweise in einem rechtsorientierten Koordinatensystem. Die Achsen drehen sich dabei mit, weswegen man dabei auch von einem "Strapdown System" spricht.

Interessante Links dazu:

• Noise Characteristics of MEMS Gyro’s Null Drift and Temperature Compensation

Beschleunigungssensoren

liefern Beschleunigungen. Aufgrund der Erdanziehung ist typischerweise zu erwarten, dass der gemessene Beschleunigungsvector die Summe aus tatsächlicher Beschleunigung und 1g nach oben ist. Dazu kommen dann natürlich noch Messfehler und systematische Fehler.

Probleme

• Messdaten sind verrauscht und haben einen Bias. Stärke des Rauschens und der Bias selbst sind auch Temperaturabhängig. Wie man das so mitbekommt, sind gerade MEMS Gyroskope stark temperaturabhängig, warum man auch in einigen Chips ein Thermometer integriert ist.

• Integration in Kombination mit Messrauschen ist "fies", sehe Abschnitt "Integration als Filter begreifen".

Integration als Filter begreifen

Wir kommen über Integration von Beschleunigung auf Geschwindigkeit und von Geschwindigkeit auf Position im Falle von Accelerometern. Bei Gyroskopen messen wir die Drehgeschwindigkeit und können per Integration die Orientierung bestimmen. Man kann dabei einen Integrator als Filter verstehen, der tiefe Frequenzen verstärkt und hohe Frequenzen dämpft. Genauer gesagt gibt es bei der Integration über die Zeit in Sekunden eine Amplitudenübertragung von 1/(2*pi*f), wobei f die Frequenz in Hz ist, und eine Phasenverschiebung von -90°. Dieses Filter arbeitet natürlich auch auf dem Messrauschen, weswegen die tieffrequenten Anteile (einschließlich eines Fehler-Offsets) leider kaum zu gebrauchen sind. Der aufmerksame Leser wird gemerkt haben, dass der DC-Offsent wegen 1/f sich unendlich verstärkt ... also mit der Zeit davon läuft. In IIR-Filter-Sprech ist das Ding "unstabil". Dies führt zu einem Drift in der Orientierung und in der Geschwindigkeit. Das heißt, dass mit der Zeit der Fehler größer wird. Gmäß "Random Walk" skaliert der Fehler bzgl Messrauschen mit der Wurzel der Zeit (und der eineinhalbfachen Potenz der Zeit bei den Positionsdaten).

Herstellerangaben zur Genauigkeit

In den Datenblättern der Sensoren findet sich so das eine oder andere Interessante. Beispielsweise Angaben zum Rauschverhalten. Diese können da in X pro Wurzel-Hertz stehen, wobei X eine Einheit der Messgröße ist, z.B. Grad pro Sekunde für einen Gyro oder Milli-G für ein Beschleunigungsmesser. Bei dieser Angabe handelt es sich um die Wurzel der spektrale Leistungsdichte des Rauschens. Diese Angabe sagt einem den RMS-Wert des Rauschens bei einer Bandbreite von einem Hertz. Aber dieser Wert skaliert eben nur mit der Wurzel der Bandbreite. Man erhält also den 10fachen RMS-Wert bei einem Frequenzband von 100 Hz -- unter der Annahme, dass die Leistungsdichte im Spektrum konstant ist.

Eine andere Angabe, die man bei Gyros findet, ist auch der ARW, angular random walk. Der Wird bei Gyros in Grad pro Wurzel-Stunde angegeben. Er sagt einem die Rausch-induzierte Streuung in Grad, wenn man das Signal eine Stunde lang integriert. Mit dieser Angabe kann man eher etwas anfangen; denn er sagt einem gleich, welche Genauigkeit in Grad man nach Integration von einer Stunde hat. Dieser Wert skaliert auch mit der Wurzel der Zahl der Stunden.

Programmcode/Algorithmen

• sg hat was gebastelt, ist aber noch relativ experimentell. Hier schonmal ein paar schön polierte Matlab/Octave-Funktionen bezüglich Rotationsmatrizen:
  • vxm.m Kreuzprodukt als Matrix
  • rodrigues.m Implementierung der Rodrigues-Formel
  • rotify.m Approximations- und Rundungsfehlerkompensation für Rotationsmatrizen
• OpenShoe-Projekt

Rotationen

Aus einer 3-achsigen Drehgeschwindigkeit vrot in rad/s und einer Zeitspanne delta_t in Sekunden lässt sich ein 3-achsiger Rotationswinkel über a=vrot*delta_t in rad bestimmen. Die drei Winkel ax, ay und az lassen sich per Rodrigues-Formel in eine Rotationsmatrix umwandeln. Allerdings erfordert die Auswertung dieser Formel eine Wurzel für die 2-Norm ||a|| von a sowie die Berechnung von sin(||a||) und cos(||a||).

R = I + f1 [a] + f2 [a]^2 
                           
mit
       |  0  -az  ay |   n = ||a||
[a] := |  az  0  -ax |   f1 = sin(n)/n
       | -ay  ax  0  |   f2 = (1-cos(n))/n^2

Für sehr kleine Drehwinkel könnte man eine Linearisierung verwenden, f1=1 und den quadratischen Teil weglassen f2=0. So käme man ohne komplizierte Funktionen wie sqrt für n, sin und cos für f1, f2 aus. Man beachte aber, dass bei solchen Approximationen für f1 und f2 in der Ebene, in der rotiert wird, der Drehwinkel fehlerbehaftet ist und in der Ebene auch eine nicht gewollte Skalierung stattfindet. Die Skalierungsfehler lassen sich aber relativ leicht wieder kompensieren. Für eine Näherung zweiter Ordnung kommt der quadratische Teil mit f2=0.5 einfach dazu. Diese Werte f1=1 und f2=0.5 sind die Grenzwerte der exakten Formeln für n gegen 0. Das Spiel lässt sich beliebig fortführen über eine abgebrochene Taylorreihe. So ist die Approximation 4. Ordnung beispielsweise gegeben durch

Visualisierung der Approximationen für Rotationen, 0-90° in Schritten von 10°

R_4th = I + (1 - n^2/6) [a] + (1/2 - n^2/24) [a]^2

Hier gibt es klar einen Performance/Genauigkeits-Zielkonflikt. Ist die Abtastrate hoch genug und dementsprechend die Drehwinkel klein, reicht wahrscheinlich die Näherung 2. Ordnung; denn es gibt ja auch noch das Messrauschen, was ggf einen viel stärkeren Einfluss auf die Genauigkeit hat als die Approximationsfehler der Rodrigues-Formel.

Die verschiedenen Genauigkeiten dieser Approximationen lassen sich als verschiedene Näherung eines Kreisbogens darstellen. Siehe Bild rechts. Der Punkt rechts unten im Bild (1;0) ist der 0°-Punkt, in dem sich alle Formeln einig sind. Mit steigendem Winkel gehen sie dann auseinander.

"Integration von Rotationen" bedeutet im Wesentlichen das Aneinandermultiplizieren von vielen kleinen Rotationsmatrizen. Die dabei entstehenden Rundungsfehler pflanzen sich leider fort und führen dazu, dass die resultierende Matrix nicht mehr viel mit einer Rotationsmatrix gemein haben muss; denn es gibt es 9 Freiheitsgrade bei 3x3-Matritzen von denen nur eine dreidimensionale Mannigfaltigkeit die Menge der Rotationsmatrizen darstellt. Man muss also ab und zu seine Matrix irgendwie geschickt auf diese Mannigfaltigkeit der Rotationsmatrizen projizieren. Code dafür ist vorhanden (siehe rotify.m). Wer das verstehen will, fragt am besten mal sg nach einer Erklärung.

Diverses

Ideen/Strategien

• Der Orientierungsdrift über die Zeit kann teilwelse über die Beschleunigungsdaten kompensiert werden, so dass oben/unten immer noch oben/unten bleibt, beispielsweise per Complementary Filter. Das klappt soweit ganz gut. Man müsste noch prüfen, ob man das nicht noch verbessern kann, wenn man die Beschleunigungsdaten integrieren will. Die Orientierung sollte da schon stimmen, wenn man die Komponente der Erdanziehung rausrechnen will.
• Um den Orientierungsdrift auch in der horizontalen Ebene zu korrigieren, werden noch andere Daten benötigt. Beispielsweise ein Kompass oder Vorwissen über das "Terrains". Das Treppensteigen sollte sich erkennen lassen. Es sollte auch klar sein, dass man nicht durch Wände gehen kann.
• Für Zu-Fuß-Navigation bietet sich an, die Sensoren an einem der Schuhe anzubringen. Das ermöglicht ZUPT (zero velocity update). Das Problem, welches dadurch gelöst wird, ist der Geschwindigkeitsdrift über die Zeit, den man sonst bekommen würde, weil die Beschleunigungsdaten verrauscht sind. Bei ZUPT nutzt man aus, dass es immer wieder Momente gibt, wo man genau weiß, dass die Geschwindigkeit Null ist bzw sein müsste. Beim Gehen könnte man versuchen über die Sensordaten die verschiedenen Phasen zu erkennen und dementsprechend die bis dahin berechnete, aktuelle Geschwindigkeit. In diesem Video kann man das schön sehen.
• Idee für Gyro-Gain Kalibrierung: Über die Beschleunigungssensoren wissen wir, wo im Objektkoordinatensystem oben ist. Dreht man das Objekt um einen bestimmten Winkel, wird der Beschleunigungsvektor auch entsprechend in eine andere Richtung zeigen. Bei kalibrierten Beschleunigungssensoren lässt sich dann dieser Winkel bestimmen und mit dem Winkel vergleichen, den man mit Hilfe der Gyros allein ausrechnen kann. Mit dem Verhältnis dieser Winkel könnte man den Gyro-Gain korrigieren. Ich denke aber, dass man zwecks Genauigkeit größere Drehungen nutzen sollte, siehe nächster Punkt

Was noch so nützllich sein könnte

• Kalman Filter
• Complementary Filter

TODO

• Die Sensoren "richtig" betreiben, für ein regelmäßig mit einer bestimmten Abtastrate abgetastetes Zeitsignal. Vielleicht ist dafür der FIFO-Modus da. Im Moment mach ich mir sorgen um Aliasing und dass etwas verloren geht.
• Self-Test Funktionen ausnutzen, um zu testen, ob die Sensoren in Ordnung sind
• Mal so ein Rauschsignal der versch. Sensoren "richtig" angucken (Leistungsdichtespektrum). Die Leistungsdichte um 0 Hz wird dann auch Aufschluss darüber geben, wie genau man überhaupt noch nach einer Stunde Integration sein kann, wenn alles korrekt kalibriert wurde.
• Temperaturabhängigkeit nochmal checken über einem größeren Bereich mit mehreren Wiederholungen.
• Mit fertig kalibrierten Sensoren verschiedene Testmessungen mal durchführen, bei denen alles aufgezeichnet wird, beispielsweise eine Ruhemessung oder mit den Sensoren am Schuh befestigt ein bisschen rumlaufen.
• Sich vielleicht um eine Funkverbindung kümmern. Sensoren am Schuh mit Bluetooth oder so?

Erledigt

• Sensoren (MPU6050) erfolgreich angesteuert (im Moment: per PIC32 mit serieller Verbindung zum Rechner)

• Die Berechnungen der Orientierungen scheinen zu funktionieren. Auf dem Bildschirm dreht sich ein Würfel, der den Sensor symbolisiert, so ähnlich wie der Sensor selbst.

• Ruhemessungen über verschiedene Temperaturen und Orientierungen, siehe unten

• (2013-02-12) Eine Kalibrierung wurde in das Programm integriert, die man über Tasten steuert. Der Gyro-Bias, Accelerometer-Gain und Accelerometer-Bias werden bestimmt, wenn man die 3 Objekt-Achsen jeweils nach oben und nach unten zeigen lässt und das Objekt dabei fixiert, dass es sich nicht bewegt. Durch Drücken der P-Taste wird eine Punkt-Messung gestartet, die alle Sensorwerte solange mittelt, bis man die P-Taste wieder loslässt. Das wiederholt man jetzt 6 Mal für alle Achsen und Oben/Unten-Kombinationen. Jedes Mal beim Loslassen der P-Taste werden die intern gespeicherten Kalibrierparameter korrigiert. Eine bestimmte Reihenfolge muss man nicht einhalten. Eine Gyro-Gain Kalibrierung ist auch vorhanden, aber der traue ich noch nicht ganz. Die Integration der Beschleunigungsdaten wurde auch implementiert und funktioniert nicht nicht ganz zufriedenstellend (das Objekt läuft aufgrund von Fehlern weg, obwohl es still steht).

• (2013-02-23) Gyro-Gain Kalibrierung ist flexibler und genauer

Bisher Gelerntes

• Die Rodrigues-Formel für Rotationsmatrizen ist 'ne tolle Sache
• Man kann leichte Rundungsfehler einer Rotationsmatrix relativ schnell reduzieren
• Die Gyroskope haben einen stark temperaturabhängigen Bias, weswegen da auch ein Temperatursensor eingebaut ist
• Random Walk (so pflanzen sich Messfehler fort)
• Der Random-Walk-Wert lässt sich aus der spektralen Leistungsdichte des Rauschens bei 0 Hz unabhängig von der Abtastrate ausrechnen
• Die Gyroskope scheinen relativ wenig zu rauschen.
• Die Berechnung der Positionsdaten erfordert eine sehr genaue Kalibrierung aller Sensoren, inklusive der Gyrpskope mit ihren Gains. Die Erdbeschleunigung macht einem da gerne einen Strich durch die Rechnung. Die muss man ja rausrechnen, was nicht klappen wird, wenn all die Sensoren nicht gut genug kalibriert sind.

Messungen

Keine Bewegung mit sich ändernder Temperatur

Gyro-Bias Rohdaten

• 

  Beschleunigungssensoren

• 

  Gyroskope

Beschreibung des Experiments

Alex' MPU6050 wurde abwechselnd auf die Heizung und auf die kalte Fensterbank gelegt und da jeweils eine Weile liegen gelassen. Die Gyros wurden im 250°/s-Modus betrieben (lösten etwa +/-250°/s mit 16 Bit auf) und die Beschleunigungssensoren wurden im 2g-Modus betrieben (lösten etwa +/-2g mit 16 Bit auf).

Ergebnis

Wie man deutlich sehen kann, ist der Bias der Gyros stark temperaturabhängig. Ihr Rauschverhalten scheint sich dabei nicht nennenswert zu ändern. Die zeitlich lokale Streuung der Messdaten wurde bestimmt und als Fehlerbalken (2-Sigma) dargestellt. Die Beschleunigungssensoren zeigen verschiedene Beschleunigungen aufgrund der Erdanziehung und der variierenden Orientierungen in den verschiedenen Phasen. Ihre Ausgabe ist weitestgehend konstant über der Temperatur.

Streuungen	X	Y	Z
Accelerometer	0.029 m/s^2	0.029 m/s^2	0.043 m/s^2
Gyroskope	0.09 °/s	0.11 °/s	0.09 °/s

Dies sind die RMS-Werte der Schwankungen. Die Abtastrate lag bei etwa 666 Hz. Unter der optimistischen Annahme von weißem Rauschen würde das einem Random Walk von

0.17 °/s / sqrt(666 Hz) = 0.0065874 °/sqrt(s) = 0.051026 °/sqrt(min) = 0.39524 °/sqrt(hr)

und

0.06 m/s^2 / sqrt(666 Hz) = 0.138 m/s/sqrt(hr)

ergeben. Allerdings ist das Rauschen bekannterweise nicht weiß sondern bei den tiefen Frequenzen stärker, was bedeutet, dass der tatsächliche RW-Wert höher sein wird. Ich schätze mal, dass wir eher bei 1 bis 2 °/sqrt(hr) liegen werden, also, dass nach Integrieren über eine Stunde der rausch-induzierte Fehler der Orientierung eine Streuung von etwa 1 bis 2 Grad und der Fehler der Geschwindigkeit eine Streuung von etwa 0.5 m/s haben wird.