从四元数中提取偏航

Posted 2023-04-12

【中文标题】从四元数中提取偏航

【英文标题】：Extracting Yaw from a Quaternion

【发布时间】：2011-08-12 13:32:44

【问题描述】：

我有一个旋转四元数，想提取关于上轴（偏航）的旋转角度。我正在使用 XNA，据我所知，没有内置功能。最好的方法是什么？

感谢您的帮助， 韦纳图

【参考方案1】：

旋转的四元数表示是轴和角度的变化。因此，如果您围绕轴 x、y、z 旋转 r 弧度，那么您的四元数 q 是：

q[0] = cos(r/2);
q[1] = sin(r/2)*x;
q[2] = sin(r/2)*y;
q[3] = sin(r/2)*z;

如果要创建仅围绕 y 轴旋转的四元数，请将 x 和 z 轴归零，然后重新-标准化四元数：

q[1] = 0;
q[3] = 0;
double mag = sqrt(q[0]*q[0] + q[2]*q[2]);
q[0] /= mag;
q[2] /= mag;

如果你想要得到的角度：

double ang = 2*acos(q[0]);

这假设存储了四元数表示：w,x,y,z。如果 q[0] 和 q[2] 都为零或接近零，则生成的四元数应为 1,0,0,0。

【讨论】：

这是否也适用于“仅”从四元数中提取音高（或滚动），例如通过将 x 和 y 设置为零。如果没有，为什么不呢？

@AlexanderPacha 它不太适合提取俯仰或滚动。原因是“偏航”通常被定义为围绕世界的“上”轴旋转。但是俯仰和滚动是相对于对象的内轴定义的。当飞行器俯仰时，它会围绕自己的机翼进行俯仰，而不管机翼与哪个世界轴对齐。由于四元数通常位于世界坐标中，因此需要额外的步骤将事物与对象本地参考系进行转换。

原谅我的愚蠢，但我这里不能格格不入，我有四元数矩阵wxyz，R的这个值在哪里？

如果 r 为负数则失败

注：如果你只是对ang感兴趣，那么不用计算2*acos(q0/sqrt(q0*q0+q2*q2))，你可以直接计算atan(q[2],q[0])*2，它也已经归一化为[-pi, pi]。

【参考方案2】：

给定一个四元数 q，您可以像这样计算横滚、俯仰和偏航：

var yaw = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
var pitch = asin(-2.0*(q.x*q.z - q.w*q.y));
var roll = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);

这应该适合 xyz 顺序的内在 tait-bryan 旋转。对于其他旋转顺序、外部和适当欧拉旋转，必须使用其他转换。

【讨论】：

我只是仔细检查了一遍，但这实际上是我将四元数值转换为 Yaw、Pitch、Roll 以在 Maya 中使用的工作代码。首先尝试了普遍接受的方法，但它没有按预期工作！您最好先尝试一下，然后再投反对票……

嗯，它现在给我返回了一些奇怪的结果：如果我稍微偏航我的相机，只有偏航改变，这是我所期望的。如果我再推销它，每个组件都会返回变化，这是出乎意料的。例如。如果我偏航和倾斜我的凸轮 0.3 弧度，我希望这会为 botch 和 0 滚动返回 0.3，但是每个组件对我来说都有一个奇怪的数字；S

有些可能使用不同的分配顺序：滚动、偏航、俯仰 - 取决于坐标系。

我在这里发布了我的具体问题：***.com/questions/23310299/… 终止“欧拉角”让我非常困惑，直到***澄清它我正在搜索“Tait-Bryan”角度（XYZ 旋转，而不是 XYX） .

等等，“飞机主轴”又是什么不同的东西？我以为是一样的。上帝......谁创造了这些术语......

【参考方案3】：

注意：我已经针对Wikipedia's equations 加上Pixhawk's documentation 验证了以下代码，并且它是正确的。

如果您正在使用无人机/航空，以下是代码（直接取自 DJI SDK）。这里 q0, q1, q2, q3 分别对应四元数的 w,x,y,z 分量。另请注意，偏航、俯仰、横滚在某些文献中可能分别称为航向、姿态和坡度。

float roll  = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
float yaw   = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));

如果您需要计算所有 3 个，则可以使用以下函数避免重新计算常用项：

//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152
EulerianAngle Flight::toEulerianAngle(QuaternionData data)

    EulerianAngle ans;

    double q2sqr = data.q2 * data.q2;
    double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0;
    double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3);
    double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2);
    double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1);
    double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0;

    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;

    ans.pitch = asin(t2);
    ans.roll = atan2(t3, t4);
    ans.yaw = atan2(t1, t0);

    return ans;


QuaternionData Flight::toQuaternion(EulerianAngle data)

    QuaternionData ans;
    double t0 = cos(data.yaw * 0.5);
    double t1 = sin(data.yaw * 0.5);
    double t2 = cos(data.roll * 0.5);
    double t3 = sin(data.roll * 0.5);
    double t4 = cos(data.pitch * 0.5);
    double t5 = sin(data.pitch * 0.5);

    ans.q0 = t2 * t4 * t0 + t3 * t5 * t1;
    ans.q1 = t3 * t4 * t0 - t2 * t5 * t1;
    ans.q2 = t2 * t5 * t0 + t3 * t4 * t1;
    ans.q3 = t2 * t4 * t1 - t3 * t5 * t0;
    return ans;

特征库注意事项

如果你使用的是 Eigen 库，它有另一种方式来进行这种转换，但是，这可能不如上面直接代码优化：

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];

【讨论】：

@thewhiteambit - 这不是“你的”答案。上面的代码来自 DJI SDK，正如我所提到的，它应该让人们在使用时更有信心。此外，如果您想计算所有 3，我还添加了避免再次执行相同点积的代码版本。投反对票是非常糟糕的，因为您认为您拥有一个数学公式，而且您没有编写有效的代码首先，这非常重要，因为这些计算需要每秒进行 100 次。

@thewhiteambit 我写这个答案的原因是为了帮助其他人为我自己编写的代码节省一些时间。 toEularianAngle() 确实消除了一个冗余（q2sqr），它更干净，并且与您声称的“无耻副本”相去甚远。 toQuternion() 使它更完整。

由于有人删除了我的一半答案，我删除了剩下的。伟大的工作消除了非常单一的乘法。

老兄，我不知道你在说什么。据我所知，只有您可以删除自己的答案。其他人不能只删除其他人的答案。那将是非常可怕的。我还向 DJI 的人提出了问题，看看他们的代码是否正确：github.com/dji-sdk/Onboard-SDK/issues/59#issuecomment-225851991。这就是你解决问题的方式，而不是通过创建和保护你的“地盘”。无论如何，随便。祝你好运。

【参考方案4】：

Conversion Quaternion to Euler

我希望您知道偏航、俯仰和滚动不适合任意旋转。欧拉角存在奇点（参见上面的链接）和不稳定性。看看 38:25 的 David Sachs 的演讲

http://www.youtube.com/watch?v=C7JQ7Rpwn2k

祝你好运！

【讨论】：

Euler Angels 并不总是有这个 Gimbal-Lock 问题，如果你正确处理它们（这意味着在第一种情况下不使用视频中的变通方法 - 但正确处理乘法顺序） . Euler 甚至可以具有 >360° 的旋转等优点。但在大多数情况下，我也更喜欢四元数。

我正在使用在使用欧拉角时不会导致奇点的实现...例如，使用 asin 提取会导致万向节锁定...我目前正在使用 3 atan2 函数可以避免这种情况。

【参考方案5】：

四元数由两个分量组成：一个 3d 向量分量和一个标量分量。

四元数的向量分量描述了围绕每个轴的独立旋转，因此您只需将向量分量的 x 和 y 分量归零并保持 z 分量不变即可解决问题对于向量项：

// Don't modify qz
double qx = 0;
double qy = 0;  

标量项表示旋转的幅度。对于一个单位四元数（例如用于表示姿态的四元数），整个四元数的大小必须为 1。因此，标量项可以通过以下方式求解：

double qw = sqrt(1 - qx*qx - qy*qy - qz*qz);

由于 qx 和 qy 为零，因此标量分量由下式给出

double qw = sqrt(1 - qz*qz); 

因此，表示偏航的完整四元数由下式给出

double qx = 0;
double qy = 0;
// Don't modify qz
double qw = sqrt(1 - qz*qz);

【参考方案6】：

从四元数到偏航、俯仰和滚动的转换取决于用于定义四元数和偏航、俯仰和滚动的约定。对于给定的约定，有许多“几乎正确”的变换适用于大多数角度，但只有一个真正正确的变换适用于所有角度，包括南极和北极，其中“几乎正确”的变换产生万向节锁定（虚假翻转和旋转）。

有关更多信息，请参阅本教程：

https://youtu.be/k5i-vE5rZR0

【讨论】：

如果您提供的链接中有关键概念或关键元素，请考虑在您的答案中进行总结（除非它已经在您的第一段中）。不要忘记链接或视频是永恒的！