2016-03-29 15 views
0

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.Ich muss den Winkel zwischen zwei Sätze von Rollen und Gieren Winkel 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.

+0

Haben Sie selbst einen Code ausprobiert? – Deusdeorum

+0

@Hugo - Noch nicht. Ich wollte die Mathematik aussortieren, bevor ich mit der Programmierung begann. Auf diese Weise weiß ich, dass ich in die richtige Richtung arbeite. – Richard

+0

Ich glaube, ich kann die Einheitsvektoren wie folgt erhalten: x = cos (Gieren) * cos (rollen), y = sin (Gieren) * cos (rollen) und z = sin (rollen) – Richard

Antwort

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.

+0

Das scheint eine großartige Möglichkeit zu sein darüber zu gehen. Ich hoffe, Sie können etwas mehr über die lat-lange Charakterisierung erfahren. Ich denke, ich würde in einer Gier-Neigungs-Orientierung arbeiten und meine Längen wären meine Frambaden und meine Breiten wären meine Stellungen? – Richard

+0

nvm, die Lat-lange Formel hat perfekt funktioniert. Mit der Ausrichtung meines Sensors markiert er die Steigung des Bohrers als Rolle. Wenn also Rolle als meine Breite und Gieren als meine Länge verwendet wird, gibt es dieselbe Winkeldifferenz wie die Einheitsvektormethode aus, jedoch mit weniger Berechnungen. – Richard

0

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.