机器人 / 系统课时
粒子滤波如何维护多个位姿假设
粒子滤波如何维护多个位姿假设
机器人经过相似走廊时,一次激光扫描可能对应几个不同位置。粒子滤波同时保留这些候选位姿,用里程计预测它们的移动,再根据传感器观测调整可信度。完成本课后,你将能运行四粒子算例,并理解粒子滤波怎样扩展为边定位边建图。
学习目标
- 解释粒子的位置、朝向和权重分别表示什么,以及为什么不能过早求平均。
- 复现运动预测、观测赋权、有效粒子数判断与系统重采样。
- 区分已知地图上的粒子定位与 Rao-Blackwellized 粒子滤波 SLAM。
同一个观测为何允许多种位姿
假设建筑里有两段形状相同的走廊,机器人可能位于地图的 或 ,朝向也相同。两处的激光距离几乎一致;若直接取均值 ,反而可能把机器人放在墙里。
每个粒子都是一份候选状态:二维机器人通常记录 ,另有非负权重 ,所有权重之和为 1。粒子群允许在两条走廊各形成一团高权重假设,而不是强迫分布只有一个中心。只有新观测能区分两处时,权重才会逐渐集中。
一帧雷达如何更新粒子
给定上时刻的粒子、里程计增量 和新激光观测 ,一次更新按以下顺序进行:
- 运动预测:从运动噪声模型采样,为每个粒子更新位置与朝向。例如里程计报告前进 ,实际候选位移还要叠加轮滑等误差。
- 观测赋权:在已知地图上,从各候选位姿预测激光距离。若实测 、模型标准差 ,可用 给出相对似然;一般将它乘以前一权重,再归一化。
- 选择性重采样:计算 。若它低于设定阈值,就按权重抽取新粒子并将其权重重置为 ;否则保留当前粒子。

这里的运动模型提供候选位置,观测模型判断这些位置与测量是否一致。重采样只改变粒子群的组成,不是额外的一次机器人运动。
用四个粒子复现权重变化
把场景简化成一维直走廊:墙位于 ,雷达朝正 方向,读数为 。四个粒子初始等权;固定四个运动噪声抽样值,便于复现。真实二维系统还会采样 和航向,并对多束有效激光联合计算权重。
在 Python 3.10 或更新版本中运行:
import math
# 固定运动噪声样本,便于重复得到相同的预测位姿
old_x = [-0.15, 0.30, 0.80, 1.25]
odom_dx = 0.20
noise_samples = [-0.05, 0.00, 0.00, 0.05]
particles = [x + odom_dx + e for x, e in zip(old_x, noise_samples)]
# 已知墙面位置;预测距离与雷达观测越接近,似然越高
wall_x, observed, sigma = 3.0, 2.0, 0.25
scores = [
math.exp(-0.5 * ((observed - (wall_x - x)) / sigma) ** 2)
for x in particles
]
weights = [s / sum(scores) for s in scores]
n_eff = 1.0 / sum(w * w for w in weights)
print("预测位姿:", [round(x, 2) for x in particles])
print("归一化权重:", [round(w, 4) for w in weights])
print("有效粒子数:", round(n_eff, 2))
if n_eff < len(particles) / 2:
# 系统重采样:固定起点 0.10;实际起点均匀采样于 [0, 1/N)
sampled = []
index, cumulative = 0, weights[0]
for u in [0.10 + k / len(particles) for k in range(len(particles))]:
while u > cumulative and index < len(weights) - 1:
index += 1
cumulative += weights[index]
sampled.append(particles[index])
print("重采样位姿:", [round(x, 2) for x in sampled])预期输出:
预测位姿: [0.0, 0.5, 1.0, 1.5]
归一化权重: [0.0003, 0.1065, 0.7868, 0.1065]
有效粒子数: 1.56
重采样位姿: [0.5, 1.0, 1.0, 1.0]四个粒子的有效数约为 ,低于 ,所以触发重采样。累计权重依次约为 ;起点为 时,四个等距采样刻度 分别命中第二、第三、第三、第三个粒子。新粒子各自的权重均为 ,但代码只打印其位置。
重采样为何不能每帧无条件做
当四个权重均为 时,;此时重采样没有明显收益,还可能随机丢掉某个位置假设。当权重高度集中时,重采样可以避免计算资源长期浪费在几乎不可能的候选上。
上述算例将 复制三次,不代表机器人已被精确定位到这一点。如果下一帧运动模型噪声设得过小,这三份粒子会长期重合,称为粒子贫化。工程中可先用 作为示例阈值,观察粒子分布、有效粒子数和定位误差,再调整重采样频率及运动噪声;不能靠每次重采样都任意加大扰动来修正模型错误。
从多位姿假设到多地图假设
上面的示例假设地图已知,扫描只给位姿打分。未知地图的 SLAM 还要估计地图:**Rao-Blackwellized 粒子滤波(RBPF)**对机器人历史轨迹进行采样,并在给定每条轨迹和观测的条件下估计对应地图。直观理解,就是每个粒子携带一条轨迹及其地图假设;相同的激光束,因位姿假设不同,可能更新到不同的栅格。

例如粒子 A 判断机器人在走廊左侧,粒子 B 判断它在右侧,它们会把同一束激光的障碍端点投到不同地图单元。后续扫描若长期支持 A,A 的权重上升,相应地图历史也更可能被保留。实现时可让粒子之间共享尚未修改的地图数据以节约内存,但逻辑上仍需区分各自的地图状态。
在 ROS 2 生态中,Nav2 的 nav2_amcl 是在已有地图上定位的粒子滤波器,不会替你从零建图;slam_toolbox 提供的是以位姿图为主的二维 SLAM,也不应直接称作 RBPF。判断是否采用粒子方案时,先明确任务是已知地图定位,还是要同时推断轨迹和地图;两者维护的状态不同,计算成本也不同。