上篇讲了UKF——用sigma点采样来避免计算雅可比矩阵。但UKF本质上还是假设状态分布是高斯的只是传播均值和协方差的方式更精确了。如果你面对的问题状态分布根本不是高斯的呢比如机器人全局定位——一个机器人被突然放到一栋大楼里它完全不知道自己在哪。这时候它的位置可能是大楼里的任何一个地方而且随着它移动和观测可能的位姿会聚集成几个簇——每个簇对应一个可能的走廊交叉口。这种多模态分布卡尔曼滤波家族全部搞不定。粒子滤波就是为这种场景而生的。它的思路特别暴力不假设分布是什么形状了直接用一堆随机采样点粒子来近似任意分布。撒豆子粒子滤波的直觉想象你面前有一张地图你不知道自己在哪。怎么办最简单的办法在地图上随机撒1000颗豆子每颗豆子代表一个我可能在这里的假设。然后你环顾四周看到左边有一面墙、右边有一扇门。你检查每颗豆子——如果这颗豆子所在位置也是左边有墙、右边有门那这颗豆子就靠谱给它加权重。如果这颗豆子所在位置和你对不上那它不靠谱减权重。经过这一轮大部分权重集中在那些和观测一致的豆子上了。然后你做重采样——把权重高的豆子复制几份权重低的豆子丢掉。这样豆子就集中到了几个可能的位置上。你往前走一步所有豆子也跟着移动根据运动模型。再观测一次再更新权重再重采样。反复几轮豆子就越来越集中到你的真实位置了。这就是粒子滤波的全部。说白了就是三步循环预测移动粒子→更新调整权重→重采样淘汰差粒子。粒子滤波的算法流程用更正式的语言描述# 初始化随机生成N个粒子 particles [random_state() for _ in range(N)] weights [1.0/N] * N for step in range(num_steps): # 1. 预测根据运动模型移动每个粒子 for p in particles: p motion_model(p, control) noise # 2. 更新根据观测计算每个粒子的权重 for i, p in enumerate(particles): weights[i] * observation_model(p, measurement) # 3. 归一化权重 weights [w / sum(weights) for w in weights] # 4. 重采样按权重重新采样 particles resample(particles, weights) weights [1.0/N] * N核心就这么多。但有几个细节决定了粒子滤波在实际中能不能用。重采样策略粒子枯竭问题重采样是粒子滤波中最关键也最容易出问题的步骤。最直接的重采样方法是轮盘赌——按权重比例随机采样。权重大的粒子被选中的概率高会被复制多份权重小的粒子大概率被丢弃。但这里有个问题如果某一步观测之后只有一个粒子的权重特别高其他粒子权重都接近零那重采样之后所有粒子都会变成那个粒子的副本。粒子多样性瞬间丧失这叫粒子枯竭particle depletion。一旦枯竭滤波器就失去了跟踪能力。解决办法有几种系统性重采样比轮盘赌更均匀保证权重高的粒子被复制但不会过度集中。工程中最常用。只有效样本数低于阈值时才重采样有效样本数N_eff 1 / sum(w_i^2)。当N_eff大于某个阈值通常取N/2时不重采样保持粒子多样性。只有当N_eff太低、粒子权重严重不均匀时才触发重采样。正则化粒子滤波重采样后给每个粒子加一点随机扰动防止所有粒子都聚到同一点。扰动的大小和核密度估计的带宽有关通常根据协方差和粒子数量来计算。粒子数量多少才够粒子数量直接决定了计算量和精度。粒子太少无法充分覆盖状态空间估计不准。粒子太多计算量爆炸实时性没法保证。经验法则是状态维度越高需要的粒子数量越多。一维定位可能100个粒子就够了二维平面定位可能需要1000到5000个六维的位姿估计x, y, yaw, vx, vy, omega可能需要上万个。在机器人定位中有个实用的技巧叫KLD采样Kullback-Leibler Distance sampling——根据当前粒子分布的复杂度动态调整粒子数量。如果粒子分布简单比如已经收敛到一个峰就减少粒子数节省计算。如果分布复杂多模态就增加粒子数保证覆盖。和卡尔曼滤波家族的对比面试中经常被问到粒子滤波和EKF/UKF怎么选。这个问题没有标准答案得看具体场景。粒子滤波的优势能处理任意分布非线性非高斯实现简单不需要推导雅可比矩阵天然适合多模态问题全局定位、 kidnapped robot problem。粒子滤波的劣势计算量大需要大量粒子才能保证精度在高维状态空间中效率低维度灾难粒子退化问题需要仔细处理。选择建议如果系统是近似线性的、分布是单峰的用标准卡尔曼滤波就够了。如果系统是非线性的、但分布还是单峰的用EKF或UKF。如果系统是非线性的、分布可能是多模态的全局定位、多目标跟踪用粒子滤波。讲真在实际的机器人产品中纯粒子滤波用得不多——计算量太大了嵌入式平台跑不动。更常见的是混合方案用EKF/UKF做局部跟踪机器人知道自己大概在哪的时候用粒子滤波做全局定位机器人不知道自己在哪的时候。或者用Rao-Blackwellized粒子滤波——部分状态用粒子滤波部分状态用卡尔曼滤波各取所长。之前做AMR的时候我们团队就试过纯粒子滤波做定位5000个粒子在树莓派上跑一帧要30ms根本跟不上。后来换成EKF做跟踪只在初始定位阶段用粒子滤波效果反而更好。面试中怎么聊面试官问粒子滤波先用撒豆子的比喻讲直觉再说算法流程初始化→预测→更新→重采样然后说关键问题粒子枯竭、粒子数量选择最后对比和卡尔曼滤波的适用场景。如果面试官追问粒子滤波的维度灾难怎么解决你可以说维度越高均匀覆盖状态空间所需的粒子数量指数增长。解决办法有几种一是Rao-Blackwellization把状态空间分成高维和低维两部分低维部分用粒子滤波高维部分用卡尔曼滤波。二是用重要性提议分布proposal distribution让粒子在采样时就考虑最新的观测信息而不是盲目地从先验分布采样。三是粒子群优化——用优化算法引导粒子向高似然区域移动。如果面试官追问粒子滤波在SLAM中的应用你可以说FastSLAM就是用粒子滤波做SLAM的经典算法。每个粒子代表一个机器人位姿假设同时每个粒子维护一张自己的地图通常是特征地图或栅格地图。粒子的权重根据粒子对应的位姿和地图对当前观测的解释程度来计算。FastSLAM的优点是能处理数据关联不确定性同一个观测可能对应不同的路标缺点是粒子数量需要很大才能同时覆盖位姿和地图的不确定性。补充一个粒子滤波在实际项目中的调试经验。当时做一台服务机器人的全局定位用粒子滤波在已知地图上定位。一开始设了3000个粒子在树莓派上跑一帧要30ms实时性不够。我们做了几个优化第一用KLD采样Kullback-Leibler Distance Sampling动态调整粒子数量——当粒子集中在一个小区域时自动减少粒子数当机器人被绑架位置突然变化时自动增加粒子数。优化后平均粒子数降到了1000左右处理时间降到了10ms。第二在重采样时用低方差重采样low-variance resampling代替轮盘赌重采样减少粒子枯竭的风险。第三把粒子的权重计算从串行改成并行用OpenMP在四核处理器上速度提升了接近4倍。这些优化手段在面试时候讲出来能体现你的工程能力。下一篇讲因子图优化入门——图模型在状态估计中的应用这是另一种和卡尔曼滤波、粒子滤波完全不同的状态估计范式。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第168篇 无迹卡尔曼滤波UKF——不用算雅可比的替代方案下一篇预告第170篇 因子图优化入门——图模型在状态估计中的应用有任何问题欢迎评论区留言我会尽量回复。