全局地图的绘制 

 在《绘制二维障碍栅格地图的一种方法1（局部地图的绘制）》http://blog.csdn.net/zhubaohua_bupt/article/details/72923373 

这篇博客里，我们讲述了如何利用深度相机去绘制单帧图像看到的局部地图。

但是只有局部地图是不够用的，在一般的地面机器人的路径规划中，往往需要整个场景的栅格地图，

这样，就可以为机器人，在场景里所有可达的目标位置，提供路径规划数据。本篇博客是接上述那篇博客讲述的。

本篇博客内容为：

在已知局部栅格地图后，怎么通过相机在不同时刻的位姿，把多帧局部地图绘制成全局栅格地图。

比如，这是一个局部地图：

不断融合后，形成的全局地图：

关键技术点：

   1坐标转换，局部栅格地图根据相机位姿，把二维栅格点转换到不断融合的全局地图     

   2 地图融合：不断融合的全局栅格地图，和新的一帧局部栅格地图的融合

坐标转换

坐标转换是把t时刻的局部地图点，经过相机t时刻的位姿，投射到全局地图中。

栅格地图的应用对象是地面机器人，它的运动一般是二维的，所以，在坐标转换过程中，我们只需要考虑

相机当前坐标系与初始坐标系（一般用它来作为全局参考系）的偏航角Yaw与t时刻相机中心在全局中心的位置。

以下默认坐标系(Z轴：相机前方向，Y轴：相机下方向，X轴：相机左方向)

 

偏航角的计算：

       如果我们知道了t时刻相机相对于全局参考坐标系的位姿T(R，t)之后，只需要姿态R，就可以转换得到

偏航角Yaw（不明白就搜旋转矩阵转RPY角）

公式如下：

其中， R为t时刻相机相对于参考系的旋转矩阵，R31为旋转矩阵第三行第一列的元素，其他类推。

t时刻相机中心在全局中心的位置 ：

地图融合

        

        这一部分讲述的是，把局部地图投影到全局地图后，怎么和已有的全局地图进行融合，

形成一个新的全局地图。我们知道，栅格地图至少有三种状态（可通行（无障碍占据）、有障碍占据、未知），

融合是解决这样一个具体问题：在全局栅格地图中，某一个栅格（称为g）肯定是这些状态中的一种，

当局部地图上的点伴随着它自己的状态，投影到栅格g上时，怎么解决栅格g状态更新的问题。

      常用的融合策略是概率融合（搜概率栅格图）。在《局部地图》这篇博客中，我们画的局部地图其实就是概率栅

格图，初始时，用像素值128表示当前栅格不确定（所用像素值的范围0-255），然后，如果当前栅格是障碍，

我们把它置0，否则就是255。

       

 

融合方法1：比较暴力但可行的融合策略。

  

现在，我们用一种粗暴但有效的方法来解决融合问题：

全局栅格地图上栅格的值，我们不让它是3值的（3状态），而是让它们分布在0-255，

栅格对应的值小于128时，越小是障碍的概率越大，栅格对应的值大于128时，越大是障碍的概率越小。

如此，我们就可利用如下思想融合地图：

如此，我们就可这样融合地图：

IF  plocalGrid==obstacle and plocalGrid -value1>=0

Then pglobleGrid-=value1;

ELSE IF plocalGrid== PassableandpglobleGrid +value2<=255

Then pglobleGrid +=value2;

Endif

value1和value2是一个经验值，比如可以都取5。

注:pglobleGrid为局部栅格plocalGrid投影到全局栅格上后，在全局栅格上的位置。栅格地图的像素值在0~255之间。

这个方法虽然比较暴力，但也是基于统计的，效果可行。

融合方法2：贝叶斯滤波

更普通的方法是利用贝叶斯滤波对栅格地图融合。先给出公式

这个公式乍一看有点复杂，但稍微经过解释，非常容易理解。下面我们来看看，这个公式表述了一个怎样的融合方法。

首先解释一下公式中的符号含义：

z : 2值观测值（每一次的观测值来源于局部地图，只有两种状态：占据、空闲）

融合t+1时刻的观测值后，栅格i被障碍占据的概率。

        融合t时刻的观测值后，栅格i被障碍占据的概率。

单纯的从t+1时刻的观测值，判断的栅格i被障碍占据的概率

一般取常数，比如,

注意：这里不是一个完备事件，

也就是说两者之和不一定为1。

说到这里，讲的可能还不是特别清楚，我们举个融合的例子来帮助理解。

通过这两个例子，也就是新观测不同的时候，全局地图融合的过程，你是否能理解这个公式呢？

地图融合注意事项：

         在融合地图时，我们只融合当前局部地图能看到的范围，我们不能往全局地图里融合未知的范围，否则就会出错。

         举个例子，比如你的全局地图刚才看到了一面墙后面的场景，当前局部地图看到的是相机至这面墙之间的空间，墙后面的看不到（对于局部地图是未知区域），这个时候，如果把未知融合到全局地图里，肯定不是我们想要的。

         最后，地图构建的效果见顶端。

转载请说明出处：http://blog.csdn.net/zhubaohua_bupt/article/details/77093334