Beschleunigungen MPU6050 Korrektur Einbaulage

Hallo Zusammen,

heute möcht ich der Gemeinschaft mal was zurückgeben. Ich habe mich mit dem MPU6050 Beschleunigungssensor beschäftigt und einen Sketch adaptiert. Das Problem ist, dass man den Sensor nie exakt in Nulllage verbauen kann. Man bekommt dann Beschleunigungskomponenten, die es gar nicht gibt und falsche Winkel angezeigt. Deshalb habe ich das Programm so erweitert, dass die Offsetwinkel nach der Montage ausgelesen werden können und dann das Koordinatensystem mithilfe von Drehmatrizen transformiert wird. Damit kann theoretisch jede beliebige Lage ausgeglichen werden.

Mit dem Code (ist auch Teil des Programms) könnt ihr die Offsetwinkel auslesen:

   // Beschleunigungen, Winkelgeschwindigkeit und Winkel mit MPU-6050
// Auslesen der Kalibrierwinkel zum Ausgleich der Einbaulage des Sensors

// 16.12.2018

#include<Wire.h>
const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
float phi, psi, teta;
//phi: Neigung der x-Achse um die y-Achse
//psi: Neigung der y-Achse um die x-Achse
//teta: Neigung der z-Achse zur ursprünglichen z-Achse

void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  accX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  accY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  //gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  //gyroY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gyroZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  accX = accX * -1;
  accY = accY * -1;
  accZ = accZ * -1;

  //Kalibrierung
  phi = acos(sqrt(pow(accY, 2)) / sqrt(pow(accX, 2) + pow(accY, 2)));
  psi = acos(sqrt(pow(accX, 2)) / sqrt(pow(accX, 2) + pow(accY, 2)));
  teta = acos(sqrt(pow(accZ, 2)) / sqrt(pow(accX, 2) + pow(accY, 2) + pow(accZ, 2)));

  if (accX >= 0 && accY >= 0)
  {
    phi = phi;
    psi = psi;
    teta = -teta; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
  }
  else if (accX < 0 && accY >= 0)
  {
    phi = -phi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    psi = -psi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    teta = teta;
  }
  else if (accX >= 0 && accY < 0)
  {
    phi = -phi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    psi = -psi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    teta = -teta; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
  }
  else if (accX < 0 && accY < 0)
  {
    phi = phi;
    psi = psi;
    teta = teta;
  }

  float gyroZoffset = gyroZ;

  Serial.println(accX); Serial.println(accY); Serial.println(accZ);
  Serial.print("phi: "); Serial.print(phi); Serial.print(" rad, "); Serial.print(phi * RAD_TO_DEG); Serial.println(" grad");
  Serial.print("psi: "); Serial.print(psi); Serial.print(" rad, "); Serial.print(psi * RAD_TO_DEG); Serial.println(" grad");
  Serial.print("teta: "); Serial.print(teta); Serial.print(" rad, "); Serial.print(teta * RAD_TO_DEG); Serial.println(" grad");
  Serial.print("(phi + psi): "); Serial.print(phi + psi); Serial.print(" rad, "); Serial.print((phi + psi)*RAD_TO_DEG); Serial.println(" grad");
  //phi + psi muss 90 Grad sein (Kontrollsumme)

  delay(1000);
}

Dem liegt die Forderung zugrunde, dass es in Ruhelage nur die Erdbeschleunigung in Richtung z-negativ geben darf. Danach dienen diese Winkel als Input des Programms. Jedoch ist damit die x- und y- Richtung noch nicht eindeutig definiert. Das heißt ihr müsst eine Beschleunigung möglichst nur z.B. in x-Richtung aufbringen und schauen, ob sich auch tatsächlich der x-Wert erhöht. Sollte sich aber dabei der y-Wert erhöhen, sind die Achsen invertiert. Dann muss die Drehreihenfolge geändert werden (entprechenden Part im Programm auskommentieren und bei anderem Kommentar aufheben).

...

...Fortsetzung:

// Beschleunigungen, Winkelgeschwindigkeit und Winkel mit MPU-6050
// inkl. Korrektur der Einbaulage des Sensors
// Sensor auslesen mit Wire.read()

// 16.12.2018


#include<Wire.h>
const int MPU_addr = 0x68; // I2C address of the MPU-6050

float aXg = 0; // Beschleunigung in x-Richtung in g (Fahrtrichtung positiv)
float aYg = 0; // Beschleunigung in y-Richtung in g (Fahrerseitig positiv)
float aZg = 0; // Beschleunigung in z-Richtung in g (oben positiv)
float aXY = 0; // Betrag der Beschleunigung auf XY-Ebene in g
float Xangle = 0; // Neigung der x-Achse in Grad zur Horizontalen um die y-Achse (bergab = negativ)
float Yangle = 0; // Neigung der y-Achse in Grad zur Horizontalen um die x-Achse (fahrerseitig links runter = negativ)
float angle = 0; // Gesamtneigung in Grad

//Konstanten aus Kalibrier-function in Radiant bzw. Radian/Sekunde
float phi = 1.16;
float psi = 0.41;
float teta = -0.24;
//float gyroZoffset = 0;

void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

/*void kalibrieren()
  {
    int16_t accX, accY, accZ;
  //int16_t gyroX,gyroY,gyroZ;

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  accX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  accY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  //gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  //gyroY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  //gyroZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  accX = accX * -1;
  accY = accY * -1;
  accZ = accZ * -1;

  //Kalibrierung
  phi = acos(sqrt(pow(accY, 2)) / sqrt(pow(accX, 2) + pow(accY, 2)));
  psi = acos(sqrt(pow(accX, 2)) / sqrt(pow(accX, 2) + pow(accY, 2)));
  teta = acos(sqrt(pow(accZ, 2)) / sqrt(pow(accX, 2) + pow(accY, 2) + pow(accZ, 2)));

  if (accX >= 0 && accY >= 0)
  {
    phi = phi;
    psi = psi;
    teta = -teta; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
  }
  else if (accX < 0 && accY >= 0)
  {
    phi = -phi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    psi = -psi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    teta = teta;
  }
  else if (accX >= 0 && accY < 0)
  {
    phi = -phi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    psi = -psi; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
    teta = -teta; //Umkehren der Drehrichtung in Drehmatrix durch negativen Winkel
  }
  else if (accX < 0 && accY < 0)
  {
    phi = phi;
    psi = psi;
    teta = teta;
  }

  //float gyroZoffset = gyroZ;

  Serial.println(accX); Serial.println(accY); Serial.println(accZ);
  Serial.print("phi: "); Serial.print(phi); Serial.print(" rad, "); Serial.print(phi * RAD_TO_DEG); Serial.println(" grad");
  Serial.print("psi: "); Serial.print(psi); Serial.print(" rad, "); Serial.print(psi * RAD_TO_DEG); Serial.println(" grad");
  Serial.print("teta: "); Serial.print(teta); Serial.print(" rad, "); Serial.print(teta * RAD_TO_DEG); Serial.println(" grad");
  Serial.print("(phi + psi): "); Serial.print(phi + psi); Serial.print(" rad, "); Serial.print((phi + psi)*RAD_TO_DEG); Serial.println(" grad");
  //phi + psi muss 90 Grad sein (Kontrollsumme)

  delay(1000);

  }*/

void loop()
{
  int16_t accX, accY, accZ;
  //int16_t gyroX,gyroY,gyroZ;

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  accX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  accY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  //gyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  //gyroY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  //gyroZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  //  // Richtungskorrektur und Umrechnung in g der Rohwerte (zum debuggen)
  //  float accXg = accX * -1 / 16384.0; // Beschleunigung in x-Richtung in g
  //  float accYg = accY * -1 / 16384.0; // Beschleunigung in y-Richtung in g
  //  float accZg = accZ * -1 / 16384.0; // Beschleunigung in y-Richtung in g
  //  Serial.print("accX: "); Serial.print(accXg);
  //  Serial.print(" accY: "); Serial.print(accYg);
  //  Serial.print(" accZ: "); Serial.println(accZg);



  //zuerst Drehung der x-Achse
  float aX = cos(teta) * cos(psi) * accX + cos(teta) * sin(psi) * accY - sin(teta) * accZ;
  float aY = -sin(psi) * accX + cos(psi) * accY;
  float aZ = sin(teta) * cos(psi) * accX + sin(teta) * sin(psi) * accY + cos(teta) * accZ;

  //zuerst Drehung der y-Achse // Um Achsen (x,y) zu invertieren, zuerst Drehung der y-Achse ausführen (ACHTUNG: noch nicht vollständig getestet!)
  //float aX = cos(phi)*accX-sin(phi)*accY;
  //float aY = cos(teta)*sin(phi)*accX+cos(teta)*cos(phi)*accY-sin(teta)*accZ;
  //float aZ = sin(teta)*sin(phi)*accX+sin(teta)*cos(phi)*accY+cos(teta)*accZ;

  // Richtungskorrektur und Umrechnung in g
  aXg = aX * -1 / 16384.0; // Beschleunigung in x-Richtung in g
  aYg = aY * -1 / 16384.0; // Beschleunigung in y-Richtung in g
  aZg = aZ * -1 / 16384.0; // Beschleunigung in z-Richtung in g

  aXY = sqrt(pow(aXg, 2) + pow(aYg, 2)); // Betrag der Beschleunigung auf XY-Ebene in g

  //  // Beträge der Gesamtbeschleunigung (Rohwert und tranformiert müssen identisch sein, da invariant!!!)
  //  float acc = sqrt(pow(accXg, 2) + pow(accYg, 2) + pow(accZg, 2)); // Betrag der Beschleunigung in g (Rohwerte)
  //  float a = sqrt(pow(aXg, 2) + pow(aYg, 2) + pow(aZg, 2)); // Betrag der Beschleunigung in g (transformiert)
  //  Serial.print("acc: "); Serial.print(acc);
  //  Serial.print(" a: "); Serial.println(a);

  Xangle = (atan2(aX, aZ)) * RAD_TO_DEG; // Neigung der x-Achse in Grad zur Horizontalen um die y-Achse (bergab = negativ)
  Yangle = (atan2(aY, aZ)) * RAD_TO_DEG; // Neigung der y-Achse in Grad zur Horizontalen um die x-Achse (fahrerseitig links runter = negativ)

  //Andere Methode zur Winkelberechnung (jedoch ohne Vorzeichen)
  //float aXangle = acos(aZ / sqrt(pow(aX, 2) + pow(aZ, 2))) * RAD_TO_DEG;
  //float aYangle = acos(aZ / sqrt(pow(aY, 2) + pow(aZ, 2))) * RAD_TO_DEG; // oder mit square(aY) statt pow(aY, 2)

  angle = acos(aZ / sqrt(pow(aX, 2) + pow(aY, 2) + pow(aZ, 2))) * RAD_TO_DEG; // Gesamtneigung in Grad

  //float gyroZgs = (gyroZ-gyroZoffset)/131.0; // Winkelgeschwindigkeit um z-Achse in Grad/Sekunde


  Serial.print("aX: "); Serial.print(aXg);
  Serial.print(" aY: "); Serial.print(aYg);
  Serial.print(" aZ: "); Serial.println(aZg);

  Serial.print("aXY: "); Serial.println(aXY);

  Serial.print("Xangle: "); Serial.print(Xangle);
  Serial.print(" Yangle: "); Serial.println(Yangle);

  Serial.print("angle: "); Serial.println(angle);

  //Serial.print("gyroZ: ");Serial.println(gyroZgs);

  delay(1000);
}

Müsste so funktionieren. Da ich kein Profi bin, kann man das sicher eleganter programmieren. Verbesserungsvorschläge sind natürlich jederzeit willkommen.

LG