机器人 3D 位姿与 5 种旋转表示法详解

2. 旋转矩阵 (Rotation Matrix)

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

• 是什么：一个 3x3 的特殊矩阵，记为 R。
• 如何工作：它直接描述了'旋转后的坐标系'在'原始坐标系'中的表示。R 的三个列向量分别是旋转后的 X', Y', Z' 轴在原始 X, Y, Z 坐标系下的表示。它可以用来变换一个向量：v_new = R · v_old。
• 优点：
  • 没有万向锁，没有歧义。
  • 组合旋转非常简单（矩阵乘法）：R_total = R_2 · R_1。
  • 求逆（反向旋转）非常简单：R^-1 = R^T（旋转矩阵是正交矩阵）。
• 缺点：
  • 冗余：用 9 个数字来表示 3 个自由度。
  • 约束：这 9 个数字必须满足严格的数学约束（R^T R = I 且 det(R) = 1）。在计算中，尤其是机器学习中，要保持这个约束很困难。

Python 代码示例

# 绕 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 单位向量 v 和一个角度 θ 来描述。
• 如何工作：表示'绕着 v 轴旋转 θ 角度'。例如，轴 (0, 0, 1) 和角度 90° 表示'绕着 Z 轴旋转 90 度'。
• 优点：
  • 物理意义清晰。
  • 比欧拉角更稳定。
• 缺点：
  • 不方便直接进行代数运算（如组合旋转）。
  • 不连续性：当 θ 达到 360 度时，会跳变回 0 度。

Python 代码示例

注意: 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+xi+yj+zk。
• 如何工作：它是'轴 - 角'表示法的一种数学扩展。
  • 对于绕单位轴 v=(vx,vy,vz) 旋转 θ 角度：
  • w=cos(θ/2)
  • x=vx·sin(θ/2)
  • y=vy·sin(θ/2)
  • z=vz·sin(θ/2)
• 约束：必须是'单位四元数'，即 w²+x²+y²+z²=1。
• 优点：
  • 没有万向锁！
  • 计算最高效：组合旋转（四元数乘法）和插值（SLERP）都非常快。
  • 紧凑：只需 4 个数字（有 1 个约束）。
• 缺点：
  • 极不直观：人类无法直观想象一个四元数代表的朝向。
  • 双重覆盖：q 和 -q 表示完全相同的旋转，这在优化中可能导致歧义。

Python 代码示例

# 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=[c_1, c_2, c_3]（ci 是列向量）。
  • 由于矩阵是正交的 (c_1, c_2, c_3 互相垂直且长度为 1)，第三列 c_3 可以通过前两列的叉乘得到：c_3 = c_1 × c_2。
  • 因此，我们只需要存储前两列 c_1 和 c_2 就足够了！
  • c_1 (3 个数) + c_2 (3 个数) = 6 个数。
• 优点 (ML 领域)：
  • 连续：附近的旋转在 6D 空间中也是附近的。
  • 无歧义：每个有效的旋转矩阵对应一个唯一的 6D 表示。
  • 无约束（或易于约束）：神经网络可以直接输出 6 个浮点数，无需担心万向锁或 w²+x²+y²+z²=1 这样的复杂约束。
• 缺点：
  • 冗余：用 6 个数表示 3 个自由度。
  • 不直观：和四元数一样，人无法看懂。

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

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

1. 欧拉角的'跳变'问题： 想象一下，一个物体从 359° 旋转到 1°。对我们来说，这只是一个很小的 2° 变动。但对于神经网络来说，它看到的是一个从 359 到 1 的巨大'跳跃'。这种不连续性使得学习平滑的动作轨迹变得极其困难。
2. 四元数/旋转矩阵的'约束'问题：
   • 四元数：神经网络输出 4 个数 (w,x,y,z) 后，我们必须手动将其归一化 (Normalize)，以满足 w²+x²+y²+z²=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 旋转）的动作向量时，你就彻底明白它到底在做什么了。