用matlab写出一个算法 来求解一个已知旋转矩阵的 Z - X - Z 欧拉角

要求解一个已知旋转矩阵的 Z - X - Z 欧拉角，可以使用以下算法： 1. 首先，将旋转矩阵输入到 MATLAB 中。假设旋转矩阵为 R。 2. 然后，计算 Z - X - Z 欧拉角。这可以通过以下步骤完成： a. 计算 Y 轴旋转角度：θ = atan2(R(3, 1), R(3, 2)); b. 计算 X 轴旋转角度：φ = atan2(-R(2, 0), R(2, 1)); c. 计算 Z 轴旋转角度：ψ = atan2(R(1, 0), R(0, 0)); 3. 最后，输出 Z - X - Z 欧拉角。 以下是相应的 MATLAB 代码： ```matlab function [theta, phi, psi] = euler_angles_from_rotation_matrix(R) % R: 3x3 rotation matrix % theta: Z-X-Z Euler angles (in degrees) % phi: X-Y Euler angles (in degrees) % psi: Z-X-Z Euler angles (in degrees) % Step 1: Extract rotation matrix elements r11 = R(1, 1); r12 = R(1, 2); r13 = R(1, 3); r21 = R(2, 1); r22 = R(2, 2); r23 = R(2, 3); r31 = R(3, 1); r32 = R(3, 2); r33 = R(3, 3); % Step 2: Convert to degrees and compute Euler angles theta = atan2d(sqrt(r33 + r32), r31); % Z-X-Z angle (in degrees) phi = atan2d(-r23, r22); % X-Y angle (in degrees) psi = atan2d(r13, r11); % Z-X-Z angle (in degrees) end ``` 使用此函数时，只需将旋转矩阵作为输入参数传递即可。例如： ```matlab R = [0.7071, -0.7071, 0; 0.7071, 0.7071, 0; 0.7071, 0.7071, 0]; % Example rotation matrix [theta, phi, psi] = euler_angles_from_rotation_matrix(R); disp(['Theta: ' num2str(deg2rad(theta))]); % Convert to degrees if needed disp(['Phi: ' num2str(deg2rad(phi))]); % Convert to degrees if needed disp(['Psi: ' num2str(deg2rad(psi))]); % Convert to degrees if needed ```