Multi-Goal Path Planning for Cooperative Sensing
Total Page:16
File Type:pdf, Size:1020Kb
CZECH TECHNICAL UNIVERSITY IN PRAGUE FACULTY OF ELECTRICAL ENGINEERING Doctoral Thesis February, 2010 Jan Faigl CZECH TECHNICAL UNIVERSITY IN PRAGUE FACULTY OF ELECTRICAL ENGINEERING DEPARTMENT OF CYBERNETICS GERSTNER LABORATORY MULTI-GOAL PATH PLANNING FOR COOPERATIVE SENSING Doctoral Thesis Jan Faigl Prague, February, 2010 Ph.D. Programme: Electrical Engineering and Information Technology Branch of study: Artificial Intelligence and Biocybernetics Supervisor: Ing. Libor Pˇreuˇcil,CSc. Abstract The thesis deals with the multi-goal path planning problem. The problem is studied in the context of the inspection task of cooperating robots in a search and rescue mission. Two models of the sensing are considered: continuous and discrete. An environment is a priori known and it is represented by a polygonal domain in both approaches. The con- tinuous sensing assumes relatively inexpensive cost of sensing in comparison to the cost of motion and can be formulated as the Watchman Route Problem (WRP). The discrete sensing leads to the decoupled approach that is motivated by problems where the cost of sensing is dominant. The decoupled approach consists of two problems: the problem of finding minimal set of sensing locations, and the multi-goal path planning problem to find a path visiting the found locations. The set of sensing locations can be found as a solution of the Art Gallery Problem (AGP) or sensor placement problem if additional vis- ibility constraints have to be considered, e.g. visibility range, incident angle. The multi- goal path planning problem can be formulated as the Traveling Salesman Problem (TSP) in which paths between cities have to be traversable by the robot. All three problems (WRP, AGP, TSP) are known to be NP-hard, thus approximate algorithms are considered to find solutions in a reasonable time. The cooperation of several robots is formulated as the multi-robot variant of the WRP, resp. the TSP, with the MinMax criterion. A new sensor placement algorithm, called the Boundary Placement is proposed to find a set of sensing locations with restricted visibility range. The algorithm is compared with approaches based on convex polygon partitioning and the randomized dual sampling schema. The proposed algorithm outperforms both algorithms in the number of found locations and also in the required length of the inspection path. The Self-Organizing Map (SOM) for the TSP is considered in a polygonal domain W in order to solve the multi-goal path planning problem. The SOM adaptation proce- dure modifies weights of neurons according to city presented to the neural network. The weights represent nodes that are organized into a ring of nodes and the ring represents a solution of the TSP. Computations of node–city paths and distances are supported by three approaches: an approximation of the geodesic path based on the convex partition of W, navigation functions, and a triangular mesh of W. The proposed algorithm is compu- tationally feasible and it provides competitive results to the GENIUS heuristic. The SOM based algorithm with navigation functions is applied to the generalized multi-goal path planning problem where a goal can be represented by a set of points instead of a single point. The proposed algorithms are demonstrated in the inspection planning with segment sensing locations, and in the problem of cooperative visits of con- vex areas of interest. A new adaptation procedure is proposed to solve the WRP with restricted visibility range in a polygonal domain W. The computation of continuous sensing is supported by a triangular mesh of W and a convex cover of W. The procedure is also used to find solutions of the MWRP variants: independent patrolling routes, and closed routes starting from a common depot. The problem of the multi-goal path planning with respect to kinematic and kinody- namic constraints is considered as a trajectory generation based on the found path. More- over a new motion planner called RRT–Pathext is proposed. The planner is able to find a solution of the multi-goal motion planning problem and it is used to regenerate a ring of nodes in the SOM based algorithm for the WRP. A solution of the WRP is found as an in- spection trajectory for one or several mobile robots satisfying kinematic and kinodynamic constraints. “Any sufficiently advanced technology is in- distinguishable from magic.” Arthur C. Clarke Acknowledgments I would like to express my thanks to my supervisor Libor Preuˇ cilˇ and colleagues at Intel- ligent and Mobile Robotics Group. I would like to express my special thanks to Miroslav Kulich who introduced me to the problematic and provided me valuable suggestions. My thanks also go to my student Jan Macˇak´ for his implementation of the finite element method and Vojta Vonasek,´ who became my colleague at IMR, for his implementation of the GENIUS algorithm, and collaboration in motion planning. Lastly, I would like to thank my family for their endless toleration and support during my research. The work presented in this thesis was supported by the European Union projects FP5- IST-38873 “PeLoTe”, FP6-IST-506958 “ECOLEAD”, Ministry of Education of the Czech Republic under projects Kontakt 10-2005-06, 2C06005, MSM6840770038, and FRVSˇ grants G1 2042/2004, G1 1606/2005 and by Czech Technical University grant CTU 0505413. Contents 1 Introduction 1 1.1 Problem motivation . .1 1.2 Inspection Task . .2 1.2.1 A Group of Cooperating Mobile Robots . .4 1.2.2 Remark about Research Direction . .5 1.3 Preview of the Thesis Contributions . .5 1.4 Thesis outline . .6 2 Related Work 7 2.1 Environment Representation . .7 2.2 Path Planning and Mobile Robot Navigation Tasks . .8 2.2.1 Motion Planning . .9 2.2.2 Coverage, Inspection, Pursuit-Evasion and Related Tasks . 12 2.3 Art Gallery and Watchmen Routes Problems . 15 2.4 Routing Problems . 20 2.5 Discussion and Research Directions . 24 3 Problem Specification and Thesis Goals 25 3.1 Problem Specification . 25 3.2 Thesis Goals . 25 4 Finding Sensing Locations 27 4.1 Problem Specification . 29 4.2 Supporting Algorithms . 29 4.2.1 Environment Representation . 29 4.2.2 Polygon Filtering . 30 4.2.3 Obstacle Growing . 31 4.2.4 Computation of Visibility . 32 4.3 AGP - Algorithms . 33 4.3.1 Convex Polygon Partitioning - CPP . 33 i 4.3.2 Randomized Dual Sampling Schema - RDS . 34 4.3.3 Boundary Placement - BP . 35 4.4 Experimental Results . 41 4.4.1 Algorithm CPP . 42 4.4.2 RDS - Randomized Dual Sampling Algorithm . 43 4.4.3 BP - Boundary Placement Algorithm . 46 4.4.4 Algorithm Comparison . 48 4.5 Discussion . 49 5 Multi-Goal Path Planning for Cooperating Robots 51 5.1 Related Work . 52 5.1.1 GENIUS . 53 5.1.2 Self-Organizing Neural Network - SOM . 53 5.2 Solving MTSP by SOM in a Polygonal Domain . 61 5.2.1 Shortest Path Maps Approach . 61 5.2.2 Approximate Solution of the Shortest Path Query . 63 5.2.3 Refinement of the Approximation of the Shortest Path . 65 5.2.4 Determination of the Ring Length . 66 5.2.5 Alternative Stop Condition of the SOM Adaptation Process . 68 5.3 SOM Adaptation Rule for a Graph Input . 69 5.4 Quality of Cooperative Plan . 70 5.5 Multi-Goal Path Planning Problem Variants . 71 5.5.1 Path Replanning - Multi-Depot MTSP . 71 5.5.2 Path Planning for Cooperating Heterogenous Entities . 71 5.6 Experimental Results . 73 5.6.1 Refinement of the Shortest Path Approximation . 74 5.6.2 Determination of the Ring Length . 74 5.6.3 Alternative Stop Condition . 75 5.6.4 Algorithm Comparison . 75 5.6.5 Required Computational Time . 77 5.6.6 Adaptation on a Triangular Mesh . 78 5.7 Discussion . 78 ii 6 Generalized Multi-Goal Path Planning 81 6.1 Inspection Planning for Segment Sensing Locations . 81 6.1.1 Art Gallery Problem with Segment Guards . 82 6.1.2 Artificial Potential Field . 83 6.1.3 MTSP with APF Navigation Functions . 85 6.1.4 SOM Adaptation Procedure for the Segment Goals . 86 6.2 Inspection Planning for Areas of Interest . 88 6.2.1 Adaptation Procedure for the AoIs . 89 6.2.2 Representative of the AoI . 90 6.2.3 Adaptation with Supporting Triangular Mesh . 91 6.3 Discussion . 91 7 Inspection Planning with Continuous Sensing 93 7.1 Supporting Structures and Algorithms . 94 7.1.1 Finding a Convex Cover . 95 7.1.2 Determination of Coverage of the Ring . 97 7.2 SOM Adaptation Procedure for the WRP . 98 7.3 Multiple Watchmen Route Problem - MWRP . 101 7.3.1 MWRP - Independent Patrolling Routes . 101 7.3.2 MWRP with the Common Depot . 102 7.4 Experiments . 104 7.5 Discussion . 107 8 Multi-Goal Path Planning with Trajectory Generation 109 8.1 Preliminary Work . 110 8.1.1 Velocity Profile for Smooth Paths . 110 8.1.2 Velocity Profile for Straight Line Segment Paths . 111 8.1.3 Discussion of Preliminary Work . 112 8.2 RRT–Pathext Algorithm - Multi-Goal Motion Planner . 113 8.2.1 RRT–Path Algorithm . 113 8.2.2 RRT–Pathext Algorithm . 115 8.2.3 Experimental Results . 118 8.3 Adaptation procedure for trajectories . 120 8.4 Discussion . 122 9 Conclusion 123 9.1 Conclusion . 123 9.2 Future Work . 125 iii A Algorithm Experiments – AGP 127 A.1 The CPP algorithm . 127 A.2 The RDS algorithm . 129 A.3 The BP algorithm . 131 A.4 Algorithms Comparison . 134 A.5 Other Environments . 135 B Algorithm Experiments – TSP/MTSP 137 B.1 Testing Environments . ..