CN111360780A - 一种基于视觉语义slam的垃圾捡拾机器人 - Google Patents

Abstract

本发明公开了一种基于视觉语义SLAM的垃圾捡拾机器人，包括机器人本体和视觉语义SLAM系统。本体包括底盘、设于底盘上方的集成仓、底盘上的行走组件、垃圾回收机构、升降机构、机械臂、摄像机。本发明利用导航摄像机获取图像信息，惯性测量传感器能检测障碍物信息，图像信息和机器人运动时的加速度和角速度信息传给SLAM系统作出决策，控制垃圾捡拾机器人进行地图构建、定位、导航和动态避障。视觉语义SLAM系统通过构建语义地图能够识别垃圾并对垃圾进行分类，机械臂控制系统控制机械臂捡拾垃圾并放入对应的垃圾回收装置中。另外还解决了语义地图构建过程中物体边界不够精确及场景中出现运动物体时系统精确性与鲁棒性降低的问题。

Description

一种基于视觉语义SLAM的垃圾捡拾机器人

技术领域

本发明涉及智能机器人技术领域，具体为一种基于视觉语义SLAM的垃圾捡拾机器人。

背景技术

移动机器人想要实现应用，一个关键问题在于实现自主定位和环境感知。在许多涉及到机器人应用的场景中，比如路径规划、环境感知、避障等，定位与建图的过程被视作是先决条件。同时定位与地图构建(Simultaneous Localization and Mapping，简称SLAM)技术可以在搭载传感器后，在没有环境先验信息的情况下，于运动过程中建立环境的地图并同时进行自主定位。用相机作为传感器时，成本较低而且容易放到商品硬件上。因此，视觉SLAM技术已能适用于大部分环境，且具有实时性强、性价比高和鲁棒性强等特点。

传统的视觉SLAM方法构建的地图只是栅格地图或拓扑地图，这些地图无法提供周围环境中物体的语义信息，相机获取的图像信息未能得到充分利用。语义在此处泛指机器人对周围环境内容的理解，例如了解环境中的物体类别以及它们的关系等。机器人能正确地理解环境的高级语义信息再结合传统的SLAM算法便可以构建出可用的语义地图。同时，传统的视觉SLAM方法只适用于静态环境，当环境中出现运动物体时，运动物体会对算法的精度与鲁棒性造成较大影响，但场景中的运动物体常常不可避免，需对其加以处理。

语义地图中获取环境语义信息的关键在于对环境中物体目标的精准识别，而近年兴起的深度学习技术恰好是当前最具潜力和优势的物体识别方法，因此深度学习和传统SLAM 方法的结合受到领域内研究者的广泛关注。Mask R-CNN图像分割模型是近年来十分具有代表性的基于深度学习的实例分割算法，可以被应用于目标检测、实例分割和目标关键点检测等领域，将Mask R-CNN与传统的视觉SLAM进行结合能达到构建实例级语义地图的目的。但通常大量的物体会有复杂且不规则的边界形状，边界形状的精确预测对于整个实例的分割影响重大，Mask R-CNN算法对于精准识别略有欠缺，需加以优化。

调查发现因为垃圾回收不充分，每分钟至少有15吨塑料垃圾流入海洋，这些垃圾不仅对环境造成破坏，也对野生动物开始产生影响，因吞食垃圾导致动物死亡的新闻屡见不鲜。最近的研究甚至也在人类的粪便中发现了微塑料。目前的垃圾回收基本全靠人工，然而垃圾回收工作任务繁琐且过程重复，而且一些垃圾的分布位置分散且危险。同时，一些垃圾也具有一定危害性，并不适合人工操作。要回收野外那些数量庞大且分布分散的垃圾，就要花费巨大的人力物力。这时候采取机器人进行垃圾捡拾，既可切实解决实际问题，也节省了大量人力物力。尤其像施工现场这样产生垃圾种类多且环境复杂的地方，垃圾捡拾机器人可有效解决垃圾污染问题。

为此，我们推出一种基于视觉语义SLAM的垃圾捡拾机器人。

发明内容

本发明要解决的问题是：一些垃圾不便于人工进行回收，为节省人力物力、缓解垃圾污染问题，提供一种全自动的、高识别率的基于语义视觉SLAM的垃圾捡拾机器人。

为实现上述目的，本发明解决其问题的技术方案是：提供一种基于视觉语义SLAM的垃圾捡拾机器人，包括：机器人本体和视觉语义SLAM系统。

机器人本体结构如图1所示：包括底盘(2)、底盘上安装的行走组件(1)、设于底盘上方的集成仓(3)、垃圾回收机构(4)、升降机构(5)、机械臂(7)、导航摄像机(9)。所述集成仓(3)内设有微型计算机、惯性测量传感器以及安装在微型计算机上的视觉语义SLAM系统；底盘(2)上安装有受所述视觉语义SLAM系统控制的行走组件(1)；垃圾回收机构(4)位于集成仓(3)上方，用于分类收集垃圾；升降机构(5)，其控制升降支架(6)的升降，机械臂(7)铰接于所述升降支架(6)上，所述机械臂(7)的末端设有抓取装置(8)，机械臂(7)受微型计算机控制；导航摄像机(9)设置于升降支架(6) 顶端。

作为上述技术方案的进一步改进，所述集成仓为箱体状构件，固接于所述底盘上。所述集成仓内部还设有受所述视觉语义SLAM系统控制的电源、开关，所述开关设于集成仓箱体左侧，所述微型计算机设于箱体底部，计算机所述惯性测量传感器设于集成仓箱体前侧，集成仓箱体后侧上则设置电源以及充电电源接口。

作为上述技术方案的进一步改进，所述底盘上的行走组件包括设于底盘下方的万向轮，设于底盘内的两个电动机和两个共轴的驱动轮，两个驱动轮的下端穿过底盘与万向轮的下端共水平面。电动机与驱动轮之间设有联轴器，电动机控制联轴器与驱动轮的传动轴联动。两个驱动轮由电动机各自驱动，驱动轮由视觉语义SLAM系统控制，使驱动轮能以不同的线速度转动，配合万向轮实现灵活转向。

作为上述技术方案的进一步改进，所述垃圾回收机构由四个可移动收集箱组成，分别收集可回收物、有害物、厨余、其他垃圾，所述四个可移动收集箱并排固定于集成仓箱体上板，集成仓箱体上板设有滑轨、受视觉语义SLAM系统控制的电动推杆，可移动收集箱与集成仓上板通过滑轨活动连接，电动推杆的两端分别与集成仓上板、可移动收集箱连接，使得电动推杆伸长时，可移动收集箱相对集成仓上板移动。可移动收集箱盖子上安装感应装置，感应收集的垃圾容量。

作为上述技术方案的进一步改进，所述升降机构包括升降支架、连接件和升降轨道，所述升降机构由下至上设置，固接于所述底盘上，侧靠于所述集成仓旁，升降支架由视觉语义SLAM系统控制升降。

作为上述技术方案的进一步改进，所述机械臂铰接于所述升降支架上方，所述机械臂的末端设有抓取装置，机械臂受微型计算机控制。

作为上述技术方案的进一步改进，还设置有用于显示垃圾收集容量、运行信息、电量等信息的显示屏，其设置于集成仓外部正面。

视觉语义SLAM系统架构如图4所示。主要分为：基于相机数据源的动态视觉SLAM进程、基于精细实例分割的实例识别进程和目标关联进程三个部分。动态视觉SLAM进程实现了机器人的自定位和三维点云地图的构建；基于精细实例分割的实例识别进程对关键帧进行实例分割获得关键帧中的所有目标实例；同时目标关联进程进行物品识别和目标关联，将携带物体语义标注信息的关键帧映射到三维点云地图中去，实现面向物体实例的三维语义地图的构建。

作为上述技术方案的进一步改进，在视觉SLAM进程中，从相机传感器中读取拍摄的图像序列，对图像信息进行预处理；对关键帧进行特征点提取和动态点检测；同时在视觉里程计中通过对每帧图像进行特征匹配和跟踪来估算两个时刻图像间相机的相对运动，以及局部地图的样子；然后通过回环检测提供空间上的约束来消除累积误差；后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化处理，得到全局一致的轨迹；根据运动轨迹，建立点云地图。

作为上述技术方案的进一步改进，在实例分割进程中以基于深度学习的Mask R-CNN 实例分割模型为基础，在原有的网络结构基础上添加边界细化模块，同时优化损失函数，然后通过使用专门的垃圾数据集TACO对改进的Mask R-CNN进行训练，使用训练好的模型对获取的图像进行实例分割，以获得当前图像中的垃圾目标实例和语义标签。

作为上述技术方案的进一步改进，对于每个已获得的实例计算其特征描述向量，在目标关联线程中完成目标关联，将图像的像素级语义标注结果映射到对应的点云地图上。

本发明的有益效果是：本发明通过在可移动的机器人本体上设置视觉语义SLAM系统，充分利用视觉传感器捕捉到的丰富的图像信息，由视觉语义SLAM系统控制垃圾捡拾机器人进行语义地图构建，实现机器人的定位、导航和动态避障，使垃圾捡拾机器人能在复杂环境下，以自主巡航的方式回收垃圾；同时，通过对图片语义信息的利用，机器人在捡拾垃圾的同时能自动识别垃圾的种类；另外解决了实例分割中对于物体边界识别不够精确，以及对场景中动态物体的识别的问题。本发明在解决垃圾处理人力不足问题的同时，也克服现有垃圾拾捡装置的单一性与低效性，可以用于山区、景区的垃圾拾捡，能够有效地对各类垃圾进行回收，极大地提高了垃圾回收的效率和安全性，节省了大量人力物力。

附图说明

图1是本发明的机器人本体结构图；

图2是本发明的整体系统架构图；

图3是传统的视觉SLAM流程图；

图4是本发明的视觉语义SLAM系统架构图；

图5是加入边界细化模块后的分割模块示意图；

图6是改进的Mask R-CNN网络框架结构图；

图7是运动物体去除算法框架图.

具体实施方式

为了使本发明所实现的技术手段、达成目的与功效易于明白理解，下面结合附图对本发明做进一步阐述。

本发明所公开的基于视觉语义SLAM的垃圾捡拾机器人，包括机器人本体和视觉语义 SLAM控制系统两个部分。

其机器人本体如图1所示，主要包括底盘(2)，底盘(2)上还设有受微型计算机控制的行走组件1；设于底盘(2)上方的集成仓(3)，所述集成仓(3)内设有微型计算机、惯性测量传感器以及安装在微型计算机上的视觉语义SLAM系统；集成仓(3)上方设置垃圾回收机构(4)，垃圾回收机构具体包括可回收垃圾、有害垃圾、厨余垃圾、其他垃圾四个部分；升降机构(5)设置于垃圾回收机构(4)上，固接于集成仓(3)上，其包括有升降支架、连接件和升降轨道；机械臂(7)连接于升降支架(6)顶端，机械臂的末端设有抓取装置(8)；导航摄像机(9)设置于升降支架(6)顶部。

本发明的整体系统架构图如图2所示，微型计算机接收用户发来的指令开始执行动作，导航摄像机直接捕获周围环境的图像信息，并将图像信息输入视觉语义SLAM系统，SLAM 系统开始建立目标在三维空间的位置信息，并检测识别环境中的垃圾信息。一旦识别到环境中的垃圾信息，发送指令给行走控制系统，行走控制系统的核心模块由两个电机、驱动电路、编码器和通信电路组成，主要完成对驱动轮的运行控制。按照SLAM系统构建的语义地图进行路径规划，驱动机器人本体进行移动。当移动到垃圾所在位置时，反馈各个电机的状态给微型计算机做分析判断。然后微型计算机调用机械臂控制系统进行物体抓取，同时按照视觉语义SLAM系统在对图像进行语义分割时传出的语义标签对垃圾进行分类，将垃圾投入到对应的垃圾收集箱中。当垃圾收集箱容量满时，收集箱盖子上的感应装置感应到收集箱装满状态，感应装置传回信息给微型计算机，由视觉语义SLAM系统控制机器人本体回到指定地点进行垃圾集中处理。

应当说明的是，本发明的垃圾捡拾机器人，根据使用场景的不同，其导航摄像机(9) 可以根据实际应用需要选择单目相机、双目(多目)相机、深度相机这常用的三类，另还有鱼眼、全景等特殊相机。单目相机简单且成本低廉，但无法得到确切的深度。双目相机可以在运动时估计深度，但双目或多目相机配置与标定较为复杂，且计算量大。深度相机可以通过红外结构光或TOF原理，直接测出图像中各像素离相机的距离，但容易受日光和材质的干扰。

在实际使用场景下，用户可将本发明的垃圾捡拾机器人，置于待操作场景内的出发点位置，通过指令输入待巡视区域、路径规划、行走计划等参数，机器人自动完成垃圾捡拾工作。

传统的视觉SLAM流程如图3所示，主要包括以下几个步骤：

传感器信息读取。从相机中获取机器人运行过程中拍摄的图像序列，对图像信息进行读取和预处理，同时读取和同步惯性传感器的信息。

视觉里程计。通过对每帧图像进行特征点提取、匹配和跟踪来估算两个时刻图像间相机的相对运动，以及局部地图的样子。

回环检测。回环检测会判断机器人是否到达过先前的位置，如果检测到同一场景不同时刻的图像，它会把信息提供给后端进行处理，提供了空间上约束来消除累积误差。

后端优化。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，处理视觉里程计估计结果的累积误差，得到全局一致的轨迹和地图。

建图。根据视觉里程计和后端优化估计的运动轨迹，建立与任务要求对应的地图。

基于本发明的垃圾捡拾机器人不仅要捡拾垃圾，还要对垃圾直接进行分类收集，我们需要建立具有语义信息的语义地图，因此在传统的视觉SLAM流程中融合Mask R-CNN实例分割方法，并在此基础上改进Mask R-CNN方法，使其能识别细小的物体，同时对运动物体加以处理，使其适用于动态场景。在机器人的实际运行过程中，视觉语义SLAM系统的系统架构如图4所示。

系统框架主要分为基于相机数据源的动态视觉SLAM进程和基于精细实例分割的实例识别进程和目标关联进程三个部分。动态视觉SLAM进程基于获取的图像信息进行特征提取、动态点检测与特征匹配，完成视觉里程计、后端非线性优化、回环检测、建图(几何地图)等工作，实现了机器人的自定位和三维点云地图的构建。实例识别进程则对选取的关键帧进行实例分割获得关键帧中的所有目标实例和语义标签。同时目标关联进程利用视觉SLAM计算得到关键帧的相机位姿，结合关键帧相机位姿和实例的特征描述向量进行物品识别和目标关联，以决定是否创建新的对象或者跟已有对象建立数据关联，然后利用视觉SLAM将携带物体语义标注信息的关键帧映射到三维点云地图中去，实现面向物体实例的三维语义地图的构建。

具体的方法包括：

步骤一：提取关键帧。选取关键帧目前是一种非常常用的方法，关键帧可以代表其附近的帧，减少了待优化的帧数。

选择关键帧主要从关键帧自身和关键帧与其他关键帧的关系两方面来考虑。一方面，关键帧自身质量要好，例如不能是非常模糊的图像、特征点数量要充足、特征点分布要尽量均匀等等；另一方面，关键帧与其他关键帧之间的关系，需要和局部地图中的其他关键帧有少量的共视关系，但大部分特征点是新特征点，以达到既存在约束，又尽量少的信息冗余的效果。

具体实施中是否需要将当前帧创建为关键帧，这里采用ORB_SLAM2中关键帧的选取标准：

(1)距离上一次重定位距离至少20帧；

(2)局部建图线程空闲，或者距离上一次加入关键帧过去了20帧,，如果需要关键帧插入(过了20帧)而局部建图线程忙，则发送信号给局部建图线程，停止局部地图优化，使得新的关键帧可以被及时处理；

(3)当前帧跟踪至少50个点，确保跟踪定位的精确度；

(4)当前帧跟踪到局部地图中参考帧的地图点数量少于90％，确保关键帧之间有明显的视觉变化。

步骤二：对关键帧进行实例分割，获取每帧关键帧中的所有的物体实例。

实例分割是将语义分割和目标检测相结合的技术，与类似的计算机视觉任务相比，这是最困难的视觉任务之一。相对目标检测的边界框，实例分割可精确到物体的边缘；相对语义分割，实例分割能够区分出图上同一物体的不同个体。这使得实例分割的应用场景更加广泛。

经典实例分割算法Mask R-CNN能在进行目标检测的同时进行实例分割，并取得了出色的效果。Mask R-CNN是在Faster R-CNN的基础上进行改进,在原来的两个分支上(分类+回归)增加了一个语义分割分支(分类+回归+分割)，即在每个候选对象RoI的基础上添加了FCN用于预测分割掩码从而产生对应的MASK分支。并用采用双线性插值法的 RoIAlign代替RoIPooling的取整量化，减小了误差，完成像素级的对齐。

Mask R-CNN是一个两阶段的框架，第一个阶段扫描图像并生成提议(proposals，即有可能包含一个目标的区域)，第二阶段分类提议并生成边界框和掩码。具体流程如下：输入提取的关键帧图像；将整张图片输入主干网络ResNet中，进行特征提取，生成特征图；得到特征图后，RPN网络会在特征图中生成候选区域，并对每个候选区域进行框回归操作和得到类别可能性；对每一个RPN得到的候选区域，进行RoIAlign操作，通过RoIAlign 层使每个RoI生成固定尺寸的特征图；这些图随后被用于两个通路：一个通路用于生成回归框和预测类别；另一路是Mask通路，用于生成Mask。

然而通常情况下，大量的物体会有复杂且不规则的边界形状，边界形状的精确预测对于垃圾捡拾机器人的操作效果影响重大。因此，为了提高边缘的分割精度，在Mask通路后添加了一个边界细化模块(即Boundary refinement模块)，该模块的设计参考了ResNet的残差块的设计，详情如图5所示。这里主要是通过一个残差结构对生成的掩膜进行细化，以进一步拟合实例边界，该模块在初步得到实例分割的分割掩码之后使用，可以使边界处的定位能力大大提升。

进一步的，在实例分割任务中常用的损失函数是联合训练损失函数：

L＝L_cls+L_box+L_mask

其中，L_cls为目标检测任务中对于兴趣区域目标类别预测的交叉熵，表示分类误差； L_box为目标检测中对于兴趣区域的位置修正损失函数，表示检测误差；L_mask为语义分割任务中预测的掩码与实际掩码的逐像素交叉熵之和，表示分割误差。在Mask RCNN的分割任务中，L_mask为平均二值交叉熵损失函数，其公式具体表示为：

其中y表示经过二值化后的ground-truth，

表示经过二值化后预测的分割结果。

但是，交叉熵损失函数在分割任务中的不足是依赖于区域信息，从而忽视了边界的预测，使在最后的分割结果上对边界分割准确度不高。所以提出在L_mask中加入BWL(边界加权损失函数)。L_mask-bwl为优化后的公式，如下：

其中α表示权重系数，B表示分割结果的边界，R表示整个分割区域，M_dist表示对ground-truth分割边框的距离变换，相当于一个距离图。这样就相当于在L_mask增加了边框损失的权重，可以使分割结果更加精确。

在训练神经网络的过程中，往往需要在多次正向反向迭代的过程中找到合适的权重，但常常因为数据集获取难度大、硬件昂贵等因素造成训练难度的增加。为了避免此类问题出现，这里采用迁移学习的方式通过使用之前在大数据集上经过训练的预训练模型，将其相应的结构和权重应用到我们正在面对的问题上。在选择预训练模型的时候需要仔细斟酌，如果问题与预训练模型训练情景下有很大的出入，模型所得到的预测结果将会非常不准确。这里选择使用COCO数据集的预训练模型的结构将所有的权重随机化，然后依据自己的数据集进行训练。COCO数据集起源于微软2014年出资标注的Microsoft COCO数据集，是一个大型丰富的物体检测、分割。这个数据集主要从复杂的日常场景中截取，对于垃圾问题的检测分割适用性很高，数据集中的图像包括91类目标，328,000影像和2,500,000个label，是目前为止语义分割的最大数据集。

具体实施中，使用改进的Mask R-CNN来对关键帧图像进行实例分割，以获取每帧关键帧图像中的所有实例。改进的Mask R-CNN网络框架结构如图6所示。具体步骤如下：

首先，使用COCO数据集的预训练模型将所有的权重随机化，选择TACO数据集作为我们的训练集进行训练。TACO是一个不断增长的野生废物图像数据集，以COCO数据集的格式提供注释，通用性很强。它包含了在不同环境下拍摄的垃圾图片:森林、道路和海滩。这些图像根据层次分类进行人工标记和分割，以训练和评估目标检测算法。目前，图片都托管在Flickr上，并且一直在更新更多的图片和注释。

然后，利用训练得到权重模型对每帧关键帧进行预测，提取关键帧中所有的物品实例，得到每个实例的目标检测框和剔除背景后的实例像素级掩码。

最后，对关键帧进行实例分割后会获得当前帧中所有的目标实例，对于每个实例需要计算其特征描述向量来完成目标关联，以决定是否在地图中创建新的对象或者跟已有对象建立数据关联。

步骤三：对关键帧中的所有物体实例进行特征点提取，并检测环境中的运动物体。若检测到运动物体，对运动物体进行去除，从而增强系统在动态场景中的精度与鲁棒性。

具体的，特征提取时需要找到图片的特征点，常用的特征点提取方法有SIFT、SURF、 ORB等等。特征点是图像里的一些特别的地方，例如图像中的角点、边缘和区块。特征点由关键点(Key-point)和描述子(Descriptor)两部分组成。关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。

SIFT是最经典的一种，它充分考虑了图像变换过程中光照、尺度、旋转等变化，但是计算量极大。FAST关键点计算特别快，它没有描述子，虽然提高了计算速度但牺牲了精度和健壮性。而ORB特征则是目前SLAM方案中，质量和性能之间较好的折中。因此这里采用ORB进行特征提取，ORB特征提取的整个过程如下：

(1)FAST角点提取——找出图像中的“角点”。在图像中选取像素p，假设他的亮度为I_p；设置一个合适的阈值T(例如，I_p的20％)；以像素p为中心，选取一个半径等于3 像素的离散化的Bresenham圆，这个圆的边界上有16个像素；假如选取的圆上有n个连续的像素点，他们的亮度大于I_p+T或小于I_p-T，那么像素p可以被认为是特征点(n 的值常设为12或者9，分别被称为FAST-9和FAST-11)；循环以上四步，对每一个像素执行相同的操作。

(2)BRIEF描述子：在特征点邻域利用BRIEF算法对前一步提取出的特征点的周围图像区域进行描述。为减少噪声干扰，先对图像进行高斯滤波(方差为2，高斯窗口为9x9)；以特征点为中心，取S×S的邻域窗口，在窗口内随机选取一对(两个)点，比较二者像素的大小，进行如下二进制赋值：

其中，p(x)，p(y)分别是随机点x＝(u1，v1)，y＝(u2，v2)的像素值；在窗口中随机选取N对随机点，重复上一步的二进制赋值，形成一个二进制编码，这个编码就是对特征点的描述，即特征描述子(一般N＝256)。

目前针对动态环境中运动物体的处理分为两种方法：一是在算法中进行动态目标的检测并将其剔除，二是将环境中的动态因素在线的建模到系统模型中。由于将动态物体建模到环境中，并在线对其进行跟踪处理，运算负担较大，很难保证SLAM算法的实时性，因此采用对运动物体进行检测并去除的方式提升SLAM算法在动态环境下的精度与鲁棒性。

由于步骤二中的实例分割可以完成对图像中物体的检测与位置识别，因此可以用于检测场景中的潜在运动物体。对环境中运动物体的检测结合了实例分割网络与稀疏的LK(Lucas-Kanade)金字塔光流法，使用稀疏的金字塔LK光流对图像中一些点进行追踪能够直接得到特征点的对应关系，其不需要描述子计算与特征匹配的过程，因此实时性较好。

Lucas-Kanade光流算法首先假定相机获取的图像是由着时间进行变化的，则图像可以看成时间的函数I(t)。对于一个坐标为(x，y)的像素点，它的灰度值为I(x，y，t)。假设三维空间中的某个固定点在成像I上t时的横纵坐标分别为x和y，随着时间的变化，它的坐标也在变化，光流法目的就是为了预测该三维固定点在不同时候成像里的位置。基于光流法的特征点追踪首先假设灰度是没有变化的，即同个空间点的灰度值随着时间的变化在平面上始终保持一致。假如某像素在t时处于(x，y)的位置，t+dt时处于(x+dx，y+dy) 位置，则它们满足：

I(x，y，t)＝I(x+dx，y+dy，t+dt)

将右边泰勒展开并保留一阶项，可得到：

结合这两个公式得到：

其中

该点灰度值沿x轴的梯度，

为该点灰度值沿y轴的梯度，dx/dt为该点沿x轴的运动速度，dy/dt为该点沿y轴的运动速度。令dx/dt为u，dy/dt为v，

为I_x，

为I_y，

为I_t，则写成矩阵形式为：

为了计算u和v，Lucas-Kanade光流算法假定图像块内的像素运动是一样。最后通过进行多次迭代，就可以获取像素在图像中的运动，从而实现像素点的追踪。

本文采用的运动物体去除算法原理(如图7所示)为：若检测得到的动态点有一定数目位于某一物体的语义分割区域，则该区域对应的物体视为运动物体；在检测到运动物体的情况下，移除运动物体轮廓内的所有ORB特征点后再进行机器人位姿的估计，以此滤除运动物体对SLAM系统的影响；如果环境中没有运动物体，直接利用视觉里程计匹配到 ORB特征点对的进行位姿估计。

步骤四：在视觉里程计中估算相邻图像间相机的运动，以及局部地图的样子。即对每个关键帧图像进行特征描述向量的计算，需要实现特征匹配和估计帧间运动的功能。在视觉里程计估计帧间运动之后，就可以得到机器人的轨迹了。

特征匹配是视觉SLAM中极为关键的一步，特征匹配解决了SLAM中数据关联的问题，也就是确定了当前看到的路标与之前看到的路标之间的对应关系，常用的特征匹配方法有暴力匹配(Brute-Force Matcher)或快速近似最近邻(FLANN)算法。估计帧间运动时，可以使用PnP或者BA算法。

这里在特征匹配过程中采用最简单的匹配方法——暴力匹配。考虑两个时刻的图像，如果在图像I_t中提取到特征点

在图像I_t+1中提取到特征点

对每一个特征点

与所有的

测量描述子的距离，然后排序，取最近的一个作为匹配点。如果匹配点数量极多，则快速近似最近邻算法更加合适，由于这些匹配算法理论已经足够成熟，而且已集成到OpenCV，这里就不再具体描述。

步骤五：融合特征点匹配和实例匹配对SLAM的位姿估计结果进行局部非线性优化，得到携带物体实例语义标注信息的关键帧。对关键帧进行实例分割和特征编码后，获得了每帧关键帧图像内所有物体实例的特征描述向量，可以利用空间金字塔匹配算法来计算两个实例的特征描述向量之间相似度。

但是在机器人一般应用场景中，可能会存在多个形状纹理相同的物体实例，仅利用特征匹配和图像空间金字塔的实例相似度度量方法不能有效区分两个形状纹理和颜色都相同的实例。而在SLAM视频流中相邻帧的实例位置是高度相关的，因此可以利用相邻帧的位姿信息来对实例进行跟踪与匹配。

进行位姿估计时，假设k-1时刻图像帧中特征坐标为

该特征对应的3D地图点在世界坐标系下的坐标为

通过特征匹配，得到k时刻图像帧中对应的特征坐标为

根据位姿变换和投影函数，得到给3D地图点在k时刻图像帧中的坐标。

在之前的过程中只估计了两帧图像间的相机位姿。进一步地，在建立地图的线程中，通过地图点的共视关系，建立局部地图，获取局部地图点，局部关键帧，固定关键帧集合，固定地图点集合。当前关键帧观测到的地图点为局部地图点，观测到局部地图点的关键帧为局部关键帧。被局部关键帧观测到，而没有被当前关键帧观察到的地图点为固定地图点，观测到固定地图点而不属于局部关键帧的关键帧为固定关键帧。通过光束平差法同时优化局部地图中关键帧位姿和地图点的位置，得到局部一致性更优的相机轨迹估计。

通过结合实例的特征描述向量和实例的空间位置相似度对实例进行有效地跟踪与匹配，完成实例的识别与目标关联，从而有效区分场景中形状外观相同的实例，提高实例匹配和物品识别的准确率。

步骤六：将携带物体实例语义标注信息的关键帧映射到实例三维点云中，构建出三维语义地图。

采用OpenGL可以进行点云的融合和更新，进而拼接成全局点云地图。在全局三维点云地图生成的同时，进行语义标注，将关键帧的像素级语义标注结果映射到对应的点云地图上，根据机器人位姿变换矩阵TWC，可将每一个像素点的相机坐标转换为世界坐标，最后根据每个像素点所对应的三维空间坐标，将关键帧图像的二维语义分割结果映射到对应的三维点云地图上，完成三维点云地图的语义标注任务，最终生成我们需要的语义地图。

语义地图构建完成后，本发明提出的垃圾捡拾机器人在运动过程中对周围环境的内容可以理解，比如认识环境中的垃圾、人、以及它们的关系等。机器人从语义地图中可以轻松获取垃圾、人、障碍物的位置和形状信息，不再需要手动设置物品之间的关系，路径规划和导航都将更加智能，人类可以更加简单地向机器人发送指令，剩下的工作则由机器人自动完成。

Claims (10)

1.一种基于视觉语义SLAM的垃圾捡拾机器人，所述的基于视觉语义SLAM的垃圾捡拾机器人包括机器人本体和视觉语义SLAM系统；其特征在于，

所述机器人本体包括底盘、设于底盘上方的集成仓，所述集成仓内设有微型计算机、惯性测量传感器以及安装在微型计算机上的视觉语义SLAM系统，集成仓外设有显示屏，底盘上还设有受微型计算机控制的行走组件；垃圾回收机构设置于集成仓上，其包括可回收垃圾、厨余垃圾、其他垃圾、有害垃圾4个部分；升降机构设置于底盘上、集成仓侧面，其包括有升降支架、连接件和升降轨道；机械臂连接于升降支架顶端，机械臂的末端设有抓取装置，机械臂受微型计算机控制；导航摄像机设置于升降支架顶部；

所述视觉语义SLAM系统由三个进程组成：基于相机数据源的动态视觉SLAM进程、基于精细实例分割的实例识别进程和目标关联进程三个部分；动态视觉SLAM进程实现机器人的自定位和三维点云地图的构建；基于精细实例分割的实例识别进程对关键帧进行实例分割获得关键帧中的所有目标实例和语义信息；目标关联进程进行物品识别和目标关联，将携带物体语义标注信息的关键帧映射到三维点云地图中去，实现面向物体实例的三维语义地图的构建。

2.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述底盘下方设置有行走组件；行走组件包括设于底盘下方的万向轮，设于底盘内的两个电动机和两个共轴的驱动轮，两个驱动轮的下端穿过底盘与万向轮的下端共水平面；电动机与驱动轮之间设有联轴器，电动机控制联轴器与驱动轮的传动轴联动；两个驱动轮由电动机各自驱动，驱动轮由系统控制。

3.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述集成仓为箱体状构件，固接于所述底盘上；所述集成仓内部设有开关、微型计算机、惯性测量传感器、电源；所述开关设于集成仓箱体左侧，所述微型计算机设于箱体底部，计算机所述惯性测量传感器设于集成仓箱体前侧，集成仓箱体后侧上则设置电源以及充电电源接口。

4.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述集成仓外部正面还设置有用于显示垃圾收集容量、运行信息、电量等信息的显示屏。

5.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述垃圾回收机构由四个可移动收集箱组成，分别收集可回收物、有害物、厨余、其他垃圾，所述四个可移动收集箱并排固定于集成仓箱体上板，集成仓箱体上板设有滑轨和电动推杆，可移动收集箱与集成仓上板通过滑轨活动连接，电动推杆的两端分别与集成仓上板、可移动收集箱连接，使得电动推杆伸长时，可移动收集箱相对集成仓上板移动；可移动收集箱盖子上安装感应装置，感应收集的垃圾容量。

6.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述升降机构包括升降支架、连接件和升降轨道，所述升降机构由下至上设置，固接于所述底盘上，侧靠于所述集成仓旁，升降支架由系统控制升降。

7.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述机械臂铰接于所述升降支架上方，所述机械臂的末端设有抓取装置，机械臂受微型计算机控制。

8.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述视觉语义SLAM系统中的动态视觉SLAM进程包括：从相机传感器中读取拍摄的图像序列，对图像信息进行预处理；在传统视觉SLAM的基础上添加动态点检测；在视觉里程计中通过特征点提取、匹配和跟踪来估算两个时刻图像间相机的相对运动，以及局部地图的样子；通过回环检测消除累积误差；后端对视觉里程计测量的相机位姿和回环检测的信息进行优化，得到全局一致的轨迹；根据运动轨迹，建立点云地图。

9.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述视觉语义SLAM系统中的精细实例分割进程包括：以基于深度学习的Mask R-CNN实例分割模型为基础，在原有的网络结构基础上添加边界细化模块，同时优化损失函数，然后通过使用专门的垃圾数据集TACO对改进的Mask R-CNN进行训练，使用训练好的模型对获取的图像进行实例分割，以获得当前图像中的垃圾目标实例和语义标签。

10.根据权利要求1所述的一种基于视觉语义SLAM的垃圾捡拾机器人，其特征在于，所述视觉语义SLAM系统中的目标关联进程包括：对于每个已获得的实例计算其特征描述向量，将图像的像素级语义标注结果映射到对应的点云地图上，完成目标关联。

Images