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

可能会发生一些事情。您没有提到重新规范化四元数。如果你不这样做，肯定会发生坏事。您也不要说在将delta-quaternion组件添加到原始四元数之前，将delta-quaternion组件乘以已经过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旋转后产生。我们在这里所做的就是将角速度矢量改变为四元数，然后通过该四元数旋转第一个方向。四元数和角速度都是轴角度表示的变化。在单位轴theta周围[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。

Answer 2

在速度和准确度之间有一个非常好的权衡的方法如何增加表示旋转状态的四元数（即，将旋转运动的微分方程积分）矢量增量角度dphi（矢量角速度omega mulptipliad按时间步长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这样的慢函数。相反，通过使用{sin和cos逼近泰勒展开，您可以获得相当大的速度增益和小角度的合理精度（如果模拟的时间步长合理的话就是这种情况） 1}}而不是norm^2。

像这样按矢量旋转四元数的快速方法：

norm

你可以通过半角度来增加准确度，再增加5次：

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;

}

甚至更精确的另一个分裂角度到一半：

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

注意：如果你使用更复杂的集成方案而不仅仅是verlet（如Runge-Kutta），你可能需要一个四元数差异，而不是新的（更新）四元数。

这可以在这里的代码中看到

  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;适用于小角度，有时可能被忽略（基本上它只是将四元数重新规范化）。

Answer 3

从＆＃34;指数地图＆＃34;简单转换到四元数。（指数映射等于角速度乘以deltaTime）。结果四元数是delta delta旋转，用于传递deltaTime和角速度＆＃34; w＆＃34;。

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

寻找更好的方法来进行四元数分化

3 个答案: