Rotation um 3 Achsen gleichzeitig

08.09.2016, 22:34

Ballsicher

Rotation um 3 Achsen gleichzeitig

Meine Frage:
Hallo!

Ich bräuchte für ein Hochschulprojekt eine kleine Hilfestellung...

Ich habe in einem Ball einen Beschleunigungssensor, sowie einen Gyrometer.

Diese senden in 50Hz die Signale Beschleunigung (in m/s²) und Winkelgeschw. (in °/s).
Mein Ziel ist es nun, aus den ausgelesenen Daten die Flugkurve bzw. die Bewegung herauszulesen. Hierfür muss ich die Drehung herausrechnen, also für jede Messung die Beschleunigung in ein erdfestes Koordinatensystem überführen. Zunächst muss das System kurz ruhen, damit ich die Erdbeschleunigung messen kann. So weiß ich, wo unten ist.
Aufzeichnen und speichern möchte ich die Koordinaten(x,y,z) des Balles zu jedem Messzeitpunkt um anschließend zu rekonstruieren wie er sich bewegt hat.

Nun ist mein Problem folgendes: Die Drehung rechne ich mit klassischen Rotationsmatrizen raus, also mit den Winkeln der x-, y-, und z-Rotation und dann z.B. \begin{pmatrix} 1 & 0 & 0 \\ 0 & cos & sin\\ 0 & -sin & cos \end{pmatrix} für die x-Rotation. Alle 3 Rotationsmatrizen hintereinander multipliziert mal den Vektor der aktuell gemessenen Beschleunigung ergibt die Beschleunigung im Bezug auf ein erdfestes System.
So dachte ich. leider kommt es jedoch darauf an, in welcher Reihenfolge die Matrizen multipliziert werden. In der Luftfahrt gibt es hier sinnvolle Konventionen, aber wie sieht es mit einem wirklich um 3 Achsen gleichzeitig rotierenden Objekt aus? Lasse ich in meinem Beispielprogramm z.B. ein Einheitskoordinatensystem gleichzeitig 4 mal um 90° in x und y Richtung rotieren, ist es anders orentiert als vorher, weil die 2. und 3. Drehmatrix von rechts sich ja schon auf das von der ersten Drehmatrix gedrehte System beziehen....

Gibt es eine Möglichkeit, eine gleichzeitige Rotation um 3 Achsen herauszurechnen?

Vielen Dank schonmal für eure Mühe!

Meine Ideen:
Plan war wie folgt:
(Pos = Position, v = Geschwindigkeit, a = Beschleunigung)

Pos0 (x,y,z) = (0,0,0)
v0 (x,y,z) = (0,0,0)
a0 = Messung des Beschleunigungssensors

Pos1 = Pos 0 + v0*t + a0*t²
v1 = a0*t

und dann weiter iterieren, wobei vorher die Beschleunigung durch eine Drehmatrix in ein erdfestes System umgerechnet wird.

09.09.2016, 10:00

Ehos

Auf diesen Beitrag antworten »

Wenn sich der Beschleunigungsssensor im Schwerpunkt des Balles befindet, dann kannst du allein aus der Kenntnis der zeitabhängigen Schwerpunkt-Beschleunigung $\begin{eqnarray*} \ddot {\vec x}(t) \end{eqnarray*}$ , welche gemessen wird, die Position $\begin{eqnarray*} \vec x(t) \end{eqnarray*}$ des Balles zu jedem Zeitpunkt nachträglich berechnen, indem du $\begin{eqnarray*} \vec {\ddot x}(t) \end{eqnarray*}$ zwei mal integrierst. Vorraussetzung ist natürlich, dass die Position $\begin{eqnarray*} \vec x(0) \end{eqnarray*}$ und die Geschwindigkeit $\begin{eqnarray*} \dot {\vec x}(0) \end{eqnarray*}$ zu Beginn bekannt sind.

Du benötigst also zur Positionsbestimmung des Balles (genauer des Schwerpunktes) gar nicht die Daten über die Winkelgeschwindigkeit des Balles. Dies wird nur notwendig, wenn der Beschleunigungssensor nicht im Schwerpunkt sitzt.

Wenn du auch die Drehung des Balles wissen willst, so gibt es in der Tat verschiedene Möglichkeiten zu deren Beschreibung. Meist benutzt man die "Eulerschen Winkel" $\begin{eqnarray*} \vec \phi=(\phi_1|\phi_2|\phi_3) \end{eqnarray*}$ . Schau' mal bei WIKIPEDIA, wie diese definiert sind. Mit dem Gyroskop und der eingebauten Elektronik lassen sich die Eulerschen Winkel direkt messen und in Abhängigkeit von der Zeit aufzeichnen.

09.09.2016, 11:57

Ballsicher

Auf diesen Beitrag antworten »

Hallo Ehos,

vielen Dank für deine Antwort! Ich bin jetzt allerdings etwas verwirrt, das doppelte Integrieren funktioniert in meiner Logik gerade nur, wenn die Beschleunigungen in einem erdfesten System aufgezeichnet werden. Habe ich da einen Denkfehler? Angenommen, der Ball wird in x-Richtung beschleunigt und rollt dann um die y-Achse. Wenn diese Rotation noch mit der Beschleunigung zusammenfallen, wird die Beschleunigung ja nicht komplett in x-Achse aufgezeichnet, sondern nur irgendwo in der xz-Ebene. Daher wollte ich für jeden Zeitpunkt die Rotation herausrechnen, um die wahre (erdfeste) Richtung zu erfahren, in die beschleunigt wird. Sonst nehme ich ja evtl. einen Wert in x- einen in z- einen in negativ x und einen in negativ z- Richtung auf und sage dann, dass es eine Kreisbewegung war, obwohl die eigentliche Bewegung gerade war, nur dass das Bezugssystem halt rotierte...

Eulerwinkel sind genau mein Ansatz, aber hier habe ich das Problem, dass Matrizen nicht kommutativ sind. Je nachdem, in welcher Reihenfolge ich die 3 Rotationsmatrizen multipliziere bekomme ich 6 verschiedene Ergebnisse. Dreht man den Ball erst in x- dann in y- und dann in z-Richtung und hält diese Reihenfolge bei Euler ein, passt das Ergebnis. Aber nur dann. Rotiere ich mein Beispielkoordinatensystem um 2 Achsen gleichzeitig (4 mal hintereinander um 90 Grad) ist es anschließend anders orientiert als vorher. Euler rotiert ja nicht um die 3 ursprünglichen Achsen, sondern die 2. und 3. Drehung erfolgen um die vorher gedrehten Achsen. Das ist dann aber nicht mehr das, was mein Gyroskop aufgezeichnet hat. Könnten Quaternionen die Lösung für mein Problem sein?

Lg!

09.09.2016, 15:08

HAL 9000

Auf diesen Beitrag antworten »

Ein wenig off-topic, da nicht zu deiner Anfrage gehörend, sondern nur allgemein zu dem Ansinnen der Flugbahnberechnung grübelnd:

Nur mal ein kurzes Gedankenexperiment: Im Vakuum würde der Sensor, nachdem man den Ball "getreten" hat (ich denke mal unwillkürlich an Fußball) nur noch Beschleunigungswerte Null liefern, bis er wieder auf dem Boden auftrifft. In der Realität liefert er natürlich durch die Abbremsung durch den Luftwiderstand dann doch Werte verschieden von Null. Dennoch scheint mir der sensibelste Part just die extrem kurz andauernde Beschleunigungsphase (also das "Treten" an sich) zu sein: D.h., liegt man dort weit daneben, dann ist eigentlich alles weitere mit einem dicken Fragezeichen versehen...

09.09.2016, 15:09

Ehos

Auf diesen Beitrag antworten »

Ich dachte, dass der Beschleunigungs-Sensor im Ball in einem erdfesten System gemessen werden - ähnlich wie bei einem Kreiselkompass, dessen Lage raumfest bleibt - egal wie das Schiff sich bewegt. Wenn das nicht so ist, musst du natürlich die Drehung "herausrechnen".

Du musst nicht die Eulerschen Winkel nehmen, sondern kannst mit den 3 Winkeln arbeiten, die dein Gyroskop aufzeichnet. Dann bekommst du andere Drehmatrizen als bei Euler. Entscheidend ist, dass die Sache eindeutig ist.

09.09.2016, 15:35

Ballsicher

Auf diesen Beitrag antworten »

@HAL 9000:

Du hast absolut recht, nachdem die Beschleunigung nicht stetig ist, sondern nur mit ca. 50 Hz aufgezeichnet wird, kann ich das, was dazwischen passiert nur approximieren. Bei sehr kurzen, starken Beschleunigungen ist die Sache also von Haus aus nicht präzise, da die lange Flugbahn die ich rekonstruieren will im Endeffekt fast nur von den paar Werten während des Tritts oder Wurfs abhängt. Mal schauen, wie sich das im Endeffekt verhält...

@Ehos: So wie ich das verstanden habe, wird lediglich die Rotation um die körperbezogenen Achsen gemessen. Rotiere ich also 180° um die z-Achse und beschleunige in die Richtung, die gerade eben noch positiv x war, zeichnet er eine Beschleunigung in negativ x-Richtung auf.

Aber auch wenn ich nicht Euler nehme, sondern die 3 Rotationswerte aus dem Sensor selbst - ich muss sie dennoch alle 3 gleichzeitig in mein System fließen lassen. Bzw. ich müsste erst um die x-Achse rotieren, dann nicht um die neue y-Achse, sondern um das, was vor der ersten Drehung mal die y-Achse war. Schließlich ist das die "Kraft", welche der Sensor aufgezeichnet hat. Dann das ganze noch mit z, und das sollte dann wirklich die Orientierung sein. Erst dann kann ich den nächsten Beschleunigungswert sinnvoll benutzen weil ich dann weiß, wohin (im Bezug auf ein erdfestes System) meine Achsen überhaupt zeigen.

Nur: Wie rechnet man das? also Rotation um das, was eben noch die y-Achse war.
Und: Geht das nicht irgendwie leichter?

Soweit ich das jetzt verstanden habe (glaube ich) kann man ein Einheitsquaternion angeben, welches die eigentliche Rotationsachse und Rotationsgeschwindigkeit angibt. Allerdings weiß ich noch nicht, wie man dieses Quaternion bekommt und ob dieser Weg eindeutig ist. Das müsste sich ja irgendwie aus den 3 eizeln aufzezeichneten Drehungen extrahieren lassen...

12.09.2016, 13:59

Ehos

Auf diesen Beitrag antworten »

Die Drehmatrizen für die Drehungen um die 3 festen Achsen x, y, z mit den jeweiligen Drehwinkeln $\begin{eqnarray*} \phi_x, \phi_y,\phi_z \end{eqnarray*}$ lauten

$\begin{eqnarray*} D_x=\begin{pmatrix} 0 & 0 & 0 \\ 0 & \cos(\phi_x) & -\sin(\phi_x) \\ 0 & \sin(\phi_x) & \cos(\phi_x) \end{pmatrix} \end{eqnarray*}$

$\begin{eqnarray*} D_y=\begin{pmatrix} \cos(\phi_y) & 0 & \sin(\phi_y) \\ 0 & 0 & 0 \\ -\sin(\phi_y) & 0 & \cos(\phi_y) \end{pmatrix} \end{eqnarray*}$

$\begin{eqnarray*} D_z=\begin{pmatrix} \cos(\phi_z) & -\sin(\phi_z) & 0 \\ \sin(\phi_z) & \cos(\phi_z) & 0 \\ 0 & 0 &0 \end{pmatrix} \end{eqnarray*}$

Im Gegensatz zu diesen ortsfesten Drehungen, sind die Drehachsen der Eulerschen Drehung nicht fest! Die gesamt Drehmatrix ist das Produkt der 3 einzelnen Drehachsen, also

$\begin{eqnarray*} D=D_zD_yD_x \end{eqnarray*}$

Die Drehrichtung ist wie folgt definiert: Der jeweilige Drehwinkel $\begin{eqnarray*} \phi_x, \phi_y,\phi_z \end{eqnarray*}$ wird positiv gezählt, wenn es sich um eine Drehung nach rechts handelt (bei Blickrichtung in Richtung der Drehachse=Koordinatenachse). Wichtig ist es, die Reihenfolge der Drehungen zu beachten!

12.09.2016, 15:13

Ballsicher

Auf diesen Beitrag antworten »

Hallo Ehos und erneut danke für deine Hilfe!

Ich glaube du hast jeweils im Eintrag (i,i) der i-ten Matrix eine 1 vergessen, kann das sein? Sonst hätte die resultierende Gesamtdrehung ja eine komplette Zeile (und Spalte) gleich Null und würde die z-Komponente jedes Vektors den ich von rechts da dran multipliziere auf Null setzen.

Die Gesamtdrehung stimmt bei mir jetzt exakt mit dem überein, was im Wiki-Eintrag zu Euler-Winkeln unter Roll-Nick-Gier steht.
(Links darf ich hier noch nicht posten...)

Aber das sind doch gerade die Rotationen um ein Körperbezogenes System. Ich glaube, ich hab einfach einen Denkfehler.....

14.09.2016, 09:28

Ehos

Auf diesen Beitrag antworten »

@Ballsicher
Du hast recht. Bei meinen Drehmatrizen fehlten die Einsen.

Ich frage nochmal nach, weil ich noch nicht ganz verstanden habe, was du misst und was du berechnen möchtest:
---------------------------------------------------------------------------------------
Stimmt es, dass du die folgende 6 Messwerte direkt messen kannst:

- die 3 Eulerschen Winkel $\begin{eqnarray*} \phi, \psi, \xi \end{eqnarray*}$ und die Beschleunigungen $\begin{eqnarray*} \tilde {\ddot x}, \tilde {\ddot y},\tilde {\ddot z}, \end{eqnarray*}$ im gedrehten Koordinatensystem (=Ball)
---------------------------------------------------------------------------------------
Stimmt es, dass du daraus folgende Werte berechnen willst?

-die Beschleunigungen $\begin{eqnarray*} \ddot x, \ddot y,\ddot z, \end{eqnarray*}$ im ortsfesten System
---------------------------------------------------------------------------------------

14.09.2016, 13:53

Ballsicher

Auf diesen Beitrag antworten »

Hallo Ehos und danke dass du dich weiter bemühst!

Um ehrlich zu sein, ob die gemessenen Winkel "Eulerwinkel" heißen weiß ich nicht.

Ich messe pro Zeiteinheit 3 Drehungen (um jede Achse eine) und besagte 3 Beschleunigungen jeweils um- bzw. entlang die Achsen des körperfesten Koordinatensystems.

Mein Ziel sind die Beschleunigungen im Erdfesten System, das hast du alles richtig interpretiert.
Im Endeffekt möchte ich den Weg des Balles in 3D darstellen. Hierfür muss ich zu jeder Messung die körperfesten Beschleunigungen in erdfeste Beschleunigungen umwandeln.

Soweit ich verstanden habe, sind Eulerwinkel ja 3 aufeinanderfolgende Rotationen, nicht jedoch 3 gleichzeitige Drehungen. Benutze ich also die oben genannte Rotationsmatrix für "Roll Nick Gier", so verzerrt mein "Bild".
Nachdem ich mich nie zuvor mit so etwas auseinandergesetzt habe, denke ich dass ich mit Euler evtl auf dem Holzweg war.

Meine Matrix (die von dir vorgeschlagene) scheint folgendes zu tun:

Rotation um x-Achse -> Rotation um die neu orientierte, durch den ersten Schritt gedrehte y-Achse
-> Rotation um die neu orientierte, durch die ersten 2 Schritte gedrehte z-Achse

Jedoch ist das nicht zielführend, schließlich liegen alle 3 Messungen gleichzeitig vor
und nicht in der Reihenfolge x, y, z.
Wenn mein Sensor also eine Drehung um die z-Achse aufzeichnet, meint er nicht die bereits um x und y gedrehte, sondern die momentane körperfeste z-Achse zum Zeitpunkt der Messung.
Um die Rotation "zurückzurechnen" brauche ich also eine Rotationsmatrix, die im Endeffekt beispielsweise so arbeitet:

Rotation um die x-Achse -> Rotation um eine nicht mitgedrehte y-Achse ->
Rotation um eine nicht mitgedrehte z-Ache

Existiert eine solche Drehmatrix?

Nochmal vielen Dank für deine Bemühungen!

15.09.2016, 13:16

Ehos

Auf diesen Beitrag antworten »

Du wunderst dich darüber, dass die 3 Teildrehungen um die Eulerschen Winkel oder um andere Winkel zeitlich hintereinander ausgeführt werden, obwohl diese in der Realität "gleichzeitig" ablaufen. Das stimmt. Aber dieser Widerspruch ist nur scheinbar:

Die Drehmatrix D(t) bildet die Anfangslage des Balles auf die aktuelle Lage zur Zeit t ab. Die Drehmatrix beschreibt also für jede Zeit t eine Drehung um eine starre Drehachse mit einem starren Drehwinkel (d.h. die Drehung "auf dem kürzesten Weg"). Die Drehmatrix D(t) enthält also keine Information darüber, wie sich der Ball im Zeitintervall [0;t] tatsächlich gedreht hat.

In der Tat sind die Eulerschen und ähnliche Winkel aus anschaulicher Sicht ziemlich umständlich. Ich beschreibe mal eine Methode, wie man die Dreh-Position deines Balles zu jedem Zeitpunkt einfach und anschaulich definiert:
---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
Wir betrachten nur die Drehung des Balles (ohne Translation). Angenommen der Ball habe zur Zeit t=0 eine Drehposition A und zur späteren Zeit t eine Drehposition B. Dabei ist völlig egal, wie der Ball von A nach B gekommen ist. Wesentlich ist folgendes: Für jede Drehpostion B existiert eine starre Drehachse $\begin{eqnarray*} \vec n \end{eqnarray*}$ (=Einheitsvektor) und ein fester Drehwinkel $\begin{eqnarray*} \alpha \end{eqnarray*}$ , um welche man den Ball von A nach B "auf kürzestem Wege" drehen kann. Das Produkt aus Drehwinkel $\begin{eqnarray*} \alpha \end{eqnarray*}$ und Drehachsen-Einheitsvektor $\begin{eqnarray*} \vec n \end{eqnarray*}$ kann man als "Koordinanten-Vektor der Drehung" auffassen.

$\begin{eqnarray*} \text{Koordinaten-Vektor der Drehung}=\underbrace{\alpha}_{\text{Drehwinkel}}\cdot \underbrace{\begin{pmatrix} \cos(\phi)\sin(\theta) \\ \sin(\phi)\sin(\theta) \\ \cos(\theta) \end{pmatrix}}_{\text{Drehachse }\vec n} \end{eqnarray*}$

Den Drehachsen-Einheitsvektor $\begin{eqnarray*} \vec n \end{eqnarray*}$ habe ich wie üblich durch Kugelkoordinaten ausgedrückt. Dieser Koordinaten-Vektor legt mit den drei Winkeln $\begin{eqnarray*} \alpha, \phi, \theta \end{eqnarray*}$ die Dreh-Position des Balles zu jedem Zeitpunkt t eindeutig fest (viel einfacher als die Eulerschen Winkel). Es ist klar, dass sich dieser Vektor mit der Zeit aufgrund der Drehung des Balles ständig ändert. Der obige Koordinaten-Vektor der Drehung ist das Analogon zum Koordinaten-Vektor (x,y,z) bei der Translation eines Massepunktes.

Der Nachteil des obigen Koordinaten-Vektors ist, dass die 3 darin auftretenden Winkel $\begin{eqnarray*} \alpha, \phi, \theta \end{eqnarray*}$ am Gyroskop meist nicht direkt gemessen werden (aus rein technischen Gründen). Im Experiment misst man meist irgendwelche anderen 3 Winkel $\begin{eqnarray*} (\phi_1, \phi_2,\phi_3) \end{eqnarray*}$ , welche die gleiche Information enthalten (z.B. die 3 Eulerschen Winkel).
-------------------------------------------------
Angenommen die Drehmatrix D(t) sei bekannt, wobei als Parameter die 3 Eulerschen Winkel $\begin{eqnarray*} (\phi_1, \phi_2,\phi_3) \end{eqnarray*}$ oder andere Parameter benutzt werden.

Frage:
Wie bekommt man man aus der Drehmatrix D den obigen "Koordinaten-Vektor der Drehung " $\begin{eqnarray*} \phi \cdot \vec n \end{eqnarray*}$ ?

Antwort:
Die Drehachse $\begin{eqnarray*} \vec n \end{eqnarray*}$ mit $\begin{eqnarray*} |\vec n|=1 \end{eqnarray*}$ ist der auf 1 normierte Eigenvektor der Drehmatrix D zum Eigenwert $\begin{eqnarray*} \lambda =1 \end{eqnarray*}$ und der Drehwinkel ist

$\begin{eqnarray*} \cos(\phi)=\tfrac{1}{2}\cdot [Spur(D)-1] \end{eqnarray*}$

15.09.2016, 13:50

Huggy

Auf diesen Beitrag antworten »

Mir scheint, die Sache ist gar nicht so kompliziert. Wenn ich es richtig verstanden habe, werden Winkelgeschwindigkeiten um die 3 Achsen des körperfesten Koordinatensystems gemessen. Die Winkelgeschwindigkeiten lassen sich im Unterschied zu den Drehungen durch Vektoren beschreiben während man für die Drehungen Drehmatrizen braucht. Die Richtung eines Vektors der Winkelgeschwindigkeit gibt die Drehachse an und sein Betrag den Betrag der Winkelgeschwindigkeit. Hat man nun 3 Winkelgeschwindigkeiten um die x-, y-, z-Achse gegeben, so ergibt die Addition ihrer Vektoren die resultierende Drehachse und den resultieren Betrag der Winkelgeschwindigkeit. Stellt man sich den betrachteten Körper als homogene Kugel vor, so entsprechen den Vektoren der Winkelgeschwindigkeit bis auf einen Proportionalitätsfaktor die Drehimpulse des Körper, die man ja auch vektoriell addieren und zerlegen kann.

Hat man den resultierenden Vektor der Winkelgeschwindigkeit bestimmt, ergibt sich daraus mit der Länge des Zeitintervalls eine Drehmatrix, die Drehung des Körpers in diesem Zeitintervall beschreibt. Eine Multiplikation von Drehmatrizen innerhalb dieses Zeitintervalls ist nicht erforderlich. Für das nachfolgende Zeitintervall bekommt man eine neue Drehmatrix und die Multiplikation der Drehmatrizen für aufeinanderfolgede Zeitintervalle beschreibt die Orientierung des Körpers relativ zum erdfesten Koordinatensystem zum jeweiligen Zeitpunkt.

Man könnte auch nur mit Drehmatrizen arbeiten, aber das wäre deutlich umständlicher und mit mehr Rundungsfehlern behaftet. Dazu unterteilt man das Zeitintervall zwischen 2 Messungen in kleinere Zeitintervalle. Diese wählt man so, dass der Drehwinkel für jede der 3 Achsen innerhalb des kleineren Zeitintervalls klein ist. Bei kleinen Drehwinkeln ist die Multplikation von Drehmatrizen näherungsweise kommutativ. Oder wie die Physiker gern sagen, infinitisimale Drehungen kommutieren.

16.09.2016, 09:15

Ehos

Auf diesen Beitrag antworten »

@Huggy
Die Unklarheit kommt daher, dass nicht klar ist, welche 3 Messwerte $\begin{eqnarray*} \phi_1,\phi_2,\phi_3 \end{eqnarray*}$ am Gyroskop werden gemessen? Du vermutest sogar, dass die Winkelgeschwindigkeit $\begin{eqnarray*} \omega_1,\omega_2,\omega_3 \end{eqnarray*}$ direkt gemessen wird (Dazu würde man 3 Tachometer benötigen). Auch das ist möglich. Wenn man wüsste, was gemessen wird, wäre die Rechnung wäre relativ einfach (mit oder ohne Drehmatrizen).

@Ballsicher
Sage uns bitte, was tatsächlich gemessen wird. Wir benötigen also den Zusammenhang zwischen den 3 Messwerten $\begin{eqnarray*} \phi_1,\phi_2,\phi_3 \end{eqnarray*}$ und der zugehörigen Drehmatrix D. Wenn man D kennt, dann lautet der Zusammenhang zwischen den Beschleunigungen $\begin{eqnarray*} \ddot {\vec x} \end{eqnarray*}$ und $\begin{eqnarray*} \ddot {\vec x}' \end{eqnarray*}$ im ruhenden bzw. im rotierenden Koordinatensystem einfach

$\begin{eqnarray*} \ddot {\vec x}=D^{T}\ddot {\vec x}' \end{eqnarray*}$

Wenn man die Beschleunigung $\begin{eqnarray*} \ddot {\vec x} \end{eqnarray*}$ im ruhenden Koordinatensytem kennt, musst man diese nur noch 2 mal integrieren und hat die Bahnkurve.

16.09.2016, 10:45

Ballsicher

Auf diesen Beitrag antworten »

Hallo Huggy, hallo Ehos!

Tut mir leid, ich hatte mich da nicht gut ausgedrückt. Der Gyro misst Winkelgeschwindigkeit in Grad pro Sekunde.
Diese hatte ich in meinem Versuchsprogramm immer mit der Zeitzwischen den Messungen multipliziert ( 0.01). Die von mir verwendeten Gyrowerte sind also Grad/Sekunde * 0.01 Sekunden = Gradänderung zwischen zwei Messungen. Mehr Information finde ich im Handbuch dazu nicht...

Die Drehmatrix, welche ich verwendete, ist die vor ein paar Tagen von Ehos vorgeschlagene. Unter dem Wikipedia Eintrag zu Eulerwinkeln die "roll, nick, gier" zur Umrechnung von Körperfest in Erdfest. Allerdings handelt es sich bei meinen Daten, soweit ich jetzt verstanden habe, eben nicht um Eulerwinkel. Das war wohl eine dumme Annahme meinerseits weil ich bei meinen Recherchen permanent über diesen Begriff gestolpert bin.

@Ehos: Exakt diese Idee verfolge ich. Die Beschleunigungen entlang der Achsen des körperfesten Koordinatensystems kenne ich. Mein Ziel ist es also, in jedem Iterationsschritt durch eine geeignete Messung die aufgezeichnete Rotation zurückzurechnen, um die aufgezeichneten Beschleunigungen (nach besagter Rechnung nun im erdfesten Koordinatensystem) aufzuintegrieren. Durch zweimaliges integrieren sollte ich dann die Position in allen 3 Richtungen zu jedem Iterationsschritt bekommen. Diese könnte ich dann einfach plotten.

Vielen Dank nochmal für eure Mühe, ich schöpfe langsam Hoffnung 😉

16.09.2016, 12:08

Ehos

Auf diesen Beitrag antworten »

Wenn ich dich richtig verstehe, hast du eine Tabelle mit Messwerten, die jedem Zeitpunkt $\begin{eqnarray*} t_k \end{eqnarray*}$ jeweils die Winkelgeschwindigkeit des Balles Winkelgeschwindigkeit und die Beschleunigung im gedrehten Systen zuordnet, also

Man könnte also die Beschleunigung im Ruhesystem so bestimmen, wie Huggy es beschrieben hat (also schrittweise, ohne Drehmatrix).

Ich melde mich am Montag nochmal.

16.09.2016, 14:09

Huggy

Auf diesen Beitrag antworten »

Zitat:

Original von Ehos
Man könnte also die Beschleunigung im Ruhesystem so bestimmen, wie Huggy es beschrieben hat (also schrittweise, ohne Drehmatrix).

Na ja, ich würde schon auch Drehmatrizen verwenden wollen, aber an einer zentralen Stelle mit Vektoren arbeiten. Ich versuche mal, meine Idee etwas detaillierter zu beschreiben.

Gesucht ist die Orientierung des Körpers zum Zeitpunkt $\begin{eqnarray*} t_i \end{eqnarray*}$ relativ zum erdfesten Koordinatensystem. Diese Orientierung kann durch eine Drehmatrix $\begin{eqnarray*} D_i \end{eqnarray*}$ beschrieben werden. Die linearen Beschleunigungen im erdfesten Koordinatensystem lassen sich dann aus den linearen Beschleunigen im körperfesten System berechnen zu:

Zitat:

Original von Ehos
$\begin{eqnarray*} \ddot {\vec x}=D^{T}\ddot {\vec x}' \end{eqnarray*}$

Ich lasse im folgenden mal das transponiert weg. Was man als die Matrix und was als die transponierte Matrix bezeichnet, ist eh eine Definitionsfrage. Aber selbstverständlich muss man darauf achten, dass durch geeignete Wahl des Vorzeichens bei den Winkeln in obiger Beziehung die passende Matrix steht.

Es sei $\begin{eqnarray*} E_i \end{eqnarray*}$ die Rotationsmatrix, die die Rotation des körperfesten Koordinatensystems vom Zeitpunkt $\begin{eqnarray*} t_i \end{eqnarray*}$ zum Zeitpunkt $\begin{eqnarray*} t_{i+1} \end{eqnarray*}$ beschreibt. Man hat dann:

$\begin{eqnarray*} D_{i+1}=E_iD_i \end{eqnarray*}$

Gegeben seien die Winkelgeschwindigkeiten $\begin{eqnarray*} (\omega_x, \omega_y, \omega_z) \end{eqnarray*}$ zum Zeitpunkt $\begin{eqnarray*} t_i \end{eqnarray*}$ . Die resultierende vektorielle Drehgeschwindigkeit ist dann durch diesen Vektor gegeben. Ihr Betrag ist

$\begin{eqnarray*} \dot \alpha = \sqrt {\omega_x^2+\omega_y^2+\omega_z^2} \end{eqnarray*}$

Die Drehachse, kann man, wie auch schon von dir ausgeführt, durch ihre Winkel in Kugelkoordinaten beschreiben:

$\begin{eqnarray*} \theta = \arccos {\frac {\omega_z}{\sqrt {\omega_x^2+\omega_y^2+\omega_z^2}}} \end{eqnarray*}$

$\begin{eqnarray*} \phi= \operatorname {atan2} (\omega_y, \omega_x) \end{eqnarray*}$

Ich hoffe, es ist klar, weshalb ich $\begin{eqnarray*} \operatorname {atan2} (\omega_y, \omega_x) \end{eqnarray*}$ statt $\begin{eqnarray*} \arctan {(y/x)} \end{eqnarray*}$ geschrieben habe. Die Drehmatrix $\begin{eqnarray*} E_i \end{eqnarray*}$ ergibt sich dann zu:

$\begin{eqnarray*} E_i=M_z(\phi) M_y (\theta) M_z (\alpha) M_y (-\theta) M_z(-\phi) \end{eqnarray*}$

mit

$\begin{eqnarray*} \alpha =\dot \alpha \Delta t \end{eqnarray*}$

$\begin{eqnarray*} M_x, M_y, M_z \end{eqnarray*}$ sind die elementaren Drehmatrizen um die x-, y-, z-Achse.

Anschließend muss man sich noch Gedanken darüber machen, wie man Drehungen und lineare Beschleunigungen numerisch miteinander verknüpft. Prinzipiell ist das immer durch eine adäquate Verfeinerung der Zeitskala möglich.

17.09.2016, 19:46

Ballsicher

Auf diesen Beitrag antworten »

Hallo Huggy!

Wow, vielen Dank für deinen Beitrag, das ist echt klasse!
Um ehrlich zu sein, so wirklich komplett verstehen tu ich es nicht, dazu bin ich einfach viel zu ungeblidet in Linearer Algebra...
Ein bisschen irritiert mich zum Beispiel, dass keine Drehmatrix in x-Richtung vorkommt. Evtl weil ich den atan2 nicht ganz verstehe. Augenzwinkern

Ich habe deinen Beitrag nun einmal nachgebaut:

erdsystem = np.array([[1, 0, 0],[0, 1, 0],[0, 0, 1]]) <- Initiierung (erstmal Einheitsmatrix)
drehung = np.array([[1, 0, 0],[0, 1, 0],[0, 0, 1]]) <-Drehung in Schritt Null = Null

vel_eTemp0 = np.array([0, 0, 0]) <-Geschwindigkeit Zeitpunkt Null
vel_eTemp1 = np.array([0, 0, 0]) <-wird im ersten Schleifendurchgang überschrieben
pos_eTemp = np.array([0, 0, 0]) <-Position Zeitpunkt Null
acc_eTemp0 = np.array([0, 0, 0]) <-Beschleunigung Zeitpunkt Null
acc_eTemp1 = np.array([acc_x[0], acc_y[0], acc_z[0]]) <-erste Messung aus Beschleunigungssensor

for i in range (0, len(gyr_x)):
gyr_x[i] = np.deg2rad(gyr_x[i]) <--Umrechnung auf Bogenmaß
gyr_y[i] = np.deg2rad(gyr_y[i])
gyr_z[i] = np.deg2rad(gyr_z[i])

for i in range(0, len(acc_x)):

alpha_dot = (gyr_x[i]**2 + gyr_y[i]**2 + gyr_z[i]**2)**.5

if alpha_dot ==0:
phi = np.deg2rad(90)
else:
phi = np.deg2rad(np.arccos(gyr_z[i] / alpha_dot))

psi = np.deg2rad(math.atan2(gyr_x[i], gyr_y[i]))
alpha = alpha_dot * dt

Mz_p = np.array([[np.cos(psi), -np.sin(psi), 0], [np.sin(psi), np.cos(psi), 0], [0, 0, 1]])
My_p = np.array([[np.cos(phi), 0, np.sin(phi)], [0, 1, 0], [-np.sin(phi), 0, np.cos(phi)]])
Mz_a = np.array([[np.cos(alpha), -np.sin(alpha), 0], [np.sin(alpha), np.cos(alpha), 0], [0, 0, 1]])
Mz_n = np.array([[np.cos(-psi), -np.sin(-psi), 0], [np.sin(-psi), np.cos(-psi), 0], [0, 0, 1]])
My_n = np.array([[np.cos(-phi), 0, np.sin(-phi)], [0, 1, 0], [-np.sin(-phi), 0, np.cos(-phi)]])

drehung1 = np.dot(Mz_p, np.dot(My_p, Mz_a))
drehung2 = np.dot(My_n, Mz_n)
drehung = np.dot(drehung1, drehung2)

erdsystem = np.dot(drehung, erdsystem.transpose())
acc_eTemp1 = np.dot(erdsystem.transpose(), np.array([acc_x[i], acc_y[i], acc_z[i]]))
acc_eTempDelta = acc_eTemp1 + acc_eTemp0 <-kein wirkliches Delta
vel_eTemp1 = vel_eTemp0 + acc_eTempDelta * dt * 0.5 <-Trapezregel als "Integral" über Beschl.
vel_eTempDelta = vel_eTemp1 + vel_eTemp0 <-auch kein Delta
pos_eTemp = pos_eTemp + vel_eTempDelta * dt*0.5 <- Trapezregel, "Integral" über Velocity
pos_e.append(pos_eTemp) <-pos_eTemp ist aktuelle Position, anhängen an Liste

acc_eTemp0 = acc_eTemp1 <-aktuelle Beschl. ist im nächsten Schritt die zuletzt gemessene
vel_eTemp0 = vel_eTemp1 <-aktuelle Geschwl. ist im nächsten Schritt die zuletzt berechnete

print(pos_e)

In einem Intervall von 15 Sekunden habe ich mit erfundenen Daten eine Abweichung von bis zu 20 Metern pro Achse gegenüber einem Testlauf ohne Rotation. Vermutlich liegt das an der erbärmlichen Implementierung meinerseits. Nachdem beim Rumprobieren (Beschleunigung von links oder rechts an erdsystem multiplizieren, invertieren oder nicht, ...) immer sehr ähnliche Ergebnisse kommen, bin ich mir nicht ganz sicher ob ich alles richtig gemacht habe.

Dennoch: Ich wollte einfach schonmal ein Feedback geben, dank eures Einsatzes bin ich der Lösung so nah wie noch nie. Dafür ein ganz herzliches Danke! Prost

Ich suche nun weiter nach Denkfehlern in meinem "Programm". Wink

18.09.2016, 09:59

Huggy

Auf diesen Beitrag antworten »

Zitat:

Ein bisschen irritiert mich zum Beispiel, dass keine Drehmatrix in x-Richtung vorkommt. Evtl weil ich den atan2 nicht ganz verstehe.

Das hat nichts mit dem atan2 zu tun. Der Grund für den atan2 ist hier beschrieben:

atan2

Das Fehlen der x-Achse liegt daran, dass diese in Polarkoordinaten mit $\begin{eqnarray*} \phi = 0 \end{eqnarray*}$ verknüpft ist. Um die Drehung um die durch $\begin{eqnarray*} (\phi, \theta) \end{eqnarray*}$ charakterisierte Achse durchzuführen, wird so vorgegangen: $\begin{eqnarray*} M_z(-\phi) \end{eqnarray*}$ dreht diese Achse in die x-z-Ebene. $\begin{eqnarray*} M_y(-\theta) \end{eqnarray*}$ dreht sie dann auf die z-Achse. Jetzt wird die gewünschte Drehung um den Winkel $\begin{eqnarray*} \alpha \end{eqnarray*}$ mit $\begin{eqnarray*} M_z(\alpha) \end{eqnarray*}$ als Drehung um z-Achse durchgeführt, denn dort liegt ja nun die Drehachse. Anschließend wird die Drehachse wieder in ihre ursprüngliche Richtung zurückgedreht. Man kann das sicher auch anders realisieren.

Um meinen Vorschlag und seine Implementierung zu testen, sollte man zuerst die einzelnen Schritte testen. Man sollte zunächst mal prüfen, ob reine Drehungen um die Achsen des körperfesten System richtig im erdfesten System dargestellt werden. Danach dasselbe für gleichzeitige Drehungen um 2 und 3 Achsen. Erst wenn das alles passt, sollte man sich um die Kombination von Drehungen und linearen Beschleunigungen kümmern. Auch hier kann man erst mal mit reinen Beschleunigungen in Richtung einer der Achsen des körperfesten Systems anfangen.

18.09.2016, 17:35

Ballsicher

Auf diesen Beitrag antworten »

Hallo Huggy,

Danke, jetzt kann ich es mir bildlich recht gut vorstellen. Der Atan2 erschließt sich mir nun auch.

Ich habe ein wenig herumprobiert wie sich die Rotationsmatrix verhält und die Beschleunigungen erstmal weggelassen. Initiiert wird immer mit der Einheitsmatrix
\begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}.

Drehungen um eine Achse (egal welche) werden tadellos ausgeführt. drehe ich insgesamt um 360°, so habe ich im Endeffekt wieder meine ursprüngliche Einheitsmatrix mit Rundungsfehlern ab der 4. Nachkommastelle (total in Ordnung).
Drehe ich um mehrere Achsen nacheinander klappt ebenfalls alles. (bsp. erst 2 mal 180 um x, dann 4 mal 90 um y)
Auch Alternieren sieht noch ganz gut aus.
Erst 270° um x, dann 90° um y, anschließend 90° um x und dann 270° um y führt zu bis zu 5% "Verlust"

Ganz aus den Fugen gerät die Sache, wenn in einem Zeitschritt 2 oder 3 Drehungen gleichzeitig passieren.
Frei erfundene Gyro-Werte
gyr_x = [0, 45, 45, 0, 90, 0, 60, 60, 0, 60, 0, 0, 0, 0, 0] <- Summe jeweils 360°
gyr_y = [0, 0, 92, 88, 50, 40, 0, 20, 40, 15, 15, 0, 0, 0, 0]
gyr_z = [0, 10, 10, 10, 10, 20, 30, 40, 50, 60, 70, 50, 0, 0, 0]

liefern diese Matrix:
\begin{pmatrix} 0.87082976 &-0.49144208 & 0.01184074 \\ 0.49143477 & 0.87090644 & 0.00371964\\ -0.01214017 & 0.00257978 & 0.99992298\end{pmatrix}

Dein Plan leuchtet mir schon ein, auch, dass die z-Achse durch dieses Vorgehen am wenigsten "leidet" ist mir nun klar, das war mir gestern beim rumprobieren aufgefallen.
In der realen Anwendung werde ich wohl selten mehr als 8° pro Zeitschritt messen, die obigen erfundenen Drehungen sind also viel zu extrem. Trotzdem scheint sich noch irgendwo ein Fehler versteckt zu haben.

19.09.2016, 08:58

Huggy

Auf diesen Beitrag antworten »

Wenn man Drehungen nacheinander ausführt, dann wird die sich ergebende Gesamtdrehung nicht durch die Addition der Drehvektoren repräsentiert. Wäre das so, dann müsste man Drehungen ja nicht durch Drehmatrizen darstellen. Und dann wären aufeinanderfolgende Drehungen auch miteinander vertauschbar, weil ja die Vektoraddition kommutativ ist. Deshalb ist doch gar nicht zu erwarten, dass sich bei deinem Beispiel zum Schluss die Einheitsmatrix ergibt.

19.09.2016, 12:44

Ehos

Auf diesen Beitrag antworten »

Da nur numerische Messwerte gegeben sind, wird eine Rekursivformel gesucht, mit der man im unbewegten Koordinatensystem aus der Beschleunigung $\begin{eqnarray*} \vec a(t) \end{eqnarray*}$ zur Zeit t die Beschleunigung $\begin{eqnarray*} \vec a(t+\Delta t) \end{eqnarray*}$ berechnet kann (also "kurze Zeit später"). Mit dieser Rekursivformel kann man die gesuchte zeitabhängige Beschleunigung $\begin{eqnarray*} \vec a(t) \end{eqnarray*}$ im unbewegten System schrittweise zusammensetzen. Offenbar setzt sich die Beschleunigungsänderung im unbewegten System aus 2 Anteilen zusammen. Der 1.Anteil kommt von der Drehung des Balles (="scheinbare Beschleunigung"). Der 2.Anteil kommt von der "echten" Änderung der Beschleunigung infolge externer Kräfte. Diese Rekursivformel lautet deshalb

$\begin{eqnarray*} \vec a_{k+1}\approx \underbrace{\vec a_k}_{\text{alter Wert}}+\underbrace{\omega_k \times \vec a_k\cdot \Delta t}_{\text{"scheinbare" Beschleunigung}}+\underbrace{D^T (a'_{k+1}-a'_k)}_{\text{"echte" Änderung}} \end{eqnarray*}$ ______________________Formel (6)

--------------------------------------------------------------------------------------------------
Bezeichnungen:

$\begin{eqnarray*} k=\text{Index, der die Datensätze zählt (diese müssen zeitlich dicht beeieinander liegen)} \end{eqnarray*}$
$\begin{eqnarray*} \Delta t=\text{kleines Zeitintervall (=Zeitdifferenz zwischen 2 Messpunkten) } \end{eqnarray*}$
$\begin{eqnarray*} \vec a_k=\text{Beschleunigung im unbewegten Koordinantensystem zu einem Zeitpunkt t} \end{eqnarray*}$
$\begin{eqnarray*} \vec \omega_k=\text{Winkelgeschwindigkeit des Balles zu einem Zeitpunkt t} \end{eqnarray*}$
$\begin{eqnarray*} \vec a'_k=\text{Beschleunigung im bewegten Koordinantensystem zu einem Zeitpunkt t} \end{eqnarray*}$
$\begin{eqnarray*} \vec a'_{k+1}=\text{Beschleunigung im bewegten Koordinantensystem zu einem Zeitpunkt }t+\Delta t \end{eqnarray*}$
$\begin{eqnarray*} D^T=\text{inverse Drehmatrix zur Zeit t} \end{eqnarray*}$
--------------------------------------------------------------------------------------------------

Mit dieser Formel kann man rekursiv die gesuchte Beschleunigung bestimmen. Als Anfangswert nimmt man die gemessene Beschleunigung im rotierenden Koordinatensystem, also $\begin{eqnarray*} \vec a_k=\vec a'_k \end{eqnarray*}$ , weil sich bei t=0 noch nichts gedreht und nicht bewegt hat. Die Drehmatrix $\begin{eqnarray*} D^T \end{eqnarray*}$ ist die Inverse Drehmatrix D(t) zur Zeit t. Man beschafft sich D(t) durch numerische Integration über die Winkelgeschwindigkeit, indem man das folgende Zeitintegral über die Winkelgeschwindigkeit berechnet (eigentlich 3 Integrale: für jede Komponente ein Integral):

$\begin{eqnarray*} \vec \phi(t)=\int_{0}^t \vec \omega(t)\,\,dt \end{eqnarray*}$

Bei WIKIPEDIA findet man unter dem Suchbegriff "Drehmatrix", wie man aus dem Betrag $\begin{eqnarray*} \phi=|\vec \phi| \end{eqnarray*}$ und dem Einheitsvektor $\begin{eqnarray*} \vec n=\tfrac{\vec \phi}{|\vec \phi|} \end{eqnarray*}$ des Vektors $\begin{eqnarray*} \vec \phi \end{eqnarray*}$ die Drehmatrix D(t) zur Zeit t bestimmen kann.

Neue Frage »

Antworten »

Rotation um 3 Achsen gleichzeitig

Verwandte Themen