SAMPLING-BASED MOTION PLANNING ALGORITHMS: ANALYSIS AND DEVELOPMENT by NATHAN ALEXANDER WEDGE Submitted in partial fulfillment of the requirements for the degree of Doctor of Philosophy Dissertation Advisor: Dr. Michael S. Branicky Department of Electrical Engineering and Computer Science CASE WESTERN RESERVE UNIVERSITY May, 2011 CASE WESTERN RESERVE UNIVERSITY SCHOOL OF GRADUATE STUDIES We hereby approve the dissertation of Nathan Alexander Wedge candidate for the Doctor of Philosophy degree∗. Michael S. Branicky (committee chair) M. Cenk C¸avu¸so˘glu (committee member) Wyatt S. Newman (committee member) Soumya Ray (committee member) March 25, 2011 ∗We also certify that written approval has been obtained for any proprietary material contained herein. Table of Contents List of Tables v List of Figures vi List of Algorithms ix Acknowledgements x Abstract xii 1 Introduction 14 1.1 Motion Planning . 14 1.1.1 Problem Characterizations . 15 1.2 Experimental Problems . 18 1.3 Contributions and Outline . 19 2 Background 23 2.1 Combinatorial Planning . 23 2.2 Sampling-based Planning Methodology . 25 2.2.1 Sampling and Randomization . 26 2.2.2 Distance and Metrics . 27 2.2.3 Local Planning . 28 2.2.4 Collision Detection . 29 i 2.3 Sampling-based Planning Algorithms . 32 2.3.1 Randomized Potential Fields . 33 2.3.2 Ariadne's Clew . 35 2.3.3 Single-query Bidirectional Lazy (SBL) . 35 2.3.4 Probabilistic Roadmap (PRM) . 38 2.3.5 Rapidly-exploring Random Tree (RRT) . 42 3 Rapidly-exploring Random Tree Analysis 47 3.1 Simplified Models . 48 3.1.1 One-dimensional Model . 49 3.1.2 Markov Chain Model . 53 3.2 Exponential and Power Decay Regimes . 59 3.3 Parameterization and Heuristics . 65 3.3.1 Step Size . 66 3.3.2 Extend versus Connect .................... 69 3.4 Implications for Problem Difficulty . 72 3.5 Summary . 76 4 Distributions and Restarts 78 4.1 Distributions to Runtimes and Restarts . 79 4.2 Continuous Problems . 82 4.2.1 Experiments . 83 4.3 Discrete Problems . 86 4.3.1 Planner Variations . 87 4.3.2 Experiments . 87 4.4 Generalizing Restarts . 90 4.4.1 General Queries . 91 4.4.2 Task-based Decomposition . 94 ii 4.4.3 Algorithmic Measures . 98 4.5 Summary . 100 5 Neighborhood-based Expansion 101 5.1 Locally-isolated Expansion . 101 5.2 Path-length Annexed Random Tree (PART) . 104 5.3 Performance . 109 5.3.1 Demonstrative Problems . 110 5.3.2 Realistic Benchmarks . 113 5.4 Roadmaps and Paths . 117 5.5 Summary . 119 6 Local Obstacle Adaptation 121 6.1 Cost-to-come Thresholds . 121 6.2 Potentially-reachable Regions . 123 6.3 Adaptive PART (APART) . 125 6.4 Performance . 129 6.4.1 Demonstrative Problems . 130 6.4.2 Realistic Benchmarks . 132 6.5 Roadmaps and Paths . 136 6.6 Summary . 138 7 Conclusion 140 7.1 Future Work . 143 7.1.1 Extended Algorithm Models . 143 7.1.2 Informed Restart Strategies . 143 7.1.3 APART with Disconnected Components . 144 7.1.4 Balancing APART Exploration . 144 7.1.5 Collision Checking in APART Path Processing . 145 iii A Derivations 146 A.1 One-dimensional RRT Model . 146 A.1.1 Recurrence . 146 A.1.2 Distribution . 147 A.1.3 Approximations . 149 Geometric . 149 Negative Binomial . 150 A.2 Power Decay . 151 A.3 Constant Restart Intervals . 152 A.3.1 Mean . 152 A.3.2 Variance . 153 A.3.3 Usefulness . 154 A.3.4 Optimality . 154 A.4 Na¨ıve Neighbor Scaling . 155 B Implementation 156 B.1 Algorithm Terminology and Notation . 156 B.2 Software . 158 Bibliography 161 iv List of Tables 5.1 Maze (bidirectional) simulation results evaluating the PART planner. 111 5.2 Kinked tunnel (bidirectional) simulation results evaluating the PART planner. 112 5.3 Bug trap (unidirectional) simulation results evaluating the PART plan- ner...................................... 113 5.4 Flange 0:95 (unidirectional) simulation results evaluating the PART planner. 116 5.5 Alpha puzzle 1:1 (bidirectional) simulation results evaluating the PART planner. 117 6.1 Maze (bidirectional) simulation results evaluating the APART planner. 130 6.2 Kinked tunnel (bidirectional) simulation results evaluating the APART planner. 131 6.3 Bug trap (unidirectional) simulation results evaluating the APART planner. 132 6.4 Flange 0:95 (unidirectional) simulation results evaluating the APART planner. 133 6.5 Alpha puzzle 1:1 (bidirectional) simulation results evaluating the APART planner. 134 6.6 Mean local tree counts for the PART and APART planners. 137 v List of Figures 1.1 An example mapping from work space to configuration space. 16 1.2 Two-dimensional experimental problems. 20 1.3 Six-dimensional experimental problems. 20 2.1 An example of combinatorial planning for a two-dimensional setting. 25 2.2 The top four layers of an oriented bounding box hierarchy. 30 2.3 A potential field function for two dimensions. 34 2.4 Path segment trees in the SBL planner. 38 2.5 Roadmaps generated by the PRM planner. 40 2.6 Non-uniform sampling distributions for the PRM planner. 41 2.7 Trees created by the RRT planner. 43 2.8 Voronoi decompositions for in-progress instances of the RRT planner. 45 2.9 Effective sampling regions from the DD-RRT planner. 46 3.1 Setup of the one-dimensional model of the RRT planner. 50 3.2 Edge effects in the (bidirectional) RRT planner. 53 3.3 Computed performance of the RRT planner in one dimension via Markov chain fundamental matrix. 56 3.4 Example discretized Markov chain models for the RRT planner. 57 3.5 Discretized Markov chain model for the RRT planner. 59 vi 3.6 Dynamics of the RRT planner in the nine-state environment as mod- eled via Markov chain. 60 3.7 Diversely-sized minimum (Euclidean) Voronoi regions in different nar- row passage situations. 61 3.8 Generalized Voronoi region volume changes with sampling. 62 3.9 Sequence of Voronoi visibility during the RRT planner's expansion on atube.................................... 63 3.10 Transition between power and exponential decay in the RRT planner. 65 3.11 Comparison of small and large step size in the RRT planner. 67 3.12 Densities of nodes created by the RRT planner with Manhattan metric at specific iterations. 68 3.13 Densities of nodes created by the RRT planner with Euclidean metric at specific iterations. 68 3.14 Densities of nodes created by the RRT planner with Chebyshev metric at specific iterations. 69 3.15 Average-case RRT instances over metric and step size. 70 3.16 Effects of step size and heuristic on overall RRT planner performance. 72 3.17 Collision checking performance of the RRT planner on a bug trap ver- sus query states. 73 3.18 Node density in the RRT planner by performance. 74 3.19 \Trick" states for the RRT planner on realistic disassembly/assembly problems. 75 3.20 RRT planner growth bias due to faraway \trick" states. 76 4.1 Balanced universal restart strategy. 80 4.2 Restart statistical diagrams. 81 4.3 The usefulness of restarts on various versions of the kinked tunnel. 84 4.4 Restart benefits for two easier versions of the alpha puzzle. 85 vii 4.5 Impact of nearest neighbor computation method on runtimes and restarts in the RRT planner. 86 4.6 Discrete example problems. 88 4.7 Performance and restart impact for RRT variants on exchange. 89 4.8 Performance and restart impact for RRT variants on 15-puzzle. 90 4.9 Constant and universal restart strategy performance of the RRT plan- ner on the kinked tunnel with a random sequence of queries. 92 4.10 RRT planner instances solving tasks. 95 4.11 Runtime survivor functions for RRT planner solving tasks. 95 4.12 Voronoi visibility issues on the.