# 基于高效子地图的自主微型飞行器探索：支持激光雷达或深度相机，采用视觉-惯性SLAM技术
索蒂里斯·帕帕西奥多罗 $^{1,2,3,4,*}$、西蒙·博切 $^{1,*}$、塞巴斯蒂安·巴尔巴斯·莱纳 $^{1,3}$、斯特凡·洛伊特内格 $^{1,2,3,4}$
摘要— 未知空间的自主探索是移动机器人在现实世界中部署的关键环节。安全导航对所有机器人应用而言都至关重要，它要求机器人能够获取精确且一致的周围环境地图。为了实现完全自主性并适应多种环境，机器人必须依赖机载状态估计系统，但该系统随时间推移容易出现漂移现象。我们提出了一种基于局部子地图的微型飞行器（MAV）探索框架，通过针对子地图相对位姿应用回环校正来保持全局一致性。为实现大规模探索，我们从局部子地图的前沿区域高效计算出覆盖整个环境的全局前沿区域，并采用基于采样的下一最佳视角探索规划器。该方法能够无缝适配激光雷达传感器或深度相机，因而适用于不同类型的MAV平台。我们在仿真环境中将该方法与先进的子地图基探索框架进行对比评估，以验证其效率与重建质量。最后，我们展示了该方法在真实MAV上的应用效果：一台搭载激光雷达，另一台搭载深度相机。
## 一、引言
未知环境的自主探索一直是移动机器人领域的研究热点，至今仍是一项挑战。它在工业或建筑工地巡检等实际机器人应用中具有重要意义。由于微型飞行器具备多功能性、灵活性，且能抵达人类或地面机器人无法到达的区域，因此成为常用的平台选择。探索目标通常被定义为尽可能快速地发现并绘制环境地图 [^1], [^2], [^3]。理想情况下，为实现完全自主性，机器人需仅依靠传感器输入，在线逐步构建出精确且完整的环境地图，该地图应包含机器人导航与定位所需的所有信息。当前先进的方法通常将这一问题转化为确定能最大化某种效用函数的下一最佳位姿，并找到通往该目标位姿的无碰撞路径。
融合互补传感器的同步定位与地图构建（SLAM）方法被广泛应用于机载
Fig. 1: MAV trajectory and live-reconstructed mesh at 10 cm resolution for the real-world experiments using a LiDAR (left) and a depth camera (right).
位姿估计。视觉-惯性SLAM（VI-SLAM）结合了视觉与惯性测量数据，虽能提供较高的短期精度，但存在漂移累积问题 [^4], [^5], [^6]。近年来，随着价格低廉的激光雷达传感器逐渐普及，旨在减少漂移的激光雷达-视觉-惯性SLAM（LVI-SLAM）成为了研究热点 [^7], [^8], [^9]。
在大规模场景中，不可避免的漂移会导致地图精度显著下降。一种潜在的解决方法是利用局部子地图，其前提是假设小范围内的漂移可忽略不计。为维持全局一致性，可在检测到回环时更新各个刚性子地图之间的相对位姿 [^10]，或将其纳入状态估计问题中 [^9], [^11]。
在使用子地图时，追踪前沿——即已观测空间与未观测空间之间的边界——会变得颇具挑战性，因为可能会涉及大量子地图。近期的研究提出了分层探索方法，用以区分局部探索规划与全局探索规划 [^12], [^13]。在我们的研究中，我们证明了实现快速高效的探索并不需要如此复杂的分层方法。
我们提出以下贡献：
- 一种快速且轻量级的3D探索框架，它利用SLAM与子地图技术，通过回环闭合来补偿里程计漂移现象。
- 一种高效的全局前沿更新方法，能够在不区分局部规划与全局规划的情况下，实现精确且完整的大规模探索。
- 所提出的系统能够开箱即用，无缝支持搭载激光雷达或深度相机传感器的无人机。
- 在仿真环境与真实世界中进行的定量与定性评估表明，与现有最先进的方法相比，该方法能够实现更快的探索速度、更低的资源占用率，同时生成更一致且完整的重建结果。
---
### II. 相关工作
### A. 多传感器SLAM
在没有先验基础设施的环境中执行自主探索任务时，需要估算机器人的状态。视觉惯性SLAM（VI-SLAM）利用摄像头图像与惯性测量单元（IMU）的数据来实现这一目的，这对于能够进行高动态运动的无人机而言尤为重要；纯视觉方法由于容易丢失跟踪目标，往往无法胜任此类任务。VI-SLAM系统通常分为基于滤波的方法，例如 [^14], [^15]，以及基于优化的方法 [^4], [^5], [^6], [^16]。后者往往能提供更高的精度，通常将位姿估计表述为因子图优化问题，旨在最小化IMU积分误差与视觉重投影误差所产生的残差。为了降低因IMU测量噪声积分而导致的VI-SLAM系统漂移现象，融合基于激光雷达的残差已被广泛研究。许多研究采用了LOAM中提出的早期几何型激光雷达特征残差（边缘与平面） [^17]，例如 [^7]。不过，基于直接匹配或迭代最近点（ICP）的变体也被证明同样有效 [^8], [^18]。在我们的研究中，我们使用了在 [^9] 中介绍的LVI-SLAM系统。
### B. 体素映射
自主探索系统中一个重要的设计决策就是所采用的地图表示方法。用于后续任务的密集三维地图可以通过体素映射来获取。开创性的工作KinectFusion [^19]将地图表示为截断有符号距离场（TSDF）。遗憾的是，基于TSDF的地图无法显式表示自由空间，因此不适合用于导航任务。采用欧几里得有符号距离场（ESDF）是解决这一问题的一种方法，例如在voxblox [^20]中便是如此。另一类研究则采用占据映射，将地图表示为占据概率的离散网格。大量研究效仿Octomap [^21]，使用八叉树数据结构作为底层地图表示方式。此后又提出了多种扩展，比如UFOMap [^22]和supereight2 [^23]。这两种方法都能显式存储未知空间信息，并支持自适应分辨率；仅对所需细节程度内的三维空间进行建模。在我们的研究中，占据子图是利用supereight2 [^23]生成的。
### C. 自主探索
机器人探索领域的相关研究通常可分为基于采样的方法和基于边界的方法。
边界的概念最早是在[^1]中提出的。在环境的逐步重建过程中，边界被定义为已知自由空间与未知空间之间的分界线，它指示了那些观测后可能带来大量信息增益的区域。这一概念在随后的大量研究中得到了应用，例如[^24]、[^25]。
另一方面，基于采样的方法会选取若干候选视点或路径，然后根据效用函数的计算结果来确定下一个目标点，这一思路源自[^2]的开创性工作。候选的下一个视点会在自由空间中选取，而最佳视点的确定依据是候选视点沿路径所能观测到的未知体积大小。通过启发式采样可以提升这些方法的效率，比如在边界附近[^3]、[^26]、[^27]，或依据其他效用函数[^28]、[^29]。
为了应对大规模环境，近期的研究提出了分层或多阶段的设计方案，以区分局部规划与全局规划。在[^30]中，局部规划由快速探索随机树（RRT）负责，用以确定下一个最佳视点。全局规划器则采用高斯过程模型，用于存储未执行视点所能带来的信息增益。GBPlanner [^13]也遵循这种两阶段模式，它由基于RRT*的局部规划器和基于图的全局规划器组成。当局部规划器陷入死胡同时，全局规划图会引导机器人前往边界区域。此外，[^13]还会对所选路径施加额外的安全约束。
无论采用何种设计方案，都需要准确的实时姿态估计，这类数据通常源自SLAM技术。为了克服因漂移导致的地图质量下降问题，已有多种解决方案被提出。[^31]通过视觉回环发生时对测量数据进行计算代价高昂的解算与重新整合来解决这一问题。此外，系统还会主动引导探索过程，以寻找潜在的回环候选区域。另一类研究则采用了子图策略：全局地图由一系列子图构成，这些子图可在发生回环或位姿图优化时重新排布，正如[^9]、[^11]、[^32]、[^33]中所描述的那样。
GLocal [^12]提出了一种子图与分层探索相结合的成功方案。基于Voxgraph [^11]的子图会定期创建，并通过位姿图优化进行更新，从而能够处理回环问题。在GLocal系统中，微型无人机首先利用基于RRT $^{*}$的局部规划器来探索周围环境。一旦附近区域的探索价值降低，全局规划器便会引导机器人前往全局边界区域。此前生成的子图中的可通行性图能够支持高效的全局路径规划。
与GLocal类似，我们的系统也建立在子图概念之上。与上述方法不同的是，我们无需区分局部规划与全局规划。借助对局部边界和全局边界的高效更新，我们可以应用统一的全局规划器，并通过效用函数设计来优先选择邻近的候选下一个位姿。
### III. 问题描述
在本研究中，我们的目标是利用配备激光雷达或深度相机的全自主微型无人机，构建出精确、完整且全局一致的环境体素表示。为了实现对大型环境的适配，我们采用子图来表示环境。
### A. 环境模型
静态环境被建模为一个有界体积$V \subset \mathbb{R}^3$，其中每个点$\mathbf{v} \in V$都对应一个占据概率$P_o(\mathbf{v})$。所有点$\mathbf{v} \in V$的初始占据状态均为未知，定义为$P_o(\mathbf{v}) = 0.5$。由于环境的几何特性以及微型无人机和传感器的特性，有些点$V_{\text{unob}} \subset V$是无法被观测到的。
---
Fig. 2: Overview of the system's main modules: (i) a VI-SLAM system, optionally with LiDAR, that estimates the MAV's state based on sensor inputs and submaps, (ii) an occupancy mapping backend dividing the global volume into spatially bounded submaps and keeping track of a submap collection, (iii) an exploration planning module computing the next best path based on global frontiers of all submaps and (iv) a linear MPC executing the planned paths.
因此，探索过程仅考虑环境中可被观测的部分$V_{\text{obs}} = V \setminus V_{\text{unob}}$。探索的目标是生成一系列子图$\mathcal{M} = \{M_i \subseteq V, i \in \{1 \ldots n_m\}\}$，从而使得$\bigcup_{i=1}^{n_m} M_i = V_{\text{obs}}$，同时将所有$\mathbf{v} \in V_{\text{obs}}$的占据概率更新为“空闲”或“已占据”状态。
### B. 微型无人机模型
微型无人机的状态$\mathbf{x}$包括位置$\mathbf{r} \in V$、朝向$\mathbf{q} \in SO(3)$以及线速度$\boldsymbol{v} \in \mathbb{R}^3$。在探索与路径规划中，我们考虑微型无人机完整状态中的一部分$\hat{\mathbf{x}} = [\mathbf{r}^T \boldsymbol{\psi}]^T \in V \times [-\pi, \pi)$，其中$\boldsymbol{\psi}$是它相对于世界坐标系$\mathcal{F}_W$的偏航角。我们还假设微型无人机被包裹在一个以$R \in \mathbb{R}^+$为中心、半径为$\mathbf{r}$的球体内，且其最大线速度为$\boldsymbol{v}_{\max} \in \mathbb{R}^+$，最大偏航率为$\boldsymbol{\omega}_{\max} \in \mathbb{R}^+$。微型无人机配备有激光雷达传感器或深度相机，其水平视场角和垂直视场角分别为$\alpha_h \in (0, 2\pi]$和$\alpha_v \in (0, \pi]$，测量范围为$[d_{\min}, d_{\max}] \subset \mathbb{R}^+$。
### IV. 系统概述
我们的系统集成了VI-SLAM、密集的3D占据地图构建、探索规划以及无人机模型预测控制器（MPC）。为了解决VI-SLAM产生的累积漂移问题，并生成局部一致且精确的地图，我们采用了空间受限的子地图机制。利用SLAM估算的位姿，将激光雷达测量数据或深度图像整合到当前活跃的子地图中。子地图及其局部边界在生成完成后会被存入子地图集合中。全局边界仅在需要时才会根据子地图中的局部边界进行更新。这种方式使得我们能够使用单一的全局探索规划器，无需像其他方法那样区分局部规划和全局规划。全局边界会用于基于采样的方法中，从信息增益的角度确定下一个最佳路径，随后由MPC对该路径进行跟踪。我们的方法无需GPU支持，因此能够部署在小型且资源受限的平台上。下文我们将介绍系统的各个模块，如图2所示。
### A. 状态估计器
我们采用OKVIS2 [^4]作为状态估计器，这是一种先进的基于优化的稀疏型VI-SLAM系统。它接收立体灰度图像对和惯性测量数据，并以惯性测量单元的频率输出状态估计结果，其中包括无人机状态x和惯性传感器偏差。对于配备激光雷达的无人机，我们还会整合之前的研究成果 [^9]，该技术基于占据地图及其梯度构建残差项，以优化(a)新测量数据与先前子地图之间的一致性，以及(b)重叠子地图之间的一致性。相关细节可参考 [^9]。
### B. 占据地图构建
我们采用supereight2 [^23]来基于八叉树构建体积占据地图。Supereight2能够显式表示自由空间，并将最小和最大的占据值传递到八叉树的根节点，从而实现安全高效的路径规划。原始的supereight2仅支持整合深度图像，这些图像要么来自深度相机，要么由结构化激光雷达扫描投影得到。为了适配动态移动的激光雷达传感器，我们采用了 [^9] 中提出的整合方案。
Supereight2构成了我们的子地图框架的基础，正如 [^9] 和 [^34] 中所述。对于激光雷达传感器，新子地图的生成基于几何重叠情况，如 [^9] 中所述；对于深度相机，则基于视觉关键帧生成，如 [^34] 中所述。在这两种情况下，子地图都会锚定在OKVIS2关键帧的状态上，并在位姿优化过程中随其一同变换，包括闭环校正。一旦新子地图创建完成，前一个子地图就会被冻结，整个任务期间仅允许对其施加刚性变换。
### C. 探索规划
我们对在 [^3] 中提出的针对整体地图的探索规划器进行了扩展，使其能适配子地图机制。该规划器的核心思路保持不变，为方便起见在此重申。候选的下一个位置 $\hat{\mathbf{r}}_j \in V, j \in \{1 \dots n_c\}$、$n_c \in \mathbb{N}^+$ 会在边界附近采样，因为这些区域在被观测后会扩展地图范围。系统会从当前无人机位置规划一条通往每个候选位置的路径，并假设无人机以 $t_j$ 和 $v_{\text{max}}$ 的速度飞行，进而计算出预计耗时 $\omega_{\text{max}}$。接着从每个候选位置 $360^\circ$ 出发执行一次 $\hat{\mathbf{r}}_j$ 地图熵射线投射，从而生成增益图像。考虑到传感器的水平视场角 $\alpha_h$，系统会对增益图像进行滑动窗口优化，以确定能产生最高潜在收益的偏航角 $\psi_j$。
---
增益为$g_{j}$。每个候选点j的效用通过$u_{j}=g_{j}/t_{j}$来计算，本质上就是最大化随时间的信息增益。效用最高的候选点会成为下一个目标，一旦MAV到达该点，这一过程便会重复。
本研究中使用的探索规划器在两个方面与[^3]有所不同。首先，我们根据子地图集合$\mathcal{M}$来计算全局边界，这作为我们的核心贡献之一在第V节中进行了介绍。其次，对于配备$\alpha_h = 360^\circ$的传感器，其在增益图像上的偏航角优化方式也与[^3]的设计不兼容。在这种情况下，候选偏航角$\psi_j$可以任意选择，而候选增益$g_j$只需在整个图像上计算即可。为简化操作，我们使用滑动窗口来计算$\psi_j$，该窗口对应的水平视场角小于$360^\circ$。
### D. MAV控制器
我们采用基于[^35]的线性MPC来实现MAV轨迹跟踪，并按照[^34]中的描述进行了修改。简而言之，对MPC的修改旨在确保即使在发生重大里程计变化（如闭环情况）时也能正确跟踪轨迹。具体做法是将轨迹锚定在OKVIS2关键帧状态上，并随着关键帧状态的实时更新而对轨迹进行弹性变形。
### V. 全局边界计算
探索规划器会在边界附近采样候选的下一个位置，因此必须计算出考虑所有子地图及其重叠区域的全局边界集。若某个子地图中的边界对应另一子地图中已被完全观测的区域，则该边界不属于全局边界。
为了实现高效的全局边界计算，我们采取以下措施：(i) 分别记录子地图局部的边界和全局边界；(ii) 仅在必要时更新两者。每次计算全局边界前，都会先更新当前活跃子地图的局部边界。子地图完成后也会更新局部边界，此后在任务剩余时间内这些边界将保持固定。在这两种情况下，仅会考虑自上次局部边界计算以来被修改的子地图区域。每次探索规划器迭代时都会重新计算全局边界，此时会综合考虑当前已知的所有局部边界。由于全局边界仅用于采样候选的下一个观测点，这种方案能确保不会进行不必要的计算。下文将详细介绍局部和全局边界的计算过程。
### A. 局部边界计算
对于每个子地图 $M_i$，我们会在时间步长 k 时记录一组局部边界 $\mathcal{F}_i^k$，这一过程是将 $M_i$ 与其他子地图隔离开来单独考虑的。局部边界指的是那些满足以下条件的空闲体素：(i) 至少有一个未知面邻居体素；或者 (ii) 位于子地图边界上。后者对于使用有限范围子地图时确定正确的全局边界至关重要。
只有当前活动子地图的局部边界 $\mathcal{F}_i^k$ 会被更新。这一更新分为两步完成。首先，重新检测所有已有的局部边界 $\mathcal{F}_i^{k-1}$，从而识别出已过时的边界 $\mathcal{F}_{i,\text{stale}}^{k-1}$。接着，仅针对那些体素进行计算，从而得出新边界的集合 $\mathcal{F}_{i,\text{new}}^k$。
Fig. 3: Local-only (blue) and global (red) frontiers in a set of submaps. The local-only submap frontiers correspond to regions mapped in other submaps and are thus not considered to be global frontiers.
这些体素自 $\mathcal{F}_i^{k-1}$ 被计算出来之后便发生了修改。最终，时间步长 k 时的局部边界可按照如下方式计算得出：
$$ \mathcal{F}_{i}^{k}=\left(\mathcal{F}_{i}^{k-1}\setminus\mathcal{F}_{i,\mathrm{s t a l e}}^{k-1}\right)\cup\mathcal{F}_{i,\mathrm{n e w}}^{k}. $$
对陈旧边界的检测$\mathcal{F}_{i,\text{stale}}^{k-1}$可以与新边界的计算$\mathcal{F}_{i,\text{new}}^{k}$并行进行。
### B. 全局边界计算
时间步k时的全局边界$\mathcal{F}_g^k$计算如下：
$$ \mathcal{F}_{g}^{k}=\bigcup_{i=1}^{n_{m}}\mathcal{F}_{g,i}^{k}, $$
其中，$\mathcal{F}_{g,i}^k \subseteq \mathcal{F}_i$是$M_i$的局部边界，同时也是时间步k时的全局边界；$\mathcal{F}_i$是$M_i$的最后已知边界。$\mathcal{F}_{g,i}^k$是通过将$\mathcal{F}_i$中的每个边界与其他所有子地图进行对比来计算的。
$$ \mathcal{F}_{g,i}^{k}=\Big\{f_{i}\in\mathcal{F}_{i},\bigwedge_{l\neq i}h(f_{i},M_{l})\Big\}, $$
其中，布尔值函数$h(f_i, M_l)$用于指示当考虑$f_i$时，$M_i$的边界$M_l$是否也属于边界。当且仅当$h(f_i, M_l)$或$f_i \notin M_l$对应$f_i$中的未知区域，或者$M_l$对应$f_i$的局部边界时，$M_l$的值为真。$\mathcal{F}_{g,i}^k$的计算可以针对每个子地图并行执行。
图3展示了局部边界和全局边界的示例。
### VI. 评估
### A. 仿真结果
我们的仿真环境由Gazebo[^36]和软件模拟的PX4自动驾驶仪[^37]组成，模拟了真实世界多旋翼飞行器（MAV）的实际配置。该仿真MAV基于RMF-Owl[^38]，是一款配备激光雷达传感器和Intel RealSense D455 RGB-D摄像头的$0.38 \times 0.38 \times 0.24$米级四旋翼飞行器。评估在图4所示的$30 \times 15 \times 9$米级仓库式环境Depot中进行。所有仿真实验均在配备Intel Core i7-1165G7 CPU和32 GB内存的计算机上开展。
我们从探索速度、环境重建的准确性和完整性，以及CPU和内存占用等方面，将我们的方法与最先进的基于子地图的3D探索规划器GLocal[^12]进行比较。GLocal
---
需要配备激光雷达传感器，且采用Voxgraph[^11]进行子地图对齐，这与我们基于[^9]的方法类似。因此，我们仅在仿真中评估基于激光雷达的探索方式。两种方法均从OKVIS2获取位姿数据，包括视觉回环信息。我们的方法包含基于[^9]的子地图对齐，而GLocal则采用来自[^11]的地图间对齐方式。两种流程均使用第IV-D节所述的MAV控制器。我们在可行的情况下尽力为两种方法设置相同的参数，其中最重要的参数列于表I中。
| Map resolution | 0.1 m | LiDAR resolution | 360 $\times$ 180 |
| R | 0.5 m | $\alpha_{h}, \alpha_{v}$ | 360 $\degree$, 90 $\degree$ |
| $v_{max}$ | 0.5 m/s | $d_{min}, d_{max}$ | 1 m, 10 m |
| $\omega_{max}$ | 0.5 rad/s | $n_{c}$ (n/a to GLocal) | 20 |
TABLE I: Simulation experiment parameters.
Fig. 4: Perspective and top-down visualisation of the Depot environment. Top: Ground-truth. Middle and Bottom: Final reconstruction (left) and mesh accuracy (right), using our approach and GLocal respectively.
1) 已探索体积：为避免不同映射框架导致的已探索体积差异，我们记录了激光雷达测量数据及对应的真实MAV位姿。利用这些数据构建统一的supereight2地图，并在每次积分后测量已探索体积。对于每种方法，我们使用OKVIS2提供的位姿进行了10次运行，另以真实位姿作为基准进行了10次运行。图5展示了两种方法所探索的总体积百分比的中位数、$10^{th}$和$90^{th}$百分位数。在探索速度、一致性和最终环境覆盖度方面，我们的方法均优于GLocal。我们的方法几乎总能在300秒内探索完100%的体积，而GLocal有时无法探索完整个体积。使用真实位姿时，各次运行间的差异更小，这归因于更一致的地图减少了边界数量，从而为MAV提供了更多可导航的自由空间。
2) 安全性评估：我们通过计算仓库真实网格模型与无人机真实轨迹上各点之间的最小距离，来评估探索规划器的安全性。图6展示了这些距离的直方图。
Fig. 5: Explored volume median, $10^{th}$ and $90^{th}$ percentiles for 10 runs in the Depot environment. Solid for SLAM, dashed/hatched for ground-truth runs.
Fig. 6: Histogram of minimum distance of the MAV to obstacles across 10 simulated missions with SLAM poses and a safety radius of 0.5 m. The bin size is 0.25 m.
该直方图涵盖了全部10次任务中的数据。总体而言，我们的方法即使在靠近障碍物的区域也能实现安全导航；而GLocal违反安全半径$R = 0.5 \, m$的情况则频繁得多，从而导致更高的碰撞风险。
3) 重建质量：我们还评估了后处理步骤中生成的、分辨率为0.1米的最终网格模型的重建质量。在我们的方法中，最终地图重建是利用最终 Bundle Adjustment 之后估算出的位姿来计算的，以尽可能获得高精度结果。我们利用仓库的真实网格模型来计算重建结果的均方根误差（RMSE）。完整性则是以重建结果与真实模型之间距离在0.2米或0.4米以内的百分比来衡量。表II列出了全部10次运行的平均RMSE和完整性指标。我们的方法在这两项指标上均取得了显著更好的结果，证明在不牺牲重建质量的前提下实现安全路径规划是可行的。某种程度上，这一现象源于GLocal所使用的voxblox建图框架为了更安全地进行路径规划而人为放大了障碍物尺寸。图4展示了最终重建结果及其精度的可视化对比。
4) 资源占用情况：我们还在表III中对比了我们的方法与GLocal所需的计算资源。尽管两种方法都只在必要时计算边界，但我们的方法得益于supereight2更高效的地图表示方式。我们只针对有限数量的候选下一个位置计算效用值，而在GLocal中，用于局部规划的树结构中的每个顶点都要计算该值，因此耗时显著更多。
---
| RMSE (m) $\downarrow$ | within 0.2 m | within 0.4 m |
| Ours | 0.112 | 86.07 | 97.73 |
| GLocal | 0.219 | 44.19 | 76.97 |
| Ours (GT) | 0.095 | 89.89 | 97.98 |
| GLocal (GT) | 0.213 | 45.29 | 78.20 |
TABLE II: Mesh reconstruction RMSE and completeness using both SLAM and ground-truth (GT) poses in the Depot environment.
规划时间包括效用值计算、探索规划和路径规划。GLocal之所以规划时间较长，是因为它有一个专门用来高速重新规划任务的线程，该线程会占用大量的CPU时间——即便排除那些耗时不到200毫秒的简单规划迭代，情况依然如此。最后，我们的系统内存占用较小，也是由于supereight2地图的高效性所致。
| Frontiers (s) | Utility (s) | Planning (s) | Memory (GB) |
| Ours | $16 \pm 7$ | $11 \pm 6$ | $134 \pm 45$ | $6.7 \pm 1.0$ |
| GLocal | $52 \pm 7$ | $275 \pm 84$ | $421 \pm 76$ | $23.5 \pm 1.8$ |
TABLE III: Mean and standard deviation of per-mission frontier, utility, and path and exploration planning computation time and memory usage.
### B. 实际场景实验
我们开展了实际场景实验，以进一步展示所提出的探索方法在资源受限平台上的有效性，并验证其同时适用于激光雷达传感器和深度相机的特性。实验在一个尺寸为$8.0 \times 8.1 \times 3.7$米×$\approx 240$米×$^3$米的房间内进行，房间内存在一些较高的障碍物。
Fig. 7: MAVs used for real-world experiments. Left: Leica BLK2Fly with LiDAR. Right: Custom MAV with Intel Realsense D455 RGB-D camera.
在激光雷达探索实验中，我们使用的是Leica BLK2Fly无人机，该无人机配备了5个摄像头、5个惯性测量单元以及单束双轴旋转激光雷达传感器。在进行视觉惯性同步定位与建图（VI-SLAM）时，仅使用前、左、底部摄像头以及底部惯性测量单元。该激光雷达的有效频率为5 Hz，两个轴向的视场角均为360°，不过由于无人机机身会遮挡部分视野，因此$\alpha_h$和$\alpha_v$均小于360°。机载激光雷达点云过滤后，每秒点数会减少到$\approx 100,000$个，即20万个点。由于制造商未开放无人机的通用机载资源，传感器数据通过WiFi传输到笔记本电脑上，由笔记本电脑运行LVI-SLAM、建图及探索规划程序。用于离线处理的笔记本电脑配备英特尔酷睿i7-13850HX处理器和32 GB内存。
在深度相机探索实验中，我们使用了一架自行组装的四旋翼无人机。该无人机基于Holybro S500框架，配备了Intel RealSense D455立体RGB-D相机以及NVIDIA Jetson Orin NX 16 GB机载计算机。包括视觉惯性同步定位与建图（VI-SLAM）、密集占据地图构建、探索路径规划以及模型预测控制（MPC）在内的所有计算任务均在机载设备上完成。
图7展示了这两种无人机平台。实验中所用的参数详见表四。
| LiDAR | Depth camera |
| Map resolution | 0.1 m | 0.1 m |
| $d_{\text{min}}, d_{\text{max}}$ | 0.25 m, 10 m | 0.2 m, 4 m |
| R | 0.6 m | 0.6 m |
| $v_{\text{max}}$ | 1.0 m/s | 0.5 m/s |
| $\omega_{\text{max}}$ | - | 0.785 rad/s |
TABLE IV: Real-world experiment parameters.
我们分别使用每种无人机平台进行了一次实验。最终的网格重建结果及估算的无人机轨迹如图1所示，而图8则展示了随时间变化的探测体积。正如预期，由于搭载的激光雷达视场角更大，配备激光雷达的无人机探测速度更快，不到一分钟便能实现近乎完全的环境覆盖。不过，即便使用视场角受限得多的深度相机，我们也能高效地完成对房间的几乎全方位探索。
Fig. 8: Explored volume over time for the two real-world experiments.
### 七、结论
在本研究中，我们提出了一种高效且轻量级的基于子图的自主探索方法，该方法能够兼容深度相机和激光雷达作为输入模态。我们的方法利用子图构建与视觉惯性（激光雷达）同步定位与建图技术，即便存在里程计漂移现象，仍能实现精确且一致的地图构建，从而确保操作的安全性。通过汇总后的子图来高效计算全局边界，我们只需采用统一的全局规划策略，无需再像现有先进方法那样区分局部规划与全局规划。与现有的基于子图的大规模探索框架相比，我们的方法不仅探索速度更快、环境重建更精确，而且资源利用率更高。该方法已在两种实际无人机平台上得到应用：一种搭载深度相机，另一种则配备激光雷达。
在未来的工作中，我们希望融入语义信息与视觉语言模型（VLM），以生成信息更丰富、对后续任务更有用的环境地图。我们还计划研究如何结合无人机动力学特性来进行轨迹规划，并制定安全的无人机控制策略。
---
## 参考文献
[^1]: B. Yamauchi, “A frontier-based approach for autonomous exploration,” in Proceedings 1997 IEEE International Symposium on Computational Intelligence in Robotics and Automation CIRA’97. ‘Towards New Computational Principles for Robotics and Automation’. IEEE, 1997, pp. 146–151.
[^2]: A. Bircher, M. Kamel, K. Alexis, H. Oleynikova, and R. Siegwart, “Receding horizon “next-best-view” planner for 3D exploration,” in 2016 IEEE international conference on robotics and automation (ICRA). IEEE, 2016, pp. 1462–1468.
[^3]: A. Dai, S. Papatheodorou, N. Funk, D. Tzoumanikas, and S. Leutenegger, “Fast frontier-based information-driven autonomous exploration with an MAV,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 9570–9576.
[^4]: S. Leutenegger, “OKVIS2: Realtime scalable visual-inertial SLAM with loop closure,” arXiv preprint arXiv:2202.09199, 2022.
[^5]: C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D. Tardós, “ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM,” IEEE Transactions on Robotics, vol. 37, no. 6, pp. 1874–1890, 2021.
[^6]: V. Usenko, N. Demmel, D. Schubert, J. Stückler, and D. Cremers, “Visual-inertial mapping with non-linear factor recovery,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 422–429, 2019.
[^7]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping," in 2021 IEEE international conference on robotics and automation (ICRA). IEEE, 2021, pp. 5692–5698.
[^8]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, "FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry," in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022, pp. 4003–4009.
[^9]: S. Boche, S. Barbas Laina, and S. Leutenegger, “Tightly-coupled LiDAR-visual-inertial SLAM and large-scale volumetric occupancy mapping,” arXiv preprint arXiv:2403.02280, 2024.
[^10]: Y. Wang, N. Funk, M. Ramezani, S. Papatheodorou, M. Popović, M. Camurri, S. Leutenegger, and M. Fallon, “Elastic and efficient LiDAR reconstruction for large-scale exploration tasks,” in 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021, pp. 5035–5041.
[^11]: V. Reijgwart, A. Millane, H. Oleynikova, R. Siegwart, C. Cadena, and J. Nieto, “Voxgraph: Globally consistent, volumetric mapping using signed distance function submaps,” IEEE Robotics and Automation Letters, vol. 5, no. 1, pp. 227–234, 2019.
[^12]: L. Schmid, V. Reijgwart, L. Ott, J. Nieto, R. Siegwart, and C. Cadena, "A unified approach for autonomous volumetric exploration of large scale environments under severe odometry drift," IEEE Robotics and Automation Letters, vol. 6, no. 3, pp. 4504–4511, 2021.
[^13]: T. Dang, S. Khattak, F. Mascarich, and K. Alexis, “Explore locally, plan globally: A path planning framework for autonomous robotic exploration in subterranean environments,” in 2019 19th International Conference on Advanced Robotics (ICAR). IEEE, 2019, pp. 9–16.
[^14]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE international conference on robotics and automation. IEEE, 2007, pp. 3565–3572.
[^15]: P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 4666–4672.
[^16]: T. Qin, S. Cao, J. Pan, and S. Shen, “A general optimization-based framework for global pose estimation with multiple sensors,” arXiv preprint arXiv:1901.03642, 2019.
[^17]: J. Zhang and S. Singh, “LOAM: Lidar odometry and mapping in real-time.” in Robotics: Science and systems, vol. 2, no. 9. Berkeley, CA, 2014, pp. 1–9.
[^18]: I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, "KISS-ICP: In defense of point-to-point ICP-simple, accurate, and robust registration if done the right way," IEEE Robotics and Automation Letters, vol. 8, no. 2, pp. 1029–1036, 2023.
[^19]: R. A. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J. Davison, P. Kohi, J. Shotton, S. Hodges, and A. Fitzgibbon, "KinectFusion: Real-time dense surface mapping and tracking," in 2011 10th IEEE international symposium on mixed and augmented reality. IEEE, 2011, pp. 127–136.
[^20]: H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3D Euclidean signed distance fields for onboard MAV planning,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 1366–1373.
[^21]: A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “OctoMap: An efficient probabilistic 3D mapping framework based on octrees,” Autonomous robots, vol. 34, pp. 189–206, 2013.
[^22]: D. Duberg and P. Jensfelt, "UFOMap: An efficient probabilistic 3D mapping framework that embraces the unknown," IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 6411–6418, 2020.
[^23]: N. Funk, J. Tarrio, S. Papatheodorou, M. Popović, P. F. Alcantarilla, and S. Leutenegger, “Multi-resolution 3D mapping with explicit free space representation for fast and accurate mobile robot motion planning,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3553–3560, 2021.
[^24]: T. Cieslewski, E. Kaufmann, and D. Scaramuzza, “Rapid exploration with multi-rotors: A frontier selection method for high speed flight,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 2135–2142.
[^25]: W. Gao, M. Booker, A. Adiwahono, M. Yuan, J. Wang, and Y. W. Yun, "An improved frontier-based approach for autonomous exploration," in 2018 15th International Conference on Control, Automation, Robotics and Vision (ICARCV). IEEE, 2018, pp. 292–297.
[^26]: D. Duberg and P. Jensfelt, “UFOExplorer: Fast and scalable sampling-based exploration with a graph-based planning structure,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 2487–2494, 2022.
[^27]: P. Zhong, B. Chen, S. Lu, X. Meng, and Y. Liang, “Information-driven fast marching autonomous exploration with aerial robots,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 810–817, 2021.
[^28]: L. Schmid, M. Pantic, R. Khanna, L. Ott, R. Siegwart, and J. Nieto, “An efficient sampling-based method for online informative path planning in unknown environments,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 1500–1507, 2020.
[^29]: A. Batinovic, A. Ivanovic, T. Petrovic, and S. Bogdan, “A shadowcasting-based next-best-view planner for autonomous 3D exploration,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 2969–2976, 2022.
[^30]: M. Selin, M. Tiger, D. Duberg, F. Heintz, and P. Jensfelt, “Efficient autonomous exploration planning of large-scale 3-D environments,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 1699–1706, 2019.
[^31]: Y. Zhang, B. Zhou, L. Wang, and S. Shen, “Exploration with global consistency using real-time re-integration and active loop closure,” in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022, pp. 9682–9688.
[^32]: A. Millane, Z. Taylor, H. Oleynikova, J. Nieto, R. Siegwart, and C. Cadena, "C-blox: A scalable and consistent TSDF-based dense mapping approach," in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 995–1002.
[^33]: Y. Wang, M. Ramezani, M. Mattamala, S. T. Digumarti, and M. Fallon, “Strategies for large scale elastic and semantic LiDAR reconstruction,” Robotics and Autonomous Systems, vol. 155, p. 104185, 2022.
[^34]: S. Barbas Laina, S. Boche, S. Papatheodorou, D. Tzoumanikas, S. Schaefer, H. Chen, and S. Leutenegger, “Scalable outdoors autonomous drone flight with visual-inertial SLAM and dense submaps built without LiDAR,” arXiv preprint arXiv:2403.09596, 2024.
[^35]: D. Tzoumanikas, W. Li, M. Grimm, K. Zhang, M. Kovac, and S. Leutenegger, “Fully autonomous micro air vehicle flight and landing on a moving target using visual-inertial estimation and model-predictive control,” Journal of Field Robotics, vol. 36, no. 1, pp. 49–77, 2019.
[^36]: Open Robotics, "Gazebo Fortress", .
[^37]: PX4 Autopilot, "PX4 Autopilot Software", .
[^38]: P. De Petris, H. Nguyen, M. Dharmadhikari, M. Kulkarni, N. Khedekar, F. Mascarich, and K. Alexis, “RMF-Owl: A collision-tolerant flying robot for autonomous subterranean exploration,” in 2022 International Conference on Unmanned Aircraft Systems (ICUAS). IEEE, 2022, pp. 536–543.