Kantelhoek aflezen met een MPU6050-accelerometer en gyroscoop

Inleiding

De MPU6050 is het goedkoopste eerlijke antwoord op de vraag “welke kant is boven”. Hij bevat twee onafhankelijke sensoren op één plakje—een driekassige accelerometer en een driekassige gyroscoop—en een zelfbalancerende robot heeft allebei nodig, want elk is op zichzelf nutteloos op een manier die de ander precies compenseert.

Deze tutorial brengt je van een kale module naar een kantelhoek in graden die je kunt vertrouwen terwijl de robot stil staat. Die hoek laten overleven terwijl de robot beweegt is het complementaire filter, de volgende tutorial, en dat bouwt voort op alles hier.

Wat elke sensor werkelijk meet

Een accelerometer meet geen kanteling. Hij meet versnelling, inclusief de versnelling door de zwaartekracht, en daarom werkt hij sowieso als kantelsensor: in rust is de enige versnelling 1 g naar beneden, en de manier waarop die vector over de assen verdeeld is vertelt je je oriëntatie.

Die “in rust” is de adder. De sensor kan zwaartekracht niet onderscheiden van enige andere versnelling. Zodra de wielen van je robot bewegen, meldt de accelerometer de vectorsom van zwaartekracht en de beweging van de robot zelf, en leest hij een kanteling die er niet is. Op een balancerende robot—die per definitie nooit stopt met versnellen—is die fout doorlopend.

Een gyroscoop meet de hoeksnelheid, in graden per seconde. Integreer de hoeksnelheid over de tijd en je hebt een hoek. Gyroscopen zijn ongevoelig voor lineaire versnelling, dus blijven ze eerlijk terwijl de robot beweegt.

Hun zwakke plek is dat integratie fouten opstapelt. Elke kleine constante afwijking in de hoeksnelheidsmeting integreert tot een hoek die divergent groeit. Een gyroscoop die 0,5 °/s aangeeft terwijl hij volstrekt stilstaat, beweert na een minuut dat je 30° gedraaid hebt. Dat heet drift, en het is niet te repareren met beter solderen.

Dus: de accelerometer heeft op de lange termijn gelijk maar op elk moment ongelijk; de gyroscoop heeft op elk moment gelijk maar op de lange termijn ongelijk. Die symmetrie is de hele reden dat sensorfusie bestaat.

Bedrading

De MPU6050 spreekt I²C, dus hij heeft vier draden nodig—en als de scanner hem niet op 0x68 vindt, legt die pagina uit waarom zijn datasheet 0xD0 zegt.

Monteer de module op een balancerende robot zo dat een van zijn assen echt horizontaal ligt en zo dicht mogelijk bij de wielas. Hogere montage vermenigvuldigt de lineaire versnelling die hij tijdens een correctie ziet, wat het fusieprobleem moeilijker maakt zonder enig voordeel.

Ruwe waarden uitlezen

De registers die je nodig hebt staan opvolgend, dus één burst-leesactie haalt alles op:

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

De module start in slaapstand. Vergeet de slaapbit in PWR_MGMT_1 te wissen, dan krijg je een stroom van nullen, die er precies uitziet als een bedradingsfout en mensen zonder reden terugstuurt naar hun soldeerbout.

Omrekenen van tellingen naar eenheden

Ruwe waarden zijn ondertekende 16-bit-tellingen in het meetbereik waarvoor de sensor is ingesteld. Bij de standaardinstellingen:

• Accelerometer: ±2 g over ±32768 tellingen → 16384 tellingen per g
• Gyroscoop: ±250 °/s over ±32768 tellingen → 131 tellingen per °/s

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

Kanteling uit de accelerometer

Met de robot rechtop en het verticaal gemonteerde bord is de kantelhoek in het vlak waarin de robot valt afkomstig uit twee van de drie assen:

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

Gebruik atan2, niet asin op één as. atan2 gebruikt beide componenten, blijft dus kloppen in alle vier de kwadranten en loopt niet uit de rails als één as naar nul gaat. Een versie met asin op één as ziet er op de werkbank prima uit bij kleine hoeken en doet daarna vreemd zodra de robot verder dan 45° overhelt.

De offsets kalibreren

Elke MPU6050 heeft een fabricageoffset. Je gyroscoop geeft geen nul aan terwijl hij stilstaat, en je accelerometer geeft niet exact 1 g op de ene as en nul op de andere. Meet de fout één keer bij het opstarten, met de robot strikt stil:

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

Trek gyroBiasY van elke volgende snelheidsmeting af. Die ene stap is het verschil tussen een gyroscoop die een graad per minuut drijft en een die een graad per seconde drijft.

Twee dingen om te weten. De sensor moet tijdens de kalibratie echt stilstaan—een robot die kalibreert terwijl jij hem vasthoudt, brandt je handtrilling in de afwijking. En de afwijking verandert met de temperatuur, dus een robot die koud gekalibreerd is, drijft licht naarmate de elektronica opwarmt. Voor een balancerende robot vangt de integraalterm in de regellus die rest op, en dat is een van de redenen dat balancerende regelaars vrijwel altijd een I-term nodig hebben.

Montagehoek is niet hetzelfde als sensorafwijking

Er is een tweede offset die kalibratie niet kan verwijderen: je IMU is niet perfect gemonteerd. Zit het bord 1,5° uit de ware verticaal, dan meldt een perfect gekalibreerde sensor nog steeds 1,5° als de robot werkelijk rechtop staat.

Op een balancerende robot telt dit enorm, en op de werkbank is het onzichtbaar. De regellus houdt getrouw de hoek aan die je als rechtop opgeeft—dus een fout van 1,5° betekent dat de robot permanent 1,5° overhelt, wat constante versnelling betekent, wat betekent dat hij door de ruimte rijdt tot hij ergens tegenaan loopt.

Meet hem door de robot met de hand rechtop te zetten, echt in balans, en te kijken wat de hoek aangeeft. Trek dat getal als constante af. Je kunt precies deze storing zien in de balance-simulator—de trim-schuifbalk voegt een montagefout toe en de robot drijft weg terwijl hij volhoudt dat hij waterpas staat.

Controleer voordat je erop verder bouwt

Print de hoek en controleer met de hand drie dingen:

1. Rechtop geeft bijna nul. Zo niet, dan heb je montagehoek om af te trekken.
2. De robot voorover kantelen verhoogt de hoek, en achterover kantelen verlaagt hem. Is het teken omgekeerd, wissel het hier om in plaats van het in de regellus te compenseren, waar het je later in de war raakt.
3. De meting is stabiel terwijl je stilstaat en direct als je beweegt. Ruisachtig terwijl je stilstaat betekent: controleer bedrading en voeding. Traag betekent: je middelt ergens te veel.

Zodra de hoek in rust te vertrouwen is, ben je klaar om hem ook in beweging te vertrouwen met het complementaire filter, en daarna de lus te sluiten met PID-balanceregeling in cascadestructuur.