1、欧拉角转换到四元数,转换顺序是ZYX
python
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
python
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])
实验的时候发现,欧拉角转到四元数,再从四元数转回来,有些角度存在问题。