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

图片名称

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

无人驾驶定位

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

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

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

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

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

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

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

  • Sense:$P(x|z) \propto P(x) P(z|x)$
  • Move:$P(x_{t+1}) = \sum P(x_t) P(x_{t+1}|x_t)$

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

一维马尔科夫定位

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

数据

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

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

这里写图片描述

这里写图片描述

这里写图片描述

这里写图片描述

动机

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

$$bel(x_t) = P(x_t| z_{1:t}, u_{1:t}, m)$$

这里额外提一下,如果求$P(x_t, m| z_{1:t}, u_{1:t})$,那么则从定位问题变成SLAM(simultaneous location and mapping)问题。

容易看到,随着$t$的增大,估计$bel(x_t)$需要的数据越来越大。我们的目标是:

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

也就是:

$$bel(x_t) = f(bel(x_{t-1}), z_t, u_t, m )$$

根据贝叶斯公式,可得:

$$\begin{split}
bel(x_t) &= P(x_t| z_{1:t}, u_{1:t}, m) \\
&= P(x_t| z_t, z_{1:t-1}, u_{1:t}, m) \\
&= \frac{P(x_t| z_{1:t-1}, u_{1:t}, m) P(z_t| x_t, z_{1:t-1}, u_{1:t}, m) }{P(z_t| z_{1:t-1}, u_{1:t}, m) }
\end{split}$$

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

  1. motion model(prediction):$P(x_t| z_{1:t-1}, u_{1:t}, m)$
  2. observation model(likelihood):$P(z_t| x_t, z_{1:t-1}, u_{1:t},m)$

Motion Model

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

$$\begin{split}P(x_t| z_{1:t-1}, u_{1:t}, m) &=\int P(x_{t-1}| z_{1:t-1}, u_{1:t}, m) P(x_t| x_{t-1}, z_{1:t-1}, u_{1:t}, m) d x_{t-1} \\
&= \int P(x_{t-1}| z_{1:t-1}, u_{1:t-1}, m) P(x_t| x_{t-1}, u_t, m) d x_{t-1} \\
&=\sum_i bel(x_{t-1}^i) P(x_t| x_{t-1}^i, u_t)
\end{split}$$

$P(x_t| x_{t-1}^i, u_t)$被称为transition model,其满足:

$$P(x_t| x_{t-1}^i, u_t) \sim N(x_t -x_{t-1}^i : u_t, \sigma_{u_t})$$

Observation Model

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

$$\begin{split}
P(z_t| x_t, z_{1:t-1}, u_{1:t},m) &= P(z_t| x_t, m) \\
&= P(z_t^1,z_t^2,…z_t^k | x_t, m) \\
&= \prod_k P(z_t^k | x_t, m)
\end{split}$$

其中,

$$P(z_t^k | x_t, m) \sim N(z_t^k: {z_t^k}^* , \sigma_{z_k})$$

这里写图片描述

代码实例

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
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;
};