Parallel Algorithms for Real-time Motion Planning Matthew McNaughton [email protected] CMU-RI-TR-xx-xx Submitted in partial fulfillment of the requirements for the degree of Doctor of Philosophy in Robotics The Robotics Institute Carnegie Mellon University Pittsburgh, Pennsylvania 15213 July 2011 Thesis Committee: Chris Urmson, Chair Anthony Stentz Guy Blelloch, Computer Science Department Dmitri Dolgov, Google Inc. c 2011 BY MATTHEW MCNAUGHTON.ALL RIGHTS RESERVED. This research was supported through the General Motors/Carnegie Mellon University Autonomous Driving Collaborative Research Laboratory. Abstract For decades, humans have dreamed of making cars that could drive themselves, so that travel would be less taxing, and the roads safer for everyone. Toward this goal, we have made strides in motion planning algorithms for autonomous cars, using a powerful new computing tool, the parallel graphics processing unit (GPU). We propose a novel five-dimensional search space formulation that includes both spatial and temporal dimensions, and respects the kinematic and dynamic constraints on a typical automobile. With this formulation, the search space grows linearly with the length of the path, compared to the exponential growth of other methods. We also propose a parallel search algorithm, using the GPU to tackle the curse of dimensionality directly and increase the number of plans that can be evaluated by an order of magnitude compared to a CPU implementation. With this larger capacity, we can evaluate a dense sampling of plans combining lateral swerves and accelerations that represent a range of effective responses to more on-road driving scenarios than have previously been addressed in the literature. We contribute a cost function that evaluates many aspects of each candidate plan, ranking them all, and allowing the behavior of the vehicle to be fine-tuned by changing the ranking. We show that the cost function can be changed on-line by a behavioral planning layer to express preferred vehicle behavior without the brittleness induced by top-down planning architectures. Our method is particu- larly effective at generating robust merging behaviors, which have traditionally required a delicate and failure-prone coordination between multiple planning lay- ers. Finally, we demonstrate our proposed planner in a variety of on-road driving scenarios in both simulation and on an autonomous SUV, and make a detailed comparison with prior work. Acknowledgements I had a lot of help with this work. While I have been learning how to program robot cars, my long-suffering wife, Jenney, has become an expert at talking down over- wrought graduate students. She has made many sacrifices to be with me through this time. She deserves more than I could hope to repay. I love this woman. Many thanks go to my advisor, Chris Urmson, for recruiting me to the Tartan Racing Team. It was an unparalleled privilege to work with so many incredibly competent, hard-working people. For that matter, thanks to the team for being such, and writing the Tartan Racing code base that I have used to support my work. Extra thanks to Jarrod Snider for keeping Boss in shape for the last four years. Thanks also to Chris for the opportunities he has given me since the big race, for keeping me on track to finish this dissertation, and gently but firmly pushing me out of the nest. There’s more waiting out there to be done. I thank GM for supporting my work, and gamely going along with my ap- proach to the problem. John Dolan deserves my undying gratitude for expertly managing our relationship with GM and for his sharp-eyed reading of many, many words on short notice. Thanks go again to Jenney, and to my dear friends Marek Michalowski, Maxim Makatchev, Karen Hecht, and Seth Ciotti, for driving and filming many test laps around the track with me, Boss, my Subaru, and an old GMC Sierra. A long, long time ago, at the University of Alberta, several professors got me off to a good start. I wouldn’t have gotten here without their advice, encourage- ment, guidance, financial support, personal time, and other good things. Thanks to Jim Hoover, Piotr Rudnicki, Jonathan Schaeffer, Duane Szafron, and Hong Zhang. Thanks also to the Government of Canada and NSERC for a generous award that allowed me to focus on this task. Thanks finally to my committee, Tony Stentz, Guy Blelloch, and Dmitri Dol- gov for lending their time and their minds to assessing my work. Contents 1 Introduction 11 1.1 Motivation . 13 1.1.1 Why Autonomous Vehicles? . 13 1.1.2 Importance of Motion Planning . 15 1.1.3 Parallel Computing Approach . 16 1.2 Problem Statement . 18 1.3 Thesis Statement . 19 1.4 Approach . 20 1.5 The Rest of This Document . 22 2 Related Work 23 2.1 Planning Approaches . 24 2.1.1 Sampling Approaches for Off-Road Planners . 24 2.1.2 Swerve-Sampling On-Road Planners . 26 2.1.3 Lane Change Maneuvers . 28 2.1.4 State Lattice . 29 2.1.5 On-road Lattice Planners . 31 2.1.6 Resolution-Equivalent Grid . 32 2.1.7 Boundary-Value Solvers . 34 2.1.8 Trajectory Deformation . 35 2.1.9 Obstacle Representations and Heuristics . 36 2.1.10 Catalog Approaches . 38 1 2.1.11 Motion Planning in Parallel . 39 2.1.12 Summary . 41 2.2 Parallel Computing . 42 2.2.1 Introduction to the GPU . 42 2.2.2 Distributed Systems . 44 2.2.3 Exotic Multi-core Platforms . 45 2.2.4 Complexity Analysis in Parallel Programming . 46 2.3 Summary of Related Work . 47 3 Planning Algorithm 49 3.1 Overview . 50 3.1.1 Paths . 50 3.1.2 Trajectories . 50 3.1.3 Trajectory Evaluation . 51 3.1.4 Extended Trajectories . 54 3.1.5 The Rest of this Chapter . 55 3.2 Vehicle Model . 56 3.2.1 Road Model . 57 3.3 Path Model . 58 3.3.1 Polynomial Spirals . 61 3.4 Search Graph . 62 3.4.1 Graph Design Considerations . 62 3.4.2 Graph Definition . 65 3.4.3 Graph Construction . 68 3.4.4 Graph Search . 70 3.4.5 Why include acceleration in the vertex tuple? . 73 3.5 Stable Path Model . 74 3.5.1 Stable Cubic Paths . 75 3.6 Path Optimization . 77 3.6.1 Path Optimization, Cubic Case . 77 3.6.2 Path Optimization, Quintic Case . 80 2 3.7 Path Integration . 80 3.8 From Paths to Trajectories . 82 3.8.1 Accelerations . 82 3.9 Summary . 85 4 Cost Functions and Behavior Tuning 86 4.1 Relentless Optimization . 87 4.2 Hysteresis and Stability . 89 4.3 Trajectory Costs . 90 4.4 Static Costs . 92 4.4.1 Lane Centering . 92 4.4.2 Static Obstacles . 95 4.5 Dynamic Costs . 98 4.5.1 Dynamic Obstacles . 98 4.5.2 Velocities and Accelerations . 100 4.5.3 Choosing the Path Type . 107 4.5.4 Cost Function Summary . 108 4.6 Distance Keeping . 109 4.7 Lane Changing . 113 4.8 Summary . 117 5 Implementation and System Integration 120 5.1 Core Planner Implementation . 121 5.1.1 Overall System Flow . 121 5.1.2 Update of the Road Search Space . 123 5.1.3 Sample Lattice Vertices . 125 5.1.4 Update Lattice Path Splines . 125 5.1.5 Initial Guess Table . 126 5.1.6 Static Cost Map . 127 5.1.7 Dynamic Cost Map . 129 5.1.8 Trajectories From the Vehicle to the Lattice . 131 3 5.1.9 Trajectories Within the Lattice . 134 5.1.10 Reconstructing the Best Trajectory . 137 5.1.11 Cost function . 137 5.1.12 Accessing the Cost Table . 141 5.2 Tartan Racing Integration . 142 5.2.1 Control Architecture . 143 5.2.2 Trajectory Queue . 144 5.2.3 Latency compensation . 144 5.3 GPU Implementation . 145 5.3.1 Processing Concepts . 146 5.3.2 Center Line-Derived Data Structures . 148 5.3.3 Mapping from X-Y to S-L Space . 150 5.3.4 Lattice Internal Paths . 151 5.3.5 Internal Static Cost Map . 152 5.3.6 External Static Cost Map . 152 5.3.7 Dynamic obstacle cost map . 154 5.3.8 Cost Table . 155 5.3.9 Summary of Initialization Phase . 155 5.3.10 Planning Onto The Lattice . 156 5.3.11 Planning Within The Lattice . 160 5.3.12 Extracting the Best Trajectory . 165 5.4 Summary . 165 6 Evaluation 166 6.1 Experimental Results . 167 6.1.1 Experimental Configuration . 168 6.1.2 High speed evasive maneuvers . 169 6.1.3 Merging into slower traffic . 171 6.1.4 Freeway lane-changing . 172 6.1.5 Merging with oncoming traffic . 174 6.1.6 Misbehaving oncoming traffic . 175 4 6.1.7 Real robot tests . 177 6.1.8 Performance . 179 6.1.9 Summary . 179 6.2 Comparison to the State of the Art . 179 6.2.1 Description of Planner Features . 180 6.2.2 Comparison to Rule-based Approaches . 186 6.2.3 Comparison to Command Fusion Approaches . 187 6.2.4 Comparison to the CMU Urban Challenge Planner . 189 6.2.5 Comparison to the Stanford Urban Challenge Planner . 190 6.2.6 Comparison to the MIT Urban Challenge Planner . 191 6.2.7 Comparison to other regular sampling approaches . 191 6.3 Summary . 192 7 Conclusions 194 7.1 Conclusions . ..