自动驾驶定位技术-粒子滤波实践

Particle Filter - Kidnapped vehicle project

Github: https://github.com/williamhyin/CarND-Kidnapped-Vehicle

Email: williamhyin@outlook.com

知乎专栏: 自动驾驶全栈工程师

1. Definition of Particle Filter

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

三种滤波器的性能对比:

Compare	State space	Belief	Efficiency	In robotic
Histogram filter	Discrete	Multi-modal	Exponential	approximate
Kalman filter	Continuous	Unimodal	Quadratic	approximate
Particle filter	Continuous	Multi-modal	?	approximate

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

1. 权重(Weights)：

   

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

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

2. 重采样(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()<