机械臂末端坐标系表达方式

PART 01

向量定义

在机器人学和计算机图形学中，有两个术语有严格的区分。

RPY 角 (Roll-Pitch-Yaw angles)

方向向量/法向向量 (Direction Vector / Normal Vector)

PART 02

方向向量/法向向量

指表示方向的单位向量，即三维空间中的一个归一化矢量。

n

（法向）就是一个方向向量。

n_x, n_y, n_z

是这个向量的三个分量，即向量在基坐标系 X、Y、Z 轴上的投影。

这种表示也被称为 方向余弦 (Direction Cosines)，因为三个分量分别是该向量与三个坐标轴夹角的余弦值。

特点

PART 03

RPY 角

一种用三个 顺序旋转角度 表示空间姿态的欧拉角约定，RPY 来源于船舶/航空术语，由绕固定轴的旋转序列定义：

• Roll（滚转） ：绕 X 轴 旋转
• Pitch（俯仰） ：绕 Y 轴 旋转
• Yaw（偏航） ：绕 Z 轴 旋转

通常是地球固定轴按 Z → Y → X 顺序施加旋转。

特点

PART 04

区别对比

RPY 和法向向量 n的关系

RPY是“过程的描述”，法向向量是“结果”。

数学关系： 给定一个旋转矩阵 R，可以用 RPY 序列表达它的旋转过程，反过来，给定 RPY 角度，可以计算出旋转矩阵。

而法向向量 n 就是旋转矩阵的 第三列 ：

无论旋转用欧拉角、四元数还是其他方式表示， 法向向量是唯一的 ，它是姿态的最终结果。

例： 末端先偏航 30°，再俯仰 45°。

• RPY：(yaw=30°, pitch=45°, roll=0°)
• 法向向量：指向空间的某一个具体方向，不再是角度，而是三个分量，例如 (0.353, 0.612, 0.707)。

RPY 变化 ⇒ 旋转矩阵变化 ⇒ 法向向量随之变化。 RPY 里的“pitch”好像和法向关系紧密，但实际上法向方向同时受 yaw、pitch 甚至 roll 的影响 。

PART 05

末端姿态表达

用法向向量 n，不能用 RPY 角度直接作方向向量

理由有两点：

1. 物理意义清晰，直接可操作 n 就是工具 Z 轴的指向，即接触法向。位移计算直接用 n：Δp=±Δd⋅n+表示前进，− 表示后退。用 RPY 无法直接做这个，不能把“pitch=30°”直接乘到位移上。
2. 避免欧拉角的多个坑

• 奇异点：pitch=±90°时，roll 和 yaw 混叠，方向突变。
• 不唯一：RPY(30°,45°,0°) 和 RPY(210°,135°,180°) 姿态完全相同，角度值却差很远。
• 顺序耦合：Yaw 改变时，工具 Z 轴方向也改变，并不只是 pitch 的事。

而 n是数值唯一的物理方向，绕过所有这些麻烦。

具体使用方法

在控制循环的第一步，做完正运动学后 不要用欧拉角 ，而是直接取旋转矩阵的第三列：

R = forward_kinematics(q)              # 得到 3x3 旋转矩阵
n = R[:, 2]                            # 取第三列 → 工具 Z 轴方向
n = n / norm(n)                        # 保证单位向量

然后用它合成位移：

delta_p = delta_x * t_x + delta_y * t_y + delta_z * n

这里 tx 、 ty 分别是旋转矩阵的第一列和第二列（切向）， n是法向。

PART 06

总结

RPY 只是计算中间的过渡产物，不适合直接用于控制。

从正运动学得到旋转矩阵后， 立即提取它的三个列向量作为工具坐标系的方向基 。

法向控制用第三列 n，切向控制用第一、二列 tx,ty 。