JOURNAL OF GUIDANCE,CONTROL, AND DYNAMICS Vol. 25, No. 1, January– February 2002 Real-Time Motion Planning for Agile Autonomous Vehicles Emilio Frazzoli ¤ University of Illinois at Urbana– Champaign, Urbana, Illinois 61801 and Munther A. Dahleh † and Eric Feron ‡ Massachusetts Institute of Technology, Cambridge, Massachusetts 02139 Planning the path of an autonomous, agile vehicle in a dynamic environment is a very complex problem, especially when the vehicle is required to use its full maneuvering capabilities. Recent efforts aimed at using randomized algorithms for planning the path of kinematic and dynamic vehicles have demonstrated considerable potential for implementation on future autonomous platforms. This paper builds upon these efforts by proposing a randomized path planning architecture for dynamical systems in the presence of xed and moving obstacles. This architecture addresses the dynamic constraints on the vehicle’s motion, and it provides at the same time a consistent decoupling between low-level control and motion planning. The path planning algorithm retains the convergence properties of its kinematic counterparts. System safety is also addressed in the face of nite computation times by analyzing the behavior of the algorithm when the available onboard computation resources are limited, and the planning must be performed in real time. The proposed algorithm can be applied to vehicles whose dynamics are described either by ordinary differential equations or by higher-level, hybrid representations. Simulation examples involving a ground robot and a small autonomous helicopter are presented and discussed. I. Introduction R ECENT advances in computational capabilities, both in terms of hardware and algorithms, communication architectures, and sensing and navigation devices have made it possible to develop autonomous, single, or multiagent systems that exhibit a high degree of reliability in their operation, in the face of dynamic and uncertain environments, operating conditions, and goals. These systems must be able to construct a proper representation of the environment and of their own state from the available sensory data and/or knowledge base and must be able to make timely decisions aiming at interacting with the environment in an optimal way. This paper is concerned with the problem of generating and ex- ecuting a motion plan for an autonomous vehicle. In other terms this paper considers developing an algorithm that enables the robot to move from its original location to a new location (presumably to accomplish an assigned task such as performing an observation or delivering a payload), while avoiding collisions with xed or moving obstacles. The problem of planning a trajectory in an environment clut- tered by obstacles has been the object of considerable interest in the robotics and articial intelligence communities 1¡4 ; most of the activity has focused on holonomic or nonholonomi c kinematic mo- tion problems. Roughly speaking, it is possible to identify three general approaches to the motion-planning problem, namely, cell decomposition methods, roadmap methods, and articial potential eld methods. 1 Many motion-planning algorithms rely on the notions of congu- ration and conguration space. A conguration of a robot identies Received 20 November 2000; revision received 20 August 2001; accepted for publication 23 August 2001. Copyright c ° 2001 by the American Insti- tute of Aeronautics and Astronautics, Inc. All rights reserved. Copies of this paper may be made for personal or internal use, on condition that the copier pay the $10.00 per-copy fee to the Copyright Clearance Center, Inc., 222 Rosewood Drive, Danvers, MA 01923; include the code 0731-5090/02 $10.00 in correspondenc e with the CCC. ¤ Assistant Professor, Department of Aeronautica l and Astronautical Engi- neering, 321b Talbot Laboratory, 104 S. Wright Street; frazzoli@uiuc.edu. Member AIAA. † Professor, Department of Electrical Engineering and Computer Science, 77 Massachusetts Avenue, Room 35-401; dahleh@lids.mit.edu. ‡ Associate Professor, Department of Aeronautics and Astronautics, 77 Massachusetts Avenue, Room 35-417; feron@mit.edu. Senior Member AIAA. the position of all of its points with respect to an inertial reference frame. We will assume that such a conguration can be described by a nite number of parameters. The conguration space is the set of all possible congurations of the robot. Cell decomposition methods rely on the partition of the con- guration space into a nite number of regions, in each of which collision-free paths can be found easily. The motion-planning prob- lem then is translated into the problem of nding a sequence of neighboring cells, including the initial and nal conditions. 5 In roadmap methods a network of collision-free connecting paths is constructed, which spans the free conguration space (i.e., the subset of the conguration space that does not result in collisions with obstacles). The path-planning problem then reduces to nding paths connecting the initial and nal conguration to the roadmap and then selecting a sequence of paths on the roadmap. There are several methods for building such a roadmap, among which we can mention visibility graphs 6;7 and Voronoi diagrams. 8 Finally, in articial potential eld methods a collision-free trajec- tory is generated by the robot moving locally according to “forces” dened as the negative gradient of a potential function. 9¡11 This function is designed to provide attractive forces toward the goal and repulsive forces, which push the robot away from obstacles (the potential function is bowl shaped with the goal at the bottom, and obstacles are represented by peaks). This class of methods is based on the denition of a feedback control policy (i.e., the con- trol is computed at each instant in time as a function of the current state), as opposed to the open-loop approach of the preceding two classes. A shortcoming of this formulation is the possible existence of local minima in which the robot might become trapped. An ar- ticial potential function, which does not have local minima, is a said navigation function, but computing such a function is in the general case as difcult as solving the motion planning problem for all initial conditions. 12 The algorithms for motion planning must be evaluated in terms of completeness and computational complexity. An algorithm is said to be complete if it returns a valid solution to the motion-planning problem if one exists and returns failure if and only if the problem is not feasible: This is what we will call a correct termination for a motion-planning algorithm. The computational complexity of some basic formulations of the motion planning problem has been stud- ied in detail. The so-called generalized mover’s problem, involving motion planning for holonomic kinematic robots made of several polyhedral parts among polyhedral obstacles, has been proven by Reif 13 to be PSPACE hard. (The complexity class PSPACE includes decision problems for which answers can be found with resources, 116