Inclinazione con accelerometro e giroscopio MPU6050

Introduzione

L’MPU6050 è la risposta più economica e onesta alla domanda «qual è il verso». Contiene due sensori indipendenti su un solo die: un accelerometro a tre assi e un giroscopio a tre assi. Un robot con bilanciamento automatico ha bisogno di entrambi, perché ciascuno è inutilizzabile da solo in un modo che l’altro compensa esattamente.

Questo tutorial ti porta dal modulo grezzo a un angolo di inclinazione in gradi attendibile mentre il robot è fermo. Rendere quell’angolo affidabile quando il robot è in movimento richiede il filtro complementare, oggetto del tutorial successivo, che dipende da tutto quanto trattato qui.

Cosa misura davvero ciascun sensore

Un accelerometro non misura l’inclinazione. Misura l’accelerazione, compresa quella dovuta alla gravità, ed è proprio per questo che può funzionare come sensore di inclinazione: fermo, l’unica accelerazione è 1 g rivolto verso il basso e il modo in cui tale vettore si distribuisce tra gli assi rivela il tuo orientamento.

L’aggettivo «fermo» è la trappola. Il sensore non distingue la gravità da qualsiasi altra accelerazione. Appena le ruote del robot si muovono, l’accelerometro segnala la somma vettoriale della gravità e del moto del robot, indicando un’inclinazione inesistente. Su un robot che si bilancia, il quale per definizione non smette di accelerare, questo errore è continuo.

Un giroscopio misura la velocità angolare, in gradi al secondo. Integrando la velocità nel tempo ottieni un angolo. I giroscopi sono immuni all’accelerazione lineare, quindi restano affidabili mentre il robot si muove.

La loro debolezza è che l’integrazione accumula errori. Qualsiasi piccolo bias costante nella lettura della velocità si integra in un angolo che cresce senza limite. Un giroscopio che segna 0.5 °/s mentre il robot è perfettamente fermo indicherà di aver ruotato di 30° dopo un minuto. Questa è la deriva, e non si risolve con saldature migliori.

Quindi: l’accelerometro è corretto nel lungo periodo ma sbagliato in qualsiasi istante; il giroscopio è corretto in qualsiasi istante ma sbagliato nel lungo periodo. Questa simmetria è l’intera ragione per cui esiste la fusione dei sensori.

Cablaggio

L’MPU6050 comunica tramite I²C, quindi richiede quattro fili. Se lo scanner non lo trova a 0x68, quella pagina spiega perché la sua scheda tecnica indichi 0xD0.

Su un robot che si bilancia, monta il modulo in modo che uno dei suoi assi sia davvero orizzontale e il più vicino possibile all’asse delle ruote. Montarlo in alto moltiplica l’accelerazione lineare percepita durante una correzione, rendendo più difficile il problema di fusione senza alcun vantaggio.

Lettura dei valori grezzi

I registri necessari sono consecutivi, quindi una singola lettura in blocco ottiene tutto:

#include <Wire.h>

const uint8_t MPU = 0x68;        // 0x69 if AD0 is tied high

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);              // PWR_MGMT_1
  Wire.write(0);                 // clear sleep bit — it boots asleep
  Wire.endTransmission(true);
}

void readRaw(int16_t& ax, int16_t& ay, int16_t& az,
             int16_t& gx, int16_t& gy, int16_t& gz) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);              // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14, (uint8_t)true);

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();      // temperature, discarded
  gx = Wire.read() << 8 | Wire.read();
  gy = Wire.read() << 8 | Wire.read();
  gz = Wire.read() << 8 | Wire.read();
}

Il modulo si avvia in stato di sospensione. Dimenticare di azzerare il bit di sospensione in PWR_MGMT_1 produce un flusso costante di zeri, identico a un guasto di cablaggio e sufficiente a far tornare qualcuno al saldatore senza alcun motivo.

Conversione dei conteggi in unità

I valori grezzi sono conteggi con segno a 16 bit sull’intervallo configurato nel sensore. Con le impostazioni predefinite:

• Accelerometro: ±2 g su ±32768 conteggi → 16384 conteggi per g
• Giroscopio: ±250 °/s su ±32768 conteggi → 131 conteggi per °/s

float accelX = ax / 16384.0;     // g
float rateY  = gy / 131.0;       // degrees per second

Inclinazione dall’accelerometro

Con il robot in posizione verticale e la scheda montata in verticale, l’angolo di inclinazione nel piano in cui il robot cade deriva da due dei tre assi:

float pitch = atan2(accelX, accelZ) * 180.0 / PI;

Usa atan2, non asin su un singolo asse. atan2 considera entrambe le componenti, resta corretto in tutti e quattro i quadranti e non diverge quando un asse si avvicina a zero. Una versione con asin su un solo asse sembra corretta sul banco ad angoli piccoli, poi si comporta in modo strano alla prima inclinazione del robot oltre 45°.

Calibrazione degli offset

Ogni MPU6050 ha un offset di fabbricazione. Il giroscopio non indicherà zero quando è fermo e l’accelerometro non indicherà esattamente 1 g su un asse e zero sugli altri. Misura l’errore una sola volta all’avvio, con il robot perfettamente fermo:

float gyroBiasY = 0;

void calibrate() {
  const int samples = 1000;
  long total = 0;
  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readRaw(ax, ay, az, gx, gy, gz);
    total += gy;
    delay(2);
  }
  gyroBiasY = (total / (float)samples) / 131.0;
}

Sottrai gyroBiasY da ogni lettura successiva della velocità. Questo solo passaggio distingue un giroscopio che deriva di un grado al minuto da uno che deriva di un grado al secondo.

Ci sono due aspetti da conoscere. Il sensore deve essere davvero fermo durante la calibrazione: se tieni il robot in mano mentre calibra, il tremore della tua mano finisce nel bias. Inoltre, il bias cambia con la temperatura, quindi un robot calibrato da freddo deriverà leggermente mentre l’elettronica si scalda. Per un robot che si bilancia, il termine integrale nel loop di controllo assorbe questo residuo; è uno dei motivi per cui i controllori di bilanciamento richiedono quasi sempre un termine I.

La regolazione del montaggio non coincide con il bias del sensore

C’è un secondo offset che la calibrazione non può eliminare: l’IMU non è montato in modo perfetto. Se la scheda è 1.5° fuori dalla verticale reale, anche un sensore perfettamente calibrato indicherà 1.5° quando il robot è davvero in posizione verticale.

Su un robot che si bilancia questo conta moltissimo, ed è invisibile sul banco. Il loop di controllo manterrà fedelmente l’angolo che gli dichiari come verticale: un errore di 1.5° significa quindi che il robot mantiene un’inclinazione permanente di 1.5°, il che significa accelerazione costante e quindi movimento per tutta la stanza fino a sbattere contro qualcosa.

Misuralo mettendo in piedi il robot a mano, realmente bilanciato, e registrando il valore letto. Sottrai quel numero come costante. Puoi osservare questo preciso guasto nel simulatore di bilanciamento: il cursore di regolazione introduce un errore di montaggio e il robot si allontana continuando a sostenere di essere in piano.

Verifica prima di costruirci sopra

Stampa l’angolo e controlla a mano tre aspetti:

1. In posizione verticale il valore è vicino a zero. Se non lo è, devi sottrarre una regolazione di montaggio.
2. Inclinando il robot in avanti l’angolo aumenta, e all’indietro diminuisce. Se il segno è invertito, cambialo qui invece di compensarlo nel loop di controllo, dove ti confonderà in seguito.
3. La lettura è stabile quando il robot è fermo e immediata quando si muove. Se è rumorosa da fermo, controlla cablaggio e alimentazione; se è lenta, stai effettuando una media eccessivamente aggressiva da qualche parte.

Una volta che l’angolo è attendibile a riposo, sei pronto a renderlo attendibile in movimento con il filtro complementare e poi a chiudere il loop nel controllo PID a cascata per il bilanciamento.