Question

对于我的高级设计项目，我试图将四元数转换为Euler Angles，但是遇到一些问题。我有两个函数从BNO055传感器中获取值，并仅显示这些值，并有助于区分转换函数是否正常工作（如下所示）

def print_euler():
    heading, roll, pitch = bno.read_euler()
    print("Pitch:{0:0.2F} Roll:{1:0.2F} Heading:{2:0.2F}".format(pitch, roll, heading))

def print_quaternion():
    x,y,z,w = bno.read_quaternion()
    print("x:{0} y:{1} z:{2} w:{3}".format(x, y, z, w))

这是我正在使用的转换函数，但我不能完全确定数学是100％正确的，因为我是从多个地方获取的。

def quaternion_to_euler(x ,y , z, w):
    # ROLL
    sin_r = 2*(w*x + y*z)
    cos_r = -1*(x*x + y*y)
    roll = math.degrees(math.atan2(sin_r, cos_r))
    #roll = math.degrees(math.atan2((2*x*w - 2*y*z) , (1 - 2*x*x - 2*z*z))) # Ignore

    # PITCH
    sin_p = 2*(w*y - z*x)
    if(math.fabs(sin_p) >= 1):
        pitch = math.degrees(math.copysign(math.pi / 2, sin_p))
    else:
        pitch = math.degrees(math.asin(sin_p))

    #HEADING
    sin_h = 2*(w*z + x*y)
    cos_h = -1*(y*y + z*z)
    heading = math.degrees(math.atan2(sin_h, cos_h))
    #heading = math.degrees(math.atan2((2*y*w - 2*x*z) , (1 - 2*y*y - 2*z*z))) # Ignore
    return heading, roll, pitch

当我运行程序时，我检查输出并使用在线转换器（Converter）查看输入四元数是否给我与传感器相同的欧拉角，但它们并不完全匹配，有时会被其他时间小数点后第一个被整数抵消。如果有人可以检查一下我的数学是否正确，那将不胜感激，因为我认为这就是问题所在。另外，如果您对四元数到欧拉角的转换有任何好的了解，请告诉我！我找到了一些，但他们更多地讨论了四元数是什么，我只想将其转换为欧拉角。感谢您的帮助！

在Python中将四元数转换为欧拉角

0 个答案: