PART 01 向量定义
在机器人学和计算机图形学中，有两个术语经常被摆在一起讨论，但它们其实有严格的区分——一个是RPY角（Roll-Pitch-Yaw angles），另一个是方向向量/法向向量（Direction Vector / Normal Vector）。别看它们都跟“方向”沾边，背后的逻辑和用处完全是两码事。下面逐步拆开看。
PART 02 方向向量/法向向量
方向向量，准确说是表示方向的单位向量，也就是三维空间里的一个归一化矢量。比如常看到的 n（法向），就是一个方向向量。它的三个分量 n_x, n_y, n_z 是向量在基坐标系 X、Y、Z 轴上的投影——这种表示也叫方向余弦，因为这三个分量恰好等于该向量与三个坐标轴夹角的余弦值。
特点一览：
| 特性 | 说明 |
|---|---|
| 定义形式 | 单位向量 (n_x, n_y, n_z)，满足 n_x² + n_y² + n_z² = 1 |
| 物理意义 | 直接指向空间中的一个具体方向 |
| 自由度 | 2 个独立自由度（因为单位向量，三个分量受一个约束） |
| 唯一性 | 对于一个给定方向，有唯一表示 |
| 奇异性 | 无奇点 |
| 直观性 | 可以直接用来做位移：位移 = 距离 × 方向向量 |
| 用途 | 几何计算、力控、导纳控制、坐标系与基坐标系之间的映射 |
| 特性 | 说明 |
|---|---|
| 定义形式 | 三个角度 (roll, pitch, yaw) |
| 物理意义 | 描述“怎么从基坐标系转到当前姿态”的过程 |
| 自由度 | 3 个独立参数 |
| 唯一性 | 不唯一：不同角度组合可表示同一姿态 |
| 奇异性 | 有万向节死锁：Pitch = ±90° 时，Roll 和 Yaw 自由度为 1 |
| 直观性 | 不直接；不能直接把角度乘到距离上做位移 |
| 用途 | 运动插补、用户界面显示、某些轨迹生成 |
PART 03 RPY 角
RPY 角是一种用三个顺序旋转角度表示空间姿态的欧拉角约定，名字来源于船舶/航空术语。它由绕固定轴的旋转序列定义：
- Roll（滚转）：绕 X 轴旋转
- Pitch（俯仰）：绕 Y 轴旋转
- Yaw（偏航）：绕 Z 轴旋转
通常按地球固定轴，顺序是 Z → Y → X 依次施加旋转。这个过程的每一步都依赖上一步的结果，所以它描述的是“怎么做”的过程，而不是“最终长什么样”。
PART 04 区别对比
| 比较维度 | RPY 角 | 法向向量 n |
|---|---|---|
| 表达什么 | 姿态本身的“旋转过程” | 某个轴的“最终方向” |
| 参数数量 | 3 | 3 个分量，2 个独立自由度 |
| 是否可直接用于位移 | 否 | 是（直接乘以距离） |
| 是否有奇异 | 有（Pitch=±90°） | 无 |
| 唯一性 | 差（同一姿态无数种表示） | 好（唯一方向） |
| 语义清晰度 | 依赖位姿序列定义 | 几何上唯一且不变 |
RPY 和法向向量 n 的关系：简单说，RPY 是“过程的描述”，法向向量是“结果”。
| 形式 | 含义 | 特点 |
|---|---|---|
| RPY 角度 | 描述“为了得到这个姿态，基坐标系按什么顺序旋转了多少度” | 三个数、有顺序依赖、不直观 |
| 法向向量 n | 描述“末端工具的 Z 轴最终指向哪里” | 一个方向向量、直接可用、无顺序歧义 |
数学关系也很清晰：给定一个旋转矩阵 R，可以用 RPY 序列表达它的旋转过程；反过来，给定 RPY 角度，可以计算出旋转矩阵。而法向向量 n 就是旋转矩阵的第三列：
无论旋转用欧拉角、四元数还是其他方式表示，法向向量是唯一的——它是姿态的最终结果。举个例子：末端先偏航 30°，再俯仰 45°。RPY 表示为 (yaw=30°, pitch=45°, roll=0°)；而法向向量则直接指向空间中的某一个具体方向，比如 (0.353, 0.612, 0.707) 这样三个分量。注意：RPY 变化 → 旋转矩阵变化 → 法向向量随之变化。很多人以为“pitch”和法向关系最紧密，但实际上法向方向同时受 yaw、pitch 甚至 roll 的影响。
PART 05 末端姿态表达
在机器人控制中，推荐用法向向量 n 来表达末端姿态，而不是直接用 RPY 角度作为方向向量。理由有两点：
- 物理意义清晰，直接可操作：n 就是工具 Z 轴的指向，即接触法向。位移计算可以直接用 n：Δp = +Δd·n 表示前进，− 表示后退。用 RPY 无法直接做这个，你不能把“pitch=30°”直接乘到位移上。
- 避免欧拉角的多个坑：奇异点（pitch=±90°时 roll 和 yaw 混叠，方向突变）；不唯一（RPY(30°,45°,0°) 和 RPY(210°,135°,180°) 姿态完全相同，角度值却差很远）；顺序耦合（yaw 改变时，工具 Z 轴方向也改变，并不只是 pitch 的事）。
而 n 是数值唯一的物理方向，绕过所有这些麻烦。
具体使用方法：在控制循环的第一步，做完正运动学后，不要用欧拉角，而是直接取旋转矩阵的第三列：
R = forward_kinematics(q)
n = R[:, 2]
n = n / norm(n)然后用它合成位移：
delta_p = delta_x * t_x delta_y * t_y delta_z * n这里 t_x、t_y 分别是旋转矩阵的第一列和第二列（切向），n 是法向。这样一来，控制信号直接基于几何方向，干净利落。
PART 06 总结
RPY 只是计算中间的过渡产物，不适合直接用于控制。从正运动学得到旋转矩阵后，立即提取它的三个列向量作为工具坐标系的方向基：法向控制用第三列 n，切向控制用第一、二列 t_x、t_y。这才是工程上顺手且可靠的做法。
