四元数到偏航桨距

Question

我正在尝试将四元数转换为偏航俯仰欧拉角。我在使用万向节锁时遇到问题。发生的第一个奇怪的事情是，当俯仰角在+ -pi / 2附近时，错误已经开始出现。我以为问题应该只发生在pi / 2处。

第二，我的代码显示了不正确的180度偏航角，俯仰为-90度。

我在this post和this site尝试了代码，但是没有一个起作用。我也尝试过pyquaternion库，但是这个库甚至都没有尝试补偿万向节锁定。最后，我将python等同于手册的这一部分。

这样做效果最好，但仍然会出现上述问题。看来这个问题必须解决1000次，但我无法查明该问题。

对于四元数：[0.86169383 0.02081877 -0.5058515 0.03412598]该代码返回正确的偏航俯仰倾斜角：[0.15911653941132517，-60.832556785346696，-9.335093630495875]

对于四元数：[0.81154224 0.01913839 -0.58165337 0.05207959]该代码返回了不正确的偏航角俯仰角：[-173.53260107524108，-71.09657335881491，0.0]

这是我的代码：

    def yaw_pitch_roll(self):
    """Get the equivalent yaw-pitch-roll angles aka. intrinsic Tait-Bryan angles following the z-y'-x'' convention

    Returns:
        yaw:    rotation angle around the z-axis in radians, in the range `[-pi, pi]`
        pitch:  rotation angle around the y'-axis in radians, in the range `[-pi/2, pi/2]`
        roll:   rotation angle around the x''-axis in radians, in the range `[-pi, pi]` 

    The resulting rotation_matrix would be R = R_x(roll) R_y(pitch) R_z(yaw)

    Note: 
        This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
    """
    self._normalise()
    qw = self.q[0]
    qx = self.q[1]
    qy = self.q[2]
    qz = self.q[3]


    print(2*(qx*qz-qw*qy), self.q)
    if 2*(qx*qz-qw*qy)>=0.94: #Preventing gimbal lock for north pole
        yaw = np.arctan2(qx*qy-qw*qz,qx*qz+qw*qy)
        roll = 0
    elif 2*(qx*qz-qw*qy)<=-0.94: #Preventing gimbal lock for south pole
        yaw = -np.arctan2(qx*qy-qw*qz,qx*qz+qw*qy)
        roll = 0
    else:
        yaw = np.arctan2(qy*qz + qw*qx,
            1/2 - (qx**2 + qy**2))
        roll = np.arctan2(qx*qy - qw*qz,
            1/2 - (qy**2 + qz**2))
    pitch = np.arcsin(-2*(qx * qz - qw * qy))

    return yaw, pitch, roll  

Answer 1

在给定四元数q的情况下，您可以像这样计算滚动，俯仰和偏航：

yaw = atan2(2.0*(qy*qz + qw*qx), qw*qw - qx*qx - qy*qy + qz*qz);
pitch = asin(-2.0*(qx*qz - qw*qy));
roll = atan2(2.0*(qx*qy + qw*qz), qw*qw + qx*qx - qy*qy - qz*qz);

这应该适合xyz阶的固有tait-bryan旋转。对于其他轮换订单，必须使用外部和适当的欧拉轮换其他转换。

这在Autodesk Maya中对我来说效果很好，在其他解决方案中，极点例外的其他解决方案都具有奇怪的万向架效果。