Adaptive Path Planning in a Dynamic Environment using a Receding HorizonProbabilistic Roadmap: Experimental Demonstration

Thomas A. Frewen, Harshad Sane, Marin Kobilarov, Sanjay Bajekal, Konda R. Chevva

We demonstrate the application of a receding-horizon motion-planning method based on probabilisticroadmaps (PRM) that uses sampling from motion primitive maneuver-automata, for the problem ofadaptive path planning in the case of a partially unknown obstacle field. Specifically, we consider tra-jectories followed by a helicopter equipped with finite-range obstacle sensors in unknown or partiallyunknown terrain. We provide a functional summary of our planning approach and an overview of thealgorithmic architecture. The planner functionality has been demonstrated through several software-in-loop (SIL) scenarios including adaptation to newly discovered obstacles and vehicle deviation fromplanned paths. This paper presents experimental results from flight tests with an electric helicoptershowing in-flight path adaptation to simulated obstacles.

Introduction

Consider a robotic vehicle navigating an obstacle-rich ter-rain with the objective to reach a prescribed goal subject toan optimality criterion e.g. in minimum time or distancetraveled. The vehicle is subject to constraints arising fromits underactuated dynamics, actuator bounds, and obstaclesin the environment. Furthermore, the vehicle has partialinformation about its surroundings and is equipped with asensor measuring the relative positions of obstacles in itsfield of view. This motion planning problem, although be-ing practically very relevant, has no closed-form analyticalsolution (Refs. 1,2) since both the dynamics and constraintsare nonlinear. The survey paper (Ref. 2) provides excel-lent background on the complexity of this problem and thelack of solutions with guarantees on completeness or op-timality. Gradient-based and potential-field methods havehad marginal success with this problem, however, they arestrongly influenced by a good starting guess as existenceof problem constraints yield many local minima and limitcompleteness. In addition, special differentiation (Ref. 3)is required to guarantee convergence due to the non-smoothnature of the constraints. The straightforward solution is todiscretize the vehicle state space and perform discrete graphsearch. Such a technique is limited to very simple problemsdue to the exponential size of the search space (state di-mension × trajectory epochs) also known as the curse ofdimensionality.

This paper proposes an approach based on a re-cent methodology in the robotics community, knownas sampling-based motion planning which includes the

T. Frewen, H. Sane (sanehs@utrc.utc.com), S. Bajekal andK. Chevva are with United Technologies Research Center,East Hartford, CT. M. Kobilarov is with California Insti-tute of Technology. Submission to 2011 AHS Specialists’Meeting on Unmanned Rotorcraft, Phoenix, AZ.

rapidly-exploring random tree (RRT) (Ref. 4) and the prob-abilistic roadmap (PRM) (Ref. 5). In our framework sam-ples are drawn from the vehicle’s continuous configurationspace, and are connected through sequences of precom-puted motion primitives (Ref. 6) that satisfy vehicle dy-namics and constraints. These form a maneuver automa-ton that encodes vehicle dynamics in the form of trim statesand inter-trim transition maneuvers, that may be stored forreal-time sampling during planning. The strength of our ap-proach lies in its ability to handle dynamic constraints andglobal planning in a unified framework, while respectingvehicle dynamics.

Construction of a fully connected dense PRM roadmapis conducted offline, while local modification of the exist-ing roadmap is performed during real-time execution. Inessence, the offline stage encodes the reachability space ofthe vehicle given prior knowledge about the environment.A dense PRM could be efficiently searched using classicalgraph search algorithms such as A* (Ref. 7) to provide aninitial solution. New sensor information is subsequently re-flected in online roadmap updates of graph nodes and edgesand replanning is accomplished using dynamic A* (D*).

In order to adapt to tracking errors and dynamic envi-ronment, we implement a receding horizon (RHC) strat-egy for PRM update and search that incorporates commonRHC concepts such as planning horizon, sensing horizon,and cost-to-go, where cost-to-go is derived directly fromthe current roadmap. In contrast to traditional RHC ap-proaches, this provides a consistent and unified way of rep-resenting planning horizon cost (reactive plan) and cost-to-go (global plan). The maneuver automaton, using lo-cally optimal motion primitives, is consistently used in theroadmap in conjunction with standard graph replanning andsearch methods. Uniform sampling and full reachabilityguarantee that an optimal solution can be asymptoticallyfound.

Fig. 1. Maxi Joker Electric rotorcraft demonstration unit.

The paper is organized as follows. We first formulate thegeneral adaptive path planning problem for a vehicle withdynamic constraints and an obstacle sensor. The probabilis-tic roadmap formulation and the computation of helicopterprimitives are then described. Our recursive PRM approachfor adaptation to newly discovered obstacles or deviationsfrom a planned path is described. Next, we characterizethe performance (feasibility, computational cost, objectivecost) through multiple Monte Carlo simulations. Software-in-loop (SIL) studies explore the relationship between num-ber of samples, environment clutter, and path optimality.Next, we discuss the SIL and experimental implementationof the proposed algorithms for the purposes of demonstra-tion on a Maxi-Joker-3 electric helicopter (Fig. 1). Lastly,we present results from SIL and experimental flight teststhat illustrate the capabilities of the recursive PRM algo-rithm. We conclude the paper by discussing opportunitiesfor improvements and future work.

Planning Problem Formulation

Consider a vehicle with state x∈X controlled using actuatorinputs u ∈U , where X is the state space and U denotes theset of controls. The vehicle’s dynamics are described by thefunction f : X×U → T X defined by

x = f (x,u) : vehicle dynamics model. (1)

In addition, the vehicle is subject to constraints arising fromactuator bounds and obstacles in the environment. Theseconstraints are expressed through the m inequalities

Fi(x(t),u(t))≥ 0 : constraints, (2)

for i = 1, ...,m.

The goal is to compute the optimal controls u∗ and finaltime T ∗ driving the system from its initial state x(0) to agiven goal region Xg ⊂ X , i.e.

(u∗,T ∗) = argminu,T

∫ T

0C(x(t),u(t))dt,

subject to x(t) = f (x(t),u(t)),

Fi(x(t),u(t))≥ 0, i = 1, ...,m.

x(T ) ∈ Xg

(3)

where C : X×U→R is a given cost function, e.g. fuel con-sumption, time to destination, or threat exposure. A typicalcost function includes a time component and a control effortcomponent, i.e. C(x,u) = 1+λ‖u‖2,λ ≥ 0.

Obstacle Constraints The vehicle operates in aworkspace that contains a number of obstacles de-noted by O1, ...,Ono with which the vehicle must notcollide. Typically, the vehicle state can be defined by(q,v) consisting of its configuration q ∈ C and velocityv ∈ Rnv (Ref. 8). Assume that the vehicle is occupyinga region A (q,v), and let prox(A1,A2) be the Euclideandistance between two sets A1,2 that is negative in the caseof intersection. Obstacle avoidance constraints in (2) canbe written as

F1(q(t)) = mini

prox(A (q(t)),Oi), for all t ∈ [0,T ]. (4)

Obstacle representation and proximity checking is a compu-tationally intensive procedure with a variety of documentedapproaches (Refs. 4, 9). For simplicity in this paper weassume a 2.5D digital elevation map (DEM) representa-tion and use standard packages (Proximity Query Package(Ref. 10)) to compute the proximity function prox.

Sensing We assume that the vehicle is equipped with afinite-range obstacle sensor measuring the relative positionsof obstacles, producing a set of points lying on the surfaceof obstacles. The sensor parameters include sensor range,scan-rate, and field of view. In this paper, we assume anidealized on-board simultaneous localization and mapping(SLAM) algorithm for map updates. The terrain, repre-sented using the 2.5D DEM is updated at a prescribed ratebased on the new occupancy data.

Planning with Probabilistic Roadmaps

We propose a motion planning method that combines PRM(Refs. 4, 5, 8) for global planning with maneuver automatafor incorporation of vehicle dynamics (Ref. 6). This solu-tion approach generates feasible trajectories that satisfy thegiven dynamics (1) and general constraints (2). A maneu-ver automaton encodes vehicle dynamics in the form of trimstates and inter-trim transition maneuvers that are stored forreal-time sampling during planning. The vehicle maneuverscan be pre-computed either through nonlinear trajectory op-timization (Ref. 11) or by recording the closed-loop trajec-tories of a simulated or real vehicle. A typical PRM pathconsisting of a sequence of maneuvers and trims is shownin Fig. 3.a. The general framework is suitable for systemssuch as aerial, underwater or ground robots if one ignoresthe pose-dependent external forces (wind, drag, friction).

PRM Algorithm

Fig. 2 provides an algorithmic overview of our approach fora roadmap G = (V,E) defined by its vertices V and edgesE. A key issue in the construction of a PRM is how to con-nect samples s; in Fig. 2 connections are made both into andfrom each state sample s with selection of closest neighbors

according to proximity function. In our framework sam-ples are connected through sequences of locally optimizedmotion primitives. The primitives are computed offline andorganized in a library for lookup during motion planning.Combining this strategy with heuristic graph search rendersthe overall approach suitable for real-time applications.

Algorithm: Construct-RoadmapInput:

s0: initial statesg: goal staten: number of nodes in the roadmapkin : number of incoming closest neighbors to each samplekout : number of outgoing closest neighbors to each sample

Output:roadmap G = (V,E)

1. V ←{s0}2. E← /0

3. while |V |< n do4. ss← Sample(S,G);5. V ←V ∪{ss}6. for all s ∈V do7. Ns← kin closest neighbors to s according to Dist8. for all s′ ∈ Ns do9. if edge = Connect(s′,s) 6= NIL then

10. E← E ∪{edge}11. Ns← kout closest neighbors from s according to Dist12. for all s′ ∈ Ns do13. if edge = Connect(s,s′) 6= NIL then14. E← E ∪{edge}

Fig. 2. Algorithm to construct a motion planning roadmap–agraph-based approximation of the reachable state space suit-able for efficient path planning.

Sampling In recent years considerable attention has beengiven to augmenting basic sampling-based motion plan-ners (e.g. see (Ref. 5)) with strategies for acceleratingthe search. In particular, biased sampling has been em-ployed to exploit either domain-specific knowledge suchas obstacle boundaries or to monitor performance based onconnectivity (Ref. 12). Such heuristic strategies are effec-tive when combined with regular (e.g. uniform) samplingin order to retain the required probabilistic completeness(Ref. 13) - complex problems can only be solved effec-tively through a proper balance of exploration and exploita-tion (Refs. 14, 15). Note that in this work we employ uni-form sampling in order to guarantee complete explorationof the state space. In general, the issue of optimal samplingis complex and considered an open problem.

Probabilistic Completeness Sampling-based methods arewell established techniques for handling (through approxi-

mation) motion planning problems in constrained environ-ments (Ref. 4). Since requiring algorithmic completenessin such settings is computationally intractable, a relaxednotion of completeness, termed probabilistic completenesshas been adopted (see for example (Ref. 16)) - if a solutionexists the probability that the algorithm will fail to find it ap-proaches zero as the number of roadmap samples n growsto infinity. In addition to being probabilistically complete,our approach employs branch-and-bound and pruning tech-niques to speed convergence towards an optimum.

Helicopter Vehicle Model and Maneuver Automata

Helicopter Model For the purposes of building PRM ma-neuver primitive library, we use a simplified helicoptermodel where the vehicle is represented by a single underac-tuated rigid body with position r ∈ R3 and orientation ma-trix R ∈ SO(3). Its body-fixed angular and linear velocitiesare denoted by ω ∈ R3 and v ∈ R3, respectively. The vehi-cle has mass m and principal moments of rotational inertiaJ1,J2,J3 forming the inertia tensor J= diag(J1,J2,J3).

a) b)Fig. 3. Computed optimal sequence of 4 maneuvers and 3 trimmotions in a simple environment (left). An optimal trajectoryin a more complex urban environment (right) requires moresophisticated sampling and pruning of a dynamic PRM.

The helicopter is controlled through a collective uc (liftproduced by the main rotor) and a side-thrust uψ (forceproduced by the rear rotor), while the direction of the liftis controlled by tilting the main rotor forward or back-ward through a pitch γp and sideways through a roll γr.The four control inputs then consist of the two forcesu = (uc,uψ) and the two shape variables γ = (γp,γr). Thestate space of the vehicle is X = SE(3)×R6 ×R2 withx = ((R, p),(ω,v),γ).

The equations of motion are[Rp

]=

[R ω

R v

], (5)[

J ω

mv

]=

[Jω×ω

mv×ω +RT (0,0,−mg)

]+F(γ)u, (6)

where the map · : R3→ so(3) is defined by

ω =

0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

,while the control matrix is defined as

F(γ) =

dr sinγr 0

dr sinγp cosγr 00 dt

sinγp cosγr 0−sinγr −1

cosγp cosγr 0

,

where dr,dt are distances from the center of mass to themain and tail rotor thrust vectors.

Helicopter Primitives The local motion planning methodis based on sequencing precomputed motion primitiveswhich satisfy the dynamics (5)–(6). The sequence consistsof trim primitives corresponding to motions with constantbody-fixed velocity and of maneuvers used to transition be-tween trims.

Trim conditions. Trim motions correspond to helices, cir-cles, or straight lines in position space. Such curves aretraced using velocity components (ω ′z,v

′x,v′y,v′z) which are

related to the body-fixed velocities v and ω through

ω = RT(φ ,θ ,0)ω

′, v = RT(φ ,θ ,0)v

′, (7)

where R(φ ,θ ,ψ) denotes the rotation corresponding to roll φ ,pitch θ , and yaw ψ and where v′ = (v′x,v

′y,v′z) and ω ′ =

(0,0,ω ′z).An invariant trajectory has a constant body-fixed veloc-

ity. Therefore, in order to determine the invariance condi-tions for given ω ′ and v′ one sets v = 0 and ω = 0 in (6)and substitutes (7) into (6). Assuming two identical inertiacomponents J2 = J3 and v′z = 0 a closed-form solution canbe found directly as

θ = 0, uy = 0, γp = 0, γr = 0,φ = arctan(−w′zv

′x/g),

uc = m(gcosφ −w′zv′x sinφ).

(8)

Assume that the helicopter is at configuration(R(φ0,θ0,ψ0), p0) and is about the execute a trim mo-tion with velocity (ω ′z,v

′x,v′y,v′z). Its state after t seconds

will evolve according to

x(t) = (ω,v,R(φ0,θ0,ψ(t)), p(t),γ), (9)

where the constant ω,v,θ ,φ ,γ are computed using (7)and (8). The yaw and position evolve according to

ψ(t) = ψ0 + tω ′z, p(t) = p0 +

cosψ0∆px− sinψ0∆pysinψ0∆px + cosψ0∆py

tv′z

,

where

∆px =

{(v′x sin tω ′z− v′y(1− cos tω ′z))/ω ′z, if ωz 6= 0tv′x, otherwise,

∆py =

{(v′x(1− cos tω ′z)+ v′y sin tω ′z)/ω ′z, if ωz 6= 0tv′y, otherwise.

In this paper we consider v′z = 0 which yields planartrims but as will be explained next maneuvers evolve in thecomplete 3-D position space.

Maneuvers. Maneuvers are computed to connect twotrimmed states. Let the map π : X → X subtractout the invariant coordinates from a given state, i.e.π((ω,v,R(φ ,θ ,ψ), p,γ)) = (ω,v,R(φ ,θ ,0),0,γ) . Then giventwo trims, the first one ending with state x1 and the secondone starting with state x2, we compute a maneuver trajec-tory x using the following optimization procedure:

Compute: T ; x : [0,T ]→ X ; u : [0,T ]→U

minimizing: J(x,u,T ) =∫ T

0(1+λ‖u(t)‖2)dt,λ > 0

subject to: π(x(0)) = x1,π(x(T )) = x2,

dynamics eq. (5),(6) for all t ∈ [0,T ].

In our case, the continuous optimal control formulationwas computationally solved through the discrete mechanicsmethodology (Refs. 11,17) which is particularly suitable forsystems with nonlinear state spaces and symmetries. Thecomputations were performed offline and optimal maneu-vers solutions were assembled in a library for lookup duringrun-time. With trims and maneuvers organized in a librarya motion plan in the absence of obstacles can be computedin closed form through inverse kinematics (Ref. 6) of a min-imal number of primitives.

Recursive PRM Algorithm

The proposed algorithm can naturally handle unmapped ob-stacles and recover from trajectory tracking failures. This isaccomplished by removing obstructed edges in the roadmapor adding new edges from the current state and performingefficient graph replanning. Fig. 4 sketches a prototypicalplanning scenario in which the vehicle must plan an op-timal route from an initial state x0 to a goal region X f inan obstacle-rich environment with only partial prior knowl-edge of the environment. Fig. 4 a) shows initial roadmapconstruction based on the prior environment information(e.g. obstacles O1,2,3). Trajectory regeneration is utilizedto plan around locally sensed, unknown obstacles obstruct-ing the vehicle’s initial path (O4 in Fig. 4 b) ; imperfectvehicle tracking (Fig. 4 c) of the planned path is addressedby the insertion of new roadmap nodes matching the cur-rent vehicle state(Fig. 4 d); node insertion is activated when

the error resulting from sensing and actuation noise, andenvironmental disturbances exceeds vehicle tracking capa-bilities.

x0O1

O2

O3

W

initial vehicle state

Xg

goal region

obstacle

roadmap

sensorobstacle

optimal path x∗

O1

O2

O3

W

Xg

x(t1)

O4

new obstacle detected

replanned optimal path

vehicle follows path

new edge added

a) b)

O1

O2

O3

W

Xg

O4actual state x(t2)

planned state xref(t2)

tracking error

vehicle deviates from path

O1

O2

O3

W

Xg

O4

rejoining the roadmap

updated optimal path

c) d)Fig. 4. A sketch of a typical planning scenario where a vehiclereplans when new obstacles are discovered or vehicle deviatesfrom prescribed planned path. Recomputing a new trajectoryin all cases is performed efficiently using D∗ search.

Figure 5 shows simulation of helicopter flight with finiterange sensor, where graph edges are updated as new obsta-cles are discovered, and, if required, new roadmap samples(corresponding to trim primitives) are added and connectedwithin the planning horizon. In either case, dynamic obsta-cles and obstacle map changes are handled automaticallythrough fast replanning in the graph structure.

The PRM performance with respect to the number ofroadmap nodes and the environment difficulty has been as-sessed in simulation. In order to limit the scope of this pa-per, we only present analysis of the algorithm computationtime (Fig. 6). In general, initial offline construction takesseveral seconds while replanning during execution requiredless than a second and can be accomplished in real-time.Optimality of computed solutions improves with increasingnumber of nodes.

Further details of our PRM algorithm analysis will be thesubject of future work; the results of these simulation stud-ies were used to guide the selection of algorithm parametersused in the experimental validation described next.

Simulation and Experimental Setup

Software-in-Loop (SIL) Implementation

Functional Layout and SIL Operation The SIL layoutdepicted in Fig. 7 combines a flight dynamics and controls

a) b)

c) d)Fig. 5. Example of receding horizon map updating and dy-namic replanning with a finite range sensor (blue semicircle).

50 100 150 200 250 300 350 40010

−5

10−4

10−3

10−2

10−1

100

101

102

# of nodes

offline construction

setup start/goal

planning

CP

U t

ime

(se

co

nd

s)

CPU computation time: 10 Monte Carlo runs

0 5 10 15 2010

−5

10−4

10−3

10−2

10−1

100

101

102

offline construction

setup start/goal

planning

environment difficulty (# of dangers)

CP

U tim

e (

seconds)

CPU computation time: 10 Monte Carlo runs

a) b)Fig. 6. Monte Carlo runs on a simulated city terrain (Fig. 3.b)showing computation time as a function of a) number ofroadmap nodes ; b) environmental difficulty.

simulator in closed-loop with path-planner, with the inten-tion to incorporate as many aspects of actual flight test aspossible. Consequently, as opposed to the model describedin the previous section, the helicopter simulator is a detailedclosed-loop helicopter model that includes aerodynamics,actuator models, implemented flight controller and asso-ciated modes, software interfaces and communication de-lays. The path planner initially creates an “offline” roadmapbased on the vehicle start state (communicated by the sim-ulator) and a selected goal state. Once the offline roadmapis complete the planner switches to “online” mode and tra-jectory commands are sent to the simulator based on thecurrent best planned path. The actual vehicle state, returnedby the simulator, is fed back to the planner in order to adaptand update the best planned path at regular intervals (re-planning frequency). The path planner has multiple threadsat different time-scales that allow for separate handling ofroadmap updates and trajectory commands to vehicle sim-

ulator. Synchronized access by each thread to shared re-sources is ensured so that deadlocks are avoided.

Fig. 7. Schematic of SIL functional layout depicting dedicatedPRM-based path planner computer connected via an ethernethub to a helicopter simulator (on a separate laptop). Trajec-tory commands are communicated via UDP packets from thepath planner to the vehicle simulator; vehicle state informa-tion is communicated by the simulator to the path planner alsothrough UDP.

Test Input Data Each SIL test run requires a start and goalstate for the vehicle, a terrain map, a maneuver automa-ton characterizing the vehicle dynamics, and a number ofPRM parameters that determine the behavior of the recur-sive PRM algorithm (e.g. the maximum number of roadmapvertices).

Primitives A motion primitive library for this specificvehicle is constructed based on vehicle simulation mod-els, while experimental data is used for library validation.The baseline maneuver automaton, for dynamics symme-try group G = SE(2)×R, consists of 31 trim primitives(nodes) and 961 connecting maneuvers (edges). Each trimprimitive corresponds to a different set of vehicle veloci-ties (ωz,vx,vy) with a maximum absolute linear velocity of10m/s and a maximum angular velocity of 30 degrees/s thatcorresponds to Maxi Joker 3 electric helicopter limits (ac-tual g-limits are higher). For comparison purposes, we alsoconstruct a pruned version of this maneuver automaton byremoving the faster primitives (nodes, edges) containing 19trims and the 361 maneuvers.

Obstacles and Map update The terrain is represented usinga DEM map loaded from a file. The map corresponds to theexperimental test scenario. The obstacle proximity func-tion prox (see (4)) between the helicopter trajectory andthe terrain is computed using the Proximity Query Pack-age (Ref. 10) that can compute closest distance between twoarbitrary meshes. The finite-range obstacle sensor param-eters include sensor range, scan-rate, and FOV. The 2.5DDEM map is updated at a prescribed frequency.

Trajectory Controls The planner output consists of a time-parametrized trajectory consisting of a sequence of trimsand maneuvers. The on-board aircraft flight control system

(FCS) is designed to follow a full-state trajectory commandand typically operates at 100Hz or more. The planner-to-FCS interface relays the trim-portions of the planned tra-jectory alone, while relying on the FCS to generate thestitching maneuvers in real-time. Although doing this intro-duces discrepancy between the maneuvers in the primitivelibrary and the FCS-generated ones, this approach results ina smoother flight performance.

Test Output Data For the duration of each testrun we record both the actual and commandedvehicle states and the associated timestamps(t,ωx,ωy,ωz,vx,vy,vz,φ ,θ ,ψ,x,y,z). For the actualflight tests, we record video from onboard and groundcameras.

Experimental Implementation

The PRM approach was validated through flight tests us-ing a Maxi Joker 3 RC electric helicopter. The Maxi Joker3 variant used in the flight tests had a flybar with a torquetube driven tail. The helicopter has gross weight capabilityof 20lb with a flight time of approximately 12 minutes. Aschematic of the experimental set-up is shown in Fig. 8).An onboard embedded processing board hosts a trajectorytracking controller that receives trajectory commands forthe current control horizon from the ground-based plannervia a dedicated radio datalink. The datalink also relays cur-rent vehicle state and actuator and sensor status as feedbackto the ground-based planner for the receding horizon plan-ner update. In the event of an emergency, a human safetypilot can override and take over control of the vehicle via adedicated datalink.

Fig. 8. Schematic of the experimental setup.

The flight tests were conducted at a baseball field ap-proximately 500 ft long and 300 ft wide. Fig. 9 shows anaerial photo of the experimental test site with virtual obsta-cles (organized in the form of a set of city buildings) su-perimposed. Since the obstacles are simulated, the rangingsensor output and obstacles locations are provided to theplanner as simulated inputs with appropriate delays, whilethe vehicle executes real flight.

Fig. 9. Aerial view of the test site with virtual obstacles super-imposed

Results

SIL Tests

Our first set of SIL tests were performed using an obsta-cle map with city buildings (Fig. 10 a) with the path plan-ner having complete a priori knowledge of the environ-ment. These tests allowed for separate assessment of pathplanning, using the selected functional architecture with thesimulator, unencumbered by the recursive planner routinesrequired when the environment is only partially known anda finite range sensor is used. Our second set of tests, de-scribed further below, probes the latter scenario for differ-ent obstacle maps - the planner is provided a partial obsta-cle map (with numerous buildings from the actual environ-ment map omitted) and only locally senses unknown terrainbased on the range of obstacle sensor.

For the selected scale parameters, the simulated citybuilding map is densely populated with buildings (Fig. 10 a)and the planner, with complete a priori knowledge of the en-vironment, prefers a path close to the perimeter of the mapwhere the free space volume is larger. (Fig. 10 b) shows thesame SIL commanded (blue) and actual (red) vehicle trajec-tories from an overhead viewpoint. Note that the roadmapsampling here is not closely coupled to the layout of ob-stacles (other than samples in obstacles being rejected) andso the likelihood of generating a “direct” path from startto goal state through the building corridors is markedly re-duced. The 200 roadmap samples used are drawn from auniform distribution. The vehicle does deviate somewhatfrom the commanded planned path in Fig. 10. This iscaused in part by path planning with an idealized maneuverset (maneuvers are not commanded) that is not entirely rep-resentative of the maneuvers the vehicle actually performs.

Fig. 11 shows SIL results for scenario where the planneris provided only a partial obstacle map and local sensing oc-curs as the vehicle moves. Only 2 of the 7 obstacles, corre-sponding to the obstacles nearest to the vehicle starting po-

a) b)Fig. 10. Example of SIL path planning for simulated citybuilding obstacle map. Two perspectives of the same simula-tion run are shown (panel (a) clearly indicates obstacle sizesand vehicle navigation around buildings) with commanded(red curve) and actual vehicle trajectories shown (blue line).

sition are known a priori to the planner with the remaindersensed when they are within vehicle range and field of view.The figure shows the sequence of map-building and replan-ning snapshots as the helicopter successfully navigates theentire obstacle set.

Flight-Tests

Fig. 12 shows the results of flight tests for real-time trajec-tory planning with a virtual city building map, correspond-ing to the SIL runs described previously; the obstacles arenow represented “virtually” by the path planner. For thesame start and goal locations as in the SIL test shown inFig. 10, two different paths are successfully navigated bythe vehicle. Both planned paths are valid but the variabilityin the route proposed by the planner raises some interest-ing questions with regard to the number of roadmap nodesrequired, the randomness of the PRM algorithm, and theneed for appropriate metrics of path suitability other thanminimum flight time that demand further investigation.

Fig. 13 shows flight test results for scenarios where theplanner is provided with only a partial obstacle map andlocal sensing occurs as the vehicle moves. The vehicle suc-cessfully navigates from start to destination state for a sce-nario where a single unknown obstacle obstructs the straightline path connecting these states. As the number of obsta-cles unknown a priori to the planner increase so does thecomputational complexity associated with map updates androute replanning. As a future step, streamlining the compu-tational expense associated with the replanning routines inour recursive planner is essential for real-time solution ofmore complicated scenarios.

Conclusions

The strength of our robust motion planning approach usingprobabilistic roadmaps lies in its ability to handle dynamicconstraints and perform global planning in a unified frame-work respecting vehicle dynamics. The method is shown to

a) b)Fig. 11. Example of SIL path planning for uncertain a prioriobstacle map with finite range sensor. Sequence of snapshotsshowing map-building and replanning are shown, with the fi-nal snapshot showing the entire trajectory. The blue ray-conerepresents the ranging sensor. The small helicopter icons rep-resent the samples placed on the planning map.

a) b)Fig. 12. Example of real path planning in a virtual obsta-cles map with city building. Two instances of an experimentwith the same start and goal locations (as in Fig.10) are shown(commanded (red curve) and actual vehicle trajectories shown(blue line)).

rapidly determine a feasible flight trajectory with solutionoptimality improving with increasing run-time. We haveprovided algorithmic details and schematics describing theplanner logic and illustrated the applicability of this method

a) b)Fig. 13. Example of real path planning using virtual obstaclesfor partially known obstacle map with finite local sensing (cor-responding to simulation result in Figure 11). Two instancesof an experiment with the same start and goal locations (as inFig.10) are shown (commanded (red curve) and actual vehicletrajectories shown (blue line)).

to several simulated scenarios.

We also successfully flight-tested our path planningapproach in an outdoor environment with numerous vir-tual obstacles. The onboard flight controller can be im-proved (Ref. 18) to be more compatible with velocity-primitive commands issued by the PRM planner. In orderto address the computational burden of map update and ob-stacle checking, it will be necessary to explore efficient ob-stacle representations and related algorithms. In general,refinements and extensions to our approach in the area ofrobust planning strategies, faster computation and samplingtechniques will be the subject of future development effortsand technical publications.

Acknowledgement

The authors would like to thank Chaohong Cai, EmilioFrazzoli, and Suresh Kannan for useful discussions andfeedback. We also thank our test pilots, Joe Acosta andEvan Richey, for their flight test support.

References

1Canny, J. F., The Complexity of Robot Motion Planning,MIT, Cambridge, 1988.

2Goerzen, C., Kong, Z., and Mettler, B., “A Surveyof Motion Planning Algorithms from the Perspective ofAutonomous UAV Guidance,” Journal of Intelligent andRobotic Systems, Vol. 57, 2009, pp. 65–100.

3Clarke, F. H., Ledyaev, Y. S., Stern, R. J., and Wolenski,P. R., Nonsmooth Analysis and Control Theory, Springer,1998.

4LaValle, S. M., Planning Algorithms, Cambridge Uni-versity Press, Cambridge, U.K., 2006.

5Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G. A.,Burgard, W., Kavraki, L. E., and Thrun, S., Principles ofRobot Motion: Theory, Algorithms, and Implementations,MIT Press, ISBN 0-262-03327-5, June 2005.

6Frazzoli, E., Dahleh, M. A., and Feron, E., “Maneuver-Based Motion Planning for Nonlinear Systems with Sym-metries,” IEEE Transactions on Robotics, Vol. 21, (6), dec2005, pp. 1077–1091.

7Russel, S. and Norvig, P., Artificial Intelligence A mod-ern Approach, Pearson, 2010.

8Latombe, J.-C., Robot Motion Planning, Kluwer Aca-demic Press, 1991.

9Ericson, C., Morgan Kaufmann Publishers, Springer-Verlag, New York, San Francisco, CA, 2005.

10Gottschalk, S., Lin, M. C., and Manocha, D., “OBBTree:A Hierarchical Structure for Rapid Interference Detection,”Eurographics/ACM SIGGRAPH Symposium on ComputerAnimation, Vol. 30, 1996, pp. 171–180.

11Kobilarov, M., Discrete Geometric Motion Control ofAutonomous Vehicles, PhD thesis, University of SouthernCalifornia, 2008.

12Hsu, D., Sanchez-Ante, G., and Sun, Z., “Hybrid PRMSampling with a Cost-Sensitive Adaptive Strategy,” Proc.IEEE Int. Conf. Robotics and Automation ICRA 2005,2005.doi: 10.1109/ROBOT.2005.1570712

13Bekris, K. E. and Kavraki, L. E., “Informed and Prob-abilistically Complete Search for Motion Planning underDifferential Constraints,” First International Symposium onSearch Techniques in Artificial Intelligence and Robotics(STAIR), July 2008.

14Powell, W., Approximate Dynamic Programming: Solv-ing the Curses of Dimensionality, Wiley Series in Probabil-ity and Statistics, 2007.

15Rickert, M., Brock, O., and Knoll, A., “Balancing explo-ration and exploitation in motion planning,” Proc. IEEE Int.Conf. Robotics and Automation ICRA 2008, 2008.doi: 10.1109/ROBOT.2008.4543636

16Ladd, A. and Kavraki, L., “Measure theoretic analy-sis of probabilistic path planning,” IEEE Transactions onRobotics and Automation, Vol. 20, (2), April 2004, pp. 229–242.doi: 10.1109/TRA.2004.824649

17Marsden, J. and West, M., “Discrete mechanics and vari-ational integrators,” Acta Numerica, Vol. 10, 2001, pp. 357–514.

18Cunha, R. and Silvestre, C., “A 3D Path-FollowingVelocity-Tracking Controller for Autonomous Vehicles,”Proceedings of the 16th IFAC World Congress, Vol. 16, July2005.