1. 传感器装上机械臂之后为什么裸数据没法直接用1.1 一次真实的翻车现场我记得第一次把六维力传感器装到机械臂末端时心里想得挺简单传感器买回来不就是读数吗上电、读串口、写个函数把六个分量拿到然后直接用来做接触检测。结果第一组数据差点让我怀疑传感器是坏的——末端什么都没碰机械臂保持一个固定姿势静止不动按道理受力应该是零加一个工具重力但屏幕上的 Fx、Fy、Fz、Mx、My、Mz 全在十几二十的范围内来回飘而且换个姿态之后数值变化的规律完全看不懂。更尴尬的是我把机械臂摆成工具朝下的姿态这时候理论上末端工具重力应该全部落在 Z 轴负方向但实际读到的 Fz 跟手算的理论值差了将近 8N。Mx、My 也不是零明明工具重心就在法兰中心附近怎么会有这么大的力矩这就是六维力传感器标定的典型场景传感器本身是准的装到机械臂上之后因为安装应力、零点漂移、工具重力耦合、温度变化等原因读数和你想要的接触力之间隔着一大截需要扣除的量。不做标定就直接拿数据做力控结果就是误触发、漏触发、力控震荡甚至把零件压坏。这篇博文我就把当时解决这个问题的完整思路写下来核心就三件事六维力传感器为什么要标定、怎么用最小二乘法把标定问题变成数学问题、标定完之后重力补偿怎么用。适合正在做机械臂力控、遥操作力反馈、装配打磨应用以及拿六维力传感器做毕业设计的朋友参考。1.2 读数里面到底混了几类东西要搞清楚标定标的是什么先得知道传感器读数里混杂了哪些成分。以我实际遇到的场景为例传感器安装在机械臂法兰和末端工具之间此时读数 D 可以拆成下面几块传感器自身的零点偏移应变片桥路不可能绝对平衡上电之后会有一个固定的偏置这个偏置受温度影响还会缓慢漂移。这是六维力传感器最基础也是最先要处理的问题。安装预紧力法兰螺丝拧紧时强力矩会让传感器弹性体产生变形反映为读数中的一个固定偏置。拧螺丝的力度不同这个偏置也不同。末端工具重力只要是装在机械臂上的载荷它的重力就会持续压在传感器上而且重力在传感器坐标系中的投影方向会随着机械臂姿态变化而变化。这是重力补偿要处理的核心对象。重力产生的力矩工具重力不经过传感器测量中心时会额外产生一个随姿态变化的力矩分量。这也是为什么明明没接触任何东西力矩读数却不为零。动态力/惯性力机械臂运动过程中加速度会让工具产生惯性力叠加在传感器输出上。这部分严格来说属于动态补偿的范畴。温度漂移和零漂传感器上电后温度缓慢升高或者环境温度变化都会让零点缓慢移动量级通常不大但对于需要精细力控的场景很致命。在静态或准静态场景下机械臂运动得很慢或到位后停稳最后一项动态力可以忽略前几项是主要矛盾。而前几项虽然来源不同但它们的共同特点是都跟当前末端姿态有确定关系。既然每换一个姿态读数就变那就把读数和姿态建立方程用一批姿态下的数据反推出系统参数——这就是标定的本质。1.3 六维力标定和重力补偿的边界在哪儿很多刚接触这个方向的人会把标定和重力补偿混在一起甚至以为标定完了就不用做重力补偿了。我自己的理解是标定是一个求参数的过程——求传感器的零偏、工具的重力向量、重心位置重力补偿是一个用参数的过程——标定求出这些参数之后在运行阶段根据实时姿态把由工具重量导致的力和力矩从传感器读数里扣掉。而且还要注意标定这一步解决的只是传感链路中的工具自重分量它不同于手眼标定。手眼标定求的是相机坐标系与机械臂末端坐标系之间的变换关系六维力传感器标定求的是传感器零偏与载荷参数两者解决的问题完全不是一个维度。很多做 ROS 机械臂开发的朋友容易在这里绕晕。另外还有一个常见误区以为六维力传感器的出厂静态校准证书就足够了。出厂校准保证的是传感器输出电压到物理力的换算精度但它无法替你解决装上工具之后这个工具多重、重心在哪里、安装偏置是多少这个问题。后者恰恰是使用环节必须自己做的。2. 从物理模型到最小二乘标定问题是怎么变成数学题的2.1 传感器的输出方程标定第一步是把读数跟姿态之间的关系写成数学表达式。先做几个约定机械臂基座坐标系定义为世界系重力方向在世界系下取单位向量 g_world [0, 0, -1]假设 z 轴朝上传感器坐标系固定安装在机械臂法兰上当前姿态由旋转矩阵 R_world_to_tool 表示这个矩阵通常能直接从机械臂控制器或运动学接口拿到。重力方向在传感器坐标系下的单位向量记为 c它的计算方法是c R_world_to_tool * g_worldc 的物理含义是载荷重力往哪边压。读六个分量时力和力矩可以分别建立方程。令工具重量为 w标量单位 N传感器力零点为 b_f [b_fx, b_fy, b_fz]那么在第 i 个姿态下力通道读数满足f_i w * c_i b_f 噪声力矩通道稍微绕一点。设工具重心在传感器坐标系中的位置为 r [rx, ry, rz]它是重力在传感器上的作用点偏移。力矩的物理关系是力乘以力臂所以力矩公式为τ_i w * (r × c_i) b_τ 噪声其中 × 表示向量叉乘b_τ [b_τx, b_τy, b_τz] 是力矩通道的零点。这就是整个标定问题的基础模型。你会发现力通道是线性的未知量为 w 和 b_f 共 4 个力矩通道里 r 和 c 做叉乘看起来是非线性的但 w 和 r 是乘积关系可以合并成一个新的未知向量 q w * r这样力矩方程同样变成线性方程τ_i q × c_i b_τ于是所有未知量合并起来是 4 6 10 个参数。每一个姿态能提供 3 个力方程和 3 个力矩方程也就是说只要采 2 个不同姿态就有 12 个方程数量上已经超过未知数。实际操作中我们当然不会只采 2 组会采几十组这就是典型的超定方程组。2.2 为什么是非用最小二乘不可超定方程组看似简单难点在于由于测量噪声、姿态误差、传感器分辨率限制等影响你无法找到一组参数让所有方程同时精确成立。最自然的思路就是让预测值和实测值的总误差最小。这就是最小二乘法的核心思想找一个参数向量 x使得残差平方和最小。写成数学形式就是min ||A x - b||^2其中 A 是由姿态信息构成的系数矩阵b 是传感器读数。把矩阵写出来会更直观。力部分的系数矩阵每个姿态贡献三行A_f 第 i 个姿态的三行 [ c_i, I_3 ] [ cx, 1, 0, 0 ] [ cy, 0, 1, 0 ] [ cz, 0, 0, 1 ]对应的参数向量是 x_f [w, b_fx, b_fy, b_fz]。力矩部分的参数向量是 x_τ [q_x, q_y, q_z, b_τx, b_τy, b_τz]每个姿态的三行是 [ -skew(c_i), I_3 ]其中 skew(c) 是 c 的反对称矩阵skew(c) [ 0, -cz, cy ] [ cz, 0, -cx ] [ -cy, cx, 0 ]代进去就是τ_i -skew(c_i) * q b_τ这里 q w * r标定完成后用 q 除以 w 就能还原出重心位置 r。那为什么最小二乘解在这种问题里效果这么好最直接的原因是当测量噪声接近高斯白噪声时最小二乘解是线性无偏估计中最优的它给出的参数估计的方差最小。另一个实际原因是实现简单直接把矩阵堆起来调用现成求解器就行。高斯-马尔可夫定理保证了在噪声是零均值、等方差、不相关的前提下最小二乘估计是所有线性无偏估计中方差最小的这正是量产工程中想要的性质。2.3 标定过程中可选的两种线性化处理我上面写的方案是把 10 个参数拆成力、力矩两个最小二乘问题分别求解。用力的数据先求出 w 和 b_f再用力矩数据求 q 和 b_τ。这种做法优点是数值稳定、实现简单缺点是严格来说忽略了力方程和力矩方程之间的噪声相关特性。另一种方案是把力方程和力矩方程合并成一个大矩阵联合求解。合并成一个 6N × 10 的矩阵一次求出 w、b_f、q、b_τ。从理论上讲联合估计能更好地利用全部信息但因为力和力矩的量纲不同N 和 N·m如果通道噪声差异很大还得考虑加权。我自己实际对比过两种方案在传感器本身噪声水平正常的情况下结果相差很小。所以我更推荐先做分步估计至少调试阶段方便定位问题——如果力参数明显异常先排查力通道数据如果力矩参数异常再排查力矩通道数据两件事互不干扰。3. 采集样本前的准备姿态怎么摆数据怎么记才算合格3.1 样本覆盖性设计标定效果一半靠算法另一半靠数据。数据质量的核心指标是姿态覆盖性。很多第一次做标定的人会偷懒让机械臂随便动几个位置就采了。我踩过这个坑第一次采了 12 组姿态全是工具水平或者接近水平的位置。结果最小二乘解出来w 的值跟实际重量差了 0.8N 以上重心位置 r 更是完全没法看离理论值十万八千里。后来分析了原因当所有姿态的 c 向量都集中在同一个方向附近时系数矩阵的列与列之间近似线性相关病态程度很高任何一个微小噪声都会被放大。正确做法是让重力方向在传感器坐标系下尽量覆盖整个球面。具体来说工具分别朝上、朝下让 c 接近 [0,0,1] 和 [0,0,-1]工具水平朝向 X 正、X 负方向让 c 接近 [1,0,0] 和 [-1,0,0]工具水平朝向 Y 正、Y 负方向让 c 接近 [0,1,0] 和 [0,-1,0]在这个基础上让工具绕自身 Z 轴旋转不同角度再采若干组比如每 45 度采一组再混合一些倾斜 45 度左右的姿态把中间区域补上。我实际做的标定数据通常是 40 到 60 组姿态覆盖上面这些位置。采集时间大概半小时到一小时但换来的是参数的稳定性。至于姿态之间的过度速度机械臂在姿态间移动时可以快速运动但到达目标姿态后必须停稳再采样这个在后面踩坑部分还会细说。3.2 采样过程中的数据判废标准每组姿态到位之后不能拿单帧数据直接用。最稳的做法是让机械臂在该姿态静止 1 到 2 秒然后取这段时间内所有帧的均值。这样能有效滤除振动和电路噪声。记录数据时我还会同时记录当前姿态的旋转矩阵或四元数以及传感器原始的六个分量。这里有一个特别容易忽略的点如果机械臂控制器输出的旋转矩阵是世界到工具还是工具到世界不同品牌定义不一样。我建议在代码里先打印一段旋转矩阵拿一个已知的简单姿态比如工具朝下去验证方向是否正确确认之后再往矩阵里填数据。不要相信文档要相信实测。另外如果传感器支持上电清零功能我建议在标定前做一次上电清零这样可以先把大数目的零偏消掉一部分让后面最小二乘求解的数值条件更好。清零之后采集时需要注意清零只是把当前读数置为零参考点并不等于完成了标定。这一步仅仅是预处理。3.3 一份可复用的数据记录表下面是我当时用的一张记录表示例结构很简单但信息很全序号姿态描述R矩阵(9个值)FxFyFzMxMyMz备注1工具竖直朝上.....................静置2s2工具竖直朝下.....................静置2s3工具水平X.....................静置2s..............................实际采集时我直接把 R 矩阵和传感器读数按 CSV 行追加写文件等采集完成统一读进来处理。不要用 Excel 手动录入容易抄错。另外建议数据文件名带上日期和工具名称同一个末端工具换过螺丝或换过安装方式之后参数会有变化需要重新标定。这个细节等你在现场被标定忽然不准坑一次就明白了。4. 最小二乘求解的 Python 实现与结果解读4.1 核心代码实现用 Python 做这件事非常直接NumPy 的 lstsq 函数就是标准最小二乘求解器。下面是我实际用的代码框架去掉了一些工程包装保留核心步骤。import numpy as np def skew(v): return np.array([ [0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0] ]) def calibrate(rot_mats, forces, torques, g_worldnp.array([0.0, 0.0, -1.0])): n len(rot_mats) A_f_list [] b_f_list [] A_t_list [] b_t_list [] for i in range(n): R_w2t rot_mats[i] c R_w2t g_world # 重力方向单位向量在传感器坐标系下的表示 # 力方程: f w * c b_f A_f_block np.hstack([c.reshape(3, 1), np.eye(3)]) A_f_list.append(A_f_block) b_f_list.append(forces[i]) # 力矩方程: tau -skew(c) * q b_tau, q w * r A_t_block np.hstack([-skew(c), np.eye(3)]) A_t_list.append(A_t_block) b_t_list.append(torques[i]) A_f np.vstack(A_f_list) b_f np.concatenate(b_f_list) x_f, res_f, rank_f, sv_f np.linalg.lstsq(A_f, b_f, rcondNone) w x_f[0] # 工具重量 b_f_est x_f[1:4] # 力零点偏置 A_t np.vstack(A_t_list) b_t np.concatenate(b_t_list) x_t, res_t, rank_t, sv_t np.linalg.lstsq(A_t, b_t, rcondNone) q x_t[0:3] # q w * r b_t_est x_t[3:6] # 力矩零点偏置 r_est q / w # 重心位置 return { weight: w, force_bias: b_f_est, torque_bias: b_t_est, centroid: r_est, residual_f: res_f, residual_t: res_t, } # 使用示例伪数据 # rot_mats 是 list of 3x3 旋转矩阵 # forces 是 list of (fx, fy, fz) # torques 是 list of (mx, my, mz) # result calibrate(rot_mats, forces, torques)这段代码有几个地方值得注意g_world 用 [0, 0, -1] 的前提是世界系 z 轴向上。如果你的机械臂基座定义不是这样请按实际调整。我用的是 R_world_to_tool 左乘世界系向量得到传感器系下的坐标。这是 ROS 里常见约定但不同机械臂 SDK 可能不同务必先用简单姿态验证。lstsq 返回的 res 是残差平方和。如果 rank 值小于参数个数说明你的数据覆盖不够系数矩阵秩亏这种情况下解不可靠。q / w 求重心坐标时如果 w 为负或者接近零说明数据或符号有问题要回头检查。4.2 标定结果怎么判断好坏拿到参数后不能只看残差小就觉得万事大吉一定要做交叉验证。我自己的做法是把采集到的几十组数据随机分成两份一份用来拟合参数另一份用来验证。用拟合得到的参数去预测验证集里的传感器读数算预测值和实测值之间的误差。判断标定质量时可以看几个指标力通道残差 RMS正常应在 0.5N 以内取决于传感器量程和噪声水平量程 100N 左右的传感器做到 0.2N 以内很常见。力矩通道残差 RMS一般在 0.02 到 0.05 N·m 以内。估计出的重量 w 与实际工具重量偏差应小于 2% 到 3%。比如一个 5kg 的工具约 49N估计值应该在 48N 到 50N 这个区间否则多半是数据覆盖或者旋转矩阵方向搞错了。重心位置 r 和实际安装位置大致符合。比如工具重心在传感器坐标系下偏 X 正方向 30mm那么 rx 就应该是 0.03 左右。如果偏差数量级不对优先查力矩通道的符号方向。我当时第一次标定的 w 估计是 47.3N实际重量 49N 左右残差也还行但重心位置明显不对。后面发现是力矩的符号约定理解错了把传感器输出力矩方向当成了和坐标系定义一致实际上在某一个通道上是反的。修正之后w 变成 49.2N重心位置也正常了。4.3 残差分布藏着哪些信息每次标定完我会画一张残差图横轴是样本序号纵轴是每个样本中 6 个通道的预测误差。如果发现残差不是随机散布而是有明显的规律性——比如某个方向总偏大或者误差随着姿态角度变大而变大——那就要警惕了。规律性残差通常意味着模型没建对而不是随机噪声。最常见的规律性残差来源有三个一是传感器非线性。大载荷传感器在小载荷段往往有迟滞和非线性这时候用简单的线性模型无法完全拟合残差会表现出和载荷水平相关的趋势。这种情况在一般工业应用里可以接受只要残差幅度小于力控阈值就行。二是姿态不准确。如果机械臂的定位精度不高或者工具安装有柔性变形那么旋转矩阵 R 本身就带误差导致 c 值不准最终残差会随姿态变化而增大。三是温度漂移。标定采样时间如果太长传感器的零点在不同时间段已经发生变化前后数据不一致残差也会显著偏大。这也是我后来坚持尽量在短时间内采完的原因。5. 重力补偿上机补偿公式、切换逻辑与力控衔接5.1 实时补偿公式标定完成之后重力补偿就变成了纯计算问题。在运行阶段从机械臂控制器拿到当前旋转矩阵 R算出 c然后实时扣除力和力矩中的重力分量。补偿公式如下f_ext f_raw - (w * c b_f) τ_ext τ_raw - (q × c b_τ)也就是用标定得到的 w、b_f、q、b_τ 作为已知常数每帧计算一次。用 Python 写出来就是def compensate(raw_force, raw_torque, R_w2t, calib): c R_w2t np.array([0.0, 0.0, -1.0]) force_gravity calib[weight] * c calib[force_bias] torque_gravity np.cross(calib[centroid] * calib[weight], c) calib[torque_bias] force_ext raw_force - force_gravity torque_ext raw_torque - torque_gravity return force_ext, torque_ext这里 c 的更新频率要跟得上机械臂运动。如果机械臂运动速度比较快建议把这段计算放到实时控制线程里使用高频运动学反馈。如果控制周期是 1kHz这个计算量完全在负担范围内。5.2 动态场景下重力补偿的边界说实话上面这个补偿公式只在准静态场景下严格成立。当机械臂末端有加速运动时工具质量会给传感器带来额外的惯性力这部分力没有被重力项覆盖。如果是做普通装配、力位混合控制机械臂速度通常不快这个影响可以忽略如果是做打磨、高速抛光的恒力控制必须把加速度项考虑进去。处理动态项的做法一般有两种。一种是在控制器里用加速度计或运动学加速度估计值把惯性力项加进补偿模型另一种是给传感器读数做低通滤波用滤波后的信号做稳定力控牺牲瞬态响应来换取稳定。对大多数入门项目第二种方案更简单可靠同时注意调整控制器的力带宽不要试图补偿高频分量。另外要提醒一点传感器读数在机械臂运动时还有振动噪声尤其是六维力传感器本身固有频率不高的情况下上机前的滤波设计很重要。我常用的是一阶低通滤波截止频率在 5 到 20Hz 之间具体要看你的控制周期和机械结构刚性。5.3 力控衔接的几个具体问题重力补偿做好之后力控才有意义。做力控最直接的判断是机械臂在静止状态你用手去推末端工具传感器补偿后的力能否准确反映你的推力。这个实验如果通过再去做力跟踪控制就顺理成章了。我在力控衔接时遇到过几个具体问题补偿后的数据在低速运动时会有轻微的残余重力现象即静止时读数接近零但动起来会有 1 到 2N 的漂移。这个通常来自旋转矩阵更新滞后和传感器动态响应差异可以用死区或者状态机去抑制。接触判定阈值设置不能只看补偿后的噪声 RMS还要看力控方向上的动态噪声。比如 Z 轴接触检测阈值至少设成补偿后 Z 轴读数噪声的 3 到 4 倍。在机械臂切换工具后一定要重新标定。电爪、吸盘、气爪这些工具的重量和重心差异很大复用旧参数轻则力控目标不准重则接触检测完全失效。6. 实战中踩过的坑与排查思路6.1 数据采集时末端没有真正停稳这是最隐蔽的坑。机械臂控制器报到位实际上末端因为减速和残余振动还在小幅度晃动这时候读传感器力数据里混着动态分量。表现就是你标定出的参数时好时坏重复采集两次结果差异很大。排查思路在采集代码里加一个稳定判据比如连续 50ms 内六维力读数变化都小于阈值才认为该姿态可以记录。不要只依赖机械臂控制器的到位信号。6.2 姿态覆盖太集中导致系数矩阵病态前面提到过如果所有姿态都让工具接近水平c 向量就都聚集在同一个平面附近导致系数矩阵条件数很大求解结果对噪声极其敏感。判断方法很简单用 numpy 打印 A 矩阵的条件数cond(A)如果超过几千说明姿态集设计不合理。重新采一组覆盖更广的姿态条件数通常会降到几十以内。6.3 温飘在做精细力控时不可忽视传感器上电半小时和刚上电时的零点能差出 0.5N 以上这对 1N 级别的接触力控制是致命的。我的处理办法是长时间工作前先预热上电后等 15 到 20 分钟再上电清零如果现场不允许等待至少保证标定和实际使用的温升状态一致。另外标定用的数据也尽量在机械臂工作一段时间、温度稳定之后采集这样标定参数和运行状态的温度条件更匹配。6.4 旋转矩阵方向和传感器坐标系对不上常见症状力补偿完之后静止读数虽然归零但把机械臂摆到另一个姿态后读数出现很大的伪接触力。这多半是 R 矩阵的坐标系约定和传感器坐标系定义不一致。排查方法很简单让机械臂工具朝下打印 c 向量理论上应该是 [0, 0, 1]如果世界系 z 轴向上重力方向朝下在传感器坐标系下就是工具坐标系 z 轴不同安装不同需要看工具坐标系的定义。如果得到的是 [0, 0, -1]说明你 R 矩阵求了转置或者用的是工具到世界的旋转矩阵。修正这个方向问题比调任何参数都重要。6.5 标定结果看起来很好但接触检测还是失效这种情况往往不是标定本身的问题而是传感器读数的量纲或符号约定在后续处理链路里出了问题。常见的有力矩单位是 N·m 而你按 N·mm 处理力矩正方向定义反了或者换了一个机械臂工具之后用了旧参数。我的建议是力控调试时先写一个简单的测试脚本让机械臂在几个固定姿态下静止打印补偿后的力再手动施加力和力矩确认六个通道的方向都正确后再开始调控制逻辑。这个习惯帮我省了至少两天的排查时间。最后再分享一个小技巧标定完参数先别急着写进正式控制程序。拿一个标准砝码比如 1kg 或 2kg 的砝码吸在工具末端换几个姿态验证补偿后的力读数是否接近砝码重量。这个测试不到十分钟但能一次性发现问题比在力控代码里做各种奇怪补偿动作有效得多。六维力传感器标定和重力补偿这个东西原理不复杂最难的反而是把每个细节做扎实。参数跑出来只是开始真正可靠的数据链路是在踩完这些坑之后才建立起来的。