Received December 12, 2013, accepted January 22, 2014, date of publication January 24, 2014, date of current

version February 4, 2014.

Digital Object Identifier 10.1109/ACCESS.2014.2302442

Sampling-Based Robot Motion

Planning: A Review
MOHAMED ELBANHAWI (Student Member, IEEE) AND MILAN SIMIC
School of Aerospace, Mechanical and Manufacturing Engineering, RMIT University, Melbourne 3083, Australia
Corresponding author: M. Elbanhawi ([email protected])
The work of M. Elbanhawi was supported by the Australian Postgraduate Award and the Research Training Scheme.

ABSTRACT Motion planning is a fundamental research area in robotics. Sampling-based methods offer an
efficient solution for what is otherwise a rather challenging dilemma of path planning. Consequently, these
methods have been extended further away from basic robot planning into further difficult scenarios and
diverse applications. A comprehensive survey of the growing body of work in sampling-based planning
is given here. Simulations are executed to evaluate some of the proposed planners and highlight some
of the implementation details that are often left unspecified. An emphasis is placed on contemporary
research directions in this field. We address planners that tackle current issues in robotics. For instance,
real-life kinodynamic planning, optimal planning, replanning in dynamic environments, and planning under
uncertainty are discussed. The aim of this paper is to survey the state of the art in motion planning and to
assess selected planners, examine implementation details and above all shed a light on the current challenges
in motion planning and the promising approaches that will potentially overcome those problems.

INDEX TERMS Planning, sampling, randomization, RRT, PRM, path, motion, autonomous robots.

I. INTRODUCTION cleaning mobile robots, planetary rovers, rescue robots and

Autonomous robots are characterized by their ability to exe- many more.
cute tasks deprived of any human intervention. Decision
making requires full or partial knowledge of the surrounding A. PATH PLANNING
environment, or workspace, in which the agent is operating. Planning is not only one of the fundamental problems in
Recent advances in sensor technology have enabled the use of robotics [13]–[15], it is perhaps the most studied [16].
reliable multisensory perception systems [1]–[5]. Uncertainty Early efforts to develop deterministic planning techniques
in the perception stage leads to accumulated localization showed that it is computationally demanding even for sim-
errors [6]. Processing of collected data and accounting for ple systems [17]. Exact roadmap methods such as visibility
errors is essential for accurate mapping and localization [7]. graphs [18]–[20], Voronoi diagrams [21], [22], Delaunay
The planning stage involves devising a collision free strat- triangulation [23], adaptive roadmaps [24] attempt to capture
egy from the current location, or configuration, to a desired the connectivity of the robot search space. Cell decomposition
goal location, or configuration. The current configuration methods, in which the workspace is subdivided into small
is estimated in the localization stage whereas a behavioral cells, have been applied in robotics [25]. Search algorithms
planner can provide a goal configuration [8], [9] (the notion such as Dijkstra [26] and A* [27] find an optimal solution
of a configuration will be discussed later). Path planning in a connectivity graph, whereas D* [28] and AD* [29] are
is a purely geometric process that is only concerned with tailored to dynamic graphs. The use of graph search methods
finding a collision free path regardless of the feasibility of the involves discretization of the workspace and their perfor-
path. Kinodynamic planning, on the other hand, considers the mance degrades in high dimensions. The work in [30]–[32],
kinematics and dynamics of the robot. Once a path is specified generates state lattices using motion primitives and combines
the final procedure is motion control or execution [10]–[12]. them with graph search algorithms but, it still suffers from
The full potential of autonomous vehicles is yet to be fully undesirable discretization. Efficient discretization can be
exploited in enriching human lives. Nevertheless there exist achieved on the expense of completeness and high-resolution
several applications such as self-driving cars, forklifts, mining discretization is computationally expensive. The emergence
trucks, unmanned aerial vehicles (UAV), military drones, of novel computational methods inspired their use in path
2169-3536
2014 IEEE. Translations and content mining are permitted for academic research only.
Personal use is also permitted, but republication/redistribution requires IEEE permission.
56 See http://www.ieee.org/publications_standards/publications/rights/index.html for more information. VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

planning. Methods such as Fuzzy Logic Control [33]–[35], sometimes referred to as forests, as an analogy to trees in
Neural Networks [36], Genetic Algorithms [37], [38], Ant RRT. As a result of maintaining the roadmap and specifying
Colony Optimization [39] and Simulated Annealing [20] have start and goal configurations in a subsequent stage, PRM is
all been applied in robot path planning. Khatib [40] proposed able to solve different instances of the problem in the same
a potential field method such that artificial forces repelled the environment. It is referred to as a multi-query planner. Plan-
robot away from the obstacles and attracted it towards the goal ning time is invested in sampling and generating a roadmap
position. Potential fields were also applied for mobile robots so that queries are solved quickly. Initially developed for
in [41], however they suffered from falling into local minima articulated robots [53]–[55]. PRM has been extended for non-
and performed poorly in narrow regions [42]. Sensor based holonomic car-like robots [57]. It was shown that PRM is
reactive planning methods have been proposed [43]–[45]. probabilistically complete [58], [59].
Control based methods require formulating accurate models RRT represents another category of sampling based plan-
for the robot and the environment [46], [47], which can be a ners, which are single-query planners. A tree is incrementally
rather daunting task. grown from the start configuration to the goal configuration,
or vice versa. A configuration is randomly selected in the
B. RANDOMIZATION AND SAMPLING IN ROBOT configuration space. If it lies in the free space, a connection
PLANNING is attempted to the nearest vertex in the tree. For single query
Sampling based planning (SBP) is unique in the fact that plan- problems, RRT is faster compared to PRM. It does not need
ning occurs by sampling the configuration space (C-space). to sample the configuration space and construct a roadmap
In a sense SBP attempt to capture the connectivity of the i.e. learning phase. RRT was shown to be probabilistically
C-space by sampling it. This randomized approach has its complete [60].
advantages in terms of providing fast solutions for difficult Expansiveness was proposed as a measure of the number
problems. The downside is that the solutions are widely of neighboring nodes to any nodes [61]. It is used as an
regarded as suboptimal. Sampling based planners are not indication whether a node will be useful in expanding the
guaranteed to find a solution if one exists, a property that is search tree. Expansive space trees (EST) were developed
referred to as completeness. They ensure a weaker notion of based on that proposed measure. Unlike RRT where sampling
completeness that is probabilistic completeness. A solution is uniform [56], EST employs a function that sets the proba-
will be provided, if one exists, given sufficient runtime of the bility of node selection based on neighboring nodes.
algorithm (in some cases infinite runtime). Ariadne’s clew is planner that builds a search tree [62],
Sampling based planning is by no means a novel concept in similar to EST and RRT, to explore the configuration space.
robotics [48]. It was proposed to overcome the complexity of The difference in this algorithm is the connection of the
deterministic robot planning algorithms for a robot with six randomly selected node. It attempts to connect a node that
degrees of freedom. The use of random computations to solve is furthest from existing nodes. This heuristic is employed
otherwise rather difficult problems, have been immensely to increase the exploration rate of the algorithm. Unlike
successful [49], [50]. Both sampling based planners and the RRT where the implementation is intuitive by connecting the
success of random computations inspired the development closest node, a genetic algorithm is used to select the node for
of the Randomized Potential Planner (RPP) [51]. RPP used expansion [62].
random walks to escape local minima of the potential field Sampling based planners have been successfully
planner. Later on, a planner based entirely on random walks, implemented in a variety of fields aside from robotic appli-
with adaptive parameters, was proposed [52]. cations. This is a testament to the generality of the pro-
The work of Barraquand and Latombe [51] paved the way posed algorithms and their ability to solve difficult and
for a new generation of motion planning algorithms that constrained problems. Interestingly, the two fields in which
employ randomization. Some of these planners are listed sampling based planning is used are digital animation and
in Table 1. Perhaps the most commonly used algorithms computational biology [16]. In digital animation, agents are
are Probabilistic Roadmap Method (PRM) [53]–[55] and constructed out of triangular meshes and paths are planned
Rapidly-exploring Random Trees (RRT) [56]. Several other using sampling based planners such as RRT [63] or PRM [64].
algorithms were developed at the same time that outper- A Gaussian-process based Spline-RRT was used to guide a
formed RPP. The intuitive implementation of both RRT and UAV to explore an unknown environment [65]. In computa-
PRM, and the quality of the solutions, lead to their widespread tional biology, molecules and proteins are modeled as articu-
adoption in robotics and many other fields. lated bodies and sampling based planners are used to simulate
PRM implements two main procedures to generate a prob- protein folding and protein-ligand interactions [66], [67].
abilistic roadmap. A learning phase occurs first, where the EST was used in architectural design to evaluate accessibility
C-space is sampled for a certain amount of time. The samples, of constrained and narrow areas [68]. Medical needles [69]
or configurations in the free space, are maintained while and, deformable objects [70] sampling based motion plan-
those in the obstacle space are discarded. This is followed ning frameworks, have been developed. Several researchers
by a query phase where the start and goal configurations investigated the use of RRT in non-linear control applications
are defined and connected to the roadmap. Roadmaps are such as pendulum control [71]. Apart from simulation based

VOLUME 2, 2014 57
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

planning, the first real life applications were reported in solutions and performances of sampling based planners
multi-robot competitive dynamic environments [72]. Ever are presented and some are evaluated using simulations in
since, welding multi-degree of freedom (DOF) robots [73], segment III. We present the problem of kinodynamic planning
industrial robots [74], domestic robots [75], [76] and in segment IV. Optimal planning algorithms are presented
urban self-driving vehicles have used sampling based plan- and evaluated in segment V. The problem of planning under
ning [77], [78]. uncertainty in dynamic environments is then presented in
segment VI. The study is finally concluded in segment VII.
TABLE 1. Main SBP algorithms.
II. SAMPLING BASED PLANNING OVERVIEW
SBP is treated as a black box that returns a feasible, collision
free path once information about the start and goal config-
urations is provided, as shown in Fig. 1. In a hierarchical
overview of motion planning for autonomous robots, SBP lies
between a high-level behavioral planner that specifies global
goals and a low-level controller that plans the execution of
path.

C. CONTRIBUTION
It must be stated that sampling based planning reviews exist
in literature. Both surveys [66], [67] focus on RRT and
PRM for computational biology and physics-based simula-
tion and modeling. The review papers [16], [79] and the
FIGURE 1. A general sampling based planner.
survey by Tsianos, et al. [80] are considered outdated.
A significant body of work exists after their publication.
Researchers have since evaluated some of the claims and open
research questions. Recommendation for planners implemen- A. PROBLEM DEFINITION
tation are proposed [81] and a benchmarking software is In order to define the motion planning problem some concepts
presented [82] but they do not survey recent research in must be introduced. SBP operate, mostly, in the configuration
the area and present only a handful of planners. Recently, space (C-space). It is the space of all possible transformations
LaValle [83], [84] published outstanding tutorials, which, by that could be applied to a robot. Lozano-Perez [85] introduced
no means, can be considered reviews. the concept of C-space planning to simplify complex planning
In this study we present a survey on state of the art sam- scenarios in the workspace of the robot. Free space, Cfree, and
pling based planners and their applications. The planners are obstacle space, Cobs , are the two regions within the C-space,
decomposed into different primitives and then differences C. This prevents the need to explicitly define obstacles. The
and similarities between planner’s primitives are exposed. We robot can be only represented by a configuration, q, at any
review some of the parameters for selected sampling based instance. The configuration, q, has equal dimensions as the
planners and, optimal planners, and provide recommenda- C-space. Common terminology to describe configurations,
tions for implementation. This highlights the importance of such as nodes, samples, or landmarks, will be used inter-
parameters and heuristics in sampling based planners and changeably in this study. A sequence of consecutively con-
evaluates some of the claims made by researchers. A particu- nected configurations represents a path, P.
lar emphasis of this research are recent direction in planning Start, qstart , and goal, qgoal , configurations are the inputs
such as optimal planning, real time kinodynamic planning and to the motion planner. The problem is to find a collision free
planning (replanning) in dynamic environments and under path, Pfree , which connects qstart to qgoal A path is considered
uncertainty. free if its entire configurations lie in Cfree and their connecting
We have introduced the paradigm of robotic planning and paths do not intersect Cobs .
highlighted some of the important classical, sensor-based,
control based and sampling based planners in section I. The B. PRIMITIVES
remainder of this paper is arranged as follows, section II an It is essential to introduce the constitutions of any SBP
overview of sampling based planners and a formal description algorithm prior to introducing the different planners. Even
of the planning problem are provided. Methods to improve though these primitives are found in most planners, their

58 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

implementation differentiates the planner. Variants of each of lision free qnew is added to the tree as shown in, as show
these primitives will be thoroughly discussed in section III in Fig. 2(d).
along with their effect on the performance of the planner. • The search terminates when qnew = qgoal , a number
1) Sampling: This procedure is used to select a configura- of iterations is exceeded or a specified time period are
tion, randomly, or quasi-randomly, and add it to the tree exceeded.
or roadmap. As mentioned earlier, the samples can be
either in the free, or obstacle configuration space. It can
be considered as the core of the planner and the main
advantage of SBP over other techniques.
2) Metric: Given two configurations qa and qb , this pro-
cedure returns a value, or cost, that signifies the effort
required to reach qb from qa . It is important that it is
truly representative of the effort, or time-to-go between
both configurations. Otherwise highly suboptimal solu-
tions will be returned.
3) Nearest Neighbor (NN): It is a search algorithm that FIGURE 2. The procedure of extending RRT.
returns that closest node(s) to the new sample. The
value is based on the predefined metric function. Some
papers refer to it as proximity search or near vertices.
4) Select Parent: This procedure selects an existing node
to connect to newly sampled node. That existing node
is considered parent. RRT selects the nearest node as
the parent. PRM connects the sample to several nodes
within its neighborhood. On the other hand, EST selects
a parent node to randomly extend based on its neigh-
boring nodes. Ariadne’s clew selects a parent node for
extension using a genetic algorithm.
5) Local planning: Given two configurations qa and
qb , this procedure attempts to establish a connection
between them. It is intuitive to employ straight-line FIGURE 3. RRT exploring free space (right) and environment with one
paths. For most robotic systems this is not a feasible obstacle (left) after 500 iterations. The root of the trees in both cases is
shown as green bold green circle, in top right corner (left) and center
plan due to kinematic or dynamic constraints. (right).
6) Collision checking (CC): It is mostly a Boolean func-
tion that returns success, or failure, when connecting The ability of RRT to explore free space in presence and
two configurations. A connection is successful, if it absence of obstacles is illustrated in Fig. 3. This property is
does not intersect Cobs . often referred to as the Voronoi bias of RRT. As a result of
uniform sampling, the planner is more likely to select samples
C. ALGORITHMS
in larger Voronoi regions and the tree is incrementally and
Algorithms for PRM and RRT are presented here as intro- rapidly grown towards that free space.
duced in [55] and [56]. They are the main algorithms used
in SBP. It must be noted that configurations may be referred 2) PRM
to, using common SBP literature terminology, as nodes or
Firstly, a roadmap is built in the learning phase,
milestones, throughout this study.
• A node, qrand , is selected from the C-space using sample
1) RRT procedure.
• The search is initialized from qstart . • qrand is discarded, if it is in Cobs .
• A node, qrand , is selected from the C-space using the • Otherwise, qrand is added to the roadmap.
sample procedure, as shown in Fig. 2(a). • Find all nodes within a specific range to qrand
• qrand is discarded, if it is in Cobs . • Attempt to connect all neighboring nodes using local
• Using Nearest Neighbor search qnear is returned accord- planner to qrand.
ing to the metric, as shown in Fig. 2(b). • Check for collision and disconnect colliding paths
• The local planner is used to connect qrand and qnear . The • This process is repeated until a certain number of nodes
planner may return qnew · qrand may not be reachable, as have been sampled.
shown in Fig. 2(c). If qrand is not reached, it is discarded. A typical roadmap, built in the learning phase, is shown in
• Collision checking is performed to ensure the path Fig. 4. In the query phase the start and goal configurations are
between qnear and qnew is collision free. If path is col- connected to the roadmap. A graph search algorithm is then

VOLUME 2, 2014 59
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

used to find the shortest path through the roadmap between tion of a roadmap that fully captures the shape of the
start and goal configurations. C-space [88]–[90].
• Boundary: Forcing sampling towards the boundary of
obstacles, as opposed to free space, was proposed
in [54].
• Gaussian: Similar to boundary sampling, this strategy
increases the probability of sampling around obstacles.
Nodes are expanded using an adaptive probability based
on obstacle and collision data [91].
• Bridge-test: This overcomes the weakness of SBP in
narrow regions. The strategy uses a short segment with
two configurations and their midpoint [92]. If the two
ends are in Cobs and their midpoint is in Cfree then a
narrow region has been identified.
• Hybrid: Combining two sampling strategies, narrow pas-
sage (bridge-test), and uniform sampling. This lead to an
increase density in narrow regions and still maintaining
randomization which is advantageous in solving difficult
problems [93]. Medial axis and narrow sampling are
FIGURE 4. Roadmap built in the PRM learning phase. combined to better capture the environment connectiv-
ity [94].
• Visibility PRM [95]: A non-uniform sampling method.
III. PARAMETERS AND HEURISTICS Sampling is performed in visibility regions. It decreases
Sampling based planners consist of a number of primitives the number of nodes maintained in the roadmap while
with varying parameters. A significant portion of research in maintaining the same coverage.
SBP is dedicated to designing algorithms with smart heuris- • Goal Biasing: It may not be considered as a sampling
tics and parameters. strategy however biasing is mentioned here as it is used
The aim of these improvements is generally twofold, reduc- to replace sampling strategy for an interval at some
ing algorithm run time and cost of solutions. In this section planning stage. Biasing attempt to greedily connect the
SBP variants are categorized and surveyed. SBP are rather goal configuration to the current tree [96]. Biasing is
sensitive to their implementation and some emphasis must recommended, between 1-10 every 100th iteration, to
be placed to selecting the correct parameters [86]. Sucan maintain randomization in sampling [13], [84].
and Kavraki [81] highlight the importance of parameters and
argue that the implementation details are often not men- The effect of sampling on the performance of SBP is still
tioned when SBP are presented. Motivated by the reliance an open research question. The experimental results presented
of RRT on heuristics, Randomized Statistical Path Planning by Lindemann and LaValle [79], Geraerts and Overmars [86],
(RSPP) applies machine learning to actively adjust the plan- [97] show that sampling has no effect on the performance
ners parameters while the algorithm is running [87]. In this of planners. It also shows that there is no single sampling
section, a number of implementations and parameters are strategy that outperforms the others in every scenario.
tested using simulations in various scenarios.
B. GUIDING THE EXPLORATION
A. SAMPLING STRATEGIES The motivation behind the attempts to guide the search is that
Sampling is the core of the SBP. It is the process through RRT expansion is more prone to fail if the node is near and
which the planner is able to extend and explore the C-space. obstacle (boundary node). A simple approach is to attempt
Initially, PRM and RRT were proposed with uniform sam- to limit the sampling domain to the visibility region, which
pling schemes [55]–[57]. This can be considered as a draw- is difficult to compute. Dynamic-Domain RRT (DD-RRT)
back because the planner has a high probability of sampling a limits the sampling domain of boundary nodes to a small ball
node from a wide region unlike a narrow free region. This is a of a predetermined radius as an alternative to the visibility
result of all configurations have uniform probability of being region [98]. Adaptive Dynamic Domain RRT (ADD-RRT)
sampled and narrow regions have less free configurations. limits the domain to a ball, whose radius changes according
Another drawback of uniform sampling is not capturing the to the extension success rate of each boundary node [99].
true connectivity of the environment. The following sampling Unlike ADD-RRT and DD-RRT, Utility-RRT influences
strategies have been suggested as means to overcome those the direction and length of extension, not the sampling
shortcomings, domain. A utility function evaluates the direction of expan-
• Medial axis: Sampling probability is increased around sion and the selected node [100]. Utility functions are com-
the medial axis (Voronoi graph) to guide the genera- puted based on the success rate of the node and previous

60 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

direction of expansion. Obstacle Based RRT (OB-RRT) gath- Non-holonomic vehicles such as car-like ground robots
ers data from obstacles and selects predetermined growth or UAV with upper-bounded curvature are common robotic
directions [101]. Utility-RRT outperforms both ADD-RRT platforms. Euclidian metric is a poor choice for those vehi-
and RRT [100], OB-RRT has only been benchmarked against cles since two configurations that are physically close may
RRT. OB-RRT relies on obstacles models consisting of trian- require complex maneuvering to reach (see subsection III-G
gles. No discussion is provided whether this method would for discussion on local planning). Calculating the true cost
extend to other representations. involves expensive computations which is infeasible given
A novel categorization divides motion planners into the frequency of the metric function usage during planning.
exploring and exploiting planners [102]. SBP presented SRRT uses a Euclidian distance to calculate the closest
here perform guided exploration. On the other hand, k-neighbors, where k is a positive integer, and then connect to
artificial potential field algorithms and wave front decom- the one with the smaller real distance [104]. Another approach
position [103] exhibit purely exploitive behavior. Explor- overestimates the distance when the Euclidian distance is less
ing/exploiting tree (EET) balances both behaviors based on than the minimum turning radius, indicating that a complex
successful expansion of the tree [102]. It attempts to use maneuver might be needed [113]. Manipulability was pro-
purely exploitive behavior to provide fast solutions for sub- posed as a metric for articulated robots to signify the ease by
problems and leverages exploring behavior of SBP when the which the robot can reach a certain configurations, especially
planner fails. that articulated have redundant configurations [114].
Several adaptive sampling strategies have been proposed. As a substitute for purely relying on a distance metric
Significant reduction in planning time for a non-holonomic to select the suitable node for expansion, the failure rate of
UAV is achieved by increasing the density of sampling around previous node expansions is factored in the selection metric,
the goal region once the tree approaches it [104]. A high an approach, that is often referred to as Resolution Complete
level planner modifies the sampling domain to influence the RRT (RC-RRT) [115], [116] and was adopted in [117]. This
behavior of a self-driving car by manipulating the Closed prevents wasting planning time on nodes that are bound to
Loop RRT (CL-RRT) growth [77]. An estimation model pre- fail simply because of their low metric value. RRT-Blossom
dicts the probability of a sample, to optimize the solution and choses an expansion node similarly [118]. However it pro-
adapts the sampling strategy accordingly, to direct the search ceeds to expand the node in all directions and removes nodes
towards lower cost regions [105]. Collision information is that are close to nodes already in the tree. This approach has
used to adapt sampling when building a roadmap in real a drawback of discretizing the control space, which is one of
time [106]. the strengths of RRT, as it operates in a continuous space.
Discretizing the control space has been shown to improve
C. METRICS planning for some nonlinear systems [119], [120]. It is yet to
Metrics are used to indicate the cost or time to go between be evaluated for differentially constrained robotic planning.
two configurations. PRM and RRT are reliant on metrics The costs that arise between two configurations simply
for extending their search. Choosing an accurate metric is account for the effort needed to drive the robot from one to
arguably as difficult as the motion problem itself [13]. It is of the other. All the previously mentioned approaches assume a
the utmost importance that metrics provide a good estimation, uniform cost C-spaces, aside from heuristic method presented
not necessarily exact, of the cost between two configurations. in [121]. Non-uniform costs are used to signify non-uniform
Metrics can be called multiple times during the planning pro- rough terrain [122], estimated uncertainty [123], or can be
cedure so it must be easily computed. A theoretical analysis user defined to bias the plan towards preferred regions [124].
of path quality measures in a plane is presented in [107]. Transition-RRT (T-RRT) [125] were proposed to handle non-
EST and Guided Expansive Space Trees (GEST) [108] uniform cost C-space, referred to as cost maps. It provides
select nodes for expansion based on their neighboring an adaptive criterion, referred to as transition test, which
nodes. Path Directed Subdivision Trees (PDST) [109] and prevents transitioning into costly regions based on the cost
KPIECE1 [110] select nodes for expansion based on their differences between parent and child nodes.
coverage, to ensure that expansion is not wasted on already
explored areas. These planners reduce their dependency on D. COLLISION CHECKING
metrics. One of the main properties of SBP is that obstacles in
Amato, et al. [111] experimentally studied the effect of dif- the environment are not explicitly defined. Planning gen-
ferent metrics on PRM and reported that the best performance erally takes place in the C-space, which is separated into
was obtained by using a weighed Euclidian metric. This Cfree and Cobs . This approach requires a module, which
metric accounted for rotation as well as linear Euclidian dis- provides information on whether a path collides with any
tance. Similarly, accounting for rotation using Euler angles, or obstacle. Since the goal of SBP is to create collision free
Quaternions, proved to be advantageous when planning with paths in the C-space, it stands to reason that collision
RRT in 3D [112]. checking (CC) will be called several times during plan-
ning. Some experiments show that more than 90% of plan-
1 Kinodynamic Planning by Interior Exterior Cell Exploration (KPIECE) ning time is spent processing CC queries [93]. It can be

VOLUME 2, 2014 61
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

noticed by from any SBL that most connections are collision parts, shown in Fig. 6(b). If the midpoint is free, the midpoints
free. of the two parts are checked. This process is continued until
Several planners use CC as a feedback mechanism to guide a certain resolution is reached.
the search [72], [98], [99], [108], [115] adapt the sampling
strategy [105], [106], or improve the connectivity of the envi-
ronment [92], [93], [126]. Proximity Query Package (PQP) is
commonly using for CC [127]. An experimental comparative
analysis shows that other packages outperform PQP [128]. FIGURE 6. Red arrows connote a CC query between two configurations
Lazy planning algorithms have been proposed to delay that are connected by the black solid line (a) Naive incremental collision
collision checking until it’s needed [129]–[131]. These algo- checking (b) SBL midpoint collision checking.

rithms will check the collision only once a path is found.

Once collision is detected the colliding segment is removed
and planning is continued. Another approach is to decrease E. HEURISTICS
the reliance of expensive CC. The distances between free In this section we introduce some methods that have been
configuration and Cobs are maintained and similarly obstacle shown to refine the solution cost or planning time of SBP.
configurations and Cfree . These distances are used to infer It must be noted that there are no theoretical guarantees
whether a new configuration or, new path segment is colliding to those claims. However, these planners have been shown
and decrease the reliance on CC [132]. to work well in various situations. We will provide some
Single-query Bidirectional Lazy (SBL) is a planner that not discussions about the strengths and shortcoming of those
only delays planning but it also performs CC in regions that tactics.
are more likely to collide [133]. The CC algorithm in SBL is RRT-Connect [96] and SBL [133] use two trees to perform
based on four observations: bidirectional search. One tree is rooted at the start, whereas
1) A small fraction of all samples is in the final path the other is at the goal. The search is complete when the
(around 0.1%), two trees are connected. This approach provides significant
2) Incrementally checking the path is computationally improvements in the search efficiency, which is illustrated
expensive, especially when no collision is detected, as in Fig. 7. Triple RRT [134] generates two trees from start
the entire path must be checked, and goal configurations and one tree from a narrow region
3) Short connections are more likely to be collision free which is identified using the bridge test. Similarly, Multiple
between two configurations in Cfree , as shown in RRT are generated from all narrow regions, in the free space
Fig. 5(b), that are identified using the bridge test [135]. A problem
4) Collision is more likely to be in the midpoint between arises when attempting to connect two trees for differentially
two configurations, as shown in Fig. 5. constrained systems where the local planning is not a simple
straight line, resulting in what is known as a boundary valued
problem [136]. Methods to overcome this problem will be
discussed in section IV.

FIGURE 7. Unidirectional search coverage area (left) and bidirectional

search (right). The search is started from the diamond shaped
configuration and the final configuration is circle-shaped. Employing two
FIGURE 5. Illustration of the observations made by Sánchez and Latombe search trees is more effective since less area is searched to find the
[133]. (a) The midpoint of a colliding path between two free solution.
configurations is more likely to be in Cobs (b) It is difficult to have a
colliding path between two free configurations that are separated by a It can be seen that using NN search to connect the sampled
short distance. The collision is still more likely to be towards the midpoint
of the short line. node to the nearest node does not necessarily improve the path
cost. A k-near RRT employs NN search to find the nearest
A collision checking algorithm is employed by SBL based k nodes, where k is a positive integer [121]. The path is
on the observations made in [133]. Naive CC is performing evaluated for all the k-nearest nodes and the node with the
incremental checking at some interval from one end to the best solution is connected to improve the overall solution. The
other along a path, shown in Fig. 6(a). SBL checks the mid- drawback of this approach is computational overhead as, NN
point between two configurations dividing the path into two search is called, and metrics are evaluated more frequently.

62 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

An alternative to relying on NN search is evaluating the path

towards a candidate node with all nodes in the tree [137].
Anytime RRT deals with lack of computational time for
path improvement by generating an initial suboptimal solu-
tion [138]. The tree is then stored and the rest of the time is
used to attempt to improve every solution by a predetermined
bound (generally 5-10%). This is achieved by applying a node
selection strategy. If the underestimated, lower-bound, path
cost through the candidate node is less than the current path
cost it is deemed ‘‘promising’’ and added to the tree. Way-
point caches were originally proposed for real-time planning
are also used to guide replanning with anytime RRT [139].
It is explicitly remarked that Anytime RRTs improve the path FIGURE 8. An illustration of post processing. The original path is highly
within the given planning time, however they provide no guar- suboptimal (grey thin line). Redundant nodes are removed and the rest
are connected to provide a shortcut path (red dotted line). Smoothing
antees on reaching an optimal solution under certain criteria techniques are then employed to fit a curve through the short path (black
and time constraints. This property is known as asymptotic thick line).
optimality and will be discussed in section V.
Post processing, as is the case with any SBP stage, is
F. POST PROCESSING limited by an amount of time. Alternating between hybridiza-
A major drawback of SBP is their widely regarded suboptimal tion and smoothing within the given time-frame have been
paths. This is as a result of the arbitrary approach used in shown to be effective and computationally efficient [151].
sampling and heuristics that are employed to speed up the Path Deformation Roadmap (PDR) extends on the notion
search. Whereas some methods attempt to guide to improve of Visibility PRM by removing redundant paths that can
the path quality during the search process [121], [138], the be deformed into other existing paths [152]. Maintaining a
algorithms in this section proceed to smooth and modify the compact deformable roadmap facilitates post processing as
path after planning is complete. Post processing is illustrated various paths between two roadmaps can be easily obtained.
in Fig. 8. The original path is shown as a thin line, the dotted Regardless of the effectiveness of these approaches, post
line is the trimmed path, and finally the bold line shows the processing does not regulate the impractical attempts to
smooth curved path. expand nodes towards suboptimal regions. It only proceeds to
Simply inspecting subsequent nodes and removing redun- optimize the path at a later stage. Planning time is wasted in
dant nodes achieve path shortcutting, or tree pruning. An effi- both the search and the optimization stages. A more efficient
cient algorithm removes redundant nodes in one dimension strategy would be to explicitly consider path quality during
at a time and provides some clearance by moving the path planning.
towards the medial axis [140].
Smoothing techniques rely on using a curve to interpo- G. LOCAL PLANNING
late or fit the given waypoints. These methods are not lim- Steering functions are employed to connect configurations,
ited to SBP but have been used in various scenarios and or landmarks, in SBP. Intuitively, a straight line joining both
with planners. Methods such as cubic polynomials [141], configurations may be proposed. In the case of differentially
quintic polynomials [142], [143], Bezier curves [144]–[147], constrained robots, or non-holonomic robots this may not
B-splines [20] and Clothoids [148] have been all applied for be feasible. A viable approach is to model the robot system
path smoothing. An study shows that Bezier and B-splines are and sample the control space for a certain period of time.
well suited for robotic planning and B-splines were shown to However, it must be noted that a tradeoff exists between
be more effective in replanning situations in dynamic envi- computational efficiency and accuracy when using numerical
ronments [149]. integration. Kinematic model for a car-like vehicle is often
Hybridization graphs (H-graphs) are constructed by coa- represented using the bicycle model as follows in Eqn. (1),
lescing multiple RRTs and attempting to optimize the solu- where x and y are the vehicle coordinates, θ is it’s orientation,
tion [150]. This work is based on the observation that RRTs ∅ is the steering angle, v is the linear velocity and L is the
are globally suboptimal, conversely some local optimality distance between the front and back wheels, as illustrated
exists. It is hoped that the locally optimal components of in Fig. 9. Non-holonomic planning is a thriving area of
different trees can be combined to achieve global optimality. research [153], which can be combined with SBP to provide
Hybridization is used with trees generated using the same effective planning techniques.
planner. No studies have been performed on the effect of
cos θ
     
using trees generated with different parameters. The effect of ẋ 0
having a portion of trees rooted at the start, others at the goal  ẏ   sin θ  0
 =  ˙ (1)
 θ̇   tan θ  v +  0  ∅
 
and utilizing bidirectional trees are prospects, which are yet L
to be investigated within the hybridization framework. ∅˙ 0 1

VOLUME 2, 2014 63
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

Dubin’s path [154] and Reeds and Shepp’s [155] are com- governed by some rules to ensure dynamic feasibility. Atlas
monly used for non-holonomic vehicles that are bound by a RRT [171] projects the highly constrained C-space manifold
minimum turning radius [57]. They combine circular arcs and into overlapping charts which are contained within an atlas to
straight lines to generate optimal paths, however the curvature overcome the complexity of C-space introduced by kinematic
of the path may not be continuous. Curvature continuous constraints.
paths were proposed using Clothoids [148], [156]. Clothoids
have no closed form representation and thus provide com- I. IMPLEMENTATION ENHANCEMENTS
putational challenges to synthesize in real time [157]–[161]. It can be argued that most of the research on SBP is focused on
Bezier curves were proposed for smoothing [144], [145] theoretical aspects and implementation details are often left
and then they were used for local planning in SRRT [65], out of the discussion [81]. SBP parameters have been shown
[104]. B-splines were proposed for planning and replanning to have a significant effect on their results [86]. Statistical
in dynamic and unknown environments [149]. learning has been used to adaptively adjust parameters [87].
An open-source library has been developed as a common
benchmarking tool that limits the effect of implementation
parameters [82]. Kd-trees have been used to improve the
efficiency of NN-search [172]. Taking advantage of powerful
CPUs by parallel processing and running multiple searches
have been shown to be effective [164], [173], [174].

J. EXPERIMENTS
In order to illustrate the significance of SBP parameters on
the results obtained several experiments are presented here.

1) EXPERIMENTAL SETUP
FIGURE 9. Illustration of bicycle model of car-like vehicle rotating, the RRTs are used to solve single queries two-dimensional envi-
instantaneous center of rotation (ICR) is shown as circular node.
ronments. We highlight the effect of several implementation
parameters such as step size used for extending the RRT, the
In order to improve the connectivity of PRM roadmap
percentage of biasing, k values in k-RRT, bidirectional search.
Delaunay triangulation have been used for local plan-
We also test some of the observations used for lazy CC in
ning [162]. Toggle PRM initially implements a straight line
SBL.
connection. If connections fails, it attempts to establish a
RRT rely on random sampling. As a result, running the
connection from the same node in different directions [126].
same algorithm with the same parameters will produce dif-
PRM is combined with RRT or EST as local planners to
ferent solutions. Some solutions can be near optimal, lucky,
take advantage of both planners’ strengths in solving com-
while others may be grossly suboptimal, pathological cases.
plex queries [163]. PRM samples milestones and maintains
Both cases are shown in Fig. 10. There are three obstacles
roadmap while single-query motion planner attempts to con-
in the environment shown as grey boxes. The goal region is
nect milestones. The planner, formalized as Sampling-Based
highlighted as the green box, the path is shown in red and the
Roadmap of Trees (SRT), was shown to be more efficient than
RRT is shown as black lines.
using a standalone PRM, RRT or EST [164].

H. SIMPLIFYING THE PLANNING PROBLEM

It is often useful to limit the search space dimensions as a
means to facilitate the planning process. Motion primitives
are often used for highly redundant robots. These robots
can solve a single query, i.e. reach a pose, in a various
configurations [165], [166]. Certain planning dimensions
are disregarded by constraining the motion of the robot to
a specific manifold or moving the planning problem into
a lower dimensional space that is more relevant the FIGURE 10. Lucky (left) and pathological (right) solutions obtained by
task [76], [167]–[169]. running the same algorithm without changing any parameter. This
environment is referred to as ‘‘narrow’’ in this study.
Maneuver based planning was proposed, in which sta-
ble trim-trajectories are known a priori and used to con-
nect nodes [137]. The concept of maneuver based planning Several measures have been put in place to ensure that
have been extended into Maneuver Automata, as alternative the presented results are truly reflective of parameter effects.
to optimal control methods [170]. They consist of a finite Firstly, any experiment is looped for 54 runs, the best and
set of interconnected motion primitives; the connections are worst two results are then omitted, and the remaining 50 are

64 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

FIGURE 13. Effect of goal biasing on the path cost of the number of
nodes explored before a solution is found, which is an indication of the
algorithm run time.
FIGURE 11. Trap environment is shown on the left and cluttered is shown
on the right.

vides more consistent improvements across all environments.

As previously mentioned, the main drawback of bidirectional
RRT is the subsequent BVP, for differentially constrained
systems, when attempting to connect two trees.

FIGURE 12. Effect of changing the step size of RRT extend on the path
cost, a minor implementation detail that has a large effect on the
performance.

FIGURE 14. Comparison between RRT, Biased RRT and Bidirectional RRT
averaged. All experiments are run on three environments, explored nodes before finding a solution.
each with its own challenges. Environment dimensions are
100x100 in all cases, obstacles are grey objects, goal region
is a green box, RRT is shown in black, and the final path
is highlighted in red. The environments are referred to as
narrow, trap and clutter in this study, shown in Fig. 10, Fig. 11
left and Fig. 11 right respectively. All the experiments are
implemented in Python.

2) RESULTS AND DISCUSSION

FIGURE 15. The range of the results (path cost) of k-RRT (blue lines)
The goal of the experiments in this segment is to highlight the compared to results returned by RRT (red lines). The small variation in the
effect of implementation parameters on RRT. It must be noted solution returned by k-RRT indicates more consistent performance and
reliability.
that the number of explored nodes is used as an indication of
the algorithm run time and the cost is the Euclidian distance.
The result of changing the step size of the RRT extension
on the path cost is presented in Fig. 12. The step size is tested
with 5, 10 and then it is unrestricted. Restricting the step
in which the RRT is incrementally grown, maybe counter-
intuitive but it generates far better solutions. The planning
time saved by unrestricting the step size will be lost in post-
processing to improve the solution.
An RRT planner is tested with no biasing, 5% and 10%.
This percentage indicates the percentage of planning in which
the planner attempts to greedily connect to the goal configura-
tion. The results of biasing are given in Fig. 13. It is expected
that biasing will pull the tree towards the goal, leading to FIGURE 16. Comparing between RRT, k-RRT and Bidirectional RRT in
decreasing the number of nodes explored. In both, the narrow terms of path cost (top) and number of explored nodes (bottom).
and cluttered environments, this is true. It is not the case in
the trap environment where the tree must first move away Performances of bidirectional RRT and k-RRT, for k = 5
from the goal then return to it. Increasing the biasing leads and 10, are compared. Path cost and the number of nodes
to increased computation. are show in Fig. 16 and the range of the results of using
Biasing is then compared with bidirectional search by gen- k-near (k = 5) is shown in Fig. 15. It can be seen that both
erating two RRTs. Results are given in Fig. 14. Bi-RRT pro- cases of k-RRT produce, better solutions than Bidirectional

VOLUME 2, 2014 65
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

RRT. However, the computational time needed by k-RRT far TABLE 3. Summary of SBP parameters and heuristics.
exceeds that of Bidirectional RRT and in the case of k = 10
the planner fails to find a solution in the trap environment in
the specified planning time. Another advantage of k-RRT is
the consistency in its results despite its reliance on sampling.
This is illustrated by the range of path cost solutions provided
by k-RRT (k = 5) in comparison to RRT, shown in Fig. 15.

TABLE 2. Path failure rate using lazy collision checking.

The motivation behind approaches that employ lazy CC

is that a small fraction of most connection collides with
obstacles. This observation is consistent with the experiments
conducted here. Once a solution is found, CC is performed. If
a resulting path collides with obstacles, the planner will either
discard the colliding segment or the entire path.
The percentage of infeasible solutions due to collision is
often not considered when employing lazy collision checking.
If a large number of paths are colliding, it may be more effec-
tive to employ an efficient CC algorithm for all connections.
The percentage of colliding paths in different environments
and under different step size is shown Table 2. As expected
as the step size decrease so does the failure rate. However, the
path failure rate remains high. It is a question of implemen-
tation, whether it is more efficient to employ lazy CC and
re-plan the path almost 30%-50% of the time, or constantly
employ efficient CC.

K. SUMMARY
Table 3 is a summary the body of work reviewed in this
section.

IV. KINODYNAMIC PLANNING

Kinodynamic planning deals with the kinematic, non-
holonomic and, or, dynamic constraints imposed on the
robotic system or vehicle. The previously presented planners
were purely geometric, considering only the feasibility of the
path. The term ‘‘Kinodynamic’’ has been coined as a syn-
ergy between kinematics and dynamics [175]. Deterministic
planners were proposed, however, they suffered from high
computational costs [175], [176].
In some cases path planning and kinodynamic constraints
are decoupled. Traditionally, Planners generate a path that
relaxes all kinodynamic constraints. Trajectory modifica-
tion can be employed to gradually modify the trajectory
to obey the constraints. Trajectory modification uses small
forces to incrementally alter the course. It has been pro-
posed for non-holonomic [177], [178] and kinodynamic con-
straints [179]. Trajectory modification has been successfully
applied for humanoids [169], car-tests [180] and multi-DOF
non-holonomic planning [181]. Discarding kinodynamic mal solutions that involve difficult maneuvers. Worse, the
constraints during planning may lead to highly subopti- robot may not be able to execute the plan, resulting in

66 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

unrecoverable situations that lead to collision. Considering Euclidian for instance will identify a state as suitable candi-
system dynamics is favorable. For some systems attempting date for extension. However, extension from this state maybe
to accurately model all the effects will overcomplicate the redundant, as it does not expand the search, or may constantly
model and increase the planning space dimensions and the lead to collision. Often a trajectory is generated in such a way
search complexity. to optimize a cost function [60]. Similar selection strategies
SBP conducts a search in the C-space by sampling config- to the ones proposed in path planning to decrease reliance on
urations, q, and attempting to extend the search towards those metrics have been used in motion planning, such as expan-
configurations. Kinodynamic SBP operates in the state space, siveness [182], state space coverage [110], [163], [184] and
χ, which contains a set of all possible states, x. State space accounting for previous success of expansion [115]. The sen-
can be considered as C-space augmented with velocities. sitivity of RRT to metrics is more problematic in differentially
Subsequently, state space has more dimensions. A state is constrained kinodynamic planning, as extending procedures
generally defined as Eqn. (2). are computationally extensive. For some systems it is possible
to formulate a pseudo-metric estimate for the true cost by
x = f (q, q̇) (2) linearization of the system dynamics and quadratization of
the cost [185].
There are several issues confronting kinodynamic planning.
It is inherently a high dimensional problem, since consider-
ing the first derivatives, for the robot configurations, effec-
tively doubles the dimensions of the search space. The state
equation of the robotic system must be known, as shown
in Eqn. (3). The control space, U , is then discretized. The
control, u, that drives the robot as close as possible to the
desired state, x, is selected [60]. Similarly, Frazzoli, et al.
[137] attempt connections from all nodes in the tree using
an optimal controller until the desired state is reached Hsu,
et al. [182] select a random state, x, and apply a random
control, u, for a fixed time. Eqn. (3) must be integrated, for
the time period that control u is applied, in order to determine
the local trajectory that joins two states. KPIECE employs a FIGURE 17. Illustrating the RG-RRT extension procedure (a) Select a
random node and find the nearest node in the tree (b) Compute the
physics simulator to generate the motion trajectory between reachability of the nearest node, shown as a grey shaded arc (c) Find the
states [110]. There is a trade-off between the accuracy of nearest reachable node to the random node, shown as a red node.
Compare the distance between the nearest node and the nearest
the trajectory generation process and its computational effi- reachable node (red) (d) Extension will only be executed if the reachable
ciency. node is closer, it is then added to the tree.

ẋ = f (x, u) (3)
Extending the tree requires integrating the equations of
Planning in a C-space that has narrow corridors, similar motions to obtain the desired trajectory. The reliance on
to Fig. 10, is one of the challenges in SBP. Kinodynamic metrics means that several extensions are wasted, as they
constraints limit the motion of the robot, essentially cre- will not contribute to find a solution. Reachability Guided
ating narrow passages in the state space. The state space RRT (RG-RRT) evaluates a reachable set for any node in
is, traditionally, defined into free and obstacle state space. the tree [186], as shown in Fig. 17(a) and (b). RG-RRT
Another subdivision exist in the free state space, referred is based on the observation that expansion of the tree is
to as Inevitable Collision States (ICS), in which the robot more expensive that sampling for differentially constrained
will collide with obstacles regardless of the control input systems. A node is added to the tree, if it’s closer to the nearest
applied [183]. This contributes to the complexity of the plan- reachable node than to the nearest node in the tree, as shown
ning dilemma, as the free state space is narrower. High dimen- in as shown in Fig. 17(c) and (d). Environmentally Guided
sional planning combined with narrow passages in the free RRT (EG-RRT) [123] combines two efficient strategies of
state space leads to slowing down SBP planners. Synergistic RG-RRT [186], of adding reachable nodes, and RC-RRT [115],
combination of layers of planning (SyCloP) is a framework of considering failure and success rate of a node prior to
that handles these issues by combining two layers of planners, selecting it.
a discrete and a continuous tree planner [184]. The deter- Kinodynamic planning has been limited to simulation
ministic layer defines where the SBP planner should start based planning applications. Planning time can reach several
planning and changes the search area if the SBP is deemed minutes in some simulation scenarios [60]. Real time kino-
stuck. dynamic planning in state space require exponential planning
Defining a metric that evaluates the true cost between time [137], [187]. Initial attempts to apply kinodynamic plan-
two states is another challenging problem in kinodynamic ning in real life situations produced inaccurate results and
planning. Poor metric selection leads to ineffective planning. resorted to a decoupled planning hierarchy where dynam-

VOLUME 2, 2014 67
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

ics are handled by another module in a step that followed nearest neighbor search and adding a node to the existing
path planning [75]. Bruce and Veloso [188] reported that graph or tree. The two different procedures are named ‘‘Near
decoupling path planning, using execution extended RRT vertices’’ and ‘‘Rewire’’. Near vertices returns a number of
(ERRT) [72], and motion control, produced more accurate nearest nodes similar to k-RRT [121]. In the case of RRT*,
and reliable results, especially when fast computations are the nodes are returned, if they are within a ball of radius, k.
needed. Recently, successful implementations of kinody- This ball radius is a function of the number of nodes in the
namic SBP have been achieved by limiting the planning tree, n, and is defined by Eqn. (4), where γ is a parameter
dimensions and planning in the task-space [76], [189] and based on the environment characteristics and d is the C-space
using visual information for localization [190]. Similarly, dimension [196].
RG-RRT implemented in task space with the use of motion
1/
log (n) d

primitives, fulfilled the task of real-time kinodynamic motion
k= γ (4)
planning [191]. (n)
A promising approach is adopted by S-RRT. It encodes
the constraints of an underactuated vehicle in the charac- The nearest vertices are returned within a ball of radius
teristics of a Bezier curve used for local planning [104]. k and stored in a set Qnear , as shown in Fig. 19(b). The
Kinodynamic trajectory generation using B-spline [149] and selected node, qnew, is connected to the node, qparent , which
Bezier curves [192] is widely studied and can be utilized by provides a shorter router to the start configuration, as shown
SBP to generate effective kinodynamic planners. The local in Fig. 19(c). All remaining nodes in Qnear are rewired to
modification support was exploited by generating a feasible qnew as their parent, if it provides a shorter route to the start
path and then subsequent local adjustments are performed configuration, as shown in Fig. 19(d). Hence every new node,
to ensure dynamic feasibility [193]. The main advantage, of qnew , will endeavor to improve all local connections within a
using splines, is that kinodynamic planning is limited to a predefined radius. An RRT* tree is shown in Fig. 18 after
lower dimensional space, a notion similar to maneuver-based 6,000 iterations.
planning proposed by [137], thus planning can be executed in
real time scenarios. Subsequently, B-spline interpolation was
used to generate smooth trajectories for an RRT planner in a
dynamic driving scenario [194].

V. OPTIMAL SBP
The ability of SBP to provide valid solutions for constrained
high dimensional problems within a reasonable timeframe is
advantageous. Despite the fact that the hit-or-miss sampling
approach is the core of the planner’s effective strategy, it
leads to the inclusion of many redundant maneuvers in the
final path. SBP generate highly suboptimal solutions and they
FIGURE 18. RRT* tree after 6,000 iterations and 4,700 explored nodes.
are highly sensitive to their implementation details, as shown
experimentally in section (III-J).
LaValle and Kuffner [60] proposed modification of the
termination condition in a way such that the SBP keeps B. REQUIREMENTS FOR RRT* TO GENERATE OPTIMAL
running to iteratively converge the path cost. The solution SOLUTIONS
convergence remained an unanswered problem, until it was The realization of an optimal solution dictates some criteria
proven that given infinite runtime RRT will not find an that must be met. Primarily, optimality is defined with respect
optimal solution [195]. Numerous variants, such as k-RRT, to a specific metric and the planner is constantly attempting
Anytime RRT, and post processing methods have been pro- to enhance the value of that metric. As previously discussed
posed to remedy the poor solutions returned by RRT. Despite SBP, defining a true metric that signifies the cost between two
their effectiveness they provide no theoretical guarantees for configurations has proved to be a non-trivial task.
reaching an optimal solution. In addition to defining a metric, a steering function must be
defined in the planner. RRT* [196] relies on the existence of
A. RRT* a steering function that drives the robot through an optimal
A recent development in SBP was set forth by Karaman and trajectory between two specified states or configurations.
Frazzoli [196]. A family of optimal sampling based planners, A likewise assumed guidance loop is the core of the work
RRT*2 , PRM* and RRG*, were presented which guaranteed by Frazzoli, et al. [137]. Such steering function does not
asymptotic optimality. These algorithms operate analogously exist for several robotic systems. Optimal control is still a
to any common SBP except in two procedures. Performing subject pursued by researchers even for simple path plan-
ning purposes [197]. An alternative to defining a steering
2 pronounced RRT star function is storing optimal trajectories and picking a suitable

68 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

trajectory when connecting two configurations, a particularly NN search is identified as a bottleneck in SBP. RRT*
useful strategy for redundant articulated manipulators [74], or proceeds to compute a set of near vertices, in each iteration,
differentially constrained dynamic systems [170]. that lie within a ball of known radius. The convergence rate
Even so for a holonomic system, whose optimal path is the is accelerated by approximating costs between nodes, when
straight line joining two configurations, the planner still guar- computing nearest vertices for a certain node [201].
antees only asymptotic optimality. This property indicates It can be observed that a large fraction of the RRT* plan-
that the planner will always reach an optimal solution when ning time is spent extending the tree into areas that are not
the runtime approaches infinite. The initial solutions will be necessarily promising, adding and rewiring redundant nodes.
suboptimal, similar to RRT, and it will continue to converge This is illustrated in Fig. 18. All areas are heavily sampled
towards optimality as the planner is running. even though most of those nodes will not contribute to the
path optimality.
To overcome the slow convergence rate an Anytime
framework for RRT* was implemented on an autonomous
forklift [202]. Anytime RRT* finds a suboptimal path and
converges towards optimality within the given planning time.
This anytime implementation is suitable for real-time applica-
tions, as the planner must return a path whenever it is called
i.e. existence of a path in real applications is far more vital
than the path optimality.
Node selection criteria were impose to limit the addition of
nodes whose shortest path is large than a certain bound [198],
[203]. Modifying the rewiring procedure to include the near-
FIGURE 19. Illustrating the operation of RRT* (a) A new random node, est nodes in the shortest path increases the convergence
qnew , is selected, shown as orange node (b) Near vertices procedure
returns a set, Qnear , of all nodes, shown as red nodes, within a certain rate [203]. A predictive model is used to estimate the probabil-
distance of the new node (circular area shaded in grey) (c) qnew is ity of a node being on the optimal path and is used to guide the
connected to the node, qparent , that has the shortest route to the start
(shown as orange path) (d) The remaining nodes in Qnear are rewired
path towards optimal regions [105]. RRT#3 replaces the local
through qnew , if it provides a shorter path towards the start. rewiring procedure by globally replanning the path [204].
Efficiently updating all the node costs, and categorizing nodes
C. CONVERGENCE TOWARDS AN OPTIMAL SOLUTION such that only promising nodes will be expanded, is the basis
RRT* is guaranteed to asymptotically converge towards an for this planner. In this context, promising nodes are those,
optimal solution under certain assumptions. The convergence which can constitute an optimal path. It is shown that RRT#
rate, however, has been shown to be rather slow. In fact, converges faster towards an optimal solution as it guarantees
certain post processing approaches outperform RRT* [151]. an optimal solution is returned, given all the present node
The desirable properties of RRT* in real-time applications costs.
are overshadowed by the planning time wasted to reach an
optimal solution. D. OPTIMAL KINODYNAMIC PLANNING
There are a number of methods that endeavor to speed The development of optimal planning and RRT* algorithm
up the convergence of RRT*. A bidirectional RRT* that has renewed interest in SBP. As an example, RRT* has
only joins promising nodes have been shown to improve been extended for vector fields, not just uniform environ-
the performance in high dimensional spaces [198]. The node ments [205]. It also led to the emergence of research in
selection strategy is similar to the one employed by Anytime optimal kinodynamic planning Karaman and Frazzoli [196]
RRT [138]. argued that RRT* is analogous to RRT, thus is a generalized
RRT*-smart removes redundant nodes every planning planner that can be applied in any planning context. Concep-
iteration and biases the sampling towards the remaining tually this statement is accurate, however, in a practical sense
nodes [199]. A naive algorithm is implemented to trim the it is a perplexing task to apply optimal SBP in kinodynamic,
tree as it checks subsequent nodes. A possible improvement real-time or, dynamic scenarios.
is the use path refinement algorithm presented in [140]. At this point, there are a handful of optimal kinodynamic
RRT*-smart resembles the anytime meta-algorithm presented planning planners. They are limited to systems with linear
in [151], as it alternates between post processing and expand- dynamics [206], [207], whose cost functions are well known
ing the tree. and can be computed between any two states.
A potential field function is coupled with the RRT* algo- Optimal kinodynamic SBP for differentially constrained,
rithm to guide the algorithm towards the optimal solu- high dimensional systems was achieved [208] by limited the
tion [200]. It attempts to strike a balance between exploitation planning to the task space [189] and using reachability guided
and exploration as suggested by [102]. However, the pre- trees [186]. Planning with task space is a general approach
sented approach does not adaptively change the behavior and
the parameters are predetermined prior to planning. 3 pronounced RRT sharp

VOLUME 2, 2014 69
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

that can be adapted to planning relative to the end-effector

of a manipulator or a center of mass of a robot. A more
challenging problem, non-holonomic kinodynamic SBP, was
resolved similarly [209]. Nonetheless, these planners are still
restricted to simulation-based applications due to their high
computational requirements.

E. EXPERIMENTS AND RESULTS

A preliminary version of this analysis was presented in [203].
The aim of these experiments is twofold,
FIGURE 21. Effect of goal biasing on path cost and convergence rate.
• Highlight the observation that RRT*, much like RRT, is
sensitive to its implementation parameters which must
be carefully chosen, 5%-10% and indicates the improvement in the path cost. This
• Demonstrate the benefits of applying node selection cri- leads to generating sparse trees, as shown in Table 4. The
teria on RRT*. effect of node selection is illustrated by comparing Fig. 22
and Fig. 18. The planner generates the almost identical solu-
1) PARAMETER SENSITIVITY tions with far less nodes explored. Aside from merely adding
As expected, the implementation of RRT* is of grave impor- promising nodes that will lead to better solutions, the work
tance to its performance. The results of modifying the step by [186] argued for the effectiveness of maintaining sparse
size can be seen in Fig. 20. Similar to RRT, decreasing the step trees especially for non-holonomic kinodynamic planning.
size of the planner extension improve the overall path cost. To
eliminate redundant nodes that will not contribute to the path TABLE 4. Average number of nodes in the tree after 10,000 iterations.
convergence, a minimum step size has been specified.

FIGURE 20. Effect of step size on the path cost and convergence rate.

Goal biasing is employed to speed up the performance of

RRT and guide it towards finding a solution. It has success- FIGURE 22. A sparse RRT* tree generated, after 6,000 iterations. Node
fully done so for RRT* as well, as can be seen in Fig. 21. selection has been employed with ε = 5%.
The planner was unable to find a solution before 1,500 itera-
tions. However with biasing it was successful before reaching
500 iterations. Additionally, biasing improves the cost of the VI. DYNAMIC AND UNCERTAIN ENVIRONMENTS
initial solution found by RRT* and it decreases its conver- A common assumption in planning algorithms is that the
gence. Biasing is recommended prior to find a quick solution environment is well defined such that the robot’s location
and then it has to be terminated due to its negative effect on relative to obstacles and goal positioned are all known. This
the convergence rate. Biasing the RRT* can be an alternative statement holds true in static environments where industrial
to the recommended use of a traditional RRT to find an initial manipulators are used or in CAD applications in which the
solution in [208] and [209]. environment is user-defined.
Autonomous vehicles and robots operate in dynamic
2) NODE SELECTION changing environments with other uncontrollable, in some
The node selection strategy, exploited by Anytime RRT cases lethargic, agents that cannot be modeled or estimated.
to bound sampling merely to promising nodes has been In general, the assumption of a well-defined static environ-
proposed as a performance enhancement for RRT*. The ment does not hold. There is an uncertainty that arises as
lower bound is defined by, ε, where the lower bound equals a result of sensing errors and noise and the imprecision of
(1- ε) times the current path cost. It is generally taken between actuators and other uncontrollable factors such as wheel slip.

70 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

Consequently, the exact location of the robot (localization) when worst-case scenario of a growing disc is considered.
and the description of the environment (mapping) is not a Growing discs assumption creates a narrow free regions in the
trivial task. In this section we present planners that tackle C-space. A scenario in which SBP perform poorly. Different
one of the two current issues in robotic motion planning, types of dynamic obstacles are shown in Fig. 23 and worst-
replanning in dynamic and/or uncertain environments. Early case growing discs are shown on the far right. Flexible-PRM
on, it was assumed that, since SBP were able to generate (F-PRM) similarly used backward A*, from the static goal
relatively rapid solutions, it would suffice to discard current towards the moving robot, in dynamic environments [124].
solutions and replan when deviations were identified in the Multipartite RRT (MP-RRT) [217] combines the strat-
environment. egy of biasing the search towards discarded configurations,
Regenerating a single-query search tree may be a valid similar to ERRT. It also rebuilds the tree, like DRRT, and
approach, given the appropriate parameters and heuristics in maintains separate detached forests, like RRF. MP-RRT dis-
certain instances. In the case for multi-query planners, such tinguishes itself from RRF by only maintaining forests for
as PRM, that invest most of their resources in connecting the a limited time so as not to waste computational time in
environment, regenerating the entire roadmap is not feasible. unpromising areas.
An outline for using PRM in dynamic environments In the course of navigating a dynamic environment, it
involved generating a roadmap while assuming an obstacle is possible that a plan is deemed unsafe, such that it will
free space [210]. The data structure of PRM was made more collide with a moving obstacle. The selected planning frame-
efficient in order to accommodate changes in the environ- work must generate an alternate feasible route. The concept
ment and consequently in the roadmap. A similar approach τ -safety ensures that at any stage during path execution there
attempts to use single query planner to connect PRM nodes is enough time, τ , for the planner to compute an alternate path
in dynamic environments and encodes obstacle positions in while following the unsafe path [137]. Greedy, Incremental,
local connections [211]. van den Berg, et al. [212] proposed Path-Directed (GRIP) [180] is a safe, replanning framework
a generalized PRM method in surroundings where obstacle that guarantees safety by considering Inevitable collisions
movements are restricted to local sectors. PDR maintains a states (ICS) during replanning and only considering safety-
roadmap whose paths can be deformed, thus numerous paths guarantees to reduce planning time. GRIP employs a similar
can be obtained between two configurations [152]. PDR has rebuilding strategy to ERRT and DRRT, node selection is
been proposed for dynamic path planning by the authors, but based on coverage, as employed by PDST, and an efficient
is yet to be evaluated in those scenarios. safe framework based on τ -safety.
RRF (Reconfigurable Random Forests) provided a frame-
work to managing either roadmap, or tree planners, under
changing settings [213]. Once changes in the environment are
detected, nodes in Cobs and colliding paths are discarded. This
leads to the emergence of separated roadmaps, or forests. The
planner then prunes the forests and attempts reconnects paths.
Lazy reconfiguration forest (LRF) used the same framework
but proceeded to perform collision checking only for the paths
involved for planning [214].
ERRT is often mentioned as the first algorithm to be FIGURE 23. Dynamic obstacles different trajectory assumptions with
implemented in a real-time dynamic situation [72], [188]. respect to time.

ERRT maintains a single tree. If that tree is collides with

the obstacle space it is discarded and another one is rebuilt. Considering stochastic sensing and dynamic conditions is a
ERRT maintains the location of the discarded configurations, relatively novel topic in motion planning. Particle RRT [218]
waypoint cache, and biasing the search slightly towards those and RRT-SLAM [219] model uncertainty using particle fil-
node locations. It is motivated by the assumption that, if the ters, which is then considered in the planning. Similar
algorithm is updated at a high frequency, a small percentage uncertainty considerations are added to RRT* framework
of the original tree needs to be modified. by Rapidly-exploring Random Belief Trees (RRBT) [220],
Dynamic RRT (DRRT) builds on the idea that it is more [221]. To guarantee the accuracy of planned path, uncertainty
efficient to repair the existing tree, than to, rebuild an entirely is encoded in path costs to guide the robot to useful areas and
new one [215]. Unlike ERRT, only the colliding configu- thus ensuring the robot will not be lost without information.
rations and their child nodes are discarded in an efficient Gaussian processes were also used to predict the motion of
manner. DRRT borrows the concept of slightly biasing the other vehicles in the environment [222]. The planner esti-
search towards invalidated areas from ERRT. Nonetheless, it mates the probability of collision and returns a path that is
outperforms ERRT by repairing the tree. AD* was coupled probabilistically collision free. This approach may serve as
with PRM to provide an efficient framework for replan- an alternative to worst-case growing discs model, however,
ning [216]. In this approach the motion of other agents the objects in the environment must be analyzed prior to
was extrapolated, the planner failed to generate solutions planning. EG-RRT [123] evaluates the collision probability

VOLUME 2, 2014 71
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

of each state in RRT tree based on the modeled uncertainty of is surveyed in this paper. Several experiments are executed
dynamics and sensing and creates a cost map of the environ- to shed a light on some of the current issues, investigated
ment. by researchers, and highlight the implementation details that
are often not discussed when planners and algorithms are
TABLE 5. Summary of replanning strategies. proposed.
An emphasis is placed on the contemporary research prob-
lems. Motion planning has moved away from path planning in
static environments for simple robots into more challenging
arenas. Methods that address real-time kinodynamic plan-
ning, optimal planning, planning under uncertainty and in
dynamic environments are given particular attention. The
continued expansion of robot motion planning boundaries
promises more realistic methods that can be utilized in real-
life scenarios.

REFERENCES
[1] A. Kelly and A. Stentz, ‘‘Rough terrain autonomous mobility—Part 2: An
active vision, predictive control approach,’’ Autonom. Robot., vol. 5, no. 2,
pp. 163–198, May 1998.
[2] P. Corke, J. Lobo, and J. Dias, ‘‘An introduction to inertial and visual
sensing,’’ Int. J. Robot. Res., vol. 26, no. 6, pp. 519–535, Jun. 2007.
[3] G. Atanacio-Jiménez, J. J. González-Barbosa, J. B. Hurtado-Ramos,
F. J. Ornelas-Rodríguez, H. Jiménez-Hernández, T. García-Ramirez, et al.,
‘‘LIDAR velodyne HDL-64E calibration using pattern planes,’’ Int. J. Adv.
Robot. Syst., vol. 8, no. 5, pp. 70–82, Oct. 2011.
[4] G. Pandey, J. R. McBride, and R. M. Eustice, ‘‘Ford campus vision and lidar
data set,’’ Int. J. Robot. Res., vol. 30, no. 13, pp. 1543–1552, Nov. 2011.
[5] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, ‘‘Vision meets robotics: The
KITTI dataset,’’ Int. J. Robot. Res., vol. 32, no. 11, pp. 1231–1237, Sep.
Generalized RRT and PRM have been proposed in which 2013.
[6] J. Borenstein and F. Liqiang, ‘‘Measurement and correction of systematic
the robot dynamics and sensors are stochastically modeled odometry errors in mobile robots,’’ IEEE Trans. Robot. Autom., vol. 12, no.
and the local planner estimates the probability of transition 6, pp. 869–880, Dec. 1996.
success and will not proceed if the probability exceeds a [7] H. Durrant-Whyte and T. Bailey, ‘‘Simultaneous localization and mapping:
Part I,’’ IEEE Robot. Autom. Mag., vol. 13, no. 2, pp. 99–110, Jun. 2006.
threshold [223]. Unlike traditional planners, generalized plan- [8] D. Ferguson, C. Baker, M. Likhachev, and J. Dolan, ‘‘A reasoning frame-
ners terminate only when a solution with a high probability work for autonomous urban driving,’’ in Proc. IEEE Intell. Veh. Symp., Jun.
of success is found. Feedback-based Information Roadmap 2008, pp. 775–780.
[9] J. G. Hurdus and D. W. Hong, ‘‘Behavioral programming with hierarchy
(FIRM) also relies on feedback from local planners to and parallelism in the DARPA urban challenge and robocup,’’ in Proc.
reduce the uncertainty propagation between states [224]. The IEEE Int. Conf. MFI Intell. Syst., Aug. 2008, pp. 503–509.
question of path planning amongst moving uncontrollable [10] R. Wallace, A. Stentz, C. Thorpe, H. Moravec, W. Whittaker, and
T. Kanade, ‘‘First results in robot road following,’’ in Proc. IJCAI, 1985,
obstacles, under stochastic dynamic and sensing conditions pp. 381–387.
represents a next step in robotic research. The amalgamation [11] G. Antonelli, S. Chiaverini, and G. Fusco, ‘‘A fuzzy-logic-based approach
of uncertainty, kinodynamic, and optimal planning in active for mobile robot path tracking,’’ IEEE Trans. Fuzzy Syst., vol. 15, no. 2,
pp. 211–221, Apr. 2007.
environments is bound to push robots into new frontiers. Plan- [12] R. N. Jazar, ‘‘Mathematical theory of autodriver for autonomous vehicles,’’
ning strategies in this section are categorized into dynamic, J. Vibrat. Control, vol. 16, no. 2, pp. 253–279, Feb. 2010.
uncertain and safe in Table 5. [13] S. M. LaValle, Planning Algorithms. Cambridge, U.K.: Cambridge Univ.
Press, May 2006.
[14] J.-C. Latombe, Robot Motion Planning. New York, NY, USA: Springer-
VII. CONCLUSION Verlag, 1990.
Sampling-based planning has established its success in solv- [15] H. M. Choset, Principles of Robot Motion: Theory, Algorithms, and Imple-
mentation. Englewood Cliffs, NJ, USA: Prentice-Hall, 2005.
ing the intricate problem of robot motion planning. Numerous [16] J.-C. Latombe, ‘‘Motion planning: A journey of robots, molecules, dig-
methods have been proposed to improve the efficiency of ital actors, and other artifacts,’’ Int. J. Robot. Res., vol. 18, no. 11,
planning and the quality of plans. They have provided sig- pp. 1119–1128, Nov. 1999.
[17] J. H. Reif, ‘‘Complexity of the mover’s problem and generalizations,’’ in
nificant advantages in a wide field of real-life and simulation Proc. 20th Annu. Symp. Found. Comput. Sci., Oct. 1979, pp. 421–427.
based scenarios. Subsequently, there exists an immense body [18] T. Asano, T. Asano, L. Guibas, J. Hershberger, and H. Imai, ‘‘Visibility-
of work on the topic of SBP. polygon search and euclidean shortest paths,’’ in Proc. 26th Annu. Symp.
Found. Comput. Sci., Oct. 1985, pp. 155–164.
This brings into the light the gap that existed in classical [19] C. Alexopoulos and P. M. Griffin, ‘‘Path planning for a mobile
motion planning algorithms to illustrate the significance of robot,’’ IEEE Trans. Syst., Man Cybern., vol. 22, no. 2, pp. 318–322,
SBP and the motivation behind their inception. We then Mar./Apr. 1992.
[20] T. Maekawa, T. Noda, S. Tamura, T. Ozaki, and K.-I. Machida, ‘‘Curvature
continue by listing the main algorithms that all state of the continuous path generation for autonomous vehicle using B-spline curves,’’
art algorithms are built upon. The growing body of work Comput.-Aided Des., vol. 42, no. 4, pp. 350–359, Apr. 2010.

72 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

[21] J. Canny, ‘‘A Voronoi method for the piano-movers problem,’’ in Proc. [46] T. M. Howard and A. Kelly, ‘‘Optimal rough terrain trajectory generation
IEEE Int. Conf. Robot. Autom., Mar. 1985, pp. 530–535. for wheeled mobile robots,’’ Int. J. Robot. Res., vol. 26, no. 2, pp. 141–166,
[22] O. Takahashi and R. J. Schilling, ‘‘Motion planning in a plane using Feb. 2007.
generalized Voronoi diagrams,’’ IEEE Trans. Robot. Autom., vol. 5, no. 2, [47] M. Werling, S. Kammel, J. Ziegler, and L. Gröll, ‘‘Optimal trajectories for
pp. 143–150, Apr. 1989. time-critical street scenarios using discretized terminal manifolds,’’ Int. J.
[23] G. E. Jan, C. C. Sun, W. C. Tsai, and T. H. Lin, ‘‘An O(nlogn) shortest path Robot. Res., vol. 31, no. 3, pp. 346–359, Mar. 2012.
algorithm based on delaunay triangulation,’’ IEEE/ASME Trans. Mecha- [48] B. R. Donald, ‘‘A search algorithm for motion planning with six degrees of
tron., Feb. 2013. freedom,’’ Artif. Intell., vol. 31, no. 3, pp. 295–353, Mar. 1987.
[24] M. Elbanhawi, M. Simic, and R. Jazar, ‘‘Autonomous mobile robot path [49] N. Metropolis and S. Ulam, ‘‘The Monte Carlo method,’’ J. Amer. Statist.
planning: A novel roadmap approach,’’ Appl. Mech. Mater., vols. 373–375, Assoc., vol. 44, no. 247, pp. 335–341, Sep. 1949.
pp. 246–254, Jan. 2013. [50] N. Metropolis, A. W. Rosenbluth, M. N. Rosenbluth, A. H. Teller, and
[25] R. A. Brooks and T. Lozano-Perez, ‘‘A subdivision algorithm in configu- E. Teller, ‘‘Equation of state calculations by fast computing machines,’’
ration space for findpath with rotation,’’ IEEE Trans. Syst., Man Cybern., J. Chem. Phys., vol. 21, no. 6, pp. 1087–1092, Jun. 1953.
vol. 15, no. 2, pp. 224–233, Mar./Apr. 1985. [51] J. Barraquand and J.-C. Latombe, ‘‘Robot motion planning: A distributed
[26] E. W. Dijkstra, ‘‘A note on two problems in connexion with graphs,’’ representation approach,’’ Int. J. Robot. Res., vol. 10, no. 6, pp. 628–649,
Numer. Math., vol. 1, no. 1, pp. 269–271, Dec. 1959. Dec. 1991.
[27] P. E. Hart, N. J. Nilsson, and B. Raphael, ‘‘A formal basis for the heuristic [52] S. Carpin and G. Pillonetto, ‘‘Motion planning using adaptive random
determination of minimum cost paths,’’ IEEE Trans. Syst. Sci. Cybern., walks,’’ IEEE Trans. Robot., vol. 21, no. 1, pp. 129–136, Feb. 2005.
vol. 4, no. 2, pp. 100–107, Jul. 1968. [53] L. Kavraki and J. C. Latombe, ‘‘Randomized preprocessing of configura-
[28] A. Stentz, ‘‘Optimal and efficient path planning for unknown and dynamic tion space for path planning: Articulated robots,’’ in Proc. IEEE/RSJ/GI
environments,’’ Int. J. Robot. Autom., vol. 10, no. 3, pp. 89–100, 1995. Int. Conf. IROS, vol. 3. Sep. 1994, pp. 1764–1771.
[29] M. Likhachev, D. Ferguson, G. Gordon, A. Stentz, and S. Thrun, ‘‘Anytime [54] N. M. Amato and Y. Wu, ‘‘A randomized roadmap method for path and
search in dynamic graphs,’’ Artif. Intell., vol. 172, no. 14, pp. 1613–1643, manipulation planning,’’ in Proc. IEEE Int. Conf. Robot. Autom., vol. 1.
Sep. 2008. Apr. 1996, pp. 113–120.
[30] T. Howard, C. Green, and A. Kelly, ‘‘State space sampling of feasible [55] L. E. Kavraki, P. Svestka, J. C. Latombe, and M. H. Overmars, ‘‘Probabilis-
motions for high performance mobile robot navigation in highly con- tic roadmaps for path planning in high-dimensional configuration spaces,’’
strained environments,’’ in Field and Service Robotics, vol. 42, C. Laugier IEEE Trans. Robot. Autom., vol. 12, no. 4, pp. 566–580, Aug. 1996.
and R. Siegwart, Eds. Berlin, Germany: Springer-Verlag, 2008, pp. 585– [56] S. M. LaValle, ‘‘Rapidly-exploring random trees: A new tool for path
593. planning,’’ Dept. Comput. Sci., Iowa State Univ., Ames, IA, USA,
[31] M. Pivtoraiko, R. A. Knepper, and A. Kelly, ‘‘Differentially constrained Tech. Rep. TR 98-11, 1998.
mobile robot motion planning in state lattices,’’ J. Field Robot., vol. 26, [57] P. Švestka and M. H. Overmars, ‘‘Motion planning for carlike robots using
no. 3, pp. 308–333, Mar. 2009. a probabilistic learning approach,’’ Int. J. Robot. Res., vol. 16, no. 2,
[32] M. Pivtoraiko and A. Kelly, ‘‘Kinodynamic motion planning with state pp. 119–143, Apr. 1997.
lattice motion primitives,’’ in Proc. IEEE/RSJ Int. Conf. IROS, Sep. 2011, [58] J. Barraquand, L. Kavraki, J.-C. Latombe, R. Motwani, T.-Y. Li, and P.
pp. 2172–2179. Raghavan, ‘‘A random sampling scheme for path planning,’’ Int. J. Robot.
[33] H. R. Beom and H. S. Cho, ‘‘A sensor-based navigation for a mobile robot Res., vol. 16, no. 6, pp. 759–774, Dec. 1997.
using fuzzy logic and reinforcement learning,’’ IEEE Trans. Syst., Man [59] D. Hsu, J.-C. Latombe, and H. Kurniawati, ‘‘On the probabilistic founda-
Cybern., vol. 25, no. 3, pp. 464–477, Mar. 1995. tions of probabilistic roadmap planning,’’ Int. J. Robot. Res., vol. 25, no. 7,
[34] H. Martínez-Alfaro and S. Gómez-García, ‘‘Mobile robot path planning pp. 627–643, Jul. 2006.
and tracking using simulated annealing and fuzzy logic control,’’ Expert [60] S. M. LaValle and J. J. Kuffner, ‘‘Randomized kinodynamic planning,’’ Int.
Syst. Appl., vol. 15, nos. 3–4, pp. 421–429, Oct./Nov. 1998. J. Robot. Res., vol. 20, no. 5, pp. 378–400, May 2001.
[35] H. Seraji and A. Howard, ‘‘Behavior-based robot navigation on challenging [61] D. Hsu, J. C. Latombe, and R. Motwani, ‘‘Path planning in expansive
terrain: A fuzzy logic approach,’’ IEEE Trans. Robot. Autom., vol. 18, no. configuration spaces,’’ in Proc. IEEE Int. Conf. Robot. Autom., vol. 3. Apr.
3, pp. 308–321, Jun. 2002. 1997, pp. 2719–2726.
[36] D. Janglová, ‘‘Neural networks in mobile robot motion,’’ Int. J. Adv. Robot. [62] J. M. Ahuactzin, K. Gupta, and E. Mazer, ‘‘Manipulation planning for
Syst., vol. 1, no. 1, pp. 15–22, 2004. redundant robots: A practical approach,’’ Int. J. Robot. Res., vol. 17, no. 7,
[37] M. Gerke, ‘‘Genetic path planning for mobile robots,’’ in Proc. Amer. pp. 731–747, Jul. 1998.
Control Conf., vol. 4. Jun. 1999, pp. 2424–2429. [63] J. Kuffner, ‘‘Autonomous agents for real-time animation,’’ Ph.D. disserta-
[38] H. Yanrong, S. X. Yang, X. Li-zhong, and M. Q. H. Meng, ‘‘A knowledge tion, Dept. Comput. Sci., Stanford Univ., Stanford, CA, USA, 1999.
based genetic algorithm for path planning in unstructured mobile robot [64] I. Karamouzas and M. Overmars, ‘‘Simulating and evaluating the local
environments,’’ in Proc. IEEE Int. Conf. Robot. Biomimet., Aug. 2004, pp. behavior of small pedestrian groups,’’ IEEE Trans. Visualizat. Comput.
767–772. Graph., vol. 18, no. 3, pp. 394–406, Mar. 2012.
[39] M. A. P. Garcia, O. Montiel, O. Castillo, R. Sepúlveda, and P. Melin, [65] Y. Kwangjin, S. K. Gan, and S. Sukkarieh, ‘‘A Gaussian process-based RRT
‘‘Path planning for autonomous mobile robot navigation with ant colony planner for the exploration of an unknown and cluttered environment with
optimization and fuzzy cost function evaluation,’’ Appl. Soft Comput., a UAV,’’ Adv. Robot., vol. 27, no. 6, pp. 431–443, Apr. 2013.
vol. 9, no. 3, pp. 1102–1110, Jun. 2009. [66] I. Al-Bluwi, T. Siméon, and J. Cortés, ‘‘Motion planning algorithms
[40] O. Khatib, ‘‘Real-time obstacle avoidance for manipulators and mobile for molecular simulations: A survey,’’ Comput. Sci. Rev., vol. 6, no. 4,
robots,’’ Int. J. Robot. Res., vol. 5, no. 1, pp. 90–98, Mar. 1986. pp. 125–143, Jul. 2012.
[41] R. C. Arkin, ‘‘Motor schema—Based mobile robot navigation,’’ Int. J. [67] B. Gipson, D. Hsu, L. E. Kavraki, and J.-C. Latombe, ‘‘Computational
Robot. Res., vol. 8, no. 4, pp. 92–112, Aug. 1989. models of protein kinematics and dynamics: Beyond simulation,’’ Annu.
[42] Y. Koren and J. Borenstein, ‘‘Potential field methods and their inherent Rev. Anal. Chem., vol. 5, pp. 273–291, Jul. 2012.
limitations for mobile robot navigation,’’ in Proc. IEEE Int. Conf. Robot. [68] C. S. Han, K. H. Law, J.-C. Latombe, and J. C. Kunz, ‘‘A performance-
Autom., vol. 2. Apr. 1991, pp. 1398–1404. based approach to wheelchair accessible route analysis,’’ Adv. Eng. Infor-
[43] J. Borenstein and Y. Koren, ‘‘The vector field histogram-fast obstacle mat., vol. 16, no. 1, pp. 53–71, 2002.
avoidance for mobile robots,’’ IEEE Trans. Robot. Autom., vol. 7, no. 3, [69] R. Alterovitz, M. Branicky, and K. Goldberg, ‘‘Motion planning under
pp. 278–288, Jun. 1991. uncertainty for image-guided medical needle steering,’’ Int. J. Robot. Res.,
[44] R. Simmons, ‘‘The curvature-velocity method for local obstacle avoid- vol. 27, nos. 11–12, pp. 1361–1374, Nov. 2008.
ance,’’ in Proc. IEEE Int. Conf. Robot. Autom., vol. 4. Apr. 1996, [70] M. Moll and L. E. Kavraki, ‘‘Path planning for deformable linear objects,’’
pp. 3375–3382. IEEE Trans. Robot., vol. 22, no. 4, pp. 625–636, Aug. 2006.
[45] F. Belkhouche and B. Bendjilali, ‘‘Reactive path planning for 3-D [71] M. S. Branicky, M. M. Curtiss, J. A. Levine, and S. B. Morgan, ‘‘RRTs for
autonomous vehicles,’’ IEEE Trans. Control Syst. Technol., vol. 20, no. 1, nonlinear, discrete, and hybrid planning and control,’’ in Proc. 42nd IEEE
pp. 249–256, Jan. 2012. Conf. Decision Control, vol. 1. Dec. 2003, pp. 657–663.

VOLUME 2, 2014 73
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

[72] J. Bruce and M. Veloso, ‘‘Real-time randomized path planning for robot [97] R. Geraerts and M. H. Overmars, ‘‘Reachability-based analysis for proba-
navigation,’’ in Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst., vol. 3. bilistic roadmap planners,’’ Robot. Auto. Syst., vol. 55, no. 11, pp. 824–836,
Oct. 2002, pp. 2383–2388. Nov. 2007.
[73] A. L. Olsen and H. G. Petersen, ‘‘Motion planning for gantry mounted [98] A. Yershova, L. Jaillet, T. Simeon, and S. M. LaValle, ‘‘Dynamic-domain
manipulators: A ship-welding application example,’’ in Proc. IEEE Int. RRTs: Efficient exploration by controlling the sampling domain,’’ in Proc.
Conf. Robot. Autom., Apr. 2007, pp. 4782–4786. IEEE ICRA, Apr. 2005, pp. 3856–3861.
[74] L.-P. Ellekilde and H. G. Petersen, ‘‘Motion planning efficient trajectories [99] L. Jaillet, A. Yershova, S. M. La Valle, and T. Simeon, ‘‘Adaptive tuning of
for industrial bin-picking,’’ Int. J. Robot. Res., vol. 32, pp. 991–1004, Aug. the sampling domain for dynamic-domain RRTs,’’ in Proc. EEE/RSJ Int.
2013. Conf. IROS, Aug. 2005, pp. 2851–2856.
[75] S. Srinivasa, D. Ferguson, C. Helfrich, D. Berenson, A. Collet, [100] B. Burns and O. Brock, ‘‘Single-query motion planning with utility-
R. Diankov, et al., ‘‘HERB: A home exploring robotic butler,’’ Auto. guided random trees,’’ in Proc. IEEE Int. Conf. Robot. Autom., Apr. 2007,
Robots, vol. 28, no. 1, pp. 5–20, Jan. 2010. pp. 3307–3312.
[76] S. S. Srinivasa, D. Berenson, M. Cakmak, A. Collet, M. R. Dogar, [101] S. Rodriguez, T. Xinyu, L. Jyh-Ming, and N. M. Amato, ‘‘An obstacle-
A. D. Dragan, et al., ‘‘HERB 2.0: Lessons learned from developing based rapidly-exploring random tree,’’ in Proc. IEEE ICRA, May 2006,
a mobile manipulator for the home,’’ Proc. IEEE, vol. 100, no. 8, pp. 895–900.
pp. 2410–2428, Jul. 2012. [102] M. Rickert, O. Brock, and A. Knoll, ‘‘Balancing exploration and exploita-
[77] Y. Kuwata, S. Karaman, J. Teo, E. Frazzoli, J. P. How, and G. Fiore, tion in motion planning,’’ in Proc. IEEE ICRA, May 2008, pp. 2812–2817.
‘‘Real-time motion planning with applications to autonomous [103] O. Brock and E. E. Kavraki, ‘‘Decomposition-based motion planning: A
urban driving,’’ IEEE Trans. Control Syst. Technol., vol. 17, no. 5, framework for real-time motion planning in high-dimensional configura-
pp. 1105–1118, Sep. 2009. tion spaces,’’ in Proc. IEEE ICRA, vol. 2. May 2001, pp. 1469–1474.
[78] Y. Jihyun and C. D. Crane, ‘‘Path planning for Unmanned Ground [104] Y. Kwangjin, ‘‘An efficient Spline-based RRT path planner for non-
Vehicle in urban parking area,’’ in Proc. 11th ICCAS, Oct. 2011, holonomic robots in cluttered environments,’’ in Proc. ICUAS, May 2013,
pp. 887–892. pp. 288–297.
[79] S. Lindemann and S. LaValle, ‘‘Current issues in sampling-based motion [105] M. Kobilarov, ‘‘Cross-entropy motion planning,’’ Int. J. Robot. Res.,
planning,’’ in Robotics Research, vol. 15, P. Dario and R. Chatila, Eds. vol. 31, no. 7, pp. 855–871, Jun. 2012.
Berlin, Germany: Springer-Verlag, 2005, pp. 36–54. [106] R. A. Knepper and M. T. Mason, ‘‘Real-time informed path sampling for
[80] K. I. Tsianos, I. A. Sucan, and L. E. Kavraki, ‘‘Sampling-based robot motion planning search,’’ Int. J. Robot. Res., vol. 31, no. 11, pp. 1231–1250,
motion planning: Towards realistic applications,’’ Comput. Sci. Rev., vol. 1, Sep. 2012.
no. 1, pp. 2–11, Aug. 2007.
[107] R. Wein, J. van den Berg, and D. Halperin, ‘‘Planning high-quality paths
[81] I. A. Sucan and L. E. Kavraki, ‘‘On the implementation of single-
and corridors amidst obstacles,’’ Int. J. Robot. Res., vol. 27, nos. 11–12, pp.
query sampling-based motion planners,’’ in Proc. IEEE ICRA, Jan. 2010,
1213–1231, Nov./Dec. 2008.
pp. 2005–2011.
[108] J. M. Phillips, N. Bedrossian, and E. E. Kavraki, ‘‘Guided expansive
[82] I. A. Sucan, M. Moll, and E. E. Kavraki, ‘‘The open motion planning
spaces trees: A search strategy for motion- and cost-constrained state
library,’’ IEEE Robot. Autom. Mag., vol. 19, no. 4, pp. 72–82, Dec. 2012.
spaces,’’ in Proc. IEEE ICRA, vol. 4. Apr./May 2004, pp. 3968–3973.
[83] S. M. LaValle, ‘‘Motion planning part II: Wild frontiers,’’ IEEE Robot.
[109] A. Ladd and L. Kavraki, ‘‘Fast tree-based exploration of state space for
Autom. Mag., vol. 18, no. 2, pp. 108–118, Feb. 2011.
robots with dynamics,’’ in Algorithmic Foundations of Robotics VI, vol. 17,
[84] S. M. LaValle, ‘‘Motion planning part I: The essentials,’’ IEEE Robot.
M. Erdmann, M. Overmars, D. Hsu, and F. der Stappen, Eds. Berlin,
Autom. Mag., vol. 18, no. 1, pp. 79–89, Jan. 2011.
Germany: Springer-Verlag, 2005, pp. 297–312.
[85] T. Lozano-Perez, ‘‘Spatial planning: A configuration space approach,’’
[110] I. Sucan and L. E. Kavraki, ‘‘A sampling-based tree planner for sys-
IEEE Trans. Comput., vol. 32, no. 2, pp. 108–120, Feb. 1983.
tems with complex dynamics,’’ IEEE Trans. Robot., vol. 28, no. 1,
[86] R. Geraerts and M. H. Overmars, ‘‘Sampling and node adding in proba-
pp. 116–131, Feb. 2012.
bilistic roadmap planners,’’ Robot. Auto. Syst., vol. 54, no. 2, pp. 165–173,
Feb. 2006. [111] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo,
‘‘Choosing good distance metrics and local planners for probabilis-
[87] R. Diankov and J. Kuffner, ‘‘Randomized statistical path planning,’’
tic roadmap methods,’’ IEEE Trans. Robot. Autom., vol. 16, no. 4,
in Proc. IEEE/RSJ Int. Conf. IROS, Nov. 2007, pp. 1–6.
pp. 442–447, Aug. 2000.
[88] L. J. Guibas, C. Holleman, and L. E. Kavraki, ‘‘A probabilistic roadmap
planner for flexible objects with a workspace medial-axis-based sam- [112] J. J. Kuffner, ‘‘Effective sampling and distance metrics for 3D rigid
pling approach,’’ in Proc. IEEE/RSJ Int. Conf. IROS, vol. 1. Oct. 1999, body path planning,’’ in Proc. IEEE ICRA, vol. 4. Apr./May 2004,
pp. 254–259. pp. 3993–3998.
[89] S. A. Wilmarth, N. M. Amato, and P. F. Stiller, ‘‘MAPRM: A prob- [113] H. Long, D. Q. Huy, and S. Mita, ‘‘Unified path planner for parking an
abilistic roadmap planner with sampling on the medial axis of the autonomous vehicle based on RRT,’’ in Proc. IEEE ICRA, May 2011, pp.
space,’’ in Proc. IEEE Int. Conf. Robot. Autom., vol. 2. May 1999, 5622–5627.
pp. 1024–1031. [114] P. Leven and S. Hutchinson, ‘‘Using manipulability to bias sampling
[90] C. Holleman and E. E. Kavraki, ‘‘A framework for using the workspace during the construction of probabilistic roadmaps,’’ IEEE Trans. Robot.
medial axis in PRM planners,’’ in Proc. IEEE Int. Conf. ICRA, vol. 2. Apr. Autom., vol. 19, no. 6, pp. 1020–1026, Dec. 2003.
2000, pp. 1408–1413. [115] C. Peng and S. M. LaValle, ‘‘Reducing metric sensitivity in randomized
[91] V. Boor, M. H. Overmars, and A. F. van der Stappen, ‘‘The Gaussian trajectory design,’’ in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., vol. 1.
sampling strategy for probabilistic roadmap planners,’’ in Proc. IEEE Int. Nov. 2001, pp. 43–48.
Conf. Robot. Autom., vol. 2. May 1999, pp. 1018–1023. [116] C. Peng and S. M. LaValle, ‘‘Resolution complete rapidly-exploring
[92] S. Zheng, D. Hsu, J. Tingting, H. Kurniawati, and J. H. Reif, ‘‘Narrow pas- random trees,’’ in Proc. IEEE ICRA, vol. 1. Jan. 2002, pp. 267–272.
sage sampling for probabilistic roadmap planning,’’ IEEE Trans. Robot., [117] J. Kim, J. M. Esposito, and V. Kumar, ‘‘Sampling-based algorithm for
vol. 21, no. 6, pp. 1105–1115, Dec. 2005. testing and validating robot controllers,’’ Int. J. Robot. Res., vol. 25, no. 12,
[93] D. Hsu and S. Zheng, ‘‘Adaptively combining multiple sampling strategies pp. 1257–1272, Dec. 2006.
for probabilistic roadmap planning,’’ in Proc. IEEE Conf. Robot., Autom. [118] M. Kalisiak and M. van de Panne, ‘‘RRT-blossom: RRT with a local flood-
Mechatron., vol. 2. Dec. 2004, pp. 774–779. fill behavior,’’ in Proc. IEEE ICRA, May 2006, pp. 1237–1242.
[94] S. Thomas, M. Morales, T. Xinyu, and N. M. Amato, ‘‘Biasing samplers to [119] S. Morgan and M. S. Branicky, ‘‘Sampling-based planning for discrete
improve motion planning performance,’’ in Proc. IEEE Int. Conf. Robot. spaces,’’ in Proc. IEEE/RSJ Int. Conf. IROS, vol. 2. Sep./Oct. 2004,
Autom., Apr. 2007, pp. 1625–1630. pp. 1938–1945.
[95] T. Siméon, J. P. Laumond, and C. Nissoux, ‘‘Visibility-based proba- [120] M. S. Branicky, M. M. Curtiss, J. Levine, and S. Morgan, ‘‘Sampling-
bilistic roadmaps for motion planning,’’ Adv. Robot., vol. 14, no. 6, based planning, control and verification of hybrid systems,’’ IEE Proc.
pp. 477–493, Jan. 2000. Control Theory Appl., vol. 153, no. 5, pp. 575–590, Sep. 2006.
[96] J. J. Kuffner and S. M. LaValle, ‘‘RRT-connect: An efficient approach [121] C. Urmson and R. Simmons, ‘‘Approaches for heuristically biasing
to single-query path planning,’’ in Proc. IEEE ICRA, vol. 2. Apr. 2000, RRT growth,’’ in Proc. IEEE/RSJ Int. Conf. IROS, vol. 2. Oct. 2003,
pp. 995–1001. pp. 1178–1183.

74 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

[122] M. B. Kobilarov and G. Sukhatme, ‘‘Near time-optimal constrained tra- [146] K. G. Jolly, R. S. Kumar, and R. Vijayakumar, ‘‘A Bezier curve based
jectory planning on outdoor terrain,’’ in Proc. IEEE ICRA, Apr. 2005, path planning in a multi-agent robot soccer system without violating the
pp. 1821–1828. acceleration limits,’’ Robot. Auto. Syst., vol. 57, no. 1, pp. 23–33, Jan. 2009.
[123] L. Jaillet, J. Hoffman, J. van den Berg, P. Abbeel, J. M. Porta, and [147] Y. Kwangjin, D. Jung, and S. Sukkarieh, ‘‘Continuous curvature path-
K. Goldberg, ‘‘EG-RRT: Environment-guided random trees for kinody- smoothing algorithm using cubic Bezier spiral curves for non-holonomic
namic motion planning with uncertainty and obstacles,’’ in Proc. IEEE/RSJ robots,’’ Adv. Robot., vol. 27, no. 4, pp. 247–258, Mar. 2013.
Int. Conf. IROS, Sep. 2011, pp. 2646–2652. [148] Y. J. Kanayama and B. I. Hartman, ‘‘Smooth local-path planning for
[124] K. Belghith, F. Kabanza, and L. Hartman, ‘‘Randomized path planning autonomous vehicles,’’ Int. J. Robot. Res., vol. 16, no. 3, pp. 263–284, Jun.
with preferences in highly complex dynamic environments,’’ Robotica, 1997.
vol. 31, no. 8, pp. 1195–1208, Dec. 2013. [149] M. Elbanhawi and M. Simic, ‘‘Spline framework for autonomous vehicles
[125] L. Jaillet, J. Cortes, and T. Simeon, ‘‘Sampling-based path planning navigation,’’ Robotica, Under Rev., 2013, to be published.
on configuration-space costmaps,’’ IEEE Trans. Robot., vol. 26, no. 4, [150] B. Raveh, A. Enosh, and D. Halperin, ‘‘A little more, a lot better: Improv-
pp. 635–646, Aug. 2010. ing path quality by a path-merging algorithm,’’ IEEE Trans. Robot., vol. 27,
[126] J. Denny and N. M. Amato, ‘‘The toggle local planner for sampling-based no. 2, pp. 365–371, Apr. 2011.
motion planning,’’ in Proc. IEEE ICRA, May 2012, pp. 1779–1786. [151] R. Luna, I. A. Sucan, M. Moll, and L. E. Kavraki, ‘‘Anytime solution
[127] S. Gottschalk, M. C. Lin, and D. Manocha, ‘‘OBBTree: A hierarchical optimization for sampling-based motion planning,’’ in Proc. IEEE ICRA,
structure for rapid interference detection,’’ in Proc. 23rd Annu. Conf. Oct. 2013, pp. 5068–5074.
Comput. Graph. Interact. Tech., 1996, pp. 171–180. [152] L. Jaillet and T. Simeon, ‘‘Path deformation roadmaps: Compact graphs
[128] M. Reggiani, M. Mazzoli, and S. Caselli, ‘‘An experimental evaluation with useful cycles for motion planning,’’ Int. J. Robot. Res., vol. 27,
of collision detection packages for robot motion planning,’’ in Proc. nos. 11–12, pp. 1175–1188, 2008.
IEEE/RSJ Int. Conf. Intell. Robots Syst., vol. 3. 2002, pp. 2329–2334. [153] J. P. Laumond, S. Sekhavat, and F. Lamiraux, ‘‘Guidelines in nonholo-
[129] C. L. Nielsen and E. E. Kavraki, ‘‘A two level fuzzy PRM for manipulation nomic motion planning for mobile robots,’’ in Robot Motion Planning and
planning,’’ in Proc. IEEE/RSJ Int. Conf. IROS, vol. 3. Nov. 2000, pp. 1716– Control, vol. 229, J. P. Laumond, Ed. Berlin, Germany: Springer-Verlag,
1721. 1998, pp. 1–53.
[130] R. Bohlin and E. E. Kavraki, ‘‘Path planning using lazy PRM,’’ in Proc. [154] B. Xuan-Nam, J.-D. Boissonnat, P. Soueres, and J. P. Laumond, ‘‘Shortest
IEEE ICRA, vol. 1. Apr. 2000, pp. 521–528. path synthesis for Dubins non-holonomic robot,’’ in Proc. IEEE Int. Conf.
Robot. Autom., vol. 1. May 1994, pp. 2–7.
[131] J. Denny, K. Shi, and N. M. Amato, ‘‘Lazy toggle PRM: A single-query
approach to motion planning,’’ in Proc. IEEE ICRA, Apr. 2013, pp. 2407– [155] J. A. Reeds and L. A. Shepp, ‘‘Optimal paths for a car that goes both
2414. forward and backward,’’ Pacific J. Math., vol. 145, no. 2, pp. 367–393,
Oct. 1990.
[132] J. Bialkowski, S. Karaman, M. Otte, and E. Frazzoli, ‘‘Efficient collision
[156] T. Fraichard and A. Scheuer, ‘‘From Reeds and Shepp’s to continuous-
checking in sampling-based motion planning,’’ in Algorithmic Foundations
curvature paths,’’ IEEE Trans. Robot., vol. 20, no. 6, pp. 1025–1035, Dec.
of Robotics X, vol. 86, E. Frazzoli, T. Lozano-Perez, N. Roy, and D. Rus,
2004.
Eds. Berlin, Germany: Springer-Verlag, 2013, pp. 365–380.
[157] L. Z. Wang, K. T. Miura, E. Nakamae, T. Yamamoto, and T. J. Wang,
[133] G. Sánchez and J.-C. Latombe, ‘‘On delaying collision checking in PRM
‘‘An approximation approach of the clothoid curve defined in the interval
planning: Application to multi-robot coordination,’’ Int. J. Robot. Res.,
[0, 5/2] and its offset by free-form curves,’’ Comput.-Aided Des., vol. 33,
vol. 21, no. 1, pp. 5–26, Jan. 2002.
no. 14, pp. 1049–1058, Dec. 2001.
[134] W. Wang, X. Xu, Y. Li, J. Song, and H. He, ‘‘Triple RRTs: An effective
[158] D. J. Walton, D. S. Meek, and J. M. Ali, ‘‘Planar G2 transition curves
method for path planning in narrow passages,’’ Adv. Robot., vol. 24, no. 7,
composed of cubic Bézier spiral segments,’’ J. Comput. Appl. Math.,
pp. 943–962, Jan. 2010.
vol. 157, no. 2, pp. 453–476, Aug. 2003.
[135] W. Wang, Y. Li, X. Xu, and S. X. Yang, ‘‘An adaptive roadmap guided [159] D. S. Meek and D. J. Walton, ‘‘An arc spline approximation to a clothoid,’’
Multi-RRTs strategy for single query path planning,’’ in Proc. IEEE ICRA, J. Comput. Appl. Math., vol. 170, no. 1, pp. 59–77, Sep. 2004.
May 2010, pp. 2871–2876.
[160] D. J. Walton and D. S. Meek, ‘‘A controlled clothoid spline,’’ Comput.
[136] C. Peng, E. Frazzoli, and S. Lavalle, ‘‘Improving the performance of Graph., vol. 29, no. 3, pp. 353–363, Jun. 2005.
sampling-based motion planning with symmetry-based gap reduction,’’ [161] J. McCrae and K. Singh, ‘‘Sketching piecewise clothoid curves,’’ Comput.
IEEE Trans. Robot., vol. 24, no. 2, pp. 488–494, Apr. 2008. Graph., vol. 33, no. 4, pp. 452–461, Aug. 2009.
[137] E. Frazzoli, M. A. Dahleh, and E. Feron, ‘‘Real-time motion planning [162] H. Yifeng and K. Gupta, ‘‘A Delaunay triangulation based node connec-
for agile autonomous vehicles,’’ J. Guid., Control, Dyn., vol. 25, no. 1, pp. tion strategy for probabilistic roadmap planners,’’ in Proc. IEEE ICRA, vol.
116–129, Jan. 2002. 1. Apr./May 2004, pp. 908–913.
[138] D. Ferguson and A. Stentz, ‘‘Anytime RRTs,’’ in Proc. IEEE/RSJ Int. [163] A. M. Ladd and L. E. Kavraki, ‘‘Using motion planning for knot untan-
Conf. Intell. Robots Syst., Oct. 2006, pp. 5369–5375. gling,’’ Int. J. Robot. Res., vol. 23, nos. 7–8, pp. 797–808, Aug. 2004.
[139] Z. Qi dan, W. Ye bin, W. Guo qiang, and W. Xin, ‘‘An improved anytime [164] E. Plaku, K. E. Bekris, B. Y. Chen, A. M. Ladd, and E. E. Kavraki,
RRTs algorithm,’’ in Proc. Int. Conf. AICI, Nov. 2009, pp. 268–272. ‘‘Sampling-based roadmap of trees for parallel motion planning,’’ IEEE
[140] R. Geraerts and M. H. Overmars, ‘‘Creating high-quality paths for motion Trans. Robot., vol. 21, no. 4, pp. 597–608, Aug. 2005.
planning,’’ Int. J. Robot. Res., vol. 26, no. 8, pp. 845–863, Aug. 2007. [165] K. Hauser, T. Bretl, J. C. Latombe, K. Harada, and B. Wilcox, ‘‘Motion
[141] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron, J. Diebel, planning for legged robots on varied terrain,’’ Int. J. Robot. Res., vol. 27,
et al., ‘‘Stanley: The robot that won the DARPA grand challenge,’’ in nos. 11–12, pp. 1325–1349, Nov./Dec. 2008.
DARPA Grand Challenge, vol. 36, M. Buehler, K. Iagnemma, and S. Singh, [166] V. Vonasek, M. Saska, K. Kosnar, and L. Preucil, ‘‘Global motion plan-
Eds. Berlin, Germany: Springer-Verlag, 2007, pp. 1–43. ning for modular robots with local motion primitives,’’ in Proc. IEEE ICRA,
[142] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. N. Clark, et May 2013, pp. 2465–2470.
al., ‘‘Autonomous driving in urban environments: Boss and the urban chal- [167] D. Berenson, S. Srinivasa, and J. Kuffner, ‘‘Task space regions: A frame-
lenge,’’ in DARPA Urban Challenge, vol. 56, M. Buehler, K. Iagnemma, work for pose-constrained manipulation planning,’’ Int. J. Robot. Res.,
and S. Singh, Eds. Berlin, Germany: Springer-Verlag, 2009, pp. 1–59. vol. 30, no. 12, pp. 1435–1460, Oct. 2011.
[143] E. Papadopoulos, I. Poulakakis, and I. Papadimitriou, ‘‘On path planning [168] X. Tang, S. Thomas, P. Coleman, and N. M. Amato, ‘‘Reachable dis-
and obstacle avoidance for nonholonomic platforms with manipulators: A tance space: Efficient sampling-based planning for spatially constrained
polynomial approach,’’ Int. J. Robot. Res., vol. 21, no. 4, pp. 367–383, systems,’’ Int. J. Robot. Res., vol. 29, no. 7, pp. 916–934, Jun. 2010.
Apr. 2002. [169] S. Dalibard, A. El Khoury, F. Lamiraux, A. Nakhaei, M. Taïx, and
[144] Y. Kwangjin and S. Sukkarieh, ‘‘An analytical continuous-curvature J.-P. Laumond, ‘‘Dynamic walking and whole-body motion planning for
path-smoothing algorithm,’’ IEEE Trans. Robot., vol. 26, no. 3, humanoid robots: An integrated approach,’’ Int. J. Robot. Res., vol. 32, pp.
pp. 561–568, Jun. 2010. 1089–1103, Aug. 2013.
[145] Y. Kwangjin and S. Sukkarieh, ‘‘3D smooth path planning for a UAV in [170] E. Frazzoli, M. A. Dahleh, and E. Feron, ‘‘Maneuver-based motion plan-
cluttered natural environments,’’ in Proc. IEEE/RSJ Int. Conf. IROS, Sep. ning for nonlinear systems with symmetries,’’ IEEE Trans. Robot., vol. 21,
2008, pp. 794–800. no. 6, pp. 1077–1091, Dec. 2005.

VOLUME 2, 2014 75
 M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

[171] L. Jaillet and J. M. Porta, ‘‘Path planning under kinematic constraints by [196] S. Karaman and E. Frazzoli, ‘‘Sampling-based algorithms for optimal
rapidly exploring manifolds,’’ IEEE Trans. Robot., vol. 29, no. 1, pp. 105– motion planning,’’ Int. J. Robot. Res., vol. 30, no. 7, pp. 846–894, Jun. 2011.
117, Feb. 2013. [197] C. Guarino Lo Bianco, ‘‘Minimum-jerk velocity planning for mobile
[172] A. Yershova and S. M. LaValle, ‘‘Improving motion-planning algorithms robot applications,’’ IEEE Trans. Robot., vol. 29, no. 5, pp. 1317–1326,
by efficient nearest-neighbor searching,’’ IEEE Trans. Robot., vol. 23, no. Oct. 2013.
1, pp. 151–157, Feb. 2007. [198] B. Akgun and M. Stilman, ‘‘Sampling heuristics for optimal motion plan-
[173] N. M. Amato and L. K. Dale, ‘‘Probabilistic roadmap methods are embar- ning in high dimensions,’’ in Proc. IEEE/RSJ Int. Conf. IROS, Sep. 2011,
rassingly parallel,’’ in Proc. IEEE Int. Conf. Robot. Autom., vol. 1. May pp. 2640–2645.
1999, pp. 688–694. [199] J. Nasir, F. Islam, U. Malik, Y. Ayaz, O. Hasan, M. Khan, et al., ‘‘RRT∗ -
[174] J. Bialkowski, S. Karaman, and E. Frazzoli, ‘‘Massively parallelizing the SMART: A rapid convergence implementation of RRT∗ ,’’ Int. J. Adv.
RRT and the RRT∗ ,’’ in Proc. IEEE/RSJ Int. Conf. IROS, Jul. 2011, pp. Robot. Syst., vol. 10, pp. 1651–1656, Jun. 2013.
3513–3518. [200] A. H. Qureshi, K. F. Iqbal, S. M. Qamar, F. Islam, Y. Ayaz, and N. Muham-
[175] J. Canny, J. Reif, B. Donald, and P. Xavier, ‘‘On the complexity of mad, ‘‘Potential guided directional-RRT∗ for accelerated motion planning
kinodynamic planning,’’ in Proc. 29th Annu. Symp. Found. Comput. Sci., in cluttered environments,’’ in Proc. IEEE ICMA, Aug. 2013, pp. 519–524.
Oct. 1988, pp. 306–316. [201] S. Choudhury, S. Scherer, and S. Singh, ‘‘RRT∗ -AR: Sampling-
[176] B. Donald, P. Xavier, J. Canny, and J. Reif, ‘‘Kinodynamic motion plan- based alternate routes planning with applications to autonomous emer-
ning,’’ J. ACM, vol. 40, no. 5, pp. 1048–1066, Nov. 1993. gency landing of a helicopter,’’ in Proc. IEEE ICRA, May 2013,
[177] S. Sekhavat, P. Svestka, J.-P. Laumond, and M. H. Overmars, pp. 3947–3952.
‘‘Multilevel path planning for nonholonomic robots using semiholonomic [202] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller, ‘‘Anytime
subsystems,’’ Int. J. Robot. Res., vol. 17, no. 8, pp. 840–857, Aug. 1998. motion planning using the RRT∗ ,’’ in Proc. IEEE ICRA, May 2011, pp.
[178] F. Lamiraux, D. Bonnafous, and O. Lefebvre, ‘‘Reactive path deformation 1478–1483.
for nonholonomic mobile robots,’’ IEEE Trans. Robot., vol. 20, no. 6, pp. [203] M. Elbanhawi and M. Simic, ‘‘On the performance of sampling-based
967–977, Dec. 2004. optimal motion planners,’’ in Proc. 7th UKSim/AMSS Eur. Symp. Comput.
[179] F. Lamiraux, E. Ferre, and E. Vallee, ‘‘Kinodynamic motion planning: Model. Simul., 2013, pp. 1–6.
Connecting exploration trees using trajectory optimization Methods,’’ [204] O. Arslan and P. Tsiotras, ‘‘Use of relaxation methods in sampling-based
in Proc. IEEE ICRA, vol. 4. Apr./May 2004, pp. 3987–3992. algorithms for optimal motion planning,’’ in Proc. IEEE ICRA, May 2013,
[180] F. Boyer and F. Lamiraux, ‘‘Trajectory deformation applied to kinody- pp. 2421–2428.
namic motion planning for a realistic car model,’’ in Proc. IEEE ICRA, [205] I. Ko, B. Kim, and F. C. Park, ‘‘VF-RRT: Introducing optimization into
May 2006, pp. 487–492. randomized motion planning,’’ in Proc. 9th ASCC, Jun. 2013, pp. 1–5.
[181] F. Lamiraux, J. P. Laumond, C. Van Geem, D. Boutonnet, and G. [206] G. Goretkin, A. Perez, R. Platt, and G. Konidaris, ‘‘Optimal sampling-
Raust, ‘‘Trailer truck trajectory optimization: The transportation of com- based planning for linear-quadratic kinodynamic systems,’’ in Proc. IEEE
ponents for the Airbus A380,’’ IEEE Robot. Autom. Mag., vol. 12, no. 1, ICRA, Oct. 2013, pp. 2429–2436.
pp. 14–21, Mar. 2005. [207] D. J. Webb and J. van den Berg, ‘‘Kinodynamic RRT∗ : Asymptotically
[182] D. Hsu, R. Kindel, J.-C. Latombe, and S. Rock, ‘‘Randomized kinody- optimal motion planning for robots with linear dynamics,’’ in Proc. IEEE
namic motion planning with moving obstacles,’’ Int. J. Robot. Res., vol. 21, ICRA, May 2013, pp. 5054–5061.
no. 3, pp. 233–255, Mar. 2002. [208] J. Jeong hwan, S. Karaman, and E. Frazzoli, ‘‘Anytime computation of
[183] T. Fraichard and H. Asama, ‘‘Inevitable collision states—A step towards time-optimal off-road vehicle maneuvers using the RRT∗ ,’’ in Proc. 50th
safer robots?’’ Adv. Robot., vol. 18, no. 10, pp. 1001–1024, Jan. 2004. IEEE CDC-ECC, Jun. 2011, pp. 3276–3282.
[184] E. Plaku, L. E. Kavraki, and M. Y. Vardi, ‘‘Motion planning with dynam- [209] S. Karaman and E. Frazzoli, ‘‘Sampling-based optimal motion planning
ics by a synergistic combination of layers of planning,’’ IEEE Trans. for non-holonomic dynamical systems,’’ in Proc. IEEE ICRA, Jan. 2013,
Robot., vol. 26, no. 3, pp. 469–482, Jun. 2010. pp. 5041–5047.
[185] E. Glassman and R. Tedrake, ‘‘A quadratic regulator-based heuristic [210] P. Leven and S. Hutchinson, ‘‘A framework for real-time path planning in
for rapidly exploring state space,’’ in Proc. IEEE ICRA, Feb. 2010, changing environments,’’ Int. J. Robot. Res., vol. 21, pp. 999–1030, Dec.
pp. 5021–5028. 2002.
[186] A. Shkolnik, M. Walter, and R. Tedrake, ‘‘Reachability-guided sam- [211] L. Jaillet and T. Simeon, ‘‘A PRM-based motion planner for dynami-
pling for planning under differential constraints,’’ in Proc. IEEE ICRA, cally changing environments,’’ in Proc. IEEE/RSJ Int. Conf. IROS, vol. 2.
May 2009, pp. 2859–2865. Sep./Oct. 2004, pp. 1606–1611.
[187] E. Frazzoli, M. A. Dahleh, and E. Feron, ‘‘Real-time motion planning [212] J. P. van den Berg, D. Nieuwenhuisen, L. Jaillet, and M. H. Overmars,
for agile autonomous vehicles,’’ in Proc. Amer. Control Conf., vol. 1. Jun. ‘‘Creating robust roadmaps for motion planning in changing environ-
2001, pp. 43–49. ments,’’ in Proc. IEEE/RSJ Int. Conf. IROS, Aug. 2005, pp. 1053–1059.
[188] J. R. Bruce and M. M. Veloso, ‘‘Safe multirobot navigation within dynam- [213] L. Tsai-Yen and S. Yang-Chuan, ‘‘An incremental learning approach to
ics constraints,’’ Proc. IEEE, vol. 94, no. 7, pp. 1398–1411, Jul. 2006. motion planning with roadmap management,’’ in Proc. IEEE ICRA, vol. 4.
[189] A. Shkolnik and R. Tedrake, ‘‘Path planning in 1000+ dimensions Apr. 2002, pp. 3411–3416.
using a task-space Voronoi bias,’’ in Proc. IEEE ICRA, May 2009, [214] R. Gayle, K. R. Klingler, and P. G. Xavier, ‘‘Lazy reconfiguration forest
pp. 2061–2067. (LRF)—An approach for motion planning with multiple tasks in dynamic
[190] M. Kazemi, K. K. Gupta, and M. Mehrandezh, ‘‘Randomized kinody- environments,’’ in Proc. IEEE Int. Conf. Robot. Autom., Apr. 2007, pp.
namic planning for robust visual servoing,’’ IEEE Trans. Robot., vol. 29, 1316–1323.
no. 5, pp. 1197–1211, Oct. 2013. [215] D. Ferguson, N. Kalra, and A. Stentz, ‘‘Replanning with RRTs,’’ in Proc.
[191] A. Shkolnik, M. Levashov, I. R. Manchester, and R. Tedrake, ‘‘Bounding IEEE ICRA, May 2006, pp. 1243–1248.
on rough terrain with the LittleDog robot,’’ Int. J. Robot. Res., vol. 30, no. 2, [216] J. van den Berg, D. Ferguson, and J. Kuffner, ‘‘Anytime path planning and
pp. 192–215, Feb. 2011. replanning in dynamic environments,’’ in Proc. IEEE ICRA, May 2006, pp.
[192] B. Lau, C. Sprunk, and W. Burgard, ‘‘Kinodynamic motion planning for 2366–2371.
mobile robots using splines,’’ in Proc. IEEE/RSJ Int. Conf. IROS, Oct. [217] M. Zucker, J. Kuffner, and M. Branicky, ‘‘Multipartite RRTs for rapid
2009, pp. 2427–2433. replanning in dynamic environments,’’ in Proc. IEEE Int. Conf. Robot.
[193] E. Koyuncu and G. Inalhan, ‘‘A probabilistic B-spline motion planning Autom., Apr. 2007, pp. 1603–1609.
algorithm for unmanned helicopters flying in dense 3D environments,’’ in [218] N. A. Melchior and R. Simmons, ‘‘Particle RRT for path planning with
Proc. IEEE/RSJ Int. Conf. IROS, Sep. 2008, pp. 815–821. uncertainty,’’ in Proc. IEEE Int. Conf. Robot. Autom., Apr. 2007, pp. 1617–
[194] K. Macek, M. Becked, and R. Siegwart, ‘‘Motion planning for car-like 1624.
vehicles in dynamic urban scenarios,’’ in Proc. IEEE/RSJ Int. Conf. Intell. [219] H. Yifeng and K. Gupta, ‘‘RRT-SLAM for motion planning with motion
Robots Syst., Oct. 2006, pp. 4375–4380. and map uncertainty for robot exploration,’’ in Proc. IEEE/RSJ Int. Conf.
[195] S. Karaman and E. Frazzoli, ‘‘Optimal kinodynamic motion planning IROS, Sep. 2008, pp. 1077–1082.
using incremental sampling-based methods,’’ in Proc. 49th IEEE CDC, [220] A. Bry and N. Roy, ‘‘Rapidly-exploring Random Belief Trees for motion
Dec. 2010, pp. 7681–7687. planning under uncertainty,’’ in Proc. IEEE ICRA, Sep. 2011, pp. 723–730.

76 VOLUME 2, 2014
M. Elbanhawi and M. Simic: Sampling-Based Robot Motion Planning

[221] M. W. Achtelik, S. Weiss, M. Chli, and R. Siegwart, ‘‘Path planning for MILAN SIMIC received the B.E.E., M.E.E., and
motion dependent state estimation on micro aerial vehicles,’’ in Proc. IEEE Ph.D. degrees in electronics from the University
ICRA, May 2013, pp. 3926–3932. of Nis, Serbia, and Grad Dip.Ed. degree from
[222] C. Fulgenzi, C. Tay, A. Spalanzani, and C. Laugier, ‘‘Probabilistic navi- RMIT University. He is a conducting management,
gation in dynamic environment using rapidly-exploring random trees and research and teaching with RMIT University. Cur-
Gaussian processes,’’ in Proc. IEEE/RSJ Int. Conf. IROS, Sep. 2008, rently, he is a Lecturer of mechatronics, electronics
pp. 1056–1062. and management with the School of Aerospace,
[223] S. Chakravorty and S. Kumar, ‘‘Generalized sampling-based motion plan-
Mechanical and Manufacturing Engineering. His
ners,’’ IEEE Trans. Syst., Man, Cybern., B, Cybern., vol. 41, no. 3, pp. 855–
main research interests are in digital communi-
866, Jun. 2011.
[224] A.-A. Agha-mohammadi, S. Chakravorty, and N. M. Amato, ‘‘FIRM: cations, robotics, autonomous systems, and green
Sampling-based feedback motion planning under motion uncertainty and energy.
imperfect measurements,’’ Int. J. Robot. Res., Nov. 2013.

MOHAMED ELBANHAWI received the B.Eng.

degree in advanced manufacturing and mechatron-
ics from RMIT University, Australia, in 2012,
where he is currently pursuing the Ph.D. degree.
His research interests include motion planning,
car-like robots, and autonomous systems.

VOLUME 2, 2014 77