引言

移动机器人与周围环境进行智能交互，在环境中执行类似于人的任务，是近些年的研究热点^[1]。同时定位与地图构建（SLAM，simultaneous localization and mapping）是移动机器人最基本的问题，要求机器人能在未知环境中探索、建模并构建有效、可用的全局地图，是该领域研究的前沿方向^[2]。将深度学习融合到视觉SLAM技术中，构建含有环境语义信息的三维稠密点云地图，这对移动机器人理解环境中的语义信息，顺利执行高级复杂的任务具有举足轻重的作用。

视觉SLAM是通过视觉传感器，在没有环境先验信息的情况下，在运动过程中估计自身运动状态并构建环境地图^[3]。目前，主流的视觉SLAM算法可分为两种^[4]：一种是基于特征的方法，该算法是通过提取每个图像中的特征，再进行匹配，从而恢复相机的姿态。如：KinectFusion^[5]使用RGB-D相机，通过ICP算法恢复相机的姿态，并通过CUDA进行加速，来构建环境的稠密点云地图；RGBD_SLAM^[6]是通过跟踪ORB特征，利用G2O算法优化相机位姿来构建稠密地图；ORB-SLAM^[7]是目前为止在该领域最优秀的SLAM系统框架，不仅支持单目、双目以及深度相机，还包含回环检测、重定位、自动初始化，可以稳健、快速地构建环境的稀疏点云地图。另一种是直接法，根据图像的强度值来估计相机位姿，常见的基于直接法的算法框架有：DSO_SLAM^[8]、LSD_SLAM^[9]、SVO_SLAM^[10]等。

在计算机视觉领域，深度学习的出现推动了目标检测的发展。文章通过卷积神经网络对环境中的物体进行检测，获取物体的类别和预测框。FCN^[11]、PSPNet^[12]和DeepLab^[13]都可对二维图像进行像素级的语义分割，R-CNN^[14]、Fast-RCNN^[15]、Faster-RCNN^[16]可通过对目标产生一个候选区域，再通过分类器和回归器回归得到候选区域的精确位置，但由于会产生很多区域建议，导致对目标进行检测的速度较慢，此类方法称为two-stage法。与之相比，目标检测速度较快的是one-stage法，如：SSD^[17]和YOLO^[18]都是在图片不同位置采用多种尺度和长宽比进行抽样，通过卷积神经网络提取特征，然后进行分类和回归。

虽然ORB-SALM2是非常完善、实用的视觉SLAM系统，但它依旧存在一些问题，如：由于点云地图中缺乏体积信息，移动机器人无从得知环境中哪些地方可通行，使得ORB-SLAM2所构建的稀疏点云地图无法实际使用；所构建的地图中缺乏语义信息，不利于机器人理解环境内容；当环境越大时，存储点云地图所占空间越大等问题。为解决以上问题，本文提出一种构建语义地图的方法，在ORB-SLAM2的基础上增加目标检测算法来构建稠密点云语义地图，并将其转换为Octomap进行存储，首先使用检测速度较快的YOLO v3算法及对象正则化来获取物体的2D标签，结合生成的稠密点云地图得到含有语义信息的点云标签，利用基于图的分割算法对稠密点云进行分割和优化，再将点云标签与分割后的点云进行点云融合，进而构建稠密点云语义地图，将所构建的点云语义地图转换为八叉树地图，以便移动机器人进行避障和导航，减小了点云地图的存储空间。

1   系统框架

1.1   语义SLAM系统

本文提出的语义SLAM系统框架如图1所示。通过深度相机获取环境的图像序列，经过目标检测网络YOLO v3检测每一个关键帧图像中的物体，从当前帧的彩色图和深度图得到每一个物体的地图点，将所有地图点坐标的均值作为物体的坐标，并计算该物体的质心、种类和边界框，给定每种类别的物体不同的颜色，将其作为2D标签。同时，经过ORB-SLAM2算法构建环境的稀疏点云地图，再通过含有2D标签的彩色图像及其对应的深度图像以及关键帧来生成稠密点云标签，对稠密点云地图进行基于图的分割和优化，然后与生成的点云标签进行融合得到效果较好的稠密点云语义地图，为了方便移动机器人执行更高级任务，减小存储空间，将点云地图转换为Octomap。

图  1  语义SLAM系统框图

Figure  1.  Block diagram of semantic SLAM system

下载: 全尺寸图片 幻灯片

构建稠密点云语义地图的具体步骤如下：

1） 得到2D标签。加载color.txt，里面存储YOLO v3能够识别的80种类别物体的rgb值，读取预训练权重yolov3.weights、配置文件yolov3.cfg及训练好的模型coco.names，对每个关键帧的彩色图像进行目标检测，得到物体的种类，给定不同物体不同的颜色并绘制物体的边界框，将其作为2D标签，若计算出的物体种类为“人”，则对2D标签进行修改。

2） 稠密点云标签。通过关键帧、彩色图像及深度图像来产生稠密点云地图，遍历深度图像，获取图中点$(m,n)$处的值，计算该点的空间坐标并从含有2D标签的彩色图中获取该点的颜色，再将该点插入稀疏点云tmp中，每新插入一个关键帧都执行此操作，点云tmp经过变换矩阵T得到点云cloud，对cloud进行泊松重建，得到每个物体的模型，然后将模型转换为三维点云，得到稠密点云地图，对稠密点云进行染色得到稠密点云标签。

3） 点云分割。将稠密点云作为输入点云，进行参数设置，输入点云经滤波后对点云进行基于图的分割，得到分割后的点云。

4） 点云语义地图。遍历稠密点云标签中的每一个点，每个点都存储了空间位置xyz值和颜色信息rgb值，如果该点的rgb值与某一物体的rgb值相等，则认为该点是这个物体点云中的点，并对分割后的点云中该点的颜色信息进行更新，得到点云语义地图。

1.2   ORB-SLAM2分析

ORB-SLAM2的原理框图如图2所示，它包含并运行3个线程：跟踪、局部建图以及全局回环检测并优化线程。跟踪线程即前端里程计的主要部分负责对每幅输入的图像提取ORB特征点，与最邻近的关键帧进行比较，计算特征点的位置，粗略估计相机位姿，如果跟踪失败，则通过词袋法（DBoW2，bag of binary words）进行重定位（place-recognition），最终决定是否插入关键帧；局部建图线程通过Bundle Adjustment对局部空间内的特征点及相机位姿进行优化，删除冗余的关键帧和低质量的地图点，得到更精确的相机位姿和特征点的空间位置；采用DBoW2算法对全局地图和关键帧进行回环检测，如果检测到回环，通过Sim3优化计算里程计的累计误差，然后将匹配到的所有点云进行对齐并融合重复的点，最后对相似性约束进行图优化，实现全局一致性。得到稀疏点云地图，该地图主要包括3D地图点、关键帧、共视关系及生长树等。

图  2  ORB-SLAM2原理图

Figure  2.  Schematic of ORB-SLAM2

下载: 全尺寸图片 幻灯片

1.3   基于深度学习的目标检测

本文使用当前检测速度较快的YOLO v3^[19]网络结构，如图3所示。其基本思想是将每幅输入图像划分成$s \times s$个栅格并对这些栅格进行分类得到目标的类别和边框。由于YOLO v3采用多个logistic分类器在保证目标检测的速度的同时又不降低多标签分类的准确性。

图  3  YOLO v3网络结构

Figure  3.  YOLO v3 network structure

下载: 全尺寸图片 幻灯片

在YOLO v3的基础上进行对象正则化，对显然不属于该环境的物体标签进行修改，得到比较精确的物体标签。如：静态环境下，数据集中的泰迪熊会被YOLO v3检测为“人”，但动态的“人”显然不属于该静态数据集，因此需要对标签“人”进行修正，改为“泰迪熊”。

本文使用对象级的Dense CRF模型对语义标签进行优化，吉布斯分布为

当$P(x)$最小时得到最优的标签结果，$Z$为正则化因子，$E(x)$为能量函数：

式中：$x$表示物体的标签；$i,j \in (1,k)$为环境中物体的数量；一元势${\phi _u}({x_i})$和二元势${\phi _p}({x_i},{x_j})$分别为

式中：$P({x_i})$为第$i$个物体标签的概率分布；${w^{(m)}}$为线性组合权重；$\mu ({x_i},{x_j})$为标签的容错函数，表示同一物体出现两种不同的标签的可能性，本文采用Potts模型：

（3）式中${k^{(m)}}({f_i},{f_j})$为高斯核函数：

式中${\theta _\alpha }$、${\theta _\beta }$表示物体$i$与物体$j$间的欧式距离小于$\alpha $、$\beta $。本文中，高斯核函数特指同一位置出现两种物体的概率${p_{i,j}}$的倒数，表达式如下：

式中：${\eta _{i,j}}$表示出现物体$i$和物体$j$同时出现的频率；${\eta _j}$表示出现物体$j$的频率。

将目标检测网络、对象正则化和语义SLAM的视觉里程计紧密结合，减小了相机位姿的估计值与真实值的差，提高了建图的精确度。具体表现为：其一，当输入一帧图像时，定义一个向量来存储图像中的所有物体，遍历一个存储YOLO v3所能够识别的80类物体的列表，根据不同种类的物体对应的类别序号各不相同，通过左上角点和右下角点得到该物体的边界框，如图4所示。其二，在语义SLAM的跟踪线程中，添加了对物体的跟踪，使得相机的绝对位姿误差、绝对轨迹误差及相对位姿误差与ORB-SLAM2方法相比有所减少。当采集RGBD图像和进行双目初始化时，在地图中添加当前帧中所有物体的地图点，将属于一个物体的所有地图点的平均位置作为物体的位置，并对地图中物体的序号和置信度进行更新；在跟踪参考关键帧时，通过一个向量objs来存储获取到的当前帧中的所有物体的序号、种类和置信度，遍历每个物体的地图点，如果参考帧中该点所属的物体与地图中的物体为同一类物体，计算该点的空间位置，当重复出现同一物体时，更新地图中物体的序号和置信度，如果出现新物体，则在地图和向量objs中添加新物体的序号、种类和置信度。

图  4  目标检测

Figure  4.  Object detection

下载: 全尺寸图片 幻灯片

1.4   点云分割

本文根据物体的类别进行点云分割，采用图割算法对点云进行基于图的分割和优化，首先将ORB-SLAM2构建的稀疏点云结合对应的关键帧及深度信息生成稠密点云，将其作为原始点云，对原始点云进行超体素分割，共得到12982个体素。构造一个加权无向邻接图$G = \left\{ {v,e} \right\}$来表示超体素的分割结果，如图5所示。

图  5  加权无向邻接图

Figure  5.  Diagram of weighted undirected adjacency

下载: 全尺寸图片 幻灯片

其中顶点${v_i} \in v$表示曲面片，边$e$为权重，表示两个曲面片的相似度，实线表示同一物体的2个不同曲面片间的权重，虚线表示不同物体的2个曲面片间的权重。经超体素分割后，环境中的每一个物体都包含多个曲面片，计算每个曲面片${v_i}$的重心${c_i}$和法向量${{{n}}_i}$，根据法向量的局部凸度信息来决定是否在图中添加边，通过RANSAC算法对曲面片进行随机采样得到候选面$P = \left\{ {{p_1},{p_2},{p_3},...,{p_m}} \right\}$，再计算每个曲面片的重心${c_i} (x',y',z')$到候选面${p_m}$的欧氏距离$d$：

其中：候选面满足$Ax + By + Cz + D = 0$，$D$为常数；$\dfrac{A}{D}$、$\dfrac{B}{D}$、$\dfrac{C}{D}$分别为候选面在$x、y、z$轴上的截距。将距离$d({c_i},{p_m}) < 4.5\;{\rm{cm}}$（经验值）范围内的所有候选面$S(P)$都作为属于每类物体的可能的候选面，如（8）式所示：

然后进行优化，得到属于每个物体的可能的候选面：

最后，对候选平面进行基于图的对象分割^[20]，完成点云分割。

2   实验结果与分析

2.1   稠密点云地图

本实验在TUM数据集上运行，使用的数据集名称为${\rm{long\_office\_household}}$，共历时87.09 s采集到2585张彩色图及对应的深度图，手持相机绕办公桌一圈，该环境包含多种物体，其中YOLO v3可识别显示屏、键盘、鼠标、椅子、泰迪熊、书、瓶子以及杯子8种类别的物体。本实验的运行平台使用i3-8100、主频为3.6 GHz的CPU，GPU型号为Nvidia GeForce 1070，显存8 G，使用Darknet框架，操作系统为Ubuntu 16.04。

本文是将深度相机采集的图像序列作为YOLO v3网络的输入，计算出物体的类别和预测框的坐标，并给定不同类别的物体不同的颜色，将其作为2D标签，通过ORB-SLAM2算法构建环境的稀疏点云地图，再根据含有2D标签的彩色图像和相应的深度图像以及关键帧来生成含有语义信息的稠密点云标签，如图6（a）、6（b）所示。从图中可以看到，通过YOLO v3能有效识别环境中的显示屏、键盘、鼠标、椅子、泰迪熊、书、瓶子以及杯子，每个类别的物体对应的颜色各不相同。

下载: 全尺寸图片 幻灯片

图  6  语义标签

Figure  6.  Semantic labels

下载: 全尺寸图片 幻灯片

由于点云标签与分割后的点云数目相同，将两者进行点云融合。遍历点云标签中的点云，每个点云不仅存储了空间位置还存储了颜色信息，当点云的颜色信息为某一个特定值时，则认为该点是属于分割后的点云中某一物体中的点，对不同种类的物体赋予不同的颜色，从而得到点云语义地图，如图7（a）、7（b）所示。点云语义地图中相同的颜色代表同一种类的物体，不同颜色代表不同种类的物体。

图  7  点云语义地图

Figure  7.  Point cloud semantic map

下载: 全尺寸图片 幻灯片

2.2   系统性能评估

对本文提出的紧密结合ORB-SLAM2和YOLO v3得到环境的稠密点云语义地图，与ORB-SLAM2构建的不含语义信息的稀疏点云地图进行性能比较。首先，根据时间戳将计算得到的相机位姿的真实值与估计值进行对齐，然后计算两者的差得到绝对轨迹误差（ATE），并通过绝对位姿误差（APE）来检测所构地图的全局一致性，通过相对位姿误差（RPE）评估系统的漂移性，以下实验结果为分别使用两种方法运行该数据集5次得到的误差平均值，其对比结果如表1所示。

表  1  误差表

Table  1.  Error table

方法	ATE/cm	APE/cm	RPE/cm	运行时间/s
ORB-SLAM2	1.0747	0.9054	0.7397	9.25
本文方法	0.9161	0.7452	0.7160	22.68

下载: 导出CSV 

| 显示表格

从表1可知，与传统的ORB-SLAM2相比，在紧密结合Yolo v3和ORB-SLAM2后能减小相机的轨迹误差和位姿误差，提高了构建稠密点云语义地图的精度，整个算法运行时间约为22.68 s，由于增加了语义信息的获取，时间消耗也略有增加。绝对位姿误差比较如图8所示，相对位姿误差比较如图9所示，其中绿色线为本文方法得到的误差，蓝色线为ORB-SALM2算法得到的误差。

图  8  绝对位姿误差

Figure  8.  Absolute pose error

下载: 全尺寸图片 幻灯片

图  9  相对位姿误差

Figure  9.  Relative pose error

下载: 全尺寸图片 幻灯片

2.3   八叉树地图

八叉树地图对于移动机器人执行高级复杂的任务至关重要，如：避障、导航或抓取物体等。使用构建的稠密点云地图生成Octomap，地图中存储了点的空间信息，每个小方块都表示是否被占据的概率，0为空闲，表示可通行，1为被占据，表示不可通行。分辨率设置的越高，方块的体积越小，通过查看每个小方块是否被占据可实现移动机器人不同精度的导航。

使用八叉树的形式存储地图，解决了点云地图中的点没有体积信息、存储空间大的问题，不同分辨率的Octomap所占存储空间也不同，如表2所示。根据实际情况，本文将分辨率设为0.05 pixel，使得地图所占存储空间由原来的194.30 MB减小至2.28 MB。

表  2  占据存储空间对比表

Table  2.  Comparison of occupied storage space

地图形式	占据存储空间/MB
点云地图	194.30
（分辨率）Octomap	（0.01）62.64	（0.05）2.28	（0.1）0.52	（0.5）0.03

下载: 导出CSV 

| 显示表格

将点云转换为Octomap并存储颜色信息，设置叶子的大小为5 cm，生成的OctoMap如图10所示，其中图10（a）～10（h）分别为书、瓶子、椅子、泰迪熊、杯子、键盘、鼠标和显示屏。

图  10  八叉树地图

Figure  10.  Octree map

下载: 全尺寸图片 幻灯片

3   结论

本文提出通过紧密结合目标检测网络YOLO v3和ORB-SLAM2来构建环境的稠密点云语义地图。将通过目标检测及对象正则化得到的物体的2D标签映射到点云地图上，结合深度信息和关键帧生成稠密点云标签，同时对稠密点云进行基于图的分割，然后将点云标签与分割后的点云进行融合得到点云语义地图，其中相同颜色为同一种类的物体，不同颜色表示不同种类。本文在SLAM的里程计部分添加了对物体的跟踪，使得相机的绝对位姿误差和轨迹误差与传统的ORB-SLAM2相比分别减少了16.02%和15.86%，提高了建图精度，将点云地图保存为Octomap形式，减少了地图的存储空间。本系统并不能在ROS系统下实时构建点云语义地图，在未来的研究中将解决这一问题，并实现机器人的导航。