Ich muss den Winkel zwischen zwei Sätze von Rollen und Gieren Winkel finden

Question

Ich habe einen Sensor an einem Bohrer befestigt. Der Sensor gibt die Ausrichtung in Fahrtrichtung und Steigung aus. Von dem, was ich sagen kann, sind dies intrinsische Rotationen in dieser Reihenfolge. Die Y-Achse des Sensors ist parallel zur Längsachse des Bohrers. Ich möchte eine Reihe von Ausgaben von dem Sensor nehmen und die maximale Änderung der Orientierung von der endgültigen Orientierung finden.

Da sich der Bohrer um die Nickachse dreht, glaube ich, dass er vernachlässigt werden kann.

Mein erster Gedanke wäre, zu versuchen Position und Rolle zu Einheitsvektoren zu konvertieren Tonhöhe unter der Annahme, 0 ist Sobald ich ihre Vektoren haben, rufen sie v und vf, der Winkel zwischen ihnen würde

Θ = arccos sein (v. vf)

Es sollte dann ziemlich geradlinig sein, Python Θ für einen gegebenen Satz von Orientierungen berechnen zu lassen und den größten herauszuziehen.

Meine Frage ist, gibt es eine einfachere Möglichkeit, dies mit Python zu tun, und wenn nicht, was ist der effizienteste Weg, um diese Eigenrotationen in Einheitsvektoren zu konvertieren.

Answer 1

Ich denke, dass sphärische Trigonometrie Ihnen helfen wird. https://en.wikipedia.org/wiki/Spherical_trigonometry https://en.wikipedia.org/wiki/Haversine_formula#The_law_of_haversines

Wenn ich Sie richtig verstanden habe, sollten die folgenden Funktionen der Trick:

double AngleBetweenRollPitch(double roll1, double pitch1, double roll2, double pitch2) { 
    return SphericalSideFromSidesAndIncludedAngle(pitch1, pitch2, roll2 - roll1); 
} 
double SphericalSideFromSidesAndIncludedAngle(double b, double c, double alpha) { /* the Law of haversines */ 
    return archaversin(haversin(b - c) + sin(b) * sin(c) * haversin(alpha)); 
} 
double SphericalAngleBetweenTwoLongLats(double long0, double lat0, double long1, double lat1) { 
    return archaversin(haversin(lat1 - lat0) + cos(lat0) * cos(lat1) * haversin(long1 - long0)); 
} 
double haversin(double x) { 
    x = sin(x * .5); 
    x *= x; 
    return x; 
} 
double archaversin(double x) { 
    if  (x >= 1.) x = M_PI * .5; 
    else if (x <= 0.) x = 0.; 
    else    x = asin(sqrt(x)) * 2.; 
    return x; 
} 

Die beiden Plätze sind die Seiten des sphärischen Dreiecks, und die Delta-Rolle ist der Winkel zwischen ihnen. Um die verbleibende Seite zu berechnen, verwenden wir das Gesetz der Haversines. Ich habe auch eine Lösung für eine lange Parametrisierung der Winkel aufgenommen, falls Sie die Orientierungen stattdessen mit einer Gier-Steigung angeben.

Answer 2

Angenommen, u(1), u(2), ..., u(m), v sind alle Einheitsvektoren. Sie möchten i so bestimmen, dass der Winkel zwischen u(i) und v maximiert ist. Dies entspricht dem Auffinden des i, so dass np.dot(u(i), v) minimiert ist. Also, wenn Sie eine Matrix U haben, wo die Zeilen die u(i) sind, können Sie i = np.argmin(np.dot(U, v)) einfach tun, um die i zu finden, die den Winkel zwischen u(i) und v maximiert hat.