Bildergalerie
Bild 1: Winkelinformationen für die Gleichgewichtsfunktion eines Roboters.
Bild 2: Blockschaltbild eines Komplementärfilters für die Gleichgewichtsfunktion  eines Roboters.
Bild 3: Der rekursive Kalman-Filteralgorithmus [3].
Bild 4: AHRS-Beispiel in der iNemo-V2-Plattform.

Mehr als nur ein Beschleunigungssensor

Der Artikel beschreibt, wie sich Beschleunigungssensoren zu einer Sensorfusions-Lösung kombinieren lassen. Zu diesem Zweck werden einige Beispiele mit komplementären Filtern (Kalman-Filter und Extended-Kalman-Filter) gezeigt, und auf diese Weise wird dem Leser die Implementierung von Sensorfusions-Lösungen näher gebracht.

Für eine ganze Reihe von Applikationen wird lediglich ein Beschleunigungssensor benötigt. Beispiele sind die Freifallerkennung und das Ermitteln der Displayorientierung sowie Schrittzähler, Neigungsmessung und Bewegungserkennung. Anspruchsvollere Anwendungen wie etwa ortsbasierte Dienste, Spiele mit erweiterter Bewegungssteuerung, Koppelnavigation für Fußgänger, das Halten des Gleichgewichts bei Robotern, Air-Mouse-Lösungen, Human Body Tracking, unbemannte Luftfahrzeuge usw. sind auf die Fusion, das heißt die Zusammenführung der Daten aus mehreren MEMS-Sensoren angewiesen, um die Genauigkeit, Auflösung, Stabilität und Reaktionsgeschwindigkeit zu verbessern. Die Sensorfusion bedient sich einer Reihe adaptiver Prädiktions- und Filteralgorithmen. Sie nutzt die verschiedenen, einander ergänzenden Informationen aus unterschiedlichen Sensoren, die auf intelligente Weise so miteinander verknüpft werden, dass die Leistungsfähigkeit des Systems insgesamt optimiert und die Möglichkeit zur Realisierung neuer, reizvoller Anwendungen geschaffen wird.

Beweggründe für die Sensorfusion

Beim Design von Systemen mit mehreren MEMS-Sensoren kommt es darauf an, über die spezifischen Vor- und Nachteile von Beschleunigungssensoren, Gyroskopen, Magnetometern und Drucksensoren Bescheid zu wissen. Mit der Sensorfusion lassen sich entscheidende Performance-Einschränkungen von 6-Achsen-Modulen beheben, die aus einem 3-Achsen-Beschleunigungssensor und einem 3-Achsen-Gyroskop bzw. einem 3-Achsen-Beschleunigungssensor und einem 3-Achsen-Magnetsensor zusammengesetzt sind.

Ein 6-Achsen-Intertialmodul, bestehend aus Beschleunigungsaufnehmer und Gyroskop, verliert durch die Langzeitdrift des Gyroskops mit der Zeit seine absolute Richtungsinformation und ist dann auf eine erneute Kalibrierung angewiesen, um eine präzise Kursreferenz wiederherzustellen. Ein 6-Achsen-Modul aus Beschleunigungssensor und Magnetometer wiederum ist anfällig gegen eine Verfälschung der Kursdaten durch ferromagnetische Materialien in der Umgebung. Ein 9-Achsen-Modul, bestehend aus Beschleunigungsaufnehmer, Gyroskop und Magnetometer, eliminiert die Drift-Effekte, die beim eigenständigen Einsatz von Sensoren auftreten, können aber durch magnetische Interferenzen beeinflusst werden. Hier werden spezielle Algorithmen benötigt, die die Sensordaten fusionieren und diese magnetischen Interferenzen kompensieren.

Die Sensorfusion nimmt die von den einzelnen Sensoren gelieferten Rohdaten entgegen, kompensiert sie gegenseitig unter Verwendung digitaler Filteralgorithmen und gibt am Schluss präzise, dynamische Lageinformationen bezogen auf die Längs- , Quer- und Hochachse aus.

Komplementäre Filter

Bei einigen Applikationen, so zum Beispiel beim Gleichgewichthalten von Robotern, bei Kamera-Bildstabilisatoren und bei 3D-Zeigegeräten ist das System mit Beschleunigungssensor und Gyroskop ausgestattet. Der Beschleunigungssensor kann präzise Informationen über den Neigungswinkel liefern, wenn sich das System in Ruhe befindet. Sobald das System allerdings Rotations- oder Translationsbewegungen ausführt, kann der Beschleunigungsaufnehmer der schnellen Bewegung nicht mehr folgen, jedoch ist das Gyroskop in der Lage, dynamische Winkelgeschwindigkeits-Informationen zu liefern. Durch einfache Integration dieser Daten über die Zeit lässt sich die Winkelverschiebung bzw. der Neigungswinkel berechnen. Allerdings nimmt der Fehler dieser Winkelinformation wegen der Bias-Drift des Gyroskops mit der Zeit immer weiter zu.

Das Komplementärfilter stellt eine einfache Möglichkeit dar, Beschleunigungssensor und Gyroskop sowie ein optionales Magnetometer zu fusionieren, um präzise und rasch aktualisierte Lageinformationen in Bezug auf die Längs- , Quer- und Hochachse zu erhalten. Das Komplementärfilter besteht aus einem gängigen Tiefpassfilter für den Beschleunigungsaufnehmer und einem Hochpass für das Gyroskop. Es ist einfacher zu verstehen und zu implementieren als ein Kalman-Filter. Im Folgenden wird exemplarisch beschrieben, wie ein Komplementärfilter für die Gleichgewichts-Funktion eines Roboters implementiert wird.

Wie der Grafik in Bild 1 zu entnehmen ist, besitzt der Roboter einen 2- oder 3-Achsen-Beschleunigungssensor zur Messung des statischen Neigungswinkels θa und ein 1- oder 2-Achsen-Gyroskop, mit dem der dynamische Neigungswinkel θg erfasst wird. Das Komplementärfilter fusioniert diese Messwerte und ermittelt daraus den finalen Neigungswinkel θ. Der Mikroprozessor nutzt diese Information zusammen mit der Winkelgeschwindigkeits-Information aus dem Gyroskop zur Ansteuerung des Motors, der den Roboter im Gleichgewicht hält. Bild 2 zeigt das Blockschaltbild eines Komplementärfilters in einer Konfiguration mit 2-Achsen-Beschleunigungssensor und einachsigem Gyroskop.

In der Formel

sind aY und aZ die normalisierten Beschleunigungswerte mit einem Wertebereich von 1g bis +1g, die nach Anwendung der Zero-g-Offset- und Skalenfaktor-Kalibrierung auf die Rohdaten des Beschleunigungssensors vorliegen. θa ist der Neigungswinkel in Grad.

Hierin handelt es sich bei ωX um die unaufbereiteten Winkelgeschwindigkeits-Daten des Gyroskops, während ωX0 den Nullpunktfehler (mit der Einheit LSB) angibt. S ist die Empfindlichkeit mit der Einheit °/s/LSB, ΔT ist das Abtastintervall und θg ist die Winkelverschiebung in Grad.

Der vom Komplementärfilter final ausgegebene Neigungswinkel berechnet sich jetzt wie folgt:

Hierin ist β eine zwischen 0 und 1 liegende Konstante. Nimmt man für β einen Wert von 0,95 an, so wird Gleichung 3 zu

Der erste Teil von Gleichung 4 wirkt wie ein Hochpassfilter, das den dynamischen Bewegungen des Roboters die Möglichkeit gibt, das Gyroskop zu durchlaufen. Der zweite Teil hingegen wirkt als Tiefpassfilter und ermöglicht der statischen bzw. quasi-statischen Bewegung des Roboters das Durchlaufen des Beschleunigungssensors.

Werden die Beschleunigungssensor- und Gyroskopdaten mit einer Rate von 100 Hz abgetastet, so beträgt das Zeitintervall ΔT genau 0,01 s. Damit ergibt sich für das Komplementärfilter die folgende Zeitkonstante:

Durch Gewichtung der Beschleunigungssensor- und Gyroskopdaten kann das Komplementärfilter wie ein einfaches Filter behandelt werden. Ist die Bewegung schneller als die Zeitkonstante von 0,19 s, so wird die Integration der Gyroskopdaten zur Berechnung der Winkelverschiebung θg höher gewichtet und das Rauschen des Beschleunigungssensors wird ausgefiltert. Ist dagegen die Bewegung langsamer als die Zeitkonstante von 0,19 s, so wird dem vom Beschleunigungssensor gemessenen Neigungswinkel θa mehr Gewicht zugemessen als dem θg-Wert des Gyroskops, um den Einfluss der Bias-Drift des Gyroskops auf die vertikale Lage zu verringern.

Die vom Komplementärfilter gelieferte Schätzung des Neigungswinkels ist damit präzise und reaktionsschnell. Sie ist außerdem unempfindlich gegenüber linearen Horizontalbeschleunigungen und der Gyroskop-Drift. Verglichen mit einem Kalman-Filter lässt sich das Komplementärfilter mit einem Mikrocontroller einfach implementieren. Nicht zuletzt ist es erweiterbar, um Beschleunigungssensor- und Gyroskopdaten mit mehreren Achsen zu fusionieren. Wenn der Nullpunktfehler oder der Bias ωX0 des Gyroskops konstant ist und sich der Roboter nicht bewegt, weist auch der vom Komplementärfilter ausgegebene Neigungswinkel einen konstanten Offset auf, der aus dem vom Beschleunigungssensor ermittelten Neigungswinkel heraus gerechnet werden kann [1]. Ist der Bias mit einer Zeit- und Temperaturdrift behaftet, so wächst der Fehler des vom Komplementärfilter ermittelten Neigungswinkels mit der Zeit immer weiter an. In diesem Fall muss ωX0 ermittelt werden, wenn der Roboter eingeschaltet ist, aber sich nicht bewegt, um die Bias-Instabilität des Gyroskops von einem Einschalten zum anderen heraus zu kompensieren. Darüber hinaus besteht immer dann, wenn der Roboter während des Betriebs bewegungslos ist, die Möglichkeit zum periodischen Einholen neuer ωX0-Werte, um Bias-Instabilitäten und kurzfristige Winkelschwankungen heraus zu rechnen [2].

Das Kalman-Filter, eine von Dr. Rudolf E. Kalman erfundene mathematische Methode, wurde seit seiner Einführung 1960 in zahlreichen Anwendungen eingesetzt. Neben den GPS-Empfängern als bekannteste Applikation wurden Kalman-Filter später auch in Lösungen verwendet, die GPS- und Trägheitsnavigation (Inertial Navigation System – INS) kombinieren. Der rekursive digitale Algorithmus dient zum Integrieren bzw. Fusionieren von GPS-Messungen mit Beschleunigungssensor- und Gyroskop-Daten mit dem Ziel, insgesamt eine optimale System-Performance zu erzielen.

Der Kalman-Filteralgorithmus liefert Schätzungen von tatsächlichen Sensor-Messwerten und den dazugehörigen berechneten Werten. Hierzu wird ein Wert vorhergesagt, um anschließend die Unsicherheit des vorhergesagten Wertes abzuschätzen und schließlich einen gewichteten Durchschnitt zwischen dem vorhergesagten Wert und dem gemessenen Wert zu errechnen. Das größte Gewicht wird dabei dem Wert mit der geringsten Unsicherheit gegeben. Die von dem Algorithmus produzierten Schätzwerte liegen tendenziell näher an den tatsächlichen Werten als die ursprünglichen Messungen, da der gewichtete Durchschnitt eine bessere geschätzte Unsicherheit aufweist als jeder der in den gewichteten Durchschnitt eingegangenen Werte.

Gegenüber dem Komplementärfilter erfordert das Kalman-Filter eine solide mathematische Grundlage unter anderem mit stochastischer Signalverarbeitung, Matrizentheorie und Regelungstheorie. Dieser Abschnitt des Artikels verwendet als Beispiel den grundlegenden Kalman-Filteralgorithmus und seine Verwendung für die Gleichgewichtsfunktion eines Roboters.

Das Kalman-Filter versucht den Zustand x eines zeitdiskret geregelten Prozesses abzuschätzen, der durch die folgende lineare Differenzengleichung [3] bestimmt wird:

  • A ist darin eine Matrix mit n-x-n-Elementen, die den Zustand im jeweils vorigen Zeitschritt k 1 zum Zustand im aktuellen Zeitschritt k in Beziehung setzt, wenn es keine Treiberfunktion und kein Prozessrauschen gibt.
  • B ist eine Matrix mit n-x-I-Elementen, die den optionalen Steuereingang zum Zustand x in Beziehung setzt.
  • H ist eine Matrix mit n-x-m-Elementen, die den Zustand zur Messung zk in Beziehung setzt.
  • wk ist das Prozessrauschen (stochastische Variablen).
  • vk ist das Messrauschen (stochastische Variablen).
  • w und v werden als voneinander unabhängig, weiß und mit normaler Wahrscheinlichkeitsverteilung angenommen, so dass gilt:

        p(w) ∼ N(0, Q)

        p(v) ∼ N(0, R)

Darin ist Q die Kovarianz-Matrix des Prozessrauschens und R die Kovarianz-Matrix des Messrauschens. In Bild 3 basieren die Kalman-Filteralgorithmen auf einem linearen dynamischen System. Daraus folgt, dass die Matrizen A, B und H konstant sind. Der Einfachheit halber werden auch die Prozess- und Mess-Kovarianzen Q und R als konstant angenommen. Der berechnete „Kalman Gain“ Kk in jedem Zeitschritt gewichtet die Messung kontinuierlich für die Fehler-Kovarianzmatrix Pk, so dass die aktualisierten Zustände präzise und stets aktuell sind.

Nun aber zurück zum eingangs beschriebenen Beispiel der Roboter-Gleichgewichtsfunktion. Es soll versucht werden, die Daten des Beschleunigungssensors und des Gyroskops für diese Anwendung zu fusionieren.

Im ersten Schritt werden die Zustände und die Messgleichung 6 aufgestellt. Offensichtlich ist der finale Neigungswinkel θ der Zustand, der abgeschätzt werden soll, um den Bias ωX0 des Gyroskops dynamisch zu kompensieren. Dies ist der andere Zustand, an dem wir interessiert sind. Darüber hinaus sollen Messwerte des Beschleunigungssensors zur Limitierung des finalen Neigungswinkels verwendet werden, damit dieser nicht infolge der Zufallsdrift des Gyroskops mit der Zeit auswandert. Es soll b = ωX0 sein und es wird angenommen, dass der Gyroskop-Bias konstant und mit weißem Rauschen behaftet ist. Die Gyroskop-Messung ωX sei der Eingang uk. Gleichung 2 kann dann wie folgt umgeschrieben werden:

Darin ist ΔT das Abtastintervall. Werden die Gyroskopdaten mit einer Rate von 100 Hz abgetastet, ist ΔT = 0,01 s.

Gemäß Gleichung 1 kann die Messung wie folgt ausgedrückt werden:

Kombiniert man die Gleichungen 7 und 8, so kann Gleichung 6 wie folgt konstruiert werden:

Aus Gleichung 9 ist ersichtlich, dass die Matrizen A, B und H konstant sind, so dass das System linear ist.

Im zweiten Schritt wird die Prozess-Kovarianzmatrix Q aus Offline-Versuchen mit dem Gyroskop und die Mess-Kovarianzmatrix R aus Offline-Experimenten mit dem Beschleunigungssensor gewonnen.

Im dritten Schritt wird der in Bild 3 gezeigte Kalman-Filteralgorithmus mit den Anfangswerten x0 und P0 in einem Mikroprozessor implementiert. Der abschließende Schritt besteht darin, die Leistungsfähigkeit des Kalman-Filters zu testen. Da der Bias des Gyroskops in der Schleife optimal geschätzt wird, ist der vom Kalman-Filter ausgegebene Neigungswinkel stets genau und unterliegt keiner Drift, sobald die Feinabstimmung der Kalman-Filterparameter Q und R erfolgt ist.

Extended-Kalman-Filter

Einige der interessantesten und erfolgreichsten Anwendungen der Kalman-Filtertechnik sind Fälle, in denen der abzuschätzende Prozess und/oder die Beziehung zwischen Messwert und Prozess nichtlinearer Natur sind. Ein Kalman-Filter, das eine Linearisierung um den aktuellen Mittelwert und die aktuelle Kovarianz vornimmt, wird als erweitertes Kalman-Filter (Extended Kalman Filter – EKF) bezeichnet. Einer Taylor-Reihe nicht unähnlich, lässt sich die Schätzung um den aktuellen Schätzwert mithilfe partieller Ableitungen der Prozess-   und Messfunktionen linearisieren, damit auch bei nichtlinearen Beziehungen Schätzwerte berechnet werden können. Anstatt sich dem EKF-Algorithmus zu widmen, beschreibt dieser Abschnitt ein Quaternion-basiertes EKF-Design zum Fusionieren der Daten von Beschleunigungssensor, Gyroskop und Magnetometer. Es wird vorausgesetzt, dass der Leser über Grundkenntnisse zu Rotationsmatrizen zwischen Körper und lokalem horizontalem Bezugssystem, Quaternionen und Eulerwinkel verfügt.

Werden ein 3-Achsen-Beschleunigungssensor, ein 3-Achsen-Gyroskop und ein 3-Achsen-Magnetometer in einem Strap-Down-System als 9-Achsen-Modul für Anwendungen, wie das Verfolgen menschlicher Bewegungen, 3-D-Spiele, unbemannte Luftfahrzeuge und Air-Mouse-Applikationen verwendet, so wird zur Berechnung der Rotationsmatrix anhand der Gyroskopmessungen meist die Quaternionen-Methode angewandt, da diese nicht dem Singularitätsproblem der Eulerwinkel unterliegen. Der EKF-Algorithmus wird implementiert, um die Daten der MEMS-Sensoren zu fusionieren und anschließend die Quaternionen und den Gyroskop-Bias zu aktualisieren und präzise Informationen zur Lage um die Längs- , Quer- und Hochachse auszugeben. Man bezeichnet dies als Kurs- und Lagereferenzsystem (Attitude Heading Reference System – AHRS). STMicroelectronics hat das 9-Achsen-MEMS-Sensor-Demonstrationsboard mit der Bezeichnung iNertial Module (iNemo) V2 im Angebot. Für das AHRS wurde das quaternionenbasierte EKF in einem STM32-Mikrocontroller implementiert (Bild 4). Wird das Board im dreidimensionalen Raum gedreht, folgt der Fischwürfel der Bewegung und gibt in Echtzeit präzise dynamische Angaben zum Nick- , Roll- und Gierwinkel sowie die Quaternionenwerte aus. Nachfolgend ist kurz das EKF-Design für das iNemo V2-Board beschrieben. Das Design des EKF läuft in folgenden Schritten ab:

1. Definition der Zustände und Messungen mit der Quaternionenmethode

2. Konstruktion des dynamischen Systemmodells und der Beziehung zwischen Messungen und Zuständen

3. Linearisieren und Diskretisieren des Systemmodells und Berechnung der Jacobimatrix für die Matrix A, B und H in jedem Zeitschritt k 

4. Ermitteln der normalisierten Quaternionenwerte

5. Berechnung Eulerwinkel um die Quer-, Längs- und Hochachse.

Schlussbemerkung

Sobald ein System mehrere MEMS-Sensoren enthält, ist die Implementierung von Sensorfusions-Algorithmen erforderlich, damit die Fähigkeiten der einzelnen Sensoren im Interesse einer besseren Gesamt-Performance bestmöglich ausgeschöpft werden können. Für Kalman- und Extended-Kalman-Filter lassen sich verschiedene Systemmodelle mit unterschiedlichen Sensor-Bias-Modellen entwerfen, während die grundlegenden rekursiven Algorithmen unverändert bleiben. Kalman- und Extended-Kalman-Filter können als zentrales Element des Sensorfusions-Konzepts betrachtet werden. Hinsichtlich der Performance ist das EKF die beste Lösung. Es bringt allerdings auch den größten Rechenaufwand und damit auch eine hohe Leistungsaufnahme mit sich, was beispielsweise in Smartphones nicht aus dem Blick geraten darf. Bei der Entscheidung, welcher Sensorfusions-Algorithmus verwendet werden soll, muss stets ein Kompromiss zwischen Leistungsfähigkeit und Stromverbrauch gefunden werden.