机器人已深入到生活中各个场景，同步定位与建图（simultaneous localization and mapping, SLAM）是机器人的最基本功能，使机器人可在复杂的环境中具有高效率。目前传统SLAM方法在依赖外部环境的纯视觉方案下，定位和建图精度容易受到光照和纹理等条件的影响。为了应对这些挑战，通过视觉传感器与IMU (intertial measurement unit)传感器耦合的VI-SLAM 方案适用范围更广，但机器人无法理解周围环境，始终是难以攻克的难题。为此，语义VI-SLAM应运而生，通过融合语义信息，使机器人不仅能构建地图和定位，还能识别环境中的物体及其关系，提升了机器人的环境理解能力和智能化水平。

语义SLAM是移动机器人研究的核心技术之一，许多研究人员重点研究语义地图构建，同时跟踪地图中语义对象^[1]，例如Whelan等^[2]将实例分割算法与RGB-D SLAM算法相结合，基于ElasticFusion修改匹配目标函数，构建室内环境精确语义地图。Mccormac等^[3]在ElasticFusion的基础上引入了卷积神经网络（convolutional neural networks, CNN），能够在帧与帧之间建立更加精确的对应关系，从而提高了系统在复杂环境中的数据关联和匹配精度，此外，将多视角的语义预测方法巧妙地融合到地图构建过程中。 Wang等^[4]提出了QISO-SLAM，该方法整合了数据关联、单帧椭球初始化和其他过程，以建立高级的3D地图。Salas-Moreno等^[5]提出了SLAM++算法，生成物体的三维模型，将其应用于实例级面向对象的3D-SLAM姿态图优化算法框架中。Dame等^[6]在SLAM算法运行过程中，使用特定对象的知识来构建精确的语义地图。

对于动态环境中的SLAM问题，许多学者结合语义信息来删除环境中的动态目标，以此提高定位精度。例如，Yu等^[7]提出DS-SLAM，结合语义分割网络SegNet与运动特征点检测来滤除每一帧中动态物体。Kaneko等^[8]提出的Mask-SLAM，通过语义分割的结果来识别动态特征点的属性标签，直接排除某些区域的特征点。Wang等^[9]利用语义分割将一些类别区域定义为背景，并将其他类别区域定义为可移动物体。Bescos等^[10-11]提出的DynaSLAM，对RGB-D输入图像的动态点做了细致的处理，提升多对象跟踪能力。Li等^[12]提出的DP-SLAM，结合了几何约束和语义分割来跟踪贝叶斯概率估计框架中的动态特征点。

有些学者致力于研究如何提高语义SLAM实时性，例如Zhao等^[13]提出了KSF-SLAM，通过一种关键分割帧的选择策略，提升了SLAM系统的实时性。Hu等^[14]提出的DeepLabv3 SLAM，利用改进的DeepLabv3(+)语义分割网络，以此减少动态目标的检测时间。Huang等^[15]利用在线三维重建过程中高效融合多视图二维特征和投影到超体素上的三维特征，构建了基于超体素的卷积神经网络，称为Supervoxel-CNN。Mccormac等^[16]提出了Fusion++，具有任意重构对象的3D图，之后通过基于Mask-RCNN的深度融合逐步细化了物体的分割^[17]。还有些学者研究提高绝对效率和更大环境的可伸缩性，如Nakajima和Saito通过使用快速和可扩展的对象检测，实现了高精度的面向对象场景实时重建，该算法减少了计算成本和内存占用^[18]。Pham等^[19]使用高效的超体素聚类与基于结构和物体线索的高阶约束的条件随机场（conditional random fields, CRF）对三维室内场景进行实时密集重建和语义分割。

在语义SLAM与深度学习结合方面，Tateno等^[20]提出的CNN-SLAM，使用卷积神经网络CNN进行深度预测，解决了单目SLAM在位姿估计和环境重建中缺少绝对尺度的问题。Clark等^[21]提出了视觉惯性网VINet（visual-inertial net），利用CNN和RNN（recurrent neural network）构建了一个视觉惯性网络VIO（visual-inertial odometry），直接输出估计的位姿结果。Detone等^[22] 提出Deep SLAM，利用CNN端对端的学习位姿然后完成SLAM中特征点的提取与匹配，后来提出Superpoint，通过直接学习特征点与描述子来实现特征提取与匹配。Chen等^[23]将DCNNs和全连接的CRFs结合起来，形成了一个端到端的系统，叫作DeepLab，CRFs可以对DCNNs的预测结果进行优化，使得分割结果更加平滑和精确。语义SLAM中闭环检测也是一个较大问题，Song等^[24]利用语义信息进行SLAM数据关联与闭环检测的表检索方法。Qian等^[25]提出的SmSLAM+LCD方法通过将高级3D语义信息与低级特征信息相结合，显著提升了系统的性能。这种融合策略不仅实现了更为准确的闭环检测，还有效地抑制了长期运行中的漂移问题。

即使视觉SLAM和语义SLAM研究成果较多，但目前仍存在一些问题： 1）单帧图像特征点较少，增加机器人SLAM的匹配难度； 2）视觉SLAM在运行过程中容易将运动物体错误地识别为地图上的静态点，致使位姿估计误差过大； 3）语义信息与图像信息结合过程的配准和实时性问题。为解决以上问题，本研究提出一种基于深度学习和稠密点云处理的语义VI-SLAM方法，通过改进ORB-SLAM3算法，添加稠密建图线程并通过点云拼接实现稠密点云地图的构建，利用深度学习YOLO v5s目标检测网络实时获取关键帧中物体的类别及其位置，基于语义信息及极线约束去除环境中的动态特征点，提升系统在动态场景下的定位精度；采用点云分割的方法对稠密点云地图进行分割，然后将语义特征与分割后的点云特征融合，完成三维语义地图构建。

1.   三维稠密点云地图构建

本研究通过在ORB-SLAM3系统原有线程的基础上增加稠密建图线程，构建稠密点云地图，利用相机成像原理，将关键帧各像素点结合其对应的深度信息映射到三维空间形成单帧稠密点云。假设图像中某一个点的像素坐标是$(u,v)$，其深度值为$d$，在三维空间中的坐标$(x,y,{\textit{z}})$的计算式为

式中：${f_x}$、${f_y}$、${c_x}$、${c_y}$均为相机的内参。得到单帧稠密点云数据后，计算每个点与其邻近点之间平均距离，将离群点滤除，建立三维体素栅格，用体素中所有点的重心近似地表示体素上的全部点，进一步滤除外点及异常点，形成单帧稠密点云。

结合ORB-SLAM3算法中该关键帧对应的相机位姿估计结果，将所有点转换到世界坐标系中，便可建立出原始的三维稠密点云地图。假设第$i$个和第$j$个关键帧生成滤波后的稠密点云分别为$C_{{\text{lou}}{{\text{d}}_i}}$和$C_{{\text{lou}}{{\text{d}}_j}}$，对应的相机位姿估计分别为${T_i}$和${T_j}$，通过下式将2个关键帧点云转换到世界坐标系下：

按照下式拼接得到全局的点云数据：

在拼接过程中要将各个单帧点云与该关键帧进行数据关联，局部BA与闭环校正后会更新关键帧的位姿估计结果，此时需要将该帧对应的点云数据从全局稠密点云地图中进行删除，并结合优化后的位姿再次转换并拼接，避免全局稠密点云图中出现重影，获得了全局稠密点云地图后再次通过体素滤波进行降采样，得到基于ORB-SLAM3的稠密点云地图，单帧和拼接后点云结果如图1所示。

图  1  三维点云图

Fig.  1  3D point cloud map

下载: 全尺寸图片

2.   基于目标检测与极线约束的动态特征筛选

本研究基于YOLOv5s网络模型提取关键帧中的语义信息，结合语义信息与极线约束对关键帧中的动态特征点进行去除，以提升本研究算法在动态场景下的性能，考虑到YOLOv5s网络目标识别技术较为成熟，获得目标语义信息的过程本研究不做赘述。去除动态特征点流程如图2所示。

图  2  动态特征点去除流程

Fig.  2  Dynamic feature point removal flow chart

下载: 全尺寸图片

通过目标检测所获得的语义信息仅能代表该目标运动的可能性，为此本研究设计基于语义信息的动态特征初筛选策略为

式中：$P$、$Q$、$R$均为二值逻辑的命题变量，$P$为特征点处于动态物体框内，$Q$为特征点处于静态物体框内，$R$为判定为动态特征点；$\wedge$为逻辑与，${{\neg }}$为逻辑非。

通过式(5)的初步筛选方法，仅当该特征点位于动态物体框且不在静态物体框内时，将其判定为动态特征点，能够减少部分特征点静动属性的误判断，但是仍有部分特征点被误判断。本研究通过光流法获取匹配的特征点对，并根据匹配的特征点对计算基础矩阵，通过基础矩阵和特征点计算特征点对应的极线，当特征点到极线的距离大于一定值，即判断为动态特征点。假设一对匹配特征点${{\boldsymbol{P}}_1}$和${{\boldsymbol{P}}_2}$的齐次坐标为${{\boldsymbol{P}}_1} = [{u_1},{v_1},1]$，${{\boldsymbol{P}}_2} = [{u_2},{v_2},1]$。其中$u$和$v$为像素横纵坐标，根据基础矩阵F，前一个关键帧中特征点${{\boldsymbol{P}}_{\text{1}}}$与当前关键帧中极线$l$的关系可表示为下式

式中：$A、B、C$为三维空间中极线$l$的方程系数，当前关键帧上匹配的特征点${{\boldsymbol{P}}_2}$，其到极线的距离$D$为

若式(7)计算的距离$D$大于阈值$\varepsilon$，认定该点为正在发生运动的动态特征点，加入到需要去除的动态特征点集合，予以去除。

本研究选择动态场景中相邻的2个关键帧，经过基于语义信息初步筛选后，对每一个动态特征点开展本研究提出的极线约束动态特征检测，检测及动态特征去除结果如图3所示。图3（a）中，红色特征点和绿色特征点分别为识别到的动态特征点和静态特征点，紫色线框中特征点为极线约束回收的动态特征点，最终去除结果如图3（b）所示，图3（a）到图3（b）行人发生了一定的位置移动，部分回收特征点被遮挡。

图  3  动态特征点去除结果

Fig.  3  Dynamic feature point removal results

下载: 全尺寸图片

3.   基于语义信息与点云分割的三维语义地图构建

本研究将结合VI-SLAM算法获取的二维语义信息与稠密点云地图构建线程，将其映射到三维空间形成三维语义标签，利用空间点云的颜色属性保存其对应的语义数据，并与三维稠密点云地图的分割结果相融合，进而构建出三维语义地图。

3.1   三维语义标签构建方法

通过训练后的YOLOv5s网络模型对关键帧进行检测，获得关键帧中物体类别与其在图像中的位置，完成对环境的二维语义信息提取，将关键帧对应的RGB图中该检测框内所有像素点颜色对应的RGB值，更改为三维语义标签对应的颜色，结合关键帧对应的位姿与深度图进行稠密点云地图构建，便可将二维语义信息转换为三维语义标签。

3.2   基于聚类及最小割的点云分割算法

基于超体素聚类的分割方法，对原始的稠密点云地图开展分割，根据点的相似性，将无规则的点云转换为面结构，经过超体素分割之后形成的曲面都有一个质心与一个法向量，点云分割可被定义为最小割问题，采用最小割的方法进行进一步分割。

对本研究构建的稠密点云地图进行超体素聚类分割，设置Rvoxel=0.008（体素大小），Rseed=0.05 m（种子分辨率），根据实验室环境调整颜色、空间距离和法向量所占的权重，获得的超体素聚类分割结果的邻接图。由于设置的种子是同时进行生长的，最后的分割结果中会造成一个物体被分割成多个区域，为了解决这种问题，将对超体素聚类分割的结果基于表面块的几何信息进行最小割，以获得更为精确的物体目标点云。

使用RANSAC算法处理曲面块以生成候选的实际平面${\boldsymbol{P}}_{\text{C}} = \{ p_{{\text{c}}_1},p_{{\text{c}}_2}, \cdot \cdot \cdot, p_{{\text{c}}_m}\}$，计算$d({c_i},p_{{\text{c}}_m})$，即各个曲面块质心${c_i}$到候选平面$p_{{\text{c}}_m}$的距离，增加一个阈值$\delta$，用于筛选所有到平面$p_{{\text{c}}_m}$距离在$\delta$内的曲面块，将满足条件的曲面块视为集合${{\varPi}} = \{ {v_i} \in V|d({c_i},p_{{\text{c}}_m}) < \delta \}$。定义

式中：$\eta$表示区分前景物体与背景的约束条件，可以理解为一个合格的实际平面应该至少有$\eta$个曲面块满足到该平面的距离小于$\delta$；$D(p_{{\text{c}}_m})$为平面$p_{{\text{c}}_m}$是否为实际平面的权重，值越大，代表到该平面距离近的曲面块数量越多，则$p_{{\text{c}}_m}$更可能为实际平面，应该将满足条件的曲面块分配给$p_{{\text{c}}_m}$。在实验中，设置$\eta = 30$和$\delta = 0.02{\text{ m}}$。图分割问题的最小化能量${P^*}$可表示为

$E(P)$为拟合的能量：

基于上述算法，可获得满足条件的预选实际平面集合${{P}}$与对应的曲面块集合${{M}}$，利用图割法最小化能量函数，将曲面块分配到实际平面。上述算法过程中${{V}}$与${{E}}$为点云最小割中的顶点与边的集合，对应了聚类分割邻接图中的${{G}} = \{ v,\varepsilon \}$，基于点云最小割理论，找出分割代价最小的分割线进行分割，即能量$E({{P}})$最小化时的边，便可根据上述的约束条件实现将各个曲面块基于空间几何信息合并为实际平面，得到更为精确的点云分割结果，实现将超体素聚类分割结果中属于同一类物体的曲面块合并。

3.3   融合语义特征与点云特征的地图构建方法

在点云分割结果中，每个聚类均有一个属性，以最终随机赋予的颜色作为区分，构建的三维语义标签与点云分割结果中点云的数量与空间坐标是完全一致的，其中语义标签中的颜色信息代表了三维语义信息，将二者融合便可获得更为精准的三维语义地图。

基于语义信息对点云分割的结果进行优化，优化策略主要有以下2点。

1)当点云分割结果中成功将该目标物体分割出来，此时该聚类中带有三维语义标签的点云超过75%，便会将整个聚类中的点云都赋予该三维语义标签。

2)当点云分割结果中未能成功分割出目标物体，仍然会结合三维语义标签为点云添加语义信息，但此时不会更改该聚类中全部点云的属性。

设置一个阈值$\alpha$，当该聚类中带有不同颜色属性点云数超过$\alpha$，就将该聚类基于颜色信息拆分为2个聚类；若点云数未超过$\alpha$，会删除这些点云带有的语义信息，即将颜色改为聚类的颜色。通过上述方法，最终得出的三维语义地图能够去除三维语义标签中的错误信息，并能优化点云分割结果，将点云分割中未分割出来的部分目标点云，基于三维语义标签分割出来，最终语义地图如图4所示。

图  4  三维语义地图

Fig.  4  Three-dimensional semantic map

下载: 全尺寸图片

由图4可以看出，通过语义标签与点云分割结果的融合，物体TV能够分割得更为完整并且成功添加了语义信息，背景墙面上的三维语义标签也被成功去除。基于三维语义地图的优化策略，小物体鼠标也被成功添加了语义信息。

4.   基于机器人平台语义VI-SLAM实验

首先，基于公开数据集EuRoC来验证本研究的VI-SLAM算法精度，并与ORB-SLAM3的位姿估计结果进行对比，分析本研究算法在RGB-D信息与IMU信息输入时的定位精度；然后，基于公开数据集TUM RGBD来验证本研究算法在动态场景中的定位精度，并与ORB-SLAM3、DS-SLAM算法进行对比；最后，搭建移动机器人作为实验平台，基于真实场景中的室内环境进行语义地图构建实验，并对构建的语义地图进行分析。

4.1   静态场景中基于RGB-D及IMU数据的机器人定位实验

由于公开的RGB-D与IMU融合的数据集并未提供机器人的真实轨迹位姿，为解决此问题，本研究实验将采用EuRoC公开的数据集进行测试，EuRoC数据集是由一个双目惯性相机测量得到的，相机频率是20 Hz，IMU频率为200 Hz。基于三角测量法根据左右眼的灰度图生成深度图并与右眼图像进行对齐，将预处理后的EuRoC数据集室内场景用于系统的性能分析，该数据集于静态场景中录制。

4.1.1   静态场景性能分析实验

EuRoC提供的视觉惯性数据集分为简单、中等、困难3个等级，在所有室内场景中进行测试，部分实验结果如图5所示，曲线数值是算法估计的机器人运动轨迹与数据集提供的真实轨迹的绝对轨迹误差（absolute pose error, APE），所有数据集下的误差对比见表1。

图  5  不同静态数据集轨迹与APE误差

Fig.  5  Trajectory and APE error of different static data sets

下载: 全尺寸图片

如表1所示，RMSE为APE均方根误差，其平均值为0.106 m，其值越小说明位姿估计的结果越好。由此可知，本研究设计的VI-SLAM系统在通过视觉惯性传感器数据融合后，于静态场景中位姿估计的平均误差在0.080 m，并且无论是在简单、中等还是困难模式下，系统都能正常运行。

4.1.2   静态场景性能对比实验

为了验证本研究方法在静态场景中基于视觉惯性的定位性能，设计如下对比实验：将本研究算法基于RGB-D信息与IMU信息输入时的位姿估计结果，与ORB-SLAM3系统双目惯性模式下直接使用该数据集的位姿估计结果进行对比分析。图6分别为简单、中等、困难3种室内场景中的位姿估计的结果对比，其他算法与本研究算法在各个室内场景中的均方根误差对比，见表2。

图  6  不同静态数据集中与ORB-SLAM3位姿估计对比

Fig.  6  Comparison with ORB-SLAM3

下载: 全尺寸图片

由表2可知，本研究算法基于视觉惯性定位的均方根误差平均值为0.106 m，与ORB-SLAM3算法的定位精度接近，与OKVIS与VINS-Fusion相比，定位精度更高。

4.2   动态场景中基于RGB-D信息的机器人定位实验

本研究中动态场景分为2种，分别是低动态场景和高动态场景。低动态场景为运动物体少、物体运动速度慢、背景和环境结构相对稳定的场景；高动态场景为运动物体多、物体运动速度快、背景和环境结构会存在显著形变或运动的场景。考虑到目前没有同时录制RGB-D与IMU的数据集，本研究开展只有RGB-D信息输入时也能进行语义信息提取与动态特征去除工作，其中TUM RGB-D数据集是包含RGB图与深度图且在动态场景办公室环境中录制的数据集，sitting数据集中人的动作幅度较小，对应了低动态场景，而walking数据集中，人始终处于行走状态，对应高动态场景。

4.2.1   动态场景性能分析实验

本研究算法在不同动态场景中的误差结果如图7所示。

图  7  不同动态数据集下APE误差

Fig.  7  APE error graph under different dynamic data sets

下载: 全尺寸图片

由图7（a）可知，在低动态场景s_halfsphere中，本研究算法的APE平均值为0.019 m，APE的RMSE为0.022 m。而由图7（b）和图7（c）可知，高动态场景中的APE平均值均为0.018 m，相应的RMSE分别为0.021 m与0.020 m。其他动态场景误差见表3。

由表3可知，本研究所设计的动态去除方法在高、低动态场景均有较好表现，在动态场景中绝对轨迹误差的平均值为0.017 m，均方根误差为0.019 m。

4.2.2   动态场景性能对比实验

基于高动态场景与低动态场景的数据集，对比了ORB-SLAM3、DS-SLAM与本研究算法在动态场景中的全局轨迹误差，如图8所示。

图  8  不同动态数据集中对比结果

Fig.  8  Compare the results of different dynamic data sets

下载: 全尺寸图片

由图8可看出，加入本研究算法之后，系统在动态场景中的位姿估计精度得到了明显提升，与基于ORB-SLAM2和SegNet网络分割算法的DS-SLAM算法相比，在定位精度上相差不大，在低动态场景即sitting数据集中，本研究算法相较于ORB-SLAM3算法在精度上基本一致，在高动态场景即walking数据集中，本研究算法相较于ORB-SLAM3算法误差均值和均方根误差都大幅降低，绝对轨迹误差的均方根误差改进值平均为85%。本研究算法在动态场景中的精度略高于DS-SLAM，绝对轨迹均方根误差为0.020 m，DS-SLAM为0.034 m，因此可验证本研究结合语义信息进行动态特征去除方法的有效性。

综上所述，本研究改进后的VI-SLAM算法适用于绝大多数室内场景，且整体的定位精度无论是在动态场景还是静态场景中都很高，完全能够满足室内移动机器人的定位需求。

4.3   基于RGB-D及IMU数据的机器人建图实验

4.3.1   移动机器人平台

搭建室内移动机器人平台开展实验，将D435i相机固定在机器人顶部，使其在运动过程中能够获取桌面以上空间中的环境信息。利用ROS主从通信机制，通过ROS话题发布D435i视觉惯性传感器采集的RGB-D信息与IMU信息，机器人平台和实验环境如图9所示。

图  9  机器人平台和实验环境

Fig.  9  Robot and experimental environment

下载: 全尺寸图片

4.3.2   基于移动机器人的室内语义地图构建实验

基于室内机器人进行了三维语义地图构建实验，根据SLAM系统关键帧对应的RGB图、深度图和位姿信息，构建出实验室的全局稠密点云地图。在实验过程中，总共生成了312个关键帧，并通过将映射到空间中的三维语义标签与经过精细处理和点云分割后的稠密点云地图进行融合，从而最终生成具有丰富语义信息的稠密点云地图，即语义地图，如图10所示。

图  10  综合三维语义地图

Fig.  10  Integrate 3D semantic map

下载: 全尺寸图片

通过实验结果可以得知，本研设计的三维语义地图构建方法能够精准地将语义信息添加到对应的物体点云上。然而，由于深度相机提供的深度值存在误差，导致三维稠密点云中部分墙面不在同一个平面上，最终点云分割结果中会出现部分杂乱的点云聚类块。另外，为了使机器人能够直接使用该语义地图进行导航，将大场景下的三维语义地图以八叉树地图的形式存储，只保留并染色具有语义信息的点云的颜色。

5.   结束语

本研究提出了一种基于特征融合及动态背景去除的室内机器人语义VI-SLAM算法，旨在提升机器人在复杂环境下的定位精度和环境理解能力。该算法充分利用了RGB-D信息和IMU传感器的位姿估计数据，能够实时构建具有语义信息的室内三维点云地图。主要结论如下：1）通过去除关键帧中的动态特征点，该方法在公开数据集上的性能评估中表现出色，尤其是在高动态场景中，定位精度显著提升，达到近85%的提升幅度，绝对轨迹误差控制在0.020 m以内，这表明该算法在处理复杂场景时具有较高的鲁棒性和精确性。2）算法将关键帧中的二维语义信息映射到三维空间，并与三维稠密点云分割结果进行融合，确保了语义地图的实时构建。在本研究设定的实验中，YOLO v5s目标检测模型达到了45 Hz的高帧率，与传统的ORB-SLAM3系统相比，表现出明显的性能优势。此外，虽然本研究所使用的深度相机在像素深度测量范围上存在一定限制，但未来工作将考虑采用双目相机，以进一步提高系统的深度感知能力和适用范围。