蒙特卡洛定位：粒子滤波如何在已知地图上站住

AMCL 是 ROS 导航栈里最常被「黑盒使用」的模块：参数 yaml 抄一份，粒子数调个 2000，能定位就算完。我在窄走廊里见过粒子全部挤在错误位置、机器人被人工搬走后永远找不回来——这些不是 AMCL 的 bug，是运动模型与观测模型不匹配的典型症状。

MCL 在估计什么

已知占据栅格地图 m，估计机器人在地图中的位姿 x_t = (x, y, θ)（2D）或 SE(3)（3D 变体）。用粒子集合 {x_t^(i), w_t^(i)} 近似后验：

text

p(x_t | z_{1:t}, u_{1:t}, m) ≈ Σ_i w_t^(i) δ(x_t - x_t^(i))

每个粒子是一个位姿假设；权重是「这个假设有多符合当前激光/深度观测」。

预测（运动更新）：用里程计/控制输入 u_t 传播粒子：

text

x_t^(i) ~ p(x_t | x_{t-1}^(i), u_t)

差速模型里加高斯噪声到 (v, ω)；噪声过小 → 粒子云收缩，丧失多样性；过大 → 永远模模糊糊，收敛慢。

更新（观测）：激光扫描 z_t 与地图匹配，更新权重：

text

w_t^(i) ∝ w_{t-1}^(i) · p(z_t | x_t^(i), m)

占据栅格上用 beam model 或 likelihood field。likelihood field 更快，是 AMCL 默认；beam model 对长走廊更精细但算力贵。

重采样：按权重抽样，淘汰低权粒子、复制高权粒子。有效粒子数 N_eff = 1 / Σ(w^(i))² 低于阈值时触发。重采样太频 → 粒子耗尽（sample impoverishment）；太稀 → 计算浪费。

机器人被人工搬走到地图另一处，运动模型仍假设「从上一帧附近移动」。粒子云跟不上真实位置，权重全部趋零，定位永久丢失。

应对策略：

产品里我会把「定位丢失 → 减速/停障」和「触发全局重定位」绑在一起，而不是让导航继续在错误位姿上跑。

调参方法：录 bag，在已知真值轨迹（或人工标注起终点）上扫参数，看定位 RMSE 与丢失次数。不要只盯 RViz 里粒子「看起来聚了」。

轮速标定错：粒子预测系统性偏，激光更新永远在「纠正」而不是「确认」。先标轮径/轴距，再调 AMCL。

地图过时：人走过的区域未清除，likelihood field 在假障碍处给低权，粒子被赶到错误通道。动态环境要配合清除层或定期重构图。

坐标系：map→odom 由 AMCL 发布，odom→base_link 由里程计发布。Nav2 消费的是 map 帧下的位姿——TF 树断裂比算法参数错更常见。

2000 粒子在 50m×50m 地图上每格不到 1 个粒子——全局定位概率极低。要么缩小 initial pose 不确定性，要么分层：局部高密度 + 稀疏全局。CPU 和定位成功率之间要实测曲线，不要抄 Nav2 默认。

多线激光 / 深度相机可跑 3D MCL，但粒子维数上升，实时性难。仓储 2D 足够时别上 6DOF 粒子——算力花在错误维度上。3D 场景用 NDT/点云定位往往是更务实的选择。

likelihood field 对 thin wall 地图友好；beam model 对 specular 误检更敏感但几何更准。玻璃墙场景 AMCL 常多峰——要么改地图语义，要么加 glass detection，不是无限加粒子能解。