在机器人学和计算机视觉领域，位姿（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)],
                    [0, 1, 0],
                    [-np.sin(pitch), 0, np.cos(pitch)]])
    
    R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0],
                    [np.sin(yaw), np.cos(yaw), 0],
                    [0, 0, 1]])
    
    # 总旋转矩阵
    R = R_z @ R_y @ R_x
    return R

# 示例
roll = np.radians(30)   # 30度转为弧度
pitch = np.radians(45)  # 45度转为弧度
yaw = np.radians(60)    # 60度转为弧度

rotation_matrix = euler_to_rotation_matrix(roll, pitch, yaw)
print("Rotation Matrix from Euler Angles:\n", rotation_matrix)

二、RPY（滚转-俯仰-航向）

RPY 是一种特殊的欧拉角表示方式，通常用于描述航天器或机械臂的姿态。RPY 的定义与欧拉角相似，但其旋转顺序固定为 ZYX。

2.1 RPY 到旋转矩阵的转换

RPY 的旋转矩阵与欧拉角相同，具体的旋转顺序为 ZYX：

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

2.2 Python 代码示例

可以复用上面的 euler_to_rotation_matrix 函数，修改函数参数名称以符合 RPY 的习惯。

def rpy_to_rotation_matrix(roll, pitch, yaw):
    return euler_to_rotation_matrix(roll, pitch, yaw)

# 示例
rotation_matrix_rpy = rpy_to_rotation_matrix(roll, pitch, yaw)
print("Rotation Matrix from RPY:\n", rotation_matrix_rpy)

三、NOAP（标准化角度表示）

NOAP 是一种使用四元数表示的旋转方式，四元数比欧拉角和旋转矩阵在某些计算中更高效且更能避免万向锁问题。

3.1 欧拉角到四元数的转换

四元数 ((q_w, q_x, q_y, q_z)) 可通过以下公式从欧拉角转换得到：

[
q_w = \cos\left(\frac{\phi}{2}\right) \cos\left(\frac{\theta}{2}\right) \cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right) \sin\left(\frac{\theta}{2}\right) \sin\left(\frac{\psi}{2}\right)
]

[
q_x = \sin\left(\frac{\phi}{2}\right) \cos\left(\frac{\theta}{2}\right) \cos\left(\frac{\psi}{2}\right) - \cos\left(\frac{\phi}{2}\right) \sin\left(\frac{\theta}{2}\right) \sin\left(\frac{\psi}{2}\right)
]

[
q_y = \cos\left(\frac{\phi}{2}\right) \sin\left(\frac{\theta}{2}\right) \cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right) \cos\left(\frac{\theta}{2}\right) \sin\left(\frac{\psi}{2}\right)
]

[
q_z = \cos\left(\frac{\phi}{2}\right) \cos\left(\frac{\theta}{2}\right) \sin\left(\frac{\psi}{2}\right) - \sin\left(\frac{\phi}{2}\right) \sin\left(\frac{\theta}{2}\right) \cos\left(\frac{\psi}{2}\right)
]

3.2 Python 代码示例

def euler_to_quaternion(roll, pitch, yaw):
    q_w = (np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) +
            np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2))
    
    q_x = (np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) -
            np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2))
    
    q_y = (np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) +
            np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2))
    
    q_z = (np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) -
            np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2))
    
    return np.array([q_w, q_x, q_y, q_z])

# 示例
quaternion = euler_to_quaternion(roll, pitch, yaw)
print("Quaternion from Euler Angles:\n", quaternion)

四、旋转矩阵的使用

旋转矩阵在机器人学中广泛应用于表示和计算物体的姿态。可以将旋转矩阵应用于坐标变换、物体的定位和运动控制等任务。

4.1 使用旋转矩阵进行坐标变换

旋转矩阵可以将一个点在世界坐标系中的表示转换为物体坐标系中的表示。

def rotate_point(rotation_matrix, point):
    # 使用旋转矩阵旋转一个点
    return rotation_matrix @ point

# 示例
point = np.array([1, 0, 0])  # 在世界坐标系中的点
rotated_point = rotate_point(rotation_matrix, point)
print("Rotated Point:\n", rotated_point)

五、总结

在机器人和计算机视觉领域，理解位姿表示及其转换至关重要。本文详细介绍了欧拉角、RPY、NOAP 和旋转矩阵的定义和转换方法，并提供了相应的 Python 代码示例。掌握这些基本概念和技术将有助于您在实际应用中更好地处理位姿数据和旋转计算。