尋找一個更好的方法來做四元數的差分

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，代碼查找delta-quaternion here, line 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和最終四元q1與角速度w旋轉爲dt量的時間之後產生的。我們在這裏所做的就是將角速度矢量改變成四元數，然後用四元數旋轉第一個方向。四元數和角速度都是軸角表示的變化。 正文圍繞單位軸[x,y,z]圍繞theta的標準方向旋轉將具有以下四元數的方向表示：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從它的規模，我們乘以返回）。當x很小時，sinc(x)函數只是使用函數的泰勒級數近似，因爲它在數值上是穩定的，而且更準確。使用這個方便的功能的能力是爲什麼我們不僅僅是除以實際量值wMag。旋轉速度不快的物體將具有非常小的角速度。由於我們預計這很常見，我們需要一個穩定的解決方案。我們最終得到的是一個四元數，表示旋轉的單步時間步dt。

Answer 2

有速度以及如何增量表示旋轉狀態一個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慢的函數。相反，如果通過taylor擴展來近似sin和cos，則只需使用norm^2而不是norm即可獲得相當可觀的速度增益和合理的小角度精度（這種情況下，如果模擬的時間步長合理小）。

像通過矢量四元數的旋轉這個快速方法：

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的（如龍格 - 庫塔），你可能會需要四元的差別，而不是新的（更新）四元數。

這是可能在這裏

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（半角的餘弦），這是approximativelly 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，有時甚至可以忽略不計（基本上它只是重新歸一化的quternion）。

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