第三章 坐标系与变换：工具坐标系、传感器坐标系、基坐标系的定义与齐次变换矩阵
做力控机器人，第一件事不是调PID，也不是写阻抗控制。而是先把坐标系搞清楚。
我见过太多项目，算法仿真跑得飞起，一上真机就抖成筛子。查到最后，往往是坐标系定义错了。你想想看，传感器读到的力是相对于自己安装位置的，而你要控制的是末端执行器在基坐标系下的运动。中间差了一个变换，结果全乱套。
这一章，我们就来把这件事彻底说清楚。
三个核心坐标系：你总得知道自己在哪
在机器人系统里，我们通常关注三个坐标系。它们各自有各自的使命。
| 坐标系 | 符号 | 原点位置 | 典型用途 |
|---|---|---|---|
| 基坐标系 | {B} | 机器人底座安装面中心 | 全局运动规划、正逆运动学 |
| 工具坐标系 | {T} | 末端执行器工作点（如夹爪中心、焊枪尖端） | 末端位姿控制、力控目标点 |
| 传感器坐标系 | {S} | 传感器测量中心（如六维力/力矩传感器中心） | 力/力矩原始数据读取 |
基坐标系是全局参考。机器人说“我要走到(0.5, 0.3, 0.2)”，这个位置就是相对于基坐标系的。工具坐标系是干活用的。你让机器人去拧螺丝，你关心的是螺丝刀尖的位置，而不是机器人法兰盘的位置。传感器坐标系是感知用的。力传感器测到的力，是作用在传感器自身坐标系上的。
关键认知：力控算法的本质，就是把传感器坐标系下测到的力，变换到工具坐标系或基坐标系下，再去做控制决策。坐标系没对齐，力控就是空中楼阁。
齐次变换矩阵：坐标系之间的“翻译官”
坐标系之间怎么转换？靠齐次变换矩阵。说白了，就是一个4x4的矩阵，同时包含了旋转和平移信息。
它的标准形式长这样：
| R t |
T = | |
| 0 1 |其中R是3x3的旋转矩阵，t是3x1的平移向量。最后一行是[0 0 0 1]，这是齐次坐标的固定格式。
为什么非要用4x4？用3x3加一个平移向量不行吗？
嗯，这里有个数学上的小陷阱。如果你只用3x3旋转矩阵加平移向量，连续变换的时候会变得非常麻烦。而齐次变换矩阵可以把旋转和平移统一成一个矩阵乘法，连续变换就是矩阵连乘，干净利落。
我的习惯：在代码里，我通常用4x4的numpy数组来表示变换矩阵。别偷懒用3x3加向量分开存，调试的时候你会感谢我的。
从传感器到工具：力控中最常见的变换
力控场景里，最经典的一个变换路径是：
- 六维力传感器测到原始力/力矩数据 F_s（在传感器坐标系{S}下）
- 通过标定得到传感器坐标系到工具坐标系的变换矩阵 T_s^t
- 将力变换到工具坐标系下：F_t = T_s^t * F_s
- 在工具坐标系下执行阻抗控制或力位混合控制
这里有个细节很多人会忽略。力的变换和位置的变换不一样。位置变换是直接乘变换矩阵，但力的变换需要用到变换矩阵的转置。
为什么会这样？
因为力是协变向量。位置是逆变向量。简单记：位置变换用T，力变换用T的逆的转置。如果T是纯旋转（没有平移），那T的逆的转置就等于T本身。但一旦有平移，两者就不一样了。
我曾经踩过的坑：有一次做打磨力控，力传感器读数一直有奇怪的偏置。查了两天，最后发现是力变换时直接用了位置变换的矩阵，没有做转置。那个偏置刚好是传感器安装偏移量导致的。从那以后，我每次写力变换代码都会在注释里加一行：# 注意！力是协变向量，要用T的逆的转置。
标定：让理论变成现实
理论讲完了，但实际中你拿不到精确的变换矩阵。因为传感器安装有误差，工具也有制造公差。所以我们需要标定。
工具坐标系标定最常用的方法是四点法：
- 让机器人以四种不同的姿态，把工具尖端对准空间中的一个固定点
- 记录下每种姿态下法兰盘的位姿
- 通过求解方程组，得到工具坐标系相对于法兰盘的偏移
传感器坐标系标定更麻烦一些。我常用的方法是重力标定法：
- 在末端挂一个已知重量的砝码
- 让机器人以多种姿态运动
- 记录传感器读数，拟合出传感器坐标系相对于法兰盘的变换
建议：标定完成后，一定要做一次验证。让机器人带着工具走一个简单的轨迹，比如画圆。如果工具尖端的实际轨迹和理论轨迹偏差在1mm以内，标定就算合格了。
知识体系总览
下面这张图把本章的核心逻辑串起来了。你可以把它当作坐标系变换的“思维导图”。
代码示例：用Python实现力变换
理论说完了，上点实际的。下面这段代码演示了如何把传感器坐标系下的力变换到工具坐标系下。
import numpy as np
def transform_force(F_s, T_s_to_t):
"""
将传感器坐标系下的力变换到工具坐标系下
参数:
F_s: 传感器坐标系下的力/力矩，6维向量 [Fx, Fy, Fz, Mx, My, Mz]
T_s_to_t: 传感器到工具的齐次变换矩阵，4x4
返回:
F_t: 工具坐标系下的力/力矩，6维向量
"""
# 提取旋转矩阵和平移向量
R = T_s_to_t[:3, :3]
p = T_s_to_t[:3, 3]
# 力的变换：F_t = R * F_s
# 力矩的变换：M_t = R * M_s + p × (R * F_s)
f_s = F_s[:3]
m_s = F_s[3:]
f_t = R @ f_s
m_t = R @ m_s + np.cross(p, f_t)
return np.concatenate([f_t, m_t])
# 示例：假设传感器安装偏移为 [0, 0, 0.1] 米，无旋转
T_s_to_t = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0.1],
[0, 0, 0, 1]
])
F_s = np.array([10.0, 0.0, 5.0, 0.0, 0.0, 0.5])
F_t = transform_force(F_s, T_s_to_t)
print("传感器力:", F_s)
print("工具力:", F_t)注意看，当传感器安装有偏移时（这里z方向偏移了0.1米），即使传感器只测到了纯力，变换到工具坐标系下也会产生额外的力矩。这就是为什么力控系统中传感器安装位置必须精确标定的原因。
实战经验：在装配任务中，我习惯把工具坐标系原点设在零件接触点。这样力控目标可以直接写成“在工具坐标系下保持Fx=0, Fy=0, Fz=-10N”，物理意义非常清晰。调试的时候，哪个方向有问题一眼就能看出来。
坐标系和变换矩阵是力控算法的地基。地基没打好，上面的算法再漂亮也是白搭。我建议你在开始任何力控项目之前，先把这三个坐标系画出来，把变换矩阵写清楚。花半小时做这件事，能省下后面几天的调试时间。