4. 重力补偿：机器人自重与负载的重力补偿算法

重力补偿，说白了就是让机器人感觉不到自己的重量和它抓的东西的重量。你想想看，一个几十公斤的机械臂，如果不对重力做补偿，你用手推它，感觉就像在推一堵墙——电机要一直出力顶着，控制器也分不清你施加的力是外力还是重力。

我刚开始做力控的时候，就踩过这个坑。当时在调试一台协作机器人，空载时力控效果还不错，一装上夹具就开始飘。查了半天才发现，重力补偿参数没更新。嗯，这节课我们就把这个事彻底讲清楚。

4.1 重力补偿的本质

重力补偿要解决什么问题？很简单：让机器人在任意姿态下，关节力矩中不包含重力分量。这样，当我们施加外力时，控制器才能准确感知到「这是人推的力」，而不是「这是重力在某个关节上的投影」。

数学上，重力补偿就是计算：

τ_gravity = J^T * G(q)

其中：

• τ_gravity — 各关节需要补偿的重力矩
• J — 雅可比矩阵（把末端力映射到关节空间）
• G(q) — 当前姿态下的重力向量

这个公式看着简单，但实际落地时，坑全在细节里。

4.2 静态重力补偿

静态补偿，就是假设机器人不动，或者动得很慢。这时候重力只跟关节角度有关，跟速度、加速度无关。

我习惯把静态补偿分成三步走：

1. 辨识连杆质量与质心 — 每个连杆的重量、重心位置，必须标定出来
2. 建立运动学模型 — 知道每个关节角度下，各连杆的位姿
3. 实时查表或计算 — 根据当前关节角，算出补偿力矩并叠加到电流环

下面是一个简化的静态补偿代码片段：

def static_gravity_compensation(q, link_params):
    """
    q: 当前关节角度 [6x1]
    link_params: 连杆参数列表，每个元素包含 mass, com (质心位置)
    """
    tau_comp = np.zeros(6)
    for i in range(6):
        # 计算第i个连杆在世界坐标系下的质心位置
        p_com = forward_kinematics(q, i, link_params[i].com)
        # 重力向量
        g = np.array([0, 0, -9.81])
        # 雅可比矩阵的第i列（对应第i个连杆的质心）
        J_com = compute_jacobian_com(q, i)
        # 补偿力矩
        tau_comp += J_com.T @ (link_params[i].mass * g)
    return tau_comp

这段代码在仿真里跑得挺好，但上了真机你就会发现——摩擦力和柔性变形会干扰辨识结果。我曾经在标定一台六轴机器人时，同一个姿态测了五次，力矩值差了15%。后来加了低通滤波和多组数据平均，才把误差压到3%以内。

4.3 动态重力补偿

静态补偿够用吗？大多数场景下够了。但如果你做的是高速搬运、抛接物体、或者人机协作中的快速响应，那就必须上动态补偿。

动态补偿要考虑什么？说白了就是惯性力和科里奥利力。机器人动起来的时候，连杆的加速度会产生惯性力矩，不同关节之间的耦合运动会产生科里奥利力矩。这些力如果不补偿，力控系统会误判为外力。

完整的动力学方程是这样的：

τ = M(q) * q̈ + C(q, q̇) * q̇ + G(q) + τ_ext

其中：

• M(q) — 惯性矩阵
• C(q, q̇) — 科里奥利与离心力矩阵
• G(q) — 重力项
• τ_ext — 外力矩（我们真正关心的信号）

动态补偿的目标，就是从传感器读到的总力矩中，减去 M(q) * q̈ + C(q, q̇) * q̇ + G(q)，剩下的就是 τ_ext。

4.4 静态 vs 动态：怎么选？

我整理了一个对比表，方便你根据场景做选择：

我个人建议：先从静态补偿做起。把静态补偿做到位了，再考虑加动态项。很多项目其实静态补偿就够用，没必要为了炫技上动态。

4.5 负载的重力补偿

机器人自己重量的补偿搞定了，那抓在手里的东西呢？负载的重力补偿，本质上跟自重补偿一样，但有个麻烦——负载参数是变化的。

你今天抓一个1kg的工件，明天换一个3kg的，后天可能抓一个重心偏的异形件。每次都要重新标定？那太不现实了。

我常用的做法是在线辨识负载参数：

1. 机器人抓取负载后，做一个小的「辨识运动」——比如在关节空间走一个正弦轨迹
2. 记录关节力矩和运动数据
3. 用递推最小二乘法（RLS）在线估计负载的质量和质心
4. 将辨识结果更新到补偿模型中

4.6 重力补偿的整体流程

下面这张图，是我做重力补偿时常用的流程框架：

这个流程里，我特别想强调一点：补偿不是一次性的。机器人用久了，关节磨损、负载变化、温度漂移，都会让补偿参数慢慢失效。我建议每隔一段时间做一次自动标定，或者在做力控之前先跑一个「归零」流程。

4.7 工程落地中的几个坑

最后，分享几个我在项目中踩过的坑，希望能帮你省点时间：

• 传感器偏置 — 力矩传感器上电后会有零偏，不归零就直接做补偿，结果全偏。我习惯每次上电后让机器人回到同一个「零位姿态」，记录传感器读数作为偏置。
• 摩擦力的干扰 — 重力补偿和摩擦力补偿经常混在一起。低速时摩擦力占比很大，如果不单独处理，辨识出来的质心参数会带偏。我一般先做摩擦力辨识，再做重力辨识。
• 奇异位形 — 在奇异点附近，雅可比矩阵的某些元素会变得非常大，导致补偿力矩剧烈抖动。解决办法是加一个阻尼项，或者直接限制补偿力矩的变化率。
• 负载变化检测 — 如果机器人抓取过程中负载掉了，补偿参数没更新，力控会突然变得很奇怪。我加了一个「残差监测」模块，当补偿后的残余力矩超过阈值时，自动触发重新辨识。