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

原创 2017年11月27日 14:17:20

这里写图片描述

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

无人驾驶定位

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

传统的定位方法有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(xt1),zt,ut,m)

根据贝叶斯公式,可得:

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

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

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

Motion Model

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

P(xt|z1:t1,u1:t,m)=P(xt1|z1:t1,u1:t,m)P(xt|xt1,z1:t1,u1:t,m)dxt1=P(xt1|z1:t1,u1:t1,m)P(xt|xt1,ut,m)dxt1=ibel(xit1)P(xt|xit1,ut)

P(xt|xit1,ut)被称为transition model,其满足:

P(xt|xit1,ut)N(xtxit1:ut,σut)

Observation Model

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

P(zt|xt,z1:t1,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;
};

贝叶斯推理(Bayes Reasoning)、独立与因式分解

1. Reasoning patterns causal reasoning:由原因到结果的一种自然推理 evidential reasoning:一种由结果到原因的反向推理...
  • lanchunhui
  • lanchunhui
  • 2017年01月13日 16:50
  • 455

GPS及惯性传感器在无人驾驶中的应用

from: http://geek.csdn.net/news/detail/107057 作者:刘少山,张哲 本文为《程序员》原创文章,未经允许不得转载,更多精彩文章请订阅2016...
  • Real_Myth
  • Real_Myth
  • 2016年10月13日 10:57
  • 2391

贝叶斯滤波

记忆力差的孩纸得勤做笔记! 滤波过程一般分为两个步骤:预测和更新。贝叶斯滤波器当然也不例外。 预测过程中,不使用当前时刻的测量值,因此预测过程中的后验概率为: ...
  • sinat_31425585
  • sinat_31425585
  • 2016年09月07日 13:12
  • 4672

SLAM Course - WS13/14 by Cyrill Stachniss (4) 贝叶斯滤波器及相关模型

课件及相关资源下载:http://blog.csdn.net/yoouzx/article/details/53098176贝叶斯滤波器测量和控制输入定义z1:tz_{1:t}为不同时刻tt的环境测量...
  • yoouzx
  • yoouzx
  • 2016年11月09日 14:22
  • 703

更好理解贝叶斯定律(Bayes Law)和卡曼滤波器(Kalman Filter)原理

在概率理论中,我们都学习过 贝叶斯理论: P(A|B) = P(A)P(B|A) / P(B)。它的意义在模式识别和卡曼滤波中是基础。理解它,是学习高级算法的前提。至于模式识别和卡曼滤波等很有价值的方...
  • sonictl
  • sonictl
  • 2013年04月11日 11:43
  • 6132

贝叶斯滤波器解释

一张纸巾联系贝叶斯滤波器和多模型中午吃饭的时候,带了一张纸巾,准备擦车,平时我一般不带,但是忘了带钥匙,于是带着这张纸巾去食堂吃饭,中间一个同学问我有没有带纸,我习惯的就说没有,但是转念一想,今天我带...
  • jihengshan
  • jihengshan
  • 2015年05月18日 12:39
  • 5666

贝叶斯递推滤波器原理 -----本博客内容来自西安交大蔡远利教授的随机系统滤波与控制课件,特此声明

本博客内容来自西安交大蔡远利教授的随机系统滤波与控制课件,特此声明!本人...
  • zhjm07054115
  • zhjm07054115
  • 2014年05月06日 18:55
  • 1944

机器人运动估计系列(番外篇)——从贝叶斯滤波到卡尔曼(中)

机器人运动估计系列(番外篇)——从贝叶斯滤波到卡尔曼(中) 上一篇文章里介绍了贝叶斯滤波的理论框架,知道了贝叶斯滤波假设了机器人的状态服从某个概率分布,并且知道了如何利用Bayes公式对其概率分布更...
  • handsome_for_kill
  • handsome_for_kill
  • 2018年01月08日 20:54
  • 90

SLAM笔记三——贝叶斯滤波器

贝叶斯滤波器定义状态估计p(x | z, u) ,即又此时的observations和之前的控制命令,估计现在的状态。 递归贝叶斯滤波器 首先定义:bel(…)代表贝叶斯模型 ...
  • qq_30159351
  • qq_30159351
  • 2016年11月29日 17:06
  • 3129

同步定位与地图构建SLAM过程

SLAM过程包含许多步骤,整个过程是为了利用环境来更新机器人的位置。由于机器人中给出自身位置的距离测量往往是不精确的,就不能直接依赖于这种测距机制。我们可以利用对环境的激光扫描来纠正机器人位置,这能通...
  • sky_in_my_mind
  • sky_in_my_mind
  • 2017年05月23日 11:43
  • 762
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:无人驾驶定位与贝叶斯滤波
举报原因:
原因补充:

(最多只允许输入30个字)