Komplementärfilter: Beschleunigung und Gyro fusionieren

Einführung

Du hast einen Beschleunigungssensor, der im Mittel recht, aber gerade jetzt falsch liegt, und ein Gyroskop, das gerade jetzt recht, aber über Zeit falsch liegt. Der Komplementärfilter ist die Beobachtung, dass diese zwei Fehlerprofile Gegensätze sind und du darum von jedem die gute Hälfte nehmen kannst.

Es ist eine Zeile Arithmetik. Für einen Balance-Roboter ist es zudem fast immer die richtige Antwort – ein Kalman-Filter kauft dir kleine Genauigkeitsgewinne zu großem Preis an Komplexität und CPU-Zeit auf einem 8-Bit-Mikrocontroller.

Die Idee

Jeder Sensor ist in einem anderen Frequenzband verlässlich.

• Das Beschleunigungsmesser ist bei niedriger Frequenz verlässlich. Über eine Sekunde gemittelt meldet es korrekt, wo unten ist; über 10 ms abgetastet meldet es meist die Eigenbewegung des Roboters.
• Das Gyroskop ist bei hoher Frequenz verlässlich. Über 10 ms ist seine integrierte Rate praktisch exakt; über eine Minute hat der akkumulierte Bias sie ruiniert.

Also: Tiefpass fürs Beschleunigungsmesser, Hochpass fürs Gyro, und addieren. Die Filter sind so entworfen, dass ihre Antworten sich bei jeder Frequenz zu exakt eins summieren – sie ergänzen einander, daher der Name. Kein Band wird doppelt gezählt, keines verpasst.

Die Implementierung

const float ALPHA = 0.98;
float angle = 0;

void updateAngle(float accelAngle, float gyroRate, float dt) {
  angle = ALPHA * (angle + gyroRate * dt) + (1.0 - ALPHA) * accelAngle;
}

Das ist der ganze Filter. Lies es als: Nimm, wohin das Gyro seit dem letzten Mal gegangen zu sein sagt, und schubse es 2 % Richtung dessen, was das Beschleunigungsmesser gerade behauptet.

Der Gyroanteil liefert Reaktionsfreude. Der kleine Beschleunigungsanteil liefert eine langsame, permanente Korrektur, die die Gyrodrift am Akkumulieren hindert – er zieht die Schätzung ständig sanft zur physischen Wahrheit zurück, sanft genug, dass transiente Beschleunigungsspitzen sie kaum bewegen.

Die Zeitkonstante wählen

ALPHA ist kein willkürlicher Knopf. Es entspricht einer Zeitkonstante:

τ = (ALPHA · dt) / (1 − ALPHA)

τ ist die Grenze zwischen den Bändern. Änderungen schneller als τ kommen vom Gyro; langsamer als τ gewinnt das Beschleunigungsmesser. Bei 5-ms-Schleife und ALPHA = 0,98 ist τ = 0,245 s – grob eine Viertelsekunde Gyro-Autorität, bevor das Beschleunigungsmesser sich wieder durchsetzt.

Beachte, was die Formel impliziert: ALPHA hängt von deiner Schleifenrate ab. Kopiere einen Wert aus einem 100-Hz-Projekt in deine 500-Hz-Schleife, hast du deinen Filter leise fünfmal langsamer gemacht. Ändert sich dein Schleifentiming, rechne ALPHA aus dem τ neu, das du wirklich willst, statt die Zahl zu behalten.

Timing muss echt sein

dt muss die wirklich verstrichene Zeit sein, nicht die beabsichtigte:

unsigned long now = micros();
float dt = (now - lastUpdate) / 1000000.0;
lastUpdate = now;

dt = 0,005 fest zu verdrahten, weil du delay(5) aufriefst, ist eine gängige Abkürzung, die das Ergebnis leise vergiftet – allein der I²C-Lesezugriff dauert fast eine Millisekunde, deine echte Schleife ist also 6 ms und jede Integration 17 % zu kurz. Der Winkel driftet dann auf eine Art, die exakt wie Gyro-Bias aussieht und sich nicht wegkalibrieren lässt.

Besser noch: Fahre die Schleife von einem Hardwaretimer mit fester Rate statt delay(). Eine Balanceschleife braucht konsistentes Timing weit mehr als eine bestimmte Rate.

Die Vorzeichen richtig hinbekommen

Der häufigste Ausfall ist eine Vorzeichen-Diskrepanz beider Quellen. Meldet das Beschleunigungsmesser +3° für Vorwärtsneigung und das Gyro negative Rate für dieselbe Bewegung, bekämpfen sich beide Anteile: Der Filterausgang hinkt, zittert und setzt sich auf falschem Wert – und kein ALPHA-Tuning fixt das.

Prüfe es direkt. Drucke accelAngle und das laufende Integral von gyroRate nebeneinander und neige den Roboter langsam von Hand vorwärts:

• Beide sollten sich in dieselbe Richtung bewegen.
• Beide sollten grob dieselbe Größe erreichen.

Laufen sie entgegengesetzt, negiere die Gyrorate. Unterscheiden sie sich um 90°, liest du die falschen Beschleunigungsachsen für die Ebene, in der dein Roboter fällt.

Warum kein Kalman-Filter?

Ein Kalman-Filter ist die statistisch optimale Antwort, wenn du die Rauscheigenschaften deiner Sensoren kennst – und es leistet etwas besser, besonders bei anhaltender Beschleunigung.

Für einen Balance-Roboter rechtfertigt der Unterschied es selten. Der Komplementärfilter kostet eine Multiplizier-Addier-Operation, hat einen abstimmbaren Parameter mit klarer physikalischer Bedeutung und fällt auf Arten aus, die du durch Drucken zweier Zahlen diagnostizierst. Ein Kalman-Filter kostet jeden Zyklus Matrixarithmetik, hat eine Kovarianzmatrix zum Abstimmen, und wenn er sich danebenbenimmt, ist der Grund wirklich schwer zu sehen.

Bringe den Roboter erst auf einem Komplementärfilter zum Balancieren. Balanciert er gut, hast du deine Antwort. Eine breitere Behandlung der Kompromisse steht in Sensorrauschen, Filterung und Fusion.

Verifiziere ihn, bevor du die Schleife schließt

Zwei Prüfungen fangen fast alles, und beide dauern eine Minute:

Halte ihn zwei Minuten still. Der gefilterte Winkel sollte stehen bleiben. Kriecht er, ist deine Gyro-Bias-Kalibrierung daneben – der Beschleunigungsanteil sollte genau das verhindern, Kriechen heißt also, ALPHA ist zu hoch für deinen Bias.

Schüttle ihn ohne zu rotieren. Bewege den Roboter körperlich seitwärts, halte ihn senkrecht. Das Beschleunigungsmesser allein schwingt wild; der gefilterte Winkel sollte sich kaum bewegen. Schwingt der gefilterte Winkel mit, ist ALPHA zu niedrig und du vertraust dem Beschleunigungsmesser während Beschleunigung – genau die Lage, in der es am schlechtesten ist.

Sobald der Winkel stillsteht, wenn still, und ehrlich bleibt, wenn geschüttelt, ist er gut genug zum Balancieren. Schließe die Schleife in kaskadierter PID-Balanceregelung oder beobachte eine abgestimmte Schleife im Balance-Simulator. Dieselbe Filterform – ein Signal, das driftet, aber glatt ist, korrigiert von einem, das verrauscht, aber begrenzt ist –, macht aus einem 1-Hz-GPS-Fix eine nutzbare Position für einen Outdoor-Roboter.