MPU6050 Berechnungsproblem

Servus,
kurz zu meinem Projekt: Ich versuche eine Art "Anzug" zu bauen, die alle Bewegungen erfasst. Damit will ich in Zukunft kleine VR Spiele entwickeln für 2 Spieler z.B., so, dass man in der Brille dann auch den Mitspieler und seine exakten Bewegungen sieht.
Dafür habe ich mir folgendes Konzept ausgedacht. An jedem Gelenkstück (also am Unterarm, am Oberarm, Unterschenkel, Oberschenkel, etc.) befindet sich je ein Sensor, der die Rotationsdaten an einen zentralen Computer schickt, der daraus dann errechnet wie z.B. gerade die Armstellung ist.

Als Sensoren habe ich mir dafür diese hier gekauft: https://www.amazon.de/gp/product/B07NP2WF7J/ref=ppx_yo_dt_b_asin_title_o02_s00?ie=UTF8&psc=1

Zur Berechnung der Winkel benutze ich einen Kalman-Filter, das Beispielprogramm habe ich aus dem Internet runtergelassen und etwas abgeändert. Ich muss zugeben, dass ich da noch nicht so richtig drin steck, aber das Prinzip dieses Filters hab ich verstanden und auch wie das Programm funktioniert.
Das Problem ist jetzt, dass die xAchse ihre Richtung ändert, sobald die yAchse > 90°, bzw. <-90° kommt, also bei waagrechter Haltung wird er dann von 180° zu 0°. Und umgekehrt die yAchse genau so.
Ich denke das liegt ziemlich sicher an der Berechnung, allerdings weiß ich nicht wo der Fehler liegt und wie ich ihn beheben kann. Ich hoffe, dass sich hier jemand auskennt und mir ein bisschen helfen kann :slight_smile:

Hier der Code:

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter


Kalman kalmanX; // Create the Kalman instances restricted to Roll
Kalman kalmanY;
Kalman kalmanZ;

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;

// restricted pitch
double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only
double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

// TODO: Make calibration routine //

void setup() {
  Serial.begin(115200);
  Wire.begin();
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
  double yaw = atan2(-accY, accX) * RAD_TO_DEG;

  kalmanX.setAngle(roll); // Set starting angle
  kalmanY.setAngle(pitch);
  kalmanZ.setAngle(yaw);
  
  gyroXangle = roll;
  gyroYangle = pitch;
  gyroZangle = yaw;

  timer = micros();
}


void loop() {
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
  gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
  gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
  gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;

  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan2(-accX, accZ) * RAD_TO_DEG;  // könnte hier der Fehler liegen?
  double yaw = atan2(-accY, accX) * RAD_TO_DEG;

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; 
  double gyroZrate = gyroZ / 131.0;

  
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
    kalmanZ.setAngle(yaw);
    kalAngleZ = yaw;
    gyroZangle = yaw;
  } else
    kalAngleZ = kalmanZ.getAngle(yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter

    
  gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
  gyroYangle += kalmanY.getRate() * dt;
  gyroZangle += kalmanZ.getRate() * dt;


  // Reset the gyro angle when it has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;
  if (gyroZangle < -180 || gyroZangle > 180)
    gyroZangle = kalAngleZ;

  double x = gyroXangle;
  double y = gyroYangle;
  double z = gyroZangle;

  Serial.println(String("") + x + ";" + y + ";" + z);//+ ";" + kalAngleY2 + ";1");
  

  Serial.print("\r\n");
  delay(50);
}

Achja: Ich benutze den Arduino Nano.

Hier mal graphisch dargestellt, wie sich die Winkel zueinander verhalten.

Das Problem an der Sache ist, dass bei den 90° kein abrupter Wechsel stattfindet, sondern schleichend. Dazu kommt natürlich, dass der Sensor leider seine Zeit braucht, bis er sich eingependelt hat und bei schnellen Bewegungen dieser Wechsel noch verzerrter wird...

Ich hoffe, dass mir irgendwer helfen kann, am besten wäre es natürlich wenn jemand direkt den Fehler im Code findet, aber ich bin auch für alternative Lösungsvorschläge sehr sehr dankbar!

Und noch was: Bei schnelleren Bewegungen reagiert dieser Sensor sehr langsam. Weiß wer, ob das ein allgemeines Problem ist, oder ob es (evtl. auch teurere) Sensoren gibt, die das besser lösen?

Danke schonmal

Lukas

MrArduino2000:
Und noch was: Bei schnelleren Bewegungen reagiert dieser Sensor sehr langsam. Weiß wer, ob das ein allgemeines Problem ist, oder ob es (evtl. auch teurere) Sensoren gibt, die das besser lösen?

Danke schonmal

Ich weiß nicht was langsam heißt, aber du setzt die Sensorwerte in deinem Code selbst auf max. +- 250 Grad/Sek. Hier könnte das Problem schon liegen, wenn du schnelle Bewegungen erkennen willst.

Wenn es das nicht ist liegt es denke ich am Kalman-Filter bzw. evtl. daran wie der eingestellt ist. Damit kenne ich mich aber nicht aus. Ich verwende als Alternative den Komplementätfilter. Die Berechnung ist einfacher und für mich leichter zu verstehen. Das ist immer von Vorteil, wenn man ein Programm nicht nur copy und pasten will sondern auch weiterentwickeln. Die Performance ist wohl etwas schlechter als beim Kalman-Filter, nach dem was ich gelesen habe.

Um das Problem mit der Richtungsumkehr zu lösen musst du dir die Winkelberechnung per linearer Beschleunigung genauer anschauen. Ich denke du benötigst dann an den jeweiligen Punkten eine Vorzeichenumkehr. Ich kenne das Verhalten, mich hat es bisher nicht gestört, da ich noch nichts messen wollte was sich um so große Winkel dreht.

Der Wechsel ist Schleichend, weil der Fehler aus den Acc Daten kommt. Die Gyro-Daten zeigen aber natürlich keine Bewegung an. Ist offenbar eine Eigenschaft des Filters, eine langsame Geschwindigkeit auszugeben, wenn der eine Sensorwert (Acc) fälschlicherweise eine sehr schnelle und der andere (Gyro) gar keine Bewegung misst.