当前位置：首页 > news >正文

AMCL自适应（KLD - Sampling: Adaptive Particle Filters）一种基于粒子滤波的移动机器人定位算法

news 2025/10/21 10:01:35

1. 概览

AMCL 是什么？
AMCL 是一种基于粒子滤波（particle filter / Monte Carlo Localization, MCL）的概率定位方法，用来估计移动机器人在已知地图（通常是 2D 占据栅格地图）中的位姿 $xt=(x,y,θ)x_t = (x, y, \theta)$ 。AMCL 的关键在于自适应地调整粒子数目，以在保证定位精度的同时节省计算资源（即 KLD-sampling）。

适用场景

已有静态地图（占据栅格 map）
传感器：激光雷达（2D LIDAR）或深度相机投影为 2D 扫描
想要在线、鲁棒、近实时的定位（包括在被动重定位 / 被 kidnap 的情况表现可控）

2. 粒子滤波基础概念

粒子滤波用于递推估计后验 $p(xt∣z1:t,u1:t)p(x_t \mid z_{1:t}, u_{1:t})$ ，其中 $u_{1:t}$ 为控制（里程计 / odom）， $z_{1:t}$ 为观测（激光）。

粒子表示：一组加权样本 ${(x_t^{[i]}, w_t^{[i]})}_{i=1}^{N}$ ，逼近后验分布。过程包含三步：

预测（motion update / proposal）：根据运动模型将粒子从 $x_{t-1}$ 预测到 $x_t'$ 。
测量更新（weighting）：用传感器模型计算每个粒子的似然 $wt[i]∝p(zt∣xt[i])w_t^{[i]} \propto p(z_t \mid x_t^{[i]})$ 。
重采样（resample）：按权重重采样生成新粒子集合以避免退化。

AMCL 在此基础上加入**自适应粒子数（KLD-sampling）**与一些工程优化（如置信度跟踪、混合全局随机粒子注入等）。

3. 核心模块与模型

3.1 状态与粒子

状态： $\theta)$ （2D 平面位姿）
粒子：位姿样本 + 权重

3.2 运动模型（Motion model）

常用基于里程计的 差分驱动噪声模型（odometry-based motion model），如 Velocity motion model 或 odometry motion model（ROS AMCL 使用的经典模型是基于概率的 odometry model，参见 Thrun 等）：

给定里程计增量 $(Δx,Δy,Δθ)(\Delta x, \Delta y, \Delta \theta)$ （在机器人自身坐标系），为每个粒子采样噪声：

前向与侧向位移噪声（与位移大小成比例）
角度噪声（与转角大小成比例）

常见参数：alpha1..alpha4（控制不同噪声项与运动量的关系）。实现通常用下列方式（伪表示）：

$δ^∗rot1=δ∗rot1+N(0,α1δrot12+α2δtrans2) δ^∗trans=δ∗trans+N(0,α3δtrans2+α4δrot12+α4δrot22) δ^∗rot2=δ∗rot2+N(0,α1δrot22+α2δtrans2) \begin{aligned} \hat{\delta}*{rot1} &= \delta*{rot1} + \mathcal{N}(0, \alpha_1 \delta_{rot1}^2 + \alpha_2 \delta_{trans}^2) \ \hat{\delta}*{trans} &= \delta*{trans} + \mathcal{N}(0, \alpha_3 \delta_{trans}^2 + \alpha_4 \delta_{rot1}^2 + \alpha_4 \delta_{rot2}^2) \ \hat{\delta}*{rot2} &= \delta*{rot2} + \mathcal{N}(0, \alpha_1 \delta_{rot2}^2 + \alpha_2 \delta_{trans}^2) \end{aligned}$

然后更新粒子位姿。

直观：运动越大，噪声越大（采样范围增大）。

3.3 观测模型（Sensor model / Measurement model）

AMCL 常用的两种激光观测模型是：

Beam-based model（beam model）：每条激光束独立计算似然，包含多项混合分布（命中、短距、随机、失效）。数学上：

$p(z_t \mid x_t) = \prod_{k=1}^K p(z_{t,k} \mid x_t)$

每一项 $p(zt,k∣xt)p(z_{t,k}\mid x_t)$ 是 Geman-McClure / mixture of Gaussians 或 Thrun 的 beam model（包含 z_hit, z_short, z_rand, z_max 等权重与对应分布）。
Likelihood Field Model（概率场 / likelihood field）：先把地图转换为到最近障碍物距离的距离场（distance transform），然后用测量值与场值计算每束激光的概率（通常使用高斯于距离值上）：

$p(z_{t,k} \mid x_t) \propto \exp\left( -\frac{(d(p_k(x_t), z_{t,k}))^2}{2\sigma^2} \right)$

其中 $p_k(x_t)$ 表示从位姿 $x_t$ 在该束方向上投射的在地图上的位置， $d(⋅)d(\cdot)$ 为地图上点到最近障碍物的距离。

4. AMCL 的自适应（KLD）采样思想

传统粒子滤波用固定粒子数 $N$ 。KLD-sampling（基于 Kullback–Leibler divergence）动态决定所需粒子数 $N$ ，以保证近似的后验与真实后验在概率距离上小于给定误差 $ϵ\epsilon$ （置信度 $1−δ1-\delta$ ）。

实现步骤概要：

将位姿空间（ $(x,y,θ)(x,y,\theta)$ ）离散成一系列 bin（箱），通常对 $(x, y)$ 做栅格， $θ\theta$ 分若干角度区间。
在重采样过程中按权重抽样，将样本落入相应 bin。
当已观测到的非空 bin 数为 $k$ 时，根据 KLD 公式计算所需最小样本数 $N$ ：

$\frac{k-1}{2\epsilon} \left(1 - \frac{2}{9(k-1)} + \sqrt{\frac{2}{9(k-1)}} z_{1-\delta}\right)^3$

其中 $z1−δz_{1-\delta}$ 为标准正态分位数（例如： $δ=0.01⇒z0.99≈2.326\delta=0.01 \Rightarrow z_{0.99} \approx 2.326$ ）。
当当前抽样得到的样本数达到该 $N$ 时停止重采样。

5. 算法完整伪代码

输入：前一时刻粒子集 { (x_{t-1}^{[i]}, w_{t-1}^{[i]}) }, 控制 u_t, 观测 z_t, map
输出：当前粒子集 { (x_t^{[i]}, w_t^{[i]}) }1. 预测（对 i=1..N_prev）x'_i ~ p(x_t | x_{t-1}^{[i]}, u_t)2. 计算权重w'_i = p(z_t | x'_i)3. 归一化权重w'_i = w'_i / sum_j w'_j4. 自适应重采样（KLD-sampling）Initialize empty new particle set S_newwhile size(S_new) < N_required:draw index i with probability w'_iadd x'_i to S_newupdate occupied bins & krecompute N_required by KLD formula5. 随机粒子注入（recovery）
6. 输出粒子集 S_new

7. 细节推导与关键公式

当然可以，以下是你提供的这段内容，所有数学公式都已改写为符合 LaTeX Markdown 语法的形式：

7. 细节推导与关键公式

7.1 权重计算（观测似然）

权重（未归一化）通常取观测模型似然：

$w_t^{[i]} = p(z_t \mid x_t^{[i]})$

若采用 beam model（独立假设）：

$p(z_t \mid x) = \prod_{k=1}^K p(z_{t,k} \mid x)$

实现中为避免数值下溢，常在对数域累积或仅使用部分 beams（例如每 5 梁取一梁）。

若用 likelihood field：

$p(z_{t,k} \mid x) \propto \exp\left(-\frac{d_k(x)^2}{2\sigma^2}\right)$

其中 $d_k(x)$ 是在位姿 $x$ 下第 $k$ 梁对应点到最近障碍物的距离（distance transform 查得）。

7.2 KLD-sampling 公式（详见 Thrun 等）

给定 $k$ 个非空箱，误差 $ϵ\epsilon$ 、置信度 $1−δ1-\delta$ ，需要的样本数约为：

$\approx \frac{k-1}{2\epsilon} \left(1 - \frac{2}{9(k-1)} + \sqrt{\frac{2}{9(k-1)}} z_{1-\delta}\right)^3$

（对小 $k$ 有数值近似注意）

8. 恢复（Recovery）与 Kidnap 处理

AMCL 通常实现如下机制：

随机粒子注入：周期性地在地图范围内注入随机粒子。
短期 / 长期平均似然对比（alpha_slow, alpha_fast）：若短期均值远低于长期均值，则加大随机注入比例。
全局定位触发：定位失败时重新进行全局采样。

9. 重采样方法

常见重采样算法：

multinomial resampling（多项式）
systematic resampling（系统重采样）——效率高且方差低，常用
stratified resampling（分层）
ROS AMCL 默认用 系统重采样（systematic），因为方差较低且实现简单。

10. 参数设置经验

以下为常见经验值（具体系统需调整）：

min_particles：100
max_particles：2000（或更高，视计算资源）
kld_err：0.05（较宽松，粒子较少）；若需更高精度可设 0.01
kld_z：0.99（对应 z≈2.326）
motion alpha1..4：如果使用里程计且其噪声小，alpha 可设小值如 0.01~0.05；若里程计漂移大，适当增大
laser_sigma_hit：通常 0.1~0.2（米），取决于激光精度与 map 分辨率
laser_z_hit：0.8，laser_z_rand：0.1（beam model 的混合系数作为起点）
update_min_d：0.2 m，update_min_a：0.2 rad（避免更新过频）

建议调参流程：

先让 system 能稳定运行：min_particles 设较大以保证稳定性（例如 500）。
调整 motion noise 使粒子随里程计移动但不过度发散。
调整 sensor noise（sigma）使得权重更新对真实 pose 有判别力（不是所有粒子权重都趋于 0）。
开启 KLD，调小 kld_err 观察粒子数量与精度折中。
最后降低 max_particles 以节省 CPU。

11. 常见问题和解决方法

问题：收敛到错误位置 / 跟踪丢失
- 检查里程计 frame 是否正确（tf）、map 与 sensor 的坐标变换是否准确。
- motion noise 过小会导致粒子不散开从而无法覆盖正确位置；过大则不稳定。
- sensor model mismatch：检查 map 和传感器是否对齐，distance field 是否正确计算。
问题：粒子数量一直很大 / 计算慢
- kld_err 过小或 bin 分辨率设得过细会导致需要大量 bin，从而需要大量粒子。
- 减小 kld_err、增大 kld_z（降低置信）或放粗 bin 分辨率。
问题：在狭长走廊里姿态不确定（朝向不稳定）
- 提高 yaw 分辨率或增加角度观测信息（若激光对于角度区分度低，可结合视觉或扫描匹配）。

调试技巧：

可视化粒子云（RViz 的 ParticleCloud），观察是否多峰或收敛到不合理位置。
打印短期/长期平均似然（alpha_fast/alpha_slow）以调试恢复注入策略。
通过限制 laser_max_beams 减少计算（例如每 5th beam）来加速调试。

12. 性能与复杂度

观测更新通常是最昂贵部分（与 beams 数量与每束 ray-casting 相关）。如果使用 likelihood field 并预先做 distance transform，单束代价较低。
粒子数 (N) 直接决定运算成本（motion + weight + resample，整体复杂度大致 (O(N \times \text{beams_used}))）。KLD 旨在在多峰情况下自动增加 N，而在单峰时降低 N，从而更有效率。

13. 核心要点总结

AMCL 是基于粒子滤波的定位方法，适合在已知 2D 地图下使用激光进行定位。
核心流程：里程计预测 → 激光似然权重 → 自适应重采样（KLD） → 重复。
关键技术点：运动噪声模型、观测模型（beam / likelihood field）、KLD-sampling、重采样策略、恢复机制。
调参要点：关注 motion noise、sensor sigma、KLD 参数与 min/max 粒子数。

14、参考文献

Dieter Fox, “KLD-Sampling: Adaptive Particle Filters and Mobile Robot Localization,” Advances in Neural Information Processing Systems, vol. 14, 2001.

Sebastian Thrun, Wolfram Burgard, Dieter Fox, Probabilistic Robotics, MIT Press, 2005.

F. Dellaert, D. Fox, W. Burgard, S. Thrun, “Monte Carlo Localization for Mobile Robots,” IEEE International Conference on Robotics and Automation (ICRA), 1999.

查看全文

http://www.dtcms.com/a/507855.html