4. 传感器误差与数据融合：编码器量化误差、IMU漂移与噪声、多传感器融合
各位同学，今天我们来聊聊传感器误差和数据融合。说实话，这是轨迹规划里最让人头疼的部分之一。我做了这么多年机器人控制，见过太多因为传感器数据不准导致系统崩溃的案例。你想想看，如果传感器给你的位置信息都是错的，那再好的规划算法也是白搭。
4.1 编码器量化误差
编码器这东西，说白了就是个角度或位置的测量工具。但它有个天生的毛病——量化误差。什么意思呢？就是它只能输出离散的脉冲数，没法告诉你两个脉冲之间的精确位置。
举个例子，一个500线的编码器，转一圈输出500个脉冲。那每个脉冲对应的角度就是360°/500 = 0.72°。如果你停在0.36°的位置，编码器会告诉你什么呢？它要么报0°，要么报0.72°，反正不会报0.36°。这个0.36°的偏差，就是量化误差。
量化误差的数学表达：
Δθ = 360° / N
其中N是编码器每转的脉冲数。N越大，量化误差越小，但成本也越高。
我在项目中遇到过一件事：用了一个便宜的编码器做AGV的轮速测量，结果定位误差越跑越大。后来一查，量化误差累积起来，跑个几十米就偏了十几厘米。嗯，这里要注意，量化误差是随机误差，但它在积分过程中会累积成系统性的漂移。
避坑指南：
我曾经在调试一个移动机器人时，发现编码器数据在低速时特别不准。后来才意识到，低速时脉冲间隔太长，量化误差占比太大。解决办法是：要么用高分辨率编码器，要么在低速时切换到其他传感器（比如IMU）做辅助。
4.2 IMU漂移与噪声
IMU（惯性测量单元）这东西，我刚开始接触时觉得它挺神奇的。加速度计和陀螺仪一结合，理论上能算出任意时刻的姿态和位置。但实际用起来，你会发现它有个致命问题——漂移。
为什么会这样？因为IMU的测量值里包含噪声。你积分一次得到速度，再积分一次得到位置。噪声在两次积分中被放大，结果就是位置估计像喝醉了酒一样，越飘越远。
我记得有一次做无人机悬停实验，只用IMU做姿态估计。刚开始几秒钟还挺稳，过了十几秒，无人机就开始往一边偏了。这就是典型的IMU漂移问题。
IMU的误差模型通常包括：
- 零偏（Bias）：静止时输出不为零，随时间缓慢变化
- 白噪声（White Noise）：高频随机波动
- 随机游走（Random Walk）：零偏的缓慢漂移
注意：IMU的零偏会随时间变化，而且受温度影响很大。我建议每次上电后先做静态校准，记录初始零偏值。否则，你可能会看到位置估计在静止状态下也在匀速运动——这显然是不合理的。
4.3 多传感器融合
既然单个传感器都有毛病，那怎么办？把多个传感器结合起来用呗。这就是多传感器融合的核心思想——取长补短。
编码器在短时间内的相对位置测量很准，但长时间会累积误差。IMU在短时间内姿态估计很准，但长时间会漂移。GPS（如果有的话）绝对位置很准，但更新频率低，而且室内不能用。你看，它们正好互补。
4.3.1 卡尔曼滤波
卡尔曼滤波是我个人最常用的融合方法。它本质上是一个最优状态估计器，能根据系统的动态模型和传感器的观测模型，给出最可能的状态估计。
卡尔曼滤波的核心步骤就两步：预测和更新。
// 预测步骤
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（过程噪声协方差）：模型的不确定性。Q越大，滤波器越相信传感器
- R（观测噪声协方差）：传感器的噪声水平。R越大，滤波器越不相信传感器
- K（卡尔曼增益）：决定预测和观测的权重
调参经验：我刚开始调卡尔曼滤波时，总觉得Q和R是玄学。后来发现一个简单方法：先让系统静止，记录传感器数据的方差作为R的初始值。然后让系统运动，观察预测值和实际值的偏差，估算Q。这样调出来的参数至少不会太离谱。
4.3.2 粒子滤波
卡尔曼滤波有个前提——系统是线性的，噪声是高斯的。但实际中很多系统是非线性的，比如机器人的运动模型。这时候粒子滤波就派上用场了。
粒子滤波的思想很简单：用一堆随机粒子来近似状态的概率分布。每个粒子代表一个可能的状态，粒子的权重代表这个状态的可能性。
粒子滤波的步骤：
- 初始化：在状态空间里撒一堆粒子
- 预测：根据运动模型，让每个粒子移动
- 更新：根据传感器观测，更新每个粒子的权重
- 重采样：去掉权重小的粒子，复制权重大的粒子
- 重复2-4步
说实话，粒子滤波的计算量比卡尔曼滤波大得多。我一般在粒子数超过1000时就开始担心实时性了。但它的好处是能处理任意分布，包括多峰分布——比如机器人不知道自己在哪里，但知道可能在A点或B点，这时候粒子滤波就能同时保留这两种可能性。
注意：粒子滤波有个常见问题——粒子退化。就是经过几次更新后，大部分粒子的权重都变得很小，只有少数几个粒子有显著权重。这时候估计就不准了。解决办法是：适当增加重采样的频率，或者在重采样时加入少量随机粒子来保持多样性。
4.4 知识体系总览
下面这张图总结了本章的核心逻辑。你看，传感器误差是源头，数据融合是手段，最终目的是得到可靠的状态估计。
好了，这一章的内容就到这里。传感器误差是绕不开的坎，但掌握了数据融合的方法，你就能把这些有缺陷的传感器用好。记住，没有完美的传感器，只有聪明的融合算法。