quaternions - 寻找更好的方法来进行四元数微分

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-四元数分量乘以已经过去的时间量 dt在将它们添加到原始四元数之前。如果您的角速度以每秒弧度为单位，但您只向前迈进了几分之一秒，那么您将走得太远。然而，即便如此，由于您正在经历一段离散的时间并试图假装它是无穷小的，奇怪的事情会发生，特别是如果您的时间步长或角速度很大。

物理引擎 ODE 提供了从角速度更新物体旋转的选项，就好像它采取了无穷小的步长，或者使用有限大小的步长进行更新。有限步更准确，但涉及一些触发。功能，所以有点慢。相关ODE源码可见here, lines 300-321 , 用代码查找 delta-四元数 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然后预先乘以 body 方向的现有四元数表示。然后，重新规范化。

编辑 - 这个公式来自哪里:

考虑初始四元数 q0和最后的四元数 q1以角速度旋转后的结果 w为 dt多少时间。我们在这里所做的就是将角速度矢量更改为四元数，然后通过该四元数旋转第一个方向。四元数和角速度都是轴角表示的变化。一个 body 是 旋转 从其规范方向 theta绕单位轴 [x,y,z]将有以下四元数表示其方向: q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z] .
一个 body 是 旋转 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的旋转。