+ All Categories
Home > Documents > Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish...

Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish...

Date post: 23-Sep-2020
Category:
Upload: others
View: 0 times
Download: 0 times
Share this document with a friend
9
Area Decomposition, Partition and Coverage with Multiple Remotely Piloted Aircraft Systems Operating in Coastal Regions Fotios Balampanis, Ivan Maza and Anibal Ollero Abstract— Several approaches can be identified in the liter- ature for area decomposition: Grid based methods, Cellular decomposition and Boustrophedon (or Morse) decomposition. This paper proposes a novel discretization method for the area based on computational geometry approaches. By using a Constrained Delaunay Triangulation, a complex area is seg- mented in cells which have the size of the projected field of view of the RPAS (UAS) on-board sensor. In addition, costs are associated to each of these cells for both partitioning the area among several RPAS and full covering each sub- area. Simulation results show that this approach manages to optimally decompose and partition a complex area, and also produces a graph that can be used for different cost-based path planning methods. Index Terms— Area decomposition, Area partition, Coverage planning, Constrained Delaunay triangulation I. I NTRODUCTION Area decomposition for path planning in robotics is a widely researched subject in coverage and tracking tasks, by formulating a path formation strategy in a given area. As in many motion planning algorithms, two approaches can be found: discrete and continuous search algorithms. Each of these cases has trade-offs; complexity and optimality to name a few. In discrete search domain, most of the research divides the problem in two parts: Area segmentation and path traversing through the segmented areas. This might look as a simplistic description as there are numerous studies that take into consideration variables such as energy consumption [1], belief maps [2] or uncertainties in motion [3]. Still, this reductionism is valid as all these variables are actually parts of either the aforementioned processes. In our case and for the reasons discussed below, a discrete, grid-like algorithm for area segregation, and known algorithms combination for path planning have been chosen. The paper is organized as follows. A short introduction including related work is presented in Sect. II, presenting decomposition strategies and algorithms for path planning for single or multiple RPAS. Section III introduces the reasons for the approach followed in this paper, indicating coastal area specific attributes. Section IV presents the computa- tional geometry tools which are used for area segregation. This work is partially supported by the MarineUAS Project funded by the European Union’s Horizon 2020 research and innovation programme, under the Marie Sklodowska-Curie grant agreement No 642153 and the AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision and Control Group, University of Seville, Avda. de los Descubrim- ientos s/n, 41092, Sevilla, Spain. Email: [email protected], [email protected], [email protected] Section V describes the cost/weight attribution algorithms that are used for path related information propagation. In Sect. VI, the two main tasks of full coverage and shortest path are studied. Section VII presents the qualitative results of the paper and Section VIII closes the paper with the conclusions and future steps of our approach. II. RELATED WORK Most of the current approaches divide the path planning problem in two parts. Initially an area decomposition strategy is performed, reducing the complexity of the configuration space or simplifying the area to be visited. In that manner, the robotic agent has discrete states that can be reached by choosing different actions. Different decisions are required for tasks such as coverage or tracking, and cost-based path planning algorithms can be applied for reaching each state. Numerous studies have extended these algorithms for multi- ple robots, swarms or distributed systems. A. Area Decomposition Regarding the decomposition process, the following ap- proaches can be identified in the literature: Grid based meth- ods, Cellular decomposition and Boustrophedon (or Morse) decomposition. In grid based methods, which are also referred as “reso- lution complete methods” [4], the area partitioning is per- formed by applying a grid overlay on top of the area. In that way there is a discrete configuration space, where if all the cells are visited, then a complete coverage is assumed. The decomposition problem is somehow bypassed, as for areas that are partially covered by the cells, the vehicle might not cover them at all in order to avoid collisions, or will go over them, ignoring the fact that they are obstacles (see Fig. 1). The latter can be seen in [2], [5], where the RPAS flies over cells that are partially inside the area of interest. In [6] even though triangles are used for the area de- composition as in the current study, the same problem is still present. Changing the shape of the cells but arranging them regardless of the area to be decomposed, the problem of having cells which are partially outside the domain of interest still remains. In general, all grid based methods tend to “pixelate” the area, a strategy that does not properly solve real world scenarios; areas do not have rectangular borders, neither do their obstacles and no-fly zones. Galceran et al. states in [4] that this problem can be solved by, locally or globally, increasing the resolution of the grid, creating smaller cells. This actually does not solve the problem in
Transcript
Page 1: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Area Decomposition, Partition and Coverage with Multiple Remotely PilotedAircraft Systems Operating in Coastal Regions

Fotios Balampanis, Ivan Maza and Anibal Ollero

Abstract— Several approaches can be identified in the liter-ature for area decomposition: Grid based methods, Cellulardecomposition and Boustrophedon (or Morse) decomposition.This paper proposes a novel discretization method for thearea based on computational geometry approaches. By usinga Constrained Delaunay Triangulation, a complex area is seg-mented in cells which have the size of the projected field ofview of the RPAS (UAS) on-board sensor. In addition, costsare associated to each of these cells for both partitioningthe area among several RPAS and full covering each sub-area. Simulation results show that this approach manages tooptimally decompose and partition a complex area, and alsoproduces a graph that can be used for different cost-basedpath planning methods.

Index Terms— Area decomposition, Area partition, Coverageplanning, Constrained Delaunay triangulation

I. INTRODUCTION

Area decomposition for path planning in robotics is awidely researched subject in coverage and tracking tasks,by formulating a path formation strategy in a given area. Asin many motion planning algorithms, two approaches canbe found: discrete and continuous search algorithms. Eachof these cases has trade-offs; complexity and optimality toname a few. In discrete search domain, most of the researchdivides the problem in two parts: Area segmentation and pathtraversing through the segmented areas. This might look asa simplistic description as there are numerous studies thattake into consideration variables such as energy consumption[1], belief maps [2] or uncertainties in motion [3]. Still, thisreductionism is valid as all these variables are actually partsof either the aforementioned processes. In our case and forthe reasons discussed below, a discrete, grid-like algorithmfor area segregation, and known algorithms combination forpath planning have been chosen.

The paper is organized as follows. A short introductionincluding related work is presented in Sect. II, presentingdecomposition strategies and algorithms for path planning forsingle or multiple RPAS. Section III introduces the reasonsfor the approach followed in this paper, indicating coastalarea specific attributes. Section IV presents the computa-tional geometry tools which are used for area segregation.

This work is partially supported by the MarineUAS Project funded bythe European Union’s Horizon 2020 research and innovation programme,under the Marie Sklodowska-Curie grant agreement No 642153 and theAEROMAIN DPI2014-C2-1-R Spanish project.

Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics,Vision and Control Group, University of Seville, Avda. de los Descubrim-ientos s/n, 41092, Sevilla, Spain. Email: [email protected], [email protected],[email protected]

Section V describes the cost/weight attribution algorithmsthat are used for path related information propagation. InSect. VI, the two main tasks of full coverage and shortestpath are studied. Section VII presents the qualitative resultsof the paper and Section VIII closes the paper with theconclusions and future steps of our approach.

II. RELATED WORK

Most of the current approaches divide the path planningproblem in two parts. Initially an area decomposition strategyis performed, reducing the complexity of the configurationspace or simplifying the area to be visited. In that manner,the robotic agent has discrete states that can be reached bychoosing different actions. Different decisions are requiredfor tasks such as coverage or tracking, and cost-based pathplanning algorithms can be applied for reaching each state.Numerous studies have extended these algorithms for multi-ple robots, swarms or distributed systems.

A. Area Decomposition

Regarding the decomposition process, the following ap-proaches can be identified in the literature: Grid based meth-ods, Cellular decomposition and Boustrophedon (or Morse)decomposition.

In grid based methods, which are also referred as “reso-lution complete methods” [4], the area partitioning is per-formed by applying a grid overlay on top of the area. In thatway there is a discrete configuration space, where if all thecells are visited, then a complete coverage is assumed. Thedecomposition problem is somehow bypassed, as for areasthat are partially covered by the cells, the vehicle might notcover them at all in order to avoid collisions, or will go overthem, ignoring the fact that they are obstacles (see Fig. 1).The latter can be seen in [2], [5], where the RPAS flies overcells that are partially inside the area of interest.

In [6] even though triangles are used for the area de-composition as in the current study, the same problem isstill present. Changing the shape of the cells but arrangingthem regardless of the area to be decomposed, the problemof having cells which are partially outside the domain ofinterest still remains. In general, all grid based methods tendto “pixelate” the area, a strategy that does not properly solvereal world scenarios; areas do not have rectangular borders,neither do their obstacles and no-fly zones. Galceran et al.states in [4] that this problem can be solved by, locallyor globally, increasing the resolution of the grid, creatingsmaller cells. This actually does not solve the problem in

Page 2: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Fig. 1. Problematic setup for grid decomposition methods. Should mostof the cells in the borders be visited? This can also occur with polygonalborders.

Fig. 2. Complete coverage is not possible applying lawnmower motions.Path is represented in black, the covered area in green and the areas out ofthe field of view are shown in gray.

our case, since the size of the projected field of view of theRPAS sensor should be considered. An exact decompositionin cells which the vehicle can visit, increases computationalefficiency and ensures full coverage. If the cells are tobe smaller, there is no guarantee that a feasible path forcomplete coverage can be produced.

In cellular decomposition, a non-convex polygon area isdecomposed into a collection of convex polygons. Eventhough it manages to decompose complex areas, it does notconsider obstacles [7] within the area or it assumes that theresulting cells will be covered by simple and coherent backand forth lawnmower motion. This is not always the case asit can be seen in Fig. 2.

In Boustrophedon decomposition [8] the problem of onlyproducing many convex polygons without holes has beenaddressed. In this strategy and its derivatives, critical pointsin the area are found and are used as crucial points inorder to divide the area in cells. Even though this method iswidely used, it still does not address the problem of coveringthe whole area, as simple boustrophedon movements of thevehicles might not be able to cover complex spaces, as itwas shown before in Fig. 2.

In almost all of the above cases, it can be noticed that thesurrounding area is rectangular and fairly simple. Also inmost of them a coverage by lawnmower paths is considered,assuming that the vehicles will always be able to compensate

uncertainties in motion or external factors like wind drifts foraerial vehicles, or surface anomalies for ground vehicles. Asit can be seen in Fig. 3, coastal areas are usually far fromsimple, convex polygons.

B. Path Planning

Two main types of path planning algorithms can be foundin the literature: Those that perform complete coverage andothers that try to find the shortest path to fulfill a specificgoal. Another type is intended to create a path in order todiscover a target, but this can be considered as a combinationof the full coverage along with a weighted shortest pathcase. In all these types, both continuous and the discreteapproaches can be found.

In order to cover an area, usually a back and forth motionstrategy is applied. This is either referred as “lawnmowerpath”, “zigzag” or boustrophedon motion. Several derivativesexist, including the minimization of turns [7], [9] and otherelaborated algorithms, especially when using grid segregatedareas. A musical Harmony approach [5] and fractal trajecto-ries (using Hilbert curves) [10] are to name a few. There havealso been efforts for sensor-driven paths [11] in order forthe agent to increase the information gain from its sensors,instead of trying to preplan a solution.

Many known algorithms have contributed to the shortestpath problem. Every cell of a segmented space is consideredas a node in a, usually, undirected graph [5]. The verticesbetween these nodes might or might not have weights, whichdescribe the transition cost. In literature, cost or weights areapplied depending on e.g. the distance from the target orfrom the information gain that might have [12].

C. Multiple vehicles

In this context, efforts have been made to extend thesolutions for multiple vehicles. Increase in computationalcapabilities and simultaneous decrease of cost have allowedthe use of multi-vehicle systems to distribute the aforemen-tioned computational burden. Acevedo et al. in [13]–[15]have introduced a decentralized algorithm for surveillancetasks which manages to propagate all task related informationbetween agents, considering the communication constrainsbetween them. Kassir et al. in [12] also takes into accountthe communication resources that each of the agent has inorder to increase the information gain of the whole system.Finally, in [2] Berger et al. propose a mixed-integer linearprogramming formulation for discrete target searching formultiple robots.

III. COASTAL AREAS

Applying algorithms in real world situations often meansthat the application will highlight problems that have tobe globally addressed by the method. Instead of trying togeneralize a simple shape decomposition algorithm, in ourwork it is assumed that coastal areas are complex. Theirborders have few straight lines, in many cases no-fly zonesexist (e.g. residential or commercial zones) and, as it is clear

Page 3: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Fig. 3. Salamina area in Greece. Numerous residential, commercial andmilitary regions in a 12 Km x 12 Km area. A complex shape is defined tosegment and cover.

in Fig. 3, it is requested to deal with non-convex polygons,and numerous no-fly zones.

Coastal areas have distinguishable properties that permitthe use of additional constraints in the algorithmic formu-lation. As starting point, in scenarios involving flights overthe sea, it is safe to assume that except obstacles and no-fly zones, the remaining area is flat, having zero altitude.Moreover, flying over the sea means that maintaining aconstant altitude, the field of view of a camera or in generalthe footprint of a sensor, is always relatively stable. Thisallows the segmentation of the area in a least sum offootprint-sized cells to have a complete coverage.

IV. AREA DECOMPOSITION

The aforementioned issues of complex areas and optimaldecomposition have been addressed in our work by applyinga refined constrained Delaunay polygon triangulation, creat-ing a mesh of triangles, each one having its sides equal tothe size of the footprint of the RPAS. In that way, there isno segmentation outside the area of interest and by visitingall triangles it can be greedily assumed that the whole areahas been visited. From the implementation point of view, theConstrained Delaunay Triangulation and mesh generation ofthe CGAL library [16] have been used, introducing a toplimit of vertex side of each triangle: the footprint of theRPAS(s) as in Fig. 4. As the triangles should be as muchhomogeneous as possible, a lesser angle of 35 degrees istested.

A. Constrained Delaunay triangulation

Basic triangulation has no control on the resulting shape[17] and produces skinny triangles having extreme angles. Incontrast, a Delaunay triangulation DT(P) is a collection oftriangles in which all points P = {p1, p2, . . . , pm−1, pm} thatbelong to P are not inside the circumcircle of any triangle inthis set. This method, tends to maximize the minimum angleof all the triangles in the triangulation, thus providing more

Fig. 4. Projected field of view of the sensor on-board. The largest sideis labelled as d, whereas wp denotes the computed waypoint. Dashed lineshows the cell generated by the triangulation.

homogeneous triangles. Moreover, connecting the centers ofthe circumcircles, produces a geometric dual of a Voronoidiagram. A Constrained Delaunay Triangulation CDT(P)introduces forced edge constraints which are part of theinput, and are useful in this paper as they actually define theboundaries of the polygonal area. This can also be appliedfor holes inside the polygon. By using this method a mesh(grid) of regions of attribute is created.

B. Mesh

A mesh is a partition of an area into shapes which satisfyseveral criteria. The mesh is defined as a graph of verticesthat can either be disjoint or share an edge. These verticescan either be part of the boundary of an area or internalconstraints of the meshed region. In the CGAL library usedin the implementation, the user can define seed points thateither mark regions to be meshed or regions not to bemeshed. In the latter case, the whole constrained region isinitially triangulated but not meshed. The applied meshingalgorithm constantly inserts new vertices to the DelaunayTriangulation as far as possible from the other vertices, andstops when the refinement criteria are met. The algorithmis guaranteed to terminate satisfying the requested criteria“if all angles between incident segments of the input planarstraight line graph are greater than 60 degrees and if thebound on the circumradius/edge ratio is greater than

√2”

[18].In order to perform these operations on the polygo-

nal coastal area, the Triangulation data structure alongwith the Constrained Delaunay Triangulation operation, pro-duce the Constrained Delaunay Triangulation Data structure(CDT DS). It includes a collection of all the edges andvertices of the triangulation as well as user defined attributes.The Delaunay mesher (CDT M) of the CGAL library isresponsible for the refinement of the triangulation in orderfor the mesh to be produced. Moreover, each produced facet(cell) has also attributes, e.g its neighbors. It is possible toinclude additional properties such as weights related to the

Page 4: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Fig. 5. Antagonizing wavefront propagation, partitioning an area as anundirected graph. Starting from initial positions, numbered as 1, in eachiteration of the algorithm each node gets an agent ID (color coded) and astep cost. Numbers indicate the absolute distance from the initial positionof the agent.

information gain, distance costs, etc. that are discussed inthe next section.

V. COST ATTRIBUTION

As it was previously stated, the whole region C, includingobstacles, is treated as a two dimensional configuration spacesuch that C = R2, where Cobs is the obstacles region andCf ree = C \Cobs is the region where the vehicle can actuallymove. By segregating Cf ree for M vehicles in a sum of cells(ψ) such that

Cf ree =M∑i=1

N∑j=1

ψi j , (1)

the motion planning problem can be reduced to a graphsearch problem of N nodes organized in the ConstrainedDelaunay Triangulation Mesh (CDT M). In each of the cellsof the segregated free space, it is possible to associate severalinformation attributes, also referred as weights or costs, thatare task specific, such as information gain, transition cost,distance from the target, remaining fuel for the vehicle, etc.In that manner, each node of the graph will contain areaand agent related information. By reducing the search spaceof the input (cell-sized information) and of the output (Ncells), computational complexity is also reduced. Moreover,matching the vehicle’s footprint with the resolution chosenfor the area, a tight coupling is achieved between the positionof information in the configuration space CDT M and thecapabilities of the RPAS. Weight or cost depends on theinitial positions of the vehicles, the morphology of the areaas well as several information attributes that one might wantto include depending on the task to be executed by the RPAS.

In an initial step, a hop and transition cost computationalgorithm is applied. This algorithm is actually an antago-nizing wavefront propagation implementation for calculat-ing the number of steps from the initial positions Z =

{z1, z2, . . . , zM } of M vehicles, to the borders of the con-figuration space, getting from every current ψ cell to all itsunvisited neighbors Qψ = {qψ1, qψ2, qψ3}. A neighbor q ofa cell, is considered every cell that is adjacent to one of itssides. As so, every cell has three neighbors. Antagonizing

Algorithm 1: Triangulation and hop cost algorithm. ψrepresents a cell, q denotes a neighbor and Dc is thecurrent depth placeholder.

Data: CDT DS Constrained Delaunay TriangulatedData Structure, CDT M ∈ Cf ree ConstrainedDelaunay Triangulation Mesh, ψZ initialpositions of agents

Result: CDT M with hop depth cost information,SM configuration space for M agent,Dhψ hop depth information for each cell ψ,Aψ agent id for each cell ψ,Bψ branch agent id for each cell ψinitialization;i ← 0;Dc ← 1;foreach ψZ do

Aψ ← i;Dhψ ← Dc;i ← i + 1;

endi ← 0;foreach q ∈ ψZ do

Bq ← i;i ← i + 1;

endwhile all ψ ∈ CDT M are not visited do

Dc ← Dc + 1;foreach ψ ∈ CDT DS do

if ψ ∈ CDT M AND ψhasDepth = true ANDDhψ , Dc AND ψisVisited = f alse thenψisVisited ← true;foreach q ∈ ψ do

if q ∈ CDT M AND qhasDepth = f alsethen

Dhq ← Dc;qhasDepth ← true;qA← Aψ;qB ← Bψ;

endend

endend

end

means that each step of the cost-to-go operation is performedbreadth-first, resulting in partitioning the whole area inseparate configuration spaces for each RPAS. Consider thesum of triangulated cells as an undirected graph, where everycell has at most three child nodes; it’s neighbors. Then, eachRPAS is assigned a node as a starting position. In each stepof the algorithm, the children of the nodes visited in theprevious step, are given a increasing step cost (also referredas hop cost) and are assigned the ID of the RPAS. Since anode cannot be reassigned a RPAS ID or a new transitioncost, the whole area is partitioned in a uniform manner (see

Page 5: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Fig. 5). In this manner, each vehicle is assigned its ownconfiguration space S ⊂ Cf ree such that for the M-th RPAS

SM =N∑j=1

ψMj + zM , (2)

which is distance-optimal to the rest. This operation alsofacilitates coverage tasks as it segregates the whole space inrelatively equal partitions between the agents. This hop depthcost Dh attribution is performed by using Algorithm 1, whichmakes the depth assignment for M active vehicles, while it’siterations resemble a breadth-first search pattern.

Moreover, another step is performed to ease path planningfor shortest path to the targets. In the initial phase of thealgorithm, each of the three neighbors of the Z startingcells, is assigned a branch agent identifier B so that forevery agent there are three qz neighbors of the initial positionz with distinct identifiers. These identifiers are propagatedfor each of the hops of the algorithm. Then, every cell inthe configuration space also includes an origin informationwhich acts as a kind of feedback planning. In that way, thestate space is limited for the next steps, when the search fora goal facet is performed, as it will be described in the nextsection.

Finally, in order to facilitate complete coverage of the area,another algorithm is performed after the triangulation. A cellψN might be adjacent to the borders of the area or to othervehicles’ area. Thus, some of its Qψ neighbors are eitheroutside of the domain or belong to another vehicle. For everyqψ < CDT M∪SM , cell ψ is given a large coverage depth costDc . Every other cell that is adjacent to those ψ cells, is givena smaller cost and so on. In that way, a watershed informationschema is created by which all cells of the M-th RPAS in itsconfiguration space SM will be visited, from the borders tothe core (see Fig. 6). Moreover, this cost attribution enablesthe partitioning of the whole region, as the cells which havethe specific border cost indicate either borders of the area ofinterest or borders between the vehicles. This computationis described in Algorithm 2.

VI. PATH PLANNING

As it was mentioned before, two main path planning casescan be distinguished. Covering the whole area and reaching(or searching) a goal cell ψG . In order for the vehiclesto perform these actions, the centroid of each triangle isconsidered as a waypoint for the path LM . By applyingdiscrete space path planning strategies, depending on thecost/weight attributes of the mesh, a path is created. Itshould be noticed that using this decomposition strategy andcost attribution for path generation, RPAS kinematics andwaypoint to waypoint trajectory planning are not considered;they are handled by the autopilot software and hardware,as described in Section VII. The proposed solution assumescomplete coverage but not optimality in trajectory planning.

A. Coverage path planningBy using this kind of segmentation, complete coverage is

performed when every centroid of the configuration space

Algorithm 2: Complete coverage algorithm. ψ representsa cell and q denotes a neighbor. On the other hand, Di

is the coverage depth iterator, Ds represents the smallestcoverage depth placeholder and Dg is the greatest cover-age depth placeholder. A Vector is populated by the cellsclosest to current one. Procedure sort sorts the items ofthe vector, smallest (closest) first. Procedure pop returnsthe first item in the vector.

Data: CDT DS Constrained Delaunay TriangulationData Structure, CDT M Constrained DelaunayTriangulation mesh,

SM configuration space for M-th RPAS,ψZ initial position of RPASResult: Dcψ coverage depth cost,LM path of the M-th RPASinitialization;Dg ←∞;Di ← Dg;LM ← ψZM

;foreach ψ ∈ CDT M do

if qψ < CDT M OR qψ < SM thenDcψ ← Di

endendwhile all ψ ∈ CDT M are not visited do

foreach ψ ∈ CDT M doif Dcqψ = Di then

Dcψ ← Di − 1end

endDi ← Di − 1

endDs ← Di

Di ← Dg;ψcurrent ← ψZM

;while Di > Ds do

foreach ψ ∈ CDT M doif Dcψ = Di then

distance← calcDistance(ψ, ψcurrent )pair ← makePair(ψ, distance)Vectorclosestψ ← pairf ound ← true

endendsort(Vectorclosestψ)ψcurrent ← pop(Vectorclosestψ)LM ← ψcurrent

if f ound = f alse thenDi ← Di − 1

endend

Page 6: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Fig. 6. Triangulation and mesh for three agents. White cells are the initialpositions for the vehicles. Red lines indicate the partitioned configurationspace for each agent. The green line indicates the coverage path for oneof them; it’s computed by taking advantage of the border-to-center cost,indicated in different color shades.

of an agent is visited. Naturally, trying to find a pathwhere all the points are visited exactly once resemblesthe travelling salesman problem. Taking advantage of thewatershed algorithm of the previous section, where everycell that is close to a border is given a high coverage costwhereas the cost is reduced for the internal cells, a graph ofdistance cost from the edges of the domain and the othervehicles is created. Starting from the initial cell ψZ andtraversing to the nearest border cell, in every iteration ofthe algorithm, all cells that have a specific depth cost arevisited and then the depth to be visited is decreased byone. In each of these iterations, each next step is chosenbased on the distance from the current one and the closestis selected, resembling a nearest neighbor algorithm (seeAlgorithm 2). These iterations manage to create an inwardspiral path that covers the partitioned area (see Fig. 6),creating relatively smooth trajectories, minimizing turns andsudden heading changes. Once again, this procedure does notguarantee optimality but complete coverage.

B. Cost based path planning

In a first step, the mesh is preprocessed without knowingthe target cell. Applying the hop cost attribution in everyfacet of the triangulation permits the system to know whichinitial branch of the graph has managed to reach the goalcell, working as a kind of feedback planning. Each facethas a reverse cost-to-go value, starting from the goal andreaching the initial position. As it was stated before in thejump cost part of the initial algorithm, a propagation of thebranch agent is performed. By decreasing the depth searchone step at a time, while remaining in the same branchagent, the shortest cost-to-go path is found. Let us consider apotential function φ(x) that tries to reduce the potential from

any cell to the goal cell inside the state space. In this casethis function tries to choose the action u (move to neighbor)

u = arg minu∈U(x)

{φ( f (x, u))} (3)

of the current cell which is closer to the target and has thesame branch agent identifier. Since from every cell of thisbranching agent, the goal is reachable, this potential functionis actually a navigation function. It is φ(x) = ∞ for every xthat belongs to another branch agent and for every state x ∈X/XG the algorithm produces a state x ′ for which φ(x ′) <φ(x). This method is described in detail in Algorithm 3.

Algorithm 3: Shortest path algorithm. ψ denotes a celland q is a neighbor. On the other hand, Bi representsthe branch agent placeholder and Di is the hop depthplaceholder.

Data: CDT DS Constrained Delaunay TriangulationData Structure, CDT M Constrained DelaunayTriangulation mesh,

ψZ initial position of the RPAS,Dhψ hop depth cost from Algorithm 1,ψGM goal cell for the M-th RPAS,Bψ branch agent id for each cell ψ as in Algorithm 1.Result: LM path of the M-th RPASinitialization;Bi ← BψGM

Di ← DhψGM

ψcurrent ← ψGM

LM ← ψcurrent

while Di > 0 doDi ← Di − 1foreach q ∈ ψcurrent do

if q ∈ CDT M AND Bq = Bi AND Dhq = Di

thendistance← calcDistance(q, ψZ)pair ← makePair(q, distance)Vectorclosestψ ← pair

endendsort(Vectorclosestψ)ψcurrent ← pop(Vectorclosestψ)LM ← ψcurrent

end

VII. RESULTS

In order to validate the algorithms and compare them withother approaches for area segmentation and path planning, asetup based on a SITL (Software In The Loop) simulationenvironment has been used. The algorithms are implementedin a ROS1 (Robot Operating System) listener which visual-izes the given area, the segmentation, the area of interestfor each RPAS and applies a color coding for visualizingthe depth information as well as the generated paths. The

1http://www.ros.org/

Page 7: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Fig. 7. System Architecture. Based on ROS, the system is highly modularand can be integrated with current and future applications.

latter are transmitted in a form of a waypoint list at the SITLinstance of each of the vehicles, and each flight is monitoredby using a ground control station (qgroundcontrol2) whichreceives emulated data from the Arduplane3 autopilot soft-ware. Arduplane is a fixed wing simulation instance of theardupilot software, emulating kinematics of a small RPASand physics of the environment.

A. System Configuration and Architecture

The system consists of an Intel Core [email protected] having 8GB of RAM and using the kUbuntu 14.04distribution of the Linux OS. The latest CGAL library (4.7)[19], ROS Indigo [20] (along with the rviz package forvisualization), Arduplane SITL and qgroundcontrol for thequalitative validation of the flight have been used in ourimplementation. Figure 7 shows the system architecture.

B. Qualitative results

A thorough testing of CGALs algorithm has been per-formed, iterating through angle bound constrains for theproduced cells. Resulting meshes have been examined, ob-taining information about triangle homogeneity. Regardingpath planning, two scenarios of coverage and shortest pathhave been tested in order to obtain information about theproduced trajectories.

Area decomposition: A non concave polygon with aninternal no fly zone was used as it can be seen in Fig. 6. Thesimulated shape is a 5km x 5km area, scaled down by a 1/10factor for visualization and notation purposes. In most cases,CGALs implementation of the triangulation has managedto produce a mesh consisting of homogeneous triangles.The triangulation and meshing functions were tested bygiving several input constrain criteria for the lesser anglebound constrain of the produced triangles, as described in

2http://qgroundcontrol.org/3http://ardupilot.com/

TABLE ICONSTRAINED DELAUNAY TRIANGULATION PERFORMANCE BASED ON

LOWER ANGLE BOUND CONSTRAINT.

Anglebound (deg)

Vertices Cells Min edgelength (m)

Min angle(deg)

0.011-8.88 304 476 10 259.16-10.02 305 478 10 2512.31 313 488 10 2915.29 321 500 9.43 3116.15 325 508 8.24 3217.01 357 568 6.32 3218.16 377 597 6.32 34

Section IV. Produced vertices, total cells, minimum resultingedge and minimum resulting angle are presented in Table I.In all trials, the input criteria for the edge size has remainedthe same; a predefined size for the RPASs footprint, asdefined in Fig. 4, of 25 meters (250 meters in the actualarea). It is noted that the sizes in these simulated shapes aremerely indicative and are chosen for visualization purposesin the rviz node. It is also noted that reaching the upper angleconstrain limits of the Delaunay triangulation, an increasednumber of sub-optimal cells were produced near the no flyzone. Moreover, the algorithm could not proceed further thanthe angle bound constraint of 18.9 degrees, a size whichis consistent to the one being referred to CGAL notes.Nevertheless the footprint criteria is always met, as in allproduced triangles at least one side has the edge size inputconstrain.

Coverage: Figure 6 shows the area to be segmentedby three agents. The path shown is the full coverage pathfor the selected agent. Initially the shape has 21 vertices.Performing the initial Delaunay triangulation, the shape wassegregated using 52 vertices. After the Constrained DelaunayTriangulation, 305 vertices are present, forming 477 cells(both inside and outside the domain of interest). The size ofthe footprint for every RPAS, that was set as a constrain tothe input of the triangulation algorithm, was once again setto 25 meters. For the RPAS shown, the area consisted of 153cells with a biggest hop depth of 26 cells and 7 separate fullcoverage depth zones. The computational time for the sumof operations was 0.23 seconds.

Shortest Path: The same area and restrictions wereassumed for the shortest path task. The footprint input forthe triangulation was decreased to 20 meters (200 metersin actual size) and the starter and goal cells were pickedat random but had not to be visible from each other (seeFig. 8). Using the feedback planning strategy, the algorithmmanaged not to reach a dead-end state. For the depictedresult, a slightly different approach was used. Every iterationof the algorithm, considers part of the path every fourthcell during computation. In that manner, a slightly smoothertrajectory is produced. For the path shown, the hop depthwas 44 cells whereas the target cell was in hop depth 41.The computational time for the sum of operations was 0.19seconds.

Page 8: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

Fig. 8. Shortest path, pruning waypoints to produce a smoother trajectory.White cell is the initial position of the RPAS whereas ’x’ marks the targetcell. In every four cells of the calculation, a new waypoint is introduced.Coloring indicates the increasing distance to the target, depicting thedistance cost attributed by Algorithm 1. Figure also shows all the waypointsproduced by the triangulation algorithm.

TABLE IISQUARE GRID AND CONSTRAINED DELAUNAY TRIANGULATION

DECOMPOSITION METHODS COMPARISON.

FootprintSize (m)

Total cells(square)

Total cells(CDT)

Partialcoverage(square)

15 331 1395 12520 211 784 9925 138 476 7330 104 352 64

Comparison with other methods: The same shape thatwas used for the simulated experiments has been segmentedby using a simple square grid decomposition strategy as inthe works that have been presented in [4]. A footprint size of15, 20, 25 and 30 meters has been tested in order to observethe amount of cells that partially cover the region of interestby that method. Note that using the strategy presented by thispaper, there are no cells that partially cover an area, and thewaypoints produced are always in a path inside the domain.Results in Fig. 9 show that even though this method managesto produce a lesser amount of cells, it’s performance is highlydependent on the shape of the area, the restricted zones asalso as the resolution of the footprint. Comparison of thetwo methods in the 25 meters sized footprint as presentedin Table II, show that half of the produced cells share aregion outside the domain of interest, raising questions onthe restrictions that have to be applied in order to either visitor not those specific cells.

VIII. CONCLUSIONS AND FUTURE WORK

A novel way of segmenting an area into cells that areconsistent with the capabilities of a system with multipleRPAS has been presented. This segregation of the state

Fig. 9. Grid decomposition performance. In the 25 meters footprint case,out of the 138 total cells that cover the area, 73 cells are partially insidethe domain of interest.

space can manage to decrease the complexity of informationpropagation among several aircraft. In this architecture thegrid serves the role of a two dimensional information cloud,containing attributes of the task to be performed.

Regarding kinematics, it is clear that this strategy can beapplied for holonomic vehicles that are able to turn instantlyand have the ability to hover, like multi-rotors. On the otherhand, non-holonomic vehicles like fixed wing RPASs havesome restrictions which depend on the speed of the RPASand the distance between the waypoints. There is a lowerlimit in distance between waypoints for a non-holonomicvehicle that can be reduced considering a waypoint visitedif the RPAS passed by an acceptable distance from it.Nevertheless this is an application specific solution that isnot generic; in this study, these kind of strategies are dealtby the autopilot software. Once again the goal of this paper isto provide an optimal decomposition for complete coverage,not to produce an optimal path and waypoint to waypointflight dynamics.

There are still numerous issues to be addressed and severalfeatures to be introduced. At first, optimization techniques,e.g Lloyd’s algorithm, can manage to produce even morehomogeneous meshes. Moreover, addition of ability-awaresegregation of space in order to reconfigure the domainsize of each vehicle is needed. This can be performed byexchanging border cells between agents. Path planning issueslike segments produced outside the domain have to be solvedby tagging those cells beforehand and producing a path thatmanages to either address them during the initial pass orin the end. Moreover, several shortest path algorithms haveto be tested in order to improve the produced trajectories,as current realization even though guarantees a non dead-end path there is no pruning of states/waypoints to producesmaller paths. Dynamic replanning in case a new obstacle ispresent or information exchange between the RPAS are alsoissues to be addressed. Finally, the proposed method will becompared with several other segmentation methods, in orderto produce some performance metrics.

In the next research steps, real world experiments by

Page 9: Area Decomposition, Partition and Coverage with Multiple ... · AEROMAIN DPI2014-C2-1-R Spanish project. Fotios Balampanis, Ivan Maza and Anibal Ollero are with the Robotics, Vision

using fixed-wing vehicles will be executed. Ideally, thesystem will be able to perform computations on-board eachaircraft. Moreover, introduction of uncertainty analysis forflight trajectories and testing on how to compensate withadditional measures that either belong to known factors orunexpected ones should be considered. Finally, our researchwill introduce a proposal of a complete mixed discrete-continuous decision architecture for heterogeneous systemsthat are able to perform in coastal areas with minimalsupervision.

REFERENCES

[1] C. D. Franco and G. Buttazzo, “Energy-Aware Coverage PathPlanning of UAVs,” 2015 IEEE International Conference onAutonomous Robot Systems and Competitions, pp. 111–117, 2015.[Online]. Available: http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=7101619

[2] J. Berger and N. Lo, “An innovative multi-agent search-and-rescuepath planning approach,” Computers & Operations Research, vol. 53,pp. 24–31, 2015. [Online]. Available: http://linkinghub.elsevier.com/retrieve/pii/S0305054814001749

[3] S. Gade and A. Joshi, “Heterogeneous UAV swarm system for targetsearch in adversarial environment,” 2013 International Conference onControl Communication and Computing, ICCC 2013, no. Iccc, pp.358–363, 2013.

[4] E. Galceran and M. Carreras, “A survey on coverage path planningfor robotics,” Robotics and Autonomous Systems, vol. 61, no. 12,pp. 1258–1276, 2013. [Online]. Available: http://dx.doi.org/10.1016/j.robot.2013.09.004

[5] J. Valente, J. D. Cerro, A. Barrientos, and D. Sanz, “Aerial coverageoptimization in precision agriculture management: A musical harmonyinspired approach,” Computers and Electronics in Agriculture, vol. 99,pp. 153–159, 2013.

[6] J. S. Oh, Y. H. Choi, J. B. Park, and Y. F. Zheng, “Complete coveragenavigation of cleaning robots using triangular-cell-based map,” IEEETransactions on Industrial Electronics, vol. 51, no. 3, pp. 718–726,2004.

[7] Y. Li, H. Chen, M. Joo Er, and X. Wang, “Coverage path planningfor UAVs based on enhanced exact cellular decomposition method,”Mechatronics, vol. 21, no. 5, pp. 876–885, 2011.

[8] H. Choset and P. Pignon, “Coverage Path Planning : The Boustrophe-don Cellular Decomposition,” Autonomous Robots, vol. 9, no. 3, pp.247–253, 1997.

[9] I. Maza and A. Ollero, Multiple UAV cooperative searching operationusing polygon area decomposition and efficient coverage algorithms,ser. Distributed Autonomous Robotic Systems. Springer Verlag, 2007,

vol. 6, pp. 221–230. [Online]. Available: http://www.springerlink.com/content/978-4-431-35869-5#section=288931&page=1&locus=0

[10] S. A. Sadat, J. Wawerla, and R. Vaughan, “Fractal Trajectories forOnline Non-Uniform Aerial Coverage,” pp. 2971–2976, 2015.

[11] L. Paull, C. Thibault, A. Nagaty, M. Seto, and H. Li, “Sensor-DrivenArea Coverage for an Autonomous Fixed-Wing Unmanned AerialVehicle,” IEEE Transactions on Cybernetics, vol. PP, no. 99,pp. 1–1, 2014. [Online]. Available: http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=6671976

[12] A. Kassir, R. Fitch, and S. Sukkarieh, “Communication-awareinformation gathering with dynamic information flow,” InternationalJournal of Robotics Research, vol. 34, no. 2, pp. 173–200, 2015.[Online]. Available: <GotoISI>://WOS:000349144800003

[13] J. J. Acevedo, B. C. Arrue, J. M. Diaz-Bañez, I. Ventura, I. Maza,and A. Ollero, “One-to-one coordination algorithm for decentralizedarea partition in surveillance missions with a team of aerial robots,”Journal of Intelligent and Robotic Systems: Theory and Applications,vol. 74, no. 1-2, pp. 269–285, 2014.

[14] J. Acevedo, B. Arrue, J. Diaz-Banez, I. Ventura, I. Maza, andA. Ollero, “Decentralized strategy to ensure information propagationin area monitoring missions with a team of UAVs under limitedcommunications,” in Proceedings of the International Conferenceon Unmanned Aircraft Systems (ICUAS 2013), 2013, pp. 565–574.[Online]. Available: http://dx.doi.org/10.1109/ICUAS.2013.6564734

[15] J. Acevedo, B. Arrue, I. Maza, and A. Ollero, “Cooperative perimetersurveillance with a team of mobile robots under communicationconstraints,” in Proceedings of the IEEE/RSJ International Conferenceon Intelligent Robots and Systems, Tokyo, Japan, 2013, pp. 5067–5072.[Online]. Available: http://dx.doi.org/10.1109/IROS.2013.6697089

[16] L. Rineau, “2D conforming triangulations and meshes,” in CGALUser and Reference Manual, 4.7 ed. CGAL Editorial Board,2015. [Online]. Available: http://doc.cgal.org/4.7/Manual/packages.html#PkgMesh2Summary

[17] J.-d. Boissonnat, O. Devillers, S. Pion, M. Teillaud, and M. Yvinec,“Triangulations in CGAL,” Computational Geometry, vol. 22, no.1-3, pp. 5–19, may 2002. [Online]. Available: http://linkinghub.elsevier.com/retrieve/pii/S0925772101000542

[18] J. R. Shewchuk, “Delaunay refinement algorithms for triangular meshgeneration,” Computational Geometry, vol. 22, no. 1-3, pp. 21–74, 2002. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0925772101000475

[19] The CGAL Project, CGAL User and Reference Manual, 4.7 ed.CGAL Editorial Board, 2015. [Online]. Available: http://doc.cgal.org/4.7/Manual/packages.html

[20] M. Quigley, K. Conley, B. P. Gerkey, J. Faust, T. Foote, J. Leibs,R. Wheeler, and A. Y. Ng, “ROS: an open-source Robot OperatingSystem,” in ICRA Workshop on Open Source Software, 2009.


Recommended