Real-Time Motion Planning for Agile Autonomous Vehiclesdahleh/pubs/2.Real-Time Motion Planning for...

JOURNAL OF GUIDANCE CONTROL AND DYNAMICS

Vol 25 No 1 JanuaryndashFebruary 2002

Real-Time Motion Planning for Agile Autonomous Vehicles

Emilio Frazzolicurren

University of Illinois at UrbanandashChampaign Urbana Illinois 61801and

Munther A Dahlehdagger and Eric FeronDagger

Massachusetts Institute of Technology Cambridge Massachusetts 02139

Planningthe pathofanautonomousagilevehicle ina dynamicenvironmentis a very complex problem especiallywhen the vehicle is required to use its full maneuvering capabilities Recent efforts aimed at using randomizedalgorithms for planning the path of kinematic and dynamic vehicles have demonstrated considerable potential forimplementation on future autonomousplatforms This paper builds upon these efforts by proposing a randomizedpath planning architecture for dynamical systems in the presence of xed and moving obstacles This architectureaddresses the dynamic constraints on the vehiclersquos motion and it provides at the same time a consistent decouplingbetween low-level control and motion planning The path planning algorithm retains the convergence propertiesof its kinematic counterparts System safety is also addressed in the face of nite computation times by analyzingthe behavior of the algorithm when the available onboard computation resources are limited and the planningmust be performed in real time The proposed algorithm can be applied to vehicles whose dynamics are describedeither by ordinary differential equationsor by higher-level hybrid representations Simulationexamples involvinga ground robot and a small autonomous helicopter are presented and discussed

I Introduction

R ECENT advances in computationalcapabilitiesboth in termsof hardwareandalgorithmscommunicationarchitecturesand

sensing and navigation devices have made it possible to developautonomoussingleormultiagentsystems that exhibita high degreeof reliability in their operation in the face of dynamic and uncertainenvironmentsoperating conditions and goals These systems mustbe able to construct a proper representationof the environment andof their own state from the available sensorydata andor knowledgebase and must be able to make timely decisionsaiming at interactingwith 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 termsthis paper considers developing an algorithm that enables the robotto move from its original location to a new location (presumablyto accomplish an assigned task such as performing an observationor delivering a payload) while avoiding collisions with xed ormoving obstacles

The problem of planning a trajectory in an environment clut-tered by obstacles has been the object of considerable interest inthe robotics and arti cial intelligence communities1iexcl4 most of theactivity has focused on holonomic or nonholonomickinematic mo-tion problems Roughly speaking it is possible to identify threegeneral approaches to the motion-planning problem namely celldecomposition methods roadmap methods and arti cial potential eld methods1

Many motion-planningalgorithmsrely on the notionsof con gu-ration and con guration space A con guration of a robot identi es

Received 20 November 2000 revision received 20 August 2001acceptedfor publication 23 August 2001 Copyright cdeg 2001 by the American Insti-tute of Aeronautics and Astronautics Inc All rights reserved Copies ofthis paper may be made for personal or internal use on condition that thecopier pay the $1000 per-copy fee to the Copyright Clearance Center Inc222 Rosewood Drive Danvers MA 01923 include the code 0731-509002$1000 in correspondence with the CCC

currenAssistant Professor Department of Aeronautical and Astronautical Engi-neering 321b Talbot Laboratory 104 S Wright Street frazzoliuiuceduMember AIAA

daggerProfessor Department of Electrical Engineering and Computer Science77 Massachusetts Avenue Room 35-401 dahlehlidsmitedu

DaggerAssociate Professor Department of Aeronautics and Astronautics 77Massachusetts Avenue Room 35-417 feronmitedu Senior MemberAIAA

the position of all of its points with respect to an inertial referenceframe We will assume that such a con guration can be describedby a nite number of parameters The con guration space is the setof 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 whichcollision-freepaths can be found easily The motion-planningprob-lem then is translated into the problem of nding a sequence ofneighboring cells including the initial and nal conditions5

In roadmap methods a network of collision-freeconnectingpathsis constructed which spans the free con guration space (ie thesubset of the con guration space that does not result in collisionswith obstacles) The path-planningproblem then reduces to ndingpaths connecting the initial and nal con guration to the roadmapand then selecting a sequence of paths on the roadmap There areseveral methods for building such a roadmap among which we canmention visibility graphs67 and Voronoi diagrams8

Finally in arti cial potential eld methods a collision-freetrajec-tory is generated by the robot moving locally according to ldquoforcesrdquode ned as the negative gradient of a potential function9iexcl11 Thisfunction is designed to provide attractive forces toward the goaland repulsive forces which push the robot away from obstacles(the potential function is bowl shaped with the goal at the bottomand obstacles are represented by peaks) This class of methods isbased on the de nition of a feedback control policy (ie the con-trol is computed at each instant in time as a function of the currentstate) as opposed to the open-loop approach of the preceding twoclasses A shortcomingof this formulation is the possible existenceof local minima in which the robot might become trapped An ar-ti cial potential function which does not have local minima is asaid navigation function but computing such a function is in thegeneral case as dif cult as solving the motion planning problem forall initial conditions12

The algorithmsfor motionplanningmust be evaluated in terms ofcompleteness and computational complexity An algorithm is saidto be complete if it returns a valid solution to the motion-planningproblem if one exists and returns failure if and only if the problemis not feasible This is what we will call a correct termination for amotion-planningalgorithmThe computationalcomplexityof somebasic formulations of the motion planning problem has been stud-ied in detail The so-called generalized moverrsquos problem involvingmotion planning for holonomic kinematic robots made of severalpolyhedral parts among polyhedral obstacles has been proven byReif13 to be PSPACE hard (The complexityclass PSPACE includesdecision problems for which answers can be found with resources

116

FRAZZOLI DAHLEH AND FERON 117

such as memory which are polynomial in the size of the inputThe run time is not constrainedThe complexity class NP is knownto be a subset of PSPACE moreover it is believed to be a propersubset14 ) An algorithm for path planning in a con guration spaceof dimensionn with obstaclesde ned by m polynomialconstraintsof degree d has been establishedby Schwartz and Sharir15 the timecomplexity of the algorithm is (twice) exponentialin n and polyno-mial in m (geometricalcomplexity) and in d (algebraiccomplexity)The most ef cient algorithmcurrentlyavailablefor solvingthe sameproblem from Canny16 is singly exponential in n The precedingresults strongly suggest that the complexity of the path-planningproblem grows exponentially in the dimension of the con gurationspace

Moreover kinematic holonomic path planning is not enough formany problems of interest particularly problems involving ldquoagilerdquoautonomous vehicles for which we have to take into account theadditional constraints on the vehiclersquos motion arising from its dy-namics or from nonholonomic constraints (that is nonintegrableconstraints on the state and its derivatives)

Even though in some cases it is possible to force a dynamicalsystem to follow trajectories generated by a kinematic model thisis not true in general especially when constraints on the availablecontrol inputs are taken into account

For example differentially at systems17 can follow any arbi-trary trajectory for the so-called at outputs18 However no generalmethod is available yet to ascertain differential atness of a givensystemMoreovereven fordifferentially at systemscurrentlythereis no straightforward way of taking into account control saturationor ight envelope constraints (some advances in this direction ap-peared recently1920 )

As another example some nonholonomicsystems are able to ap-proximate arbitrary paths but usually this requires the executionofcomplex sequencesof small-amplitude movements about the refer-ence path which could possibly result in a large loss in terms ofperformance of the nal trajectory321iexcl23

If the output of a kinematic planner is used as a reference tra-jectory for an inner loop consisting of a tracking control law thediscrepanciesbetween the planned trajectory and the trajectory ac-tually executed by the system can be relevant and lead to collisionwith obstacle even in the case in which the planned trajectory wascollision freeThis is true for example in cases in which the charac-teristic dimensionsof the environment(eg the size of the obstaclesand of the gaps between them) and of the vehiclersquos dynamics (egthe turning radius at some nominal velocity) are comparable

As a consequence it is desirable that motion-planningstrategiestake fully into account the dynamicsof the vehicle In otherwords itis desirable that the output from the motion planning be executableby the system dynamics this is the object of a recent direction inmotionplanningresearchusuallyreferredto as kinodynamicmotionplanning

Clearly such a problemis at least as dif cult as the path-planningproblem Moreover constraints deriving from the systemrsquos dynam-ics or from nonholonomicconstraintscannotbe representedas ldquofor-bidden zonesrdquo in the state space (ie the space encoding the con g-uration of the vehicle as well as its velocities) As a consequencethe direct application of kinematic motion-planning techniques tothe dynamic case is not possible in the general case

Perhaps the best formulated general method for addressingmotion-planning problems is the use of optimal control2425 How-ever the solution of such a problem using the traditional optimalcontrol tools (such as variational calculus the minimum princi-ple of Pontryagin and other necessary conditions26 and dynamicprogramming27 ) is computationally intractable for all but trivialcases In addition some techniques from kinematic motion plan-ning can be also extended to the dynamic case by planning motionsin the state space instead of the con guration space

However there is strongevidencethat any deterministicand com-plete algorithm for the solution of kinodynamic motion-planningproblems will require at least exponential time in the dimensionof the state space of the dynamical system which is usually atleast twice the dimension of the underlying con guration spaceand polynomialin the numberof obstaclesAs a consequenceavail-

able algorithmsare implementable in practice at least at the currenttechnology levels only for systems of very small dimension (egless than ve) Because the state space of aerospace vehicles is atleast 12 (when vehicles are modeled as rigid bodies) one has toresort to heuristic techniquesor seek alternative formulationsof theproblem

To circumvent the computational complexity of deterministiccomplete algorithms a new class of motion-planning algorithmsknown as probabilistic roadmap (PRM) planners was introducedby Kavraki et al28 and Overmars and Svestka29 and subsequentlyre ned430iexcl33 The PRM path-planning architecture was rst intro-ducedas a fast and ef cient algorithmfor geometricmultiple-querypath planning The original PRM planner is based on an off-linepreprocessing phase and an on-line query phase The preprocess-ing phase is aimed at constructing a graph of feasible paths in theentire con guration space (the roadmap) which would make futurequerieseasy to solveThe on-linequery phase selectsan appropriatepath from the already computed roadmap together with the com-putation of two ldquoshortrdquo paths to connect starting and ending pointsto the closest nodes (or milestones) of the roadmap The PRM algo-rithm has beenproven to be complete in a probabilisticsense that isthe probabilityof correct termination approaches unity as the num-ber of milestones increases Moreover performance bounds havebeen derived as a function of certain characteristicsof the environ-ment (ie its expansivenesstied to the rate at which the set of pointsthat can be connected to the roadmap grows with the number ofmilestones) which prove that the probabilityof correct terminationapproaches one exponentially fast in the number of milestones33

However for many path-planning applications building aroadmap a priori may not be required or even feasible (eg forplanning in a dynamic rapidly changing environment) In additionthe basic roadmap algorithms neglect the vehicle dynamics the ve-hicle usually needs to navigate along a piecewise linear trajectorywith very sharp turning points To address both points a new ran-domized motion-planningalgorithm was developed by introducingthe conceptof rapidlyexploringrandomtrees (RRTs)34iexcl36 The RRTalgorithm consists of building on-line a tree of feasible trajectoriesby extending branches toward randomly generated target pointsAlthough in the PRM approach the idea was to explore the con g-uration space exhaustively in the preprocessing phase RRTs tendto achieve fast and ef cient single-queryplanning by exploring theenvironment as little as possible A signi cant feature of the RRTsis that the resulting trajectories are by de nition executable by theunderlyingdynamicalsystem In additionunderappropriatetechni-cal conditions the RRT algorithmhas been proven probabilisticallycomplete36 that is the probability of nding a path from origin todestination converges to one if such a feasible path exists

In Hsu et al37 a new incremental roadmap building algorithm isintroduced that provides not only probabilistic completeness butalso recovers performanceguaranteeson the algorithm That is theprobabilityof thealgorithm ndinga solutionif oneexists convergesto oneexponentiallyfastwith thenumberof randomsamplesused tobuild the tree Interestinglythe rate of convergencedoes not dependon the number of obstaclesbut ratheron geometric propertiesof theenvironment (eg its expansiveness) One dif culty pointed out bythe authors however lies with the fact that algorithm performancereliesuponuniformsamplingofmilestonesin the reachablespaceofthe tree Practical implementations (which have reportedly demon-strated excellent performance37 ) rely upon uniform sampling of thesystemrsquos control inputs insteadwhich in general does not guaranteeuniform sampling of the workspace

Motivated by these recent developments a new randomized in-crementalmotion-planningalgorithmis proposedin this paperThisincremental roadmap building algorithm is able to effectively dealwith the systemrsquos dynamics in an environment characterized bymoving obstacles Central to this algorithm is the assumption thatan obstacle-freeguidance loop is availablewhich is able to steer thesystem from any state (including con guration and velocity) to anydesired con guration at rest assuming that there are no obstaclesin the environment This guidance loop enables uniform samplingof the workspace while generating trajectories that are executableby the dynamical system As a consequence it is shown that this

118 FRAZZOLI DAHLEH AND FERON

path-planning algorithm satis es the technical conditions elicitedby Hsu et al37 and therefore offers guaranteed performance in theform of bounds on the convergence rate to unity of the probabilityof correct termination Considering real-time computation issuesthe path-planningalgorithmprovides safety guarantees in the sensethat it provides intermediatemilestoneswith guaranteedbuffer timebefore a collision occurs If possible and practical the buffer timecan be extended to in nity thus resultingin principlein hard safetyguarantees on the generated motion plan Moreover in the case inwhich the obstacle-free planner is optimal with respect to somemeaningfulcost it is possible to implement branch-and-boundalgo-rithms to bias the search for a solution to maximize the performanceof the computed solution

The paper is organizedas follows rst the planningframework isintroduced including speci cations on the system closed-loop dy-namicson the environmentand on thepath-planningproblemin thepresence of obstacles The randomized real-time motion-planningalgorithm for agile vehicles is then introduced and discussed andits performance is analyzed Finally the randomized algorithm isdemonstratedon a groundrobotexample rst and then on the modelof a small autonomous helicopter Simulation results are presentedand discussed

II Motion-Planning FrameworkThis section introduces the elements used to formulate the path-

planning algorithm The algorithm for path planning presupposesthe existenceof a closed-looparchitecturethat enables the guidanceof the vehicle from any state to any con guration at rest Thusrather thanworkingwith anopen-loopsystemas presentedin earlierpublications343637 ourbasicdynamicalsystemis a closed-loopone

A System DynamicsIn this sectionwe introducethedynamicsof the systems forwhich

the motion-planning algorithm is applicable Because this paperconcentrates mostly on guidance and planning tasks we introducetwo guidance models which we believe are relevant to the problemunder consideration

1 System Representation via Ordinary Differential EquationsThe usual representation of the dynamics of an autonomous ve-

hicle or robot is a set of ordinary differential equations (ODEs) ofthe form

dx

dtD f x u (1)

where x 2 X is the state belonging to a n-dimensional manifold X(the state space) and u is the control input taking values in the setU micro Rm The precedingformulationcan includeboth nonholonomicand dynamic constraints38 In some cases additional inequalitycon-straints of the form

F x middot 0 (2)

must be added on the state variables to ensure safe operation of thesystem (eg ight envelope protection) In Eq (2) Fx can repre-sent a vector of constraints and the inequality must be understoodcomponent-wise

Finally assume that the state space X can be decomposed atleast locally into the product C pound Y and that the system dynamicsas well as the ight envelope constraints are invariant with respectto group actions (eg translations or rotations) on C

The space C is a reduced con guration space in the sense that itis de ned by a subset of the con guration variables of the systemwith respect to which the system has certain symmetry propertiesIn a Lagrangian mechanics setting these variables correspond tothe so-called cyclic variables which do not appear explicitly in theLagrangian of the system39 In many cases of interest one is effec-tively interested in planning the motion on such a reduced con gu-ration space For examplewhen planning the motionof a helicopterone is interested in specifyinga nal hoveringpositionand possiblyheadingPitch and roll angle at this nal conditionare not speci edas long as they are such to allow hovering

The space Y encodes the remaining con guration variables aswell as the vehiclersquos velocity and higher-order derivatives of thecon guration variables These somewhat abstract notions are moreclearly illustrated on simple examples

Example 1 (system with integrators) Consider the system de-scribed by the following set of ODEs

Pz D y Py D f y u

In this case the spaceC is spannedby the vector z whereas the spaceY is spanned by y

Example 2 (attitude dynamics) Consider the attitude dynamicsof a spacecraft in the absence of a gravitational eld

PR D R O J P D iexcl pound J C M u

In the preceding equations J is the inertia tensor of the spacecraftM denotes the moments generated by the controls u and the skewmatrix O is de ned as the unique matrix for which Ov D pound v forall vectors v 2 R3 In this case the space C corresponds to the groupof rotations R in the three-dimensionalspace SO3 and the spaceY corresponds to the linear space of velocities in body axes 2 R3

Example 3 (helicopterdynamics) Finally considerthe followingsimpli ed model of the motion of a helicopter4041

Px D v m Pv D mg C RFbv u

PR D R O J P D iexcl pound J C Mbv u

where x is the position of the center of mass of the vehicle v isits velocity in an inertial reference frame R 2 SO3 represents theattitudeof the helicopterand is the vector of angular velocities inbody axes The mass and rotational inertia of the vehicle are indi-cated by m and J and g represents the gravity accelerationFinallyFb and Mb are respectively the forces and moments (expressed inbody axes) generated by the controls u In this case because thedynamics of the vehicle do not depend on its position and on itsheading angle the space C corresponds to the space of translationsin the position space and rotations about a vertical axis The vari-ables in Y encode the remaining degrees of freedom (namely rolland pitch rotations) as well as the translationaland rotational ratesThis decomposition is only local

2 Hybrid System RepresentationAlthough the formulation in terms of ODEs is probably the most

commonly used representation of a vehiclersquos dynamics it mightnot be the most appropriate choice for the purpose of vehicle guid-ance In particular the state space of nontrivial systems is typicallyvery large and the ldquocurse of dimensionalityrdquomakes the solution ofmotion-planningproblems in such large-dimensionspaces compu-tationally intractable

An alternative approach is represented by the formulation of thesystem dynamics and hence of the motion-planning problem inwhat can be regarded as the maneuver space of the vehicle42iexcl44

This alternative formulation of the vehicle dynamics builds on aformalizationof the concept of maneuverwhich in a more unstruc-tured form is commonly referred to in the aerospace literature45iexcl49

The main feature of a maneuver model is the selection of ap-propriate motion primitives Closely related to the invarianceof thesystem dynamics with respect to group actions on C is the existenceof trim trajectories (or relative equilibria) Loosely speaking a trimtrajectory can be de ned as a trajectory where the vehicle is in astate of equilibrium relative to a body- xed reference frame Dur-ing a trim trajectory the state variables in C evolve according to thesystem dynamics whereas the state variables in Y are xed as arethe controls u The existence of trim trajectories is a fundamentalproperty of the system dynamics and it is natural to consider trimtrajectories as a rst class of motion primitives A nite numberof such trajectories can be selected for example by gridding thecompact subset of Y pound U de ned by the ight envelope and controlsaturation constraints


Example 4 (autonomous vehicle dynamics) Most ground and airvehicles exhibit invarianceto translationin the horizontalplane andto rotation about a vertical axis If altitude changes are limited (andhence air density variations are negligible) we can also assumeinvariance to vertical translation In this case trim trajectories arehelicoidal curves with a vertical axis

Sucha selectionof trajectoryprimitives is appropriatefor systemsin which the dynamic behavior can be neglected (eg kinematicsystems aircraft models for air traf c control) If the dynamics ofthe system cannot be neglected the transients related to switchesbetween different trim trajectories must be accounted for A sec-ond class of primitives called maneuvers are de ned as feasible nite-time transitions between trim trajectories Maneuvers can bedesigned by solving a local optimal control problem or can be de-rived from ight data

Although maneuvers have a de nite time duration trim trajecto-ries can in principle be followed inde nitely As a consequencethedecision variables or control inputs consist of the amount of timethe vehiclemust remain in the trim trajectory (coasting time) and inwhich maneuver it has to execute next (jump destination)

The result of the process of extracting trajectory primitives is thecreation of a guidance-level system representation as a maneuverautomatonwhose states are the trim states of the vehicle and whosetransitions correspond to maneuvers The maneuver automaton canbe seen as a new model of the vehicle in which the continuous dy-namics ODEs (1) are replacedby the transitionrules on the directedgraph representing the automaton and by the associatedhybrid sys-tem evolutionThe full state of the system will then be describedbythe maneuver primitive being executedand the time and positiononC of its inception Using this representation the assumption of in-variance to group actions in C is enough to ensure that the maneuverautomaton encodes all of the relevant information about dynam-ics and the ight envelope constraints of any vehicle in a space ofsmaller dimension that is M D C pound QT where QT is the set ofindices of the selected trim trajectories

Example 5 Many planar nonholonomic robot models assumethat the robot cruising at constant speed can only move straightforward or turn left with a given radius or turn right with the sameturning radius In this case assuming the robot to be a kinematicrigid body C would be the set of all positions and orientations ofthe robot and QT D fmove straight turn left turn rightg

A full discussionof themaneuverautomatonframeworkis outsideof the scope of this paper but is available on other publications bythe authors44 to which we refer the interested reader

In the rest of the paper to simplify the notationwe simply use theletter X to indicate the state space with the understandingthat thiscan be regarded as either the continuous state space or the hybridmaneuver space M

B Obstacle-Free Guidance SystemThe second major element that is assumed to be available is a

guidancealgorithmin environmentswith no obstacles In this paperwe consider problems in which the desired destination is a relativeequilibriumor trim trajectoryMore speci cally we assume knowl-edge of a guidance law that can steer the system in the absence ofobstaclesfrom any state to a particulartarget set T xeq centeredata relative equilibrium xeq Because the system dynamics are invari-ant with respect to group actions on C a family of relative equilibriacan be expressed by a point in C pound f Nyg where Ny 2 Y is a constant

For simplicity in the examples we will only consider equilibriumpoints (ie terminal conditions with zero velocity) and will con-sider only autonomous vehicles which are indeed able to ldquostoprdquoThese include autonomoushelicoptersvertical takeoff and landingaircraft spacecraft ground vehicles and surfaceunderwater ves-sels However the algorithm that will be presented is applicable toother vehicles such as xed-wing aircraft and paragliders as longas the terminal equilibrium condition for the guidance law such asldquohover at point prdquo is replaced by a relative equilibrium conditionsuch as ldquoenter a steady turn starting at point prdquo

Althoughadmittedly nding such a guidancelaw is per se a verydif cult problem it also has been the object of an enormousamountof work over the past century in many cases ef cient obstacle-

free guidance laws can be computed analytically2425 This is thecase of system with linear dynamics with a quadratic cost It alsoincludes numerous cases of aerospace interest such as double ortriple integratorswith control amplitude and rate limits In additionmany of theseproblems althoughthey might not admit closed-formsolutions can be solved numerically via the approximate or exactsolution to an appropriate optimal control problem by minimizinga cost functional of the form

J [xcent ucent] DZ t f

t0

deg [xt ut] dt (3)

for some initial conditions x0 under the boundary conditionxt f 2 T xeq and the dynamics and ight envelope constraints(1) and (2) We make the additionalassumption that the incrementalcost deg x u is invariant with respect to group actions on C Sucha formulation includes for example minimum time minimum pathlength and minimum energy control problems

Advances in computer power combined with appropriate plantsimpli cations(such as the introductionof the maneuvermodel out-lined earlier) make it possible in many cases of practical interestto compute and store an approximate expression for the optimalcost-to-go function (or return function) J currenx xeq for all x 2 X and all equilibrium points xeq 2 C pound f Nyg using for example iterativemethods4250 Consideringthecaseof a small autonomoushelicopterrepresented by a maneuver model such as introduced earlier in thispaper storage of such a cost function required about 1 MB of mem-ory easily implementable on a computer (the cost function wasapproximated using a simple look-up table with semilogarithmicspacing)42 Other approachesfor kinematicmotion planningof non-holonomicvehicles involve theconstructionof optimal solutionsviathe interconnectionof canonical paths51iexcl53

If the optimal cost function J currenx xeq is known for all x 2 Xand all equilibrium points xeq 2 C pound f Nyg then it is relatively easyto recover the optimal control policy frac14 X pound C U as a (feed-back) policy that returns at each time instant the control input thatminimizes the total (future) cost-to-go to the target54 The feedbackpolicy frac14 can be thoughtof as a functionof the state x parameterizedby the destination equilibrium point xeq

The solution to an optimal control problem in the free space thusprovides us with a control policy frac14 that ensures that the system isdriven toward a target set with a nite cost Moreover the optimalcost-to-go function provides a meaningful measure of the distancebetween the initial conditions and the target

Along with the optimal control law in an obstacle-free environ-ment we assume the availability of a simulator of the system thatis a function which is able to predict the evolution of the closed-loop system and generate the corresponding(state time) trajectorySuch a simulatorcan be easily developedfrom the knowledgeof thesystemrsquos dynamics and of the optimal control law

C Environment CharacterizationWe consider an environment in which both xed and moving

obstaclesare presentand we assumethat themotionof theobstacles(or conservativeestimates thereof) is known in advance In this caseobstacle avoidance constraints can be written a priori as

Gx t middot 0 (4)

where Gx t can be a vector of constraints and the inequalitymust be understood component-wise Because the environmentis time-varying collisions must be checked on (state time) pairsx t 2 X pound R For this purpose a collision-checkingalgorithm isassumed to be availablevia trajectorysamplingor otherappropriatemethod

The assumption that information on the obstacle motion is avail-able a priori is admittedly very limiting because in most practicalapplications this kind of information is not available especiallyif the obstacles are actually other vehicles moving according totheir own control laws and goals Nevertheless for the time beingwe will concentrate on this problem which is already extremelychallenging


It is important to explicitly remark that the simple fact that theobstacle avoidance constraints (4) are time varying (ie the ob-stacles are moving) translates into hard real-time requirements forthe motion-planning algorithm even under a perfect informationassumption on the motion of the obstacles This is a consequenceof the fact that the feasibility of the computed solution with respectto the obstacle avoidance constraints has to be checked on (statetime) couples the solution must be a time-parameterizedtrajectorywhere the time ldquotagsrdquoareof fundamentalimportanceBecauseof the nite computation times in physical systems the motion planninghas to be carried out starting from initial conditions at some time inthe future (ie a nite lead time is needed) If the motion-planningalgorithm is too late in delivering the solution to the vehicle thatis if the computation time exceeds the allowed lead time the latesolution is in general not guaranteed to be feasible

Before proceeding to the problem formulation we need to intro-duce some notationThe feasible set F frac12 X pound R is de ned to be theset of all pairs x t for which no collisions occur and the ightenvelopeconstraintsare satis ed Given an initial condition x0 t0a pair x f t f is said reachableif it is possible to nd a control func-tion Ou [t0 t f ] U such that the ensuing trajectory of the systemfrom the preceding initial conditions is feasible and terminates in-side the set T x f In other words we say that x f t f is reachablefrom x0 t0 if the time-parameterized curve Acirc [t0 t f ] X is anintegral curve of the ODE (1) (or can be executed as a sequence ofmaneuvers encoded in the maneuvermodel) given the control inputOut and is such that Acirct0 D x0 Acirct f 2 T x f and [Acirct t] 2 F for all t 2 [t0 t f ] We can de ne the reachable set Rx0 t0 frac12 F asthe set of all points that are reachable from x0 t0 Accordinglygiven a set S frac12 F we de ne

RS D[

x t 2 S

Rx t

D Problem FormulationThe motion-planningproblemcan now be statedas followsgiven

an initialstate x0 2 X at time t0 anda goalequilibriumcon gurationx f 2 C pound f Nyg nd a control input v [t0 t f ] U that can steer thesystem the system from x0 to T x f A motion-planningalgorithmis said complete if it returns a feasible solution whenever thereexists a time t f such that x f t f 2 Rx0 t0 and returns failureotherwise Although the usual formulation of the motion-planningproblemis concernedonly with ndinga feasible trajectoryin manyengineeringapplicationswe are also interestedin ndinga trajectoryminimizing some cost In this paper we will assume a cost of theform (3)

The motion-planning problem even in its simplest formula-tion has been proven computationally hard It is possible tocircumvent this dif culty through the de nition of a randomizedmotion-planningalgorithmwhich by replacing completenesswithprobabilistic completeness (in the sense that the probability of thealgorithmterminatingcorrectly approachesone as the number of it-erationsgrows) achievescomputationaltractabilitywhile retainingformal guarantees on the behavior of the algorithm

To implement our randomized motion-planning algorithm weneed to limit the feasible trajectories of the system to a compactsubset of the state space X and hence a compact subset of the re-duced con guration space C This can be done easily through theintroductionof a con nementconstraintof the form(4) The motiva-tion for such a requirementderives from the necessity of generatinguniform distributionsof target equilibria

III Motion Planning in the Presence of ObstaclesThe motion-planning algorithm in the presence of obstacles is

basedon the determinationof a sequenceof randomattractionpointsxr and the corresponding control laws frac14cent xr which effectivelysteer the system to the desired con guration while avoiding obsta-cles In this way the obstacle-free solution to an optimal controlproblem forms the basis for the problem of motion planning in thepresence of obstacles Such an approach casts the location of theequilibrium con guration as a function of time as a pseudo-controlinput for the system Because the actual control inputs can be com-

puted from the knowledgeof the optimal controlpolicyfrac14cent xr thismeans that the low-level control layer (the layer actually interactingwith the vehicle) and the high-level guidance layer are effectivelydecoupledwhile at the same time ensuringfull consistencybetweenthe two levels In other words the output of the guidance layer iscontrol policies not reference states or inputs As a consequenceunlikeearlier randomizedmotion-planningapproachesthe motion-planning algorithmcan be run at a rate that is much slower than therate required for the low-level control layer

An additional advantage of the proposed approach is that theschedulingof feedbackcontrolpoliciesprovidesrobustnessto exter-nal disturbances uncertainties and modeling errors which is miss-ing in other open-loop motion-planningapproaches

The ideas just outlined in a probabilistic roadmap setting can beseen as a motion-planning technique through schedulingof controlpolicies and corresponding Lyapunov functions [ie the optimalcost-to-go functions J currencent xr ] Although the concept is not entirelynew in control theory55iexcl57 to the authorrsquos knowledge this is the rst application to motion planning in a workspace with movingobstaclesA fundamental difference can also be seen in the fact thatin our algorithm the ordering of Lyapunov functions is performedon-linewhereas in the referencesthe orderingwas determineda pri-ori In other words in the references just mentioned there exists apreestablished partial ordering of the Lyapunov functions the sys-tem is driven through the attraction domains of a prede ned se-quence of Lyapunov functions (and correspondingcontrol policies)that draws the state closer and closer to the origin In our algorithmsuch a sequence is computed on-line

Imposing that the motion plan be constructed as a sequence oflocal optimal controlpoliciessteering the system toward a sequenceof attraction points limits the class of possible motion plans whichare output by the algorithm Hence the motion-planningalgorithmwe are going to present will not be complete in the sense that itis not capable of generating all possible feasible trajectories Onthe other hand we can analyze the completeness of the motion-planning algorithm with respect to the class of trajectories it is ableto generate

Given the controlpolicy frac14 we say that a point x f t f is frac14 reach-able from xi ti if the closed-loop evolution of the system underthe control policy frac14cent x f with initial conditions xi ti is such thata feasible collision-freetrajectory is generated through x f t f (ora neighborhood thereof) The frac14 reachable set Rfrac14 xi ti is thus de- ned as the set of all (state time) couples which are frac14 reachablefrom xi ti this set is bounded from below (in the sense of time)by a manifold with the same dimension as the symmetry group H embedded in the larger space X pound R Accordingly we can de nethe frac14 reachable set of a set S as

Rfrac14 S D[

x t 2 S

Rfrac14 x t

In the followingwe will consider a weaker notion of completenesslimited to trajectories that can be generated by the application of asequence of given control policies from the initial conditions

A Overview of the AlgorithmThe basic idea of the algorithm is the following starting from the

initial conditions xi ti we incrementally build a tree of feasibletrajectories trying to explore ef ciently the reachable set Rxi ti (A tree is a directed graph with no cycles in which all nodes haveseveral outgoing edges to each of which correspondsanother nodecalled a child A child node can in turn have several other childrenAll nodes excluding the root have one and only one incomingedge which comes from the parent The root has no parent Acommonexampleof a tree is thedirectorystructurein a computer lesystem)

At each step we will add a new branch (edge) and a new milestone(node) to the tree For this purpose at each tree expansion step wehave to address two points 1) which node do we want to expandand 2) in which direction shall we explore

The rst incrementalrandomizedmotion-planningalgorithmwasrecently introduced by LaValle and Kuffner35 based on previouswork by Kavraki et al30 with the name of RRT In the originalRRT


the answers to the preceding questions were provided (roughly)in the following way 1) pick a con guration xr at random andchoose the node to expandas the closest (in the sense of a Euclideandistance) node currently in the tree and 2) apply a constant inputfor a time plusmnt in such a way that the nal point is moved as close aspossible to xr If the resulting trajectory is feasible then it is addedto the tree The precedingprocedureis iterateduntil one of the nodesof the tree is close enough to the target point x f

Although the RRT algorithm proved to be extremely ef cient inmany dif cult problems as reported in the literature34iexcl36 it couldnot be appropriatein the generalcase of a dynamicalsystemSelect-ing the control inputs accordingto a greedy policy on the Euclideandistance could result into instability for the dynamical system (itessentially amounts to pure proportional feedback) Also only aprobabilisticcompletenessproof is availableEven thoughit is guar-anteed that the probability of correct termination of the algorithmconverges to one as the number of iterations increases there are noindications on how fast this convergence will occur Moreover ina dynamic environment case this approach is not probabilisticallycompleteand might fail altogetheras will be shown in the examples

In Hsu et al37 a different approach was introducedbased on thefollowingmain steps 1) choose the node to be expandedat randomand 2) apply a random control input for an interval plusmnt

The algorithm has been proven to be probabilisticallycompleteeven in the presence of moving obstaclesMoreover its proponentswere able to prove performancebounds on the algorithm providedthat at each iterationthe reachableset of the set of currentmilestones(nodes in the tree) is explored uniformly This is not accomplishedin the general case by the two steps just outlineda randomselectionof the inputs from a uniform distributiondoes not necessarily resultinto a uniform distribution on the outputs (the location of the newcandidate milestone) Moreover a uniform random choice of thenode to be expanded does not result in a uniform explorationof thereachable set of the tree because the reachable sets from each of thenodes overlap and can have different volumes As a consequencethe explorationmechanism considered in the proof was not the oneoutlined in the preceding two steps but was instead assumed to bethe output of an idealized procedure denoted as IDEAL-SAMPLEThe randomselectionof nodes and control inputswas used as an ap-proximation of IDEAL-SAMPLE in the practical implementationMoreover the complete randomness of both the main steps in thetree constructionresults in the generationsof trees which appear tolack in senseof purposeand do not appear to explorevery ef cientlythe free con guration space (ef cient exploration was on the otherhand the main advantage of the RRT algorithm)

To address these problems we advocate the fact that the optimalcost function in the obstacle-free environment provides the mostappropriateinformation to address both the issues of node selectionand trajectory generation Using the optimal control function andthe correspondingoptimal control policy we will indeed be able toimplement an IDEAL-SAMPLE procedure and achieve the corre-sponding performance bounds37 The key ideas are the followingthe correct measure of distance is the optimal cost-to-go and theoptimal input can be derived from the associated optimal controlpolicy Moreover the optimal (feedback) control policy can be seenas the inversion mechanism which translates the uniform distribu-tion in the output space (ie the location of the newly generatedcandidate milestone xr ) into the corresponding distribution in theinput

In our algorithmwe proceedas follows1) pick a con gurationxr

at random and try to expand all of the nodes in the tree in sequencein order of increasing cost J currenxi xr [ie starting from the closestnode using the measure of distance provided by J currencent xr ] and 2)apply the optimal control policy frac14cent xr until the system gets to xr

(or a neighborhoodof xr )As it can be easily recognized the preceding steps correspond to

the steps in the RRT algorithm with two fundamental differencesat each iteration we try to connect the new candidate milestone inturn to each one of the nodes currently in the tree before discardingit as unreachable from the current tree (in the original RRT onlythe closest node was tested for reachability) The RRT criterion oftesting the closest node translates into the heuristics of testing the

nodes in ascending distance order The second main difference isthat the optimal cost function in the obstacle-free case is used as ameasure of distance both in the selection of nodes to expandand inthe computation of the optimal control

In the next subsectionswe will discuss more in detail the compo-nents of the motion-planningalgorithm and how they t together

B Data StructureFirst of all we will discuss brie y the elements that characterize

each node (also referred to as milestone) and each edge (or branch)in the tree

1 Data Stored at NodesThe main piece of information stored for each milestone consists

of the predicted(state time)couple that will be attainedif the chosenmotion plan includes the node at hand this can be computed byintegrating over time the equations describing the dynamics of thesystem

Moreover at each node in the tree we can store data used to com-pute estimates (lower and upper bounds) on the total cost of motionplans including the node The total cost of a trajectory [assumingthe cost functional is additive of the form (3)] can be split into anaccumulated cost and a cost-to-go The accumulated cost will ingeneral be available through bookkeeping it is a consequence ofdecisionsmade in the past The cost-to-gois in generalmuch moredif cult to compute

On the other hand we can easily compute a lower bound onthe cost-to-go this is given by the value of the optimal cost in theabsenceof obstaclesThe cost in the presenceof obstaclescannotbelower than the obstacle-free cost because the presence of obstaclestranslates into additional constraints on the problem

The upper bound on the cost-to-go is a priori unknown as amatter of fact we do not even know before starting the computa-tions if the problem is feasible or not As a consequence we mustinitialize the upper bound on the cost-to-goto the symbolic value ofin nity However if the optimal trajectory from a node to the targetis collision free then the correspondingcost clearly gives an upperbound on the cost-to-go For a node that can be connected to thetarget by an optimal trajectory the lower bound and upper boundcoincide Methods for updating the upper bound on the cost will bediscussed in Sec IIIG

As a nal piece of information for each node we store the totalnumber of other nodes that are part of its offspring

2 Data Stored at EdgesWhereas nodes in the tree represent states of the system along

trajectoriesedges can be thought of as representingdecisions takenin the constructionof the motion plan

As a consequence the main pieces of information to be stored atedges are the parameters identifying the control law implementedin the transitionnamely the next (randomly generated) equilibriumpoint xr

As a convenience for bookkeeping purposes we can store theincremental cost incurred along the edge In other words this cor-responds to the difference in the accumulated costs at the edge des-tination node and at the edge source node

C InitializationThe rst step in the algorithm is the initializationof the tree The

rst nodewhich will become the root of the tree containsthe initialconditionsprojected at some point in the future which depends onthe amount of time that will be allocated to the computation of themotion plan As an example if the vehicle at the initial time t0 isat position x0 moving at constant velocity v0 and we devote micro sto the computation of a motion plan the root node must be set atx0 C v0micro t0 C micro

The accumulated cost at the root node can be set to zero (alldecision variables will have effect later in time so there is no pointwith starting with a nonzero value) The lower bound on the cost-to-go can be computed as the value of the optimal cost functionfrom the root state To set the upper bound we attempt to reach


the target state x f using the optimal control law frac14cent x f If theresulting trajectory is collision free the optimal motion planningproblem is evidently solved and the algorithm can return with theoptimal solution Otherwise the upper bound is set to the symbol1 (ie it is not yet known whether the motion planning problemadmits a feasible solution or not) and the tree-building iteration isentered

D Tree Expansion StepThe main function to be performed at each step in the iteration

is the expansion of the tree The expansion is aimed at adding newnodes or milestones to the tree in such a way that the volume ofthe frac14 -reachable space of the tree is increased rapidly

At each iteration a new random target con guration xr is gen-erated from a uniform distribution This requires that the motion-planning algorithmconcentrateson a compact subset of the con g-uration space This limitation can be the result of an obstacle avoid-ance constraint (such as in the cases in which the vehicle moveswithin an enclosed environment) or can be arti cially imposed bylimiting the distance the vehicle can stray from the initial and nalpoints

Given the new random candidate milestone xr we must checkwhether or not it is in the frac14 -reachable set of the current tree To dothis we apply the control policy frac14cent xr starting from the (statetime) initial condition of each node in the tree until a feasiblecollision-free trajectory which reaches xr (or a neighborhood) attime tr is generated If no such a trajectory is found then xr is notin the frac14 -reachableset of the current tree and is discardedOtherwise(and pendingthe safetycheckdiscussedin the next section) the newtrajectorysegment is added to the tree as well as the new milestonexr tr

It is easy to see that the function just outlined does indeed resultin uniform sampling of the frac14 -reachable set of the current tree (ieit is an implementation of an IDEAL-SAMPLE procedure on thesymmetry group H ) The new candidate milestone xr is generatedfrom a uniform distributionon H and a thorough test is carried outto checkwhetheror not it is in the frac14 -reachableset of the current treeThe feedbackcontrollawfrac14cent xr is usedas the inversionmechanismthat provides the appropriate inputs at each instant in time

The order in which themilestonescurrentlyin the treeare checkedis as yet left unspecied A random ordering is acceptable How-ever some performance improvements are obtained in simulations(see Sec V) if frac14 reachability of the random candidate milestonexr is tested from tree nodes in a more ef cient way As a matter offact in randomized motion-planningalgorithms most of the time istypically spent in checkingtrajectoriesfor collisionswith obstaclesand methods are sought to reduce the number of collision checks58

1 Exploration HeuristicsBefore a feasible trajectory is found the emphasis of the algo-

rithm is on exploration that is on the addition on new milestonesto the tree that enlarge its reachable set To enhance explorationwhile keeping the number of collision checks to a minimum it canbe convenient to sort the nodes in ascending order of distance fromxr where as a measure of distance we use the value of the optimalcost function in the obstacle-free case J currencent xr At the cost of theadditional computation time required by the sorting of the nodes(and by the evaluationof distances) reachability is tested rst fromthe nodes that are closer to the candidate milestone and hopefullymore likely to provide a collision-free trajectory This will resultin a lower number of calls to the collision-checkingroutine (eg aroutine that evaluates Gx t along newly generated trajectories)This exploration heuristics is very closely related to the RRT algo-rithm with the only differencethat potentiallyeverynode in the treeis tested during the expansion step whereas in the RRT algorithmonly the closest node is tested

2 Optimization HeuristicsOnce a feasible trajectory has been found the focus of the search

shifts from exploration to the optimization of the computed trajec-tory To enhance the quality of the new additions to the tree wecan sort the nodes in ascending order of total cost to reach xr This

total cost consists of the accumulated cost up to the generic nodexi ti plus the cost to go from xi to xr that is J currenxi xr At thecost of the computationtime required to sort the nodes reachabilityis tested rst from the nodes which will provide the best trajectoryto the new milestoneThis will likely result in a better quality of thecomputed solution This optimization heuristic is most appropriatewhen a feasible solution is already available

As a last detail every time a new node is added to the tree thechildren counter of all the nodes on the path from the new node tothe root of the tree must be increasedby one A pseudocodeversionof the tree expansion function is given as Algorithm 1

Algorithm1Pseudocodefor the functionEXPAND-TREE (Tree)1) Generate a random con guration xr

2) sort the nodes in the tree according to the desired heuristics[random j exploration j optimization]

3) for all nodes in the tree (in the order establishedat the previousstep) do

4) Generate a trajectory to xr using the control policy frac14cent xr 5) Check the generated trajectory for obstacle avoidance6) if the trajectory is collision-free then7) return generated trajectory8) return failure

E Safety CheckBecause obstacles are moving checking for the absence of colli-

sion point-wise in time could not be enough If we assume boundedaccelerations the reachable set of any collision-free point can bemade arbitrarily small (eg a point a distance d in front of an ob-stacle moving with velocity v is collision free but will not be suchd=v seconds in the future) To ensure that the tree being built doesnot include such dead ends (ie milestoneswith a very small reach-able set) before adding a new milestone to the tree we check for itssafety over a time buffer iquest We will call iquest safety the property of amilestone to be collision free over a time iquest Accordingly we willsay that a point x f t f is frac14 iquest reachable from xi ti and belongsto the set Riquest

frac14 xi ti if it is frac14 reachable and iquest safeThe iquest -safety check can be carried out on the predicted evolu-

tion of the system at the relative equilibrium corresponding to thenew milestone If possible and practical the time buffer iquest can beextended to in nity This is the case for example for static environ-ments Otherwiseiquest should be chosen long enough that the success-ful computationof anothermotionplan is very likelyAssuming thatthe initial conditions are iquest safe the iquest -safety check ensures that thesame safety properties are maintained along the computed motionplan In the case in which iquest D 1 the iquest safety check translates intohard safety guaranteesfor the whole motionplan In cases for whichsafety cannot be ensuredover an in nite time horizoniquest safety onlyensures that the algorithm will always have at least iquest seconds tocompute a new solution

F Improving PerformanceThe tree expansion step outlined in Sec IIID generates trajec-

tories consisting of jumps from equilibrium point to equilibriumpointmdashor in the general case from a relative equilibrium to thesame relative equilibriumin a different locationon the reducedcon- guration space Cmdashand as such is unlikely to provide satisfactoryperformance in terms of the cost (3)

However and building upon previous ideas55iexcl57 performancemay be restored by realizing that the available guidance policy maynotonly steer the vehicle from equilibriumstate to equilibriumstatebut from any state to an equilibriumstate This suggests introducingthe following step Consider the tree at some point in time and anewly added milestone to the tree (which we will denote as a pri-mary milestone) A secondary milestone is de ned to be any stateof the system along the path leading from the parent node in the treeto the newly added milestone We can split any newly generatededge into n gt 1 segments at random the breakpoints will be thesecondary milestones and the endpoint is the primary milestoneBecause the vehicle is in motion along the path the secondarymile-stonesare likely to be at points in the state space that are far from theequilibrium manifold Secondary milestones are made available for


future tree expansion steps All secondarymilestones by construc-tion have a iquest -safe primary milestone in a child subtree and henceare themselves iquest safe

G Update on the Cost-to-Go EstimatesEach time a new milestone is added to the tree a check is made

to see if the nal target point is in its frac14 -reachable set In otherwords we try applying the control policy frac14cent x f to each newlygenerated milestone In the case in which this results in a feasiblecollision-freetrajectorywe havefounda sequenceofmilestonesandcontrol policies which steers the system from the initial conditionsto the nal target point along a feasible collision-free trajectoryand hence the feasibility problem is solved However we might beinterested in looking for the best possible trajectory satisfying thefeasibility problem and eventually try to approximate the solutionof the optimal motion planningprobleminvolving the minimizationof the cost functional (3)

With this purpose in mind every time a new feasible solution isfound the estimates of the upper bound on the cost-to-go of the af-fected node in the tree are updated The affected nodes are those inthe path from the newly added node to the root As a consequencein order to update the upper bound on the costs we climb the treebackward toward the root at each step we compare the old upperbound at the parentnode with the upper boundon the cost followingthe newly found trajectory If the latter is smaller that the old upperboundwe updatethe boundandreiteratefrom theparentOtherwisethe procedure stops (there is another subtree of the parent whichpresents a lower upper bound on the cost) A pseudocodeversionofthe cost estimate update function is given in Algorithm 2

Algorithm 2 Pseudocode for the function UPDATE-COST-ESTIMATES (Tree node target)

1) nodeLowerBoundAtilde J curren (nodex targetx )2) Generate the obstacle-free optimal trajectory to x f using the

control policy frac14centtargetx3) Check the generated trajectory for satisfaction of the obstacle

avoidance constraints4) if the trajectory is collision-free then5) nodeUpperBoundAtilde nodeLowerBound6) while node 6D Treeroot and nodeParentUpperBoundgt

node UpperBoundC nodeincomingEdgecost do7) nodeParentUpperBoundAtilde nodeUpperBoundC node

incomingEdgecost8) nodeAtilde nodeParent9) return success10) else11) nodeUpperBoundAtilde C112) return failure

H Tree PruningThe upper and lower boundson the cost-to-gostored for each tree

milestone can be pro tably used for pruning the tree and speedingup computations Recall that the lower bound coincides with theoptimal cost-to-go in the obstacle-free case and the upper boundis equal to the cost of the best trajectory from the milestone to thedestination x f if this trajectory has been found or C1 otherwise

Every time a new feasible solution is found the upper boundson the cost-to-go can be updated by climbing the tree backwardalong that feasible solution toward the tree root While performingthis operation it is also possible to look at all of the children of thenode being updated If the lower bound on the total cost-to-go forsuch children (plus the cost of the corresponding edge) is higherthan the upper bound on the cost-to-go for the current node thecorrespondingsubtree can be safely removed as it cannot possiblyprovide a better solution than the one that has just been found

The end result of such a process is the removal from the trajec-tory tree of all of the provably bad candidates for the optimal solu-tion The trajectory tree following this pruning process contains asmaller number of nodes thus improving the overall computationalef ciency However it must be kept in mind that tree pruning canonly be carried out once a feasible solution has been found and isof no help before that happens

I Real-Time ConsiderationsA signi cant issue arising from the usage of randomized algo-

rithms for path planning is the distinct possibility of driving thesystem toward a dead end as a result of nite computation timesThe notion of iquest safety was introduced in Sec IIIE to prevent suchsituations to develop

The iquest safety of the generated plan derives from the fact that allof the primary milestones are by construction iquest safe and all sec-ondary milestoneshave at least one primary milestones in their sub-tree Maintainingsafety guaranteesin the face of nite computationtimes is particularly important because the algorithm itself has nodeterministic guarantees of success In the sense just outlined thealgorithm will always produce safe motion plans even in the casein which a feasible trajectory to the target set has not been found

The time available for computation is bounded by either micro or bythe duration of the current trajectory segment When the time is upa new tree must be selected from the children of the current root Ifthere are nonebecauseeveryprimarymilestone is iquest safe the systemhas at least iquest seconds of guaranteed safety available for computinga new tree (secondary milestones always have at least one child) Ifthe current root has children then two cases arise

1) At least one of the children leads to the destination throughan already computed feasible solution If there are more than onesuch feasible solutions the solution with the least upper bound onthe cost-to-go is chosen

2) No feasiblesolutionshavebeen computedyet In this case thereis no clear indication of the best child to explore Maintaining thesame approachat the basis of the algorithmthe child to descendcanbe selected randomly according either to a uniform distribution orto a distributionweighted on the total numberof primary milestonesin the subchildrenof each tree In the latter case the selected tree islikely to cover a bigger portion of the reachable set

J Complete AlgorithmThe ow of the algorithm then proceeds as follows After the

initialization of the tree a loop is entered until the target has beenreached As the very rst step in each iteration of the loop an at-tempt is made to join the current root (the initial conditions) to thetarget con guration using the optimal control law computed for theobstacle-free case If the attempt is successful an optimal solutionhas been found and the algorithm terminates successfully

Otherwise an inner loop is entered which tries to expand thetrajectory tree At each iteration of the inner loop a random con g-uration is sampled and an attempt is made through the EXPAND-TREE functionto join a node in the current tree to the new candidatemilestone If this is possible and results in a iquest -safe trajectory (iethe candidate milestone is in the frac14 iquest -reachable set of the cur-rent tree) then the generated trajectory is split up into a number ofsegments which are added to the tree as new edges and the corre-sponding endpointswhich are added to the tree as new (primary orsecondary) milestones

From the newly generatedmilestonesan attempt is made to reachthe nal target If successful this attempt enables the update of theupper bound estimates on the cost-to-go on the tree nodes In thiscase the tree can be pruned to eliminate nodes in the tree which areguaranteedto bebadcandidatesfor furtherexploration(becausetheycannot improve on the quality of the solution already computed)

This procedure is perhapsbetter understood through an examplegiven in Fig 1 The current tree is depicted as a set of thick linesthe squares represent the primary milestones and the circles rep-resent secondary milestones The target point x f is not reachablefrom any of the milestones currently in the tree by using the policyfrac14cent x f (ie the optimal policy to x f assuming there are no obsta-cles) Thus a new candidate milestone xr is randomly generatedand then attempts are made to join the nodes of the tree to xr Theclosest milestone in the sense induced by the cost-to-go functionJ currencent xr is indicated by the number one Application of the policyfrac14cent xr with initial condition corresponding to this milestone re-sults in a collision with one of the obstacles The same can be saidfrom the second closest milestone indicated by the number twoWith the third-closest milestone we have better luck and we cangenerate a new collision-free trajectory A new primary milestone


Fig 1 Example of tree expansion procedure

is created at xr a new secondary milestone is created at a randompoint on the newly generated trajectoryand the two new milestonesare connectedto the tree throughedges encoding the new trajectoryFinally we check whether or not the target point is reachable fromthe new milestones In this case the answer is indeed positive andthe feasibility problem is solved If this is not the case (or a bettersolution is sought) the iteration would be started again with a newrandom point xr

This iterationis carriedout until the time allowed for computationhas elapsed This time is given by the time duration of the edge ofthe tree that is currently being executedand is therefore not a priori xed The iteration can be preempted with no adverse consequenceon the algorithm

At this point if at least one feasible solution had been found thesubtreeof the rootcorrespondingto the least upperboundon thecostis chosen If no feasible solution has been found yet a subtree todescend is chosen randomly from a distribution weighted with thetotal numberof milestones in each subtree If the current root has nochildren a new root is created which corresponds to the predictedroot state at some time in the future (this time interval being thecomputation time allotted for the next main loop iteration)

The rest of the tree is deleted and removed from memory be-cause it contains only trajectories that can no longer be reached (itis not possible to go back in time) The main loop iteration is thenrepeated A pseudocode version of the main algorithm is given inAlgorithm 3

Algorithm 3 Pseudocode for the motion-planningalgorithm1) Initialize Tree with the initial conditions at time t0 C micro 2) loop3) if UPDATE-COST-ESTIMATES (Tree root target)D suc-

cess then4) Terminate with success5) repeat6) newTrajectoryD EXPAND-TREE (Tree)7) if newTrajectory 6D failure and newTrajectory is iquest safe

then8) Split newTrajectory and generate primary and secon-

dary milestones9) for all new milestones do10) UPDATE-COST-ESTIMATES (Tree node target)11) Prune tree12) until Time is up13) if TreerootUpperBoundlt 1 fFeasible solution foundg

then14) nd the child of root which gives the least upper bound

on the cost-to-go15) TreerootAtilde best child

16) else if Root has children then17) Choose a child according to a random distribution

weighted with the number of children in each subtree18) else19) propagate root for a time micro in the future

IV AnalysisThis sectionaimsat analyzingthebehaviorof thealgorithmprov-

ing probabilistic completeness and obtaining performance boundsThis requires additional de nitions and assumptions about the en-vironment characteristics The remainder of the section presentsthe concepts supporting most of the available results about algo-rithms based on probabilisticroadmapsNew de nitions are neededto adapt earlier results to the motion algorithm presented in thischapter in particular to accountfor the closed-loopdynamicsand tobetter characterizethe assumptionsabout the dynamicenvironment

A Assumptions on the EnvironmentFirst of all we require that for all iquest -safe equilibrium points the

frac14 iquest -reachable set be not too small taking into account also com-putational delays This property corresponds to the sup2 goodness of astatic workspace59 The planning environment (as de ned by boththe workspaceand the control policy) is said to be sup2 iquest good if forall sets Siquest iexclmicro frac12 F of iquest iexcl micro safe equilibrium points the followingholds

sup1poundRiquest

frac14 Siquest iexcl micro curren

cedil sup2

In the preceding sup1S indicates the volume of the projection ofthe set S frac12 X D C pound Y on C The volume measure is normalized insuch a way that the volume of the compact subset of X de ningthe workspace is equal to one The proof of algorithm performancerelies upon the following properties which will be assumed to betrue for the system under consideration

Let macr be a constant in 0 1] de ne the macr-lookout of S frac12 F as

macr-lookoutS Dcopy

p 2 Sshyshysup1

iexclRiquest

frac14 pdeg

Scent

cedil macrsup1[RSnS]ordf

We requirethat the dimensionsof the macr lookoutof any set shouldnotbe too small this property is called reg macr expansiveness33 Giventwo constantsreg macr in 0 1] theenvironmentis said reg macr expansiveif for all sets S 2 F the following holds

sup1[macr-lookoutS] cedil regsup1S

B Algorithm PerformanceConsider the initial condition x0 t0 and assume it is an equilib-

rium point (if not generate a primary milestoneusing the algorithmpresented in the preceding section) De ne the end-game regionE frac12 H as a region such that all equilibrium points contained in itcan be connected without collisions to the desired destination x f

using the policy frac14cent x f for all times t Then if the environmentis reg macr expansive and the desired destination x f is contained inthe reachable set Rx0 t0 it can be shown that the probability ofthe algorithmreturning a feasible trajectory connecting x0 to x f ap-proaches unity exponentiallyfast The results that will be presentedin the followingare basedon Hsu et al37 with a few minormodi ca-tions to address the usage of the policy frac14 in the explorationprocessas opposed to random piecewise constant inputs The proofs of thelemmas are available in the reference and are omitted here for lackof space

First of all we need to characterize the expected number of look-out points in a tree We have the following

Lemma 1 (number of look-out points37) In a tree with r mile-stones the probability of having k look-out points is at least1 iexcl k expiexclregbr=kc

The following step is to quantify the size of the frac14 iquest reachableset given the presence of at least k lookout points in the tree

Lemma 2 (size of reachable set37 ) If a sequence of milestonescontains k lookout points the volume of its frac14 iquest reachable set isat least 1 iexcl expiexclmacrk

Finally we can state the main theorem


Theorem 1 (performance of the randomized algorithm37 ) A se-quence of r (primary) milestones contains a milestone in the end-game region E with probability at least 1 iexcl deg if

r cedil k=reg log2k=deg C [2=sup1E] log2=deg

where k D 1=macr log[2=sup1E]Proof Split the sequence of milestones into two subsequencesof

r1 and r2 milestonesrespectivelyIf the numberof milestones in the rst sequence is greater than k D 1=macr log[2=sup1E] then the frac14 iquest reachable set of the rst sequence has an intersection of volume atleast sup1E=2 with the end-game region E Let us call event A theevent that the number of lookout points is greater than k This eventoccurs with a probability

PrA cedil 1 iexcl expiexclregbr1=kc

If we want to make this probability at least 1 iexcl deg =2 we need

r1 cedil k=reg log2=deg

Assume that the intersection of the frac14 iquest -reachable set of the rstsequence of milestones with the end-game region is sup1E=2 Thenthe probability that at least one of the milestones in the secondsequence will be in the end-game region (event B) is

PrB cedil 1 iexcl exp[iexclr2sup1E =2]

To make this probability at least 1 iexcl deg =2 we need

r2 cedil [2=sup1E] log2=deg

If events A and B occur thenoneof themilestonesin the completesequence is in the end-game region We have that if r cedil r1 C r2

PrA ^ B D PrAPrB cedil 1 iexcl deg =22 cedil 1 iexcl deg

which proves the resultTo our knowledgethe algorithmpresentedin this paper is the rst

one to which Theorem 1 fully applies with the de nitions and un-der the assumptionsgiven earlier in this section because the frac14 iquest-reachable set is indeed uniformly sampled In some sense the mostsigni cant contributionof this paper is to propose to shift the searchfor reachable milestones from an open-loop process whereby ex-ploration is done by randomly sampling the controls available tothe system to a closed-loop process whereby the exploration isdone by randomly (and uniformly) sampling the milestones andthe obstacle-freeguidancesystem then chooses the controls leadingthe vehicle from its current state to that milestone In this sense theguidancelaw can in fact be interpretedas a practical implementationof the ideal inversion mechanism considered in the literature37

Moreover we are able to recover by sorting nodes according todistance the levels of performance shown in practice by RRT al-gorithms As a consequence we are able to achieve probabilisticcompleteness and formal performance bounds in a dynamic envi-ronment and at the same time we can exploit the rapid explorationcapabilitesof RRTs in this sense the algorithmpresented in Sec IIIrecovers all of the best properties of its predecessors

The performance bounds that can be obtained for this algorithmestablish only its theoretical soundness but cannot be used for ob-taining an explicit estimate of the probability of successful termi-nation becausereg macr and sup2 cannot be computed easily for nontrivialenvironments An interesting fact is that the computational com-plexity (in terms of convergence rate of the probability of correcttermination) does not depend on the dimension of the state spaceor on the number of obstacle directly Instead it dependson param-eters that in some sense quantify the geometric complexity of theenvironment

Using secondary milestones does not adversely impact the mainresults on probabilistic completeness (when evaluated on the num-berof primarymilestones)becausetheycanonly increasethe frac14 iquest-reachable set of the whole tree In addition secondary milestoneshave been found to help the convergence of the algorithm whenclose to the end-game region and enhance the overall quality of theresulting trajectory (ie the trajectory is faster or in general lessexpensive with respect to the assigned cost)

V Application ExamplesIn the next sections we present three examples that show the

power and the exibility of the proposed algorithm We consider rst a linear system subject to saturation constraintsand then a sys-tem endowed with a control architecture patterned after the hybridcontrol structure described in Sec IIA2 All algorithms have beenimplemented in (soft) real time on a CCC on a Pentium II 300MHz machine running Linux using the LEDA library60 Compar-isons will be presented on the computation times required by thefollowing variations of incremental randomized planners

Algorithm A Only one node chosen randomly from the currenttree is tested for expansion at each EXPAND-TREE step Thiscorresponds roughly to the algorithm proposed by Hsu et al37

AlgorithmB Only one node chosen as the closest one to the can-didate milestone is tested for expansion at each EXPAND-TREEstep This corresponds to the RRT algorithm35 with the differ-ence that the obstacle-free optimal cost-to-go is used to computedistances

Algorithm C All nodes are tested in random orderAlgorithmD All nodesare tested in increasingorderof distance

This correspondsto the full- edgedimplementationof thealgorithmin Sec III

The inputs are always chosen according to the optimal controlpolicy in the obstacle-free case Random control inputs are not ap-propriate for most of the dynamical systems we are consideringbecause they could lead easily to instability (for example a heli-copter is an open-loop unstable system) If distances are measuredaccording to the optimal cost function in the obstacle-free case agreedy search for the control generation (as in the RRT algorithm)does provide a stabilizing control law Moreover in all cases anyadditionalcomputationtime availableafter the rst feasiblesolutionis found is used to optimize the solution

The statistical data are referred to a data set of 1000 simulationruns for each example

A Ground RobotIn this sectionwe are interestedin minimumtime motionplanning

for a planar system with (scaled) equations of motion

Rx1 C Px1 D u1 Rx2 C Px2 D u2 (5)

The magnitude of each control u1 and u2 is assumed to be boundedby umax Although this system model is quite simple it is a goodrepresentation of the ground robots used by the Cornell Universityteam to win the RoboCup-2000 contest6162 The following controllaw is adapted from the same references

1 Minimum-Time Minimum-Energy Control LawFor any one axis let the initial position and velocity be x0 and v0

the nal (equilibrium)conditionsare characterizedby a desired nalposition x f and zero velocity The minimum time maneuver fromorigin to destination for each of the degrees of freedom (assuminga general maximum control intensity umax) is a bang-bang controllaw24 given by

ut D U for 0 lt t lt t1

ut D iexclU for t1 lt t lt t1 C t2 (6)

The sign of the initial control value U can be determined throughthe switching function

10 Draquo

x0 iexcl x f C v0 iexcl umax log1 C v0=umax for v0 cedil 0

x0 iexcl x f C v0 C umax log1 iexcl v0=umax for v0 lt 0(7)

If the initial conditions are such that 10 cedil 0 then U D iexclumax andU D umax otherwise

The time lengthof the two bang-bangsegmentscanbedeterminedas follows

t1 D t2 iexcl C=U

t2 D logpound1 C

p1 iexcl expC=U 1 iexcl v0=U

curren(8)

with C D x0 C v0 iexcl x f


The policy frac14 used to control the vehicle described by Eqs (5)is then de ned as follows Considering the two degrees of freedomx1 and x2 the slowest axis is determined rst and the correspond-ing time optimal control is applied Let tcurren

min be the minimum timecorresponding to that axis

The other fastest axis is then controlled using a minimum effortsolution by solving the minimum time problem using Eqs (6)with U D sectdeg umax and by iterating over the parameter deg 2 0 1until tmin D tcurren

min

2 Fixed and Moving SpheresThe randomized path planning has been tested in several exam-

ples including cases with both xed and moving obstacles and ingeneral proved to be very fast and reliable

The rst example involves navigating the ground robot through aset of obstacles representedas spheres in the con guration space Inthe tests both xed spheresand spheresmovingat a constantrandomspeed were considered with no signi cant statistical difference inthe data sets This example was very easily handled by the random-ized algorithms A feasible solution was found by all algorithms inless than 20 ms in 50 of the cases with no signi cant differencesin performance A feasible solution was found by all algorithms inall test cases A summary of the average time required to nd the rst feasible solution tfeas and its standard deviation is reported inTable 1

The cost of the trajectory that is eventually executed has beenanalyzed In the examples the lower bound on the cost is 1139 s(this is the cost of an optimal trajectory in the obstacle-free casetheredoesnot exista uniqueoptimal trajectorybutmany trajectoriesminimize the cost) In all cases but for Algorithm 2 an optimaltrajectory was found in at least 50 of the test cases On averagethe cost of the solution was less than 5 higher than the lowerbound showing that the algorithms do indeed provide a solutionthat is almost optimal In this scenarioAlgorithms A and C relyingcompletely on randomization (as opposed to RRT-like heuristics)seemed to fare slightly better than the others A summary of theaverage cost of the solution t f provided by the algorithms and itsstandard deviation is also reported in Table 1

In this case the environment is open and all of the randomizedalgorithms can achieve extremely high levels of performance evenin the presence of moving obstacles

Table 1 Ground robot moving amidst xed spheressimulation results

Average St dev Average St devAlgorithm (tfeas ) ms tfeas ms (t f ) s (t f ) s

A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055

Fig 2 Ground robot moving through sliding doors trajectory tree traces Algorithm A (left) and algorithm D (right)

3 Sliding DoorsIn the secondexamplethe robotmustgo throughmovingopenings

in two walls The walls are 40 m apart and have ldquodoorsrdquo 10 m wide(Fig 2) Both the walls slide along their longitudinalaxis accordingto a harmonic law The frequencies of the two oscillations are 05rads for the wall at the bottom and 025 rads for the wall at the topof the picture The amplitude of the harmonic motion in both thecases is 40 m yielding a maximum door velocity of 20 ms twiceas much as the maximum velocity of the robot

This scenariowas expected to be a dif cult challengefor the ran-domized planners however most of the algorithms were able todeal with it quite effectively (see Table 2) Algorithm A was slowin nding a feasible solution (it took an average time of over 40 s)but the quality of the resulting plans was consistently good Algo-rithms C and D were again the best performers nding a feasibletrajectory within a few seconds Moreover the average cost of theplans generated by Algorithm C is within 22 of the lower boundon the optimal cost

The reason for the poor performanceof Algorithm A in this caseis easily seen from Fig 2 this algorithm suffers from the lack ofef cient exploration and nding the narrow passage between themoving doors requires on average much more samples than usingthe other algorithms In the gures the heavy line represents the tra-jectory that is eventuallyexecuted (ie the best computedsolution)whereas the lighter lines represent the other trajectories (edges) inthe trajectory tree The squares represent primary milestones andthe circles represent secondary milestones Milestones that can beconnected to the target by the obstacle-freeoptimal control law are lled in a darker shade

B Small Autonomous HelicopterOne of the prime motivations for the development of the algo-

rithm presented in this chapter is the path-planning task for a smallautonomous helicopter

This section presents simulation results for a test case involvinga small autonomous helicopter The simulations rely upon a fullynonlinear helicopter simulation based on the model presented inRefs 40 and 41 and in the references therein The motion-planningalgorithms operating on the nominal maneuver automaton structurediscussed in Sec IIA2 are complemented by a nonlinear trackingcontrol law41 to ensure tracking of the computed trajectory

Table 2 Ground robot moving through sliding doorssimulation results

Average St dev Average St devAlgorithm (tfeas ) s tfeas s (t f ) s (t f ) s

A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588


Fig 3 Helicopter ying through sliding doors trajectory tree traces Algorithm B (left) and algorithm D (right) The RRT-like algorithm on the leftcan get ldquolockedrdquo by the rst computed trajectories

Table 3 Helicopter moving amidst xed spheressimulation results


A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242

The trajectory primitive library was built by choosing trim tra-jectories in level ight with no sideslip and ying at the followingvelocitiesf0 125 25 5 10g ms with the followingheading turn-ing rates fiexcl1 iexcl05 0 05 1g rads This gives a total of 25 trimtrajectories Time-optimal maneuvers were generated connectingeach of the trim trajectories to all of the others giving a total of 600maneuvers

The plannerwas testedusing the same scenariosas for the groundrobot examples The output of the simulations was scaled in such away as to provide a meaningful comparison of the two cases Thecost function used in all of the examples is the total time needed togo to the destinationThe initial conditionsare at hover headingdueEast (ie to the right in the pictures)

1 Fixed and Moving SpheresThe rst example involvesnavigating the helicopterthrougha set

of xed spheres As in the case of the ground robot this examplewas very easily handled by the proposed planners the average timerequired for computation of a feasible solution is of the order of afew tenths of a second (see Table 3)

2 Sliding DoorsIn the last example the helicoptermust go through moving open-

ings in two walls (see Fig 3) This scenario proved to be very chal-lenging for the randomized planners Algorithm A failed to nd afeasible solution within two minutes in the totality of the simula-tions runs (1000) AlgorithmB did better ndinga feasiblesolutionwithin two minutes in 10 of the cases (in which the feasible trajec-tory was found extremelyquickly in about 25 s) AlgorithmsC andD on the other hand always succeeded in nding a solution eventhough it took about 50 s on average The total cost of the solutionsfound was slightly above one minute on average (see Table 4)

C Discussion of the ResultsFrom the analysis of the simulation results both in the ground

robot example and in the helicopter example we can conclude thatall of the tested algorithms perform equally well in simple caseswith very open environmentsAs the dif culty of the test scenariosincreases it is apparent that Algorithms C and D (two versionsof the algorithm proposed in this chapter) performed better than

Table 4 Helicopter ying through sliding doorssimulation results (If the computation times exceededthe allotted time of 120 s the value of 120 s was taken

into account in the statistics)


A mdashmdash mdashmdash mdashmdash mdashmdashB 10826 3558 11804 2878C 4889 4233 6483 2579D 5244 3851 6818 3259

Algorithms A and B both in terms of computation time requiredto solve the feasibility problem and in terms of the quality of thecomputed solution

Although Algorithm A does have probabilistic completenessguarantees in the case of dynamic environments (as opposed toAlgorithm B whichdoesnot) it suffersfrominef cientexplorationas it can easily be recognized from Fig 2

Algorithm B although performing well in static environmentsshows its limits in the case of dynamic environments (in fact it isnot probabilisticallycomplete in this case) This algorithm is char-acterized by a very ef cient explorationof the workspacehoweverit tends to get locked in the rst computed trajectorieswhich eventhough close to the objective could not result in feasible trajecto-ries to the target (Fig 3) It is believed that this is the reason of thebehavior noted in the case of the helicopter ying through slidingdoors

Algorithms C and D provided the best results among the algo-rithms tested Even though the performance characteristicsare verysimilar Algorithm D seems to nd the rst feasible solution in ashorter time than C The reason for this can be found in the fact thatby sorting the nodes in order of increasing distance from the can-didate milestone the ef cient exploration characteristics of RRTsare recoveredwhile ensuringprobabilisticcompletenessand expo-nential convergence to one of the probability of correct executioneven in a dynamic environment On the other hand Algorithm Cgenerally resulted in solutionswith a lower cost this can be seen asanothereffect of the fact that RRT-like algorithms tend to get lockedin the rst solutions they compute and do not explore other options

VI ConclusionsIn this paper a randomized motion-planning algorithm was pre-

sentedbased on using obstacle-freeguidancesystemsas local plan-ners in a probabilistic roadmap framework The main advantage ofthe algorithm is the capability to address in an ef cient and naturalfashion the dynamics of the system while at the same time provid-ing a consistent decoupling between the motion planning and thelow-level control tasks The resulting algorithm has been shown tobe exible enough to handle a broad variety of dynamical systems


including systems described by ordinary differential equations aswell as hybrid systems In a related work by the authors the samealgorithmhas been applied to dynamical systems evolving on man-ifolds as in the case of attitude motion planning for spacecraft63

From a theoretical point of view it was shown how to performuniform sampling in the reachable space of the vehicle as opposedto sampling in the input space Real-time issues were directly ad-dressed In the case in which nite computation time and availableresources do not allow the computation of a feasible solution be-fore a decision has to be made it was shown how to ensure safetyand how to choose likely candidates for further explorationFuturework will address motion planning in uncertain environmentswithlimited sensor range and multivehicle operations

AcknowledgmentsThis research was supported by the C S Draper Laboratory

through the IRampD Grant DL-H-505334 by the US Air Force Of- ce of Scienti c Research under Grant F49620-99-1-0320and bythe Of ce of Naval Research under Young Investigator Award N-00014-99-1-0668The authors would like to thank the anonymousreviewers for many valuable comments and suggestions

References1Latombe J-C ldquoMotion Planning A Journey of Robots Molecules

Digital Actors and Other Artifactsrdquo International Journal of Robotics Re-search Vol 18 No 11 1999 pp 1119ndash1128

2Latombe J-C Robot Motion Planning Kluwer Dordrecht TheNetherlands 1991 Chaps 1ndash4

3Li Z and Canny J (eds) Nonholonomic Motion Planning KluwerAcademic Norwell MA 1993 Chaps 1ndash11

4Laumond J-P (ed) RobotMotionPlanningandControl Lecture Notesin Control and Information Sciences Vol 229 Springer-Verlag London1998

5Lozano-Perez T ldquoAutomaton Planning of Manipulator Transfer Move-mentsrdquo IEEE Transactions on Systems Man and Cybernetics Vol 11No 10 1981 pp 681ndash698

6Lozano-Perez T andWesley M ldquoAn AlgorithmforPlanningCollision-Free Paths Among Polyhedral Obstaclesrdquo Communications of the ACMVol 22 No 10 1979 pp 560ndash570

7Edelsbrunner H Algorithms in Combinatorial Geometry Springer-Verlag Berlin 1987 pp 275ndash278

8OrsquoDunlaing P and Yap C ldquoA Retraction Method for Planning theMotion of a Discrdquo Journal of Algorithms Vol 6 No 1 1982 pp 104ndash111

9Khatib O ldquoReal-Time Obstacle Avoidance for Manipulators and Mo-bile Robotsrdquo International Journal of Robotics Research Vol 5 No 11986 pp 90ndash98

10Hwang Y and Ahuja N ldquoA Potential Field Approach to Path Plan-ningrdquo IEEE Transactions of Robotics and Automation Vol 8 No 1 1992pp 23ndash32

11Barraquand J Langlois B and Latombe J ldquoNumerical PotentialField Techniques for Robot Path Planningrdquo IEEE Transactions on SystemsMan and Cybernetics Vol 22 No 2 1992 pp 224ndash241

12Rimon E and Koditscheck D ldquoExact RobotNavigation Using Arti -cial Potential Fieldsrdquo IEEE Trasactionson Robotics and Automation Vol 8No 5 1992 pp 501ndash518

13Reif J ldquoComplexity of the Moverrsquos Problem and GeneralizationsrdquoProceedings of the 20th IEEE Symposium on Foundationsof Computer Sci-ence Inst ofElectrical and Electronics Engineers New York1979pp421ndash

42714Sedgewick P Algorithms 2nd ed Addison Wesley Longman Read-

ing MA 1988 Chap 4515Schwarz J and Sharir M ldquoOn the Piano Moverrsquos Problem I The

Case of a Two-Dimensional Rigid PolygonalBody Moving Amidst Polygo-nal Barriersrdquo Communications on Pure and Applied Mathematics Vol 36No 1 1983 pp 345ndash398

16Canny J The Complexity of Robot Motion Planning ACM DoctoralDissertation Award Massachusetts Inst of Technology Press CambridgeMA 1988

17Fliess M Levine J Martin P and Rouchon P ldquoFlatness and Defectof Nonlinear Systems Introductory Theory and Examplesrdquo InternationalJournal of Control Vol 61 No 6 1995 pp 1327ndash1361

18van Nieuwstadt M J and Murray R M ldquoReal-Time Trajectory Gen-eration for Differentially Flat Systemsrdquo InternationalJournal of Robust andNonlinear Control Vol 8 No 11 1998 pp 995ndash1020

19Faiz N Agrawal S and Murray R ldquoTrajectory Planningof Differen-tially Flat Systems with Dynamics and Inequalitiesrdquo Journal of GuidanceControl and Dynamics Vol 24 No 2 2001 pp 219ndash227

20Milam M B Mushambi K and Murray R ldquoA Computational Ap-proach to Real-Time Trajectory Generation for ConstrainedMechanical Sys-temsrdquo Proceedings of the 34th IEEE Conference on Decision and ControlInst of Electrical and Electronics Engineers New York 2000 pp 845ndash851

21Laumond J-P ldquoSingularities and Topological Aspects in Nonholo-nomic Motion Planningrdquo Nonholonomic Motion Planning edited by Z Liand J Canny Kluwer Academic Norwell MA 1993 pp 149ndash200

22Laferriere G and Sussmann H J ldquoA Differential Geometric Ap-proach to Motion Planningrdquo Nonholonomic Motion Planning edited byZ Li and J Canny Kluwer Academic Norwell MA 1993 pp 235ndash270

23Bullo F and Lynch K M ldquoKinematic Controllability for DecoupledTrajectory Planning in Underactuated Mechanical Systemsrdquo IEEE Transac-tions on Robotics and Automation Vol 17 No 4 2001 pp 402ndash412

24Bryson A E and Ho Y C Applied Optimal Control HemisphereNew York 1975 Chaps 2 3

25Athans M and Falb P Optimal Control McGrawndashHill New York1966 Chaps 1ndash10

26Pontryagin L Boltanskii V Gramkrelidze R and Mischenko EThe Mathematical Theory of Optimal Processes Interscience New York1962 Chaps 1 2

27Bellman R Dynamic Programming Princeton Univ Press PrincetonNJ 1957 Chap 3

28Kavraki L E Kolountzakis M N and Latombe J ldquoAnalysis ofProbabilistic Roadmaps for Path Planningrdquo Proceedings of the 1996 IEEEInternational Conference on Robotics and Automation Vol 4 Inst of Elec-trical and Electronics Engineers New York 1996 pp 3020ndash3025

29Overmars M H and Svestka P ldquoA Probabilistic Learning Approachto Motion Planningrdquo TR UU-CS-1994-03 Dept of Computer ScienceUtrecht Univ Utrecht The Netherlands Jan 1994

30Kavraki L Svestka P Latombe J and Overmars M ldquoProbabilisticRoadmaps for Path Planning in High-Dimensional Con guration SpacesrdquoIEEE Transactions on Robotics and Automation Vol 12 No 4 1996pp 566ndash580

31Kavraki L E Latombe J Motwani R and Raghavan P ldquoRandom-ized Query Processing in Robot Path Planningrdquo Journal of Computer andSystem Sciences Vol 57 No 1 1998 pp 50ndash60

32Hsu D Kavraki L Latombe J Motwani R and Sorkin S ldquoOnFinding Narrow Passages with Probabilistic Roadmap Plannersrdquo RoboticsTheAlgorithmicPerspective editedby P K Agarwal LKavraki M Masonand A K Peters Natick MA 1998 pp 141ndash153

33Hsu D Latombe J-C and Motwani R ldquoPath Planning in ExpansiveCon guration Spacesrdquo International Journal of Computational Geometryand Applications Vol 9 No 4ndash5 1999 pp 495ndash512

34LaValle S M ldquoRapidly-Exploring Random Trees A New Tool forPath Planningrdquo Iowa State Univ TR 98-11 Ames IA Oct 1998

35LaValle S and Kuffner J ldquoRandomized Kinodynamic PlanningrdquoProceedings of the 1999 IEEE International Conference on Robotics andAutomation Vol 1 Inst of Electrical and Electronics Engineers New York1999 pp 473ndash479

36Kuffner J and LaValle S ldquoRRT-Connect An Ef cient Approach toSingle-Query Path Planningrdquo IEEE International Conference on Roboticsand Automation Vol 2 Inst of Electrical and Electronics Engineers NewYork 2000 pp 995ndash1001

37Hsu D Kindel R Latombe J-C and Rock S ldquoRandomized Kino-dynamic Motion Planning with Moving Obstaclesrdquo Algorithmic and Com-putational Robotics New Directions edited by B Donald K Lynch andD Rus A K Peters Boston 2001 pp 247ndash264

38Barraquand J and Latombe J ldquoNonholonomic Multibody MobileRobots Controllability and Motion Planning in the Presence of ObstaclesrdquoAlgorithmica Vol 10 No 2ndash4 1993 pp 121ndash155

39Arnold V Mathematical Methods of Classical Mechanics 2nd edGTM Vol 60 Springer-Verlag New York 1989 Chap 3

40Koo T and Sastry S ldquoOutputTracking ControlDesign of a HelicopterModel Based on Approximate Linearizationrdquo Proceedings of the IEEE Con-ference on Decision and Control Vol 4 Inst of Electrical and ElectronicsEngineers New York 1998 pp 3635ndash3640

41Frazzoli E Dahleh M and Feron E ldquoTrajectory Tracking Con-trol Design for AutonomousHelicopters Using a Backstepping AlgorithmrdquoAmerican Control Conference Vol 6 Inst of Electrical and ElectronicsEngineers New York 2000 pp 4102ndash4107

42Frazzoli E Dahleh M and Feron E ldquoA Hybrid ControlArchitectureforAggressiveManeuveringofAutonomousHelicoptersrdquo IEEE ConferenceonDecision andControl Vol 3 Inst ofElectrical and ElectronicsEngineersNew York 1999 pp 2471ndash2476

43Frazzoli E Dahleh M and Feron E ldquoRobust Hybrid Control forAutonomous Vehicle Motion Planningrdquo IEEE Conference on Decision andControl Vol 1 Inst of Electrical and Electronics Engineers New York2000 pp 821ndash826

44Frazzoli E ldquoRobust Hybrid Control for Autonomous Vehicle Mo-tion Planningrdquo PhD Dissertation Dept of Aeronautics and AstronauticsMassachusetts Inst of Technology Cambridge MA June 2001


45Shaw R L Fighter Combat Tactics and Maneuvering Naval InstPress Annapolis MD 1985 Chaps 2ndash4

46Austin F Carbone G Falco M Hinz H and Lewis M ldquoGameTheory for Automated Maneuvering During Air-to-Air Combatrdquo Journalof Guidance Control and Dynamics Vol 13 No 6 1990 pp 1143ndash

114947Smith R and Dike B ldquoLearning Novel Fighter Combat Maneuver

Rules via Genetic Algorithmsrdquo International Journal of Expert SystemsVol 8 No 3 1995 pp 247ndash276

48Thomson D and Bradley R ldquoMathematical De nition of HelicopterManeuversrdquo Journal of the American Helicopter Society Vol 42 No 41997 pp 307ndash309

49Boyle D P and Chamitoff G ldquoAutonomous Maneuver Tracking forSelf-PilotedVehiclesrdquo JournalofGuidanceControlandDynamics Vol 22No 1 1999 pp 58ndash67

50Bertsekas D P and Tsitsiklis J Neuro-Dynamic ProgrammingAthena Scienti c Belmont MA 1996 Chap 6

51Dubins L ldquoOn Curves of Minimal Length with a Constraint on Aver-age Curvature and with Prescribed Initial and Terminal Positions and Tan-gentsrdquo American Journal of Mathematics Vol 79 No 1 1957 pp 497ndash

51652Reeds J and Shepp R ldquoOptimal Paths for a Car That Goes Both

Forwards and Backwardsrdquo Pacic Journal of Mathematics Vol 145 No 21990 pp 367ndash393

53Soueres P and Boissonnat J ldquoOptimal Trajectories for Nonholo-nomic Mobile Robotsrdquo Robot Motion Planning and Control edited byJ Laumond Lecture Notes in Control and Information Sciences Vol 229Springer-Verlag London 1998 pp 93ndash170

54Bertsekas D Dynamic Programming and Optimal Control AthenaScienti c Belmont MA 1995 Chap 1

55Leonessa A Chellaboina V and Haddad W M ldquoGlobally Stabi-lizing Controllers for Multi-Mode Axial Flow Compressors via Equilibria-

DependentLyapunovFunctionsrdquoProceedingsof the 1997American ControlConference Vol 2 Inst of Electrical and Electronics Engineers New York1997 pp 993ndash997

56Burridge R R Rizzi A A and Koditscheck D E ldquoSequentialDecomposition of Dynamically Dexterous Robot Behaviorsrdquo InternationalJournal of Robotics Research Vol 18 No 6 1999 pp 534ndash555

57McConley M Appleby B Dahleh M and Feron E ldquoA Compu-tationally Ef cient Lyapunov-Based Scheduling Procedure for Control ofNonlinear Systems with Stability Guaranteesrdquo IEEE Transactions on Auto-matic Control Vol 45 No 1 2000 pp 33ndash49

58Bohlin R and Kavraki L E ldquoA Randomized Algorithm for RobotPath Planning Based on Lazy Evaluationrdquo Handbookon Randomized Com-puting edited by P Pardalos S Rajasekaran and J Rolim Kluwer Aca-demic Norwell MA 2000

59Kavraki L and Latombe J ldquoProbabilistic Roadmaps for Robot PathPlanningrdquo Practical Motion Planning in Robotics Current Approaches andFuture Directions edited by K Gupta and A del Pobil Wiley New York1998 pp 33ndash53

60Mehlhorn K and Naher S The LEDA Platform of CombinatorialandGeometric Computing Cambridge Univ Press Cambridge England UK1999 Chaps 1ndash3 5 6 11 12

61DrsquoAndrea R Kalmar-Nagy T Ganguly P and Babish M ldquoTheCornell Robot Soccer Teamrdquo RoboCup-00 Robot Soccer World Cup IVedited by P Stone Lecture Notes in Computer Science Springer-Verlag(to be published)

62Kalmar-Nagy T Ganguly P and DrsquoAndrea R ldquoReal-Time Near-Optimal Trajectory Control of an Omni-Directional Vehiclerdquo Proceedingsof the ASME IMECErsquo01 Symposium on Modeling Control and Diagnosticsof Large-Scale Systems (to be published)

63Frazzoli E Dahleh M Feron E and Kornfeld R ldquoA Random-ized Attitude Slew Planning Algorithmfor AutonomousSpacecraftrdquo AIAAPaper 2001-4155 Aug 2001


such as memory which are polynomial in the size of the inputThe run time is not constrainedThe complexity class NP is knownto be a subset of PSPACE moreover it is believed to be a propersubset14 ) An algorithm for path planning in a con guration spaceof dimensionn with obstaclesde ned by m polynomialconstraintsof degree d has been establishedby Schwartz and Sharir15 the timecomplexity of the algorithm is (twice) exponentialin n and polyno-mial in m (geometricalcomplexity) and in d (algebraiccomplexity)The most ef cient algorithmcurrentlyavailablefor solvingthe sameproblem from Canny16 is singly exponential in n The precedingresults strongly suggest that the complexity of the path-planningproblem grows exponentially in the dimension of the con gurationspace

Moreover kinematic holonomic path planning is not enough formany problems of interest particularly problems involving ldquoagilerdquoautonomous vehicles for which we have to take into account theadditional constraints on the vehiclersquos motion arising from its dy-namics or from nonholonomic constraints (that is nonintegrableconstraints on the state and its derivatives)

Even though in some cases it is possible to force a dynamicalsystem to follow trajectories generated by a kinematic model thisis not true in general especially when constraints on the availablecontrol inputs are taken into account

For example differentially at systems17 can follow any arbi-trary trajectory for the so-called at outputs18 However no generalmethod is available yet to ascertain differential atness of a givensystemMoreovereven fordifferentially at systemscurrentlythereis no straightforward way of taking into account control saturationor ight envelope constraints (some advances in this direction ap-peared recently1920 )

As another example some nonholonomicsystems are able to ap-proximate arbitrary paths but usually this requires the executionofcomplex sequencesof small-amplitude movements about the refer-ence path which could possibly result in a large loss in terms ofperformance of the nal trajectory321iexcl23

If the output of a kinematic planner is used as a reference tra-jectory for an inner loop consisting of a tracking control law thediscrepanciesbetween the planned trajectory and the trajectory ac-tually executed by the system can be relevant and lead to collisionwith obstacle even in the case in which the planned trajectory wascollision freeThis is true for example in cases in which the charac-teristic dimensionsof the environment(eg the size of the obstaclesand of the gaps between them) and of the vehiclersquos dynamics (egthe turning radius at some nominal velocity) are comparable

As a consequence it is desirable that motion-planningstrategiestake fully into account the dynamicsof the vehicle In otherwords itis desirable that the output from the motion planning be executableby the system dynamics this is the object of a recent direction inmotionplanningresearchusuallyreferredto as kinodynamicmotionplanning

Clearly such a problemis at least as dif cult as the path-planningproblem Moreover constraints deriving from the systemrsquos dynam-ics or from nonholonomicconstraintscannotbe representedas ldquofor-bidden zonesrdquo in the state space (ie the space encoding the con g-uration of the vehicle as well as its velocities) As a consequencethe direct application of kinematic motion-planning techniques tothe dynamic case is not possible in the general case

Perhaps the best formulated general method for addressingmotion-planning problems is the use of optimal control2425 How-ever the solution of such a problem using the traditional optimalcontrol tools (such as variational calculus the minimum princi-ple of Pontryagin and other necessary conditions26 and dynamicprogramming27 ) is computationally intractable for all but trivialcases In addition some techniques from kinematic motion plan-ning can be also extended to the dynamic case by planning motionsin the state space instead of the con guration space

However there is strongevidencethat any deterministicand com-plete algorithm for the solution of kinodynamic motion-planningproblems will require at least exponential time in the dimensionof the state space of the dynamical system which is usually atleast twice the dimension of the underlying con guration spaceand polynomialin the numberof obstaclesAs a consequenceavail-

able algorithmsare implementable in practice at least at the currenttechnology levels only for systems of very small dimension (egless than ve) Because the state space of aerospace vehicles is atleast 12 (when vehicles are modeled as rigid bodies) one has toresort to heuristic techniquesor seek alternative formulationsof theproblem

To circumvent the computational complexity of deterministiccomplete algorithms a new class of motion-planning algorithmsknown as probabilistic roadmap (PRM) planners was introducedby Kavraki et al28 and Overmars and Svestka29 and subsequentlyre ned430iexcl33 The PRM path-planning architecture was rst intro-ducedas a fast and ef cient algorithmfor geometricmultiple-querypath planning The original PRM planner is based on an off-linepreprocessing phase and an on-line query phase The preprocess-ing phase is aimed at constructing a graph of feasible paths in theentire con guration space (the roadmap) which would make futurequerieseasy to solveThe on-linequery phase selectsan appropriatepath from the already computed roadmap together with the com-putation of two ldquoshortrdquo paths to connect starting and ending pointsto the closest nodes (or milestones) of the roadmap The PRM algo-rithm has beenproven to be complete in a probabilisticsense that isthe probabilityof correct termination approaches unity as the num-ber of milestones increases Moreover performance bounds havebeen derived as a function of certain characteristicsof the environ-ment (ie its expansivenesstied to the rate at which the set of pointsthat can be connected to the roadmap grows with the number ofmilestones) which prove that the probabilityof correct terminationapproaches one exponentially fast in the number of milestones33

However for many path-planning applications building aroadmap a priori may not be required or even feasible (eg forplanning in a dynamic rapidly changing environment) In additionthe basic roadmap algorithms neglect the vehicle dynamics the ve-hicle usually needs to navigate along a piecewise linear trajectorywith very sharp turning points To address both points a new ran-domized motion-planningalgorithm was developed by introducingthe conceptof rapidlyexploringrandomtrees (RRTs)34iexcl36 The RRTalgorithm consists of building on-line a tree of feasible trajectoriesby extending branches toward randomly generated target pointsAlthough in the PRM approach the idea was to explore the con g-uration space exhaustively in the preprocessing phase RRTs tendto achieve fast and ef cient single-queryplanning by exploring theenvironment as little as possible A signi cant feature of the RRTsis that the resulting trajectories are by de nition executable by theunderlyingdynamicalsystem In additionunderappropriatetechni-cal conditions the RRT algorithmhas been proven probabilisticallycomplete36 that is the probability of nding a path from origin todestination converges to one if such a feasible path exists

In Hsu et al37 a new incremental roadmap building algorithm isintroduced that provides not only probabilistic completeness butalso recovers performanceguaranteeson the algorithm That is theprobabilityof thealgorithm ndinga solutionif oneexists convergesto oneexponentiallyfastwith thenumberof randomsamplesused tobuild the tree Interestinglythe rate of convergencedoes not dependon the number of obstaclesbut ratheron geometric propertiesof theenvironment (eg its expansiveness) One dif culty pointed out bythe authors however lies with the fact that algorithm performancereliesuponuniformsamplingofmilestonesin the reachablespaceofthe tree Practical implementations (which have reportedly demon-strated excellent performance37 ) rely upon uniform sampling of thesystemrsquos control inputs insteadwhich in general does not guaranteeuniform sampling of the workspace

Motivated by these recent developments a new randomized in-crementalmotion-planningalgorithmis proposedin this paperThisincremental roadmap building algorithm is able to effectively dealwith the systemrsquos dynamics in an environment characterized bymoving obstacles Central to this algorithm is the assumption thatan obstacle-freeguidance loop is availablewhich is able to steer thesystem from any state (including con guration and velocity) to anydesired con guration at rest assuming that there are no obstaclesin the environment This guidance loop enables uniform samplingof the workspace while generating trajectories that are executableby the dynamical system As a consequence it is shown that this










dx

dtD f x u (1)


F x middot 0 (2)






Pz D y Py D f y u




























t0

deg [xt ut] dt (3)








Gx t middot 0 (4)






RS D[

x t 2 S

Rx t












Rfrac14 S D[

x t 2 S

Rfrac14 x t






































































































sup1poundRiquest


cedil sup2



macr-lookoutS Dcopy

p 2 Sshyshysup1

iexclRiquest

frac14 pdeg

Scent
















































10 Draquo





t1 D t2 iexcl C=U

t2 D logpound1 C


curren(8)






min








A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055











A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588





A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242






































































































dx

dtD f x u (1)


F x middot 0 (2)






Pz D y Py D f y u




























t0

deg [xt ut] dt (3)








Gx t middot 0 (4)






RS D[

x t 2 S

Rx t












Rfrac14 S D[

x t 2 S

Rfrac14 x t






































































































sup1poundRiquest


cedil sup2



macr-lookoutS Dcopy

p 2 Sshyshysup1

iexclRiquest

frac14 pdeg

Scent
















































10 Draquo





t1 D t2 iexcl C=U

t2 D logpound1 C


curren(8)






min








A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055











A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588





A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242











































































































t0

deg [xt ut] dt (3)








Gx t middot 0 (4)






RS D[

x t 2 S

Rx t












Rfrac14 S D[

x t 2 S

Rfrac14 x t






































































































sup1poundRiquest


cedil sup2



macr-lookoutS Dcopy

p 2 Sshyshysup1

iexclRiquest

frac14 pdeg

Scent
















































10 Draquo





t1 D t2 iexcl C=U

t2 D logpound1 C


curren(8)






min








A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055











A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588





A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242
































































































RS D[

x t 2 S

Rx t












Rfrac14 S D[

x t 2 S

Rfrac14 x t






































































































sup1poundRiquest


cedil sup2



macr-lookoutS Dcopy

p 2 Sshyshysup1

iexclRiquest

frac14 pdeg

Scent
















































10 Draquo





t1 D t2 iexcl C=U

t2 D logpound1 C


curren(8)






min








A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055











A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588





A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242





























































































































































































sup1poundRiquest


cedil sup2



macr-lookoutS Dcopy

p 2 Sshyshysup1

iexclRiquest

frac14 pdeg

Scent
















































10 Draquo





t1 D t2 iexcl C=U

t2 D logpound1 C


curren(8)






min








A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055











A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588





A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242

































































































































10 Draquo





t1 D t2 iexcl C=U

t2 D logpound1 C


curren(8)






min








A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055











A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588





A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242

































































































min








A 1768 1010 1152 029B 1634 793 1203 068C 1812 1001 1155 031D 173 857 1186 055











A 4267 2725 1495 647B 1453 2568 1878 596C 312 1316 1387 554D 129 843 1735 588





A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242

































































































A 0365 0390 1644 230B 0117 0060 1836 359C 0181 0124 1705 258D 0183 0132 1726 242





























































































Real-Time Motion Planning for Agile Autonomous Vehiclesdahleh/pubs/2.Real-Time Motion Planning for...

Documents

Transcript of Real-Time Motion Planning for Agile Autonomous Vehiclesdahleh/pubs/2.Real-Time Motion Planning for...