从SE(3)变换、手眼标定到ACT策略中的旋转表示解析

在深度学习与机器人技术（Embodied AI）结合的今天，很多同学从 CV（计算机视觉）转行而来，容易在模型训练时忽略底层的几何逻辑。跑通 ACT 或 Diffusion Policy 的代码并不难，难的是当机械臂抓取位置总偏离 1 厘米时，你能否意识到这是 SE(3) 空间变换的问题。

本文将剥开公式的外衣，带你理解机器人如何感知与理解空间。

空间变换的核心：SE(3)与齐次变换矩阵

要控制机器人，首先要学会描述“位置”与“姿态”。在物理世界中，刚体（Rigid Body）是指在运动过程中内部任意两点间距离保持不变的物体。描述刚体在三维空间中状态的数学工具，正是李群（Lie Group）中的特殊欧氏群 SE(3)。

为什么是 SE(3)？

SE(3) 全称为 Special Euclidean Group in 3 dimensions。它的定义揭示了刚体运动的两个本质约束：

• 距离保持（Isometry）： 刚体不会发生形变。
• 手性保持（Orientation Preserving）： 刚体不会像镜像一样发生翻转（行列式为+1）。

从集合论的角度来看，SE(3) 是由三维旋转群 $SO(3)$ 和三维平移群 ${\mathbb{R}}^3$ 组成的半直积（Semidirect Product）：$$SE(3)={\mathbb{R}}^3⋊SO(3)$$这个公式告诉我们一个深刻的道理：旋转和平移并不是地位对等的。旋转发生在一个弯曲的流形（Manifold）上，而平移发生在一个平坦的欧几里得空间中。这种几何结构的差异，是导致机器人学习中旋转估计远比位置估计困难的根本原因。

齐次变换矩阵的魔力

在工程实践中，单独处理 $3\times 3$ 的旋转矩阵 $R$ 和 $3\times 1$ 的平移向量 $t$ 是极其繁琐的，尤其是在处理连续变换时（例如：从指尖到手腕，从手腕到手肘，从手肘到基座）。为了将这种“旋转+平移”的仿射变换（Affine Transformation）统一为一个线性的矩阵乘法操作，数学家引入了射影几何（Projective Geometry）中的齐次坐标（Homogeneous Coordinates）。
通过增加一个维度，我们将三维向量 $\mathbf{p}=[x,y,z{]}^T$ 扩展为 $\tilde{\mathbf{p}}=[x,y,z,1{]}^T$。这使得我们可以构建一个 $4\times 4$ 的齐次变换矩阵 $T$：$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ r_{31}& r_{32}& r_{33}& t_z\\ 0& 0& 0& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$这个矩阵不仅是数据的容器，它本身就是一个算子（Operator）。

• 左上角 $3\times 3$ ($R$)： 描述了新坐标系相对于旧坐标系的姿态。更直观的理解是：$R$ 的三列分别代表了新坐标系的 X、Y、Z 轴在旧坐标系中的单位向量投影。
• 右上角 $3\times 1$ ($t$)： 描述了新坐标系原点在旧坐标系中的位置。
• 最后一行：这一行看似多余，实则至关重要。它锁定了尺度因子，确保了变换是刚体运动而非透视投影。如果这一行不是 $$，它就变成了计算机图形学中的透视投影矩阵 。

矩阵乘法的物理意义 齐次变换矩阵最强大的特性在于其可链式组合性（Chainability）。如果我们知道末端相对于相机的变换 $T_{cam}^{ee}$，以及相机相对于基座的变换 $T_{base}^{cam}$，那么末端相对于基座的变换就是简单的矩阵乘法：$$T_{base}^{ee}=T_{base}^{cam}\cdot T_{cam}^{ee}$$注意顺序！ 在标准的列向量约定（Column-Major，如机器人学主流）中，变换是从右向左进行的。这一点在代码实现中经常导致严重的Bug。如果你的机器人向左移动而它本该向右，或者绕着奇怪的轴旋转，首先检查你的矩阵乘法顺序是否反了 。

坐标系层级：机器人的世界观

一个机器人系统中存在多个坐标系，它们构成了一棵树状结构（TF Tree）。理解这些坐标系的相对关系是进行任何控制算法设计的前提。

世界坐标系 (World Frame, $F_W$)

这是绝对真理的参考点。在仿真环境（如MuJoCo或Isaac Gym）中，它是 $(0,0,0)$ 原点。在现实实验室中，它通常被定义在光学平台的一个角，或者由放置在地板上的标定板（Calibration Board）决定。

• 关键作用： 多机器人协作的基准。如果有两个机器人协作搬运物体，它们必须统一到同一个世界坐标系下，否则它们的动作将无法协调。

基座坐标系 (Base Frame, $F_{base}$)

这是机器人本体的根节点，通常位于机械臂底座的几何中心。

• 常见误区： 初学者常认为基座坐标系就是世界坐标系。在单臂固定底座的场景下，这往往设为重合（$T_{base}^{world}=I$）。但在**移动操作机器人（Mobile Manipulation，如Mobile ALOHA）**中，基座是随移动底盘运动的。此时，基座坐标系相对于世界坐标系的变换 $T_{world}^{base}(t)$ 是随时间变化的函数。忽略这一点会导致导航与操作的解耦失败 。

末端坐标系 (End-Effector Frame, $F_{ee}$ / TCP)

这是机器人与环境交互的界面。它通常被定义在夹爪（Gripper）的中心点，被称为工具中心点（Tool Center Point, TCP）。

• 深度学习的输出目标： 在基于任务空间（Task Space）的模仿学习中，ACT策略输出的通常是 $F_{ee}$ 相对于 $F_{base}$ 的目标位姿。
• 偏移量（Offset）： 机械臂法兰盘（Flange）与实际TCP之间往往有一个刚性变换 $T_{flange}^{tcp}$。如果更换了夹爪，必须重新测量这个变换，否则旋转时TCP会画圆弧而不是原地旋转。

相机坐标系 (Camera Frame, $F_{cam}$)

这是机器人的“眼睛”。所有的视觉感知（检测框、分割掩码、深度图）最初都发生在这个坐标系中。

• 轴向地狱（Axis Convention Hell）： 这是一个极其经典的大坑。
  • 光学惯例（OpenCV）： Z轴指向前方（光轴方向），X轴向右，Y轴向下。
  • 机器人惯例（ROS/Robot）： X轴指向前方，Y轴向左，Z轴向上。
• 修正方法： 在将视觉坐标转换为机器人坐标时，必须插入一个静态的旋转矩阵 $R_{optical}^{robotic}$ 来修正轴向定义。忽略这个旋转会导致坐标系“翻转”，使得机器人的上下左右颠倒 。$$R_{optical}^{robotic}=\left[\begin{array}{ccc}0& 0& 1\\ -1& 0& 0\\ 0& -1& 0\end{array}\right]$$

手眼标定 (Hand-Eye Calibration)

如果说SE(3)是骨架，那么手眼标定就是连接视觉神经与运动肌肉的关节。手眼标定的核心任务是计算相机坐标系与机器人坐标系之间的刚性变换关系。没有这个变换，神经网络预测出的像素点 $(u,v)$ 永远无法准确转化为物理世界的抓取指令。

两种构型：Eye-in-Hand vs. Eye-to-Hand

标定的数学形式取决于相机的安装位置。这不仅影响公式，更深刻影响了机器人的感知能力和策略设计。

Eye-in-Hand (眼在手上)

• 物理描述： 相机被刚性固定在机械臂的末端法兰或夹爪上，随机械臂一起运动。
• 数学关系： 待求解的是 相机相对于末端 的变换 $T_{ee}^{cam}$（或其逆 $T_{cam}^{ee}$）。这个变换是恒定不变的。
• 优势：
  • 高精度： 随着机械臂接近目标物体，相机也随之接近，图像分辨率提高，感知精度在操作关键阶段达到峰值。
  • 规避遮挡： 机械臂可以通过运动寻找最佳视角，绕过遮挡物。
• 劣势：
  • 视场受限（FOV）： 离物体太近时可能无法看到全局。
  • 运动模糊： 机械臂快速运动时图像会模糊，影响实时推理。
• 应用场景： 精密装配、插入任务、焊接 。

Eye-to-Hand (眼在手外)

• 物理描述： 相机固定在环境中（如三脚架、天花板或桌面支架），不随机械臂运动。
• 数学关系： 待求解的是 相机相对于基座 的变换 $T_{base}^{cam}$。这个变换是恒定不变的。
• 优势：
  • 全局视野： 能够监控整个工作空间，适合多物体规划。
  • 图像稳定： 相机静止，无运动模糊。
• 劣势：
  • 遮挡问题： 机械臂在操作时，其本体很容易遮挡住相机对目标的视线（Self-Occlusion）。
  • 精度随距离衰减： 无论怎么移动，相机与物体的距离基本不变，无法通过抵近观察提升精度。
• 应用场景： 物流分拣、码垛、全局监控 。

深度整合（ALOHA模式）：

像Mobile ALOHA这样的先进系统通常采用混合架构——两个手腕相机（Eye-in-Hand）用于精细操作，两个顶部相机（Eye-to-Hand）用于全局感知。这就要求系统同时维护多套标定参数，并在推理时融合不同视角的特征 。

核心方程解析：$AX=XB$

手眼标定的数学本质是求解方程 $AX=XB$。这是一类经典的矩阵方程，其中 $A$、$B$、$X$ 都是 $4\times 4$ 的齐次变换矩阵。

变量定义

为了求解未知的 $X$，我们需要移动机器人多次，记录下多组数据。假设我们在时刻 $t_1$ 和 $t_2$ 记录了两组状态。

• 对于 Eye-in-Hand 系统：
  • $X$ (未知量)： 也就是 $T_{ee}^{cam}$。
  • $A$ (手的相对运动)： 机械臂末端从 $t_1$ 到 $t_2$ 的运动。
    $$A=(T_{base}^{ee}(t_2){)}^{-1}\cdot T_{base}^{ee}(t_1)$$这里求逆的顺序非常关键，且容易出错。A代表的是在基座坐标系下观察到的末端相对位移。$B$ (眼的相对运动)： 相机从 $t_1$ 到 $t_2$ 的运动。这通常是通过观测一个静止的标定板（如棋盘格、AprilTag）计算出来的。$$B=T_{cam}^{board}(t_2)\cdot (T_{cam}^{board}(t_1){)}^{-1}$$因为标定板是静止的，相机相对于标定板的变动反映了相机在空间中的绝对运动。
    方程推导：由于“手”带着“眼”动，手转过的角度和位移，在经过 $X$ 变换后，必须等于眼转过的角度和位移。即构成了闭环约束：$$A\cdot X=X\cdot B$$
• 对于 Eye-to-Hand 系统：
  • $X$ (未知量)： 也就是 $T_{base}^{cam}$。
  • 方程形式依然是 $AX=XB$，但 $A$ 和 $B$ 的物理意义会发生互换或求逆，具体取决于坐标系的定义方向 。

求解方法

$AX=XB$ 看似简单，但因为 $A,B,X\in SE(3)$，包含旋转矩阵，所以不能简单地移项求解（矩阵除法不存在，且旋转是非线性的）。

• 分离法 (Separable Solutions, 如 Tsai-Lenz 算法)：
  • 原理： 先只看旋转部分 $R_A{R}_X=R_X{R}_B$。利用旋转轴的不变性（旋转轴在变换前后方向不变），可以先解出 $R_X$。
  • 得到 $R_X$ 后，带入平移部分的方程 $(R_A-I)t_X=R_X{t}_B-t_A$，这就变成了一个线性方程组，可以解出 $t_X$。
  • 缺点： 误差累积。第一步旋转计算的误差会传播并放大到第二步的平移计算中 。
• 联立法 (Simultaneous Solutions, 如 Kronecker Product / Dual Quaternions)：
  • 原理： 使用对偶四元数（Dual Quaternions）或克罗内克积（Kronecker Product）将旋转和平移耦合在一起进行优化。
  • 优点： 精度更高，更鲁棒，是现代计算机视觉库（如OpenCV的 cv2.calibrateHandEye）的首选默认算法 。
• 迭代优化法 (Iterative Optimization)：
  • 原理： 在解析解的基础上，定义一个非线性损失函数（如重投影误差），使用 Levenberg-Marquardt 算法进行微调。$$\underset{X}{\min }\sum _i\Vert \text{project}(T_{base}^{ee}\cdot X\cdot P_{board})-\text{detected\_pixels}{\Vert }^2$$

误差的代价：为什么“不准”是致命的？

旋转误差的放大效应

假设标定结果中，旋转矩阵 $R_{base}^{cam}$ 存在仅仅 1度 ($1^{\circ }$) 的误差。

1. 如果物体距离相机 10厘米，位置误差约为 $0.1\times \sin (1^{\circ })\approx 1.7\text{mm}$。这可能还可以接受。
2. 如果物体距离相机 1米（常见的机械臂工作空间），位置误差约为 $1.0\times \sin (1^{\circ })\approx 1.7\text{cm}$。1.7厘米的误差意味着什么？
3. 对于抓取任务：二指夹爪的开合宽度通常只有几厘米，1.7cm的偏差会导致夹爪直接撞击物体侧面，而不是包络物体。
4. 对于插入任务（如USB插拔）：这是完全毁灭性的失败。

深度歧义 (Depth Ambiguity)

从像素 $(u,v)$ 恢复 $(x,y,z)$ 的过程叫做反投影（Back-projection）。$${\mathbf{P}}_{cam}=z_{depth}\cdot K^{-1}\cdot \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$这里必须依赖深度值 $z_{depth}$。如果手眼标定不准，导致相机光心位置 $O_{cam}$ 估计错误，那么即使深度读数是准的，整个射线（Ray）在世界坐标系中的发射原点和方向也是错的。这会导致“幻影物体”现象——机器人以为物体在 A 处，实际上物体在 B 处 。
实战教训： 在调试机器人抓取失败时，90%的情况是标定问题。不要先去调神经网络的参数，先检查你的标定重投影误差（Reprojection Error）。如果平均误差超过 2-3 个像素，或者旋转误差超过 0.5 度，必须重新标定。

旋转表示法

在ACT或Diffusion Policy等现代策略网络中，网络需要输出一个代表末端姿态的张量。选择何种数学形式来表示旋转（Rotation Representation），直接决定了模型的训练收敛速度和最终性能。这就是著名的“旋转表示战争”。

欧拉角 (Euler Angles)：直观但有毒

欧拉角（Roll, Pitch, Yaw）是将旋转分解为绕三个轴的连续旋转。
5. 直观性： 人类很容易理解“低头（Pitch）”、“转头（Yaw）”。
6. 致命缺陷：万向锁 (Gimbal Lock)。
- 当中间的旋转轴转到 $90^{\circ }$ 时，第三个轴和第一个轴会重合。此时，系统失去了一个自由度。
- 在深度学习中： 万向锁意味着在奇异点附近，微小的姿态变化需要欧拉角数值发生剧烈跳变（梯度爆炸）。
7. 不连续性： $179^{\circ }$ 和 $-179^{\circ }$ 在物理上只差 $2^{\circ }$，但在数值上差了 $358$。对于使用 MSE（均方误差）损失函数的神经网络来说，这简直是灾难。网络会试图输出平均值 $0^{\circ }$，这完全是错误的指向 。
8. 结论： 绝对不要在神经网络的输出层使用欧拉角。

四元数 (Quaternion)：标准但有坑

单位四元数 $\mathbf{q}=w+xi+yj+zk$ 是机器人学中的标准表示。
9. 优点： 无万向锁，计算效率高。
10. 致命缺陷：双倍覆盖 (Double Cover)。
- 四元数 $\mathbf{q}$ 和 $-\mathbf{q}$ 代表的是完全相同的旋转。
- 在深度学习中： 这是一个拓扑学问题。假设你在训练数据中有两帧相似的动作，一帧的真值是 $\mathbf{q}$，另一帧是 $-\mathbf{q}$（这是完全合法的）。神经网络作为函数拟合器，会试图去拟合这两个值的“平均值”。然而，$\mathbf{q}$ 和 $-\mathbf{q}$ 的欧氏平均值接近零向量 $0$，而零向量不是一个合法的单位四元数（不代表任何旋转）。
- 这会导致训练震荡，loss 难以下降。虽然可以通过特定的 Loss 函数（如 $\min (\Vert \mathbf{q}-\hat{\mathbf{q}}\Vert ,\Vert \mathbf{q}+\hat{\mathbf{q}}\Vert )$）来缓解，但这增加了优化的复杂性 。

D旋转表示法 (6D Rotation Representation)：ACT的首选

为了解决上述问题，Zhou et al. 在 CVPR 2019 发表了里程碑式的论文 “On the Continuity of Rotation Representations in Neural Networks”。

核心理论：连续性的代价

论文从拓扑学证明了一个结论：对于三维旋转群 SO(3)，不存在从欧氏空间 ${\mathbb{R}}^n$ 到 SO(3) 的连续满射，除非 $n\ge 5$。这意味着，任何试图用 3 个数（欧拉角）或 4 个数（四元数）来表示旋转的神经网络，注定在输出空间中存在不连续点。这些不连续点就是模型训练困难、预测抖动的根源 。

6D 表示法是如何工作的？

为了获得完美的连续性，我们选择冗余的 6 维向量作为输出。ACT策略网络的输出包含一个 6 维向量 $[{\mathbf{v}}_1,{\mathbf{v}}_2]$，其中 ${\mathbf{v}}_1,{\mathbf{v}}_2\in {\mathbb{R}}^3$。如何将其转换为 $3\times 3$ 的旋转矩阵 $R$？使用 格拉姆-施密特正交化 (Gram-Schmidt Orthogonalization)：
1. 归一化第一列： 取 ${\mathbf{v}}_1$，归一化得到 $R$ 的第一列 $\mathbf{x}$。$$\mathbf{x}=\frac{{\mathbf{v}}_1}{\Vert {\mathbf{v}}_1\Vert }$$
2. 正交化第二列： 取 ${\mathbf{v}}_2$，减去它在 $\mathbf{x}$ 方向的分量，得到垂直于 $\mathbf{x}$ 的向量。$${\mathbf{y}}^{\prime }={\mathbf{v}}_2-(\mathbf{x}\cdot {\mathbf{v}}_2)\mathbf{x}$$$$\mathbf{y}=\frac{{\mathbf{y}}^{\prime }}{\Vert {\mathbf{y}}^{\prime }\Vert }$$
3. 计算第三列： 利用右手定则，通过叉积得到第三列 $\mathbf{z}$。$$\mathbf{z}=\mathbf{x}\times \mathbf{y}$$
4. 组合： $R=[\mathbf{x},\mathbf{y},\mathbf{z}]$。

为什么它是深度学习的最佳选择？

• 连续性： 输入向量 ${\mathbf{v}}_1,{\mathbf{v}}_2$ 的微小变化，永远只会导致旋转矩阵 $R$ 的微小变化。没有突变，没有奇异点。
• 无约束优化： 神经网络的输出可以是任意实数，不需要像四元数那样强制归一化（模长为1）。这种“软约束”让梯度下降算法更容易在参数空间中“游走”，从而更快收敛 。
• 实证结果： 在人体姿态估计、物体6D位姿估计以及像ACT这样的机器人操作任务中，6D表示法的误差显著低于欧拉角和四元数。

附录：欧拉角四元数转换代码

import numpy as np
from scipy.spatial.transform import Rotation as R

def test_euler_to_quaternion_and_back():
    print("Testing Euler <-> Quaternion Conversion...\n")

    # 1. 定义一组测试用的欧拉角 (Roll, Pitch, Yaw) - 单位：弧度
    # 示例：绕X轴30度，绕Y轴45度，绕Z轴60度
    original_euler = np.array([np.deg2rad(30), np.deg2rad(45), np.deg2rad(60)])
    print(f"Original Euler (radians): {original_euler}")
    print(f"Original Euler (degrees): {np.rad2deg(original_euler)}")

    # 2. 欧拉角 -> 四元数
    # 注意：'xyz' 表示旋转顺序为 x轴 -> y轴 -> z轴（静态坐标系，extrinsic）
    # 如果是动态坐标系（intrinsic），通常用大写 'XYZ'。机器人学常用 'xyz' 或 'zyx'。
    # 这里假设是标准的 xyz 顺序，您需要根据实际机械臂定义调整。
    rotation = R.from_euler('xyz', original_euler)
    quaternion = rotation.as_quat() # 格式通常为 [x, y, z, w] (scipy默认)
    print(f"\nConverted Quaternion [x, y, z, w]: {quaternion}")

    # 3. 四元数 -> 欧拉角
    rotation_back = R.from_quat(quaternion)
    recovered_euler = rotation_back.as_euler('xyz')
    print(f"\nRecovered Euler (radians): {recovered_euler}")
    print(f"Recovered Euler (degrees): {np.rad2deg(recovered_euler)}")

    # 4. 验证误差
    error = np.abs(original_euler - recovered_euler)
    print(f"\nAbsolute Error: {error}")
    
    # 检查误差是否在极小范围内
    if np.all(error < 1e-6):
        print("\n✅ SUCCESS: Conversion is accurate!")
    else:
        print("\n❌ FAILURE: Significant error detected.")
        # 提示：有时候欧拉角存在多解（如万向节死锁），虽然数值不同但表示的旋转可能是一样的。
        # 这种情况下需要比较旋转矩阵是否一致。
        matrix_orig = R.from_euler('xyz', original_euler).as_matrix()
        matrix_recov = R.from_euler('xyz', recovered_euler).as_matrix()
        matrix_error = np.abs(matrix_orig - matrix_recov).sum()
        if matrix_error < 1e-6:
             print("  (However, the rotation matrices match, so the rotation is actually correct. Euler angles have multiple valid representations.)")

    print("-" * 30)
    
    # 5. 批量测试随机数据
    print("\nRunning Batch Test with Random Angles...")
    num_tests = 1000
    random_eulers = (np.random.rand(num_tests, 3) - 0.5) * 2 * np.pi # [-pi, pi]
    
    rotations = R.from_euler('xyz', random_eulers)
    quats = rotations.as_quat()
    recovered = R.from_quat(quats).as_euler('xyz')
    
    # 处理欧拉角周期性问题 (-pi 和 pi 是同一个位置) 和别名问题
    # 最可靠的方法是比较旋转矩阵或四元数距离
    # 这里简单起见，我们比较重构后的四元数和原四元数是否一致（注意 q 和 -q 代表同一旋转）
    
    rotations_recovered = R.from_euler('xyz', recovered)
    # 计算两个旋转之间的角度差
    diff_rotations = rotations_recovered * rotations.inv()
    diff_angles = diff_rotations.magnitude()
    
    max_error = np.max(diff_angles)
    print(f"Max rotation error in {num_tests} random samples: {max_error:.2e} radians")
    
    if max_error < 1e-5:
        print("✅ Batch Test Passed!")
    else:
        print("❌ Batch Test Failed!")

if __name__ == "__main__":
    test_euler_to_quaternion_and_back()

参考文献

Geist et al., “On the Continuity of Rotation Representations in Neural Networks” (Review/Extensions), 2024.
Chi et al., “Diffusion Policy: Visuomotor Policy Learning via Action Diffusion,” 2023.
Zhou et al., “On the Continuity of Rotation Representations in Neural Networks,” CVPR 2019.
Tsai, R. Y., & Lenz, R. K., “A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration,” IEEE Transactions on Robotics and Automation, 1989.
Zhao et al., “Learning Fine-Grained Bimanual Manipulation with Low-Cost Hardware,” (ACT Paper), 2023.
De Los Santos, “Homogeneous Transformations in Robotics: A Comprehensive Tutorial,” Medium.
OpenCV Documentation, “Camera Calibration and 3D Reconstruction.”
Shah, M., “Solving the Robot-World/Hand-Eye Calibration Problem Using the Kronecker Product,” ASME, 2013.
Wikipedia, “Hand-eye calibration problem.”
CS224R Report, “Adaptive Action Chunking with Transformers.”
Hugging Face, “Action Chunking with Transformers (ACT) Documentation.”