python 将旋转矩阵转为XYZ外旋欧拉角

将旋转矩阵转换为 XYZ 外旋欧拉角是一个常见的任务，在计算机图形学、机器人学等领域经常遇到。以下是 Python 中如何实现这一过程的详细介绍： --- ### 背景知识 1. **旋转矩阵**：三维空间中的旋转可以由一个 \(3 \times 3\) 的正交矩阵表示，通常记作 R。 2. **欧拉角**：欧拉角是一种描述刚体方向的方式，通过依次绕三个坐标轴进行三次旋转来确定最终的方向。 对于 XYZ 外旋顺序（即先绕 Z 轴旋转再绕 Y 轴最后绕 X 轴），欧拉角可以通过解析法从旋转矩阵中提取出来。 公式推导如下： 给定旋转矩阵 \(R = [r_{ij}]\)，假设 \(φ\), \(θ\), 和 \(ψ\) 分别对应于围绕 \(X\), \(Y\), 和 \(Z\) 轴的外旋角度，则有以下映射关系： - 如果中间的角度 \(θ ≠ ±90°\) (避免奇异点)， \[ φ = atan2(r_{32}, r_{33}) \] \[ θ = asin(-r_{31}) \] \[ ψ = atan2(r_{21}, r_{11}) \] 其中 `atan2(y,x)` 是反正切函数，返回值域范围为 [-π, π]。 --- ### 实现步骤 #### 使用 numpy 进行计算 ```python import numpy as np def rotation_matrix_to_euler_xyz(R): # 确保输入的是有效的3x3旋转矩阵 if not isinstance(R, np.ndarray) or R.shape != (3, 3): raise ValueError("请输入一个合法的3×3旋转矩阵") sy = np.sqrt(R[0,0]**2 + R[1,0]**2) singular = sy < 1e-6 if not singular: x_angle = np.arctan2(R[2,1], R[2,2]) y_angle = -np.arcsin(R[2,0]) z_angle = np.arctan2(R[1,0], R[0,0]) else: # 奇异情况处理 (接近 Gimbal lock) x_angle = np.arctan2(-R[1,2], R[1,1]) y_angle = -np.sign(R[2,0]) * np.pi / 2 z_angle = 0 return np.array([x_angle, y_angle, z_angle]) # 示例测试 if __name__ == "__main__": import math from scipy.spatial.transform import Rotation as RotScipy test_matrix = RotScipy.from_euler('xyz', [math.radians(45), math.radians(30), math.radians(60)]).as_matrix() angles_rad = rotation_matrix_to_euler_xyz(test_matrix) print("原始欧拉角 (度):", [45, 30, 60]) print("重建后的欧拉角:", np.degrees(angles_rad)) ``` 上述代码首先检查传入是否是标准形式的\(3\times3\)旋转矩阵，并且根据是否有奇异性分别采用了不同的解算策略。 注意：这里我们用到了 SciPy 库生成了一个示例旋转矩阵用于验证结果准确性。 --- ### 注意事项 - 欧拉角存在所谓的“万向锁”问题(Gimbal Lock)，当某个特定角度组合导致两个旋转自由度丢失时会出现数值不稳定现象； - 因此实际应用中常推荐采用四元数替代传统欧拉角表述方式；