ClusterFusion: Real-time Relative Positioning and Dense Reconstruction for UAV Cluster 论文阅读

ClusterFusion: Real-time Relative Positioning and Dense Reconstruction for UAV Cluster 论文阅读

摘要

摘要——随着机器人技术的进步，对密集点云地图的需求越来越大。然而，使用单个无人机进行密集重建在飞行速度和电池功率方面受到限制，导致重建缓慢且覆盖率低。集群无人机系统为地图构建提供了更大的灵活性和更广泛的覆盖范围。集群无人机的现有方法面临着准确的相对定位、比例漂移和高速密集点云图生成的挑战。为了解决这些问题，我们提出了一个用于大规模密集重建和实时协作定位的集群框架。该框架的前端是一种改进的视觉里程计，可以有效地处理大规模场景。通过两阶段联合优化算法和相对姿态优化算法实现无人机之间的协同定位，有效地实现了无人机的精确相对定位，缓解了尺度漂移。估计姿态用于实现点云地图的实时密集重建和融合。为了评估我们提出的方法的性能，我们对真实世界的数据进行了定性和定量实验。结果表明，我们的框架可以有效地抑制尺度漂移，实时生成大规模密集点云图，并且随着系统中无人机的增加，重建速度也会加快。

简介

使用无人机进行实时大规模重建在城市规划、应急救援、救灾和农业监测等各个领域发挥着重要作用。然而，复杂多变的应用场景要求大规模三维密集重建技术在保持重建精度的同时具有更好的实时性能。目前，重建的基本方法是从运动中重建结构（SfM），但它通常会产生大量的计算和内存成本。在大多数情况下，SfM方法需要数小时才能产生最终结果。此外，在更大规模的场景中，计算时间会急剧增加。基于同时定位和映射（SLAM）技术的更快的密集重建方法，如DenseFusion[1]，依赖于无人机在不同时间的精确姿态估计。尽管实时密集重建是可能的，但由于单架无人机飞行速度和电池功率的限制，重建覆盖率和速度无法进一步提高。无人机集群由于其协同工作模式，可以更灵活高效地执行测绘任务，从而解决了这些问题。

单架无人机的实时点云重建受其飞行续航能力和扫描范围的限制。传统的多无人机协作SLAM技术，如[2]，需要大量的计算资源和通信带宽来进行协作姿态估计，而协作姿态估计依赖于公共视图关系。需要考虑实时性能和更高精度之间的良好平衡，然而，由于以下未解决的问题，目前的研究很少解决这个问题。对于多无人机实时密集重建，需要多架无人机的精确相对姿态，因此需要充分利用多架无飞行器收集的图像数据来估计其相对位置并进行进一步的联合优化。另一方面，多架无人机的定位面临以下挑战：首先，为了确保多架无机重建的效率，多架UAV之间获取的图像往往重叠度较低。其次，在检测到有效的环回之前，仅使用图像信息实现无人机之间的相对定位是困难的。

为了应对上述挑战，提出并开发了一种使用多架无人机进行联合实时密集重建的框架。为了对单个无人机进行精确定位，基于GSLAM框架[3]，开发了一种名为PISLAM的大型场景SLAM。此外，还提出了一种用于无人机相对位置优化的混合方法，该方法分为两部分：分布式优化和联合优化。对于无人机的轻型定位要求，将两阶段联合GNSS优化算法应用于分布式优化部分，可以充分利用视觉和GNSS信息对局部和全局位姿进行初始优化。该算法利用GNSS的先验信息，以最小的计算代价有效地对无人机进行聚类。为了得到更准确的相对姿态，在联合优化部分利用图像的共视关系对其进行了进一步的优化。图1展示了多无人机密集重构的演示，该框架的体系结构如图2所示。本文的工作总结如下：

• 提出了一种集群重构框架，该框架能够以较低的计算和通信开销扩展无人机的数量，并实时完成协同重构任务。
• 提出了一种两阶段联合优化算法，该算法可以在不增加通信开销的情况下，对局部姿态和尺度进行优化，并估计相对姿态。
• 为了更准确地估计无人机之间的位姿，提出了一种相对位姿优化方法，该方法首先检测图像之间的共视关系，然后基于单应性对相对位姿进行优化。
• 创建用于现实世界中多个无人机的大规模3D密集重建的数据集1，该数据集是公共访问的。

相关工作

无人机的协同姿态估计和密集重构技术涉及多个领域。在本节中，我们将简要介绍相关工作。

A.Swarm定位和映射

近年来，无人机机群技术已成为一个研究热点。提出了许多方法，特别是在视觉SLAM领域。本节概述了用于多个机器人的协作视觉SLAM框架。

大多数群体定位和映射方法都依赖于集中式架构，这意味着有必要建立一个中心节点来处理相对姿态估计和映射。与许多工作中研究的去中心化架构相比，集中式架构更容易实现和部署。Andersson[4]和Kim等人[5]应用了一个框架，该框架收集中心节点的所有测量值并估计全局地图。基于ORB-SLAM[6]，Schmuck等人提出了一种多无人机协同定位框架CCM[2]，成功解决了相关的3D估计挑战。CCM维护全局地图，并使用地图信息来确定单个机器人的相对姿态。为了限制内存，Jimenez等人[7]提出了一种将生成的地图分布在多个机器人之间进行存储的方法。

除了集中式体系结构外，还提出了一些去中心化集群SLAM体系结构。去中心化SLAM架构，如[8]、[9]中提出的架构，在计算、通信和隐私方面比集中式架构具有显著优势。Tian等人提出了一种多机器人框架Kimeramulti[10]，该框架在以完全分布式方式操作的同时，实现了与集中式SLAM系统相当的估计误差。

现有的群解决方案主要集中在提高定位性能上，很少关注构建密集的点云图，或者最多关注构建稀疏的点云地图。为了实现大规模密集点云地图的实时构建，有必要开发一种集本地化和密集地图功能于一体的集群架构。

B.视觉SLAM

提出了许多视觉SLAM方法。不可能在有限的空间内覆盖所有这些，因此，我们只介绍最常见的视觉SLAM解决方案和GNSS辅助定位方法。

第一个实时单SLAM系统是基于扩展卡尔曼滤波器（EKF）的单SLAM[11]。PTAM[12]是第一个并行跟踪线程和映射线程的SLAM系统。除了跟踪和映射之外，ORB-SLAM[6]中还增加了一个额外的循环关闭线程，可以有效地消除检测到环回的累积错误。也有一些直接的方法来估计姿态并建立半密集或密集地图，如[13]-[15]。然而，直接方法是基于灰度级不变的假设。这种假设在大规模场景中可能是无效的。除了单眼SLAM之外，还提出了一些立体SLAM系统。ORB-SLAM2（立体声）[16]、proSLAM[17]和Vins Fusion（立体声）可以根据相机外部参数恢复定位和点云的规模。然而，在大规模场景中，由相机的外部参数恢复的尺度是不准确的。因此，全球导航卫星系统信息被用于协助我们的工作进行定位。

全球导航卫星系统（GNSS）可以在大规模场景中提供相对准确的定位，并且在规模上是可观测的。因此，视觉里程计（VO）系统可以与GNSS相结合，用于优化全球定位、相对定位和尺度恢复。秦[18]提出了一种将全球传感器（GNSS、气压计、磁力计等）与本地里程计相耦合的框架。这个框架可以容纳各种传感器，但只有当位移足够时，才能准确地恢复标尺。Simon等人[19]提出了一种实现最先进性能的VI-GPS紧密耦合SLAM系统。然而，惯性测量单元可能对大规模场景无效。

C. 密集重建

基于视觉几何特征的密集重建技术已广泛应用于深度相机、离线地图重建等。SfM方法过去相当全面，但受算法框架的限制，无法实现实时三维重建。

立体匹配算法是三维重建的关键组成部分，它从多个视角重叠的图像中找到相应的像素，并估计三维场景的密集点云图。传统的立体匹配算法可分为局部匹配方法和全局能量最小化方法。对于大规模图像，由于搜索域的限制，局部匹配方法通常表现得更有效。Bleyer等人[20]提出的方法应用倾斜支撑窗口，并在像素级别计算3D平面。使用ELAS[21]在接近恒定的时间内执行局部立体声匹配。该算法通过Delaunay三角测量从校正后的立体图像中使用稳健的支持点来生成2D网格。贝叶斯推理用于根据每个三角形内的像素差异快速独立地匹配它们。

基于深度学习的多视图重建领域在过去几年中发展迅速。姚等人[22]提出了一种标准的端到端多视图立体深度估计网络，该网络可以恢复图像的深度，进而可以实现点云的密集重建。使用一种名为Soft Argmin[23]的方法来拟合深度，这可以在一定程度上解决问题。由于深度图是在每个视图的基础上预测的，因此当将多个局部重建组合到全局3D模型中时，预测中的误差将导致不一致。NeRF[24]由Mildenhall，Ben等人提出，它是一种隐含的场景表示。基于NeRF的场景重建已经做了很多工作，取得了良好的效果，例如Manhattan SDF[25]。然而，这些方法很难应用于大规模场景中的实时重建。

方法

图2描述了集群重建系统的框架，它主要由四个部分组成：偏态估计、密集重构、相对位姿估计和点云融合。框架的流水线将在以下小节中介绍。为了节省篇幅，在后面的方法描述中，我们只给出了优化问题的残差公式。完整的优化方程如下所示，其中X是要优化的变量；K是构造的残差集合；EN是残差，Wn是信息矩阵，表示残差的权重

A.自我姿态和相对姿态估计

尽管在单无人机状态估计方面取得了重大进展，但由于通信带宽、计算能力、定位精度和尺度漂移有限，用于集群映射的多无人机协同定位仍然面临挑战。为了解决这些问题，已经实现了一种利用全球导航卫星系统信息和共视关系的轻量级和可扩展的多无人机定位系统，以满足相对姿态估计和尺度恢复的需求。单无人机定位由基于视觉的里程计前端和两阶段联合优化后端组成。一旦获得了单个无人机的姿态，就会自动识别多个无人机的重叠观测区域，并应用相对姿态优化方法来优化它们的相对姿态。我们提出的方法充分利用了GNSS提供的先验信息来加速重叠区域的检索。为了最大限度地利用多核CPU，提高系统的实时性，该系统采用多线程并行处理。多无人机定位系统的流程图如图3所示。

1） 视觉里程计：

准确有效的姿态估计对于大规模场景的3D重建至关重要。然而，在处理无人机拍摄的高分辨率图像时，ORB特征点提取和匹配方法在准确性和计算效率方面存在局限性。我们采用SiftGPU[26]进行特征点检测和描述符计算。这种方法不仅提供了高鲁棒性，而且利用了GPU的并行计算能力来实现实时处理速度。一旦获得了两帧之间的匹配特征点，就提出了一个优化问题来确定无人机姿态。

考虑到优化函数可以适用于不同的相机模型，假设本节中的匹配点位于归一化平面上。让pci（uci，vci）和pcj ucj、vcj 是在归一化平面上的两个帧中观察到的相同3D点。根据重新投影公式，可以将pci点重新投影到pcj所在帧的相机归一化平面，以构建残差函数。三维坐标表示为Pcj Xcj、Ycj、Zcj 其从从i帧重新投影到j帧的pci（uci，vci）的归一化坐标恢复。重投影公式可以写如下：

其中，Twci是从第i帧的相机坐标系到初始世界坐标系的变换，这意味着第i帧在世界坐标系中的姿态。T-1 wcj是从第j帧的世界坐标系到相机坐标系的变换。λ是特征点的深度。视觉残差最终可以表示为：

在通过优化获得图像之间的相对姿态后，通过三角测量恢复稀疏点云的深度信息。然后将该深度信息用于密集重建。如果GNSS信息可用，则将图像姿态和稀疏点云转换到以地球为中心、固定地球（ECEF）的坐标系，以获得重建区域的全局位置。

2) 两阶段联合优化：

采用两阶段联合优化算法估计多架无人机的相对位姿，并计算点云和位姿的空间尺度。在VO前端，获得无人机的局部位姿TWC。
在第一阶段，将这些局部位姿与从GNSS获得的观测信息相结合，恢复相对位姿的尺度，并优化当前全局位姿。
在第二阶段，利用GNSS数据估计多个无人机的相对位姿。第一帧GNSS数据用于在初始化时建立初始ENU(East-North-Up)坐标系，后续的GNSS数据将被转换到该坐标系中。同时将VO初始局部坐标系设置为原点世界坐标。

在获得VO和GNSS数据之后，对准两个源的时间戳，并且构造优化函数来估计全局位姿并计算度量尺度。在优化过程之前，必须求解尺度参数s的初始值

其中（gxk，gyk，gzk）表示从当前ENU坐标系到初始ENU坐标系统的平移。（pxk，pyk，pzk）表示当前帧在VO原点坐标系中的平移，而（px0，py0，pz0）是VO原点坐标系统中的初始平移。两阶段联合优化残差函数由两部分组成。第一部分可以表示为：

其中eq是由VO的估计姿态生成的旋转约束。
et是由VO估计的姿态生成的平移约束。
qlj，qli，tl j，tl i分别表示与VO局部坐标系中的GNSS时间戳对准后两个相邻坐标之间的姿态变换。
s是要优化的规模。
qwj，qwi，twj，twi表示ENU坐标系中两个相邻坐标之间的姿态变换，这两个坐标也是分别要优化的变量。
 是对四元数误差状态的操作。
第二部分是全球导航卫星系统观测信息的制约因素：

其中tGNSS k是当前帧到GNSS初始坐标系的平移。twk是当前帧在世界坐标系中的平移。s是要优化的规模。经过评估，以上构建的优化方程可以快速收敛，准确计算出姿态的尺度。由于密集点云图的计算需要每个图像在相机坐标系中的姿态，因此有必要将优化后的姿态转换到相机坐标系。
两阶段定位算法除了可以恢复相机姿态和稀疏点云的比例外，还可以估计相对姿态。为了估计多个无人机的相对姿态，我们定义了一个中心无人机，其姿态表示为R0-lw，P0-GNSS。这里，R0-lw表示中心VO局部坐标系到世界坐标系（ENU）的旋转。类似地，P0-GNSS表示中央无人机的GNSS原始坐标系（ENU）的经度、纬度和高度。对于第k架无人机，其姿态定义为Rk-lw，Pk-GNSS。因此，第k架无人机到中心无人机的旋转为R0−1 lw Rk lw，平移为t0k，由P0-GNSS、Pk-GNSS计算。T0k用于表示从第k架无人机的初始坐标系到中心无人机的原始坐标系的转换，以便于后面的表达。

在大规模和长期重建中，两阶段联合GNSS定位算法可以有效降低定位漂移。根据Tk=T0−1 lw T0kTk lw，可以实时更新相对姿态。

3） 相对姿态优化：

虽然上述算法能够确定多架无人机之间的初始相对姿态，但由于GNSS数据中的误差，仅依靠GNSS信息进行点云对准可能无法达到所需的精度水平。为了提高多架无人机之间相对姿态的准确性，有必要结合图像中的额外信息进行进一步优化。本文提出的框架的重点是大规模场景中的密集重建，因此获得的图像很好地符合平面假设，可以用于实现无人机的精确相对定位。
我们提出了一种使用单应矩阵来优化多架无人机的相对位置的方法。由于计算的限制，每对无人机的相对姿态都是逐一优化的。首先，我们利用从前面提到的两阶段联合优化算法中获得的相对姿态来选择候选重叠图像对。使用重叠阈值、相机固有参数和飞行高度来计算所需距离。我们分别使用无人机的图像位置序列构建KD树来加速检索，并检索与所需距离匹配的序列。使用来自不同序列的对应图像对来构造同态矩阵。通过分解矩阵，可以获得图像序列之间的位置观测。
基于单应性分解的观测姿态表示为qH c1c2，tH c1c2。通过计算从每架无人机到中心无人机的变换，可以找到两架无人机之间的相对位置。转换定义为：

其中，qcore c1、tcore c1和qcore c2、tcore c2表示从两个无人机的坐标系到中央无人机的座标系的变换；qc1 k1，tc1 k1和qc2 k2，tc2 k2表示两个无人机的姿态。在优化相对姿态之前，有必要将无人机的相对姿态与单应矩阵分解得到的变换之间的比例对齐。可以通过以下方式恢复尺度：

要优化的变量是qcore c1，tcore c1和qcore c2，tcore c2。多架无人机的图像信息用于提高相对姿态的准确性，这些信息将用于后期的点云融合阶段。

图3。多无人机定位系统分为三个部分：VO、两阶段GNSS联合优化和多无人机联合优化。VO组件使用所收集的大规模图像来估计自我姿态。两阶段GNSS联合优化组件优化无人机的全球姿态，并使用GNSS信息计算其初始相对姿态。多无人机联合优化组件利用初始相对姿态信息来识别重叠图像，并通过图像之间的公共视图关系来改进相对姿态。

B.测绘

映射过程涉及密集3D地图的重建，可分为两个主要阶段：致密化和融合。密集化涉及由单个无人机重建3D地图，而由多个无人机重建的地图块在第二阶段融合在一起，称为融合。

1)致密化：在通过共定位过程获得图像帧之间的精确相对姿态估计之后，可以执行密集映射过程。在小规模或室内重建场景中，通常使用立体相机来生成密集点云。然而，在大规模场景中，立体相机的有限基线使得实时生成精确的密集点云具有挑战性。我们构建虚拟立体对使用本地准确的姿态估计VO和在两个不同的时刻捕获的图像。这允许精确的计算和实时性能。使用深度图生成密集点云图。为了提高匹配效率，我们使用Bouguet [27]提出的方法来最大化左右视图的公共区域。在构建密集视差图的过程中，采用快速立体匹配方法ELAS [21]。

2） 融合：在点云融合过程中，中心无人机接收到每架无人机构建的密集三维点云图，以及无人机之间的实时相对姿态。对于第k架无人机重建的密集点云中的一个点pk t，将其转换为中心无人机构建的点云图，如下所示：p0 t=t−1 k pk t。融合的密集点云地图可能由于场景重叠而具有重叠，这显著增加了点云处理的内存消耗和计算复杂性。为了消除这种重叠，使用体素化方法对点云地图进行下采样，可以在一定程度上降低点云的规模。由于我们的方法适用于大规模场景，我们使用位置信息对点云进行分割，并将其存储在不同的点云中，以便于写入点云的非活动部分，从而节省内存。每个点的指数计算如下：

其中Px、Py是点云到中心坐标系中x−y平面上位置的坐标。dc是点云块的宽度。以这种方式存储点云信息不仅可以节省内存，而且在后端点云的体素化等操作中可以有效限制点云的规模。

图4。这组图像显示了使用环普数据集进行密集重建的细节。第一行图像显示由ClusterFusion生成的密集点云的详细信息，而第二行显示由DenseFusion产生的图像。
图5。这组图像展示了在大规模场景中进行密集重建的效果。这两行中的图像分别表示使用ClusterFusion和DenseFusion的重建结果。数据集1为工厂，数据集2为五台，数据集3为雨花寨。

实验

我们使用C实现了一个实时多无人机协同映射框架，并使用大规模场景中的多无人机映射数据集来评估其性能。该数据集主要通过真实世界中的航空摄影获得。该数据集包括多架无人机获得的高分辨率图像数据以及相应的全球导航卫星系统数据。每张图像包含超过12000000个像素，涵盖了城市和农田等各种场景。该数据集适用于地图制作、使用多架无人机的协作地图绘制和机器学习算法训练等应用。使用该数据集构建的点云地图如图4和图5所示。可以看出，多无人机协同测绘构建的点云地图在精度上与DenseFusion相似。在环普数据集中，这两种方法都能有效地恢复建筑物和游乐场的细节。当涉及到大规模场景时，本文提出的方法生成的点云图与DenseFusion相当，尤其是在雨花寨场景中，更符合平面假设。

为了验证所提出的框架的性能，我们设计了三个实验来比较定位和映射的鲁棒性、速度和准确性。实验在台式计算机上进行，该计算机配备Intel（R）Xeon（R）CPU E5-2676 v3@2.40GHz、GeForce GTX 1080和32G RAM。首先，比较了两种算法的规模恢复速度和稳定性。其次，将我们的方法的重建速度与之前的研究DenseFusion[1]进行了比较。最后，通过将我们的算法与SIBITO2进行比较来评估重建精度，结果表明我们的方法达到了类似的重建精度。

A.全球导航卫星系统优化

对于无人机集群的密集重建，调整无人机之间的姿态估计尺度对于点云融合的成功至关重要。计算单个无人机的平移尺度，然后与使用VINSGlobalFusion[18]方法计算的尺度进行比较。结果如图6所示。实验结果表明，该方法能够更快、更稳定地计算姿态估计的尺度。由于我们的数据分析表明，视觉里程计的刻度漂移有限，估计的刻度应随GNSS误差而波动，并保持相对稳定。对多个数据集生成的数据进行比较表明，我们提出的方法有效地消除了GNSS数据误差，提高了尺度估计的稳健性。

B.计算性能

使用相同的设备和数据集，通过将所提出的多无人机协同重建框架与Densfusion进行比较来评估性能。比较结果如表I所示。结果表明，在相同的建筑面积下，ClusterFusion的效率高于DenseFusion。得益于精心设计的多无人机重建架构，多架无人机可以在不消耗太多计算资源和带宽的情况下协作定位、重建和点云融合。这大大提高了重建效率。在单个计算平台上运行这个实验限制了潜在的加速。如果我们将框架分布在多个计算平台上，则可以进一步提高计算速度。三维重建的计算速度不仅受到无人机数量的影响，还受到图像分辨率的影响。在通过来自虚拟立体对的匹配像素点实现3D的阶段，这种影响尤其明显。为了提高算法的效率，利用GPU的并行计算能力可能是一种有价值的优化策略。

三维重建的计算速度不仅受到无人机数量的影响，还受到图像分辨率的影响。在通过来自虚拟立体对的匹配像素点实现3D的阶段，这种影响尤其明显。为了提高算法的效率，利用GPU的并行计算能力可能是一种有价值的优化策略。

C.准确性评估

对于点云的精度评估，目前还没有完善的评估方法。商业软件SIBITO基于SfM方法构建的点云在理论上具有更高的精度。我们建议使用我们的框架为数据集中的各种场景生成点云地图，然后将其与SIBITO软件构建的离线点云地图进行比较。我们使用最小二乘平面法来计算两点云之间的距离。这种方法使我们能够评估我们的框架在为不同场景生成准确的点云地图方面的有效性。一些评估结果如图7所示。结果表明，在环普数据集中，93.7%的点的误差小于2.0m，74.2%的点的精度小于1.0m；在雨花寨数据集中，90.1%的点的误差小于2.0m，74.8%的点的精度小于1.0m。映射的准确性主要受单个无人机的姿态估计和多个无人机之间的相对姿态估计精度的影响。通过双边滤波算法，可以去除某些坏点，提高点云的质量。与SIBITO相比，该框架的准确性相对接近，速度快了几倍。

结论

在本文中，我们提出了一个用于多架无人机的协作实时密集重建框架，包括视觉里程计、两阶段联合GNSS优化、密集化、相对姿态优化和密集点云图融合。与以前的密集重建框架相比，所提出的框架具有几个优点。首先，它比以前的工作更快，使用相同的运算平台，处理速度是以前的两倍多。其次，该框架可以移植到集群无人机上进行联合重建。在分布式平台上，计算效率和鲁棒性将得到进一步提高。最后，它是可扩展的，支持以低成本扩大无人机的数量。

所提出的框架在大规模密集重建任务中表现更好，但在未来的研究中仍有一些方面需要考虑。加强框架对不同环境的适应性将是有益的。优化框架和算法以充分利用GPU的并行计算性能可以提高性能。最后，在完成密集重建后，实时地图分析处理是必要的，以实现对环境变化的快速响应。