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

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

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

为了解决这个问题,我们将测量数据拆分成:

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

由于分母只和测量数据相关,而测量数据一但生成就不会修改了,所以分母相当于一个常数,不影响我们最大值的选取.所以上式简化为:

我们把这里的叫做观察模型(Observation Model),叫做运动模型(Motion Model).

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

运动模型

根据全概率公式:

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

我们可以这样理解,我们把当做矩形的宽,把当做矩形的高,所以可以理解为所有矩形面积之和:

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

ok,我们理解了全概率公式的积分形式之后,来对运动模型运用全概率公式进行一下转换:

根据马尔可夫假设只取决于,,.

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

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

此时公式中的与公式(1)刚好差了一个时间间隔,所以可以替换,即:

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

运动模型的计算

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

来看其中的,表示上一个位置出现的概率再乘以上一个位置的置信度.

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

观察模型

同样的道理,只依赖于,所以观察模型可以简化为:

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

观察模型的计算

你有可能没有理解的含义,更别提计算了.我们来画个草图:

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

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

马尔科夫定位

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

由于相当于没有加测量值时的状态,也就是的预测值,所以有的文献里将写为即:

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

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

发表评论