Positionsbestimmung mittels Lagesensor

Wissenschaft Mathematik
#1

Guten Abend liebe Mittüftler,

Vorhanden ist ein 3 axiger Sensor, der jeweils für jede der Achsen (pitch/roll/yaw) alle 4 ms einen Wert bereitstellt, welcher die Winkelbeschleunigung in °/s (grad/sekunde) angibt.
Man stelle sich diesen Sensor auf einem fahrbaren Untersatz vor der in einem Koordinatensystem am Punkt x0/y0/z0. Nun beginnt die Beispielrundfahrt.

Der fährt nun in den ersten 4ms 1cm geradeaus (pitch:0[°/s]¦roll:0[°/s]¦yaw:0[°/s]) Danach steht er im Koordinatensystem auf x1/y0/z0
In den nächsten 4ms macht er einen leichten Bogen(1°) nach rechts. Die Strecke bleibt 1cm.
Der Wert nach weiteren 4 ms sieht also so aus: (pitch:0[°/s]¦roll:0[°/s]¦yaw:250[°/s])
(–> das sind zumindest meine Erwartungen)
Erläuterung:
Yaw: 250°/s
Time: 4ms
1s/0.004s = 250, also 250° / 250 = 1°

Ich weiss also die länge meines Bogens und wie sich der Winkel vom Start zum Endpunkt verhält.
Aber wo steht der Wagen jetzt im Koordinatensystem und wo steht der Wagen nach 5, 10 oder „x“ solcher Schritte?

Ich habe div. Lösungsansätze bei denen ich meinen Bogen (also jeweils die start und endpunkte verbunden) als Hypotenuse nehme mit Sin und Cos Funktionen meinen Standpunkt errechne, um die Funktion zu überprüfen lasse ich den Wagen theoretisch 1 Mal im Kreis fahren mit 360 solcher 1° Schritte, doch an meinen Startpunkt zurückgekehrt bin ich leider noch nie :smile:
Ich scheitere schon alleine daran beim 2ten Schritt die neue Positionsänderung mit der alten Position zu verrechnen, der Wagen steht ja dann schon in einem 1° Winkel am Anfang des 2ten Bogens und dies muss noch hineinverrechnet werden.

Ich bin froh um jeden Tipp und jede Anregung und danke allen Mitdenkenden im voraus. … mehr auf http://w-w-w.ms/qumr2#li_03

#2

Hallo,

gehst du denn davon aus, dass sich das Objekt in jedem Zeitschritt um 1 cm weiter bewegt? Ansonsten kannst du die Position ja gar nicht bestimmen, weil dir Messwerte fehlen.
Ich würde Position und Ausrichtung des Objekts als 4x4-Matrix verwalten. Dann beschränken sich die Rotation und Bewegung auf einfache Matrix-Multiplikationen. Natürlich geht dir da ein Stück Genauigkeit verloren, da du die Integration diskretisierst. Aber du bekommst ja sowieso keine kontinuierlichen Messwerte.
Das prinzipielle Vorgehen wäre dann folgendermaßen:
aktuelle Ausrichtung: O
Ausrichtung im nächsten Schritt:
O := O * R_\phi * T_{1 cm}
Wobei R_\phi eine dreidimensionale Rotationsmatrix ist und
T_{1 cm}=\left( \begin{array}{cccc}
1 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & 1 cm \
ß & 0 & 0 & 1 \end{array} \right)
Das entspricht der Rotation des Objekts an seiner derzeitigen Positionen und anschließender Bewegung um 1 cm in Richtung seiner lokalen z-Achse.
Um den Fehler zu minimieren kannst du den 4 ms-Zeitschritt in kleinere Abschnitte teilen und nacheinander ausführen. Die Berechnung von \phi solltest du aus der Winkelgeschwindigkeit und der vergangenen Zeit einfach nachvollziehen können. Die Rotationsmatrix besteht dann tatsächlich aus drei Teilmatrizen - jeweils um eine Achse. Die Reihenfolge müsstest du selbst festlegen. Bei genügend kleinen Zeitschritten sollte die Wahl aber nicht so große Auswirkungen haben.

Nico