机器人 3D 位姿详解：5 种旋转表示法与机器学习应用

2. 旋转矩阵 (Rotation Matrix)

这是最基础、最没有歧义的表示方式。

• 是什么：一个 3x3 的特殊矩阵，记为 $R$。
• 如何工作：它直接描述了'旋转后的坐标系'在'原始坐标系'中的表示。$R$ 的三个列向量分别是旋转后的 X', Y', Z' 轴在原始 X, Y, Z 坐标系下的表示。它可以用来变换一个向量：$v_{new} = R \cdot v_{old}$。

优点：

• 没有万向锁，没有歧义。
• 组合旋转非常简单（矩阵乘法）：$R_{total} = R_2 \cdot R_1$。
• 求逆（反向旋转）非常简单：$R^{-1} = R^T$（旋转矩阵是正交矩阵）。

缺点：

• 冗余：用 9 个数字来表示 3 个自由度。
• 约束：这 9 个数字必须满足严格的数学约束（$R^T R = I$ 且 $\det(R) = 1$）。在计算中，尤其是机器学习中，保持这个约束很困难。

# 绕 Z 轴旋转 90 度
theta = np.pi / 2  # 90 degrees in radians
c, s = np.cos(theta), np.sin(theta)

# Z 轴旋转矩阵
mat_z = np.array([[c, -s, 0],
                  [s, c, 0],
                  [0, 0, 1]])
r_matrix = R.from_matrix(mat_z)

print("\n--- 旋转矩阵 ---")
print(r_matrix.as_matrix())

# 从矩阵转换回欧拉角
print(f"对应的欧拉角 (zyx): {r_matrix.as_euler('zyx', degrees=True)}")

3. 轴 - 角 (Axis-Angle)

这是一种在物理上很直观的表示。

• 是什么：用一个 3D 单位向量 $\mathbf{v}$ 和一个角度 $\theta$ 来描述。
• 如何工作：表示'绕着 $\mathbf{v}$ 轴旋转 $\theta$ 角度'。例如，轴 (0, 0, 1) 和角度 90° 表示'绕着 Z 轴旋转 90 度'。

优点：物理意义清晰；比欧拉角更稳定。

缺点：不方便直接进行代数运算（如组合旋转）；当 $\theta$ 达到 360 度时，会跳变回 0 度（不连续性）。

注意: scipy 中使用'旋转向量 (Rotation Vector)'，它是'轴 - 角'的一种紧凑表示，即 vector = axis * angle。

# 绕 Z 轴 (0, 0, 1) 旋转 90 度 (pi/2)
# 旋转向量 = [0, 0, 1] * (np.pi / 2)
rot_vec = np.array([0, 0, np.pi / 2])
r_rotvec = R.from_rotvec(rot_vec)

print("\n--- 轴 - 角 (旋转向量) ---")
print(f"对应的旋转矩阵:\n{r_rotvec.as_matrix()}")

4. 四元数 (Quaternion)

这是在图形学、机器人和航空中最常用的一种。

• 是什么：一个 4D 向量 $q=(w, x, y, z)$，或写作 $q = w + x\mathbf{i} + y\mathbf{j} + z\mathbf{k}$。
• 如何工作：它是'轴 - 角'表示法的一种数学扩展。
  • 对于绕单位轴 $\mathbf{v} = (v_x, v_y, v_z)$ 旋转 $\theta$ 角度：
  • $w=\cos(\theta/2)$
  • $x=v_x \cdot \sin(\theta/2)$
  • $y=v_y \cdot \sin(\theta/2)$
  • $z=v_z \cdot \sin(\theta/2)$

约束：必须是'单位四元数'，即 $w^2 + x^2 + y^2 + z^2 = 1$。

优点：

• 没有万向锁！
• 计算最高效：组合旋转（四元数乘法）和插值（SLERP）都非常快。
• 紧凑：只需 4 个数字（有 1 个约束）。

缺点：

• 极不直观：人类无法直观想象一个四元数代表的朝向。
• 双重覆盖：$q$ 和 $-q$ 表示完全相同的旋转，这在优化中可能导致歧义。

# Scipy 的 from_quat 期望 (x, y, z, w) 顺序
# 绕 Z 轴 90 度 (theta=pi/2)
# w = cos(pi/4) = 0.707
# z = sin(pi/4) = 0.707
quat_xyzw = np.array([0, 0, np.sin((np.pi/2)/2), np.cos((np.pi/2)/2)])
r_quat = R.from_quat(quat_xyzw)

print("\n--- 四元数 ---")
print(f"对应的旋转矩阵:\n{r_quat.as_matrix()}")

5. 6D 旋转 (6D Rotation)

这是专门为 机器学习 设计的表示法，也是 VLA（视觉 - 语言 - 动作）模型中的新宠。

• 是什么：用一个 6D 向量来表示 3D 旋转。
• 如何工作：
  • 它利用了旋转矩阵的冗余性。一个 3x3 旋转矩阵 $R=[\mathbf{c}_1, \mathbf{c}_2, \mathbf{c}_3]$（$\mathbf{c}_i$ 是列向量）。
  • 由于矩阵是正交的 ($\mathbf{c}_1, \mathbf{c}_2, \mathbf{c}_3$ 互相垂直且长度为 1)，第三列 $\mathbf{c}_3$ 可以通过前两列的叉乘得到：$\mathbf{c}_3 = \mathbf{c}_1 \times \mathbf{c}_2$。
  • 因此，我们只需要存储前两列 $\mathbf{c}_1$ 和 $\mathbf{c}_2$ 就足够了！
  • $\mathbf{c}_1$ (3 个数) + $\mathbf{c}_2$ (3 个数) = 6 个数。

优点 (ML 领域)：

• 连续：附近的旋转在 6D 空间中也是附近的。
• 无歧义：每个有效的旋转矩阵对应一个唯一的 6D 表示。
• 无约束（或易于约束）：神经网络可以直接输出 6 个浮点数，无需担心万向锁或 $w^2+x^2+y^2+z^2=1$ 这样的复杂约束。

缺点：

• 冗余：用 6 个数表示 3 个自由度。
• 不直观：和四元数一样，人无法看懂。

3. 深入 ML：为什么神经网络偏爱 6D 旋转？

这才是问题的核心。为什么不让神经网络直接输出欧拉角（3 个数）或四元数（4 个数）呢？

1. 欧拉角的'跳变'问题： 想象一下，一个物体从 359° 旋转到 1°。对我们来说，这只是一个很小的 2° 变动。但对于神经网络来说，它看到的是一个从 359 到 1 的巨大'跳跃'。这种不连续性使得学习平滑的动作轨迹变得极其困难。

2. 四元数/旋转矩阵的'约束'问题：

   • 四元数：神经网络输出 4 个数 $(w,x,y,z)$ 后，我们必须手动将其归一化 (Normalize)，以满足 $w^2+x^2+y^2+z^2=1$ 的约束。这个归一化步骤是一个'黑盒'，会破坏损失函数的梯度流，使训练不稳定。
   • 旋转矩阵：让网络输出 9 个数并同时满足 $R^T R = I$ 和 $\det(R) = 1$ 的约束，几乎是不可能的。

6D 旋转是完美的'甜点'：

神经网络可以直接、无约束地输出 6 个数字。我们只需要一个明确的、可微分的数学步骤，就能将这 6 个数'修复'回一个标准、有效的 3x3 旋转矩阵。

这个修复过程，就是 格拉姆 - 施密特正交化 (Gram-Schmidt process)。

4. 核心代码：从 6D 向量到旋转矩阵

下面，我们用 Python 和 NumPy 来实现这个转换。这就是在许多 VLA 模型脚本中能找到的核心函数。

import numpy as np

def six_d_to_rotation_matrix(d6: np.ndarray) -> np.ndarray:
    """
    将 6D 旋转表示 (旋转矩阵的前两列) 转换回一个 3x3 旋转矩阵。
    使用格拉姆 - 施密特正交化。

    参数:
        d6: shape (..., 6) 的 6D 旋转向量
    返回:
        R: shape (..., 3, 3) 的旋转矩阵
    """
    # 1. 将 6D 向量重塑为两个 3D 列向量 a1 和 a2
    # a1 是旋转矩阵的第一列，a2 是第二列
    a1 = d6[..., 0:3]
    a2 = d6[..., 3:6]

    # 2. 正交化：计算 b1 (第一列)
    # b1 = a1 / ||a1||
    b1 = a1 / np.linalg.norm(a1, axis=-1, keepdims=True)

    # 3. 正交化：计算 b2 (第二列)
    # b2_un = a2 - (a2·b1) * b1
    # (从 a2 中减去它在 b1 上的投影，使其与 b1 正交)
    proj_a2_on_b1 = np.sum(b1 * a2, axis=-1, keepdims=True) * b1
    b2_un = a2 - proj_a2_on_b1

    # b2 = b2_un / ||b2_un||
    b2 = b2_un / np.linalg.norm(b2_un, axis=-1, keepdims=True)

    # 4. 计算 b3 (第三列)
    # b3 = b1 x b2 (叉乘)
    b3 = np.cross(b1, b2, axis=-1)

    # 5. 将 b1, b2, b3 堆叠成 3x3 矩阵
    # (..., 3, 3)
    rot_mats = np.stack((b1, b2, b3), axis=-1)
    return rot_mats

5. 总结：没有最好，只有最合适

我们来总结一下这五种表示法：

正如你所见，没有'一个'完美的公式，而是一个转换网络。你总是在：A -> 旋转矩阵 -> B 或者 A -> 四元数 -> B 之间切换。

• 当你 (人类) 需要调试或设置一个姿态时，你使用欧拉角。
• 当物理引擎需要高效、稳定地计算插值时，它使用四元数。
• 当神经网络需要学习和输出一个平滑、无约束的姿态时，它使用 6D 旋转。

下次当你看到一个机器人模型输出一个 6 维或 7 维（3D 位置 + 4D 四元数）或 9 维（3D 位置 + 6D 旋转）的动作向量时，你就彻底明白它到底在做什么了。