无人驾驶技术入门（二十）| 手把手教你用粒子滤波实现无人车定位

前言

在《无人驾驶技术入门（十八）| 手把手教你写扩展卡尔曼滤波器》和《无人驾驶技术入门（十九）| 手把手教你实现多传感器融合技术》中，我以激光雷达和毫米波雷达数据，对障碍物进行目标跟踪为例，介绍了卡尔曼滤波器（KF）、扩展卡尔曼滤波器（EKF）在无人驾驶中的应用。卡尔曼滤波器能够很好地解决线性的状态估计问题；在遇到非线性的状态估计问题时，可以利用扩展卡尔曼滤波器将非线性问题线性化，只是会有一些精度损失。

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

本文将以优达学城（Udacity）无人驾驶工程师学位中的定位课程为例讲解粒子滤波如何解决无人驾驶中的定位问题。

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

链接：pan.baidu.com/s/1HVVvlw

提取码：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[]) {
    num_particles = 100;  // Set the number of particles
    particles.resize(num_particles);

    std::default_random_engine gen;
    std::normal_distribution<double> dist_x(x, std[0]);
    std::normal_distribution<double> dist_y(y, std[1]);
    std::normal_distribution<double> dist_theta(theta, std[2]);

    for (size_t i = 0; i < num_particles; ++i) {
        particles.at(i).x = dist_x(gen);
        particles.at(i).y = dist_y(gen);
        particles.at(i).theta = dist_theta(gen);
        particles.at(i).weight = 1.0;
    }

    is_initialized = true;
}

代码：预测

我曾在介绍KF和EKF的文章中讨论过障碍物的预测，当时讨论的模型是[x,y,x',y']，相对简单，仅涉及位置和速度，并不涉及航向角的估计。而在进行无人车定位时，航向是一个比较重要的状态量，因此需要重新选择模型。

在无人车的目标跟踪、状态估计领域中有一个较为常用的模型——CTRV Model（Constant Turn Rate and Velocity Magnitude Model，即恒定转向速率和速度模型）。

CTRV模型的状态方程为[px, py, v, yaw, yaw_rate]，其中px, py表示无人车在坐标系中的横纵向坐标，v表示的是无人车的行驶速度，其方向为运动轨迹（图中蓝线）的切线方向，yaw表示的是无人车的航向角，yaw_rate表示的是航向角的变化率。

CTRV模型规定了在极短时间（前后两帧观测时间）内行驶速度v和航向角的变化率yaw_rate保持不变，这意味着，在连续两帧之间，可以假设无人车是在做圆周运动的。

k时刻到k+1时刻的状态方程变化如下所示，其中△t是两个时间所经过的时间。为了简化表示，我将yaw用φ表示，yaw_rate用φ的导数表示，入下所示：

因为行驶速度v和航向角的变化率φ'为常量，v的导数和φ''均为0，用已知量将上面的式子重新表示：

其中 △t 为 k时刻 与 k+1时刻 的时间差

由于CTRV模型的预测过程推导过程会占用一定的篇幅，且不是本次分享的重点，我直接给出结论：

1.当φ'为0，即速度、航向角变化率、航向角均不发生变化，汽车此时沿直线行驶，只有位置发生变化：

2.当φ'不为0，k和k+1的时间段内汽车做圆周运动，其状态方程变化如下：

如果对状态方程的推导感兴趣，可以通过以下链接下载。

链接：pan.baidu.com/s/125i9fS

提取码：bs5w

代码部分声明了一个prediction函数，该函数需要输入4个参数，第一个参数delta_t为连续两帧的时间差，也就是公式中的△t ，第二个参数std_pos[]为预测时所引入的预测噪声，第三个参数velocity为k时刻的车速v，第四个参数为k时刻的yaw_rate。其中v和yaw_rate为k时刻车载传感器观测到的数据（车速、航向角变化率），是已知量。

void ParticleFilter::prediction(double delta_t, double std_pos[], 
                                double velocity, double yaw_rate) {
    std::default_random_engine gen;
    std::normal_distribution<double> noisy_x(0, std_pos[0]);
    std::normal_distribution<double> noisy_y(0, std_pos[1]);
    std::normal_distribution<double> noisy_theta(0, std_pos[2]);

    for (size_t i = 0; i < num_particles; ++i) {
         double x0 = particles.at(i).x;
        double y0 = particles.at(i).y;
        double theta0 = particles.at(i).theta;

        double x_pre = 0.0;
        double y_pre = 0.0;
        double theta_pre = 0.0;

        if (fabs(yaw_rate) < 0.00001) {
            x_pre = x0 + velocity * delta_t * cos(theta0);
            y_pre = y0 + velocity * delta_t * sin(theta0);
            theta_pre = theta0;
        } else {
            x_pre = x0 + velocity / yaw_rate * (sin(theta0 + yaw_rate * delta_t) - sin(theta0));
            y_pre = y0 + velocity / yaw_rate * (cos(theta0) - cos(theta0 + yaw_rate * delta_t));
            theta_pre = theta0 + yaw_rate * delta_t;
        }
        // 统一角度至0-2π
        while (theta_pre > 2 * M_PI) theta_pre -= 2. * M_PI;
        while (theta_pre < 0.0) theta_pre += 2. * M_PI;

        particles.at(i).x = x_pre + noisy_x(gen);
        particles.at(i).y = y_pre + noisy_y(gen);
        particles.at(i).theta = theta_pre + noisy_theta(gen);
    }
}

代码：更新粒子权重

从代码的角度更新粒子权重要分为如下几步。一、将车体坐标系下观测到的路标转换到平面坐标系下；二、将统一到平面坐标系下的观测路标与真实路标进行匹配（数据关联）；三、根据匹配的结果重新计算粒子的权重。

坐标转换

根据上述预测原理，我们得到了k+1时刻每个粒子的位置，同时在k+1时刻传感器对路标进行了一次观测，观测结果所在的坐标系为车体坐标系。

接下来，我们需要对每个预测后的粒子的权重进行更新。在前面的感性分析中，我们知道，更新权重前需要将观测到的路标与地图中的路标进行一一配对，也就是“匹配”，而“匹配”的前提是参与匹配的路标都是在同一坐标系下的。

目前地图上路标的参考坐标系是平面坐标系，车载传感器观测的路标的参考坐标系是车体坐标系，因此，首先需要将这两个坐标系统一。如下所示，我们用XmYm表示平面坐标系（m表示map），用XcYc表示车体坐标系（c表示car）。其中(xp,yp,θ)表示预测后的粒子在平面坐标系（map）中的位置和朝向，(xc,yc)表示在车体坐标系下观测到的路标位置。

现在需要知道车体坐标系下观测到的路标(xc,yc)，在平面坐标系下如何表示。

坐标转换的基本思路是先进行旋转变换，再进行平移。

我们先忽略平移，计算旋转变换后的xm如何用xc,yc以及θ表示，随后再计算ym。

如下所示，将平面坐标系的坐标原点与车体坐标系原点重合，随后做出辅助线求xm。

可以得到

同理，将平面坐标系的坐标原点与车体坐标系原点重合，随后做出辅助线求ym。

可以得到

以上是旋转变换的计算，随后我们加上平移，即可得到最终的坐标转换关系：

使用C++实现坐标转换的代码如下所示，其中observations表示的是k+1时刻无人车观测到的所有路标集合，obs_in_map用于存储坐标转换后，观测的路标在平面坐标系下的位置。

数据关联

将无人车观测到的路标转到平面坐标系后，我们将它们与高精地图上的路标进行“数据关联”，也就是找出观测到的路标与地图中实际存在的路标的对应关系。

路标间的数据关联可以看成点与点之间的关联。最简单也最常用的关联方法叫做“最近邻”，顾名思义，就是根据距离，找到离当前点最近的点作为关联点。

代码部分为我们声明了一个dataAssociation函数，该函数需要输入2个参数，predicted表示已知的地图上的路标集，每个路标都有平面坐标系下的位置x和y，以及独一无二的id；obvservations表示经过坐标转换后的观测路标，经过数据关联后，每个观测到的路标会与实际的路标通过id进行绑定。

数据关联部分的代码实现并不复杂，如下所示。通过多次循环，找到距离最小的路标，并将该路标的id赋给观测的路标，以此建立关联关系。

void ParticleFilter::dataAssociation(vector<LandmarkObs> predicted, 
                                     vector<LandmarkObs>& observations) {
    for (LandmarkObs& obs : observations) {
        double distance_min = std::numeric_limits<double>::max();
        for (int i = 0; i < predicted.size(); ++i) {
            LandmarkObs pre = predicted[i];
            double distance2 = (pre.x - obs.x) * (pre.x - obs.x) + (pre.y - obs.y) * (pre.y - obs.y);
            if (distance2 < distance_min) {
                distance_min = distance2;
                obs.id = pre.id;
            }
        }
    }
}

计算粒子权值

完成数据关联后，我们会发现每一个观测到的路标，都与真实世界中的某个路标有对应关系。至此，我们就可以根据他们之间的距离关系计算出一个权值，这个权值能够反映当前位置观测到的路标与真实世界中的路标匹配地是否足够好。

公式如下：

其中P(x,y)表示计算得到的匹配概率，x和y表示观测路标的位置，σx和σy表示的是观测与真实路标位置在x和y方向上的噪声，ux和uy表示的是真实世界中路标的位置。当x与ux、y与uy越接近，则P(x,y)的值也将越大。

计算粒子权值的代码部分，如下所示。首先根据数据关联的id找到真实世界中的路标，进而得到ux和uy，随后套用公式计算即可，有多少个观测路标就会连续乘以多少次权值，最终得到每个粒子对应的权重。

代码：重采样

完成每一个粒子的权重计算后，就需要根据权重的大小，删除权重小的粒子，同时在权重大的粒子附近抛洒新的粒子，以此保持整个粒子集的数量不变。

重采样可以采用C++提供的离散分布函数进行实现，离散分布是基于随机采样的原理。首先根据每个粒子的权重建一个离散分布集合，然后在这个集合上随机采样，权重越大，被采样的可能性也越高。

void ParticleFilter::resample() {
    /**
   * Resample particles with replacement with probability proportional
   *   to their weight.
   * You may find std::discrete_distribution helpful here.
   *   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution
   */
    std::random_device rd;
    std::mt19937 gen(rd());
    std::discrete_distribution<> d{std::begin(weights), std::end(weights)};

    // build resampled particles
    vector<Particle> resampled_particles(num_particles);
    for (size_t i = 0; i < num_particles; ++i) {
        resampled_particles[i] = particles[d(gen)];
    }

    particles = resampled_particles;

    weights.clear();
}

重采样完成后，又有了100个新的粒子，重复上面预测、坐标转换、数据关联、更新权重的过程，就可以让粒子群越来越集中，所求得的最高权重粒子与真值位置也就越接近。

到此，便完成了整个基于粒子滤波的无人车定位功能。完整版的工程代码可以通过以下链接获取。

链接：pan.baidu.com/s/19UVacw

提取码：glbp

将以上过程部署到工程代码中运行，并开启模拟器，便可以像如下视频中，完成无人车的定位了。视频中与蓝色小车接近重合的蓝色圈圈就是我们计算得到的无人车定位位置。

无人车定位之粒子滤波

结语

以上就是利用粒子滤波实现无人车定位从感性分析到理性实现的过程。

因为本文主要目的是介绍粒子滤波的原理及实现，因此对包括高精度地图、匹配算法等内容做了一些简化，实际的无人车定位时用到的定位元素除了路标外还包含车道线，交通信号灯等等，这时需要考虑的问题更多，算法也会更加复杂。

好了\(^o^)/~，本次关于粒子滤波的分享就到这了，如果你有任何疑问，可以在文章后面留言。

如果你觉得我写的还不错，就给我个赞吧，能关注一下就更好啦~