4. 重力补偿技术：工具重力辨识、负载重力辨识、实时重力补偿算法、零力控制模式

各位同学，今天我们来聊聊力控拖动示教里最核心、也最绕不开的一个话题——重力补偿。

说实话，我最早接触这个课题时，觉得不就是算个重力嘛，有啥难的？结果第一次在六轴机器人上做拖动示教，手一松，末端直接砸下去了。嗯，那场面，挺尴尬的。后来我才明白，重力补偿做不好，所谓的“零力控制”就是个笑话。

4.1 为什么需要重力补偿？

你想想看，机器人手臂本身就有重量，末端还装了工具，甚至抓着工件。这些重量在关节上会产生力矩。如果不做补偿，你推机器人时，感觉就像在推一个沉甸甸的铁疙瘩——费力不说，还根本没法精准控制。

说白了，重力补偿的目的就是：让机器人“感觉”不到自身和负载的重量。这样你轻轻一碰，它就能顺滑地跟着你的手走。

4.2 工具重力辨识

工具是装在机器人法兰盘上的。焊枪、夹爪、吸盘，五花八门。每个工具的重量、重心位置都不一样。

我在项目中遇到过最头疼的情况：客户换了个工具，没告诉我，结果拖动示教直接废了。所以，工具重力辨识必须做，而且要做准。

4.2.1 辨识原理

工具重力辨识，说白了就是求两个参数：工具质量 m_tool 和 重心位置 (x_c, y_c, z_c)。

怎么求？用最小二乘法。让机器人摆几个不同的姿态，记录关节力矩，然后反推。

我习惯用五点法：让机器人分别运动到5个不同的姿态，每个姿态下记录六维力传感器或关节力矩的数据。然后建立方程组求解。

4.2.2 辨识流程

1. 空载标定： 先拆掉工具，记录机器人自身重力影响（这个通常在出厂时做）。
2. 安装工具： 装上待辨识的工具。
3. 采集数据： 控制机器人运动到N个不同姿态（N≥4），记录每个姿态下的关节角度和力矩。
4. 建立方程： 利用机器人运动学，将工具重力映射到各关节。
5. 求解参数： 用最小二乘法解出工具质量和重心。

4.3 负载重力辨识

工具辨识完了，但机器人干活时还要抓工件。工件重量会变，比如抓一个10kg的箱子。这时候就需要负载重力辨识。

负载辨识和工具辨识原理一样，但有个区别：负载是动态变化的。每次抓取前，最好都能快速辨识一次。

4.3.1 快速辨识方法

我推荐一种“单点辨识法”：

• 让机器人保持一个固定姿态（比如竖直向下）。
• 记录抓取前后的关节力矩差值。
• 根据当前姿态，直接计算出负载重量。

这种方法快，但精度一般。如果要求高，还是得用多点法。

4.4 实时重力补偿算法

辨识出参数后，接下来就是实时补偿。这部分是算法的核心。

4.4.1 算法框架

实时重力补偿的流程如下：

1. 读取关节角度： 从编码器获取当前各关节角度 q。
2. 计算工具/负载重力： 根据运动学，将工具和负载的重力转换到基坐标系下。
3. 计算关节补偿力矩： 利用雅可比矩阵，将末端重力映射到各关节。
4. 叠加到控制指令： 将补偿力矩加到电机电流指令中。

这里有个关键点：雅可比矩阵。它负责把末端力转换到关节空间。公式如下：

τ_comp = J^T * F_gravity

其中，τ_comp 是关节补偿力矩，J^T 是雅可比矩阵的转置，F_gravity 是末端重力矢量。

4.4.2 代码示例（伪代码）

// 实时重力补偿函数
void gravityCompensation() {
    // 1. 读取当前关节角度
    float q[6] = readJointAngles();
    
    // 2. 计算工具重心在基坐标系下的位置
    Vector3d p_tool = forwardKinematics(q) * tool_cog;
    
    // 3. 计算重力矢量（假设重力沿Z轴负方向）
    Vector3d F_gravity(0, 0, - (m_tool + m_load) * g);
    
    // 4. 计算雅可比矩阵
    Matrix6x6 J = computeJacobian(q);
    
    // 5. 计算关节补偿力矩
    Vector6d tau_comp = J.transpose() * F_gravity;
    
    // 6. 叠加到电流指令
    for (int i = 0; i < 6; i++) {
        motor_current[i] += tau_comp[i] * torque_to_current;
    }
}

4.5 零力控制模式

重力补偿做好了，零力控制就水到渠成了。

零力控制，说白了就是：你推机器人，它跟着你走，感觉不到任何阻力。除了重力被补偿掉，摩擦力、惯性力也要尽量抵消。

4.5.1 控制架构

零力控制通常采用力矩控制模式。架构如下：

目标力矩 = 重力补偿力矩 + 摩擦力补偿 + 惯性力补偿 + 外力估计

其中：

• 重力补偿力矩： 上面算出来的 τ_comp
• 摩擦力补偿： 根据关节速度查表或模型计算
• 惯性力补偿： 根据加速度估算（通常忽略，因为拖动时速度慢）
• 外力估计： 通过力矩传感器或观测器得到

4.5.2 实现要点

我总结了几条实战经验：

• 摩擦力补偿是关键： 机器人关节的摩擦力很大，不补偿的话，推起来会有“卡顿感”。我习惯用Stribeck模型来拟合摩擦力曲线。
• 控制周期要短： 至少1kHz，最好2kHz以上。周期长了，手感会“肉”。
• 安全限幅： 补偿力矩不能无限大，要设置上下限，防止意外。

4.6 知识体系总览

为了让大家更直观地理解本章内容，我画了一张流程图：

4.7 总结

重力补偿技术，是力控拖动示教的基石。没有它，后面的一切都是空中楼阁。

我个人习惯把这一章的内容总结为三步走：

• 第一步： 把工具和负载的重力参数辨识准——这是基础，马虎不得。
• 第二步： 用雅可比矩阵实时计算补偿力矩——这是核心，要快且准。
• 第三步： 叠加摩擦力补偿，实现零力控制——这是目标，追求极致手感。

嗯，今天就讲到这里。记住，重力补偿做得好不好，直接决定了你的拖动示教系统是“真香”还是“真坑”。