Math 从四元数到euler角和euler角的转换不正确

我正在将角度轴表示转换为Euler角度。我决定检查并确保我从转换中得到的欧拉角将返回到原始轴角。我打印出了值，但它们不匹配！我在这个网站上读到了和以及类似的转换问题

在下面的代码中，我从角度“angle”和轴（rx，ry，rz）开始，然后将其转换为四元数（q0，q1，q2，q3）。我将四元数转换为欧拉角（横摇、俯仰、偏航）。然后，为了检查它，我将（横滚、俯仰、偏航）转换回轴角，如cAngle和（cRx、cRy、cRz）。然后，我对（横滚、俯仰、偏航）进行一些边界检查，使数字保持在-pi和pi之间，然后打印出来。应该是cAngle=角度和（cRx，cRy，cRz）=（rx，ry，rz），但这两个都是错误的

旋转的顺序是Z*Y*X，我相信这是很常见的。我的数学有问题吗？我计划最终为音高为0或PI时添加特殊情况，如中所示，但现在我认为问题是独立的

        //input is angle 'angle' and axis '(rx,ry,rz)'

        //convert rx,ry,rz, angle, into roll, pitch, yaw
        double q0 = Math.Cos(angle / 2);
        double q1 = Math.Sin(angle / 2) *Math.Cos(rx);
        double q2 = Math.Sin(angle / 2) * Math.Cos(ry);
        double q3 = Math.Sin(angle / 2) * Math.Cos(rz);
        double roll = Math.Atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
        double pitch = Math.Asin(2 * (q0 * q2 - q3 * q1));
        double yaw = Math.Atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));

        //convert back to angle axis
        double cAngle = 2 * Math.Cos(Math.Cos(roll / 2) * Math.Cos(pitch / 2) * Math.Cos(yaw / 2) + Math.Sin(roll / 2) * Math.Sin(pitch / 2) * Math.Sin(yaw / 2));
        double cRx = Math.Acos((Math.Sin(roll / 2) * Math.Cos(pitch / 2) * Math.Cos(yaw / 2) - Math.Cos(roll / 2) * Math.Sin(pitch / 2) * Math.Sin(yaw / 2)) / Math.Sin(cAngle / 2));
        double cRy = Math.Acos((Math.Cos(roll / 2) * Math.Sin(pitch / 2) * Math.Cos(yaw / 2) + Math.Sin(roll / 2) * Math.Cos(pitch / 2) * Math.Sin(yaw / 2)) / Math.Sin(cAngle / 2));
        double cRz = Math.Acos((Math.Cos(roll / 2) * Math.Cos(pitch / 2) * Math.Sin(yaw / 2) - Math.Sin(roll / 2) * Math.Sin(pitch / 2) * Math.Cos(yaw / 2)) / Math.Sin(cAngle / 2));

        //stay within +/- PI of 0 to keep the number small
        if (roll > 3.1416) roll = -Math.PI + (roll - Math.PI);
        if (roll < -3.1416) roll = Math.PI + (roll - (-1) * Math.PI);
        if (pitch > 3.1416) pitch = -Math.PI + (pitch - Math.PI);
        if (pitch < -3.1416) pitch = Math.PI + (pitch - (-1) * 3.1416F);
        if (yaw > 3.1416) yaw = -Math.PI + (yaw - Math.PI);
        if (yaw < -3.1416) yaw = Math.PI + (yaw - (-1) * Math.PI);

        Console.WriteLine("original angle, axis " + angle + ": " + rx + ", " + ry + ", " + rz);
        Console.WriteLine("converted angle, axis " + cAngle + ": " + cRx + ", " + cRy + ", " + cRz);
        Console.WriteLine("quats " + q0 + ", " + q1 + ", " + q2 + ", " + q3);
        Console.WriteLine("roll,pitch,yaw:  " + roll + ", " + pitch + ", " + yaw);

//输入是角度“angle”和轴（rx、ry、rz）'
//将rx、ry、rz、角度转换为横摇、俯仰、偏航
双q0=数学Cos（角度/2）；
双q1=数学Sin（角度/2）*数学Cos（rx）；
双q2=数学Sin（角度/2）*数学Cos（ry）；
双q3=数学Sin（角度/2）*数学Cos（rz）；
双辊=数学Atan2（2*（q0*q1+q2*q3），1-2*（q1*q1+q2*q2））；
双节距=数学Asin（2*（q0*q2-q3*q1））；
双偏航=数学Atan2（2*（q0*q3+q1*q2），1-2*（q2*q2+q3*q3））；
//转换回角度轴
双横摇=2*Math.Cos（Math.Cos（roll/2）*Math.Cos（pitch/2）*Math.Cos（yaw/2）+Math.Sin（roll/2）*Math.Sin（pitch/2）*Math.Sin（yaw/2））；
双cRx=Math.Acos（（Math.Sin（roll/2）*Math.Cos（pitch/2）*Math.Cos（yaw/2）-Math.Cos（roll/2）*Math.Sin（pitch/2）*Math.Sin（yaw/2））/Math.Sin（cAngle/2））；
双哭=Math.Acos（（Math.Cos（roll/2）*Math.Sin（pitch/2）*Math.Cos（yaw/2）+Math.Sin（roll/2）*Math.Cos（pitch/2）*Math.Sin（yaw/2）/Math.Sin（cAngle/2））；
双cRz=Math.Acos（（Math.Cos（roll/2）*Math.Cos（pitch/2）*Math.Sin（yaw/2）-Math.Sin（roll/2）*Math.Sin（pitch/2）*Math.Cos（yaw/2））/Math.Sin（cAngle/2））；
//保持在0的+/-PI范围内，以保持数值较小
如果（roll>3.1416）roll=-Math.PI+（roll-Math.PI）；
如果（滚动<-3.1416）滚动=数学PI+（滚动-（-1）*数学PI）；
如果（音高>3.1416）音高=-Math.PI+（音高-Math.PI）；
如果（节距<-3.1416）节距=数学PI+（节距-（-1）*3.1416F）；
如果（偏航>3.1416）偏航=-Math.PI+（偏航-Math.PI）；
如果（偏航<-3.1416）偏航=数学PI+（偏航-（-1）*数学PI）；
控制台写入线（“原始角度，轴“+角度+”：“+rx+”，“+ry+”，“+rz”）；
控制台写入线（“转换角度，轴“+cAngle+”：“+cRx+”，“+cRy+”，“+cRz”）；
控制台写入线（“quats”+q0+”、“+q1+”、“+q2+”、“+q3”）；
控制台写入线（“横滚、俯仰、偏航：“+横滚+”、“+俯仰+”、“+偏航”）；

我没有（也不会）检查您的代码即使您的代码是正确的，您的测试失败至少有两个原因。

• 使用Euler角表示相同的旋转。你也可以看到，这个问题基本上和你遇到的问题是一样的

• 四元数具有所谓的属性：两个单位四元数对应于每个旋转

也就是说，即使您的转换是正确的，您也可以得到另一个表示，而不是您开始使用的表示。不管你是从欧拉角还是四元数开始

---

如果您想测试代码，我建议检查单位基向量的正交旋转。例如，将

[1,0,0]

适当旋转90度，以获得

[0,1,0]

。检查您是否确实获得了预期的

[0，1，0]

等。 如果所有3个基向量的旋转都正确，那么代码很可能是正确的

此测试的优点是明确无误如果你搞错了什么（例如公式中的符号），此测试将帮助你发现错误

---

---

我不会用欧拉角来表示。它们不是。使用四元数是不正确的

我想补充一下这个答案。人们习惯使用四元数，即使应用程序不正确，这一点也很重要

如果旋转主体无法滚动，则使用四元数表示旋转是不正确的。四元数将三维旋转编码到系统中，如果系统只有两个，则表示不匹配且不正确

四元数是一种过于复杂的旋转表示，用于固定万向节锁，并且仅为涉及俯仰、偏航和滚动的旋转提供更好的可组合性

如果你只有俯仰和偏航。四元数变换可以给你一个涉及滚动的答案，这是根本不正确的。将滚动角度归零不会阻止变换具有滚动值。四元数没有使用没有滚动的旋转概念进行编码，因此在这里使用它是不正确的

---

对于只能俯仰和偏航而不能滚动的物体，请使用不涉及“滚动”概念的实体，如三维笛卡尔坐标或球坐标（非欧拉角）。这就足够了，而且更加正确。在这种情况下，您将不会受到万向节锁的影响。。。用四元数来做这件事不仅过分，而且是错误的。

+1。我终于知道四元数是什么了。一分钟内获得的信息比谷歌的几天还要多。希望你能得到你的答案。非常感谢你的完整解释。我想我现在应该避免使用欧拉角了，我有一个坏主意，就是用它们作为中介来组成ro