Filtro complementare: unire accelerometro e giroscopio

Introduzione

Hai un accelerometro corretto in media ma sbagliato in questo istante e un giroscopio corretto in questo istante ma sbagliato nel tempo. Il filtro complementare parte dall’osservazione che questi due profili di errore sono opposti e che quindi puoi prendere la parte buona di ciascuno.

È una sola riga di calcolo. Per un robot autobilanciato è anche, quasi sempre, la risposta giusta: un filtro di Kalman offre un piccolo miglioramento della precisione al costo di un aumento notevole della complessità e del tempo della CPU su un microcontrollore a 8 bit.

L’idea

Ogni sensore è affidabile in una diversa banda di frequenza.

• L’accelerometro è affidabile alle basse frequenze. Mediato nell’arco di un secondo, indica correttamente la direzione del basso; se campionato ogni 10 ms, indica soprattutto il movimento proprio del robot.
• Il giroscopio è affidabile alle alte frequenze. Su 10 ms la velocità integrata è essenzialmente esatta; dopo un minuto il bias accumulato lo ha completamente compromesso.

Quindi: applica un filtro passa-basso all’accelerometro, un filtro passa-alto al giroscopio e somma i risultati. I filtri sono progettati in modo che le risposte sommano a esattamente uno a ogni frequenza: si complementano a vicenda ed è da qui che deriva il nome. Nessuna banda viene contata due volte e nessuna viene persa.

L’implementazione

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

Questo è tutto il filtro. Leggilo così: prendi il punto raggiunto dal giroscopio rispetto all’ultima lettura e spostalo del 2% verso ciò che attualmente indica l’accelerometro.

Il termine del giroscopio fornisce la prontezza. Il piccolo termine dell’accelerometro fornisce una correzione lenta e permanente che impedisce al drift del giroscopio di accumularsi: riporta continuamente la stima verso la verità fisica, con una delicatezza tale che i picchi transitori di accelerazione la spostano a malapena.

Scelta della costante di tempo

ALPHA non è una manopola arbitraria. Corrisponde a una costante di tempo:

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

τ è il confine tra le bande. Le variazioni più rapide di τ provengono dal giroscopio; quelle più lente derivano dall’accelerometro. Con un anello a 5 ms e ALPHA = 0.98, τ = 0.245 s, ovvero circa un quarto di secondo di autorità del giroscopio prima che l’accelerometro inizi a imporsi nuovamente.

Nota cosa implica la formula: ALPHA dipende dalla frequenza del tuo anello. Se copi un valore da un progetto che gira a 100 Hz e lo usi nel tuo anello a 500 Hz, hai reso il filtro cinque volte più lento senza accorgertene. Se la temporizzazione dell’anello cambia, ricalcola ALPHA partendo dal τ effettivamente desiderato invece di mantenere il valore numerico.

La temporizzazione deve essere reale

dt deve essere il tempo realmente trascorso, non quello che intendevi far trascorrere:

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

Imporre dt = 0.005 perché hai chiamato delay(5) è una scorciatoia comune che avvelena in silenzio il risultato: da sola la lettura I²C richiede quasi un millisecondo, quindi il tuo anello reale dura 6 ms e ogni integrazione è più breve del 17%. L’angolo deriva poi in un modo che sembra esattamente un bias del giroscopio e che non è possibile eliminare con una calibrazione.

Meglio ancora, esegui l’anello da un timer hardware a frequenza fissa invece di usare delay(). Un anello di bilanciamento richiede una temporizzazione coerente molto più di quanto richieda una frequenza particolare.

Ottieni i segni corretti

Il guasto più comune è una discordanza di segno tra le due fonti. Se l’accelerometro indica +3° per un’inclinazione in avanti e il giroscopio indica una velocità negativa per lo stesso movimento, i due termini si contraddicono: l’uscita del filtro avrà ritardo, oscillerà e si stabilizzerà sul valore sbagliato, e nessuna regolazione di ALPHA risolverà il problema.

Verificalo direttamente. Stampa affiancati accelAngle e l’integrale corrente di gyroRate, poi inclina lentamente il robot in avanti con la mano:

• Entrambi devono muoversi nella stessa direzione.
• Entrambi devono raggiungere all’incirca la stessa grandezza.

Se si muovono in direzioni opposte, inverte il segno della velocità del giroscopio. Se differiscono di 90°, stai leggendo gli assi sbagliati dell’accelerometro per il piano in cui il robot cade.

Perché non usare un filtro di Kalman?

Un filtro di Kalman è la risposta statisticamente ottimale se conosci le caratteristiche del rumore dei sensori, e funziona leggermente meglio, in particolare durante le accelerazioni prolungate.

Per un robot bilanciato la differenza raramente lo giustifica. Il filtro complementare richiede una sola moltiplicazione e una sola addizione, ha un unico parametro regolabile con un significato fisico chiaro e fallisce in modi che puoi diagnosticare stampando due numeri. Un filtro di Kalman richiede aritmetica matriciale a ogni ciclo, ha una matrice di covarianza da regolare e, quando si comporta male, il motivo è davvero difficile da individuare.

Fai prima equilibrarsi il robot con un filtro complementare. Se si equilibra bene, hai la risposta. Nel capitolo sul rumore dei sensori, il filtraggio e la fusione trovi una trattazione più ampia dei compromessi.

Verificalo prima di chiudere l’anello

Due controlli intercettano quasi tutto e richiedono entrambi un minuto:

Tienilo fermo per due minuti. L’angolo filtrato deve restare fermo. Se scivola lentamente, la calibrazione del bias del giroscopio è errata: il termine dell’accelerometro dovrebbe impedirlo esattamente, quindi uno scorrimento significa che ALPHA è troppo alto per il tuo bias.

Scuotilo senza ruotarlo. Sposta il robot lateralmente nel suo insieme, mantenendolo verticale. L’accelerometro da solo oscillerà violentemente; l’angolo filtrato dovrebbe muoversi a malapena. Se anche l’angolo filtrato oscilla, ALPHA è troppo basso e durante l’accelerazione stai fidandoti dell’accelerometro, proprio nella situazione in cui è peggio.

Quando l’angolo resta fermo da fermo e si mantiene affidabile quando scuoti il robot, è abbastanza buono da consentire il bilanciamento. Chiudi l’anello nel controllo PID in cascata per il bilanciamento oppure osserva un anello regolato in funzione nel simulatore di equilibrio. La stessa forma di filtro, un segnale che deriva ma è regolare corretto da uno che è rumoroso ma limitato, è ciò che permette a un robot all’aperto di trasformare una correzione GPS a 1 Hz in una posizione utilizzabile.