蒙特卡洛粒子滤波定位算法_自动驾驶定位技术粒子滤波实践

Particle Filter - Kidnapped vehicle project

1. Definition of Particle Filter

粒子滤波器是贝叶斯滤波器或马尔可夫定位滤波器的实现。粒子过滤器基于“适者生存的原理”主要用于解决定位问题。粒子滤波的优势在于易于编程并且灵活。 三种滤波器的性能对比:

正如你在上面的图片中看到的，红点是对机器人可能位置的离散猜测。每个红点都有 x 坐标、 y 坐标和方向。粒子滤波器是由几千个这样的猜测组成的机器人后验信度表示。一开始，粒子是均匀分布的，但过滤器使他们生存的比例正比于粒子与传感器测量的一致性。

• 权重(Weights)：

粒子滤波器通常携带离散数量的粒子。每个粒子都是一个包含 x 坐标、 y 坐标和方向的矢量。颗粒的存活取决于它们与传感器测量结果的一致性。一致性是基于实际测量和预测测量之间的匹配度来衡量的，这种匹配度称为权重。

权重意味着粒子的实际测量与预测测量的接近程度。在粒子滤波器中，粒子权重越大，生存概率越高。换句话说，每个粒子的生存概率与权重成正比。

• 重采样(Resampling)

重采样技术是用于从旧粒子中随机抽取N个新粒子，并根据重要权重按比例进行置换。重采样后，权重较大的粒子可能会停留下来，其他粒子可能会消失。粒子聚集在后验概率比较高的区域。 为了进行重采样，采用了重采样轮技术.

原理：每个粒子被选中的概率都和这个粒子轮所占的周长成正比，权重大的粒子有更多的机会被选中。 初始index为6，假设随机的beta= 0 + 随机权重> w6, 则index +1，beta=beta-w6. 此时beta < w7, 7号粒子被选中添加到仓库中。之后进行下一轮循环，此时beta 和 index 仍然保留前一轮循环的值, beta= beta + 随机权重 > w7 + w8, 因此index递增两次，到达index=1，此时w1 > beta, w1被选中放入仓库中, 随后进行下一轮循环。 重采样的代码：

p3 = []
index= int(random.random()*N)
beta=0.0
mw=max(w)
for i in range(N):
    beta +=random.random()*2.0*mw
    while beta>w[index]:
        beta-=w[index]
        index=(index+1)%N
    p3.append(p[index])
p=p3

2. Particle Filters implementation

粒子过滤器有四个主要步骤:

• 初始化步骤: 我们从 GPS 输入估计我们的位置。在这个过程中的后续步骤将完善这个估计，以定位我们的车辆

<