2. 数学基础
在机器人6D位姿估计与抓取工程中，几乎所有算法最终都要回答同一个问题：物体相对于相机或机器人基座，究竟处在什么位置、以什么姿态朝向？位置可以用三维平移向量描述，而姿态则必须借助旋转表示。理解旋转矩阵、欧拉角、四元数以及齐次坐标之间的转换关系，是后续点云配准、位姿回归、抓取位姿生成和手眼标定的共同基础。
一、核心原理：刚体变换的四个基本量
三维空间中的刚体变换由旋转和平移共同构成。一个完整的6D位姿通常写作 T = (R, t)，其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。旋转矩阵满足正交性 RᵀR = I 且 det(R) = 1，它保持向量长度和夹角不变，因此适合描述刚体姿态。但旋转矩阵有9个元素却只有3个自由度，存在冗余约束，直接优化时容易失去正交性。
欧拉角用三个角度（如绕 Z-Y-X 的偏航、俯仰、滚转）直观描述姿态，便于人类理解，但存在万向节死锁和插值不连续问题。四元数用四个数 q = (w, x, y, z) 表示旋转，单位四元数满足 ||q|| = 1，没有死锁、插值平滑，是SLAM、位姿网络和机器人中间件中最常用的旋转表示。齐次坐标则把旋转和平移统一进一个 4×4 矩阵，使连续变换可以通过矩阵乘法串联。
二、方法流程：从旋转表示到齐次变换
实际工程中，不同模块输出的旋转表示往往不同：视觉位姿网络常输出四元数，机械臂示教器可能使用欧拉角，点云配准库（如Open3D、PCL）内部多用旋转矩阵，而运动学链和手眼标定则统一使用齐次变换矩阵。因此需要掌握它们之间的转换流程：
- 欧拉角 → 旋转矩阵：按约定旋转顺序（如 ZYX）依次相乘，得到
R = Rz(yaw)·Ry(pitch)·Rx(roll)。 - 旋转矩阵 → 四元数：通过矩阵迹和元素符号判断，构造单位四元数，注意符号选择保证
w ≥ 0。 - 四元数 → 旋转矩阵：使用标准公式展开，避免三角函数，计算效率高。
- 旋转+平移 → 齐次变换：将
R和t填入 4×4 矩阵，最后一行固定为[0,0,0,1]。 - 变换串联：多个坐标系之间的变换通过矩阵乘法组合，如
T_base_cam = T_base_gripper · T_gripper_cam。
import numpy as np
from scipy.spatial.transform import Rotation as R
# 欧拉角(roll, pitch, yaw) -> 旋转矩阵
r = R.from_euler('xyz', [0.1, 0.2, 0.3])
R_mat = r.as_matrix()
# 旋转矩阵 -> 四元数 (x, y, z, w)
q = r.as_quat()
# 构造齐次变换矩阵
t = np.array([0.5, -0.2, 0.8])
T = np.eye(4)
T[:3, :3] = R_mat
T[:3, 3] = t
print(T)
三、案例/场景：抓取位姿的坐标链
假设相机识别到目标物体，输出物体在相机坐标系下的位姿 T_cam_obj。要驱动机器人抓取，需要将其转换到机器人基座坐标系：T_base_obj = T_base_cam · T_cam_obj。其中 T_base_cam 来自手眼标定，通常由旋转矩阵和平移向量组成。若夹爪需要沿物体局部 Z 轴接近，则抓取位姿还需左乘一个工具偏移 T_obj_grasp，最终得到 T_base_grasp = T_base_obj · T_obj_grasp。这一连串乘法正是齐次坐标最大的价值：把旋转和平移统一成矩阵运算，避免分别处理旋转和平移带来的混乱。
geometry_msgs/Quaternion 传递，而变换矩阵仅在需要时临时构造。这样既节省存储，又避免旋转矩阵正交性漂移。
四、常见误区与复盘
| 误区 | 后果 | 正确做法 |
|---|---|---|
| 欧拉角顺序混用 | 姿态完全错误，抓取方向偏离 | 明确约定旋转顺序，如 ZYX 或 XYZ，并在文档中固定 |
| 四元数未归一化 | 旋转矩阵出现缩放，位姿失真 | 每次插值或更新后执行 q /= ||q|| |
| 旋转矩阵直接线性插值 | 结果不再正交，产生剪切变形 | 改用四元数球面插值（SLERP） |
| 齐次变换乘法顺序颠倒 | 坐标系变换方向相反 | 牢记 T_A_C = T_A_B · T_B_C，从右向左读 |
| 忽略平移与旋转的耦合 | 绕非原点旋转时位置计算错误 | 先平移到旋转中心，旋转后再平移回去 |
q 与 -q 表示同一旋转，但在插值和网络回归中若符号不一致，会导致姿态跳变。工程上常通过判断点积符号并翻转来保证连续性。
总结来看，旋转矩阵适合理论推导和点云变换，欧拉角适合人机交互和示教，四元数适合优化、插值和传输，齐次坐标适合串联整个坐标链。掌握它们之间的转换与适用边界，才能在6D位姿估计与抓取系统中游刃有余地处理相机、物体、夹爪和机器人基座之间的复杂关系。