Go Down

Topic: Quaternion zu EulerWinkel umrechnen (Read 6597 times) previous topic - next topic

Ketchup

May 14, 2012, 07:31 pm Last Edit: May 14, 2012, 10:35 pm by Ketchup Reason: 1
Hallo

Ich experimentiere mit einem MPU6050
Als Bibliothek und BeispielCode verwende ich Dieses.
Die Ausgabe der Fusionierten Daten sind Quaternion:

Code: [Select]
// display quaternion values in easy matrix form: [w, x, y, z]
       q[0] = (float)((fifoBuffer[0] << 8) + fifoBuffer[1]) / 16384;
       q[1] = (float)((fifoBuffer[4] << 8) + fifoBuffer[5]) / 16384;
       q[2] = (float)((fifoBuffer[8] << 8) + fifoBuffer[9]) / 16384;
       q[3] = (float)((fifoBuffer[12] << 8) + fifoBuffer[13]) / 16384;


Nun müsste ich diese in EulerWinkel umrechnen damit ich PID Regler für 3 Achsen nutzen kann.
Mit dem was ich bis jetzt darüber gefunden habe komme ich noch nicht zurecht.

Für eure Hilfe wäre ich sehr dankbar.

Gruss Kay

Udo Klein

Also in Wikipedia findest Du hier http://de.wikipedia.org/wiki/Quaternion#Drehungen_im_dreidimensionalen_Raum die Abbildung zwischen Quaternionen und Rotationsmatrizen. Und hier http://de.wikipedia.org/wiki/Eulersche_Winkel die Abbildung von Rotationsmatrizen und Eulerscher Winkeldarstellung. Was mir allerdings nicht klar ist: wieso willst Du überhaupt umrechnen. Laut der Beschreibung auf dem von Dir angegebenen Link:

Quote

Digital-output of 6 or 9-axis MotionFusion data in rotation matrix, quaternion, Euler Angle, or raw data format


D.h. das Teil sollte direkt die von Dir gewünschten Daten liefern können.
Check out my experiments http://blog.blinkenlight.net

Ketchup

#2
May 15, 2012, 05:09 pm Last Edit: May 15, 2012, 05:11 pm by Ketchup Reason: 1
Danke für die Links.
Ist ziemlich harter Stoff für mich.

Deine Idee die Euler Winkel direkt auszulesen ist natürlich die eleganteste Lösung.
Leider habe ich kein Beispiel gefunden.

Die Rohdaten liest sich so (MPU6050_raw Bibliothek):

Code: [Select]
// read raw accel/gyro measurements from device
   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

   // these methods (and a few others) are also available
   //accelgyro.getAcceleration(&ax, &ay, &az);
   //accelgyro.getRotation(&gx, &gy, &gz);


Die Quaternion so wie oben beschrieben.

Hier habe ich nichts gefunden:
http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf

Wenn du eine Idee hast wie man die Euler Winkel ausliesst, das wäre natürlich super.

Gruss Kay

Udo Klein

Also ich hätte erwartet, dass
Code: [Select]
accelgyro.getRotation(&gx, &gy, &gz); genau die von Dir gewünschten Winkel liefert. Wieso gehst Du davon aus, dass das nicht so ist?
Check out my experiments http://blog.blinkenlight.net

Ketchup

Leider Nein.

Ich hatte das getestet.

Dieses gibt nur die ACC Werte:
Code: [Select]
accelgyro.getAcceleration(&ax, &ay, &az);

Und diese die Gyro Werte:
Code: [Select]
accelgyro.getRotation(&gx, &gy, &gz);

Hast du eventuell noch eine Idee?

Gruss Kay

Udo Klein

Und wie unterscheiden sich die Gyro Werte von den gewünschten Euler Winkeln???
Check out my experiments http://blog.blinkenlight.net

Razor1990

Kann es sein, dass er die Winkel im Bogenmaß ausgibt.

Ketchup

#7
May 17, 2012, 02:10 pm Last Edit: May 17, 2012, 02:17 pm by Ketchup Reason: 1
Der ACC gibt im Beispiel oben nur "seine" Werte wieder, die Winkelgeschwindigkeit der Gyros werden nicht berücksichtigt.
Wiederum sind die Gyro Werte ohne Berücksichtigung des ACC.
Also als ob man zwei einzelne Sensoren ausliest.

Was ich versuche auszulesen sind die Fusionierte Daten, in Euler Winkel.

Ich hoffe Ihr versteht was ich meine.

Gruss Kay

Udo Klein

Es ist mir unklar was Du meinst. Was wären Deiner Meinung nach die "fusionierten" Daten?
Check out my experiments http://blog.blinkenlight.net

Ketchup



Es ist mir unklar was Du meinst. Was wären Deiner Meinung nach die "fusionierten" Daten?


Mit Fusionierten Daten meine ich das was man sonst selber im Atmega machen muss(te).


Ein Gyro liefert sehr gute Winkelgeschwindigkeiten, wenn man diese mit der Zeit Integriert auch die absoluten Winkel.
Leider Triftet so ein Gyro sehr, nach ein paar Sekunden stimmt das Integrieren nicht mehr.
Um einen Absoluten Winkel zu bekommen muss man den Trift der Gyros mit einem Beschleunigungssensor (ACC) raus rechnen.
Ein ACC gibt zwar auch Absolute aus, leider stimmen die nur wenn sich der Sensor in Ruhe befindet.
Man nutzt also von beiden Sensoren (Gyro, ACC) die Daten.

Der ACC wird Tiefbassgefiltert, der Gyro mit einem Hochpass, und mit verschiedener Gewichtung zusammen verrechnet, im Prinzip ist das ein Komplementärfilter.
Manche benutzen zur Fusion auch ein Kalmanfilter, bringt bei einem kleinen Atmega aber nicht bessere Ergebnisse.

Da nun der MPU6050 einen eigenen kleinen Rechner eingebaut hat, und die Daten von ACC und Gyro eigenständig Fusioniert (das nennt sich im Chip DMP, Digital Motion Processor) würde ich gerne an diese Daten ran.

In meinem ersten Post in diesem Thread habe ich zu einer Bibliothek und einem Beispielsketch gelinkt, da Funktioniert das auslesen der Fusionierte Daten vom Chip, leider aber nur als Quaternion.
Nun sind wir wieder am Anfang mit drei Möglichkeiten:

Ich rechne die Quaternion in Euler Winkel um. (Elegant ist was anderes)
Ich finde einen Weg die DMP Daten als Euler Winkel direkt auszulesen (Was laut Werbung auch geht)
Ich benutze einen Komplementärfilter (dafür habe ich den MPU6050 nicht gekauft)

Ist das besser beschrieben?

ich hoffe ja immer noch das es eine elegante Lösung gibt.

Gruss Kay


Udo Klein

Aha, jetzt ist überhaupt zum ersten Mal klar was Du eigentlich willst. Auf der Herstellerseite sind megabytegroße Downloads als selbstentpackende .exe Dateien. Da ich unter Linux unterwegs bin kann ich das nicht ausprobieren. Ich würde bei der Größe aber erwarten, daß dort Beispiele zu finden sein sollten. Hast Du dort schon mal nachgeschaut?
Check out my experiments http://blog.blinkenlight.net

Ketchup

#11
May 19, 2012, 05:52 pm Last Edit: May 19, 2012, 05:56 pm by Ketchup Reason: 1
Habe ich schon nachgeschaut

Ich glaube das einzige was man da brauchen könnte ist die Register Map and Descriptions:

http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf

Eventuell sieht man da wie man was auslesen könnte.
Leider ist das für mich noch zu hoch, als das ich mich da zurechtfinden kann.


Gruss Kay

Razor1990

#12
May 19, 2012, 11:09 pm Last Edit: May 19, 2012, 11:12 pm by Razor1990 Reason: 1
http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

Da weiter unten. Wenn du atan2 benutzt, was im arduino math-Header ist, erhältst du die Winkel, ist jetzt nicht all zu komplex und als erste Lösung brauchbar bis du weißt, wie du die Winkel direkt bekommst.

Wenn du bis morgen wartest und immer noch nicht klar kommst, produzier ich dir den Code. Ist ne gute Übung für mich. Hab im aktuellen Semester C-Programmierung, also die Übung kann ich gebrauchen.

Ketchup

#13
May 19, 2012, 11:49 pm Last Edit: May 20, 2012, 12:40 am by Ketchup Reason: 1
Danke für das Angebot
Ich habe kürzlich  Hier (weit unten) in der Art etwas gefunden.
Ist für mich momentan harter Stoff, ich habe die Logik dahinter noch nicht verstanden.
Wenn du ein Code machen willst da du die Übung brauchen kannst finde ich gut, da ich aber gerne verstehe was da abgeht könnte es sein das ich diesbezüglich sicher noch Fragen haben werde, wenn ich dein Code durchgehe um zu lernen.

Besten Dank

Gruss Kay

Razor1990

Code: [Select]
const float pi = 3.14159;

void quaternion2euler(float* q0,float* q1,float* q2, float* q3,float* phi,float* theta,float* psi){ //quaternion2euler(&q0,&q1,&q2,&q3,&phi,&theta,&psi);
*phi = atan2(2*(*q0 * *q1+*q2 * *q3),1-2*(pow(*q1,2)+pow(*q2,2)))/pi*180;                          //So sieht der Prozeduraufruf aus
*theta=asin(2*(*q0 * *q2-*q3 * *q1))/pi*180;                                                       //Einfach die Formeln nachgebildet
*psi = atan2(2*(*q0 * *q3+*q1 * *q2),1-2*(pow(*q2,2)+pow(*q3,2)))/pi*180;                          //Es werden Grad zurückgegeben


So bitte sehr. Es ist keine Magie dabei. Du solltest vorher noch checken, wie lange eine Ausführung dauert, nicht dass es dann für eine Echtzeitanwendung nicht mehr ausreicht. Pointer sollten dir ein Begriff sein, falls du dich fragst, was diese komischen Sterne vor den Variablen bedeuten.

Go Up