Question

我有一个旋转四元数，想要提取围绕向上轴（偏航）的旋转角度。我正在使用XNA，据我所知，没有内置功能。这样做的最佳方式是什么？

感谢您的帮助， Venatu

Answer 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}。

Answer 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旋转。对于其他旋转指令，必须使用外部和适当的欧拉旋转其他转换。

Answer 3

Conversion Quaternion to Euler

我希望你知道偏航，俯仰和滚转不利于任意旋转。欧拉角受到奇点（参见上述链接）和不稳定性的影响。请看David Sachs演讲的38:25

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

祝你好运！

Answer 4

注意：我已经针对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];

Answer 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);

从四元数中提取偏航

5 个答案: