手把手教你用粒子滤波实现无人车定位

手把手教你用粒子滤波实现无人车定位

除了KF和EKF，还有另外一种经典的滤波算法，名为粒子滤波（Particle Filter）。粒子滤波的思想基于蒙特卡洛方法，利用粒子集来表示概率，可以用在任何形式的状态空间模型上。相比于其他滤波算法，粒子滤波在解决非线性、非高斯的问题上，有着较大的优越性。在无人驾驶领域，粒子滤波常被用于解决无人车的定位问题。

附赠自动驾驶最全的学习资料和量产经验：链接

优达学城提供的工程代码和模拟器的下载地址如下：

链接：https://pan.baidu.com/s/1HVVvlwgyNmu2AArQ1_e7_w

提取码：kcnk

---

正文

在正式介绍粒子滤波实现定位的理论之前，我们先以图示的方式，感性地了解一下它的工作原理。

地图输入

首先，我们有一张局部的高精度电子地图，在这个地图上有很多绿色的路标，对应的是高精度地图上的感兴趣点POI（Point of Interest）。

我们将当前的场景简化成一个平面坐标系，在该坐标系下选定某个参考点为原点Om（m代指map），Xm轴指向东，Ym轴指向北，在XmOmYm坐标下下，每个路标相对于原点均有唯一的坐标和ID。

一般情况下，高精度电子地图上的POI位置都是以经纬度的形式存储的，通过引入平面坐标系的方法即可将经纬度转化为局部坐标，常用于转换坐标的开源库有geolib、proj.4等，关于经纬度转局部坐标的理论不是本次分享的重点，这里不做过多阐述。后续的讨论都基于平面坐标系展开。

初始化

无人车定位系统初始化时，会收到GPS的初始观测信息，这些信息包括无人车在XmOmYm坐标系下的位置XY（图中黑点所在位置）和航向角θ（图中黑色箭头与X方向的夹角）。由于传感器本身的存在测量误差，无人车位置的真值应该在初始观测位置附近，即上图中的观测误差圈内。

大部分传感器的测量误差是符合高斯分布的，我们分别在初始观测的位置X、Y和航向角θ上加上高斯噪声，创造出N个粒子（图中橙色点）。初始创造时，每个粒子的权重都相等，值为100%。理论上，当N足够大时，总会有一个粒子的位置和航向角与真值一致，如果我们能够找到这个粒子，就能找到真值。这就是粒子滤波的基本思想。

预测

根据GPS的位置完成所有粒子的初始化后，无人车开始运动。在运动过程中，定位模块能够收到自车的车速v和航向角变化率yaw_rate等信息，进而可以推测出运动△t时间后，自车所在的位置。

根据车速、航向角变化率推算（预测）出自车位置的过程叫做里程计（Odometry）。除了可以使用车速和航向角变化率外进行位置推算外，IMU等传感器也可以实现类似的功能。

由于每个粒子都有可能是无人车的真实位置，因此我们对所有的粒子都进行预测（里程计推算），如下图所示。每个粒子有自己的初始位置和航向角，执行预测后，粒子群的分布也不再那么集中。

更新粒子权重

粒子预测完成后，我们需要对所有粒子的权重进行更新（所有粒子的初始权重都相等）。权重更新的依据是无人车对路标位置的感知。由于车载传感器是安装在车上的，感知到的路标位置都是基于车体坐标系的。

如下所示，(x0,y0)和(x1,y1)为传感器在车体坐标系下，探测到的两个路标。

我们将感知结果绘制到平面坐标系上（此过程包含了车体坐标系到平面坐标系的坐标转换）。为了方便读者查看，我这里只绘制三个比较典型的粒子所观测到的路标，如下所示，绿色的为地图上的路标，橙色粒子的观测结果，用橙色绘制，紫色和蓝色的同理。

从图上可以很直观地看出，在粒子O1、O2和O3中，传感器感知到的路标与实际中地图路标最匹配的是O1（橙色），其次是O2（蓝色），最后是O3（紫色），其中蓝色是由于位置原因导致的匹配性降低，紫色是由于航向原因导致的匹配性降低。

这也意味着经过权重更新后，所有的粒子由相同的权重，更新为权重：粒子O1>粒子O2>粒子O3，可以选择粒子O1作为无人车当前所在的位置。可以想象，当粒子数量足够多时，权重越高的粒子，其位置和航向就越接近真值。

重采样

利用前面提到的方法更新每个粒子的权重后，我们就要进行删除粒子和新增粒子的操作，这个删除和新增的过程就叫做重采样（Resample）。重采样的目的是，保持粒子总数不变的情况下，删除掉那些权重较低的粒子，同时在权重较高的粒子附近抛洒新的粒子。

随着重采样次数的增加，粒子的集中度会越来越高，留下来的都是极其接近真值的粒子。在这些粒子中筛选出权重最高的粒子，即为无人车的位置。

以上是从感性上介绍了粒子滤波算法在无人车定位中的应用，下面我们从理性上对粒子滤波这个过程进行公式推导和代码编写。

---

代码：地图输入

下载好前面提到的工程代码后，打开data目录，可以看到一个名为map_data.txt文件，该文件有42行，每一行表示一个路标（前文图中的绿色方块），每个路标拥有唯一的坐标和ID（ID从1开始依次累加）。地图文件提供的坐标不是经纬度，而是以米为单位的数值（也就是在平面坐标系下的位置）。

这些路标在模拟其中显示如下图中的⊕所示，有些路标跟蓝色小车之间有绿色的连线，这表示这些路标在车载传感器的感知范围内，且能够被检测到，蓝色小车代表着真实的定位位置（Groundtruth）。

代码：初始化

粒子滤波初始化需要一个初始的位置、初始的航向以及测量误差，根据这些参数，我们就可以建立N个粒子。

工程代码为我们建立了一个名为ParticleFilter的类，并声明了一个init函数，该函数需要输入4个参数，前3个是x，y和theta，也就是初始的位置和初始的航向，第4个参数为一个数组，这个数组存储的是x方向、y方向以及航向的测量误差。

首先设定粒子的数量，比如设置为100，随后使用for循环的方式新建粒子。新建粒子时，我们在初始位置和初始航向的基础上加入一个随机的高斯分布测量误差。新建粒子的过程如下代码所示：

void ParticleFilter::init(double x, double y, double theta, double std[