欧拉角和旋转矩阵

### 欧拉角与旋转矩阵的转换方法 #### 1. 欧拉角到旋转矩阵的转换 欧拉角通常由三个角度组成：偏航角 \( \psi \)（Yaw，绕 Z 轴旋转）、俯仰角 \( \theta \)（Pitch，绕 Y 轴旋转）和滚转角 \( \phi \)（Roll，绕 X 轴旋转）。为了将欧拉角转化为旋转矩阵，需分别构建每个轴的基础旋转矩阵并按指定顺序相乘。 基础旋转矩阵如下： - 绕 Z 轴旋转的角度为 \( \psi \)，其对应的基础旋转矩阵为： \[ R_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}[^2] \] - 绕 Y 轴旋转的角度为 \( \theta \)，其对应的基础旋转矩阵为： \[ R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}[^2] \] - 绕 X 轴旋转的角度为 \( \phi \)，其对应的基础旋转矩阵为： \[ R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix} \] 按照 ZYX 的旋转顺序（即先绕 Z 轴旋转，再绕新坐标系下的 Y 轴旋转，最后绕新坐标系下的 X 轴旋转），最终的旋转矩阵可表示为： \[ R = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi) \] #### 2. 旋转矩阵到欧拉角的转换 给定一个标准正交的 3×3 旋转矩阵 \( R \)，可以从该矩阵提取对应的欧拉角。以下是基于 ZYX 旋转顺序的具体公式： - 如果中间项满足条件 \( |r_{32}| \neq 1 \)，则有： \[ \theta = -\arcsin(r_{32})[^4] \] \[ \psi = \arctan2(r_{31}, r_{33}) \] \[ \phi = \arctan2(r_{12}, r_{22})[^4] \] 其中，\( r_{ij} \) 表示旋转矩阵中的第 i 行 j 列元素。 需要注意的是，在某些情况下可能会遇到奇异性问题（如万向锁现象），此时需要特别处理以避免数值不稳定的情况发生。 #### 示例代码实现 以下是一个 Python 实现的例子，展示如何完成两者间的互换操作： ```python import numpy as np def euler_to_rotation_matrix(roll, pitch, yaw): Rx = np.array([ [1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)] ]) Ry = np.array([ [np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)] ]) Rz = np.array([ [np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1] ]) return Rz @ Ry @ Rx def rotation_matrix_to_euler(R): theta = -np.arcsin(R[2][1]) psi = np.arctan2(R[2][0], R[2][2]) phi = np.arctan2(R[0][1], R[1][1]) return (psi, theta, phi) # 测试部分 roll, pitch, yaw = np.radians([10, 20, 30]) # 输入欧拉角 rotation_matrix = euler_to_rotation_matrix(roll, pitch, yaw) print("Rotation Matrix:\n", rotation_matrix) euler_angles = rotation_matrix_to_euler(rotation_matrix) print("Euler Angles:", np.degrees(euler_angles)) ```