基于图像语义的多机器人协同全局地图构建系统与方法

本发明涉及多机器人协同建图，尤其是涉及一种基于图像语义的多机器人协同全局地图构建系统与方法。

背景技术：

1、近年来机器人已经融入到生产生活的各个方面，不仅在工农业、医疗服务业得到应用，也在安防、国防、危险场合和探测领域发挥重要作用，而机器人安全稳定运行的核心在于能够自主完成对作业环境的精确感知。此外，考虑到在复杂大规模作业环境背景下作业的难度，同时放置多个机器人来协同作业的方案已经被提出，这种方案要求多机器人在初始位置未知的情况下，协同构建出所处作业环境的全局地图。目前多机器人协同建图吸引了大量研究者的目光，已经成为机器人技术发展研究中的热点方向。由于多机器人协同建图突破了单机器人在感知、决策及执行能力等方面受到的限制，能够从本质上提高作业效率，在此基础上能够使多机器人完成更多高级任务，如海洋勘探、行星勘探、合作物体运输和搜索救援行动等等，因而具有很高的研究价值和意义。

2、现有基于单机器人的同时定位与建图技术(simultaneous localization andmapping，slam)已经较为成熟，但对于多机器人协同建图研究而言，在提升系统鲁棒性和准确性方面仍存在亟待改进的问题。亟待改进的问题包括：(1)地图构建过程主要依赖于不稳定的几何特征，如点、线和平面，而忽略了图像中稳定的语义特征，图像中几何特征容易受视点、尺度、照明等环境因素的变化影响，导致构建的地图精度下降；(2)目前多机器人协同地图构建方案构建出的地图以稀疏的特征点地图为主，这种地图缺少对作业环境中语义信息的表征，对后续完成路径规划、场景理解等高级任务是远不足够的。这些问题对多机器人协同地图构建提出了挑战，因此有必要针对复杂大规模作业环境背景下的多机器人协同定位展开深入研究，以期在提升建图精度的同时，构建出密集的全局语义地图。

技术实现思路

1、本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于图像语义的多机器人协同全局地图构建系统与方法，通过结合凸形几何和语义信息，能够使多机器人在复杂大规模作业环境中实现精确的协同定位，在此基础上进行地图构建，能够有效提升建图的精度并构建出全局语义地图。

2、本发明的目的可以通过以下技术方案来实现：一种基于图像语义的多机器人协同全局地图构建系统，包括依次连接的图像采集单元、图像特征提取单元、特征融合单元、单机器人独立定位单元、多机器人协同定位单元和全局语义地图构建单元组成，所述图像采集单元和图像特征提取单元还分别连接至全局语义地图构建单元，

3、所述图像采集单元用于获取作业环境信息，包括rgb图像和深度图像，并将rgb图像转换为灰度图像；

4、所述图像特征提取单元用于接收图像采集单元输出的rgb图像、灰度图像，并对应提取出图像像素级语义标签、图像orb(oriented fast and rotated brief)特征点集；

5、所述特征融合单元用于将orb特征点投影到rgb图像平面上，并将语义标签标注到对应的orb特征点上，以融合图像的几何和语义特征；

6、所述单机器人独立定位单元对特征融合单元输出的数据进行相应处理，输出得到单机器人的局部轨迹；

7、所述多机器人协同定位单元对多个单机器人的局部轨迹进行相应处理，输出得到多机器人的全局轨迹；

8、所述全局语义地图构建单元结合多机器人的全局轨迹、图像采集单元输出的深度图像以及图像特征提取单元输出的语义标签，构建得到全局语义地图。

9、进一步地，所述图像采集单元包括依次连接的图像采集模块、图像预处理模块和数据传输模块，所述数据传输模块分别连接图像特征提取单元和全局语义地图构建单元；所述图像采集模块使用相机捕获作业环境信息，包括rgb图像和包含距离信息的深度图像；所述图像预处理模块将rgb图像转换成灰度图像；所述数据传输模块上传经过预处理的rgb图像、灰度图像和深度图像。

10、进一步地，所述图像特征提取单元包括几何特征提取模块和语义特征提取模块，所述几何特征提取模块和语义特征提取模块分别连接特征融合单元，所述语义特征提取模块还连接至全局以与地图构建单元；所述几何特征提取模块接收数据传输模块发送的灰度图像，并且通过基于快速旋转的二进制独立稳定描述子特征(oriented fast and rotatedbinary robust independent elementary features，orb)提取算法提取出orb特征点构成特征点集；所述语义特征提取模块接收数据传输模块发送的rgb图像，并且利用语义分割网络将输入的rgb图像经过网络的分类预测与掩码预测处理后，输出该rgb图像上像素点的语义标签。

11、进一步地，所述特征融合单元包括语义标注模块，所述语义标注模块连接单机器人独立定位单元，所述语义标注模块接收到几何特征提取模块发送的orb特征点集和语义特征提取模块发送的rgb图像上像素点的语义标签后，将orb特征点投影到rgb图像平面上，并且将语义标签标注到对应的orb特征点上，以融合图像的几何和语义特征。

12、进一步地，所述单机器人独立定位单元包括依次连接的轨迹跟踪模块、闭环检测模块和局部轨迹生成模块，所述局部轨迹生成模块连接多机器人协同定位单元；所述轨迹跟踪模块用于处理每个rgb图像帧，并选择关键帧插入到局部轨迹中，经过语义标注的orb特征点集合用于匹配已有地图点，以实现轨迹跟踪；

13、所述闭环检测模块基于词袋法(bag of word，bow)检测关键帧序列中存在的闭环，与当前关键帧共享相似orb特征的所有关键帧都要计算与当前关键帧的相似性分数，并选择分数超过预设相似度阈值的几个关键帧作为闭环候选，判断闭环候选与当前关键帧间的转移矩阵是否小于预设闭环阈值，判断通过的候选将被确定为闭环，之后将此闭环作为约束利用光束法平差法(bundle adjustment，ba)优化关键帧的位姿信息；

14、所述局部轨迹生成模块使用三角剖分算法将关键帧中的orb特征点三角化为新的地图点，并且使用ba算法细化局部地图中的关键帧和地图点，；利用经过优化的关键帧位姿信息组成单机器人的局部轨迹。

15、进一步地，所述多机器人协同定位单元包括依次连接的重叠区域检测模块、转移矩阵计算模块和全局轨迹生成模块，所述全局轨迹生成模块连接全局语义地图构建单元；所述重叠区域检测模块基于词袋法检测局部地图之间重叠，此模块对两个机器人建立的关键帧进行遍历、特征匹配、和筛选，orb特征匹配超过预设匹配度阈值的关键帧将被接受为粗略候选，并且利用n点透视(perspective-n-point，pnp)算法求得转移矩阵，判断机器人间位姿差距是否小于预设位姿阈值确定重叠帧；

16、所述转移矩阵计算模块计算当前关键帧与重叠帧之间的转移矩阵，使用特征匹配算法得到帧间的匹配特征点集，并使用迭代最近邻点(iterative closest point,icp)算法迭代计算转移矩阵；

17、所述全局轨迹生成模块将转移矩阵计算模块得到的转移矩阵作为约束，将多个机器人的局部轨迹联系在一起用ba算法优化位姿信息，消除累计误差，生成全局轨迹。

18、进一步地，所述全局语义地图构建单元包括依次连接的位姿融合模块、局部语义地图生成模块和全局语义地图融合模块，所述位姿融合模块接收全局轨迹生成模块发送的多机器人全局轨迹，并根据全局轨迹较准待融合关键帧的位姿，获得多机器人全局位姿；

19、所述局部语义地图生成模块利用数据传输模块发送的深度图像和语义特征提取模块发送的图像语义标签，生成局部语义地图；

20、所述全局语义地图融合模块根据多机器人全局位姿融合局部语义地图，并滤除冗余地图点，从而构建出全局语义地图。

21、一种基于图像语义的多机器人协同全局地图构建方法，包括：1)图像特征提取及融合流程；2)多机器人协同定位流程；3)全局语义地图构建流程；其中，所述图像特征提取及融合流程具体包括以下步骤：

22、首先图像采集模块捕获作业环境信息，包括rgb图像和深度图像，然后由图像预处理模块将rgb图像转换成灰度图像，以方便后续进行几何特征提取；

23、数据传输模块将灰度图像传输给几何特征提取模块提取orb特征点集，同时图像预处理模块将rgb图像数据通过数据传输模块传输给语义特征提取模块进行像素级的语义分割，得到图像上像素点的语义标签，最后通过语义标注模块对特征点集进行语义标注。

24、进一步地，所述多机器人协同定位流程具体包括以下步骤：

25、首先单个机器人通过轨迹跟踪模块对图像序列进行帧间特征匹配，获得一组代表着图像间几何信息关联的匹配特征对，而后利用匹配特征对计算帧间转移矩阵，实现对机器人移动轨迹的跟踪；

26、闭环检测模块遍历关键帧数据库，计算其当前关键帧的相似性分数，筛选出候选相似帧，进而判断候选相似帧与当前帧的几何距离是否小于阈值，如果不小于该阈值，则重新遍历关键帧数据库进行查询，如果小于该阈值，则说明找到闭环，能够利用闭环信息优化位姿消除累计误差；

27、局部轨迹生成模块使用三角剖分算法将关键帧转换为地图点，用ba算法优化地图点位姿信息，消除累计误差，并生成单机器人局部轨迹；

28、当两个机器人进入到可通信范围内，首先加载两个机器人构建的局部关键帧数据库，由重叠区域检测模块遍历双方数据库，通过计算帧间相似性分数得到高分的候选匹配帧，此外，通过判断特征点的语义标签是否一致筛除错误的匹配特征对，判断特征点数量是否满足阈值要求，如果不满足，则为错误候选匹配帧，如果满足，则进一步通过pnp算法粗略计算帧间转移矩阵，进而判断候选匹配帧位姿关系是否满足空间要求，如果不满足，则删除错误候选匹配帧，如果满足，则表明找到正确的匹配帧，转移矩阵计算模块使用icp算法迭代计算转移矩阵，全局轨迹生成模块将转移矩阵计算模块得到的转移矩阵作为约束，将两个机器人的局部轨迹联系在一起、并采用ba算法优化位姿信息，消除累计误差，生成多机器人全局轨迹。

29、进一步地，所述全局语义地图构建流程具体包括以下步骤：

30、位姿融合模块根据全局轨迹校准待融合关键帧的位姿，即将所有地图点坐标校准到同一坐标系下，局部语义地图生成模块在统一坐标系下利用深度图像和rgb图像构建局部地图，并利用图像语义信息对局部地图进行语义标注、生成局部语义地图，全局语义地图融合模块利用全局位姿信息融合局部语义地图，并通过体素滤波器滤除冗余地图点，最后输出构建的全局语义地图。

31、与现有技术相比，本发明具有以下优点：

32、一、本发明通过设置依次连接的图像采集单元、图像特征提取单元、特征融合单元、单机器人独立定位单元、多机器人协同定位单元和全局语义地图构建单元组成，将图像采集单元和图像特征提取单元分别连接至全局语义地图构建单元。利用图像采集单元获取rgb图像、深度图像，并将rgb图像转换为灰度图像；利用图像特征提取单元对应提取出图像像素级语义标签、图像orb特征点集；利用特征融合单元将orb特征点投影到rgb图像平面上，并将语义标签标注到对应的orb特征点上；利用单机器人独立定位单元输出得到单机器人的局部轨迹；利用多机器人协同定位单元输出得到多机器人的全局轨迹；利用全局语义地图构建单元构建得到全局语义地图。由此建立出一种融合几何和语义特征的多机器人协同定位与全局语义地图构建系统，能够充分结合图像几何和语义信息，实现多机器人协同定位和构建密集全局语义地图的功能。

33、二、本发明为有效实现融合几何和语义特征的多机器人协同定位，设计出包含图像特征提取及融合流程、多机器人协同定位流程、全局语义地图构建流程的方法过程：首先单个机器人使用语义分割算法获得语义标签，并将其标注到使用传统特征提取方法获得的特征点集上，实现几何和语义特征的融合；然后基于经过语义标注的特征点集，通过特征匹配、闭环检测等步骤实现对单个机器人局部轨迹的跟踪；最后在接收到多个机器人发送的位姿及地图数据后，通过重叠区域检测步骤找到不同机器人之间的关联帧，在经过基于语义标签的特征点筛选步骤后，准确的关联帧被用作机器人之间的位姿约束，以联合优化多个机器人的全局轨迹，从而获得精准的全局定位结果，在此基础上进行全局语义地图的构建，能够有效提升建图的精度并构建出全局语义地图。

34、三、充分利用了图像语义不受视点、尺度、照明等因素影响和包含更多高级作业环境信息的特性，通过融合图像几何特征和语义特征，能够改进多机器人协同定位技术，有效地解决传统多机器人协同地图构建方法主要依赖于几何特征从而导致的问题，提升了构建地图的精度，降低了环境变化对于系统的影响，改善了系统鲁棒性。