机器人如何'看见'动作？
当我们训练机器人执行任务，比如'拿起桌上的杯子'，不能只给模糊指令。我们需要一串精确的、机器可读的数据。这个核心概念就是位姿 (Pose)。
在机器人学和 3D 视觉中，位姿描述了物体在空间中的完整状态。本文将深入探讨描述'朝向'的五种主流方法，理解这些数学表示，你就能明白为什么现代机器人（尤其是由机器学习驱动的）会使用一些看起来非常'奇怪'的数值。
1. 基础：位姿 = 位置 + 姿态
一个完整的位姿包含两部分：
- 位置 (Position)：物体在世界坐标系中的坐标点。
- 姿态 (Orientation/Rotation)：物体的朝向。
位置：简单明了
这部分通常用一个 3D 向量 (x, y, z) 表示，即我们熟悉的笛卡尔坐标。它回答了'物体在哪里？'
姿态：真正的挑战
这部分复杂得多，它回答了'物体朝向何方？'
想象一个杯子固定在桌面的 (x, y, z) 位置，但它可以'正着放'、'倒着放'或'躺着放'——这就是姿态的差异。
描述 3D 旋转有多种方式，每种都有其独特的优缺点。下面我们将详细介绍五种最主要的姿态表示法，从最直观的到最适合机器学习的。
2. 五种主要的姿态表示法
1. 欧拉角 (Euler Angles) & RPY
这是最直观、最'人类友好'的方式。
- 是什么：用三个角度来描述旋转。
- 如何工作：想象有三个主轴（X, Y, Z）。按特定顺序绕这些轴旋转三个角度。例如，'ZYX 顺序'意味着：先绕 Z 轴转 α，再绕 Y 轴转 β，最后绕 X 轴转 γ。
- RPY (Roll, Pitch, Yaw)：这是欧拉角的一种常见约定，通常对应于 ZYX 顺序。
- Roll (横滚): 绕 X 轴（前进方向）
- Pitch (俯仰): 绕 Y 轴（侧向）
- Yaw (偏航): 绕 Z 轴（垂直方向）
优点：非常直观，容易理解；只用 3 个数字，非常紧凑。
缺点：
- 万向锁 (Gimbal Lock)：致命缺陷。在特定姿态下（例如 Pitch 为 90 度时），Roll 和 Yaw 会合并成同一个旋转，导致失去一个自由度。
- 歧义性：必须严格定义旋转顺序（如 ZYX, XYZ, ZXZ…）。
import numpy as np
from scipy.spatial.transform import Rotation as R
# 创建一个欧拉角：绕 Z 轴 30 度，Y 轴 45 度，X 轴 60 度
# 'zyx' 是旋转顺序，degrees=True 表示单位是度
r_euler = R.from_euler('zyx', [30, 45, 60], degrees=True)
# 转换为旋转矩阵
print("--- 欧拉角 ---")
print(r_euler.as_matrix())
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 个数）呢？
-
欧拉角的'跳变'问题： 想象一下，一个物体从 359° 旋转到 1°。对我们来说，这只是一个很小的 2° 变动。但对于神经网络来说，它看到的是一个从 359 到 1 的巨大'跳跃'。这种不连续性使得学习平滑的动作轨迹变得极其困难。
-
四元数/旋转矩阵的'约束'问题：
- 四元数：神经网络输出 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. 总结：没有最好，只有最合适
我们来总结一下这五种表示法：
| 表示法 | 维度 | 优点 | 缺点 |
|---|---|---|---|
| 欧拉角 (RPY) | 3 | 直观，紧凑 | 万向锁, 顺序依赖 |
| 旋转矩阵 | 9 (3x3) | 无歧义，易于组合 | 冗余 (9 个数), 强约束 |
| 轴 - 角 | 4 (轴 3+ 角 1) | 物理意义清晰 | 不易于计算组合 |
| 四元数 | 4 | 无万向锁, 计算高效 | 极不直观，双重覆盖 |
| 6D 旋转 | 6 | ML 友好, 连续，无约束 | 冗余 (6 个数), 不直观 |
正如你所见，没有'一个'完美的公式，而是一个转换网络。你总是在：A -> 旋转矩阵 -> B 或者 A -> 四元数 -> B 之间切换。
- 当你 (人类) 需要调试或设置一个姿态时，你使用欧拉角。
- 当物理引擎需要高效、稳定地计算插值时，它使用四元数。
- 当神经网络需要学习和输出一个平滑、无约束的姿态时，它使用 6D 旋转。
下次当你看到一个机器人模型输出一个 6 维或 7 维（3D 位置 + 4D 四元数）或 9 维（3D 位置 + 6D 旋转）的动作向量时，你就彻底明白它到底在做什么了。
