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

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

无人驾驶定位

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

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

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

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

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

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

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

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

一维马尔科夫定位

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

数据

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

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

动机

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

这里额外提一下,如果求

,那么则从定位问题变成SLAM(simultaneous location and mapping)问题。

容易看到,随着tt的增大,估计

需要的数据越来越大。我们的目标是:

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

也就是:

根据贝叶斯公式,可得:

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

  1. motion model(prediction):P(xt|z1:t−1,u1:t,m)P(x_t| z_{1:t-1}, u_{1:t}, m)
  2. observation model(likelihood):P(zt|xt,z1:t−1,u1:t,m)P(z_t| x_t, z_{1:t-1}, u_{1:t},m)

Motion Model

根据马尔科夫假设,可得:

被称为transition model,其满足:

Observation Model

根据马尔科夫假设,可得:

其中,

代码实例

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;
};

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

发表于

我来说两句

0 条评论
登录 后参与评论

相关文章

来自专栏机器之心

比特币突破8000美元,我们找到了用DL预测虚拟货币价格的方法

选自GitHub 作者:David Sheehan 机器之心编译 截至 11 月 22 日,比特币的价格再创历史新高(约 1 比特币兑 8120 美元),在惊讶...

370100
来自专栏新智元

NLP历史突破!谷歌BERT模型狂破11项纪录,全面超越人类!

谷歌AI团队新发布的BERT模型,在机器阅读理解顶级水平测试SQuAD1.1中表现出惊人的成绩:全部两个衡量指标上全面超越人类!并且还在11种不同NLP测试中创...

17240
来自专栏CreateAMind

DeepDriving: Learning Affordance for Direct Perception inDriving

http://blog.csdn.net/bcj296050240/article/details/53897535

14550
来自专栏TensorFlow从0到N

TensorFlow从0到1 - 13 - AI驯兽师:神经网络调教综述

在未来的AI时代,“手工程序”将变得越发稀有,而基于通用AI程序,通过大数据“习得”而生的程序,会无所不在。到那时,程序员将光荣卸任,取而代之的是一个新职业物...

44670
来自专栏AI研习社

多任务深度学习框架在 ADAS 中的应用 | 分享总结

在 8 月 10 日AI 研习社邀请了北京交通大学电子信息工程学院袁雪副教授给我们讲解了在高级辅助驾驶系统(ADAS)中的多任务深度学习框架的应用。 ADAS ...

42160
来自专栏数据派THU

20篇顶级深度学习论文(附链接)

本文讲述了深度学习正值快速发展进化阶段,新技术,新工具以及新的应用实现正在深刻改变着机器学习领域并不断获得累累硕果。

14530
来自专栏灯塔大数据

塔荐 | 比特币突破8000美元,我们找到了用DL预测虚拟货币价格的方法

前 言 截至 11 月 22 日,比特币的价格再创历史新高(约 1 比特币兑 8120 美元),在惊讶于虚拟货币「不可战胜」的同时,我们或许能可以从这一波热潮...

38280
来自专栏机器之心

资源 | 价值迭代网络的PyTorch实现与Visdom可视化

选自GitHub 作者:Xingdong Zuo 机器之心编译 参与:吴攀 《价值迭代网络(Value Iteration Networks)》是第 30 届神...

41880
来自专栏机器之心

解读 | 如何用进化方法优化大规模图像分类神经网络?

机器之心原创 作者:Angulia Chao 参与:Joni、侯韵楚、高振 让机器具备生物一样的进化能力一直是计算机科学的一个热门研究领域,今年三月份,谷歌的...

378110
来自专栏大数据文摘

Kaggle大神带你上榜单Top2%:点击预测大赛纪实(下)

16220

扫码关注云+社区

领取腾讯云代金券