logo标签列表

从四元数中提取偏航角

mathxnaquaternions
41

我有一个旋转四元数，想提取绕上方向轴（偏航）的旋转角度。我正在使用XNA，据我所知，它没有内置的函数可以实现这一点。如何最好地完成此操作？

谢谢任何帮助， Venatu

6个回答
46

四元数表示旋转是一种基于轴和角度的变化。如果您围绕轴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]都是0或接近于0，那么得到的四元数应该是{1,0,0,0}。

1
这个方法是否也适用于从四元数中“仅”提取俯仰（或翻滚）角，例如通过将x和y设置为零。如果不行，为什么？ - Alexander Pacha
12
对于提取俯仰角或滚转角的效果不是很好。原因是，'偏航'通常被定义为绕世界的'上'轴旋转。但是俯仰和滚转是相对于物体的内部轴定义的。当飞行器俯仰时，它围绕自己的机翼旋转，而不管机翼与世界轴的对齐方式如何。由于四元数通常是在世界坐标系中，因此需要额外的步骤将事物转换为/从对象本地参考系。 - JCooper
1
@Eric 当r为负数时，它仍然有效。绕x、y、z轴旋转-r与绕-x、-y、-z轴旋转r是相同的。如果你否定四元数的所有元素，你会得到相同的旋转。acos()总是返回一个正值。所以你会得到0 <= ang < 2*pi。但是如果你想要一个负角度（例如，如果0表示向前，+/-角度表示左/右转），你只需要这样做：if (ang > pi) ang -= 2*pi - JCooper
3
请注意，如果您只对ang感兴趣，那么您可以直接计算atan(q[2],q[0])*2，而不是计算2*acos(q0/sqrt(q0*q0+q2*q2))，这也已经归一化为[-pi, pi]。 - chtz
@JCooper 这个话题虽然很老，但仍然有意义。偏航的定义是绕着z轴在机体坐标系中的旋转，它是否与惯性坐标系中的z轴匹配取决于欧拉角的旋转顺序（例如321，323等）。 您提供的解决方案给出了方位角 - 将航向矢量投影到惯性XY平面上的结果。 - undefined
显示剩余3条评论
41

给定一个四元数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旋转。对于其他旋转顺序，需要使用外禀和proper-euler旋转进行其他转换。

3
我刚刚仔细检查了一下，这是我用于将四元数值转换为Maya中Yaw、Pitch、Roll的工作代码。我先尝试了通常被接受的方法，但结果并不如预期！在投票之前最好先尝试一下... - thewhiteambit
2
嗯，目前它给我返回了一些奇怪的结果：如果我稍微偏航我的相机，只有偏航会改变，这是我预期的。但如果我接着俯仰相机，每个返回的组件都会改变，这是意外的。例如，如果我将相机偏航和俯仰0.3弧度，我希望它返回botch为0.3和0滚动，但每个组件都给我一个奇怪的数字；S - Ray
2
一些人可能会根据坐标系使用不同的分配顺序：滚转、偏航、俯仰。 - thewhiteambit
2
我在这里发布了我的具体问题：https://dev59.com/DX_aa4cB1Zd3GeqPyxyU 直到我在维基百科上澄清之前，“欧拉角”这个术语让我非常困惑。我正在寻找“Tait-Bryan”角度（XYZ旋转，而不是XYX）。 - Ray
3
等等，“飞机主轴”又是另一回事？我以为它是一样的。天啊...谁编造这些术语... - Ray
显示剩余13条评论
19

注意：我已经验证了下面的代码，与维基百科的方程式和Pixhawk的文档相符，是正确的。

如果您正在处理无人机/航空领域，以下是该代码（直接从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));

如果你需要计算全部三个，则可以使用以下函数来避免重复计算公共项：

//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库的注释

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

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];
@thewhiteambit - 这不是“你”的答案。如我所提到的，上面的代码来自DJI SDK，应该能够让人们更有信心使用。此外，我添加了代码的版本，如果您想要计算全部三个则可以避免再次进行相同的点积。仅因为您认为自己拥有一个数学公式并且事实上您没有编写有效的代码，所以对其进行贬低投票非常不好。首先，这非常重要，因为这些计算每秒需要进行100多次。 - Shital Shah
2
@thewhiteambit 我写这篇答案的原因是为了帮助其他人节省一些时间，以避免他们自己编写代码。toEularianAngle（）确实消除了一个冗余项（q2sqr），使代码更简洁，并且远非您所声称的“无耻复制”。而toQuternion()则使其更加完整。 - Shital Shah
由于有人删除了我的一半答案，我删除了剩下的答案。太好了，消除了那个单一的乘法。 - thewhiteambit
兄弟，我不知道你在说什么。据我所知，只有你可以删除自己的答案。别人不能随便删除别人的答案，那太可怕了。我还向DJI的开发人员提交了问题，以查看他们的代码是否正确：https://github.com/dji-sdk/Onboard-SDK/issues/59#issuecomment-225851991。这就是解决问题的方法，而不是创建和保护自己的“地盘”。无论如何，祝你好运。 - Shital Shah
4
1
欧拉角并不总是存在万向锁问题，如果你正确处理它们的话（这意味着首先不使用像视频中那样的解决方法，而是正确处理乘法顺序），欧拉角甚至可以具有大于360°的旋转优势。但在大多数情况下，我仍然更喜欢四元数。 - thewhiteambit
1
我正在处理不会在使用欧拉角时导致奇异性的实现...例如，使用asin进行提取会导致万向节锁定...我目前正在使用3个atan2函数的实现来避免这种情况。 - Tcll
2

四元数由两个部分组成：一个三维向量部分和一个标量部分。

四元数的向量部分描述了每个轴独立旋转的角度，因此只需要将向量部分的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);
0

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

请参阅此教程以获取更多信息：

https://youtu.be/k5i-vE5rZR0

如果您提供的链接中有关键概念或关键元素，请考虑在回答中进行总结（除非已经在第一段中）。不要忘记链接或视频并非永久存在！ - XouDo
网页内容由stack overflow 提供, 点击上面的
可以查看英文原文，
原文链接