3、坐标系与变换：工具坐标系（TCP）、用户坐标系、力传感器坐标系标定、坐标系变换矩阵
做力控碰撞检测，坐标系这东西绕不开。我刚开始接触机器人时，觉得坐标系不就是个数学概念嘛，有啥好纠结的？直到有一次，我标定的TCP偏了2毫米，结果力控一启动，机器人直接往工件上怼——嗯，那次教训挺深刻的。
说白了，坐标系就是机器人的“世界观”。它怎么感知位置？怎么理解力的方向？全看坐标系怎么定义。今天咱们就把工具坐标系、用户坐标系、力传感器坐标系，还有它们之间的变换矩阵，一次性讲透。
核心观点：坐标系标定的精度，直接决定了力控碰撞检测的可靠性。标定差1mm，力控就可能误报或漏报。
3.1 工具坐标系（TCP）标定
TCP，Tool Center Point，工具中心点。你想想看，机器人末端装了个夹爪、焊枪或者打磨头，它真正干活的那个点在哪？就是TCP。机器人控制的是法兰盘，但干活的是工具尖端。
为什么TCP这么重要？
- 力控碰撞检测需要知道力作用在哪个点上
- 轨迹规划需要精确的工具姿态
- 安全保护区域需要基于工具轮廓计算
我个人的习惯是，每次更换工具后，第一件事就是重新标定TCP。哪怕只是换了个夹爪的指头，也别偷懒。
3.1.1 TCP标定方法
常用的方法有四种，我列个表对比一下：
| 方法 | 精度 | 操作复杂度 | 适用场景 |
|---|---|---|---|
| 四点法 | 中等 | 低 | 通用工具 |
| 六点法 | 高 | 中 | 精密工具 |
| 直接输入法 | 取决于测量 | 低 | 已知尺寸的工具 |
| 激光跟踪仪法 | 极高 | 高 | 大型或高精度场景 |
我最常用的是六点法。操作起来其实不复杂：
- 找一个固定的尖点（比如工作台上的锥形标定块）
- 手动操作机器人，让工具尖端对准这个点
- 变换6个不同的姿态，每次都对准同一个点
- 系统自动计算出TCP的偏移量
小技巧：标定时，尽量让姿态变化大一些。如果6个姿态都差不多，算法会退化，精度反而不好。我一般会让工具旋转至少90度。
3.2 用户坐标系
用户坐标系，说白了就是“你说了算”的坐标系。机器人默认有个基坐标系，但有时候用起来不方便。比如你有个倾斜的工作台，或者需要沿着工件的边缘走轨迹，这时候定义个用户坐标系就舒服多了。
用户坐标系的作用：
- 简化编程：沿着工件表面移动，不用算复杂的角度
- 方便力控：力的方向可以基于用户坐标系定义
- 安全区域：保护区域可以跟着工件走
我记得有一次做打磨项目，工件是斜着放的。如果不用用户坐标系，轨迹点要算半天，还容易出错。后来我定义了一个用户坐标系，直接把工件表面当成XY平面，编程效率翻了三倍。
3.2.1 用户坐标系标定步骤
标定用户坐标系，通常用三点法：
- 原点：选一个参考点作为坐标系原点
- X轴方向：从原点沿X方向选第二个点
- XY平面：选第三个点，确定XY平面
代码示例（伪代码）：
// 用户坐标系标定
P1 = get_point("原点")
P2 = get_point("X轴方向")
P3 = get_point("XY平面内一点")
// 计算坐标系
X_axis = normalize(P2 - P1)
temp = normalize(P3 - P1)
Z_axis = normalize(cross(X_axis, temp))
Y_axis = cross(Z_axis, X_axis)
// 构建变换矩阵
user_frame = [X_axis, Y_axis, Z_axis, P1]注意：三个点不能共线！否则算不出唯一的坐标系。我见过有人随手点了三个点，结果共线了，系统报错还一脸懵。
3.3 力传感器坐标系标定
力传感器装在机器人和工具之间。它测出来的力，是在传感器自己的坐标系里的。但我们需要知道这个力在工具坐标系或者用户坐标系里是多少。
这就涉及到力传感器坐标系的标定了。
为什么要标定力传感器坐标系？
- 传感器安装有偏差，不可能完全对齐
- 工具的重力会影响读数，需要补偿
- 力的方向需要转换到控制坐标系
3.3.1 标定流程
我一般分两步走：
- 零点标定：空载时记录传感器读数，作为零点偏移
- 重力补偿：在不同姿态下记录数据，计算出工具的重力和重心位置
重力补偿的数学原理：
// 力传感器读数 = 外力 + 工具重力
F_measured = F_external + R * G_tool
// 其中：
// R 是当前姿态的旋转矩阵
// G_tool 是工具重力在传感器坐标系中的表示
// 标定时，让机器人做多个姿态
// 用最小二乘法求解 G_tool
for each pose:
F_measured_i = R_i * G_tool + noise
// 解出 G_tool 后，实时补偿
F_external = F_measured - R * G_tool避坑指南：我曾经遇到过一个问题——标定完重力补偿后，力控还是不准。查了半天，发现是工具上沾了铁屑，重量变了。所以标定前一定要确保工具干净，没有额外负载。
3.4 坐标系变换矩阵
好了，现在我们有三个坐标系：工具坐标系、用户坐标系、力传感器坐标系。它们之间怎么转换？答案就是变换矩阵。
变换矩阵，说白了就是一个4x4的矩阵，包含了旋转和平移信息。它能把一个点从一个坐标系映射到另一个坐标系。
变换矩阵的形式：
| R11 R12 R13 Tx |
| R21 R22 R23 Ty |
| R31 R32 R33 Tz |
| 0 0 0 1 |
// R 是 3x3 旋转矩阵
// T 是 3x1 平移向量3.4.1 常见的坐标系变换
在实际项目中，我们经常需要做这些变换：
- 传感器力 → 工具力：把力传感器测到的力，转换到工具坐标系
- 工具力 → 用户力：把工具坐标系中的力，转换到用户坐标系
- 基座标 → 用户坐标：用于轨迹规划和碰撞检测
举个例子，力控碰撞检测中，我们通常需要在用户坐标系里设置安全阈值。但力传感器给的数据是在传感器坐标系里的。所以需要做两次变换：
// 传感器坐标系 -> 工具坐标系
F_tool = T_sensor_to_tool * F_sensor
// 工具坐标系 -> 用户坐标系
F_user = T_tool_to_user * F_tool
// 合并变换
F_user = T_sensor_to_user * F_sensor
// 其中 T_sensor_to_user = T_tool_to_user * T_sensor_to_tool关键点：变换矩阵的乘法顺序很重要！先旋转后平移，还是先平移后旋转，结果完全不同。我习惯用齐次坐标，把旋转和平移统一到一个矩阵里，这样就不会搞混了。
3.5 知识体系总览
为了让你更直观地理解这些坐标系之间的关系，我画了一张图：
这张图把四个坐标系和变换矩阵的关系画清楚了。你看，基坐标系是根，用户坐标系和工具坐标系都从它派生。力传感器坐标系又挂在工具坐标系下面。所有的变换，最终都通过变换矩阵来沟通。
做力控项目时，我建议你先把这张图画清楚。哪个坐标系是控制目标？哪个坐标系是测量来源？中间需要几步变换？画明白了，代码写起来就顺了。
最后提醒：坐标系标定不是一劳永逸的事。工具磨损了、传感器拆装过、工件换了位置，都需要重新标定。我见过最惨的案例——有人标定完用了半年没管，结果工具磨损了3mm，力控阈值还是原来的，碰撞了都没检测到。
好了，坐标系与变换这块就聊到这儿。记住一句话：坐标系是机器人的世界观，变换矩阵是沟通不同世界观的桥梁。标定做扎实了，力控碰撞检测就成功了一半。
公众号:蓝海资料掘金营，微信deep3321