无人驾驶高精度定位技术（1）-递归贝叶斯滤波

在随后的几篇文章中，陆续介绍一些无人驾驶汽车中高精定位相关的技术原理，包括贝叶斯滤波器(Bayes Filter)、直方图滤波器(Histogram Filter)、卡尔曼滤波(Kalman Filter)、粒子滤波(Particle Filter)，这些算法也都是机器人技术中的基础算法。

如图，开始机器人不知道自己在哪里，跟人在陌生的环境中一样，一脸茫然，觉得四周哪里都一样，对机器人来说就是在任何地方的置信度都相等。

置信度(Belief)是什么

要让机器人理解物理世界，就需要给机器人安装各种传感器，比如摄像头、GPS、轮速记等等，通过传感器让机器人可以对周围环境进行建模，从而理解自己所处的位置和处境。真实的世界是非常复杂、动态变化的，机器人没有能力也不可能把周围环境都刻画出来。比如机器人运动过程中，受风速或者地面湿度影响，它通过轮速计知道自己走了1m，由于地面打滑或者大风影响，导致实际只向前走了0.9m。采用越多越精确的传感器，就能够把环境刻画的越精确，但是刻画的越精确，参数越多，计算就越复杂，甚至不可行。即使我们可以通过传感器刻画出环境的所有纬度，由于传感器自身测量存在的误差或者噪声，导致机器人仍然不可能确认自己的位置。

由于环境的不可完全观测性，我们引入一个置信度(Belief)的概念，表示每一个可能的状态(state)的概率。

除此之外，由于实际的环境变化是时间连续的，为了简化问题，我们把环境变化按照固定时间间隔进行切分，把环境的变化看成是离散的，用离散的变化来模拟连续的变化，用x(t)表示t时刻的环境状态(state)。对于二维空间上的移动机器人，x(t)=(x,y,Θ), (x,y)表示t时刻机器人的位置，Θ表示t时刻机器人的朝向。

为了确定自己的位置，机器人需要对周围环境进行观察，我们采用e(t)表示在t时刻的观察值。这种采用贝叶斯滤波算法和基于机器人对环境观察获取环境置信度的过程就就做贝叶斯滤波。

采用贝叶斯算法用历史信息推测现在时刻的状态，然后利用周围环境的观察结果，更新当前时刻的状态置信度的过程叫做递归贝叶斯滤波。

用历史信息推测当前时刻的状态置信度叫预测置信度(predicted belief)：

状态转移模型。描述了所有从x(t-1)状态转移到x(t)状态的概率:

传感器观察模型。描述从状态x(t)得到观测值e(t)的概率:

马尔可夫假设。为了简化问题，假设状态转移模型和传感器观察模型t时刻的状态都只与t-1时刻的状态有关，而与之前所有时刻的状态无关。

递归贝叶斯算法:

通过前一时刻的置信度得到当前时刻的置信度，这一步称为预测(projection)。

通过新的观测e(t+1)和预测置信度bel(x(t+1))计算贝叶斯置信度的过程，称为更新(update)。

η是归一化参数，保证所有概率之和等于1。为了保证递归正常运行，需要一个初始值bel(x0)，通常情况下我们可以赋予每一个可能的状态相同的概率值，当然如果有先验的知识，可以赋予一个结合先验知识的概率值。

继续机器人的定位故事：

走到第一个门的时候，通过观察看到一个门，因为机器人通过地图应该有三个门，它就确定自己应该在三个门之一的任何一个位置，于是在三个门处的概率就大大增加。

由于机器人安装了里程计，走到第二个门的时候，根据自己走的距离和两个门的信息已经有绝大部分把握知道自己在哪里了。