Skip to content
状态估计与 AI阅读约 9 分钟
Particle FilterSMCResamplingRBPFSLAM

从序贯重要性采样(SIS)出发,解析多峰后验分布建模、有效粒子数退化机理、系统重采样实现以及 Rao-Blackwellized 粒子滤波结构化降维。

粒子滤波(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):

粒子集采样轨迹 x_1:k仅采样低维非线性位姿例如 SE(2) (x, y, θ)保留多峰假设分支条件解析滤波 p(m | x_1:k, z)给定轨迹后,路标或栅格地图彼此条件独立,可用 EKF 解析更新大幅压缩所需粒子总数 MN_eff 自适应判定N_eff < 0.5 M 触发O(M) 系统重采样+ 粗糙抖动注入多模态后验位姿 + 地图

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),自动注入随机全局粒子

讨论区开启了自动与人工双重审核,文明交流的评论会在短时间内展示。

Built with VitePress