1. 项目概述：让机器人“感知”重量

在机器人抓取与操作领域，我们常常关注视觉、力觉和触觉，但有一个基础且关键的物理量时常被忽视，那就是被操作物体的 重量 。你可能会问，给机器人装个称不就行了？事情远没有这么简单。想象一下，你闭着眼睛去拿一个看似是空纸箱的物体，结果里面装满了书，你的手臂会瞬间调整发力，避免失手或拉伤。这个“瞬间调整”背后，就是你的神经系统对重量预期的“感知”与实际负载的“反馈”进行了快速比对和修正。对于工业机械臂、服务机器人乃至未来的家庭助理机器人而言，缺乏这种重量感知能力，意味着它们要么小心翼翼、动作迟缓以防过载，要么鲁莽操作、损坏工件或自身。

“Weight Perception for Robot Manipulators”这个项目，核心目标就是为机器人操纵器（机械臂）赋予类似人类的重量感知与预测能力。它不是简单地加装一个高精度秤，而是构建一套从 无接触预测 到 在线实时估计 的完整算法框架。其价值在于，让机器人在真正接触并提起物体之前，就能对其重量有一个合理的先验估计；在操作过程中，能实时感知负载变化，从而动态调整运动规划、力控参数甚至抓取策略。这直接关系到操作的 安全性 （避免超载损坏）、 效率 （优化运动轨迹）和 智能性 （实现更拟人的交互）。

这个项目融合了机器人学、计算机视觉、动力学建模与状态估计等多个领域。它适合机器人工程师、算法研究员以及对机器人感知与控制深度融合感兴趣的朋友。无论你是想提升现有机械臂的智能化水平，还是在进行灵巧操作、人机协作等前沿课题研究，理解并实现重量感知都将是一个极具价值的技能点。

2. 核心思路与方案选型：为何不直接用力传感器？

刚接触这个课题，最直接的想法可能是：在机械臂末端或关节安装高精度力/力矩传感器，通过动力学方程反算不就行了？这个思路没错，但它存在几个关键瓶颈：

1. 接触后感知的滞后性 ：传感器只能在机器人与物体发生物理接触并产生力交互后才能读数。这意味着在抓取动作执行的初期（如闭合夹爪时），机器人对负载是“无知”的，无法提前规划最优的提起加速度。
2. 成本与可靠性 ：高质量的六维力传感器价格昂贵，且在一些恶劣环境（如粉尘、油污）或需要频繁碰撞的场景下，其可靠性和寿命面临挑战。
3. 动态操作中的复杂性 ：在机器人快速运动或操作非刚性物体（如装有液体的瓶子）时，传感器数据混杂了惯性力、科氏力等多种成分，从中准确剥离出纯负载重量需要复杂的动力学补偿和滤波，实时性要求高。

因此，更先进的思路是构建一个 分层感知系统 ：

• 层一：视觉先验预测 。在抓取前，利用相机获取物体的视觉信息（尺寸、类别、材质表观），通过深度学习模型预测其重量范围。这给了机器人一个“心理预期”。
• 层二：触觉初值校准 。在抓取接触的瞬间，利用触觉传感器或关节电流等信息，快速对视觉预测的重量进行初次校准和修正。
• 层三：操作中在线估计 。在提起、移动物体的过程中，利用关节编码器、电机电流等本体传感器，结合机器人动力学模型，实时在线估计负载重量及质心变化。这是核心的 无额外传感器 方案。

我们的项目将重点聚焦在 层一 和 层三 ，即如何利用视觉进行先验预测，以及如何仅凭机器人本体传感器实现高精度的在线重量估计。这是一种性价比极高且极具普适性的技术路径。

2.1 视觉重量预测：从“看”到“猜”

视觉预测的核心是建立从图像特征到物理属性的映射。这里不追求绝对精确的克数，而是提供一个有意义的范围（如：轻于500g，500g-2kg，重于2kg），或者一个相对准确的估计值。

方案选择：基于深度学习的回归/分类模型

• 为什么是深度学习？ 物体的重量与体积、密度紧密相关。体积可以从图像通过3D重建或基于深度学习直接估计得到，但密度与材质高度相关。深度学习模型能够端到端地从RGB或RGB-D图像中，同时学习与体积和材质相关的隐含特征，并映射到重量，避免了显式建模“材质密度”这个极其复杂的变量。
• 网络架构选型 ：
  • 主干网络 ：采用在ImageNet上预训练的ResNet、EfficientNet等作为特征提取器。它们的强大表征能力足以捕捉物体形状、纹理等关键信息。
  • 任务头 ：
    • 分类头 ：如果只需重量范围，可在主干网络后接全连接层，输出每个重量类别的概率。损失函数用交叉熵。
    • 回归头 ：如需具体数值，则输出一个连续的重量值。损失函数常用平滑L1损失或Huber损失，它们对异常值比均方误差更鲁棒。
  • 输入融合 ：如果使用RGB-D相机，可以将深度图作为额外通道与RGB图像拼接输入，也可以使用两个并行的主干网络分别处理RGB和Depth，在特征层进行融合。后者通常效果更好，但更复杂。

实操心得 ：数据是关键中的关键。公开的重量-图像数据集极少，需要自己构建。一个实用的方法是：搭建一个固定相机位的拍摄平台，将已知重量的物体（从螺丝钉到书本、水杯）放置上去拍摄多角度图片。标注重量时，建议对重量值取对数（log）再进行归一化，因为重量分布通常是长尾的（轻物体多，重物体少），取对数可以使分布更均匀，有利于模型训练。

2.2 本体传感器在线估计：机器人“自知”的学问

这是项目的算法核心，其本质是一个 状态估计 问题。我们将负载的质量和质心位置作为需要估计的“状态”，将机器人的关节位置、速度、电流或力矩作为“观测”，利用机器人动力学方程作为“系统模型”进行估计。

方案选择：基于递归最小二乘法与扩展卡尔曼滤波

• 为什么用动力学模型？ 机器人的运动方程清晰地描述了关节力矩与运动（位置、速度、加速度）以及负载参数之间的关系。对于一个n自由度的机械臂，其动力学方程可简化为： τ = M(q)¨q + C(q, ˙q)˙q + G(q) + J(q)^T F_ext 其中， τ 是关节力矩， q 是关节位置， M 是惯性矩阵， C 是科氏力和向心力项， G 是重力项， J 是雅可比矩阵， F_ext 是末端外力。负载的变化会直接影响 M 、 G 等项中的参数。
• 参数化与线性化 ：关键一步是将负载的影响参数化。通常，我们将未知负载等效为一个质量为 m 、质心在末端坐标系中位置为 [x, y, z] 的质点。这样，负载对动力学的影响（主要体现在重力项和惯性项）可以写成关于 m , mx , my , mz 的线性形式。这个技巧将非线性估计问题转化为了 线性参数估计 问题。
• 递归最小二乘法 ：这是最常用、最直观的在线估计算法。我们将动力学方程在每一个控制周期（如1ms）离散化，整理成 y = Φ * p 的形式。其中， y 是可测量的关节力矩（或电流换算的力矩）与已知的机器人本体动力学计算出的力矩之差， Φ 是由关节位置、速度、加速度等构成的回归矩阵， p 就是待估计的参数向量 [m, mx, my, mz]^T 。RLS算法可以在线递归地更新对 p 的估计。
  • 加速度获取 ：这是RLS实现中的难点。关节加速度通常不能直接测量，需要对关节位置进行数值微分。二次微分会放大噪声，因此需要设计良好的低通滤波器。
• 扩展卡尔曼滤波 ：如果将负载参数视为随时间缓慢变化的状态，并考虑过程噪声和观测噪声，EKF提供了一个更严谨的贝叶斯估计框架。它比RLS更擅长处理噪声和非线性，但实现也更复杂。

注意事项 ：在线估计算法要想起效，必须满足 持续激励 条件。简单说，就是机器人不能做匀速或静止的运动，必须包含足够的加速度变化，这样回归矩阵 Φ 才满秩，参数才能被唯一辨识。因此，在实践中，我们常常在机器人开始搬运后，故意让它执行一个包含多方向小幅加速运动的“辨识轨迹”，在几秒钟内快速收敛出负载参数。

3. 系统实现与集成：从算法到实机

理论方案需要嵌入到真实的机器人控制系统中才能发挥作用。这里以最常用的机器人中间件ROS为例，阐述实现框架。

3.1 软件架构设计

我们需要构建一个松耦合、模块化的系统，通常包含以下节点：

1. 视觉预测节点 ：

   • 输入 ：来自相机的RGB或RGB-D话题 ( /camera/rgb/image_raw , /camera/depth/image_raw )。
   • 核心 ：运行训练好的PyTorch或TensorFlow模型。使用OpenCV或 cv_bridge 转换图像格式。
   • 输出 ：发布一个自定义消息，如 /weight_prior ，包含预测的重量值和置信度。
   • 技巧 ：该节点可以只在抓取决策阶段触发一次，降低计算负载。

2. 动力学参数估计节点 （核心）：

   • 输入 ：
     • 机器人关节状态 ( /joint_states , 含位置、速度)。
     • 关节力矩或电机电流 ( /joint_torques 或 /motor_currents )。如果控制器不提供力矩话题，需要用电机电流和模型换算，这是一个误差来源。
     • 机器人URDF描述。用于实时计算机器人本体的动力学项（ M(q) , C(q, ˙q) , G(q) ）。这里推荐使用 Pinocchio 或 KDL 库，它们计算效率远高于ROS自带的 robot_state_publisher 。
   • 核心 ：
     • 状态预处理 ：对关节位置进行滤波和数值微分，得到速度和加速度。我常用一个截止频率为20-50Hz的二阶巴特沃斯低通滤波器。
     • 动力学计算 ：每个控制周期，利用当前 q, ˙q, ¨q 计算不含负载的机器人本体动力学力矩 τ_model 。
     • RLS/EKF更新 ：计算力矩残差 y = τ_measured - τ_model ，构建回归矩阵 Φ ，执行RLS迭代更新公式，输出最新的负载参数估计 p_hat 。
   • 输出 ：发布估计结果，如 /estimated_load ，包含质量、质心、估计误差协方差等信息。

3. 决策与控制节点 ：

   • 输入 ：订阅 /weight_prior 和 /estimated_load 。
   • 逻辑 ：
     • 抓取前：利用视觉先验重量，调整抓取器的夹持力设定值（例如，预测重的物体，初始夹持力更大）。
     • 提起瞬间：将在线估计的初始值用视觉先验进行加权融合，加快收敛。
     • 运动中：根据实时估计的重量和质心，动态调整运动规划的加速度上限，或者作为力位混合控制的力反馈参考值。

3.2 关键实现细节与代码片段

以下是一个简化的RLS核心更新循环的伪代码，通常在估计节点的定时回调函数中执行：

// 伪代码，演示RLS核心逻辑
void estimationCallback(const JointStateMsg& js, const JointTorqueMsg& tau_meas) {
    // 1. 获取并滤波关节状态
    q = lowPassFilter(js.position); // 位置
    q_dot = numericalDifferentiation(q); // 速度
    q_ddot = numericalDifferentiation(q_dot); // 加速度（噪声大，需重滤波）

    // 2. 计算机器人本体动力学力矩 (使用Pinocchio)
    pinocchio::computeAllTerms(model, data, q, q_dot);
    tau_model = data.tau; // 包含重力、惯性、科氏力

    // 3. 构建回归矩阵 Phi (与负载参数线性相关部分)
    // 这需要根据机器人动力学模型推导出 Phi 的具体形式
    // 通常，Phi 与末端执行器的重力方向在关节空间的影响有关
    Eigen::MatrixXd Phi = computeRegressor(q, gravity_vector);

    // 4. RLS 更新
    Eigen::VectorXd y = tau_meas - tau_model; // 力矩残差
    Eigen::MatrixXd K = P * Phi.transpose() * (Phi * P * Phi.transpose() + lambda).inverse(); // 增益矩阵
    p_hat = p_hat + K * (y - Phi * p_hat); // 参数更新
    P = (I - K * Phi) * P / lambda; // 协方差更新

    // 5. 从 p_hat (=[m, m*x, m*y, m*z]) 中解析出质量 m 和质心 [x,y,z]
    estimated_mass = p_hat(0);
    if (estimated_mass > threshold) {
        estimated_com = p_hat.tail(3) / estimated_mass;
    }

    // 发布 estimated_mass 和 estimated_com
}

实操心得 ：参数 lambda 是RLS的遗忘因子，通常取0.995~0.999。越接近1，记忆效应越长，估计越平滑但对变化反应慢；越小则对新数据更敏感但估计波动大。需要根据机器人运动频率调整。另外，一定要给估计的质量和质心设置合理的物理上下限，并加入“重置”逻辑。当机器人检测到末端负载消失（如物体掉落）时，应立即将 p_hat 重置为零，并可能将协方差矩阵 P 重置为一个较大的初值，以准备下一次估计。

4. 实验验证与性能调优

任何算法离开实验验证都是纸上谈兵。我们需要设计实验来评估重量感知系统的性能。

4.1 实验设计

1. 静态估计算法验证 ：

   • 方法 ：让机械臂末端固定一个已知质量（如500g、1kg、2kg）的配重块。控制机械臂执行一段设计好的、包含多个方向运动的“激励轨迹”（如正弦波点动）。
   • 测量 ：记录RLS算法估计的质量值随时间收敛的过程。
   • 指标 ：
     • 收敛时间 ：估计值进入真实值±5%误差带所需的时间。
     • 稳态误差 ：收敛后估计值的平均误差百分比。
     • 抗噪性 ：在关节力矩信号中加入高斯白噪声，观察估计性能的衰减情况。

2. 动态操作场景测试 ：

   • 方法 ：让机器人执行完整的“视觉识别 -> 抓取 -> 提起 -> 搬运 -> 放置”任务。被抓取物体选择不同材质（金属块、塑料盒、装满水的瓶子）和形状（规则、不规则）的物体。
   • 测量 ：
     • 视觉预测模块的输出是否合理。
     • 在线估计值在抓取瞬间的跳变和收敛过程。
     • 机器人是否根据估计的重量调整了运动速度（例如，拿起重物后自动减速）。

4.2 常见问题与排查技巧

在实际部署中，你几乎一定会遇到以下问题：

问题1：估计值发散或振荡剧烈。

• 可能原因A：加速度计算噪声过大。 这是最常见的原因。对位置信号二次微分得到的加速度信噪比极低。
  • 排查 ：绘制出计算出的加速度曲线，看是否充满高频毛刺。
  • 解决 ：优化滤波器。尝试更高阶的低通滤波器，或者采用 Kalman Filter 对位置、速度、加速度进行联合状态估计。也可以考虑使用 State Observer （状态观测器）理论来设计观测器。
• 可能原因B：动力学模型不准确。 URDF模型中的连杆质量、质心、惯性张量参数不准确，导致 tau_model 计算有系统误差。
  • 排查 ：在空载情况下，让机器人缓慢运动（忽略惯性力），此时关节力矩应主要克服重力。比较 tau_meas 和 tau_model ，如果存在恒定偏差，说明重力补偿参数有误。
  • 解决 ：先进行机器人本体的动力学参数辨识（这是一个更大的课题），或者对重力项进行简单的线性补偿校准。

问题2：估计值收敛慢。

• 可能原因：激励不足。 机器人运动轨迹过于平缓，回归矩阵 Phi 病态。
  • 解决 ：确保辨识轨迹包含所有关节的、频率丰富的运动。可以设计一个有限时间的“最优激励轨迹”，使 Φ 矩阵在某种意义下（如行列式值最大）达到最优。

问题3：抓取瞬间估计值跳变异常。

• 可能原因：接触冲击导致力矩尖峰。 抓取瞬间的碰撞力被力矩传感器检测到，干扰了基于平滑动力学模型的估计。
  • 解决 ：在算法中引入一个简单的接触检测逻辑。当检测到力矩变化率超过阈值时，暂停RLS更新几个周期，或者使用一个更大的过程噪声协方差矩阵 Q （如果使用EKF），让滤波器暂时更相信预测而非观测。

问题4：视觉预测对未知物体失效。

• 可能原因：数据集覆盖不足。 模型没见过此类物体。
  • 解决 ：没有银弹。持续扩充数据集是根本。在工程上，可以设置一个置信度阈值，当预测置信度过低时，系统回退到保守的“默认重量”假设，并更依赖在线估计模块。

5. 进阶应用与扩展思考

实现基础的重量感知后，我们可以将其作为基石，探索更高级的应用：

1. 质量-刚度联合估计 ：对于柔性物体（如海绵、线缆），不仅质量变化，其连接刚度也会影响操作。可以扩展状态向量，同时估计负载的等效质量和刚度，用于更精细的力控。
2. 非刚性物体内部状态推断 ：搬运一个装有液体的杯子。液体晃动会产生时变的动力学效应。通过分析在线估计的重量/质心参数的微小波动，可以推断液体的晃动状态，进而规划抑振轨迹。
3. 人机协作中的意图理解 ：在与人共同搬运物体时，通过实时估计负载分担的变化，可以推断人的运动意图（是想往左还是往右？是想停下还是加速？），从而实现更自然的协作。
4. 与学习方法的结合 ：将基于模型的RLS/EKF估计器与基于数据的神经网络相结合。例如，用神经网络来学习动力学模型中难以建模的摩擦、非线性等残差部分，再用RLS估计主要负载参数。这种“模型+学习”的混合架构是当前的研究热点。

重量感知，这个看似简单的物理量感知，实则打开了机器人精细化、智能化操作的一扇大门。它要求工程师不仅要有扎实的机器人动力学和控制理论功底，还要具备信号处理、状态估计乃至机器学习的跨领域知识。从推导公式到写出实时代码，从仿真验证到实机调试，每一步都充满了挑战和乐趣。当你第一次看到机械臂准确地“感知”到自己提起了一个重物，并自动调整运动姿态时，那种让机器拥有“本体感觉”的成就感，正是机器人技术的魅力所在。