Auf der Suche nach einer besseren Methode, um Quaternion Differentiaton durchzuführen

Question

Ich habe ein Quaternion (4x1) und einen Winkelgeschwindigkeitsvektor (3x1) und ich nenne eine Funktion, um das Differentialquartion zu berechnen, wie in diesem erläutert Netz. Der Code sieht so aus:

    float wx = w.at<float>(0);
float wy = w.at<float>(1);
float wz = w.at<float>(2);
float qw = q.at<float>(3); //scalar component 
float qx = q.at<float>(0);
float qy = q.at<float>(1);
float qz = q.at<float>(2);

q.at<float>(0) = 0.5f * (wx*qw + wy*qz - wz*qy);    // qdiffx
q.at<float>(1) = 0.5f * (wy*qw + wz*qx - wx*qz);    // qdiffy
q.at<float>(2) = 0.5f * (wz*qw + wx*qy - wy*qx);    // qdiffz
q.at<float>(3) = -0.5f * (wx*qx + wy*qy + wz*qz);   // qdiffw

Also habe ich jetzt die differentielle Quaternion in Q und dann die Quaternion von aktualisiert Einfach hinzufügen Diese differentielle Quaternion.

Ist diese Methode geeignet, um die Bewegung starrer Objekte vorherzusagen, oder gibt es eine bessere Methode zur Vorhersage von Quaternion mit Winkelgeschwindigkeit? Dies funktioniert, aber ich bekomme nicht die erwarteten Ergebnisse.

Answer 1

Es gibt ein paar Dinge, die möglicherweise vor sich gehen. Sie erwähnen nicht, dass diese Quaternion neu ist. Schlechte Dinge werden definitiv passieren, wenn Sie das nicht tun. Sie sagen auch nicht, dass Sie die Delta-Quaternion-Komponenten mit der Zeit, die verabschiedet wurde, multiplizieren dt Bevor Sie sie zum ursprünglichen Quaternion hinzufügen. Wenn Ihre Winkelgeschwindigkeit in Radiant pro Sekunde ist, Sie aber nur um einen Bruchteil einer Sekunde nach vorne treten, werden Sie zu weit gehen. Trotzdem, da Sie eine diskrete Zeit und versuchen, so zu tun, als ob es infinitesimal ist, werden seltsame Dinge passieren, insbesondere wenn Ihre Zeitschritte oder Winkelgeschwindigkeit groß ist.

Die Physik -Engine ODE bietet die Möglichkeit, die Drehung eines Körpers aus seiner Winkelgeschwindigkeit zu aktualisieren, als ob sie einen infinitesimalen Schritt unternommen hätte oder mit einem endlich großen Schritt aktualisiert würde. Der endliche Schritt ist viel genauer, beinhaltet aber etwas Trig. Funktionen und so ist es etwas langsamer. Der relevante ODE -Quellcode ist zu sehen Hier, Zeilen 300-321, mit Code findet das Delta-Quaternion Hier, Zeile 310.

float wMag = sqrt(wx*wx + wy*wy + wz*wz);
float theta = 0.5f*wMag*dt;
q[0] = cos(theta);  // Scalar component
float s = sinc(theta)*0.5f*dt;
q[1] = wx * s; 
q[2] = wy * s;
q[3] = wz * s;

Wo sinc(x) ist:

if (fabs(x) < 1.0e-4) return (1.0) - x*x*(0.166666666666666666667);
else return sin(x)/x;

Auf diese Weise können Sie das Problem der Divide-by-Null vermeiden und ist immer noch sehr präzise.

Das Quaternion q wird dann auf die vorhandene Quaterniondarstellung der Ausrichtung des Körpers vorgefertigt. Dann normieren Sie neu.

---

Bearbeiten-Woher diese Formel stammt:

Betrachten Sie die erste Quaternion q0 und letzte Quaternion q1 welche nach dem Drehen mit Winkelgeschwindigkeit resultiert w zum dt Zeitraum. Alles, was wir hier tun, ist, den Winkelgeschwindigkeitsvektor in ein Quaternion zu verwandeln und dann die erste Ausrichtung durch dieses Quaternion zu verwandeln. Sowohl Quaternionen als auch Winkelgeschwindigkeiten sind Variationen der Achsenwinkeldarstellung. Ein Körper, der ist gedreht von seiner kanonischen Orientierung durch theta um die Einheitsachse [x,y,z] wird die folgende Quaternion -Darstellung seiner Orientierung haben: q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]. Ein Körper, der ist rotieren theta/s um die Einheitsachse [x,y,z] wird Winkelgeschwindigkeit haben w=[theta*x theta*y theta*z]. Um zu entscheiden, wie viel Rotation übertrifft dt Sekunden extrahieren wir zunächst die Größe der Winkelgeschwindigkeit: theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2). Dann finden wir den tatsächlichen Winkel durch Multiplizieren mit dt (und gleichzeitig durch 2 teilen, um dies in ein Quaternion zu verwandeln). Da müssen wir die Achse normalisieren [x y z], wir teilen auch durch theta. Dort das sinc(theta) Teil kommt von. (seit theta hat ein Extra 0.5*dt In der Größe multiplizieren wir das wieder aus). Das sinc(x) Funktion verwendet nur die Taylor -Serie -Näherung der Funktion, wenn x ist klein, weil es numerisch stabil und genauer ist. Die Fähigkeit, diese praktische Funktion zu verwenden, ist der Grund, warum wir uns nicht nur durch die tatsächliche Größe geteilt haben wMag. Körper, die sich nicht sehr schnell drehen, haben sehr kleine Winkelgeschwindigkeiten. Da wir erwarten, dass dies ziemlich häufig ist, brauchen wir eine stabile Lösung. Was wir am Ende haben, ist eine Quaternion, die einen einzelnen Schrittzeitschritt darstellt dt der Rotation.

Answer 2

Es gibt ein Metod mit einem sehr guten Kompromiss zwischen Geschwindigkeit und Genauigkeit wie man Inkrementieren Sie ein Quaterniom, das den Rotationszustand darstellt (dh die Differentialgleichung der Rotationsbewegung integrieren) durch kleine Vektorinkrements des Winkels dphi (Das ist Vektorwinkelgeschwindigkeit omega Mulptipliad nach Zeitschritt dt ).

Das Exakte (und langsame) Rotationsmethode von Quaternion durch Vektor:

void rotate_quaternion_by_vector_vec ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;
  double norm = Math.sqrt( r2 );

  double halfAngle = norm * 0.5d;
  double sa = Math.sin( halfAngle )/norm; // we normalize it here to save multiplications
  double ca = Math.cos( halfAngle );
  x*=sa; y*=sa; z*=sa;  

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;
}

Das Problem ist, dass Sie langsame Funktionen berechnen müssen wie cos, sin, sqrt. Stattdessen können Sie einen erheblichen Geschwindigkeitsgewinn und eine angemessene Genauigkeit für kleine Winkel erhalten (was der Fall ist, wenn der Zeitschritt Ihrer Simulation gering ist), indem Sie sich annähern sin und cos durch Taylor -Expansion, die gerade verwendet wird norm^2 Anstatt von norm.

So was Schnelle Drehmethode von Quaternion durch Vektor:

void rotate_quaternion_by_vector_Fast ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;

  // derived from second order taylor expansion
  // often this is accuracy is sufficient
  final double c3 = 1.0d/(6 * 2*2*2 )      ; // evaulated in compile time
  final double c2 = 1.0d/(2 * 2*2)         ; // evaulated in compile time
  double sa =    0.5d - c3*r2              ; 
  double ca =    1    - c2*r2              ; 

  x*=sa;
  y*=sa;
  z*=sa;

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

}

Sie können die Genauigkeit erhöhen, indem Sie einen halben O -Winkel durchführen, der 5 weitere Multiplikationen:

  final double c3 = 1.0d/( 6.0 *4*4*4  ) ; // evaulated in compile time
  final double c2 = 1.0d/( 2.0 *4*4    ) ; // evaulated in compile time
  double sa_ =    0.25d - c3*r2          ;  
  double ca_ =    1     - c2*r2          ;  
  double ca  = ca_*ca_ - sa_*sa_*r2      ;
  double sa  = 2*ca_*sa_                 ;

oder noch genauer durch einen anderen Spliting -Winkel auf die Hälfte:

  final double c3 = 1.0d/( 6 *8*8*8 ); // evaulated in compile time
  final double c2 = 1.0d/( 2 *8*8   ); // evaulated in compile time
  double sa = (  0.125d - c3*r2 )      ;
  double ca =    1      - c2*r2        ;
  double ca_ = ca*ca - sa*sa*r2;
  double sa_ = 2*ca*sa;
         ca = ca_*ca_ - sa_*sa_*r2;
         sa = 2*ca_*sa_;

---

HINWEIS: Wenn Sie ein ausgefeilteres Integrationsschema verwenden als nur Veret (wie Rang-Kutta) Sie würde wahrscheinlich ein Differential des Quaternion brauchen, anstatt das neue (aktualisierte) Quaternion.

Dies ist im Code hier möglich

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

Es könnte als Multiplikation des alten (nicht aktualisierten) Quaternion von interpretiert werden ca (Cosinus des halben Winkels), der ungefähr ca ~ 1 für kleine Winkel und Hinzufügen des Restes (einige Kreuzwechselwirkungen). Also das Differential einfach:

  dq[0] =  x*qw + y*qz - z*qy + (1-ca)*qx;
  dq[1] = -x*qz + y*qw + z*qx + (1-ca)*qy;
  dq[2] =  x*qy - y*qx + z*qw + (1-ca)*qz;
  dq[3] = -x*qx - y*qy - z*qz + (1-ca)*qw;

wo Begriff ( 1 - ca ) ~ 0 Für kleine Winkel und manchmal vernachlässigt werden (im Grunde genommen renormiert es nur das QUTERNion).

Answer 3

Einfache Konvertierung von "Exponential Map" in Quaternion. (Exponentielle Karte, die der Winkelgeschwindigkeit multipliziert mit Deltatime entspricht). Ergebnis Quaternion ist die Delta -Rotation für bestandene Deltatime- und Winkelgeschwindigkeit "W".

Vector3 em = w*deltaTime; // exponential map
{
Quaternion q;
Vector3 ha = em/2.0; // vector of half angle

double l = ha.norm();
if(l > 0)
{
    double ss = sin(l)/l;
    q = Quaternion(cos(l), ha.x()*ss, ha.y()*ss, ha.z()*ss);
}else{
    // if l is too small, its norm can be equal 0 but norm_inf greater 0
    q = Quaternion(1.0, ha.x(), ha.y(), ha.z());
}