Cooperative Task Assignment and Path Planning for ...

Cooperative Task Assignment and Path Planning for Multiple UAVs

63

Sangwoo Moon, David Hyunchul Shim, and Eunmi Oh

Contents 63.1 63.2 63.3 63.4 63.5

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1548 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1550 Integrated Hierarchical Structure for Multi-UAV Coordination . . . . . . . . . . . . . . . . . . . . . . . . . . 1551 Negotiation-Based Task Assignment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1552 Intersection-Based Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1554 63.5.1 Additional Procedures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1556 63.5.2 Overall Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1560 63.6 Potential Field-Based Collision Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1561 63.7 Simulation and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1564 63.7.1 Intersection-Based Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1564 63.7.2 Negotiation-Based Task Assignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1566 63.8 Flight Experiments and Validations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1568 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1576

Abstract

In this chapter, a hierarchical framework for path planning and task assignment for multiple unmanned aerial vehicles in a dynamic environment is presented. For multi-agent scenarios in dynamic environments, a candidate algorithm should be

S. Moon (!) Department of Aerospace and Mechanical Engineering, Korea Air Force Academy, Cheongju, South Korea e-mail: [email protected] D.H. Shim Department of Aerospace Engineering, Korean Advanced Institute of Science and Technology (KAIST), Daejeon, South Korea e-mail: [email protected] E. Oh CNS/ATM & Satellite Navigation Research Center, Korea Aerospace Research Institute, Daejeon, South Korea e-mail: [email protected] K.P. Valavanis, G.J. Vachtsevanos (eds.), Handbook of Unmanned Aerial Vehicles, DOI 10.1007/978-90-481-9707-1 82, © Springer Science+Business Media Dordrecht 2015

1547

1548

S. Moon et al.

able to replan for a new path to perform the given tasks without any collision with obstacles or other agents. The path-planning algorithm proposed here is based on the visibility and shortest-path principles in Euclidean space. Instead of typical visibility graph-based methods that scan through all nodes, A* algorithm is adopted to find an admissible path in a “best-first” approach during the search process. Since the direct outcome from such algorithms may not produce admissible paths in complex environments due to the problems including cul-desac, additional procedures are conceived to find a solution with a lower cost by avoiding local minima and eliminating any redundant nodes. The path planner is augmented with a potential field-based trajectory planner, which solves for a detouring trajectory around other agents or pop-up obstacles. Task assignment is achieved by a negotiation-based algorithm, which assigns a task with the lowest cost to each agent after comparing all task costs of all participating agents. These algorithms are implemented on MATLAB/Simulink, which can run with simulated vehicle models or actual UAVs through a communication network. In the simulations, the algorithms were validated to perform task assignment and path planning flawlessly. In actual flight tests, the proposed algorithms were tested with a number of fixed-wing UAVs in a fully realistic situation under various reality factors such as communication loss or tracking errors. The flight test shows, even in the presence of such uncertainties and logistic factors, the algorithms were able to perform all of the given tasks without any collision with other agents or obstacles.

63.1

Introduction

Unmanned aerial vehicles (UAVs) have been found highly effective in many applications where human presence in the aircraft is deemed unnecessary or dangerous. Furthermore, as the mission becomes more complicated, it has been suggested to perform the mission more effectively by “sharing the burden” among multiple UAVs. In such scenarios, UAVs are required to fly through the designated waypoints associated with tasks in the given mission. When tasks are performed by multiple agents, the question about “who is doing which task in what order” naturally arises. Also, when multiple UAVs fly together in a given area visiting waypoints, planning a safe and efficient path for each agent is very important. Finally, all agents should be able to avoid obstacles such as terrain or buildings by replanning the path dynamically. When planning for a mission of a UAV, the flight paths for all UAVs in the scenario are determined by assigning all tasks to participating agents. When the order of the tasks to be performed by each agent is decided, the path for each agent can be easily determined by simply connecting the waypoints associated with the tasks sequentially. However, when the mission area are filled with obstacles, the original path should be modified to pass through the designated waypoints while staying clear from the forbidden zones, which may be known a priori or suddenly “pop-up.” Also, when the UAVs are required to fly close to the ground, they need

63 Cooperative Task Assignment and Path Planning for Multiple UAVs

1549

to avoid collisions with ground objects or buildings, which are already mapped or detected by onboard sensors such as laser scanners or radars. Assigning tasks to UAVs is not a straightforward problem especially when the UAVs fly in a dynamic environment where obstacles are not fully mapped a priori or when the sensors for obstacle detection have errors. Mixed Integer Linear Programming (MILP) has been found as a powerful method because it can handle dynamic systems with discrete decision variables and there exist many efficient solvers. Bellingham used MILP for task assignment to handle waypoint visiting problems (Bellingham et al. 2001). However, since the complexity of the problem rapidly grows as the number of variables increases, it is often not suitable for realtime applications. Furthermore, since the MILP is formulated with object functions for the entire system, it needs to be implemented as a centralized system, which may not function well when the central control station or communication links are broken (Bertsekas 1991; Ren et al. 2007; Choi et al. 2009; Sujit et al. 2007; Viguria et al. 2010). Path planning is another important problem for efficient and collision-free operations for multiple UAVs. A path planner should satisfy completeness, optimality, computational tractability, and scalability. In addition, collision and threat avoidance is required for multi-agent scenarios and there are many algorithms for this problem. Many researches have been conducted on the path-planning problem since 1960s (Latombe 1991; Lavalle 2006; Hart et al. 1968; Redding et al. 2007; Yang et al. 2010). However, the path-planning problem becomes more complicated when solving for dynamic environments with incompletely known obstacles or pop-up threats. For such cases, the planner should be able to find a conflict-free path in real time. Schouwenaars et al. used mixed integer programming for multi-vehicle path planning (Schouwenaars et al. 2001). Richards showed an aircraft trajectory planning with collision avoidance using MILP (Richards et al. 2003). Also, some variations of MILP have been applied to this problem. Earl and D’Andrea developed an iterative MILP approach (Earl and D’Andrea 2005), and Jain and Grossmann proposed hybrid MILP/CP (constrained programming) models for a class of the optimization problems (Jain and Grossmann 2001). Chang and Shadden introduced the gyroscopic forces for collision avoidance, which is similar to the potential field approach (Chang et al. 2003). More recently, Shim et al. pioneered the dynamic path-planning problem using model predictive control (MPC) algorithm for a collision-avoidance problem of 16 homogeneous rotary UAVs with obstacles (Shim 2008). In this chapter, a hierarchical framework for efficient path planning and task assignment for multiple UAVs in dynamic environments is presented. For task assignment, a negotiation-based algorithm is proposed. In this approach, each agent initially chooses the task with the lowest cost, which is assumed directly proportional to the distance to the waypoint associated with a task. The choices of all other agents are notified to each agent, and when there are conflicts, i.e., when two or more agents choose the same task, their costs are compared, and the agent with the lowest cost are assigned with the task, while the remaining agents repeat the same negotiation process until all agents are assigned with a task without any

1550

S. Moon et al.

conflicts. The proposed algorithms are computationally light enough to handle realtime path planning in partially known environments. The task-assignment problem is essentially tightly coupled with the path planning. A path-planning algorithm based on the shortest-path principle in Euclidean space is chosen in this framework, where obstacles are initially modeled as polygons and a path between the two waypoints around obstacles is found using the visibility and the shortest-path principles in Euclidean space. In order to search for an admissible path without scanning over the entire solution set, A* algorithm is deployed for an effective search. When the solution is found by running the A* algorithm (and not by an exhaustive search), the solution of the planner may converge to a less optimal or even inadmissible path. Therefore, additional filters such as a cul-desac filter, a recursive filter, and a Fibonacci filter are introduced. These filters help to alleviate issues due to local minima while eliminating unnecessary segments in a candidate path. In addition, since the path solution found prior to the mission may have conflicts with pop-up obstacles or other agents, it is locally adjusted with a potential field-based collision-avoidance algorithm, which is responsible for solving a detouring trajectory around the obstacles intersecting with the original path solution. The proposed algorithms are first validated in simulations, where multiple UAVs perform the given tasks while avoiding obstacles and other agents. The proposed algorithm implemented on MATLAB/Simulink is shown to be capable of generating admissible paths in completely/partially known or completely unknown environments. Then the algorithm is validated in a flight test using three fixedwing UAVs to validate the task-assignment and path-planning algorithms together in an environment with pop-up threats. The proposed algorithms were shown to be capable of real-time path planning and task assignment in outdoor environments even with simulated dynamic obstacles.

63.2

Problem Formulation

In the following, the problem for task assignment for multiple UAVs in dynamic environments is defined. Hereinafter, tasks are defined as a set of actions to accomplish a job, a problem, or an assignment. It can be represented as a number of lower-level goals that are necessary for achieving the overall goal of the system (Pettersson and Doherty 2006; Maza et al. 2011). In this chapter, tasks is defined as visiting a waypoint, through which at least one agent should pass once to complete the associated task. First of all, the rules for the task assignment and the path planning are established as below (Moon and Shim 2010): (a) Obstacles are defined as the objects that obstruct the motion. In this research, they are represented as polygons. (b) The environment is modeled as a two-dimensional Euclidean space. (c) UAVs must avoid all obstacles. In other words, all UAVs must remain in admissible areas, clear from obstacles with a margin. Therefore, paths of all UAVs solved by the proposed procedure should be admissible. Path can intersect, but should not lead to collision among UAVs.


1551

Task Assignment -Task scheduling and allocation -Cooperation between UAVs -Find a suboptimal solution for task scheduling -Decentralized method

Require

Cost

Path Planning -Obstacle avoidance -Using knowledge on environment -No interactions between UAVs -Searching for an admissible path to reach to a task -Solutions are used as costs in task assignment layer

Collision Avoidance -Low-level trajectory planning -Focus on the interaction between UAVs -Can consider pop-up obstacles -Searching for an admissible trajectory to reach waypoints from the path planning

Fig. 63.1 Integration of task assignment, path planning, and collision avoidance

(e) The task assignment and the path planning should run in real time. In the following, the task-assignment and the path-planning algorithms based on rules listed above are presented.

63.3

Integrated Hierarchical Structure for Multi-UAV Coordination

Figure 63.1 explains how the task-assignment layer, the path-planner layer, and the collision-avoidance layer are integrated altogether. The integration of the task assignment and the path planner is a crucial step for architecting a mission-planning system for multi-agent scenarios (Moon et al. 2013). In this research, the task-

1552

S. Moon et al.

assignment layer needs to communicate with the path planner to compute the cost for task assignment, which is a function of the distance to reach the waypoint. During the task assignment, the path-planning layer is repeatedly queried until a feasible solution for task assignment is found. The next step is, if any unmapped obstacle is detected while flying along the original path, to find a collision-free path based on the original path. It is noted that the path-planner and the collision-avoidance layer may appear to do a similar task of finding a conflict-free path. The crucial difference is that the collisionavoidance layer finds a path locally detouring from the original path computed by the path planner, which only uses the obstacle information known before starting the mission. Therefore, these two algorithms work in a complementary manner to generate a collision-free path efficiently. As can be seen in Fig. 63.1, these two procedures are in a hierarchical structure, and the path-planning algorithm is invoked when the situation is in one of the following three cases: (i) before carrying out the mission, (ii) a new task is allocated on an agent participating in the mission, or (iii) new obstacles or threats suddenly appear, or “pop-up.” The collision-avoidance algorithm is activated when the agent flies to the target point while following the path. For multi-UAV scenarios, the collision avoidance between two or more agents should be considered. The avoidance algorithm presented later in this chapter is used to generate a collisionfree path.

63.4

Negotiation-Based Task Assignment

During the last decade, many algorithms have been developed for task-assignment problems (Bertsekas 1991; Ren et al. 2007; Choi et al. 2009; Viguria et al. 2010). The task-assignment algorithm presented in this chapter is based on the negotiation algorithm (Moon et al. 2013). The proposed method should generate a feasible solution within a reasonable time for real-time applications. In addition, this algorithm is an event trigger-based process, which means that it runs only when an agent sends a message to invoke this algorithm. This is a desirable attribute for decentralized systems running in real time with limited communication bandwidth. Events can be classified into three cases: (i) a mission is given to the system and the agents start to achieve the goal of the mission, (ii) a task in the given mission is accomplished by an agent, and (iii) the given mission is completely finished. If any of these events occurs during the mission, the presented task-assignment process is activated. Upon activation, all agents can be assigned with the different tasks, and this result is dependent on the conditions of the tasks. On the other hand, the costs for negotiation are defined by using (63.1). This formula consists of two parts: the distance from the current location of an agent to the task location and the characteristics of an agent and the assigned task, i.e., the capability set of an agent and the type of the task. The total cost is a linear combination of these two elements, and there are weights assigned for each term:


UAV #1 Task Assignment Requirement Before Negotiation

1553

UAV #2

UAV #3

Calculate the cost individually

Calculate the cost individually

Send the message for task assignment Calculate the cost individually

Cost: 40.68 @ Task A Cost: 40.96 @ Task A Cost: 42.09 @ Task C 1st Negotiation Cost: 42.09 @ Task C

Cost: 40.68 @ Task A

Cost: 41.60 @ Task C Cost: 42.09 @ Task C 2nd Negotiation Cost: 40.68 @ Task A Cost: 41.60 @ Task C Cost: 45.76 @ Task B

Fig. 63.2 Negotiation-based task allocation for three UAVs with three tasks

Jtask D wd Jdist C wc Jcharacter

(63.1)

In (63.1), Jdist is the cost related with the distance between the current position of an agent and the location of the given task. If there are multiple tasks with different characteristics, Jcharacter can be given with different costs to affect the behaviors of all agents in this scenario. The negotiation-based task assignment is performed in the following manner. Initially, each agent chooses its first task, which has the lowest cost from the cost list. Here, without loss of generality, the cost associated with the task is the distance to the task. The choices of all agents are broadcast to other agent, and when there are conflicts, i.e., more than one agents choose the same task, the costs of conflicting agents are compared, and the agent with the lowest cost has the right to claim the task, while other agents without assigned tasks repeat the same compare-and-claim process until all conflicts are resolved. In Fig. 63.2, an example with three UAVs is given. At first, each UAV chooses their task and the cost is compared. At the first negotiation, UAVs #1 and #2 choose the same task (task A), and they negotiate to find UAV #1 has a lower cost (40.68 vs. 40.96). So UAV #2 chooses a task other than A, which is task C with cost 41.60. This choice now conflicts with that of UAV #3 with cost 42.09, which is higher than the cost of UAV #2. Therefore, UAV #3 is forced to choose task B with cost 45.76. This approach has no guarantee to converge to the global minimum with an exception that it does converge to the global minimum when the number of the agent

1554

S. Moon et al.

is equal to the number of tasks. However, this algorithm has a low computing loads and suitable for decentralized scenarios with limited communication bandwidth.

63.5

Intersection-Based Path Planning

In Euclidean space, the shortest path between two points is the straight line between them. Denoting these points as xs and xg , the minimum cost function, i.e., the distance of the shortest path, can be represented as (63.2): min J D dist.xs ; xg / D jjxs ! xg jj

(63.2)

where dist(•) function is the distance defined in the Euclidean space. However, if there are any obstacles or forbidden region on the line, the cost found by (63.2) is not valid since the path is not admissible. In order to find an admissible path in this situation, the intersection point by the path from (63.2) and the boundary of the obstacle are used (Moon et al. 2013). The first step of the proposed algorithm is to find the intersection point. If an obstacle in the two dimensional space is modeled as a polygon, then the intersection point xm in Fig. 63.3 can be found easily. In such a case, a new path that passes the boundary points of a detected obstacle, xOi1 or xOi 2 should be found. Therefore, these two points are the candidates of waypoints for path planning. The proposed algorithm uses A* approach expressed as (63.3): f .x/ D g.x/ C h.x/ D jjxs ! xw jj C jjxg ! xw jj

(63.3)

In (63.3), xw is the boundary point of detected edge so that the first term of (63.3) is the distance from the starting point to a waypoint candidate and the other is the distance from the waypoint candidate to the goal point. If the detected line is not the first since the operation started, this equation should be modified to (63.4) because the vehicle is already assigned to a waypoint.

Xg XOi 1

Xm Oi

Fig. 63.3 Intersection point between shortest path and obstacle

Xs

XOi 2

63 Cooperative Task Assignment and Path Planning for Multiple UAVs Table 63.1 Pseudo code for path planning

1555

Algorithm 1 Intersection-based path planning (fundamental) 1: P InitializePath(xs ; xg ) 2: xi xs 3: while Dist(xg ; xi / ! dadmissible do 4: xm DrawLine(xg ; xi ) 5: if xm exist 6: xi SelectOptimalNode(xg ; xs ; x1 ; x2 ; : : : ; xi ) 7: P AddPath(P,xi ) 8: end if 9: i i+1 10: end while

f .x/ D g.x/ C h.x/ D

N X i D1

jjxi ! x.i "1/ jj C jjxg ! xw jj

(63.4)

In the two-dimensional space, the suboptimal waypoints and the candidate waypoints appear as a pair, and the suboptimal waypoints receive a higher priority for calculation. If multiple obstacles are located in the area, the intersection point which is the closest among the intersection points and the obstacle of that point is considered as the next optimal candidate. As A* algorithm is used in this procedure, all the nodes found by the intersection method are prioritized, and the best solution is chosen using the priority value. Therefore, those nodes related with the intersection points are applied by this method, and the procedures from (63.2) to (63.4) will be performed until the solved node is the goal point (see Table 63.1). When A* algorithm is applied from one root to another, the resulted path is not necessarily optimal. Therefore, a number of additional steps are introduced to refine the path although this process only guarantees to converge to a local minimum. Nonetheless, this process greatly helps to improve the quality of the resulted path, which has much less redundant segments and also avoids getting stuck in a cul-de-sac. Figure 63.4 shows a result obtained using the proposed algorithm. The given environment consists of ten arbitrary-shaped obstacles, which do not have any concave parts that can cause the solver to fall into an infinite loop. Therefore, additional procedures should be added, which will be introduced in the next section. In addition, the configuration space is also considered because UAVs are assumed to occupy certain area (volume), not a point. The dashed line is the solved path, and the lines are the candidate paths to obtain the shortest path. Initially, the algorithm finds a path, which travels to the left side of an obstacle. However, this solution is discarded due to the longer traveling distance around the next obstacle. After several iterations, a new path is found (dashed line), which connects the starting point and the goal point without any conflict with all obstacles.

1556

S. Moon et al. 50 Starting Point 40 30

Y direction (m)

20 10 0 –10 –20 –30 –40 –50

Goal Point –60

–40

–20

0

20

40

60

X direction (m)

Fig. 63.4 Result from the algorithm proposed in Table 63.1

63.5.1 Additional Procedures Although the algorithm proposed in Table 63.1 can generate an admissible path in most cases, it is also possible that the planner may get stuck to local minima. Even if the path is admissible, it may contain extra nodes that make the overall path less optimal. Therefore, in this section, three additional procedures are introduced: cul-de-sac filtering, Fibonacci filtering, and recursive filtering. These additional procedures further improve the proposed algorithm to find the optimal path more reliably although these procedures render the overall algorithm computationally more demanding.

63.5.1.1 Cul-de-sac Filtering Cul-de-sac refers to a dead end where there is no outlet other than the inlet. If the proposed algorithm is applied to the situation with concave obstacles that creates cul-de-sac, the algorithm may fall into an infinite loop, thus unable to find any admissible path. Therefore, an additional filter is introduced to resolve this problem (see Table 63.2). In Fig. 63.5, a case with three concave obstacles is considered. The direct output from the intersection-based planning finds a path that falls into the cul-de-sac point located on the middle of the diagram. Two paths were generated

63 Cooperative Task Assignment and Path Planning for Multiple UAVs Table 63.2 Pseudo code for cul-de-sac filtering

50 40 30

Y direction (m)

20

1557

Algorithm 2 Cul-de-sac filtering 1: if xi D xi!1 2: xm xi 3: xn xi!1 4: while xm D xn do 5: xn SelectNeighborVertex(xi / 6: xm SelectOptimalNode(xg ; xs ; x1 , . . . ,xi!1; xn ; xm / 7: end while 8: end if 9: xi xm

Starting Point

Discarded Path

10 0 –10 Final path

–20 –30 –40 –50

Goal Point –60

–40

–20

0

20

40

60

X direction (m)

Fig. 63.5 Cul-de-sac filtering

after applying the cul-de-sec filtering. When the path on the left is chosen, the other blue path indicated as “Discarded Path” is selected first. However, after running the original algorithm with the cul-de-sec filtering, the dashed red line is generated. Figure 63.5 shows the result after applying the cul-de-sac filtering algorithm.

63.5.1.2 Recursive Filtering As the intersection-based path planning considers only one obstacle at a time, if another obstacle is near the one being considered for path planning, the generated

1558 Table 63.3 Pseudo code for recursive filtering

S. Moon et al. Algorithm 3 Recursive filtering 1: if CheckOverlapObstacle(O1 ; O2 ,. . . ,On; xi!1 ; xi / 2: Ps InitializePath(xi!1 ; xi / 3: xj xi!1 4: xm DrawLine(xi ; xj / 5: if xm exist 6: xi SelectOptimalNode(xi ; xi!1 ; xj / 7: Ps AddPath(Ps , xi / 8: end if 9: j j +1 10: P MergePath(P; Ps / 11: end if

path may conflict with other obstacles. So the recursive filtering is proposed, which checks if the path around one obstacle passes through any other obstacles. This filtering process checks the path between i -th and the previous node, which can be expressed as N

xi "1 xi \ [ Oj ¤ ; j D1

(63.5)

Equation (63.5) implies that the line segment between the previous node xi "1 and the current node xi intersects with any obstacles along the line. In such cases, the intersection-based path-planning algorithm is called with xi "1 and xi as the input argument for temporary starting and the goal points, respectively, find to a conflict-free path (see Table 63.3). Recursive filtering is performed after running the main planning algorithm and the cul-de-sac filtering introduced above. This filtering is frequently required when the given environment is filled with closely located obstacles and consumes a more computation time. Figure 63.6 shows an example of the recursive filtering. Since the line between the starting point and the goal point intersects with the obstacle on the right, the blue path is obtained by the intersection-based algorithm. Since it overlaps with the obstacle on the left, the recursive filtering is applied for the segment between the starting point and the first node, resulting in the red-dashed line, which is conflict-free.

63.5.1.3 Fibonacci Filtering Fibonacci filtering is named after the well-known Fibonacci sequence from the fact that the current waypoint is computed using the two preceding points. It is developed to eliminate any extra nodes that increase overall cost (i.e., a longer path) when there is an admissible path with a lower cost. In Fibonacci sequence, each number is the sum of the previous two numbers, starting with 0 and 1. Similarly, Fibonacci filtering takes only the previous two nodes as the input argument. If two nodes xi "2 and xi satisfy (63.6), this filtering moves on to the next node. Therefore, Fibonacci filtering is the opposite concept of the recursive filtering described in the previous subsection:


1559

50 40

Goal point

30

Y direction (m)

20 10 0 –10 –20 –30 Starting point

–40 –50

–60

–40

–20

0 X direction (m)

20

40

60

Fig. 63.6 Recursive filtering. Solid lines (blue): suboptimal paths before the filtering. Dashed line (red): optimal path obtained by the recursive filtering

Table 63.4 Pseudo code for Fibonacci filtering

Algorithm 4 Fibonacci filtering 1: if !CheckOverlapObstacle(O1 ; O2 ,. . . ,On ; xi!2 ; xi / 2: xj !1 xi 3: i i "1 4: end if

N

xi "2 xi \ [ Oj D ; j D1

(63.6)

In (63.6), the line that connects the point before the previous node xi "2 and present node xi does not overlap with any obstacles, then the previous point xi "1 should be cancelled out to straighten the generated path. This procedure is performed for all nodes that satisfy (63.6), and repeated until (63.6) is not satisfied for all nodes (see Table 63.4). As is the case with the additional filtering algorithms introduced above, this filtering is more frequently needed if the given environment is complex and more computation time is needed for this algorithm along with other additional filtering algorithms mentioned above. In Fig. 63.7, the first two segments

1560

S. Moon et al. 50 40

Goal point

30

Y direction (m)

20 10 0 –10 –20 –30 –40 –50

Starting point –60

–40

–20

0 X direction (m)

20

40

60

Fig. 63.7 Fibonacci filtering. Solid line (blue): suboptimal paths before the filtering. Dashed line (red): optimal path obtained by the Fibonacci filtering

from the starting point on the blue line are less optimal than the first line segment of the red-dashed line due to the extra node, which is eliminated by running the Fibonacci filtering.

63.5.2 Overall Procedure Table 63.5 is the pseudo code for the all algorithms proposed so far. When the path planning is started, the intersection-based method is first applied and an initial candidate path is found. At this step, the cost function of the candidate paths are computed to run the A* algorithm to find the path with the least cost. Here, the path may get stuck in cul-de-sac, which is avoided by the proposed filtering (recursive filtering). In this process, the path may have extra node, which is then eliminated by the Fibonacci filtering. The filtering procedures are performed after the intersection-based algorithm. When the path reaches to the goal point, this algorithm is terminated. Figure 63.8 shows the path planning result with the recursive filtering and Fibonacci filtering in addition to the intersection-based method. Here, the recursive filtering process was responsible for finding the optimal path passing by obstacles 1

63 Cooperative Task Assignment and Path Planning for Multiple UAVs Table 63.5 Pseudo code for iterated algorithm

1561

Algorithm 5 Intersection-based path planning (overall) 1: P InitializePath(xs ; xg / 2: xi xs 3: while Dist(xg ; xi / > dadmissible do 4: xm DrawLine(xg ; xi / 5: if xm exist 6: xi SelectOptimalNode (xg ; xs ; x1 ; x2 ,. . . ,xi / 7: if xi D xi!1 8: xi CulDeSacFiltering (xg ; xs ; x1 ; x2 ,. . . ,xi / 9: end if 10: if CheckOverlapObstacle (O1 ,. . . ,On; xi!1 ; xi / 11: P,xi RecursiveFiltering (xg ; xs ; x1 ,. . . ,xi / 12: end if 13: if !CheckOverlapObstacle (O1 ,. . . ,On; xi!2 ; xi / 14: xj !1 xi 15: i i "1 16: end if 17: P AddPath (P , xi / 18: end if 19: i i +1 20: end while

and 2. The Fibonacci filtering is responsible for finding the better (i.e., shorter) path among the three paths ending up near obstacle #3.

63.6

Potential Field-Based Collision Avoidance

Collision avoidance is a dynamic path generation technique, which finds an admissible path to avoid a collision among agents or with obstacles in the environment. The algorithm proposed here is based on the potential field approach with several improvements presented below (Moon et al. 2013). Most algorithms based on the potential field are based on the distance between the vehicle and target points at one time frame. However, in case of moving obstacles, it is desirable to consider the relative direction of motion as well. The proposed algorithm utilizes the cost function for the potential field as the function of the distance and direction of the obstacle using the normal vector as J D f n.x; v; n/

(63.7)

where x and v represent the position and velocity of the vehicle, respectively. n is the set of normal vectors of the planes on the polyhedra representing the obstacles in the environment. Figure 63.9 illustrates the relationship among a UAV, an obstacle, and a waypoint along with the current and future position and velocity vectors of a UAV and a normal vector ni .

1562

S. Moon et al. 50 40 30

Starting Point

Y direction (m)

20

1 2

10 0 –10 –20

3

–30

Goal Point

–40 –50

–60

–40

–20

0

20

40

60

X direction (m)

Fig. 63.8 Result after running recursive filtering and Fibonacci filtering. Solid lines: suboptimal paths before the additional filterings. Dashed line: optimal path selected by the filtering

Xwaypoint

qobstacle + p ni

Vt+∆t

Xt

Fig. 63.9 Variables for cost function

Xobstacle

Xt+∆t Vt

In this algorithm, a set of path candidates over a finite horizon into the future are constructed. For each path, the cost is computed with respect to the attraction forces from the waypoints or goal points and the repulsion forces from the agents. Among those candidates, the best candidate that has the lowest cost can be selected, and the


1563

Fig. 63.10 Cost function for waypoint tracking 0.8

cost

0.6 0.4 0.2 0 6 4 2 θ (radian)

0

0

10

5

15

20

distance (meter)

UAV moves in one step along the selected path. This procedure is iterated until the UAV reaches to the target point. The cost function for the waypoints generated from the path planning can be expressed as (63.8), which can be visualized as shown in Fig. 63.10. Note that (63.8) is not dependent on the vehicle’s heading angle. In order to prevent (63.8) from dominating other terms, a denominator that divides by the distance from the waypoint to the current position of an UAV is included: Jwaypoint D 1 ! exp

jjxwaypoint ! xt C!t jj jjxwaypoint ! xt jj

!

(63.8)

The cost function for the obstacles and threats is expressed as (63.9), which is visualized as in Fig. 63.11, and again it is independent from the vehicle’s heading. If the angle between the normal vector of the plane on an obstacle and the vehicle’s velocity vector is very small, this indicates that the vehicle approaches to the object and therefore the cost function grows exponentially. However, if this angle is close to 180ı, the cost function decreases rapidly because the UAV moves away from the object. The cost function in Eq. (63.9) also includes a denominator to scale the magnitude of the normal vector is included to divide the term by a magnitude vector from the object to the current position of an UAV: " ! jjni jj Jobstacle;i D .0:5 cos "obstacle C 0:5 C Cobstacle / exp ! jjxt "x obstable jj

where

"obstacle D cos"1

!

ni #vt C!t jjni jjjjvt C!t jj

"

!#

(63.9)

1564

S. Moon et al.

1.5

cost

1

0.5

0 6 20

4

θ (rad)

15 10

2 0

5 0

distance (meter)

Fig. 63.11 Cost function for obstacles and threats

63.7

Simulation and Analysis

In order to evaluate the performance of the proposed algorithm, a series of simulations are conducted. In the proposed hierarchy, since the task-assignment layer cannot function without path planning, the intersection-based path planner is first implemented and validated.

63.7.1 Intersection-Based Path Planning It is assumed that the UAV has an onboard sensor for detecting obstacles such as the laser scanner, which has finite detection range. In the simulation, 20 randomly shaped obstacles are placed where only ten of them are known a priori. The simulation was constructed in MATLAB running on a PC with Intel CPU Centrino 2 Duo 2.4 GHz. Figure 63.12 shows the path-planning result when all of the obstacles are known a priori. UAV 1 flies to the goal point without any conflict with obstacles. Whenever obstacles unknown at the initial path-planning phase are discovered during the flight, the path-planning algorithm runs repeatedly in real time to find a conflict-free path (left side of Fig. 63.13). If none of the obstacles were known a priori, the pathplanning algorithm runs in real time from the start until the end, building the obstacle

63 Cooperative Task Assignment and Path Planning for Multiple UAVs Fig. 63.12 Path planning in totally known environment

1565

50 UAV 1 Start

40

Y direction (m)

30 20 10 0 –10 –20 –30 UAV 1 Goal 60

– 40 – 50

–60

–40

50 30

40 30

20

Y direction (m)

Y direction (m)

40

50 UAV 1 Start

40

10 0 –10 –20 –30 UAV 1 Goal

–40 –50

–20 0 20 X direction (m)

– 60

– 40

– 20

0

20

X direction (m)

40

60

UAV 1 Start

20 10 0

– 10 – 20 – 30 – 40 – 50

UAV 1 Goal

– 60

– 40

– 20

0

20

40

60

X direction (m)

Fig. 63.13 Path planning in partially known environment (left) and totally unknown environment (right)

map on the fly using onboard sensors (right side of Fig.63.13). Note that the obtained paths in the three cases with identical obstacles but with different sensing methods are different from one another. It is generally agreed that finding a feasible path in a given environment is a hard problem and finding the optimal solution, whichever the definition of optimality is, is a difficult and there is not a general solution for such work. The proposed algorithm is viable in the sense that it can find an admissible path with a smaller computation load with all obstacles modeled as polygons. Such simplified obstacle is acceptable with path planning for UAVs, which needs to stay clear from obstacles for safety and to avoid turbulence caused by the airflow bouncing back from the obstacles. The proposed algorithm runs very fast in environments with fewer obstacles. However, if the environment is filled with many obstacles, the computation load can be quite heavy because of the additional filtering processes.

1566

S. Moon et al.

0.25 Totally Known Environment Partially Known Environment

Computation Time (sec)

0.2

0.15

0.1

0.05

0

0

10

20

30

40 50 60 Operation Time (sec)

70

80

90

100

Fig. 63.14 Computation time versus elapsed time

Figure 63.14 shows the computation time during the simulation. The case with totally known environment is the blue lines with square marks, and the case with partially known environment is the red lines with plus marks. The totally known environment case does not require much computation except for the first path planning. However, the partially or totally unknown case requires more computation load during the simulation whenever new obstacles are detected (t = 53 s).

63.7.2 Negotiation-Based Task Assignment In the simulation, three UAVs carry out a mission in an area with ten randomly shaped obstacles. Ten task points are arbitrarily placed in the field. As mentioned above, a task is defined as a visit to its associated waypoint. The margin for obstacle avoidance is assumed 2 m here. In this simulation, a simple kinematic model for UAVs were used such that Vx D V cos " (63.10) Vy D V sin " where

"P 2 Œ!ı; ı$

(63.11)

To evaluate the performance of the proposed algorithm, a set of simulation is conducted. A scenario with three UAVs carrying out a mission in an area

63 Cooperative Task Assignment and Path Planning for Multiple UAVs 50

50 40

40

TASK 10

UAV 3 Start

10

TASK 1

0

TASK 9 TASK 3

TASK 8

–10 UAV 2 Start

–20

TASK 7

UAV 1 Start –60

–40

UAV 2 Goal

TASK 4

20

–20

UAV 3 Start

10

TASK 5

0

–30

UAV 3 Goal 20

40

–40 –50

60

TASK 1

TASK 9 TASK 3 TASK 8

UAV 2 Start UAV 1 Start –60

TASK 7

–40

–20

0

UAV 3 Goal 20

40

60

50

40

40

TASK 10

TASK 10

30

30 UAV 3 Start

10

TASK 1

TASK 2

TASK 9

UAV 1 Goal

TASK 3

0 TASK 8

–10 UAV 2 Start

–20

TASK 6

–30 UAV 1 Start –60

TASK 7

–40

UAV 2 Goal

TASK 4

–20

20 Y direction (m)

20 Y direction (m)

TASK 5

X direction (m)

50

–50

UAV 2 Goal

TASK 4

TASK 6

X direction (m)

–40

UAV 1 Goal

TASK 2

0 –10 –20

TASK 6

–30

–50

UAV 1 Goal

TASK 2

Y direction (m)

Y direction (m)

20

TASK 10

30

30

–40

1567

UAV 3 Start

10

0 X direction (m)

UAV 3 Goal 20

40

60

TASK 2

–30 –40 –50

TASK 9

UAV 1 Goal

TASK 3

0 TASK 8

–10

UAV 2 Start

–20 TASK 5

TASK 1

UAV 1 Start –60

TASK 6 TASK 7

–40

UAV 2 Goal

TASK 4

–20

TASK 5

0

UAV 3 Goal 20

40

60

X direction (m)

Fig. 63.15 Task assignment in a totally known environment (t = 10,60,120, and 185 s from left side of top to right side of bottom)

with ten randomly shaped obstacles is considered. Ten task points are arbitrarily placed in the field. As mentioned, tasks are defined as a visit to a waypoint. The margin for obstacle avoidance is 2 m. Simple kinematic equations for UAVs were used in this simulation. Figure 63.15 shows the simulation result. As the mission progresses, ten negotiations occur. UAV 1 is assigned with three tasks (task 4, task 7, and task 9) and participates in the whole negotiation process. UAV #2 is also assigned with three tasks (task 5, task 6, and task 8), and UAV #3 is assigned with four tasks (task 1, task 2, task 3, and task 10) during the mission. At fifth negotiation, UAV #1 sends the information of the determined task to UAV #2 and chooses to undertake task 4 because it is better that UAV #2 carries out task 5. Such a task swapping also occurs at the ninth negotiation process between UAV #1 and UAV #2 (see Table 63.6). There are nine peaks in the plot of the elapsed time for negotiation process (Fig. 63.16). Although the maximum computation time (=0.9 s) of the first process is greater than the iterated time to run the proposed algorithm, it does not affect the real-time performance of the while mission because the task-assignment algorithm is executed offline prior to the mission. Since the computation time to perform each negotiation in the iterative algorithm is very short, the proposed task-assignment algorithm is a viable solution for real-time application. It is also noted that the

1568

S. Moon et al.

Table 63.6 Task-assignment procedure for simulation with three UAVs

UAV #1 Task 7 Task 7 Task 7 Task 5 Task 4 Task 4 Task 4 Task 4 Task 9 Task 9

Step 1 Step 2 Step 3 Step 4 Step 5 Step 6 Step 7 Step 8 Step 9 Step 10

UAV #2 Task 8 Task 8 Task 6 Task 6 Task 5 Task 5 Task 5 Task 9 Finish Finish

UAV #3 Task 1 Task 2 Task 2 Task 2 Task 2 Task 3 Task 9 Task 10 Task 10 Finish

0.9 0.8

Computation Time (sec)

0.7 0.6 0.5 0.4 0.3 0.2 0.1

Fig. 63.16 Elapsed time versus computation time

0

0

20

40

60

80

100

120

140

160

180

200

Elapsed Time under Mission (sec)

proposed negotiation-based algorithm exhibits a greedy behavior as each agent thrives to minimize its cost at each time frame in a fully decentralized manner so that the overall task-assignment algorithm generally produces suboptimal results than centralized, non-greedy algorithms.

63.8

Flight Experiments and Validations

As the final step, in order to validate the proposed algorithm in a realistic environment, actual flight tests were conducted using a fixed-wing UAV based on a blended wing body (BWB) airframe as shown in Fig. 63.17. The airplane is powered by an electrical motor and a lightweight in-house flight control computer is installed for automatic waypoint navigation. The test UAV’s specification is given in Table 63.7. The experiment is conducted in the following steps. When the flight computer is initialized, each vehicle is launched using a bungee cord. After the vehicle climbs to


APC 12×6 Prop. Axi 2217/16 Motor

Eleven-left

1569

Telemetry Antenna

Servo ESC Elevon-right

Video Antenna

Servo

GPS Antenna Nose Camera

Fig. 63.17 Testbed UAV based on BWB airplane Table 63.7 Testbed UAV specification

Base platform Dimensions

Weight Powerplant

Operation time Avionics

Operation Autonomy

Blended wing body Wing span: 1.52 m(W) Wing area: 0.52 m2 Aspect Ratio: 4.43 2.6 kg (fully instrumented) Axi 2217/16 DC brushless motor Lithium-ion-polymer (11.1 V 5,300 mAH)