Neigungswinkel aus MPU6050: Beschleunigung und Gyro

Einleitung

Der MPU6050 ist die billigste ehrliche Antwort auf „wo ist oben“. Er enthält zwei unabhängige Sensoren auf einem Chip — ein dreiachsiges Beschleunigungsmesser und ein dreiachsiges Gyroskop — und ein selbstbalancierender Roboter braucht beide, weil jeder allein auf eine Art nutzlos ist, die der andere exakt kompensiert.

Diese Anleitung bringt dich vom nackten Modul zu einem Neigungswinkel in Grad, dem du trauen kannst, solange der Roboter stillsteht. Diesen Winkel auch in Bewegung überleben zu lassen ist der komplementäre Filter — die nächste Anleitung, die auf allem hier aufbaut.

Was jeder Sensor wirklich misst

Ein Beschleunigungssensor misst keine Neigung. Er misst Beschleunigung, inklusive Erdbeschleunigung — weshalb er überhaupt als Neigungssensor funktioniert: Im Stillstand ist die einzige Beschleunigung 1 g nach unten, und wie sich dieser Vektor auf die Achsen verteilt, verrät dir deine Orientierung.

Dieses „im Stillstand“ ist der Haken. Der Sensor kann Schwerkraft von keiner anderen Beschleunigung unterscheiden. Sobald die Räder deines Roboters laufen, meldet das Beschleunigungsmesser die Vektorsumme aus Schwerkraft und Eigenbewegung und liest eine Neigung, die nicht da ist. An einem balancierenden Roboter — der per Definition nie aufhört zu beschleunigen — ist dieser Fehler dauerhaft.

Ein Gyroskop misst Winkelrate in Grad pro Sekunde. Integriere die Rate über Zeit, und du bekommst einen Winkel. Gyros sind immun gegen lineare Beschleunigung, bleiben also ehrlich, während der Roboter fährt.

Ihre Schwäche: Integration häuft Fehler an. Jede kleine konstante Verzerrung in der Ratenlesung integriert sich zu einem Winkel, der grenzenlos wächst. Ein Gyro, das im perfekten Stillstand 0,5 °/s liest, behauptet nach einer Minute, du seist 30° rotiert. Das ist Drift, und sie ist nicht durch besseres Löten zu fixen.

Also: Das Beschleunigungsmesser hat langfristig recht, aber in keinem Moment; das Gyro hat in jedem Moment recht, aber langfristig nicht. Diese Symmetrie ist der ganze Grund, warum es Sensorfusion gibt.

Verdrahtung

Der MPU6050 spricht I²C, braucht also vier Drähte — und wenn der Scanner ihn nicht auf 0x68 findet, erklärt jene Seite, warum sein Datenblatt 0xD0 sagt.

An einem balancierenden Roboter montiere das Modul so, dass eine seiner Achsen ehrlich horizontal und so nah wie möglich an der Radachse sitzt. Hoch montiert multipliziert es die lineare Beschleunigung, die es bei einer Korrektur sieht — was das Fusionsproblem ohne Nutzen schwerer macht.

Rohwerte lesen

Die nötigen Register liegen hintereinander, also holt ein einziger Burst-Lesevorgang alles:

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

Das Modul startet schlafend. Das Sleep-Bit in PWR_MGMT_1 zu löschen vergessen liefert einen stetigen Strom von Nullen, was exakt wie ein Verdrahtungsfehler liest und Leute grundlos zum Lötkolben zurückschickt.

Zählwerte in Einheiten umrechnen

Rohwerte sind vorzeichenbehaftete 16-Bit-Zählwerte über den jeweils konfigurierten Bereich. Bei Standardeinstellungen:

• Beschleunigung: ±2 g über ±32768 Zählwerte → 16384 Zählwerte pro g
• Gyro: ±250 °/s über ±32768 Zählwerte → 131 Zählwerte pro °/s

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

Neigung aus dem Beschleunigungsmesser

Mit aufrecht stehendem Roboter und vertikal montierter Platine kommt der Neigungswinkel in der Fallebene des Roboters aus zwei der drei Achsen:

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

Nutze atan2, nicht asin einer einzelnen Achse. atan2 nimmt beide Komponenten, bleibt also durch alle vier Quadranten korrekt und explodiert nicht, wenn eine Achse gegen null geht. Eine einachsige asin-Variante sieht auf dem Tisch bei kleinen Winkeln brav aus und benimmt sich dann seltsam, sobald sich der Roboter erstmals über 45° neigt.

Die Offsets kalibrieren

Jeder MPU6050 hat einen Fertigungsoffset. Dein Gyro liest im Stillstand nicht null, und dein Beschleunigungsmesser liest nicht exakt 1 g auf einer Achse und null auf den anderen. Miss den Fehler einmal beim Start, mit stillgehaltenem Roboter:

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

Ziehe gyroBiasY von jeder folgenden Ratenlesung ab. Dieser eine Schritt ist der Unterschied zwischen einem Gyro, das pro Minute ein Grad driftet, und einem, das pro Sekunde eines driftet.

Zwei Dinge dazu. Der Sensor muss während der Kalibrierung wirklich still sein — ein Roboter, der kalibriert, während du ihn hältst, bäckt dein Handzittern in den Bias ein. Und der Bias ändert sich mit der Temperatur, also driftet ein kalt kalibrierter Roboter leicht, wenn die Elektronik warm wird. Für einen balancierenden Roboter schluckt der Integralanteil in der Regelschleife diesen Rest — einer der Gründe, warum balancierende Regler fast immer einen I-Anteil brauchen.

Montagetrimm ist nicht Sensorbias

Es gibt einen zweiten Offset, den Kalibrierung nicht entfernt: Deine IMU sitzt nicht perfekt. Sitzt die Platine 1,5° neben echt vertikal, meldet ein perfekt kalibrierter Sensor trotzdem 1,5°, wenn der Roboter ehrlich aufrecht steht.

An einem balancierenden Roboter zählt das enorm, und auf dem Tisch ist es unsichtbar. Der Regelkreis hält treu den Winkel, den du ihm als aufrecht nennst — also heißt 1,5° Fehler, der Roboter hält eine dauerhafte 1,5°-Schräglage, was konstante Beschleunigung heißt, was heißt, er fährt durchs Zimmer, bis er etwas trifft.

Miss ihn, indem du den Roboter von Hand aufrecht balancierst, ehrlich ausbalanciert, und aufzeichnest, was der Winkel liest. Ziehe diese Zahl als Konstante ab. Genau dieses Versagen kannst du im Balance-Simulator beobachten — der Trimm-Schieber injiziert einen Montagefehler, und der Roboter driftet davon, während er darauf besteht, er sei waagerecht.

Prüfen, bevor du darauf aufbaust

Drucke den Winkel und prüfe drei Dinge von Hand:

1. Aufrecht liest nahe null. Wenn nicht, hast du Montagetrimm abzuziehen.
2. Den Roboter vorwärts kippen erhöht den Winkel, rückwärts verringert ihn. Ist das Vorzeichen invertiert, tausche es hier statt in der Regelschleife zu kompensieren, wo es dich später verwirrt.
3. Die Lesung ist stabil im Stillstand und sofortig bei Bewegung. Verrauscht im Stillstand heißt Verdrahtung und Versorgung prüfen; träge heißt, du mittelst irgendwo zu aggressiv.

Sobald der Winkel in Ruhe vertrauenswürdig ist, bist du bereit, ihn in Bewegung vertrauenswürdig zu machen mit dem komplementären Filter — und dann die Schleife zu schließen in kaskadierter PID-Balanceregelung.