用Python+NumPy彻底掌握机器人位姿变换：从数学公式到代码实现

在机器人学和计算机视觉领域，位姿变换是最基础却最容易让人困惑的概念之一。很多初学者在第一次接触旋转矩阵、欧拉角、齐次变换矩阵时，往往会被各种数学符号和坐标系转换绕得晕头转向。传统教材通常从纯数学角度讲解这些概念，而本文将采用一种全新的学习路径—— 通过Python代码实现来反向理解数学原理 。这种方法不仅能让你真正掌握位姿变换的本质，还能获得可以直接用于实际项目的代码工具集。

1. 坐标系表示与基础旋转

1.1 用NumPy表示坐标系

在三维空间中，一个坐标系可以用三个相互垂直的单位向量表示。让我们先用NumPy数组来定义一个坐标系：

import numpy as np

# 定义世界坐标系（标准基）
world_frame = np.eye(3)  # 3x3单位矩阵
print("世界坐标系：\n", world_frame)

# 定义一个绕Z轴旋转45度的坐标系
theta = np.pi/4  # 45度
rot_z = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta), np.cos(theta), 0],
    [0, 0, 1]
])
print("\n绕Z轴旋转45度的坐标系：\n", rot_z)

这段代码展示了如何用3×3矩阵表示坐标系的方向。矩阵的每一列代表坐标系的一个轴在世界坐标系中的方向向量。

1.2 基本旋转矩阵实现

机器人学中最常用的三种基本旋转是绕X、Y、Z轴的旋转。我们可以将这些旋转封装成函数：

def rotation_x(theta):
    """绕X轴旋转theta弧度"""
    return np.array([
        [1, 0, 0],
        [0, np.cos(theta), -np.sin(theta)],
        [0, np.sin(theta), np.cos(theta)]
    ])

def rotation_y(theta):
    """绕Y轴旋转theta弧度"""
    return np.array([
        [np.cos(theta), 0, np.sin(theta)],
        [0, 1, 0],
        [-np.sin(theta), 0, np.cos(theta)]
    ])

def rotation_z(theta):
    """绕Z轴旋转theta弧度"""
    return np.array([
        [np.cos(theta), -np.sin(theta), 0],
        [np.sin(theta), np.cos(theta), 0],
        [0, 0, 1]
    ])

验证旋转矩阵的性质 ：

• 旋转矩阵是正交矩阵：RᵀR = I
• 行列式为1：det(R) = 1

# 验证旋转矩阵性质
R = rotation_z(np.pi/4)
print("\nR的转置乘以R：\n", R.T @ R)
print("R的行列式：", np.linalg.det(R))

2. 复合旋转与RPY角

2.1 实现复合旋转

实际应用中，我们经常需要组合多个旋转。需要注意的是，旋转的顺序非常重要——不同的顺序会导致完全不同的结果。

# 先绕X转30度，再绕Y转45度，最后绕Z转60度
angles = np.radians([30, 45, 60])  # 转换为弧度
R = rotation_z(angles[2]) @ rotation_y(angles[1]) @ rotation_x(angles[0])
print("\n复合旋转矩阵：\n", R)

2.2 RPY角与旋转矩阵的相互转换

RPY（Roll-Pitch-Yaw）角是一种常用的姿态表示方法，分别对应绕固定坐标系的X、Y、Z轴旋转。

从RPY角到旋转矩阵 ：

def rpy_to_matrix(roll, pitch, yaw):
    """将RPY角转换为旋转矩阵"""
    R_x = rotation_x(roll)
    R_y = rotation_y(pitch)
    R_z = rotation_z(yaw)
    return R_z @ R_y @ R_x  # 注意顺序：先X，后Y，最后Z

从旋转矩阵到RPY角 ：

def matrix_to_rpy(R):
    """从旋转矩阵提取RPY角"""
    pitch = np.arctan2(-R[2,0], np.sqrt(R[0,0]**2 + R[1,0]**2))
    
    if np.abs(np.cos(pitch)) > 1e-6:  # 非奇异情况
        roll = np.arctan2(R[2,1]/np.cos(pitch), R[2,2]/np.cos(pitch))
        yaw = np.arctan2(R[1,0]/np.cos(pitch), R[0,0]/np.cos(pitch))
    else:  # 万向节锁情况
        roll = 0
        yaw = np.arctan2(-R[0,1], R[1,1])
    
    return np.array([roll, pitch, yaw])

测试转换函数 ：

# 测试RPY转换
original_rpy = np.radians([30, 45, 60])
R = rpy_to_matrix(*original_rpy)
recovered_rpy = matrix_to_rpy(R)

print("\n原始RPY角(度)：", np.degrees(original_rpy))
print("恢复的RPY角(度)：", np.degrees(recovered_rpy))

注意：当俯仰角(Pitch)接近±90度时会出现万向节锁现象，此时Roll和Yaw角无法唯一确定。

3. 齐次变换与位姿表示

3.1 齐次变换矩阵实现

齐次变换矩阵可以同时表示旋转和平移，是机器人学中最常用的位姿表示方法。

def pose_to_homogeneous(R, t):
    """将旋转矩阵和平移向量组合成齐次变换矩阵"""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def homogeneous_to_pose(T):
    """从齐次变换矩阵提取旋转矩阵和平移向量"""
    return T[:3, :3], T[:3, 3]

3.2 坐标系变换实践

假设有两个坐标系A和B，B相对于A的位姿用齐次变换矩阵表示：

# 坐标系B相对于A的位姿
R_AB = rpy_to_matrix(np.radians(30), np.radians(45), np.radians(60))
t_AB = np.array([1, 2, 3])
T_AB = pose_to_homogeneous(R_AB, t_AB)

# 点P在B坐标系中的坐标
P_B = np.array([4, 5, 6])

# 将P从B坐标系变换到A坐标系
P_A = R_AB @ P_B + t_AB  # 常规方法
P_A_hom = T_AB @ np.append(P_B, 1)  # 齐次变换方法

print("\n常规方法结果：", P_A)
print("齐次变换结果：", P_A_hom[:3])

3.3 齐次变换的逆

计算一个齐次变换矩阵的逆（即求相对位姿）：

def inverse_homogeneous(T):
    """计算齐次变换矩阵的逆"""
    R = T[:3, :3]
    t = T[:3, 3]
    inv_R = R.T
    inv_t = -inv_R @ t
    inv_T = np.eye(4)
    inv_T[:3, :3] = inv_R
    inv_T[:3, 3] = inv_t
    return inv_T

# 计算T_BA = inv(T_AB)
T_BA = inverse_homogeneous(T_AB)
print("\nT_AB的逆矩阵：\n", T_BA)

4. 可视化与调试技巧

4.1 使用Matplotlib进行3D可视化

理解位姿变换最有效的方法之一就是可视化。我们可以使用Matplotlib创建简单的3D坐标系可视化：

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def plot_frame(ax, T, label, length=1):
    """绘制一个坐标系"""
    origin = T[:3, 3]
    x_axis = origin + T[:3, 0] * length
    y_axis = origin + T[:3, 1] * length
    z_axis = origin + T[:3, 2] * length
    
    ax.quiver(*origin, *(x_axis-origin), color='r', arrow_length_ratio=0.1)
    ax.quiver(*origin, *(y_axis-origin), color='g', arrow_length_ratio=0.1)
    ax.quiver(*origin, *(z_axis-origin), color='b', arrow_length_ratio=0.1)
    ax.text(*origin, label, fontsize=12)

# 创建图形
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')

# 绘制世界坐标系
plot_frame(ax, np.eye(4), "World")

# 绘制变换后的坐标系
plot_frame(ax, T_AB, "Frame B")

# 设置图形属性
ax.set_xlim([0, 4])
ax.set_ylim([0, 4])
ax.set_zlim([0, 4])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.title('坐标系变换可视化')
plt.show()

4.2 常见问题调试

在实际编码过程中，经常会遇到以下问题：

1. 旋转顺序错误 ：确保按照正确的顺序组合旋转矩阵
2. 坐标系混淆 ：明确每个变换是相对于哪个坐标系进行的
3. 数值精度问题 ：旋转矩阵应始终保持正交性

验证旋转矩阵正交性 ：

def is_rotation_matrix(R):
    """检查矩阵是否是有效的旋转矩阵"""
    # 检查行列式是否接近1
    det_valid = np.isclose(np.linalg.det(R), 1.0, atol=1e-6)
    
    # 检查是否正交
    ortho_valid = np.allclose(R.T @ R, np.eye(3), atol=1e-6)
    
    return det_valid and ortho_valid

print("\n旋转矩阵有效性检查：", is_rotation_matrix(R_AB))

5. 实际应用案例

5.1 机器人末端执行器定位

假设我们有一个机械臂，已知基座到关节的变换和各关节之间的变换，可以计算末端执行器的位姿：

# 假设有三个关节的变换矩阵
T_base_joint1 = pose_to_homogeneous(rotation_z(np.radians(30)), [0, 0, 0.5])
T_joint1_joint2 = pose_to_homogeneous(rotation_y(np.radians(45)), [0.5, 0, 0])
T_joint2_end = pose_to_homogeneous(rotation_x(np.radians(60)), [0, 0, 0.3])

# 计算末端执行器相对于基座的位姿
T_base_end = T_base_joint1 @ T_joint1_joint2 @ T_joint2_end
print("\n末端执行器位姿：\n", T_base_end)

5.2 相机标定中的位姿估计

在计算机视觉中，我们经常需要估计相机相对于某个标定板的位姿：

def estimate_pose(pts_3d, pts_2d, camera_matrix):
    """使用PnP算法估计相机位姿"""
    _, rvec, tvec = cv2.solvePnP(pts_3d, pts_2d, camera_matrix, None)
    R, _ = cv2.Rodrigues(rvec)  # 将旋转向量转换为旋转矩阵
    return pose_to_homogeneous(R, tvec.flatten())

# 假设有一些3D-2D对应点和相机内参
# T_camera_board = estimate_pose(board_points, image_points, K)

6. 性能优化与工程实践

6.1 批量变换计算

当需要对大量点进行相同的坐标变换时，可以使用NumPy的广播机制提高效率：

# 假设有1000个点需要从B坐标系转换到A坐标系
points_B = np.random.rand(1000, 3)  # 1000个3D点
points_A = (R_AB @ points_B.T).T + t_AB  # 向量化计算

# 使用齐次坐标表示
hom_points_B = np.column_stack([points_B, np.ones(1000)])
hom_points_A = (T_AB @ hom_points_B.T).T[:, :3]

6.2 四元数表示（可选扩展）

虽然旋转矩阵直观，但在某些情况下使用四元数更为高效：

from scipy.spatial.transform import Rotation

# 旋转矩阵转四元数
rot = Rotation.from_matrix(R_AB)
quat = rot.as_quat()
print("\n旋转矩阵对应的四元数：", quat)

# 四元数转回旋转矩阵
R_recovered = Rotation.from_quat(quat).as_matrix()

在实际工程项目中，根据具体需求选择合适的姿态表示方法非常重要。旋转矩阵直观但需要9个参数，四元数只有4个参数且插值方便，而欧拉角虽然直观但存在万向节锁问题。