logo

马尔可夫定位

本文来讨论一下无人驾驶汽车的定位问题.有人可能会有疑问,定位直接用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}) 可以理解为所有矩形面积之和:

image

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

image

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 .

image

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

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) ,所以我们要循环地图上所有的位置,对每个位置分别计算运动模型:

image

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

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

image

观察模型

同样的道理, 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) 的含义,更别提计算了.我们来画个草图:

image

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

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

马尔科夫定位

结合公式(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
评论加载中...
发表评论