AMCL（Adaptive Monte Carlo Localization）算法的原理详解

1. AMCL 的背景与定位任务

在移动机器人中，我们常遇到 定位问题： 已知机器人的地图（Map）和传感器观测数据（激光雷达、里程计等），推断机器人在地图中的位置与姿态（

x

,

y

,

θ

x, y, \\theta

x,y,θ）。

这个任务属于 全局定位问题 或 跟踪定位问题：

• 全局定位（Global Localization）：机器人初始位置未知
• 跟踪定位（Tracking）：已知初始位置，大部分时候是位置跟踪

AMCL 是 蒙特卡洛定位（MCL） 的改进版，本质是 基于粒子滤波的贝叶斯定位方法，并引入 KLD-采样 动态调整粒子数量，减少计算量。

2. 粒子滤波（MCL）的基本原理

粒子滤波是通过一组粒子

{

x

t

[

m

]

}

m

=

1

M

\\{x_t^{[m]}\\}_{m=1}^M

{xt[m]​}m=1M​ 来近似位置的概率分布

p

(

x

t

∣

z

1

:

t

,

u

1

:

t

)

p(x_t|z_{1:t}, u_{1:t})

p(xt​∣z1:t​,u1:t​)。

粒子滤波步骤：

3. AMCL 的核心改进：KLD-Sampling

标准 MCL 缺点：

• 粒子数固定，计算量大
• 如果粒子数太少 → 定位精度差
• 如果粒子数太多 → 浪费计算资源

AMCL 引入自适应采样（KLD-sampling）： 根据当前估计的 位置分布复杂度 动态调整粒子数。

3.1 KLD-Sampling 原理

使用 Kullback–Leibler Divergence（KL散度）来约束近似分布和真实分布的差距。

KL 散度定义：

D

K

L

(

p

∣

∣

q

)

=

∑

x

p

(

x

)

log

⁡

p

(

x

)

q

(

x

)

D_{KL}(p||q) = \\sum_{x} p(x) \\log \\frac{p(x)}{q(x)}

DKL​(p∣∣q)=x∑​p(x)logq(x)p(x)​

AMCL 要求：

• 在置信度 $1-\delta$
• 近似误差不超过 $ϵ$

推导后得到粒子数上界公式：

M

=

k

−

1

2

ϵ

[

1

−

2

9

(

k

−

1

)

+

2

9

(

k

−

1

)

z

1

−

δ

]

3

M = \\frac{k-1}{2\\epsilon} \\left[1 – \\frac{2}{9(k-1)} + \\sqrt{\\frac{2}{9(k-1)}} z_{1-\\delta} \\right]^3

M=2ϵk−1​[1−9(k−1)2​+9(k−1)2​

​z1−δ​]3

其中：

• $k$
• $z_{1-\delta }$

粒子数

M

M

M 会根据观测和分布稀疏程度动态调整。

4. AMCL 数学推导流程

4.1 贝叶斯定位公式

机器人定位问题可用递归贝叶斯滤波表示：

p

(

x

t

∣

z

1

:

t

,

u

1

:

t

)

=

η
 

p

(

z

t

∣

x

t

,

m

)

∫

p

(

x

t

∣

u

t

,

x

t

−

1

)
 

p

(

x

t

−

1

∣

z

1

:

t

−

1

,

u

1

:

t

−

1

)
 

d

x

t

−

1

p(x_t|z_{1:t}, u_{1:t}) = \\eta \\, p(z_t|x_t, m) \\int p(x_t|u_t, x_{t-1}) \\, p(x_{t-1}|z_{1:t-1}, u_{1:t-1}) \\, dx_{t-1}

p(xt​∣z1:t​,u1:t​)=ηp(zt​∣xt​,m)∫p(xt​∣ut​,xt−1​)p(xt−1​∣z1:t−1​,u1:t−1​)dxt−1​

4.2 粒子滤波近似

用粒子集合近似：

p

(

x

t

∣

z

1

:

t

,

u

1

:

t

)

≈

∑

m

=

1

M

w

t

[

m

]

δ

(

x

t

−

x

t

[

m

]

)

p(x_t|z_{1:t}, u_{1:t}) \\approx \\sum_{m=1}^M w_t^{[m]} \\delta(x_t – x_t^{[m]})

p(xt​∣z1:t​,u1:t​)≈m=1∑M​wt[m]​δ(xt​−xt[m]​)

4.3 权重更新公式

激光雷达测量模型：

w

t

[

m

]

∝

exp

⁡

(

−

1

2

σ

2

∑

i

=

1

K

[

z

t

i

−

z

^

(

x

t

[

m

]

,

m

)

]

2

)

w_t^{[m]} \\propto \\exp\\left(-\\frac{1}{2\\sigma^2} \\sum_{i=1}^K \\left[ z_t^i – \\hat{z}(x_t^{[m]}, m) \\right]^2 \\right)

wt[m]​∝exp(−2σ21​i=1∑K​[zti​−z^(xt[m]​,m)]2)

其中：

• $z_t^i$
• $\hat{z}(\cdot )$

4.4 自适应采样条件

当粒子数量满足 KLD-sampling 公式时停止采样，否则继续采样更多粒子。

5. AMCL 算法流程图

┌──────────────┐
│ 初始化粒子集 │
└──────┬───────┘
↓
┌──────────────┐
│ 运动更新(Pred)│ ← 里程计数据
└──────┬───────┘
↓
┌──────────────┐
│ 测量更新(Upd) │ ← 激光雷达数据
└──────┬───────┘
↓
┌──────────────┐
│ 权重归一化 │
└──────┬───────┘
↓
┌──────────────┐
│ KLD采样计算 │
└──────┬───────┘
↓
┌──────────────┐
│ 重采样 │
└──────┬───────┘
↓
┌──────────────┐
│ 输出位姿估计 │
└──────────────┘

6. AMCL 的优缺点

优点：

• 粒子数自适应，节省计算
• 能处理全局定位与跟踪
• 对传感器噪声有鲁棒性

缺点：

• 初始全局定位仍然耗时
• 对运动模型和测量模型依赖大
• 地图不准确时会漂移

7. Python 简易 AMCL 示例

import numpy as np

class SimpleAMCL:
def __init__(self, num_particles, map_size):
self.num_particles = num_particles
self.particles = np.random.rand(num_particles, 3) * map_size # x, y, theta
self.weights = np.ones(num_particles) / num_particles

def motion_update(self, u, noise_std=0.1):
self.particles[:, 0] += u[0] + np.random.randn(self.num_particles) * noise_std
self.particles[:, 1] += u[1] + np.random.randn(self.num_particles) * noise_std
self.particles[:, 2] += u[2] + np.random.randn(self.num_particles) * noise_std

def measurement_update(self, z, z_pred, sigma=0.5):
error = z – z_pred(self.particles)
self.weights = np.exp(–0.5 * (error / sigma)**2)
self.weights /= np.sum(self.weights)

def resample(self):
indices = np.random.choice(self.num_particles, self.num_particles, p=self.weights)
self.particles = self.particles[indices]
self.weights.fill(1.0 / self.num_particles)

def estimate(self):
return np.average(self.particles, axis=0, weights=self.weights)

# 使用示例
if __name__ == "__main__":
amcl = SimpleAMCL(500, map_size=10)
amcl.motion_update((0.5, 0, 0.1))
amcl.measurement_update(z=5.0, z_pred=lambda p: p[:, 0])
amcl.resample()
print("估计位置：", amcl.estimate())

8.参考文献

1. 原始论文与开创性工作

2. 机器人学与概率机器人学经典教材

3. ROS 与工程实现相关资料

4. 扩展与改进性论文