前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >无人驾驶定位与贝叶斯滤波

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

作者头像
用户1147754
发布2018-01-05 17:04:08
1.6K0
发布2018-01-05 17:04:08
举报
文章被收录于专栏:YoungGy
这里写图片描述
这里写图片描述

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

无人驾驶定位

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

传统的定位方法有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

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

其中,

这里写图片描述
这里写图片描述

代码实例

代码语言:javascript
复制
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;
};
本文参与 腾讯云自媒体同步曝光计划,分享自作者个人站点/博客。
原始发表:2017年11月27日,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 作者个人站点/博客 前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体同步曝光计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • 无人驾驶定位
  • 定位的数学问题之贝叶斯滤波
  • 一维马尔科夫定位
    • 数据
      • 动机
        • Motion Model
          • Observation Model
            • 代码实例
            相关产品与服务
            图数据库 KonisGraph
            图数据库 KonisGraph(TencentDB for KonisGraph)是一种云端图数据库服务,基于腾讯在海量图数据上的实践经验,提供一站式海量图数据存储、管理、实时查询、计算、可视化分析能力;KonisGraph 支持属性图模型和 TinkerPop Gremlin 查询语言,能够帮助用户快速完成对图数据的建模、查询和可视化分析。
            领券
            问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档