机器人中的传感器噪声、偏置与滤波

每一台机器人都在依据嘈杂、有延迟、有偏置、有时干脆缺失的测量行动。把原始读数当成真理，是造出一台行为不可预测的机器人最快的办法。本文讲清在信任一颗传感器之前你需要知道的词汇和取舍。在车传感器模拟器里把每一种效果都实验一遍。

噪声、偏置和漂移是三个不同的问题

• 噪声逐次采样随机变化。平均或滤波能减小它。
• 偏置是相对真值的一个固定偏移。滤波去不掉它；你必须在标定时量出来并减掉。
• 漂移是一个随时间缓慢变化的偏置，通常随温度。开机时做的标定会逐渐对不上现实，这就是长跑需要周期性重新标定、或需要一个可供对照的参考的原因。

把这三者搞混就会用错修法——再多滤波也永远治不好一个偏置。

怎么选滤波器

三种常见滤波能覆盖大多数情况：

• 滑动平均和指数低通都能平滑随机噪声，靠各自的窗口或时间常数在响应性与平滑度之间做交换。
• 中值滤波更擅长去掉偶发的尖峰和丢包，因为少数坏样本不会移动中间那个值。

选满足使用者要求的最轻的那一个——而不是画出来最好看的那个。

滤波永远会增加延迟

每一个平滑滤波器都会滞后于真实信号，而平滑越多滞后越多。低通滤波器的时间常数越长，拒掉的噪声越多，但落在快速变化后面的距离也越远。永远要把滤波后的估计和原始采样、以及真值一起画出来，好让你在把滤波器放进控制环之前就看见那个延迟——而在环里，延迟会导致不稳定。

class LowPass:
    def __init__(self, alpha):
        self.alpha = alpha      # smaller alpha = smoother but more lag
        self.value = None
    def update(self, sample):
        if self.value is None:
            self.value = sample
        else:
            self.value += self.alpha * (sample - self.value)
        return self.value

传感器融合能做什么、不能做什么

融合把几颗传感器的信息合成一个估计，而它只有当各传感器提供的是互补信息、并且它们的不确定度被诚实地表达出来时才有帮助。把两个都带偏置的测量合在一起并不会造出真理。互补滤波就是一个例子，它在高频信任一颗传感器、低频信任另一颗；卡尔曼滤波则按各自的不确定度给每个来源加权。无论哪一种，都要先做好标定、时间戳和共享坐标系，然后再谈融合。

一切都打时间戳

采样很少以完美的固定间隔到达，而滤波和融合都假定你知道每个值是什么时候被测量的。时间戳让你能发现抖动、对齐不同传感器的数据、并拒绝过期读数。把原始值和经过处理的值并排记录，好让削顶和丢包这类失败保持可复现。

先量你的传感器，再决定怎么滤

在还不知道噪声长什么样之前就选滤波器，那是猜。十分钟的记录就能定论，而对任何传感器流程都一样：

1. 把传感器纹丝不动地固定在一个已知的、不变的输入上。
2. 记录几千个带时间戳的原始样本。
3. 算出均值和标准差。
4. 把样本画出来，再画一张它们的直方图。

这四个数和两张图告诉你三种故障里你中了哪一种：

直方图是大家跳过的那一步，而它信息量最大。高斯噪声画出一个钟形；离群点画出一个钟形加上尾巴上远远的几个点。这两者需要不同的滤波器，而无论怎么盯着时间序列图看，都没法像直方图那样清楚地把它们分开。

滤波器选择，量化一下

第一行里的 √N 是有用的那个数：平均 4 个采样把噪声减半；平均 100 个采样把它降到十分之一，代价是 50 个采样的延迟。降噪的收益迅速递减，而延迟不会，这就是很长的平均几乎总是错误答案的原因。

对 EMA 来说，时间常数与 α 的关系是：

tau = dt x (1 - alpha) / alpha

所以在 10 ms 的循环上，α = 0.1 给出 90 ms 的时间常数。凭感觉设 α，正是人们最后得到比自己以为慢得多的滤波器的方式——算一次，你就知道自己手里是什么了。

延迟不是免费的，这里是它的代价

每一个平滑滤波器都会延迟信号，而在控制环里，这个延迟与被控对象响应变慢是无法区分的。它直接降低了环在振荡之前能容忍的增益。

最后一行值得停下来想一想。α = 0.02 会给出一条漂亮平滑的曲线，以及半秒的延迟——机器人是在按障碍物 15 cm 之前的位置在转向。这是一个出于好意的滤波器把机器人弄糟的最常见方式，而除非你把滤波后的曲线和原始曲线放在一起、旁边再放上真值，否则它是看不见的。

选满足要求的最轻的那个滤波，并且永远检查这份延迟换算到机器人真正在意的单位里要多少——毫米，或者度，而不是采样。

互补滤波，以及怎么选它的常数

互补滤波是业余机器人里的主力，因为它是三行代码，而且它解决的正是真问题：两颗以相反方式失效的传感器。

// gyroRate in deg/s, accelAngle in degrees, dt in seconds.
angle = alpha * (angle + gyroRate * dt) + (1.0f - alpha) * accelAngle;

alpha 设定的是截止频率——在它之上你信任陀螺仪，在它之下你信任加速度计：

tau = alpha x dt / (1 - alpha)          # the filter's time constant, seconds
f_crossover = 1 / (2 x pi x tau)        # Hz

从物理而不是感觉来选它：**时间常数应当比你想要拒掉的扰动长，比陀螺仪的漂移率短。**一颗偏置 1 °/s 的陀螺仪一秒就累积 1° 误差，所以几秒的时间常数会让真实误差在加速度计把它拉回来之前先积累起来。

还要注意 α 依赖于 dt。从一个循环频率不同的项目里抄来一个 α，得到的就是另一个滤波器——而这是“这些增益在我这儿不管用”这句话一个常见又隐蔽的来源。

融合做不到的事

融合不是魔法，有三条局限值得直说：

**它去不掉共有的偏置。**两颗都读高 2° 的加速度计，融合出来的估计同样读高 2°。融合利用的是相互独立的误差；共模误差原封不动地活下来。

**它造不出并不存在的信息。**再多滤波也给不了一颗六轴 IMU 一个绝对航向。偏航没有重力参考，所以它会漂，而只有磁力计或一个外部参考能修正它。

**它修不好坏时间戳。**把 10 Hz 的 GPS 定位和 200 Hz 的陀螺仪融合，需要知道每一个是何时被测量的，精确到那个更快间隔的一个零头。一个过期 100 ms 却被当成当前值的定位，会主动破坏一个单靠陀螺仪本来是对的估计。

时间戳，具体来说

“一切都打时间戳”这句话点头很容易，跳过也很容易。实用版本：

struct Sample {
  float value;
  unsigned long micros;    // when it was MEASURED, not when it was read
};

// Reject anything too old to be useful, rather than fusing it.
bool isFresh(const Sample &s, unsigned long maxAgeUs) {
  return (micros() - s.micros) < maxAgeUs;
}

时间戳要尽量贴近测量时刻捕获——在中断里，而不是解析之后。一条在定位产生 80 ms 之后才解析、并且用解析时刻打戳的 GPS 语句，就是错 80 ms，而下游什么都补不回来。

故障排查

自己试

在车传感器模拟器里你可以调出噪声、偏置、漂移和丢包，然后把原始数据和低通、滑动平均、中值估计作比较。这是感受噪声与延迟之间取舍最快的方式——也正是决定机器人在避障模拟器里如何反应的那同一个取舍。