一种面向灾后救援场景的多机器人协同建图方法

1.本发明涉及同步定位与建图领域，具体涉及一种面向灾后救援场景的多机器人协同建图方法。


背景技术：

2.如何在地震、火灾、核泄漏等灾难事故发生后实施快速、准确、高效的人道主义搜救，一直都是一项关乎民生安全与发展大计的极富挑战性的难题。根据目前救援界的共识，灾难发生后的救援行动存在一个“黄金72小时”，指的是人在受灾被困后的存活率会随时间的流逝呈递减趋势，72小时内的救援行动对于减少伤亡至关重要。时间就是生命，提高搜救效率的重要性显而易见。但是灾后的搜救环境具有不可预知性和复杂不稳定性，灾难可能导致不规则的道路环境、建筑物不同程度的损毁、人员被困区域含氧量下降、甚至有毒有害和可燃气体泄露等危险状况，随时可能发生二次灾害，威胁搜救人员和被困者的生命。
3.面对高低不平的路面和狭窄的缝隙管道等复杂环境地形，单机器人难以快速完成全局场景的重构任务，存在效率不高、可执行任务单一、系统容错性较低等问题。因多机器人协同作业能快速覆盖搜救区域、极大提高探索速度和效率，通过多机器人视觉协同建图，实现了面向复杂场景的自主化、智能化重构，该平台能够在第一时间进入复杂多变的灾难现场，搜集相关信息，为进一步救援行动提供了准确的地形信息指导，同时还可以辅助搜救人员迅速安全地实施搜救行动，从而降低搜救风险。现有基于视觉的多机器人建图技术集中在建好的地图和地图之间进行匹配方式，如(伍一维,左韬,张劲波,胡新宇.基于knn-prosac和改进orb的多机器人slam地图融合算法[j].高技术通讯,2021,31(07):766-772.)，通过提取栅格地图的orb特征，计算最优匹配点及最优匹配集对应的单应矩阵，计算量大，精度不高。


技术实现要素：

[0004]
为了解决现有方法存在的问题，本发明提供一种面向灾后救援场景的多机器人协同建图方法，采用基于rgb-d相机的多机器人改进orb-slam系统实现多机器人协同建图，按帧与地图的匹配方式实现地图重叠检测，计算量小且精度高。
[0005]
本发明的目的通过以下技术方案实现。
[0006]
一种面向灾后救援场景的多机器人协同建图方法，包括如下步骤：
[0007]
s1、两个单机器人分别在通过rgb-d相机的基于改进的orb-slam系统上建立三维稠密点云地图；
[0008]
s2、基于公共路标的地图重叠检测方法对两个单机器人的建图结果进行地图重叠检测；
[0009]
s3、当地图重叠检测成功后，将pnp算法结果作为初始位姿，采用icp算法进行相对位姿精确求解；
[0010]
s4、通过坐标转换和地图拼接实现多机器人的环境地图融合。
[0011]
进一步地，所述步骤s1中，基于改进的orb-slam系统包括：在原orb-slam系统的基础上引入图像分块提取和四叉树均匀化的策略，保证特征点均匀分布在整幅图像中；采用二次回环检测方法，先使用图像全局颜色特征进行一级回环检测，再通过视觉词袋模型进行二级回环检测。
[0012]
进一步地，所述步骤s2包括以下步骤：
[0013]
s21、两个单机器人分别在slam的过程中保存所有关键帧的特征点集合，并且同时建立词袋向量集，特征点即为路标，匹配上的特征点即为公共路标；
[0014]
s22、单个机器人在获取到当前帧的同时都在另一个机器人的词袋向量集中进行遍历，当匹配到的特征点的数量大于阈值时则继续进行pnp相对位姿估计，否则为错误匹配并跳过这一帧，其中阈值选择为经验数值，区间为[15,20]；
[0015]
s23、当匹配到的特征点的数量大于阈值，进行pnp相对位姿估计之后计算重投影误差当误差小于设定阈值时地图重叠检测成功：
[0016][0017]
式中，ui为第i个特征点的像素坐标，pi为特征位置坐标，k为相机内参矩阵，为李代数变换矩阵，si为第i个特征点到投影点的距离，n为特征点的数量。
[0018]
进一步地，一级回环检测采用hsv颜色空间统计图像的颜色直方图，首先对hsv颜色空间进行量化，按照色彩的不同将其划分为若干个不同的区间，其次分别计算落在各个量化区间内的像素点数量与图像总像素数的比例即可得到归一化的颜色直方图，求得的颜色直方图是256维的浮点型向量，通过图像颜色直方图的相似度进行回环检测判断，采用巴氏距离来度量图像直方图的相似性，即公式：
[0019][0020]
式中，d为巴氏距离，h1和h2颜色直方图向量，n为总维度值，和是直方图n个维度的均值,h1(i)是h1向量的第i维向量，h2(i)是h2向量的第i维向量；
[0021]
计算当前帧与其共视帧之间颜色直方图的平均相似度作为当前关键帧的先验相似度，并根据该先验相似度计算相对相似度d：
[0022][0023]
式中，h
t
和分别是当前帧和回环检测的候选帧的颜色直方图向量，h
t-cov
是共视帧颜色直方图向量，是当前帧与其共视帧之间颜色直方图的平均相似度，是当前帧和回环检测候选帧的相似度。
[0024]
进一步地，在步骤s4中，当检测到出现地图重叠区域时，则进行环境地图融合，通过计算两个机器人之间的相对位姿关系来联系两个机器人的建图坐标系。
[0025]
进一步地，所述二级回环检测采用基于dbow2(bag of words)的视觉词袋模型进行闭环检测。
[0026]
进一步地，视觉词袋模型的建立包括：对rgb-d相机拍摄的各帧图像提取特征并计算描述子，生成过程以离线的方式进行，使用基于k-means聚类算法对描述子进行聚类生成词典，并迭代至下层继续聚类，直至抵达叶子节点，最终n层生成词典树；依据每一帧图像所提取的特征和描述子，通过词典数对图像帧进行聚类，生成图像帧的词袋向量。
[0027]
进一步地，所述三维稠密点云地图建立方法为，将rgb-d相机采集的rgb图和深度图对齐使得每个像素点找到对应深度信息；对于每一帧关键帧，通过对齐深度图和rgb图，获得每一帧关键帧的像素、深度和颜色信息；结合相机内参，获得每一帧图像的像素点信息，并计算出每个像素点的x、y,、z信息；在跟踪线程中，对关键帧的rgb图进行特征点二次采样，得到三维稠密点云地图。
[0028]
进一步地，在三维稠密建图后使用统计滤波器进行滤波优化。
[0029]
进一步地，所述统计滤波器用于筛除稀疏离群噪点。
[0030]
与现有的技术相比，本发明的有益效果为：
[0031]
本发明提出了一种面向灾后救援场景的多机器人协同建图方法，用于灾后复杂场景的多机器人协同三维稠密点云地图构建，针对orb特征提取过程中容易出现特征聚集、影响特征匹配效果的问题，在特征提取方面加入了特征筛选均化策略，能够解决特征点集中导致的局部特征信息丢失的问题；针对回环检测运算量大、视觉词袋模型受特征提取影响大的问题，在回环检测阶段加入了图像全局颜色特征进行初步回环检测，再通过视觉词袋模型进行二次检测，提高了回环检测效率和准确率。
附图说明
[0032]
图1为本发明实施例一种面向灾后救援场景的多机器人协同建图方法的流程图；
[0033]
图2为本实施例基于改进orb-slam的稠密三维重建框架图；
[0034]
图3为本实施例基于颜色直方图和视觉词袋模型的回环检测流程图；
[0035]
图4为本实施例为公共路标示意图；
[0036]
图5为本实施例基于公共路标的地图重叠检测流程图。
具体实施方式
[0037]
本部分将详细描述本发明的具体实施例，本发明之较佳实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的整体技术方案，但其不能理解为对本发明保护范围的限制。
[0038]
如图1所示，一种面向灾后救援场景的多机器人协同建图方法，包括如下步骤：
[0039]
s1、两个单机器人分别在通过rgb-d相机的基于改进orb-slam系统来进行稠密三维建图，具体过程包括：
[0040]
作为一种优选的实施例，通过两个搭载realsense 2(rgb-d相机)的机器人分别单独在室外灾后模拟环境下基于改进的orb-slam系统三维建图，模拟场景大小为10m*10m，设
有障碍物模型。两个机器人采用5g无线网卡与服务端通信，实时将建图信息上传到服务端；服务端为运行linux操作系统的服务器。不要求两个机器人由同一起点出发，但两个机器人的移动路线需要有一定的重叠区域，以便建立两个机器人建立的三维点云地图之间的联系。
[0041]
如图2所示，基于改进的orb-slam系统包括：
[0042]
引入图像分块提取和四叉树均匀化的策略，来保证特征点尽可能地均匀分布在整幅图像中；采用二次回环检测方法，先使用图像全局颜色特征进行粗略的一级回环检测，再通过视觉词袋模型进行二级回环检测，并在成功检测到回环之后对回环进行验证，以确保回环的正确性。
[0043]
其中，三维稠密点云地图的建立，首先是根据rgb图和深度图对齐使得每个像素点找到对应深度信息；对于每一帧关键帧，通过对齐深度图和彩色图，获得每一帧关键帧的像素、深度和颜色信息；结合相机内参，获得每一帧图像的像素点信息，并计算出每个像素点的x,y,z信息；在跟踪线程中，对关键帧的彩色图进行特征点二次采样，用于三维稠密建图；单个机器人初始位姿由局部ba给出，在回环检测和全局ba阶段更新优化，使点云地图拼接更为精确。
[0044]
如图3所示，回环检测是为了解决位置估计中累积误差导致的轨迹漂移问题，主要是通过检测当前帧与历史关键帧间的图像相似度来实现的。为了提高回环检测效率和准确率，本发明采用二级回环检测方法，使用图像全局颜色特征进行初步回环检测，再通过视觉词袋模型进行二次检测，并在成功检测到回环之后对回环进行验证，以确保回环的正确性。一级回环检测采用最为常用的hsv颜色空间统计图像的颜色直方图，首先对hsv颜色空间进行量化，按照色彩的不同将其划分为若干个不同的区间。其次分别计算落在各个量化区间内的像素点数量与图像总像素数的比例即可得到归一化的颜色直方图。求得的颜色直方图是一个256维的浮点型向量，通过图像颜色直方图的相似度可以初步进行回环检测判断，采用巴氏距离来度量图像直方图的相似性，即公式：
[0045][0046]
式中，d为巴氏距离，h1和h2颜色直方图向量，n为维度值，和是直方图n个维度的均值；
[0047]
计算当前帧与其共视帧之间颜色直方图的平均相似度作为当前关键帧的先验相似度，并根据该先验相似度计算相对相似度：
[0048][0049]
式中，h
t
和分别是当前帧和回环检测的候选帧的颜色直方图向量，h
t-cov
是共视帧，是当前帧与其共视帧之间颜色直方图的平均相似度；
[0050]
若两帧图像间的相对相似度大于预设阈值，则这两帧之间可能具有回环关系，此
处阈值使用原orb-slam中的预设阈值15。接下来可使用视觉词袋模型对图像相似度进行进一步判断。
[0051]
二级回环检测采用基于dbow2(bag of words)的视觉词袋模型进行闭环检测。视觉词袋模型的建立首先需要对图像集合中的各帧提取特征并计算描述子，生成过程通常以离线的方式进行，使用基于k-means聚类算法对描述子进行聚类生成词典，并迭代至下层继续聚类，直至抵达叶子节点，最终n层生成词典树。依据每一帧图像所提取的特征和描述子，通过词典数对图像帧进行聚类，可生成图像帧的词袋向量。通过对词袋向量进行汉明距离(hamming distance)求解，找到最为相似的图像帧，能够实现高效的闭环检测。
[0052]
s2、基于公共路标的地图重叠检测方法对s1中的两个单机器人的三维建图结果进行地图重叠检测，具体过程包括：
[0053]
服务端得到两个机器人分别上传的三维稠密点云地图之后，以两个机器人为例，如图4所示，ai和bi分别表示机器人a和机器人b的运动轨迹，pi表示为机器人观测的路标。当机器人a移动到位置a3，机器人b移动到位置b4时，可知两个机器人观测到了同一个路标p4。根据公共路标p4，可以计算出机器人a在位置a3时相对于机器人b在位置b4的相对位姿，进而建立机器人a和机器人b的坐标系之间的联系。
[0054]
作为一种优选的实施例，如图5所示，步骤s2包括以下步骤：
[0055]
s21、两个单机器人分别在slam的过程中保存所有关键帧的特征点集合，并且同时建立词袋向量集，特征点即为路标，匹配上的特征点即为公共路标；
[0056]
s22、单个机器人在获取到当前帧的同时都在另一个机器人的词袋向量集中进行遍历，当匹配到的特征点的数量大于阈值时则继续进行pnp相对位姿估计，否则为错误匹配并跳过这一帧，其中阈值选择为经验数值，区间为[15,20]；
[0057]
s23、当匹配到的特征点的数量大于阈值，进行pnp相对位姿估计之后计算重投影误差当误差小于设定阈值时地图重叠检测成功，其中阈值结合具体相机内参依经验选定；
[0058][0059]
式中，ui为第i个特征点的像素坐标，pi为特征位置坐标，k为相机内参矩阵，为李代数变换矩阵。
[0060]
在基于特征点法视觉里程计的单机器人视觉slam过程中，保存所有关键帧的特征点集合，特征点即为路标。本项目基于视觉词袋建立词袋向量集，对不同机器人间的词袋向量集进行遍历、匹配和筛选，选取候选匹配帧并进行验证。当匹配帧之间的匹配特征点足够多的时，使用pnp进行机器人间相对位姿估计，并计算匹配点对的重投影误差，通过设置阈值进一步过滤误差较大的帧，最终实现高效准确的地图重叠检测。
[0061]
s3、当地图重叠检测成功后，在服务端将s2中pnp算法结果作为初始位姿，采用icp算法进行相对位姿精确求解。以两个机器人为例，成功检测地图重叠区域后，可计算出机器人a和机器人b间的相对位姿关系，从而联系两个机器人的建图坐标系。假设地图融合时是以机器人a为参考坐标的，则可利用相对变换位姿将机器人b的子地图进行坐标变换，与机
器人a的子地图进行融合。(如果不成功就继续分别建图，检测到重叠才进行地图融合)
[0062]
s4、当检测到出现地图重叠区域时，则进行环境地图融合，将两个机器人实时上传的三维稠密地图在服务端进行处理，通过s3中计算的两个机器人之间的相对位姿关系来联系两个机器人的建图坐标系并将地图融合。
[0063]
在三维稠密建图后，为提高点云地图效果，加入地图优化工作，使用统计滤波器进行滤波优化。统计滤波器主要作用是筛除稀疏离群噪点，在点云采集过程中因为设备测量噪声难免会引入离群的噪点，其在空间分布稀疏；统计滤波器的主要思想是假设所有的点云与其最近的n个临近点的平均距离满足高斯分布，根据均值和方差确定一个距离阈值，当某个点与其最近的n个点的平均距离大于这个计算出来的阈值时，被判定为离群点，通过统计滤波器筛选剔除孤立噪声点，实现点云地图的优化，最后协同建图信息将完整保存在服务端。
[0064]
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

技术特征：
1.一种面向灾后救援场景的多机器人协同建图方法，其特征在于，包括如下步骤：s1、两个单机器人分别在通过rgb-d相机的基于改进的orb-slam系统上建立三维稠密点云地图；s2、基于公共路标的地图重叠检测方法对两个单机器人的建图结果进行地图重叠检测；s3、当地图重叠检测成功后，将pnp算法结果作为初始位姿，采用icp算法进行相对位姿精确求解；s4、通过坐标转换和地图拼接实现多机器人的环境地图融合。2.根据权利要求1所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：所述步骤s1中，基于改进的orb-slam系统包括：在原orb-slam系统的基础上引入图像分块提取和四叉树均匀化的策略，保证特征点均匀分布在整幅图像中；采用二次回环检测方法，先使用图像全局颜色特征进行一级回环检测，再通过视觉词袋模型进行二级回环检测。3.根据权利要求1所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：所述步骤s2包括以下步骤：s21、两个单机器人分别在slam的过程中保存所有关键帧的特征点集合，并且同时建立词袋向量集，特征点即为路标，匹配上的特征点即为公共路标；s22、单个机器人在获取到当前帧的同时都在另一个机器人的词袋向量集中进行遍历，当匹配到的特征点的数量大于阈值时则继续进行pnp相对位姿估计，否则为错误匹配并跳过这一帧，其中阈值选择为经验数值，区间为[15,20]；s23、当匹配到的特征点的数量大于阈值，进行pnp相对位姿估计之后计算重投影误差ζ
*
，当误差小于设定阈值时地图重叠检测成功：式中，u
i
为第i个特征点的像素坐标，p
i
为特征位置坐标，k为相机内参矩阵，exp(ζ^)为李代数变换矩阵，s
i
为第i个特征点到投影点的距离，n为特征点的数量。4.根据权利要求2所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：一级回环检测采用hsv颜色空间统计图像的颜色直方图，首先对hsv颜色空间进行量化，按照色彩的不同将其划分为若干个不同的区间，其次分别计算落在各个量化区间内的像素点数量与图像总像素数的比例即可得到归一化的颜色直方图，求得的颜色直方图是256维的浮点型向量，通过图像颜色直方图的相似度进行回环检测判断，采用巴氏距离来度量图像直方图的相似性，即公式：式中，d为巴氏距离，h1和h2颜色直方图向量，n为总维度值，和是直方图n个维度的均值,h1(i)是h1向量的第i维向量，h2(i)是g2向量的第i维向量；
计算当前帧与其共视帧之间颜色直方图的平均相似度作为当前关键帧的先验相似度，并根据该先验相似度计算相对相似度d：式中，h
t
和分别是当前帧和回环检测的候选帧的颜色直方图向量，h
t-cov
是共视帧颜色直方图向量，是当前帧与其共视帧之间颜色直方图的平均相似度，是当前帧和回环检测候选帧的相似度。5.根据权利要求1所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：在步骤s4中，当检测到出现地图重叠区域时，则进行环境地图融合，通过计算两个机器人之间的相对位姿关系来联系两个机器人的建图坐标系。6.根据权利要求2所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：所述二级回环检测采用基于dbow2(bag of words)的视觉词袋模型进行闭环检测。7.根据权利要求6所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：视觉词袋模型的建立包括：对rgb-d相机拍摄的各帧图像提取特征并计算描述子，生成过程以离线的方式进行，使用基于k-means聚类算法对描述子进行聚类生成词典，并迭代至下层继续聚类，直至抵达叶子节点，最终n层生成词典树；依据每一帧图像所提取的特征和描述子，通过词典数对图像帧进行聚类，生成图像帧的词袋向量。8.根据权利要求2所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：所述三维稠密点云地图建立方法为，将rgb-d相机采集的rgb图和深度图对齐使得每个像素点找到对应深度信息；对于每一帧关键帧，通过对齐深度图和rgb图，获得每一帧关键帧的像素、深度和颜色信息；结合相机内参，获得每一帧图像的像素点信息，并计算出每个像素点的x、y,、z信息；在跟踪线程中，对关键帧的rgb图进行特征点二次采样，得到三维稠密点云地图。9.根据权利要求1所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：在三维稠密建图后使用统计滤波器进行滤波优化。10.根据权利要求9所述的一种面向灾后救援场景的多机器人协同建图方法，其特征在于：所述统计滤波器用于筛除稀疏离群噪点。

技术总结
本发明公开了一种面向灾后救援场景的多机器人协同建图方法，包括以下步骤：S1、两个单机器人分别在通过RGB-D相机的基于改进ORB-SLAM系统来进行稠密三维建图；S2、基于公共路标的地图重叠检测方法对S1中的两个单机器人的三维建图结果进行地图重叠检测；S3、当地图重叠检测成功后，将PnP算法结果作为初始位姿，采用ICP算法进行相对位姿精确求解；S4、通过坐标转换和地图拼接实现多机器人的环境地图融合。本发明在检测阶段加入了图像全局颜色特征进行初步回环检测，再通过视觉词袋模型进行二次检测，提高了回环检测效率和准确率。提高了回环检测效率和准确率。提高了回环检测效率和准确率。


技术研发人员：魏武 邹扬忠 李雨妮 李达贤 柳雄顶
受保护的技术使用者：华南理工大学
技术研发日：2023.03.06
技术公布日：2023/7/31