第二讲：传感器噪声建模与滤波

做机器人路径规划这些年，我踩过最大的坑，就是「传感器数据太干净」。

你想想看，仿真里跑得飞快的算法，一上真车就开始抽风。为什么？因为真实世界的传感器，永远带着噪声。今天我们就来聊聊怎么跟这些噪声打交道。

2.1 噪声模型：先认识你的敌人

传感器噪声不是随机乱来的。它们有规律可循。我个人习惯，先把噪声分类，再对症下药。

2.1.1 高斯噪声

这是最常见的噪声模型。激光雷达、摄像头、IMU，基本都逃不掉。

高斯噪声的概率密度函数长这样：

p(x) = (1 / (σ * √(2π))) * exp(-(x - μ)² / (2σ²))

其中 μ 是均值，σ 是标准差。说白了，大部分噪声都集中在真实值附近，离得越远越少见。

实战经验：我在做AGV定位时，发现激光雷达的测距噪声其实不是纯高斯。远距离时噪声会变大。后来我用了「分段高斯模型」——3米以内 σ=0.01m，3-10米 σ=0.03m，10米以上 σ=0.08m。效果立竿见影。

2.1.2 椒盐噪声

这名字挺形象。椒盐嘛，就是黑点和白点。在图像里表现为随机像素被置为0或255。

为什么会这样？传感器坏点、传输丢包、电磁干扰，都可能导致。我遇到过最离谱的一次，是AGV经过大功率电机旁边，图像直接变成「雪花屏」。

椒盐噪声的数学模型很简单：

P(像素被破坏) = p
P(变成黑点) = p/2
P(变成白点) = p/2
P(保持原样) = 1 - p

其中 p 是噪声密度，一般取 0.01~0.05。

避坑指南：我曾经在视觉SLAM里直接用原始图像做特征匹配，结果遇到椒盐噪声，特征点全跑到噪点上去了。后来加了中值滤波预处理，匹配成功率从60%提升到92%。

2.2 滤波方法：把噪声滤掉

知道了噪声长什么样，接下来就是怎么处理。这里我重点讲两种：中值滤波和卡尔曼滤波。

2.2.1 中值滤波

对付椒盐噪声，中值滤波是神器。原理很简单：把窗口内的像素排序，取中间值代替中心像素。

// 3x3中值滤波示例
原始窗口：
[10, 12, 255]
[11, 200, 13]
[ 9, 255, 14]

排序后：9, 10, 11, 12, 13, 14, 200, 255, 255
中值：13

输出：13

你看，200和255这些噪点，直接被排除了。中值滤波对椒盐噪声的抑制效果，比均值滤波好得多。

2.2.2 卡尔曼滤波原理

卡尔曼滤波，说白了就是「用预测修正观测」。它假设系统是线性的，噪声是高斯分布。

核心就五个公式：

  1. 预测状态： x̂_k|k-1 = A * x̂_k-1|k-1 + B * u_k
  2. 预测协方差： P_k|k-1 = A * P_k-1|k-1 * A^T + Q
  3. 卡尔曼增益： K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^(-1)
  4. 更新状态： x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)
  5. 更新协方差： P_k|k = (I - K_k * H) * P_k|k-1

别被公式吓到。我拆开给你讲：

  • 预测：根据上一时刻的状态，猜当前时刻应该在哪
  • 观测：传感器告诉你当前在哪
  • 融合：猜的和测的，谁更可信就多信谁

卡尔曼增益 K 就是「信任权重」。传感器噪声大（R大），K就小，更信预测。模型不准（Q大），K就大，更信观测。

实战经验：我在做AGV里程计融合时，Q和R的调参花了两周。后来总结出经验：先固定Q，调R让滤波后的轨迹平滑；再固定R，调Q让响应速度够快。来回迭代三次，基本就稳了。

2.3 扩展卡尔曼滤波：对付非线性

卡尔曼滤波要求系统是线性的。但现实世界哪有那么多线性？

比如：

  • 里程计模型：x_k = x_k-1 + v * cos(θ) * dt —— 非线性
  • 观测模型：距离 = sqrt((x - x_beacon)² + (y - y_beacon)²) —— 非线性

怎么办？扩展卡尔曼滤波（EKF）登场。它的核心思想：线性化。

怎么线性化？泰勒展开，取一阶项。说白了，就是在当前估计点附近，用切线代替曲线。

// EKF 线性化示例
非线性函数：f(x) = x²
在 x=3 处线性化：
f(x) ≈ f(3) + f'(3) * (x - 3)
     = 9 + 6 * (x - 3)
     = 6x - 9

EKF的流程和KF一样，只是把A和H换成了雅可比矩阵：

// 雅可比矩阵计算
F_k = ∂f/∂x | x̂_k-1|k-1
H_k = ∂h/∂x | x̂_k|k-1

注意：EKF的线性化只在局部有效。如果初始误差太大，或者运动模型非线性太强，EKF会发散。我遇到过两次，一次是AGV打滑导致状态突变，一次是UWB信号跳变。后来加了异常检测，发现残差超过3σ就重置滤波器。

2.4 知识体系总览

下面这张图，是我梳理的本章知识结构。建议你保存下来，做项目时对照着看。

传感器噪声建模与滤波知识体系 噪声模型 滤波方法 高斯噪声 椒盐噪声 中值滤波 卡尔曼滤波 线性KF 扩展KF (EKF) 雅可比矩阵线性化 定位、导航、建图 常见坑：参数调不好、非线性太强发散、传感器异常未检测 解决方案：分段建模、异常检测、残差监控 噪声相关 滤波相关 避坑指南

2.5 代码实战：EKF定位

光说不练假把式。下面是一个简化版的EKF定位代码，用于AGV在二维平面上的定位。

import numpy as np

class EKF:
    def __init__(self):
        # 状态向量 [x, y, theta]
        self.x = np.array([0.0, 0.0, 0.0])
        # 协方差矩阵
        self.P = np.eye(3) * 0.1
        # 过程噪声
        self.Q = np.diag([0.05, 0.05, 0.01])
        # 观测噪声
        self.R = np.diag([0.1, 0.1])
        
    def predict(self, v, omega, dt):
        """运动模型预测"""
        theta = self.x[2]
        # 非线性运动模型
        self.x[0] += v * np.cos(theta) * dt
        self.x[1] += v * np.sin(theta) * dt
        self.x[2] += omega * dt
        
        # 计算雅可比矩阵 F
        F = np.eye(3)
        F[0, 2] = -v * np.sin(theta) * dt
        F[1, 2] = v * np.cos(theta) * dt
        
        # 更新协方差
        self.P = F @ self.P @ F.T + self.Q
        
    def update(self, z):
        """观测更新"""
        # 预测观测
        h = np.array([self.x[0], self.x[1]])
        # 观测残差
        y = z - h
        # 观测雅可比 H
        H = np.array([[1, 0, 0],
                      [0, 1, 0]])
        # 卡尔曼增益
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        # 状态更新
        self.x = self.x + K @ y
        # 协方差更新
        self.P = (np.eye(3) - K @ H) @ self.P
        
        return self.x

# 使用示例
ekf = EKF()
# 模拟一次预测+更新
ekf.predict(v=0.5, omega=0.1, dt=0.1)
ekf.update(np.array([0.52, 0.08]))
print(f"估计位置: {ekf.x[:2]}")

调参建议：Q矩阵的对角线值，我一般设成运动模型不确定度的平方。比如速度误差0.1m/s，那位置误差就是0.1*dt。R矩阵直接看传感器手册，或者做静态实验算方差。

嗯，这一章的内容就到这。噪声建模和滤波是路径规划的基石，地基打不牢，上层算法再花哨也白搭。下一章我们会聊更实际的——怎么在动态环境中做局部路径规划。

可可精品资料库

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