粒子滤波在车辆定位中的应用

粒子滤波算法流程

粒子滤波算是一个非常经典的算法了，具体的思想步骤不再详述，这里和这里讲的都比较详细了。
下图就是算法的伪代码，分为以下几个步骤：

• 第一行是算法的初始化，将在这一步根据GPS的数据进行一个粗略的位置估计。
• 第三行是预测，为所有的粒子添加yaw rate和velocity
• 四五行是更新，更新每个粒子的权重（权重就是之后resample的依据）
• 最后一个for循环就是重采样，权重大的点更有可能被选中

下图是粒子滤波车辆定位算法的流程图。

初始化

在车辆定位问题中，不能将所有点的分布视为平均分布，比较好的办法就是根据GPS的信息，并考虑到传感器的高斯噪声来随机撒点。具体的做法就是将GPS读取的信息作为均值，传感器高斯噪声作为方差，让所有点服从这两个参数下的正态分布，代码如下：

void ParticleFilter::init(double x, double y, double theta, double std[]) {
    default_random_engine gen;
    normal_distribution<double> dist_x(x, std[0]);
    normal_distribution<double> dist_y(y, std[1]);
    normal_distribution<double> dist_theta(theta, std[2]);
    
    num_particles = NUM_PARTICLES;  // TODO: Set the number of particles
    particles.resize(num_particles);
    
    for(auto &p : particles){
        p.x = dist_x(gen);
        p.y = dist_y(gen);
        p.theta = dist_theta(gen);
        p.weight = 1;
    }
    is_initialized = true;

}

预测

更新需要根据车辆的运动学方程，匀速运动的车不必多说，单位时间内的位置更新就是简单的速度*夹角正弦或者余弦。如果是变速运动，那么用如下公式计算：

当然，我们仍然需要叠加上高斯噪声，具体代码如下：

void ParticleFilter::prediction(double delta_t, double std_pos[], 
                                double velocity, double yaw_rate) {
    default_random_engine gen;
    
    // 更新部分叠加高斯噪声。后续要加到x,y,theta上
    normal_distribution<double> new_x(0, std_pos[0]);
    normal_distribution<double> new_y(0, std_pos[1]);
    normal_distribution<double> new_theta(0, std_pos[2]);
    
    for(auto &p : particles){
        // 这里需要判断是加速运动还是匀速运动
        if(fabs(yaw_rate) < 0.0001){
            p.x += velocity * delta_t * cos(p.theta);
            p.y += velocity * delta_t * sin(p.theta);
        }else{
            p.x += velocity / yaw_rate * (sin(p.theta + yaw_rate * delta_t) - sin(p.theta));
            p.y += velocity / yaw_rate * (cos(p.theta) - cos(p.theta + yaw_rate*delta_t));
            p.theta += yaw_rate * delta_t;
        }
        // 最后叠加上高斯噪声
        p.x += new_x(gen);
        p.y += new_y(gen);
        p.theta += new_theta(gen);
    }
}

更新

在更新之前，我们还需要解决一个问题。众所周知无人驾驶中，我们通常通过lidar或者radar等传感器获取位置信息，那么这些传感器就一定会存在误差。以lidar为例，如下所示，蓝色点表示物体的位置，黄色点表示lidar检测返回的点云数据点。那么我们如何确定哪一个雷达点返回的是我们定位所需的landmark所反射回来的呢？

一个很简单的方法就是用最邻近点表示，也就是图中红色圈出的部分，用检测到的最邻近的点表示landmark所在位置。这样做的好处毫无疑问就是理解简单，编程简单，并且能解决大部分场景的问题。但是问题也是突出的，那就是鲁棒性不够，在landmark周围点云密集时，当传感器存在较大噪声时等场景都会出现误差较大的情况。以下是代码实现：

void ParticleFilter::dataAssociation(vector<LandmarkObs> predicted, 
                                     vector<LandmarkObs>& observations) {
    for(auto &obs : observations){
        double minD = numeric_limits<float>::max();
        
        for(const auto &pred : predicted){
            double distance = dist(obs.x, obs.y, pred.x, pred.y);
            if(minD > distance){
                minD = distance;
                obs.id = pred.id;
            }
        }
    }

}

下面就是更新权重的步骤了，那么权重如何计算呢，可以利用多元高斯公式，如下：

代码的总体思路就是针对每一个粒子，首先找到距离范围内可用的landmark，再利用最近点法找到相应的测量值，用测量值和真值利用多元高斯公式计算权重进行更新。其中还要注意的是，测量的坐标系是车，也就是行进方向为x，左为y；landmark坐标系是地图坐标系，要进行转换。

void ParticleFilter::updateWeights(double sensor_range, double std_landmark[], 
                                   const vector<LandmarkObs> &observations, 
                                   const Map &map_landmarks) {
    for(auto &p : particles){
        p.weight = 1.0;
        
        // 找到范围内的landmark
        vector<LandmarkObs> predictions;
        for(const auto &landmark : map_landmarks.landmark_list){
            if(dist(p.x, p.y, landmark.x_f, landmark.y_f) < sensor_range){
                predictions.push_back(LandmarkObs{landmark.id_i, landmark.x_f, landmark.y_f});
            }
        }
        
        // 车坐标系和map坐标系转换
        vector<LandmarkObs> observations_map;
        double cos_theta = cos(p.theta);
        double sin_theta = sin(p.theta);
        
        for(const auto &obs : observations){
            LandmarkObs tmp;
            tmp.x = obs.x * cos_theta - obs.y * sin_theta + p.x;
            tmp.y = obs.x * sin_theta + obs.y * cos_theta + p.y;
            observations_map.push_back(tmp);
        }
        
        dataAssociation(predictions, observations_map);
        
        for(const auto &obs_m : observations_map){
            Map::single_landmark_s landmark = map_landmarks.landmark_list.at(obs_m.id-1);
            double x_tmp = pow(obs_m.x - landmark.x_f, 2) / (2 * pow(std_landmark[0], 2));
            double y_tmp = pow(obs_m.y - landmark.y_f, 2) / (2 * pow(std_landmark[1], 2));
            double w = exp(-(x_tmp + y_tmp)) / (2 * M_PI * std_landmark[0] * std_landmark[1]);
            p.weight *= w;
        }
        weights.push_back(p.weight);
    }

}

重采样

最后来到了重采样这一步，这一步思想上很简单，那就是更新粒子群的集合，更新的准则就是权重大的有更大的机会被选入。c++中的discrete_distribution可以很好地实现这一目的。代码如下：

void ParticleFilter::resample() {
    // generate distribution according to weights
    random_device rd;
    mt19937 gen(rd());
    discrete_distribution<> dist(weights.begin(), weights.end());

    // create resampled particles
    vector<Particle> resampled_particles;
    resampled_particles.resize(num_particles);

    // resample the particles according to weights
    for(int i=0; i<num_particles; i++){
      int idx = dist(gen);
      resampled_particles[i] = particles[idx];
    }

    // assign the resampled_particles to the previous particles
    particles = resampled_particles;

    // clear the weight vector for the next round
    weights.clear();
}

运行结果

可以看到粒子滤波很好地实现了二维平面上小车的定位问题。