State Space Sampling of Feasible Motions for High ......State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments Thomas M. Howard,

• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • •

State Space Sampling ofFeasible Motions for

High-Performance MobileRobot Navigation in Complex

Environments

Thomas M. Howard, Colin J. Green,and Alonzo KellyRobotics InstituteCarnegie Mellon University5000 Forbes AvenuePittsburgh, Pennsylvania 15213e-mail: [email protected],[email protected], [email protected]

Dave FergusonIntel Research Pittsburgh4720 Forbes AvenuePittsburgh, Pennsylvania 15213e-mail: [email protected]

Received 21 December 2007; accepted 3 April 2008

Sampling in the space of controls or actions is a well-established method for ensuring fea-sible local motion plans. However, as mobile robots advance in performance and compe-tence in complex environments, this classical motion-planning technique ceases to be ef-fective. When environmental constraints severely limit the space of acceptable motions orwhen global motion planning expresses strong preferences, a state space sampling strat-egy is more effective. Although this has been evident for some time, the practical ques-tion is how to achieve it while also satisfying the severe constraints of vehicle dynamicfeasibility. The paper presents an effective algorithm for state space sampling utilizing amodel-based trajectory generation approach. This method enables high-speed navigationin highly constrained and/or partially known environments such as trails, roadways, anddense off-road obstacle fields. C© 2008 Wiley Periodicals, Inc.

1. INTRODUCTION

Outdoor mobile robot navigation is a challengingproblem because environments are often complexand only partially known, dynamics can be difficultto predict accurately, and both planning time andcomputational resources are limited. We can gener-ally model the dynamics of a vehicle by a nonlinear

differential equation of the form

·x = f(x, u). (1)

The input or control vector u and the state vector xare both time-varying points in input and state space,respectively. The complexity of such accurate mod-els of mobility combined with the scale of outdoor

Journal of Field Robotics 25(6–7), 325–345 (2008) C© 2008 Wiley Periodicals, Inc.Published online in Wiley InterScience (www.interscience.wiley.com). • DOI: 10.1002/rob.20244

326 • Journal of Field Robotics—2008

Figure 1. Hierarchical motion planning. The planner uses a search space that accounts for vehicle mobility constraints inthe area close to the vehicle, and it uses a grid that ignores vehicle mobility constraints in the area far from the vehicle.

mobile robot navigation leads to a difficult trade-offbetween the computational demands of perceptiveintelligence at the local level and deliberative intelli-gence at the global level. It is difficult to be both smartand fast when computation is limited.

A common approach to this problem is to em-ploy a hierarchical motion planning architecture togenerate behaviors that are both intelligent and re-sponsive. Figure 1 illustrates a case in which the en-vironment is a continuous cost field. Cases based ona more topological environment representation, suchas a road network, are shown in Figure 2.

The architecture is hierarchical in the sense thattwo levels of detail are used for the modeling ofboth the vehicle and the environment. The higher-level motion planner (global planner) is responsiblefor directing the vehicle to achieve mission goals. Itproduces a large-scale, long-term motion plan based

Figure 2. Search spaces generated by sampling in control space vs. state space are shown in environments that are highlyconstrained (e.g., road networks). The majority of the options generated by sampling in control space leave the lane or areoriented to do so shortly, whereas those generated by sampling in state space remain within the lane.

on simplified vehicle models and coarse represen-tations of the environment. Conversely, the lower-level motion planner (local planner) is used to keepthe vehicle safe. It generates a finer-scale, short-term motion plan based on higher-fidelity vehiclemodels and finer-resolution representations of theenvironment.

Whereas it is a common approach to use higher-fidelity models to subsequently smooth the trajecto-ries produced by lower-fidelity planners, the result-ing smooth path may be reduced in optimality andit may no longer avoid obstacles. For a continuouslymoving vehicle, such a planner failure leads at bestto the inefficiencies associated with stopping and atworst to a damaging collision. Such considerationslead to a desire for the smoothing algorithm to in-terpret the environmental model in its computationsand hence become a planner in its own right. Once

Journal of Field Robotics DOI 10.1002/rob

Howard et al.: Mobile Robot Navigation in Complex Environments • 327

the smoother becomes a planner, any short-term por-tion of the path produced by the global planner be-comes somewhat redundant because it will always bereplaced by the results of this planner, which is morecompetent on the local scale.

The models used by each of these planners arealso typically associated with mutually exclusive spa-tial regions. Local planner models are used in a smallregion centered around the vehicle that extends out-ward as far as useful perception data are available.Global planner models are used outside the bound-ary of the local region. The local region moves withthe vehicle so that decisions are based on models thatare, at least locally, as accurate as computation andsensing will allow.

1.1. Motivation

The capacity of a vehicle to move in the environmentdepends on both the characteristics of the vehicle andthe environment. Typical vehicle mobility constraintsinclude, for example, a restriction that wheels rollwith no slip or with specified or terrain-dependentslip profiles. Others include the dynamics of the loco-motion and steering actuators and of the propulsionsystem. Motions that satisfy such constraints are saidto be “feasible.” Environmental constraints may in-clude terrain geometric or soil characteristics thatmodulate tractive forces, body-contacting obstaclesthat actively oppose motion, or contextual informa-tion such as the requirement that the vehicle remainin its assigned lane of the road most of the time.

In the context of the architecture described previ-ously, the opportunity for a degree of optimizationarises regularly when many candidate paths in thelocal region are obstacle free. For any locally gener-ated path, the characteristics of a path to the goal thatstarts where the local solution ends can be used to de-fine the best local solution and select it for execution.In this sense, the global planner can serve as guidanceto the selection process.

Furthermore, as we will show, different sets of lo-cal paths comprising the local search space may befundamentally better than others in terms of the qual-ity of the solutions they produce. This paper dealswith the problem of improving the quality of localmotion-planning search spaces that satisfy feasibil-ity and environmental constraints while attemptingto maximally exploit global guidance information.

This paper also addresses a difficult issue thatarises in the context of hierarchical planning architec-

tures and in differentially constrained motion plan-ning in general. The formation of the local planningproblem involves constraints and utilities that aremost conveniently expressed in two spaces:

• State space: Those arising from the environ-ment

• Control space: Those arising from vehicle mo-bility

Environmental constraints are easily satisfied whenplanned trajectories are expressed in state space, buttheir dynamic feasibility cannot be easily guaranteed.Conversely, when trajectories are expressed in con-trol (input) space, feasibility constraints can be triv-ially satisfied but the satisfaction of environmentalconstraints cannot be easily guaranteed. When mostfeasible motions are likely to satisfy environmentalconstraints, control space sampling is an effective ap-proach. However, this traditional approach suffers ifthe environment imposes significant limits on accept-able motions (Figure 2).

A second consideration is path-sampling effi-ciency. All approaches to motion planning funda-mentally select one trajectory from a set of alterna-tives. The problem is the design of a search spacethat will best utilize the computational resources inthe time available. Separation of trajectories mattersbecause nearly identical trajectories will likely inter-sect the same obstacles and are therefore inefficient.We define a well-separated set of trajectories to beone that covers a majority of the reachable state spacewith a minimum of overlap.

For example, consider a control space samplingconsisting of a set of uniformly sampled constantcurvature arcs (Figure 3) applied to different initialvehicle states. The trajectories are denser in the direc-tion of the initial angular velocity because the vehi-cle’s limited maximum turning rate (dω/dt) leads tovery similar outputs for several distinct inputs. Con-versely, sampling in state space would permit directcontrol over the spacing of the endpoints of thesetrajectories.

1.2. Related Work

There has been substantial research in the genera-tion of expressive and complete trajectory sets for lo-cal motion-planning search spaces. Some early workin this appears in Kelly and Stentz (1998), wherethe local motion-planning search space is generated



Figure 3. Irregular mapping from control space to state space. Accurate dynamic simulations of a set of uniformly sampledconstant curvature arcs (control space sampling) are shown for a variety of different initial vehicle states. Notice that theresponses are not uniformly separated despite the uniform separation of controls.

by sampling in the control space of curvature. Eachcontrol is passed through a vehicle dynamics modelto estimate the response of the vehicle to the con-trol. The shape of the response is highly dependenton the vehicle model and the initial vehicle state(curvatures and velocities). Similar approaches havebeen adapted in a variety of other unmanned groundvehicles (Kelly et al., 2006; Wettergreen, Tompkins,Urmson, Wagner, & Whittaker, 2005). Egographs(Lacze, Moscovitz, DeClaris, & Murphy, 1998) rep-resent another method for generating local motion-planning search spaces. This approach generates,offline, a well-separated dynamically feasible searchspace for a limited set of initial states. Online adap-tation to changes in terrain or vehicle models arenot considered. Precomputed arcs and point turnscomprised the control primitive sets that were usedto autonomously drive Spirit and Opportunity dur-ing the Mars Exploration Rover mission (Besiadecki,Leger, & Maimone, 2007). Trajectory selection wasbased on a convolution on a cost or “goodness”map. This approach was an extension of Morphin, anarc-planner variant in which terrain shape was con-sidered in the trajectory selection process (Simmonset al., 1995; Singh et al., 2000). Another closely relatedalgorithm is the one presented in Bonnafous, Lacroix,and Simeon (2001), where an arc-based search spaceis evaluated based on considering risk and interest.

Rapidly exploring random trees (RRTs) haverecently been applied to the problem of generatingdynamically feasible controls through complex en-vironments. This method is well suited to the prob-lem of navigating complex environments because ofits ability to search high-dimensional input spacesand can consider vehicle dynamics and terrain shape

in its solution (Berg, Ferguson, & Kuffner, 2006;Melchior, Kwak, & Simmons, 2007). Frazzoli, Dahleh,and Feron (2001) use RRTs to navigate among staticand dynamic obstacles, where tree expansions aredone in state space rather than input space. A con-troller is applied to determine the feasible action, ifone exists, that connects the current state to the newbranch state. Chen and Fraichard (2007) applied par-tial motion planning, an iterative technique based onRRT that considers vehicle model constraints suchas acceleration, steering velocity, and steering anglebounds and the real-time operation constraint of thesystem for navigation in urban environments.

Potential fields (Haddad, Khatib, Lacroix, &Chatila, 1998; Koren & Borenstein, 1991) have reg-ularly been applied to obstacle avoidance and nav-igation, where attractive forces represent goals andrepulsive forces represent obstacles. In Shimoda,Kuroda, and Iagnemma (2005), potential fields gener-ate actions that considered dynamic hazards such asrollover and terrain shape in their evaluation. Lacroixet al. (2002) applied potential fields for navigation insimple terrain. For more difficult environments, theyapplied arcs and arc-trees that consider terrain shapein their forward simulation to generate the naviga-tion search space.

Another important group of work involves nav-igators that solve for an optimal or near-optimal tra-jectory in the continuum. These methods typically arefast and solve for the local, but not global, optimalsolution. A navigator based on optimizing a utilityfunction in the continuum is described in Cremeanet al. (2006). Horizon-limited trajectories are gener-ated by minimizing the steering and acceleration con-trol effort subject to a vehicle dynamics model that



considers limited steering rates and rollover con-straints. Obstacles are avoided by representing themas very low-speed regions and are naturally avoidedin this framework. The local minima problem is han-dled by seeding the optimization function with acoarse spatial path. The dynamic window approach(Fox, Burgand, & Thrun, 1997) selects translationaland rotational velocities by maximizing an objectivefunction based on the heading to the target posi-tion, distance to the closest obstacle, and velocity ofthe robot. The search space is generated using arcsand is restricted to reachable and safe velocities at itscurrent state.

The satisfaction of environmental constraints inlocal motion-planning search spaces is related to lanefollowing, a problem that has generated a great dealof interest with the recent DARPA Grand Challengecompetitions. In Miller et al. (2006), Bezier splinesare constructed with terminal points defined by alateral offset and the road shape. Dynamically infea-sible trajectories are culled through a postprocess-ing technique, and an optimal navigation strategy isdetermined through minimizing a weighted combi-nation of integrated path cost and steering effort. Arelated approach involves Bezier splines whose con-trol points are located and adjusted by the locationof obstacles (Berglund, Jonsson, & Soderkvist, 2003;Trepagnier, Nagel, Kinney, Koutsougeras, & Dooner,2006). In Urmson et al. (2006), a geometric plannerthat conforms environmental constraints (a trail orroad network) is applied to the problem of navigat-ing in desert environments. Dynamic feasibility ispassed to a path tracker, and a speed planner en-sures safety by slowing the vehicle during aggres-sive maneuvers. Another similar system is describedin Leedy et al (2006), where the deliberative plan-ner generates a path by searching a cost map us-ing A* and following the trajectory using pure pur-suit (Coulter, 1992) and then checking it for safety.This method trades off inherent dynamic feasibilityfor global guidance. A reactive approach is also pre-sented in Leedy et al. (2006) that uses a fuzzy logiccontroller to follow roads and avoid local obstacles.Nudges and swerves (Thrun et al., 2006), represent-ing smooth and aggressive lateral offsets around abase trajectory, have proven effective for generatingexpressive local motion-planning search spaces forobstacle avoidance and lane following. Search in tra-jectory space (κ ,v) bounded by hazards and dynamiceffects including steering limits, wheel–terrain inter-action, rollover, and sideslip constraints is presented

in Spenko, Kuroda, Dubowsky, and Iagnemma (2006)and demonstrated to be an effective approach atspeeds up to 9 m/s.

This research is related to another body of workconcerning the need for expressive motion sets. Mo-tion planning has been concerned with the expres-siveness of sequences of motion primitives sinceDubins (1957), and work continues in this area today(Frazzoli, Dahleh, & Feron 2003). The importance ofseparation in a local planning search space has beendiscussed in Green and Kelly (2007), where it wasshown that the mutual separation of a set of paths isrelated to the relative completeness of the motion set.

This paper differentiates itself from the priorart through applying a state-based sampling strat-egy to generate path sets that consider global guid-ance and satisfy environmental constraints while alsoguaranteeing dynamic feasibility through the use ofa model-predictive trajectory generator. Controllingthe state-based sampling increases the expressivenessand completeness of the search space. The model-predictive trajectory generator guarantees dynamicfeasibility of the actions (to the fidelity of the mo-tion model that describes the dynamics) and requiresno postprocessing of the trajectory set. The explicitrequirement that trajectories terminate at a regularand predefined horizon provides a better basis fora trajectory arbiter function to select optimal controlinputs.

1.3. Technical Approach

An effective local planning search space would ide-ally be optimal, efficient, and robust. The search spacewould be optimal if it could maximally exploit globalguidance, efficient if it could control path separa-tion, and robust if it searched only feasible motions.Recent advances in real-time, model-based trajec-tory generation have provided the capability to makeprogress toward achieving these goals. In Howardand Kelly (2007) a general method is presented thatcomputes control inputs that satisfy a pair of bound-ary states subject to the vehicle dynamics model, andin Howard, Knepper, and Kelly (2006) we applied itto the problem of path following in the absence ofobstacles.

This paper improves on Howard et al. (2006)by generating a set of feasible actions derived bysampling in the surrounding state space. Motionsare generated to alternative terminal states based onglobal guidance—in the form of a global planner or



knowledge of road lane configuration. The techniqueis superior to control space sampling in its efficiency(mean separation of trajectories) and its satisfactionof environmental constraints by construction.

The approach in this paper differs from that ofall of the prior work cited in its capacity to gener-ate expressive local motion-planning search spacesusing a state space sampling approach while enforc-ing arbitrary dynamic constraints. Section 2 describesthe model-predictive trajectory generation techniqueused to generate dynamically feasible paths betweenarbitrary boundary state constraints. Section 3 de-scribes ideas related to the expressiveness and com-pleteness of search spaces. In Section 3, the argu-ments for why expressiveness and completeness areimportant in search space generation are reviewed.Section 4 describes the methodology for generatingadaptive search spaces that sample in state spacesubject to vehicle dynamic and environmental con-straints, and Section 5 discusses some of the practi-cal considerations for how such a system would bedeployed on a real field robot. Simulation and fieldresults for two mobile robot platforms are presentedand discussed in Section 6.

2. MODEL-PREDICTIVE TRAJECTORYGENERATION

Our state-based sampling approach requires amethod for generating the action between specifiedpairs of vehicle states. This was achieved by applyingthe real-time model-predictive trajectory generatorfrom Howard and Kelly (2007) that numerically lin-earizes and inverts simulated models of vehicle mo-tion. The continuum optimization method efficientlymodifies parameterized control inputs to minimizeboundary state constraint error.

Explicitly requiring that the actions in our searchspace satisfy kinodynamic constraints of the vehicleis important for two reasons. First, it allows the mo-tion planner to reason about the feasibility and truecost of actions in the search space. Sampling in thespace of controls ensures feasibility of the predictedmotion. In a state-based sampling search space gen-eration approach, an algorithm (such as the model-predictive trajectory generator) is needed to invertthe vehicle dynamics to provide the input that con-nects the states. The navigation function (the methodby which the local motion planner selects the best tra-jectory from the set of candidate actions) must havethe best information possible to operate effectively.

Paths that violate the kinodynamic constraints maynot represent the actual vehicle path and yet can in-fluence the navigation function, resulting in subop-timal behaviors. The second reason for embeddinga sophisticated model of dynamics in the trajectorygenerator is that the following error of such paths canbe minimized. Predicting some disturbances well forfeedforward purposes reduces the difficulty of pathfollowing.

The model-predictive trajectory generator can beused to solve for parameterized controls specified byarbitrary boundary state constraints (positions, head-ings, curvatures, rates of curvature, velocities, etc.).In this paper we will consider boundary state con-straints only on the position x, y and heading ψ atthe terminal state xF. Curvature continuity is not con-sidered in this example because the vehicle will likelynever execute the entire selected motion between re-planning cycles and because the global motion plan-ner places no constraints on curvature. In contrast tothe terminal state, the full initial vehicle state xI is nec-essary to initialize the vehicle dynamics model usedby the motion simulation.

Other considerations for the algorithm includethe controls parameterization and the vehicle dy-namics model. The number of freedoms in the pa-rameterized controls is typically a function of thenumber of state constraints, how well the actionrepresents all feasible motions over that distance, andwhether it is desired to optimize something overthe path. For skid-steered or Ackerman-steered mo-bile robots required to satisfy position and head-ing state constraints, second-order curvature splines(a spline function with three knot points) and sim-ple linear velocity functions (constant, linear, trape-zoidal, etc.) have proven to be effective. The secondconsideration, the motion model, is itself a trade-offbetween speed (dynamics are computed tens of thou-sands of times per second) and fidelity (how well itcan accurately predict the response to the inputs). Wehave found first-order models of linear and angularvelocity response, along with a rigid body motionsimulation, to be an effective trade-off that encodesthe major constraints of motion feasibility.

For example, consider the trajectory planningproblem exhibited in Figure 4. A simulated vehicleis attempting to reach a target terminal state on top ofa hill by executing an action with no path error servoto compensate reactively for path-following error. Ini-tially, a trajectory that does not consider the terrainshape and model dynamics is generated. Using a



Figure 4. Model-predictive trajectory generation. Two trajectories are generated and executed with and without predictivemodels of motion that consider terrain shape and simple model dynamics. By incorporating a predictive motion model thatunderstands kinodynamic constraints and wheel–terrain interaction into the trajectory generator, an action that compen-sates for these effects can be found.

more sophisticated motion model in the simulation,we see that the action terminates at a point that is notthe target terminal state. By folding a more accuratemodel of motion into the vehicle model used by themodel-predictive trajectory generator, an action canbe generated that reaches the target terminal state.

Typically, feedback control is applied to com-pensate for discrepancies in the motion model, butin general it cannot be assumed that the gener-ated trajectory is feasible or easily followable in thefirst place. By applying a method that integrates amore sophisticated vehicle model at the trajectory-planning stage, we can reduce the amount of errorthat a feedback controller must compensate whilehaving a more accurate prediction of cost for candi-date action. If the forward predictive model is in factgood enough, then we can execute controls directlyfrom the model-predictive trajectory generator.

3. SEARCH SPACE SEPARATION

In Green and Kelly (2007) it was shown that one im-portant characteristic of a search space is how well

Figure 5. Regions that may contain obstacles: (a) two separated differential regions, (b) two overlapping differential re-gions, and (c) two finite regions (swept volumes) that overlap.

separated its encoded paths are. A portion of that dis-cussion follows. Given a limited amount of compu-tation time, the number of paths that form a searchspace is necessarily limited and should be selected tomaximize the probability of finding a solution whenone exists, which we define as relative completeness.Additionally, the incremental benefit of finding mul-tiple solutions is small compared to the penalty forfailing to find any solutions. So we would like to max-imize the probability of finding at least one solution.

Consider next two nonempty differential regionsdR1 and dR2. If the two regions do not overlap[Figure 5(a)] and obstacles are considered to havezero width, then the probability that dR1 intersectsan obstacle is independent of the probability that dR2intersects an obstacle. But if the two regions do over-lap [Figure 5(b)] then the probability that both regionsintersect an obstacle increases with the area of theoverlap. Additionally, if obstacles have some size as-sociated with them, then increasing the separation oftwo regions that do not overlap decreases the prob-ability that one individual obstacle could intersectboth regions at the same time.



For two finite regions R1 and R2 that could repre-sent the swept volume of a vehicle following a path[Figure 5(c)], the correlation depends on the influenceof every element of R2 on every element of R1. Forsuch sets we approximate the effect of the separationof each element by relating the probability that twopaths will intersect the same obstacle to the area be-tween the two paths.

Based on the above argument, it would be a poorchoice if two paths in a search space were unnecessar-ily close to each other because if one were in collision,the other would also be highly likely to be in collision.Furthermore, if one were not in collision, the otherwould likely be a redundant solution whose presenceis probabilistically of little value from the perspectiveof completeness—because only one safe path is re-quired. So a search space in which the encoded pathsare close to each other is probabilistically less likelyto solve a planning query, and has a lower relativecompleteness, compared to a search space with bet-ter separated paths.

4. ADAPTIVE SEARCH SPACES

The algorithm in Howard and Kelly (2007) createsan opportunity to produce controlled distributionsof sampled terminal states, in a local search space,that satisfy environmental and separation constraintswhile also producing feasible trajectories that ad-here to a nontrivial dynamics model. Section 4.1 out-lines our general approach to structuring the searchspace adaptation algorithm, and Sections 4.2 and 4.3discuss and provide examples for exploiting globalguidance information and satisfying environmentalconstraints, respectively.

4.1. Adaptive Search Space Set Design

A search space for any motion-planning problemcan be considered to be simply a set of candidatepaths through state (or less generally, configuration)space. The paths may be specified implicitly or ex-plicitly in any sufficiently expressive system of coor-dinates. One form of state space sampling techniquegenerates a set of actions (encoding paths indirectly)by solving for trajectories between n boundary statepairs xN. The first state in each pair is the initial orcurrent state of the vehicle xI, and the second state isthe target terminal state xF, which is to be reached atthe end of the trajectory:

xN =[

xI

xF

]=

[xI,0 xI,1 . . . xI,n

xF,0 xF,1 . . . xF,n

], (2)

where

x = [x y ψ κ v . . .

]T.

The problem becomes that of determining the properinput or control u(p, x) that satisfies both the bound-ary state constraint pair xI, xF and the system dynam-ics f(x, u):

uN(p, x) =

⎡⎢⎢⎢⎣

u0(p0, x)u1(p1, x)

...un(pn, x)

⎤⎥⎥⎥⎦ . (3)

Because the navigation function typically relies onthe convolution of the trajectory with a “goodness”or cost map and not simply the input itself, a costci is computed for each valid control that connects aboundary state pair in the set:

cN = [c0 c1 . . . cn

]T. (4)

Note that this definition is independent of themethod of generating the search space because alltrajectories start and end somewhere. The basic out-line of the adaptive search space generation methodis shown in Algorithm 1, which takes the initialstate of the vehicle, a model of the system dynam-ics, and a data structure containing information aboutthe desired shape of the search space. The first stepin the algorithm is to generate the boundary statepairs, which in this version is accomplished throughAlgorithm 2. Once the set of boundary state pairs isestablished, the method calls the GENERATETRAJEC-TORY() method to determine the control input andtrajectory that satisfies the pair of state boundary con-straints. In our implementation we utilize the model-predictive trajectory generation algorithm describedin Section 2 because of its ability to generate feasibleactions between boundary state pairs in real time sub-ject to arbitrary models of dynamics, although in gen-eral other techniques could be substituted. Our ap-proach is based on designing rules and parameters,such as the ones presented in Algorithms 2–4, thatdefine the shape of the boundary state constraint setbased on expressiveness and completeness of trajec-tory sets, global guidance, environmental constraints,and initial state information.

An array of search space shape parameters pss

defines the shape of the search space. These variablesare typically tuned based on the reachable searchspace of the vehicle, the desired density of the search



Algorithm 1 . GENERATESEARCHSPACE (xI,f(x, u(p, x)))

Input: pss (shape parameter vector), xI (initial state),f(x, u(p, x)) (vehicle motion model)

Output: control set (uN(pN, x)), cost set (cN)1 xN ← GENERATEUNIFORMBOUNDARYSTATES(pss,xI)2 for i ∈ n do3 ui(pi, x)← GENERATETRAJECTORY(xI,i ,xF,i ,f(x, ui(pi , x)))4 if ui(pi, x) exists then5 ci ← COMPUTETRAJECTORYCOST(xI,ui(pi, x))6 end7 end8 return uN(pN, x),cN

Algorithm 2 . GENERATEUNIFORMBOUNDARYSTATES (pss,xI)

Input: pss (shape parameters), xI (initial vehicle state)Output: boundary state pair set (xN)

1 for i = 0 to (np − 1) do2 for j = 0 to (nh − 1) do3 n ← i · nh + j

4 xI,n ← xI5 α ← αmin + (αmax − αmin) · i/(np − 1)6 xF,n ← xI + d · cos(α + ψI )7 yF,n ← yI + d · sin(α + ψI )8 ψF,n ← ψI + ψmin + (ψmax − ψmin) · j/(nh − 1) + α

9 end10 end11 return xN

space, and the number of boundary state constraintswe wish to satisfy. In the example shown in Figure 6,the parameter array contains seven constants: thenumber of samples in terminal state position andheading np,nh, the terminal position horizon d, theangular range of the terminal position sampling

Figure 6. Uniform terminal state sampling for adaptive search space generation with varied initial states. The ability tocontrol the shape of the search space for three initial states: (a) turning left, (b) straight, and (c) turning right.

αmin,αmax, and the angular range of the terminal head-ing angle offsets (ψmin,ψmax):

pss =

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣

np

nh

d

αminαmaxψminψmax

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦

. (5)

In the scenarios considered in Figure 6, search spacesare generated with the following shape parametervector values, with varying initial curvature values:

pss =

⎡⎢⎢⎢⎢⎣

np = 30nh = 3

d = 5.0 mαmin, αmax = ±45 degψmin, ψmax = ±45 deg

⎤⎥⎥⎥⎥⎦ . (6)

The result of this approach is a set of sophisticatedmaneuvers that adapt automatically to varied initialconditions. Furthermore, the trajectories span most ofthe feasible set while remaining roughly equidistantfrom each other at their terminal states. This resultcontrasts dramatically with the results of the controlspace space sampling techniques from Figure 3.

4.2. Utilization of Global Guidance

The global planner may produce a preferred path tofollow or a cost field or navigation function ϕ(x, y)encoding the optimal distance to the goal from everystate. Both cases are referred to here more genericallyas global guidance. On the assumption that deviation



from global guidance is less likely to lead efficientlyto the goal, a local motion-planning search space willimprove if it biases its search to be most consistentwith global guidance. We typically use a global plan-ner that continuously provides a navigation function(consisting of (an infeasible) path cost from any pointto the goal) to the local planner. Given such informa-tion, it is better on average to sample terminal statesat a higher density in lower-global-cost regions andat a lower density in higher-cost regions, as shown inFigure 7. Some samples are retained in higher-cost re-gions because the low-cost regions produced by theglobal planner may not reflect actual dynamic con-straints of the vehicle, or an accurate map of the sur-roundings, and the global planner may not be able toreact quickly enough to recently perceived obstacles.

The shape parameters for the navigationfunction–influenced search space include all of theparameters from the uniformly sampled searchspace with the addition of the number of navigationfunction samples ns :

pss =

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

ns

np

nh

d

αminαmaxψminψmax

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

. (7)

Algorithm 3 is applied to bias the search spacewith respect to global guidance in the example shown

Figure 7. Focused terminal state sampling for adaptive search space generation. The ability to exploit global guidance viastate space sampling generates local motion-planning search spaces that are denser in the direction of minimum global cost(and therefore more likely to reduce the cost to the goal). Examples (a) and (b) show the same setup as in Figure 6 butfocused in the direction of minimum global cost.

in Figure 7. It is similar to Algorithm 2 except thatthe α parameter is not sampled uniformly; it is biasedto sample more densely in regions where the naviga-tion function ϕ(x, y) is near a minimum. This is ac-complished by sampling the navigation function at afixed horizon uniformly and creating a distributionby taking the difference between the maximum andthe sampled values and dividing by the sum. Thenthe integral of this conditional distribution is sam-pled uniformly, generating a nonuniform samplingof the angular value of the terminal position α. Thisgenerates denser sampling in the low-cost regions ofthe navigation function, corresponding to a densersearch space in those regions. The number of samplestaken of the navigation function ns can be dynami-cally modified based on the complexity of the envi-ronment and how long each sample takes to compute.We use linear interpolation to approximate the navi-gation function values between sampled states. All ofthe shared shape parameters used in Figure 7 werethe same as in Figure 6 with ns = 100.

4.3. Satisfaction of Environmental Constraints

It is valuable for mobile robots operating in struc-tured environments such as road networks and for-est trails to consider environmental constraints inthe design of their local motion-planning searchspace. Figure 2 exhibited the effects of ignoring suchconstraints, leading to utterly ineffective planningbecause many actions lie outside of the accept-able navigation region. In situations in which



environmental constraints are important, it is desir-able to adjust the sampling strategies to be (at leastpartially) based on these constraints. For example,consider the search spaces shown in Figure 8. Thestate-based sampling strategy here is parameterizedon the road shape at some forward distance along thepath and sampled along the perpendicular of the cen-terline of the road. The alternative (noncenterline) ac-tions exhibited here are generated for obstacle avoid-ance and lane-switching capabilities.

Algorithm 3 . GENERATEGLOBALLYGUIDEDBOUNDARY

STATES (pss,xI,ϕ(x, y))

Input: pss (shape parameters), xI (initial vehicle state),ϕ(x, y) (global navigation function)

Output: boundary state pair set (xN)1 for i = 0 to (ns − 1) do2 αsi ← αmin + (αmax − αmin) · i/(ns − 1)3 cnavi ← ϕ(xI + d · cos(ψI + αsi ), yI + d · sin(ψI + αsi ))4 end5 cnavsum ← ∑n−1

i=0 cnavi

6 cnavmax ← max(cnav)7 for i = 1 to (ns − 1) do8 cnavi ← (cnavmax − cnavi )/(cnavmax · ns − cnavsum)9 end

Figure 8. State-based sampling applied to navigation in road networks, an example of exploitation and satisfaction ofenvironmental constraints. In each of these examples, a local motion-planning search space is generated that adapts to theshape of the road, inherently satisfying the environmental constraint of staying within or oriented along the lanes in thedirection of travel.

10 for i = 0 to (np − 1) do11 for j = 0 to (nh − 1) do12 n ← i · nh + j

13 xI,n ← xI14 α ← FINDINTERSECTION (

∫ ααmin

cnav, i/(np − 1))15 xF,n ← xI + d · cos(α + ψI )16 yF,n ← yI + d · sin(α + ψI )17 ψF,n ← ψI + ψmin + (ψmax − ψmin) · j/(nh − 1) + α


To generate the search spaces exhibited inFigure 8, we apply an algorithm similar to Algo-rithms 2 and 3 except that the sampling strategy isdefined by some environmental constraints, such ascenterlanes and road boundaries. As shown in Fig-ure 8, in the presence of multiple valid lanes, wecan easily generate single or full sets of trajecto-ries to evaluate the relative cost of a lane changemaneuver. The search space parameter array thatcorresponds to the provided example consists ofroad centerlines lcenter, lane headings lheading, lanewidths lwidth, vehicle width vwidth, lane horizons d, the



number of lateral offsets to generate for each lane np,and the number of lanes itself nl :

pss =

⎡⎢⎢⎢⎢⎢⎢⎢⎢⎣

lcenterlheadinglwidthvwidth

d

np

nl

⎤⎥⎥⎥⎥⎥⎥⎥⎥⎦

. (8)

Algorithm 4 generally operates by finding thestate at some distance d along each lane defined inour search space parameter array. In this particularexample, the terminal positions are sampled with auniform lateral offset δ from a lane centerline at afixed distance down the lane. Although it is not re-quired, all of the terminal headings are constrainedto be aligned with the tangent of the centerline.

Algorithm 4 . GENERATELANEGUIDEDBOUNDARY

STATES (pss,xI)

Input: pss (shape parameters), xI (initial vehicle state)Output: boundary state pair set (xN)

1 for i = 0 to (np − 1) do2 for j = 0 to (nl − 1) do3 xcenter ←COMPUTESTATEATDISTANCEALONGLANE

(lcenter,j, d)4 n ← i · nl + j

5 xI,n ← xI6 δ ← −0.5(lwidth,j − vwidth) + (lwidth − vwidth) ·i/

(np − 1)7 ψF,n ← lheading,j8 xF,n ← xcenter − δ sin(ψcenter)9 yF,n ← ycenter + δ cos(ψcenter)


This is important for two reasons. The first isthat because all trajectories terminate at an equal dis-tance along the path, the trajectory selection functionis easier to design because they match better with theglobal guidance heuristic. The penalty for deviatingfrom the centerline of the road shape is accountedfor when all paths terminate at an equal distancealong the road. Control-sampled search spaces thatconsider a constant distance for each trajectory donot have this feature, and this is why in part suchapproaches fail to navigate effectively in road net-works. The second reason is in the control of the ter-minal heading at points along the path. It is impor-

tant to consider whether an evasive action will in-evitably lead the vehicle out of the lane, which wouldbe unacceptable in a real-world application. The abil-ity to control terminal heading and curvature ensuresthat the vehicle will stay in the lane after an evasivemaneuver.

5. PRACTICAL CONSIDERATIONS

The ability to apply an arbitrary state-based samplingstrategy is powerful, but actual implementation re-quires solutions to a few additional practical prob-lems. This section will describe potential and appliedsolutions to several application challenges, includingsearch space envelope determination, state samplingstrategies, trajectory selection function, and modelidentification.

5.1. Adaptive Search Envelope Determinationin Unconstrained Environments

The envelope of the adaptive search space dependsheavily on the dynamic limitations of the mobilerobot and the initial conditions. The shape and po-sition of the search space horizon should adapt to thecurrent speed because of braking distance and obsta-cle avoidance limitations. Curvature and angular ac-celeration limits may decrease the range of terminalheadings.

An effective way to determine the envelopeof the reachable set is simply to exhaustively anddensely sample control space and record the extremesachieved in state space. Such a method would haveto be employed offline, and it could not be used inrough terrain or for vehicles whose models changeover time. Another approach is to exploit the con-tinuity of the dynamic model and evaluate severalaggressive maneuvers (such as max-turn right andleft) to form rough bounds on reachable positionsand headings at the horizon. This approach is sim-ple and fast, and it can be used for adaptive vehiclemodels (whose motion model can change over time),and so we have preferred it over the offline method.

5.2. Sampling the Search Envelope

Whereas it is important to have an accurate represen-tation of the envelope of the reachable set to constrainthe search in regions where trajectories are likely tobe feasible, another equally important considerationinvolves sampling of boundary states in this searchenvelope. Although it has already been mentioned



that it is beneficial to focus the terminal state sam-pling in the direction of global guidance, it is alsoimportant to generate a search space that is reason-ably well separated. Given the high number of free-doms still available in the definition of the boundarystate pairs (including initial state curvature and ve-locity commands, terminal state positions, headingsand curvatures, and velocities), it is important to gen-erate rules based on the expressiveness of trajectoriesthat adhere to the model dynamics to generate well-separated search spaces.

5.3. Trajectory Selection Function

In any approach in which an optimal trajectory mustbe determined from a local motion-planning searchspace, generating the correct optimization functionfor this procedure is important and often difficult. Itis important to generate a cost function that correctlyconsiders some mixture of risk, energy consumption,slope dwell (time spent on slopes), global guidance,smoothness, aggressiveness, and other “costs” thatcan be associated with a trajectory. Although mostcurrent approaches are hand tuned by robot opera-tors, a promising approach is to apply machine learn-ing techniques to discover the proper mapping fromaction and environment to cost, but this method stillis human or self-supervised and requires a wealth ofuseful feature inputs and representative data for thetraining process. We have used a similar approach ap-plied by other research projects by minimizing someweighted combination of integrated cost and steeringactivity.

5.4. Model Identification

It is important in the application of these model-predictive techniques to have an accurate trajectoryprediction, as the validity of the motion plans is di-rectly related to the fidelity of the forward vehiclemodel. If a high-fidelity vehicle model is available,we have the ability to directly execute commandsfrom the local planner without the need for a pathfollower. In applications in which no model is avail-able, the dynamics are too complex, or the model maydegrade over time, it may be necessary to apply somefeedback control to attempt to follow the selected tra-jectory. The use of predictive motion models in mo-tion planning and navigation simplifies the obstacleavoidance problem by considering only dynamicallyfeasible actions in its search space.

Methods applied for model identification on thetwo projects detailed in Section 6 consist of of-fline learning and parameterized model tuning fromprior data. Neural networks have proven to be ef-fective for offline learning of the forward predictivemodel (Bode, 2007), but it can be difficult to pro-vide comprehensive training data. We have imple-mented parameterized models that represent the un-derlying physics tuned for accuracy from prior datalogs, which has proven to be an effective, howeverinefficient, method. More sophisticated approachesinvolving online learning and real-time system iden-tification will inevitably lead to better motion predic-tion and higher-performance navigators for complexenvironments.

6. EXPERIMENTS AND EXPERIMENTAL RESULTS

To evaluate the performance of our approach,the adaptive search space algorithm presented inSection 4 was tested in a series of simulation and fieldexperiments. The simulation experiments are com-parisons, in a series of randomized worlds, of ourapproach against constant curvature control basedsearch spaces. The field results consist of two sepa-rate field robotic programs operating in complex anddistinct environments: the off-road DARPA UGCV-PerceptOR Integrated (UPI) project and the TartanRacing project that produced Carnegie Mellon’s win-ning entry to the DARPA Urban Grand Challenge in2007. The vehicles used on each of these programs areshown in Figure 9. Section 6.1 describes and detailsthe simulation experiments, and Section 6.2 demon-strates new capabilities for the two field roboticsprojects.

6.1. Simulation Experiments

This section describes the set of simulation exper-iments performed to assess the value of the pro-posed methods in a setting where experiments canbe controlled and a statistically significant number ofthem can be performed. Simulation is the most cost-effective way to judge the relative performance of theproposed algorithms because a sufficient quantity offield results would be prohibitively expensive to col-lect. Also, field experiments spanning long periodsof time may be too difficult to control because con-ditions such as weather, visibility, and vehicle healthwill not be sufficiently controllable.



Figure 9. The two mobile robot platforms used to perform the field experiments. (a) Crusher is a six-wheeled, skid-steeredfield robot for the DARPA UPI program. (b) Boss, a converted Chevrolet Tahoe, was the platform for Tartan Racing and wasCarnegie Mellon’s winning entry in the 2007 DARPA Urban Grand Challenge.

6.1.1. Simulation Setup

The experiment consisted of a vehicle drivingthrough a randomly generated world populated withrandomly sized circular obstacles, simulating a forestor boulder field environment. Obstacle positions andsizes are drawn from bounded uniform distributions.A region of points within a fixed radius from the ve-hicle represents the portion of the world known to thevehicle from its limited horizon perception system.This region is updated in each planning cycle withall obstacles. The vehicle model used for these simu-lations consists of a simple first-order (change in ve-locity is proportional to velocity error) approximationfor linear and angular velocity based on observationsfrom the Crusher platform. There is no uncertainty inthis simulation; both vehicle model and obstacle ob-servations are perfect.

During each planning cycle, the vehicle followsthe path that was determined to be obstacle free forthe greatest length. That selected path is followedfor a distance corresponding to how far the vehiclewould travel during the time required for planningand perception calculations. The global planner usedis Field D* (Ferguson & Stentz, 2006). In the case of atie, the path with the lowest Field D* distance to thegoal (based on the portion of the world that is knownat that point) is selected. In the simulation the goal is1 km away and the local search space path lengthsare 17 m. The method of evaluating a constant setof paths has been applied to the obstacle avoidanceproblem at least as far back as Daily et al. (1988). Aview of the two search spaces, the environment, anda comparison of two runs can be found in Figure 10.

For simplicity, the vehicle cannot adjust its speedduring a simulation. Instead, if the vehicle is in a po-sition where is cannot avoid an obstacle at its fixedspeed, then the vehicle heading is changed to pointalong the Field D* path to goal to simulate a stop-and-turn correction.

6.1.2. Simulation Results

One hundred simulated runs were performed foreach of the search spaces. For each pair of runs, onesimulated world was generated and both the con-stant curvature arc-based and adaptive search spaceswere tested on that world. The two search spaceshad equal numbers of trajectories. Simulations thatended without the vehicle reaching the goal werenot used in the final results. A representative runfrom the comprehensive set is shown in Figure 10(c).Note that there were no successful simulations forboth search spaces at 5 m/s for the higher obstacledensities, and so no data are provided in that region.On average, the overall path length was between25% and 60% shorter in high-obstacle-density worldsfor the adaptive search space as shown in Figure 11.This demonstrates that the improved flexibility andefficiency of the adaptive search space provides aperformance advantage over constant-curvaturesearch spaces in complex environments.

6.2. Field Experiments

As previously mentioned, the field experiments wereconducted on two mobile robot platforms designed



Figure 10. Examples of the arc-based (a) and adaptive search space (b) driving through a simulated random world with alimited perceptual horizon. (c) Plots of the resulting paths for each search space for one simulated world.

Figure 11. Simulation results. The percent improvement in path length provided by the adaptive search space comparedto simple constant curvature controls for various densities and vehicle speeds.



for operation in different environments. The first setof experiments was performed on Crusher, an au-tonomous six-wheeled, skid-steered field robot de-signed to operate in the most difficult outdoor en-vironments. The second set of field experiments wasperformed on Boss, a converted Chevrolet Tahoe de-signed for autonomous operation in urban environ-ments. The Crusher experiments are a series of com-parative tests to an arc-based search space, and theBoss experiments exhibit new capabilities gained anddifficult problems made easy by applying this ap-proach to highly constrained environments consider-ing dynamic obstacles.

Boss and Crusher operated with similar naviga-tion architectures. A global motion planner (Field D*for Crusher, a lane-based planner for Boss) providedglobal guidance to a local motion planner based onthe presented adaptive search space algorithm tonavigate complex environments. A testament to thevalue of model-predictive motion planners was thatneither system relied on a path tracker; the controlsgenerated by the model-predictive trajectory gener-ator were accurate enough when executed withoutpath-relative error feedback to ensure safe and effec-tive navigation.

6.2.1. UPI/Crusher

Expressiveness of maneuvers is important for com-petent operation of mobile robots when obstacles aredense, speeds are high, or stopping and reorient-ing of the robot is undesirable or unacceptable. Theautonomy system on Crusher has proven the capa-bility to traverse more than 30 km/day under suchcircumstances with an average distance of more than10 km between human interventions.

This adaptive search space has been used exten-sively on the Crusher platform during UPI field tests.For these field tests the adaptive search space wasinserted into a full planning system. This system in-cludes Field D* to provide global guidance and a se-ries of reactive behaviors that are called to help thevehicle navigate through difficult situations that in-volve backing up. The adaptive search operates in themain behavior, which is responsible for forward navi-gation when there are no error conditions. These newtrajectories replace a set of constant curvature con-trols that were used in that behavior for many years.Online, trajectories are selected to minimize the sumof the convolved cost along the trajectory, the FieldD* distance to goal from the end of the trajectory, and

a set of costs that penalize large curvature commandsand deviation from the previously planned path.

It would be difficult and expensive to performstatistically significant tests of our hypothesis on thismobile robot. Routine software upgrades are per-formed several times per week in order to enhancethe performance of the system in DARPA controlledfield tests, and a freeze on all other development ac-tivities for our purposes for the times required is sim-ply not feasible. Nonetheless, a small number of com-parisons were completed. Two runs were completedon a short, 2-km course during which the only pa-rameter changed was the set of trajectories used inthe forward navigation behavior. The path traversedby the vehicle using each trajectory set is shown inFigure 12(b). As shown in Figure 12(a), the adaptivesearch space achieved a lower path cost early in theruns and maintained that advantage throughout thecourse. This set of runs was repeated three times. Av-eraging the results of these three runs, the adaptivesearch space had a 3.5% lower total path cost com-pared to constant curvature controls, where cost canbe related to the roughness of the terrain or probabil-ity of mission failure.

In addition to these specific comparisons, manysubjective observations led the field team to believethat this search space offered some improvementsover our legacy constant curvature controls. Becauseof these factors, this adaptive search space has beensuccessfully used on the UPI program for 5 monthswith more than 250 autonomous kilometers drivenduring field tests.

6.2.2. Tartan Racing/Boss

The second platform for experiments in adaptivesearch spaces, Boss, was designed to compete inthe 2007 DARPA Urban Grand Challenge, a 60-mile(96.5 km), 6-hour race through an urban environ-ment. To date Boss has driven more than 2,000 au-tonomous miles using the described adaptive searchspace algorithm. This algorithm was chosen for keyparts of the local motion planner design based onits ability to conform the search space to highly con-strained road environments and its generation of tra-jectories that satisfy a representative dynamic model.The latter was especially important for navigationamong movable obstacles, as we needed an accurateprediction of vehicle motion to evaluate the safety ofeach action. The application of motion models thatdo not accurately represent reality (instantaneous



Figure 12. Comparative experiment between arc-based and adaptive search space local motion planners on the Crushermobile robot. (a) Graph showing the total path cost as it accumulates along the paths followed by the vehicle for each searchspace. (b) The two paths overlaid on a cost map of the site.

Figure 13. In-lane navigation. These images show the local motion-planning search space of Boss navigating in a single-lane road. (a) The trajectory that acquires the centerline path of the lane is selected because of the absence of obstacles.(b) The vehicle selects an alternative trajectory to the left of the centerline to avoid the parked car obstructing the right-hand side of the lane.



Figure 14. Generation of search spaces for passing. The ability to choose an arbitrary state sampling strategy allows simplegeneration of local motion-planning search spaces for lane switching. Boss senses that it would like to pass a slower-movingcar, so it generates and evaluates candidate actions for switching lanes.

velocity, infinite acceleration, the absence of angularvelocity or angular acceleration limits) could lead toselecting suboptimal and/or dangerous maneuversin the presence of both static and dynamic obstacles.This section will highlight two capabilities of apply-ing this method to on-road navigation from the Ur-ban Grand Challenge event that took place in Vic-torville, California, on November 3, 2007.

6.2.2.1. In-Lane Navigation

The majority of urban driving involves navigatingin a single lane and adhering to local traffic laws.Typically, it is optimal to drive in the center of theseroad lanes as this provides the most distance betweenthe vehicle and other obstacles in the environment.In some situations, it is necessary to make small ad-justments inside the lane to avoid cones, potholes,pedestrians, parked cars, or other typical hazards. Itis therefore necessary to not only generate trajecto-ries that will drive the vehicle to the center of theroad lane but to generate other candidate actions thatoffset the vehicle within the lane. Figure 13 shows

two situations where the vehicle plans to drive downthe center of the lane [Figure 13(a)] and the vehi-cle selects an action that drives the vehicle to theleft (but still within the lane) to avoid the parkedcar that is obstructing the right-hand side of the lane[Figure 13(b)].

6.2.2.2. Lane Switching

When lanes can be blocked in a multilane road orwhen driving as fast as possible is important, the ca-pacity to switch lanes is necessary. When a higher-level planner in the motion-planning hierarchy sug-gests to the local motion planner that it should switchlanes, the state space sampling strategy for the searchspace adapts to the desired lane and leverages themodel-based trajectory generator to determine the ac-tions necessary to reach those states. For example,consider Figure 14, where the vehicle is attemptingto pass a slower-moving vehicle on a two-lane road.When Boss realizes that it is necessary to changelanes, a series of candidate actions to make the s-turnmaneuver to change lanes are evaluated. Notice that



a single trajectory to a forward point of the currentlane is generated to provide an alternative action inthe situation in which all trajectories that drive to thedesired lane collide with obstacles, are infeasible, orare less optimal than driving in the current lane. Al-ternatively, a control space sampling technique thatconsidered actions more sophisticated than arcs (e.g.,clothoids) could also be used to generate the trajecto-ries for lane switching. This approach would, how-ever, be considerably less efficient as it could notguarantee that all of the actions generated satisfy theenvironmental constraints (staying within the roadnetwork) and thus would incur the increased com-putational burden of generating the large set of ac-tions needed to densely search the more complex con-trol space. Limiting the number of sampled actionsnegates the added computation requirements but sac-rifices completeness as the control space is sampledsparsely. Using the state space sampling frameworkfor navigating in constrained environments workswell because it allows the local motion planner to fo-cus its search in the direction of interest using moresophisticated control inputs.

7. CONCLUSIONS

We have leveraged our own recent work in model-predictive trajectory generation to create the ca-pacity to efficiently generate state-based sampled,dynamically feasible search spaces. This approachgenerates search spaces that are more expressiveand outperform contemporary approaches in inputor control space when navigating in complex en-vironments. This is important because the numberof trajectories that we can evaluate is limited (be-cause computational resources and replanning timeare constrained) and more complete search spacesare more likely to find the optimal solution. We havedemonstrated that this approach produces searchspaces that are less influenced by the initial state ofthe vehicle, can exploit global guidance, and can sat-isfy environmental constraints, all within a simpleunified framework. When navigating in an uncon-strained environment, the terminal boundary statesin the search space can be focused in the direction ofminimum global cost. In a constrained environment,such as in lane following or navigating in traffic, diffi-cult problems such as lane switching or in-lane obsta-cle avoidance become simple. The inherent feasibilityof the search space provides the navigation functionwith better information to represent the actual result

of a selected action. Because the actions consider pre-dictive dynamic models, they can be applied directlywithout the explicit need for a path follower.

The provided simulation and field experimentsdemonstrate that the algorithm performs betterwhen directly compared to a contemporary constantcurvature input space sampling technique. We alsoadvocate that this approach simplifies several rele-vant problems for mobile robot navigation in roadnetworks, including lane changes and in-lane navi-gation. This algorithm has been used to generate lo-cal motion-planning search spaces for two outdoormobile robots at speeds up to 30 mph (13.4 m/s),has been used to navigate thousands of autonomouskilometers in both constrained on-road and complexoff-road environments, and was generally used asthe search space generation component for CarnegieMellon’s winning entry in the 2007 Urban GrandChallenge.

ACKNOWLEDGMENTS

This work was sponsored by the Defense AdvancedResearch Projects Agency (DARPA) under contractUnmanned Ground Combat Vehicle PerceptOR Inte-gration (contract number MDA972-01-9-0005). Addi-tional field results are presented from Tartan Racing,with the generous support of our sponsors includingGeneral Motors, Caterpillar, and Continental and fur-ther supported by DARPA under contract HR0011-06-C-0142. The views and conclusions contained inthis paper are those of the authors and should not beinterpreted as representing the official policies, eitherexpressed or implied, of the U.S. Government.

REFERENCES

Berg, J., Ferguson, D., & Kuffner, J. (2006). Anytime pathplanning and replanning in dynamic environments(pp. 2366–2371). In Proceedings of the IEEE Conferenceon Robotics and Automation, Gaithersburg, MD. IEEE.

Berglund, T., Jonsson, H., & Soderkvist, I. (2003). Anobstacle-avoiding minimum variation b-spline prob-lem (pp. 156–161). In Proceedings of the 2003 Interna-tional Conference on Geometric Modeling and Graph-ics, Los Alamitos, CA. Thousand Oaks, CA: Sage.

Besiadecki, J., Leger, P., & Maimone, M. (2007). Trade-offs between directed and autonomous driving onthe Mars exploration rovers. International Journal ofRobotics Research, 26(91), 91–104.

Bode, M. (2007). Learning the forward predictive model foran off-road skid-steer vehicle (Tech. Rep. CMU-RI-TR-07-32). Pittsburgh, PA: Carnegie Mellon University.



Bonnafous, D., Lacroix, S., & Simeon, T. (2001). Motion gen-eration for a rover on rough terrains (pp. 784–789). InProceedings of the 2001 IEEE/RSJ International Con-ference on Intelligent Robots and Systems, Maui, HI.IEEE.

Chen, G., & Fraichard, T. (2007). A real-time navigationarchitecture for automated vehicles in urban environ-ments (pp. 1223–1228). In Proceedings of the 2007IEEE Intelligent Vehicles Symposium, Istanbul, Turkey.IEEE.

Coulter, R. (1992). Implementation of the pure pursuitpath tracking algorithm (Tech. Rep. CMU-RI-TR-92-01). Pittsburgh, PA: Carnegie Mellon University.

Cremean, L., Foote, T., Gillula, J., Hunes, G., Kogan,D., Kriechbaum, K., Lamb, J., Leibs, J., Lindzey, L.,Rasmussen, C., Stewart, A., Burdick, J., & Murray, R.(2006). Alice: An information-rich autonomous vehi-cle for high-speed desert navigation. Journal of FieldRobotics, 9(23), 777–810.

Daily, M., Harris, J., Keirsey, D., Olin, K., Payton, D., Reiser,K., Rosenblatt, J., Tseng, D., & Wong, V. (1988). Au-tonomous cross-country navigation with the ALV (vol-ume 2, pp. 718–726). In Proceedings of the IEEE In-ternational Conference on Robotics and Automation,Leuven, Belgium. IEEE.

Dubins, L. E. (1957). On curves of minimal length with aconstraint on average curvature and with prescribedinitial and terminal positions and tangents. AmericanJournal of Mathematics, 79, 497–516.

Ferguson, D., & Stentz, A. (2006). Using interpolation to im-prove path planning: The Field D* algorithm. Journalof Field Robotics, 23(2), 79–101.

Fox, D., Burgand, W., & Thrun, S. (1997). The dynamic win-dow approach to collision avoidance. IEEE Roboticsand Automation Magazine, 4, 22–33.

Frazzoli, E., Dahleh, M., & Feron, E. (2001). Real-time mo-tion planning for agile autonomous vehicles (pp. 43–49). In Proceedings of the American Control Confer-ence, Arlington, VA. IEEE.

Frazzoli, E., Dahleh, M. A., & Feron, E. (2003). A maneuver-based hybrid control architecture for autonomous ve-hicle motion planning (pp. 299–323). In Software en-abled control: Information technology for dynamicalsystems. Piscataway, NJ: IEEE Press.

Green, C., & Kelly, A. (2007). Towards optimal sampling inspace of paths. To appear in Proceedings of the Inter-national Symposium on Robotics Research, Hiroshima,Japan.

Haddad, H., Khatib, M., Lacroix, S., & Chatila, R. (1998). Re-active navigation in outdoor environments using po-tential fields (volume 2, pp. 1232–1237). In Proceedingsof the 1998 IEEE Conference on Robotics and Automa-tion, Leuven, Belgium. IEEE.

Howard, T., & Kelly, A. (2007). Optimal rough terrain tra-jectory generation for wheeled mobile robots. Interna-tional Journal of Robotics Research, 26(2), 141–166.

Howard, T., Knepper, R., & Kelly, A. (2006). Constrainedoptimization path following of wheeled mobile robotsin natural terrain (pp. 343–352). In Proceedings of theInternational Symposium on Experimental Robotics,Rio de Janeiro, Brazil. Berlin: Springer-Verlag.

Kelly, A., & Stentz, T. (1998). Rough terrain autonomousmobility—Part 2: An active vision and predictive con-trol approach. Autonomous Robots, 5, 163–198.

Kelly, A., Stentz, T., Amidi, O., Bode, M., Bradley,D., Diaz-Calderon, A., Happold, M., Herman, H.,Mandelbaum, R., Pilarski, T., Rander, P., Thayer, S.,Vallidis, N., & Warner, R. (2006). Toward reliableoff road autonomous vehicles operating in challeng-ing environments. International Journal of RoboticsResearch, 25(5).

Koren, Y., & Borenstein, J. (1991). Potential field methodsand their inherent limitations for mobile robot naviga-tion (pp. 1398–1404). In Proceedings of the IEEE Con-ference on Robotics and Automation, Sacramento, CA.

Lacroix, S., Mallet, A., Bonnafous, D., Bauzil, G., Fleury,S., Herrb, M., & Chatila, R. (2002). Autonomous rovernavigation on unknown terrains functions and integra-tion. International Journal of Robotics Research, 21(10–11), 917–942.

Lacze, A., Moscovitz, Y., DeClaris, N., & Murphy, K.(1998). Path planning for autonomous vehicles driv-ing over rough terrain (pp. 50–55). In Proceedingsof the 1998 IEEE ISIC/CIRA/ISAS Joint Conference,Gaithersburg, MD.

Leedy, B., Putney, J., Bauman, C., Cacciola, S., Webster, J., &Reinholtz, C. (2006). Virginia Tech’s twin contenders:A comparative study of reactive and deliberative nav-igation. Journal of Field Robotics, 23(9), 709–727.

Melchior, N., Kwak, J., & Simmons, R. (2007). Particle rrt forpath planning in very rough terrain. In Proceedings ofthe NASA Science Technology Conference 2007.

Miller, I., Lupashin, S., Zych, N., Moran, P., Schimpf, B.,Nathan, A., & Garcia, E. (2006). Cornell University’s2005 DARPA Grand Challenge entry. Journal of FieldRobotics, 23(8), 625–652.

Shimoda, S., Kuroda, Y., & Iagnemma, K. (2005). Potentialfield navigation of high speed unmanned ground vehi-cles on uneven terrain (pp. 2828–2833). In Proceedingsof the 2005 IEEE Conference on Robotics and Automa-tion, Barcelona, Spain.

Simmons, R., Krotkov, E., Chrisman, L., Cozman, F.,Goodwin, R., Hebert, M., Katragadda, L., Koenig,S., Krishnaswamy, G., Shinoda, Y., Whittaker, W., &Klarer, P. (1995). Experience with rover navigation forlunar-like terrains (volume 1, pp. 441–446.). In Pro-ceedings of the 1995 IEEE Conference on IntelligentRobots and Systems.

Singh, S., Simmons, R., Smith, T., Stentz, T., Verma, V.,Yahja, A., & Schwehr, K. (2000). Recent progress inlocal and global traversability for planetary rovers(pp. 1194–1200). In Proceedings of the IEEE Interna-tional Conference on Robotics and Automation, SanFrancisco, CA.

Spenko, M., Kuroda, Y., Dubowsky, S., & Iagnemma, K.(2006). Hazard avoidance for high-speed mobile robotsin rough terrain. Journal of Field Robotics, 23(5), 311–331.

Thrun, S., Montemerlo, M., Dahlkamp, H., Stavens, D.,Aron, A., Diebel, J., Fong, P., Gale, J., Halpenny, M.,Goffmann, G., Lau, K., Oakley, C., Palatucci, M., Pratt,V., & Stang, P. (2006). Stanley: The robot that won the



DARPA Grand Challenge. Journal of Field Robotics,23(9), 661–692.

Trepagnier, P., Nagel, J., Kinney, P., Koutsougeras, C.,& Dooner, M. (2006). Kat-5: Robust systems forautonomous vehicle navigation in challenging andunknown terrain. Journal of Field Robotics, 23(8), 509–526.

Urmson, C., Ragusa, C., Ray, D., Anhalt, J., Bartz, D.,Galatali, T., Gutierrez, A., Johnston, J., Harbaugh, S.,Kato, H., Messner, W., Miller, N., Peterson, K., Smith,

B., Snider, J., Spiker, S., Ziglar, J., Whittaker, W., Clark,M., Koon, P., Mosher, A., & Struble, J. (2006). A robustapproach to high-speed navigation for unrehearseddesert terrain. Journal of Field Robotics, 23(8), 467–508.

Wettergreen, D., Tompkins, P., Urmson, C., Wagner, M.,& Whittaker, W. (2005). Sun-synchronous robotics ex-ploration: Technical description and field experimenta-tion. International Journal of Robotics Research, 24(1),3–30.


Documents

State Space Sampling of Feasible Motions for High ......State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments Thomas M. Howard,