一种融合点云和图像的轮式机器人语义建图方法及系统与流程

本发明涉及同步定位与建图技术领域，具体涉及一种融合点云和图像的轮式机器人语义建图方法及系统。

背景技术：

随着人工智能的迅猛发展，移动机器人领域正发生翻天覆地的变化，机器人的智能化进程也随之加快。机器人依靠slam算法构建可用于导航的地图这一方法已相当成熟，而当环境十分复杂或处于动态变化之中，机器人需要具备理解场景及其中各个物体部件的能力，语义地图作为这项能力的基本实现方式同时也是智能导航及人机交互的基础。

现有技术中，slam(simultaneouslocalizationandmapping同步定位与建图)技术按使用的传感器可以分为激光slam和视觉slam。

本申请发明人在实施本发明的过程中，发现现有技术的方法，至少存在如下技术问题：

激光slam中，单线激光雷达在低成本服务机器人领域使用广泛但其提取的二维点云信息匮乏以致难以提取语义，多线激光雷达价格昂贵且多用于自动驾驶场景。视觉slam，视觉传感器成本较为低廉，但难以支持slam算法构建可供机器人导航的稠密地图。

由此可知，现有技术中的建图方法构建的地图仅支持机器人导航，包含的信息不够丰富。

技术实现要素：

本发明提出一种融合点云和图像的轮式机器人语义建图方法及系统，用于解决或者至少部分解决现有技术的方法构建的地图信息不够丰富的技术问题。

本发明面向搭载单线激光雷达和相机的轮式机器人，提供一种构建语义地图的方法。以二维点云和图像为输入，输出标记有各个物体的二维栅格地图。该发明为使用低成本室内移动机器人实时构建语义地图提供一种可靠的解决方案，具有非常大的研究和应用价值。

本发明第一方面提供了一种融合点云和图像的轮式机器人语义建图方法，包括：

s1：采用目标检测卷积神经网络从单目相机读取的图像中提取标识物体位置的包围盒及对应的语义类别和置信度；

s2：将单线激光雷达读取的二维点云基于几何特征进行分割，得到点云分割；

s3：将包围盒与点云分割进行匹配，将匹配得到的点云分割、与包围盒对应的语义类别和置信度组合成语义激光；

s4：将语义激光作为输入，采用激光slam算法构建二维栅格地图，在仅包含占据概率的栅格中加入由各类别累计置信度、更新次数组成的语义结构体，通过点云帧间匹配找到语义激光观测到的栅格后，累计指定类别的置信度并将更新次数递增，当前子图构建完成时计算每个栅格中平均置信度最大的语义类别并保存，其中，占据概率表示栅格被障碍物占据的可能性大小；

s5：利用slam的后端优化调整子图和子图所包含的栅格的全局位姿，采用聚类方法对栅格进行聚类，得到最终的轮式机器人语义地图。

在一种实施方式中，s1中去除置信度低于预设值、语义类别不属于常见室内静态物体以及包围盒不经过激光扫描平面的目标检测结果。

在一种实施方式中，s2具体包括：

根据处于同一水平面的激光扫描点之间的几何距离是否大于阈值对二维点云进行分割，同时对处于相机视野内外的两部分点云作区分，将视野外的点云直接用于激光slam建图，将视野内的点云与图像的信息融合再用于slam建图。

在一种实施方式中，s3具体包括：

s3.1：根据预先获得的激光雷达与单目相机之间的标定关系，将分割后的二维点云投影至图像上；

s3.2：将投影后的结果与包围盒进行匹配，根据落入包围盒内的点云投影点数在总点数中的占比，判定点云分割是否与包围盒匹配。

在一种实施方式中，s4具体包括：

s4.1：通过点云帧间匹配找到语义激光观测到的栅格，对于每一个栅格，更新栅格的占据概率，同时累计语义激光对应语义类别的置信度并使更新次数递增；

s4.2：设置子图大小并融入相应数量的点云，子图构建完成后，计算每个栅格平均置信度最大的语义类别，当该类别对应置信度小于阈值时，将该栅格的语义类别视为背景并清除语义信息。

在一种实施方式中，s5具体包括：

s5.1：将具有除背景外语义类别的栅格与所在子图绑定，采用slam后端优化调整子图位姿后获取上述栅格的全局位姿，并在全局地图相应位置进行语义标注，语义标注包括语义类别、物体序号和包围盒；

s5.2：利用基于密度的聚类算法dbscan算法对栅格进行聚类，剔除因错误语义更新出现的离群点，组合具有相同语义类别的邻近栅格并以包围盒标注，最后分配标签号以代表各个独立物体。

基于同样的发明构思，本发明第二方面提供了一种融合点云和图像的轮式机器人语义建图系统，包括：

语义提取模块，用于采用目标检测卷积神经网络从单目相机读取的图像中提取标识物体位置的包围盒及对应的语义类别和置信度；

点云分割模块，用于将单线激光雷达读取的二维点云基于几何特征进行分割，得到点云分割；

语义匹配模块，用于将包围盒与点云分割进行匹配，将匹配得到的点云分割、与包围盒对应的语义类别和置信度组合成语义激光；

二维栅格地图构建模块，用于将语义激光作为输入，采用激光slam算法构建二维栅格地图，在仅包含占据概率的栅格中加入由各类别累计置信度、更新次数组成的语义结构体，通过点云帧间匹配找到语义激光观测到的栅格后，累计指定类别的置信度并将更新次数递增，当前子图构建完成时计算每个栅格中平均置信度最大的语义类别并保存，其中，占据概率表示栅格被障碍物占据的可能性大小；

优化模块，用于利用slam的后端优化调整子图和子图所包含的栅格的全局位姿，采用聚类方法对栅格进行聚类，得到最终的语义地图。

本申请实施例中的上述一个或多个技术方案，至少具有如下一种或多种技术效果：

本发明提供的一种融合点云和图像的轮式机器人语义建图方法，首先利用目标检测卷积神经网络从单目相机读取的图像中提取出物体的语义，包括标识物体位置的包围盒、类别和置信度；然后，将单线激光雷达读取的二维点云基于几何特征进行分割；接着将包围盒与点云分割进行匹配，将匹配得到的点云分割、与包围盒对应的语义类别和置信度组合成语义激光；然后将语义激光作为输入，采用激光slam算法构建二维栅格地图，累计栅格中各类别的语义置信度，所在子图构建完成时计算置信度最大的语义类别，最后利用slam的后端优化调整语义的全局位姿，对包含明确语义类别的栅格进行聚类，生成全局一致的语义地图。

通过本发明的方法，可以将神经网络提取的物体语义与单线激光雷达读取的二维点云进行融合，并形成语义激光用于二维栅格地图的构建，更新二维栅格地图中原有占有概率的同时，累计栅格中各类别的语义置信度，语义激光按照局部slam估计的位姿融入地图，最后利用slam的后端优化调整语义的全局位姿，使得构建的地图具有丰富的语义信息，使得机器人能够突破仅能自主导航的局限，通过对语义地图的信息推理完成智能导航、人机交互等任务。

本发明面向安装单线激光雷达和单目相机的轮式机器人，装置简单、成本低且应用范围广。

本发明使用的轻量级目标检测神经、二维点云分割算法、聚类算法等能够满足常见室内环境下低成本轮式机器人的实时建图需求。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的一种融合点云和图像的轮式机器人语义建图方法的示意图；

图2为本发明实施例的二维点云与图像信息融合模块的物理结构示意图；

图3为本发明采用的二维点云语义提取方法的实例示意图；

图4为本发明提供的语义建图方法在走廊环境下的实例示意图；

图5是本发明提供的一种融合点云和图像的轮式机器人语义建图系统的结构框图。

具体实施方式

本发明针对激光slam构建的地图，包含的信息不够丰富仅支持机器人导航的问题，提供一种融合点云和图像的轮式机器人语义建图方法，具有实时、装置简单、地图信息丰富等优点，旨在实现室内移动机器人智能导航及人机交互。

为了实现上述目的，本发明的主要构思如下：

通过深度学习方法准确地从图像提取语义，融合点云和图像来进行轮式机器人语义建图，本发明的方法是一种从二维点云中提取语义，在二维栅格地图中保存、更新和优化语义的方法，其主要步骤包括图像上基于深度学习的目标检测、点云分割、包围盒与分割的匹配、地图单元语义更新，以及基于slam全局优化和聚类的语义优化。本发明方法具有实时、装置简单、地图信息丰富等优点，旨在实现室内移动机器人智能导航及人机交互。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

本实施例提供了一种融合点云和图像的轮式机器人语义建图方法，该方法包括：

s1：采用目标检测卷积神经网络从单目相机读取的图像中提取标识物体位置的包围盒及对应的语义类别和置信度；

s2：将单线激光雷达读取的二维点云基于几何特征进行分割，得到点云分割；

s3：将包围盒与点云分割进行匹配，将匹配得到的点云分割、与包围盒对应的语义类别和置信度组合成语义激光；

s4：将语义激光作为输入，采用激光slam算法构建二维栅格地图，在仅包含占据概率的栅格中加入由各类别累计置信度、更新次数组成的语义结构体，通过点云帧间匹配找到语义激光观测到的栅格后，累计指定类别的置信度并将更新次数递增，当前子图构建完成时计算每个栅格中平均置信度最大的语义类别并保存，其中，占据概率表示栅格被障碍物占据的可能性大小；

s5：利用slam的后端优化调整子图和子图所包含的栅格的全局位姿，采用聚类方法对栅格进行聚类，得到最终的轮式机器人语义地图。

具体来说，目标检测卷积神经网络可以采用现有的神经网络，提取的结果为物体的语义。

激光slam算法可以为cartographer算法，其运用子图的概念，将得到的点云数据进行匹配，一个子图通常包括若干数量的点云数据。更新栅格占据概率的同时，会统计栅格中每一个类别被语义激光更新的次数。

图1示出了本发明描述的语义建图方法的系统框图，以单目相机采集的rgb图像和单线激光雷达采集的二维点云为输入，点云和图像的信息传输、地图的输出和可视化均通过机器人操作系统(ros)实现。

在一种实施方式中，s1中去除置信度低于预设值、语义类别不属于常见室内静态物体以及包围盒不经过激光扫描平面的目标检测结果。

具体实施过程中，置信度预设值可以根据实际情况设置，例如0.5、0.6、0.7等等。采用的卷积神经网络可以为yolov3的darknet-53网络，获取可识别物体的类别、置信度和标识位置的包围盒，接着将以上信息封装为ros机器人操作系统消息发布到指定话题以供slam节点读取，该节点建有一定长度的缓冲队列保存消息以应对消息过多的情况。

在一种实施方式中，s2具体包括：

根据处于同一水平面的二维点云之间的几何距离是否大于距离阈值，对二维点云进行分割，同时对处于相机视野内外的两部分点云作区分，将视野外的点云直接用于激光slam建图，将视野内的点云与图像的信息融合再用于slam建图。

具体实施过程中，距离阈值基于激光雷达的扫描分辨率和室内环境复杂程度设置。

在一种实施方式中，s3具体包括：

s3.1：根据预先获得的激光雷达与单目相机之间的标定关系，将分割后的二维点云投影至图像上；

s3.2：将投影后的结果与包围盒进行匹配，根据落入包围盒内的点云投影点数在总点数中的占比，判定点云分割是否与包围盒匹配。

具体来说，通过预先得到的激光雷达-相机联合标定获取两个传感器之间的相对位姿变换，将点云投影到图像上并与包围盒一一匹配，单个包围盒可以匹配多个点云分割。具有最佳匹配效果的点云分割与包围盒被封装为语义激光，并作为建图的语义观测输入。

图2展示了二维点云与图像信息融合模块的物理结构。图中符号标识含义如下：box表示第一步获得的目标检测结果中的物体包围盒，seg表示第二步获得的点云分割，世界坐标系可视作slam开始时机器人的位姿，雷达坐标系与相机坐标系间的位姿变换由旋转矩阵r和平移向量t表示。该物理结构假设地面水平，只有当机器人具备imu传感器时才能缓解地面不平造成的点云帧间匹配困难这一问题。

本发明在该物理结构基础上作点云与图像的信息融合，其主要步骤包含点云向图像的投影及之后的包围盒-点云分割匹配，包围盒-点云分割匹配标准是点云投影点落入某个包围盒内的个数占总数的比例，选择比例最高的包围盒作为该点云分割的匹配，点云分割与包围盒的关系为多对一，以应对物体遮挡、检测框抖动等情况。下面对点云投影过程作详细描述：

投影的必要步骤是解算相机坐标系与激光雷达坐标系之间位姿变换关系，即[r，t]。该过程利用了最小二乘解算，解算式如下：

其中p为某3d空间点的在图像上的像素坐标，p^l为该空间点在雷达坐标系下的坐标，n为采集坐标对的数目，k是经过平面标定法得来的相机内参。将处于相机视野内的激光扫描点根据下式变换到像素平面：

p＝k(r·p^l+t)

在一种实施方式中，s4具体包括：

s4.1：通过点云帧间匹配找到语义激光观测到的栅格，对于每一个栅格，更新栅格的占据概率，同时累计语义激光对应语义类别的置信度并使更新次数递增；

s4.2：设置子图大小并融入相应数量的点云，子图构建完成后，计算每个栅格平均置信度最大的语义类别，当该类别对应置信度小于阈值时，将该栅格的语义类别视为背景并清除语义信息。

具体来说，基于激光slam算法cartographer构建二维栅格地图。对于每一个处于语义激光观测范围的栅格，更新其栅格占据概率的同时，累计各类别置信度并使更新次数递增。置信度阈值根据所使用神经网络的精度设置。

在一种实施方式中，s5具体包括：

s5.1：将具有除背景外语义类别的栅格与所在子图绑定，采用slam后端优化调整子图位姿后获取上述栅格的全局位姿，并在全局地图相应位置进行语义标注，语义标注包括语义类别、物体序号和包围盒；

s5.2：利用基于密度的聚类算法dbscan算法对栅格进行聚类，剔除因错误语义更新出现的离群点，组合具有相同语义类别的邻近栅格并以包围盒标注，最后分配标签号以代表各个独立物体。

具体来说，slam和目标检测都会引入误差，语义激光也是按照局部slam估计的位姿融入地图，一次性的语义标注会在地图随slam后端优化动态调整的过程中造成混乱。

因此本发明采用了优化方法，首先将具有除背景语义类别的栅格与所在子图绑定，借助slam后端优化调整子图位姿后获取上述栅格的全局位姿，并在全局地图相应位置作语义标注，且使该标注随slam后端优化的结果动态调整位置；然后基于密度的聚类算法dbscan算法对栅格进行聚类，剔除因错误语义更新出现的离群点，组合具有相同语义类别的邻近栅格并以包围盒标注，最后分配标签号以代表各个独立物体。

其中dbscan算法的相关参数：单个邻域内点个数和内点距离阈值，可以根据激光雷达的扫描分辨率设置。

图3展示了二维点云语义提取实例，本实施例选择了椅子、微波炉、雨伞和冰箱四种典型的室内物件作语义提取示例。(a)展示了目标检测的结果，物体的包围盒以边框显示，边框左上角显示语义类别及置信度，例如chair0.98，chair表示语义类别，0.98表示置信度。(b)展示了点云分割的结果，视野内外的点云以不同颜色渲染，灰色为视野内的点云，黑色为视野外的点云。(c)示出了点云向图像投影的结果，图中显示了点云分割的序号及其两个端点在所有激光扫描点中的序号。二维点云语义提取的效果会因各个物体的表面平整程度和雷达与物体间的距离变化而变化，总的来说，物体表面越平整，物体与背景的几何距离越大，语义提取的效果越好。

图4为本发明提供的语义建图方法在走廊环境下的实例示意图，(a)到(d)分别表示建图过程的几个关键节点，即从图像上检测到物体或语义优化完毕从而出现语义标注。如图4(c)(d)所示，识别的人因不属于静态物体会在建图过程中忽略，盆栽因点云分割失败也不会出现在地图上。slam的后端优化在建图过程中一直运行，地图上的语义标注也随之不断调整。

实施例二

基于同样的发明构思，本实施例提供了一种融合点云和图像的轮式机器人语义建图系统，请参见图5，该系统包括：

语义提取模块201，用于采用目标检测卷积神经网络从单目相机读取的图像中提取标识物体位置的包围盒及对应的语义类别和置信度；

点云分割模块202，用于将单线激光雷达读取的二维点云基于几何特征进行分割，得到点云分割；

语义匹配模块203，用于将包围盒与点云分割进行匹配，将匹配得到的点云分割、与包围盒对应的语义类别和置信度组合成语义激光；

二维栅格地图构建模块204，用于将语义激光作为输入，采用激光slam算法构建二维栅格地图，在仅包含占据概率的栅格中加入由各类别累计置信度、更新次数组成的语义结构体，通过点云帧间匹配找到语义激光观测到的栅格后，累计指定类别的置信度并将更新次数递增，当前子图构建完成时计算每个栅格中平均置信度最大的语义类别并保存，其中，占据概率表示栅格被障碍物占据的可能性大小；

优化模块205，用于利用slam的后端优化调整子图和子图所包含的栅格的全局位姿，采用聚类方法对栅格进行聚类，得到最终的语义地图。

由于本发明实施例二所介绍的系统，为实施本发明实施例一中融合点云和图像的轮式机器人语义建图方法所采用的装置，故而基于本发明实施例一所介绍的方法，本领域所属人员能够了解该系统的具体结构及变形，故而在此不再赘述。凡是本发明实施例一的方法所采用的系统都属于本发明所欲保护的范围。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

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