Rapidly-Exploring Roadmaps:Weighing Exploration vs. Refinement in Optimal Motion Planning

Ron Alterovitz, Sachin Patil, and Anya Derbakova

Abstract— Computing optimal motion plans requires bothexploring the configuration space to identify free space regionsas well as refining understanding of explored regions in orderto optimize the plan. We present the rapidly-exploring roadmap(RRM), a new method for single-query optimal motion planningthat allows the user to explicitly consider the trade-off betweenexploration and refinement. Prior methods have focused solelyon exploration or refine prematurely, resulting in wastedcomputation. RRM initially explores the configuration spacelike an RRT. Once a path is found, RRM uses a user-specifiedparameter to weigh whether to explore further or refine theexplored space by adding edges to the current roadmap toenable finding higher quality paths in the explored space.We demonstrate the performance of RRM and the trade-offbetween exploration and refinement using two examples, a pointrobot moving in a plane and a concentric tube robot capableof following curved trajectories that has the potential to assistphysicians in minimally invasive medical procedures.

I. INTRODUCTION

Motion planning requires exploring the configurationspace of a robot or agent in order to find a sequenceof feasible actions that will maneuver the robot or agentaround obstacles to a goal. However, not all motion plansare equal. In optimal motion planning, the objective isto find the best solution that optimizes relevant criteria,such as a path of minimum length, greatest clearance fromobstacles, or minimum control effort. With improvements incomputation speed and algorithms, we can strive to computeoptimal motion plans in cases where in the past even feasiblesolutions were difficult to obtain.

The most successful approaches to motion planning inpractice involve building discrete representations (trees orroadmaps) of the free space, the subset of the robot’sconfiguration space where it can move without collision withobstacles. When a start and goal are specified and we areonly concerned about a single query, building this discreterepresentation of the robot’s free space introduces an inherenttrade-off between exploration and refinement.

Exploration seeks to find free space where the robotcan travel without colliding with obstacles. In contrast,refinement seeks to improve understanding of configurationspace in a local region but does not explore new regions.Refinement introduces multiple pathways to reach the samepoint, enabling selection of a path through a known freespace region that minimizes or maximizes some criteria.Computing a globally optimal plan in general requires bothexploration and refinement.

R. Alterovitz, S. Patil, and A. Derbakova are with Department ofComputer Science, University of North Carolina at Chapel Hill, ChapelHill, NC 27517, USA {ron,sachin,anya}@cs.unc.edu

Fig. 1. Exploration seeks to find new regions of free space while refinementseeks to find better paths within the explored regions. In the RRM above,exploration could identify an entirely new region of free space, whilerefinement could reduce the length of the path to the goal. RRMs allow theuser to explicitly specify the trade-off between exploration and refinementwith the guarantee that the planner will return the optimal solution withprobability 1 as computation time is allowed to increase.

We introduce a new motion planning method, the rapidly-exploring roadmap (RRM), that allows the user to explicitlybalance exploration and refinement while converging to anoptimal plan. Our focus is on optimal single-query motionplanning in constrained spaces. We are given start and goalconfigurations, obstacle locations, properties of the robot oragent, and a quality metric with which to evaluate plans.Our objective is to compute an optimal plan from the startconfiguration to the goal based on the given quality metric.RRM works for quality metrics that are additive over a path,such as distance. In computing a plan, RRM initially onlyiteratively explores the configuration space because thereis insufficient information about the free space to refine.Once a feasible path from the start to the goal has beenfound, the method uses a user-specified parameter to weighwhether to (1) explore for new free space regions, or (2)refine the current explored space to find a solution thatis closer to optimal. The method guarantees, under certainassumptions, that the computed path will converge to theglobally optimal solution with probability 1 as the numberof iterations increases.

RRMs combine ideas from the two most popularsampling-based motion planners: probabilistic roadmaps(PRMs) and rapidly-exploring random trees (RRTs) [8], [17].For both PRMs and RRTs, the probability that the methodfails to find a feasible solution (when one exists) decreasesexponentially with the number of iterations [19]. However,Karaman and Frazzoli proved that RRTs as originally definedwill fail to return an optimal solution with probability 1 [13].RRM combines the rapid exploration property of RRTs withthe ability to optimize paths over a roadmap as in PRMs,

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.

allowing us to quickly find optimal solutions for single-queryproblems. As shown in Fig. 1, the RRM is represented usinga graph in configuration space. In RRMs, exploration corre-sponds to sampling the free space and expanding the graphtoward the sample in an RRT, tree-like manner. Refinementcorresponds to connecting existing samples using additionaledges to build a roadmap over which a shortest path can becomputed or some other criteria can be optimized.

Since collision detections in complex environments can beone of the most computationally expensive steps of sampling-based planning, our method strives to minimize collisionchecks and avoid any duplication. This is facilitated byseparating exploration from refinement in RRM construction;there is significantly more benefit in refining nodes alongthe path to the goal than nodes on sub-trees that have beenexplored but cannot reach the goal. This is in contrast toprior sampling-based methods such as the original PRM [14]and to more recent optimal methods such as RRT* [13] thatdeterministically perform collision detections in all exploredregions of the configuration space regardless of whether theseregions can be part of the final optimal solution.

We first demonstrate the potential of RRM using examplesbased on a point robot moving on a plane. We then applyRRM to compute motion plans for a concentric tube robot,a device composed of nested nitinol tubes that be controlledto follow curved paths through open air. These devices havethe potential to assist physicians in performing minimallyinvasive surgical procedures by maneuvering through con-strained anatomical spaces such as the trachea and bronchito reach clinical targets previously inaccessible using needle-like devices. By explicitly considering the trade-off betweenexploration and refinement using RRM, we can providephysicians with the flexibility to best utilize available com-putation time based on the task at hand.

We discuss related work on motion planning in Sec. II. InSec. III, we present the RRM algorithm, including buildingthe roadmap and querying for a solution. We also analyzethe performance of the method and show that it is bothprobabilistically complete and returns the optimal solution,under certain assumptions, with probability 1 as the numberof iterations increases. In Sec. IV, we apply the algorithmto the test cases discussed above and illustrate the benefitsof separating exploration and refinement and demonstratethe convergence of RRM plans to global optimality ascomputation time is allowed to increase.

II. RELATED WORK

Although solving the general motion planning problem isPSPACE-hard, sampling-based methods work well in prac-tice for a wide variety of practical problems [8], [17]. RRTsare commonly used to compute feasible motion plans forsingle-query problems involving holonomic, nonholonomic,and kinodynamic systems [18]. However, RRTs focus purelyon exploration and make no effort at refinement, a necessarystep for computing optimal motion plans. In contrast, PRMsexplore (via sampling) and refine (via adding multiple edgesper sample) at every iteration [14]. While highly effective

for multi-query problems, for single-query problems this canresult in a large number of wasteful collision checks. PRMsalso explore the entire configuration space rather than onlythe space accessible from the start state, which may result inrefining the roadmap in free space regions that are disjointfrom the start configuration. RRM combines the benefitsof RRTs and PRMs and allows balancing exploration andrefinement.

Some prior sampling-based motion planners perform ex-ploration and refinement as distinct phases of the planningprocess, although they do not balance the two within aunified framework. Fuzzy PRM [23], Lazy PRM [4], and C-PRM [29] planners initially generate a randomly generatedgraph over configuration space and attempt to find a feasible(and optimal) path using lazy validation of the graph edgesin a bid to reduce the number of collision checks.

Rather than uniformly sampling the configuration space,guided exploration utilizes properties of the workspace andconfiguration space to bias future samples [10], [1], [12],[28], [32], [7]. Many of these strategies could be directlyincorporated into the exploration iterations of RRMs. As anextension of guided exploration, Rickert et al. [25] proposean exploring/exploiting tree (EET) to balance between ex-ploring new regions of the configuration space and exploitingthe results of exploration using potential fields. EET does notconsider path optimality.

Because most single-query sampling-based planners pro-duce jerky, unoptimized paths, many methods have beendeveloped to improve path quality in a post-processingstep [5], [15], [11]. These approaches are tailored for aspecific criterion and only work on refining the path within itshomotopy class. In contrast, our approach provides a unifiedplanning framework to compute, in the limit, an optimal pathacross homotopy classes without requiring a post-processingrefinement step. However, post-processing could be used inconjunction with RRM in order to find a smooth solutionamong the homotopic pathways that have been found whenonly a small number of iterations have been executed.

Recent variants of single-query sampling-based plannershave introduced ideas to optimize the quality of the solutionduring the planning process. Urmson and Simmons [30]propose heuristics to bias the tree growth in a RRT plannertowards regions of the configuration space that result in low-cost solutions, but the method is not guaranteed to convergeto optimality in the limit. Ferguson and Stentz [9] have con-sidered running the RRT algorithm multiple times in orderto progressively improve the quality of the solution. Theyshowed that each run of their algorithm results in a path withsmaller cost, even though the procedure is not guaranteed toconverge to an optimum solution. Recently, Karaman andFrazzoli [13] propose a new incremental sampling-basedalgorithm called the rapidly-exploring random graph (RRG)and a tree-variant of the same algorithm called RRT*, whichalways converge to an optimal solution. Using results fromrandom geometric graph theory, these methods retain theasymptotic computation complexity of RRTs under certainassumptions, and we use these results in the computational

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.

complexity analysis of RRMs. Our approach is similar inspirit to the RRG algorithm in that a tree is augmented withadditional edges in the neighborhood of a node to enableoptimization. RRG and RRT* are constantly refining, whichresults in unnecessary collision detection effort for pathwaysthat have little or no chance of reaching the target, whichcan be computationally expensive. Our approach refinespathways starting from homotopic solutions that reach thegoal and could be thought of as a lazy variant of the RRGalgorithm where we seek to minimize the number of collisionchecks while still retaining the optimality guarantees. Wealso explicitly provide a user-controlled weighting parameterthat balances exploration and refinement.

A different set of approaches that also offer optimalityguarantees is based on graph search algorithms (such as A*)applied over a grid that discretizes the configuration space[17]. These methods have been extended to run in an anytimefashion to deal with dynamic environments [21] and nonholo-nomic systems [20]. These methods only ensure optimalityup to the chosen grid resolution, and the computationalcomplexity grows exponentially with the dimensionality ofthe configuration-space.

III. ALGORITHM

A. Notation and Problem DefinitionLet C be the configuration space of the robot with d ∈ N

degrees of freedom. Let Cfree ⊆ C denote the subspace ofthe configuration space for which the robot is not in collisionwith an obstacle. We assume that the free space Cfree is finite,i.e. it is bounded by obstacles or the reachability of the robot.Let q ∈ C denote a configuration of the robot. The RRMplanner requires as input the start configuration of the robot,qinit and a set of goal configurations, Qgoal ⊆ Cfree.

Similar to motion planning algorithms like PRM and RRT,RRM requires the definition of the certain problem-specificfunctions:

• collision free(q1, q2): This function returns false

if the path (computed by a local planner [8]) fromconfiguration q1 to q2 collides with an obstacle andtrue otherwise.

• cost(q1, q2): This function specifies the cost associatedwith moving between two configurations q1 and q2,which can equal control effort, the Euclidean distancein d-dimensional space, or any problem-specific user-specified metric. We require cost to be continuous overits input parameter space.

• distance(q1, q2): This function specifies the distancein configuration space between two configurations q1

and q2. Lower distances imply less effort to evaluatecollision between the two configurations. We requiredistance to be continuous over its input parameterspace.

• nearest neighbor(V, q): From a given set of config-urations V , return the nearest neighbor to configurationq using the distance metric defined by distance.

• nearest neighbors(V, q): From a given set of con-figurations V , return a set of neighbors within some

distance of configuration q. This distance can varybased on the current state of the algorithm and must benon-increasing over time.

The objective of standard motion planning is to find a pathdefined by a sequence of configurations (q0, q1, q2, . . . , qend)such that q0 = qinit and qend ∈ Qgoal and each local path ofsequential pairs of configurations lies in Cfree. The objectiveof optimal motion planning is to find a feasible path suchthat the cost of the path is minimized, where the cost of apath is defined to be the summation or maximum of the costsof sequential pairs of configurations along the path.

The RRM algorithm also requires an additional parameter,wexplore, that weighs exploration versus refinement. Settingthis parameter to 1 results in exploration only, which isequivalent to a standard RRT. Setting this parameter to 0causes the RRM to refine immediately whenever possible,which slows does the exploration process. As discussed inSec. III-D, appropriately setting wexplore will guarantee thatRRM will return the optimal plan with probability 1 as thenumber of iterations increases.

B. RRM Algorithm

The RRM R is defined by the tuple G, where G = (V,E)is a weighted directed graph with vertices V and weightededges E. A vertex is defined by a configuration as welladditional information that will be described below. Thegraph is directed because we do not assume that movingfrom qi to qj is equivalent to moving from qj to qi (e.g.moving forward may incur a different cost from moving inreverse).

We build the RRM using the algorithm RRM plan definedin Alg. 1. Initially, the vertex set V contains only qinit. Thealgorithm also maintains two global lists: U and S. U is theset of vertices that should be refined but have not yet beenrefined. S is the set of vertices that have already been refined.The algorithm then enters a loop. If U is not empty, then thealgorithm explores the configuration space with probabilitywexplore or refines the RRM with probability 1 − wexplore.The algorithm iterates until the user stops the process.

The exploration algorithm RRM explore, defined in Alg.2, proceeds similarly to an iteration of the original RRTalgorithm [17]. The algorithm generates a sample config-uration qsample, which in our implementation is a uni-form random sample from C. We find the sample’s nearestneighbor qnear in V . We then consider the edge that ex-tends from qnear toward qsample up to a distance of dstep,which is a user-specified constant. We define this edge as(qnear, qnew, cost(qnear, qnew)). If this edge is collision free,we add the new vertex qnear and the new edge to G.

If we add qnear to G and it is inside Qgoal, then we havefound a new feasible path from qinit to the goal. This newfeasible path may be a new homotopic path, so we mark forrefinement all vertices on that path by adding them to U .(For computational efficiency, we note that we only need tobacktrack from qnear until we reach the first vertex in S orU , adding vertices to U along the way.) We also add qnear

to U if its edge connects it to a vertex that is in S since

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.

Alg. 1 RRM plan: Build the RRM data structure and computea path.Input:

qinit: initial configuration, Qgoal: goal regiondstep: maximum size of edge for RRT expansionwexplore: weight of exploration relative to refinement,

wexplore ∈ [0, 1)Output:

P : Sequence of configurations defining a collision-freepath from qinit to qgoal

1 counter← 02 V ← {qinit, qgoal}3 prev[qinit]← nil; prev[qgoal]← nil4 counter add[qinit]← nil5 counter refine[qinit]← nil6 E ← ∅; U ← ∅; S ← ∅7 do8 w ← uniform random number in range[0, 1]9 if |U | = 0 or w ≤ wexplore then10 RRM explore()11 else12 RRM refine()13 endif14 until user stops process15 P ← shortest path in weighted directed graph

G = (V,E)16 return P

this new vertex is already adjacent to a refined vertex. Thisprocedure is handled by Alg. 3.

The refinement algorithm RRM refine, defined in Alg. 4,randomly selects a configuration qrefine from U to refine.The algorithm then finds the nearest neighbors of qrefine

and saves them to the set Qnear. In our implementation,nearest neighbors returns all vertices within distancedstep since we assume that dstep is the maximum distanceedge we are willing to conduct a collision check on. Wealternatively can define a distance that decreases as thenumber of vertices increases, as in Karaman et al. [13]. Foreach configuration qnear in Qnear, the algorithm checks ifthe edge from qrefine to qnear is collision-free and adds theedge if possible. In doing this, we need to be careful to avoidduplicating collision checks that were already performed. Weonly perform collision checks in the following cases:

1) The vertex qnear was not already refined (i.e. qnear /∈ Sor was refined before vertex qrefine was added). Wehandle the latter check in O(1) computation time byintroducing a global counter. The global counter isinitialized at 0 and is incremented each time a vertexis added or refined. Using vectors counter add andcounter refine, we maintain the appropriate countervalues for each vertex.

2) The edge was not already checked for collisions duringthe exploration stage. The algorithm already performed

Alg. 2 RRM explore: Expand a RRM by exploring.Input:

Global variables G = (V,E), prev, dstep, U , S,counter add, counter refine, and counter fromRRM plan

Output:G = (V,E), prev, U , counter add, counter refine, and

counter are updated

1 qsample ← a randomly chosen configuration fromC

2 qnear ← nearest neighbors(V, qsample, 1)3 qnew ← the point along the straight line from

qnear to qsample that is at most distancedstep away from qnear

4 if collision free(qnear, qnew) then5 V ← V ∪ {qnew}6 E ← E ∪ {(qnear, qnew, cost(qnear, qnew)}7 prev[qnew]← qnear

8 if qnear ∈ S then9 RRM mark(qnew)10 if qnew ∈ Qgoal then11 RRM mark(qnew)12 E ← E ∪ {(qnew, qgoal, 0)}13 endif14 counter add[qnew]← counter ++15 counter refine[qnew]←∞16 endif17 return

a collision check from prev[qnear] to qnear whenadding qnear to the graph.

By only checking collisions for edges they meet both of theabove criteria, we guarantee that each edge that should bechecked for collision is examined exactly once. We repeat theabove for the opposite direction edge from qnear to qrefine.After an edge is added, we also check if the vertices shouldbe marked for refinement and added to set U using the samerequirements as during RRM explore.

The algorithm RRM plan iterates and adds configurationsand edges to the graph until the user stops the process. Thealgorithm then computes and returns the shortest path in theRRM graph using the user-specified cost metric.

C. Computational Complexity

The computational complexity of general, complete mo-tion planning algorithms is exponential [24]. However, it-erative sampling-based planners like RRTs and PRMs haveperformed well in practice for finding feasible plans. Thecomputational complexity of sampling-based algorithms isdependent on the algorithm used for identifying nearestneighbors. Algorithms based on kd-trees or BBD-trees returnnearest neighbors in O(log |V | exp d) time [3], [33], whichoutperforms brute-force nearest neighbor queries for low

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.

Alg. 3 RRM mark: Mark a path from a given configuration toqinit for refinement.Input:

q: configurationGlobal variables G = (V,E), prev, U , and S, and

counter from RRM plan

Output:Global variable U (the set of vertices waiting to be

refined) is updated

1 while q /∈ S and q /∈ U and q 6= nil2 U ← U ∪ {q}3 q ← prev[q]4 endwhile5 return

dimensional spaces [33]. For high dimensional spaces, brute-force O(|V |) is the fastest option.

Let k be the number of iterations that RRM plan isrun for before computing the shortest path. We note that|V | ≤ k since at most k vertices can be added to thegraph, corresponding to the case where we always exploreand each exploration results in a collision-free edge beingadded to the graph. We also note O(|E|) ≤ O(k2). Inpractice, |E| will be dependent on the exact implementationof nearest neighbors.

Let D be the computational complexity of detecting ifa collision occurs as the robot moves between two statesfor a distance up to dstep. We note that D grows forenvironments with more complex obstacles and is dependenton the collision detection algorithm used. We note thatalthough RRM refine and RRM explore make multiple callsto RRM mark, no vertex is ever marked more than once. Sothe total computational complexity of RRM mark over theentire execution of RRM build is capped at O(|V |). Also,we can implement constant-time access to U and S bystoring these sets as lists and including pointers to entriesin those lists from each vertex in the graph. Hence, bothRRM explore and RRM refine called by RRM plan havea computational complexity of O(|V |D) using brute-forcenearest neighbor searching and an asymptotic expected com-plexity of O(D log |V |) if nearest neighbors is restrictedto balls of volume proportional to log |V |/|V | [13].

The search at the end of RRM plan has computational com-plexity O(|V | log |V |) for Dijkstra’s algorithm or other sim-ilar shortest path algorithms, although this is dominated bythe prior k iterations. Hence, the algorithm RRM plan has atotal computational complexity of O(k2D) in the worst caseand an asymptotic expected complexity of O(k log k exp d+kD log k) when nearest neighbors are restricted to balls asspecified above.

In comparison, the computational complexity of an RRTis O(k2 + kD) for brute-force nearest neighbor searchingand O(k log k exp d + kD) for kd-trees or BBD-trees. Theonly computational complexity overhead of RRM relative

Alg. 4 RRM refine: Expand a RRM by adding edges arounda configuration.Input:

Global variables G = (V,E), Qgoal, prev,counter refine, U , S, and counter from RRM plan

Output:Global variables G = (V,E), U , S, counter refine, and

counter are updated

1 qrefine ← randomly chosen configuration from U2 Qnear ← nearest neighbors(V, qrefine)3 for each qnear ∈ Qnear

4 if (qnear /∈ S or counter refine[qnear]< counter add[qrefine]) and(qnear /∈ Qgoal or qrefine /∈ Qgoal)

5 if prev[qnear] = qrefine

6 RRM mark(qnear)7 else collision free(qrefine, qnear)8 E ← E ∪ {(qrefine, qnear,

cost(qrefine, qnear))}9 RRM mark(qnear)10 endif11 if prev[qrefine] = qnear

12 RRM mark(qrefine)13 else collision free(qnear, qrefine)14 E ← E ∪ {(qnear, qrefine,

cost(qnear, qrefine))}15 RRM mark(qrefine)16 endif17 endif18 endfor19 counter refine[qrefine]← counter ++20 U ← U − {qrefine}21 S ← S ∪ {qrefine}22 return

to RRT is the additional collision detections for the extraedges, which is required for optimal motion planning. Wenote that the worst case complexity of RRM is equivalentto the worst case complexity of RRT* [13] when explicitlyconsidering the cost for collision checks but not countingthe linear complexity of propagating shortest paths duringRRT* iterations that rewire the tree. We also note that thecomputational complexity of a RRM is equivalent to thatof a PRM when brute-force nearest neighbor searching isperformed [14].

D. Discussion

RRM retains the exponential rate exploration properties ofthe RRT algorithm, and the cost of the minimum-cost pathfound by the RRM algorithm converges to the optimal costwith probability 1 as the number of iterations increases. Thisresult is based on the fact that a random geometric graph ina d-dimensional configuration space with n uniformly ran-domly sampled vertices, where each vertex is connected with

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.

all vertices within a distance of dstep = γ(log n/n)(1/d), willresult in a fully-connected graph whenever γ is larger thana certain hard lower threshold γL [2]. The optimal cost pathwill lie in this graph and will be discovered by the RRMalgorithm as the number of iterations approaches infinity. Adetailed proof which considers obstacles in the configurationspace is provided by Karaman and Frazzoli [13]. Relatedmethods have also been used for analyzing PRMs [16].

Just as with RRTs, the effectiveness of RRM for a par-ticular problem is dependent on the parameter dstep. Lowdstep can result in an excessive number of configurationsbeing added to the graph, which slows the method. A largedstep can result in a large number of edges colliding withobstacles. As with RRTs, we can consider a variable dstep

obtained by extending edges dynamically until collision[17], while making sure that dstep is always greater thanγL(logn/n)(1/d) to ensure completeness.

For convergence to optimality, we require that the set ofvertices yet to be refined, U , does not grow at an unboundedrate. For wexplore < 0.5, the expected rate at which config-urations are removed from U when |U | > 0 is greater thanthe expected rate at which configurations are added, thusguaranteeing convergence. For wexplore ≥ 0.5, our resultspresented below look promising but more investigation isneeded to assess optimality guarantees.

Unlike prior methods which implicitly impose a weightingbetween exploration and refinement, RRM allows the userto select the weighting. For wexplore = 1.0, the RRM isequivalent to an RRT and only explores the configurationspace. For wexplore = 0, the RRM is equivalent in the limitto RRG, although RRM does not begin refinement untilan initial solution is found in order to improve algorithmperformance when little computation time is available. Infuture work, we will investigate automatically setting andtuning the weighting. In particular, we will investigate theimpact of narrow passages, which require comparativelymore exploration, on the optimal setting of wexplore. Wewill also consider approaches that compare the decreasein entropy (increase in information) [6] between recentexploration and refinement steps to determine which step ismore likely to decrease entropy.

IV. IMPLEMENTATION AND EVALUATION

We applied the RRM framework to two problems. Thefirst problem, which we describe in Sec. IV-A, is to find apath for a point robot moving on a plane amongst obstacles.We then apply RRM in Sec. IV-B to compute motion plansfor a concentric tube robot capable of following curvedtrajectories, which have the potential to assist physiciansin minimally invasive medical procedures. All experimentswere performed on a 2.2 GHz Intel Core 2 Duo laptop.

A. RRM for a Point Robot

We consider a holonomic point robot that moves on aplane. We define the workspace as a rectangle and defineobstacles as polygons.

(a) wexplore = 1.0

(b) wexplore = 0.8

(c) wexplore = 0.5

400 iterations 800 iterations

Fig. 2. (a) RRM for wexplore = 1.0, where the method is equivalent to anRRT and only explores the space. Although the space is well explored by800 iterations, the resulting path is suboptimal due to the lack of refinementto take advantage of the exploration. (b) RRM for wexplore = 0.8. Themethod explores less of the space than the RRT, but the small amount ofrefinement finds a near-optimal solution by 800 iterations that is homotopicto the true optimal solution. (c) RRM for wexplore = 0.5, where explorationand refinement are balanced and the method converges toward the optimalsolution.

We execute the RRM algorithm for different weights ofwexplore between 0 and 1 and show example runs in Fig.2. In Fig. 3, we show the ratio of the cost of the computedplan relative to ground-truth optimality averaged over 100runs. The results converge toward the optimal solution forlow values of wexplore. Since RRM focuses refinement alongknown pathways to the goal, small amounts of refinement canresult in close to optimal plans even for values of wexplore

as high as 0.9, at the benefit of only spending time refininga small fraction of the sampled vertices.

We also show the number of vertices refined, which iscorrelated to collision detection effort, as well as computationtimes. For an equal number of iterations, decreasing wexplore

requires more computation time since the computational costof a refinement operation is greater than an explorationoperation. Combining the results in the cost ratio graph andthe computation time graph suggests that refinement can beused sparingly (e.g. use a low value of wexplore) to obtainnear optimal solutions at substantially lower computation

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.

1 1.1 1.2 1.3 1.4 1.5 1.6

200 400 600 800 1000 1200

Cos

t rat

io to

opt

imal

Iterations

0 100 200 300 400 500 600

200 400 600 800 1000 1200

Ref

ined

ver

tices

|S|

Iterations

0

0.2

0.4

0.6

0.8

1

200 400 600 800 1000 1200

Com

puta

tion

time

(s)

Iterations

1 0.9 0.8 0.7 0.5 0.2 0

Fig. 3. For the point robot example, we show the ratio of the cost of the computed plan relative to ground-truth optimality for different values of wexplore,averaged over 100 runs. We also show the number of vertices refined, which is correlated to collision detection effort, and the total computation time forroadmap construction and computing shortest paths. By focusing refinement along known pathways to the goal, small amounts of refinement can result inclose to optimal plans and require much less computation time.

cost compared to always refining as is done in RRG andRRT*.

B. RRM for a Concentric Tube Robot

To demonstrate the potential of RRM for higher dimen-sional configuration spaces, we apply RRM to a concentrictube robot. These needle-like robots are composed of nestednitinol tubes and can be controlled to follow curved pathsthrough open air as well as soft tissues [31], [27], [22].These devices have the potential to assist physicians inperforming minimally invasive surgical procedures by ma-neuvering through constrained anatomical spaces to providesurgical access to clinical targets previously inaccessibleusing needle-like devices.

Each tube of the concentric tube robot, which consistsof a straight transmission segment followed by a pre-bentsegment, can be telescoped and axially rotated with respectto the other tubes. A device containing N tubes thus providesthe user with 2N degrees of freedom. As the tubes rotate andtranslate within one another, the global shape of the devicechanges. We compute this shape (i.e. forward kinematics)using a model that considers the stiffness and torsionalenergy of the interacting tubes and minimizes energy [26].

We note that unlike bevel-tip steerable needles that havebeen well studied, the motion of concentric tube robotscannot be modeled with high accuracy solely using thetip pose since the entire shaft shape changes with eachconfiguration change [26]. Prior work on motion planningfor concentric tube robots has not considered these globalshape changes [22].

As a proof-of-concept, we apply RRM to a N = 3-tuberobot as shown in Fig. 4. We consider a tubular environ-ment with protrusions, inspired by what a physician wouldencounter when performing targeted biopsies or localizedradiation cancer treatment in the trachea or bronchi. Weconsider the objective of minimizing robot motion, whichwe quantify by the integral of the distance moved byevenly spaced points along the robot’s shaft. We plan toconsider other objectives, such as maximizing clearance fromobstacles, in future work. We note that this single-querymotion planning problem is not well suited for a PRMsolution due to the highly constrained workspace; over 80%of configuration space samples are in collision due to contactwith the cylinder, and hence are not feasible from the given

Fig. 4. Snapshots from an RRM motion plan for a concentric tube robot.The workspace is represented as a tubular environment with protrusions, aswould arise in anatomical structures such as tracheae and bronchi.

start configuration.We executed the RRM algorithm for 5000 iterations for

wexplore = 1.0, 0.8, and 0.5, which required 33, 61, and 73seconds, respectively. The objective values for wexplore =0.8 and wexplore = 0.5 were both 32.1% better than theRRT equivalent wexplore = 1.0. For 10,000 iterations, theobjective value for the RRT equivalent did not change andthe values for wexplore = 0.8 and wexplore = 0.5 were 39.6%and 40.2% better than the RRT equivalent, respectively.Computation times in seconds increased to 86 for wexplore =1.0, 166 for wexplore = 0.8, and 183 for wexplore = 0.5. Weillustrate snapshots of a plan in Fig. 4. With RRM, physicianswould have the opportunity to use all available computationtime to improve plan quality.

V. CONCLUSION

Computing optimal motion plans requires both exploringthe configuration space to identify free space regions aswell as refining understanding of explored regions in orderto optimize the plan. We presented the rapidly-exploringroadmap (RRM), a new method for single-query optimalmotion planning that allows the user to explicitly consider thetrade-off between exploration and refinement. RRM initiallyexplores the configuration space like an RRT. Once a pathis found, RRM uses a user-specified parameter to weighwhether to (1) explore further to learn the free space, or(2) refine the explored space by adding edges to the currentroadmap to enable finding higher quality paths. Explorationseeks to find new regions of free space while refinementseeks to find better paths within the explored regions.

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.

We demonstrated the performance of RRM and the trade-off between exploration and refinement. In the first example,we applied RRM to plan motions for a point robot movingin a plane and show that properly weighing exploration andrefinement can lead to more quickly finding optimal paths. Inthe second example, we applied RRM to plan motions for aconcentric tube robot whose curved shape can be controlledto reach a target in a constrained space by inserting androtating its constituent tubes.

In future work, we will investigate automatically settingand tuning wexplore based on workspace information andentropy reduction of exploration and refinement steps. Wewill also investigate incorporating new sampling strategiesbased on ideas from the vast PRM/RRT literature. We alsoplan to investigate new applications of single-query optimalmotion planning where RRM can help balance the trade-offbetween exploration and refinement.

VI. ACKNOWLEDGEMENT

This work was supported in part by the US NationalScience Foundation under award IIS-0905344. The authorsthank Nate Dierk for creating visualizations of the concentrictube robots and Lisa Lyons for implementing concentric tubeshape computation.

REFERENCES

[1] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo,“OBPRM: An obstacle-based PRM for 3D workspaces,” in Robotics:The Algorithmic Perspective: 1998 WAFR, P. Agarwal et al., Eds.Natick, MA: AK Peters, Ltd., 1998, pp. 156–168.

[2] M. J. B. Appel and R. P. Russo, “The connectivity of a graph onuniform points on [0, 1]d,” Statistics and Probability Letters, vol. 60,no. 4, pp. 351–357, 2002.

[3] S. Arya, D. M. Mount, N. S. Netanyahu, R. Silverman, and A. Y.Wu, “An optimal algorithm for approximate nearest neighbor searchingfixed dimensions,” Journal of the ACM, vol. 45, no. 6, pp. 891–923,Nov. 1998.

[4] R. Bohlin and L. E. Kavraki, “Path planning using lazy PRM,” in Proc.IEEE Int. Conf. Robotics and Automation (ICRA), 2000, pp. 521–528.

[5] O. Brock and O. Khatib, “Elastic strips: A framework for motiongeneration in human environments,” Int. J. Robotics Research, vol. 21,no. 2, pp. 1031–1052, 2002.

[6] B. Burns and O. Brock, “Single-query entropy-guided path planning,”in Proc. IEEE Int. Conf. Robotics and Automation (ICRA), Apr. 2005,pp. 2124–2129.

[7] ——, “Toward optimal configuration space sampling,” in Proc.Robotics: Science and Systems, Cambridge, MA, June 2005.

[8] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard,L. E. Kavraki, and S. Thrun, Principles of Robot Motion: Theory,Algorithms, and Implementations. MIT Press, 2005.

[9] D. Ferguson and A. Stentz, “Anytime RRTs,” in Proc. IEEE/RSJ Int.Conf. on Intelligent Robots and Systems (IROS), 2006, pp. 5369–5375.

[10] R. Geraerts and M. Overmars, “Sampling and node adding in prob-abilistic roadmap planners,” in Journal of Robotics and AutonomousSystems (RAS), vol. 54, 2006, pp. 165–173.

[11] ——, “Creating high-quality paths for motion planning,” in Int. J.Robotics Research, vol. 26, 2007, pp. 845–863.

[12] D. Hsu, T. Jiang, J. Reif, and Z. Sun, “The bridge test for samplingnarrow passages with probabilistic roadmap planners,” in Proc. IEEEInt. Conf. Robotics and Automation (ICRA), 2003, pp. 4420–4426.

[13] S. Karaman and E. Frazzoli, “Incremental sampling-based algorithmsfor optimal motion planning,” in Proc. Robotics: Science and Systems,2010.

[14] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars, “Proba-bilistic roadmaps for path planning in high dimensional configurationspaces,” IEEE Trans. Robotics and Automation, vol. 12, no. 4, pp.566–580, 1996.

[15] J. Kim, R. Pearce, and N. M. Amato, “Extracting optimal paths fromroadmaps for motion planning,” in Proc. IEEE Int. Conf. Robotics andAutomation (ICRA), 2003, pp. 2424–2429.

[16] A. L. Ladd and L. Kavraki, “Measure theoretic analysis of probabilisticpath planning,” IEEE Trans. Robotics and Automation, vol. 20, no. 2,pp. 229–242, 2004.

[17] S. M. LaValle, Planning Algorithms. Cambridge, U.K.: CambridgeUniversity Press, 2006.

[18] S. M. LaValle and J. J. Kuffner, “Rapidly-exploring random trees:Progress and prospects,” in Algorithmic and Computational Robotics:New Directions, B. R. Donald et al., Eds. Natick, MA: AK Peters,2001, pp. 293–308.

[19] S. M. LaValle and J. James J. Kuffner, “Randomized kinodynamicplanning,” Int. J. Robotics Research, vol. 20, no. 5, pp. 378–400, May2001.

[20] M. Likhachev and D. Ferguson, “Planning long dynamically-feasiblemaneuvers for autonomous vehicles,” Int. J. Robotics Research,vol. 28, no. 8, pp. 933–945, 2009.

[21] M. Likhachev, D. Ferguson, G. Gordon, A. Stentz, and S. Thrun,“Anytime search in dynamic graphs,” Artificial Intelligence Journal,vol. 172, no. 14, pp. 1613–1643, 2008.

[22] L. A. Lyons, R. J. Webster III, and R. Alterovitz, “Planning activecannula configurations through tubular anatomy,” in Proc. IEEE Int.Conf. Robotics and Automation (ICRA), May 2010, pp. 2082–2087.

[23] C. L. Nielsen and L. E. Kavraki, “A two level fuzzy PRM formanipulation planning,” in Proc. IEEE/RSJ Int. Conf. on IntelligentRobots and Systems (IROS), 2000, pp. 1716–1721.

[24] J. H. Reif, “Complexity of the mover’s problem and generalizations,”in 20th Annual IEEE Symposium on Foundations of Computer Science,Oct. 1979, pp. 421–427.

[25] M. Rickert, O. Brock, and A. Knoll, “Balancing exploration andexploitation in motion planning,” in Proc. IEEE Int. Conf. Roboticsand Automation (ICRA), May 2008, pp. 2812–2817.

[26] D. C. Rucker and R. J. Webster III, “Parsimonious evaluation ofconcentric-tube continuum robot equilibrium conformation,” IEEETrans. Biomedical Engineering, vol. 56, no. 9, pp. 2308–2311, Sept.2009.

[27] P. Sears and P. Dupont, “A steerable needle technology using curvedconcentric tubes,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robotsand Systems (IROS), Oct. 2006, pp. 2850–2856.

[28] T. Simeon, J.-P. Laumond, and C. Nissoux, “Visibility-based proba-bilistic roadmaps for motion planning,” Journal of Advanced Robotics,vol. 14, no. 6, pp. 477–494, 2000.

[29] G. Song, S. Thomas, and N. M. Amato, “A general frameworkfor PRM motion planning,” in Proc. IEEE Int. Conf. Robotics andAutomation (ICRA), 2003, pp. 4445–4450.

[30] C. Urmson and R. Simmons, “Approaches for heuristically biasingRRT growth,” in Proc. IEEE/RSJ Int. Conf. on Intelligent Robots andSystems (IROS), 2003.

[31] R. J. Webster III, A. M. Okamura, and N. J. Cowan, “Toward activecannulas: Miniature snake-like surgical robots,” in Proc. IEEE/RSJ Int.Conf. on Intelligent Robots and Systems (IROS), 2006, pp. 2857–2863.

[32] Y. Yang and O. Brock, “Adapting the sampling distribution in PRMplanners based on an approximated medial axis,” in Proc. IEEE Int.Conf. Robotics and Automation (ICRA), 2004, pp. 4405–4410.

[33] A. Yershova and S. M. LaValle, “Improving motion planning algo-rithms by efficient nearest-neighbor searching,” IEEE Trans. Robotics,vol. 23, no. 1, pp. 151–157, Feb. 2007.

CONFIDENTIAL. Limited circulation. For review only.

Preprint submitted to 2011 IEEE International Conference onRobotics and Automation. Received September 15, 2010.