基于视觉多机器人即时定位与同步建图方法、系统及介质与流程

1.本发明涉及机器人的技术领域，具体地，涉及基于视觉多机器人即时定位与同步建图方法、系统及介质。


背景技术：

2.机器人如何在无法借助其他定位系统(如gps)的情况下，对未知环境进行探测与地图构建具有重要的现实意义，例如灾难现场、深空探测、甚至水下或战场环境等。视觉即时定位与建图技术为此提供了一种有效手段，也成为了自主机器人的研究热点问题之一。
3.伴随着对任务复杂性、多样性和系统稳定性要求的提升，单个机器人的视觉slam在任务执行效率、探测范围和系统稳定性方面已受到挑战。slam相关领域的研究已经取得了丰富的成果，基于视觉的slam在实际中已有较多成功的应用案例。但是相对于单机器人视觉slam技术而言，多机器人协作slam相关的研究成果有待进一步完善。
4.尤其在国内，对于多机器人视觉slam的研究还处于起步和探索阶段。多机器人协作slam的研究重点主要有机器人间位姿融合和地图构建。就机器人间位姿融合来说，目前大多采用的位姿融合方案有两种，一种是基于机器人相遇的位姿融合方案，这种方法能够在小范围内有效的将机器人间的位姿进行融合，但是其弊端也很明显，那就是忽略了在地图较大时机器人间根本不会相遇这一情况；第二是基于公共区域检测的方案，这种方案避免了相遇这一条件，但是对于公共区域的判定需要非常严格的标准，并且在一定程度上会产生误匹配从而影响全局地图的准确性。
5.就地图构建来说，目前的方案大多数是以栅格地图或少数特征点为基础进行构建，这种方法虽然可以简洁的表达环境，但这忽略了太多的环境信息，很大的降低了地图的直观性，对后续的任务开展具有较为明显的限制。
6.针对以上问题，本发明以多机器人视觉协同slam为研究内容，以融合多机器人位姿和地图三维重建为研究目标开展工作。
7.基于多机器人系统的协同建图，主要的研究问题包括如何精确地实现机器人节点间的数据关联、子地图的精确融合以及全局地图的误差消除等。对于这方面的研究，尽管国外已有一些成果，但是多机器人协同建图和多机器人位姿优化算法还不成熟，建图的精确度还有待提高等，且大多数的地图重建所构建的地图为稀疏地图。国内对于多机器人协作建图的研究还在起步阶段，研究成果还不成熟。
8.本发明以基于视觉的同时定位与地图构建算法原理为基础，研究多机器人节点位姿估计和地图融合方法，以稠密地图三维重建为目标，完成多机器人节点高效且准确的子地图构建和地图融合，同时对各个子地图和全局地图进行位姿优化以提高重建精度，为未知场景的多机器人系统协同探测提供一定的技术支撑。


技术实现要素：

9.针对现有技术中的缺陷，本发明的目的是提供一种基于视觉多机器人即时定位与
同步建图方法、系统及介质。
10.根据本发明提供的一种基于视觉多机器人即时定位与同步建图方法，所述方法包括如下步骤：
11.步骤s1：第一机器人和第二机器人执行自身的slam程序，建立各自的第一视觉词典、第一局部子地图和第二视觉词典、第二局部子地图；
12.步骤s2：各机器人将自身的视觉词典与其他机器人存储的词典进行比较，构建索引；
13.步骤s3：索引完成，进行图像的特征匹配、基于pnp的位姿求解来判定匹配结果；
14.步骤s4：匹配结果判定完成，使用基于icp进行位姿求解；
15.步骤s5：将第一机器人和第二机器人之间的匹配结果的关键帧进行边的绘制；
16.步骤s6：用图优化的方式进行后端全局位姿优化，对第一机器人和第二机器人建立的第一局部子地图和第二局部子地图向全局地图进行融合，建立全局一致的机器人坐标和地图更新。
17.优选地，所述方法还包括步骤：将第一机器人和第二机器人的子坐标进行坐标转换；
18.优选地，将第一机器人和第二机器人的子坐标通过坐标转换统一化为全局坐标，将第一机器人和第二机器人构建的第一局部子地图和第二局部子地图融合成全局地图。
19.优选地，所述单机器人slam程序包括作为前端的视觉里程计、作为后端的回环检测、位姿图优化和三维地图构建。
20.优选地，所述方法采用基于自然路标的公共区域检测方法进行视觉信息的融合，找出机器人的场景重叠部分，进行重叠区域的数据信息关联，确定重叠区域机器人间的坐标转换关系。
21.优选地，所述全局位姿优化中第一机器人和第二机器人在检测到公共区域，计算出公共区域处机器人间匹配成功的位姿节点间的转移矩阵，得到多机器人全局位姿的误差二范数。
22.本发明还提供一种基于视觉多机器人即时定位与同步建图系统，所述系统包括如下模块：
23.模块m1：第一机器人和第二机器人执行自身的slam程序，建立各自的第一视觉词典、第一局部子地图和第二视觉词典、第二局部子地图；
24.模块m2：各机器人将自身的视觉词典与其他机器人存储的词典进行比较，构建索引；
25.模块m3：索引完成，进行图像的特征匹配、基于pnp的位姿求解来判定匹配结果；
26.模块m4：匹配结果判定完成，使用基于icp进行位姿求解；
27.模块m5：将第一机器人和第二机器人之间的匹配结果的关键帧进行边的绘制；
28.模块m6：用图优化的方式进行后端全局位姿优化，对第一机器人和第二机器人建立的第一局部子地图和第二局部子地图向全局地图进行融合，建立全局一致的机器人坐标和地图更新。
29.优选地，所述系统还包括模块：将第一机器人和第二机器人的子坐标进行坐标转换；
30.优选地，将第一机器人和第二机器人的子坐标通过坐标转换统一化为全局坐标，将第一机器人和第二机器人构建的第一局部子地图和第二局部子地图融合成全局地图。
31.本发明还提供一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现上述中的方法的步骤。
32.与现有技术相比，本发明具有如下的有益效果：
33.1、本发明通过多机器人的子地图构建和全局地图的融合，实现稠密地图三维重建，提高定位和建图的精度；
34.2、本发明消耗更少的时间，可以更快，更精准的得到最终的结果。
附图说明
35.通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
36.图1为本发明的方法流程示意图；
37.图2为本发明的单机器人slam算法流示意图；
38.图3为本发明的公共区域检测算法流程示意图；
39.图4为本发明的全局优化位姿部分示意图。
具体实施方式
40.下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
41.如图1，本发明提供的一种基于视觉的多机器人即时定位与同步建图方法，其实现过程如下：
42.步骤1：第一机器人和第二机器人执行自身的slam算法，并建立第一词典、第二词典和第一局部地图、第二局部地图；步骤2：第一机器人和第二机器人不断地将自身的词典与其他机器人存储的词典进行比较；步骤3：若索引成功，将进行图像的特征匹配、基于pnp算法的位姿粗求解来判定是否是正确匹配；步骤4：如果是正确匹配再使用基于icp算法进行位姿精确求解；步骤5：将第一机器人和第二机器人的子坐标通过坐标转换统一化为全局坐标，实现机器人间的合作slam并将单机器人构建的子地图有效融合成全局地图；步骤6：将第一机器人与第二机器人之间成功匹配的关键帧进行边(edge)的绘制，用图优化的方式来进行后端位姿优化；步骤7：基于统一后的机器人间的坐标和优化更新后的全局位姿,完成了由第一机器人和第二机器人建立的子地图向全局地图的融合,建立全局一致机器人坐标和更为精确的全局地图。
43.如图2，单机器人slam算法部分，共四个部分，分别为作为前端的视觉里程计、作为后端的回环检测与位姿图优化以及三维地图构建。
44.如图3，公共区域检测算法部分，采用一种基于自然路标(特征点)的公共区域检测方法进行视觉信息的融合，找出机器人的场景重叠部分，进行重叠区域的数据信息关联，确定重叠区域机器人间的坐标转换关系，有效避免了机器人之间通过相遇这种小概率事件才
能够进行地图融合的弊端，同时进行类似于闭环检测的方法进行a和b间路标点的遍历、匹配、和筛选，就可以去除大部分误匹配项，减少图像误匹配的风险。
45.如图4，全局位姿优化部分的第一机器人和第二机器人在检测到公共区域时，即可计算出公共区域处机器人间匹配成功的位姿节点间的转移矩阵，并得到多机器人全局位姿的误差二范数。
[0046][0047]
其中和分别为第一机器人和第二机器人各自的误差二范数，为公共区域处的误差二范数。利用高斯牛顿法可消除机器人间的误差并更新位姿。由于融合后的全局地图在位姿误差项中多出一部分可优化的误差量即机器人间的全局误差项，从而在理论上会有更加精确的全局位姿估计的结果。
[0048]
本发明通过多机器人的子地图构建和全局地图的融合，实现稠密地图三维重建，提高定位和建图的精度；且本发明消耗更少的时间，可以更快，更精准的得到最终的结果。
[0049]
本发明还提供一种基于视觉多机器人即时定位与同步建图系统，包括如下模块：
[0050]
模块m1：第一机器人和第二机器人执行自身的slam程序，建立各自的第一视觉词典、第一局部子地图和第二视觉词典、第二局部子地图；模块m2：各机器人将自身的视觉词典与其他机器人存储的词典进行比较，构建索引；模块m3：索引完成，进行图像的特征匹配、基于pnp的位姿求解来判定匹配结果；模块m4：匹配结果判定完成，使用基于icp进行位姿求解；模块m5：将第一机器人和第二机器人之间的匹配结果的关键帧进行边的绘制；模块m6：用图优化的方式进行后端全局位姿优化，对第一机器人和第二机器人建立的第一局部子地图和第二局部子地图向全局地图进行融合，建立全局一致的机器人坐标和地图更新。
[0051]
系统还包括将第一机器人和第二机器人的子坐标进行坐标转换；将第一机器人和第二机器人的子坐标通过坐标转换统一化为全局坐标，将第一机器人和第二机器人构建的第一局部子地图和第二局部子地图融合成全局地图。
[0052]
本发明还提供一种存储有计算机程序的计算机可读存储介质，计算机程序被处理器执行时实现上述中的方法的步骤。
[0053]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
[0054]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。