从四元数中提取偏航

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

注意：我已经根据维基百科的方程和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));

如果您需要计算所有 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];