3. 基于力矩的碰撞检测原理：阈值法、动态阈值法、机器学习法、模型预测法

聊到力矩感知，大家最关心的其实就一个问题：怎么判断机器人撞上了？

我刚开始做导航调参那会儿，用的还是最笨的办法——看电流。电机电流一飙，我就知道八成是卡住了。但后来发现，电流这东西太粗糙了，负载变化、地面摩擦、电池电压波动都能让它乱跳。真正靠谱的，还得是力矩传感器直接读出来的数据。

这一章，我把四种主流的碰撞检测方法掰开揉碎了讲。每种方法我都亲手调过、踩过坑，希望能帮你少走弯路。

核心观点：没有万能的方法。阈值法简单但僵硬，动态阈值法适应性强，机器学习法精度高但数据贵，模型预测法理论完美但计算重。选哪种，取决于你的硬件算力和场景复杂度。

基于力矩的碰撞检测方法体系 力矩碰撞检测 阈值法 动态阈值法 机器学习法 模型预测法 阈值法 固定阈值 计算量最小 误报率高 动态阈值法 自适应基线 中等计算量 工程最常用 机器学习法 需要训练数据 精度高 部署成本高 模型预测法 需要动力学模型 理论最优 实时性挑战 选择原则：算力够 → 模型预测法；数据够 → 机器学习法； 工程折中 → 动态阈值法；快速验证 → 阈值法

3.1 阈值法：最简单，也最容易翻车

阈值法的思路很直白：设定一个力矩上限，超过就认为撞了。比如关节力矩超过 5 N·m，立刻触发急停。

听起来简单对吧？但实际用起来，问题一堆。

我举个例子。你让机器人在平地上走，力矩基线可能是 2 N·m。但换到地毯上，基线直接跳到 3.5 N·m。如果你阈值设 4 N·m，平地上撞到软物体可能检测不到；地毯上正常行走反而误报。

我曾经踩过的坑：给一台差速底盘设了固定力矩阈值，结果在瓷砖地面跑得好好的，一上斜坡就疯狂误报。后来发现是重力分量没去掉。阈值法对安装角度、地面坡度极其敏感，千万别以为设一个值就万事大吉。

阈值法的代码实现很简单，但要注意滤波：

// 阈值法碰撞检测（C++ 伪代码）
const double TORQUE_THRESHOLD = 5.0;  // N·m
const double FILTER_ALPHA = 0.2;      // 一阶低通系数

double filteredTorque = 0.0;

bool detectCollision(double rawTorque) {
    // 一阶低通滤波，去掉高频噪声
    filteredTorque = FILTER_ALPHA * rawTorque + (1 - FILTER_ALPHA) * filteredTorque;
    
    if (filteredTorque > TORQUE_THRESHOLD) {
        return true;  // 碰撞！
    }
    return false;
}

你看，代码就几行。但实际工程里，阈值法最大的问题是：你永远找不到一个完美的固定值。负载变了、电池电压掉了、关节磨损了，阈值都得跟着调。

3.2 动态阈值法：工程中最实用的方案

既然固定阈值不行，那就让阈值自己动起来。动态阈值法的核心思想是：实时估计当前工况下的力矩基线，然后在这个基线之上加一个浮动窗口。

我个人习惯用滑动窗口均值 + 标准差的方式来做。具体来说：

  • 维护一个长度为 N 的力矩历史队列
  • 计算队列的均值 μ 和标准差 σ
  • 阈值设为 μ + k × σ，k 通常取 3~5

这样做的好处很明显：机器人上坡时基线自然抬高，阈值也跟着抬高，不会误报；撞到障碍物时力矩突变，瞬间超出动态阈值，检测灵敏。

我的经验值：窗口长度 N 取 50~100（10Hz控制周期下对应5~10秒），k 值取 4。太小的窗口容易被瞬时噪声带偏，太大的窗口响应迟钝。k 值取 3 以下误报多，取 6 以上可能漏检软碰撞。

代码实现也不复杂：

// 动态阈值法碰撞检测（C++ 伪代码）
class DynamicThresholdDetector {
private:
    std::deque<double> history;
    int windowSize = 80;
    double k = 4.0;
    
public:
    bool detectCollision(double torque) {
        history.push_back(torque);
        if (history.size() > windowSize) {
            history.pop_front();
        }
        
        // 计算均值和标准差
        double sum = 0.0, sumSq = 0.0;
        for (double t : history) {
            sum += t;
            sumSq += t * t;
        }
        double mean = sum / history.size();
        double variance = sumSq / history.size() - mean * mean;
        double stddev = std::sqrt(variance);
        
        double threshold = mean + k * stddev;
        
        return torque > threshold;
    }
};

动态阈值法是我在项目里用得最多的方法。它不需要额外传感器，不需要训练数据，算力消耗也低。说白了，就是性价比最高的方案。

3.3 机器学习法：数据够多，效果确实好

如果你手头有大量带标签的碰撞数据，机器学习法能给你最好的检测精度。常见的做法是把力矩、电流、速度、加速度拼成一个特征向量，扔给分类器。

我试过几种模型：

模型 精度 推理速度 训练难度 我的评价
SVM 中等 快 低 小样本够用，特征工程累
随机森林 较高 中等 低 不容易过拟合，推荐入门
轻量级神经网络 高 中等 高 效果好，但部署麻烦
LSTM 最高 慢 高 能捕捉时序特征，算力要求高

注意：机器学习法最怕的是训练数据和实际部署场景不一致。我曾经在一个项目里用实验室数据训练了随机森林模型，精度 98%。结果一上产线，地面有油污、有焊缝，力矩特征全变了，精度直接掉到 60%。数据分布偏移是机器学习法的头号杀手。

如果你决定走这条路，我的建议是：先用动态阈值法做 baseline，再尝试用机器学习法提升。不要一上来就上深度学习，除非你有充足的算力和数据。

3.4 模型预测法：理论上的最优解

模型预测法的思路和前面三种完全不同。它不直接看力矩大小，而是：用动力学模型预测当前应该输出多少力矩，然后比较预测值和实际值的残差。

残差大了，说明模型和现实对不上，大概率是撞了。

具体流程是这样的：

  1. 建立机器人动力学模型（比如牛顿-欧拉法）
  2. 输入当前关节角度、角速度、角加速度
  3. 模型预测出理论力矩 τpred
  4. 传感器读回实际力矩 τactual
  5. 计算残差 Δτ = |τactual - τpred|
  6. 如果 Δτ 超过阈值，判定为碰撞

这个方法的好处是：能区分「正常负载变化」和「碰撞」。比如机器人抓了一个重物，力矩变大，但模型预测也会相应变大，残差不会跳。而碰撞是外力突然介入，模型预测不到，残差瞬间飙升。

关键点：模型预测法的精度完全取决于动力学模型的准确度。摩擦参数、惯性参数、重心位置，任何一个不准，残差基线就会漂移。我见过一个团队花了三个月标定动力学参数，就为了让残差阈值能设到 0.5 N·m 以下。

代码示意（简化版）：

// 模型预测法碰撞检测（C++ 伪代码）
class ModelBasedDetector {
private:
    RobotDynamicsModel model;  // 动力学模型
    double residualThreshold = 0.8;  // N·m
    
public:
    bool detectCollision(double q, double qd, double qdd, double tauActual) {
        // 模型预测力矩
        double tauPred = model.computeTorque(q, qd, qdd);
        
        // 计算残差
        double residual = std::abs(tauActual - tauPred);
        
        // 残差超过阈值 → 碰撞
        return residual > residualThreshold;
    }
};

模型预测法在协作机器人上用得很多，比如 Universal Robots 的碰撞检测就基于这个原理。但说实话，对于移动机器人导航场景，这个方法有点「杀鸡用牛刀」——你得先有精确的动力学模型，而大多数移动底盘的模型精度都不够。

3.5 四种方法怎么选？一张表说清楚

维度 阈值法 动态阈值法 机器学习法 模型预测法
实现难度 ★☆☆☆☆ ★★☆☆☆ ★★★★☆ ★★★★★
计算开销 ★☆☆☆☆ ★★☆☆☆ ★★★☆☆ ★★★★☆
检测灵敏度 低 中 高 最高
误报率 高 中 低（数据好时） 低（模型准时）
适用场景 快速原型验证 大多数工程场景 数据充足、算力够 高精度协作机器人
我的推荐指数 ★★☆☆☆ ★★★★★ ★★★☆☆ ★★★☆☆

嗯，看到这里你应该明白了。动态阈值法是我最推荐的入门方案，它平衡了实现难度和检测效果。如果你后续发现动态阈值法不够用，再往机器学习或模型预测方向升级也不迟。

我个人习惯的做法是：先用动态阈值法跑通整个系统，同时采集力矩数据。等数据积累到一定程度，再尝试用机器学习法做精度提升。这样既保证了项目进度，又为后续优化留了空间。

← 上一章 📖 返回目录 下一章 →