1. 从“力不准”到“力感知”：为什么我们需要重力补偿？

做机器人力控的朋友，十有八九都踩过力传感器读数的坑。你可能也遇到过这种情况：机器人手臂明明静止地悬在空中，什么都没碰，但力传感器读出来的数值却不是零，而是有一个稳定的偏置。这个偏置，很大程度上就是机器人末端执行器（比如夹爪、焊枪）自身的重力造成的。

这个重力偏置不消除，力控就无从谈起。想象一下，你想让机器人以5牛顿的力去轻轻接触一个物体，结果传感器本身就有3牛顿的重力偏置，控制器收到的信号永远不准，要么接触不上，要么一下就撞上去。传统的补偿方法，比如在几个固定姿态下采集数据，然后计算一个固定的重力矢量进行扣除，在简单场景下还行。但一旦末端工具复杂、重心不明确，或者机器人在运动过程中姿态变化剧烈，这种静态补偿的误差就很大了。

这就是为什么我们要在ROS2里引入机器学习来做重力补偿优化。核心思路很简单：把重力补偿看作一个从机器人姿态（关节角度或末端位姿）到六维力/力矩偏置的映射关系。这个关系可能是非线性的，而机器学习模型，特别是像多项式回归、神经网络这类模型，最擅长的就是学习复杂的非线性映射。

我实测下来，用机器学习模型优化后的重力补偿，在UR机器人、Franka等平台上，静态精度提升非常明显，动态过程中的力读数也稳定得多。这为后续的导纳控制、直接力控、精密装配这些高精度任务打下了坚实基础。下面，我就把自己在ROS2 Foxy和Humble版本上趟过的路、踩过的坑，以及最终跑通的完整流程，掰开揉碎了分享给你。

2. 第一步：高质量数据采集——模型好坏的基石

任何机器学习项目，数据都是命根子。重力补偿模型训练也不例外。我们的目标是收集一系列“机器人姿态-对应的力传感器原始读数”配对数据。此时，必须确保机器人处于完全自由、无接触的状态，这样传感器测到的“干扰力”就100%是末端工具的重力。

2.1 搭建数据采集节点

通常，力传感器（如ATI、Robotiq FT）的数据会通过一个驱动节点发布到类似 /ft_sensor/wrench_raw 这样的ROS2话题上，消息类型是 geometry_msgs/msg/WrenchStamped。同时，我们需要知道机器人的实时姿态，这可以通过机器人的状态话题（如 /joint_states）或直接通过TF查询 base_link 到 tool0（或你的工具坐标系）的变换来获得。

我们需要写一个数据采集节点。这个节点的逻辑很简单：

1. 订阅原始力话题和关节状态话题。
2. 在回调函数中，确保两个数据的时间戳基本同步。
3. 将当前的关节角度（或计算出的末端姿态）和对应的六维力/力矩原始数据，一起记录到一个CSV文件中。

这里给一个简化的节点代码框架：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import WrenchStamped
from sensor_msgs.msg import JointState
import csv
from datetime import datetime

class BiasDataRecorder(Node):
    def __init__(self):
        super().__init__('bias_data_recorder')
        # 订阅
        self.wrench_sub = self.create_subscription(
            WrenchStamped,
            '/ft_sensor/wrench_raw',
            self.wrench_callback,
            10)
        self.joint_state_sub = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_state_callback,
            10)

        # 变量初始化
        self.current_joint_positions = None
        self.current_wrench = None
        self.record = False

        # 创建CSV文件
        filename = f"bias_data_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        self.csv_file = open(filename, 'w', newline='')
        self.writer = csv.writer(self.csv_file)
        # 写入表头：6个关节角 + 3个力 + 3个力矩
        self.writer.writerow(['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'fx', 'fy', 'fz', 'tx', 'ty', 'tz'])

        self.get_logger().info("数据采集节点已启动，等待数据...")

    def joint_state_callback(self, msg):
        # 假设是6轴机器人，取前6个位置值
        if len(msg.position) >= 6:
            self.current_joint_positions = msg.position[:6]

    def wrench_callback(self, msg):
        self.current_wrench = msg.wrench
        if self.record and self.current_joint_positions is not None:
            self.save_data_point()

    def save_data_point(self):
        data_row = list(self.current_joint_positions)
        data_row.extend([
            self.current_wrench.force.x,
            self.current_wrench.force.y,
            self.current_wrench.force.z,
            self.current_wrench.torque.x,
            self.current_wrench.torque.y,
            self.current_wrench.torque.z
        ])
        self.writer.writerow(data_row)
        self.csv_file.flush() # 及时写入磁盘

    # 可以通过服务或参数来控制开始/停止记录
    def start_recording(self):
        self.record = True
        self.get_logger().info("开始记录数据...")

    def stop_recording(self):
        self.record = False
        self.get_logger().info("停止记录。")
        self.csv_file.close()

def main(args=None):
    rclpy.init(args=args)
    node = BiasDataRecorder()
    # 这里需要一种方式触发 start_recording，比如通过ROS2服务
    # 简单起见，可以设置一个定时器，延迟几秒后开始，运行一段时间后停止
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2.2 “多姿态”采集的关键技巧

采集数据不是随便动动机器人就行，这里面有讲究。目标是让模型学习到重力在传感器坐标系下，随着姿态变化的完整映射。所以，姿态的覆盖要尽可能均匀、广泛。

1. 关节空间采样：这是最直接的方法。让每个关节在其运动范围内，以一定间隔（比如10度）运动，其他关节随机或固定。这样可以系统性地覆盖整个工作空间。你可以手动示教多个点，或者写一个简单的脚本让机器人自动走一遍。
2. 操作空间采样：更符合物理直觉。想象末端工具在空间中的不同位置和朝向。让工具坐标系绕X、Y、Z轴分别旋转，在不同俯仰、偏航角下记录数据。特别注意奇异点附近，这里的姿态变化小但关节变化大，多采些数据有助于模型学习。
3. 数据量：根据我的经验，对于一个6轴机器人，想要一个比较鲁棒的模型，至少需要采集500-1000个不同的静态姿态点。数据太少，模型容易欠拟合；数据覆盖不全，则在未训练过的姿态下预测会不准。
4. 环境与滤波：采集时，确保工作台稳定，没有风吹或振动。可以在采集节点里加入一个简单的低通滤波器，对力读数做一下平滑，滤掉高频噪声，让数据更干净。

提示：采集前，务必通过 ros2 topic echo /tf_static 或 ros2 run tf2_ros tf2_echo base_link tool0 确认 base_link 到 tool0 的TF变换是稳定且正确的。这是所有计算的基础。

3. 第二步：模型训练与优化——从数据到智能

数据采集好之后，我们会有多个CSV文件。接下来就是合并数据、训练模型，并保存为一个可以随时加载的模型文件。

3.1 数据预处理与合并

首先，将多个采集批次的数据合并成一个大的数据集。同时，需要进行必要的预处理：

import pandas as pd
import numpy as np
from sklearn.model_selection import train_test_split

# 合并多个CSV文件
file_list = ['bias_data_1.csv', 'bias_data_2.csv', 'bias_data_3.csv']
df_list = [pd.read_csv(f) for f in file_list]
df = pd.concat(df_list, ignore_index=True)

# 检查是否有缺失值
df.dropna(inplace=True)

# 分离特征（X：关节角度）和目标（y：力/力矩偏置）
# 注意：这里我们直接用关节角作为特征。更优的做法是将其转换为末端姿态（位置+四元数），但关节角对于模型来说也是可学习的。
X = df[['j1', 'j2', 'j3', 'j4', 'j5', 'j6']].values
y = df[['fx', 'fy', 'fz', 'tx', 'ty', 'tz']].values

# 划分训练集和测试集（例如 80%/20%）
X_train, X_test, y_train, y_test = train_test_split(X, y, test_size=0.2, random_state=42)

3.2 模型选择与训练

对于重力补偿这个问题，多项式回归（Polynomial Regression） 是一个非常好的起点。它比简单的线性回归强大，能捕捉非线性关系，又比深度学习模型简单、训练快、且不易过拟合。我们可以使用 sklearn 的 Pipeline 和 PolynomialFeatures。

from sklearn.preprocessing import PolynomialFeatures, StandardScaler
from sklearn.linear_model import LinearRegression
from sklearn.pipeline import make_pipeline
from sklearn.metrics import mean_squared_error

# 创建一个多项式回归的Pipeline
# 我们尝试2次多项式，因为重力与姿态的关系通常是二次非线性的
degree = 2
model = make_pipeline(
    StandardScaler(),        # 第一步：标准化特征，加速收敛
    PolynomialFeatures(degree=degree, include_bias=False), # 第二步：生成多项式特征
    LinearRegression()       # 第三步：线性回归
)

# 训练模型
model.fit(X_train, y_train)

# 在测试集上评估
y_pred = model.predict(X_test)
mse = mean_squared_error(y_test, y_pred, multioutput='raw_values')
print(f"各维度MSE: {mse}")
print(f"平均RMSE: {np.sqrt(mse.mean()):.4f} N/Nm")

如果多项式回归效果不理想，可以考虑其他模型，如：

• 支持向量回归（SVR）：对于中小型数据集表现不错。
• 随机森林回归（Random Forest Regressor）：能捕捉更复杂的交互，不易过拟合，但推理速度稍慢。
• 轻量级神经网络（如MLP）：使用 sklearn.neural_network.MLPRegressor 或 PyTorch 搭建一个小网络。这在姿态-重力映射极其复杂时可能有用，但需要更多数据和调参。

3.3 模型保存与集成

训练好满意的模型后，用 joblib 或 pickle 保存它。

import joblib

# 保存模型
model_filename = 'bias_compensation_model.pkl'
joblib.dump(model, model_filename)
print(f"模型已保存至 {model_filename}")

# 之后在ROS2节点中加载
# loaded_model = joblib.load(model_filename)

现在，你有了一个“黑盒子”，输入当前的6个关节角度，它就能输出预测的6维重力偏置。

4. 第三步：集成到ROS2——实时补偿节点

模型准备好了，下一步就是把它塞进ROS2，创建一个实时运行的“重力补偿节点”。这个节点的职责是：

1. 订阅原始力传感器话题 (/ft_sensor/wrench_raw) 和关节状态话题 (/joint_states)。
2. 根据当前关节状态，利用加载的机器学习模型，实时预测出重力偏置。
3. 将原始读数减去预测偏置，得到补偿后的力/力矩。
4. 将补偿后的数据发布到新话题 (/ft_sensor/wrench_compensated)。

4.1 补偿节点核心代码

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import WrenchStamped
from sensor_msgs.msg import JointState
import joblib
import numpy as np

class MLForceBiasCompensator(Node):
    def __init__(self):
        super().__init__('ml_force_bias_compensator')
        # 加载训练好的模型
        self.model = joblib.load('/path/to/your/bias_compensation_model.pkl')

        # 订阅
        self.wrench_sub = self.create_subscription(
            WrenchStamped,
            '/ft_sensor/wrench_raw',
            self.wrench_callback,
            10)
        self.joint_state_sub = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_state_callback,
            10)

        # 发布补偿后的数据
        self.compensated_pub = self.create_publisher(
            WrenchStamped,
            '/ft_sensor/wrench_compensated',
            10)

        self.current_joints = None
        self.latest_wrench_msg = None

        self.get_logger().info("机器学习重力补偿节点已启动！")

    def joint_state_callback(self, msg):
        if len(msg.position) >= 6:
            self.current_joints = np.array(msg.position[:6]).reshape(1, -1)

    def wrench_callback(self, msg):
        self.latest_wrench_msg = msg
        if self.current_joints is not None:
            self.compensate_and_publish()

    def compensate_and_publish(self):
        # 1. 使用模型预测当前姿态下的偏置
        predicted_bias = self.model.predict(self.current_joints)
        # predicted_bias 是一个形状为 (1, 6) 的数组 [fx, fy, fz, tx, ty, tz]

        # 2. 创建新的WrenchStamped消息
        compensated_msg = WrenchStamped()
        compensated_msg.header = self.latest_wrench_msg.header # 保持时间戳

        # 3. 执行补偿：原始值 - 预测偏置
        raw = self.latest_wrench_msg.wrench
        compensated_msg.wrench.force.x = raw.force.x - predicted_bias[0, 0]
        compensated_msg.wrench.force.y = raw.force.y - predicted_bias[0, 1]
        compensated_msg.wrench.force.z = raw.force.z - predicted_bias[0, 2]
        compensated_msg.wrench.torque.x = raw.torque.x - predicted_bias[0, 3]
        compensated_msg.wrench.torque.y = raw.torque.y - predicted_bias[0, 4]
        compensated_msg.wrench.torque.z = raw.torque.z - predicted_bias[0, 5]

        # 4. 发布补偿后的数据
        self.compensated_pub.publish(compensated_msg)

def main(args=None):
    rclpy.init(args=args)
    node = MLForceBiasCompensator()
    rclpy.spin(node)
    rclpy.shutdown()

4.2 Launch文件集成与启动优化

为了让节点能随机器人系统一起启动，并处理可能的TF树未就绪问题，我们需要优化启动配置。就像原始文章提到的，使用 TimerAction 是个很实用的技巧。

在你的机器人启动文件（例如 robot_bringup.launch.py）中：

from launch_ros.actions import Node
from launch.actions import TimerAction
from launch import LaunchDescription

def generate_launch_description():
    # ... 其他节点定义 (如 robot_state_publisher, 控制器管理器等) ...

    # 重力补偿节点 - 延迟启动
    force_bias_node = TimerAction(
        period=5.0,  # 延迟5秒启动，确保TF树和传感器驱动已稳定
        actions=[
            Node(
                package='your_package_name',
                executable='ml_force_bias_compensator',
                name='ml_force_bias_compensator',
                output='screen',
            )
        ]
    )

    return LaunchDescription([
        # ... 其他节点 ...
        force_bias_node,
    ])

这5秒的延迟非常关键，它确保了 base_link 到 tool0 的TF变换已经在 tf2 缓冲区里稳定存在，避免了节点一启动就因查不到变换而报错。

5. 第四步：效果验证与控制器配置

节点跑起来之后，不能光看日志说没问题，必须用数据说话，进行严格的验证。

5.1 话题数据对比验证

打开三个终端，分别运行：

# 终端1：查看原始力数据
ros2 topic echo /ft_sensor/wrench_raw

# 终端2：查看补偿后的力数据
ros2 topic echo /ft_sensor/wrench_compensated

# 终端3：发布命令，让机器人缓慢运动到几个不同姿态
# （例如，通过你的示教器或MoveIt2发送目标点）

验证方法：

1. 静态验证：让机器人静止在多个不同姿态（包括数据采集过的和没采集过的）。观察 /ft_sensor/wrench_compensated 话题的输出。理想情况下，力和力矩的数值应该非常接近于零（仅在传感器噪声范围内波动）。而 /ft_sensor/wrench_raw 则会显示一个随着姿态变化而变化的非零偏置。
2. 动态观察：让机器人缓慢、平稳地运动。观察补偿后的数据是否保持平稳，没有因为姿态突变而产生跳变。好的补偿模型，在运动过程中，只要没有接触外力，补偿后的读数也应该围绕零点小幅波动。

5.2 配置力控控制器使用补偿后数据

验证通过后，最关键的一步是让下游的力控制器使用我们补偿后的数据。这通常需要修改控制器的配置文件。

找到你的力控配置文件（比如 cartesian_compliance_controller.yaml 或类似），将力传感器话题指向补偿后的：

# controller_manager.yaml 或独立的控制器yaml
your_cartesian_force_controller:
  ros__parameters:
    # ... 其他参数 ...
    ft_sensor_topic: "/ft_sensor/wrench_compensated" # 将这里从原始话题改为补偿后话题
    # 例如，之前可能是 /ft_sensor/wrench_raw

修改后，重新加载或启动控制器。现在，你的导纳控制、力位混合控制等算法，接收到的就是已经剔除了重力干扰的“纯净”接触力信号了，控制精度和稳定性会得到质的提升。

6. 避坑指南与高阶技巧

在实际部署中，我遇到过不少问题，这里总结几个常见的“坑”和解决办法：

1. TF变换不稳定或延迟：这是最大的坑。确保 robot_state_publisher 正确发布关节状态到TF。如果使用外部跟踪设备，延迟可能更大。可以在补偿节点里加入 tf2_ros.Buffer 和 TransformListener，以同步的方式查询指定时间戳的变换，而不是用最新关节状态对应可能延迟的力数据。
2. 模型在训练集外姿态表现差：这说明数据采集的覆盖度不够。补救方法是进行“在线更新”或“增量学习”。可以设计一个简单的机制：当机器人检测到长时间稳定无接触时，将当前姿态和力读数作为一个新的数据点，在线更新模型（对于线性模型或随机森林，有部分算法支持）。
3. 末端工具更换：换工具后，重力模型完全变了。最好的办法是为每个工具训练一个独立的模型文件（tool1_model.pkl, tool2_model.pkl）。在节点启动时，通过ROS2参数服务器动态加载对应的模型。
4. 补偿后数据仍有漂移：可能是温度漂移或传感器零点漂移。机器学习模型补偿的是与姿态相关的部分，对于缓慢的时间漂移无能为力。可以考虑在补偿节点中加入一个高通滤波器，或者在长时间空闲时，自动重新校准零点。
5. 性能问题：多项式特征在阶数高时，特征数量爆炸（对于6关节2次多项式，特征数可达27个）。如果推理频率要求高（如1kHz），需评估节点耗时。用 scikit-learn 的模型一般都能满足100Hz的需求。如果不行，可以考虑将模型转换为 ONNX 格式并用更快的运行时，或者用C++重写推理部分。

这套基于机器学习的重力补偿流程，我从最初的摸索到现在的稳定应用，花了相当一段时间。它最大的优势是普适性强，你不需要精确知道工具的质心和质量，只要“教”一遍，它就能学个八九不离十。尤其是在进行像精密插装、曲面打磨这类对力感知精度要求极高的任务时，这套方法带来的提升是立竿见影的。刚开始搭环境、调数据可能会觉得有点繁琐，但一旦跑通，它就是机器人高精度力控路上一个非常可靠的基础设施。