4

我有一个四元数（4x1）和一个角速度向量（3x1），我调用一个函数来计算微分四元数，如本网站所述。代码如下所示：

    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

有几件事可能正在发生。您没有提到重新规范化该四元数。如果你不这样做，坏事肯定会发生。您也没有说将 delta-quaternion 分量乘以在dt将它们添加到原始四元数之前经过的时间量。如果您的角速度以每秒弧度为单位，但您只向前迈出了几分之一秒，您就会走得太远。然而，即便如此，由于您正在经历一段离散的时间并试图假装它是无穷小的，所以会发生奇怪的事情，尤其是当您的时间步长或角速度很大时。

物理引擎 ODE 提供了从角速度更新物体旋转的选项，就好像它正在采取无限小的步长一样，或者使用有限大小的步长进行更新。有限步更准确，但涉及一些三角函数。功能，所以有点慢。相关的 ODE 源代码可以在这里看到，第 300-321 行，代码在这里找到 delta-quaternion ，第 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^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;

}

您可以通过做半 o 角来提高精度，再乘以 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 更复杂的集成方案（如 Runge-Kutta ），您可能需要四元数的微分，而不是新的（更新的）四元数。

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

  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 回答