python矩阵转欧拉角zyx

要将一个矩阵转化为欧拉角ZYX，可以按照以下步骤进行：

1. 计算旋转矩阵的欧拉角：

import numpy as np

def rotation_matrix_to_euler_angles(R):
    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6
 
    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])
        y = np.arctan2(-R[2, 0], sy)
        z = np.arctan2(R[1, 0], R[0, 0])
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arctan2(-R[2, 0], sy)
        z = 0
 
    return np.array([x, y, z])

2. 将欧拉角转换为ZYX顺序：

def euler_angles_to_zyx(euler_angles):
    alpha = euler_angles[0]
    beta = euler_angles[1]
    gamma = euler_angles[2]
    
    R_z = np.array([[np.cos(gamma), -np.sin(gamma), 0],
                    [np.sin(gamma), np.cos(gamma), 0],
                    [0, 0, 1]])
    
    R_y = np.array([[np.cos(beta), 0, np.sin(beta)],
                    [0, 1, 0],
                    [-np.sin(beta), 0, np.cos(beta)]])
    
    R_x = np.array([[1, 0, 0],
                    [0, np.cos(alpha), -np.sin(alpha)],
                    [0, np.sin(alpha), np.cos(alpha)]])
    
    R = np.dot(R_z, np.dot(R_y, R_x))
    
    angles = rotation_matrix_to_euler_angles(R)
    
    return angles[::-1]

3. 最终的函数：

def matrix_to_zyx(matrix):
    euler_angles = rotation_matrix_to_euler_angles(matrix)
    return euler_angles_to_zyx(euler_angles)

这样就可以将一个矩阵转化为ZYX欧拉角了。