无人驾驶定位与贝叶斯滤波

无人驾驶需要精确的定位。本文将简要介绍无人驾驶定位的相关方法，重点介绍贝叶斯滤波框架进行递归的状态估计。同时附上一维马尔科夫定位的实例及代码。

无人驾驶定位

定位是指在空间中确定自己的位置。

传统的定位方法有GPS（Global Positioning System），但是GPS的误差较大且不稳定（m级）。无人驾驶对定位精度要求较高需要达到cm级的误差，因此需要多传感器融合定位。

定位的数学问题之贝叶斯滤波

定位的目标：记汽车的位置为x，定位即是求解P(x)。

那么，为了确定汽车的位置，我们有哪些数据呢？

• 地图数据m，包含地图上标志物的位置信息
• 观测数据z，包含汽车感知到的标志物与汽车的相对位置信息
• 控制数据u，包含汽车的油门转弯等控制信息

定位本身是一种位置不确定性的度量，我们的目标是尽量减少这种不确定性。那么，上面的数据信息是如何帮助我们减少不确定性的呢？

• Sense：P(x|z)∝P(x)P(z|x)
• Move：P(xt+1)=∑P(xt)P(xt+1|xt)

贝叶斯滤波是一种使用递归进行状态估计的框架。其交替利用Move阶段的prediction和Sense阶段的update，便可以对汽车的位置P(x)做更精准的描述。

一维马尔科夫定位

一维马尔科夫定位、卡尔曼滤波、粒子滤波都属于贝叶斯滤波的一种，这里将简要介绍一维马尔科夫定位。

数据

这里使用的数据包含以下三种，目的是要获得汽车在t时刻位置的置信为bel(xt)。

• 地图数据m，包含地图上标志物的位置信息
• 观测数据z，包含汽车感知到的标志物与汽车的相对位置信息
• 控制数据u，包含汽车的油门转弯等控制信息

动机

记汽车在t时刻位置的置信为bel(xt)，有

bel(xt)=P(xt|z1:t,u1:t,m)

这里额外提一下，如果求P(xt,m|z1:t,u1:t)，那么则从定位问题变成SLAM（simultaneous location and mapping）问题。

容易看到，随着t的增大，估计bel(xt)需要的数据越来越大。我们的目标是：

1. 减少估计所用的数据量
2. 需要的数据不随时间增加

也就是：

bel(xt)=f(bel(xt−1),zt,ut,m)

根据贝叶斯公式，可得：

bel(xt)=P(xt|z1:t,u1:t,m)=P(xt|zt,z1:t−1,u1:t,m)=P(xt|z1:t−1,u1:t,m)P(zt|xt,z1:t−1,u1:t,m)P(zt|z1:t−1,u1:t,m)

上面的公式主要包含两部分，下面将对这两部分分别求解：

1. motion model（prediction）：P(xt|z1:t−1,u1:t,m)
2. observation model（likelihood）：P(zt|xt,z1:t−1,u1:t,m)

Motion Model

根据马尔科夫假设，可得：

P(xt|z1:t−1,u1:t,m)=∫P(xt−1|z1:t−1,u1:t,m)P(xt|xt−1,z1:t−1,u1:t,m)dxt−1=∫P(xt−1|z1:t−1,u1:t−1,m)P(xt|xt−1,ut,m)dxt−1=∑ibel(xit−1)P(xt|xit−1,ut)

P(xt|xit−1,ut)被称为transition model，其满足：

P(xt|xit−1,ut)∼N(xt−xit−1:ut,σut)

Observation Model

根据马尔科夫假设，可得：

P(zt|xt,z1:t−1,u1:t,m)=P(zt|xt,m)=P(z1t,z2t,...zkt|xt,m)=∏kP(zkt|xt,m)

其中，

P(zkt|xt,m)∼N(zkt:zkt∗,σzk)

代码实例

void bayesianFilter::process_measurement(const MeasurementPackage &measurements,                                             const map &map_1d,                                         help_functions &helpers){    /******************************************************************************     *  Set init belief of state vector:     ******************************************************************************/    if(!is_initialized_){        //run over map:        for (unsigned int l=0; l< map_1d.landmark_list.size(); ++l){            //define landmark:            map::single_landmark_s landmark_temp;            //get landmark from map:            landmark_temp = map_1d.landmark_list[l];            //check, if landmark position is in the range of state vector x:            if(landmark_temp.x_f > 0 && landmark_temp.x_f < bel_x_init.size() ){                //cast float to int:                int position_x = int(landmark_temp.x_f) ;                //set belief to 1:                bel_x_init[position_x]   = 1.0f;                bel_x_init[position_x-1] = 1.0f;                bel_x_init[position_x+1] = 1.0f;            } //end if        }//end for    //normalize belief at time 0:    bel_x_init = helpers.normalize_vector(bel_x_init);    //set initial flag to true:    is_initialized_ = true ;    }//end if    /******************************************************************************     *  motion model and observation update    ******************************************************************************/    std::cout <<"-->motion model for state x ! \n" << std::endl;    //get current observations and control information:    MeasurementPackage::control_s     controls = measurements.control_s_;    MeasurementPackage::observation_s observations = measurements.observation_s_;    //run over the whole state (index represents the pose in x!):    for (unsigned int i=0; i< bel_x.size(); ++i){        float pose_i = float(i) ;        /**************************************************************************         *  posterior for motion model        **************************************************************************/        // motion posterior:        float posterior_motion = 0.0f;        //loop over state space x_t-1 (convolution):        for (unsigned int j=0; j< bel_x.size(); ++j){            float pose_j = float(j) ;            float distance_ij = pose_i-pose_j;            //transition probabilities:            float transition_prob = helpers.normpdf(distance_ij,                                                    controls.delta_x_f,                                                    control_std) ;            //motion model:            posterior_motion += transition_prob*bel_x_init[j];        }        /**************************************************************************         *  observation update:        **************************************************************************/        //define pseudo observation vector:        std::vector<float> pseudo_ranges ;        //define maximum distance:        float distance_max = 100;        //loop over number of landmarks and estimate pseudo ranges:        for (unsigned int l=0; l< map_1d.landmark_list.size(); ++l){            //estimate pseudo range for each single landmark             //and the current state position pose_i:            float range_l = map_1d.landmark_list[l].x_f - pose_i;            //check, if distances are positive:             if(range_l > 0.0f)            pseudo_ranges.push_back(range_l) ;        }        //sort pseudo range vector:        sort(pseudo_ranges.begin(), pseudo_ranges.end());        //define observation posterior:        float posterior_obs = 1.0f ;        //run over current observation vector:        for (unsigned int z=0; z< observations.distance_f.size(); ++z){            //define min distance:            float pseudo_range_min;            //check, if distance vector exists:            if(pseudo_ranges.size() > 0){                //set min distance:                pseudo_range_min = pseudo_ranges[0];                //remove this entry from pseudo_ranges-vector:                pseudo_ranges.erase(pseudo_ranges.begin());            }            //no or negative distances: set min distance to maximum distance:            else{                pseudo_range_min = distance_max ;            }            //estimate the posterior for observation model:             posterior_obs*= helpers.normpdf(observations.distance_f[z],                                             pseudo_range_min,                                            observation_std);         }        /**************************************************************************         *  finalize bayesian localization filter:         *************************************************************************/        //update = observation_update* motion_model        bel_x[i] = posterior_obs*posterior_motion ;    };     //normalize:    bel_x = helpers.normalize_vector(bel_x);    //set bel_x to belief_init:    bel_x_init = bel_x;};