Gierwinkel driftet nach Datenfusion (IMU ICM20948)

[clin1010]

Hallo,

für mein Projekt möchte ich mir von einem Fahrzeug die Roll-, Nick- und Gierwinkelverstellung ausgeben lassen, dafür verwende ich einen 9-DOF IMU (Adafruit TDK ICM20948) und einen ESP32 als Mikrocontroller. Zur Sensorfusion verwende ich das Beispiel „calibrated_orientation“ aus der Bibliothek „Adafruit AHRS“ und zur Kalibrierung des Magnetometers „Adafruit Sensor Calibration“ (Storing Calibrations | How to Fuse Motion Sensor Data into AHRS Orientation (Euler/Quaternions) | Adafruit Learning System). Weil der ICM20948 noch nicht in dem Beispiel eingebunden war, habe ich das noch gemacht. Es werden mir Daten ausgegeben und der Roll- und Nickwinkel sind auch in Ordnung, nur der Gierwinkel bringt mich zur Verzweiflung. Nach dem flashen braucht der Gierwinkel relativ lange bis er sich einschwingt und einen stabilen Wert erreicht. Wenn man den Sensor um die z-Achse dreht, dann schwingt der Gierwinkel zunächst stark in die entgegengesetzte Richtung und braucht auch wieder lange, bis sich der Winkel richtig eingestellt hat.

Im Beispielcode sind drei Variationen hinterlegt und bei allen ist das Ergebnis dasselbe.

• Adafruit_NXPSensorFusion filter;
• Adafruit_Madgwick filter; (Beispielbild)
• Adafruit_Mahony filter;

Im Internet gibt es zwar einiges zum Gierdrift, aber nichts hat mich weiter gebracht. Kennt vielleicht jemand das Problem und hätte einen Vorschlag?

Das angehängte Bild zeigt eine Bewegung aus der Ruheposition um 90° in Uhrzeigersinn verdreht (rot = gieren, gelb = rollen, grün = nicken).

Ich habe vollständigskeithalber den Code angehängt, obwohl ich außer der Zeile Außer der Zeile #include "ICM20948.h" nichts veränder habe.

#include <Adafruit_Sensor_Calibration.h>
#include <Adafruit_AHRS.h>

Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;

// uncomment one combo 9-DoF!
//#include "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
//#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
//#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
#include "ICM20948.h"

// pick your filter! slower == better quality output
//Adafruit_NXPSensorFusion filter; // slowest
Adafruit_Madgwick filter;  // faster than NXP
//Adafruit_Mahony filter;  // fastest/smalleset

#if defined(ADAFRUIT_SENSOR_CALIBRATION_USE_EEPROM)
  Adafruit_Sensor_Calibration_EEPROM cal;
#else
  Adafruit_Sensor_Calibration_SDFat cal;
#endif

#define FILTER_UPDATE_RATE_HZ 100
#define PRINT_EVERY_N_UPDATES 10
//#define AHRS_DEBUG_OUTPUT

uint32_t timestamp;

void setup() {
  Serial.begin(115200);
  while (!Serial) yield();

  if (!cal.begin()) {
    Serial.println("Failed to initialize calibration helper");
  } else if (! cal.loadCalibration()) {
    Serial.println("No calibration loaded/found");
  }

  if (!init_sensors()) {
    Serial.println("Failed to find sensors");
    while (1) delay(10);
  }
  
  accelerometer->printSensorDetails();
  gyroscope->printSensorDetails();
  magnetometer->printSensorDetails();

  setup_sensors();
  filter.begin(FILTER_UPDATE_RATE_HZ);
  timestamp = millis();

  Wire.setClock(400000); // 400KHz
 
}


void loop() {
  float roll, pitch, heading;
  float gx, gy, gz;
  static uint8_t counter = 0;

  if ((millis() - timestamp) < (1000 / FILTER_UPDATE_RATE_HZ)) {
    return;
  }
  timestamp = millis();
  // Read the motion sensors
  sensors_event_t accel, gyro, mag;
  accelerometer->getEvent(&accel);
  gyroscope->getEvent(&gyro);
  magnetometer->getEvent(&mag);
#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("I2C took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  cal.calibrate(mag);
  cal.calibrate(accel);
  cal.calibrate(gyro);
  // Gyroscope needs to be converted from Rad/s to Degree/s
  // the rest are not unit-important
  gx = gyro.gyro.x * SENSORS_RADS_TO_DPS;
  gy = gyro.gyro.y * SENSORS_RADS_TO_DPS;
  gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;

  // Update the SensorFusion filter
  filter.update(gx, gy, gz, 
                accel.acceleration.x, accel.acceleration.y, accel.acceleration.z, 
                mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Update took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif

  // only print the calculated output once in a while
  if (counter++ <= PRINT_EVERY_N_UPDATES) {
    return;
  }
  // reset the counter
  counter = 0;

#if defined(AHRS_DEBUG_OUTPUT)
/*  Serial.print("Raw: ");
  Serial.print(accel.acceleration.x, 4); Serial.print(", ");
  Serial.print(accel.acceleration.y, 4); Serial.print(", ");
  Serial.print(accel.acceleration.z, 4); Serial.print(", ");
  Serial.print(gx, 4); Serial.print(", ");
  Serial.print(gy, 4); Serial.print(", ");
  Serial.print(gz, 4); Serial.print(", ");
  Serial.print(mag.magnetic.x, 4); Serial.print(", ");
  Serial.print(mag.magnetic.y, 4); Serial.print(", ");
  Serial.print(mag.magnetic.z, 4); Serial.println("");*/
#endif

  // print the heading, pitch and roll
  roll = filter.getRoll();
  pitch = filter.getPitch();
  heading = filter.getYaw();
  Serial.print("Orientation: ");
  Serial.print(heading);
  Serial.print(" ");
  Serial.print(pitch);
  Serial.print(" ");
  Serial.println(roll);
/*
  float qw, qx, qy, qz;
  filter.getQuaternion(&qw, &qx, &qy, &qz);
  Serial.print("Quaternion: ");
  Serial.print(qw, 4);
  Serial.print(", ");
  Serial.print(qx, 4);
  Serial.print(", ");
  Serial.print(qy, 4);
  Serial.print(", ");
  Serial.println(qz, 4);  
  */
#if defined(AHRS_DEBUG_OUTPUT)
  Serial.print("Took "); Serial.print(millis()-timestamp); Serial.println(" ms");
#endif
}

Viele Grüße