Robotics-机器人建模中的坐标系建立

机器人建模中的坐标系建立

在机器人建模中，坐标系建立和原点选取看起来像是“画图时的约定”，但它们会直接影响后续的运动学建模、动力学建模、控制算法和程序实现。

同一台机器人，如果坐标系建得清晰，后面的变换矩阵、雅可比矩阵、运动学方程会自然很多；如果坐标系建得混乱，公式会变长，符号容易出错，调试时也很难判断问题到底出在哪里。

所以，机器人建模的第一步，往往不是写公式，而是先把坐标系放对。

---

1. 为什么机器人建模需要坐标系？

机器人运动本质上是在空间中描述位置和姿态。

例如我们想回答这些问题：

• 机械臂末端执行器在哪里？
• 末端执行器朝向哪个方向？
• 某个关节转动后，后面的连杆会如何运动？
• 相机坐标系下看到的目标，如何转换到机器人基坐标系？
• 轮式机器人在地图坐标系中的位姿是多少？

这些问题都离不开坐标系。

在机器人建模中，常见坐标系包括：

• 世界坐标系 {W}：描述全局环境，例如地图、地面、工作台。
• 基座坐标系 {B}：固定在机器人底座上，用来描述机器人整体。
• 关节坐标系 {i}：固定在每个关节或连杆上，用来描述相邻连杆之间的关系。
• 末端坐标系 {E}：固定在夹爪、工具或末端执行器上。
• 传感器坐标系 {S}：固定在相机、激光雷达、IMU 等传感器上。

坐标系的作用，是给空间中的点、向量、姿态和运动提供统一的表达标准。

---

2. 坐标系建立的核心原则

建立坐标系没有唯一答案，但好的坐标系通常满足几个原则。

2.1 尽量贴合机器人的结构

坐标轴最好和机器人的自然运动方向一致。

例如：

• 移动机器人中，通常让 X 轴指向车体前方；
• Y 轴指向车体左侧；
• Z 轴指向上方；
• 机械臂中，关节坐标轴通常与关节转轴重合；
• 末端坐标系通常让某一轴指向工具的工作方向。

这样做的好处是，坐标含义直观。

比如移动机器人中，如果 X 轴指向前方，那么速度 vx 就表示前进速度；如果 Z 轴向上，那么绕 Z 轴旋转就对应平面内的偏航角。

2.2 保持右手坐标系

机器人建模中通常使用右手坐标系。

判断方法是：

右手四指从 X 轴转向 Y 轴，大拇指指向 Z 轴。

或者记住叉乘关系：

X × Y = Z
Y × Z = X
Z × X = Y

使用右手坐标系可以避免旋转方向、角速度方向和叉乘结果出现符号混乱。

2.3 让相邻坐标系之间的变换尽量简单

一个好的坐标系选取，应该让变换矩阵尽量简洁。

例如，如果两个坐标系的原点重合，只需要考虑旋转；如果坐标轴方向一致，只需要考虑平移。

建模时不要只看当前坐标系是否好画，还要看它和前后坐标系之间的关系是否好算。

2.4 保持命名和方向一致

坐标系一旦确定，就要在图纸、公式、代码和文档中保持一致。

例如：

{W}：世界坐标系
{B}：机器人基坐标系
{E}：末端执行器坐标系
{C}：相机坐标系

同时要明确写清楚每个坐标系：

• 原点在哪里；
• X 轴指向哪里；
• Y 轴指向哪里；
• Z 轴如何由右手法则确定；
• 坐标系是固定在世界中，还是跟随机器人运动。

---

3. 原点选取为什么重要？

坐标轴决定方向，原点决定位置。

同一个坐标系，如果轴方向不变，只改变原点，那么点的坐标、平移向量、变换矩阵都会发生变化。

在机器人建模中，原点选取会影响：

• 位姿表达是否直观；
• 连杆参数是否简洁；
• 运动学方程是否容易推导；
• 控制点和实际作用点是否一致；
• 传感器数据转换是否方便。

原点选得好，公式会短很多；原点选得不好，后面会不断出现额外的偏移量。

---

4. 常见原点选取方法

4.1 世界坐标系原点

世界坐标系通常固定在环境中。

常见选法包括：

• 地图起点；
• 实验场地角点；
• 工作台中心；
• 机器人初始位置；
• 标定板或外部测量系统的参考点。

世界坐标系的原点应该稳定、可复现，并且便于测量。

例如，在移动机器人导航中，地图坐标系 {map} 的原点通常由建图系统定义；在机械臂工作站中，世界坐标系可能固定在工装夹具或工作台上。

4.2 基座坐标系原点

基座坐标系通常固定在机器人本体上。

对于机械臂，基座坐标系原点常选在：

• 第一个关节轴线上；
• 底座安装面的中心；
• 厂商定义的机器人 base 坐标系位置。

对于移动机器人，基座坐标系原点常选在：

• 车体几何中心；
• 两个驱动轮轴线的中点；
• 机器人旋转中心；
• IMU 或控制参考点位置。

如果机器人主要做平面运动，原点最好选在运动学模型中的自然参考点上。

例如差速小车中，常把原点选在左右轮连线的中点。这样线速度和角速度模型会非常简洁：

v = (v_r + v_l) / 2
ω = (v_r - v_l) / L

其中 L 是左右轮间距。

4.3 关节坐标系原点

机械臂建模时，每个关节或连杆通常都要建立局部坐标系。

关节坐标系原点常选在：

• 关节轴线与连杆公共法线的交点；
• 相邻关节轴线的交点；
• 关节转轴上的某个几何中心；
• DH 建模规则规定的位置。

在 Denavit-Hartenberg 建模中，坐标系不是随意放置的，而是遵循一套规则，让相邻连杆之间的变换可以用四个参数描述：

a_i     连杆长度
α_i     连杆扭角
d_i     连杆偏距
θ_i     关节角

这样做的目的，是把复杂的三维连杆关系转化成标准化的矩阵乘法。

4.4 末端执行器坐标系原点

末端坐标系一般固定在工具或执行器上。

原点常选在：

• 夹爪中心；
• 工具尖端；
• 吸盘中心；
• 焊枪端点；
• TCP，也就是 Tool Center Point。

TCP 的选取非常关键。

机器人真正控制的通常不是法兰盘中心，而是工具实际作用点。例如焊接机器人关心的是焊丝端点，抓取机器人关心的是夹爪中心，点胶机器人关心的是针头出口。

如果末端坐标系原点没有选在实际作用点上，就必须额外补偿工具偏移。

4.5 传感器坐标系原点

传感器坐标系固定在传感器本体上。

例如相机坐标系通常定义为：

• 原点在相机光心；
• Z 轴沿光轴向前；
• X 轴沿图像水平方向；
• Y 轴沿图像垂直方向。

激光雷达坐标系通常固定在雷达扫描中心。

IMU 坐标系通常固定在惯性测量单元芯片或模组中心附近。

传感器坐标系和机器人坐标系之间的外参非常重要。所谓外参，本质上就是一个坐标变换：

B_T_C

表示从相机坐标系 {C} 到机器人基坐标系 {B} 的变换关系。

---

5. 坐标系方向如何确定？

原点确定之后，还需要确定三个坐标轴方向。

5.1 移动机器人常用方向

移动机器人中，一个常见约定是：

X：机器人前方
Y：机器人左方
Z：机器人上方

这个约定符合右手坐标系。

在这个坐标系下：

• x 表示前后位置；
• y 表示左右位置；
• yaw 表示绕 Z 轴的偏航角；
• 正角速度通常表示逆时针旋转。

这也是 ROS 中常见的坐标约定之一。

5.2 机械臂关节坐标系方向

机械臂中，关节坐标系经常按转轴方向确定。

一般来说：

• Z_i 轴沿第 i 个关节的运动轴；
• 对转动关节，Z_i 是旋转轴；
• 对移动关节，Z_i 是平移方向；
• X_i 轴通常沿相邻关节轴线的公共法线；
• Y_i 轴由右手法则确定。

这类规则可以让机械臂的正运动学表达更系统。

---

6. 原点选取对公式的影响

来看一个简单例子。

假设移动机器人在平面上运动，如果基座原点选在车体旋转中心，那么机器人位姿可以写成：

p = [x, y, θ]^T

控制输入可以写成：

u = [v, ω]^T

运动学模型为：

x_dot = v cos θ
y_dot = v sin θ
θ_dot = ω

这个模型非常清晰。

但如果原点选在车头某个偏移点，假设它距离旋转中心有长度 l，那么该点速度还会受到角速度影响：

x_dot = v cos θ - l ω sin θ
y_dot = v sin θ + l ω cos θ
θ_dot = ω

这说明原点不同，模型复杂度会不同。

原点不是随便放的。它应该服务于建模、控制和计算。

---

7. 坐标系建立中的常见错误

7.1 左手系和右手系混用

这是非常常见的问题。

如果某些坐标系使用右手系，某些坐标系不小心变成左手系，旋转矩阵的方向、叉乘结果、姿态角都会出错。

7.2 坐标轴方向只在图上好看，但不利于计算

有时为了画图方便，把坐标轴随意放置，结果导致后续平移和旋转都很复杂。

建模时更应该关心变换关系是否清楚，而不是图是否看起来对称。

7.3 原点没有放在真实控制点上

例如机械臂末端工具安装后，如果仍然把法兰盘中心当作末端原点，而实际任务点在工具尖端，就会出现定位误差。

这时必须建立工具坐标系，并标定 TCP。

7.4 忘记坐标系是固定的还是运动的

有些坐标系固定在世界中，有些坐标系固定在机器人身上，有些坐标系固定在关节或传感器上。

如果没有明确这一点，就容易把“点在动”和“坐标系在动”混在一起。

7.5 变换方向写反

例如：

A_T_B

通常表示把 B 坐标系下的坐标转换到 A 坐标系下：

A p = A_T_B · B p

如果把它误用成：

B p = A_T_B · A p

结果就会错。

正确做法是使用逆矩阵：

B p = B_T_A · A p
B_T_A = inverse(A_T_B)

---

8. 建模时的实用流程

实际建模时，可以按下面的流程进行。

第一步：明确建模目标

先问清楚：

• 是要做正运动学？
• 是要做逆运动学？
• 是要做动力学？
• 是要做控制？
• 是要做传感器标定？
• 是要做导航定位？

不同目标下，坐标系选取重点不同。

第二步：确定全局参考坐标系

选择世界坐标系或地图坐标系，明确它的原点和轴方向。

第三步：确定机器人基坐标系

选择一个和机器人结构、运动模型最匹配的位置作为基座原点。

第四步：为关节、连杆、工具建立局部坐标系

机械臂可以使用 DH 规则或改进 DH 规则，也可以使用 URDF 中的 link/joint 结构。

移动机器人可以为底盘、轮子、IMU、相机、雷达分别建立坐标系。

第五步：写出相邻坐标系之间的变换

不要一开始就写复杂的全局公式。

先写清楚每一段：

W_T_B
B_T_1
1_T_2
2_T_3
E_T_tool
B_T_camera

然后再通过矩阵连乘得到整体变换。

第六步：检查方向和单位

重点检查：

• 是否都是右手坐标系；
• 角度单位是度还是弧度；
• 长度单位是米还是毫米；
• 旋转方向是否符合右手法则；
• 平移向量是在哪个坐标系下表达的。

---

9. 一个简单案例：机械臂末端工具坐标系

假设机械臂末端法兰盘坐标系为 {F}，工具坐标系为 {T}。

工具安装在法兰盘前方 l 米的位置，且方向与法兰盘一致。

那么可以写：

F_T_T = [ I   [0, 0, l]^T
          0      1       ]

如果已知法兰盘相对于基座的位姿：

B_T_F

那么工具相对于基座的位姿就是：

B_T_T = B_T_F · F_T_T

这说明工具坐标系并不是额外的麻烦，而是让控制目标更加准确。

机器人控制末端时，真正应该关注的是 {T}，而不是 {F}。

---

10. 一句话总结

机器人建模中的坐标系建立，本质上是在为空间运动建立一套清晰的语言。

原点决定“从哪里开始量”，坐标轴决定“朝哪个方向量”。

好的坐标系应该满足：

• 符合右手法则；
• 贴合机器人结构；
• 原点靠近真实运动中心或控制点；
• 相邻坐标系之间的变换简单；
• 在公式、代码和文档中保持一致。

最终目标不是把坐标系画得复杂，而是让机器人运动关系表达得清楚、稳定、可计算。

坐标系建得好，后面的运动学、动力学和控制都会轻松很多。