旋转矩阵求欧拉角

### 从旋转矩阵提取欧拉角的方法及公式 #### 方法概述 为了从旋转矩阵 \(R\) 提取欧拉角（yaw, pitch, roll），需要考虑旋转顺序以及可能存在的奇异情况（如万向锁 gimbal lock）。以下是基于常见内旋 XYZ 序列的推导方法。 --- #### 数学公式推导 对于标准的内旋 XYZ 序列，假设给定的旋转矩阵为： \[ R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \] 可以通过以下公式计算对应的欧拉角： 1. **Pitch 计算** \[ \text{pitch} = -\arcsin(r_{31}) \quad [\text{单位：弧度}] \] 这里需要注意的是当 \(|r_{31}|=1\) 时会进入万向锁状态[^1]。 2. **Yaw 和 Roll 的条件分支** 如果未发生万向锁 (\(|r_{31}|<1|\))： \[ \text{roll} = \arctan2(r_{32}, r_{33}) \] \[ \text{yaw} = \arctan2(r_{21}, r_{11}) \] 若发生了万向锁，则需特殊处理。通常设定其中一个角度固定并重新定义其余两个角度的关系[^3]。 --- #### Python 实现代码 下面是一个实现上述公式的 Python 函数： ```python import numpy as np def rotation_matrix_to_euler_angles(R): """ 将旋转矩阵转换为欧拉角 (yaw, pitch, roll)。 参数: R (numpy.ndarray): 输入的 3x3 旋转矩阵 返回: tuple: 欧拉角 (yaw, pitch, roll)，单位为弧度 """ 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]) # roll y = np.arctan2(-R[2, 0], sy) # pitch z = np.arctan2(R[1, 0], R[0, 0]) # yaw else: x = 0 # roll 设为零 y = np.arctan2(-R[2, 0], sy) # pitch z = np.arctan2(R[1, 2], R[1, 1]) # yaw 特殊处理 return z, y, x ``` --- #### 奇异性和注意事项 在某些特定条件下，可能会遇到所谓的“万向锁”现象，即由于第二个轴的角度达到极端值而导致第一个和第三个轴失去独立性。此时应采取适当措施解决该问题，例如强制设某个角度为常数或调整其他参数以保持一致性。 ---