已知相机旋转矩阵求相机位姿角度
时间: 2025-02-13 12:08:50 浏览: 44
### 从相机旋转矩阵中提取姿态角
对于给定的相机旋转矩阵 \( R \),可以从该矩阵中解算出对应的欧拉角,即滚转角 (Roll)、俯仰角 (Pitch) 和偏航角 (Yaw)[^1]。
假设旋转矩阵的形式如下:
\[
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 轴旋转 yaw 角度,再绕新的 Y’轴旋转 pitch 角度,最后绕最终 X''轴旋转 roll 角度[^2]。
具体来说,可以通过下面的方法计算各个角度:
#### 计算 Pitch(俯仰)
当 \( |r_{31}| < 0.998 \):
\[
pitch = -arcsin(r_{31})
\]
否则如果接近于 ±π/2,则需要特殊处理以避免奇异性问题:
- 如果 \( r_{31} ≈ 1 \), 则设置 \( pitch=-\pi / 2, roll=0,\text{ 并且 } yaw=\operatorname{atan2}\left(-r_{12}, r_{13}\right)\)
- 若 \( r_{31}≈−1 \), 设置 \( pitch=\pi / 2,roll=0,\text{ 并且 } yaw=\operatorname{atan2}(r_{12}, r_{13}) \)
#### 计算 Roll(滚动)
\[
roll = atan2(r_{32}/cos(pitch), r_{33}/cos(pitch))
\]
#### 计算 Yaw(偏航)
\[
yaw = atan2(r_{21}/cos(pitch), r_{11}/cos(pitch))
\]
下面是 Python 实现的例子:
```python
import numpy as np
def rotation_matrix_to_euler(R):
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 = np.arctan2(-R[1, 2], R[1, 1])
y = np.arctan2(-R[2, 0], sy)
z = 0
return np.array([x, y, z])
# 示例输入
rotation_matrix_example = np.array([[0.7071, -0.7071, 0],
[0.7071, 0.7071, 0],
[0 , 0, 1]])
result = rotation_matrix_to_euler(rotation_matrix_example)
print(f"Extracted Euler angles: {np.degrees(result)} degrees")
```
阅读全文
相关推荐


















