3、传感器信号调理：IMU滤波与校准，关节力矩传感器噪声处理，足底压力传感器数据融合

传感器信号调理，说白了就是让原始数据变得「能用」。我见过太多团队在算法上花了大把时间，结果发现抖动根源是传感器噪声没处理好。嗯，这一章我们就来把这几个关键传感器的信号调理讲透。

3.1 IMU 滤波与校准

IMU 是人形机器人的「内耳」。它告诉你身体倾斜了多少、转得有多快。但原始数据里全是噪声——尤其是低成本 MEMS 传感器。

我个人习惯把 IMU 处理分成两步：滤波和校准。先滤波，再校准，顺序不能乱。

3.1.1 互补滤波：简单够用

为什么用互补滤波？因为加速度计低频准、高频噪，陀螺仪高频准、低频漂。互补滤波就是把两者的优点「互补」起来。

// 互补滤波核心代码（C语言风格）
float complementary_filter(float acc_angle, float gyro_rate, float dt) {
    static float angle = 0.0f;
    const float alpha = 0.98f;  // 权重系数

    // 陀螺仪积分
    angle += gyro_rate * dt;
    // 加速度计修正
    angle = alpha * angle + (1.0f - alpha) * acc_angle;

    return angle;
}

3.1.2 卡尔曼滤波：进阶选择

互补滤波虽然简单，但遇到剧烈运动或振动时容易露馅。卡尔曼滤波能动态调整信任度。你想想看，当机器人快速摆臂时，加速度计数据基本不可信，卡尔曼会自动降低它的权重。

我曾经在一个双足机器人项目里，用互补滤波跑起来总是晃，换成卡尔曼后姿态估计误差从 3° 降到了 0.5° 以内。代价是计算量大了不少——不过现在的 MCU 基本扛得住。

3.1.3 六面校准法

加速度计和磁力计都有零偏和尺度误差。六面校准法，说白了就是把传感器朝六个方向各放一次，记录数据，然后拟合出偏差参数。

3.2 关节力矩传感器噪声处理

关节力矩传感器，说白了就是测每个关节用了多大力。但它的噪声来源很杂：电机 PWM 干扰、机械共振、温度漂移……

我建议分三步走：硬件滤波 → 软件滤波 → 动态补偿。

3.2.1 硬件滤波：先挡掉高频

在传感器信号进入 ADC 之前，先加一个 RC 低通滤波器。截止频率一般设在 100~500 Hz。为什么？因为关节的机械响应频率通常不超过 50 Hz，高于 500 Hz 的基本都是噪声。

// RC 低通滤波截止频率计算
// f_c = 1 / (2 * π * R * C)
// 例如：R = 10kΩ, C = 0.1μF
// f_c = 1 / (2 * 3.1416 * 10000 * 0.0000001) ≈ 159 Hz

3.2.2 软件滤波：陷波滤波器

硬件滤波挡不住所有噪声。特别是电机 PWM 的开关频率（通常 10~20 kHz）会耦合进传感器信号。这时候需要陷波滤波器，专门干掉某个频率。

我记得有一次调试一个六轴机器人，力矩信号里总有 1.2 kHz 的尖峰。查了半天发现是电机驱动器的 PWM 频率。加了一个二阶陷波滤波器后，信号干净得像教科书一样。

// 二阶陷波滤波器（数字实现）
// 中心频率 1.2 kHz, 采样率 10 kHz
float notch_filter(float input) {
    static float x[3] = {0, 0, 0};
    static float y[3] = {0, 0, 0};

    const float b0 = 0.9565f;
    const float b1 = -1.7821f;
    const float b2 = 0.9565f;
    const float a1 = -1.7821f;
    const float a2 = 0.9130f;

    x[0] = input;
    y[0] = b0*x[0] + b1*x[1] + b2*x[2] - a1*y[1] - a2*y[2];

    x[2] = x[1]; x[1] = x[0];
    y[2] = y[1]; y[1] = y[0];

    return y[0];
}

3.2.3 零点偏置与温漂补偿

力矩传感器在无负载时输出不为零，这就是零点偏置。更麻烦的是，这个偏置会随温度变化。

我的做法是：每次机器人上电后，先让所有关节处于空载状态，采集 200 个样本取平均作为零点。然后每隔 5 分钟重新校准一次——如果温度变化超过 2°C 的话。

3.3 足底压力传感器数据融合

足底压力传感器，说白了就是机器人的「脚感」。它告诉你脚底哪里受力、受力多大。但单个传感器容易受安装位置、地面不平的影响。

我建议用多传感器数据融合来解决。一般足底会放 4~6 个压力传感器，分布在脚掌和脚跟。

3.3.1 加权融合算法

每个传感器的权重取决于它的位置和当前受力状态。比如脚跟先着地时，脚跟传感器权重应该更高；脚掌离地时，脚掌传感器权重降低。

// 加权融合计算 COP（压力中心）
typedef struct {
    float x;  // 传感器 X 坐标 (mm)
    float y;  // 传感器 Y 坐标 (mm)
    float f;  // 当前压力值 (N)
} FootSensor;

float calc_cop_x(FootSensor sensors[], int n) {
    float total_force = 0.0f;
    float weighted_sum = 0.0f;

    for (int i = 0; i < n; i++) {
        total_force += sensors[i].f;
        weighted_sum += sensors[i].x * sensors[i].f;
    }

    if (total_force < 0.1f) return 0.0f;  // 防止除零
    return weighted_sum / total_force;
}

3.3.2 零速修正（ZUPT）

足底压力传感器还有一个妙用：判断机器人是否处于「支撑相」或「摆动相」。当所有足底传感器读数都接近零时，说明脚已经离地了——这时候 IMU 的积分漂移可以重置。

这就是零速修正（ZUPT）的核心思想。我做过一个实验：只用 IMU 走 10 步，位置误差累积到 30 cm；加上 ZUPT 后，误差控制在 2 cm 以内。

3.3.3 跨传感器标定

足底压力传感器和关节力矩传感器之间需要做交叉标定。为什么？因为足底测的是地面反力，关节测的是电机输出力——两者理论上应该满足力平衡。

我曾经发现一个奇怪的问题：机器人站着不动，足底显示受力 600 N，但膝关节力矩算出来只有 550 N。差了 50 N 去哪了？后来发现是膝关节的摩擦力矩没补偿。加上摩擦力模型后，两者就吻合了。

嗯，传感器信号调理这块，说白了就是「脏活累活」。但做好了，后面的控制算法才能发挥真正实力。你想想看，如果输入的数据都是错的，再牛的算法也白搭。