1. 为什么你的机械臂一装上工具就“手抖”先搞懂重力补偿到底在补什么六维力传感器装在机械臂末端和工具之间测的是三个方向的力Fx、Fy、Fz加三个方向的力矩Mx、My、Mz。理想情况下机械臂静止、没有任何外力接触时传感器应该输出接近零的读数。但现实很骨感——只要末端挂了工具比如夹爪、焊枪、打磨头传感器立刻就会读到一个稳定的非零值。这个值不是外界施加的而是工具自身重力在传感器坐标系下的分量。很多人第一次遇到这个现象会以为是传感器坏了或者标定没做对。其实传感器没坏它只是太诚实了。工具的重心不在传感器坐标系原点上重力方向在传感器坐标系里也不是恒定的——机械臂姿态一变重力在传感器各轴上的投影就跟着变。所以你会看到机械臂水平伸展时Fz读到一个大值竖直朝下时Fz又变成另一个值力矩Mx、My也跟着姿态变化。这就是重力干扰。重力补偿要干的事情说白了就一句话根据当前机械臂姿态和工具的重力参数实时算出工具重力在传感器坐标系下产生的力和力矩分量然后从原始读数里减掉。减完之后传感器输出的才是真正由外部接触产生的力/力矩信号。没有这一步后面的力控、柔顺控制、碰撞检测全是空中楼阁。这个内容适合谁看如果你正在做机械臂力控相关的项目用的是ATI、宇立、坤维、鑫精诚这类六维力传感器机械臂品牌可能是UR、Franka、遨博、法奥、节卡、埃夫特等那这篇就是写给你的。Python基础不需要多深能看懂numpy矩阵运算、会写函数就行。ROS2环境下做开发的话会更顺但本文的算法逻辑不依赖任何特定框架你用什么平台都能移植。我自己的经历是第一次做重力补偿时工具是一个1.2kg的打磨头重心大概在传感器坐标系下方8cm处。没做补偿之前机械臂从水平转到竖直Fz读数从-11.8N变到11.7N力矩Mx从0.9Nm变到-0.9Nm。这个量级在打磨力控里完全不可接受因为目标接触力可能才20N。补偿之后静态残差控制在0.15N以内力控才真正跑得起来。2. 重力补偿的数学模型从物理直觉到可编程公式2.1 工具重力在传感器坐标系中的分解逻辑先建立坐标系关系。六维力传感器有自己的测量坐标系通常标注在传感器本体上一般Z轴垂直于法兰面。工具固定在传感器末端工具的重心在传感器坐标系下的位置记作r [rx, ry, rz]^T单位是米。工具的质量记作m重力加速度g 9.81 m/s²。重力在世界坐标系下永远指向下方方向向量是[0, 0, -g]^T假设世界坐标系Z轴朝上。但传感器读到的是传感器坐标系下的分量所以需要把世界坐标系下的重力向量旋转到传感器坐标系G_sensor R^T · [0, 0, -mg]^T其中R是传感器坐标系相对于世界坐标系的旋转矩阵。这个旋转矩阵从哪来从机械臂的正运动学。你通过机械臂的关节角度和DH参数算出末端法兰的位姿再乘以传感器到法兰的安装变换就得到了传感器坐标系在世界坐标系下的姿态。有了G_sensor [Gx, Gy, Gz]^T重力在传感器各轴上产生的力就是这三个分量。力矩部分则需要用重心位置做叉乘M_gravity r × G_sensor展开就是Mx ry · Gz - rz · GyMy rz · Gx - rx · GzMz rx · Gy - ry · Gx最终补偿后的六维力向量为F_compensated F_raw - [Gx, Gy, Gz, Mx, My, Mz]^T这就是整个重力补偿的核心公式。看起来简单但魔鬼在细节里——旋转矩阵的精度、重心位置的标定、质量参数的准确性每一个都会直接影响补偿效果。2.2 旋转矩阵怎么来从关节角到传感器姿态假设你用的是六轴串联机械臂标准DH参数已知。从基座到末端法兰的变换矩阵是T_base_flange T1 · T2 · T3 · T4 · T5 · T6每个Ti由关节角θi和DH参数a, α, d决定。算完得到4×4齐次变换矩阵取左上角3×3就是法兰在基座坐标系下的旋转矩阵R_base_flange。传感器安装在法兰上有一个固定的安装旋转R_flange_sensor这个由你的机械安装方式决定。如果传感器Z轴和法兰Z轴对齐那这个矩阵就是单位矩阵如果有偏转需要根据实际安装角度构造。最终传感器在基座坐标系下的旋转矩阵R_base_sensor R_base_flange · R_flange_sensor而重力补偿需要的是传感器坐标系下的重力分量所以G_sensor R_base_sensor^T · [0, 0, -mg]^T这里有个容易搞混的地方到底是R还是R的转置。记住一个原则——世界坐标系下的向量转到传感器坐标系用的是从传感器到世界的旋转矩阵的转置也就是从世界到传感器的旋转矩阵。如果你算出来的补偿方向反了十有八九是转置搞错了。注意不同品牌的机械臂正运动学约定不同有的用改进DH有的用标准DH旋转矩阵的构造方式有差异。建议先用示教器把机械臂移到几个已知姿态手动验证你的正运动学输出和示教器显示的位姿是否一致确认无误后再往下做。2.3 参数标定质量、重心、安装角怎么测理论上如果你知道工具的CAD模型质量和重心可以直接从模型里读。但实际中工具往往经过改装、加装线缆、换过配件CAD模型和实物有偏差。更可靠的做法是实验标定。标定思路是这样的让机械臂带着工具走N个不同姿态N≥3每个姿态下记录传感器原始读数和对应的旋转矩阵。因为工具重力参数m, rx, ry, rz是固定的每个姿态给出的方程都是线性的可以组成超定方程组用最小二乘求解。具体来说对于第i个姿态F_raw_i R_base_sensor_i^T · [0, 0, -mg]^T F_ext_i假设没有外部接触F_ext_i 0那么F_raw_i R_base_sensor_i^T · [0, 0, -mg]^T把未知量写成向量p [mg·rx, mg·ry, mg·rz, mg]^T或者直接用[m, rx, ry, rz]做非线性优化都可以。我一般用线性形式因为求解快、不需要初值。实际操作中我会取12到20个姿态覆盖机械臂工作空间的主要方向——水平四个方向、竖直向上、竖直向下、以及几个倾斜姿态。姿态越多标定结果越稳。但注意每个姿态下要等机械臂完全静止再采样否则加速度会引入额外惯性力。标定完成后把得到的m、rx、ry、rz写进配置文件运行时直接调用。3. Python实现从零搭建重力补偿模块3.1 环境准备与依赖安装我用的环境是Python 3.10 numpy scipy。如果你在ROS2下开发rclpy也会用到但核心算法只依赖numpy。安装命令pip install numpy scipy如果你还没装Python建议直接装Miniconda省去很多环境配置的麻烦。VSCode里配好Python解释器路径装个Pylance插件写代码体验会好很多。提示numpy的版本建议1.24以上低版本在某些矩阵运算上性能差异明显。scipy主要用来做最小二乘优化如果你只用线性标定方法scipy可以不用。3.2 正运动学与旋转矩阵计算下面是一个通用的正运动学函数输入关节角和DH参数表输出末端位姿矩阵。DH参数表格式为每行 [a, alpha, d, theta_offset]其中theta_offset是关节零位偏移。import numpy as np def dh_transform(a, alpha, d, theta): 标准DH参数构造单关节变换矩阵 ct np.cos(theta) st np.sin(theta) ca np.cos(alpha) sa np.sin(alpha) T np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0, sa, ca, d], [0, 0, 0, 1] ]) return T def forward_kinematics(joint_angles, dh_params): joint_angles: 关节角列表单位弧度 dh_params: Nx4数组每行 [a, alpha, d, theta_offset] 返回: 4x4 末端位姿矩阵 T np.eye(4) for i, (a, alpha, d, theta_off) in enumerate(dh_params): theta joint_angles[i] theta_off T T dh_transform(a, alpha, d, theta) return T这个函数返回的是末端法兰在基座坐标系下的位姿。如果你需要传感器坐标系的旋转矩阵再右乘一个安装矩阵def get_sensor_rotation(joint_angles, dh_params, R_flange_sensor): 返回传感器坐标系在基座坐标系下的旋转矩阵 T_flange forward_kinematics(joint_angles, dh_params) R_flange T_flange[:3, :3] R_sensor R_flange R_flange_sensor return R_sensor3.3 重力补偿核心函数有了旋转矩阵补偿计算就是几行代码的事def gravity_compensation(F_raw, R_sensor, mass, com): F_raw: 传感器原始读数 [Fx, Fy, Fz, Mx, My, Mz] R_sensor: 3x3 传感器在基座坐标系下的旋转矩阵 mass: 工具质量 kg com: 工具重心在传感器坐标系下的位置 [rx, ry, rz] 单位米 返回: 补偿后的六维力 g 9.81 # 基座坐标系下的重力向量 G_base np.array([0, 0, -mass * g]) # 转到传感器坐标系 G_sensor R_sensor.T G_base # 重力产生的力矩 M_gravity np.cross(com, G_sensor) # 组装补偿量 bias np.concatenate([G_sensor, M_gravity]) # 减去偏置 F_compensated np.array(F_raw) - bias return F_compensated这段代码里R_sensor.T G_base就是把基座坐标系下的重力转到传感器坐标系。np.cross(com, G_sensor)算的是重力对传感器原点的力矩。最后从原始读数里减掉。注意com的单位是米不是毫米。很多传感器手册给的重心位置是毫米直接填进去会导致力矩补偿差1000倍。我第一次做的时候就踩过这个坑补偿后力矩反而更大了排查了半天才发现是单位问题。3.4 标定脚本最小二乘求解工具参数标定脚本的思路是采集多个姿态的数据构造线性方程组用numpy的lstsq求解。假设我们采集了N个姿态每个姿态有旋转矩阵R_i和原始力读数F_i。def calibrate_gravity_params(R_list, F_list): R_list: N个3x3旋转矩阵 F_list: N个6维力读数 返回: mass, com N len(R_list) A [] b [] g 9.81 for i in range(N): R R_list[i] F F_list[i] # 重力在传感器坐标系下的方向单位向量不含质量 g_dir R.T np.array([0, 0, -g]) # 力部分方程: Fx m * g_dir_x, 等等 # 力矩部分: Mx m * (ry * g_dir_z - rz * g_dir_y) # 未知量: [m, m*rx, m*ry, m*rz] A.append([g_dir[0], 0, 0, 0]) A.append([g_dir[1], 0, 0, 0]) A.append([g_dir[2], 0, 0, 0]) A.append([0, 0, g_dir[2], -g_dir[1]]) A.append([0, -g_dir[2], 0, g_dir[0]]) A.append([0, g_dir[1], -g_dir[0], 0]) b.extend(F) A np.array(A) b np.array(b) x, residuals, rank, sv np.linalg.lstsq(A, b, rcondNone) m x[0] com x[1:] / m return m, com这个标定方法的好处是不需要初值直接线性求解。但要注意如果采集的姿态不够多或者姿态变化不够丰富A矩阵可能秩亏解出来的结果不可信。我一般会检查residuals如果残差太大说明数据有问题需要重新采集。4. 实操全流程从采集数据到验证补偿效果4.1 数据采集的姿势与节奏控制标定数据采集是整个流程里最需要耐心的环节。我的做法是手动示教或者用程序控制机械臂走到预定姿态每个姿态停稳后等2秒再采样连续采100帧取平均。为什么要等2秒因为机械臂停止后会有微小振动伺服系统也需要时间稳定。100帧平均能有效抑制随机噪声。姿态选择上我通常选16个姿态以传感器坐标系为参考Z轴朝上、朝下、朝前、朝后、朝左、朝右各一个再加上8个45度倾斜方向。这样覆盖了重力在传感器坐标系下所有可能的方向标定矩阵的条件数比较好。采集时记录的数据包括关节角、原始六维力读数、时间戳。关节角用来算旋转矩阵力读数用来做标定。如果你用的是ROS2可以直接订阅joint_states和wrench话题用rosbag录下来再离线处理。提示采集数据时确保工具上没有外部接触线缆也要固定好避免线缆的拉扯力被传感器读到。我遇到过线缆太硬导致某个姿态下读数异常的情况换软线后问题消失。4.2 标定结果验证与残差分析标定完成后用另一组没参与标定的姿态数据做验证。计算补偿后的残差def validate_calibration(R_list, F_list, mass, com): 返回每个姿态的补偿残差 residuals [] for R, F in zip(R_list, F_list): F_comp gravity_compensation(F, R, mass, com) residuals.append(F_comp) return np.array(residuals)好的标定结果残差应该在传感器噪声水平附近。以我用的某国产六维力传感器为例量程200N精度0.5%FS噪声大概在0.1N左右。补偿后残差如果控制在0.2N以内说明标定合格。如果残差超过1N需要检查旋转矩阵是否正确、重心单位是否搞错、采集数据时是否有外部接触。我一般会把残差画成图看六个分量随姿态的变化。如果某个分量呈现明显的正弦规律说明该方向的参数没标定准。比如Fz残差随姿态角呈正弦变化通常是质量m有偏差Mx残差呈正弦变化通常是ry或rz有偏差。4.3 实时补偿的代码集成与性能考量标定参数确定后实时补偿就是查表加计算。在ROS2节点里我通常这样组织class GravityCompensator: def __init__(self, dh_params, R_flange_sensor, mass, com): self.dh_params dh_params self.R_flange_sensor R_flange_sensor self.mass mass self.com com def compensate(self, joint_angles, F_raw): R_sensor get_sensor_rotation( joint_angles, self.dh_params, self.R_flange_sensor ) return gravity_compensation(F_raw, R_sensor, self.mass, self.com)性能上一次补偿计算大概消耗几十微秒对于1kHz的控制循环完全够用。如果你在Python里做实时控制建议把numpy运算预先分配好数组避免在循环里频繁创建新对象。更极致的做法是用numba做JIT编译或者把核心计算用C实现Python只做接口。注意实时补偿时关节角的时间戳要和力传感器的时间戳对齐。如果机械臂运动较快时间不同步会导致补偿量算错。我一般用最近邻插值对齐时间戳或者用硬件触发同步采集。5. 踩坑实录那些让我熬夜排查的典型问题5.1 补偿方向反了转置矩阵的经典陷阱这是最常见的错误。现象是补偿后力读数不但没变小反而变大了而且随姿态变化的规律和补偿前相反。原因就是旋转矩阵用反了。记住从基座坐标系转到传感器坐标系用的是R_sensor的转置。如果你把R_sensor直接乘上去相当于把重力转到了错误的方向。排查方法很简单取一个姿态手动算一下重力在传感器坐标系下应该指向哪个方向和代码输出对比。比如机械臂竖直朝下时传感器Z轴朝下重力在传感器坐标系下应该是Z方向因为传感器Z轴和重力同向。如果代码输出是-Z那就是转置搞反了。5.2 力矩补偿量级不对毫米与米的单位混淆前面提过重心位置的单位是米。但很多传感器手册、CAD软件默认用毫米。如果你直接把毫米数值填进代码力矩补偿会放大1000倍。现象是力补偿看起来正常但力矩补偿后反而引入了一个巨大的偏置。我的习惯是所有长度单位统一用米在代码注释里明确标注。如果从手册拿到的是毫米先除以1000再填入。这个坑我踩过两次第二次是因为换了一个工具手册给的是毫米我忘了转换。5.3 标定残差过大数据采集的隐藏干扰标定残差大不一定是算法问题很可能是数据采集时引入了外部干扰。常见的干扰源包括线缆拉扯工具上的线缆如果绷得太紧会在某些姿态下产生额外力气管推力气动工具的气管在通气状态下会有微小推力机械臂振动伺服未完全稳定就采样温度漂移传感器预热不充分零漂随温度变化我的做法是采集数据前让机械臂空跑10分钟预热采集时确保线缆松弛、气管断开每个姿态等3秒再采样。如果残差还是大检查传感器是否过载过过载后的传感器可能产生永久零漂。5.4 动态场景下的补偿失效惯性力的影响重力补偿只补偿静态重力机械臂加速运动时工具惯性力也会被传感器读到。如果你在高速运动下做力控会发现补偿后仍然有周期性波动那就是惯性力在作怪。解决思路有两个一是降低运动加速度让惯性力小到可以忽略二是做惯性补偿需要知道工具的惯量矩阵通过关节加速度算惯性力。惯性补偿比重力补偿复杂得多一般工业场景下如果加速度不超过0.5m/s²惯性力通常小于1N可以接受。问题现象可能原因排查方法解决措施补偿后力变大旋转矩阵转置错误手动验证单姿态重力方向改用R_sensor.T力矩补偿异常大重心单位用了毫米检查com数值量级除以1000转为米标定残差1N线缆拉扯或振动检查线缆状态延长稳定时间固定线缆等3秒再采样动态下波动惯性力未补偿降低加速度观察是否改善限制加速度或做惯性补偿某方向残差呈正弦对应参数标定不准分析残差随姿态变化规律增加该方向姿态数据重新标定6. 进阶技巧让重力补偿更稳的几个实战经验6.1 温度漂移的在线估计与补偿六维力传感器对温度敏感冷机状态和热机状态的零漂可能差0.5N以上。如果你的应用对精度要求高建议做温度补偿。简单做法是在传感器上贴一个温度传感器记录不同温度下的零漂拟合一条温度-零漂曲线运行时根据实时温度修正。更工程化的做法是每次机械臂上电后先走一个已知姿态用重力补偿模型算出的理论值和实际读数对比得到一个零漂修正量后续运行中持续更新。这个方法不需要额外温度传感器但要求机械臂每次上电后的初始姿态一致。6.2 多工具快换的参数管理如果你的产线上有多个工具需要快换每个工具的重力参数都不一样。我的做法是给每个工具分配一个ID参数存在配置文件里快换完成后通过工具ID加载对应参数。配置文件用YAML格式方便人工编辑tool_0: name: 打磨头 mass: 1.23 com: [0.002, -0.001, 0.085] tool_1: name: 夹爪 mass: 0.87 com: [0.001, 0.003, 0.062]快换盘上可以加一个电阻编码或者RFID自动识别工具ID避免人工选错。6.3 补偿效果的量化评估方法怎么判断补偿做得好不好我一般看三个指标静态残差机械臂静止时补偿后六维力的均方根值。好的结果应该在传感器噪声的1.5倍以内。姿态一致性机械臂在多个姿态下补偿后同一方向的外力读数应该一致。比如你用手在工具末端施加一个恒定的5N推力不管机械臂什么姿态补偿后读数都应该是5N左右。重复性同一姿态多次往返补偿后读数的标准差。这个指标反映的是系统稳定性。我通常会在标定完成后跑一个自动化测试脚本让机械臂走20个姿态每个姿态记录补偿后读数自动计算上述指标并生成报告。这样每次换工具或者改参数后都能快速验证效果。6.4 与力控算法的衔接注意事项重力补偿是力控的前置环节补偿后的信号直接送给力控器。这里有几个衔接细节滤波补偿后的信号建议做低通滤波截止频率根据力控带宽定。我一般用二阶巴特沃斯截止频率50Hz。零漂在线更新长时间运行时传感器零漂会缓慢变化。可以在机械臂确认无接触的时段比如空程移动时自动更新零漂偏置。坐标系一致性确保重力补偿输出的坐标系和力控算法期望的坐标系一致。有的力控算法期望基座坐标系下的力有的期望工具坐标系下的力需要做相应旋转。提示如果你在ROS2下用moveit做规划注意moveit的力控插件可能自带重力补偿功能但参数需要你自己填。我建议先用本文的方法独立验证补偿效果再和moveit的补偿结果对比确认一致后再用moveit的。7. 代码仓库结构与快速复现指南7.1 推荐的项目文件组织我把整个重力补偿模块组织成以下结构方便复用和移植gravity_compensation/ ├── config/ │ ├── dh_params.yaml # 机械臂DH参数 │ ├── sensor_mount.yaml # 传感器安装矩阵 │ └── tool_params.yaml # 工具质量与重心 ├── src/ │ ├── kinematics.py # 正运动学 │ ├── compensation.py # 重力补偿核心 │ ├── calibration.py # 标定算法 │ └── validation.py # 验证与残差分析 ├── scripts/ │ ├── collect_data.py # 数据采集 │ ├── run_calibration.py # 执行标定 │ └── test_compensation.py # 在线测试 └── README.md这种组织方式的好处是标定和运行分离参数和代码分离。换工具时只需要改config里的YAML文件代码不用动。7.2 从零复现的步骤清单如果你拿到一套新设备想从头做一遍重力补偿按这个顺序走确认机械臂DH参数用示教器验证正运动学输出确认传感器安装矩阵如果不确定先假设为单位矩阵后续标定中修正采集16个姿态的静态数据每个姿态100帧平均运行标定脚本得到质量m和重心com用验证脚本检查残差如果残差大回到第3步检查数据质量把标定结果写入配置文件在线运行补偿节点手动施加已知力验证补偿效果接入力控算法观察实际控制效果整个过程如果顺利半天到一天能完成。如果遇到数据问题可能需要反复几次。7.3 不同品牌传感器的适配要点不同品牌的六维力传感器在输出格式、坐标系定义、温度特性上都有差异。ATI的传感器通常输出的是传感器坐标系下的力坐标系标注在传感器侧面。宇立的传感器有的型号输出的是工具坐标系下的力需要确认。坤维的传感器在零漂和温漂控制上做得不错但价格也高一些。适配时重点确认三件事输出坐标系是哪个、力的正方向定义、力矩的参考点在哪里。这三件事搞清楚了剩下的就是套公式。我在实际项目里用过ATI、宇立和鑫精诚的传感器算法层面完全通用只是配置文件里的安装矩阵和符号约定需要调整。建议每换一种传感器先用已知重量的标准砝码做一次简单验证确认方向对了再上工具。最后分享一个小技巧如果你不确定重心位置可以先假设重心在传感器坐标系原点只补偿力不补偿力矩看看效果。如果力补偿后残差很小但力矩残差大说明重心确实偏离原点需要标定。如果力补偿后残差就很大那可能是质量参数或者旋转矩阵有问题先解决这两个再管重心。