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

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

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

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

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

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

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

📍 位置 (Position)：简单明了

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

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

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

想象一个杯子在桌上的 (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…）。

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 的特殊矩阵，记为 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 个数）呢？

欧拉角的'跳变'问题：想象一下，一个物体从 359° 旋转到 1°。对我们来说，这只是一个很小的 2° 变动。但对于神经网络来说，它看到的是一个从 359 到 1 的巨大'跳跃'。这种不连续性使得学习平滑的动作轨迹变得极其困难。
四元数/旋转矩阵的'约束'问题：
- 四元数：神经网络输出 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. 总结：没有最好，只有最合适

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

表示法	维度	优点	缺点
欧拉角 (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 旋转）的动作向量时，你就彻底明白它到底在做什么了。

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

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

📍 位置 (Position)：简单明了

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

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

1. 欧拉角 (Euler Angles) & RPY

2. 旋转矩阵 (Rotation Matrix)

3. 轴 - 角 (Axis-Angle)

4. 四元数 (Quaternion)

5. 6D 旋转 (6D Rotation)

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

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

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

更多推荐文章

相关免费在线工具

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

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

📍 位置 (Position)：简单明了

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

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

1. 欧拉角 (Euler Angles) & RPY

2. 旋转矩阵 (Rotation Matrix)

3. 轴 - 角 (Axis-Angle)

4. 四元数 (Quaternion)

5. 6D 旋转 (6D Rotation)

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

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

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

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具