100、运动控制中的传感器融合：粒子滤波

运动控制中的传感器融合：粒子滤波

从一次炸机事故说起

去年调试四足机器人，在户外草地做高速奔跑测试。IMU和里程计融合用的扩展卡尔曼，仿真跑了两周都没问题。结果真机一上，第三个弯道直接侧翻——草地打滑导致里程计瞬间漂移，EKF的协方差矩阵直接崩了，机器人以为自己在直线前进，实际已经转了45度。

拆开看，电机没烧，但关节限位撞坏了。后来复盘，问题出在EKF对非高斯噪声的脆弱性。草地打滑产生的里程计误差根本不是高斯分布，而是突然出现的野值。这时候我意识到，对于运动控制这种强非线性、多模态噪声的场景，粒子滤波才是更靠谱的选择。

粒子滤波到底在解决什么问题

先别急着看公式。想象你在一个黑暗的仓库里定位一辆AGV。你只能通过轮子编码器知道大概走了多远（运动模型），偶尔听到超声波传感器报出距离墙面的数值（观测模型）。但仓库地面有油污，轮子会打滑；墙壁有货架，超声波可能反射到奇怪的地方。

卡尔曼滤波假设所有不确定性都是高斯分布的——就像告诉你“误差在±5cm内，概率呈钟形”。但实际打滑可能让误差瞬间变成50cm，这根本不是高斯能描述的。粒子滤波的思路很暴力：用一堆随机点（粒子）来近似任意分布。每个粒子代表一个可能的位姿假设，权重代表这个假设的可信度。

核心三步走：预测、更新、重采样

预测：让粒子跟着运动模型瞎跑

假设当前有N个粒子，每个粒子代表一个状态x_i和权重w_i。运动模型来了，比如编码器告诉你“走了0.1米，转了2度”。但别信它——真实运动会有噪声。对每个粒子，