一种基于机器人平台的室内墙角二维语义地图构建方法

技术领域

本发明属于机器人平台建图定位导航领域，具体涉及一种基于机器人平台的室内墙角二维语义地图构建方法。

背景技术

地图构建是智能机器人平台完成定位导航任务的第一步也是关键，随着对机器人平台智能化和自动化程度的要求越来越高，要求机器人平台能理解周围环境中所包含的几何和语义信息，但利用传统激光SLAM方法构建出的拓扑地图或者栅格地图只能表达环境中拓扑信息与几何信息，缺少对环境语义信息的提取与描述，导致机器人平台无法真正地理解环境。利用视觉SLAM可以获取环境语义，它可以获取丰富的纹理信息，但其受光线影响较大，边界不够清晰，暗处纹理少，且运算负荷大，地图构建存在累计误差，不利于构建二维语义地图。同时环境中物体的移动导致建立好的语义地图出现较大改动，因此加入某些环境中特定物体语义(如墙角)作为环境地图的固有特征十分有意义。

发明内容

本发明提出一种包含墙角信息的室内二维语义地图构建方法，利用激光slam算法进行栅格地图创建，同时结合视觉传感器和深度学习模型进行目标识别检测，对环境中墙角等物体语义信息进行提取，并结合贝叶斯估计的方法，对栅格中是否含有物体进行增量式估计，将得到的激光地图和物体语义地图进行信息融合，并将得到的语义地图进行表示，得到包含墙角的二维语义栅格地图。其将为机器人平台定位导航提供更为丰富的特征，从而能完成更为高级复杂的任务。

本发明为解决现有技术中存在的问题采用的具体技术方案为一种基于机器人平台的室内墙角二维语义地图构建方法。

所述机器人平台其特征在于，包括：机器人平台底盘、主控机、激光雷达传感器、深度相机；

所述的机器人平台底盘装载所述的主控机、激光雷达传感器、深度相机；

所述主控机分别与所述的机器人平台底盘、激光雷达传感器、深度相机通过有线方式依次连接。

所述室内墙角二维语义地图构建方法，其特征在于，包括以下步骤：

步骤1：主控机控制机器人平台底盘驱动所述机器人平台在室内运动，激光雷达传感器实时采集室内物体与机器人平台的距离、室内物体与机器人平台的方向角，并传输至主控机，主控机将室内物体与机器人平台的距离、室内物体与机器人平台的方向角通过Gmapping算法处理得到环境栅格地图、机器人平台实时位姿；

步骤2：主控机上构建语义分割数据集，将语义分割数据集作为训练集，将语义分割数据集中每张非墙角样本图像输入至DeepLab v2网络进行预测得到预测的非墙角语义标签，进一步结合非墙角语义标签构建DeepLab v2网络损失函数，通过优化训练得到优化后DeepLab v2网络；构建墙角目标检测数据集，将墙角目标检测数据集作为训练集，将墙角目标检测数据集中每张墙角样本图像输入至SSD网络进行预测得到预测的外界矩形边框、以及预测的外界矩形边框中物体类型，进一步结合预测框、墙角标记框中物体类型构建SSD网络损失函数，通过优化训练得到优化后SSD网络；

步骤3：主控机利用深度相机获取机器人平台视角彩色图像，将视角彩色图像输入到优化后DeepLab v2网络预测，识别到视角彩色图像中非墙角的语义标签；将视角彩色图像通过优化后SSD目标检测网络，识别到视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型；将视角彩色图像中非墙角的语义标签、视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型进行坐标变换处理，依次得到非墙角的三维点云坐标、墙角的三维点云坐标，将非墙角的三维点云坐标、墙角的三维点云坐标分别通过基于统计方法的滤波器进行点云滤波处理，得到滤波后非墙角的三维点云坐标、滤波后墙角的三维点云坐标；

步骤4：主控机将滤波后非墙角的三维点云坐标、滤波后墙角的三维点云坐标，结合机器人平台实时位姿分别进行点云坐标变换，得到非墙角的物体在环境栅格地图坐标系下的坐标以及墙角的物体在环境栅格地图坐标系下的坐标，通过非墙角的物体在环境栅格地图坐标系下的坐标、墙角的物体在环境栅格地图坐标系下的坐标构建物体栅格地图；

步骤5：重复执行步骤3至步骤4，直至主控机控制机器人平台完成室内环境遍历，从而得到完整的环境栅格地图、完整的物体栅格地图，进一步将完整的环境栅格地图、完整的物体栅格地图进行合并，得到室内墙角二维语义地图。

作为优选，步骤1所述机器人平台实时位姿为：

c

其中，c

作为优选，步骤2所述语义分割数据集为：

I＝{data

其中，M表示语义分割数据集I中非墙角样本图像的数量，U表示语义分割数据集I中每张非墙角样本图像的列数，V表示表示语义分割数据集I中每张非墙角样本图像的行数，s表示语义分割数据集I中每张非墙角样本图像的像素所属类别的数量，data

步骤2所述DeepLab v2网络损失函数为交叉熵损失函数；

步骤2所述通过优化训练得到优化后DeepLab v2网络为：

将所述交叉熵损失函数最小化为优化目标，

通过SGD算法寻优得到优化后DeepLab v2网络；

步骤2所述目标检测数据集为：

p∈[1,P],x∈[1,X],y∈[1,Y],n∈[1,N

其中，P表示目标检测数据集中墙角样本图像的数量，X表示目标检测数据集中每张墙角样本图像的列数，Y表示表示目标检测数据集中每张墙角样本图像的行数，P表示目标检测数据集中第p张墙角样本图像中外界矩形边框的数量，s表示像素点所属的类别；data

步骤2所述SSD网络损失函数：由用于分类的log loss损失函数和用于回归的smooth L1损失函数构成；

步骤2所述通过优化训练得到优化后SSD网络为：

通过SGD算法寻优得到优化后SSD网络；

作为优选，步骤3所述视角彩色图像中非墙角的语义标签为：

*w

其中，*data(u,v)

步骤3所述视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型为：

其中，

步骤3所述将视角彩色图像中非墙角的语义标签、视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型进行坐标变换处理：

结合*w

W

其中，B表示集合中当前图片中非墙角语义像素点总数；(i

得到墙角语义在视角彩色图像像素坐标系中的像素坐标集合：

E

其中，T表示集合中当前图片中墙角语义像素点总数；(i

将上述得到的当前彩色图像素坐标系下非墙角语义坐标(i

式子，(X

将上述得到的当前彩色图像素坐标系下墙角语义坐标(i

得到相机坐标系下墙角语义点云坐标(X

其中，(X

步骤3所述将非墙角的三维点云坐标(X

基于统计分析方法的滤波器从点云数据中去除离散点云后，对物体点云提取距离机器人平台最近的一簇点云，相当于提取视角下物体的外轮廓点云，得到滤波后非墙角语义点云坐标(X'

作为优选，所述步骤4所述的点云坐标变换如下：

将得到的非墙角语义点云在相机坐标系下的点云坐标(X'

将得到的墙角语义点云在相机坐标系下的点云坐标(X'

式中R

通过上面转换便得到机器人平台坐标系下非墙角语义点云坐标(X

再结合利用Gmapping建图算法获取的机器人平台实时位姿(x

移动机器人平台坐标系与世界坐标系之间的转换关系为：

式中，R

最后将真实世界坐标系下的坐标点(X

式中(X

通过上面一系列坐标变换便得到了物体栅格地图坐标系中各语义点坐标值(X

作为优选，步骤5所述将完整的环境栅格地图、完整的物体栅格地图进行合并为：

获取到完整的环境栅格地图的中心，将完整的环境栅格地图的中心与完整的物体栅格地图的中心对齐，遍历非墙角的物体在环境栅格地图坐标系下的坐标、墙角的物体在环境栅格地图坐标系下的坐标，将该处对应的标记加入到环境栅格地图对应位置，完整的环境栅格地图的中心与完整的物体栅格地图的中心和姿态要保持一致，最后得到室内墙角二维语义地图。

本发明具有如下优点：

本发明利用语义分割深度学习模型进行物体语义分割，可以得到准确的物体语义，使得原本只有激光信息建立得到的环境栅格地图具有了语义层面的含义。同时，结合SSD目标检测模型进行环境中的墙角进行识别，将提取到的墙角语义增加的原本语义地图中，相比于地图中其它物体语义墙角具有更高的稳定性，对于环境而言，它们的位置不会轻易变更，为机器人平台定位导航提供更为丰富可靠的特征，从而能完成更为高级复杂的任务。

附图说明

图1：为本发明方法流程图；

图2：为实验场景图；

图3：为机器人平台物体检测分割示意图；

图4：为深度图及其可视化图；

图5：为椅子初步语义提取图；

图6：为镂空式椅子点云二维映射图；

图7：为误检测时语义映射图；

图8：为增量式方法消除“假语义”效果图；

图9：为语义映射地图；

图10：为合成得到的语义栅格地图；

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

下面结合图1至图10介绍本发明的具体实施方式为本发明的第一具体实施例为一种基于机器人平台的室内墙角二维语义地图构建方法。

所述机器人平台其特征在于，包括：机器人平台底盘、主控机、激光雷达传感器、深度相机；

所述的机器人平台底盘装载所述的主控机、激光雷达传感器、深度相机；

所述主控机分别与所述的机器人平台底盘、激光雷达传感器、深度相机通过有线方式依次连接。

主控机选用的是酷睿M4I7-D迷你主机；

机器人平台底盘选用的是pibot品牌的Arduino驱动板底盘；

激光雷达传感器选用性能稳定的SICK lms111激光雷达；

深度相机选用的是kinect v2。

所述室内墙角二维语义地图构建方法，其特征在于，包括以下步骤：

步骤1：主控机控制机器人平台底盘驱动所述机器人平台在室内运动，激光雷达传感器实时采集室内物体与机器人平台的距离、室内物体与机器人平台的方向角，并传输至主控机，主控机将室内物体与机器人平台的距离、室内物体与机器人平台的方向角通过Gmapping算法处理得到环境栅格地图、机器人平台实时位姿；

步骤1所述机器人平台实时位姿为：

cok＝(x

其中，c

步骤2：主控机上构建语义分割数据集，将语义分割数据集作为训练集，将语义分割数据集中每张非墙角样本图像输入至DeepLab v2网络进行预测得到预测的非墙角语义标签，进一步结合非墙角语义标签构建DeepLab v2网络损失函数，通过优化训练得到优化后DeepLab v2网络；构建墙角目标检测数据集，将墙角目标检测数据集作为训练集，将墙角目标检测数据集中每张墙角样本图像输入至SSD网络进行预测得到预测的外界矩形边框、以及预测的外界矩形边框中物体类型，进一步结合预测框、墙角标记框中物体类型构建SSD网络损失函数，通过优化训练得到优化后SSD网络；

步骤2所述语义分割数据集为：

I＝{data

其中，M表示语义分割数据集I中非墙角样本图像的数量，U表示语义分割数据集I中每张非墙角样本图像的列数，V表示表示语义分割数据集I中每张非墙角样本图像的行数，s表示语义分割数据集I中每张非墙角样本图像的像素所属类别的数量，data

步骤2所述DeepLab v2网络损失函数为交叉熵损失函数；

步骤2所述通过优化训练得到优化后DeepLab v2网络为：

将所述交叉熵损失函数最小化为优化目标，

通过SGD算法寻优得到优化后DeepLab v2网络；

步骤2所述目标检测数据集为：

p∈[1,P],x∈[1,X],y∈[1,Y],n∈[1,N

其中，P表示目标检测数据集中墙角样本图像的数量，X表示目标检测数据集中每张墙角样本图像的列数，Y表示表示目标检测数据集中每张墙角样本图像的行数，P表示目标检测数据集中第p张墙角样本图像中外界矩形边框的数量，s表示像素点所属的类别；data

步骤2所述SSD网络损失函数：由用于分类的log loss损失函数和用于回归的smooth L1损失函数构成；

步骤2所述通过优化训练得到优化后SSD网络为：

通过SGD算法寻优得到优化后SSD网络；

步骤3：主控机利用深度相机获取机器人平台视角彩色图像，将视角彩色图像输入到优化后DeepLab v2网络预测，识别到视角彩色图像中非墙角的语义标签；将视角彩色图像通过优化后SSD目标检测网络，识别到视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型；将视角彩色图像中非墙角的语义标签、视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型进行坐标变换处理，依次得到非墙角的三维点云坐标、墙角的三维点云坐标，将非墙角的三维点云坐标、墙角的三维点云坐标分别通过基于统计方法的滤波器进行点云滤波处理，得到滤波后非墙角的三维点云坐标、滤波后墙角的三维点云坐标；

步骤3所述视角彩色图像中非墙角的语义标签为：

*w

其中，*data(u,v)

步骤3所述视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型为：

其中，

步骤3所述将视角彩色图像中非墙角的语义标签、视角彩色图像中墙角的预测外接矩形边框及墙角的预测外接矩形边框中物体类型进行坐标变换处理：

结合*w

W

其中，B表示集合中当前图片中非墙角语义像素点总数；(i

得到墙角语义在视角彩色图像像素坐标系中的像素坐标集合：

E

其中，T表示集合中当前图片中墙角语义像素点总数；(i

将上述得到的当前彩色图像素坐标系下非墙角语义坐标(i

式子，(X

将上述得到的当前彩色图像素坐标系下墙角语义坐标(i

得到相机坐标系下墙角语义点云坐标(X

其中，(X

步骤3所述将非墙角的三维点云坐标(X

基于统计分析方法的滤波器从点云数据中去除离散点云后，对物体点云提取距离机器人平台最近的一簇点云，相当于提取视角下物体的外轮廓点云，得到滤波后非墙角语义点云坐标(X'

步骤4：主控机将滤波后非墙角的三维点云坐标、滤波后墙角的三维点云坐标，结合机器人平台实时位姿分别进行点云坐标变换，得到非墙角的物体在环境栅格地图坐标系下的坐标以及墙角的物体在环境栅格地图坐标系下的坐标，通过非墙角的物体在环境栅格地图坐标系下的坐标、墙角的物体在环境栅格地图坐标系下的坐标构建物体栅格地图；

所述步骤4所述的点云坐标变换如下：

将得到的非墙角语义点云在相机坐标系下的点云坐标(X'

将得到的墙角语义点云在相机坐标系下的点云坐标(X'

式中R

通过上面转换便得到机器人平台坐标系下非墙角语义点云坐标(X

再结合利用Gmapping建图算法获取的机器人平台实时位姿(x

移动机器人平台坐标系与世界坐标系之间的转换关系为：

式中，R

最后将真实世界坐标系下的坐标点(X

式中(X

通过上面一系列坐标变换便得到了物体栅格地图坐标系中各语义点坐标值(X

步骤5：重复执行步骤3至步骤4，直至主控机控制机器人平台完成室内环境遍历，从而得到完整的环境栅格地图、完整的物体栅格地图，进一步将完整的环境栅格地图、完整的物体栅格地图进行合并，得到室内墙角二维语义地图；

步骤5所述将完整的环境栅格地图、完整的物体栅格地图进行合并为：

获取到完整的环境栅格地图的中心，将完整的环境栅格地图的中心与完整的物体栅格地图的中心对齐，遍历非墙角的物体在环境栅格地图坐标系下的坐标、墙角的物体在环境栅格地图坐标系下的坐标，将该处对应的标记加入到环境栅格地图对应位置，完整的环境栅格地图的中心与完整的物体栅格地图的中心和姿态要保持一致，最后得到室内墙角二维语义地图。

下面介绍本发明的第二实施例：

步骤1：在真实实验场景下借助LMS111激光雷达传感器和移动机器人平台实验平台，使用Gmapping算法在ROS操作平台上完成机器人平台的即时定位与栅格地图的创建，实验场景图如图2所示。

机器人平台在环境中移动不断完善地图，根据机器人平台获取的里程计数据以及里程计运动学模型，估计机器人平台当前时刻位姿，每个粒子都包含机器人平台建图开始至当前所有时刻的位姿以及当前环境地图，然后根据激光数据利用激光似然域模型进行扫描匹配，计算粒子中包含地图与建立出的地图的匹配程度，进行权重更新和重采样，更新粒子的地图，得分最高的粒子的位姿即为最优位姿，实时获取机器人平台位置，并逐步生成环境栅格地图。

步骤2：利用深度相机获取机器人平台视角图，将彩色图像输入到深度学习检测模型中，识别视野中的目标物体以及墙角。

首先，本文基于数据所处场景适中且广泛存在、数据类别和数量均衡的原则进行室内语义分割数据集和室内物体检测数据集的制作。收集了多视角多距离多亮度下的室内场景图片，并添加了机器人平台实际使用场景的图片，组成了本文室内语义分割任务的INDOOR1数据集以及检测识别任务的数据集INDOOR2。为了进一步丰富数据集，同时提高模型的泛化能力，训练前对对数据集进行颜色改变、尺度变换、随机裁剪等数据增强操作。搭建DeepLab v2和SSD网络，采用ImageNet数据集上预训练的网络权重初始化SSD网络模型的权重，在GPU模式下利用制作好的数据集进行模型训练。

在机器人平台在环境中移动建图的过程中，由搭载的深度相机获取的视角彩色图像作为训练好的检测模型的输入，得到当前视角下物体检测识别结果，如机器人平台处于图3中(a)所示场景时，墙角的检测效果如图3中(d)所示,检测结果图中的实心圆圈代表检测到的墙角的在图片中的位置，视角中门和椅子的检测分割效果如图3中(c)所示，其中非黑色部分代表的分别是检测出的门和柜子。可以看出，得到的检测效果还是不错，把得到的结果作为接下来语义点云提取的输入。

步骤3：结合同视角下对应的深度图，得到识别墙角在相机坐标系下的三维点云坐标，对于生成的点云数据，进行相应的滤波处理，去除其中的假语义点云。

完成对RGB图中物体检测后，我们便可以获取到二维图像中物体的类别和位置信息，结合深度图信息获得实际距离信息实现三维信息的转换，上图中所示场景对应的深度图如图4所示。

在获取到彩色图与深度对应关系后，Kinect v2相机坐标系下的点云(X

式子中u，v为RGB图像中分割出的物体的像素坐标值，d为深度图像中像素坐标为(u,v)的深度值，c

通过上述的坐标转换，可以得到识别物体在相机坐标系下的3D点云坐标。

考虑到训练模型的误检测以及点语义生成的计算量，对初始3D语义点云进行部分滤除。对于如图5中(a)所示环境中椅子利用训练模型实现分割后得到的彩色分割图(c)，由于它镂空式的结构，分割图中有部分点云透过椅子其实是在椅子后面墙壁上的点云，若我们直接对所有这些点云进行下一步的操作，得到的语义信息中会有较多的假语义。不作任何处理直接将这些3D点云映射到二维平面上，效果如图6中(a)所示，通过映射图可以看出有些明显不属于椅子部分的点云，而且得到的物体投影显得较为杂乱，因此对于生成的点云数据，进行相应的滤波处理很有必要。

考虑到深度相机Kinect v2的量程，对于过近或过远的测量结果都会有较大的出入，我们这里设定只对0.5～5米范围内的点云进行处理，超过或低于设定范围的点云进行去除。为方便滤波处理，对于剩下的点云进行降维操作，先转换成二维点云，再使用基于统计分析方法的滤波器从点云数据中去除噪声点(离群点)。该滤波器的原理：计算输入点云集中每个点到所有临近点的平均距离，得到的结果符合高斯分布，计算出均值和方差，去除那些远离均值的二维点。对于上面图中的椅子点云信息进行上述的点云去除操作后，得到的该椅子点云二维映射效果图如下图6中(b)所示。

可以看出生成的椅子语义信息更为准确。为更好地表示提取出物体的语义信息，我们对物体点云提取距离机器人平台最近的一簇点云，相当于提取视角下物体的外轮廓点云，具体的实现：得到物体语义映射图后，提取出图像中每一列中距离机器人平台最近的栅格点，对于的点云保留下来进行接下来的坐标系转换操作，提取效果如图6中(c)图。

在对相机坐标下的物体点云坐标滤波处理后，得到相机平面下物体外轮廓语义点云的映射坐标。

步骤4：利用Gmapping建图算法获取的机器人平台实时位姿，将机器人平台坐标系下语义坐标转换得到世界坐标系下，进而根据增量式估计的方法映射到二维栅格坐标系中，并将其在创建的物体栅格地图上表示；

物体语义栅格地图是和激光栅格地图同时开始创建的，我们首先创建一张正方形的空白栅格地图，地图的中心作为建图起点开始建图，它是要把环境分成许多个大小相同的栅格单元，它的地图分辨率值和Gmapping算法建图分辨率值设置一致，不同的是，构建的物体语义栅格地图中只包含提取出的物体语义映射信息，其余无识别物投影的栅格认为是栅格空闲状态。考虑到存在的误差，对于在不同时刻不同位置检测得到的同一个物体的相同部位在地图上的映射坐标会不一致，这里考虑结合栅格点的历史状态进行判定。

为了尽可能减少这种误差，我们采用基于贝叶斯方法的增量式估计对栅格状态进行更新。

对于栅格地图，使用Q(s)表示栅格状态，它有栅格被占据的概率p(s＝1)和栅格空闲的概率p(s＝0)决定：

当接收到传感器数据后，得到一个新模型测量值z，观测值的状态只有两种(0或1)，我们要对栅格状态Q(s|z)进行更新，由是(2)可得：

由上式(3)可得在观测值为z时栅格被占据和空闲的概率：

根据式(3)和(4)计算可得Q(s|z)：

在式(5)左右两边同时去对数可得：

由(6)式可以看出，状态更新后的栅格概率值仅第一项与测量值相关，第二项为更新之前栅格占据概率值。我们对上式进行相似化处理，栅格更新前后状态分别用Q

Q

利用上述增量式估计的方法进行物体语义栅格地图构建时，对于初始空白地图，每个栅格对应初始状态值Q

建图过程中，当传感器数据更新时，也即机器人平台位姿发生改变，才利用上述方法对栅格状态进行更新，避免将同一帧数据重复作为测量值进行栅格状态更新。利用增量式估计的方法可有有效消除由于某些帧误检测分割而带来的“假语义”，比如对于图7所示，当机器人平台识别在它周围的物体时，在某一帧中将木方块误分割成了门；若是直接将识别到的物体进行语义映射，得到的结果中就会存在不正确的语义信息，得到如图中(c)所示的效果。当结合增量式估计方法进行建图时，这些某些帧误识别带来的误检测会被消除，操作机器人平台使其在不同位置对误识别处进行多次检测，以此消除误识别处栅格的概率值，得到准确的物体语义，过程中得到的效果图如图8所示。

步骤5：在机器人平台遍历环境完成后，得到物体栅格地图和环境栅格地图，再把两幅栅格地图进行合并，得到反映环境语义信息的包含墙角信息的二维语义栅格地图；

在明确了每一帧数据中扫描到的栅格点以及其状态后，根据移动机器人平台不同时刻的位姿状态和传感器信息重复获取以上数据，同时根据介绍的贝叶斯估计原理更新栅格地图坐标系中每一个栅格单元状态，完成机器人平台使用环境遍历后，根据最终每个栅格的物体概率值是否大于一定设定阈值来判断该栅格是否存在某一类物体，完成室内物体语义栅格地图的创建，如图9所示，其中(a)图为得到的激光环境栅格地图，(b)图表示环境中物体语义映射图，(c)图表示检测出的环境墙角语义图。

初始得到的墙角语义地图中墙角投影点比较杂乱，我们对其进行图像形态学操作，对地图做联通区域分析，去除这些细小的孤立点区域，将靠近主体部分的孤立点进行连接，提取各部分中小作为该角点中心，以其为圆心绘制一定大小的实心圆代替角点，得到如图9中(b)所示优化后的墙角语义图。

将激光地图和物体语义地图进行融合可得到如图10中(b)所示的合成语义地图，合并的具体过程如下：由于物体栅格地图和环境栅格地图是同步进行建立的，两幅地图地图的栅格分辨率相同，因此生成的两幅地图相同物体间的尺寸是一样的，它们的方向是一致的，只需两幅地图姿态对齐合并过程就方便进行了。首先利用Gmapping建立出来的环境栅格地图生成的地图文件，获取到地图中心(建图原点)的位置；生成的物体栅格地图为图片格式，它的中心即为图片中心，只需将环境栅格地图中心与物体栅格地图的中心对齐，保持方向不变，这样两幅地图的中心便对齐了；接下来就可以进行地图的合成，实际上可以看成是图片操作，可以使用OpenCV作为工具遍历读取物体栅格地图的非黑色区域(即目标物)，将该区域的颜色加入到环境栅格地图对应的位置中，过程中两幅图的中心和姿态要保持一致，遍历完物体栅格地图中所有像素值后环境栅格地图中的颜色区域也添加完毕，最后生成的环境栅格地图即为合成的含有墙角信息语义栅格地图。

为了更好地表达出环境中的高层次语义信息，对构建的语义图进行一些优化处理，使用不同形状的彩色快代替环境中在某一区域中的不同物体，结果如图10中(c)所示。对合成的语义地图中每一个物体提取中心位置，将它们在地图中的坐标存储到相应的描述文件中，与构建的语义地图一起存储起来，以便后续利用其实现语义图中物体位置的查询，完成更高层次的定位导航应用任务。由合成的语义地图可以看出，物体与栅格地图中的对应障碍物基本重合，物体位置表达准确，能正确反映了环境语义信息，二维语义栅格地图创建完成。

本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。