acc/gyro winkel bestimmen

[Ray M. (ray_m)]

Hi,

Ich hab jetzt 3 Tage das Netz durchsucht und leider
keine passende Antwort gefunden, deshalb hier mal eine Frage
zum Thema.

Vorweg, ich benutze aktuell ein MPU6050.

Ich berechne die Winkel mit dem angehängten Test-Code.
Soweit so gut.

Wenn ich den Sensor um  eine Achse kippe, hat das kurzzeitig
auch Auswirkungen auf die andere Achse.

Bsp.

In Ruhelage habe ich
  -0.0 : -0.1

Wenn ich den Sensor bewege kurzzeitig z.B.
  -3.1 : 15.4

Wenn ich den Sensor dann in dieser Lage halte pendelt sich
das ganze korrekt auf die erwarteten Werte ein
  -0.1 : 15.3

Ich würde gern den die Änderung (im Bsp. Pitch) unterdrücken
wenn ich den Sensor auf der anderen Achse (im Bsp. Roll) bewege.

Ist das möglich und wenn ja wie ?

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 accelgyro1(0x68);

int16_t ax1, ay1, az1;
int16_t gx1, gy1, gz1;
float aPitch, aRoll;
float fx1, fy1, fz1;
float fPitch, fRoll;
float dt = 0.001;

const    long LOOP_DEF    = 100; // in ms
unsigned long LOOP_DEF_pM = 0;

void setup() {
  Wire.begin();
  delay(100);
  Serial.begin(115200);
  delay(100);
  accelgyro1.initialize();
  delay(100);
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  unsigned long currentMillis = millis();

  accelgyro1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);

  aPitch  = -(atan2(ax1, sqrt(ay1*ay1 + az1*az1))*180.0)/M_PI;
  aRoll   = (atan2(ay1, az1)*180.0)/M_PI;

  fx1     = 0.98*(fx1+gx1*dt)+0.02*(ax1);
  fy1     = 0.98*(fy1+gy1*dt)+0.02*(ay1);
  fz1     = 0.98*(fz1+gz1*dt)+0.02*(az1);
  fPitch  = -(atan2(fx1, sqrt(fy1*fy1 + fz1*fz1))*180.0)/M_PI;
  fRoll   = (atan2(fy1, fz1)*180.0)/M_PI;

  if ( currentMillis - LOOP_DEF_pM >= LOOP_DEF ) {
    LOOP_DEF_pM = currentMillis;

    Serial.print(aPitch,1); Serial.print(" : "); Serial.print(aRoll,1);
    Serial.println();
    Serial.print(fPitch,1); Serial.print(" : "); Serial.print(fRoll,1);
    Serial.println();
    Serial.println("---------------\n");
    
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }
}

[DraconiX (Gast)]

Auf was für einem µC betreibst du denn die Berechnung o.O

Bist du dir denn ganz sicher das du die Achse (Pitch) nicht scheinbar 
doch um 3° bewegst während du die andere Achse auf 15° drehst? Ich 
meine, so ruhig kann keine menschliche Hand drehen. Oder ist deine 
"Apparatur auf ein" genulltes Drehgestell montiert?

Ansonsten, nen Deadtimer für die jeweils anderen Achsen mit einfügen 
wenn du aktuell nur die eine Achse sehen willst.

Liest du das über das Gyro aus oder hast du am MPU6050 ein 3 Achsen 
Kompass angeschlossen?!

[RP6conrad (Gast)]

Dieses Algoritme mache eine Mischung zwischen ACC und Gyro, wo das gyro 
anteil 98% betragt und ACC 2%. Daneben siehe ich auch die sample zeit dt 
= 0.001, oder 1 ms. Aber ich siehe in die loop nicht diese feste 
samplezeit zuruck. Wen das nicht stimmt, stimmt der Integration von gyro 
auch nicht !  Damit haben sie dan immer Fehler nach eine Bewegung, und 
bei Stillstand pendelt sich das wieder richting durch das 2% ACC Anteil. 
chnVersuch erst mal auf feste Zeiten den ACC und Gyro zu samplen, und 
auch ihre Berechnung durch zu fuhren. 1ms scheint mir ziemlich wenig, 
besser soll 10 ms sein.

[Ray M. (ray_m)]

DraconiX schrieb:

[...]

> Liest du das über das Gyro aus oder hast du am MPU6050 ein 3 Achsen
> Kompass angeschlossen?!

Kein Kompass, nur acc/gyro vom MPU6050

[Ray M. (ray_m)]

RP6conrad schrieb:
> Dieses Algoritme mache eine Mischung zwischen ACC und Gyro, wo das gyro
> anteil 98% betragt und ACC 2%. Daneben siehe ich auch die sample zeit dt
> = 0.001, oder 1 ms. Aber ich siehe in die loop nicht diese feste
> samplezeit zuruck. Wen das nicht stimmt, stimmt der Integration von gyro
> auch nicht !  Damit haben sie dan immer Fehler nach eine Bewegung, und
> bei Stillstand pendelt sich das wieder richting durch das 2% ACC Anteil.
> chnVersuch erst mal auf feste Zeiten den ACC und Gyro zu samplen, und
> auch ihre Berechnung durch zu fuhren. 1ms scheint mir ziemlich wenig,
> besser soll 10 ms sein.

ist es das was du meinst ?
es scheint mit dem folgendem code auch etwas besser zu werden

include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 accelgyro1(0x68);

int16_t ax1, ay1, az1;
int16_t gx1, gy1, gz1;
float aPitch, aRoll;
float fx1, fy1, fz1;
float fPitch, fRoll;
float dt = 0.001;

const    long LOOP_DEF    = 100; // in ms
unsigned long LOOP_DEF_pM = 0;
unsigned long LAST_LOOP_TIME = 0;

void setup() {
  Wire.begin();
  delay(100);
  Serial.begin(115200);
  delay(100);
  accelgyro1.initialize();
  delay(100);
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  unsigned long currentMillis = millis();

  accelgyro1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);

  dt = (currentMillis - LAST_LOOP_TIME) / 1000;
  LAST_LOOP_TIME = currentMillis;

  aPitch  = -(atan2(ax1, sqrt(ay1*ay1 + az1*az1))*180.0)/M_PI;
  aRoll   = (atan2(ay1, az1)*180.0)/M_PI;

  fx1     = 0.98*(fx1+gx1*dt)+0.02*(ax1);
  fy1     = 0.98*(fy1+gy1*dt)+0.02*(ay1);
  fz1     = 0.98*(fz1+gz1*dt)+0.02*(az1);
  fPitch  = -(atan2(fx1, sqrt(fy1*fy1 + fz1*fz1))*180.0)/M_PI;
  fRoll   = (atan2(fy1, fz1)*180.0)/M_PI;

  if ( currentMillis - LOOP_DEF_pM >= LOOP_DEF ) {
    LOOP_DEF_pM = currentMillis;

    Serial.print(aPitch,1); Serial.print(" : "); Serial.print(aRoll,1);
    Serial.println();
    Serial.print(fPitch,1); Serial.print(" : "); Serial.print(fRoll,1);
    Serial.println();
    Serial.println("---------------\n");
    
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }
}

[Ray M. (ray_m)]

das war mist, dt ist in meinem code immer 0 ;)

aber wenn ich dt auf 0 stelle scheint der effect zu verschwinden,
ich versteh nur noch bahnhof ...

[Ray M. (ray_m)]

so funktioniert es auch nicht

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 accelgyro1(0x68);

int16_t ax1, ay1, az1;
int16_t gx1, gy1, gz1;
float aPitch, aRoll;
float fx1, fy1, fz1;
float fPitch, fRoll;

uint32_t timer;

const    long LOOP_DEF    = 100; // in ms
unsigned long LOOP_DEF_pM = 0;

void setup() {
  Wire.begin();
  delay(100);
  Serial.begin(115200);
  delay(100);
  accelgyro1.initialize();
  delay(100);
  pinMode(LED_BUILTIN, OUTPUT);
  timer = micros();
}

void loop() {
  unsigned long currentMillis = millis();

  accelgyro1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);

  double dt = (double)(micros() - timer) / 1000000;
  timer = micros();

  aPitch  = -(atan2(ax1, sqrt(ay1*ay1 + az1*az1))*180.0)/M_PI;
  aRoll   = (atan2(ay1, az1)*180.0)/M_PI;

  fx1     = 0.98*(fx1+gx1*dt)+0.02*(ax1);
  fy1     = 0.98*(fy1+gy1*dt)+0.02*(ay1);
  fz1     = 0.98*(fz1+gz1*dt)+0.02*(az1);
  fPitch  = -(atan2(fx1, sqrt(fy1*fy1 + fz1*fz1))*180.0)/M_PI;
  fRoll   = (atan2(fy1, fz1)*180.0)/M_PI;

  if ( currentMillis - LOOP_DEF_pM >= LOOP_DEF ) {
    LOOP_DEF_pM = currentMillis;

    Serial.print(aPitch,1); Serial.print(" : "); Serial.print(aRoll,1);
    Serial.println();
    Serial.print(dt,10); Serial.print(" : ");
    Serial.print(fPitch,1); Serial.print(" : "); Serial.print(fRoll,1);
    Serial.println();
    Serial.println("---------------\n");
    
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }
}

nur wenn ich dt = 0 setze scheint es in etwa hin zu kommen,
dass versteh ich leider garnicht

[RP6conrad (Gast)]

Wan dt 0 ist, wird nur noch den ACC ausgewertet, damit scheint es besser 
zu functionieren. Aber ACC hat das Nachteil das alle Beschleunigungnen 
mit verrechtnet werden, und damit nur bei Stillstand eine gute 
Auswertung gibt. Versuch mal die gyro-integration separat auszuwerten :
10ms loop {fx1=gx1*dt+fx1;Serial.print(fx1);}
Damit sollen sie einen Winkel bekommen die langzam wegdriftet, aber bei 
Drehungen soll er auch richtig reagieren.
Sehr wichtig bei einen gyro ist die drift-kompensierung ! Sie vermessen 
die gyro ausgabe bei Stillstand uber einige Secunden, das ist dan die 
offset. Dieses offset wird dan immer verrechtnet, so das die Drift 
minimiert wird.

[Ray M. (ray_m)]

RP6conrad schrieb:
> Wan dt 0 ist, wird nur noch den ACC ausgewertet, damit scheint es besser
> zu functionieren. Aber ACC hat das Nachteil das alle Beschleunigungnen
> mit verrechtnet werden, und damit nur bei Stillstand eine gute
> Auswertung gibt. Versuch mal die gyro-integration separat auszuwerten :
> 10ms loop {fx1=gx1*dt+fx1;Serial.print(fx1);}
> Damit sollen sie einen Winkel bekommen die langzam wegdriftet, aber bei
> Drehungen soll er auch richtig reagieren.
> Sehr wichtig bei einen gyro ist die drift-kompensierung ! Sie vermessen
> die gyro ausgabe bei Stillstand uber einige Secunden, das ist dan die
> offset. Dieses offset wird dan immer verrechtnet, so das die Drift
> minimiert wird.

ich verstehe das so leider nicht, hast du ein bsp.

[RP6conrad (Gast)]

ACC vermesst direct die erdbeschleunigung (g). Das wird berechtnet aus 
die X und Z komponente mit hilfe von Atan2(X,Z). Das gibt dan ein Winkel 
gegenueber die Senkrechte.
Ein gyro gibt eine Drehrate aus (°/s).Bei Stillstand sollen alle gyro 
achse 0°/s ausgeben. Wen sie einen Winkel hieraus berechnen wollen, 
mussen sie diese Drehrate integrieren (= aufaddieren). Beispiel : gyro 
gibt ein Drehrate von 10°/s, dan ist die Winkelaenderung nach 5 s 50°. 
Auf diese art ist eine continuerliche Messung von Winkel moglich. 
Einfach immer diese Drehrate integrieren. Davon kommt diese Berechnung : 
fx1=gx1*dt+fx1. fx1 ist die forherige Winkel, gx1 ist die Drehrate, dt 
ist die Zeit zwischen eine Durchlauf. Nachteil von gyro : Er hat keine 
"nullposition", das bedeutet das bei Anfang steht die Winkel immer auf 
0. Daneben gibt eine "offset", bei Stillstand gibt er noch immer eine 
kleine Drehgeschwindigkeit an. Damit sehen sie diese Winkel langsam 
"wegdriften".
Um die richtige "sensorfusion" zu machen, muss du erst sicher stellen 
das ACC und Gyro berechnungen stimmen ! Darum separat auswerten.

[Ray M. (ray_m)]

ok, ich werde nich mal versuchen ... danke