无人驾驶高精度定位技术（1）-直方图滤波Histogram Filter

直方图滤波(Histogram Filter)的算法思想

把整个状态空间dom(x(t))切分为互不相交的部分b1、b2、...,b(n-1)，使得：

然后定义一个新的状态空间y(t)∈，当且仅当x(t)∈b(i)时，y(t)=i,由于y(t)是一个离散的状态空间，我们就可以采用离散贝叶斯算法计算bel(y(t))。bel(y(t))是对bel(x(t))的近似，它给出x(t)在每一个b(i)的概率，b(i)划分的越精细，计算的结果就越精确，当然精确的代价是计算成本的上升。

举个例子

如下图所示，无人驾驶汽车在一维的宽度为5m的世界重复循环，因为世界是循环的，所以如果无人驾驶汽车到了最右侧，再往前走一步，它就又回到了最左侧的位置; 车上安装有Sensor可以检测车辆当前所在位置的颜色，当然这种检测不是100%准确的，存在一定的错误率；无人驾驶汽车以自认为1m/step的恒定速度不断向右移动，移动本身也可能存在误差，它自以为的向右移动由于误差的存在可能待在原地不动，可能向右移动1m，也可能向右移动2m。

从数学的语言来描述这个问题：机器人只有一个状态变量：位置，pos(t)∈[0,5)。为了应用直方图滤波(Histogram Filter)，对状态空间做如下分解，b(0)∈[0,1)，b(1)∈[1,2)，b(2)∈[2,3)，b(3)∈[3,4)，b(4)∈[4,5)，于是得到一个新的状态空间y(t)∈，它是对连续状态空间的近似，在某一时刻车辆只能在这些离散状态中的一个。

虽然车辆自认为在向右运动，每一步运动1m，我们假设存在5%的概率，无人驾驶汽车仍待在原地没动；存在90%的概率车辆在向右移动1m；存在5%的概率无人驾驶汽车在向右运动2m。

无人驾驶汽车的Sensor假设90%的概率感知结果是正确的，还有10%的情况下它感知的结果是错误的。

我们用一个5维的向量来描述t时刻，无人驾驶汽车位于第1个格子、第2个格子、第3个格子、第4个格子、第5个格子的概率。

无人驾驶汽车对自己所在位置一无所知，假设它连续三次【运动-向右走一步】-【观测】,三次观测结果分别是orange、blue、orange，我们一步步无人驾驶汽车是如何通过【运动】-【观测】过程逐步确认自己的位置的。

t=0时刻

没有任何先验知识，无人车不知道自己在哪里,所以在各个位置概率相等：

t=1时刻

首先向右走1m，用运动模型进行位置预测

可以看出无人车虽然向前运动一步，但它仍然对自己所在位置一无所知。这也和我们的认知相同，刚开始完全不知道自己在哪里，走了一步自然也完全不知道自己在哪里。

再用更新模型通过Sensor感知环境，更新当前位置的置信度。

orange的颜色感知信息使得传感器认为自己很可能位于第二个和第五个方格中。

t=2时刻

运动模型-向右走1m

更新模型-sensor环境感知

t=3时刻

运动模型-向右走1m

感知更新模型-sensor环境感知

可以看到经过三次的运动和观测后，无人驾驶汽车已经有73%的概率确认自己位于第二个网格中，事实再经过三次的运动观测，无人驾驶汽车可以有94%的概率确认自己的位置。