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])

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