Trajectory Bundle Estimation For Perception-Driven Planning by Abraham Galton Bachrach Submitted to the Department of Electrical Engineering and Computer Science in partial fulfillment of the requirements for the degree of Doctor of Philosophy at the MASSACHUSETTS INSTITUTE OF TECHNOLOGY February 2013 (cid:13)c Massachusetts Institute of Technology 2013. All rights reserved. Author ............................................................. Department of Electrical Engineering and Computer Science December 31, 2012 Certified by ......................................................... Nicholas Roy Professor of Aeronautics and Astronautics Thesis Supervisor Accepted by......................................................... Leslie Kolodziejski Professor of Electrical Engineering and Computer Science Chair, Department Committee on Graduate Students 2 Trajectory Bundle Estimation For Perception-Driven Planning by Abraham Galton Bachrach SubmittedtotheDepartmentofElectricalEngineeringandComputerScience onDecember31,2012,inpartialfulfillmentofthe requirementsforthedegreeof DoctorofPhilosophy Abstract When operating in unknown environments, autonomous vehicles must perceive and un- derstand the environment ahead in order to make effective navigation decisions. Long rangeperceptioncanenableavehicletochooseactionsthattakeitdirectlytowarditsgoal, avoiding dead ends. In addition, the perception range is critically important for ensuring thesafetyofvehicleswithconstraineddynamics. Ingeneral,thefasteravehiclemoves,the more constrained its dynamics become due to acceleration limits imposed by its actuators. Thismeansthatthespeedatwhichanautonomousagentcansafelytravelisoftengoverned by its ability to perceive and understand the environment ahead. Overall, perception range is one of the most important factors that determines the performance of an autonomous vehicle. Today, autonomous vehicles tend to rely exclusively on metric representations built using range sensors to plan paths. However, such sensors are limited by their maximum range, field of view, and occluding obstacles in the foreground. Together, these limitations make up what we call the metric sensing horizon of the vehicle. The first two limitations are generally determined by the weight, size, power, and cost budget allocated to sensing. However,rangesensorswillalwaysbelimitedbyocclusions. If we wish to develop autonomous vehicles that are able to navigate directly toward a goal at high speeds through unknown environments, then we must move beyond the sim- ple range-sensor based techniques. We must develop algorithms that enable autonomous agents to harness knowledge about the structure of the world to interpret additional sensor information (such as appearance information provided by cameras), and make inferences aboutpartsoftheworldthatcannotbedirectlyobserved. Wedevelopanewrepresentationbasedaroundtrajectorybundles,thatmakesthischal- lenging task more tractable. Rather than attempt to explicitly model the geometry of the worldinfrontofthevehicle(whichcanbeincrediblycomplex),wereasonabouttheworld intermsofwhatthevehiclecanandcannotdo. Trajectorybundlesaredesignedtocapture an abstract concept such as the command “go straight and then turn towards the right” in a concrete and actionable manner. We employ a library of trajectory bundles to reason about the layout of obstacles in the environment based on which bundles in the library are predicted to be feasible. Trajectory bundles provide a lens through which we can look at 3 perception tasks, allowing us to leverage machine learning tools in much more effective waysfornavigation. In this thesis we introduce trajectory bundles, and develop algorithms that use them to enable perception-driven planning. We develop a trajectory clustering algorithm that enables us to construct a set of trajectory bundles. We then develop a Bayesian filtering framework that enables us to estimate a belief over which trajectory bundles are feasible based on the history of actions and observations of the vehicle. We test our algorithms by using them to navigate a simulated fixed wing air vehicle at high speeds through an unknownenvironmentusingamonocularcamerasensor. ThesisSupervisor: NicholasRoy Title: ProfessorofAeronauticsandAstronautics 4 Acknowledgments Ihavelearnedsomuch,andhadsomuchfuntheselastseveralyears. Iamextremelylucky tohavehadsuchanamazingopportunity. Iwouldliketothankthemanypeoplewhohave helped me get here, and who have made the experience what it was. Without your patient support,guidance,friendshipandloveIwouldnotbewhereIamtoday. In particular, I would like to thank my advisor, professor Nicholas Roy for his support and guidance. He has been a great sounding board for ideas, and instrumental in molding my loose ramblings into coherent research. He has in all ways exemplified what it means tobeamentor. MythesiscommitteeconsistingofSethTeller,andTomasLozano-Perezwhohavebeen invaluable resources, and helped refine both the thesis problem, as well as the technical approach. In addition, I would like to thank professors Russ Tedrake, John Leonard, Bill Freeman, and Antonio Torralba for always being happy to discuss my research, and give metheirtakeonthings. I would also like to thank the many friends and collaborators who I have worked with. Samuel Prentice and Ruijie He, my conspirators in everything quadrotor related. Adam Bry for bringing a host of new challenges and ideas to make our research more dynamic and exciting. John Roberts for always being willing to debate anything and everything. Ourmanydartgameswereimportantrelaxation,yetalsofosteredmanyimportantresearch discussions. AlbertHuangfortheusefulguidance,instruction,andalwaysbeingwillingto show me how to do things “the right way”. Thanks to Charles Richter, Will Vega-Brown, and Johnathan Kelly for continuing on with the air vehicle research. I cannot wait to see wherethingsprogress. I also benefited tremendously from all the discussions about anything and everything with everyone else in 33x and elsewhere in CSAIL. In particular, I’d like to thank (in alphabetical order) Alex Bahr, Marec Doniec, Maurice Fallon, Hordur Johannsson, Josh Joseph,MichaelKaess,SertacKaraman,RossKnepper,OlivierKoch,TomKollar,Stefanie Tellex,FinaleDoshi-Velez,JavierVelez,andMattWalter. Thankyouforbeingsogenerous withyourtime. 5 In addition to the people more directly involved in my academic pursuits, I would like to thank my friends in Boston, California, and elsewhere who helped distract me from my work when needed to keep me sane. In particular Katharine Wolf, for your constant care, affection, support, and also for always helping me see that the glass was half full (and this thesiswashalfdone;-) Finally, I am grateful to my sisters Rashmi, Devra, Lela, and Bimla for always encour- aging me, and being such great role models for me to try to live up to. Last, but certainly not least, I would like to thank my parents for always supporting me, while at the same timepushingmetoexcel. Thankyouallsomuch! 6 Contents 1 Introduction 13 1.1 Perception-DrivenPlanning . . . . . . . . . . . . . . . . . . . . . . . . . . 17 1.2 TrajectoryBundles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 1.3 EstimationofTrajectoryBundleFeasibility . . . . . . . . . . . . . . . . . 26 1.4 ThesisGoal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 1.5 Hypothesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 1.6 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 1.7 Organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 2 RelatedWork 31 2.1 EnvironmentRepresentations . . . . . . . . . . . . . . . . . . . . . . . . . 31 2.1.1 PointClouds . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 2.1.2 OccupancyMaps . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 2.1.3 PolygonalObstacleMaps . . . . . . . . . . . . . . . . . . . . . . 35 2.1.4 TopologicalMaps . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 2.2 PlanningThroughUnknownEnvironments . . . . . . . . . . . . . . . . . 37 2.2.1 OccupancyMapBasedNavigation . . . . . . . . . . . . . . . . . . 38 2.2.2 OpticalFlowBasedObstacleAvoidance . . . . . . . . . . . . . . . 39 2.2.3 MappingSensingToActions . . . . . . . . . . . . . . . . . . . . . 40 2.2.4 DepthFromaMonocularImage . . . . . . . . . . . . . . . . . . . 42 2.2.5 TrajectoryLibraries . . . . . . . . . . . . . . . . . . . . . . . . . . 44 2.2.6 TrajectoryPrediction . . . . . . . . . . . . . . . . . . . . . . . . . 45 2.2.7 DARPALearningAppliedtoGroundRobotsProgram . . . . . . . 47 7 3 TrajectoryLibraryBasedPlanning 49 3.1 SimulationModel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 3.1.1 VehicleModel . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 3.1.2 SensorModels . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 3.1.3 EnvironmentModel . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.2 BaselinePlanningSystem . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 3.2.1 TrajectoryLibraryGeneration . . . . . . . . . . . . . . . . . . . . 53 3.2.2 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 3.2.3 Backtracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55 3.3 BaselinePlanningPerformance . . . . . . . . . . . . . . . . . . . . . . . . 56 4 TrajectoryBundleLibraryGeneration 61 4.1 TrajectorySimilarityMetrics . . . . . . . . . . . . . . . . . . . . . . . . . 62 4.1.1 DynamicTimeWarping . . . . . . . . . . . . . . . . . . . . . . . 63 4.2 AffinityPropagation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.3 ClusteringAlgorithmEvaluation . . . . . . . . . . . . . . . . . . . . . . . 67 4.4 TrajectoryBundleLibraryGeneration . . . . . . . . . . . . . . . . . . . . 69 5 TrajectoryBundlePrediction 71 5.1 NearestNeighborPrediction . . . . . . . . . . . . . . . . . . . . . . . . . 72 5.1.1 ClassificationPerformance . . . . . . . . . . . . . . . . . . . . . . 75 5.1.2 OccupancyMapPrediction . . . . . . . . . . . . . . . . . . . . . . 77 5.2 SpatialConditionalRandomFieldModel . . . . . . . . . . . . . . . . . . 79 5.2.1 Independent-NodeShared-FeatureModel . . . . . . . . . . . . . . 82 5.2.2 Chow-LiuTreeModel . . . . . . . . . . . . . . . . . . . . . . . . 83 5.2.3 CRFPredictionPerformance . . . . . . . . . . . . . . . . . . . . . 84 5.3 PlanningPerformance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85 6 BayesianFilteringInTheSpaceofTrajectoryBundles 89 6.1 HiddenMarkovModels . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 6.2 DynamicBayesNetworkModels . . . . . . . . . . . . . . . . . . . . . . . 92 8 6.3 BayesFilterModel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93 6.4 Inference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 6.5 PredictionAccuracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96 6.6 PlanningPerformance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 6.6.1 CombiningPredictionswithRangeSensing . . . . . . . . . . . . . 99 7 ExperimentalAnalysis 101 7.1 DataCollection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102 7.2 Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 8 Conclusion 107 8.1 FutureWork . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 9 10