Prodotti
Alessandro
Filtro complementare e filtro di Kalman
Tutorials -
Domenica 30 Gennaio 2011 16:28
Scritto da Alessandro

L’utilizzo di accelerometro e giroscopio nella costruzione di robottini, ad esempio il self balancing, impone l’utilizzo di un filtro per poter effettuare la fusione dei segnali restituiti dai due sensori.

Il giroscopio tende infatti ad avere un drift, una deriva, che nel tempo gli fa rilevare valori completamente sbagliati. L’accelerometro, invece, pur dando un valore fedele quando l’accelerazione è progressiva, soffre moltissimo le vibrazioni, durante le quali restituisce valori dell’angolo sbagliati ed eccessivi.

Il filtro principe utilizzato anche da Gbm in questo sito o in moltissime realizzazioni è quello di Kalman. E’ il miglior filtro che si può utilizzare, anche da un punto di vista teorico, poiché è quello che minimizza l’errore rispetto al segnale vero. Tuttavia, anche guardando i listati di Gbm, è molto difficile. Non nego di avere difficoltà a capire come funziona da un punto di vista teorico e di non essere in grado di utilizzarlo al meglio. Infatti il filtro richiede di saper calcolare dei coefficienti delle matrici, basati sull’errore del processo, sull’errore di misura etc che non sono banali.

Ultimamente stanno emergendo i filtri cosiddetti complementari. Infatti gestiscono al loro interno filtri passa basso e passa alto contemporaneamente. I filtri passa basso filtrano i segnali con frequenza elevata (ad esempio l’accelerometro nel caso delle vibrazioni) e filtri passa alto che filtrano i segnali con bassa frequenza (ad esempio il drift del giroscopio). Combinando opportunamente questi filtri si ottiene un segnale buono, senza le complicazioni del filtro di Kalman.

Volendo fare un approfondimento da un punto teorico, il discorso si complica ed esula dallo scopo di questo tutorial. Si consideri che i filtri complementari possono essere di ordini diversi. Si parte da filtri cosiddetti del primo ordine che filtrano già bene a filtri del secondo ordine che filtrano ancora meglio. Chiaramente passando dal primo al secondo ordine l’algoritmo da utilizzare si complica, forse senza che vi sia un guadagno così evidente da giustificare l’aumento di complessità.

Una ottima introduzione ai filtri complementari del primo ordine applicati all’accelerometro e giroscopio, arriva addirittura dal MIT: http://web.mit.edu/scolton/www/filter.pdf

Introduce in modo molto semplice ed immediato la tematica. E proprio su questo testo è basato il primo algoritmo su Arduino che ho ottenuto:

 

// a=tau / (tau + loop time)
// newAngle = angle measured with atan2 using the accelerometer
// newRate =  angle measured using the gyro
// looptime = loop time in millis()

float tau=0.075;
float a=0.0;

float Complementary(float newAngle, float newRate,int looptime) {
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC);
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
return x_angleC;
}

 

 

E’ sufficiente scegliere il tempo di risposta tau, passare gli argomenti, cioè la misurazione dell’angolo con accelerometro, giroscopio e tempo del loop e si ottiene, in due sole righe, l’angolo.

L’algoritmo alla base del filtro complementare del secondo ordine è descritto qui: http://www.rcgroups.com/forums/showatt.php?attachmentid=2477096&d=1240377331

Effettivamente non è per niente descritto, ma ormai abbiamo già capito come funziona il filtro complementare dalla documentazione del MIT. Il principio è lo stesso, si complica l’algoritmo. La traduzione di questo algoritmo per Arduino è la seguente:

 

// newAngle = angle measured with atan2 using the accelerometer
// newRate =  angle measured using the gyro
// looptime = loop time in millis()


float Complementary2(float newAngle, float newRate,int looptime) {
float k=10;
float dtc2=float(looptime)/1000.0;

x1 = (newAngle -   x_angle2C)*k*k;
y1 = dtc2*x1 + y1;
x2 = y1 + (newAngle -   x_angle2C)*2*k + newRate;
x_angle2C = dtc2*x2 + x_angle2C;

return x_angle2C;
}

 

Anche qui, basta impostare il k e si ottiene magicamente il risultato.

Volendo invece applicare il filtro di Kalman, si può fare in modo apparentemente facile, avendo già disponibili numerosi listati che per noi lo implementano. Questo è il listato che ho copiato dal forum di Arduino: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418/15

// KasBot V1  -  Kalman filter module

float Q_angle  =  0.01; //0.001
float Q_gyro   =  0.0003;  //0.003
float R_angle  =  0.01;  //0.03

float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float  y, S;
float K_0, K_1;

// newAngle = angle measured with atan2 using the accelerometer
// newRate =  angle measured using the gyro
// looptime = loop time in millis()

float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
P_01 +=  - dt * P_11;
P_10 +=  - dt * P_11;
P_11 +=  + Q_gyro * dt;

y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle +=  K_0 * y;
x_bias  +=  K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

return x_angle;
}

 

Qui per ottenere il risultato, bisogna impostare 3 parametri: Q_angle, R_angle, R_gyro. Il discorso si complica  un po’.

Ma cosa succede applicando questi algoritmi? Si ottengono curve simili? Ecco un confronto:

Ci sono 5 curve:

rossa – accelerometro

verde – gyro

blu –  filtro di kalman

nera – filtro complementare

gialla – filtro complementare secondo ordine

Come si vede i segnali filtrati si comportano tutti in modo molto simile. Si noti come in presenza di vibrazioni, l’accelerometro (rosso) impazzisca e come generalmente sia molto 'nervoso' e frastagliato. Si noti anche come il giroscopio (verde) abbia un drift molto forte che nel tempo si incrementa.

Vediamo adesso un confronto soltanto tra segnali filtrati. Cioè presenti kalman (verde), complementare (nero) e complementare 2° ordine (giallo). Si vede come il kalman è un po’ in ritardo rispetto ai filtri complementari, ma come risponda meglio alle vibrazioni. In questo caso il filtro del secondo ordine non restituisce proprio una curva ideale, probabilmente c’è da lavorare un po’ sui coefficienti.

In conclusione penso che il filtro complementare, nella fattispecie quello del primo ordine, si possa tranquillamente utilizzare al posto del filtro di Kalman. La ‘lisciatura’ è sicuramente di buon livello e l’algoritmo è molto più semplice di quello di Kalman.

L’hardware che ho utilizzato è composto da:

-        Arduino 2009

-        IMU a 6 assi Sparkfun Razor 6 DOF

Ho usato 2 assi per l’accelerometro e 1 solo asse per il giroscopio. L’intero listato che ho utilizzato è scaricabile qui.



 

Gioblu Robotics © 2010 - 2012 · Sitemap · privacy

gioscarab@gmail.com · Via Savona 123 20146 Milano MI · PI 06977550968 · Codice fiscale MTLGNN86S09F205F

Gioblu BOTServer è online dal 10 Aprile 2010 - 319.232 Visite - 1.027.175 Pagine visualizzate - 182.309 Visitatori unici - 536 utenti attivi