基于ros的多机器人自主探索方案（改进开源方案可自定义配置机器人）

基于ros的多机器人自主探索方案（改进开源方案可自定义配置机器人）

本文提出了一种基于动态Voronoi图与未知连通区域质心的新型多机器人自主探索方法。与现有方法相比，本文的关键新颖性体现在两点：

1）++动态Voronoi分区 + 参数不敏感效用函数++ ：利用质心信息评估探索增益，引导机器人探索未知区域；

2）++全局/局部分层窗口的质心检测机制++：当局部窗口未检测到质心时触发全局窗口，以补足目标并提升覆盖。

Tips

在目标搜索与灾难救援等应用中，机器人往往面临环境信息几乎完全未知的情形，因此需要快速完成自主探索与建图。CVRD的分层框架核心是：++让每个机器人只探索其负责的Voronoi分区，并在GEW/LEW窗口触发下把未知连通区域质心转化为导航目标++，从而显著降低探索时间与路径成本。

导读

!TIP

1. 用动态Voronoi把探索任务分配给不同机器人分区，降低重复探索。
2. 用"未知连通区域质心"替代前沿，作为探索目标。
3. 通过GEW/LEW分层窗口提高质心发现率，补足覆盖空缺。
4. 设计参数不敏感效用函数，直接评估质心探索增益。
5. 用目标重新选择机制抑制机器人在目标间来回切换。
6. 质心首先用于规划目标生成，再由导航模块执行。
7. 探索效率与路径成本一并优化。
8. 适配未知环境下的自主建图与定位。
9. 在多场景仿真与真实任务验证可行性。
10. 与MR-RBE相比具有显著时间与成本优势。

阅读导航

• ++方法主线++：动态Voronoi分区 -> GEW/LEW分层质心检测 -> 效用评估与目标重选。
• ++核心优势++：用质心替代前沿，减少质心检测开销并提升覆盖。
• ++实验结论++：探索时间消耗与路径成本最高分别降低50.7%与34.4%。

一、引言

!TIP

导读：先读清楚"为什么前沿/采样协作难以兼顾效率与可行性"，再理解本文为何用"未知连通区域质心"驱动探索决策。

自主机器人探索旨在找到一条可行路径，以高效地探索未知环境并同时创建其地图。在过去几十年中，自主机器人探索因其在许多场景中的潜在应用而受到广泛关注，如检查、目标搜索、灾难救援和资源勘探。特别是，机器人在受限空间、地下环境等中执行任务时，比人类具有更少的安全问题。与单机器人探索相比，多机器人探索在以下方面具有更多优势：

• 1）一组协同的机器人在探索未知环境时比单个机器人更快；
• 2）多机器人团队可以共享传感器信息，这有助于机器人在包含所有机器人而非单个机器人的传感器信息的地图上导航；
• 3）使用多个机器人而非单个机器人更具容错性和稳定性。

然而，设计一种既能确保探索效率又能保证可行性的多机器人探索策略仍然具有挑战性。多机器人探索的核心问题是如何实施协作策略，并合理确定由哪个机器人探索相应的未知区域，以最小化总体时间。现有方法之一是在评估前沿时引入协调机制。在文献中，每个机器人试图导航到最近的可到达、未访问的前沿。文献中的工作决定，当到已分配给其他机器人的其他前沿的距离小于传感器感知范围时，降低该前沿的效用值。另一种现有的协作探索方法基于市场模型，其中机器人团队通过拍卖和投标系统协商如何探索候选目标。其他流行的方法是通过将探索空间划分为一组子空间来协调机器人团队。探索空间的划分方法包括K均值聚类算法和环境分割方法。上述方法都是基于前沿来实现的。文献中的工作表明，随着地图变大，上述方法的前沿检测时间会增加，因此前沿检测的效率会降低。结果，探索效率也会相应降低，因为当环境边界变化时，检测不能及时更新。综

上所述，现有多机器人探索方法的主要问题包括：

• 1）前沿检测效率随着地图大小的增加而降低；
• 2）机器人在两个目标之间来回移动；
• 3）不同的机器人同时探索相同的未知区域。

受现有方法上述局限性的启发，我们提出了一种新颖的方法CVRD，它利用未知连通区域的质心来加速多机器人自主探索。所提出的方法在模拟和实际实验中进行了测试。我们在六种不同类型的模拟场景中与最先进的方法进行了比较。比较结果表明，本文提出的方法可以在更短的时间内以更低的路径成本完成探索任务。所提出方法的效率可以提高多达 50.7 % {50.7}\% 50.7%（定义为总体探索时间）。并且路径成本可以降低多达34.4%。

综上所述，本工作的主要贡献和创新点如下：

++贡献1：质心驱动的分层探索框架++。提出一种基于未知连通区域质心的新型多机器人探索方法CVRD，采用自适应全局/局部探索窗口的分层机制实现质心快速检测与定位，并更准确地表征未知连通区域面积。

++贡献2：参数不敏感效用评估与目标重选++。提出一种无需调参的效用函数，并结合基于转向成本的目标重新选择机制，精确评估质心，同时抑制机器人在两个目标之间来回移动。

++贡献3：在多场景验证探索效率优势++。在多种具有挑战性的环境中实现多机器人自主探索，通过大量模拟对比与实验验证了其在时间效率与路径成本方面的优越性能。

二、相关工作

!TIP

导读：按"前沿式/随机采样式/协作式"三类方法对照阅读，重点找出本文要解决的效率瓶颈与目标分配缺口。

目前，有许多关于单个机器人或机器人团队自主探索的研究。这些方法已应用于各种平台，如轮式机器人、四足机器人、空中机器人和水下航行器19。这些现有方法用于处理不同类型的环境，包括室内环境、野生森林和地下空间。因此，开发高效可行的探索方法具有实际应用价值。

当前，一类流行的探索方法是基于前沿的方法。基于前沿的方法最早由山内提出，它将已知自由空间和未知空间之间的边界称为前沿边缘，位于边缘的网格聚类中心称为前沿，用于引导机器人进行探索。山内将这种方法扩展到多机器人探索，其中机器人共享地图信息并探索其最近的、可到达的前沿。基于前沿的方法被许多研究改进。文献中的工作将基于前沿的方法应用于探索三维（3D）环境。西蒙斯等人使用信息增益来评估前沿，合作探索策略是使团队的信息增益最大化。为了加快前沿的检测，凯达尔和卡明卡提出了两种广泛采用的检测器：波前前沿检测器（WFD）和快速前沿检测器（FFD）。与现有的基于前沿的方法不同，我们提出了一个自适应全局和局部探索窗口框架，以检测未知连通区域的质心而不是前沿，来引导机器人团队进行探索。

另一类流行的方法是基于随机采样的探索方法。一些基于随机采样的方法使用快速探索随机图（RRG）或快速探索随机树（RRT）来找到未探索区域。乌马里和穆克霍帕德希亚使用RRT来搜索前沿，其中RRT不断扩展，直到其节点位于未知区域才被视为前沿。王等人提出了一种新的基于图的探索方法，在探索过程中根据探索空间中的随机点增量构建路线图，直到从路线图中获得一条到探索目标的可行全局路径。在中，路线图被应用于探索3D环境。中的工作提出了一种用于地下环境的基于图的方法，其中探索规划器由两部分组成，全局规划器和局部规划器。在这项工作中，局部规划器使用RRG来计算无碰撞路径，然后机器人选择并执行信息增益最大化的路径。此外，全局规划器负责引导机器人回到设定点或未访问的分支。曹等人提出了一个用于探索3D环境的分层框架。在这项工作中，找到一组视点来覆盖已知空间和非自由空间之间的边界，然后使用分层框架在局部规划范围内规划详细路径，在全局尺度内规划粗略路径，机器人遵循该路径时覆盖未知空间。中提出了一种基于最佳下一个视图规划器（NBVP）的方法，其中生成一个RRT并在树中找到最具信息性的分支，然后机器人将跟随最佳分支的第一条边。与基于前沿的方法相比，基于随机采样的方法更容易扩展到3D环境。然而，这些方法存在轨迹重叠、被困在狭窄空间以及容易错过小区域的问题。此外，由于没有完善的机制来评估候选目标或分配探索目标，这些方法可能存在机器人在两个探索目标之间来回切换的问题。针对上述问题，我们精心设计了一个新的效用函数来评估候选目标和一个目标重新选择机制来分配探索目标。

使用多个机器人可以提高探索效率。多机器人自主探索的关键问题是如何通过协作策略为单个机器人分配目标。一种主流方法是让机器人独立负责分配的区域。索拉纳斯和加西亚使用K均值算法将环境划分为与机器人数量相同的不相交区域。武尔姆等人将空间划分为多个部分，并将相应的机器人派往各个部分。一些基于行为的技术也用于协作策略。例如，中使用了三种行为，包括前往边界、避开障碍物和其他机器人。在中，基于行为的方法与一种高效的数据结构相结合，用于存储已探索区域，并让机器人有效地探索环境。然而，基于行为的方法仅在一些简单环境中表现良好。它们在探索过程中的某些位置存在局部极小值问题。中的工作指出，一些多机器人探索方法存在的一个难题是不同的机器人可能会覆盖相同的区域。在本文中，提出了一种新的基于Voronoi的协作策略来解决不同机器人探索相同区域的问题。

III. 问题定义

!TIP

导读 ：这一节主要是符号与任务形式化；建议重点抓住"未知连通区域质心集合 C \mathcal{C} C"与"Voronoi分配/GEW/LEW窗口"的关系。

多机器人自主探索问题可以描述为一个多机器人团队 R \mathcal{R} R在没有任何先验信息的情况下共同探索一个空间，同时自主构建二维未知空间 H = { H e x , H u n } \mathcal{H} = \left\{ {{\mathcal{H}}{\mathrm{{ex}}},{\mathcal{H}}{\mathrm{{un}}}}\right\} H={Hex,Hun}的二维占用网格地图 M = { M f r \mathcal{M} = \left\{ {\mathcal{M}}{\mathrm{{fr}}}\right. M={Mfr, M o c , M u n } \left. {{\mathcal{M}}{\mathrm{{oc}}},{\mathcal{M}}{\mathrm{{un}}}}\right\} Moc,Mun}。未探索空间 H un {\mathcal{H}}{\text{ un }} H un 可以被映射 M un {\mathcal{M}}{\text{ un }} M un ，其中 M un {\mathcal{M}}{\text{ un }} M un 表示机器人车载传感器尚未扫描的网格集合。已探索空间 H e x {\mathcal{H}}{\mathrm{{ex}}} Hex可以被映射 M f r {\mathcal{M}}{\mathrm{{fr}}} Mfr或 M o c {\mathcal{M}}{\mathrm{{oc}}} Moc，其中 M f r {\mathcal{M}}{\mathrm{{fr}}} Mfr和 M o c {\mathcal{M}}_{\mathrm{{oc}}} Moc分别表示自由网格和占用网格。

我们将多机器人团队中的第 k k k个机器人定义为 R k . P k = p k , q k {\mathcal{R}}{k}.{\mathbf{P}}{k} = \left\lbrack {{\mathbf{p}}{k},{\mathbf{q}}{k}}\right\rbrack Rk.Pk=pk,qk，其定义为 R k {\mathcal{R}}{k} Rk的位姿，其中 p k ∈ R 2 {\mathbf{p}}{k} \in {\mathbb{R}}^{2} pk∈R2和 q k ∈ S O ( 2 ) {\mathbf{q}}{k} \in \mathbf{{SO}}\left( \mathbf{2}\right) qk∈SO(2)分别表示 R k {\mathcal{R}}{k} Rk的位置和方向。为了实现多机器人探索的协作，我们首先将空间 H \mathcal{H} H划分为 n n n个Voronoi图分区 V = { v k ∣ k = 1 , 2 , 3 , ... , n } \mathcal{V} = \left\{ {{\mathbf{v}}{k} \mid k = 1,2,3,\ldots ,n}\right\} V={vk∣k=1,2,3,...,n}，如图1所示，其中 n ∈ Z + n \in {\mathbb{Z}}^{ + } n∈Z+是机器人数量， v k {\mathbf{v}}{k} vk是第 k k k个Voronoi图分区。设 G \mathcal{G} G和 L k {\mathcal{L}}{k} Lk分别为多机器人团队的全局探索窗口（GEW）和第 k k k个机器人 R k {\mathcal{R}}{k} Rk的局部探索窗口（LEW）。在此之后，将从 M un {\mathcal{M}}{\text{ un }} M un 中获得未知连通区域 S ⊂ H un \mathcal{S} \subset {\mathcal{H}}{\text{ un }} S⊂H un 。设 s i ∈ S {\mathbf{s}}{i} \in \mathcal{S} si∈S表示第 i i i个未知连通区域， C = { c i ∣ i = 1 , 2 , 3 , ... , N } \mathcal{C} = \left\{ {{\mathbf{c}}{i} \mid i = 1,2,3,\ldots ,N}\right\} C={ci∣i=1,2,3,...,N}表示所有未知连通区域的质心集合，其中 c i {\mathbf{c}}{i} ci是 s i {\mathbf{s}}{i} si的质心。图1展示了这些定义和符号的说明。

问题1：给定未知地图 M un {\mathcal{M}}{\text{ un }} M un 和机器人的位置 p 1 ,    p 2 ,    ... ,    p n {\mathbf{p}}{1},\;{\mathbf{p}}{2},\;\ldots ,\;{\mathbf{p}}{n} p1,p2,...,pn，该问题的目标是迭代地找到 n n n个探索目标 c 1 ∗ , c 2 ∗ , ... , c n ∗ {\mathbf{c}}{1}^{ * },{\mathbf{c}}{2}^{ * },\ldots ,{\mathbf{c}}{n}^{ * } c1∗,c2∗,...,cn∗，这些目标引导 n n n个机器人覆盖 M un {\mathcal{M}}{\text{ un }} M un ，直到 M un = ∅ {\mathcal{M}}_{\text{ un }} = \varnothing M un =∅，即探索任务完成。

四、方法概述

!TIP

导读：按三步流程理解模块衔接：动态Voronoi协作分区 -> GEW/LEW分层质心检测 -> 效用评估与目标重选。

高效协同探索方法的流程和模块如图2所示。所提方法的架构是集中式的。所提探索方法的输入是机器人的位置和合并后的地图，其输出是从候选质心选择的机器人导航目标。所提方法的步骤如下。

首先，为避免多个机器人同时探索同一未知区域，设计了一种基于Voronoi的协同策略，以使机器人相对分散。该方法使用Voronoi图动态划分机器人周围的空间，然后相应地为每个机器人分配一个分区进行探索。

其次，提出了一种包括GEW和LEW的分层框架来检测未知连通区域的质心。质心用于替代边界，以快速、精确地定位未知区域，并引导机器人完成探索任务。

第三，为评估检测到的质心，提出了一种新的参数不敏感效用函数如下：

f ( c i , r i , p k ) = s ( c i , r i ) d ( c i , p k ) , (1) f\left( {{\mathbf{c}}{i},{r}{i},{\mathbf{p}}{k}}\right) = \frac{s\left( {{\mathbf{c}}{i},{r}{i}}\right) }{d\left( {{\mathbf{c}}{i},{\mathbf{p}}_{k}}\right) }, \tag{1} f(ci,ri,pk)=d(ci,pk)s(ci,ri),(1)

s ( c i , r i ) = ∑ ( U c ) ⋅ m r 2 , (2) s\left( {{\mathbf{c}}{i},{r}{i}}\right) = \sum \left( {U}{\mathrm{c}}\right) \cdot {m}{\mathrm{r}}^{2}, \tag{2} s(ci,ri)=∑(Uc)⋅mr2,(2)

d ( c i , p k ) = ∥ c i − p k ∥ , (3) d\left( {{\mathbf{c}}{i},{\mathbf{p}}{k}}\right) = \begin{Vmatrix}{{\mathbf{c}}{i} - {\mathbf{p}}{k}}\end{Vmatrix}, \tag{3} d(ci,pk)= ci−pk ,(3)

其中， s ( c i , r i ) s\left( {{\mathbf{c}}{i},{r}{i}}\right) s(ci,ri)表示围绕候选质心 c i {\mathbf{c}}{i} ci在一定半径 r i {r}{i} ri内的未知连通区域的面积，即未知单元格 U c {U}{\mathrm{c}} Uc的数量乘以地图分辨率 m r {m}{\mathrm{r}} mr的平方。需要注意的是， r i {r}{i} ri不是人为给定的常数，它会根据候选质心周围未知连通区域的大小在线自动调整。 r i {r}{i} ri的确定过程将在第五节D部分详细介绍。 d ( c i , p k ) d\left( {{\mathbf{c}}{i},{\mathbf{p}}{k}}\right) d(ci,pk)表示探索成本，即候选质心 c i {\mathbf{c}}{i} ci的位置与相应机器人 p k {\mathbf{p}}{k} pk当前位置之间的欧几里得距离。与文献18中提出的现有效用函数不同，我们的效用函数对参数不敏感，并且不需要凭经验调整权重，因为它采用了单位成本增益的概念，可视为机器人从单位距离质心获得的探索增益。

最后，有了效用函数后，设计了一种目标重新选择机制，以防止机器人在两个目标之间来回移动，并更新下一个探索目标，即每个机器人的导航目标，并且机器人可以使用现有的运动规划算法32、33、34、35、36安全地导航到目标。机器人在导航过程中使用现有的同步定位与地图构建（SLAM）算法37构建未知空间的地图。

V. 方法

!TIP

导读：本节是算法主体，建议依次读 A-E：分区（避免重复）-> 窗口（避免遗漏）-> 质心提取（获得目标）-> 效用评估与参数不敏感性（选择目标）-> 目标重选（抑制来回切换）。

A. 动态Voronoi图划分

!TIP

导读：这里的关键是动态Voronoi如何把机器人"分配到空间"，从机制上减少多机器人同时探索同一区域。

为实现多机器人协同探索，将空间划分为Voronoi图分区，其中Voronoi图单元格的数量设为机器人的数量。我们将每个机器人的位置作为每个Voronoi图单元格的基点。设基点与二维空间中每个点之间的欧几里得距离为Voronoi图划分的原则，即当前Voronoi图单元格中一个点与基点之间的距离比该点与其他基点之间的距离更近。第 k k k个Voronoi图分区 v k {\mathbf{v}}_{k} vk定义为

v k = { p ∈ H ∣ D ( p , b k ) ≤ D ( p , b j ) , j ≠ k , k , j ∈ Z + } , (4) {\mathbf{v}}{k} = \left\{ {\mathbf{p} \in \mathcal{H} \mid D\left( {\mathbf{p},{\mathbf{b}}{k}}\right) \leq D\left( {\mathbf{p},{\mathbf{b}}_{j}}\right) ,j \neq k,k,j \in {\mathbb{Z}}^{ + }}\right\} , \tag{4} vk={p∈H∣D(p,bk)≤D(p,bj),j=k,k,j∈Z+},(4)

其中， H \mathcal{H} H表示二维空间， p \mathbf{p} p是第 k k k个Voronoi图分区中的一个点。 D ( ⋅ ) D\left( \cdot \right) D(⋅)表示计算欧几里得距离的函数为

D ( p , b k ) = ∥ p − b k ∥ , (5) D\left( {\mathbf{p},{\mathbf{b}}{k}}\right) = \begin{Vmatrix}{\mathbf{p} - {\mathbf{b}}{k}}\end{Vmatrix}, \tag{5} D(p,bk)= p−bk ,(5)

其中， b k {\mathbf{b}}{k} bk是 v k {\mathbf{v}}{k} vk的基点。

需要注意的是，由于机器人的位置一直在变化，分区过程是动态且实时的。每个机器人被分配一个唯一的分区。通过使用动态Voronoi图，我们可以确保机器人与其负责分区内某一点之间的距离小于该点与其他机器人之间的距离。所提出的方法可以防止多个机器人同时探索同一区域。当机器人协作探索时，重复探索的区域面积会大幅减少。

B.LEW和GEW

!TIP

导读：LEW负责局部引导，GEW在局部缺失质心时补全目标；读懂触发逻辑即可理解"为何不会漏掉远处区域"。

机器人没有关于未知环境的任何信息，包括其大小。通过SLAM生成二维占用网格地图。如果该方法直接在整个地图上提取未知连接区域并检测它们的质心，一些远离机器人甚至在正在探索的建筑物外部的质心可能会被选中。当将这些质心选作探索目标时，机器人很容易在遥远的未知区域和附近的未知区域之间来回探索，或者忽略它们周围的未知区域。为了解决这个问题，所提出的方法根据每个机器人的位置选择LEW和GEW。目前，机器人团队只需要考虑在选定的窗口上探索未知区域，即所提出的方法只需要在选定的窗口上提取未知连接区域并检测它们的质心，而不是在整个地图上。选择LEW和GEW的详细信息如下。

1. LEW：我们为每个机器人选择一个LEW。为了确保所选窗口包含一些未知区域，窗口大小会动态调整。首先，将机器人的位置作为窗口中心，窗口大小逐渐增加，直到一条边处于未知环境中或一条边位于整个地图的边界。选择LEW的完整过程总结在算法1中。 o k {o}{k} ok是在计算窗口四个顶点坐标时相对于第 k k k个机器人 R k {\mathcal{R}}{k} Rk的位置 p k {\mathbf{p}}{k} pk的偏移量。函数GetWindowSides ( p k , o k ) \left( {{\mathbf{p}}{k},{o}{k}}\right) (pk,ok)根据四个顶点获取窗口的四条边 W s {\mathbf{W}}{\mathrm{s}} Ws（第3 - 4行）。函数CheckSidesLocation ( W s i ) \left( {{\mathbf{W}}{\mathrm{s}}\left\lbrack i\right\rbrack }\right) (Wsi)检查是否有任何边位于未知区域或整个地图的边界上，如果是则返回true（第6行）。此时，由 W s {\mathbf{W}}{\mathrm{s}} Ws组成的窗口用于表示 L k {\mathcal{L}}_{k} Lk，这是算法1的输出（第13 - 14行）。

LEW允许机器人优先探索其周围的未知区域。它避免了机器人先探索远离其初始位置的未知区域，然后再返回探索其初始位置附近未知区域的情况。并且从LEW中检测质心比从整个地图中检测更快。

2. GEW：在探索过程中，LEW主要用于引导机器人进行探索。LEW的大小会动态调整以找到未知区域。当仅应用LEW时，图3中会出现一个问题。红色机器人错误地认为探索任务已完成，因为红色LEW位于地图边界且不包含任何未知区域。其他机器人探索LEW中包含的未知区域。最终，地图中心和顶部的未知区域可能会被遗漏而未被探索。因此，应用GEW（图3中的黄色矩形）来解决这个问题。当任何一个LEW中没有未知连接区域时，所提出的方法从GEW中检测未知连接区域，然后将这些区域分配给相应的机器人进行探索。

GEW是一个包含所有机器人的矩形框，如图3中的黄色矩形所示。GEW根据每个机器人的实时位置进行选择，因此GEW是动态调整的。具体来说，我们首先计算一个包含所有机器人的最小包围矩形框。接下来，将矩形框按一定偏移量放大。如果放大后的矩形框的一条边超过地图边界，则将该边设置为地图边界的位置。最终，这个矩形框被视为GEW。

C. 检测质心

!TIP

导读：理解质心计算链路：轮廓提取（OpenCV）-> 零/一阶矩（像素坐标）-> 物理坐标映射（分辨率与原点）。

如图1所示，在得到Voronoi图分区并选择探索窗口后，有许多未知的连通区域。所提出的方法通过使用质心引导机器人执行探索任务。为了获得质心，我们需要将LEW和GEW转换为可由OpenCV处理的图像。接下来，从图像中提取未知连通区域的轮廓，然后我们可以通过以下方式得到轮廓的空间矩：零阶矩 m 00 {m}{00} m00 和一阶矩 m 01 , m 10 {m}{01},{m}_{10} m01,m10。

m j i = ∑ w , h ( array ⁡ ( w , h ) ⋅ w j ⋅ h i ) ( j , i = 0 , 1 ) , (6) {m}{ji} = \mathop{\sum }\limits{{w,h}}\left( {\operatorname{array}\left( {w,h}\right) \cdot {w}^{j} \cdot {h}^{i}}\right) \left( {j,i = 0,1}\right) , \tag{6} mji=w,h∑(array(w,h)⋅wj⋅hi)(j,i=0,1),(6)

其中 ( w , h ) \left( {w,h}\right) (w,h) 表示轮廓中一个像素的位置。轮廓质心的位置表示为 ( w ˉ , h ˉ ) \left( {\bar{w},\bar{h}}\right) (wˉ,hˉ)。然后 ( w ˉ , h ˉ ) \left( {\bar{w},\bar{h}}\right) (wˉ,hˉ) 可以通过以下方式计算：

w ˉ = m 10 m 00 , h ˉ = m 01 m 00 , (7) \bar{w} = \frac{{m}{10}}{{m}{00}},\bar{h} = \frac{{m}{01}}{{m}{00}}, \tag{7} wˉ=m00m10,hˉ=m00m01,(7)

其中 ( w ˉ , h ˉ ) \left( {\bar{w},\bar{h}}\right) (wˉ,hˉ) 是图像参考系中的坐标表示。设 ( x ˉ , y ˉ ) \left( {\bar{x},\bar{y}}\right) (xˉ,yˉ) 表示全局地图参考系中质心的位置。设 m r {m}{\mathrm{r}} mr 和 ( X o \left( {X}{\mathrm{o}}\right. (Xo， Y o {Y}_{\mathrm{o}} Yo ) 分别表示全局地图的分辨率和原点，那么我们可以通过以下方式计算 ( x ˉ , y ˉ ) \left( {\bar{x},\bar{y}}\right) (xˉ,yˉ)：

x ˉ = w ˉ ⋅ m r + X o , y ˉ = h ˉ ⋅ m r + Y o . (8) \bar{x} = \bar{w} \cdot {m}{\mathrm{r}} + {X}{\mathrm{o}},\bar{y} = \bar{h} \cdot {m}{\mathrm{r}} + {Y}{\mathrm{o}}. \tag{8} xˉ=wˉ⋅mr+Xo,yˉ=hˉ⋅mr+Yo.(8)

图4展示了一个示例，用于解释如何使用OpenCV从LEW中提取未知连通区域的轮廓并检测其质心。如图4所示，首先将LEW转换为图像。图像的绿色区域代表地图的未知区域。然后，使用OpenCV提取绿色连通区域的轮廓。最后，通过应用公式(6)-(8)计算全局地图上质心的坐标。

需要注意的是，候选质心可能位于封闭障碍物内，即机器人无法到达该质心。起初，我们不删除这个质心，因为它仍可引导机器人探索未知环境。之后，当机器人无法规划通往该质心的路径时，我们将其标记并删除。

D. 使用参数不敏感效用函数进行探索

!TIP

导读：把效用函数看成"探索增益/移动成本"的比值；核心收益是用单位成本增益实现对参数选择的鲁棒性。

在第五节C中获得的质心称为候选质心。使用参数不敏感效用函数（公式(1)）来评估候选质心。文献18中的效用函数参数需要根据环境进行调整。然而，本文提出的效用函数具有通用性，无需在不同环境中调整参数。使所提出的效用函数最大化的质心 c ∗ {\mathbf{c}}^{ * } c∗被选作探索目标

c ∗ = arg ⁡ max ⁡ c ∗ ∈ C f ( c i , r i , p k ) = s ( c i , r i ) d ( c i , p k ) , (9) {\mathbf{c}}^{ * } = \underset{{\mathbf{c}}^{ * } \in \mathcal{C}}{\arg \max }f\left( {{\mathbf{c}}{i},{r}{i},{\mathbf{p}}{k}}\right) = \frac{s\left( {{\mathbf{c}}{i},{r}{i}}\right) }{d\left( {{\mathbf{c}}{i},{\mathbf{p}}_{k}}\right) }, \tag{9} c∗=c∗∈Cargmaxf(ci,ri,pk)=d(ci,pk)s(ci,ri),(9)

其中 s ( c i , r i ) s\left( {{\mathbf{c}}{i},{r}{i}}\right) s(ci,ri)和 d ( c i , p k ) d\left( {{\mathbf{c}}{i},{\mathbf{p}}{k}}\right) d(ci,pk)在第四节中介绍。

计算效用函数的具体步骤是，首先判断候选质心位于哪个Voronoi图分区中。其次，计算候选质心与负责该分区的机器人之间的效用函数。关键问题是在计算 s ( c i , r i ) s\left( {{\mathbf{c}}{i},{r}{i}}\right) s(ci,ri)时如何找到自适应调整参数 r i {r}{i} ri。寻找 r i {r}{i} ri的整个过程在算法2中介绍。函数FindKnownCell ( M , c i , r i ) \left( {\mathcal{M},{\mathbf{c}}{i},{r}{i}}\right) (M,ci,ri)（第3行）确定以 c i {\mathbf{c}}{i} ci为中心、 r i {r}{i} ri为半径的圆形区域内是否包含已知网格单元。如果圆形区域有任何已知网格单元，函数返回true， r i {r}{i} ri不再增加（第4 - 5行）。否则， r i {r}{i} ri以一定步长 r step {r}_{\text{ step }} r step 增加（第7行）。

如图5(a)所示，灰色区域是未知连通区域，黑点表示其质心。首先，可以以质心为圆心获得一个小半径的圆。其次，如果该圆仅包含未知网格（图5(a)中的红色虚线圆），则该圆以一定步长 r step ( r step > 0 ) {r}{\text{ step }}\left( {{r}{\text{ step }} > 0}\right) r step (r step >0)扩展。最后，当圆包含任何已知网格时不再扩展。并且该圆的半径（图5(a)中的红色实线圆）用于计算探索增益。

本文提出的算法2比现有方法18更准确地估计未知区域的面积。原因是算法2根据未知区域的大小自适应地调整用于估计未知区域面积的圆的半径。然而，现有方法采用固定半径的圆（图5(b)中的蓝色实心圆）来估计前沿周围未知区域的面积。

E.A目标重新选择机制

!TIP

导读 ：该机制的目的不是"换更好目标"本身，而是抑制在两个高效目标之间的来回切换，保证探索轨迹稳定。

利用效用函数式(9)，我们可以为每个机器人选择最佳探索目标。之后，机器人开始通过导航到目标来探索环境，同时在导航过程中以恒定频率更新地图和探索目标。现有的基于效用函数的方法通常会导致机器人在多个目标之间来回移动。如图6所示，初始探索目标是当前目标 T c {T}{\mathrm{c}} Tc。当机器人经过位置2且系统开始更新探索目标时，新目标 T n {T}{\mathrm{n}} Tn的效用函数值更大，因此探索目标更新为 T n {T}{\mathrm{n}} Tn。然而，当机器人经过位置1且系统开始更新探索目标时， T c {T}{\mathrm{c}} Tc的效用函数值又更大，所以探索目标更新为 T c {T}_{\mathrm{c}} Tc。因此，机器人在两个目标之间来回移动，这导致探索效率低下。为了解决上述问题，我们提出了一种新的目标重新选择机制以加速探索。接下来，描述该机制的核心思想。

当 T n {T}{\mathrm{n}} Tn的效用函数值更大时，探索目标不会立即更新为新目标 T n {T}{\mathrm{n}} Tn。只有当 A n {A}{\mathrm{n}} An小于 A c {A}{\mathrm{c}} Ac时，探索目标才会更新为 T n {T}_{\mathrm{n}} Tn，即

c ∗ = { T n ,    A n < A c T c , otherwise (10) {\mathbf{c}}^{ * } = \left\{ \begin{array}{l} {T}{\mathrm{n}},\;{A}{\mathrm{n}} < {A}{\mathrm{c}} \\ {T}{\mathrm{c}},\text{ otherwise } \end{array}\right. \tag{10} c∗={Tn,An<AcTc, otherwise (10)

其中 A c {A}{\mathrm{c}} Ac是向量 z c {\mathbf{z}}{\mathrm{c}} zc与向量 z r {\mathbf{z}}{\mathrm{r}} zr之间的夹角。 A n {A}{\mathrm{n}} An是向量 z n {\mathbf{z}}{\mathrm{n}} zn与 z r {\mathbf{z}}{\mathrm{r}} zr之间的夹角。向量 z r , z c {\mathbf{z}}{\mathrm{r}},{\mathbf{z}}{\mathrm{c}} zr,zc和 z n {\mathbf{z}}_{\mathrm{n}} zn在图6中有详细介绍。通过目标重新选择机制，解决了机器人总是在两个目标之间来回移动的问题。此外，所提出的机制在防止机器人急剧改变探索方向方面起着至关重要的作用。因此，减少了额外的能量并提高了探索效率。

目标重新选择机制的作用在图8(b)所示的迷宫环境中进行了测试。当所提出的方法不应用该机制时，机器人会出现往返于两个目标之间的问题，如图7所示。在开始时 ( t 1 = 0.0    s ) \left( {{t}{1} = {0.0}\mathrm{\;s}}\right) (t1=0.0s)，如图7(a)所示，机器人的探索目标设置为 T c {T}{\mathrm{c}} Tc，机器人导航到 T c ( t 2 = 5.0    s ) {T}{\mathrm{c}}\left( {{t}{2} = {5.0}\mathrm{\;s}}\right) Tc(t2=5.0s)。然后，目标更新为 T n ( t 3 = 7.0    s ) {T}{\mathrm{n}}\left( {{t}{3} = {7.0}\mathrm{\;s}}\right) Tn(t3=7.0s)，机器人开始导航到 T n {T}{\mathrm{n}} Tn。接下来，目标再次更新为 T c ( t 4 = 11.0    s ) {T}{\mathrm{c}}\left( {{t}{4} = {11.0}\mathrm{\;s}}\right) Tc(t4=11.0s)，机器人又开始前往 T c {T}{\mathrm{c}} Tc。最后，机器人总是在 T c {T}{\mathrm{c}} Tc和 T n {T}{\mathrm{n}} Tn之间来回移动。然而，当所提出的方法采用该机制时，机器人能够顺利、高效地探索环境。对比测试结果表明，所提出的机制可以解决机器人在两个目标之间来回移动的问题。

VI. 仿真与实验

!TIP

导读：先看ROS实现与对比基线，再看指标如何定义"效率（覆盖/时间/成本）"与"是否重复探索"。

A. 评估指标

!TIP

导读：建议逐条对应公式语义：覆盖率衡量完成度，探索时间衡量效率，路径成本衡量代价，行进路线用于观察重复探索现象。

1. 探索覆盖率：探索覆盖率是指机器人已探索区域与目标环境总面积的比率。

2. 探索时间：探索时间是指当探索覆盖率达到预设阈值时，多机器人团队探索环境的总时间。

3. 路径成本：我们记录每个机器人所行驶路径的长度，并将所有机器人路径长度之和作为机器人团队的路径成本。

4. 行进路线：我们记录每个机器人在探索过程中的行进路线。行进路线使我们能够直接观察不同机器人是否会重复探索同一区域。

B. 仿真

!TIP

导读：重点关注平台/传感器设置与场景集合如何覆盖"通用性"；

20260614-192803

VII. 结论

!TIP

导读：用一句话复盘贡献：质心驱动的分层窗口框架 + 参数不敏感效用评估 + 目标重选机制，并对应实验验证结论。

在本文中，我们为多机器人系统探索未知环境提出了一种新颖的自主探索方法。开发了一个包括LEW和GEW的分层框架，以快速检测用于精确定位和表征未知区域的质心。然后，精心设计了一个新的效用函数和目标重新选择机制，以评估候选质心并防止机器人在两个目标之间来回移动。我们在模拟和实际实验中测试了所提方法。所有测试结果证明，所提方法能够以更少的探索时间和路径成本完成探索任务。与基准方法相比，所提方法具有显著更高的探索效率。++结论：CVRD通过质心驱动的分层窗口与目标重选机制实现更高效的未知环境覆盖。++