Self Balancing Robot - parte 8 - calcolare l'angolo di inclinazione

Per determinare l'angolo di inclinazione del robot è necessario disporre di una IMU (Inertial Misurement Unit). Ci sono molti tipi di imu in commercio. Quella che serve per questo tipo di progetto è una imu composta da un accelerometro e da un giroscopio. Nel mio caso ho utilizzato una imu un po' più complessa, dotata anche di una bussola, che però è rimasta inutilizzata. Tali apparati restituiscono 3 misure, una per ogni asse X,Y, Z. La MiniIMU-9 v2 della Pololu che ho utilizzato, avendo anche la bussola, restituisce quindi in tutto 9 valori. I dati provenienti dall'accelerometro e dal giroscopio in genere sono troppo grezzi per poter essere utilizzati così come sono per determinare l'angolo che misurano. Il fatto è che ne l'accelerometro ne il giroscopio misurano un angolo, bensì entità diverse quale l'accelerazione  lungo gli assi e la velocità angolare, dati che, tramite una elaborazione possono determinare l'angolo di rotazione dell'oggetto. Nel caso del mio robot, dato che si suppone che possa ruotare solo su un'unico asse, quello delle ruote, i dati necessari per la determinazione della rotazione sono in realtà solo 3: l'accelerazione su i due assi X e Z, e la velocità angolare sull'asse Y. Ottenere da questi 3 valori un angolo necessità di due passaggi: il primo è relativamente semplice, un po' di trigonometria permette di determinare l'angolo partendo dall'accelerazione su X e Z, con la stessa semplicità si calcola l'angolo restituito dal giroscopio. Il problema però è un'altro: entrambi questi strumenti di misura soffrono per così dire di alcuni difetti congeniti:

- l'accelerometro soffre di nervosismo acuto, cioè restituisce un dato che è inficiato dalla minima vibrazione, per cui oltre a misurare l'accelerazione dovuta alla rotazione del robot, "sporca" la misura con tutte le vibrazioni dovute ai motori, alle imperfezioni della superficie su cui il robot si muove, ecc.

- il giroscopio soffre di deriva. Ovvero, a forza di girare aggiunge alla misura un errore ad ogni lettura. Inoltre, non misurando un angolo ma una velocità di rotazione, il suo riferimento iniziale non necessariamente corrisponde alla posizione di equilibrio, determinando di fatto un offset della misura che va ovviamente considerato runtime e non può essere predeterminato.

Per queste problematiche esistono delle soluzione matematiche più o meno complesse, che rientrano sotto la famiglia dei filtri complementari e del famoso filtro di Kalman. 

Chi fosse questo Kalman lo ignoro, ma il destino ha fatto sì che il suo cognome fosse determinante per spiegare cosa fa il suo filtro: di fatto "calma" il segnale.

Il video seguente, realizzato con lo sketch in allegato a questo post e uno sketch per Processing derivato da quello proposto da Kristian Sloth Lauszus e pubblicato su github spiega meglio di mille parole.

Nel video si vedono 4 distinti segnali:

• in blu il valore dell'angolo calcolato dalla velocità angolare Y del giroscopio
• in verde il valore dell'angolo calcolato combinando l'accelerazione su X e Z
• in giallo il valore dell'angolo calcolato con un filtro complementare
• in rosso il valore dell'angolo calcolato con il filtro di Kalman

E' evidente come il valore blu determinato dal giroscopio, segua fedelmente il movimento reale del robot, ma "sfalsato" di un offset praticamente fisso.

L'accelerometro (in verde) invece evidenzia un nervosismo dovuto alla lettura di tutte le vibrazioni e piccoli movimenti che il robot deve affrontare.

In rosso e in giallo sono mostrati gli esiti dell'applicazione dei due tipi di filtro: il risultato è praticamente identico e si vede chiaramente come l'uscita del filtro segue fedelmente senza ritardi lo spostamento del robot, ma non recepisce le microvariazioni lette dall'accelerometro e non viene inficiato dalla deriva e dall'offset del giroscopio.

Se facciamo un passo indietro e leggiamo il post relativo al PID, è evidente che l'angolo è il valore passato al PID, e tanto più "sporco" è questo segnale maggiore sarà l'errore riprodotto e in un certo senso amplificato dal PID, valore che sarà scaricato direttamente sui motori con tutte le conseguenze del caso.

Vediamo l'implementazione.

Per leggere i dati dall'accelerometro e dal giroscopio sono necessarie due diverse librerie, disponibili su github e linkate sul sito del produttore Pololu

#include        // accelerometro https://github.com/pololu/lsm303-arduino
#include               // giroscopio https://github.com/pololu/l3g-arduino

Per il calcolo dell'angolo partendo dai valori di accelerazione sui due assi, bisogna applicare un po' di semplice trigonometria:

double getXangle() {
  double accXval = (double)readAccX(); //-zeroAccX;
  double accZval = (double)readAccZ(); //-zeroAccZ;
  double angle = (atan2(accXval,accZval)+PI)*RAD_TO_DEG;
  return angle;
}

  accXangle = getXangle();

Per il giroscopio, il calcolo non si basa sulla trigonometria ma su quanto il valore restituito è un valore di velocità angolare, normalmente espressa in °/s . Se noi sappiamo la posizione iniziale e il tempo intercorrente tra una lettura e l'altra, è possibile stabilire l'angolo corrente aggiungendo lo spostamento avuto tra una lettura e l'altra.
La cosa complicata non è di per sè questo calcolo, bensì determinare quanto è lo spostamento tra una lettura e l'altra, perchè il valore in digit restituito dall'accelerometro va proporzionato al grado di sensibilità del giroscopio per ottenere il corretto valore di velocità angolare in °/s e quindi infine il valore dell'angolo in °.
Vediamo questi passaggi, essenziali per il corretto calcolo.
Per prima cosa andiamo sul datasheet del giroscopio e determiniamo qual'è la sua sensibilità. Poniamo attenzione al fatto che il giroscopio può lavorare con sensibilità diverse, e dipende da come viene inizializzato.

Per ottenere il rate è necessario moltiplicare il valore del giroscopio (tolto il bias iniziale) e moltiplicarlo per la sensibilità (espressa in mdps/digit), diviso 1000 per convertire in dps/digit. "dps" vuol dire proprio "°/s" nella notazione inglese (degree per second)

  gyroXrate = -(((double)readGyroX()-zeroGyroX  )*GYRO_SENS/1000);  

Questo rappresenta la porzione di angolo percorsa nell'unità di tempo di riferimento, espressa però in dps.
Per avere l'angolo è sufficiente normalizzare il valore in dps rispetto al periodo di letto

 gyroXangle += gyroXrate*((double)(deltaTIME)/1000000); //  (1M = 1000 millisec)

dove deltaTIME, da non confondersi, è espresso in microsecondi e non in millisecondi.

Questi due dati accXangle e gyroXangle devono essere "fusi" assieme per ottenere una lettura pulita dell'angolo.

Possiamo applicare il filtro complementare o il filtro di Kalman.
Non entrerò nella spiegazione matematica, ma mi limiterò a dirvi che il filtro complementare è decisamente più semplice e quindi anche dal punto di vista delle risorse utilizzate da arduino per il calcolo, è decisamente più leggero.
Il filtro complementare è implementabile con un'unica riga di codice :

 compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(deltaTIME)/1000000)))+(0.07*accXangle);

mentre per il filtro di Kalman potete utilizzare questa funzione:

float Q_angle  =  0.001; 
    float Q_gyro   =  0.003;  
    float R_angle  =  0.03; 

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

float kalmanCalculate(float newAngle, float newRate,int looptime) {
    dt = float(looptime)/1000000;                                   
    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;
  }

   kalAngleX= kalmanCalculate(accXangle, gyroXrate , deltaTIME)  ;

Per concludere un solo consiglio:
ho perso almeno un mese utilizzando una libreria consigliata sul sito della Pololu che in teoria avrebbe dovuto restituire una angolo filtrato e pulito. Il progetto in questione è dotato anche di una bellissima GUI per Windows che mostra la posizione in 3D dell'oggetto. L'implementazione, guardando il codice, non sembra essere delle più semplici perchè per determinare l'angolo vengono applicati dei filtri piuttosto complessi e poco documentati. Questa ha determinato quasi il fallimento del mio progetto.
Il passo chiave per far funzionare le cose, è stato tornare a leggere i valori raw dall'accelerometro e giroscopio, mediante le librerie base fornite da Pololu, e poi applicare il filtro di Kalman.

Scarica lo sketch per Arduino e per Processing. 

Ringrazio X-Firm e Kristian Lauszus (TKJ Electronics) per gli utilissimi esempi pubblicati e le relative spiegazioni, gran parte delle quali sono state fondamentali per assemblare gli sketch di questo post.

Nel prossimo post: assemblaggio del robot