企业官网建设流程全解析

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 · Gy
• My = rz · Gx - rx · Gz
• Mz = 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_sensor

3.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, rcond=None) 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，可以接受。

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 从零复现的步骤清单

如果你拿到一套新设备，想从头做一遍重力补偿，按这个顺序走：

1. 确认机械臂DH参数，用示教器验证正运动学输出
2. 确认传感器安装矩阵，如果不确定，先假设为单位矩阵，后续标定中修正
3. 采集16个姿态的静态数据，每个姿态100帧平均
4. 运行标定脚本，得到质量m和重心com
5. 用验证脚本检查残差，如果残差大，回到第3步检查数据质量
6. 把标定结果写入配置文件
7. 在线运行补偿节点，手动施加已知力验证补偿效果
8. 接入力控算法，观察实际控制效果

整个过程如果顺利，半天到一天能完成。如果遇到数据问题，可能需要反复几次。

7.3 不同品牌传感器的适配要点

不同品牌的六维力传感器在输出格式、坐标系定义、温度特性上都有差异。ATI的传感器通常输出的是传感器坐标系下的力，坐标系标注在传感器侧面。宇立的传感器有的型号输出的是工具坐标系下的力，需要确认。坤维的传感器在零漂和温漂控制上做得不错，但价格也高一些。

适配时重点确认三件事：输出坐标系是哪个、力的正方向定义、力矩的参考点在哪里。这三件事搞清楚了，剩下的就是套公式。

我在实际项目里用过ATI、宇立和鑫精诚的传感器，算法层面完全通用，只是配置文件里的安装矩阵和符号约定需要调整。建议每换一种传感器，先用已知重量的标准砝码做一次简单验证，确认方向对了再上工具。

最后分享一个小技巧：如果你不确定重心位置，可以先假设重心在传感器坐标系原点，只补偿力不补偿力矩，看看效果。如果力补偿后残差很小但力矩残差大，说明重心确实偏离原点，需要标定。如果力补偿后残差就很大，那可能是质量参数或者旋转矩阵有问题，先解决这两个再管重心。