机器人位姿数据形式转换与旋转矩阵总结（欧拉角、RPY、NOAP）

最新推荐文章于 2024-12-17 11:46:52 发布

原创

最新推荐文章于 2024-12-17 11:46:52 发布 · 2k 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#机器人 #矩阵 #线性代数

在机器人学和计算机视觉领域，位姿（Position and Orientation）描述了一个物体在三维空间中的位置和方向。位姿通常表示为位置向量和方向表示。方向可以用多种方式表示，包括欧拉角（Euler Angles）、RPY（Roll-Pitch-Yaw）、NOAP（Normalized Angle with Quaternion Representation）等。本文将详细介绍这些表示方式之间的转换，以及旋转矩阵的使用，并提供相关的 Python 代码示例。

一、欧拉角

欧拉角是一种通过三个旋转角度描述空间中物体朝向的方法，通常使用三个角度来表示绕三个坐标轴的旋转。欧拉角的顺序会影响最终的朝向，常用的顺序包括 ZYX（航向-俯仰-滚转）和 XYZ（滚转-俯仰-航向）等。

1.1 欧拉角到旋转矩阵的转换

假设给定的欧拉角为 ((\phi, \theta, \psi))（滚转、俯仰、航向），其旋转矩阵 (R) 可以通过以下公式计算：

[
R = R_z(\psi) R_y(\theta) R_x(\phi)
]

其中，旋转矩阵分别为：

[
R_x(\phi) = \begin{bmatrix}
\cos(\phi) & -\sin(\phi) & 0 \
\sin(\phi) & \cos(\phi) & 0 \
0 & 0 & 1
\end{bmatrix}
]

[
R_y(\theta) = \begin{bmatrix}
\cos(\theta) & 0 & \sin(\theta) \
0 & 1 & 0 \
-\sin(\theta) & 0 & \cos(\theta)
\end{bmatrix}
]

[
R_z(\psi) = \begin{bmatrix}
\cos(\psi) & -\sin(\psi) & 0 \
\sin(\psi) & \cos(\psi) & 0 \
0 & 0 & 1
\end{bmatrix}
]

1.2 Python 代码示例

以下代码将给出如何将欧拉角转换为旋转矩阵的示例：

import numpy as np

def euler_to_rotation_matrix(roll, pitch, yaw):
    # 计算旋转矩阵
    R_x = np.array([[1, 0, 0],
                    [0, np.cos(roll), -np.sin(roll)],
                    [0, np.sin(roll), np.cos(roll)]])
    
    R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)],