4

クォータニオン（4x1）と角速度ベクトル（3x1）があり、このWebで説明されているように、微分クォータニオンを計算する関数を呼び出します。コードは次のようになります。

    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

これで、差分クォータニオンがqに格納され、この差分クォータニオンを追加するだけでクォータニオンが更新されます。

この方法は、剛体の動きを予測するのに適していますか、それとも角速度でクォータニオンを予測するためのより良い方法がありますか？これは機能しますが、期待した結果が得られません。

4

3 に答える 3

5

起こっているかもしれないいくつかのことがあります。そのクォータニオンを再正​​規化することについては言及していません。そうしないと、間違いなく悪いことが起こります。dtまた、デルタクォータニオンコンポーネントに、元のクォータニオンに追加する前に経過した時間を掛けるとは言いません。角速度がラジアン/秒であるが、ほんの一瞬だけ前進している場合は、踏み過ぎになります。ただし、それでも、離散的な時間をステップスルーし、それが微小であると偽ってみようとしているため、特にタイムステップや角速度が大きい場合は、奇妙なことが起こります。

物理エンジンであるODEは、物体の回転を角速度から微小なステップを踏んでいるかのように更新するか、有限サイズのステップを使用して更新するオプションを提供します。有限ステップははるかに正確ですが、いくつかのトリガーが含まれます。機能などが少し遅くなります。関連するODEソースコードはここの300〜321行目で、デルタクォータニオンを見つけるコードは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;

どこsinc(x)にありますか：

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

これにより、ゼロ除算の問題を回避でき、それでも非常に正確です。

次に、クォータニオンqは、体の向きの既存のクォータニオン表現に事前に乗算されます。次に、再正規化します。

編集-この式はどこから来ていますか：

角速度で一定時間回転した後に生じる初期クォータニオンq0と最終クォータニオンについて考えてみます。ここで行っているのは、角速度ベクトルをクォータニオンに変更し、そのクォータニオンによって最初の方向を回転させることだけです。クォータニオンと角速度はどちらも、軸-角度表現のバリエーションです。正規の方向から単位軸を中心に回転したボディには、その方向の次のクォータニオン表現があります。単位軸を中心に回転している物体の角速度はです。だから、どのくらいの回転が起こるかを決定するためにq1wdttheta[x,y,z]q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z] theta/s[x,y,z]w=[theta*x theta*y theta*z]dt秒、最初に角速度の大きさを抽出します：theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2)。次に、を掛けて実際の角度をdt求めます（同時に、これをクォータニオンに変換するのに便利なように2で割ります）。軸を正規化する必要があるため[x y z]、で除算しthetaます。そこからそのsinc(theta)部分が生まれます。（マグニチュードであることからtheta余分なものがあるので、それを乗算して元に戻します）。0.5*dtこの関数は、数値的に安定しており、より正確であるため、が小さいsinc(x)場合は関数のテイラー級数近似を使用しています。xこの便利な関数を使用できるので、実際の大きさで割っただけではありません。wMag。あまり速く回転していない物体は、角速度が非常に小さくなります。これはかなり一般的であると予想されるため、安定したソリューションが必要です。dt最終的には、回転のシングルステップタイムステップを表すクォータニオンになります。

于 2012-01-13T18:48:04.287 に答える
0

角度の小さなベクトル増分（時間ステップで多重化されたベクトル角速度）によって回転状態を表す四分位をインクリメントする方法（つまり、回転運動の微分方程式を統合する方法）には、速度と精度の間に非常に良いトレードオフの方法があります。dphiomegadt

ベクトルによるクォータニオンの正確な（そして遅い）回転方法：

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;
}

問題は、のような遅い関数を計算する必要があることですcos, sin, sqrt。sin代わりに、近似し、の代わりにをcos使用して表現されたテイラー展開によって、小さな角度でかなりの速度ゲインと妥当な精度を得ることができます（シミュレーションの時間ステップが妥当な場合）。norm^2norm

このように、ベクトルによるクォータニオンの高速回転方法：

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;

}

角度を半分にして、さらに5回乗算することで、精度を上げることができます。

  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_                 ;

または、他の分割角度を半分にするとさらに正確になります。

  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_;

注：ベレ（ルンゲクッタ法など）よりも高度な統合スキームを使用する場合は、新しい（更新された）クォータニオンではなく、クォータニオンの差分が必要になる可能性があります。

これはここのコードで見ることができます

  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;

caこれは、古い（更新されていない）クォータニオンに（半角のコサイン）を掛けたものと解釈できます。これは、おおよそca ~ 1小さな角度で、残りを追加します（いくつかの相互作用）。したがって、差分は単純に：

  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;

ここで( 1 - ca ) ~ 0、小さな角度の用語であり、無視されることもあります（基本的には、クターニオンを繰り込みます）。

于 2014-05-20T20:26:00.600 に答える
0

「指数写像」からクォータニオンへの単純な変換。（角速度にdeltaTimeを掛けたものに等しい指数マップ）。結果のクォータニオンは、渡されたdeltaTimeと角速度「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());
}
于 2014-05-21T08:55:42.860 に答える