+ All Categories
Home > Documents > Meeting-Merging-Mission: A Multi-robot Coordinate ...

Meeting-Merging-Mission: A Multi-robot Coordinate ...

Date post: 16-Oct-2021
Category:
Upload: others
View: 3 times
Download: 0 times
Share this document with a friend
8
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, Chao Xu, and Fei Gao AbstractThis letter presents a complete framework Meeting-Merging-Mission for multi-robot exploration under communication restriction. Considering communication is lim- ited 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 utilizes specific polytopes to maintains free space and super frontier information (SFI) 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. I NTRODUCTION Recently, thanks to the maturity of multi-robot cooperative technology, swarm exploration has received increasing atten- tion in many application area. 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 lim- itation 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 com- munication 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 ex- ceeds the bearing network capacity. (2) Limited communication range (LR). Robots are con- strained to maintain continuous connectivity or execute tasks lonely, introducing great challenges to exploration. To resolve the above issues, we propose a complete frame- work Meeting-Merging-Mission for multi-robot exploration, All authors are with the State Key Laboratory of Industrial Control Technology, Zhejiang University, Hangzhou 310027, China, and also with the Huzhou Institute of Zhejiang University, HuZhou 313000, China. E- mails: {ymgao, yj wang, cxu, fgaoaa}@zju.edu.cn. * Equal contributors. Fig. 1. Composite image of the meeting phase of a multi-robot exploration experiment under communication limit in a large underground parking lot. 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 explo- ration and rendezvous stably for sharing collected informa- tion. 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 pro- posed 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: arXiv:2109.07764v1 [cs.RO] 16 Sep 2021
Transcript
Page 1: Meeting-Merging-Mission: A Multi-robot Coordinate ...

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

Yuman Gao*, Yingjian Wang*, Xingguang Zhong, Tiankai Yang, Mingyang Wang, Zhixiong Xu,Yongchao Wang, Chao Xu, and Fei Gao

Abstract— This letter presents a complete frameworkMeeting-Merging-Mission for multi-robot exploration undercommunication restriction. Considering communication is lim-ited in both bandwidth and range in the real world, we proposea lightweight environment presentation method and an efficientcooperative exploration strategy. For lower bandwidth, eachrobot utilizes specific polytopes to maintains free space andsuper frontier information (SFI) as the source for explorationdecision-making. To reduce repeated exploration, we developa mission-based protocol that drives robots to share collectedinformation in stable rendezvous. We also design a completepath planning scheme for both centralized and decentralizedcases. To validate that our framework is practical and generic,we present an extensive benchmark and deploy our system intomulti-UGV and multi-UAV platforms.

I. INTRODUCTION

Recently, thanks to the maturity of multi-robot cooperativetechnology, swarm exploration has received increasing atten-tion in many application area. Multiple robots can explorewider regions in the time unachievable by a single one,with better fault tolerance and uncertainty compensation.However, in actual exploration missions, communication lim-itation introduces great challenges to multi-robot explorationtasks and makes the advantages brought by multi robotsdifficult to leverage. In the real-world, especially large-scaleenvironment, it is unrealistic for robots to have global com-munication capabilities. Besides, transmitting high volumesof sensor data could overwhelm the network capacity. Dueto the above realistic factors, the system developed undercommunication restriction is necessary.

The communication limitations are considered from thefollowing two aspects:

(1) Limited communication bandwidth (LB). LB makestransmitting the commonly used voxel map or point cloudthat are convenient for planning and decision-making ex-ceeds the bearing network capacity.

(2) Limited communication range (LR). Robots are con-strained to maintain continuous connectivity or execute taskslonely, introducing great challenges to exploration.

To resolve the above issues, we propose a complete frame-work Meeting-Merging-Mission for multi-robot exploration,

All authors are with the State Key Laboratory of Industrial ControlTechnology, Zhejiang University, Hangzhou 310027, China, and also withthe Huzhou Institute of Zhejiang University, HuZhou 313000, China. E-mails: {ymgao, yj wang, cxu, fgaoaa}@zju.edu.cn.* Equal contributors.

FAST LAB 2021/7/27 | Fle|d AutonomousSystem & compuTmg I

Fig. 1. Composite image of the meeting phase of a multi-robot explorationexperiment under communication limit in a large underground parking lot.

composed of a lightweight environment presentation methodand 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 canrepresent the frontiers which is the boundary of known space.For more efficient exploration decision-making, we generateSuper Frontier Infomation (SFI), an integrated informationstructure representing high-level frontiers and viewpoints. Bytransmitting star-convex polytopes and SFI, robots obtain thenecessary environment information with low bandwidth cost.

For LR, we introduce a new mission-based protocol for ateam of robots to execute exploration tasks without globalcommunication. The key is assigning missions to robots thatguide them to disconnect actively for independent explo-ration and rendezvous stably for sharing collected informa-tion. Besides, we give a complete path planning schemeto balance both exploration mission and requirement ofrendezvous in all process of exploration.

Compared with existing state-of-the-art works, our pro-posed system can explore large-scale environments in lesstime. We perform comprehensive tests in simulation andreal world to validate the efficiency and practicability of ourframework. Summarizing our contributions as follows:

arX

iv:2

109.

0776

4v1

[cs

.RO

] 1

6 Se

p 20

21

Page 2: Meeting-Merging-Mission: A Multi-robot Coordinate ...

Host Decision

Centralized PlanningRendezvous-based

Vehicle Routing Problem

Meeting HandlerEnv-Info Library

Free Space Info

FreeSpaceKeyPose

Free Space Generation

Decentralized PlanningMission-Constrained

Capacitated VRP

Local Handler

FreeSpace Info

Super FrontierInfo

Robot 1

Robot 0

Information AggregationFrontier Mesh Clustering

Super Viewpoint

Super Frontier Info

SFI Update

Fig. 2. An overview of our multi-robot exploration system. Each robot maintain an Environment Library through Local Handler(Sec.III). When theconnection is built between robots, Meeting Handler will be triggered(Sec.IV).

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

(2) A new mission-based protocol for multi-robot explo-ration in the absence of global communication. The dis-tributed protocol reduces repeated exploration and increasesexploration efficiency.

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

II. RELATED WORK

A. Environment RepresentationFor large-scale scenarios, a lightweight environment rep-

resentation to is of vital importance to meet the practicalcommunication limit. Some works [1, 2] use the Gaussianmixture model(GMM) as a global spatial representation ofthe environment. GMM learns a density function of obstaclepoint clouds via the expectation-maximization (EM), com-presses a huge amount of data as several parameters. How-ever, unnecessary computation and inaccuracy have beenintroduced by GMM, as the free space is not recorded but hasto 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 guaranteedconvex space among arbitrarily cluttered obstacles. In thisway, the visibility and free space information of a complexenvironment are extracted by the polytope, which is anothercompact representation.

The above two groups of representation can both driverobots explore. Leveraging the GMM method to model theobserved obstacles, information entropy can be calculatedfor the next viewpoint with large information gain [1].Furthermore, the polytope-based method generates free spaceto distinguish known and unknown regions to drive robotexploration. When all the unobserved aims are eliminated byfree space, exploration completes. Yang uses convex polyhe-drons to estimate 3D free space in [6]. However, the constrain

of convex limits its representation capacity, reducing theeliminating efficiency of unobserved aims. Williams [7] usesthe method in [4] to generate meshes as frontiers. Whilewithout maintain free space, the deletion of frontiers is doneby visibility check. The frontier is not visible if there existsanother one intersected by the raycast line between the robotand the frontier, which leads the deletion operation to beconservative and not accurate enough, especially when thefree space shape is complex.

B. Multi-robot Exploration

Based on the communication mechanism, multi-robot ex-ploration can be summarized into three categories: withoutany connection requirement, with continuous connectionrequirement, and with active disconnection and reconnection.

In the first category [8]–[13], communications are episodicand opportunistic, which could result in repeated explo-ration and useless energy consumption [14]. The secondcategory requires robots to keep continuous connection,which is the most restrictive class. In [15], robots explorea building subject to the constraint of maintaining line-of-sight communications. In [16], authors present a system inwhich robots explore the environment while permanentlymaintaining wireless networking. Jensen [17, 18] proposesseveral systems which feature a ”mild” form of continuousconnection that allows robots to reconnect if it accidentallydisconnects in exploration. However, the connection require-ments of these approaches might over-constrain the missionobjective, resulting in reducted behaviors.

Besides, the third family of the approaches allows robotsto disconnect and reconnect actively. De Hoog [19, 20] in-novatively propose a role-based exploration framework, andextend it to cover communication-limited cases. Consideringthe base station, robots are divided into explorers and relaysand coordinate through appointed rendezvous positions. Theformer is assigned to explore unknown environments, and thelatter moves back and forth only to deliver information. Later,some work refines the framework. Andre [21] focus on therouting protocols required to share information. Cesare [22]

Page 3: Meeting-Merging-Mission: A Multi-robot Coordinate ...

Our Research

(a)

Star-convex Polytope

Frontier Mesh

Sample Points Frontier Cluster Center123

Viewpoint

Local Voxel Map

Super Viewpoint

(b) (c)

Viewpoint Super Viewpoint

123

Fig. 3. The process of free space and super frontier information generation. (a) Multi-raduis sampling to create point cloud used for star-convex polytopes.(b) The current free space represented by a star-convex polytope. (c) The SFI of the current frame.

presents an interesting feature that UAVs land and act as fixedrelays 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-lessflights, constraining the exploration process.

Wildly different from existing work, without base-station,our proposed framework considers robots equally. We expectthem to disconnect actively for independent exploring butreduce information-less flight via our developed mission-based exploration strategy, resulting in efficient explorationin 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 torepresent known free space. Moreover, we employ meshesof these polytope as frontiers to represent the boundary ofknown and unknown space. We use sampling method togenerate star-convex polytope (Sec.III-A). Then we clusterall the frontier meshes extracted from the polytopes intofrontier clusters (FC) (Sec.III-B). For better observation ofthe robots, we attach a best viewpoint (VP) to each frontiercluster and further integrate viewpoints into super viewpoint(SVP) for decision-making (Sec.III-C). When the free spaceupdates, old frontiers are deleted efficiently (Sec.III-D). AllSVPs and included information in them compose superfrontier infomation (SFI), as listed in Tab. I.

A. Star-Convex based Free Space Generation

Star-convex polytope is a specific polytope which canrepresent known free space by meshes, as shown in Fig.3(b). To generate a star-convex polytope, we use a point setSstar with points sampled in a local voxel map. As shownin Fig.3(a), we uniformly sample points in the cylindricalcoordinate system whose origin locates at the position of therobot Pr with proportional decreasing radius {r = ri|ri =Rsensor/2

i, i ∈ 0, 1, ..., n}. Sampling at multiple radius untilr < rthr makes free space more accurate near the robot.

For each sampled point Ps, we cast a ray from Pr toPs. If the ray is unobstructed and the sample radius equalsto Rsensor, Ps is added to a point set Sfree. Otherwise, ifthe ray hits obstacles, the first point hit obstacles is addedto another point set Sobs. Furthermore, to obtain densersampling in obstacle-rich regions, we insert points between

each pair of adjacent points Pi, Pj recursively, until allsampled points satisfy the following conditions:

arccos((Pj − Pi)(Pj − Pr)

||Pj − Pi||||Pj − Pr||) < θthr, (1)

||Pi − Pj || < Dthr. (2)

We then use Sstar, the union of Sfree and Sobs, togenerate star-convex polytopes based our previous work [5].

TABLE ISUPER FRONTIER INFORMATION

Symbol ExplanationFi Frontier mesh with center ci and normal ni

FCj Frontier Cluster with center Cj and normal matrix Nj

VPj Viewpoint of FCj

SVPk Super viewpoint

B. Frontier Mesh Clustering

We represent frontiers of the environment using themeshes of star-convex polytopes. To obtain these frontiermeshes, we delete meshes whose all vertices belong to Sobs

and consider the rest meshes as the set of frontier F . Wedenote the purple meshes shown in Fig.2(c) as F .

To reduce the number of meshes for efficient decision-making, we conduct spectral clustering [23] on F , whichneed a degree matrix D and a similarity matrix S. To ob-taion D, we connect meshes with their k-nearest euclidean-distance neighbors to form a graph, then the degree matrixof the graph is D. For S, we consider the similarity betweenmeshes from the following three aspects:

1) Tangential distance: dt(Fi, Fj) = ‖(ci − cj) · nj‖2.2) Normal distance: dn(Fi, Fj) = ‖(ci − cj)× nj‖2.3) Normal difference: δn(Fi, Fj) = ‖(ni − nj)/2‖2.

where ci, cj and ni, nj are the center and normal vector of Fi

and Fj , respectively. Based on the above criteria, we have:

S =(exp(−s(Fi, Fj)

2/2σ2))i,j=1...J

, (3)

s(Fi, Fj) = ‖[dt, dn, δn]‖ω, (4)

where s(Fi, Fj) is the weighted sum of above three distanceand σ is the preset parameter of Gaussian function. Given D

Page 4: Meeting-Merging-Mission: A Multi-robot Coordinate ...

Algorithm 1: Viewpoint GenerationInput: frontier cluters FC = {1, ..., J}.Output: viewpoints V P = {1, ..., J}.

1 for each FCj do2 nave =

∑Fi∈FCj

ni/|FCj |;3 (NPCA)3×3, (λPCA)3×1 ← PCA(FCj);4 Nj ← calNormalMatrix(nave, NPCA);5 repeat6 Sampling ω1 ∈ [0, ωmax];7 ω2 ∈ [0, ωmax · λPCA,2/λPCA,1];8 Ri ∈ [Rmin, Rmax] ;9 ω = [1, ω1, ω2]T ;

10 VP j = Cj +Njω

‖Nj‖‖ω‖ ·Ri ;11 until isInFreeSpace(VP j);12 end

Our Research

𝐍𝐍𝟎𝟎

(a) (b)

𝐍𝐍𝟏𝟏

Fig. 4. (a) An illustration of vector N0 and N1. (b) The sample regionof viewpoint.

and S, We can finally get frontier clusters using spectralclustering. The clustering example is shown in Fig.3(c),where red numbers represent clusters of frontiers.

C. Viewpoint and Super Viewpoint Generation

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

As Fig.4(a) shows, each FC is modeled as an ellipsoidwhose pose can be described by three mutually orthogonalunit vectors [N0, N1, N2]. We denote Nj = [N0, N1, N2].To get N0 of a frontier cluster FCj , we calculate it as theaverage normal vector nave. Then, by performing principalcomponent analysis (PCA) for FCj , we get a eigenvectormatrix (NPCA)3×3 and eigenvalue matrix (λPCA)3×1 =[λPCA,0, λPCA,1, λPCA,2] sorted descendingly. Then weproject the last two eigenvectors NPCA,1, NPCA,2 to theellipsoid plane as N1, N2.

Using Nj and the sampling radius and angle, the sampleregion can be determined as Fig.4(b) shows. If sampledviewpoint is not in free space, we gradually change theangle and radius from their initial values and generate againuntill VP j is feasible. An example of generated viewpointsis shown in Fig.3(c).

Furthermore, for viewpoints contained in the same spherewith a given thresholding radius, we integrate them as a superviewpoint SV Pk, as Fig.3(c) shows. Finally, we get SV P ,each of which consists of J frontier clusters FC = {1, ..., J}with viewpoint V P = {1, ..., J}. All the new generated partof SFI will be stored in the Environment Library.

Our Research

(a) (b)

Unit Sphere

Fig. 5. (a) We project mesh on a unit rasterized sphere and generate theAABB. (b) The query result. Green and red points are inside and outside apolytope, respectively.

D. Frontier Deletion

When a new star-convex polytope is generated, frontiermeshes inside free space should be deleted. To this end,we need efficiently query whether a mesh is in a polytopeand thus propose MeshTable to query if a mesh is insidea star-convex polytope. As Fig.5 (a) shows, given a star-convex polytope, we firstly project all its meshes to a unitrasterized sphere. For each projected mesh, its axis-alignedbounding box (AABB) on the sphere can be obtained. Theneach cell in the AABB with its corresponding meshes form aMeshTable. For a mesh Fi to be queried, we project its centerci to the unit sphere and get the corresponding cell. Then,using the MeshTable, meshes correspond to this cell can beretrieved. We connect vertices of each mesh with the robot’sposition to formulate a tetrahedron. If ci is inside one of thesetetrahedrons, Fi is inside a star-convex polytope. Therefore,a mesh is classified as lying outside free space and waitingto be deleted if it is not inside any star-convex polytopes.

For speeding up the query, we build a KD-tree of key posesof star-convex polytopes. Then, we search key poses withina radius R centered at ci using this KD-tree, and obtain theircorresponding star-convex polytopes. Here R = Rsensor +dt,thr and dt,thr is a preset maximum distance between twomeshes in the same FC. If Fi is judged as inside one of thesepolytopes using the above-mentioned MeshTable query, wedelete Fi from its frontier cluster FCj . The result of queryingis shown in Fig. 5 (b).

IV. MISSION-BASED EXPLORATION

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

A. Mission-based Protocol for Multi-robot exploration

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

At the first of each process of an exploration task, robotsare assigned a mission in the first meeting, as shown in

Page 5: Meeting-Merging-Mission: A Multi-robot Coordinate ...

(a)

(c) (d)

(b)

Fig. 6. The process of a simple demo exploration based on our protocol.Robots are with the colors red, blue, and green, and missions are pink andpurple. Grey cells mean unexplored. and light blue cells mean explored.

Fig. 6 (a). Then, robots spread out to explore independently.As the environment explored and new frontiers generated,each robot constantly replans by the proposed decentralizedplanner, which guarantees the appointed meeting position isarrived on time. However, actually, robots may accidentallymeet in the Lonely Exploration phase. For this case, wedefine an extra rule: if robots with the same mission meetaccidentally, they decide a new mission and only one of themkeep the old mission, as shown in Fig. 6 (b). In this way,robots explore the whole environment based on scheduledmeetings sequentially, as shown in Fig. 6 (c) and (d).

B. Rule-based Host Decision and Information Aggregation

Once meeting, robots need firstly decide a host in chargeof information aggregation and centralized decision. After-ward, other participants only send their information to thishost. In detail, we design three rules for deciding the hostrobot as follows:

1) Robots establishing a new meeting decide the hostbased on the timestamp of detecting other robots td. .

2) When a robot enters a host-decided meeting, it followsother old meeting participants to keep the same host,as shown in Fig. 7 1©.

3) If the host leaves a meeting, the rest participants re-decide the host by td as shown in Fig. 7 2©.

1 2

New Participant

Meeting Host

Meeting ParticipantMeeting ParticipantNew Meeting Host

Quit

Enter

Old Meeting Host

Fig. 7. Even if someone enters or leaves a meeting, there is only onerecognized chairman in the meeting.

Once the host is fixed, it receives messages from partici-pants, merges them, and sends the merged information backto other robots. The received messages include robots’ gen-erated free space information and SFI. After merging, somefrontiers are inside the union of all free space. Therefore, thehost eliminates them, with a process similar to Sec. III-D.

C. Centralized Optimal Decision Planning

After information aggregation, by formulating a con-strained integer optimization problem, the host decides amission and assigns it, including the position and time ofthe next rendezvous. We consider the following rules:

1) Robots start from their positions, cross intermediateSVPs and reach a rendezvous position.

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. (5)

We then let Rc = {1, ..., n} and Sc = {n + 1, ...,m}denote positions of robots and super viewpoints and definethree binary decision variables:• xkij : set to 1 iff robot k goes from node i to j• yki : set to 1 iff the node i is crossed by robot k• ti: set to 1 iff the node i is the rendezvous positionThen the centralized decision planning problem is formu-

lated as follows:

minxkij ,y

ki ,ti,t̄i

J =∑i∈Nc

∑j∈Sc

dij∑k∈Rc

xkij ,

s.t. t̄i = 1− ti, ∀i ∈ Sc,∑i∈Nc

xkih = ykh, ∀k ∈ Rc, h ∈ Sc,∑k∈Rc

∑j∈Sc

xkhj = 1, ∀h ∈ Rc∑j∈Sc

xkhj t̄h = ykh t̄h, ∀k ∈ Rc, h ∈ Sc,∑j∈Sc

xkhjth = 0, ∀k ∈ Rc, h ∈ Sc, (6)∑k∈Rc

ykh t̄h = t̄h, ∀h ∈ Rc,∑k∈Rc

ykhth = nth, ∀h ∈ Rc,∑i∈Sc

ti = 1,

where Nc = Rc ∪Sc and the cost dij of crossing from nodei to node j is calculated by dij = Tm(pi, pj)

D. Hierarchical Sub-optimal Decision Planning

As formulated in Equ. 6, introducing ti renders the op-timization intractable to solve. To this end, we develop ahierarchical approach by firstly determining the rendezvousposition and then solving a simplified problem. We havefollowing optional ways to determine the meeting position:

Page 6: Meeting-Merging-Mission: A Multi-robot Coordinate ...

1) Furthest-Meeting: choose the node with the maxi-mum distance to all robots as rendezvous position.

2) Nearest-Meeting: choose the node with the minimumdistance to all robots as rendezvous position.

3) Shortest-Meeting: retrieve all nodes and select thenode with the minimum cost J as rendezvous position.

Each of these methods can simplify Equ. 6 with sometrade-off on its optimality. In Sec. V, we compare the per-formances of using these methods and choose the Furthest-Meeting for best balancing efficiency and optimality.

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

Until now, we have determined a rendezvous position Pc

and some paths Ψ = {1, ..., n} for robots, where Ψk ={i|yki = 1}. Fig. 8 shows the paths by using the Furthest-Meeting method. We then choose the maximum cost of pathsas the basic time Tb and set an extra time Te for betterexploration. Besides, to guarantee robots have enough timeto rendezvous sequentially, the rendezvous time Tc is:

Tc = Te +max(Tb + Tcur, T1l + Tm(P 1

l , Pc), (7)..., Tn

l + Tm(Pnl , Pc)),

where (P kl , T

kl ) is the last mission of the robot k and Tcur

is the current time.

Obstacle Map

Star-convex Polytope

Planned Path

Super Viewpoints

Fig. 8. Planned path using the Furthest-Meeting method.

E. Decentralized Path Planning for Single Robot

After the mission and paths are assigned to robots, theyspread out to explore independently. Meanwhile, as environ-ment is explored and new SVPs are generated, each robotcontinuously replans paths to cross some SVPs and arrivesat the next rendezvous positions on time. Furthermore, toalleviate repetitive exploration, we introduce penalties to thearea 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 ofSVPs. Then, we define the penalty pi of node i:

1https://en.wikipedia.org/wiki/Metaheuristic

pi =

j∈Pm∪Sd

dij (i ∈⋃k 6=r

Ψk)

0 (i /∈⋃k 6=r

Ψk)

where Ψk is the path assigned to the robot k.Finally, we formulate the decentralized path planning as

minxij ,yi

J =∑i∈Nd

∑j∈Pm∪Sd

dijxij +∑

i∈Pm∪Sd

piyi,

s.t.∑i∈Nd

xih = yh, ∀h ∈ Pm ∪ Sd,∑j∈Sc

xhj = yh, ∀h ∈ Sd,∑j∈Sc

xhj = 0, h ∈ Pm, (8)

yh = 1, ∀h ∈ Pr ∪ Pm,∑i∈Nd

∑j∈Pm∪Sd

dijxij ≤ Tm − Tcur,

where Nd = Pr∪Pm∪Sd, Tm is the scheduled time of nextmeeting, and Tcur is the current time.

The decentralized path planning problem can be consid-ered as a variant of capacitated vehicle routing problem(CVRP) [24]. While a robot moving, it utilizes the latestplanned path as an initial solution and refine it using EGLS.

V. EXPERIMENT

In this section, we conduct various simulation compar-isons and real-world experiments to validate our proposedframework and present its advanced performance.

A. Comparisons and Benchmark1) Bandwidth Comparisons: In this part, we compare

bandwidth cost with GMM-based methods mentioned inSec.II-A. In GMM method, point cloud Z is modeled as Jnormal distributions. According to [1], we take J = |Z|/Rc

with Rc = 160 to yield good performance. With the samefrequency, we compare the bandwidth of data volume of theenvironment represented by GMM and our method. We testwith different obstacle densities and sensor ranges. As theresult shown in Fig. 9, the bandwidth cost of our methodis lower than the GMM method in all cases, especially fordense obstacles and large sensor range.

R = 5m R = 10m R = 20m

Fig. 9. Bandwidth comparision of environment information used to driveexploration using GMM and our method.

Page 7: Meeting-Merging-Mission: A Multi-robot Coordinate ...

Fig. 10. The process of a exploration experiment with three quadrotors. Areas in different colors denote the region explored by different robots.

2) Meeting Position Decision Comparisons: We comparethree methods of meeting position selection as mentionedin Sec.IV-C. We simulate three cases: three robots with 30-35 SVPs, six robots with 45-60 SVPs, and ten robots with100-120 SVPs. Each experiment is conducted 20 times in100m x 100m environments with 100-150 obstacles. Fig. 11shows the cost J and solving time t of three methods. Asshown, the method with minimum cost consumes the mosttime, while the Furthest-Meeting method better trade-offbetween solving time and cost.

Case 01 Case 02 Case 03

Furthest-Meeting

Nearest-Meeting

Shortest-Meeting

Furthest-Meeting

Nearest-Meeting

Shortest-Meeting

Furthest-Meeting

Nearest-Meeting

Shortest-Meeting

Fig. 11. Solving time and cost comparisons between three different meetingplace selection strategies.

Fig. 12. Simulation environment and trajectories of three drones

3) Strategy Benchmark: We conduct various simulatedexperiments to compare our method with Burgard’s [8] andRooker’s [16] methods. They are representative works ofexploration without communication constraints and with con-tinuous connection requirements, respectively. We simulate

several environments with obstacles, shown as Fig. 12. Wetest with different building sizes and robot numbers, withfour criteria including exploration time, repeated explorationproportion, independent exploration proportion, and lengthof trajectories. The results are shown in Tab. II and Tab. III.According to the statistics, our proposed method outperformsin exploration time, and efficiently reduces repeated explo-ration in all cases, especially in large-scale environment.

Fig. 13. Generated star-convex polytopes in a real-world experiment.

TABLE IIROBOTS 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 663Rooker’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 1233Ours 403 20.3 29.5 309

#Robots=4 Burgard’s [8] 451 63.5 59.2 422Rooker’s [16] 1107 95.7 94.1 907

B. Real-World ExperimentReal-world experiments are presented on both UGVs and

UAVs platforms, as shown in Fig. 14. Each of these robots

Page 8: Meeting-Merging-Mission: A Multi-robot Coordinate ...

TABLE IIIENVIRONMENT 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 131Rooker’s [16] 128 98.1 99.2 122

Ours 283 20.0 55.3 2715500m2 Burgard’s [8] 398 80.9 89.2 372

Rooker’sk [16] 801 98.2 97.7 753Ours 440 22.2 61.1 401

10000m2 Burgard’s [8] 683 70.3 79.3 663Rooker’s [16] 1581 96.7 95.4 1439

is equipped with a lidar-inertia localization module and amulti-robot planning module. They are deployed in a largeunderground parking lot for exploration.

Fig. 14. Different platforms used for real-world experiments.

In the 50m × 30m UGV testing area, we conduct ex-periments with a 2.5m communication range and a 5msensor range. In the 60m × 40m UAV testing area, we seta 4.5m communication range and an 8m sensor range. Inall experiments, our proposed framework can drive multi-robot the exploration efficiently under communication limits.We refer readers to the video for more information. Asshown in Fig. 13, our generated star-convex polytopes coverthe whole explored environment. One of the experimentprocesses is shown in Fig. 10. In this experiment, 3 UAVsexplore coordinately with a max velocity of 1m/s. Even ifwithout global communication, they finish the explorationin 250s. For comparison, we also conduct a single UAVexploration. However, the UAV fail to accomplish the taskafter it run out of battery after 8min operation.

VI. CONCLUSION

In this paper, we develop a Meeting-Merging-Missionframework for multi-robot exploration under communicationlimit circumstances. To reduce transmission bandwidth, weutilize star-convex polytopes to represent explored free spaceand incrementally update SFI to drive exploration. In orderto coordinate in the absence of global communication, weintroduce a mission-based protocol for robots to exploreindependently and reach appointed places at the scheduledtime for sharing collected information. Furthermore, varioussimulation comparisons and real-world experiments validatethat our proposed framework is both robust and efficient.

REFERENCES

[1] M. Corah, C. O’Meadhra, K. Goel, and N. Michael, “Communication-efficient planning and mapping for multi-robot exploration in largeenvironments,” IEEE Robotics and Automation Letters, vol. 4, no. 2,pp. 1715–1721, 2019.

[2] C. O’Meadhra, W. Tabib, and N. Michael, “Variable resolution occu-pancy mapping using gaussian mixture models,” IEEE Robotics andAutomation Letters, vol. 4, no. 2, pp. 2015–2022, 2018.

[3] S. Katz and A. Tal, “On the visibility of point clouds,” in Proceedingsof the IEEE International Conference on Computer Vision, 2015, pp.1350–1358.

[4] S. Katz, A. Tal, and R. Basri, “Direct visibility of point sets,” in ACMSIGGRAPH 2007 papers, 2007, pp. 24–es.

[5] X. Zhong, Y. Wu, D. Wang, Q. Wang, C. Xu, and F. Gao, “Generatinglarge convex polytopes directly on point clouds,” arXiv preprintarXiv:2010.08744, 2020.

[6] F. Yang, D.-H. Lee, J. Keller, and S. Scherer, “Graph-based topologicalexploration planning in large-scale 3d environments,” arXiv preprintarXiv: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 anduav exploration using direct point cloud visibility,” in 2020 IEEEInternational Conference on Multisensor Fusion and Integration forIntelligent Systems (MFI). IEEE, 2020, pp. 263–270.

[8] W. Burgard, M. Moors, C. Stachniss, and F. E. Schneider, “Coordi-nated 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 theIEEE, vol. 94, no. 7, pp. 1325–1339, 2006.

[10] R. Zlot, A. Stentz, M. B. Dias, and S. Thayer, “Multi-robot explorationcontrolled by a market economy,” in Proceedings 2002 IEEE interna-tional conference on robotics and automation (Cat. No. 02CH37292),vol. 3. IEEE, 2002, pp. 3016–3023.

[11] T.-M. Liu and D. M. Lyons, “Leveraging area bounds informationfor autonomous decentralized multi-robot exploration,” Robotics andAutonomous Systems, vol. 74, pp. 66–78, 2015.

[12] L. Matignon, L. Jeanpierre, and A.-I. Mouaddib, “Coordinated multi-robot exploration under communication constraints using decentralizedmarkov decision processes,” in Twenty-sixth AAAI conference onartificial 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 ofcommunication-restricted environments: A survey,” IEEE IntelligentSystems, vol. 32, no. 6, pp. 48–57, 2017.

[15] R. C. Arkin and J. Diaz, “Line-of-sight constrained exploration forreactive multiagent robotic teams,” in 7th International Workshop onAdvanced Motion Control. Proceedings (Cat. No. 02TH8623). IEEE,2002, pp. 455–461.

[16] M. N. Rooker and A. Birk, “Multi-robot exploration under the con-straints of wireless networking,” Control Engineering Practice, vol. 15,no. 4, pp. 435–445, 2007.

[17] E. A. Jensen, E. Nunes, and M. Gini, “Communication-restrictedexploration for robot teams,” in Workshops at the Twenty-Eighth AAAIConference on Artificial Intelligence, 2014.

[18] E. A. Jensen, L. Lowmanstone, and M. Gini, “Communication-restricted exploration for search teams,” in Distributed AutonomousRobotic Systems. Springer, 2018, pp. 17–30.

[19] J. De Hoog, S. Cameron, and A. Visser, “Role-based autonomousmulti-robot exploration,” in 2009 Computation World: Future Com-puting, Service Computation, Cognitive, Adaptive, Content, Patterns.IEEE, 2009, pp. 482–487.

[20] J. De Hoog, S. Cameron, A. Visser, et al., “Autonomous multi-robotexploration in communication-limited environments,” in Proceedingsof 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 2015IEEE international conference on robotics and automation (ICRA).IEEE, 2015, pp. 2230–2235.

[23] U. Von Luxburg, “A tutorial on spectral clustering,” Statistics andcomputing, vol. 17, no. 4, pp. 395–416, 2007.

[24] P. Munari, T. Dollevoet, and R. Spliet, “A generalized formulation forvehicle routing problems,” arXiv preprint arXiv:1606.01935, 2016.

[25] P. Mills, “Extensions to guided local search,” Ph.D. dissertation,Citeseer, 2002.


Recommended