gbm
Filtro di Kalman con Arduino
Tutorials -
Martedì 01 Giugno 2010 04:46
Scritto da gbm

Ciao a tutti. Durante il workshop di domenica 30 maggio 2010 abbiamo studiato il filtro di Kalman e le sue possibili applicazioni nel campo della robotica, in particolare in strutture Self-balancing, cioè in grado di auto bilanciarsi con solo uno o due punti d'appoggio.

Un esempio:

 

 

Questo filtro è un sistema matematico che produce un filtro ricorsivo in grado di valutare lo stato di un sistema dinamico partendo dall'acquisizione di dati soggetti a rumore [o spurie (in inglese "bias")]. Detto in italiano Fico è una diavoleria matematica che partendo da dati sporchi è in grado di produrre una stima dinamica sull'andamento (e il rapporto) dei dati stessi.

Questo strumento puo' venire decisamente utile nella programmazione di IMU's.

Una IMU (Inertial mesurement unit) cioè un'unità di misurazione inerziale, o meglio piattaforma inerziale è una strumentazione in grado di determinare l'accelerazione e la velocità angolare alla quale è sottoposta. Questa viene utilizzata massivamente in aviazione, ed è ciò che ha permesso l'evoluzione dei caccia da terza a quarta generazione (vedi fly by wire). In questi ultimi anni è una tecnologia che è alla portata di molti, grazie all'aeromodellismo e alla robotica amatoriale.

Abbiamo utilizzato il filtro presente nel codice di Nicola Lugato (vedi Arduino self balancing). Il suo progetto secondo me è un esempio di come il fenomeno DIY stia cambiando il mondo e la scienza. Grazie all'esperienza dei presenti e ai consigli di Nicola siamo riusciti ad adattare questo filtro a una IMU main 5 gradi di libertà prodotta da sparkfun. Grazie al codice sottostante è possibile ottenere un dato preciso che descrive l'angolazione della IMU rispetto al terreno, analizzando due assi dell'accelerometro e il giroscopio presenti slla IMU main board. Questo codice puo' essere adattato a qualsiasi accelerometro e giroscopio MEMS con uscita analogica.

Dopo aver caricato il codice, aprite il serial monitor e controllate il dato angolo. Se questo è spostato da 90 gradi regolate calibration oppure i due valori sottratti alla lettura dell'accelerometro (linea 131) fino a che il valore angolo non è a 90 gradi con la imu parallela al terreno. Assicuratevi della precisione del sistema ruotando la IMU di 90 gradi e controllando il valore ottenuto via seriale. Se volete visualizzare graficamente i dati ottenuti dalla piattaforma inerziale vi consiglio di leggere il tutorial Grafico con Processing.

Ricordiamoci di includere le librerie Servo.h e Math.h

/////////////////////////////////////////
#define NUMREADINGS 5  //Filtro spurie Giro [media 5 valori]
int readings[NUMREADINGS];  //Lettura del giroscopio
int index = 0;              //Index della lettura corrente
int total = 0;              //Il totale
int average = 0;            //La media
int inputPin =0;            //Gyro Analog input
///////////////////////////////////////

float dt = .1;              
  int mydt = 20;            //in ms.
  static float PP[2][2] = { //(Kalman)
    {
      1, 0   }
    ,                       //(Kalman)
    {
      0, 1   }
    ,                       //(Kalman)
};                          //(Kalman)


//I due stati, angolo ed errore sistematico del giroscopio. Come sottoprodotto
//della computazione dell'angolo, otteniamo anche la velocità angolare pulita 
//dall'errore [bias]

float angle = 0.0;         //(Kalman)
float q_bias = 0.0;        //(Kalman)
float rate = 0.0;          //(Kalman)
float q_m = 0.0;           //(Kalman)

double ax_m=0;
double ay_m=0;
int cnt = 0;               //Counter
unsigned long lastread=0;
unsigned long startmillis=0;

//R rappresenta il valore della convarianza della lettura. 
//In questo caso utiliziamo una matrice 1x1 che produce un' ipotesi
//di distorsione della lettura di 0.3 rad

float R_angle = .3;


//Q è una matrice 2x2 in cui viene processata la covarianza.
//Questa struttura ci permette di capire quanto, in rapporto alla 
//covarianza stessa, è possibile fidarci dell'accelerometro o del giroscopio
 
static const float Q_angle = 0.001; //(Kalman)
static const float Q_gyro = 0.003; //(Kalman)
int ledPin = 13;

byte displayState = 0;    
float oldAngle = 0.0;

float P = 0.0; //dichiarazione P
float I = 0.0; //dichiarazione I
float D = 0.0; //dichiarazione D

void setup() {
  pinMode(ledPin, OUTPUT);
  Serial.begin(9600);
  
  for (int i = 0; i < NUMREADINGS; i++)
    readings[i] = 0;                    
    startmillis = millis();
  }
  float sqr(float a) {
   return a*a;  
  }
  float sgn (float a) {
    if (a > 0)
        return +1;
    else
        if (a < 0)
            return -1;
    else
        return 0;
}

void loop() {
  int delta = millis()-lastread;
  if( delta >= mydt) {    //lettura ogni dt ms -> 1000/dt hz.
    lastread = millis();
    
    total -= readings[index];    // Sottrai la scorsa lettura giroscopio
    readings[index] = analogRead(2); // Lettura giroscopio
    total += readings[index];     // Aggiungi lettura al totale
    index = (index + 1);         // Aggiornamento indice

    if (index >= NUMREADINGS) // Se siamo alla fine dell'array
      index = 0;                 // Torniamo all'inizio

    average = (total / NUMREADINGS)-500; // Calcolo media input

    dt = ((float)delta) / 1000.0;

    q_m= ((float)average)*(1500.0/1024.0)*PI/180 ;  // HAC remove 1.5 mult

    // Togliamo le spurie dal giroscopio
    const float q = q_m - q_bias;  //(Kalman)

    const float Pdot[2 * 2] = {
      Q_angle - PP[0][1] - PP[1][0],  /* 0,0 */ //(Kalman)
      - PP[1][1], /* 0,1 */
      - PP[1][1], /* 1,0 */
      Q_gyro /* 1,1 */
    };

    // Salviamo in rate la stima del valore giroscopio pulito
    rate = q; //(Kalman)

    /*
// Aggioniamo la stima dell'angolazione

    /* Aggiorniamo la matrice della covarianza*/
    PP[0][0] += Pdot[0] * dt; //(Kalman)
    PP[0][1] += Pdot[1] * dt; //(Kalman)
    PP[1][0] += Pdot[2] * dt; //(Kalman)
    PP[1][1] += Pdot[3] * dt; //(Kalman)


    //INSERIAMO QUI I PIN DEI DUE ASSI DELL'ACCELEROMETRO
    ax_m = analogRead(3)- 338; // 338 = calibrazione gradi
    ay_m = analogRead(4)- 338; // idem
    const float angle_m = atan2( ay_m, ax_m ); //(Kalman)
    const float angle_err = angle_m - angle;   //(Kalman)
    const float C_0 = 1;                       //(Kalman)
    const float PCt_0 = C_0 * PP[0][0];        //(Kalman)
    const float PCt_1 = C_0 * PP[1][0];        //(Kalman)
    const float E =R_angle+ C_0 * PCt_0;       //(Kalman)
    const float K_0 = PCt_0 / E;               //(Kalman)
    const float K_1 = PCt_1 / E;               //(Kalman)
    const float t_0 = PCt_0; 
    /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */
    const float t_1 = C_0 * PP[0][1]; 
    /* + C_1 * P[1][1]  = 0 (Kalman) */
    PP[0][0] -= K_0 * t_0;                     //(Kalman)
    PP[0][1] -= K_0 * t_1;                     //(Kalman)
    PP[1][0] -= K_1 * t_0;                     //(Kalman)
    PP[1][1] -= K_1 * t_1;                     //(Kalman)
    angle += K_0 * angle_err;                  //(Kalman)
    q_bias += K_1 * angle_err;                 //(Kalman)

  //VALORE DI CALIBRAZIONE 
  float calibration = -4.5; 
  float myangle=(angle*57.2957795130823)-90.0 + calibration;
  if ((millis()-startmillis) > 6000 ) {
    digitalWrite(ledPin, HIGH);
    float rate_deg = rate * 57.2957795130823;
    }
    oldAngle = myangle; 
    Serial.print(" Angolo ");  
    Serial.print(int(angle_m * 57.295779513082) + 180, DEC);
    Serial.print(" Accelerometro Y ");
    Serial.print(ay_m, DEC); 
    Serial.print(" Accelerometro X ");
    Serial.print(ay_m, DEC); 
  }
    digitalWrite(ledPin, LOW);
}
 

Gioblu Robotics © 2010 - 2012 · Sitemap · privacy

gioscarab@gmail.com

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