Ich muss über 6 Laser wissen, wo sich ein Multirotor in einem rechteckigen Raum befindet, 2 auf jeder Achse.

Das Problem ist wie folgt:

Eingaben:

• Raum: Quadrat => 10 mal 10 Meter
• 6 Positionen des Lasers: Feste auf dem Rahmen
• 6 Orientierungen des Lasers: Feste auf dem Rahmen
• Die 6 Messungen der Laser
• Die Quaternion von der IMU meines Fluglotsen (PixHawk).
• Der Ursprung ist auf dem Schwerpunkt des Mehrrotors zentriert und so definiert, als ob die Wände senkrecht zu jeder Achse stehen (die Normale der Wand in X ist (-1,0,0)).

Ausgabe :

• Position in 3D (X, Y, Z)
• Winkelposition (Quaternion)

Da ich die Winkelposition des Multi-Rotors erhalten habe, habe ich die Laserpositionen und -orientierungen über das Quaternion gedreht, dann über die 6 Messungen extrapoliert und die 3 Wände erhalten. (Die Ausrichtung der Wände ist trivial, dann reicht nur ein Punkt aus, um ihre Position zu bestimmen.

Leider habe ich festgestellt, dass die Giermessung (Rotation um z) vom PixHawk unzuverlässig ist . Dann sollte ich das Gieren von den Lasern messen, aber es gelingt mir nicht, es zu tun. Wenn das 2D-Problem einfach ist, bin ich in 3D verloren.

Weiß jemand, ob es irgendwo [ Algorithmus zum Erkennen der XYZ-Position und der Quaternion aus 6 Messungen ] gibt? Oder wie geht man dieses Problem richtig an?

Die Frage: Wie könnte ich das Gieren aus 2 Messungen von 2 Lasern erhalten, bei denen ich die ursprüngliche Position, Ausrichtung und das Nick- und Rollverhalten kenne?

HINWEIS: Grüne Zeiger sind die Ursprungsposition, rote Zeiger sind die "Endposition", können jedoch (aufgrund von Gieren) um den roten Kreis gedreht werden.

Lösung: Gibt es eine andere Lösung ohne Vorrotation von Vektoren?

Ich habe endlich eine Lösung gefunden und hier ist sie.

Python, ROS-Geometriebibliothek , numpy

Mein aktueller Code / Mathe in Kürze:

1) Drehen Sie die Position und Ausrichtung der Laser durch Rollen und Neigen. Die axes='sxyz'Mittel: Statische Achse, Roll, Pitch, Gieren anwenden.

quaternion_matrix Erstellt eine 4x4-Transformationsmatrix aus dem Quaternion.

laser = (1,1,1,0) # laser position
orientation = (1,0,0,0) # laser orientation

roll, pitch, _ = list(euler_from_quaternion(q, axes='sxyz'))
q = quaternion_from_euler(roll, pitch, 0, axes="sxyz")
laser = numpy.dot(quaternion_matrix(q), laser)
orientation = numpy.dot(quaternion_matrix(q), orientation)

2) Algebrische Lösung: Drehung um Z in Abhängigkeit vom Gieren

laser       = [-sin(a)*laser[1] + cos(t)*laser[0], 
                cos(t)*laser[1] + sin(t)*laser[0],
                laser[2]]

orientation = [-sin(a)*orientation[1] + cos(t)*orientation[0], 
                cos(t)*orientation[1] + sin(t)*orientation[0],
                orientation[2]]

3) Algebrische Lösung: Extrapolation aus den Messungen in Funktion des Gierens

Wichtiger Hinweis: Da die Rotation keine Vektoren skaliert, ist der Nenner des K-Faktors eine Konstante. Dann können wir es vereinfachen, indem wir die Länge des Orientierungsvektors vorberechnen.

M = 100 # distance
K = sqrt(M^2 / (orientation[0]^2 + orientation[01]^2 + orientation[1]^2))
PointOnWall = [ K * orientation[0] + laser[0],
                K * orientation[1] + laser[1],
                K * orientation[2] + laser[2]]

4) Algebrische Lösung: Daraus erhalten Sie mit zwei Lasern Wände.

Die beiden "PointOnWall" -Gleichungen sollten genügend Daten liefern, um das Gieren zu erhalten. Da ich weiß, dass dies eine (-1,0,0) Normale ist, kann ich aus den beiden Punkten zwei Ebenen finden:

5) Algebrische Lösung: Messen Sie die YAW.

Ein Flugzeug im anderen (Via XMaxima) haben wir:

def getYaw(position1, orientation1, measure1, position2, orientation2, measure2):
    length1 = length(orientation1)
    length2 = length(orientation2)
    k1 = measure1/length1
    k2 = measure2/length2
    numerator   = -k2*orientation2[0] + k1*orientation1[0] + position1[0] - position2[0]
    denominator = -k2*orientation2[1] + k1*orientation1[1] + position1[1] - position2[1]
    return atan(numerator/denominator)

Wie erwartet stören Roll & Pitch NICHT, da die Positionen und Ausrichtungen vorab gedreht werden.