1.7 运动控制：坐标系·姿态·欧拉角·旋转·齐次变换

本文档需要配套对应视频观看，请联系助手获取视频（微信：ffcv1024）

为保证复现效果，请在资料中心下载最新版本的 Episode1 上位机（ 0.9.3 以上）、RoboDK 驱动脚本（1.1 以上）、API Demo（1.2 以上）。

1. 引言

上节课通过倒推可知，实现 3D 视觉抓取的核心在于准确获取目标的三维坐标并完成机械臂位姿映射。为此，需要掌握深度相机的使用、手眼标定、机械臂运动学等知识。其中具体问题及本课程使用的方法如下：

如何获取物体三维坐标
- 使用目标检测算法获取 RGB 图像中物体的几何中心
- 使用深度相机 D435 获取物体中心位置三维坐标
物体三维坐标转为机械臂基坐标系
- Aruco 眼在手外标定
机械臂如何运动到目标点
- 获取物体位置相对于机械臂基座坐标系的坐标：
  - 直接调用 Move XYZ
  - 或者使用逆运动学得到机械臂关节角度，调用 MoveJ：
- 六轴机械臂特殊处理：姿态是什么，如何保持固定

其中前 2 个知识我们再代码中将会具体讲解，这里先分析机械臂如何运动到目标点：

我们需要了解坐标系与姿态
我们需要知道有了位姿后，机械臂如何运动

温馨提示：本课程侧重实战应用，理论知识部分因篇幅所限无法深入展开。若您想系统学习机器人运动学原理，强烈推荐搭配观看《机器人运动学——林沛群》课程，林老师的讲解生动透彻、层层递进，能帮助您打下更扎实的理论基础！

2. 坐标系与姿态的基本概念

2.1 机械臂的基坐标系（Base Frame）

一般颜色对应轴规则：

X：红色

Y：绿色

Z：蓝色

定义：六轴机械臂的基坐标系（Base Coordinate System）通常是整台机械臂进行运动学和轨迹规划的参考坐标系，也是后续建立各关节或连杆坐标系（如 Denavit-Hartenberg 架构）时的“零点”。对基坐标系的定义并没有绝对统一的国际标准，不同厂商（如 KUKA、ABB、FANUC、安川等）可能有不同的默认设定方式，但在大多数情况下会遵循右手坐标系和主要关节轴朝向的一些通用原则。
- 把机械臂的底座看作"家"的位置（基坐标系），所有目标点都用"离家多远（X/Y/Z）"来描述。
- 类比：就像告诉快递员"我家东边 3 米，北边 2 米，楼上 1 米"的位置。
多数六轴机械臂将基坐标系的原点放在底座安装面或机械臂第一关节（J1）所在的旋转轴中心线上（或两者某种结合）。例如，一些机型会把原点放在底座与第一关节轴心线相交的位置。

这样做的优点是：运动学建模中，第一关节的旋转就可直接围绕 Z 轴（或 Y 轴、X 轴）展开，从而简化 D-H 参量或者齐次变换矩阵的表达，以便在运动学建立与后续控制中有清晰且简化的数学描述。
如何确定坐标系的三轴方向：一般用右手定则：
- Z 轴：通常与第一关节的旋转轴重合。若第一关节是“肩”关节且绕垂直方向旋转，那么 Z 轴可以沿垂直方向；若第一关节是水平旋转，则可能把 Z 轴水平设置，以保持和关节旋转对齐。但在更多的通用场合（尤其是将机器人立式安装在地面时），Z 轴也常设为垂直向上。
- X 轴、Y 轴：在确定 Z 轴后，根据右手坐标系的规则及具体厂商/项目的需求，进一步选定 X 轴、Y 轴的朝向。常见做法是：
  - X 轴：指向机械臂的前方或某个主要工作区域方向。
  - Y 轴：由右手定则自动确定，即中指朝向 Z 轴，拇指指向 X 轴，此时食指指向 Y 轴。
课程使用的 Episode 1 六轴机械臂也遵循此原则。
与 D-H 参数建立的关系：后文会介绍。
练习：
- 打开 RoboDK 仿真软件，导入 Episode 1 机械臂，显示参考坐标系，并放大坐标系
- 自己用右手定则确定一下坐标系三轴方向

2.2 工具/末端坐标系（Tool Frame / End Effector Frame）

末端法兰	负压吸盘

定义：在工业机器人领域，工具坐标系（Tool Frame / End Effector Frame）是相对于机械臂末端法兰或实际安装的工具（如抓手、焊枪、喷枪等）建立的坐标系，用于描述工具在空间中的位置与姿态，以及进行示教、轨迹规划、碰撞检测等操作时对末端执行器进行更直观的控制。
工具坐标系的放置位置
- 末端法兰处（如上左图）：
  - 对于通用的六轴机器人，其末端通常会有一个机械法兰（Tool Flange），用来安装抓手、焊枪等执行器。最基础的“工具坐标系”往往默认就定义在这个法兰上，原点即法兰中心或安装面中心。
  - 这样做能方便控制器直接计算“末端姿态”，并对外部加装的工具参数进行修正（TCP 偏移）。
- 工具实际工作点（TCP，如上右图）
  - 很多应用中，真正需要精准定位的是工具最前端的“作用点”，比如抓手的夹爪尖端、焊枪电弧点、喷涂枪的喷口等。
  - 此时，可以在机器人示教器或离线编程软件中额外定义一个新的工具坐标系，使其原点（TCP，Tool Center Point）位于实际的工作点位置；并将坐标系各轴的朝向与工具的主要工作方向对齐。
  - 这样在示教或轨迹规划时，命令机器人“走到某个点”就等同于让“工具的工作点”走到那个点，消除了法兰到工具尖端这段空间位姿差异带来的繁琐计算。
工具坐标系的轴向朝向

如同基坐标系一样，工具坐标系也应符合右手定则，但在具体方向的定义上，工业上常有如下通用约定（也会因厂商而异）：
1. Z 轴
  - 在大多数厂商的默认设定中，工具坐标系的 Z 轴通常 从法兰沿工具外伸方向 指出（可视为“工具的主要工作方向”或“TCP 朝向”）。
  - 对焊枪而言，可以让 Z 轴指向焊丝/电弧喷出方向；对抓手而言，Z 轴可能指向夹取工件时的“正面”。
2. X 轴 与 Y 轴
  - 在已确定 Z 轴方向之后，通过右手定则来定义 X、Y 的方向。
  - 常见的惯例是：
    - X 轴 沿“工具的辅助方向”或者“工具某侧关键安装面”；
    - Y 轴 则由右手定则自动确定。如果你让拇指对准 X 轴，食指对准 Z 轴，三个轴自然是右手坐标系。
  - 具体如何选择 X、Y，往往取决于工具自身结构及使用场景（如喷涂时可能更关心一个侧向、一个水平向等），只要保证与 Z 轴以及右手关系统一即可。
3. 多种工具、多个坐标系
  - 若一台机器人会换装不同工具（如抓手、相机、焊枪等），则每个工具都可以定义自己的工具坐标系（尤其是主要“工作点”和“工作方向”各不同）。
  - 通过在控制器中激活不同的“Tool Frame”，机器人就能识别当前所用工具的 TCP 偏移与方向，自动转换运动指令。
与运动学和编程的关系
1. 运动学计算中的末端姿态
  - 机器人在计算正逆运动学时，通常会先计算末端法兰的位姿；如果定义了工具坐标系，则相当于在末端法兰坐标系上再做一层刚性变换（平移 + 旋转），即可得到工具坐标系的位姿。
    
    比如 Episode 1 机械臂的负压吸嘴，就是沿着 Z 轴平移约 48mm
  - 因此，工具坐标系相对于法兰的变换矩阵，称作或类似符号。
2. 示教和编程
  - 在实际操作中，示教器通常允许用户设定“当前工具坐标系”为活动坐标，这样在示教时输入的点位、姿态都是针对于该工具坐标系；机器人控制器后台自动将其转化为法兰乃至基坐标系下的实际关节角度等信息。
  - 相对于法兰坐标，工具坐标系更加符合操作人员的直觉：例如，“Z+ 方向运动”就意味着工具尖端往前移动，“Z- 方向运动”则往后缩回。
练习：
- 下载负压吸盘.tool
- Robodk 安装 TCP 工具
- 鼠标右键-选项，可以看到工具中心点与机械臂法兰的的相对位姿（Z 偏移 48mm）：
- 自己修改一下对应的变量，感受 TCP 位姿的变化
- 激活 TCP，可以看到右侧坐标系面板变化：
  - 工具坐标系（TCP）相对于机器人法兰：Z 轴多了 48mm
  - 参考坐标系相对于机器人基座：不变，全部为 0，即以机器人基座作为参考坐标系
  - 工具坐标系相对于参考坐标系：这里是机械臂法兰坐标系叠加工具坐标系的结果，即
- 此时在”工具坐标系相对于参考坐标系“输入的点位、姿态都是针对于该工具坐标系；软件自动将其转化为法兰乃至基坐标系下的实际关节角度等信息。
  
  这里我们将 Z 设置为 0，可以看到吸盘末端和底座处在同一平面，而不是法兰
- 你可以尝试修改"工具坐标系（TCP）相对于机器人法兰"一些参数，感受运动效果。
- 设 Z=60mm（真机负压吸盘有弹性，会稍微长一些），有条件的连上 Episode 1 真机，看看是否真的吸盘末端和底座处在同一平面。

2.3 位置与姿态（Position & Orientation）

2.3.1 位置（Position）

在线体验地址

“末端执行器的 TCP 在基坐标系下的位置为 (500, 200, 150) mm”，意思是这个点在 X 方向 500mm，Y 方向 200mm，Z 方向 150mm。

“位置”描述的是一个点在空间中的坐标位置，通常用一个三维向量来表示。

定义：

在某一参考坐标系（如基坐标系）下，一个刚体或点的位置由其相对于该坐标系原点的坐标向量表示：

其中：

( x, y, z ) 是该点在参考坐标系下的空间坐标；
单位通常是米（m）或毫米（mm）；
表示的是该点在三维空间中“在哪里”。

当我们表达一个坐标系相对于另一个坐标系的位置的时候，以坐标原点为向量即可，如：在下面的动画中，以坐标系 A 原点做为参考，坐标系 B 原点位置是。

▶

🎬 演示视频 out3.mp4
在线观看（需登录恩培资料中心）

2.3.2 姿态（Orientation）

Episode 1 在同一个位置，不同姿态的状态。

“姿态”描述的是一个物体在空间中的朝向，也就是说：不光知道它“在哪里”，还得知道它“怎么转的、朝哪边”。

同样是一个焊枪在位置 (500, 200, 150) mm，它可能朝上、朝下、朝左……这就是姿态决定的。

姿态是一个刚体在空间中相对于参考坐标系的旋转关系，常用以下几种方式来表示：

欧拉角
旋转矩阵
四元数

因为在机器人运动学、视觉跟踪、计算机图形学中，经常需要使用旋转矩阵和欧拉角，并在这两种表示之间来回切换，我们重点介绍一下他们。

2.3.2.1. 欧拉角（Euler Angles）：

用三个旋转角度描述一个刚体的姿态，常见形式如 ZYX（也叫 roll-pitch-yaw）：

每个角度对应绕某个轴的旋转；
旋转方向遵循右手法则，即面对某个坐标轴（用右手大拇指指向轴的正方向）时，正角度的旋转是逆时针方向。

示例 1：绕 Z 轴旋转 90°

绕 Z 轴旋转 90°，绕 Y 轴旋转 0°，绕 X 轴旋转 0°，也就是欧拉角 [90, 0, 0]（ZYX 顺序）

▶

🎬 演示视频 out1.mp4
在线观看（需登录恩培资料中心）

这个姿态表示：物体的 X 轴原本朝前，现在绕 Z 转了一圈变成朝右了。

示例 2：绕 Z 轴旋转 90°，再绕 Y 轴旋转 90°

即欧拉角 [90, 90, 0]（ZYX 顺序），可以看成在示例 1 基础上，再绕 Y 轴旋转 90 度：

先绕 Z 轴 90 度	再绕 Y 轴 90 度

注意：

这里的多次旋转，是围绕物体自身的局部坐标系旋转，即内禀旋转：

外禀旋转（Extrinsic Rotation）：每次旋转都绕全局坐标系固定的 X/Y/Z 轴。
内禀旋转（Intrinsic Rotation）：每次旋转都绕物体自身的局部坐标系，轴会随前一次旋转而变。

在中文互联网，似乎没有这两个概念，大家说的默认就是“内禀”。

对于绝大多数六轴工业机器人，每个关节都绕“当前连杆/局部坐标系”的那条轴旋转，所以在运动学建模或几何推导时，本质上属于内禀旋转。

虽然在做数学推导时，有时也会用等效的外禀描述来简化，但就物理结构和直观理解而言，六轴机械臂一般默认是“内禀”。

你可以在 RoboDK 中创建两个坐标系，感受一下内禀和外禀旋转：

注意选择相对参考坐标系

注意：内禀旋转下 ZYX 和 XYZ 旋转顺序的影响

这是 ZYX 顺序欧拉角 [30, 60, 90]的旋转

▶

🎬 演示视频 out5.mp4
在线观看（需登录恩培资料中心）

这是 XYZ 顺序欧拉角 [30, 60, 90]的旋转

▶

🎬 演示视频 out6.mp4
在线观看（需登录恩培资料中心）

可以看到与直觉不同的是，旋转顺序对最终姿态的影响非常大，所以要特别注意旋转顺序的选择。

ZYX [30, 60, 90]	XYZ [30, 60, 90]

注意：

查看 RoboDK 的欧拉角，它经常变动，不是固定的 XYZ 或 ZYX 顺序，我们以 SE3 代码为准，如下方，是 ZYX 顺序（从右往左看）

Episode 1 机械臂 API 支持两种顺序：ZYX、XYZ（详情参考 API 文档）

你可以使用 Episode 1 API 位置模式，让机械臂运动到 RoboDK 预设的一个姿态，查看是否匹配

有的时候 IK 可能无解

2.3.2.2. 旋转矩阵（方向余弦矩阵）：

在线计算体验地址

你也可使用 RoboDK 复制姿态，查看对应旋转矩阵

是一个 (3 x 3) 的正交矩阵；
每一列代表目标坐标系的 X、Y、Z 轴在基坐标系下的方向向量；
表示刚体局部坐标系与全局坐标系之间的旋转关系。

具体来说：

的第 1 列向量，表示目标坐标系的 (X) 轴在基坐标系下的方向向量；
的第 2 列向量，表示目标坐标系的 (Y) 轴在基坐标系下的方向向量；
的第 3 列向量，表示目标坐标系的 (Z) 轴在基坐标系下的方向向量。

也就是说，如果我们把目标坐标系的三个坐标轴都用基坐标系中的坐标向量来表达，那么它们各自就是旋转矩阵的三列。下面举一些例子：

示例 1：单位矩阵（目标坐标系与基坐标系重合）

若目标坐标系和基坐标系完全重合，则旋转矩阵为单位矩阵：

以第 1 列为例：，表示目标坐标系的轴与基坐标系的轴一致。

示例 2：绕基坐标系 Z 轴旋转 +90°

▶

🎬 演示视频 out1.mp4
在线观看（需登录恩培资料中心）

设目标坐标系是通过绕基坐标系的轴旋转 +90° 得到的。

几何上变化如下：

目标系对应基系；
目标系对应；
目标系保持与一致。

则在基坐标系中，这三个轴的方向向量为：

构造出的旋转矩阵为：

示例 3：绕基坐标系 Z 轴旋转 +30°

▶

🎬 演示视频 out2.mp4
在线观看（需登录恩培资料中心）

情况稍微复杂一些，但是利用三角函数知识，我们可以计算出它的旋转矩阵：

即

单轴旋转矩阵

对于单轴旋转更一般的，我们可以得到：

绕 X 轴旋转

绕 Y 轴旋转

绕 Z 轴旋转

为什么需要旋转矩阵

旋转矩阵用三角函数形成的这种排列，是在满足“长度不变、角度不变”的前提下，对坐标进行旋转时最简洁、最直接的线性映射表达。
形式上的体现了几何上“绕某一轴旋转度”的本质；正交单位向量则确保了不发生畸变和镜像。
在机器人、图形学、航天等众多领域，这样的矩阵形式能与其他平移或变换矩阵无缝衔接，是做三维坐标变换必不可少的基础工具。
- 在后续使用中，你会感受到这个。比如以 XYZ 顺序绕对应轴分别旋转 30、45、60 度，求最终的旋转矩阵，从线性代数的角度，这个过程等效于下列矩阵乘积（右乘向量的情形）：
  
  你可以用 RoboDK 验证。
  
  带入数值，你可以很方便计算得到：
  
  如果有一个向量按这个旋转（用该旋转矩阵作用到一个向量），旋转后的向量只需要左乘旋转矩阵：

2.4 位置 + 姿态 = 空间位姿（Pose）

在机器人学中，我们通常用一个齐次变换矩阵（Homogeneous Transformation Matrix）来同时描述位置与姿态：

在 RoboDK 中复制的姿态数据就是位姿。

其中是旋转矩阵，描述姿态；
是位置向量；
整个矩阵就描述了一个物体在三维空间中的完整状态（叫“位姿”）。

旋转矩阵部分的作用我们已经前面见过，那位置部分的作用？

当矩阵作用于向量时，给定一个齐次坐标向量：

经过变换：

可以看到：

平移效果：将旋转后的点平移到另一个位置。也就是说，它描述了新坐标系原点相对于原坐标系原点的位移。
坐标系的偏移：在描述两个坐标系（比如机器人基座和工具坐标系）之间的关系时，表示原点之间的相对位置。这对理解对象在空间中的绝对位置非常关键。

具体的例子：

给定向量：

扩展为齐次坐标向量：

设齐次变换矩阵为：

其中，是 3×3 单位矩阵（相当于不做任何旋转），为位置（平移）向量。

对向量进行变换：

去掉最后的齐次分量，新向量为：

如果现在同时以 XYZ 顺序绕对应轴分别旋转 30、45、60 度，再偏移，则为：

再计算：

类似旋转矩阵，齐次变换矩阵不仅是数学“好看”，齐次变换矩阵将旋转和平移统一成一个 4 x 4 的矩阵，利用矩阵连乘实现复杂变换，使得机器人运动学、图形学和计算机视觉中的坐标转换更加简洁高效。

2.4.1 齐次变换矩阵的必要性及应用举例

齐次变换矩阵用于统一表达旋转和平移，方便通过矩阵乘法复合多个几何变换，而无需分别处理旋转矩阵和位移向量。下面举几个例子说明其应用场景。

2.4.1.1 机器人运动学

分级坐标转换
每个机器人关节都有自己的局部坐标系，齐次变换矩阵可用来描述从一个坐标系到下一个坐标系的旋转和平移。多个变换通过矩阵乘法组合：

得到末端执行器在基坐标系下的完整位姿。

2.4.1.2. 计算机图形学

模型变换
物体的缩放、旋转和平移都可表示为齐次变换矩阵。例如，模型变换矩阵可以写作：

这样，顶点的坐标变换可通过单次矩阵乘法完成，便于硬件加速和渲染流水线的统一处理。

2.4.1.3. 计算机视觉与 SLAM

相机位姿表示
齐次变换矩阵常用于描述相机在世界坐标系中的位姿，例如：

可快速完成从世界坐标到相机坐标的转换，便于实现定位与地图构建。

2.4.1.4. 多设备坐标对接

跨设备坐标转换
在多台机器人或设备协同工作时，各设备的坐标系之间可用齐次矩阵来进行统一描述和相互转换，简化系统集成和控制流程。

2.5 概念总结

概念	描述内容	表达形式
位置（Position）	空间中“在哪里”	三维向量：
姿态（Orientation）	空间中“朝哪边”或“如何旋转”	旋转矩阵、欧拉角、四元数等
位姿（Pose）	位置 + 姿态（完整描述）	齐次变换矩阵（4×4）

2.6 SE3 代码示例

更多示例参考：SE3 代码示例


# 都作用
# 定义平移：沿 X 平移 1，沿 Y 平移 2，沿 Z 平移 3
translation = SE3.Tx(1) * SE3.Ty(2) * SE3.Tz(3)

# 定义旋转
rotation = SE3.Rz(60, unit='deg') * SE3.Ry(45, unit='deg') * SE3.Rx(30, unit='deg')  # xyz

# 综合平移和旋转
pose = translation * rotation

# 定义一个三维点，假设为 (1, 1, 1)
point = [1, 1, 1]

# 用 pose 对该点做变换
transformed_point = pose * point

print(pose)
print("原始点：", point)
print("变换后的点：", transformed_point)

需要注意旋转顺序从右往左计算：


SE3.Rz(60, unit='deg') * SE3.Ry(45, unit='deg') * SE3.Rx(30, unit='deg')  # xyz


SE3.Rx(gamma, unit='deg') * SE3.Ry(beta, unit='deg') * SE3.Rz(alpha, unit='deg') # zyx

2.7 Episode 获取姿态

使用 RoboDK 获取当前姿态
使用 API 运动到对应位置
使用 API 获取当前姿态，与 RoboDK 进行对比。