Komplementärfilter: Grundlagen und Sensorfusion

Abb. 1: Komplementärfilter zur Sensorfusion

Autor:	Prof. Dr.-Ing. Schneider
Versuch 3	MTR Fachpraktikum Mechatronik, MTR-B-2-5.10 BSE Angewandte Mechatronik, BSE-M-2-2.09

Einleitung

Bei der Zustandsschätzung eines mobilen Roboters werden häufig mehrere Sensoren eingesetzt, um eine möglichst zuverlässige Schätzung der Position und Orientierung zu erhalten. Dabei liefern unterschiedliche Sensoren Informationen über dieselbe physikalische Größe, besitzen jedoch unterschiedliche Messfehler. Ein typisches Beispiel ist die Schätzung des Kurswinkels ψ eines Differentialantriebs. Hierfür können die Raddrehgeber (Odometrie) und das Gyroskop einer inertialen Messeinheit (IMU) verwendet werden.

Beide Verfahren besitzen spezifische Vor- und Nachteile:

Sensor	Vorteil	Nachteil
Odometrie	Liefert eine kontinuierliche Schätzung der zurückgelegten Strecke und Orientierung.	Fehler durch Radschlupf, unterschiedliche Raddurchmesser und mechanische Toleranzen akkumulieren.
Gyroskop	Erfasst Winkelgeschwindigkeiten mit hoher zeitlicher Auflösung und reagiert schnell auf Drehbewegungen.	Ein Messoffset (Bias) führt bei der Integration zu einem zunehmenden Winkelfehler (Drift).

Die alleinige Verwendung eines Sensors führt somit zu einer fehlerbehafteten Orientierungsschätzung.

Die Grundidee der Sensorfusion besteht darin, die Vorteile beider Messverfahren zu kombinieren. Ein geeignetes Verfahren hierfür ist das Komplementärfilter (englisch: complementary filter).

Lernziele

Nach der Bearbeitung dieses Artikels können Studierende:

• das Grundprinzip eines Komplementärfilters erläutern,
• die unterschiedlichen Fehlercharakteristiken von Gyroskop und Odometrie beschreiben,
• die kontinuierliche und diskrete Filtergleichung herleiten,
• den Einfluss des Filterparameters α beurteilen,
• einen Komplementärfilter in MATLAB oder Simulink implementieren und experimentell untersuchen.

Grundprinzip des Komplementärfilters

Ein Komplementärfilter kombiniert zwei Messsignale, deren Eigenschaften sich hinsichtlich ihres Frequenzverhaltens ergänzen.

Im vorliegenden Anwendungsfall werden zwei Schätzungen des Kurswinkels betrachtet:

1. Gyroskopbasierte Winkelschätzung: Durch Integration der gemessenen Winkelgeschwindigkeit wird der Kurswinkel bestimmt. Das Gyroskop erfasst schnelle Änderungen der Orientierung sehr gut. Allerdings führen Bias und niederfrequente Fehler langfristig zu einer Drift.
2. Odometriebasierte Winkelschätzung: Aus den zurückgelegten Strecken des linken und rechten Rades wird die Orientierungsänderung berechnet. Die Odometrie liefert eine zusätzliche Referenz, kann jedoch durch Schlupf und systematische Fehler verfälscht werden.

Das Komplementärfilter kombiniert beide Informationen:

• Das Gyroskop liefert den hochfrequenten Anteil der Orientierungsschätzung.
• Die Odometrie liefert den niederfrequenten Anteil und korrigiert langfristige Abweichungen der Gyroskopintegration.

Die Bezeichnung Komplementärfilter ergibt sich daraus, dass sich die Übertragungsfunktionen des Tiefpass- und Hochpassanteils zu eins ergänzen.
$H_{TP}(s)+H_{HP}(s)=1$

Damit werden die relevanten Frequenzanteile beider Eingangssignale gemeinsam zur Zustandsschätzung genutzt.

Mathematische Beschreibung

Winkelbestimmung mit dem Gyroskop

Ein Gyroskop misst die Winkelgeschwindigkeit ω um eine bestimmte Achse. Für einen mobilen Roboter wird insbesondere die Drehung um die vertikale Achse (Gierrate) betrachtet.

Der Kurswinkel ergibt sich durch Integration der Winkelgeschwindigkeit:
${\psi }_{Gyro}(t)={\psi }_0+\underset{0}{\overset{t}{\int }}{\omega }_z(\tau )\mathrm{d}\tau$

In der Realität enthält die Messung einen Messoffset (Bias) sowie Rauschen:
${\omega }_{mess}(t)={\omega }_{wahr}(t)+b_g(t)+v_g(t)$

Dabei sind:

• ${\omega }_{mess}$: gemessene Winkelgeschwindigkeit
• ${\omega }_{wahr}$: tatsächliche Winkelgeschwindigkeit
• $b_g$: Gyroskopbias
• $v_g$: Messrauschen

Ein konstanter Bias führt zu einem linear anwachsenden Winkelfehler:
$\mathrm{\Delta }\psi (t)=b_g\cdot t$

Beispiel: Ein Bias von 0,5 °/s verursacht nach 60 s einen Winkelfehler von 30°.

Eine Kalibrierung im Stillstand ermöglicht die Bestimmung des anfänglichen Bias. Temperaturabhängige und zeitlich veränderliche Biasanteile können jedoch weiterhin auftreten.

Winkelbestimmung mit der Odometrie

Ein Differentialantrieb besitzt zwei unabhängig angetriebene Räder. Aus den zurückgelegten Strecken des linken und rechten Rades lässt sich die Orientierungsänderung berechnen.

Für ein Rad mit Radius r und n gezählten Encoderimpulsen gilt:
$\mathrm{\Delta }s=\frac{2\pi r}{N}n$

Dabei bezeichnet $N$ die Anzahl der Encoderimpulse pro Radumdrehung.

Die Änderung des Kurswinkels ergibt sich aus:
$\mathrm{\Delta }{\psi }_{Odo}=\frac{\mathrm{\Delta }{s}_R-\mathrm{\Delta }{s}_L}{l}$

mit:

• $\mathrm{\Delta }{s}_R$: zurückgelegte Strecke des rechten Rades
• $\mathrm{\Delta }{s}_L$: zurückgelegte Strecke des linken Rades
• l: Spurweite des Roboters

Der Kurswinkel wird durch fortlaufende Integration der Winkeländerungen bestimmt:
${\psi }_{Odo,k}={\psi }_{Odo,k-1}+\mathrm{\Delta }{\psi }_{Odo,k}$

Auch diese Schätzung ist nicht fehlerfrei. Insbesondere Radschlupf, Unebenheiten des Untergrundes und unterschiedliche effektive Raddurchmesser können zu einer zunehmenden Abweichung führen.

Frequenzabhängige Kombination

Das Komplementärfilter nutzt einen Tiefpass und einen Hochpass, um die beiden Eingangssignale frequenzabhängig zu gewichten.

Für ein Filter erster Ordnung wird die Tiefpassübertragungsfunktion definiert als:
$H_{TP}(s)=\frac{1}{1+\tau s}$

Die zugehörige Hochpassübertragungsfunktion lautet:
$H_{HP}(s)=\frac{\tau s}{1+\tau s}$

Dabei bezeichnet τ die Filterzeitkonstante.

Es gilt:
$H_{TP}(s)+H_{HP}(s)=\frac{1+\tau s}{1+\tau s}=1$

Die Ausgangsgröße des Komplementärfilters ergibt sich somit zu:
$\psi (s)=H_{HP}(s){\psi }_{Gyro}(s)+H_{TP}(s){\psi }_{Odo}(s)$

Der Tiefpass unterdrückt hochfrequente Störungen der Odometrie. Der Hochpass reduziert den Einfluss niederfrequenter Fehler der gyroskopbasierten Winkelschätzung.

Diskrete Implementierung

Für die Implementierung auf einem Mikrocontroller oder in Simulink wird das kontinuierliche Filter in eine zeitdiskrete Form überführt.

Die Gyroskopintegration erfolgt näherungsweise mit:
${\psi }_{Gyro,k}={\psi }_{k-1}+{\omega }_{z,k}\mathrm{\Delta }t$

Die korrigierte Winkelschätzung ergibt sich aus der gewichteten Kombination:
${\hat{\psi }}_k=\alpha ({\hat{\psi }}_{k-1}+{\omega }_{z,k}\mathrm{\Delta }t)+(1-\alpha ){\psi }_{Odo,k}$

Dabei sind:

• ${\hat{\psi }}_k$: geschätzter Kurswinkel zum Zeitpunkt k
• ${\hat{\psi }}_{k-1}$: geschätzter Kurswinkel zum vorherigen Zeitpunkt
• ${\omega }_{z,k}$: gemessene Gierrate
• $\mathrm{\Delta }t$: Abtastzeit
• ${\psi }_{Odo,k}$: aktueller Kurswinkel aus der Odometrie
• α: Gewichtungsfaktor des Komplementärfilters

Der Gewichtungsfaktor liegt im Bereich:
$0\le \alpha \le 1$

Bedeutung des Gewichtungsfaktors

Der Parameter α bestimmt das Verhältnis zwischen der Gyroskopintegration und der odometriebasierten Winkelschätzung.

Parameter	Wirkung
α = 1	Ausschließliche Verwendung der Gyroskopintegration. Es findet keine Korrektur durch die Odometrie statt.
α = 0,5	Beide Schätzungen werden gleich gewichtet.
α = 0	Ausschließliche Verwendung der Odometrie.

Ein hoher Wert von α führt dazu, dass schnelle Orientierungsänderungen des Gyroskops stärker berücksichtigt werden. Ein niedriger Wert führt zu einer stärkeren Gewichtung der Odometrie.

Die Wahl von α beeinflusst somit das dynamische Verhalten und die Fehlercharakteristik der resultierenden Schätzung.

Zusammenhang zwischen α und Filterzeitkonstante

Für die zeitdiskrete Implementierung mit der Abtastzeit Δt ergibt sich bei einer Rückwärtsdifferenzdiskretisierung:
$\alpha =\frac{\tau }{\tau +\mathrm{\Delta }t}$

Durch Umstellen erhält man:
$\tau =\frac{\alpha }{1-\alpha }\mathrm{\Delta }t$

Die Grenzfrequenz des Filters erster Ordnung beträgt:
$f_c=\frac{1}{2\pi \tau }$

Damit lässt sich der Gewichtungsfaktor über die gewünschte Filterzeitkonstante oder Grenzfrequenz bestimmen.

Beispielrechnung

• Ein mobiler Roboter besitzt eine Abtastzeit von 10 ms. Für die Sensorfusion wird ein Gewichtungsfaktor von α = 0,95 gewählt.
• Die aktuelle gyroskopbasierte Winkelschätzung beträgt 42 °. Die Odometrie liefert einen Kurswinkel von 40°.
• Die Winkelgeschwindigkeit des Gyroskops beträgt 20 °/s.

Zunächst wird die Winkeländerung aus der Gyroskopmessung berechnet:
$\mathrm{\Delta }{\psi }_{Gyro}=20\frac{{}^{\circ }}{s}\cdot 0,01s=0,2^{\circ }$

Damit ergibt sich die gyroskopbasierte Winkelschätzung:
${\psi }_{Gyro,k}=42^{\circ }+0,2^{\circ }=42,2^{\circ }$

Die gewichtete Kombination lautet:
${\hat{\psi }}_k=0,95\cdot 42,2^{\circ }+0,05\cdot 40^{\circ }=42,09^{\circ }$

Die geschätzte Orientierung beträgt somit 42,09°.

Die Filterzeitkonstante beträgt:
$\tau =\frac{0,95}{1-0,95}\cdot 0,01s=0,19s$

Die Grenzfrequenz ergibt sich zu:
$f_c=\frac{1}{2\pi \cdot 0,19s}\approx 0,84Hz$

Praktische Implementierung

Das Komplementärfilter lässt sich mit wenigen Rechenschritten in MATLAB^®/Simulink realisieren.

Die folgende MATLAB-Funktion berechnet die geschätzte Orientierung aus Gyroskop und Odometrie.

function psiEst = KomplementaerFilter(psiAlt, gyroZ, psiOdo, Ts, alpha)

    % Gyroskopintegration
    psiGyro = psiAlt + gyroZ * Ts;

    % Komplementärfilter
    psiEst = alpha * psiGyro + (1-alpha) * psiOdo;
end

Voraussetzung ist, dass alle Winkel in derselben Einheit vorliegen. Die Winkelgeschwindigkeit muss entsprechend in °/s oder rad/s angegeben werden.

Bei Winkeln mit einem Wertebereich von beispielsweise -180° bis +180° muss außerdem ein Winkelsprung bei der Berechnung der Winkeldifferenz berücksichtigt werden. Andernfalls kann ein Übergang von +179° auf -179° fälschlich als Änderung um -358° interpretiert werden.

Praktische Grenzen und Fehlerquellen

Ein Komplementärfilter kann die Qualität der Orientierungsschätzung verbessern. Die Verbesserung hängt jedoch wesentlich von den Eigenschaften der verwendeten Sensoren ab.

Insbesondere sind folgende Aspekte zu berücksichtigen:

Gyroskopbias	Ein nicht korrigierter Bias führt zu einer systematischen Abweichung der Winkelintegration. Der Komplementärfilter kann diese Drift durch die Odometrie begrenzen, sofern deren Winkelschätzung hinreichend zuverlässig ist.
Radschlupf	Schlupf führt zu einer fehlerhaften odometriebasierten Winkelschätzung. Eine starke Gewichtung der Odometrie kann deshalb die Schätzqualität verschlechtern.
Abtastzeit	Eine unregelmäßige Abtastzeit führt zu Fehlern bei der numerischen Integration. Für die Implementierung sollte möglichst die tatsächlich verstrichene Zeit verwendet werden.
Zeitliche Synchronisation	Die Messwerte beider Sensoren müssen zeitlich zueinander passen. Verzögerungen können insbesondere bei schnellen Drehbewegungen zu Fehlern führen.
Winkelperiodizität	Winkel sind periodische Größen. Vor der Kombination muss sichergestellt werden, dass beide Winkel konsistent dargestellt werden. Bei der Winkeldifferenz ist eine geeignete Winkelnormalisierung erforderlich.

Abgrenzung zum Kalman-Filter

• Das Komplementärfilter ist ein vergleichsweise einfaches Verfahren zur Sensorfusion. Es benötigt keine explizite Modellierung der Prozess- und Messrauschkovarianzen.
• Ein Kalman-Filter berücksichtigt dagegen ein Zustandsraummodell und die statistischen Eigenschaften der Prozess- und Messunsicherheiten. Dadurch kann die Gewichtung der Messinformationen systematisch aus den Unsicherheiten abgeleitet und dynamisch angepasst werden.
• Das Komplementärfilter eignet sich insbesondere als Einstieg in die Sensorfusion, da die Wirkung der beiden Sensoren und der Filterparameter unmittelbar nachvollzogen werden kann.

Zusammenfassung

Das Komplementärfilter kombiniert zwei unterschiedlich fehlerbehaftete Schätzungen derselben physikalischen Größe. Im betrachteten Anwendungsfall liefert das Gyroskop eine dynamisch gute Winkeländerung, während die Odometrie eine zusätzliche Referenz zur Begrenzung der langfristigen Drift bereitstellt.

Durch die frequenzabhängige Kombination werden die Vorteile beider Sensoren genutzt. Die Qualität der Schätzung hängt dabei wesentlich von der Wahl der Filterzeitkonstante, der Abtastzeit und den Fehlercharakteristiken der Sensoren ab.

Das Komplementärfilter stellt damit ein einfaches und anschauliches Verfahren zur Einführung in die Sensorfusion dar.

Literatur

[1]	LIU, Mei; CAI, Yuanli; ZHANG, Lihao und WANG, Yiqun. Attitude Estimation Algorithm of Portable Mobile Robot Based on Complementary Filter. In: Micromachines. 2021, 12(11), 1373. DOI: 10.3390/mi12111373
[2]	VALENTI, Roberto G.; DRYDEN, Daniel; FENG, Jizhong und HEWITT, John. Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. In: Sensors. 2015, 15(8), S. 19302–19330. DOI: 10.3390/s150819302.
[3]	SUGIHARA, Tomomichi; MASUYA, Ken und YAMAMOTO, Motoji. A Complementary Filter for High-fidelity Attitude Estimation based on Decoupled Linear/Nonlinear Properties of Inertial Sensors. In: Journal of the Robotics Society of Japan. 2013, 31(3), S. 251–262. DOI: 10.7210/jrsj.31.251.

---

→ zurück zum Hauptartikel: Versuchsleitfaden: Rapid Control Prototyping