首页旋转矩阵转换成欧拉角

旋转矩阵转换成欧拉角

时间: 2025-06-23 19:09:50 浏览: 11

### 将旋转矩阵转换为欧拉角为了将旋转矩阵 $ R $ 转换为欧拉角（航向角 yaw、俯仰角 pitch 和滚转角 roll），可以根据不同的坐标系约定和旋转顺序应用特定的公式。对于常见的 ZYX 旋转序列（即先绕 X 轴旋转，再绕 Y 轴旋转，最后绕 Z 轴旋转），可以使用以下公式： \[ \text{pitch} = \arcsin(-R_{31}) \] 如果 $ |R_{31}| < 0.998 $: \[ \text{roll} = \arctan2(R_{32}, R_{33}) \] \[ \text{yaw} = \arctan2(R_{21}, R_{11}) \] 否则: \[ \text{roll} = 0 \] \[ \text{yaw} = \arctan2(-R_{12}, R_{22}) \] 其中 $ R_{ij} $ 表示旋转矩阵第 i 行 j 列的元素[^1]。下面是一个 Python 函数实现上述公式的例子： ```python import numpy as np def rotation_matrix_to_euler_angles(rotation_matrix): sy = np.sqrt(rotation_matrix[0, 0] * rotation_matrix[0, 0] + rotation_matrix[1, 0] * rotation_matrix[1, 0]) singular = sy < 1e-6 if not singular: x = np.arctan2(rotation_matrix[2, 1], rotation_matrix[2, 2]) # Roll y = np.arctan2(-rotation_matrix[2, 0], sy) # Pitch z = np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0]) # Yaw else: x = 0 # Roll y = np.arctan2(-rotation_matrix[2, 0], sy) # Pitch z = np.arctan2(-rotation_matrix[1, 2], rotation_matrix[1, 1]) # Yaw return np.array([z, y, x]) # 测试函数 R_example = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0], [0 , 0, 1]]) print("Euler angles:", rotation_matrix_to_euler_angles(R_example)) ``` 此代码片段定义了一个名为 `rotation_matrix_to_euler_angles` 的函数，该函数接收一个 3×3 的旋转矩阵作为输入参数并返回相应的欧拉角数组。测试部分展示了如何调用此函数以及预期的结果输出。

阅读全文