Author Topic: MPU - 6050 "Sensorfusion" - Frage: Kann mir jemand weiter helfen?  (Read 23902 times)

House

  • freakyfriday
  • Jr. Member
  • *
  • Posts: 95
  • Karma: +1/-0
    • Hausammann-Dekor
Ich versuche im Moment eine "Sensordatenfusion" des Accelerometers und des Gyros des MPU-6050. Ich hab mich zum Thema einwenig im Netz erkundigt(Gimbal, Sensordrift usw.). Das Auslesen der Daten mit der Library zur MPU-6050 funktioniert tiptop. Ich kann die verschiedenen Sensorachsen Mappen und jeweils das Servo-Sample auf eine Achse anwenden(Servo bewegt sich lageabhängig). Jedoch kann ich mit diesen Werten nicht wahnsinnig viel anfangen. Nun zu meiner Frage: Kann mir jemand den Komplementärfilter erklären resp. wie ich Ihn anwenden kann, bzw. wie ich ihn in meinem Sketch anwenden kann, der Kalmanfilter erscheint mir zu rechenintensiv und noch viel komplexer. Im Prinzip suche ich ne Möglichkeit 3 Motoren davon am Heckmotor einen Servo, um dem Drehmoment mittels Neigung entgegen zu wirken. Die vorderen beiden Motoren laufen gegeneinander, d.h. bei gleicher Geschwindigkeit heben sich beide Drehmomente auf. Vielleich hat ja schon jemand Versuche in diese Richtung unternommen(Autobalancing,Tricopter, o.ä.). Freue mich über alle Tips und Anregungen!

Vielen Dank im Voraus

P. Hausammann

House

  • freakyfriday
  • Jr. Member
  • *
  • Posts: 95
  • Karma: +1/-0
    • Hausammann-Dekor
Re: MPU - 6050 "Sensorfusion" mit Komplementärfilter
« Reply #1 on: July 07, 2014, 08:49:38 PM »
Wie ich allen die am letzten Freaky Friday dabei waren demonstrieren konnte, habe ich mein Problem mit dem Complementärfilter gelöst, bzw. ich habe es hinbekommen, dass drei Servos die vom Gyro und Accelerometer gemessene Abweichung kompensieren können.

Die Formel für den Komplementärfilter lautet : Winkel=0.98*(Winkel+GyroDaten*Abtastfrequenz)+0.02*(AccelerometerDaten)

Dazu muss über die X-Achse vom Gyro und die X-Achsen vom Accelerometer ein Komplementärfilter (siehe oben) gelegt werden. Dies wiederholt man jeweils für die Y-Achsen und auch für Z-Achsen.

Hoffe, ich konnte betreffend Autobalancing weiterhelfen.

MfG
Patric Hausammann
« Last Edit: September 25, 2014, 08:57:43 AM by House »

dinoi

  • freakyfriday
  • Sr. Member
  • *
  • Posts: 441
  • Karma: +7/-0
    • Dinoi
Re: MPU - 6050 "Sensorfusion" - Frage: Kann mir jemand weiter helfen?
« Reply #2 on: July 10, 2014, 04:59:04 PM »
Hallo Patric, ja vielen Dank, ich habe immer noch einen Balancing Robot der noch nicht wirklich balanciert. Verstehe ich die Formel richtig:

<Neuer Winkel>=0.98*(<Alter Winkel>+<Aktuell gemessener Winkel vom Gyro> * Abtastfrequenz)+0.02*(<Aktuelle Beschleunigung vom  Accelerometer>)

Hast Du mir Beispieldaten damit ich dass mit meinem Coding prüfen kann?

Danke und Gruss
Reto


House

  • freakyfriday
  • Jr. Member
  • *
  • Posts: 95
  • Karma: +1/-0
    • Hausammann-Dekor
Re: MPU - 6050 "Sensorfusion" - Ansatz
« Reply #3 on: July 10, 2014, 07:49:06 PM »
Hoi Reto,

Ich habe bei der Formel "nur" eine Variable für den Winkel eingesetzt, wie in meinem Formelbeispiel. Ich füge mal unten den Code an dem ich gearbeitet habe an. Hoffe es hilft Dir weiter! Achja, ich denke der Begriff/Variabel Abtastfrequenz hab ich mal da mir dieser Ausdruck nicht gepasst hat durch Abtastrate ersetzt. Im Skript ist dt die Variabel für diese Abtastrate und ich verwende den Wert 0,02, was bedeutet, das die Werte circa 50 mal pro Sekunde (50 Hz) abgerufen werden. Ausserdem nimmt mein Skript erst vor der Ausgabe den Vergleich zwischen alten und neuen Winkelwerten vor. Denke mir aber man kann es auch wie in Deiner Formel/Gleichung lösen.
Hoffe ich hab nichts vergessen...

MfG
Patric Hausammann

Code:

//MPU-6050 mit Komplementärfilter und drei Servos
//Erstellt mit hilfe der Servo-, Wire-, I2Cdev-, und der MPU-6050-Library durch Patric Hausammann release vom 10.07.2014. Veröffentlicht unter GNU-License.


#include <Servo.h> // Alle Librarys aufrufen

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#define dt 0.01  //Abtastrate für Komplementärfilter
 
MPU6050 mpu;
 
int16_t ax, ay, az;   // In der Form erhalten wir die Accelerometer-Werte
int16_t gx, gy, gz;   //In der Form erhalten wir die Gyro-Daten
 
 
int correctureX;  //Meine Variable, welche für den Winkel beim Komplementärfilter steht(X-Achse).
int correctureY;  //Meine Variable, welche für den Winkel beim Komplementärfilter steht(Y-Achse).
int correctureZ;  //Meine Variable, welche für den Winkel beim Komplementärfilter steht(Z-Achse).
 
Servo myservo;  //Servo-Objekt erzeugen
Servo myservo1;
Servo myservo2;
 
int val;  //Korrigierter resp. gemapter Wert des Winkels
int prevVal;  //Letzter bzw. vorheriger korrigierter resp. gemapter Wert des Winkels

int val1;
int prevVal1;
 
int val2;
int prevVal2;
 
void setup()
{
    Wire.begin();
    Serial.begin(115200);
 
    Serial.println("Initialize MPU");
    mpu.initialize();
    Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
    myservo.attach(A1, 1000, 2000);  // Hier wird das Servo-Objekt auf A1 gelegt, die Signale,
    myservo1.attach(A2, 1000, 2000);  //die wir erzeugen sind Servosteuersignale, deren Werte von 1000 bis 2000 reichen,
    myservo2.attach(A3, 1000, 2000);  //Mittelstellung des Servos liegt bei 1500

}
 
void loop()
{
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  // Hier lesen wir die Werte der MPU-6050 ein.

 
 correctureX = 0.98*(correctureX+gx*dt)+0.02*(ax);  //Dies ist die Formel die ich zur Korrektur einsetze (Komplementärfilter: Winkel=0.98*(Winkel+GyroDaten*Abtastrate)+0.02*(AccelerometerDaten), wobei ich diesen Vorgang auf allen drei Achsen wiederhole.
 correctureY = 0.98*(correctureY+gy*dt)+0.02*(ay);
 correctureZ = 0.98*(correctureZ+gz*dt)+0.02*(az);
 
 
    val = map(correctureX, -32768, 32767, 0, 180);  //Hier  passe ich die erhaltenen Werte (360 Grad) auf die möglichen Werte der Servoausgabe (180 Grad)an(auch jede Achse einzeln).
    val1 = map(correctureY, -32768, 32767, 0, 180);
    val2 = map(correctureZ, -32768, 32767, 0, 180);
   



    if (val != prevVal)  //Vergleich des Aktuellen mit dem vorherigen Wert
   
    {
        myservo.write(val); //Gibt den korrigierten Wert an den Servo aus
        prevVal = val;}

  if(val1 !=prevVal1)
  {
        myservo1.write(val1);
        prevVal1 = val1;}
       
        if(val2 !=prevVal2){
        myservo2.write(val2);
        prevVal2 = val2;
       
Serial.println("val");  //Beschriftung der serielle Ausgabe unseres korrigierten Wertes
Serial.println(val);    //Serielle Ausgabe unseres korrigierten Wertes
Serial.println("val1");
Serial.println(val1);
Serial.println("val2");
Serial.println(val2);

    }
 

}
« Last Edit: July 11, 2014, 05:12:59 PM by House »

Ad87

  • Newbie
  • *
  • Posts: 1
  • Karma: +0/-0
Re: MPU - 6050 "Sensorfusion" - Frage: Kann mir jemand weiter helfen?
« Reply #4 on: February 25, 2016, 10:06:12 PM »
Ich versuche mich zur Zeit auch an einem selbstbalancierenden Roboter und verwende dazu den MPU 6050 Gyroskop und Accelormeter. Ich kann die Daten des Gyros und des Beschleunigungssensor einlesen,umrechnen und erhalte gute,realistische Werte für °/s und G-Kraft,und meine Motoren reagieren an sich auch recht gut auf die Lageänderung und auf auch die Geschwindigkeit. Allerdings ist das ganze nicht wirklich immer stabil und zeigt oft mit gleichen Parametern total unterschiedliches Verhalten.
Habe jetzt mehrmals versucht den Komplementärfilter anzuwenden aber er liefert mir einfach keine brauchbaren Werte.
 
Du sagst,dass dt die Variable für die Abtastrate ist. Du hast aber auch geschrieben dass du dafür den Wert 0,02 eingesetzt hast,weil deine Daten mit 50Hz reinkommen. Also ist dt jetzt auch 0,02?! Stehe da irgendwie auf dem Schlauch.

Desweiteren habe ich das Problem dass ich nicht genau weiß wie ich die Abtastrate ermittle..
Ich verwende die I2C BUS Schnittstellen der NI myRIO zum Einlesen der Daten. Mein gesamtes Programm läuft in einem Time Loop mit 1 kHz und wird sequenziell abgearbeitet. Die Schleifendurchlaufzeit beträgt 1ms. Kann ich dann davon ausgehen, dass ich auch jede ms einen neuen Wert erhalte? Oder läuft auf dem I2C BUS in der Zeit noch mehr ab? Habe leider zu dem Thema wenig brauchbare Informationen gefunden.

Danke im Vorraus für Hilfe!

pylon

  • freakyfriday
  • Full Member
  • *
  • Posts: 158
  • Karma: +16/-0
Re: MPU - 6050 "Sensorfusion" - Frage: Kann mir jemand weiter helfen?
« Reply #5 on: February 26, 2016, 03:21:04 PM »
Vielleicht solltest Du Deinen Code posten, damit wir uns ein Bild machen können. Wie bestimmst Du die Schleifendurchlaufzeit oder anders: woher weisst Du, dass der Time Loop mit 1kHz läuft? Hast Du das gemessen? Schreibst Du wie House auf die serielle Schnittstelle? Ich vermute Probleme im Timing, das je nach Ablauf unterschiedlich ausfällt und deshalb nicht-deterministisches Verhalten zeigt, aber ohne Code ist das pure Spekulation.

House

  • freakyfriday
  • Jr. Member
  • *
  • Posts: 95
  • Karma: +1/-0
    • Hausammann-Dekor
Re: MPU - 6050 "Sensorfusion" - Antwort zu Ad87's Fragen
« Reply #6 on: February 21, 2017, 12:44:14 PM »
Lieber Ad87,
Leider hatte ich sehr wenig Zeit, weshalb mir Deine Frage leider nicht auffiehl, wofür ich mich in aller Form entschuldigen muss. Ich habe mich, in letzter Zeit viel mit Kryptografie beschäftigt, was meine ganze Aufmerksamkeit in Beschlag nahm.

Betreffend Deiner Frage zur Abtastrate: Es ist schon ewig her, dass ich mir dieses Sample(Modul) geschrieben habe, weshalb ich mich auch erst wieder schlau machen musste. Ich habe dieses Skript extra so erstellt, dass man sich bezüglich der Abtastrate eigentlich keine Gedanken machen muss (also für die, die nicht gerne rechnen ;D). Ich denke, um es so einfach wie möglich zu erklären, sollten wir im Hinterkopf haben, dass dt einen Bezug zur Masseinheit Sekunden hat und mit 0,01 angegeben wird. Also 1 Sekunde durch dt (0,01),  entspricht 100 (1/0.01 = 100), also 100 mal pro Sekunde, was wiederum 100 Hz entspricht. Wenn wir nun die selbe Aktion mit der zweiten Abtastrate durchführen, so erhalten wir mit 1/0.02, die Zahl 50. Damit ist ebenfalls die  Taktfrequenz gemeint, also mal pro Sekuden, resp. 50 Hz. Du kannst also, wenn Du dies verstanden hast, nicht jede ms (Millisekunde) einen Schleifendurchlauf machen! Denn unsere Messdaten kommen ja einiges langsamer herein. Vielleicht hätte ich die zweite Abtastrate mit einer Variabel deklarieren sollen, was möglicherweise vom Verständnis her weitergeholfen hätte.

Ich hoffe, dass ich damit weiter helfen konnte, wenn auch spät...

Bezüglich Deiner Probleme, denke ich, dass Pylon recht hat. Es ist sehr schwer für uns zu wissen, an was es bei Deinem Projekt hapert, wenn wir den Code nicht vor uns haben. Ich vermute jedoch, dass es sich, wie Pylon ebenfalls erwähnt hat, um ein Timingproblem handelt. Hängen noch andere Sensoren am System? Möglicherweise benötigt ein Teil des Codes zu viel Zeit zur Berechnung.  Leider kann ich ohne weitere Angaben auch nur spekulieren.
« Last Edit: February 23, 2017, 10:00:22 AM by House »

pylon

  • freakyfriday
  • Full Member
  • *
  • Posts: 158
  • Karma: +16/-0
Re: MPU - 6050 "Sensorfusion" - Frage: Kann mir jemand weiter helfen?
« Reply #7 on: February 22, 2017, 01:35:32 PM »
Lieber House,

Quote
Ich denke, um es so einfach wie möglich zu erklären, sollten wir im Hinterkopf haben, dass dt einen Bezug zur Masseinheit Sekunden hat und mit 0,01 angegeben wird. Also 1 Sekunde durch 0,01, dies entspricht 100 Millisekunden (1/0.01 = 100).

Ich korrigiere nur ungern, aber hier ist ein Durcheinander passiert. Wenn ich 1 durch die Abtastrate (in diesem Fall der Zeitraum zwischen zwei Messungen, ich nehme an dt steht für "delta time") teile, dann kriege ich eine Frequenz (also 100Hz) und nicht den Abstand zweier Erfassungen (der wäre 10 Millisekunden). Bei der zweiten Abtastrate kriegen wir entsprechend 20 Millisekunden und 50 Hz. Deine Schlussfolgerung unterstütze ich hingegen voll.

House

  • freakyfriday
  • Jr. Member
  • *
  • Posts: 95
  • Karma: +1/-0
    • Hausammann-Dekor
Re: MPU - 6050 "Sensorfusion" - Frage: Kann mir jemand weiter helfen?
« Reply #8 on: February 23, 2017, 10:10:15 AM »
Lieber Pylon,
Vielen Dank, dass Du mich auf diese Verwechslung hingewiesen hast! Ich habe meinen Beitag nun korrigiert, und hoffe, dass es so passt. Ich musste mich erst wieder ein lesen, und habe scheinbar die Einheit verwechselt. Du hast natürlich Recht, es sind nicht 100 Millisekunden, sondern 100 Herz(dt), resp. 50 Herz gemeint. Betreffend der Variable "dt" hast Du ebenfalls Recht mit "delta time".