Quaternion zu EulerWinkel umrechnen

Hallo

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

// 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

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:

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.

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):

// 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

Also ich hätte erwartet, dass accelgyro.getRotation(&gx, &gy, &gz); genau die von Dir gewünschten Winkel liefert. Wieso gehst Du davon aus, dass das nicht so ist?

Leider Nein.

Ich hatte das getestet.

Dieses gibt nur die ACC Werte:

accelgyro.getAcceleration(&ax, &ay, &az);

Und diese die Gyro Werte:

accelgyro.getRotation(&gx, &gy, &gz);

Hast du eventuell noch eine Idee?

Gruss Kay

Und wie unterscheiden sich die Gyro Werte von den gewünschten Euler Winkeln???

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

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

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

[quote author=Udo Klein link=topic=105839.msg797684#msg797684 date=1337306403] Es ist mir unklar was Du meinst. Was wären Deiner Meinung nach die "fusionierten" Daten? [/quote]

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

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?

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

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.

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

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.

Für eine Echtzeitanwendung auf so einem schwachen Prozessor wie ihn der Arduino hat kannst Du das vermutlich vergessen. Es wäre sehr viel sinnvoller das den Chip machen zu lassen, schliesslich ist er dafür konstruiert. Ich bin mir sehr sicher, dass das leichter ist als sich die Theorie hinter den Quaternionen anzueignen.

Eine ziemlich gute Einführung findet man hier http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation. Das Problem ist nur, wenn man sich noch nicht mit Matrizen / symmetrischen Matrizen und Matrizenalgebren nichtkommutativen Algebren auskennt, dann wird diese Einführung vermutlich nicht ganz so leicht zu verstehen sein.

ich hab jetz mal interessehalber ne Messung auf meinem Arduino Mega gemacht.

2000 Ausführungen in 1043 ms.

Als Notlösung also meiner Meinung nach akzeptabel. Viel besser wäre es entweder tatsächlich mit den Quaternionen weiterzurechnen oder herauszufinden wie man die Winkel direkt vom Chip bekommt.

D.h. 0.5ms pro Durchlauf. Und der Prozessor soll ja vermutlich auch noch andere Dinge machen. Wie gesagt: nehmt die Beispiele vom Hersteller, es wäre verwunderlich wenn das was gesucht ist nicht dabei ist. Wenn nicht kann man ja auch mal per Email anfragen.

Was die Umrechnungslösung angeht wäre ja auch noch zu klären ob sie überhaupt numerisch stabil ist.

Wenn man die Umrechnung in Grad weglässt sinds 871 ms.

Kommt natürlich auf die Anwendung an, aber ich halte das für “okay”. Kommt darauf an,was mit den Werten dann gesteuert werden soll, aber diese Systeme haben auch eine Ansprechzeit und eventuell macht es keinen Sinn diese mit so vielen Werten zu befeuern. Aber das muss der TE für seine Anwendung entscheiden.

Wenn es einfach wäre an die Werte zu kommen, hätte derjenige, der die Library geschrieben hat, es sicher berücksichtigt.

Wenn es einfach wäre an die Werte zu kommen, hätte derjenige, der die Library geschrieben hat, es sicher berücksichtigt.

Der Hersteller wirbt damit, dass das direkt geht. Das Beispiel welches der OP verwendet hat ist aus dem Internet. Die Herstellerbeispiele sind soweit ich das sehe in einer großen selbstentpackenden Datei für Windows. Meiner Meinung nach sollten die Herstellerbeispiele mehr hergeben als das was man im Internet findet. Es ist nicht hilfreich hin und her zu spekulieren. Besser wäre wenn jemand mit Windows (der OP?) mal die Herstellerbeispiele auspacken und anschauen würde. Das sage ich schon die ganze Zeit.