系列文章目录

协同探索与导航文献整理（一）

---

---

1. SOAR:异构无人机协同探索实现快速自主重建

1.1 介绍

摘要： 无人机（UAVs）在场景重建领域已获得显著关注。本文介绍了SOAR，一个专为复杂环境快速自主重建设计的LiDAR-视觉异构多无人机系统。我们的系统包括一个配备大视场（FoV）LiDAR的探索者以及配备相机的拍摄者。为确保快速获取场景表面几何信息，我们采用了一种基于表面前沿的探索策略。随着表面的逐步探索，我们识别未覆盖区域并逐步生成视点。这些视点通过解决一致多仓库多旅行商问题（Consistent-MDMTSP）分配给拍摄者，该问题在确保任务一致性的同时优化了扫描效率。最后，拍摄者利用分配的视点确定最佳覆盖路径以获取图像。我们在现实模拟器中进行了广泛的基准测试，验证了SOAR与经典及最新方法相比的性能。

主要创新点：
我们在模拟中比较了我们的方法与经典和最先进的方法。结果表明，在基准场景中，我们的方法实现了更高的效率和卓越的重建质量。总之，本文的贡献总结如下：
1.一种新颖的LiDAR-视觉异构多无人机系统，能够快速高效地完成重建任务。
2.一种增量视点生成方法，随着表面信息的逐步获取，生成最少数量的视点以确保全面覆盖。
3.一种任务分配方法，迭代优化扫描效率，同时确保连续任务分配的一致性。
4.所提出的方法已在两个现实模拟环境中得到广泛验证。我们系统的源代码将会发布

1.2 运行结果

1.3 总体框架

如图2所示，该系统包括一个探索者和多个拍摄无人机。探索者采用基于表面前沿的探索方法（第V-A节）快速获取场景的几何信息。同时，随着更多表面的探索，视点会逐步生成（第V-B节）。这些视点通过具有高任务一致性的Consistent-MDMTSP方法均匀高效地分配给每个摄影师（第V-C节）。摄影师利用接收到的视点聚类任务作为局部路径规划和生成轨迹的全局指导（第V-D节），确保以最短时间完成分配的任务。最后，图像-姿态对被发送进行离线3D重建，生成带有纹理的3D模型。

1.4 核心技术

A. 基于表面前沿的探索

为了为场景覆盖提供足够的先验结构信息，我们旨在让探索者专注于场景的表面。受[13]的启发，我们采用表面前沿引导的规划方法进行快速探索。

表面前沿检测和视点生成

一个占据前沿体素 $v_u$ 可以定义为一个自由体素，它有一个占据的邻近体素 $v_o$ 和一个未知的邻近体素 $v_u$，其中 $v_o$ 和 $v_u$ 也是邻居。类似于[14]，所有 $v_u$ 首先基于连通性进行聚类，然后使用基于PCA的聚类方法将较大的聚类拆分成更小的聚类。随着探索的进行，过时的聚类被删除，新的聚类被检测到。当没有表面前沿时，探索结束。

我们计算 $N_c$ 个聚类的质心，并在一定半径内从质心采样一组具有不同偏航角的视点，以更好地观察水平面上的倾斜平面，例如屋顶，我们添加了不同的采样高度以生成视点。对于每个聚类，我们选择具有最大可见性的视点 $v{p}_s=(p_s,{\psi }_s)$ 作为该聚类的代表，其中 $p_s$ 和 ${\psi }_s$ 分别表示视点的位置和偏航角。$v{p}_s$ 将被保留以指导聚类的路径规划。

探索路径规划

寻找 $N_c$ 个视点的最短访问路径将提高探索效率。我们将这个问题建模为一个不对称旅行商问题（ATSP），并设计成本矩阵 $C_{\text{exp}}$，该矩阵可以被 Lin-Kernighan-Helsgaun 启发式求解器[15]（LKH-Solver）处理。$C_{\text{exp}}$ 是一个 $N_c+1$ 维的方阵，其中 $(N_c+1)\times N_c$ 的块由每对视点和探索者当前位置 $(p_0,{\psi }_0)$ 之间的成本组成，表示为

$$C(i,j)=\max \left\{\frac{\text{Len}(P(p_i,p_j))}{v_{\max }},\frac{|{\psi }_i-{\psi }_j|}{{\psi }_{\max }}\right\},\text{\hspace{1em}(1)}$$

其中 $P(p_i,p_j)$ 表示通过 A* 算法搜索的 $p_i$ 和 $p_j$ 之间的无碰撞路径，$v_{\max }$ 和 ${\psi }_{\max }$ 分别表示最大速度和偏航角变化率。从当前位置到自身的成本将被设置为零，因为我们的方法不考虑返回的成本。最终，$C_{\text{exp}}$ 可以表示为

$$C_{\text{exp}}(i,j)=\{\begin{array}{llc}0,& 0\le i\le N_c,0\le j\le N_{c}C(i,j),& \text{其他情况}\end{array}.\text{\hspace{1em}(2)}$$

通过求解 ATSP 问题，我们可以确定下一个要访问的视点，然后利用 MINCO[16] 从当前位置生成一条连续的无碰撞轨迹到下一个视点，从而快速探索场景表面。

B. 用于任务分配的一致性多目标旅行商问题

在本节中，我们介绍了一种新的任务分配方法，该方法基于视点聚类任务结构（第V-C.1节），同时确保连续任务分配中的扫描效率和一致性（第V-C.2节）。

视点聚类任务结构

广泛的感知范围和快速的探索节奏使得需要分配给摄影师的视点数量相当可观，如果直接分配，会带来显著的时间开销。为应对这一挑战，我们借鉴了[19]的思路，采用视点聚类方法将所有视点划分为多个子集。此外，我们设计了一种视点聚类任务（VCT）结构，以增量方式维护VCT的状态。

每个VCT由四个参数组成：$V{P}_i$，$p_{avg,i}$，$h_{cost,i}$，和$L_{cost,i}$。$V{P}_i$表示VCT中所有视点的位置，$p_{avg,i}$表示$V{P}_i$的平均位置，$h_{cost,i}$表示VCT的执行成本，近似仅取决于$V{P}_i$中的视点数量。数学表达式如下：

$$h_{\text{cost},i}={\lambda }_h\ast (\text{NUM}(V{P}_i)-1)\ast d_{\text{thr}}\text{\hspace{1em}(5)}$$

其中，$\text{NUM}(V{P}_i)$表示VCT中的视点数量，$d_{\text{thr}}$表示视点聚类的距离阈值。$L_{\text{cost},i}$表示$V{P}_i$与其他所有VCT的平均位置之间的A*路径距离。数学表达式如下：

$$L_{\text{cost},i,j}=\text{Len}[P(p_{\text{avg},i},p_{\text{avg},j})]\text{\hspace{1em}(6)}$$

请注意，$d_{\text{thr}}$相对于整个场景较小，因此假设$p_{avg,i}$在整个后续计算过程中相对稳定。因此，我们可以增量地维护$L_{\text{cost},i}$。

所提出的视点聚类方法主要依赖于可见性和距离进行聚类。它确保了聚类内的任意两个视点之间没有遮挡，并且它们之间的距离小于距离阈值$d_{\text{thr}}$。每当添加一个新的视点时，首先根据距离优先级在$d_{\text{thr}}$范围内与现有的VCT进行迭代匹配。如果新视点可以与$V{P}_i$中的视点进行无遮挡的射线投射，并且成对距离都小于阈值$d_{\text{thr}}$，则该视点被合并到VCT中。否则，它初始化为一个新的VCT。当VCT中的视点$v{p}_j$被访问时，我们从$V{P}_j$中移除该视点，并更新$p_{avg,j}$和$h_{\text{cost},j}$。如果$V{P}_j$没有视点，VCT将被移除。

一致性多目标旅行商问题

将多个任务分配给多个无人机，同时最小化每架无人机的最大旅行时间的优化问题可以表述为多旅行商问题（MTSP）。尽管有成熟的求解器[15]用于解决MTSP，但直接使用这些求解器存在两个主要问题：1）由于地图信息不完整，每次仅获得局部最优解会导致一致性较差，从而导致摄影师不必要的绕路。2）由于任务每次都会根据较小的变化进行增量更新，因此重新计算整体分配结果是不必要的。为了解决这些问题，我们提出了一种基于遗传算法（GA）的一致性MDMTSP方法。该方法结合了与任务一致性相关的成本项，并通过利用之前的计算结果迭代生成新的分配结果。

在我们的方法中，我们采用了多染色体遗传表示[20]，这使得问题的解码和编码更加高效。如图4所示，种群中的每个个体包含多个染色体，每个个体代表问题的一个解决方案。假设有 $N_p$ 个摄影师和 $N_{vct}$ 个尚未完成的VCT。令单个个体 I = { P A T H 1 , … , P A T H N p } I = \{PATH_1, \ldots, PATH_{N_p}\} I={ PATH1​,…,PATHNp​​}，其中 P A T H i = { x i , 1 , … , x i , M i } PATH_i = \{x_{i,1}, \ldots, x_{i,M_i}\} PATHi​={ xi,1​,…,xi,Mi​​} 且 ${\sum }_{i=1}^{N_p}{M}_i=N_{vct}$。这里，$PAT{H}_i$ 表示第 $i$ 个摄影师的访问路径序列，$x_{i,j}$ 表示 $PAT{H}_i$ 中第 $j$ 个要访问的VCT的ID，$M_i$ 表示 $PAT{H}_i$ 中的VCT数量。

我们的适应度函数被设计为距离成本和一致性成本的组合。为了评估距离成本，我们引入了一个加权有向图 $G=(V_d,E_d)$，其中 $V_d$ 包含 $N_p$ 个摄影师节点和 $N_{vct}$ 个VCT节点，$E_d$ 表示边的集合。我们维护两个权重矩阵，$C_{d,vct}$ 和 $C_{vct}$：前者表示所有摄影师和所有VCT之间的距离成本，后者表示VCT之间的距离成本。

$$C_{d,vct}(k_1,k_2)=\text{Len}[P(p_{d,k_1},p_{avg,k_2})]+h_{cost,k_2}\text{\hspace{1em}(7)}$$

k 1 ∈ { 1 , 2 , … , N p } , k 2 ∈ { 1 , 2 , … , N v c t } k_1 \in \{1, 2, \ldots, N_p\}, \quad k_2 \in \{1, 2, \ldots, N_{vct}\} k1​∈{ 1,2,…,Np​},k2​∈{ 1,2,…,Nvct​}，其中，前者表示所有摄影师和所有VCT之间的距离成本，后者表示所有VCT之间的距离成本：

$$C_{vct}(k_3,k_4)=L_{cost,k_3,k_4}+h_{cost,k_4}\text{\hspace{1em}(8)}$$

k 3 , k 4 ∈ { 1 , 2 , … , N v c t } k_3, k_4 \in \{1, 2, \ldots, N_{vct}\} k3​,k4​∈{ 1,2,…,Nvct​}

计算 $PAT{H}_i$ 的 $cos{t}_{dis,i}$ 如下：

$$cos{t}_{dis,i}=C_{d,vct}(i,x_{i,1})+\sum _{j=1}^{M_i-1}{C}_{vct}(x_{i,j},x_{i,j+1})\text{\hspace{1em}(9)}$$

为了提高任务分配的一致性，当距离成本相对相似时，我们希望当前的分配结果尽可能接近之前的分配结果。给定第 $i$ 个摄影师之前的访问路径序列，记为 P A T H i ′ = { x i , 1 ′ , … , x i , M i ′ ′ } PATH_i' = \{x_{i,1}', \ldots, x_{i,M_i'}'\} PATHi′​={ xi,1′​,…,xi,Mi′​′​}，我们的目标是最大化 $PAT{H}_i$ 和 $PAT{H}_i^{\prime }$ 之间的公共前缀长度，赋予初始段更高的权重，从而提高整体规划的一致性。

$$cos{t}_{con,i}=-\sum _{k=1}^{K_{\text{same}}}R\cdot e^{-\alpha \cdot \text{DSUM}(k)}\text{\hspace{1em}(10)}$$

其中，公共前缀的长度记为 $K_{\text{same}}$，而 $\text{DSUM}(k)$ 表示第 $i$ 个摄影师路径 ${\text{PATH}}_i$ 上前 $k$ 个VCT的累计距离。参数 $R$ 和 $\alpha$ 分别控制一致性的权重和距离衰减率。较低的 $cos{t}_{con,i}$ 表示任务一致性更高。

${\text{PATH}}_i$ 的总成本由下式给出：

$$cos{t}_{all,i}=cos{t}_{dis,i}+cos{t}_{con,i}\text{\hspace{1em}(11)}$$

为了在摄影师之间实现VCT的平衡分配并保持高任务一致性，我们定义个体 $I$ 的适应度函数如下：

F i t ( I ) = − ( max ⁡ { c o s t a l l , i } i = 1 M i + ϵ ∗ ∑ i = 1 M i c o s t a l l , i ) (12) Fit(I) = -\left( \max\{cost_{all,i}\}_{i=1}^{M_i} + \epsilon * \sum_{i=1}^{M_i} cost_{all,i} \right)\tag{12} Fit(I)=−(max{ costall,i​}i=1Mi​​+ϵ∗i=1∑Mi​​costall,i​)(12)

为了最小化任何摄影师产生的最大成本，我们识别出成本最高的部分并相应地进行优化。此外，添加一个小的惩罚项以在最大成本相似时最小化总成本。负号用于将成本函数转换为适应度函数。

鉴于在地图更新之间只有一小部分VCT被修改，我们采用一种利用前一代最佳个体的策略。我们不是在每次迭代中随机初始化种群，而是利用前一次迭代中适应度最高的个体，记为 $I_{\text{best,prev}}$，作为生成当前初始种群 $P_{\text{init,cur}}$ 的基础。具体来说，我们从 $I_{\text{best,prev}}$ 中排除已执行的VCT，构建 $I_{\text{tmp}}$。然后，我们将所有新增的VCT随机插入到 $I_{\text{tmp}}$ 的染色体中，从而获得 $P_{\text{init,cur}}$ 中的一个个体。通过多次重复这一随机操作，完成初始种群的构建。这种方法显著减少了迭代次数。

最后，经过 $K_{\text{GA}}$ 次迭代后，优化过程结束，选择适应度最高的个体作为分配结果。然后将该结果传达给所有摄影师。

2. RACER: 一种使用分散式无人机群进行快速协同探索的方法

2.1介绍

摘要： 尽管多无人机系统（UAVs）在快速自主探索方面具有巨大潜力，但仍未受到足够的关注。在本文中，我们提出了一种基于去中心化无人机队伍的快速协同探索（RACER）方法。为了有效地调度无人机，采用基于在线网格空间分解的配对交互方法，确保所有无人机同时探索不同的区域，仅使用异步和有限的通信。此外，我们优化了未知空间的覆盖路径，并通过车辆路径规划问题将工作负载均衡分配给每个无人机。在任务分配的基础上，每个无人机不断更新覆盖路径并逐步提取关键信息来支持探索规划。分层规划器寻找探索路径，细化局部视点，并生成最短时间的轨迹以顺利安全地探索已知空间。所提出的方法被广泛评估，展现了高效的探索能力、可扩展性和在有限通信下的鲁棒性。此外，首次实现了在真实世界中完全去中心化的多无人机协同探索。我们将发布我们在现实世界中的开源实现。

2.2 运行结果

生成自己的PCD环境

2.3 总体框架

多旋翼无人机协同探索系统的整体结构如图2所示。与大多数最新研究[2, 4, 11]一样，我们的目标是构建指定空间的完整体积地图。系统中的每个无人机执行分散式状态估计以定位自身和其他无人机。在实验中，我们将[62]中的状态估计方法集成到我们的系统中。在估计出相对位置后，每个无人机在通信可用时频繁地与附近的无人机交换地图信息，以便进行更明智的决策。状态估计和映射的实现细节在第VII节中介绍。无人机团队的协调依赖于在线hgrid分解和成对交互。随着每个无人机探索和收集信息，整个未知空间（如大多数方法一样，由预定义边界限定）不断分解为包含不同大小单元的hgrid，这些单元作为基本任务单元分配给无人机。通过请求-响应方案调度，无人机对通过成对交互进行通信并更新单元的所有权（第IV节）。单元通过CVRP公式进行划分，该公式最小化了CP的长度并平衡了待探索的未知空间的体积，如第V节所述。

2.4 核心技术

A 基于CVRP的工作负载划分

为了适当划分属于一对交互无人机的hgrid单元（算法2），设计了一种CVRP公式。关键思想是最小化无人机覆盖路径（CPs）的总长度，并平衡分配给它们的未知空间量，使它们更有效地协作。

CVRP公式

可以通过车辆路径问题（VRP）[66]找到hgrid单元的最优CPs，该问题将旅行商问题（TSP）推广到多辆车。在我们的设置中，有两个车辆对应于交互无人机对$q_1,q_2$。与标准VRP不同，标准VRP中有一个中央仓库，车辆的路径形成闭合环路，我们需要的是从无人机位置开始并通过hgrid单元的开放路径。我们通过引入虚拟仓库并适当设计连接成本，将这种变体简化为非对称VRP。

1. 成本矩阵：假设有$N_h$个hgrid单元，非对称VRP涉及$N_h+3$个节点，其中$N_h$个节点用于hgrid单元，2个节点用于$q_1,q_2$，一个用于虚拟仓库。相关的成本矩阵$C_{avrp}\in {\mathbb{R}}^{(N_h+3)\times (N_h+3)}$具有以下形式：

$$C_{avrp}=\left[\begin{array}{ccc}0& -M_{inf}& 0\\ 0& 0& C_{q,h}\\ 0& C_{q,h}^T& C_h\end{array}\right]\text{\hspace{1em}(2)}$$
$C_h$ 是一个 $N_h\times N_h$ 的块，对应于所有hgrid单元之间的连接成本。由于我们的目标是找到最短的CPs，因此使用单元对之间的路径长度作为连接成本：

$$C_h(h_1,h_2)=C_h(h_2,h_1)=\text{Len}[P(c_{h_1},c_{h_2})],h_1,h_2\in [1,N_h]\text{\hspace{1em}(3)}$$

这里 $c_{h_1},c_{h_2}$ 表示 $h_1$-th 和 $h_2$-th 单元中未知体素的质心，而 $P(c_{h_1},c_{h_2})$ 是无碰撞路径，可以通过标准路径搜索算法找到。

无人机和hgrid单元之间的成本由 $C_{q,h}$ 计算，这是一个 $2\times N_h$ 的块。连接成本与方程3类似，除了一个一致性项：

$$C_{q,h}(i,h)=\text{len}[P(p_{q_i},c_h)]+c_{\text{con}}(i,h),\text{\hspace{1em}(4)}$$

$$h\in [1,N_h],i\in [1,2]$$

$$c_{\text{con}}(i,h)=\{\begin{array}{ll}{\beta }_{\text{con}},& \text{如果 }{q}_i\text{ 直接连接到第 }h\text{-th 单元}\\ 0,& \text{否则}\end{array}\text{\hspace{1em}(5)}$$

观察到有时存在多个具有可比长度但覆盖模式不同的解决方案（图5）。因此，引入 $c_{\text{con}}(\cdot )$ 以防止路径在不同模式之间频繁变化，这可能导致不一致的运动并减慢探索速度。

为了将我们的问题简化为非对称VRP，从虚拟仓库到两个无人机的连接成本被赋予块 $-M_{\text{inf}}=-[M_{\text{inf}},M_{\text{inf}}]$，其中 $M_{\text{inf}}$ 是一个很大的值。大的负成本使虚拟仓库的节点直接连接两个无人机，因为它极大地降低了输出路径的总体成本。通过这种方式，非对称VRP的输出由所需的最短路径和四个额外的边组成，其中两条边将仓库连接到无人机，而另外两条边将两个单元连接到仓库。我们的问题简化为非对称VRP的过程在图5中更好地说明。

图6. 两个无人机的覆盖路径及相应的未知空间分配。顶部：仅考虑总路径长度。未知空间的分配是不平衡的，在这种情况下，红色的无人机可以更快地完成全部探索。底部：通过引入容量约束，两个无人机的工作负载更加均衡。

2. 容量约束：尽管路线的长度已经优化，但每个无人机探索的实际未知区域数量并未被考虑，这有时仍会导致工作负载分配不均衡，如图6所示。因此，我们引入了车辆的容量约束，以进一步平衡分配给无人机的工作负载。对于VRP中的每个节点$v_k$，如果它与一个hgrid单元相关联（假设是第$h$个单元），则分配一个等于未知体素数量$u_{h_k}$的需求$p_k$。否则，设置为零需求。我们将每辆车的容量限制为未知体素总数的一定百分比。将无人机$q_i$路线中的节点集表示为$R_i$，容量约束为：

$$\sum _{v_k\in R_i}{p}_k\le {\alpha }_p\sum _{h=1}^{N_h}{u}_h,i=1,2\text{\hspace{1em}(6)}$$

通过容量约束，问题变成了一个CVRP。为了解决它，我们利用了由Helsgaun扩展的Lin-Kernighan-Helsgaun求解器[67]。该算法将VRP转换为等效的标准旅行商问题（TSP），并使用惩罚函数来处理容量约束。尽管这个问题已知是NP难的，但我们的问题规模较小，大多数情况下可以获得最优解[67]。

路径搜索的稀疏图

CVRP中涉及的主要开销是计算$C_h$和$C_{q,h}$，这需要$O(N^2)$路径搜索。在大规模环境中，这可能相当昂贵，因为需要在体积图上直接搜索大量全局路径。我们从[21, 68]中获得灵感，以减轻这种计算负担。具体来说，维护一个嵌入在hgrid中的稀疏图，利用它进行全局路径搜索。对于每个hgrid单元，附加一个图节点，并且如果两个单元的范围内存在无碰撞路径，则通过加权边将其与每个相邻单元的节点连接起来。每当hgrid单元更新时，就在体积图上重新计算到每个相邻单元的路径。如果找到解决方案，则通过路径长度更新边权重。否则，将边从图中移除。利用这个稀疏图，可以更容易地计算hgrid单元和无人机之间的连接成本。对于每对hgrid单元，在稀疏图上搜索一条路径来近似体积图上的最短路径。对于远离无人机的hgrid单元，成本通过无人机到最近hgrid单元的路径长度加上稀疏图上两个单元之间的路径长度来近似。在实践中，近似成本导致的CVRP解决方案与使用精确成本相比具有可比性，但大大减少了计算时间。