欧拉角和四元数之间的python转换代码

2024-01-03 10:26:38

1、欧拉角转换到四元数,转换顺序是ZYX

def rpy2quaternion(roll, pitch, yaw):
    x=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2)
    y=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2)
    z=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2)
    w=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2)
    return np.array([w, x, y, z ])

2、四元数转换到欧拉角,转换顺序是ZYX

def quart_to_rpy(w,x, y, z):
    roll = atan2(2 * (w * x + y * z), 1 - 2 * (x ** 2 + y ** 2))
    pitch = asin(2 * (w * y - z * x))
    yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2))

    return np.array([roll, pitch, yaw])

实验的时候发现,欧拉角转到四元数,再从四元数转回来,有些角度存在问题。

文章来源:https://blog.csdn.net/qq_42744739/article/details/135356268
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。