クォータニオン微分を行うより良い方法をお探しですか

Question

私は四元数（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に保存された差分四元を持っているし、私はは単にこの差分四元追加することにより、四元数を更新します。

この方法は、剛体の動きを予測するのに適していますか？角速度の四元数を予測するには、より良い方法がありますか？これは機能しますが、期待される結果が得られません。

Answer 1

起こっている可能性のあることがいくつかあります。あなたはその四元数を再正規化することについて言及していません。あなたがそれをやっていないなら、間違ったことは間違いなく起こります。また、元の四元数に加算する前に、デルタ四元数のコンポーネントにdtが合格した時間を乗算したとは言いません。あなたの角速度が毎秒ラジアンですが、ほんの一瞬だけ前へ進んでいるならば、あまりにも遠くに歩きます。しかし、そうであっても、あなたは離散的な時間を踏んで、それが極微量であるとふりそそうとしているので、特にあなたのタイムステップや角速度が大きい場合、奇妙なことが起こります。

物理エンジンは、ODEは、それが微小ステップを取っていたかのようにその角速度から体の回転を更新するか、有限の大きさのステップを使用して更新するためのオプションを提供します。有限のステップははるかに正確ですが、いくつかのトリグが必要です。関数は少し遅くなります。関連するODEのソースコードはhere, lines 300-321で、デルタクォータニオンのコードはhere, line 310です。これは、ゼロ除算の問題を回避するためにあなたを可能にし、まだ非常に正確である

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

：sinc(x)がある

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; 

。

q

四元は次に身体の向きの既存の四元数表現に予め乗算されます。次に、再正規化します。

---

編集 - この式から来ている：

初期四元q0と時間の量dtする角速度wで回転させた後、その結果、最終的な四元q1を考えます。ここでは、角速度ベクトルを四元数に変更し、その四元数で最初の方向を回転させるだけです。四元数および角速度は両方とも軸 - 角度表現の変化である。 q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]：を[x,y,z]軸部周りthetaにより、その正規の向きからを回転させて本体は、その向きの次の四元数表現を有することになります。 [x,y,z]軸部の周囲にtheta/s回転ある 体は、角速度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を表す四元数です。

Answer 2

速度と精度方法インクリメントの間に非常に良好なトレードオフを有するmetod回転状態を表すquaterniomはベクトル角度dphi（の小さいベクトル増分だけ（すなわち、回転運動の微分方程式を積分）があります角速度omega刻印時間ステップdt）。

正確（と遅い）ベクトルによって四元の回転方法：

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のような遅い関数を計算しなければならないということです。代わりにnormの代わりにnorm^2を使用して表現されたテイラー展開によって、sinとcosを近似することで、小さな角度（かなりの時間ステップが妥当な場合）でかなりのスピード・ゲインと合理的な精度を得ることができます。ベクトルによって四元の回転のこの速い方法と同様に

：あなたは半分O角を行うことによって、精度を向上させることができ

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

---

注：あなただけ（ルンゲクッタのような）verletよりも、より洗練された積分スキームを使用する場合は、おそらくかなり新しい（更新）四元数よりも、四元の差が必要になります。

これは、それが小さな角度とするためにapproximativelly ca ~ 1あるca（半角の余弦）によって、古い（更新されていない）四元数の乗算と解釈される可能性がここに

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; 

コードで表示することが可能です残りの部分を追加する（いくつかの相互作用）。だから、単純に差分：小さな角度、時には無視することができた（基本的にはそれだけでquternionを再正規化）のための

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。

Answer 3

「指数マップ」から四元数への簡単な変換。 （角速度に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()); 
}