本文来讨论一下无人驾驶汽车的定位问题.有人可能会有疑问,定位直接用GPS不就好了么?为什么搞的那么麻烦?因为GPS误差太大了,好的时候GPS误差为3~5m,坏的情况GPS的误差可以达到几十米.这显然不符合无人车的要求.几米的误差肯定要出车祸的.

无人车有哪些可供我们精确定位的资源呢?首先,无人车上有很多传感器,比如雷达与激光雷达,时间t内累积的测量数据记为z_{1:t},不过再贵的传感器都是由误差的.我们还知道车辆时间t内累积的运行数据u_{1:t},比如说车辆的速度、加速度等.我们还知道周围道路的电子地图m,这里我们假设地图是已知的并且不会随着时间变化.我们把车辆在时刻t的状态记为x_t,车辆的状态不仅包括车辆在地图上的位置,还包括运行数据.那么车辆的定位问题用数学的形式表达出来就是:

bel(x_t) = p(x_t|z_{1:t}, u_{1:t}, m) \tag{1}

其中车辆的位置一定在地图内,而地图m又是已知的,所以遍历地图里的所有位置一定可以找到使公式概率最大的x_t.当然这里遍历的地图可以使用GPS确定到一个比较小的范围.不过这种计算有个致命的问题在于数据过于庞大,传感器每小时可以产生8GB的测量数据,而且随着时间的增加数据量在不断的增加,每次一都使用之前所有的数据进行计算时不现实的.

为了解决这个问题,我们将测量数据z_{1:t}拆分成z_{t}z_{1:t-1}:
p(x_t|z_{1:t}, u_{1:t}, m) = p(x_t|z_{t},z_{1:t-1}, u_{1:t}, m) \tag{2}

接下来我们使用贝叶斯定理对公式(1)进行变换:

p(x_t|z_{t},z_{1:t-1}, u_{1:t}, m) = \dfrac{p(z_t|x_t,z_{1:t-1},u_{1:t},m)p(x_t|z_{1:t-1},u_{1:t},m)}{p(z_t|z_{1:t-1},u_{1:t},m)} \tag{3}

由于分母只和测量数据相关,而测量数据一但生成就不会修改了,所以分母相当于一个常数,不影响我们p(x_t|z_{1:t}, u_{1:t}, m)最大值的选取.所以上式简化为:

bel(x_t)= \eta \times p(z_t|x_t,z_{1:t-1},u_{1:t},m)p(x_t|z_{1:t-1},u_{1:t},m)  \tag{4}

我们把这里的p(z_t|x_t,z_{1:t-1},u_{1:t},m)叫做观察模型(Observation Model),p(x_t|z_{1:t-1},u_{1:t},m)叫做运动模型(Motion Model).

额外的,你可能听说过无人车的定位问题就是预测-更新的循环,这里的运动模型所做的事情是预测过程,观察模型对运动模型的值进行修正是更新过程.

运动模型

根据全概率公式:

P(B)=\sum _{n}P(B\mid A_{n})P(A_{n})

对于连续型随机变量可以写成积分的形式:

我们可以这样理解,我们把\Pr(B_{n})当做矩形的宽,把P(A\mid B_{n})当做矩形的高,所以\sum _{n}P(A\mid B_{n})P(B_{n})可以理解为所有矩形面积之和:

当随机变量是连续的时候,求面积就变成了这样,与积分的定义是一致的:

ok,我们理解了全概率公式的积分形式之后,来对运动模型运用全概率公式进行一下转换:
p(x_t|z_{1:t-1},u_{1:t},m) = \int p(x_t|x_{t-1},z_{1:t-1},u_{1:t},m)p(x_{t-1}|z_{1:t-1},u_{1:t},m) dx_{t-1}

根据马尔可夫假设x_t只取决于x_{t-1},z_t,u_t.

所以运动模型可以化简为:

p(x_t|z_{1:t-1},u_{1:t},m) = \int p(x_t|x_{t-1},u_t,m)p(x_{t-1}|z_{1:t-1},u_{1:t},m) dx_{t-1}

又由于x_{t-1}不取决于u_t,因为u_t相对于x_{t-1}是未来的数据,未来的数据不会对过去的数据产生影响,所以公式可以进一步变换:

p(x_t|z_{1:t-1},u_{1:t},m) = \int p(x_t|x_{t-1},u_t,m)p(x_{t-1}|z_{1:t-1},u_{1:t-1},m) dx_{t-1}

此时公式中的p(x_{t-1}|z_{1:t-1},u_{1:t-1},m)与公式(1)刚好差了一个时间间隔,所以可以替换p(x_{t-1}|z_{1:t-1},u_{1:t-1},m)bel(x_{t-1}),即:

p(x_t|z_{1:t-1},u_{1:t},m) = \int p(x_t|x_{t-1},u_t,m)bel(x_{t-1}) dx_{t-1}  \tag{5}

如果用离散的形式表示为:

p(x_t|z_{1:t-1},u_{1:t},m) = \sum\limits_{i} p(x_t|x_{t-1}^{(i)}, u_t, m)bel(x_{t-1}^{(i)})  \tag{6}

运动模型的计算

因为我们要找到车辆在地图上概率最大的位置,也就是说我们要对地图上所有的位置计算bel(x_t),所以我们要循环地图上所有的位置,对每个位置分别计算运动模型:

来看其中的\sum\limits_{i} p(x_t|x_{t-1}^{(i)}, u_t, m)bel(x_{t-1}^{(i)}),表示上一个位置x_{t-1}出现x_t的概率再乘以上一个位置的置信度.

因为上一个位置可能是任意位置,所以这里我们还要循环所有位置采用高斯分布计算概率,两个位置离的越近,概率越高,离的越远概率越低.

观察模型

同样的道理,z_t只依赖于x_t,所以观察模型p(z_t|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|x_t,m)  \tag{7}

由于测量值z_t是一个矩阵,直接计算概率可能比较麻烦,不过由于各测量直接是独立的,所以我们可以转换为各测量值概率的乘积形式:

p(z_t|x_t,m) = p(z_t^1,z_t^2,z_t^3,...,z_t^k|x_t,m) = \prod_{i=1}^k p(z_t^i|x_t,m)  \tag{8}

观察模型的计算

你有可能没有理解\prod_{i=1}^k p(z_t^i|x_t,m)的含义,更别提计算了.我们来画个草图:

我们可以根据已知的地图计算出车辆与地标直接的距离,然后我们又有传感器的距离.我们需要计算的就是测量值与地图值的总体误差.

和运动模型一样,我们需要估计所有位置的概率,所以这里也需要循环地图上所有位置计算观察模型.

马尔科夫定位

结合公式(4)(5)(7)得:

bel(x_t)= \eta \times p(z_t|x_t,m) \int p(x_t|x_{t-1},u_t,m)bel(x_{t-1}) dx_{t-1}  \tag{9}

由于bel(x_{t-1})相当于bel(x_t)没有加测量值时的状态,也就是bel(x_t)的预测值,所以有的文献里将bel(x_{t-1})写为\widehat{bel}(x_{t-1})即:

bel(x_t)= \eta \times p(z_t|x_t,m) \int p(x_t|x_{t-1},u_t,m)\widehat{bel}(x_{t-1}) dx_{t-1}  \tag{10}

这个公式可以只用t-1时刻的状态及t时刻的测量值递归的估计t时刻的状态.该公式叫做定位滤波器,也叫马尔可夫定位.

posted @ 2018-07-19 14:57:13
评论加载中...

发表评论