四元数旋转忽略偏航

Question

我正在使用四元数和一个 LSM6DSO32 捕获陀螺仪 + 加速器。所以我融合了来自俘虏我的数据，然后我有了一个四元数，一切正常。

现在我想检测我的四元数是否围绕初始四元数旋转超过 90°，这是我所做的，首先我有 q1 是我的初始四元数，q2是来自我的融合数据的四元数，用于检测 q2 是否从 q1 旋转超过 90° 我这样做：

q_conj = conjugateQuaternion(q2);
q_mulitply = multiplyQuaternion(q1, q_conj);

float angle = (2 * acos(q_mulitply.element.w)) * RAD_TO_DEG;

if(angle > 90.0f)
   do something

这很好用我可以检测到 q2 是否旋转超过 90°。但我的“问题”是我也检测到偏航 90° 旋转，我不想在我的测试中集成偏航。是否可以在不修改 w、x 和 y 分量的情况下使偏航（我的四元数中的 z 分量）无效？

我最后的 objective 是检测超过 90° 的旋转但不关心偏航，我不想使用欧拉角因为我想避免万向节锁定

编辑：我想计算 q1 和 q2 之间的幅度，而不关心偏航

Answer 1

四元数的“yaw”在q_roll * q_pitch * q_yaw组成的四元数中一般表示q_yaw。因此没有偏航的四元数将是 q_roll * q_pitch。如果手头有俯仰和滚动值，最简单的方法就是在忽略 q_yaw.

的同时重建四元数

但是，如果我们真的要处理一个完全任意的四元数，我们将不得不从 q_roll * q_pitch * q_yaw 到 q_roll * q_pitch。

我们可以通过在等式末尾附加相反的变换来实现：q_roll * q_pitch * q_yaw * conj(q_yaw)。只要我们只处理归一化四元数，q_yaw * conj(q_yaw) 就保证是恒等四元数。由于我们正在处理旋转，因此这是一个足够安全的假设。

换句话说，去除四元数的“偏航”将涉及：

求四元数的偏航
将四元数乘以它的共轭。

所以我们需要找到四元数的偏航角，也就是前向矢量被那个四元数绕上轴旋转了多少。

最简单的方法就是尝试一下，然后衡量结果：

通过四元数变换参考前向向量（在地平面上）
把它投影回地平面。
获取此投影与参考向量之间的角度。
围绕上轴形成一个“偏航”四元数。

将所有这些放在一起，假设您使用的是 Y=up 坐标系，它大致如下所示：

quat remove_yaw(quat q) {
  vec3 forward{0, 0, -1};
  vec3 up{0, 1, 0};

  vec3 transformed = q.rotate(forward);

  vec3 projected = transformed.project_on_plane(up);
  if( length(projected) < epsilon ) {
    // TODO: unsolvable, what should happen here?
  }

  float theta = acos(dot(normalize(projected), forward));

  quat yaw_quat = quat.from_axis_angle(up, theta);

  return multiply(q, conjugate(yaw_quat));
}

这显然可以简化一点。例如，轴角四元数的共轭与绕同一轴的负角四元数是一回事，我相信这里还有其他可能的简化。但是，我想尽可能清楚地说明原理。

当俯仰恰好为±90°时，也存在一个奇点。在这些情况下，偏航被万向节锁定，与滚动无法区分，所以你必须弄清楚当 length(projected) < epsilon.

时你想做什么

四元数旋转忽略偏航

Quaternion rotation ignoring yaw

c++

quaternions