Question

我有以下matlab代码，它给出了Quaternion的旋转矩阵。这样做的人说他使用了一些图书馆，它最初来自这个旋转矩阵

根据代码的旋转矩阵看起来像

对角线元素和符号发生变化。可以向我解释两者之间差异的数学或条件。

Q0 = Quat(1);
Q1 = Quat(2);
Q2 = Quat(3);
Q3 = Quat(4);

%% set f2q to 2*q0 and calculate products
f2q = 2 * Q0;
f2q0q0 = f2q * Q0;
f2q0q1 = f2q * Q1;
f2q0q2 = f2q * Q2;
f2q0q3 = f2q * Q3;

 %% set f2q to 2*q1 and calculate products
 f2q = 2 * Q1;
 f2q1q1 = f2q * Q1;
 f2q1q2 = f2q * Q2;
 f2q1q3 = f2q * Q3;

 %% set f2q to 2*q2 and calculate products
 f2q = 2 * Q2;
 f2q2q2 = f2q * Q2;
 f2q2q3 = f2q * Q3;
 f2q3q3 = 2 * Q3 * Q3;

 %% calculate the rotation matrix assuming the quaternion is normalized
 R(1, 1) = f2q0q0 + f2q1q1 - 1;
 R(1, 2) = f2q1q2 + f2q0q3;
 R(1, 3) = f2q1q3 - f2q0q2;
 R(2, 1) = f2q1q2 - f2q0q3;
 R(2, 2) = f2q0q0 + f2q2q2 - 1;
 R(2, 3) = f2q2q3 + f2q0q1;
 R(3, 1) = f2q1q3 + f2q0q2;
 R(3, 2) = f2q2q3 - f2q0q1;
 R(3, 3) = f2q0q0 + f2q3q3 - 1;

也...

旋转矩阵用于查找同样处理万向节锁定的欧拉角度

 %%calculate the pitch angle -90.0 <= Theta <= 90.0 deg
        pitchdeg = asin(-R(1, 3)) * (180 / pi);

        %% calculate the roll angle range -180.0 <= Phi < 180.0 deg
        rolldeg = atan2(R(2, 3), R(3, 3)) * (180 / pi);

        %% map +180 roll onto the functionally equivalent -180 deg roll
        if (rolldeg == 180)

            rolldeg = -180;
        end

        %% calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
        if (pitchdeg == 90)

            %% vertical upwards gimbal lock case
            yawdeg = (atan2(R(3, 2), R(2, 2)) * (180 / pi)) + rolldeg;
        elseif (pitchdeg == -90)

            %% vertical downwards gimbal lock case
            yawdeg = (Math.Atan2(-R(3, 2), R(2, 2)) * (180 / pi)) - rolldeg;

        else

            %% general case
            yawdeg = atan2(R(1, 2), R(1, 1)) * (180 / pi);
        end

        %% map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
        if (yawdeg < 0)

            yawdeg = yawdeg + 360;
        end

        %% check for rounding errors mapping small negative angle to 360 deg
        if (yawdeg >= 360)

            yawdeg = 0;
        end

        Euler.Roll = rolldeg;
        Euler.Pitch = pitchdeg;
        Euler.Yaw = yawdeg;

谷歌搜索公式从四元数转换为欧拉角给了我一个我无法附加的公式，因为我没有足够的声誉，但你可以在维基找到它Conversion_between_quaternions_and_Euler_angles

与旋转矩阵的对角线相比，这些术语是不同的。另外，云台锁的补偿背后有数学吗？

IMU使用东北向下系统

我正在使用欧拉角来显示陀螺仪。任何形式的指导都很明显

一些四元数代码和万向节锁后面的数学

0 个答案: