一种低带宽全局一致的多机稠密建图方法与流程

1.本发明涉及三维视觉领域，具体涉及一种低带宽全局一致的多机稠密建图方法。


背景技术：

2.重建稠密体积场景是计算机视觉和机器人领域的一项重要任务，在工厂自动化、搜救、增强现实、文化遗产保护和城市建模。虽然现有的单机机器人重建系统在在线定位和建图方面表现良好，但在城市级重建和对重建时间具有较高要求的扫描任务中仍难以应用。有鉴于此，此发明中我们强调多机器人协同稠密重建值得更多关注，因为它们比单机器人系统允许快速探索和更高的稳定性。
3.最近，一些基于视觉或lidar的多机器人同步定位和建图(slam)系统已经出现，并在机器人间定位方面表现出最先进的性能。然而，他们中的大多数都专注于提高相机姿态的定位和优化的精确度，而不是稠密建图结果。实践中的主要挑战是在有实际带宽使用限制的情况下的传输稠密地图数据。现有的多机器人重建方法尝试进行稠密重建时，大多假设客户端之间存在高质量的网络连接，更注重任务分配和协同路径规划，否则只优化和传输运动轨迹，然后只调整各机器人的局部地图而不是全局稠密地图。
4.需要注意的是，它们都不能在传输带宽有限的情况下实现全局一致的多机器人稠密重建。


技术实现要素：

5.本发明针对现有技术的不足，实现了一个可以以极低带宽要求构建具备全局一致性稠密地图的多机建图方法。建图带宽需求相较传统方法减少约90％，且能够在线地纠正回环误差，维持地图的全局一致性。
6.在稠密地图的构建过程中，本发明提出一种用于稠密地图传输的数据格式，在客户端能够将稠密地图压缩至10％的原始大小，传输至服务器端后，执行基于 sdf的闭环异常值过滤和图融合优化，能够及时地恢复至完整稠密地图。并在此过程中，实验表明，本发明的压缩传输方法对原始地图精度的降低可以忽略不计。在服务器端接收到地图后，可以对稠密地图进行优化对齐，纠正里程计误差，维持全局一致性。
7.为了实现上述目的，本发明采用如下技术方案：
8.一种低带宽全局一致的多机稠密建图方法，包括以下步骤：
9.步骤1：客户端子地图重建阶段
10.每个客户端以客户端
‑
服务器的方式运行视觉惯性里程计和密集sdf重建模块；在执行重建任务时，利用视觉惯性里程计跟踪相机姿态并将关键帧数据传输到服务器；同时，密集sdf重建模块根据相机姿态和传感器之间的变换，将点云数据集成到截断符号距离函数tsdf体素中，生成当前tsdf地图；
11.客户端周期性地将当前tsdf地图作为子地图发送到服务器中，所述的子地图以网格数据包的形式进行发送；然后各客户端从一个空的tsdf地图开始构建新的子地图；
12.步骤2：服务器端回环检测阶段
13.在服务器端，通过闭环检测模块从客户端中的视觉里程计前端收集关键帧数据，基于稠密子地图进行回环匹配筛选，具体为：搜索关键帧匹配并计算来自不同客户端的关键帧之间的变换，然后将匹配到的关键帧和其变换根据稠密子地图信息评估可靠性，最后将关键帧匹配和变换发送至优化模块；
14.步骤3：服务器端子地图恢复阶段
15.通过子地图恢复模块接收从客户端发送来的子地图数据，将网格数据包恢复到tsdf子地图，然后将其转换为欧几里得有符号距离函数esdf，以便进行后续优化，并将里程计约束发送到优化模块；
16.步骤4：服务器端子地图优化阶段
17.优化模块基于三种类型的优化约束，对轨迹和恢复后的子地图的位姿进行联合优化，优化后得到客户端地图的相对变换并将其用于机器人定位；最后，各子地图及其平面网格按优化后的位姿融合，获得全局重建结果。
18.进一步的，步骤1中tsdf地图以网格数据包的中介格式传输，所述的网格数据包由稠密地图网格、每个网格三角形的观测历史、以及各帧相机的位姿构成；在服务器端进行子地图恢复阶段时，子地图恢复模块根据每个网格三角形的观测历史，将每个网格点按照其观测帧和自身的位姿重新融合进新地图中，得到恢复后的tsdf地图。
19.进一步的，步骤2中所述的服务器端回环检测阶段中，闭环检测模块从客户端收集到的关键帧首先通过查询词袋数据库进行匹配，所有客户端共享一个数据库，以实现跨客户端的闭环检测；然后对最佳的n个候选进行对应搜索，在候选关键帧的匹配地图点对之间计算三维变换关系，并进行三维到二维的随机一致性检测。
20.进一步的，步骤3中所述的根据稠密子地图信息评估可靠性，具体为：
21.3.1：轨迹碰撞检查：对于每个闭环候选关键帧的匹配地图点对，即子地图s
k
和s
l
，通过检查轨迹碰撞来判断闭环变换的有效性，如果变换到子地图s
l
中的所有子地图s
k
的轨迹点始终位于子地图s
l
的自由空间中，则闭环变换有效；反之，如果变换后的轨迹点在子地图s
l
中被障碍物占据，则闭环变换无效；
22.3.2：配合性检查：采用基于sdf的配合评估方法，将子地图s
k
的等值面点首先变换到子地图s
l
中，如果变换后的等值面点仍位于子地图s
l
的等值面中，则闭环变换有效；否则，继续判断仍处在子地图s
l
的等值面中的等值面点占所有等值面点的比例，若比例高于或等于阈值，则闭环变换有效，若比例低于阈值，则闭环变换无效。
23.进一步的，所述的步骤4中，优化过程以恢复后的子地图位姿为优化节点，考虑以下3个优化约束：
24.闭环约束，即由服务器端回环检测阶段计算出的出现回环的子地图之间的相对位姿应满足：
[0025][0026][0027]
其中，表示闭环约束的残差，分别表示子地图s
k
、s
l
在世界坐标
系下的位姿，表示优化变量函数，表示在子地图s
k
中t
a
处的相机位姿，表示在子地图s
l
中t
b
处的相机位姿，表示在t
a
和t
b
处的相机姿态之间的闭环变换；
[0028]
里程计约束，即由视觉惯性里程计估计的子地图之间的相对位姿应满足：
[0029][0030]
其中，表示里程计约束的残差，表示同一客户端的子图的里程计估计相对姿态；
[0031]
配准约束，即存在重叠的子地图之间应保持点云重合，满足：
[0032][0033][0034]
其中，是配准约束的残差，是子地图s
k
的等值面点数，表示子地图s
k
、s
l
的残差，表示子地图s
k
中的第i个等值面点，表示优化变量函数。
[0035]
现有技术相比，本发明的优势在于：
[0036]
1.本发明提出了一种低带宽全局一致的多机稠密建图方法，用于实时集中式多机器人协作密集重建，具有紧凑的传输数据格式和子地图优化功能。紧凑表示可以以较高的精确度替代符号距离函数(sdf)地图，快速传输到服务器对密集地图进行在线优化，实现全局一致性。其中每个客户端以生产者
‑
消费者的方式执行视觉slam和sdf重建，并以固定频率将sdf集成到子地图中，然后将子图转换为紧凑的表示，在本发明中命名为网格包，再利用一种基于轨迹查询的方法，将网格包转换回服务器中的子图，对于客户端计算性能要求较低，建图带宽需求相较传统方法减少约90％，尤其适用于计算能力受限的机器人平台。
[0037]
2.本发明通过扩展在线地图融合优化和闭环校正方法，能够在线地纠正回环误差，实现多机器人的全局一致性重建，显著提高了建图的精确度，尤其是在里程计前端存在显著累计漂移误差时。
附图说明
[0038]
图1是本发明的整体框架示意图；
[0039]
图2是本发明的子地图压缩传输过程中网格包包含内容的示意图；
[0040]
图3是本发明的后端优化器中的因子图；
[0041]
图4是本发明子地图压缩传输前后生成地图的对比；
[0042]
图5是本发明重建过程中各消息类型所需带宽；
[0043]
图6是本发明各消息所需带宽随时间变化的曲线表示；
[0044]
图7是本发明在euroc数据集上的重建结果
[0045]
图8是本发明用3个机器人在一个实验室场景的重建结果。
具体实施方式
[0046]
下面根据说明书附图对本发明进行详细说明。
[0047]
一、系统框架：
[0048]
如图1展示了本发明重建系统的系统框架。如图所示，本系统中，客户端运行如下若干模块：
[0049]
1.1：每个客户端运行视觉惯性里程计(简称视觉slam模块)和密集sdf 重建模块。
[0050]
在执行重建任务时，视觉slam模块会跟踪相机姿态并将关键帧数据传输到服务器。同时，密集sdf重建模块根据相机姿态和传感器之间的变换，将点云集成到tsdf(截断符号距离函数)体素中。在每个周期之后，客户端中的当前 tsdf地图作为子地图发布到服务器，然后各客户端从一个空的tsdf地图开始构建新的子地图。为了减少传输tsdf子地图的通信使用量，利用子地图压缩传输模块将子地图作为网格数据包发送，然后再在服务器中恢复到tsdf子地图。
[0051]
而服务器端则运行下列模块：
[0052]
1.2：服务器端回环检测阶段
[0053]
在服务器端，闭环检测模块从客户端视觉里程计前端收集关键帧数据，基于稠密子地图进行回环匹配筛选，搜索关键帧匹配并计算来自不同客户端的关键帧之间的变换。然后将匹配到的关键帧和其变换根据稠密地图信息评估其可靠性，最后将关键帧匹配和转换转发到优化模块。
[0054]
1.3：服务器端子地图恢复阶段
[0055]
子地图恢复模块从客户端接收子地图数据，将网格数据包恢复到tsdf子地图，然后将其转换为esdf(欧几里得有符号距离函数)以便进行后续优化，并将里程计约束发送到位姿图优化模块。
[0056]
1.4：服务器端子地图优化阶段
[0057]
本发明通过轨迹和恢复的子图的联合优化开发了一种鲁棒的地图融合方法。位姿图优化模块基于三种类型的约束来优化子图的姿态：里程计、闭环和配准约束。优化后，可以确定客户端地图的相对变换并将其用于机器人间定位。最后，根据子图位姿对sdf子图和网格进行组合和滤波，以获得全局重建结果。
[0058]
二、1.2中基于稠密子地图的回环匹配筛选包含如下细节：
[0059]
基于sdf传输方法，在服务器中获取客户端的sdf映射。对于每个计算的子图s
k
和s
l
之间的闭环变换除了常用的基于关键帧的成对一致性检查步骤之外，我们还设置了基于密集图采取额外的闭环异常值拒绝步骤。
[0060]
2.1轨迹碰撞检查：对于每个闭环候选对，即子图s
k
和s
l
，我们通过检查轨迹碰撞来确认接收到的闭环变换的有效性：如果将s
k
的所有轨迹点变换到第二个子图s
l
中，轨迹点应始终位于第二个子图的自由空间中。如果有轨迹点在第二个子图的空间被占据，则说明回环匹配为错误匹配。这一步检查是双向的，即s
l
的轨迹也转化到s
k
进行检查。如果在碰撞检查中最小部分的轨迹姿态保留在自由空间中，我们认为回环变换通过了该检查。
[0061]
2.2配合性检查：对于配合性检查，我们采用基于sdf的健身评估类似的方法。s
k
的等值面点首先转换到s
l
的坐标系统。如果给定正确的相对变换，变换后的等值面点仍应位于另一个子图的等值面中。如果不是，则可以通过仍处在等值面上的点的比例确定配合度
分数。同理，这一步骤会被双向执行，即s
l
的等值面点也转化到s
k
进行检查。
[0062]
三、子地图压缩传输模块具体细节为：
[0063]
为了以最小的数据大小发布优化所需的数据，我们利用了网格表示稠密地图的能力。网格以此方法生成：从sdf图中，通过使用行进立方体方法连接零级体素来提取网格。通过这种方式，我们可以使用网格顶点来表示等值面点。此外，为了在服务器端重建sdf地图，除了等值面点之外，我们还需要知道这些点的观测射线，以便将它们重新整合为sdf地图。所以我们记录并发布每个网格三角形的可见性信息，即它在哪些帧中被观察到，以及每个子图中所有帧的位姿。因此，有了等值面点和相关的相机姿态，我们可以重新整合点以获得恢复的sdf 地图，其机载计算成本可忽略不计，带宽使用最少。
[0064]
总之，在每个子图的压缩过程中记录了三种类型的数据，在本工作的其余部分称为网格包，如图2所示，包括：
[0065]
1)子图的网格数据，包含所有的网格三角形。
[0066]
2)每个网格三角形的可见性信息，即在哪些帧中观测到了相应体素。
[0067]
3)每个子图中的相机姿势。
[0068]
值得注意的是，每个网格三角形只包含三个顶点和颜色，但是在子图周期内，可以在许多帧中观察到各个三角形，这意味着三角形可能包含的可见行信息的信息容量超过三角形本身。但是，直观上三角形的观察历史具有连续性，即如果在 t帧观察到顶点v，则很可能在t附近的相邻帧中也观察到v，同时不连续观察更有可能是由含噪声的不准确的传感器数据引起的。因此，从观察连续性的角度来看，历史向量可以在传输之前进行修剪。在这一步中，历史向量被转换为子序列的起始和终止索引。如果一个子序列具有连续性，其中相邻索引之间的差异不超过kd并且它至少包含kn索引，则提取有效子序列。在实验中，我们取kd ＝3和kn＝4。
[0069]
这种简洁的方法可以有效地减少网格包的数据大小，因此网格包需要的网络流量仅为原始tsdf子图的10％，而没有经过可见性信息修建的传输流程，约需要30％的原始tsdf子图的带宽。我们在实验部分评估了此恢复方法，恢复的sdf的质量如图4所示。然后我们全局优化子图姿势，并生成全局地图。
[0070]
四、在接收到有效的回环消息后，与其他多机器人定位系统直接对轨迹进行回环约束全局优化以纠正漂移不同，我们提出了一种鲁棒的地图融合方法，通过轨迹和sdf子图的联合优化，并通过优化重叠子图之间的配准约束以达到全局稠密地图一致性。在此步骤中，考虑3种约束：
[0071]
4.1闭环约束：闭环消息中涉及到的相关子图的位姿首先通过变换进行调整，推导如下，其中代表在子地图s
k
中t
a
处的相机位姿，也是如此，，表示在子地图s
l
中t
b
处的相机位姿，并且是在t
a
和t
b
处的相机姿态之间接收到的闭环变换：
[0072][0073]
因此，闭环约束给出为：
[0074][0075]
表示闭环约束的残差。在多客户端地图融合的情况下，分别表
示子地图s
k
、s
l
在世界坐标系下的位姿，默认情况下，世界坐标系设置为与一号客户端的里程计坐标系相同，即
[0076]
4.2里程计约束：来自同一客户端的子图的里程计估计相对姿态在子图s
k
和s
l
之间，作为约束添加如下，
[0077][0078]
4.3配准约束：voxgraph(victor reijgwart,alexander millane,helen oleynikova,roland siegwart,cesar cadena,juan i.nieto.voxgraph:globally consistent,volumetric mapping using signed distance function submaps.ieee robotics autom.lett.5(1):227
‑
234,2020)中提出的无对应注册约束被包含在优化中。首先，通过比较位姿和边界框来检测重叠子图s
k
、s
l
。然后从子图s
k
的欧几里得有符号距离函数(esdf)中提取等值面上的点并投影到子图s
l
的坐标系中。从投影点到子图s
l
的等值面的距离可以通过读取该点的esdf值来确定。然后形成配准约束，表示为从点到s
l
的等值面的所有平方距离：
[0079][0080]
其中，是子图s
k
的等值面点数。
[0081]
那么残差由下式给出：
[0082][0083]
其中，对于所有等直面点存在而是χ优化中变量的函数：
[0084][0085]
考虑三种约束后，本发明构建的优化位姿图如图3所示，回环约束存在于检测到回环的子地图之间，里程计约束存在于相邻子地图之间，配准约束存在于相互有重叠区域的子地图之间。
[0086]
五、输出结果：
[0087]
根据上述三个约束项优化各客户端子地图的位姿，随后进行如下两个步骤以生成两种输出：
[0088]
5.1确定客户端坐标系转换。为了实现多机器人定位，在优化得出各子地图位姿后，我们利用各子地图间相互位置关系，优化得出各客户端里程计地图坐标系的相互变换，优化节点为从全局坐标系到各客户端坐标系变换，优化目标为最小化由每对子地图间相互变换的误差：
[0089][0090]
5.2生成全局地图。各子地图按优化后的位姿融合得到全局地图，同时也将各子地图的平面网格融合，以得到更好的可视化效果。
[0091]
实施例
[0092]
为了进一步展示本发明的实施效果，我们实用euroc数据集以及实机运行结果，测评本发明的子地图传输方法和重建精确度。
[0093]
实验1:我们首先在euroc数据集中，测试本发明子地图传输方法的带宽及重建精确度损失。评价指标为，原始方法重建后的子地图，经本发明编解码方法传输、恢复后生成的子地图，与原始地图的重建平面均方误差。均方误差越小，则两地图间误差损失越小。
[0094]
实验过程为，我们以vins
‑
mono结合voxblox的方式重建出euroc数据集中v1_01,mh_01,mh_02,mh_03序列的房间稠密地图，随后经编码与解码，生成恢复后的地图，与原地图间计算平面均方误差。从表1中可以看出，经压缩恢复后的子地图与原地图比较仅会造成极小的重建误差。图4展示了两地图的可视化结果，左图为原始结果，有图为压缩恢复后的地图。
[0095]
表1：在euroc数据集上的子地图传输方法造成的平面重建误差
[0096]
场景v1_01mh_01mh_02mh_03rmse(m)0.0590.0650.0690.029
[0097]
实验2:我们进一步在euroc的mh_01，mh_02，mh_03三个序列中，测试本发明的多机建图性能。这三个序列为采集了同一个地点的三个不同飞行轨迹的图像信息，并且三个轨迹中存在部分重叠，是多机定位及建图系统的良好的测试数据。在此实验2中，评价指标为：本发明多机定位生成的轨迹与其他方法生成的轨迹的定位误差，以及本发明多机建图的平面重建误差，以及本发明在重建过程中不同消息类型产生的带宽负载大小。
[0098]
实验过程为：我们在euroc mh_01,mh_02,mh_03三个序列上同时运行三个客户端以及一个服务器，记录重建过程中的带宽负载，已经重建结束后的各客户端的稠密地图及其轨迹，以及服务器中全局稠密地图及轨迹，计算其与数据集真实值的误差。表2记录了vins
‑
mono(tong qin,peiliang li,shaojie shen. vins
‑
mono:a robust and versatile monocular visual
‑
inertial state estimator. ieee trans.robotics 34(4):1004
‑
1020,2018)，以及本系统在直接模式(即不应用子地图压缩传输方法，而直接传输完整地图)和压缩模式(即应用子地图压缩传输)产生的轨迹误差，从中可以看出本方法在优化稠密地图的同时，有效的修正了轨迹漂移误差。表3则记录了本发明在直接与压缩模式地图重建的平面误差，从中可以看出本方法重建精度很高，并且压缩模式仅会损失极少的精确度。图5和图6则记录了不同类型消息的带宽负载，本发明的压缩方法可以减少90％的带宽要求，其中mesh.w.c.h为未经可见性信息修剪的网格包数据，mesh.w.o.h 为不包含可见性信息的网格，mesh.w.t.h为经过可视化信息修建的网格包数据。图7展示了三序列联合建图的可视化结果。
[0099]
表2:在euroc数据集上各方法的轨迹误差
[0100][0101]
表3:在euroc数据集上本发明的平面重建误差
[0102]
方法mh_01mh_02mh_03融合后融合网格直接0.2380.2670.2640.116
‑
压缩0.2470.2690.2620.1290.111
[0103]
实验3:我们进一步测试了本发明的重建系统在实机运行时的性能。我们使用本发明用3个机器人(即客户端)在线重建了一个实验室室内场景。本实验的评价指标为重建时的带宽负载，客户端的计算负载以及重建的可视化结果。从表 4中可以看出，本系统内用于定位的关键帧消息需要大约20kb/s的带宽，而压缩稠密地图也仅需要25kb/s的带宽。
[0104]
表4:在重建一室内场景时的带宽负载
[0105]
信息类型平均带宽带宽标准差关键帧20.25kb/s6.58kb/s压缩地图25.24kb/s11.70kb/s
[0106]
在实验中客户端运行在一nuc10i7fnh迷你电脑上，稠密建图及传输模块仅消耗约1.1个核心，而里程计前端则消耗约1.8个核心，此迷你电脑可认为拥有12个计算核心。图8则展示了建图的可视化结果。
[0107]
根据对比实验可以得出以下结论：
[0108]
本发明的压缩传输方法可以显著降低稠密地图的传输带宽负载，且稠密地图优化方法可有效纠正稠密地图及轨迹的漂移误差。