Complementair filter: accelerometer- en gyroscoopdata fuseren

Inleiding

Je hebt een accelerometer die gemiddeld klopt maar nu fout is, en een gyroscoop die nu klopt maar over de tijd fout is. Het complementaire filter is de observatie dat deze twee foutprofielen elkaars tegengestelde zijn, en dat je dus de goede helft van elk kunt overnemen.

Het is één regel rekenwerk. Het is ook, voor een zelfbalancerende robot, vrijwel altijd het juiste antwoord — een Kalman-filter levert je een kleine nauwkeurigheidswinst op voor een grote prijs in complexiteit en CPU-tijd op een 8-bits microcontroller.

Het idee

Elke sensor is betrouwbaar in een ander frequentieband.

• De accelerometer is betrouwbaar bij lage frequentie. Gemiddeld over een seconde meldt hij correct waar omlaag is; neem je hem over 10 ms, dan meldt hij vooral de beweging van de robot zelf.
• De gyroscoop is betrouwbaar bij hoge frequentie. Over 10 ms is zijn geïntegreerde snelheid vrijwel exact; over een minuut heeft de opgebouwde bias hem verpest.

Dus: laagdoorlaat de accelerometer, hoogdoorlaat de gyroscoop, en tel ze op. De filters zijn zo ontworpen dat hun respons bij elke frequentie precies tot één optelt — ze vullen elkaar aan, en daar komt de naam vandaan. Geen band wordt dubbel geteld en geen enkele wordt gemist.

De implementatie

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;
}

Dat is het hele filter. Lees het als: neem waar de gyroscoop zegt dat we sinds de vorige keer zijn heen bewogen, en duw dat 2% op de kant van wat de accelerometer nu beweert.

De gyroterm levert het responsgedrag. De kleine accelerometerterm levert een langzame, permanente correctie die voorkomt dat de drift van de gyroscoop ophoopt — hij trekt de schatting voortdurend terug naar de fysieke waarheid, zacht genoeg dat voorbijgaande acceleratiepieken hem nauwelijks raken.

De tijdconstante kiezen

ALPHA is geen willekeurige knop. Hij komt overeen met een tijdconstante:

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

τ is de grens tussen de banden. Veranderingen sneller dan τ komen van de gyroscoop; langzamer dan τ wint de accelerometer. Bij een lus van 5 ms en ALPHA = 0.98 is τ = 0,245 s — ruwweg een kwart seconde gyroscoopgezag voordat de accelerometer zich weer laat gelden.

Merk op wat de formule inhoudt: ALPHA hangt af van je lusfrequentie. Kopieer je een waarde uit een project dat op 100 Hz draait naar je lus van 500 Hz, dan heb je je filter stilletjes vijf keer langzamer gemaakt. Verandert je lustiming, bereken ALPHA dan opnieuw uit de τ die je echt wilt in plaats van het getal vast te houden.

Timing moet echt zijn

dt moet de werkelijk verstreken tijd zijn, niet de tijd die je van plan was te laten verstrijken:

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

dt = 0.005 vast hardcoderen omdat je delay(5) riep is een gangbare afkorting die het resultaat stilletjes vergiftigt — alleen de I²C-leesduur kost bijna een milliseconde, dus je echte lus is 6 ms en elke integratie is 17% te kort. De hoek drijft daarna op een manier die er precies uitziet als gyrobias en die je niet weg kunt kalibreren.

Beter nog: draai de lus vanuit een hardwaretimer op een vaste frequentie in plaats van met delay(). Een balancerende lus heeft veel meer aan consistente timing dan aan een bepaalde frequentie.

De tekens goed krijgen

De meest voorkomende fout is een tekenverschil tussen de twee bronnen. Meldt de accelerometer +3° voor een voorwaartse kanteling en de gyroscoop een negatieve snelheid voor dezelfde beweging, dan vechten de twee termen: de filteruitgang loopt achter, rammelt en komt op de verkeerde waarde tot rust, en geen hoeveelheid ALPHA-afstelling lost dat op.

Controleer het rechtstreeks. Druk accelAngle en de lopende integraal van gyroRate naast elkaar af en kantel de robot dan langzaam met de hand vooruit:

• Beide moeten in dezelfde richting bewegen.
• Beide moeten ongeveer dezelfde grootte bereiken.

Bewegen ze tegengesteld, neem dan de gyrosnelheid met een minteken. Verschillen ze 90°, dan lees je de verkeerde accelerometerassen voor het vlak waarin je robot valt.

Waarom geen Kalman-filter?

Een Kalman-filter is het statistisch optimale antwoord als je de ruiskenmerken van je sensoren kent — en hij presteert inderdaad iets beter, vooral bij langdurige acceleratie.

Voor een balancerende robot rechtvaardigt dat verschil zelden de moeite. Het complementaire filter kost één vermenigvuldiging en optelling, heeft één afstelbare parameter met een heldere fysieke betekenis, en faalt op manieren die je kunt diagnosticeren door twee getallen af te drukken. Een Kalman-filter kost matrixrekenwerk elke cyclus, heeft een covariantiematrix om af te stellen, en als hij zich misgedraagt is de reden werkelijk moeilijk te zien.

Laat de robot eerst balanceren op een complementair filter. Doet hij dat goed, dan heb je je antwoord. Er staat een bredere behandeling van de afwegingen in sensorruis, filtering en fusie.

Verifieer het voordat je de lus sluit

Twee controles vangen bijna alles op, en beide kosten een minuut:

Houd hem twee minuten stil. De gefilterde hoek zou op zijn plaats moeten blijven. Kruipt hij, dan klopt je gyrobiaskalibratie niet — de accelerometerterm zou juist dit moeten voorkomen, dus kruipen betekent dat ALPHA te hoog is voor je bias.

Schud hem zonder hem te draaien. Beweeg de robot met zijn geheel van links naar rechts en houd hem verticaal. De accelerometer alleen gaat wild slingeren; de gefilterde hoek zou nauwelijks mogen bewegen. Slingert de gefilterde hoek ook, dan is ALPHA te laag en vertrouw je de accelerometer tijdens acceleratie — precies de situatie waarin hij het slechtst doet.

Zodra de hoek stil blijft als je stil staat en eerlijk blijft als je schudt, is hij goed genoeg om op te balanceren. Sluit de lus in cascaderegeling voor balancering, of kijk naar een afgestelde lus aan het werk in de balancesimulator. Dezelfde filtervorm — een signaal dat drift maar glad is, gecorrigeerd door een signaal dat ruisachtig maar begrensd is — is hoe een buitenrobot een 1 Hz GPS-fix in een bruikbare positie verandert.