Practical Search Techniques in Path Planning for Autonomous Driving

We describe a practical path-planning algorithm that generates smooth paths for an autonomous vehicle operating in an unknown environment, where obstacles are detected online by the robot’s sensors. This work was motivated by and experimentally validated in the 2007 DARPA Urban Challenge, where robotic vehicles had to autonomously navigate parking lots. Our approach has two main steps. The first step uses a variant of the well-known A* search algorithm, applied to the 3D kinematic state space of the vehicle, but with a modified state-update rule that captures the continuous state of the vehicle in the discrete nodes of A* (thus guaranteeing kinematic feasibility of the path). The second step then improves the quality of the solution via numeric non-linear optimization, leading to a local (and frequently global) optimum. The path-planning algorithm described in this paper was used by the Stanford Racing Teams robot, Junior, in the Urban Challenge. Junior demonstrated flawless performance in complex general path-planning tasks such as navigating parking lots and executing U-turns on blocked roads, with typical fullcycle replaning times of 50–300ms. Introduction and Related Work We address the problem of path planning for an autonomous vehicle operating in an unknown environment. We assume the robot has adequate sensing and localization capability and must replan online while incrementally building an obstacle map. This scenario was motivated, in part, by the DARPA Urban Challenge, in which vehicles had to freely navigate parking lots. The path-planning algorithm described below was used by the Stanford Racing Team’s robot, Junior in the Urban Challenge (DARPA 2007). Junior (Figure 1) demonstrated flawless performance in complex general path-planning tasks—many involving driving in reverse—such as navigating parking lots, executing Uturns, and dealing with blocked roads and intersections with typical full-cycle replanning times of 50–300ms on a modern PC. One of the main challenges in developing a practical path planner for free navigation zones arises from the fact that the space of all robot controls—and hence trajectories—is continuous, leading to a complex continuous-variable optimization landscape. Much of prior work on search algorithms for Copyright c © 2008, American Association for Artificial Intelligence (www.aaai.org). All rights reserved. Figure 1: Junior, our entry in the DARPA Urban Challenge, was used in all experiments. Junior is equipped with several LIDAR and RADAR units, and a high-accuracy inertial measurement system. path planning (Ersson and Hu 2001; Koenig and Likhachev 2002; Ferguson and Stentz 2005; Nash et al. 2007) yields fast algorithms for discrete state spaces, but those algorithms tend to produce paths that are non-smooth and do not generally satisfy the non-holonomic constraints of the vehicle. An alternative approach that guarantees kinematic feasibility is forward search in continuous coordinates, e.g., using rapidly exploring random trees (RRTs) (Kavraki et al. 1996; LaValle 1998; Plaku, Kavraki, and Vardi 2007). The key to making such continuous search algorithms practical for online implementations lies in an efficient guiding heuristic. Another approach is to directly formulate the path-planning problem as a non-linear optimization problem in the space of controls or parametrized curves (Cremean et al. 2006), but in practice guaranteeing fast convergence of such programs is difficult due to local minima. Our algorithm builds on the existing work discussed above, and consists of two main phases. The first step uses a heuristic search in continuous coordinates that guarantees kinematic feasibility of computed trajectories. While lacking theoretical optimality guarantees, in practice this first Figure 2: Graphical comparison of search algorithms. Left: A* associates costs with centers of cells and only visits states that correspond to grid-cell centers. Center: Field D* (Ferguson and Stentz 2005) and Theta* (Nash et al. 2007) associate costs with cell corners and allow arbitrary linear paths from cell to cell. Right: Hybrid A* associates a continuous state with each cell and the score of the cell is the cost of its associated continuous state. step typically produces a trajectory that lies in a neighborhood of the global optimum. The second step uses conjugate gradient (CG) descent to locally improve the quality of the solution, producing a path that is at least locally optimal, but usually attains the global optimum as well. Another practical challenge is the design of a cost function over paths that yields the desired driving behavior. The difficulty stems from the fact that we would like to obtain paths that are near-optimal in length, but at the same time are smooth and keep a comfortable distance to obstacles. A common way of penalizing proximity to obstacles is to use a potential field (Andrews and Hogan 1983; Khatib 1986; Pavlov and Voronin 1984; Miyazaki and Arimoto 1985). However, as has been observed by many researchers (Tilove 1990; Koren and Borenstein 1991), one of the drawbacks of potential fields is that they create high-potential areas in narrow passages, thereby making those passages effectively untraversable. To address this issues, we introduce a potential that rescales the field based on the geometry of the workspace, allowing precise navigation in narrow passages while also effectively pushing the robot away from obstacles in wider-open areas. Hybrid-State A* Search The first phase of our approach uses a variant of the wellknown A* algorithm applied to the 3D kinematic state space of the vehicle, but with a modified state-update rule that captures continuous-state data in the discrete search nodes of A*. Just as in conventional A*, the search space (x, y, θ) is discretized, but unlike traditional A* which only allows visiting centers of cells, our hybrid-state A* associates with each grid cell a continuous 3D state of the vehicle, as illustrated in Figure 2. As noted above, our hybrid-state A* is not guaranteed to find the minimal-cost solution, due to its merging of continuous-coordinate states that occupy the same cell in the discretized space. However, the resulting path is guaranteed to be drivable (rather than being piecewise-linear as in the case of standard A*). Also, in practice, the hybrid-A* solution typically lies in the neighborhood of the global optimum, allowing us to frequently arrive at the globally optimal solution via the second phase of our algorithm (which uses gradient descent to locally improve the path, as described below). The main advantage of hybrid-state A* manifests itself in maneuvers in tight spaces, where the discretization errors become critical. Our algorithm plans forward and reverse motion, with penalties for driving in reverse as well as switching the direction of motion. Heuristics Our search algorithm is guided by two heuristics, illustrated in Figure 3. These heuristics do not rely on any properties of hybrid-state A* and are also applicable to other search methods (e.g., discrete A*). The first heuristic—which we call “non-holonomicwithout-obstacles”—ignores obstacles but takes into account the non-holonomic nature of the car. To compute it, we assume a goal state of (xg, yg, θg) = (0, 0, 0) and compute the shortest path to the goal from every point (x, y, θ) in some discretized neighborhood of the goal, assuming complete absence of obstacles.1 Clearly, this cost is an admissible heuristic. We then use a max of the non-holonomicwithout-obstacles cost and 2D Euclidean distance as our heuristic. The effect of this heuristic is that it prunes search branches that approach the goal with the wrong headings. Notice that because this heuristic does not depend on runtime sensor information, it can be fully pre-computed offline and then simply translated and rotated to match the current goal. In our experiments in real driving scenarios, this heuristic provided close to an order-of-magnitude improvement in the number of nodes expanded over the straightforward 2D Euclidean-distance cost. The second heuristic is a dual of the first in that it ignores the non-holonomic nature of the car, but uses the obstacle map to compute the shortest distance to the goal by performing dynamic programming in 2D. The benefit of this heuristic is that it discovers all U-shaped obstacles and dead-ends in 2D and then guides the more expensive 3D search away from these areas. Both heuristics are mathematically admissible in the A* sense, so the maximum of the two can be used. Analytic Expansions The forward search described above uses a discretized space of control actions (steering). This means that the search will never reach the exact continuouscoordinate goal state (the accuracy depends on the resolution of the grid in A*). To address this precision issue, and to further improve search speed, we augment the search with analytic expansions based on the Reed-Shepp model (Reeds and Shepp 1990). In the search described above, a node in the tree is expanded by simulating a kinematic model of the car—using a particular control action—for a small period of time (corresponding to the resolution of the grid). In addition to children generated in such a way, for some nodes, an additional child is generated by computing an optimal Reed-and-Shepp path from the current state to the goal (assuming an obstacle-free environment). The Reed-andShepp path is then checked for collisions against the current obstacle map, and the children node is only added to We used a 160x160 grid with 1m resolution in x-y and 5◦ angular resolution.