Arduino und MPU6050 Beschleunigungsmesser und Gyroskop-Tutorial

Codebeschreibung: Also müssen wir zuerst die Wire.h-Bibliothek einbinden, die für die I2C-Kommunikation verwendet wird, und einige Variablen definieren, die zum Speichern der Daten benötigt werden.

Im Setup-Abschnitt müssen wir die Kabelbibliothek initialisieren und den Sensor über das Power-Management-Register zurücksetzen. Dazu müssen wir einen Blick auf das Datenblatt des Sensors werfen, wo wir die Registeradresse sehen können.

Wenn wir möchten, können wir auch den Full-Scale-Bereich für den Beschleunigungsmesser und das Gyroskop mithilfe ihrer Konfigurationsregister auswählen. Für dieses Beispiel verwenden wir den Standardbereich von +-2 g für den Beschleunigungsmesser und den Bereich von 250 Grad/s für das Gyroskop, daher lasse ich diesen Teil des Codes kommentiert.

// Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true);
  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  */ Codesprache:Arduino (arduino) 

Im Schleifenabschnitt beginnen wir mit dem Lesen der Beschleunigungsmesserdaten. Die Daten für jede Achse werden in zwei Bytes oder Registern gespeichert und wir können die Adressen dieser Register aus dem Datenblatt des Sensors entnehmen.

Um sie alle zu lesen, beginnen wir mit dem ersten Register und fordern mit der Funktion requiestFrom() an, alle 6 Register für die X-, Y- und Z-Achse zu lesen. Dann lesen wir die Daten aus jedem Register, und da die Ausgänge Zweierkomplemente sind, kombinieren wir sie entsprechend, um die richtigen Werte zu erhalten.

// === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value Codesprache:Arduino (arduino) 

Um Ausgabewerte von -1g bis +1g zu erhalten, die für die Berechnung der Winkel geeignet sind, teilen wir die Ausgabe mit der zuvor ausgewählten Empfindlichkeit.

Schließlich berechnen wir mit diesen beiden Formeln die Roll- und Nickwinkel aus den Beschleunigungsmesserdaten.

// Calculating Roll and Pitch from the accelerometer data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58) Codesprache:Arduino (arduino) 

Als nächstes erhalten wir mit der gleichen Methode die Gyroskopdaten.

Wir lesen die sechs Kreiselregister aus, kombinieren ihre Daten passend und dividieren sie durch die zuvor gewählte Empfindlichkeit, um die Ausgabe in Grad pro Sekunde zu erhalten.

// === Read gyroscope data === //
  previousTime = currentTime;        // Previous time is stored before the actual time read
  currentTime = millis();            // Current time actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0; Codesprache:Arduino (arduino) 

Hier können Sie feststellen, dass ich die Ausgabewerte mit einigen kleinen berechneten Fehlerwerten korrigiere, die ich gleich erklären werde, wie wir sie erhalten. Da die Ausgaben in Grad pro Sekunde sind, müssen wir sie jetzt mit der Zeit multiplizieren, um nur Grad zu erhalten. Der Zeitwert wird vor jeder Leseiteration mit der Funktion millis() erfasst.

// Correct the outputs with the calculated error values
  GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
  GyroY = GyroY - 2; // GyroErrorY ~(2)
  GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw =  yaw + GyroZ * elapsedTime; Codesprache:Arduino (arduino) 

Schließlich fusionieren wir die Daten des Beschleunigungssensors und des Gyroskops unter Verwendung eines komplementären Filters. Hier nehmen wir 96 % der Gyroskopdaten, weil sie sehr genau sind und nicht unter äußeren Kräften leiden. Die Kehrseite des Gyroskops ist, dass es driftet oder im Laufe der Zeit Fehler in die Ausgabe einführt. Daher verwenden wir langfristig die Daten des Beschleunigungsmessers, in diesem Fall 4 %, genug, um den Driftfehler des Gyroskops zu eliminieren.

// Complementary filter - combine acceleromter and gyro angle values
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY; Codesprache:Arduino (arduino) 

Da wir das Gieren jedoch nicht aus den Beschleunigungsmesserdaten berechnen können, können wir den Komplementärfilter nicht dafür implementieren.

Bevor wir uns die Ergebnisse ansehen, lassen Sie mich kurz erklären, wie Sie die Fehlerkorrekturwerte erhalten. Um diese Fehler zu berechnen, können wir die benutzerdefinierte Funktion compute_IMU_error() aufrufen, während sich der Sensor in einer flachen Ruheposition befindet. Hier machen wir 200 Messwerte für alle Ausgänge, wir summieren sie und dividieren sie durch 200. Da wir den Sensor in einer flachen Ruheposition halten, sollten die erwarteten Ausgangswerte 0 sein. Mit dieser Berechnung können wir also den durchschnittlichen Fehler des Sensors erhalten macht.

void calculate_IMU_error() {
  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
  // Read accelerometer values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    c++;
  }
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  // Read gyro values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;
  }
  //Divide the sum by 200 to get the error value
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  // Print the error values on the Serial Monitor
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
} Codesprache:Arduino (arduino) 

Wir drucken einfach die Werte auf dem seriellen Monitor aus und sobald wir sie kennen, können wir sie wie zuvor gezeigt im Code implementieren, sowohl für die Roll- und Nickberechnung als auch für die 3 Gyroskopausgänge.

Schließlich können wir mit der Serial.print-Funktion die Roll-, Pitch- und Yaw-Werte auf dem seriellen Monitor drucken und sehen, ob der Sensor richtig funktioniert.

MPU6050 Orientierungsverfolgung – 3D-Visualisierung

Um das 3D-Visualisierungsbeispiel zu erstellen, müssen wir als Nächstes nur diese Daten akzeptieren, die das Arduino über die serielle Schnittstelle in der Processing-Entwicklungsumgebung sendet. Hier ist der vollständige Verarbeitungscode:

/*
    Arduino and MPU6050 IMU - 3D Visualization Example 
     by Dejan, https://howtomechatronics.com
*/
import processing.serial.*;
import java.awt.event.KeyEvent;
import java.io.IOException;
Serial myPort;
String data="";
float roll, pitch,yaw;
void setup() {
  size (2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200); // starts the serial communication
  myPort.bufferUntil('\n');
}
void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + "     Pitch: " + int(pitch), -100, 265);
  // Rotate the object
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  
  // 3D 0bject
  textSize(30);  
  fill(0, 76, 153);
  box (386, 40, 200); // Draw box
  textSize(25);
  fill(255, 255, 255);
  text("www.HowToMechatronics.com", -183, 10, 101);
  //delay(10);
  //println("ypr:\t" + angleX + "\t" + angleY); // Print the values to check whether we are getting proper values
}
// Read data from the Serial Port
void serialEvent (Serial myPort) { 
  // reads the data from the Serial Port up to the character '.' and puts it into the String variable "data".
  data = myPort.readStringUntil('\n');
  // if you got any bytes other than the linefeed:
  if (data != null) {
    data = trim(data);
    // split the string at "/"
    String items[] = split(data, '/');
    if (items.length > 1) {
      //--- Roll,Pitch in degrees
      roll = float(items[0]);
      pitch = float(items[1]);
      yaw = float(items[2]);
    }
  }
} Codesprache:Arduino (arduino) 

Hier lesen wir die eingehenden Daten vom Arduino und setzen sie in die entsprechenden Roll-, Pitch- und Yaw-Variablen. In der Hauptzeichnungsschleife verwenden wir diese Werte, um das 3D-Objekt zu drehen, in diesem Fall ist das ein einfaches Kästchen mit einer bestimmten Farbe und Text darauf.

Wenn wir die Skizze ausführen, können wir sehen, wie gut der MPU6050-Sensor für die Verfolgung der Orientierung ist. Das 3D-Objekt verfolgt die Ausrichtung des Sensors ziemlich genau und reagiert auch sehr schnell.

Wie ich bereits erwähnt habe, besteht der einzige Nachteil darin, dass das Gieren mit der Zeit abdriften wird, da wir den komplementären Filter dafür nicht verwenden können. Um dies zu verbessern, müssen wir einen zusätzlichen Sensor verwenden. Das ist normalerweise ein Magnetometer, das als Langzeitkorrektur für die Gierdrift des Kreisels verwendet werden kann. Die MPU6050 verfügt jedoch tatsächlich über eine Funktion namens Digital Motion Processor, die für Onboard-Berechnungen der Daten verwendet wird und in der Lage ist, die Gierdrift zu eliminieren.

Hier ist dasselbe 3D-Beispiel mit dem verwendeten Digital Motion Processor. Wir können sehen, wie genau die Orientierungsverfolgung jetzt ist, ohne die Gierdrift. Der Onboard-Prozessor kann auch Quaternionen berechnen und ausgeben, die zur Darstellung von Orientierungen und Drehungen von Objekten in drei Dimensionen verwendet werden. In diesem Beispiel verwenden wir tatsächlich Quaternionen zur Darstellung der Orientierung, die auch nicht unter der Gimbal-Sperre leidet, die bei der Verwendung von Euler-Winkeln auftritt.

Trotzdem ist es etwas komplizierter, diese Daten vom Sensor zu erhalten, als wir zuvor erklärt haben. Zuerst müssen wir einen zusätzlichen Draht mit einem digitalen Pin von Arduino verbinden. Das ist ein Interrupt-Pin, der zum Lesen dieses Datentyps von der MPU6050 verwendet wird.

Der Code ist auch etwas komplizierter, deshalb verwenden wir die i2cdevlib-Bibliothek von Jeff Rowberg. Diese Bibliothek kann von GitHub heruntergeladen werden und ich werde einen Link dazu in den Website-Artikel einfügen.

Sobald wir die Bibliothek installiert haben, können wir das Beispiel MPU6050_DMP6 aus der Bibliothek öffnen. Dieses Beispiel ist mit Kommentaren für jede Zeile gut erklärt.

Hier können wir auswählen, welche Art von Ausgabe wir wollen, Quaternionen, Euler-Winkel, Gieren, Nicken und Rollen, Beschleunigungswerte oder Quaternionen für die 3D-Visualisierung. Diese Bibliothek enthält auch eine Verarbeitungsskizze für das 3D-Visualisierungsbeispiel. Ich habe es nur geändert, um die Kastenform wie im vorherigen Beispiel zu erhalten. Hier ist der Verarbeitungscode für die 3D-Visualisierung, der mit dem Beispiel MPU6050_DPM6 für die ausgewählte „OUTPUT_TEAPOT“-Ausgabe funktioniert:

/*
    Arduino and MPU6050 IMU - 3D Visualization Example 
     by Dejan, https://howtomechatronics.com
*/
import processing.serial.*;
import java.awt.event.KeyEvent;
import java.io.IOException;
Serial myPort;
String data="";
float roll, pitch,yaw;
void setup() {
  size (2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200); // starts the serial communication
  myPort.bufferUntil('\n');
}
void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + "     Pitch: " + int(pitch), -100, 265);
  // Rotate the object
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  
  // 3D 0bject
  textSize(30);  
  fill(0, 76, 153);
  box (386, 40, 200); // Draw box
  textSize(25);
  fill(255, 255, 255);
  text("www.HowToMechatronics.com", -183, 10, 101);
  //delay(10);
  //println("ypr:\t" + angleX + "\t" + angleY); // Print the values to check whether we are getting proper values
}
// Read data from the Serial Port
void serialEvent (Serial myPort) { 
  // reads the data from the Serial Port up to the character '.' and puts it into the String variable "data".
  data = myPort.readStringUntil('\n');
  // if you got any bytes other than the linefeed:
  if (data != null) {
    data = trim(data);
    // split the string at "/"
    String items[] = split(data, '/');
    if (items.length > 1) {
      //--- Roll,Pitch in degrees
      roll = float(items[0]);
      pitch = float(items[1]);
      yaw = float(items[2]);
    }
  }
} Codesprache:Arduino (arduino) 

Hier erhalten wir also mit der Funktion serialEvent() die Quaternionen, die vom Arduino kommen, und in der Hauptzeichenschleife verwenden wir sie, um das 3D-Objekt zu drehen. Wenn wir die Skizze ausführen, können wir sehen, wie gut Quaternionen zum Drehen von Objekten in drei Dimensionen geeignet sind.

Um dieses Tutorial nicht zu überladen, habe ich das zweite Beispiel, den DIY-Arduino-Gimbal oder die selbststabilisierende Plattform, in einem separaten Artikel platziert.

Fühlen Sie sich frei, Fragen zu diesem Tutorial in den Kommentaren unten zu stellen, und vergessen Sie auch nicht, meine Sammlung von Arduino-Projekten zu überprüfen.