椭圆旋转矩阵求解欧拉角

在三维空间中，旋转矩阵和欧拉角是描述坐标系或物体取向的两种常用方式。当需要从椭圆（或其他刚体）的旋转矩阵中提取对应的欧拉角时，可以通过解析旋转矩阵的元素来推导出三个旋转角度。 假设给定的旋转矩阵 $ R $ 是一个 $ 3 \times 3 $ 的正交矩阵，形式如下： $$ R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} $$ 对于 ZYX 欧拉角顺序（即依次绕 z 轴、y 轴、x 轴旋转），欧拉角分别为偏航角（yaw, $\psi$）、俯仰角（pitch, $\theta$）、滚转角（roll, $\phi$），其对应的旋转矩阵可以表示为： $$ R = R_z(\psi) R_y(\theta) R_x(\phi) $$ 根据该旋转矩阵的形式，可以反推出对应的欧拉角公式如下： - 偏航角（$\psi$）： $$ \psi = \arctan2(r_{21}, r_{11}) $$ - 俯仰角（$\theta$）： $$ \theta = \arcsin(-r_{31}) $$ - 滚转角（$\phi$）： $$ \phi = \arctan2(r_{32}, r_{33}) $$ 需要注意的是，当俯仰角接近 ±90° 时，会出现“万向锁”现象，导致偏航角和滚转角无法独立确定[^2]。 ### 示例代码 以下是一个 Python 函数，用于从给定的旋转矩阵中提取 ZYX 顺序的欧拉角： ```python import numpy as np def rotation_matrix_to_euler_angles(R): # 提取偏航角 (psi) psi = np.arctan2(R[1, 0], R[0, 0]) # 提取俯仰角 (theta) theta = np.arcsin(-R[2, 0]) # 提取滚转角 (phi) phi = np.arctan2(R[2, 1], R[2, 2]) return np.array([psi, theta, phi]) # 示例：构造一个 ZYX 旋转矩阵 def euler_to_rotation_matrix(angles): psi, theta, phi = angles Rz = np.array([ [np.cos(psi), -np.sin(psi), 0], [np.sin(psi), np.cos(psi), 0], [0, 0, 1] ]) Ry = np.array([ [np.cos(theta), 0, np.sin(theta)], [0, 1, 0], [-np.sin(theta), 0, np.cos(theta)] ]) Rx = np.array([ [1, 0, 0], [0, np.cos(phi), -np.sin(phi)], [0, np.sin(phi), np.cos(phi)] ]) return Rz @ Ry @ Rx # 测试 euler_angles = np.array([np.radians(45), np.radians(30), np.radians(60)]) R = euler_to_rotation_matrix(euler_angles) extracted_angles = rotation_matrix_to_euler_angles(R) print("Original Euler Angles (rad):", euler_angles) print("Extracted Euler Angles (rad):", extracted_angles) ``` 上述代码首先定义了一个将欧拉角转换为旋转矩阵的函数 `euler_to_rotation_matrix`，然后定义了从旋转矩阵提取欧拉角的函数 `rotation_matrix_to_euler_angles`。最后通过测试用例验证了算法的正确性。 ### 注意事项 - 当俯仰角接近 ±90° 时，可能会出现数值不稳定的情况，此时应考虑使用其他参数化方法如四元数或旋转向量。 - 不同的旋转顺序（如 XYZ、ZYZ 等）会有不同的转换公式，需根据具体应用选择合适的顺序。 ---