3D坐标系转换实战：如何用Python快速实现旋转矩阵与平移向量的组合变换

3D坐标系转换实战：如何用Python快速实现旋转矩阵与平移向量的组合变换

在机器人路径规划和计算机视觉领域，3D坐标系的转换是最基础却至关重要的操作。当我们需要让机械臂抓取物体，或者让AR眼镜在真实世界中叠加虚拟物体时，都需要精确计算不同坐标系之间的位置关系。本文将带你用NumPy从零实现这些变换，并解决实际开发中的常见陷阱。

1. 理解3D变换的数学本质

任何3D空间的刚体运动都可以分解为旋转和平移两部分。想象你手持一个魔方：转动魔方相当于旋转操作，而将它放到桌子的另一侧则是平移操作。数学上，我们用3×3的旋转矩阵R和3×1的平移向量t来描述这个过程。

旋转矩阵有一个关键特性：它是正交矩阵，即满足RᵀR = I（转置等于逆）。这意味着旋转操作不会改变物体的形状和大小。验证这一特性的Python代码如下：

import numpy as np

# 生成一个随机旋转矩阵
theta = np.pi/4  # 45度
R = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta), np.cos(theta), 0],
    [0, 0, 1]
])

# 验证正交性
print("R的转置乘R:\n", R.T @ R)  # 应接近单位矩阵

注意：实际计算中由于浮点精度，结果可能包含极小的非零值（如1e-16），这属于正常现象。

2. 齐次坐标：简化变换的数学工具

当旋转和平移组合时，公式会变得复杂（a₁ = Ra₂ + t）。为了简化表示，我们引入齐次坐标——给3D坐标增加一个维度，通常设为1。这样旋转和平移就能合并成一个4×4的变换矩阵T：

T = [[R   t]
     [0   1]