【具身智能】机器人如何“看见”动作？一文读懂 3D 位姿与 5 种旋转表示法

当我们教一个机器人执行任务时，比如“拿起桌上的杯子”，我们到底在教它什么？我们不能只说“去拿杯子”。相反，我们必须给它一串精确的、机器可读的指令。

这个指令的核心，就是 “位姿 (Pose)”。

在机器人学和 3D 视觉中，位姿是描述一个物体在空间中完整状态的术语。这篇博客将深入探讨这个概念，特别是描述“朝向”的五种主流方法。理解这些，你就能明白为什么现代机器人（尤其是那些由机器学习驱动的）会使用一些看起来非常“奇怪”的数学表示。

1. 基础：位姿 (Pose) = 位置 + 姿态

一个完整的“位姿”由两部分组成：

1. 位置 (Position)：物体在世界坐标系中的哪个点。
2. 姿态 (Orientation/Rotation)：物体的朝向。

📍 位置 (Position)：简单明了

这部分很简单。我们通常用一个 3D 向量 (x,y,z)(x, y, z)(x,y,z) 来表示，这就是我们都熟悉的笛卡尔 (Cartesian) 坐标。它回答了“物体在哪里？”

🔄 姿态 (Orientation)：真正的挑战

这部分复杂得多。它回答了“物体朝向何方？”

想象一个杯子在桌上的 (x,y,z)(x, y, z)(x,y,z) 位置是固定的，但它可以“正着放”、“倒着放”或“躺着放”——这就是姿态。

描述 3D 旋转有非常多种方式，每种都有其独特的优缺点。下面，我们将详细介绍五种最主要的“姿态”表示法，从最直观的到最适合机器学习的。

2. 🤖 五种主要的“姿态”表示法

1. 欧拉角 (Euler Angles) & RPY

这是最直观、最“人类友好”的一种方式。

• 是什么：用三个角度来描述旋转。
• 如何工作：想象有三个主轴（X, Y, Z）。我们按特定顺序绕这些轴旋转三个角度。例如，“ZYX 顺序” 意味着：先绕 Z 轴转 α\alphaα，再绕 Y 轴转 β\betaβ，最后绕 X 轴转 γ\gammaγ。
• RPY (Roll, Pitch, Yaw)：这是欧拉角的一种常见约定，通常（但不总是）对应于 ZYX 顺序。
  • Roll (横滚): 绕 X 轴（前进方向）
  • Pitch (俯仰): 绕 Y 轴（侧向）
  • Yaw (偏航): 绕 Z 轴（垂直方向）
• 优点：
  • 非常直观，容易理解。
  • 只用 3 个数字，非常紧凑。
• 缺点：
  • 万向锁 (Gimbal Lock)：一个致命缺陷。在特定姿态下（例如，Pitch 为 90 度时），Roll 和 Yaw 会“合并”成同一个旋转，导致你失去一个自由度。这在机器人和动画中是灾难性的。
  • 歧义性：必须严格定义旋转顺序（如 ZYX, XYZ, ZXZ…）。

Python 代码示例 (使用 scipy)

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 的特殊矩阵，记为 RRR。
• 如何工作：它直接描述了“旋转后的坐标系”在“原始坐标系”中的表示。RRR 的三个列向量分别是旋转后的 X’, Y’, Z’ 轴在原始 X, Y, Z 坐标系下的表示。它可以用来变换一个向量：vnew=R⋅voldv_{\text{new}} = R \cdot v_{\text{old}}vnew​=R⋅vold​。
• 优点：
  • 没有万向锁，没有歧义。
  • 组合旋转非常简单（矩阵乘法）：Rtotal=R2⋅R1R_{\text{total}} = R_2 \cdot R_1Rtotal​=R2​⋅R1​。
  • 求逆（反向旋转）非常简单：R−1=RTR^{-1} = R^TR−1=RT（旋转矩阵是正交矩阵）。
• 缺点：
  • 冗余：用 9 个数字来表示 3 个自由度。
  • 约束：这 9 个数字必须满足严格的数学约束（RTR=IR^T R = IRTR=I 且 det⁡(R)=1\det(R) = 1det(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\mathbf{v}v 和一个角度 θ\thetaθ 来描述。
• 如何工作：表示“绕着 v\mathbf{v}v 轴旋转 θ\thetaθ 角度”。例如，轴 (0,0,1)(0, 0, 1)(0,0,1) 和角度 90∘90^\circ90∘ 表示“绕着 Z 轴旋转 90 度”。
• 优点：
  • 物理意义清晰。
  • 比欧拉角更稳定。
• 缺点：
  • 不方便直接进行代数运算（如组合旋转）。
  • 不连续性：当 θ\thetaθ 达到 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, x, y, z)q=(w,x,y,z)，或写作 q=w+xi+yj+zkq = w + x\mathbf{i} + y\mathbf{j} + z\mathbf{k}q=w+xi+yj+zk。
• 如何工作：它是“轴-角”表示法的一种数学扩展。
  • 对于绕单位轴 v=(vx,vy,vz)\mathbf{v} = (v_x, v_y, v_z)v=(vx​,vy​,vz​) 旋转 θ\thetaθ 角度：
  • w=cos⁡(θ/2)w = \cos(\theta/2)w=cos(θ/2)
  • x=vx⋅sin⁡(θ/2)x = v_x \cdot \sin(\theta/2)x=vx​⋅sin(θ/2)
  • y=vy⋅sin⁡(θ/2)y = v_y \cdot \sin(\theta/2)y=vy​⋅sin(θ/2)
  • z=vz⋅sin⁡(θ/2)z = v_z \cdot \sin(\theta/2)z=vz​⋅sin(θ/2)
• 约束：必须是“单位四元数”，即 w2+x2+y2+z2=1w^2 + x^2 + y^2 + z^2 = 1w2+x2+y2+z2=1。
• 优点：
  • 没有万向锁！
  • 计算最高效：组合旋转（四元数乘法）和插值（SLERP）都非常快。
  • 紧凑：只需 4 个数字（有 1 个约束）。
• 缺点：
  • 极不直观：人类无法直观想象一个四元数代表的朝向。
  • 双重覆盖：qqq 和 −q-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=[c1,c2,c3]R = [\mathbf{c}_1, \mathbf{c}_2, \mathbf{c}_3]R=[c1​,c2​,c3​]（ci\mathbf{c}_ici​ 是列向量）。
  • 由于矩阵是正交的 (c1,c2,c3\mathbf{c}_1, \mathbf{c}_2, \mathbf{c}_3c1​,c2​,c3​ 互相垂直且长度为 1)，第三列 c3\mathbf{c}_3c3​ 可以通过前两列的叉乘得到：c3=c1×c2\mathbf{c}_3 = \mathbf{c}_1 \times \mathbf{c}_2c3​=c1​×c2​。
  • 因此，我们只需要存储前两列 c1\mathbf{c}_1c1​ 和 c2\mathbf{c}_2c2​ 就足够了！
  • c1\mathbf{c}_1c1​ (3 个数) + c2\mathbf{c}_2c2​ (3 个数) = 6 个数。
• 优点 (ML 领域)：
  • 连续：附近的旋转在 6D 空间中也是附近的。
  • 无歧义：每个有效的旋转矩阵对应一个唯一的 6D 表示。
  • 无约束（或易于约束）：神经网络可以直接输出 6 个浮点数，无需担心万向锁或 w2+x2+y2+z2=1w^2+x^2+y^2+z^2=1w2+x2+y2+z2=1 这样的复杂约束。
• 缺点：
  • 冗余：用 6 个数表示 3 个自由度。
  • 不直观：和四元数一样，人无法看懂。

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

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

1. 欧拉角的“跳变”问题：
   想象一下，一个物体从 359° 旋转到 1°。对我们来说，这只是一个很小的 2° 变动。但对于神经网络来说，它看到的是一个从 359 到 1 的巨大“跳跃”。这种不连续性使得学习平滑的动作轨迹变得极其困难。
2. 四元数/旋转矩阵的“约束”问题：
   • 四元数：神经网络输出 4 个数 (w,x,y,z)(w, x, y, z)(w,x,y,z) 后，我们必须手动将其归一化 (Normalize)，以满足 w2+x2+y2+z2=1w^2+x^2+y^2+z^2=1w2+x2+y2+z2=1 的约束。这个归一化步骤是一个“黑盒”，会破坏损失函数的梯度流，使训练不稳定。
   • 旋转矩阵：让网络输出 9 个数并同时满足 RTR=IR^T R = IRTR=I 和 det⁡(R)=1\det(R) = 1det(R)=1 的约束，几乎是不可能的。

6D 旋转是完美的“甜点”：

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

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

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

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

import numpy as np defsix_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 旋转）的动作向量时，你就彻底明白它到底在做什么了。