- Email: [email protected]
PROBABILISTIC ROADMAPS AND ANT COLONY OPTIMIZATION FOR UAV MISSION PLANNING
PROBABILISTIC ROADMAPS AND ANT COLONY OPTIMIZATION FOR UAV MISSION PLANNING
Florian Adolf, Augusto Langer, Lucas de Melo Pontes e Silva, Frank Thielecke German Aerospace Center (DLR), Institute of Flight Systems, Systems Automation Department D-38108 Braunschweig, Germany
This paper presents a two-tiered approach to mission planning based on Probabilistic Roadmaps for path planning and Ant Colony Optimization for task planning. The path planner searches paths in a finite, obstacle-constrained three dimensional space whereas the task planner determines a near optimal task order for a set of tasks and a group of heterogeneous agents. The mission planning system is employed in an unmanned helicopter project that researches on intelligent technologies for small, low-altitude aerial vehicles. Results from simulation experiments demonstrate the feasibility of combining a dedicated path planner with a task planner. Keywords: Intelligent Autonomous Vehicles, UAVs, Multiple Vehicle Systems
1. INTRODUCTION The DLR Institute of Flight Systems is developing the experimental flying platform ARTIS (Autonomous Rotorcraft Testbed for Intelligent Systems), an autonomous miniature helicopter. The main focus of the project is to develop new concepts and algorithms for autonomous, intelligent functions. The vision for this project involves the coordination of multiple agents in cluttered spaces. Hence, mission planning algorithms are required to plan safe, unobstructed 3D paths while optimizing task assignment and task ordering for multiple UAVs. This paper presents a combined approach for mission planning comprised of a 3D path planner and a multi-agent task planner. The path planning component’s obstacle representation is based on a closed polyhedral model. To avoid known arbitrary obstacles, a set of path points is sampled offline. A graph representation of the sample connections allows fast path searches in finite three dimensional spaces. Since the planning domain comprises urban areas with potentially narrow regions, the offline sampling method is extended by "bridge tests". The task planning algorithm is designed in a centralized manner. It assigns and optimizes the task ordering for each agent using a variation of an Ant Colony Optimization algorithm. The task planner is the first step towards cooperativeness. It evaluates a potential increase of the overall mission utility and allows the sharing of tasks among agents.
The paper outline is as follows: 2 explains the mission planning problem in more detail while existing solutions are discussed for both path planning and task planning. 3 describes the path planner and 4 presents the task planner. 5 concludes with results and areas of future work. 2. MISSION PLANNING PROBLEM Given an environment with known obstacles, a mission is specified. It comprises of a number of waypoints, at which a set of one or more agents has to pass through or stop. A set of waypoints (e.g. waypoints defining a search area), or an action at a waypoint location (e.g. “take a photo”) is abstracted into a task. To accomplishment a mission efficiently, one must solve three different problems: how to assign the tasks to the given agents, how to schedule the tasks for each agent, and how to plan a path between tasks in an efficient manner. 2.1 Path Planning Problem. Simply stated, the path planning problem treats the need of a robot to “get from A to B” (Capozzi, 2001). Given an initial and a final robot configuration, a set of intermediate configurations is generated which connects them together (Hsu, et al., 1999). Here a
configuration is considered to be a point in threedimensional space with an associated heading angle (x, y, z, θ). Based on this notion the concept of the configuration space arises, which is the vector space of configurations. The problem of finding a path in an obstacle-constrained, three-dimensional environment with optimal (here: shortest) length has been proven to be NP-hard (Canny, et al., 1987). Related Work. This proof has fostered work on approximation algorithms, which guarantee ε– optimality. That is, the length of the obtained path is within (1+ε) the length of the optimal path. Research in the area of mobile robotics historically only focused on the two-dimensional case. A rather traditional approach in path planning makes use of an artificial potential field function. A finish point exerts an attracting potential and obstacles exert a repulsive force. A gradient descent is performed to search the path. However, a gradient descent may get trapped in local minima. Canny, et al. (2003) describes an approach with techniques to overcome this shortcoming. Other approaches have been developed that rely on randomness. In Xiao (1997) evolutionary computation is used to find a solution. A set of randomly generated paths “evolves” by changing them through certain operators until a solution is found. Another random-based method for path planning makes use of probabilistic roadmaps (PRMs). A PRM is a weighted graph whose nodes are randomly chosen points in the configuration spacewith edges representing collision-free paths between nodes. Each edge’s weight is related to a metric (e.g. Euclidean distance). This method is introduced in Kavraki, et al. (1994) for planar articulated robots. In Petersson (2006) it is applied to rotorcraft UAVs. Discussion. The ARTIS project has the following requirements for a path planner: • 3D world model: Under certain limitations 2-D path planners have shown to be suitable for ARTIS (Dittrich, 2006). However, a planner capable of dealing with three-dimensional obstacles is considered to be more suitable for the low-altitude UAV domain. • Quick computational time: As re-planning might be necessary during a mission, it is imperious that a new path be determined quickly. • Completeness: A necessary feature of planning algorithms is that they are complete, that is, they find a solution whenever one exists. • Multi-agent support: Planning paths for multiple agents can result into a considerably large set of path planning problems. Therefore, it is advisable that each planner instance shares information with other instances to improve efficiency in solving the overall problem. Based on these requirements, the PRM-based approach was chosen. PRMs trade off optimality for shorter run-times. Instead of performing a full deterministic search of the configuration space, a
result is found by probabilistic offline sampling and subsequent graph searches. Although a PRM-based algorithm is not complete due to its stochastic nature, it can be shown to be probabilistically complete (Hsu, 1999). Given sufficient iterations, the probability of finding a solution tends to one. 2.2 Task Planning Problem. The ARTIS project focuses on cooperative UAV groups. In such Multi-Agent Systems (MASs), sole path planning is not sufficient for mission planning given multiple goals and multiple agents. A so-called task planner minimizes either the mission duration or the overall flight distance. This requires a more complex mission description. Related Work. Task planning is an instance of the “traveling salesman”2 (TSP) problem. Hence, it is a NP-hard combinatorial optimization problem (Shima, et al. 2006). A previous approach developed for the ARTIS project optimized the flight distance for one agent using a genetic algorithm (Gabrielli, 2005). However, this approach was developed for homogeneous agents and tasks, inspiring work on cases in which tasks require specific agent capabilities or resources. Literature shows that distributed task planning approaches can be flexible and robust. They are less predictable and their optimality is not guaranteed, however, when agent cooperativeness increases. Hence, as a first step, this work focuses on a centralized offline planning approach. In a centralized approach there exists a leader that aggregates information about the other agents, plans optimally for the entire set of agents, and finally propagates the task assignments to each team member. Based on complete situational knowledge, such master-slave architectures have the advantage that an (near) optimal solution can be found. Near-optimal solutions utilize stochastic methods (Shima, et al. 2006; Pongpunwattana, et al. 2003; Stuetzle, et al. 1997) or likewise mixed integer linear programming techniques (Darrah, et al. 2005, Richards, et al. 2002, Shumacher, et al. 2004). These methods have been successfully applied for taskordering in a group. Another promising method is the biologically inspired Ant Colony Optimization (ACO) approach (Dorigo, et al. 1996; Mazza, et al. 2004). As in nature, virtual ants communicate indirectly to each other by depositing and sensing pheromones. The more pheromones deposited, the greater the probability of other ants choosing the same trail. It leads to reinforcement of better trails found by the colony. In particular, the ACO variant in Dorigo (1996), known as Max-Min Ant Systems, is known to returns the best results.
12
http://www.tsp.gatech.edu
Discussion. In the context of the ARTIS project, two main requirements for the task planner exist. First, an operator on the ground needs control over the task planning process. In particular, this forbids approaches that tend to show emergent behaviors (e.g. totally distributed approaches). Second, since configurations may vary among UAVs, it has to consider potential differences in each UAV’s capabilities. Therefore, an ACO is used to approximate solutions to the centralized, NP-hard task order optimization. 3. PATH PLANNER PRM-based path planning has three main phases: • A pre-computation step, in which the PRM itself is generated. • A query step, which consists of a search within the generated PRM using conventional search algorithms. • A post-processing step can be performed to obtain “smoother” paths. 3.1 The Pre-computation Step. Adapted from Petersson (2006), this step generates the PRM, its corresponding search graph and checks the graph’s edges against collisions. PRM Generation. The graph is stored as a node set, N , and a weighted edge set, E . First, N is filled with n randomly selected nodes in 3D space. Second, these nodes are connected, generating the set E . For each node Q , a set of neighbor nodes is chosen and each pair of nodes (Q, Qn ) is connected using a local path planner. Since the nodes in our representation do not have a heading information, straight line connections are sufficient. Otherwise, it would be possible to use splines or circle-line-circle (CLC) curves as node connections. If the local path has no collisions, both nodes and the path cost (Euclidean distance) are added to E . Collision Checking. In general, collision checks demand most of the runtime during path planning. Hence, it is important to have efficient collisiondetecting routines. Popular solutions are AxisAligned Bounding Boxes (AABBs) and Oriented Bounding Boxes (OBBs), which simplify the obstacle representation. However, they tend to overestimate space occupied by obstacles and may fail to represent the environment precisely (e.g. dense urban areas). Thus, an arbitrary polyhedral model is used for obstacle representation. Any object is constituted by a set of plane triangles, arbitrarily oriented in three-dimensional space. All obstacles are assumed to be closed polyhedral surfaces without overlapping. This polyhedral model requires two collision detection routines: One that checks whether a node is in free space and one that assures collision free paths.
The first collision check verifies if a point Q is connected to a point Qn . Qn exceeds the bounds of the environment (e.g. at very high altitude). If this line does not cross any nearby triangles, or an even number of them (implying an obstacle entirely above Q), then Q lies in free space. Otherwise, it is contained within an obstacle. Furthermore it must be ensured that Q lies far enough from obstacles. For that purpose, one must calculate the distance from Q to all nearby triangles. A collision-free path segment crosses no triangles at all, and each point within it lies far enough from obstacles as well as each node in the PRM. Simple geometric routines can implement such tests. The Bridge Test for Narrow Passages. One known shortcoming of sampling methods is that they tend to under-populate narrow passages which lie between obstacles. This is due to their smaller volume compared to the environment. A solution can be found in Hsu (2003), using the so-called bridge test. This algorithm can create samples within narrow passages while still using random-based methods. First, two random samples, Q1 and Q2 , which both lie inside obstacles, are generated. A middle point QB between (Q1 , Q2 ) , called bridge point, is added to the graph if it belongs to free space. 3.2 The Query Step. Once an underlying path has been constructed, finding a path becomes a significantly less complex
r
task. The first step is to connect the start point Po and the finish point
r Pf to the PRM. Then, virtually
any graph search algorithm can be applied to that problem. Due to the existence of a good heuristic (distance of each point to the finish point) and only non-negative weights (Euclidean distance), Dijkstra’s A* was chosen. The usual definition of the A* assigns all nodes to an OPEN list, with all but the start node having g = ∞ , where g is the sum of all costs from the start node to g . In this particular implementation, only the start node is initially added to the OPEN list; nodes which would normally have g = ∞ are not added to any list. This way, less data is stored in the OPEN list, allowing faster node insertion and removal. A heap data structure was used, which allows O(log n) node insertion and O(1) node removal. 3.3 The Post-Processing Step. While the randomness of the PRM shows advantages with respect to short planning time, the paths generated by this process may be full of sharp turns and may be visibly far from an optimal solution. Thus this step aims on optimizing and smoothing a given path by using shortcuts. For a given node Q if
its predecessor Q p and successor Qs may be connected by a collision-free path, then Q is removed. This way, shorter and smoother paths can be generated, with a relatively small number of collision checks. Three pointers (current, previous and next) are iterated through the entire path, searching for possible shortcuts, until no further optimizations can be found.
Scenario 3 simulates an urban-like area with a constrained section (Fig. 3). It can only be accessed through the two arches or over the lower wall. Having both, narrow passages and wide open areas, it is a challenging scenario. Its obstacles comprise 258 triangles. 1000 random points + bridge points 500 random points + bridge points
4
3.5
3.4 Experimental results. This section shows paths and evaluates aspects of different scenarios. Scenario 1 assesses the overall performance by simulating the Hannover International Airport (Fig. 1) with over 400 triangles forming the boundaries of the obstacles. Generating a PRM with 1000 purely random points and only a few hundred “bridge” points results in an acceptable increase in build time (Fig. 4), while significantly improving the path search performance, as seen in the next scenario.
PRM generation time (s)
3
2.5
2
1.5
1
0.5
0 500
600
700
800
900 1000 1100 Number of nodes
1200
1300
1400
1500
Fig. 4. A growing number of “bridge points” results in an acceptable growth of the PRM build time.
Fig. 1. Scenario 1 with the underlying PRM. Each dot marks a sample point in 3D. The edges highlight collision-free paths.
Fig. 5. Scenario 2 shows the advantage of “bridge test” heuristics in combination with randomness.
Scenario 2 assesses “bridge tests” and path smoothing concepts (). It is comprised of deep, narrow corridors and as such a particularly hard setup. Figure 7 shows that the chance of finding a path in 50 experiment repetitions rises significantly with bridge testing.
4. TASK PLANNER
Fig. 2. Scenario 2 assessing the “bridge test” and the path smoothing technique. Bright lines are the “raw” paths, dark lines are smoothed paths.
Fig. 3. Scenario 3 assessing both, narrow passages and wide areas.
To reduce the planning complexity, the task planner is decomposed into the following phases: • The task refinement maps agents to tasks that require certain capabilities. • The conflict solving addresses conflicts that can arise when a task is supposed to be executed by only one agent, but the task refinement identified more than one potential agent. • A scheduling optimizer maximizes the overall mission utility by altering the potentially suboptimal task order of each agent’s task list. 4.1 Task Refinement. Given a group of agents with certain capabilities and a set of tasks with requisites (e.g. task 5 needs a photo camera), the planner searches those agents that map to the required capabilities of tasks. The refinement is represented as capability matrix. Before a further task order optimization can take place, all possible paths are stored in a cost matrix. Given n agents and m tasks, the path planner has to compute all n²+m²+n*m paths.
4.2 Conflict Solver. To solve conflicts, the cost and capability matrixes need to be transformed into a weighted graph representation (Fig. 6). Edge weights represent costs (path length) determined by the path planner. A shortest path search is performed using a Minimum Spanning Tree3. Per conflicting task, the agent with the least cost is assigned to that task.
Fig. 7. Example Scenario with three heterogeneous agents and 30 tasks.
Fig. 6. A mission (left) and an agent’s task list from that mission (right) shown as undirected graph. One conflict occurs between agents G1,G2,G3 for task T5. 4.3 Scheduling Optimizer. During the ACO, artificial ants "walk" in each agent’s graph. Pheromones are deposited on a path in proportional quantity to the solution quality. Each path found corresponds to a possible solution for the optimization problem. Note that ants have memory which they use to take decisions in the search graph (e.g. avoid paths that were already visited before). The paths are chosen probabilistically, proportional to the costs and the amount of pheromones deposited on a path. After the ants’ search converged to a feasible solution, an optimization of search areas (shareable tasks) takes place. The sum of all agents’ path length and the number of turns within the search area define the costs. The minimum cost is computed through combinatorial search. The ACO key parameter is the pheromone evaporation rate, since it sets the search space size. It is determined empirically (Fig. 9). A greedy heuristic solution4 sets the initial seed for the ant’s path position in the search graph. For remaining parameters (e.g. heuristic or pheromone factor) values from literature are used. 4.4 Performance Results The performance of the task planner is measured by the expected mission duration. The test scenario in Fig. 7 is used to vary ACO parameters. It comprises 3 agents, 4 search tasks and 26 waypoint tasks. A significant decrease of the duration is achieved, when >1 agents and >10 tasks are available (Fig. 8). This supports a general speculation in the ARTIS project that “usual” domain scenarios will not require more than 3 or 4 agents. However, robustness can be improved with even more agents.
3 4
http://citeseer.ist.psu.edu/nesetril00otakar.html, Otakar Boruvka http://en.wikipedia.org/wiki/Greedy_algorithm
Fig. 8. Tasks vs Agents in the example scenario: Influence on the mission time for different setups.
Fig. 9. Differences in the convergence rate of the ACO for the scenario in Section 4.4. 5. CONCLUSION This paper showed the feasibility of a combined approach for UAV mission planning using a 3D path planner and a centralized task planner. The proposed path planner has practical advantages over optimal solutions, particularly for the lowaltitude UAV domain. It demands less computational effort for path searches through offline search graph construction. Moreover, it is probabilistically complete and as such an optimal approximation. Implicitly, it supports multi-agent planning through quick path results by using a previously built PRM. Experimental results in Section 3.4 show that the longer duration of a pre-processing step pays off for individual queries, which take milliseconds; a large number of these are required by the task planner. The proposed centralized task planner handles both, heterogeneous UAV agents and tasks that require specific agent capabilities or resources. It approximates an optimal task ordering for individual agents using an ACO variant, which is known to be
the best approach for TSP-like problems. Moreover, it implements UAV cooperativeness by evaluating the overall mission utility of task sharing. The limits of the vehicle’s dynamic behavior pose yet another challenge to the path planning algorithm presented in this work. At low speeds, linear path segments can be followed by the UAV within a certain error distance, which is added to the safety margin used in the planner. As flight speed increases, however, piecewise linear paths must be further smoothed, taking flight dynamics in consideration. Finally, research is underway to enhance the PRM for online path re-planning, where cases of disappearing obstacle are expected to be solved more quickly. Furthermore, the applicability of distributed task planning is evaluated for ARTIS. ACKNOWLEDGEMENTS The authors of this work would like to thank the project FINEP-CAPTAER for supporting Augusto Langer and Lucas Silva regarding their stay at DLR. The ARTIS project is supported by QNX Software Systems through a research grant. The ARTIS flight control computer runs the QNX real-time operating system. REFERENCES Alighanbari, M.; Kwata, Y.; How, J. (2003) CoOrdination And Control Of Multiples Uavs With Timing Constraints And Loitering. Proceedings Of The American Control Conference, vol.6, Pages 5311-5316. Canny, J. F.; Reef, J. (1987) Lower Bounds for Shortest Path and Related Problems. In: Annual IEEE Symposium on Foundations of Computer Science, 28., Los Angeles. Canny, J. F.; Lin, M. C. (2003) An Opportunistic Global Path Planner. Algorithmica, vol.10, n. 24, p. 102-120. Capozzi, B. J. (2001) Evolution-Based Path Planning and Management for Autonomous Vehicles, 363f. These (Doctor of Philosophy – Aeronautics Engineering) – University of Washington, Washington Darrah, M. A.; Niland, W. M.; Stolarik, B. M. (2005) Multiple UAV Dynamic Task Allocation using Mixed Integer Linear Programming in a SEAD Mission. In: Infotec@Aerospace, Arlington, VA. Dittrich, J. S.; Adolf, F. M.; Langer, A.; Thielecke, F. (2007) Mission Planning for Small VTOL UAV Systems in Unknown Environments. In: AHS International Specialists’ Meeting on Unmanned Rotorcraft, American Helicopter Society International, Chandler, AZ. Dorigo, M.; Gambardella, L. M. (1996) Solving Symmetric and Asymmetric TSP by Ant Colonies. In: IEEE International Cooference On Evolutionary Computation. Proceedings IEEE Press, pp. 622-627.
Gabrielli, L. H. (2005) Mission Planning And Dynamic Avoidance For A Group Of Autonomous Agents. 61F. Trabalho De Conclusao De Curso. (Graduacao)- Instituto Tecnologico De Aeronautica, Sao Jose Dos Campos Hsu, D.; Kavraki, L. E.; Latombe, J-C.; Motwani, R. (1999) Capturing The Connectivity Of HighDimensional Geometric Spaces By Parallelizable Random Sampling Techniques. In P.M. Pardalos And S. Rajasekaran, Editors, Advances In Randomized Parallel Computing, Pages 159182. Kluwer Academic Publishers, Boston, MA Hsu, D.; Jiang, T.; Reif, J.; Sun, Z. (2003) The Bridge Test for Sampling Narrow Passages with Probabilistic Path Planners. In IEEE International Conference on Robotics and Automation, Taiwan. Kavraki, L.; Svetska, P.; Latombe, J-C.; Overmars, M. (1994) Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, CS-TR-94-1519. Mazza, I; Ollero, A. (2004) Multiple UAV Cooperative Searching Operation Using Polygon Area Decomposition and Efficient Coverage Algorithm. In: 7th International Symposium On Distributed Autonomous Robotics Systems, Toulouse, DRAS. Petersson, P-O. (2006) Sampling Based Path Planning for an Autonomous Helicopter. 147f. Tese (Licentiate of Engineering) – Linköping universitet, Linköping. Pongpunwattana, A.; Rysdyk, R.; Vagners, J. Market-Based (2003) Co-Evolution Planning For Multiples Autonomous Vehicles. In: AIAA Unmanned Unlimited Conference. Richards, A.; Bellingham, J.; Tillerson, M.; How, J. (2002) Co-ordination and Control of Multiples UAVs. In: AIAA Guidance, Navigation And Control Conference, Monterey, CA. Shima, T.; Rasmussen, S. J.; Sparks, A. G.; Passino, K. M. (2006) Multiple Task Assignments For Cooperating Uninhabited Aerial Vehicles Using Genetic Algorithms, Computers & Operations Research, vol.33, no.11, p.3252-3269. Shumacher, C.; Chandler, P.; Pachter, M. (2004) UAV Task Assignment with Timing Constraints. AIAA Guidance, Navigation And Control Conference And Exhibit, Austin, TX. Stuetzle, T.; Hoos, H. (1997) Max-Min Ant System And Local Search For Combinatorial Optimization Problems. In: 2Nd International Conference On Metaheuristics, SophiaAntilopis., France, July 21-24. Weerdt, M.; Witteveen, C (2006). Multiagent Planning: Problem Properties That Matter. In: AAAI Spring Symposium On Distributed Plan And Schedule Managements. Proceedings AAAI, 155—156. Xiao, J.; Michalevicz, Z.; Zhang, L.; Trojanowski, K. (1997) Adaptive Evolutionary Planner-Navigator For Mobile Robots. IEEE Transactions On Evolutionary Computation, vol.1, no.1, P. 18-28.
Florian Adolf, Augusto Langer, Lucas de Melo Pontes e Silva, Frank Thielecke German Aerospace Center (DLR), Institute of Flight Systems, Systems Automation Department D-38108 Braunschweig, Germany
This paper presents a two-tiered approach to mission planning based on Probabilistic Roadmaps for path planning and Ant Colony Optimization for task planning. The path planner searches paths in a finite, obstacle-constrained three dimensional space whereas the task planner determines a near optimal task order for a set of tasks and a group of heterogeneous agents. The mission planning system is employed in an unmanned helicopter project that researches on intelligent technologies for small, low-altitude aerial vehicles. Results from simulation experiments demonstrate the feasibility of combining a dedicated path planner with a task planner. Keywords: Intelligent Autonomous Vehicles, UAVs, Multiple Vehicle Systems
1. INTRODUCTION The DLR Institute of Flight Systems is developing the experimental flying platform ARTIS (Autonomous Rotorcraft Testbed for Intelligent Systems), an autonomous miniature helicopter. The main focus of the project is to develop new concepts and algorithms for autonomous, intelligent functions. The vision for this project involves the coordination of multiple agents in cluttered spaces. Hence, mission planning algorithms are required to plan safe, unobstructed 3D paths while optimizing task assignment and task ordering for multiple UAVs. This paper presents a combined approach for mission planning comprised of a 3D path planner and a multi-agent task planner. The path planning component’s obstacle representation is based on a closed polyhedral model. To avoid known arbitrary obstacles, a set of path points is sampled offline. A graph representation of the sample connections allows fast path searches in finite three dimensional spaces. Since the planning domain comprises urban areas with potentially narrow regions, the offline sampling method is extended by "bridge tests". The task planning algorithm is designed in a centralized manner. It assigns and optimizes the task ordering for each agent using a variation of an Ant Colony Optimization algorithm. The task planner is the first step towards cooperativeness. It evaluates a potential increase of the overall mission utility and allows the sharing of tasks among agents.
The paper outline is as follows: 2 explains the mission planning problem in more detail while existing solutions are discussed for both path planning and task planning. 3 describes the path planner and 4 presents the task planner. 5 concludes with results and areas of future work. 2. MISSION PLANNING PROBLEM Given an environment with known obstacles, a mission is specified. It comprises of a number of waypoints, at which a set of one or more agents has to pass through or stop. A set of waypoints (e.g. waypoints defining a search area), or an action at a waypoint location (e.g. “take a photo”) is abstracted into a task. To accomplishment a mission efficiently, one must solve three different problems: how to assign the tasks to the given agents, how to schedule the tasks for each agent, and how to plan a path between tasks in an efficient manner. 2.1 Path Planning Problem. Simply stated, the path planning problem treats the need of a robot to “get from A to B” (Capozzi, 2001). Given an initial and a final robot configuration, a set of intermediate configurations is generated which connects them together (Hsu, et al., 1999). Here a
configuration is considered to be a point in threedimensional space with an associated heading angle (x, y, z, θ). Based on this notion the concept of the configuration space arises, which is the vector space of configurations. The problem of finding a path in an obstacle-constrained, three-dimensional environment with optimal (here: shortest) length has been proven to be NP-hard (Canny, et al., 1987). Related Work. This proof has fostered work on approximation algorithms, which guarantee ε– optimality. That is, the length of the obtained path is within (1+ε) the length of the optimal path. Research in the area of mobile robotics historically only focused on the two-dimensional case. A rather traditional approach in path planning makes use of an artificial potential field function. A finish point exerts an attracting potential and obstacles exert a repulsive force. A gradient descent is performed to search the path. However, a gradient descent may get trapped in local minima. Canny, et al. (2003) describes an approach with techniques to overcome this shortcoming. Other approaches have been developed that rely on randomness. In Xiao (1997) evolutionary computation is used to find a solution. A set of randomly generated paths “evolves” by changing them through certain operators until a solution is found. Another random-based method for path planning makes use of probabilistic roadmaps (PRMs). A PRM is a weighted graph whose nodes are randomly chosen points in the configuration spacewith edges representing collision-free paths between nodes. Each edge’s weight is related to a metric (e.g. Euclidean distance). This method is introduced in Kavraki, et al. (1994) for planar articulated robots. In Petersson (2006) it is applied to rotorcraft UAVs. Discussion. The ARTIS project has the following requirements for a path planner: • 3D world model: Under certain limitations 2-D path planners have shown to be suitable for ARTIS (Dittrich, 2006). However, a planner capable of dealing with three-dimensional obstacles is considered to be more suitable for the low-altitude UAV domain. • Quick computational time: As re-planning might be necessary during a mission, it is imperious that a new path be determined quickly. • Completeness: A necessary feature of planning algorithms is that they are complete, that is, they find a solution whenever one exists. • Multi-agent support: Planning paths for multiple agents can result into a considerably large set of path planning problems. Therefore, it is advisable that each planner instance shares information with other instances to improve efficiency in solving the overall problem. Based on these requirements, the PRM-based approach was chosen. PRMs trade off optimality for shorter run-times. Instead of performing a full deterministic search of the configuration space, a
result is found by probabilistic offline sampling and subsequent graph searches. Although a PRM-based algorithm is not complete due to its stochastic nature, it can be shown to be probabilistically complete (Hsu, 1999). Given sufficient iterations, the probability of finding a solution tends to one. 2.2 Task Planning Problem. The ARTIS project focuses on cooperative UAV groups. In such Multi-Agent Systems (MASs), sole path planning is not sufficient for mission planning given multiple goals and multiple agents. A so-called task planner minimizes either the mission duration or the overall flight distance. This requires a more complex mission description. Related Work. Task planning is an instance of the “traveling salesman”2 (TSP) problem. Hence, it is a NP-hard combinatorial optimization problem (Shima, et al. 2006). A previous approach developed for the ARTIS project optimized the flight distance for one agent using a genetic algorithm (Gabrielli, 2005). However, this approach was developed for homogeneous agents and tasks, inspiring work on cases in which tasks require specific agent capabilities or resources. Literature shows that distributed task planning approaches can be flexible and robust. They are less predictable and their optimality is not guaranteed, however, when agent cooperativeness increases. Hence, as a first step, this work focuses on a centralized offline planning approach. In a centralized approach there exists a leader that aggregates information about the other agents, plans optimally for the entire set of agents, and finally propagates the task assignments to each team member. Based on complete situational knowledge, such master-slave architectures have the advantage that an (near) optimal solution can be found. Near-optimal solutions utilize stochastic methods (Shima, et al. 2006; Pongpunwattana, et al. 2003; Stuetzle, et al. 1997) or likewise mixed integer linear programming techniques (Darrah, et al. 2005, Richards, et al. 2002, Shumacher, et al. 2004). These methods have been successfully applied for taskordering in a group. Another promising method is the biologically inspired Ant Colony Optimization (ACO) approach (Dorigo, et al. 1996; Mazza, et al. 2004). As in nature, virtual ants communicate indirectly to each other by depositing and sensing pheromones. The more pheromones deposited, the greater the probability of other ants choosing the same trail. It leads to reinforcement of better trails found by the colony. In particular, the ACO variant in Dorigo (1996), known as Max-Min Ant Systems, is known to returns the best results.
12
http://www.tsp.gatech.edu
Discussion. In the context of the ARTIS project, two main requirements for the task planner exist. First, an operator on the ground needs control over the task planning process. In particular, this forbids approaches that tend to show emergent behaviors (e.g. totally distributed approaches). Second, since configurations may vary among UAVs, it has to consider potential differences in each UAV’s capabilities. Therefore, an ACO is used to approximate solutions to the centralized, NP-hard task order optimization. 3. PATH PLANNER PRM-based path planning has three main phases: • A pre-computation step, in which the PRM itself is generated. • A query step, which consists of a search within the generated PRM using conventional search algorithms. • A post-processing step can be performed to obtain “smoother” paths. 3.1 The Pre-computation Step. Adapted from Petersson (2006), this step generates the PRM, its corresponding search graph and checks the graph’s edges against collisions. PRM Generation. The graph is stored as a node set, N , and a weighted edge set, E . First, N is filled with n randomly selected nodes in 3D space. Second, these nodes are connected, generating the set E . For each node Q , a set of neighbor nodes is chosen and each pair of nodes (Q, Qn ) is connected using a local path planner. Since the nodes in our representation do not have a heading information, straight line connections are sufficient. Otherwise, it would be possible to use splines or circle-line-circle (CLC) curves as node connections. If the local path has no collisions, both nodes and the path cost (Euclidean distance) are added to E . Collision Checking. In general, collision checks demand most of the runtime during path planning. Hence, it is important to have efficient collisiondetecting routines. Popular solutions are AxisAligned Bounding Boxes (AABBs) and Oriented Bounding Boxes (OBBs), which simplify the obstacle representation. However, they tend to overestimate space occupied by obstacles and may fail to represent the environment precisely (e.g. dense urban areas). Thus, an arbitrary polyhedral model is used for obstacle representation. Any object is constituted by a set of plane triangles, arbitrarily oriented in three-dimensional space. All obstacles are assumed to be closed polyhedral surfaces without overlapping. This polyhedral model requires two collision detection routines: One that checks whether a node is in free space and one that assures collision free paths.
The first collision check verifies if a point Q is connected to a point Qn . Qn exceeds the bounds of the environment (e.g. at very high altitude). If this line does not cross any nearby triangles, or an even number of them (implying an obstacle entirely above Q), then Q lies in free space. Otherwise, it is contained within an obstacle. Furthermore it must be ensured that Q lies far enough from obstacles. For that purpose, one must calculate the distance from Q to all nearby triangles. A collision-free path segment crosses no triangles at all, and each point within it lies far enough from obstacles as well as each node in the PRM. Simple geometric routines can implement such tests. The Bridge Test for Narrow Passages. One known shortcoming of sampling methods is that they tend to under-populate narrow passages which lie between obstacles. This is due to their smaller volume compared to the environment. A solution can be found in Hsu (2003), using the so-called bridge test. This algorithm can create samples within narrow passages while still using random-based methods. First, two random samples, Q1 and Q2 , which both lie inside obstacles, are generated. A middle point QB between (Q1 , Q2 ) , called bridge point, is added to the graph if it belongs to free space. 3.2 The Query Step. Once an underlying path has been constructed, finding a path becomes a significantly less complex
r
task. The first step is to connect the start point Po and the finish point
r Pf to the PRM. Then, virtually
any graph search algorithm can be applied to that problem. Due to the existence of a good heuristic (distance of each point to the finish point) and only non-negative weights (Euclidean distance), Dijkstra’s A* was chosen. The usual definition of the A* assigns all nodes to an OPEN list, with all but the start node having g = ∞ , where g is the sum of all costs from the start node to g . In this particular implementation, only the start node is initially added to the OPEN list; nodes which would normally have g = ∞ are not added to any list. This way, less data is stored in the OPEN list, allowing faster node insertion and removal. A heap data structure was used, which allows O(log n) node insertion and O(1) node removal. 3.3 The Post-Processing Step. While the randomness of the PRM shows advantages with respect to short planning time, the paths generated by this process may be full of sharp turns and may be visibly far from an optimal solution. Thus this step aims on optimizing and smoothing a given path by using shortcuts. For a given node Q if
its predecessor Q p and successor Qs may be connected by a collision-free path, then Q is removed. This way, shorter and smoother paths can be generated, with a relatively small number of collision checks. Three pointers (current, previous and next) are iterated through the entire path, searching for possible shortcuts, until no further optimizations can be found.
Scenario 3 simulates an urban-like area with a constrained section (Fig. 3). It can only be accessed through the two arches or over the lower wall. Having both, narrow passages and wide open areas, it is a challenging scenario. Its obstacles comprise 258 triangles. 1000 random points + bridge points 500 random points + bridge points
4
3.5
3.4 Experimental results. This section shows paths and evaluates aspects of different scenarios. Scenario 1 assesses the overall performance by simulating the Hannover International Airport (Fig. 1) with over 400 triangles forming the boundaries of the obstacles. Generating a PRM with 1000 purely random points and only a few hundred “bridge” points results in an acceptable increase in build time (Fig. 4), while significantly improving the path search performance, as seen in the next scenario.
PRM generation time (s)
3
2.5
2
1.5
1
0.5
0 500
600
700
800
900 1000 1100 Number of nodes
1200
1300
1400
1500
Fig. 4. A growing number of “bridge points” results in an acceptable growth of the PRM build time.
Fig. 1. Scenario 1 with the underlying PRM. Each dot marks a sample point in 3D. The edges highlight collision-free paths.
Fig. 5. Scenario 2 shows the advantage of “bridge test” heuristics in combination with randomness.
Scenario 2 assesses “bridge tests” and path smoothing concepts (). It is comprised of deep, narrow corridors and as such a particularly hard setup. Figure 7 shows that the chance of finding a path in 50 experiment repetitions rises significantly with bridge testing.
4. TASK PLANNER
Fig. 2. Scenario 2 assessing the “bridge test” and the path smoothing technique. Bright lines are the “raw” paths, dark lines are smoothed paths.
Fig. 3. Scenario 3 assessing both, narrow passages and wide areas.
To reduce the planning complexity, the task planner is decomposed into the following phases: • The task refinement maps agents to tasks that require certain capabilities. • The conflict solving addresses conflicts that can arise when a task is supposed to be executed by only one agent, but the task refinement identified more than one potential agent. • A scheduling optimizer maximizes the overall mission utility by altering the potentially suboptimal task order of each agent’s task list. 4.1 Task Refinement. Given a group of agents with certain capabilities and a set of tasks with requisites (e.g. task 5 needs a photo camera), the planner searches those agents that map to the required capabilities of tasks. The refinement is represented as capability matrix. Before a further task order optimization can take place, all possible paths are stored in a cost matrix. Given n agents and m tasks, the path planner has to compute all n²+m²+n*m paths.
4.2 Conflict Solver. To solve conflicts, the cost and capability matrixes need to be transformed into a weighted graph representation (Fig. 6). Edge weights represent costs (path length) determined by the path planner. A shortest path search is performed using a Minimum Spanning Tree3. Per conflicting task, the agent with the least cost is assigned to that task.
Fig. 7. Example Scenario with three heterogeneous agents and 30 tasks.
Fig. 6. A mission (left) and an agent’s task list from that mission (right) shown as undirected graph. One conflict occurs between agents G1,G2,G3 for task T5. 4.3 Scheduling Optimizer. During the ACO, artificial ants "walk" in each agent’s graph. Pheromones are deposited on a path in proportional quantity to the solution quality. Each path found corresponds to a possible solution for the optimization problem. Note that ants have memory which they use to take decisions in the search graph (e.g. avoid paths that were already visited before). The paths are chosen probabilistically, proportional to the costs and the amount of pheromones deposited on a path. After the ants’ search converged to a feasible solution, an optimization of search areas (shareable tasks) takes place. The sum of all agents’ path length and the number of turns within the search area define the costs. The minimum cost is computed through combinatorial search. The ACO key parameter is the pheromone evaporation rate, since it sets the search space size. It is determined empirically (Fig. 9). A greedy heuristic solution4 sets the initial seed for the ant’s path position in the search graph. For remaining parameters (e.g. heuristic or pheromone factor) values from literature are used. 4.4 Performance Results The performance of the task planner is measured by the expected mission duration. The test scenario in Fig. 7 is used to vary ACO parameters. It comprises 3 agents, 4 search tasks and 26 waypoint tasks. A significant decrease of the duration is achieved, when >1 agents and >10 tasks are available (Fig. 8). This supports a general speculation in the ARTIS project that “usual” domain scenarios will not require more than 3 or 4 agents. However, robustness can be improved with even more agents.
3 4
http://citeseer.ist.psu.edu/nesetril00otakar.html, Otakar Boruvka http://en.wikipedia.org/wiki/Greedy_algorithm
Fig. 8. Tasks vs Agents in the example scenario: Influence on the mission time for different setups.
Fig. 9. Differences in the convergence rate of the ACO for the scenario in Section 4.4. 5. CONCLUSION This paper showed the feasibility of a combined approach for UAV mission planning using a 3D path planner and a centralized task planner. The proposed path planner has practical advantages over optimal solutions, particularly for the lowaltitude UAV domain. It demands less computational effort for path searches through offline search graph construction. Moreover, it is probabilistically complete and as such an optimal approximation. Implicitly, it supports multi-agent planning through quick path results by using a previously built PRM. Experimental results in Section 3.4 show that the longer duration of a pre-processing step pays off for individual queries, which take milliseconds; a large number of these are required by the task planner. The proposed centralized task planner handles both, heterogeneous UAV agents and tasks that require specific agent capabilities or resources. It approximates an optimal task ordering for individual agents using an ACO variant, which is known to be
the best approach for TSP-like problems. Moreover, it implements UAV cooperativeness by evaluating the overall mission utility of task sharing. The limits of the vehicle’s dynamic behavior pose yet another challenge to the path planning algorithm presented in this work. At low speeds, linear path segments can be followed by the UAV within a certain error distance, which is added to the safety margin used in the planner. As flight speed increases, however, piecewise linear paths must be further smoothed, taking flight dynamics in consideration. Finally, research is underway to enhance the PRM for online path re-planning, where cases of disappearing obstacle are expected to be solved more quickly. Furthermore, the applicability of distributed task planning is evaluated for ARTIS. ACKNOWLEDGEMENTS The authors of this work would like to thank the project FINEP-CAPTAER for supporting Augusto Langer and Lucas Silva regarding their stay at DLR. The ARTIS project is supported by QNX Software Systems through a research grant. The ARTIS flight control computer runs the QNX real-time operating system. REFERENCES Alighanbari, M.; Kwata, Y.; How, J. (2003) CoOrdination And Control Of Multiples Uavs With Timing Constraints And Loitering. Proceedings Of The American Control Conference, vol.6, Pages 5311-5316. Canny, J. F.; Reef, J. (1987) Lower Bounds for Shortest Path and Related Problems. In: Annual IEEE Symposium on Foundations of Computer Science, 28., Los Angeles. Canny, J. F.; Lin, M. C. (2003) An Opportunistic Global Path Planner. Algorithmica, vol.10, n. 24, p. 102-120. Capozzi, B. J. (2001) Evolution-Based Path Planning and Management for Autonomous Vehicles, 363f. These (Doctor of Philosophy – Aeronautics Engineering) – University of Washington, Washington Darrah, M. A.; Niland, W. M.; Stolarik, B. M. (2005) Multiple UAV Dynamic Task Allocation using Mixed Integer Linear Programming in a SEAD Mission. In: Infotec@Aerospace, Arlington, VA. Dittrich, J. S.; Adolf, F. M.; Langer, A.; Thielecke, F. (2007) Mission Planning for Small VTOL UAV Systems in Unknown Environments. In: AHS International Specialists’ Meeting on Unmanned Rotorcraft, American Helicopter Society International, Chandler, AZ. Dorigo, M.; Gambardella, L. M. (1996) Solving Symmetric and Asymmetric TSP by Ant Colonies. In: IEEE International Cooference On Evolutionary Computation. Proceedings IEEE Press, pp. 622-627.
Gabrielli, L. H. (2005) Mission Planning And Dynamic Avoidance For A Group Of Autonomous Agents. 61F. Trabalho De Conclusao De Curso. (Graduacao)- Instituto Tecnologico De Aeronautica, Sao Jose Dos Campos Hsu, D.; Kavraki, L. E.; Latombe, J-C.; Motwani, R. (1999) Capturing The Connectivity Of HighDimensional Geometric Spaces By Parallelizable Random Sampling Techniques. In P.M. Pardalos And S. Rajasekaran, Editors, Advances In Randomized Parallel Computing, Pages 159182. Kluwer Academic Publishers, Boston, MA Hsu, D.; Jiang, T.; Reif, J.; Sun, Z. (2003) The Bridge Test for Sampling Narrow Passages with Probabilistic Path Planners. In IEEE International Conference on Robotics and Automation, Taiwan. Kavraki, L.; Svetska, P.; Latombe, J-C.; Overmars, M. (1994) Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces, CS-TR-94-1519. Mazza, I; Ollero, A. (2004) Multiple UAV Cooperative Searching Operation Using Polygon Area Decomposition and Efficient Coverage Algorithm. In: 7th International Symposium On Distributed Autonomous Robotics Systems, Toulouse, DRAS. Petersson, P-O. (2006) Sampling Based Path Planning for an Autonomous Helicopter. 147f. Tese (Licentiate of Engineering) – Linköping universitet, Linköping. Pongpunwattana, A.; Rysdyk, R.; Vagners, J. Market-Based (2003) Co-Evolution Planning For Multiples Autonomous Vehicles. In: AIAA Unmanned Unlimited Conference. Richards, A.; Bellingham, J.; Tillerson, M.; How, J. (2002) Co-ordination and Control of Multiples UAVs. In: AIAA Guidance, Navigation And Control Conference, Monterey, CA. Shima, T.; Rasmussen, S. J.; Sparks, A. G.; Passino, K. M. (2006) Multiple Task Assignments For Cooperating Uninhabited Aerial Vehicles Using Genetic Algorithms, Computers & Operations Research, vol.33, no.11, p.3252-3269. Shumacher, C.; Chandler, P.; Pachter, M. (2004) UAV Task Assignment with Timing Constraints. AIAA Guidance, Navigation And Control Conference And Exhibit, Austin, TX. Stuetzle, T.; Hoos, H. (1997) Max-Min Ant System And Local Search For Combinatorial Optimization Problems. In: 2Nd International Conference On Metaheuristics, SophiaAntilopis., France, July 21-24. Weerdt, M.; Witteveen, C (2006). Multiagent Planning: Problem Properties That Matter. In: AAAI Spring Symposium On Distributed Plan And Schedule Managements. Proceedings AAAI, 155—156. Xiao, J.; Michalevicz, Z.; Zhang, L.; Trojanowski, K. (1997) Adaptive Evolutionary Planner-Navigator For Mobile Robots. IEEE Transactions On Evolutionary Computation, vol.1, no.1, P. 18-28.