3. 坐标系与标定：工具坐标系、用户坐标系、传感器零点标定与重力补偿

做力控这么多年，我越来越觉得一个道理：坐标系搞不明白，力控就是空中楼阁。你想想看，机器人感知到的力，到底是哪个方向？是工具末端碰到的力，还是法兰盘受到的力？如果坐标系没对齐，你读到的力数据全是错的，后面做再多算法也是白搭。

这一章，咱们就把坐标系和标定这件事彻底讲透。我会结合自己踩过的坑，把工具坐标系、用户坐标系、传感器零点标定和重力补偿这几个核心概念掰开揉碎了说。

3.1 工具坐标系（TCP）—— 机器人的“指尖”

什么叫工具坐标系？说白了，就是告诉机器人：你的“手指尖”在哪儿。

机器人默认的坐标系在法兰盘中心。但你装上夹爪、焊枪、打磨头之后，实际接触工件的位置变了。如果不重新定义TCP，机器人以为自己在A点，其实工具末端在B点。力控的时候，你读到的力是法兰盘上的，不是工具末端的。

我记得有一次做精密装配项目，机器人抓着一个精密轴往孔里插。一开始怎么都插不进去，力反馈乱跳。查了半天，发现是TCP标定偏了2毫米。就这2毫米，导致机器人以为轴是正的，实际是歪的。重新标定后，一次成功。

3.1.1 TCP标定方法

常用的TCP标定方法有几种，我挑最实用的说：

• 六点法： 最经典的方法。让机器人以不同姿态，把工具尖端对准一个固定尖点。记录6个姿态，系统自动算出TCP位置。精度高，但操作麻烦。
• 三点法： 简化版，只用3个姿态。精度稍差，但快。适合粗标定。
• 直接输入法： 如果你知道工具的设计尺寸，可以直接输入XYZ偏移量。我一般先用设计值，再用六点法微调。

3.1.2 力控中的TCP意义

在力控里，TCP的作用更关键。因为力传感器通常装在法兰盘上，但我们要控制的是工具末端的力。这中间有个力/力矩转换的问题。

举个例子：你用一个长杆工具去推工件。传感器测到的力是法兰盘处的力，但工具末端受到的力，会因为杠杆效应而不同。如果不做TCP转换，力控就会出错。

// 伪代码：TCP处的力计算
// 已知：传感器读数 F_sensor, T_sensor
// 已知：TCP相对于传感器的偏移量 (dx, dy, dz)
// 计算TCP处的力
F_tcp = F_sensor  // 力不变（刚体）
T_tcp = T_sensor + cross(F_sensor, [dx, dy, dz])  // 力矩需要转换

这段代码看着简单，但实际项目中，很多人忘了做力矩转换。我见过一个工程师，调试了半天力控震荡，最后发现就是少了这一行代码。

3.2 用户坐标系（UCS）—— 让机器人理解你的世界

用户坐标系，就是告诉机器人：工件是怎么放的。

机器人默认的世界坐标系是固定的。但你的工件可能斜着放、倒着放、转了个角度。如果不定义用户坐标系，你编程的时候就得算一堆三角函数，累死人。

我个人习惯，每个工位都定义一个用户坐标系。这样编程的时候，直接沿着工件表面走，省心省力。

3.2.1 用户坐标系标定

标定用户坐标系，通常用三点法：

1. 原点： 机器人移动到工件的一个角点，记录位置。
2. X轴方向点： 沿工件X方向移动一段距离，记录位置。
3. Y轴方向点： 沿工件Y方向移动一段距离，记录位置。

系统会根据这三个点，自动计算出坐标系。注意，Z轴是通过右手定则自动生成的。

3.2.2 力控中的用户坐标系

在力控里，用户坐标系的作用是让力的方向与工件对齐。

比如你要沿着工件表面打磨。如果不用用户坐标系，力的方向是世界坐标系下的Z方向。但工件是斜的，Z方向就不垂直于表面。用了用户坐标系，力的方向就自动变成垂直于工件表面，控制起来简单多了。

3.3 传感器零点标定—— 把“零”找准

力传感器有个毛病：它永远有零点漂移。温度变化、时间推移、甚至你碰了一下传感器，零点都会变。

什么叫零点？就是传感器不受力时，它应该输出0。但实际上，它可能输出个0.5N、-0.3N。如果不标定，你读到的力全是偏的。

3.3.1 零点标定步骤

零点标定其实很简单：

1. 确保机器人处于一个稳定的姿态，工具末端不接触任何东西。
2. 记录当前传感器的读数，作为零点偏移。
3. 后续所有读数都减去这个偏移量。

// 零点标定代码示例
// 采集100次数据取平均，减少噪声影响
float zero_offset[6] = {0};
for (int i = 0; i < 100; i++) {
    float* reading = read_sensor();
    for (int j = 0; j < 6; j++) {
        zero_offset[j] += reading[j];
    }
    delay(1);  // 1ms采样间隔
}
for (int j = 0; j < 6; j++) {
    zero_offset[j] /= 100.0;
}
// 后续使用
float get_force(int axis) {
    return read_sensor()[axis] - zero_offset[axis];
}

3.4 重力补偿—— 把工具重量“减掉”

这是力控里最容易忽略、也最容易出问题的一步。

你想想看，机器人装上工具后，传感器一直承受着工具的重量。比如一个5公斤的夹爪，传感器上一直有50N的力。如果不做重力补偿，你读到的力永远是50N起步。那还怎么控制接触力？

重力补偿的核心思想是：把工具自身的重量从传感器读数中减去。但难点在于，工具的姿态变了，重力在传感器各轴上的分量也会变。

3.4.1 重力补偿原理

重力补偿需要知道两个东西：

• 工具的重心位置： 相对于传感器坐标系。
• 工具的质量： 总重量。

有了这两个参数，再结合机器人当前的姿态，就能算出重力在传感器各轴上的分量。

// 重力补偿计算
// 已知：工具质量 m，重心位置 (cx, cy, cz)
// 已知：当前姿态的旋转矩阵 R（从传感器坐标系到世界坐标系）
// 已知：重力向量 G_world = (0, 0, -9.81)

// 1. 将重力转换到传感器坐标系
Vector3 g_sensor = R.inverse() * G_world;

// 2. 计算重力产生的力和力矩
Vector3 F_gravity = m * g_sensor;
Vector3 T_gravity = cross(Vector3(cx, cy, cz), F_gravity);

// 3. 从传感器读数中减去
Vector3 F_contact = F_sensor - F_gravity;
Vector3 T_contact = T_sensor - T_gravity;

3.4.2 自动重力补偿标定

手动测量工具重心和质量很麻烦。我一般用自动标定方法：

1. 让机器人以多个不同姿态运动（至少4个姿态，越多越好）。
2. 在每个姿态下，记录传感器读数和机器人姿态。
3. 用最小二乘法拟合出工具的质量和重心位置。

这个方法的好处是，不需要知道工具的具体设计参数。只要让机器人动一动，系统就能自动算出来。

3.5 知识体系总览

说了这么多，我画了一张图，把这一章的核心逻辑串起来。你一看就明白了：

这张图把这一章的核心逻辑说清楚了。你看，从高精度力控这个目标出发，下面四个模块缺一不可。每个模块都有自己的一套方法，也都有常见的坑。做项目的时候，这四个方面都要检查一遍，才能保证力控的精度。

好了，这一章的内容就到这里。坐标系和标定这件事，说难不难，说简单也不简单。关键是要理解每个步骤背后的物理意义，而不是机械地走流程。下次你遇到力控不准的问题，不妨先从坐标系和标定入手排查——我敢打赌，十有八九问题就出在这里。

---