粒子滤波简介

最新推荐文章于 2022-12-15 14:21:44 发布

Young_Gy

最新推荐文章于 2022-12-15 14:21:44 发布

阅读量6.1k

点赞数 4

文章标签：粒子滤波

本文链接：https://blog.csdn.net/Young_Gy/article/details/78645921

版权

图片名称

粒子滤波基于蒙特卡洛方法，用后验概率中随机抽取的粒子集对目标概率密度函数进行近似。本文将简要介绍如何用粒子滤波进行定位并附上相关代码实例。

粒子滤波概述

粒子滤波，和卡尔曼滤波、一维马尔科夫定位都是贝叶斯滤波的一种方法。其最大特点是原理与实现特别简单。

其核心思想是：用很多个粒子代表定位物体，每个粒子有权重 $w$ 代表该粒子位置的可信度。在prediction阶段，根据物体的控制信息 $u$ （速度、转角等）与motion model预测出每个粒子下个时刻的位置；在update阶段，根据物体的观测值 $z$ 与地图值 $z_l$ 计算出每个粒子的权重 $w$ ；在resample阶段，根据粒子的 $w$ 重新采样粒子。这样，粒子的位置会越来越趋近真实的物体位置。

其步骤图如下，包含下面几个步骤：

初始化：用gps初始化粒子群的位置
prediction：根据motion model与物体的控制信息 $u$ 预测下个时刻粒子群中粒子的位置
update：根据物体的观测值 $z$ 与地图值 $z_l$ 计算出每个粒子的权重 $w$
resample：根据粒子的 $w$ 重新采样粒子

这里写图片描述

其伪代码如下：

这里写图片描述

下面，将分阶段具体介绍粒子滤波。

Motion Model

在介绍粒子滤波之前，需要对定位的物体进行建模，我们定位的物体是汽车，使用的模型是Bicycle Model。

这里写图片描述

Bicycle Model假定汽车的四个轮子可以简化成一个轮子，类似自行车故得此名。

初始化

初始化较为简单，根据GPS信息对每个粒子初始化即可，需要注意的是初始化的时候需要考虑到GPS的定位误差。

void ParticleFilter::init(double x, double y, double theta, double std[]) {
    // TODO: Set the number of particles. Initialize all particles to first position (based on estimates of
    //   x, y, theta and their uncertainties from GPS) and all weights to 1.
    // Add random Gaussian noise to each particle.
    // NOTE: Consult particle_filter.h for more information about this method (and others in this file).
    cout<<"Init Start"<<endl;

    num_particles = 100;


    /*******************************************************
     * Set particles
     ******************************************************/
    double std_x = std[0];
    double std_y = std[1];
    double std_theta = std[2];

    // set random engine
    normal_distribution<double> dist_x(x, std_x);
    normal_distribution<double> dist_y(y, std_y);
    normal_distribution<double> dist_theta(theta, std_theta);

    for (int i = 0; i < num_particles; ++i) {
        Particle curr_particle;
        curr_particle.id = i;
        curr_particle.x = dist_x(gen);
        curr_particle.y = dist_y(gen);
        curr_particle.theta = dist_theta(gen);
        curr_particle.weight = 1.0;
        particles.push_back(curr_particle);
    }

    /******************************************************
     * Set others
     ******************************************************/
    weights.assign(num_particles, 1.0);
    is_initialized = true;

    cout<<"Weights Size: "<<weights.size()<<endl;
    cout<<"Particles Size: "<<particles.size()<<endl;
    cout<<"Init Finished"<<endl;
}

Prediction

Prediction阶段，即：

x t + 1 = f (x t, u t)

$x_{t+1} = f(x_t, u_t)$

此处根据yaw_rate是否为0分为两种情况，分别是：

这里写图片描述

代码如下：

void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) {
    // TODO: Add measurements to each particle and add random Gaussian noise.
    // NOTE: When adding noise you may find std::normal_distribution and std::default_random_engine useful.
    //  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
    //  http://www.cplusplus.com/reference/random/default_random_engine/


    double std_x = std_pos[0];
    double std_y = std_pos[1];
    double std_theta = std_pos[2];
    normal_distribution<double> dist_x(0, std_x);
    normal_distribution<double> dist_y(0, std_y);
    normal_distribution<double> dist_theta(0, std_theta);


    for (int i = 0; i < num_particles; ++i) {
        // main
        if (fabs(yaw_rate)>0.001){
            double theta0 = particles[i].theta;
            particles[i].x += velocity/yaw_rate * ( sin(theta0+yaw_rate*delta_t) - sin(theta0) );
            particles[i].y += velocity/yaw_rate * ( -cos(theta0+yaw_rate*delta_t) + cos(theta0) );
            particles[i].theta += yaw_rate * delta_t;
        } else {
            particles[i].x += velocity * delta_t * cos(particles[i].theta);
            particles[i].y += velocity * delta_t * sin(particles[i].theta);
        }
        // add random noise
        particles[i].x += dist_x(gen);
        particles[i].y += dist_y(gen);
        particles[i].theta += dist_theta(gen);
    }

}

Update

更新粒子权重的依据是粒子的观测值与地图标志物相似度的高低，越高的话该粒子的权重越大。可是，该过程存在如下问题：

粒子观测值的坐标系是以粒子为中心的，粒子前进方向为x轴的坐标系，不同与地图标志物的坐标系。需要统一到地图坐标系。
地图的标志物太多，只需要考虑粒子传感器范围内的标志物即可。
粒子观测值与地图标志物需要建立一一匹配的关系。
更新粒子的权重。

下面，将针对每点分开介绍。

坐标变换

坐标变换，需要求解的问题如下：

(x m a p, y m a p) = f (x p a r t i c l e, y p a r t i c l e, θ p a r t i c l e, x o b s, y o b s)

$(x_{map},y_{map}) = f(x_{particle}, y_{particle}, \theta_{particle}, x_{obs}, y_{obs})$

在求解该问题之前，引入坐标变换的相关公式：

这里写图片描述

对于上图中的坐标 $x,y$ 到坐标 $x', y'$ ，其变换公式如下：

x' y' = x cos (θ) + y sin (θ) = - x sin (θ) + y cos (θ)

$\begin{split} x' &= x\cos(\theta) +y\sin(\theta) \\ y' &= -x\sin(\theta) +y\cos(\theta) \end{split}$

对于粒子滤波器中的坐标变换，需要将小车的观测值变换到地图坐标系下粒子的观测值。

记粒子的方向顺时针离水平位置的角度是 $\theta$ ，那么从粒子的坐标系到地图的坐标系变换的角度为 $- \theta$ ，所以有：

x' y' = x cos (θ) - y sin (θ) = x sin (θ) + y cos (θ)

$\begin{split} x' &= x\cos(\theta) - y\sin(\theta) \\ y' &= x\sin(\theta) +y\cos(\theta) \end{split}$

经过上述公式变换后的坐标是以粒子为圆心，在地图坐标系下的坐标，考虑到粒子的坐标，经过平移变换后可得：

x' y' = x cos (θ) - y sin (θ) + x p = x sin (θ) + y cos (θ) + y p

$\begin{split} x' &= x\cos(\theta) - y\sin(\theta) + x_p\\ y' &= x\sin(\theta) +y\cos(\theta) + y_p \end{split}$

这里写图片描述

以上图为例，OBS1变换后的坐标为：(6，3)。

选择传感器范围内的标识物

获得粒子在地图坐标系下的观测值后，需要对地图中的标志物进行筛选，筛选的标准是标志物到粒子的距离需要小于传感器的感受距离。

Association

经过上面对观测值的坐标变换以及传感器范围内的筛选，对于每个粒子可以获得其观测标志物在地图坐标系下的坐标值以及地图标志物本身的坐标值，下一个问题就是怎么将这两项坐标值两两匹配。

比较简单的方法是，对于每个观测值，选择离其最近的标志物进行匹配即可。该方法的优缺点如下：

这里写图片描述

Update

对观测值与地图真实的标志物两两匹配后，下一个阶段是计算匹配是否可信，也就是对每个粒子计算其权重 $w$ 。

一个粒子可能包含多个观测值，每个观测值服从以匹配标志物为中心的多元高斯分布。具体地：

P (x, y) = 1 2 π σ x σ y exp - (( x - μ x ) 2 2 σ 2 x + ( y - μ y ) 2 2 σ 2 y)

$P(x,y) = \frac{1}{2\pi \sigma_x \sigma_y} \exp^{-(\frac{(x-\mu_x)^2}{2\sigma_x^2}+\frac{(y-\mu_y)^2}{2\sigma_y^2})}$

w = \prod P (x, y)

$w = \prod P(x,y)$

代码

void ParticleFilter::dataAssociation(std::vector<LandmarkObs> predicted, std::vector<LandmarkObs>& observations) {
    // TODO: Find the predicted measurement that is closest to each observed measurement and assign the
    //   observed measurement to this particular landmark.
    // NOTE: this method will NOT be called by the grading code. But you will probably find it useful to
    //   implement this method and use it as a helper during the updateWeights phase.


    const double BIG_NUMBER = 1.0e99;

    for (int i = 0; i < observations.size(); i++) {

        int current_j, id_j;
        double current_smallest_error = BIG_NUMBER;

        for (int j = 0; j < predicted.size(); j++) {

            const double dx = predicted[j].x - observations[i].x;
            const double dy = predicted[j].y - observations[i].y;
            const int id_j = predicted[j].id;
            const double error = dx * dx + dy * dy;

            if (error < current_smallest_error) {
                current_j = j;
                current_smallest_error = error;
            }
        }
        observations[i].ii = current_j;
        observations[i].id = id_j;
    }

}


void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
                                   const std::vector<LandmarkObs> &observations, const Map &map_landmarks) {
    // TODO: Update the weights of each particle using a mult-variate Gaussian distribution. You can read
    //   more about this distribution here: https://en.wikipedia.org/wiki/Multivariate_normal_distribution
    // NOTE: The observations are given in the VEHICLE'S coordinate system. Your particles are located
    //   according to the MAP'S coordinate system. You will need to transform between the two systems.
    //   Keep in mind that this transformation requires both rotation AND translation (but no scaling).
    //   The following is a good resource for the theory:
    //   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
    //   and the following is a good resource for the actual equation to implement (look at equation
    //   3.33
    //   http://planning.cs.uiuc.edu/node99.html

    // iterate particles
    weights.clear();
    for (int i = 0; i < num_particles; ++i) {

        // get particle infomation
        double x_p = particles[i].x;
        double y_p = particles[i].y;
        double theta = particles[i].theta;

        // clear
        particles[i].associations.clear();
        particles[i].sense_x.clear();
        particles[i].sense_y.clear();

        /*****************************************************
         * Step 1: coordinate system change from VEHICLE'S to MAP'S
         ****************************************************/
        vector<LandmarkObs> map_observations;
        for (int j = 0; j < observations.size(); ++j) {
            double x_o = observations[j].x;
            double y_o = observations[j].y;

            double x_m, y_m;
            x_m = x_p + cos(theta) * x_o - sin(theta) * y_o;
            y_m = y_p + sin(theta) * x_o + cos(theta) * y_o;

            LandmarkObs observation = {
                    observations[j].id,
                    0,
                    x_m,
                    y_m
            };

            map_observations.push_back(observation);
        }

        /*****************************************************
         * Step 2: choose landmarks which has the distance with particle less than sensor_range
         ****************************************************/
        vector<LandmarkObs> effect_landmark_list;
        for (int k=0; k < map_landmarks.landmark_list.size(); ++k) {
            float x_l_i = map_landmarks.landmark_list[k].x_f;
            float y_l_i = map_landmarks.landmark_list[k].y_f;
            int id_l_i = map_landmarks.landmark_list[k].id_i;
            double dist = sqrt( pow(x_p-x_l_i, 2) + pow(y_p-y_l_i, 2) );
            if (dist < sensor_range){
                LandmarkObs curr_landmark = {
                        id_l_i,
                        0,
                        x_l_i,
                        y_l_i
                };
                effect_landmark_list.push_back(curr_landmark);
            }
        }


        /*****************************************************
         * Step 3: associate landmarks in range to landmarks obeservations
         ****************************************************/
        ParticleFilter::dataAssociation(effect_landmark_list, map_observations);

        /*****************************************************
         * Step 4: update weight
         ****************************************************/
        double weight_p = 1.0;
        if (effect_landmark_list.size()==0){
            weight_p = 0.0;
        } else{
            for (int j = 0; j < map_observations.size(); ++j) {
                // obeseration im map coordinate
                double x_m = map_observations[j].x;
                double y_m = map_observations[j].y;
                // landmark im map coordinate
                double x_l = effect_landmark_list[map_observations[j].ii].x;
                double y_l = effect_landmark_list[map_observations[j].ii].y;

                double std_x = std_landmark[0];
                double std_y = std_landmark[1];
                double p_one_landmark = 1/(2*M_PI*std_x*std_y)*exp(-( pow(x_m-x_l,2)/(2*std_x*std_x) + pow(y_m-y_l,2)/(2*std_y*std_y) ));

                weight_p *= p_one_landmark;

                particles[i].associations.push_back(map_observations[j].id);

                // !!!!! I need help in follow two lines, please help me!!!!!
                // !!!!! I cannot run my program when I uncomment the follow two lines. Why and How can I solve it?
                //particles[i].sense_x.push_back(x_m);
                //particles[i].sense_y.push_back(y_m);
            }
        }

        // update particle
        particles[i].weight = weight_p;
        weights.push_back(weight_p);
    }
}

Resample

Update阶段获得每个粒子的权重后，根据权重重新筛选粒子即可，代码如下：

void ParticleFilter::resample() {
    // TODO: Resample particles with replacement with probability proportional to their weight.
    // NOTE: You may find std::discrete_distribution helpful here.
    //   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution
    discrete_distribution<int> index (weights.begin(), weights.end());

    std::vector<Particle> new_particles;
    for (int i = 0; i < num_particles; ++i) {
        new_particles.push_back(particles[index(gen)]);
    }
    particles = new_particles;

}