Lire l'angle d'inclinaison d'un accéléromètre et gyro MPU6050

Introduction

Le MPU6050 est la réponse honnête la moins chère à « où est le haut ». Il contient deux capteurs indépendants sur une puce — un accéléromètre trois axes et un gyroscope trois axes — et un robot équilibriste a besoin des deux, parce que chacun est inutile seul d’une façon que l’autre compense exactement.

Ce tutoriel vous mène d’un module nu à un angle d’inclinaison en degrés auquel faire confiance pendant que le robot est immobile. Faire survivre cet angle au robot qui bouge est le filtre complémentaire, prochain tutoriel qui dépend de tout ici.

Ce que chaque capteur mesure vraiment

Un accéléromètre ne mesure pas l’inclinaison. Il mesure l’accélération, y compris l’accélération due à la gravité, et c’est pourquoi il marche comme capteur d’inclinaison : à l’arrêt, la seule accélération est 1 g vers le bas, et la façon dont ce vecteur se répartit sur les axes dit votre orientation.

Ce « à l’arrêt » est le hic. Le capteur ne peut distinguer la gravité d’aucune autre accélération. Dès que les roues de votre robot bougent, l’accéléromètre rapporte la somme vectorielle de la gravité et du mouvement propre du robot, et lit une inclinaison qui n’existe pas. Sur un robot équilibriste — qui par définition n’arrête jamais d’accélérer — cette erreur est continue.

Un gyroscope mesure la vitesse angulaire, en degrés par seconde. Intégrez la vitesse dans le temps et vous obtenez un angle. Les gyros sont insensibles à l’accélération linéaire, donc ils restent honnêtes pendant que le robot bouge.

Leur faiblesse est que l’intégration accumule l’erreur. Tout petit biais constant dans la lecture de vitesse s’intègre en un angle qui croît sans limite. Un gyro lisant 0,5 °/s parfaitement immobile affirmera que vous avez tourné de 30° après une minute. C’est la dérive, et elle ne se corrige pas en mieux soudant.

Donc : l’accéléromètre a raison sur la durée mais faux à chaque instant ; le gyro a raison à chaque instant mais faux sur la durée. Cette symétrie est toute la raison d’être de la fusion de capteurs.

Câblage

Le MPU6050 parle I²C, donc il lui faut quatre fils — et si le scanner ne le trouve pas en 0x68, cette page explique pourquoi sa fiche technique dit 0xD0.

Sur un robot équilibriste, montez le module pour qu’un de ses axes soit vraiment horizontal et aussi près de l’essieu des roues que possible. Le monter haut multiplie l’accélération linéaire qu’il voit pendant une correction, ce qui rend le problème de fusion plus dur sans bénéfice.

Lire les valeurs brutes

Les registres utiles sont consécutifs, donc une seule lecture en rafale prend tout :

#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();
}

Le module démarre endormi. Oublier d’effacer le bit de sommeil dans PWR_MGMT_1 renvoie un flot régulier de zéros, qui se lit exactement comme une panne de câblage et renvoie les gens à leur fer à souder pour rien.

Convertir les comptes en unités

Les valeurs brutes sont des comptes signés 16 bits sur la plage configurée du capteur. Aux réglages par défaut :

• Accéléromètre : ±2 g sur ±32768 comptes → 16384 comptes par g
• Gyroscope : ±250 °/s sur ±32768 comptes → 131 comptes par °/s

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

L’inclinaison depuis l’accéléromètre

Avec le robot droit et la carte montée verticalement, l’angle d’inclinaison dans le plan où le robot tombe vient de deux des trois axes :

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

Utilisez atan2, pas asin d’un seul axe. atan2 prend les deux composantes, donc elle reste juste dans les quatre quadrants et n’explose pas quand un axe approche zéro. Une version asin mono-axe semble bien sur l’établi aux petits angles puis se comporte bizarrement la première fois que le robot penche passé 45°.

Étalonner les décalages

Chaque MPU6050 a un décalage de fabrication. Votre gyro ne lira pas zéro à l’arrêt, et votre accéléromètre ne lira pas exactement 1 g sur un axe et zéro sur les autres. Mesurez l’erreur une fois au démarrage, robot tenu immobile :

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

Soustrayez gyroBiasY de chaque lecture de vitesse suivante. Cette seule étape est la différence entre un gyro qui dérive d’un degré par minute et un qui dérive d’un degré par seconde.

Deux choses à savoir. Le capteur doit être vraiment immobile pendant l’étalonnage — un robot étalonné tenu en main cuira le tremblement de votre main dans le biais. Et le biais change avec la température, donc un robot étalonné à froid dérivera légèrement en chauffant l’électronique. Pour un robot équilibriste, le terme intégral dans la boucle de commande absorbe ce résidu, et c’est l’une des raisons pour lesquelles les contrôleurs d’équilibre exigent presque toujours un terme I.

Le trim de montage n’est pas le biais du capteur

Il y a un second décalage que l’étalonnage ne peut retirer : votre IMU n’est pas montée parfaitement. Si la carte se tient à 1,5° de la vraie verticale, un capteur parfaitement étalonné rapportera quand même 1,5° quand le robot est vraiment droit.

Cela compte énormément sur un robot équilibriste, et c’est invisible sur l’établi. La boucle de commande tiendra fidèlement l’angle qu’on lui dit être la verticale — donc une erreur de 1,5° signifie que le robot tient une inclinaison permanente de 1,5°, ce qui signifie accélération constante, ce qui signifie qu’il traverse la pièce jusqu’à heurter quelque chose.

Mesurez-le en tenant le robot debout à la main, vraiment équilibré, et en notant ce que lit l’angle. Soustrayez ce nombre comme constante. Vous pouvez regarder cette panne exacte dans le simulateur d’équilibre — le curseur de trim injecte une erreur de montage et le robot dérive en insistant qu’il est de niveau.

Vérifiez avant de construire dessus

Affichez l’angle et vérifiez trois choses à la main :

1. Droit lit près de zéro. Sinon, vous avez un trim de montage à soustraire.
2. Pencher le robot vers l’avant augmente l’angle, et vers l’arrière le diminue. Si le signe est inversé, inversez-le ici plutôt que de compenser dans la boucle de commande, où il vous embrouillera plus tard.
3. La lecture est stable à l’arrêt, et immédiate en bougeant. Bruyante à l’arrêt signifie vérifiez câblage et alimentation ; en retard signifie que vous moyennez trop agressivement quelque part.

Une fois l’angle fiable au repos, vous êtes prêt à le rendre fiable en mouvement avec le filtre complémentaire, puis à boucler dans la commande d’équilibre PID en cascade.