2. 传感器噪声排查：IMU数据滤波方法、编码器信号毛刺处理、力传感器振动干扰识别

传感器噪声，说白了就是机器人“感觉”到的假信号。

你想想看，人形机器人要站稳，全靠传感器告诉控制器“我现在是什么姿势”。如果传感器数据里掺了噪声，控制器就会做出错误判断——抖动就这么来了。

我个人习惯把传感器噪声分成三类：IMU的高频振动噪声、编码器的脉冲毛刺、力传感器的谐振干扰。咱们一个一个说。

2.1 IMU数据滤波：低通滤波与卡尔曼滤波

IMU（惯性测量单元）是人形机器人的“内耳”，负责感知姿态和加速度。但它有个毛病——对振动特别敏感。

我记得有一次调试双足行走，机器人站着不动，IMU输出的角速度却像心电图一样上下跳。后来发现是电机PWM切换时产生的振动传到了IMU安装座上。

2.1.1 低通滤波（LPF）——简单粗暴但有效

低通滤波的原理很简单：让低频的真实信号通过，把高频的噪声滤掉。

最常用的是一阶RC低通滤波，代码实现就几行：

// 一阶低通滤波
float lowPassFilter(float input, float prevOutput, float alpha) {
    return alpha * input + (1.0f - alpha) * prevOutput;
}
// alpha = T / (T + RC)，T为采样周期，RC为时间常数
// 一般alpha取0.1~0.3效果较好

这里有个坑——alpha值怎么选？

选大了，滤波效果差，噪声还在；选小了，信号延迟大，机器人反应变慢。我一般先看IMU的采样频率，再根据机械结构的共振频率来定。比如采样率200Hz，机械共振在20Hz左右，我会把截止频率设在10Hz，alpha算出来大概0.15。

我的经验：低通滤波适合噪声频率远高于信号频率的场景。如果噪声和信号频率接近，就别用LPF了，会连真实信号一起滤掉。

2.1.2 卡尔曼滤波——更“聪明”的滤波方式

低通滤波有个硬伤：它不知道系统的运动模型。卡尔曼滤波就不一样了，它会根据上一时刻的状态预测当前时刻的值，再用测量值去修正预测值。

说白了，卡尔曼滤波是在“猜”和“测”之间找一个最优平衡。

对于IMU的姿态估计，我常用的是线性卡尔曼滤波，状态量取角度和角速度偏差：

// 卡尔曼滤波核心步骤（简化版）
// 预测
x_pred = A * x_est + B * u;
P_pred = A * P_est * A' + Q;

// 更新
K = P_pred * H' * inv(H * P_pred * H' + R);
x_est = x_pred + K * (z - H * x_pred);
P_est = (I - K * H) * P_pred;

调卡尔曼滤波最头疼的是调Q和R矩阵。Q是过程噪声协方差，R是测量噪声协方差。

我曾经在一个项目里花了两天调这两个参数。后来总结出一个笨办法：先让机器人静止，记录IMU输出的方差作为R的初始值；然后让机器人做已知运动，看估计值和真实值的偏差来调Q。

关键对比：
对比项 低通滤波 卡尔曼滤波
计算量 极低（1次乘加） 中等（矩阵运算）
延迟 固定延迟 自适应延迟
适用场景 噪声频率固定、信号变化慢 噪声特性变化、需要高精度
调参难度 低（1个参数） 高（Q、R矩阵）

2.2 编码器信号毛刺处理

编码器是机器人的“关节角度传感器”。但增量式编码器有个通病——在低速或静止时，信号边缘会出现毛刺。

为什么会这样？因为编码器的A、B相脉冲在边沿处可能因为电气噪声产生额外的跳变。控制器读到这些假脉冲，就会以为电机在动，然后输出错误的控制量。

我处理编码器毛刺的方法有三个：

  1. 硬件去抖：在编码器信号线上加RC低通滤波器，时间常数取1~5μs。这个最简单，但会引入微小延迟。
  2. 软件去抖：在中断服务程序里做“连续采样确认”。比如连续读到3次相同的电平才认为信号有效。
  3. 速度阈值判断：如果计算出的瞬时速度超过物理极限，直接丢弃该数据。
注意：千万不要在编码器中断里做复杂的滤波运算！中断服务程序要短平快，否则会影响其他实时任务。我一般只在中断里做简单的边沿计数，滤波放在主循环或低优先级任务里做。

代码示例——软件去抖：

// 编码器信号软件去抖
#define DEBOUNCE_COUNT 3

uint8_t debounceEncoder(uint8_t raw_signal) {
    static uint8_t stable_count = 0;
    static uint8_t last_stable = 0;
    
    if (raw_signal == last_stable) {
        stable_count++;
        if (stable_count >= DEBOUNCE_COUNT) {
            return last_stable;
        }
    } else {
        stable_count = 0;
        last_stable = raw_signal;
    }
    return last_stable; // 返回上一次的稳定值
}

2.3 力传感器振动干扰识别

力传感器（通常是六维力/力矩传感器）安装在机器人的脚底或手腕。它的数据直接用于力控和平衡。

但力传感器有个让人头疼的问题——它会把机械结构的振动“翻译”成力的变化。

我记得有一次做单腿站立实验，力传感器输出的Z轴力一直在±20N之间振荡，而实际机器人根本没动。后来发现是腿部连杆的弹性模态被激励了，力传感器测到的其实是结构振动的反作用力。

识别力传感器振动干扰，我一般看三个特征：

  • 频率特征：振动干扰通常集中在某个频段（比如10~30Hz），而人的正常行走力变化在0~5Hz。做个FFT就能看出来。
  • 幅值特征：振动干扰的幅值通常较小（比如满量程的5%~10%），但变化快。如果力信号在几个毫秒内剧烈跳动，大概率是振动。
  • 相关性特征：如果力传感器和IMU的加速度信号在某个频段上高度相关，那力信号里的这个成分很可能是振动引起的。
实用技巧：在力传感器数据上串联一个陷波滤波器（Notch Filter），专门滤掉机械共振频率。比如共振频率是25Hz，就在25Hz处挖一个坑。这样既保留了低频的力信息，又去掉了振动干扰。

陷波滤波器的离散实现：

// 二阶陷波滤波器，中心频率25Hz，采样率200Hz
float notchFilter(float input) {
    static float x1 = 0, x2 = 0;
    static float y1 = 0, y2 = 0;
    
    float b0 = 0.9565f;
    float b1 = -1.7821f;
    float b2 = 0.9565f;
    float a1 = -1.7821f;
    float a2 = 0.9130f;
    
    float output = b0 * input + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
    
    x2 = x1;
    x1 = input;
    y2 = y1;
    y1 = output;
    
    return output;
}

2.4 本章知识体系总览

下面这张图把三种传感器噪声的排查思路串起来了。你可以把它当作一个快速排查指南：

传感器噪声排查知识体系 传感器噪声三大来源 IMU 高频噪声 编码器脉冲毛刺 力传感器振动 低通滤波 卡尔曼滤波 硬件去抖 软件去抖 FFT频谱分析 陷波滤波器 干净姿态数据 准确角度数据 真实力觉数据 机器人稳定控制

嗯，传感器噪声排查这块，说白了就是“对症下药”。IMU噪声用滤波，编码器毛刺用去抖，力传感器振动用频谱分析加陷波。但最关键的还是先搞清楚噪声的来源——是电气干扰？机械共振？还是安装方式不对？

我曾经在一个项目里折腾了三天，最后发现IMU的安装螺丝松了半圈。所以啊，排查噪声之前，先检查硬件安装，别一上来就调参数。

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