MCFS：任意形状环境中的多机器人路径规划

作者：Jingtao Tang, Hang Ma

编译：东岸因为@一点人工一点智能公众号

我们介绍了Multi-Robot Connected Fermat Spiral（MCFS），这是一个新颖的算法框架，用于多机器人覆盖路径规划（MCPP），首次将来自计算机图形界的连通费马螺旋线（Connected Fermat Spiral，CFS）适应到多机器人协调中。

MCFS独特地实现了多个机器人的阵列，生成环绕任意形状障碍物的覆盖路径，这在传统方法中尤为缺乏。我们的框架不仅增强了区域覆盖并优化了任务性能，尤其是在具有不规则障碍物的工作空间中，使时间度最小化，而且还通过生成功能平滑的路径而无需分解工作空间来解决非完整机器人的路径连续性和曲率的挑战。MCFS通过构建等值线图解决MCPP，并将MCPP转化为一个组合优化问题，目标是最小化时间度同时覆盖所有顶点。

我们的贡献包括开发一个统一的CFS版本，用于可扩展和可适应的MCPP，将其扩展到具有成本降低和路径连续性以及平滑度的新型优化技术的MCPP，并通过大量实验证明MCFS在时间度、路径曲率、覆盖比率和重叠比率方面优于现有的MCPP方法。我们的研究在MCPP方面标志着一个重要的步骤，展示了计算机图形和自动规划原则的融合，以提高复杂环境中多机器人系统的能力。

01 简介

在不断发展的多机器人系统领域中，多机器人覆盖路径规划（MCPP）的效率和有效性（Almadhoun等，2019）在各种应用中至关重要，包括从环境监测（Collins等，2021）到复杂工作空间中的搜救行动（Song等，2022）。传统的方法学，如区域分解（Latombe和Latombe，1991；Acar等，2002）和基于网格的方法（Gabriely和Rimon，2001；Hazon和Kaminka，2005），为理解和应对这些任务中固有挑战奠定了坚实的基础。然而，随着环境复杂性和对更高效覆盖的需求增加，迫切需要创新战略，能够灵活处理充满不规则障碍物的工作空间，具有高精度和适应性。

除了任务效率之外，MCPP中的一个关键挑战是管理非完整（nonholonomic）机器人所需的减速和急转弯。传统方法（Lu等人，2023年；Vandermeulen，Groß和Kolling，2019年）通常侧重于最小化路径转弯，局限于矩形工作空间，并依赖将区域分解为矩形。这种方法在任意形状的环境中效果较差。相反，我们MCFS框架的核心在于其全局覆盖策略，将路径构想为一系列相互连接的螺旋线，无缝整合多台机器人的运动。这种策略可以产生平滑的覆盖路径，无需分解，固有地考虑了路径曲率——这是高效机器人导航的一个重要因素。

我们的MCFS框架从CFS（连通费马螺旋线）在3D打印中的最初应用中得到灵感（Gibson等人，2021年），创新地将CFS应用于解决MCPP问题，通过将一组等距等高线转化为CFS，生成连续平滑的覆盖路径。MCFS首先构建了一个等高线图，将每个顶点与一条等高线联系起来，并将其连接到相邻等高线的相关顶点。然后，它将MCPP问题简化为Min-Max根树覆盖（MMRTC），这是一个组合优化问题，用于找到一组树来覆盖图的所有顶点，并最小化时间度。

我们的框架多才多艺，允许覆盖路径从MCPP所需的任意起点开始，并优化多台机器人之间对多个整体等高线和等高线部分的覆盖分配，展示了一种创新方法，有效管理每个机器人的时间度，曲率和路径连续性。

工作主要贡献总结如下：

1）我们提出了一个统一的CFS版本，标准化了相邻等高线的拼接，允许在选择拼接点时进行定制优先级，并通过使覆盖路径能够从任意给定的初始机器人位置开始，提供了可扩展性和易于适应MCPP。 

2）我们展示了我们的MCFS如何将这一统一版本的CFS扩展到MCPP，并有效解决了相应的MMRTC问题。 

3）我们引入了两种优化技术：一种是在非相邻但可连接的等高线对之间添加边来扩展解决方案空间，另一种是为了平衡路径成本和减少多机器人覆盖中的重叠而完善MMRTC解决方案。

4）我们提出了广泛的实验结果，验证了我们的MCFS在时间度、路径曲率、覆盖比率和重叠比率等指标上优于最先进的MCPP方法，展示了它在多种覆盖场景中的有效性。

02 相关工作

我们将现有的单机器人覆盖路径规划（CPP）和多机器人覆盖路径规划（MCPP）方法分类为基于网格的方法、区域分解法和全局方法。我们建议感兴趣的读者参考（Tomaszewski 2020）获取更详细的分类信息。

基于网格的方法：基于网格的覆盖方法将工作空间抽象为方形网格（Hazon和Kaminka 2005；Kapoutsis，Chatzichristofis和Kosmatopoulos 2017；Tang，Sun和Zhang 2021），允许应用各种图算法。其中一个突出的方法是最小生成树覆盖（STC）（Gabriely和Rimon 2001），它构建最小生成树，然后在树上生成环绕路径以覆盖工作空间。基于STC的MCPP方法（Hazon和Kaminka 2005；Tang和Ma 2023年）通过找到一组树共同访问所有顶点，并为每个机器人分配在树上环绕的路径来工作。虽然方便，基于网格的MCPP的最优解决复杂度随着工作空间大小和机器人数量呈指数级增长。

区域分解法：这些方法通过检测几何临界点（如梯形分解（Latombe和Latombe 1991）和Morse分解（Acar等人，2002））将工作空间分解为子区域。CPP方法在这些子区域中生成之字形路径进行覆盖（Choset 2000；Wong和MacDonald 2003），而MCPP方法连接并分配这些充满之字形路径的子区域给机器人进行协同覆盖（Rekleitis等人，2008；Mannadiar和Rekleitis 2010；Karapetyan等人，2017）。此外，一些研究对单个机器人的之字形路径方向进行了优化（Oksanen和Visala 2009；Bochkarev和Smith 2016）。尽管高效，由于依赖于几何分区，这些方法不太适用于障碍物丰富或非矩形工作空间。

全局方法：全局CPP方法直接生成路径来覆盖工作空间而不对其进行分解。它们分为两种类型：第一种类型生成环绕障碍物的单独路径（Yang等人，2002年），第二种类型生成一个闭合路径，包括螺旋路径（Ren，Sun和Guo 2009）和连续平滑路径而著称的CFS。CFS路径特别方便，因为它们的入口和出口相邻，便于多路径的整合。最近的一篇论文基于精确的测地线距离场构建了一个覆盖地形表面的CFS路径（Wu等人，2019年）。但据我们所知，目前尚未开发出用于MCPP的全局方法。

03 连通费马螺旋线(CFS)

我们的CFS是对原始CFS概念的一种改编。原始CFS采用两阶段过程，将一组等距等高线转化为覆盖输入多边形工作空间的闭合路径。它利用图结构，其中顶点代表单个等高线，边连接具有相邻段的等高线的顶点。最初，原始CFS识别一组“袋（pockets）”，即图的生成树上的连通分量。第一阶段将每个袋内的等高线转化为费马螺旋（Lockwood 1967年），第二阶段使用图的边穿越这些袋，将这些孤立的费马螺旋拼接起来构造最终的连通费马螺旋。原始CFS的详细信息可在原论文附录A中找到。

我们的CFS通用版本修改了等高线的图构建，并将原始的两阶段过程整合为CPP问题的一个通用的、连贯的操作。我们方法的主要修改在于拼接阶段。与明确识别袋然后拼接获得单机器人的费马螺旋的方式不同，我们的方法时一个通用版本，同时解决了在袋内将等高线转化为费马螺旋和这些螺旋的互相连接两个问题。这个集成过程应用于每一对可拼接的等高线，有效地将转化和拼接阶段合并。通过穿越图的有根生成树，得到了与原始CFS相同的连通费马螺旋。我们统一CFS方法的优势是双重的。首先，它增强了可扩展性，促进了多种效用在框架内的整合。其次，它简化了将CFS扩展到MCPP的过程。

3.1 构建等高线和等高线图

我们描述了我们用于生成带有分层等高线的给定多边形工作空间和构建等高线图的方法。多边形由其边界包围，包括一组代表障碍物的内部边界折线和一个外部边界折线。

生成带有分层等高线：该过程从在多边形内均匀采样2D网格点开始。为这些点构建了一个距离场，代表它们到多边形边界的最短距离（包括内部障碍边界折线和外部边界折线）。我们将相邻图层上的等高线之间的距离表示为l ，以及所有点到多边形边界的最大距离表示为l_{max}。然后我们使用Marching Squares算法（Maple 2003）为每一层i=1,2,...,[l_{max}/l]生成分层等高线。这确保了处于第i层等高线上的每个点与多边形边界的距离为l\times i。最后一步是重新采样沿着每条等高线的等距点，保持相邻点之间l的一致距离。

构建等高线图：将分层等高线的等高线图定义为一个向量图G=(V,E)，其中V是等高线顶点的集合，每个顶点与唯一的等高线相关联。为方便调用，我们让I_v和L_v表示与任意v\in V相关联的等高线及其相应的图层。与原始CFS类似，我们定义了一对isovertices u,v\in V在相邻图层（即|L_u-L_v|=1）上的连接段集合O_{u \rightarrow v}为：

其中d(p,I)表示点p 与等高线I 之间的距离。当O_{u \rightarrow v}不为空时，不同于原始CFS直接构建一个无向边(u,v) ，我们同时考虑了O_{u \rightarrow v} 用于边的构建。这种考虑在CFS上下文中为在任何顺序和从任何根isovertex遍历等高线图提供了灵活性。它也避免了添加边(u,v) ，其中相应的等高线I_u 和I_v 之间被多个等高线分隔，因为在CPP上下文中这样的对可能不适合进行拼接（请参阅第3.4节的案例研究）。因此，我们为任意相邻图层中的u,v\in V 定义了一组拼接元组O_{u,v} ，如下所示：

\mathcal{C}_u(p)表示沿着等高线I_u 到点p 的最近点。随后，对于任意相邻图层中具有非空O_{u,v} 的u,v\in V ，形成了一个无向边(u,v) 。每个(p,q)\in O_{u,v} 都作为一个候选的拼接元组，通过将p 拼接到q 和通过逆时针顺序沿着等高线I_u 的p 前面的点\mathcal{B}_u(p) 拼接到\mathcal{B}_u(q) ，来连接等高线I_u 和I_v 。图1显示了四条等高线是如何通过方块作为拼接点连接的。

尽管原始CFS为每条边分配了|O_{u\rightarrow v}| 的权重，方便在确定等高线图遍历顺序时保持低曲率路径，但目前我们将权重定义视为特定应用，并将明确其应用于拼接元组选择器中的每个拼接操作。

3.2 通用CFS算法（Unifying the CFS algorithm）

我们详细介绍了我们CFS算法的通用版本，见算法1，在该算法中，输入是一个等高线图G 和一个入口点p_0 。

算法解析，详见论文原文

3.3 拼接元组选择器

我们现在提出三种拼接元组选择器，每种都旨在从给定的O_{u,v} 集合中选择一个适当的拼接元组o ，以连接等高线I_u 和I_v 。图1展示了这些选择器的示例。

随机选择器：随机选择器f_{rnd} 从集合O_{u,v} 中随机选择一个拼接元组。

连续费马螺线（CFS）选择器：CFS选择器f_{cfs} 将我们的通用版本的CFS与原始CFS进行对齐。它尝试从O_{u,v} 选择一个拼接元组，对于边(u,v)\in E ，该拼接元组是与先前由f_{cfs} 选择的(r,u)\in E 或(r,v)\in E 的拼接元组相邻的拼接元组。在DFS遍历中，(r,u) 或(r,v) 中较早到达的顶点，其由f_{cfs} 选择的拼接元组将在(u,v) 之前访问（第3行）。假设(r,u) 首先被访问，并且已选择了拼接元组(p',q')\in O_{r,u} ，f_{cfs} 然后检查O_{u,v} 中的o=(p,q) ，其中\mathcal{B}(p)=q' 。如果存在这样的元组，它将被选择为(u,v) ；否则将选择O_{u,v} 中的第一个元组。

最小曲率拼接（MCS）选择器：MCS选择器f_{mcs} 遍历O_{u,v} 以确定在拼接前后最小化曲率差\Delta \kappa(o) 的拼接元组o=(p,q) ，其定义为：

其中\kappa _\pi(p) 和\kappa _{I_u}(p) 分别表示在使用o 形成的新拼接路径\pi 上任意点p 的曲率和原始等高线I_u 上的曲率。形式上，MCS选择器被定义为

3.4 案例研究：通用版与原始CFS

我们将讨论在CPP环境中构建等高线图的边集时对我们的通用版本CFS的修改的必要性。与原始CFS在等式（1）中使用单向O_{u\rightarrow v} 进行边集构建并总是从最低层等高线顶点开始遍历不同，我们的通用CFS定义了更灵活的双向O_{u,v} (等式（2）)。这种修改解决了CPP（和MCPP）要求从任意给定点p_0 开始覆盖路径的需求，正如算法1中所考虑的。我们的通用CFS从包含p_0 的等高线的等高线顶点r 开始图的遍历，而不受r 是否是最低层等高线顶点的限制。因此，如果只像原始CFS那样考虑从第i 层到第i+1 层的单向拼接元组，那么仅能构建边缘并不存在有效的拼接元组。而且，具有局部内部最内层等高线的等高线顶点u 可能会发现对于任意满足L_v=L_u+1 的等高线顶点v ，存在非空的O_{u\rightarrow v} ，将(u,v) 视为边缘，从而潜在地导致路径重叠。图2-(a)举例说明了一些局部最内层等高线拼接到相邻层的等高线，但却被其他等高线分隔的情况，这种情况在我们的通用CFS中得到了有效处理（图2-(b)），但在使用原始CFS定义时可能会出现问题。

04 多机器人CFS覆盖

在这一部分，我们介绍我们的MCFS框架，用于解决MCPP问题。MCFS从输入的等高线图计算多棵树，每棵树对应于一个不同的机器人，然后对每棵树应用CFS算法计算单独的覆盖路径。在4.1节中，我们详细介绍了基于CFS的MCPP的制定，并介绍了其简化为Min-Max Rooted Tree Cover（MMRTC）（Even等人，2004年；Tang和Ma，2023年）。然后，我们在4.2节介绍了两种优化技术，即等高线图增强和4.3节中的MMRTC解决方案完善，旨在进一步增强MCPP解决方案。

4.1 问题公式

我们提出了MCPP的问题表述，该表述有利于拓展CFS。MCPP的问题是寻找一组覆盖路径\Pi=\{\pi_i\}_{i\in I} ，其中I 是机器人的集合，使得其最小化时间度（即最大路径成本）。按照现有文献的做法（Zheng等人，2010年；Tang，Sun和Zhang，2021年），我们假设每个机器人都在给定位置开始和结束，这对应于CFS环境中的一对相邻的入口和出口点。形式上，MCPP的目标是最小化时间度\tau ，表示为：

当使用CFS生成\Pi 中的每条覆盖路径时，路径长度与|\pi| 成正比，因此任何路径\pi 的成本可以表示为c(\pi)=|\pi| ，因为CFS中的每条等高线包含等距点（详见第3.1节）。对于等高线图G=(V,E) ，每个v\in V 被赋予权重w_v = |I_v| ，表示等高线I_v 中的点的数量。因此，树T\subseteq G 的成本为c(T)=\sum_{v\in V(T)}w_v 。MMRTC问题与MCPP类似，其目标是找到一组根树，使得每个图顶点至少被一棵树覆盖，以最小化时间度。给定图G=(V,E) 和根等高线集R=\{r_i\}_{i\in I}\subseteq V ，MMRTC的目标定义如下：

图片

每个T_i \in \mathcal{T}都是以r_i为根的树，c(T_i)是它的树成本。记V(T)和E(T)分别表示任意树T的顶点集和边集。解集\mathcal{T}必须满足v\in \cup _{i\in I}V(T_i)以确保所有v\in V都被覆盖。由于CFS将每个v\in V(T_i)的等高线I_v拼接到构建覆盖路径\pi _i \in Pi，我们有c(\pi)=|\pi |=\sum_{v\in V(T_i)}|I_v|=c(T_i)。因此，对于任意等高线图G和机器人的根等高线集R，等式（4）和等式（5）中的启发式值在CFS下是相同的，有效地将MCPP简化为MMRTC。

我们采用(Tang and Ma 2023)提出的混合整数规划（MIP）模型来优化解决MMRTC问题。所得到的树的最优集合然后通过在每棵树上应用我们的统一CFS（算法1）来生成覆盖路径。图4-(a)和(b)说明了一个包含两棵树的MMRTC实例及其相应的解。模型的详细信息请参见附录B。

4.2 优化：等高线图增强

回顾一下，等高线图构建过程仅考虑相邻层中两条等高线之间的边。尽管这样做有效，但通常会导致等高线图中的图结构比较稀疏，从而得到一个不理想的MMRTC解，其中特定的等高线顶点会被多个树重复覆盖。一个常见的例子是所谓的切割等高线顶点，即从图中去除该顶点会增加连接组件的数量。随着树的数量（机器人）增加或根节点成群聚集，这种重复会变得更为常见，从而增加了时间度并降低了MCPP解的整体质量。为了缓解这一问题，我们提出通过在非相邻层中连接等高线顶点来增强稀疏的等高线图。这种增强旨在减少等高线图的稀疏性，并允许MMRTC树探索新的联合覆盖路径，从而减少重复，平衡树的成本。

增加一个等值图G=(V,E)可以通过添加一组增强边E^{\#}来完成，定义如下：

其中，d_G(·,·)表示等值图G中任意两个等值点之间的图距离，\delta是设置增强级别的超参数。对于E^\#中的边，缝合元组的构造方式与原始等值图边集E中的边不同。不失一般性，我们考虑一条边(v_1,v_{k+1} ) \in E^\#及其在原始G中的最短路径(v_1,v_2,...,v_{k+1})（即每个段(v_i,v_{i+1})是E的一部分，k是v_1和v_{k+1}之间的图距离）。集合O_{v_1,v_{k+1}}包括v_1等值线上的点p_1和v_{k+1}等值线上的点p_{k+1}，这些点可以合理连接，形成有效的缝合元组(p_1,p_{k+1})。这些点是可连接的，当且仅当它们形成连续缝合元组的序列(p_1,p_2)\in O_{v_1,v_2,...,(p_k,p_{k+1})} \in O_{v_k,v_{k+1}} ，确保这两点之间的直线段不会与工作空间内超过k-1个或任何障碍物相交。图3展示了增强边(v_1,v_3)添加过程的示例，其中包含三个有效的缝合元组O_{v_1,v_3}，并展示了如何通过p_2将p_1和p_3连接起来。给定邻接等值线之间距离设定为l，我们为E^\#中每个e=(u,v) \in E^\#的边赋予权重w_e=l\times k，其中k为层差（即|L_u-L_v|=k），该权重近似表示了任何包含e的额外路径成本。任何树T的成本被更新为c(T)=\sum _{v\in V(T)}w_v +\sum_{e\in E(T)}w_e ，并且在解决MMRTC问题时更新了。一旦构建了增强边集E^\# 和相应的缝合元组集O ，原始等值图G被更新为E=E \cup E^\#，并在增强后的G上解决相同的MMRTC模型。

4.3 优化：MMRTC解决方案改进

尽管等值图增强可以减少最佳MCPP解决方案中的等值点重复，但在实现更好的MCPP解决方案时仍然存在两个瓶颈。第一个瓶颈是由等值点重复造成的，仅通过增强无法解决，特别是当多个机器人共享相同的根等值点或多棵树使用相同的顶点时。第二个瓶颈源于最佳MCRTC解决方案在等值线的遍历成本变化显着时平衡树成本的限制。

为了解决上述两个瓶颈，我们提出了MMRTC解决方案改进过程（Alg. 2），利用两个函数PAIRWISEISOVERTICESSPLITTING(PIS)和ADDIMPROVINGREPETITION(AIR)：

PIS将具有重复的等值点的等值线覆盖分散在多个机器人之间，而AIR通过有选择地将高成本树中的等值点添加到低成本树中来引入改进重复。PIS和AIR在优化MMRTC解决方案中非常重要：PIS直接解决等值点重复的问题，而AIR战略地调整覆盖负载分布以平衡树之间的成本，增强整体MCPP解决方案。

算法解析，详见论文原文

4.4 案例研究：MMRTC解决方案的优化

我们给出一个具体的例子，以更好地说明前面提到的等值图增强（Aug）和解决方案改进（Ref）如何改善从原始MIP模型获得的MMRTC解决方案。如图5所示，我们使用图6中char-P的实例，其中四棵树都以相同的等值点为根。

第一行中的原始MMRTC解决方案展示了四个重复的等值点（填充颜色），导致树之间成本高度不平衡。使用了Aug（\delta设置为4）后，第二行中等值图G的稀疏性降低，从而提供了更多的路由选项从根开始，减少了解决方案中的等值点的重复，并使成本更加平衡。在第三行中，同时使用Aug和Ref，通过去重所有具有重复的等值点并动态调整树之间的成本，进一步改进了解决方案。

05 实验评估

本节将在一台配置了16GB RAM、3.49 GHz的Apple® M2 CPU笔记本电脑上展示我们的实验结果。

配置：MCFS的MMRTC MIP模型使用Gurobi求解器（Gurobi Optimization, LLC 2023）进行求解，设置运行时间限制为30分钟，并使用基于最小生成树（MST）的初始解法进行热启动（Tang and Ma 2023）。每当MCFS配备等值图增强时，超参数\delta设置为min\{|I|,4\}，其中|I| 是MCPP实例的机器人数量，以在MMRTC模型复杂度和解决方案质量之间进行平衡。

实例：由于现有的MCPP基准如（Tang and Ma 2023）专门针对二维网格地图上的基于网格的方法，我们使用了一组更多样化的工作空间来设计图6中显示的MCPP实例，从完全非矩形（2-环）到大部分矩形（办公室）不等。所有实例中相邻等值线之间的距离l为0.1，这也是机器人的覆盖直径。实例中的机器人数（|I|）从2到9不等。在char-I和char-P中，分别有两个和四个机器人共享相同的根等值点。在2-环中，三对机器人分别共享三个根等值点。在所有其他实例中，机器人从不同的根等值点出发。

图7进一步展示了四种MCFS变体在两个实例中随着机器人数量的增加而逐渐改善的性能。它表明随着机器人数量的增加，优化变得至关重要，因为每个机器人需要覆盖的等值线更少，提供了更稳健的MMRTC解决方案改进，从而降低了完成时间。

图8和图9分别通过TMC和TMSTC∗可视化了覆盖路径。这些路径呈现出来回的曲折模式，导致曲率很高，并且在复杂障碍物周围覆盖不完全。相比之下，MCFS在生成平滑路径方面表现显著出色，能够有效地围绕任意形状的障碍物，如图6所示，这是与其他方法明显的视觉优势。

06 结论

本文提出了MCFS框架，这是一种创新的方法，将计算机图形学和自动规划的原理相结合，以应对复杂MCPP任务中覆盖任意形状工作空间的挑战。

未来的工作包括提高等值线质量以进一步提高覆盖率，将动力学约束纳入等值线的生成和缝合过程中，以及开发启发式方法来加速PIS函数和解决大量机器人或等值线的MMRTC。