Path Planning in Dynamic Environments Typesetting: This thesis was typeset by the author using LATEX2 Cover design: The cover was designed by Lucius, Groningen, using Adobe Illustrator CS2 Cover illustration: Lucius, Compositie 29 in groen en geel: ‘Lentewandeling’ (detail) Printed in The Netherlands by Drukkerij Bariet, Ruinen ISBN: 978-90-393-4480-4 Copyright © 2007 by Jur van den Berg. All rights reserved. Path Planning in Dynamic Environments Padplanning in dynamische omgevingen (met een samenvatting in het Nederlands) Proefschrift ter verkrijging van de graad van doctor aan de Universiteit Utrechtopgezagvanderectormagnificus,prof.dr.W.H. Gispen, ingevolge het besluit van het college voor promoties in het openbaar te verdedigen op woensdag 4 april 2007 des middags te 2.30 uur door JurPietervandenBerg geboren op 27 mei 1981 te Groningen Promotor: Prof. dr. M.H. Overmars This work was financially supported by the IST Programme of the EU as a Shared-cost RTD (FET Open) Project under Contract No IST-2001-39250 (MOVIE - Motion Planning in Virtual Environments). The Road Not Taken Two roads diverged in a yellow wood, And sorry I could not travel both And be one traveler, long I stood And looked down one as far as I could To where it bent in the undergrowth; Then took the other, as just as fair, And having perhaps the better claim, Because it was grassy and wanted wear; Though as for that the passing there Had worn them really about the same, And both that morning equally lay In leaves no step had trodden black. Oh, I kept the first for another day! Yet knowing how way leads on to way, I doubted if I should ever come back. I shall be telling this with a sigh Somewhere ages and ages hence: Two roads diverged in a wood, and I — I took the one less traveled by, And that has made all the difference. Robert Frost, 1916 Contents 1 Introduction 1 1.1ConfigurationSpace................................... 2 1.2PathPlanninginStaticEnvironments......................... 3 1.2.1Two-DimensionalConfigurationSpaces................... 4 1.2.2HigherDimensionalConfigurationSpaces.................. 5 1.2.3IncompleteApproaches............................. 7 1.2.4Single-Shotvs.Multiple-Shot.......................... 7 1.3PathPlanninginDynamicEnvironments....................... 8 1.3.1Configuration-TimeSpace........................... 8 1.3.2PlanninginKnownConfiguration-TimeSpaces............... 10 1.3.3OnlinePlanningvs.OfflinePlanning..................... 11 1.3.4PlanninginPartiallyKnownDynamicEnvironments............ 12 1.3.5AccelerationBoundsvs.VelocityBounds................... 13 1.4Corridors.......................................... 13 1.4.1Definition..................................... 14 1.4.2PlanningBackbonePathsforCorridors.................... 15 1.5OrganizationofthisThesis............................... 15 1.5.1PlanninginDynamicEnvironments...................... 16 1.5.2CorridorPlanning................................ 17 2Preliminaries 19 2.1ShortestPathAlgorithms................................ 19 2.1.1Dijkstra’s...................................... 19 2.1.2A*.......................................... 20 2.2ProbabilisticRoadmapMethods............................ 22 2.2.1 The Basic PRM Approach............................ 22 2.2.2DetailsandVariants............................... 23 2.2.3Analysis...................................... 25 vii Contents I Planning in Dynamic Environments 27 3 Planning in Known Dynamic Environments 29 3.1Introduction........................................ 29 3.2ProblemDescription................................... 31 3.2.1TheRoadmap................................... 31 3.2.2TheProblem.................................... 32 3.2.3Discretization................................... 32 3.3GlobalApproach..................................... 33 3.4LocalPaths......................................... 35 3.4.1TheConfiguration-TimeGrid.......................... 35 3.4.2FindingaLocalPath............................... 36 3.5GlobalPaths........................................ 39 3.5.1TheIntervalTree................................. 40 3.5.2Probes....................................... 40 3.5.3FindingaGlobalPath.............................. 42 3.5.4DeterminingwhetheranIntervalisUnvisited................ 43 3.5.5CoordinatingMultipleProbesonanEdge.................. 43 3.6Optimizations....................................... 44 3.6.1LaunchingProbes................................. 45 3.6.2DeletingProbes.................................. 45 3.6.3ClosingandOpeningVertices.......................... 45 3.7ExperimentalResults................................... 46 3.7.1APreliminaryExperiment............................ 47 3.7.2VaryingtheQuantities.............................. 48 3.7.3ArticulatedRobots................................ 52 3.7.4ComparisonwiththeStraightforwardApproach............... 52 3.8DiscussionandConclusion............................... 53 4 Planning for Multiple Robots 55 4.1Introduction........................................ 55 4.2ProblemDefinition.................................... 57 4.2.1Definition..................................... 57 4.2.2Discretization................................... 57 4.2.3QualityMeasure.................................. 58 4.3PrioritizedPlanning................................... 58 4.3.1PathPlanninginDynamicEnvironments................... 58 4.3.2Prioritization................................... 60 4.3.3AnExample.................................... 61 4.4AnalyzingthePrioritization............................... 61 4.5ComparisonwithaCoordinatedApproach...................... 65 4.5.1OptimalRoadmapCoordination........................ 65 4.5.2ExperimentalResults............................... 66 4.6AnalyzingtheSpectrum................................. 67 4.7Conclusion......................................... 69 viii Contents 5 Planning in Partially-Known Dynamic Environments 71 5.1Introduction........................................ 72 5.2ProblemDescription................................... 73 5.3Approach.......................................... 74 5.3.1ConstructingtheRoadmap........................... 75 5.3.2PlanningovertheRoadmap........................... 75 5.3.3RepairingthePlan................................ 77 5.4ExperimentsandResults................................. 79 5.4.1ImplementationDetails............................. 79 5.4.2ExperimentalSetup............................... 80 5.4.3Results....................................... 81 5.4.4Extensions..................................... 83 5.5Discussion......................................... 83 6 Planning in Unpredictable Dynamic Environments 85 6.1Introduction........................................ 86 6.2ProblemDefinition.................................... 87 6.3PropertiesofShortestPaths............................... 88 6.3.1MaximalVelocity................................. 89 6.3.2Straight-LineSegmentsandSpiralSegments................. 89 6.3.3PathSmoothness................................. 90 6.3.4DepartureCurves................................. 90 6.4ANaiveAlgorithm..................................... 92 6.5AnEfficientAlgorithm.................................. 94 6.5.1DiscswithEqualGrowthRates......................... 94 6.5.2GeneralCase:DiscsHaveDifferentGrowthRates.............. 96 6.5.3ImplementationDetails............................. 97 6.6ExperimentalResults................................... 98 6.7Conclusion.........................................100 7 Planning in Repetitive Dynamic Environments 101 7.1Introduction........................................101 7.2NaiveApproach......................................103 7.2.1ProblemSpecification..............................103 7.2.2 PRM forStaticEnvironments..........................104 7.2.3 PRM forPeriodicConfiguration-TimeSpaces................104 7.2.4QueryStage....................................106 7.3AdvancedApproach...................................107 7.3.1 Advanced PRM forRepetitiveEnvironments.................108 7.3.2QueryStage....................................109 7.3.3OverlappingMovingObstacles.........................110 7.4Experiments........................................111 7.5Conclusion.........................................113 ix Contents II Corridor Planning 115 8 The Visibility-Voronoi Complex 117 8.1Introduction........................................117 8.1.1Applications....................................119 8.1.2Outline.......................................120 8.2Preliminaries.......................................120 8.2.1VisibilityGraphs.................................120 8.2.2VoronoiDiagramsofPolygons.........................121 8.2.3MinkowskiSums.................................122 8.3 The VV(c)-Diagram....................................123 8.3.1 Constructing the VV(c)-Diagram........................123 8.3.2 Querying the VV(c)-Diagram..........................125 8.4TheVV-Complex.....................................125 8.4.1ThePreprocessingStage.............................128 8.4.2QueryingtheVV-Complex............................132 8.4.3ProofofCorrectness...............................134 8.4.4ComplexityAnalysis...............................139 8.4.5HandlingNon-ConvexObstacles........................140 8.5ImplementationDetails.................................141 8.5.1VoronoiArcs....................................142 8.5.2DilatedObstacleBoundaries..........................143 8.6ExperimentalResults...................................143 8.7Conclusion.........................................145 9 Planning Optimal Corridors 147