Meeting-Merging-Mission: A Multi-robot Coordinate Framework for Large-Scale Communication-Limited Exploration

Yuman Gao*, Yingjian Wang*, Xingguang Zhong, Tiankai Yang, Mingyang Wang, Zhixiong Xu,
Yongchao Wang, Yi Lin1, Chao Xu, and Fei Gao
This work was supported in part by the DJI-ZJU FAST Autonomous Drone Research Funding, in part by the National Natrual Science Foundation of China under Grant 62088101. Y. Lin1 is with Dji Co, Shenzhen, China. E-mail: ylinax@connect.ust.hk. All the other authors are with the State Key Laboratory of Industrial Control Technology, Zhejiang University, Hangzhou, China, and also with the Huzhou Institute of Zhejiang University, HuZhou, China. Corresponding author: Fei Gao. E-mails: {ymgao, yj_wang, cxu, fgaoaa}@zju.edu.cn. * Equal contributors.
Abstract

This letter presents a complete framework Meeting-Merging-Mission for multi-robot exploration under communication restriction. Considering communication is limited in both bandwidth and range in the real world, we propose a lightweight environment presentation method and an efficient cooperative exploration strategy. For lower bandwidth, each robot uses specific polytopes to maintain free space and to generate Super Frontier Information (SFI), which serves as the source for exploration decision-making. To reduce repeated exploration, we develop a mission-based protocol that drives robots to share collected information in stable rendezvous. We also design a complete path planning scheme for both centralized and decentralized cases. To validate that our framework is practical and generic, we present an extensive benchmark and deploy our system into multi-UGV and multi-UAV platforms.

I Introduction

Recently, thanks to the maturity of multi-robot cooperative technology, swarm exploration has received increasing attention in many application areas. Multiple robots can explore wider regions in the time unachievable by a single one, with better fault tolerance and uncertainty compensation. However, in actual exploration missions, communication limitation introduces great challenges to multi-robot exploration tasks and makes the advantages brought by multi robots difficult to leverage. In the real world, especially large-scale environment, it is unrealistic for robots to have global communication capabilities. Besides, transmitting high volumes of sensor data could overwhelm the network capacity. Due to the above realistic factors, the system developed under communication restriction is necessary.

The communication limitations are considered from the following two aspects:

(1) Limited communication bandwidth (LB). LB makes transmitting the commonly used voxel map or point cloud that are convenient for planning and decision-making exceeds the bearing network capacity.

(2) Limited communication range (LR). Robots are constrained to maintain continuous connectivity or execute tasks lonely, introducing great challenges to exploration.

Refer to caption
Refer to caption
Figure 1: Composite image of the meeting phase of a multi-robot exploration experiment under communication limit in a large underground parking lot.
Refer to caption
Figure 2: An overview of our multi-robot exploration system. Each robot maintain an Environment Library through Local Handler(Sec.III). When the connection is built between robots, Meeting Handler will be triggered(Sec.IV).

To resolve the above issues, we propose a complete framework Meeting-Merging-Mission for multi-robot exploration, composed of a lightweight environment presentation method and an efficient cooperative exploration strategy.

For LB, in order to reduce bandwidth for transmission, we use star-convex polytopes to represent known free space. Moreover, utilizing the meshes of the polytopes, we can represent the frontiers which is the boundary of known space. For more efficient exploration decision-making, we generate Super Frontier Infomation (SFI), an integrated information structure representing high-level frontiers and viewpoints. By transmitting star-convex polytopes and SFI, robots obtain the necessary environment information with low bandwidth cost.

For LR, we introduce a new mission-based protocol for a team of robots to execute exploration tasks without global communication. The key is assigning missions to robots that guide them to disconnect actively for independent exploration and rendezvous stably for sharing collected information. Besides, we give a complete path planning scheme to balance both exploration mission and requirement of rendezvous in all process of exploration.

Compared with existing state-of-the-art works, our proposed system can explore large-scale environments in less time. We perform comprehensive tests in simulation and real world to validate the efficiency and practicability of our framework. Summarizing our contributions as follows:

(1) A lightweight environment representation using star-convex polytopes and SFI offering essential environment information to drive exploration.

(2) A new mission-based protocol for multi-robot exploration in the absence of global communication. The distributed protocol reduces repeated exploration and increases exploration efficiency.

(3) A complete path planning scheme in all processes of exploration, including centralized planning in joint meeting phase and decentralized planning in lonely exploration phase.

II Related Work

II-A Environment Representation

For large-scale scenarios, a lightweight environment representation to is of vital importance to meet the practical communication limit. Some works [1, 2] use the Gaussian mixture model (GMM) as a global spatial representation of the environment. GMM learns a density function of obstacle point clouds via the expectation-maximization (EM), compresses a huge amount of data as several parameters. However, unnecessary computation and inaccuracy have been introduced by GMM, as the free space is not recorded but has to be reconstructed for the component update. Katz et al. [3] propose to use the HPR (Hidden Point Removal) operator [4] to determine the visibility of a point cloud given a viewpoint, without reconstruction or normal estimation. Based on HPR, Zhong [5] efficiently generates large, free, and guaranteed convex space among arbitrarily cluttered obstacles. In this way, the visibility and free space information of a complex environment are extracted by the polytope, which is another compact representation.

The above two groups of representation can both drive robots explore. Leveraging the GMM method to model the observed obstacles, information entropy can be calculated for the next viewpoint with large information gain[1]. Furthermore, the polytope-based method generates free space to distinguish known and unknown regions to drive robot exploration. When all the unobserved aims are eliminated by free space, exploration completes. Yang uses convex polyhedrons to estimate 3D free space in [6]. However, the convex constraint makes it conservative, especially when robot is in the intersections, reducing the unknown region eliminating efficiency. Williams [7] uses the method in [4] to generate meshes as frontiers. While without maintaining free space, the deletion of frontiers is done by visibility check. The frontier is not visible if there exists another one intersected by the raycast line between the robot and the frontier, which leads the deletion operation to be conservative and not accurate enough, especially when the free space shape is complex.

Refer to caption
Figure 3: The process of free space and super frontier information generation. (a) Generation process of star-convex polytope. (b) The current free space represented by a star-convex polytope. (c) The SFI of the current frame. Note that cluster FC1 which is marked by red number 1 is generated above a low obstacle showing the spatial representation capability in the z-axis direction.

II-B Multi-robot Exploration

Based on the communication mechanism, multi-robot exploration can be summarized into three categories: without any connection requirement, with continuous connection requirement, and with active disconnection and reconnection.

In the first category[8, 9, 10, 11, 12, 13], communications are episodic and opportunistic, which could result in repeated exploration and useless energy consumption [14]. The second category requires robots to keep continuous connection, which is the most restrictive class. In [15], robots explore a building subject to the constraint of maintaining line-of-sight communications. In [16], authors present a system in which robots explore the environment while permanently maintaining wireless networking. Jensen [17, 18] proposes several systems which feature a ”mild” form of continuous connection that allows robots to reconnect if it accidentally disconnects in exploration. However, the connection requirements of these approaches might over-constrain the mission objective, resulting in reducted behaviors.

Besides, the third family of the approaches allows robots to disconnect and reconnect actively. De Hoog[19, 20] innovatively propose a role-based exploration framework, and extend it to cover communication-limited cases. Considering the base station, robots are divided into explorers and relays and coordinate through appointed rendezvous positions. The former is assigned to explore unknown environments, and the latter moves back and forth only to deliver information. Later, some work refines the framework. Andre [21] focus on the routing protocols required to share information. Cesare [22] presents an interesting feature that UAVs land and act as fixed relays when run out of battery. However, even if the role-based framework resolves the limited communication range, the periodic meetings will result in many information-less flights, constraining the exploration process.

Different from existing work, without base-station, our proposed framework considers robots equally. We expect them to disconnect actively for independent exploring but reduce information-less flight via our developed mission-based exploration strategy, resulting in efficient exploration in large-scale communication-limited environments.

III Environment Representation

To reduce the bandwidth requirements for transmission, we use the union of a series of star-convex polytopes to represent known free space. We use sampling method to generate star-convex polytope (Sec.III-A). Moreover, we extract meshes from these polytope as frontiers to represent the boundary of known and unknown space. When the free space updates, old frontiers are deleted efficiently (Sec.III-B). Then we cluster frontier meshes into frontier clusters (FC) (Sec.III-C). For better observation for robots, we attach a best viewpoint (VP) to each FC and further integrate viewpoints into super viewpoint (SVP) for decision-making (Sec.III-D). All SVPs and included information in them compose super frontier infomation (SFI), as listed in Tab. I.

TABLE I: Super Frontier Information
Symbol        Explanation
Fi Frontier mesh with center ci and normal ni
FCj Frontier Cluster with center Cj and normal Nj
VPj Viewpoint of FCj
SVPk Super viewpoint

III-A Star-Convex based Free Space Generation

Star-convex polytope is a specific polytope which can represent known free space by meshes, as shown in Fig.3(b).

We firstly construct a point set Sstar as the source for star-convex polytope generation by sampling in a local voxel map. We uniformly sample points in the cylindrical coordinate system whose origin locates at the position of the robot Pr with radius equals to sensor range Rsensor. And the sampling angle range is within robot’s field of view. For each sampled point Ps, we cast a ray from Pr to Ps. If the ray is unobstructed, Ps is added to a point set Sfree. Otherwise, if the ray hits obstacles, the first point hit obstacles is added to another point set Sobs.

Given point set Sstar=SfreeSobs, we take Pr as origin and use the following sphere mapping function to flip all points in Sstar with radius r:

P=F(P)=PPr+2(rPPr2)PPrPPr2. (1)

Then a convex hull of the flipped points is calculated and a star-convex polytope is determined by the points on the convex hull after sphere mapping inherently, as shown in Fig.3(a). For more details, we refer readers to our previous work [5]. A star-convex polytope is generated when the robot travels a certain distance. The union of a series of star-convex polytopes constitute known free space.

III-B Frontier Generation and Deletion

Refer to caption
Figure 4: (a) We project mesh on a unit rasterized sphere and generate the AABB. (b) The query result. Green and red points are inside and outside a polytope, respectively.

We represent frontiers of the environment using the meshes of star-convex polytopes. To obtain these frontier meshes, we delete meshes whose all vertices belong to Sobs and consider the rest meshes as the set of frontier F. We denote the purple meshes shown in Fig.3(c) as F. For each Fi, the center ci and the normal ni of it are calculated. As the normal has two directions, we choose the one satisfying (Prci)ni>0.

When a new star-convex polytope is generated, frontier meshes inside free space should be deleted. To this end, we need efficiently query whether a mesh is in a polytope and thus propose MeshTable to query if a mesh is inside a star-convex polytope. As Fig.4 (a) shows, given a star-convex polytope, we firstly project all its meshes to a unit rasterized sphere. For each projected mesh, its axis-aligned bounding box (AABB) on the sphere can be obtained. Then each cell in the AABB with its corresponding meshes form a MeshTable. In other words, the MeshTable records which projected mesh each grid is covered by.

For a mesh Fi to be queried, we project its center ci to the unit sphere and get the corresponding cell. Then, using the MeshTable, meshes corresponded to this cell can be retrieved. We connect vertices of each mesh with the origin of the polytope to formulate a tetrahedron. If ci is inside one of these tetrahedrons, Fi is inside a star-convex polytope. A mesh is classified as lying outside free space and deleted if it is not inside any star-convex polytopes.

To speed up the query, we build a KD-tree of all star-convex polytopes’ origins. Then, we search within a radius R centered at ci using this KD-tree, and obtain corresponding polytopes of Fi. If Fi is judged as inside one of these polytopes using the above-mentioned MeshTable query, we delete it. The result of querying is shown in Fig. 4 (b).

III-C Frontier Mesh Clustering

To reduce the number of meshes for efficient decision-making, we cluster the frontier meshes. We consider the similarity between meshes from the following three aspects:

  1. 1.

    Tangential distance: dt(Fi,Fj)=(cicj)nj2,

  2. 2.

    Normal distance: dn(Fi,Fj)=(cicj)×nj2,

  3. 3.

    Normal difference: δn(Fi,Fj)=(ninj)/22,

where ci,cj and ni,nj are the center and normal vector of Fi and Fj, respectively. The above similarity criteria are hard to be described by a vector in N-dimensional Euclidean space requiring by most of cluster methods like K-means. So we choose spectral clustering[23], which only needs the similarity matrix between the data.

For spectral clustering, we need calculate a degree matrix D and a similarity matrix S firstly. To obtaion D, we connect meshes with their k-nearest euclidean-distance neighbors to form a graph, then the degree matrix of the graph is D. For S, based on the above criteria, we have:

S=(exp(s(Fi,Fj)2/2σ2))i,j=1J, (2)
s(Fi,Fj)=ω1dt+ω2dn+ω3δn, (3)

where s(Fi,Fj) is the weighted sum of above three distance and σ is the preset parameter of Gaussian function. Given D and S, We can finally get frontier clusters using spectral clustering. The clustering example is shown in Fig.3(c), where red numbers represent clusters of frontiers, and the positions of the numbers represent the center of clusters.

Input: Frontier cluters FCi contains frontier meshes {Fj,j=1,2,,J}.
Output: Viewpoints VPi.
1
2Ni=FjFCinj/J;
3 Ci=FjFCicj/J;
4 𝐕=𝐜𝐲𝐥𝐢𝐧𝐝𝐫𝐢𝐜𝐚𝐥𝐒𝐚𝐦𝐩𝐥𝐞(Ni,Ci);
5 sbest=0;
6
7foreach vk𝐕 do
8 dθ=𝐚𝐜𝐨𝐬((skCi)NiskCi2);
9 dR=𝐚𝐛𝐬(skCi2Ropt);
10 scorek=ωθdθ+ωRdR;
11 if scorek>sbest then
12 sbest=scorek;
13 VPi=vk;
14
15 end if
16
17 end foreach
18
Algorithm 1 Viewpoint Generation

III-D Viewpoint and Super Viewpoint Generation

To observe frontier cluster at an appropriate angle and distance, we generate the best viewpoint for each FC by the method presented in Algorithm 1.

As Algorithm 1 presents, we firstly calculate the normal Ni and center Ci by averaging all the meshes blong to FCi. Then we score all the points sampled from cylindrical coordinates whose origin locates at Ci. The point with smaller angle error to Ni and colser to appropriate distance Ropt has higher weighted score. An example of generated viewpoints is shown in Fig.3(c).

To further reduce the scale of decision-making problem, for viewpoints contained in the same sphere with a given thresholding radius, we integrate them as a super viewpoint SVP, as Fig.3(c) shows. Finally, we get SVP, each of which consists of J frontier clusters {FCj,j=1,2,,J} with viewpoint {VPj,j=1,2,,J}. All the new generated part of SFI will be stored in the Environment Library.

IV Mission-based Exploration

In this section, we describe an efficient multi-robot exploration strategy with a proposed mission-based protocol. We divide the process of collaborative exploration into two phases: Joint Meeting and Lonely Exploration, corresponding to the Meeting and Local Handler shown in Fig. 2.

IV-A Mission-based Protocol for Multi-robot exploration

We expect robots to move independently for exploring and meet jointly for sharing information, even in the absence of global communication. We define each appointed rendezvous as a mission for robots, including meeting position and time. To achieve our expectation, we develop a centralized planner (see Sec.IV-B) in phase Joint Meeting for mission decision, and further propose a decentralized planner (see Sec.IV-D) in phase Lonely Exploration for path planning.

At the beginning of each exploration task, robots are assigned a mission in the first meeting, as shown in Fig. 5 (a). Then, robots spread out to explore independently. As the environment explored and new frontiers generated, each robot constantly replans by the decentralized planner, which guarantees the appointed meeting position is arrived on time.

However, actually, robots may accidentally meet in the Lonely Exploration phase. For this case, we define an extra rule: if robots with the same mission meet accidentally, they decide a new mission and only one of them keep the old mission. An example is shown in Fig. 5 (b), robots (blue and green) with the same mission (pink) meet accidentally and share information. There is no need for all of them to arrive at scheduled position in pink mission. Thus they decide a mission between them (purple) and only let the blue robot meet with the red robot. By this way, the green robot can spend more time on exploration and improve the efficiency.

Based on the above mission-based protocol, robots explore the whole environment by meeting sequentially with limited communication range, as shown in Fig. 5 (c) and (d).

Besides, in each meeting, robots autonomously cooperate to complete information aggregation. All robots send their message, including generated free space information and SFI, to a ”host” robot. The host robot merges them and eliminates frontiers that are inside the union of all free space, with similar process to Sec.III-B. Then the merged map information is send back to all meeting robots.

Refer to caption
Figure 5: The process of a simple demo exploration based on our protocol. Robots are with the colors red, blue, and green, and missions are pink and purple. Grey cells mean unexplored. and light blue cells mean explored.

IV-B Centralized Optimal Decision Planning

After information aggregation, by formulating a constrained integer optimization problem, the host decides a mission and assigns it, including the position and time of the next rendezvous. We consider the following rules:

  1. 1.

    Robots start from their positions, cross intermediate SVPs and reach a rendezvous position.

  2. 2.

    Each intermediate SVP is crossed only once.

To perform optimization, a motion cost between two points is required. It is calculated by:

Tm(pi,pj)=Length(pi,pj)vmax, (4)

where the path length is estimated by A* search on posegraph similar to [24], which is not the point of our paper.

We then let Rc={1,,n} and Sc={n+1,,m} denote positions of robots and super viewpoints and define three binary decision variables:

  • xijk: set to 1 iff robot k goes from node i to j

  • yik: set to 1 iff the node i is crossed by robot k

  • ti: set to 1 iff the node i is the rendezvous position

The centralized planning problem is formulated as follows:

minxijk,yik,ti,ti¯J=iNcjScdijkRcxijk,
s.t.ti¯=1ti,iSc, (5)
iNcxihk=yhk,kRc,hSc, (6)
kRcjScxhjk=1,hRc (7)
jScxhjkth¯=yhkth¯,kRc,hSc, (8)
jScxhjkth=0,kRc,hSc, (9)
kRcyhkth¯=th¯,hRc, (10)
kRcyhkth=nth,hRc, (11)
iScti=1, (12)

where Nc=RcSc and the cost dij of crossing from node i to node j is calculated by dij=Tm(pi,pj). Eq.(7) means each robot starts from their current positions. Eq.(9) and Eq.(11) mean that robots arrive at the viewpoint which is chose as rendezvous position, and Eq.(8) and Eq.(10) mean that other viewpoints is crossed by a robot once. Eq.(12) means that rendezvous position is unique.

IV-C Hierarchical Sub-optimal Decision Planning

Actually, routing problem is commonly considered after rendezvous position is fixed. Thus, optimizing rendezvous variables ti and path planning variables {xijk,yik} jointly is difficult and we plan to take it as future work. In this section, we aim to develop a hierarchical approach by firstly determining the rendezvous position and then solving a simplified problem. Firstly, we define the distance between a node and robots as

d(pv)=kRcTm(pv,pk)

where pv and pk is positions of node v and robot k. Then we have following options to determine the meeting position:

  1. 1.

    Furthest-Meeting: take the node v=maxvNcd(pv) as the rendezvous position.

  2. 2.

    Nearest-Meeting: take the node v=minvNcd(pv) as the rendezvous position.

  3. 3.

    Shortest-Meeting (Optimal): retrieve each node i, assume ti=1 and conduct optimization to get optimal cost Ji. Then select the node v=minvNcJv as rendezvous position.

Each of these methods can simplify formulated problem. In Sec.V, we compare the performances of using these methods and choose the Furthest-Meeting for best balancing efficiency and optimality.

As the rendezvous position is determined, the decision planning problem turns into a vehicle routing problem (VRP) [25]. We firstly use a heuristic function for initial path search and then utilize meta-heuristics method for local route search. In detail, from the positions of robots, we extend paths by iteratively adding the cheapest arc to the routes. In this way, we obtain an initial solution efficiently. Finally, we adopt the extended guided local search (EGLS) algorithm [26] to find an improved solution.

Until now, we have determined a rendezvous position Pc and some paths Ψ={1,,n} for robots, where Ψk={i|yik=1}. Fig. 6 shows the paths by using the Furthest-Meeting method. We then choose the maximum cost of paths as the basic time Tb and set Te as extra time for exploration. Besides, to guarantee robots have enough time to rendezvous sequentially, the rendezvous time Tc is:

Tc=Te+max(Tb+Tcur,Tl1+Tm(Pl1,Pc), (13)
,Tln+Tm(Pln,Pc)),

where (Plk,Tlk) is the last mission of the robot k and Tcur is the current time.

Refer to caption
Figure 6: Planned path using the Furthest-Meeting method.

IV-D Decentralized Path Planning for Single Robot

After the mission and paths are assigned to robots, they spread out to explore independently. Meanwhile, as environment is explored and new SVPs are generated, each robot continuously replans paths to cross some SVPs and arrives at the next rendezvous positions on time. Furthermore, to alleviate repetitive exploration, we introduce penalties to the area that is assigned to other robots for exploring.

For a single robot r, we let Pr be the position of it, Pm be the next appointed meeting position, and Sd be the set of SVPs. Then, we define the penalty pi of node i:

pi={jPmSddij(ikrΨk)0(ikrΨk)

where Ψk is the path assigned to the robot k.

Finally, we formulate the decentralized path planning as

minxij,yiJ=iNdjPmSddijxij+iPmSdpiyi,
s.t.iNdxih=yh,hPmSd, (14)
jSdxhj=yh,hPrSd, (15)
jSdxhj=0,hPm, (16)
yh=1,hPrPm, (17)
iNdjPmSddijxijTmTcur, (18)

where Nd=PrPmSd, Tm is the scheduled time of next meeting, and Tcur is the current time. Note that adding penalty iPmSdpiyi is to avoid robot exploring areas that are assigned to other robots. Eq.(14) and Eq.(15) provide constrained relationship between xij and yi. Eq.(16) denotes that the robot arrives at meeting position and does not leave. Eq.(17) means that the robot must start from its current position and end with the meeting position. Eq.(18) means that the robot is guaranteed to arrive meeting in time.

The decentralized path planning problem can be considered as a variant of capacitated vehicle routing problem (CVRP) [25]. While a robot moving, it utilizes the latest planned path as an initial solution and refine it using EGLS.

V Experiment

In this section, we conduct various simulation comparisons and real-world experiments to validate our proposed framework and present its advanced performance. As for multi-robot autonomous navigation, we use EGO-Swarm [27] to generate smooth and safe tajectories.

V-A Comparisons and Benchmark

V-A1 Bandwidth Comparisons

Refer to caption
Figure 7: Bandwidth comparision of environment information used to drive exploration using GMM and our method.
Refer to caption
Figure 8: The process of a exploration experiment with three quadrotors. Areas in different colors denote the region explored by different robots.

In this part, we compare bandwidth cost with GMM-based methods mentioned in Sec.II-A. In GMM method, point cloud Z is modeled as J normal distributions. According to the method and parameters described in [1], we take J=|𝒵|/Rc with Rc=160 to yield good performance. With the same frequency, we compare the bandwidth of data volume of the environment represented by GMM and our method when the data is transmitted over the network. We test with different obstacle densities (percent by volume) and sensor ranges. As the result shown in Fig. 7, the bandwidth cost of our method is lower than the GMM method in all cases, especially for dense obstacles and large sensor range.

V-A2 Meeting Position Decision Comparisons

We compare three methods of meeting position selection as mentioned in Sec.IV-B. We simulate three cases: three robots with 30-35 SVPs, six robots with 45-60 SVPs, and ten robots with 100-120 SVPs. Each experiment is conducted 20 times in 100m x 100m environments with 100-150 obstacles. Fig. 9 shows the cost J and solving time t of three methods. As shown, the method with minimum cost consumes the most time, while the Furthest-Meeting method better trade-off between solving time and cost.

Refer to caption
Figure 9: Solving time and cost comparisons between three different meeting place selection strategies.
Refer to caption
Figure 10: Simulation environment and trajectories of three drones

V-A3 Strategy Benchmark

We conduct various simulated experiments to compare our method with Burgard’s [8] and Rooker’s [16] methods. They are representative works of exploration without communication constraints and with continuous connection requirements, respectively. We simulate several environments with obstacles, shown as Fig. 10. The sensor range and communication range are set to 10m and 3m, respectively. We test with different building sizes and robot numbers, with four criteria including exploration time, repeated exploration proportion, independent exploration proportion, and length of trajectories. The results are shown in Tab. II and Tab. III. According to the statistics, our proposed method outperforms in exploration time, and efficiently reduces repeated exploration in all cases, especially in large-scale environment.

Refer to caption
Figure 11: Generated star-convex polytopes in a real-world experiment.
TABLE II: Robots Number Benchmark
Scenario Method time(s) repeated(%) independent(%) ltraj(m)
Ours 440 22.2 61.1 401
#Robots=2 Burgard’s[8] 683 63.6 80.1 663
Rooker’s[16] 1581 98.1 98.7 1439
Ours 397 20.0 36.5 351
#Robots=3 Burgard’s[8] 492 80.9 89.7 475
Rooker’s[16] 1337 96.8 95.4 1233
Ours 403 20.3 29.5 309
#Robots=4 Burgard’s[8] 451 63.5 59.2 422
Rooker’s[16] 1107 95.7 94.1 907
TABLE III: Environment Size Benchmark
Scenario Method time(s) repeated(%) independent(%) ltraj(m)
Ours 118 20.1 60.1 109
2500m2 Burgard’s[8] 139 63.6 81.8 131
Rooker’s[16] 128 98.1 99.2 122
Ours 283 20.0 55.3 271
5500m2 Burgard’s[8] 398 80.9 89.2 372
Rooker’sk[16] 801 98.2 97.7 753
Ours 440 22.2 61.1 401
10000m2 Burgard’s[8] 683 70.3 79.3 663
Rooker’s[16] 1581 96.7 95.4 1439

V-B Real-World Experiment

Real-world experiments are presented on both UGVs and UAVs platforms, as shown in Fig. 12. Each of these robots is equipped with a lidar-inertia localization module and a multi-robot planning module. They are deployed in a large underground parking lot for exploration.

In the 50m×30m UGV testing area, we conduct experiments with a 2.5m communication range and a 5m sensor range. In the 60m×40m UAV testing area, we set a 4.5m communication range and an 8m sensor range. In all experiments, our proposed framework can drive multi-robot the exploration efficiently under communication limits. We refer readers to the video for more information. As shown in Fig. 11, our generated star-convex polytopes cover the whole explored environment. One of the experiment processes is shown in Fig. 8. In this experiment, 3 UAVs explore coordinately with a max velocity of 1m/s. Even if without global communication, they finish the exploration in 250s. For comparison, we also conduct a single UAV exploration. However, the UAV fails to accomplish the task after it runs out of battery after 8min operation.

Refer to caption
Figure 12: Different platforms used for real-world experiments.

VI Conclusion

In this paper, we develop a framework for multi-robot exploration under communication limits. To reduce transmission bandwidth, we utilize star-convex polytopes to represent explored free space and incrementally update SFI to drive exploration. To coordinate without global communication, we introduce a mission-based protocol for robots to explore independently and rendezvou to share information. Future works will be extended to the multi-robot exploration considering localization drift. Robots will plan to actively improve localization quality. Avoiding representing free space via a single occupancy map, we can conduct loop closures without intractable volumetric map and frontier fusion.

References

  • [1] M. Corah, C. O’Meadhra, K. Goel, and N. Michael, “Communication-efficient planning and mapping for multi-robot exploration in large environments,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 1715–1721, 2019.
  • [2] C. O’Meadhra, W. Tabib, and N. Michael, “Variable resolution occupancy mapping using gaussian mixture models,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 2015–2022, 2018.
  • [3] S. Katz and A. Tal, “On the visibility of point clouds,” in 2015 IEEE International Conference on Computer Vision, 2015.
  • [4] S. Katz, A. Tal, and R. Basri, “Direct visibility of point sets,” in ACM SIGGRAPH 2007 papers, 2007, pp. 24–es.
  • [5] X. Zhong, Y. Wu, D. Wang, Q. Wang, C. Xu, and F. Gao, “Generating large convex polytopes directly on point clouds,” arXiv preprint arXiv:2010.08744, 2020.
  • [6] F. Yang, D.-H. Lee, J. Keller, and S. Scherer, “Graph-based topological exploration planning in large-scale 3d environments,” arXiv preprint arXiv:2103.16829, 2021.
  • [7] J. Williams, S. Jiang, M. O’Brien, G. Wagner, E. Hernandez, M. Cox, A. Pitt, R. Arkin, and N. Hudson, “Online 3d frontier-based ugv and uav exploration using direct point cloud visibility,” in 2020 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI). IEEE, 2020, pp. 263–270.
  • [8] W. Burgard, M. Moors, C. Stachniss, and F. E. Schneider, “Coordinated multi-robot exploration,” IEEE Transactions on robotics, vol. 21, no. 3, pp. 376–386, 2005.
  • [9] D. Fox, J. Ko, K. Konolige, B. Limketkai, D. Schulz, and B. Stewart, “Distributed multirobot exploration and mapping,” Proceedings of the IEEE, vol. 94, no. 7, pp. 1325–1339, 2006.
  • [10] R. Zlot, A. Stentz, M. B. Dias, and S. Thayer, “Multi-robot exploration controlled by a market economy,” in 2002 IEEE International Conference on Robotics and Automation. IEEE.
  • [11] T.-M. Liu and D. M. Lyons, “Leveraging area bounds information for autonomous decentralized multi-robot exploration,” Robotics and Autonomous Systems, vol. 74, pp. 66–78, 2015.
  • [12] L. Matignon, L. Jeanpierre, and A.-I. Mouaddib, “Coordinated multi-robot exploration under communication constraints using decentralized markov decision processes,” in Twenty-sixth AAAI conference on artificial intelligence, 2012.
  • [13] T. Andre and C. Bettstetter, “Collaboration in multi-robot exploration: to meet or not to meet?” Journal of intelligent & robotic systems, vol. 82, no. 2, pp. 325–337, 2016.
  • [14] F. Amigoni, J. Banfi, and N. Basilico, “Multirobot exploration of communication-restricted environments: A survey,” IEEE Intelligent Systems, vol. 32, no. 6, pp. 48–57, 2017.
  • [15] R. C. Arkin and J. Diaz, “Line-of-sight constrained exploration for reactive multiagent robotic teams,” in 7th International Workshop on Advanced Motion Control. Proceedings (Cat. No. 02TH8623). IEEE, 2002, pp. 455–461.
  • [16] M. N. Rooker and A. Birk, “Multi-robot exploration under the constraints of wireless networking,” Control Engineering Practice, vol. 15, no. 4, pp. 435–445, 2007.
  • [17] E. A. Jensen, E. Nunes, and M. Gini, “Communication-restricted exploration for robot teams,” in Workshops at the Twenty-Eighth AAAI Conference on Artificial Intelligence, 2014.
  • [18] E. A. Jensen, L. Lowmanstone, and M. Gini, “Communication-restricted exploration for search teams,” in Distributed Autonomous Robotic Systems. Springer, 2018, pp. 17–30.
  • [19] J. De Hoog, S. Cameron, and A. Visser, “Role-based autonomous multi-robot exploration,” in 2009 Computation World: Future Computing, Service Computation, Cognitive, Adaptive, Content, Patterns. IEEE, 2009, pp. 482–487.
  • [20] J. De Hoog, S. Cameron, A. Visser, et al., “Autonomous multi-robot exploration in communication-limited environments,” in Proceedings of the Conference on Towards Autonomous Robotic Systems. Citeseer, 2010, pp. 68–75.
  • [21] T. Andre, “Autonomous exploration by robot teams: coordination, communication, and collaboration,” Ph.D. dissertation, PhD thesis, Alpen-Adria-Univ, 2015.
  • [22] K. Cesare, R. Skeele, S.-H. Yoo, Y. Zhang, and G. Hollinger, “Multi-uav exploration with limited communication and battery,” in 2015 IEEE International Conference on Robotics and Automation. IEEE.
  • [23] U. Von Luxburg, “A tutorial on spectral clustering,” Statistics and computing, vol. 17, no. 4, pp. 395–416, 2007.
  • [24] E. M. Lee, J. Choi, H. Lim, and H. Myung, “Real: Rapid exploration with active loop-closing toward large-scale 3d mapping using uavs,” in 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE.
  • [25] P. Munari, T. Dollevoet, and R. Spliet, “A generalized formulation for vehicle routing problems,” arXiv preprint arXiv:1606.01935, 2016.
  • [26] P. Mills, “Extensions to guided local search,” Ph.D. dissertation, Citeseer, 2002.
  • [27] X. Zhou, J. Zhu, H. Zhou, C. Xu, and F. Gao, “Ego-swarm: A fully autonomous and decentralized quadrotor swarm system in cluttered environments,” in 2021 IEEE International Conference on Robotics and Automation. IEEE.