第3章：传感器噪声分析与滤波实战

做轨迹规划这些年，我踩过最大的坑就是——明明算法写得天衣无缝，机器人跑起来却像喝醉了酒。后来才明白，问题出在传感器数据上。说白了，你喂给控制器的信号本身就是脏的，再好的算法也救不回来。

这一章，咱们就聊聊传感器噪声的几种常见类型，以及怎么用滤波手段把它们收拾干净。我会结合自己实际项目里的经验，给你讲清楚每种方法的适用场景和坑。

3.1 噪声类型：你面对的是哪种脏数据？

传感器噪声不是一种东西。不同场景、不同传感器，噪声的脾气完全不一样。我个人习惯先分两类：高斯噪声和椒盐噪声。

噪声类型	来源	特点	典型场景
高斯噪声	电子热噪声、量化误差	幅度服从正态分布，每个采样点都有微小扰动	IMU加速度计、激光雷达测距
椒盐噪声	通信丢包、传感器瞬间失效	随机出现极值（白点或黑点），稀疏但剧烈	视觉传感器、超声波传感器

我在做AGV小车的时候遇到过一件事：激光雷达数据偶尔会跳出一个2米外的点，但实际前面就是墙。这就是典型的椒盐噪声。如果不处理，路径规划直接认为前方有障碍物，小车就原地打转了。

3.2 三种滤波方法实战对比

好，知道了敌人长什么样，接下来就是怎么打。我挑三个最常用的滤波方法，逐个拆解。

3.2.1 低通滤波：最简单的平滑器

低通滤波的原理很简单——把高频抖动滤掉，保留低频趋势。公式就一行：

y[k] = α * x[k] + (1 - α) * y[k-1]

其中 α 是平滑系数，取值0到1。α越小，滤波越强，但延迟也越大。

我记得第一次调这个参数的时候，α设了0.1，结果轨迹平滑得像蜗牛爬，响应慢得让人抓狂。后来调到0.3，才找到平衡点。

3.2.2 中值滤波：椒盐噪声的克星

中值滤波的思路更直接——取窗口内所有数据的中间值。比如窗口大小是5，就把5个点排序，取第3个。

为什么它对椒盐噪声有效？你想想看，椒盐噪声是极值点，排序后要么在最前面，要么在最后面，根本进不了中间位置。所以中值滤波能干净利落地把异常点剔除。

// 中值滤波示例（窗口大小=5）
float medianFilter(float data[], int len) {
    // 排序
    for (int i = 0; i < len - 1; i++) {
        for (int j = 0; j < len - i - 1; j++) {
            if (data[j] > data[j + 1]) {
                float temp = data[j];
                data[j] = data[j + 1];
                data[j + 1] = temp;
            }
        }
    }
    // 取中间值
    return data[len / 2];
}

3.2.3 卡尔曼滤波：状态估计的瑞士军刀

卡尔曼滤波就高级一些了。它不光是滤波，更是在做状态估计。它把系统模型和观测数据融合起来，给出最优估计。

核心公式五条，我直接给你列出来：

// 预测
x_pred = A * x_est + B * u
P_pred = A * P_est * A^T + Q

// 更新
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z - H * x_pred)
P_est = (I - K * H) * P_pred

看着复杂，其实核心就两个东西：Q（过程噪声协方差）和R（测量噪声协方差）。Q设大了，滤波器更相信测量值；R设大了，滤波器更相信模型预测。

我在做无人机悬停控制的时候，卡尔曼滤波帮了大忙。GPS信号偶尔会飘，但加上IMU的预测，位置估计稳得很。

3.3 三种滤波方法对比总表

对比维度	低通滤波	中值滤波	卡尔曼滤波
适用噪声	高斯噪声	椒盐噪声	高斯噪声
计算量	极低	低（窗口小）	中等
延迟	有（α越小延迟越大）	有（窗口越大延迟越大）	低
需要模型	否	否	是
参数调优难度	低（一个α）	低（一个窗口大小）	高（Q、R矩阵）
轨迹平滑效果	中等	好（对异常点）	优秀

3.4 实战选择策略

说了这么多，到底怎么选？我一般按这个流程来：

1. 先看噪声类型：如果是均匀的小抖动，走高斯噪声路线；如果是偶尔的毛刺，走椒盐噪声路线。
2. 再看计算资源：嵌入式MCU上就别跑卡尔曼了，低通滤波或者中值滤波更实在。
3. 最后看实时性要求：要求高响应速度，卡尔曼滤波优先；要求平滑，中值滤波或低通滤波加大窗口。

3.5 知识体系总览

下面这张图是我自己梳理的传感器噪声滤波知识框架，帮你快速建立全局认知：

嗯，这张图把整个知识脉络串起来了。从传感器原始数据出发，先判断噪声类型，再选择对应的滤波方法，最后输出平滑稳定的轨迹数据。每一步都有对应的实战技巧，后面几章我们会逐个深入。