主题
粒子滤波(PF)与非高斯多峰状态估计:权重退化、重采样与 RBPF 分解
无论是 EKF 还是 UKF,底层都暗含一个强前提:后验概率分布可以近似为单峰高斯分布。然而在机器人全局定位(如对称走廊中的重定位“绑架问题”)或高度非线性的多目标关联场景下,状态后验往往呈现多峰、偏态甚至截断分布。粒子滤波(Particle Filter, PF) 通过一组带权随机样本直接逼近任意非高斯后验密度,成为解决多模态状态估计的核心工具。
1. 序贯重要性采样与权重更新
粒子滤波以集合 { xk(i), wk(i) }i=1..M 近似真实后验分布 p(xk | z1:k)。当选取状态转移分布 p(xk | xk−1(i)) 作为建议分布(Bootstrap Filter)时,每个粒子的权重递推退化为简洁的似然乘积:
Sequential Importance Sampling & Effective Sample Size
wk(i) ∝ wk−1(i) · p(zk | xk(i)), Σi=1..M w̃k(i) = 1(1) 重要性权重递推
Neff ≈ 1 / Σi=1..M (w̃k(i))2(2) 有效粒子数评估
随着递推步数增加,权重方差会不可避免地单调放大,最终几乎所有权重集中在极少数粒子上,导致其余粒子对后验毫无贡献——这就是粒子权重退化(Weight Degeneracy)。通过监控有效粒子数 Neff(通常取阈值 Neff < M/2),可在必要时触发重采样以淘汰极低权重样本、复制高后验密度区域样本。
2. 系统重采样与 Rao-Blackwellized 结构化降维
朴素多项式重采样会引入额外的蒙特卡洛方差,且若每帧无条件重采样会导致样本快速失去多样性(Sample Impoverishment)。工程上广泛采用低方差的系统重采样(Systematic Resampling),并在高维状态空间引入 Rao-Blackwellized 粒子滤波(RBPF):
3. O(M) 系统重采样参考实现
相比朴素二分查找累积分布(O(M log M)),系统重采样仅生成一个均匀随机起点并配合单调双指针扫描,即可在严格 O(M) 时间内完成低方差抽样:
python
import numpy as np
def systematic_resample(weights: np.ndarray, rng: np.random.Generator) -> np.ndarray:
"""O(M) low-variance systematic resampling over normalized particle weights."""
n = weights.shape[0]
cdf = np.cumsum(weights)
cdf[-1] = 1.0 # Guard floating-point rounding drift
u0 = rng.uniform(0.0, 1.0 / n)
positions = u0 + np.arange(n, dtype=np.float64) / n
indices = np.empty(n, dtype=np.int64)
i, j = 0, 0
while i < n:
if positions[i] <= cdf[j]:
indices[i] = j
i += 1
else:
j += 1
return indices
def effective_sample_size(weights: np.ndarray) -> float:
"""Compute effective sample size N_eff = 1 / sum(w_i^2)."""
return float(1.0 / np.sum(np.square(weights)))4. 粒子数目、退化与工程调优对照
| 关键问题 | 失效表现 | 工程解决策略 |
|---|---|---|
| 维度灾难 | 状态维数超过 10 维时所需粒子数呈指数级增长 | 利用 RBPF 将线性高斯子状态边缘化,仅对低维非线性位姿采样 |
| 粒子贫化(Impoverishment) | 所有粒子坍缩至同一个点,丧失环境变化后的恢复能力 | 引入 KLD-Sampling 动态调节粒子数,并在重采样后叠加小幅高斯抖动(Roughening) |
| 传感器似然过尖(Peaky Likelihood) | 单帧激光雷达匹配得分极度尖锐,瞬间杀死 99% 粒子 | 对似然打分施加温度退火因子 τ ∈ (0, 1) 或混合均匀离群概率底噪 |
| 机器人绑架恢复 | 位置突变后所有粒子均落在错误模态,全局定位失效 | 监控长期与短期似然指数滑动平均比值(Augmented MCL),自动注入随机全局粒子 |