Survey: Motion Planning Algorithms

by Leslie Glaves

lglaves@optonline.net

CSE638 Topics in Computational Geometry

Professor Joe Mitchell

Stony Brook University, New York 11794

May 2004

Abstract

This paper introduces and surveys current progress in path/motion planning al-gorithms with greater attention given to recent work with RRTs (Rapidly-ExploringRandom Trees).

1 Introduction

The perennial pursuit of artificial intelligence, that of constructing a robot that can learn

on it’s own can be distilled to solving a few essential problems. For a mobile robot, a basic

problem is how to plan to move, alternately referred to as path planning or motion planning.

Although robotic applications are currently one of the most popular and highly publicized

topics in motion planning, planning and solving collision free paths is required for many

other computational pursuits. Motion planning is used in graphic animation and video

game software, navigation within virtual environments, physics-based simulation, CAGD

(computer-aided geometric design), molecular dynamics (such as binding and folding), vir-

tual prototyping, computer controlled medical surgery and for computerized navigation of

all types of vehicles (wheeled, aircraft, spacecraft, etc). Study of motion planning is encom-

passed by several disciplines, including (but not limited to): Computer Science, Artificial

Intelligence, Robotics, Mechanical Engineering, Computational Geometry and Differential

Geometry.

1

The most basic motion planning problem is to compute a collision-free path from start loca-

tion to goal location in a static environment containing obstacles which must be manuvered

around. In reality, motion planning often involves reaching a goal location while obstacles

are moving in the environment and optimized paths are desired. Motion planning has been

a serious study area for at least three decades now. What has become the ”classical” motion

planning problem was first analyzed in 1979 by Reif (also referred to as the ”piano mover’s

problem”). The problem required finding a path to move a rigid 6 degrees of freedom object

among obstacles from one location to another.

With the considerable increases in computing power over the past decade or so, there has

been a flurry of new research into robotics motion planning. Motion planning analysis,

research and experimentation with robotics is expected to be generalizable to many other

areas. This paper will introduce some necessary terminology and problem specification

terms first, and then describe some of the more current motion planning algorithms and

their applications, discuss shortcomings and suggest some future research directions.

2 Problem Specification and Terminology

2.1 Constraints on Motion

The motions of all moving objects are subject to physical laws and geometric constraints.

We refer to holonomic and non-holonomic constraints in studying motion planning.

Holonomic Path Planning Holonomic refers to a basic path planning problem in which

the robot’s absolute position and orientation is considered to be attainable, or is simply not

considered as a constraint on the problem solution. This would be the most basic path

planning problem we can consider. Holonomic kinematics can be expressed as algebraic

equations. An example of holonomic motion would be that of the point robot in a two-

dimensional planar world.

Non-holonomic Path Planning A Non-holonomic system is one in which a return to

an absolute position and orientation cannot be guaranteed without considering differential

relationships, such as relationship between wheel rotation and position/orientation of the

vehicle or robotic base. Nonholonomic planning requires incorporating corrections for wheel

slippage, as in the case of a moving vehicle. Calculation of non-holonomic constraints

2

requires the use of differential geometry and control theory. Examples of non-holohomic

motions are those achieved by pushing, pulling, rolling, bouncing and flying.

2.2 Motion Planning Goal

Path A continuous, collision-free path of movement is to be found, given an initial S

(start) position and a final position, T (terminal). For state space, a common notation is

xinit for a start location, and xgoal for the goal location, or Xgoal for a goal region.

Kinodynamic Path Planning Kinodynamic path planning incorporates dynamic con-

straints on velocity and acceleration into the planner, in addition to obstacle avoidance.

Earlier path planning algorithms would first generate a solution path in the configuration

space and then afterward determine control inputs required for the robot to travel along

the path. Real, physical world path planning problems must incorporate both static and

dynamic constraints. The motion planning goal of the algorithms described in this paper

address this by employing the state space problem specification, which will be described

later in this section.

2.3 Configuration Space Specification

DOF or Degrees of Freedom refers to direction of movement for the object or robot

which will travel the path. In the R2 plane, 2 dof would refer to movement in the x and y

directions, whereas 3 dof would in the R2 plane would include rotation. Translation in R3

results in 3 dof for movement in the x, y, and z directions. If the robot can also rotate in

R3, it has 6 dof .

Motion planning in simpler cases involves locating a path of travel within a fixed (2-

dimensional or 3-dimensional) environment which usually contains obstacles. A conven-

tional combinatorial paradigm consists of the following:

C-space or configuration space refers to the parameter space within which a robot may

move. Robot may generally be used to refer to any moving object. In the simplist case,

we consider movement in the 2-dimensional Euclidean plane. The regions not accessible to

the robot are called configuration space obstacles. Free space denoted C-free, is the total

configuration space minus C-obs, the space that is occupied by obstacles.

3

Various methods developed in computational geometry have been used to represent the

configuration space. Several examples described in LaValle’s Planning Algorithms are:

• Cellular Decomposition – Partition C-free into simpler (connected) neighboring cells.

Construct a graph with vertices representing the cells, and edges between the vertices

of connected cells.

• Voronoi Roadmap – Can be used to construct a roadmap which is centered between

C-obs, which are represented as polygons.

• Visibility Roadmap – By constructing line segments between polygon vertices of C-

obs, and treating start and goal positions as vertices, the optimal (shortest) path

can be found. Subsequent minor modification to provide clearance between obstacles

results in a path planning solution.

Point Robot Simplification In this most basic representation of the motion planning

problem, the Robot is reduced to a single point translating in planar ”world”. Robot, R or P,

generally refers to any moveable object with particular geometric characteristics. The point

robot is typically a reference point somewhere within or on the boundary of the geometric

configuration of the robot or moving object.

Minkowski Sums are used in motion planning to dilate the geometric configuration of

obstacles in the C-space to account for the robot’s footprint and degrees of freedom when

working with the point robot simplification.

2.4 State Space Specification

As formulated in [LK01]:

We can formulate the problem in terms of the following six components:

1. State Space: A topological space, X

2. Boundary Values: xinit ∈ X and xgoal ∈ X, or Xgoal ⊂ X

3. Collision Detector: A function, D : X −→ {true,false}, that determines whether global

constraints are satisfied from state x. This could be a binary-valued or real-valued function.

4. Inputs: A set, U , which specifies the complete set of controls or actions that can affect the

state.

5. Incremental Simulator: Given the current state, x(t), and inputs applied over a time interval,

{u(t)|t ≤ t≤t +4t).

4

6. Metric: A real-valued function, ρ : X × X −→ [0,∞), which specifies the distance between

pairs of points in X.

The inputs (or controls) and incremental simulator will specify possible changes in the state

of the system. The incremental simulator will indicate reachable next discrete states for the

system (given a time-step), which are the response of a discrete-time system, or a continuous

time system that has been discretized.

3 Brief History

Since the 1980’s, researchers in Computer Science, Artificial Intelligence, Mechanical Engi-

neering and Mathematics have been investigating the theoretical and practical aspects of

motion planning. Most recently, the focus on pratical applications has resulted in numerous

successful implementations by developing motion-planning software systems specific to the

particular problem.

Research in robotic path planning has been studied since the early 1970’s [18]. The first

formally proposed motion planner (for a robot) was presented at the 1st International Joint

Conference on AI, by Nilsson of Stanford Research Institute. This motion planner used the

visibility graph method with an A* search algorithm to find a minimized collision-free path

through polygonal obstacles [18].

In the early 1980’s, working with the concept of configuration space and reduction of the

planning problem to a point robot in a parameter space which incorporates the robot’s

dofs was introduced by Lozano-Perez and Wesley [28]. Additionally, formal analysis of the

complexity of motion planning was conducted by John Reif in 1979.

As a result of the computational complexity of motion planning, instead of searching for

optimal paths, focus has been on achieving practical results by using probabilistic algorithms.

The first well known sampling-based motion planner (developed at Stanford in 1990) was

the (RPP) Randomized Path Planner of Barraquand & Latombe.

4 Algorithmic Complexity

One of the earliest algorithmic examinations of motion planning, conducted by John Reif,

now referred to as the ”piano mover’s problem”, or ”Generalized Mover’s Problem” [36],

is an analysis of moving a robot with arbitrary numbers of degrees of freedom from a

5

start to a finish goal among static obstacles in both 2-D and 3-D space. Reif found the

problem in 3-D to be PSPACE-hard, polynomial space hard for a robot with n dof ′s. The

complexity analysis simulated the dof ′s with a polynomial-space bounded Turing Machine

moving among obstacles which make the robot’s motions simulate the Turing Machine

computation.

Later analyses of path planning based on algebraic decomposition of the configuration

space has shown the problem to be doubly exponential in the number of dof ′s [37]. A

singly-exponential algorithm which computes the free space as a network of 1-D curves was

theoretically developed by [6], but not implemented.

In 1988, the motion planning problem was proven to be PSPACE-hard by John Canny. We

now know the lower bound to be PSPACE-hard and the upper bound is suspected to be

exponential in number of dof of the robot.

There has been much analysis of varieties of algorithmic motion planning problems, includ-

ing shortest path, minimal-time trajectory planning, dynamic motion planning (maneuver-

ing among moving obstacles), motion planning with uncertainty (deviation from planned

path due to control and position sensing errors), and kinodynamic motion planning. All

are prohibitively complex in computations. Practical solutions succeed by using approxi-

mations, randomization (with uniform coverage), and parallelization.

5 Motion Planning Algorithm Types

5.1 Complete Algorithms

A complete (or exact) algorithm in motion planning is one that either finds the path between

a start and final configuration (should one exist), or reports that there is no such path.

Complete algorithms require a mapping of the configuration space to work with. The

two most popular techniques used are cell decomposition and roadmap methods. Cellular

decomposition maps free space to a graph which encodes the connections between cells of

the space. In the roadmap method, a network of curves is extracted from free space and

then different roads are connected to plan the path from start to goal state.

6

5.2 Probabilistic Algorithms

Randomized probabilistic algorithms for motion planning are not guaranteed to find a path

from start to goal, but if the algorithm does successfully locate a path, it will be found

relatively quickly. By using a probabilistic algorithm, a trade off is made between exactness

and running time. Sometimes the algorithm will solve the planning problem quickly, other

times not so quickly, and analysis of the algorithm’s success is based on an average, expected

running time and space. Since a different path planning solution can be generated each time

the algorithm is run, accurate analysis of the algorithm’s speed requires averaging over many

trials.

5.3 Deterministic

A deterministic algorithm does not employ any random selections. Therefore, the solution

generated by a deterministic algorithm will be exactly the same every time the algorithm

is run for a specific motion planning task.

5.4 Heuristic Algorithms

Heuristic algorithms find solutions from among all possible ones, but do not guarantee the

best solution. Heuristic algorithms find a solution close to the best in a quick way. Often a

heuristic algorithm employs a greedy method and ignores or simplifies some of the problem’s

demands.

Numerous algorithms using heuristic techniques have been explored. These algorithms

usually deal with the free configuration spaces modeled as a regular grid over which a search

is conducted for the goal state. One such approach uses potential fields, another heuristic

technique employs hierarchical space decomposition, where some grid cells are chosen over

others (in a hierarchical fashion) to be searched for the goal state.

6 Motion Planning Classifications

As described in LaValle’s Planning Algorithms book, we can classify existing approaches

to motion planning as either combinatorial, sampling-based, or decision-theoretic. The

motion planning problem has been analyzed combinatorially for complexity results and

early motion planners were combinatorial. More recently, sampling-based motion planning

7

is being analyzed in simulation and implemented in robotic hardware. Some practical results

appear to have been achieved. This survey will not cover any decision-theorectic motion

planning algorithms.

6.1 Combinatorial Motion Planning

Combinatorial motion planning requires a preprocessed computational representation of a

map of the entire configuration space of the problem to search for path planning solutions.

In the 1980s the predominant procedure followed for motion planning strategy began with

constructing a computational representation of C-space. The combinatorial motion plan-

ning algorithm is a ”complete” algorithm, in that it is guaranteed to find a path between

two configurations, should such a path exist. The classical grid search, combinatorial ap-

proach to motion planning is prohibitively expensive to compute for any but the simplest

C-space representations. The ”curse of dimensionality”, [Bellman61] refers to the exponen-

tial growth of hypervolume as a function of dimensionality. For practical motion planning,

we resort to approximation techniques, such as random sampling and employing probabilistic

algorithms.

6.2 Sampling-Based Motion Planning

Sampling-based motion planning does not require an existing map to plan with, but instead

consists of an integrated system which incorporates several separate modules. Planning is

accomplished by incremental steps, taking samples of the configuration space and planning

in discrete increments. The motion planning algorithm obtains its information about C-free

from the collision detection module.

A current typical decomposition of the problem uses three modules:

Integrated System

• Geometric Model– handles representation of the configuration space

• Collision Detection – probes the configuration space for accessibility

• Motion Planner– generates motion decisions

By decomposing the problem into modules, research and development can be addressed

independently, and hence, generalization of the motion planning problem into different

8

motion planning domains is simplified. This approach has been particularly successful for

useful applications in recent years.

7 Current Research – Sampling-Based

Dealing with he complexity of motion planning has necessitated a focus on the more practical

development of motion planners which most often are specific to the application required.

Motion planning is a very extensive research field with many open problems and much room

for improvement. There is now greater demand for a practical and inexact probabilistic

approach to solving motion planning than there is for further understanding of theory

and computational complexity of the problem. Even though greater computing speed and

number of computations has enabled significantly successful execution of motion planning

algorithms, developing better practical results still resolves to the basic necessity of reducing

computational complexity as much as possible. Two recent developments in motion planning

which are actively being researched and developed are the PRM (Probabilistic Roadmap)

and the RRT (Rapidly-exploring Random Tree).

7.1 The (PRM) Probabilistic Roadmap

The PRM was originated by Kavraki in her PhD Thesis, [12]. It has been used to compute

collision-free paths in configuration spaces containing stationary obstacles. The planner

consists of a two-stage algorithm:

Step One – PreprocessingFrom [23], Algorithm to build the PRM:

BUILD PRM

1 G.init();

2 for i = 1 to N

3 q ←− RAND FREE CONF(q);

4 G.add vertex(q);

5 for eachv ∈ NBHD(q,G);

6 if ((not G.same component(q, v) and

7 CONNECT(q, v) then;

8 G.add edge(q, v);

The algorithm constructs a PRM with N vertices. In Step 3, a pseudo-random configuration in C-free

is found by repeatedly picking a pseudo-random configuration until one is determined by a collision

detection algorithm to be in C-free. The NBHD function in line 5 is a range query in which all

vertices within a specified distance from q are returned, sorted by distance from q (other variations

9

possible). ... Over a certain range space, it is important to have the points distributed so that

NBHD contains a sufficient number of points. In Step 6, it is sometimes preferable to replace the

condition(not G.same component(q, v))with G.vertex degree(q) ¡ K, for some fixed K (e.g., K = 15).

... The CONNECT function in line 7 uses a fast local planner to attempt a connection between q and v.

Usually, a ”straight line” path in C-free is evaluated between q and v by stepping along incrementally

with a collision detection algorithm.

Step Two – QueryFrom [23]:

Once the PRM has been constructed, the query phase attempts to solve planning problems.

Essentially, qinit and qgoal are treated as new nodes in the PRM, and connections are

attempted. Then, standard graph search is performed to connect qinit to qgoal. If the

method fails, then either more vertices are needed in the PRM, or there is no solution.

This is analogous to the problem of insufficient resolution in the classical grid search.

These two stages need not be executed entirely sequentially. By repeatedly alternating

them, the roadmap can be made more adaptable to difficult configuration spaces and serve

as a type of incremental learning algorithm.

7.1.1 PRM Variations

NOTE: This section may be expanded at some future time.

7.1.2 PRM Synopsis

The PRM has been demonstrated to be a reliable path planner suitable for multiple-query

holonomic planning. Multiple queries refer to searching the configuration space repeatedly

by using the same initial preprocessed mapping. The probabilistic roadmap planner is

known to encounter difficulties in long, narrow spaces, and there has been experimentation

with numerous variants to find a better approach for these specific situations. The PRM

constructs a graph of C-space by generating random configurations in C-free and then using

a local planner to connect pairs of nearby configurations. This basic PRM planner is not

efficient for capturing dynamic or nonholonomic constraints, due to the likelihood of having

to search thousands of states to find a solution in these cases, however, variations to the

basic PRM have been reported to achieve efficient high dof planning.

On the other hand, the RRT which will be explored in the next section, can manage dynamic

and nonholonomic constraints because its data structure, a graph, is built from an initial

state to the next possible states by using the set of available actions, versus the larger cost

of mapping a sampling scheme of all configurations.

10

The RRT is more recent and appears suitable for a larger variety of motion planning prob-

lems, acordingly, this paper will focus more on the Rapidly-Exploring Random Tree.

7.2 The (RRT) Rapidly-exploring Random Tree

The RRT is the algorithm and data structure developed by Steve LaValle in 1998. The

RRT is useful for motion planning in high-dimensional configuration spaces with differential

constraints (both nonholonomic and dynamic) and algebraic constraints (due to obstacles)

[21]. The RRT algorithm incrementally generates a search tree of the configuration space.

Its key feature is to bias exploration toward the largest unexplored areas of C-space, (which

consist of the largest volume voronoi regions, or areas of largest dispersion). The RRT is a

path planning module which must be incorporated into an integrated system. By random

searching, kinodynamic planning RRTs are able to find (non-optimal, but close to optimal)

path plans in high dimensional state spaces and avoid the prohibitive computation which

would be required to calculate all discrete state spaces.

As described by LaValle and available at [http://msl.cs.uiuc.edu/rrt/about.html]:

Here is a brief description of the method for a general configuration space, C. The configuration space

can be considered as a general state space that might include position and velocity information. The

pseudocode for constructing an RRT that is rooted at a configuration qinit and has K vertices is as

follows:

BUILD RRT(qinit, K,4q)

1 G.init(qinit);

2 for k = 1 to K

3 qrand ←− RAND CONF();

4 qnear ←− NEAREST VERTEX(qrand, G);

5 qnew ←− NEW CONF(qnear,4q);

6 G.add vertex(qnew);

7 G.add edge(qnear, qnew);

8 Return G

Step 3 chooses a random configuration, qrand, in C. Alternatively, one could replace RAND CONF with

RAND FREE CONF, and sample configurations in C-free (by using a collision detection algorithm to

reject samples in C-obs).

It is assumed that a metric ρ has been defined over C. Step 4 selects the vertex, xnear, in the RRT, G

that is closest to qrand. This can be implemented as shown below.

NEAREST VERTEX(q, G)

1 d←−∞;

2 for each v ∈ V

3 if p(q, v) < d then

4 vnew = v; d←− p(q, v);

5 Return q;

11

In Step 5 of BUILD RRT, NEW CONF selects a new configuration, qnew, by moving from qnear an

incremental distance, 4q, in the direction of qrand. This assumes that motion in any direction is

possible. If differential constraints exist, then inputs are applied to the corresponding control system,

and new configurations are obtained by numerical integration. Finally, a new vertex, qnew is added,

and a new edge is added from qnear to qnew.

7.2.1 RRT Variations, Experimentation and Analysis

• RRT-Connect

Single-query planning refers to solving a single path from start to goal position quickly,

without preprocessing of the configuration space. RRT-Connect is an algorithm origi-

nally developed for use in manipulation planning for animated characters in 3D virtual

environments. The algorithm utilizes a greedy heuristic to connect two trees, which

start from the initial and goal configurations. The two trees alternately extend them-

selves, while exploring C-free and trying to connnect to each other. In [14] Kuffner

and LaValle performed simulation experiments using RRT-Connect and found it to

improve running time in uncluttered environments by a factor of 3 or 4 (based on

analysis of ”hundreds” of experiments with over a dozen examples, and averaging

over 100 trials for each). They prove the planner to be probabilistically complete

and describe suggestions for further experimental evaluation. In [11], RRT-Connect

is described as having been used to solve two of the benchmark puzzles, the ”Alpha

1.0 puzzle” and the ”Flange 1.0 problem”, which are difficult due to narrow passages

in C-space. [14] mention the need for theoretical analysis of convergence rate using

RRTs.

• RRT Theoretical Analysis

[24] examine several variations of path planners using RRTs. Although certain aspects

of RRTs can be verified by theoretical analysis, LaValle and Kuffner, in [24] describe

experimental successes which cannot be theoretically analyzed by current methods.

In this paper, it is proven that the RRT will come arbitrarily close to any point in

both convex and nonconvex space. For holonomic and nonholonomic RRT planners,

probability that the RRT will find a path from init to goal approaches 1 as the number

of vertices approach infinity. It is proven that if a connection sequence of length k

exists, the expected number of iterations to connect the path from init to goal is

not more than kp , where p represents the probability of a successful outcome with

iterations considered as Bernoulli trials. Additionally, it is proven that the probability

of success increases exponentially with the number of iterations. This article addresses

12

the need to maximize the efficiency of both the Nearest-Neighbor Algorithm used in

the integrated system and also the efficiency of the Collision Detection Module.

• An Application: Execution Extended RRT (ERRT)

Bruce and Veloso [5] developed an RRT based system that interleaves planning and

execution and have evaluated it in simulation and for application to real physical

robots. Extensions to the RRT were made, using a waypoint cache and adaptive

cost penalty search. The ERRT described in this paper used a KD-tree for nearest

neighbor lookup. The waypoint cache consisted of maintaining a constant size array

of states and placing all states from a plan into the cache with random replacement,

whenever a plan was found. Thus, three possibilities were now available for next target

states: goal, waypoint (a random waypoint), and uniformly random state selection.

The probabilities of selection choice are adjustable. The adaptive beta search was

used to allow the planner to modify a parameter to help find shortest paths. By

adjusting the distance metric by multiplying some gain value (beta), path length

from root to leaves of the RRT versus amount of exploration of state space can be

adjusted. A higher beta value would result in shorter paths and a decrease in state

space exploration, keeping the path close to the initial state (a ”bushy” tree). A beta

of 0 is equivalent to the basic RRT algorithm. Employing an adaptive bias schedule,

the ERRT was made to shorten the path length as successive trials were run. Bruce

and Veloso explored this algorithm in 2D configuration space and also for a multi-robot

(RoboCup) control system. It was found that waypoints improved path planning to

escape local minima or oscillation points, without the use of adaptive beta. They

used KD-tree for nearest neighbor lookup and verified that it significantly improved

performance, (expected complexity advantage, E[O(log(n)) vs. E[O(n)] for the naive

version). The robot system incorporates a vision system to detect the environment,

path planner module, collision detection module and a movement control module.

Using ERRT with post processing to iteratively test to replace the head of the plan

for a straight path for Robocup F180 robot (small robot team) control resulted in the

best performance of any other systems tried on the robots (in 2002). Speedup was

from 1.2ms to 1.7m

s . Additionally, replacing the pre-existing reactive control with the

ERRT was not difficult.

• Resolution Complete RRTs (RC-RRT)

Resolution completeness refers to finding a motion planning solution after a finite num-

ber of iterations. In [7], a resolution complete trajectory design for an underactuated

12-dof spacecraft with 3 thrusters moving in a 3D grid was analyzed and experi-

13

mented with. Cheng and LaValle describe proofs of resolution completeness and two

experimental results comparing RC-RRT with regular goal-biased RRT and RC-RRT

with RRTExtExt (Bi-directional planning using RRTs). Resolution completeness is

achieved by using Lipschitz conditions and an accessibility graph. Optimization of

path planning solutions is explored.

• Current Issues

Randomized sampling-based motion planners obtain their information about the con-

figuration space through a collision detector, and incrementally explore configuration

space, rather then depend on creating a preprocessed mapping of C-free. Linde-

mann and LaValle [26] credit the current success with motion planning algorithms

to separating the analysis of C-obs from the motion planner and relegating this task

to the collision detector. In their paper ”Current Issues in Sampling-Based Motion

Planning”, a history and survey of important issues to be addressed for further de-

velopment and improvement of sampling-based planners is discussed. The authors

suggest that future investigation of deterministic sampling might provide a better un-

derstanding of the motion planning problem, rather than reliance on more randomized

schemes.

• Run-Cost Variance as a Performance Measure

In [35], the authors advocate for and describe use of run-cost variance to more accu-

rately evaluate and compare performance characteristics of various motion planning

algorithms. Randomization in motion planners can result in wide variations in running

time required to solve path plans. Not only does this prevent an accurate assessment

of performance, but also can result in unreliable performance. Accurate performance

assessment and comparisons seem to be lacking in published literature. The authors

define an efficiency measure:

Let F (C) be the total number of collision-free configurations examined by the planner until

examination of configuration C.

Let g(C) be the distance from start configuration to the current configuration, C.

Then, O(C) =F (C)g(C)

By using O(C) to both measure and restrict the local planner (in the case of PRM), an

adaptable limit to reduce overall roadmap construction cost is created. At the local

planner level, whenever O(C) reaches the threshold level, further searching of that

configuration is halted. Experimental runs of 240 trials using two different adaptive

strategies were found to be successful for reducing run-cost and standard deviation

of the run-cost. It is noted that the run-cost could be applied the the RRT planner.

14

Also noted is the need to rely on empirical evaluation due to the lack of a theoretical

model for random search techniques and run-cost restrictions on the search of paths

in C-space.

7.2.2 RRT Synopsis

The RRT (or RDT, as mentioned in the slide presentation to accompany this survey) is

still in early stages of development and evaluation. It appears to be a very viable motion

planning algorithm, and one which may prove useful in an Artificial Intelligence, or what is

referred to as ”machine learning” type of application, given it’s exploration strategy.

8 Other Motion Planning Ideas

Other approaches to motion planning that are being developed include the following:

• Constraint-Based Planners Maxim Garber and Ming Lin of University of North

Carolina have been working with this planner. It considers motion planning to be a

dynamical system with constraints as forces imposed on the system. They employ

accelerated graphics hardware to compute an approximation of the voronoi diagram,

referred to as a GVD, (see below).

• (GVD) Generalized Voronoi Diagram Planners The GVD uses the voronoi

diagram to provide paths of maximal clearance among obstacles in the configuration

space for path planning. Accelerated GVD calculation is done by using the graphics

hardware Z-buffer (depth buffer) to quickly get information for motion planning.

• Fast Marching Methods and Level Set Methods The Fast Marching Method is

a boundary value formulation and the Level Set refers to an initial value formulation.

These numerical techniques designed to track the evolution of interfaces are being

employed for some type of path planning.

• Meta-Planning Systems Framework systems to automatically select and use spe-

cialized modules based on configuration specficis.

• Various Visibility Based Planners

15

9 Discussion

Given the myriad of motion planning algorithms (and variations) being developed, and the

claims on their performance, a unified comparision method to gauge performance would be

most useful. There is a particular difficulty in comparing a map-first, then search method

to an incremental method. Benchmarks being posted on the internet may be useful, but

published results of empirical side-by-side performance comparisons between planners seem

to be unavailable. More work in both testing and theoretical analysis remains to be done.

Of course, development of a practical and reliable motion planner that can be marketed is

the more pressing concern of most software developers.

Suggestions:

• Establish specific simple, basic geometric models for evaluating motion planning al-

gorithms.

• Publish more statistical results on motion planner problem solving to enable useful

comparisons.

• Classify effective sampling strategies based upon motion constraints and configuration

space characteristics.

• Can there be an equitable comparison method between the PRM and RDT? Measures

to consider: Time to solve the path, number of computations to solve a path, number

of graph vertices traversed to solve the path, number of collision checks.

16

10 Public Domain Software Packages

10.1 Motion Planning Software

• Move 3Dhttp://www.laas.fr/~nic/Move3D/

Move3D is a software platform for experimenting with robot motion planning. Move3D can

model different mechanical systems: free-flying, manipulators, nonholonomic mobile vehicles,

mobile manipulators, etc.

The library contains a set of functionalities to assist in the development of new motion planners:

mechanical system modeling, collision checkers (currently I COLLIDE and V COLLIDE devel-

opped by Lin and Manocha), steering methods (currently linear paths, Reeds & Shepp paths,

Manhattan paths, helices . . . )

One motion planner included is the Probabilistic RoadMap (PRM) algorithm introduced by

Kavraki, Latombe, Overmars and Svetska. PRM generates collision-free configurations randomly

and tries to link them with a simple steering method. A roadmap is then computed, tending to

capture the connectivity of the collision-free configuration space.

A second planner is included, which takes advantage of the visibility notion [IROS99]. While

usually each collision-free configuration generated by the PRM algorithm is integrated to the

roadmap, our algorithm keeps only configurations which either connect two connected components

of the roadmap, or are not “visible” by some so-called guards.

• Motion Strategy Libraryhttp://msl.cs.uiuc.edu/msl/

The Motion Strategy Library (MSL) allows easy development and testing of motion planning

algorithms for a wide variety of applications. The software architecture is object-oriented and the

general design is highly modular. It was developed on a Linux system using GNU C++, STL,

and the FOX GUI Toolkit.

The MSL is available as Open Source, free for both academic and commerical use. Presently MSL

includes planners based on Rapidly-exploring Random Trees (RRTs), Probabilistic Roadmaps

(PRMs), and forward dynamic programming (FDP).

• Motion Planning Puzzles (aka Benchmarks)http://parasol.tamu.edu/groups/amatogroup/benchmarks/

Models for a collection of benchmark problems to be used for comparing various motion planning

algorithms. Available for public, non-commercial use provided that appropriate reference is made

to the source/creator of the model. In BYU format, which is an ASCII file format for polygonal

meshes.

• MPK - Motion Planning Kithttp://robotics.stanford.edu/~mitul/mpk/

MPK is free for research and non-commercial use

MPK features a C++ library and workbench for motion planning

10.2 Nearest Neighbor Software

• ANN: Approximate Nearest Neighbor Softwarehttp://www.cs.umd.edu/~mount/ANN/

17

ANN is a library written in C++, which supports data structures and algorithms for both exact

and approximate nearest neighbor searching in arbitrarily high dimensions. It was written by

David Mount and Sunil Arya.

• Ranger - Nearest Neighbor Search in Higher Dimensionshttp://www.cs.sunysb.edu/~algorith/implement/ranger/implement.shtml

OBSOLETE: Ranger is a tool for visualizing and experimenting with nearest neighbor and or-

thogonal range queries in high-dimensional data sets, using multidimensional search trees. It was

developed by Michael Murphy as his masters project under Steven Skiena at Stony Brook. Four

different search data structures are supported by Ranger: Naive k-d, Median k-d, Sproull k-d and

VP-tree.

10.3 Collision Detection Software

• ColDet - Free 3D Collision Detection Libraryhttp://photoneffect.com/coldet/

A free collision detection library for generic polyhedra. Its purpose is mainly for 3D games where

accurate detection is needed between two non-simple objects.

• GJK Algorithmhttp://web.comlab.ox.ac.uk/oucl/work/stephen.cameron/distances/index.html

Enhanced version of the distance routine of Gilbert, Johnson andKeerthi (GJK), which allows

the tracking of the distance between a pair of convex polyhedra in time that is expected to be

bounded by a constant.

• I-Collidehttp://www.cs.unc.edu/~geom/I_COLLIDE/index.html

I-Collide is an interactive and exact collision detection library for large environments composed

of convex polyhedra . Many non-convex polyhedra may be decomposed into a set of convex

polyhedra, which may then be used with this library. I-Collide exploits coherance (the property

of a simulation to change very litle between consecutive time steps) and the properties of convexity

to achieve very fast collision detection which is exact to the accuracy of the input models.

• V-Cliphttp://www.merl.com/projects/vclip/

The Voronoi Clip, or V-Clip, algorithm is a low-level collision detection algorithm for polyhe-

dral objects. The V-Clip library is a C++ implementation of this algorithm, with facilities for

constructing and manipulating geometries.

• UNC Collision Detection/Proximity Query Packageshttp://www.cs.unc.edu/~geom/collide/packages.html

The UNC Collide Research Group has implemented several collision detection and proximity query

packages. It includes the following packages: DEEP, SWIFT++, PIVOT, SWIFT, H-COLLIDE,

RAPID, PQP, V-COLLIDE, I-COLLIDE, IMMPACT

• Quick-CDhttp://www.ams.sunysb.edu/~jklosow/quickcd/QuickCD.html

18

QuickCD is a general-purpose collision detection library, capable of performing fast and exact

collision detection on highly complex models. No assumption is made about the structure of the

input. QuickCD robustly handles unstructured inputs consisting of a ”soup” of polygons. No

adjacency information is needed; the models are only specified as a collection of triangles.

19

11 Research Groups

• Amato – Texas A&M University, Parasol Lab

http://parasol.tamu.edu/~amato/

• Pankaj Aggarwal – Duke

http://www.cs.duke.edu/~pankaj/

• Oliver Brock – University of Massachusetts, Amhurst

http://www-robotics.cs.umass.edu/~oli/

• Jean-Daniel Boissonnat – INRIA, Sophia-Antipolis, France

http://www-sop.inria.fr/geometrica/team/JeanDaniel.Boissonnat/index.html

• John Canny – U.C. Berkeley

http://HTTP.CS.Berkeley.EDU/~jfc/

• Howie Choset – CMU

http://voronoi.sbp.ri.cmu.edu/~choset/

• Frank Dellaert – Georgia Tech College of Computing

http://www.cc.gatech.edu/~dellaert/

• Bruce Donald – Dartmouth College

http://www.cs.dartmouth.edu/~brd/

• Hugh Durrant-Whyte – University of Sydney, Austrialia

http://www.acfr.usyd.edu.au/people/academic/hdurrant-whyte/

• Mike Erdmann – CMU

http://www.cs.cmu.edu/afs/cs.cmu.edu/user/me/www/whois-me.html

• Deiter Fox – University of Washington

http://www.cs.washington.edu/homes/fox/

• Maria Gini – University of Minnesota, Minneapolis

http://www-users.cs.umn.edu/~gini/

• Ken Goldberg – U.C. Berkeley

http://queue.IEOR.Berkeley.EDU/~goldberg/

• Leonidas Guibas – Stanford

http://graphics.stanford.edu/~guibas/

20

• Kamal Gupta – Simon Fraser University, Burnaby, BC, Canada

http://www.ensc.sfu.ca/people/faculty/kamal.html

• Dan Halperin – Tel Aviv University, Israel

http://www.math.tau.ac.il/~halperin/

• Seth Hutchinson – University of Illinois, Urbana-Champaign

http://www-cvr.ai.uiuc.edu/~seth/

• Lydia Kavraki – Rice University

http://www.cs.rice.edu/~kavraki/

• Kuffner – Carnegie Mellon University, Pittsburgh, PA. Robotics Institute

http://www.kuffner.org/james/

• Jean-Claude Latombe – Stanford University, Robotics Lab

http://robotics.stanford.edu/~latombe/

• Christian Laugier – INRIA, Grenoble, France

http://www.inria.fr/Equipes/SHARP-eng.html

• Jean-Paul Laumond – LAAS, Toulouse, France

http://www.laas.fr/~jpl/

• Ming C. Lin – Motion Planning Group at UNC

http://www.cs.unc.edu/~lin/

• Steve LaValle – Iowa State University

http://msl.cs.uiuc.edu/~lavalle/

• Tomas Lozano-Perez – MIT

http://www.ai.mit.edu/people/tlp/tlp.html

• Vladimir Lumelsky – University of Wisconsin - Madison

http://www.engr.wisc.edu/me/faculty/lumelsky_vladimir.html

• Kevin Lynch – Northwestern University

http://lims.mech.nwu.edu/~lynch/

• Matt Mason – CMU

http://www.cs.cmu.edu/afs/cs/user/mason/www/index.html

21

• Toshihiro Matsui – ETL, Tsukuba, Japan

http://www.etl.go.jp/~matsui/

• Dinesh Minchocha – UNC

http://www.cs.unc.edu/~dm/

• Mark Overmars – Utrecht University, The Netherland

http://www.cs.ruu.nl/~markov/

• John H. Reif – Duke University, NC

http://www.cs.duke.edu/~reif/

• Daniela Rus – Dartmouth College

http://www.cs.dartmouth.edu/~rus/

• Shankar Sastry – U.C. Berkeley

http://robotics.eecs.berkeley.edu/~sastry/sastry.html

• Achim Schweikard – Technische Universitat Munchen, Germany

http://wwwradig.informatik.tu-muenchen.de/personen/schweika.html

• Thierry Simeon, Jean-Paul Laumond – Lab. d’Automatique et d’Architectures des

Systmes

http://www.laas.fr/RIA/RIA.html

• Micha Sharir – Tel Aviv University, Israel

http://www.math.tau.ac.il/~sharir/

• Anthony Stentz – CMU

http://www.frc.ri.cmu.edu/~axs/

22

References

[1] A. Atramentov and S. M. LaValle. Efficient nearest neighbor searching for motion

planning. In In Proc. IEEE Int’l Conf. on Robotics and Automation, pages 632–637,

2002.

[2] R. Bellman. Adaptive Control Processes: A Guided Tour. Princeton University Press,

New Jersey, 1961.

[3] R. Bohlin and L. E. Kavraki. A randomized algorithm for robot path planning based on

lazy evaluation. In S. R. P. Pardalos and J. Rolim, editors, Handbook on Randomized

Computing, pages 221–249. Kluwer Academic Publishers, 2001.

[4] M. Branicky and M. Curtiss. Nonlinear and hybrid control via rrts. In Proc. Intl.

Symp. on Mathematical Theory of Networks and Systems, South Bend, IN, August.

[5] J. Bruce and M. M. Veloso. Real-time randomized path planning for robot navigation.

In Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots

and Systems (IROS ’02), October 2002.

[6] J. Canny. The complexity of robot motion planning. MIT Press, Cambridge, MA, 1988.

[7] P. Cheng and S. M. LaValle. Resolution complete rapidly-exploring random trees. In

In Proc. IEEE Int’l Conf. on Robotics and Automation, page 267.

[8] M. A. D. E. Frazzoli and E. Feron. Real-time motion planning for agile autonomous

vehicles. AIAA Journal of Guidance and Control, 2002.

[9] M. J. Gregory Dudek. Computational principles of mobile robotics, chapter 5, pages

121–148. Cambridge University Press, Cambridge, UK, 2000.

[10] D. Hsu, R. Kindel, J. Latombe, and S. Rock. Randomized kinodynamic motion plan-

ning with moving obstacles, 2000.

[11] S. K. M. I. H. I. J. Kuffner, K. Nishiwaki. Motion planning for humanoid robots. 2003.

[12] L. Kavraki. Random Networks in Configuration Space for Fast Path Planning. PhD

thesis, Stanford University, 1995.

[13] J. Kim and J. P. Ostrowski. Motion planning of aerial robot using rapidly-exploring

random trees with dynamic constraints. In In IEEE Int. Conf. Robot and Autom.,

2003.

23

[14] J. J. Kuffner and S. M. LaValle. Rrt-connect: An efficient approach to single-query path

planning. In In Proc. IEEE Int’l Conf. on Robotics and Automation, pages 995–1001,

2000.

[15] J. L. L. Kavraki, P. Svestka and M. Overmars. Probabilistic roadmaps for path plan-

ning in high dimensional configuration spaces. IEEE Transactions on Robotics and

Automation, 12(4):566–580, 1996.

[16] A. Ladd and L. E. Kavraki. Generalizing the analysis of prm. In Proceedings of the

2002 IEEE International Conference on Robotics and Automation (ICRA 2002), pages

2120–2125, Washington, DC, May 2002. IEEE Press,(Refereed).

[17] F. Lamiraux and J. Laumond. On the expected complexity of random path planning. In

IEEE International Conference on Robotics and Automation (ICRA’96), pages 3014–

3019, 1996.

[18] J. Latombe. Motion planning: a journey of robots, molecules, digital actors, and other

artifacts. International Journal of Robotics Research, Special Issue on Robotics at the

Millennium, 1999.

[19] S. LaValle and J. Ku. Randomized kinodynamic planning, 1999.

[20] S. M. LaValle. Control Problems in Robotics, page 19.

[21] S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning. TR

98-11, Computer Science Dept., Iowa State University, Oct. 1998.

[22] S. M. LaValle. Planning Algorithms. [Online], 1999-2003. Available at

http://msl.cs.uiuc.edu/planning/.

[23] S. M. LaValle, M. S. Branicky, and S. R. Lindemann. On the relationship between

classical grid search and probabilistic roadmaps. International Journal of Robotics

Research. Selected for publication in late 2003 from among papers that appeared at

the 2002 Workshop on the Algorithmic Foundations of Robotics.

[24] S. M. LaValle and J. J. Kuffner. Rapidly-exploring random trees: Progress and

prospects. In B. R. Donald, K. M. Lynch, and D. Rus, editors, Algorithmic and

Computational Robotics: New Directions, pages 293–308. A K Peters, Wellesley, MA,

2001.

[25] T. Li and Y. Shie. An incremental learning approach to motion planning with roadmap

management. In In IEEE Int. Conf. Robot and Autom., 2002.

24

[26] S. R. Lindemann and S. M. LaValle. Current issues in sampling-based motion planning.

In In P. Dario and R. Chatila, editors, Proc. Eighth Int’l Symp. on Robotics Research,

To appear, Berlin, 2004. Springer-Verlag.

[27] S. R. Lindemann and S. M. LaValle. Incrementally reducing dispersion by increas-

ing voronoi bias in rrts. In In Proc. IEEE International Conference on Robotics and

Automation, Under review., 2004.

[28] T. Lozano-Prez and M. A. Wesley. An algorithm for planning collision-free paths among

polyhedral obstacles. Communications of the ACM, 22:560–570, 1997.

[29] M. O. A. F. v. d. S. J. V. M. de Berg, M.J. Katz. Models and motion planning. Available

at: http://www.library.uu.nl/digiarchief/dip/dispute/2001-0219-155740/2000-41.pdf.

[30] M. O. O. S. M. deBerg, M. van Kreveld. Computational Geometry, chapter 13, pages

267–289. Springer-Verlag, Berlin Heidelberg New York, 1998.

[31] J. L. Michael S. Branicky, Michael M. Curtiss and S. Morgan. Rrts for nonlinear,

discrete, and hybrid planning and control. In Proc. IEEE Conf. on Decision and

Control, Maui.

[32] S. Morgan and M. S. Branicky. Sampling-based planning for discrete spaces. In Proc.

IEEE/RSJ Intl. Conf. Intelligent Robots and Systems, 2004.

[33] J. O’Rourke. Computational geometry in C. Cambridge University Press.

[34] M. L. D. M. D. M. B. M. D. M. S. M. D. P. E. S. J. S. L. J. G. S. S. O. W. H. E. J. E.

M. I. S. H.-P. J. H. C. J. L. K. Pankaj K. Agarwal, Patrice Koehl. Algorithmic issues

in modeling motion. ACM Computing Surveys (CSUR) archive, 34, 2002.

[35] M. M. Pekka Isto and J. Tuominen. On addressing the run-cost variance in random-

ized motion planners. In IEEE International Conference on Robotics and Automation

(ICRA03), pages 2934–2939, 2003.

[36] J. Reif. Complexity of the mover’s problem and generalizations. Proc. FOCS, pages

421–427, 1979.

[37] J. Schwartz and M. Sharir. On the ’piano movers’ problem: Ii. general techniques

for computing topological properties of real algebraic manifolds. Advances in applied

mathematics, (4):298–351, 1983.

[38] M. Sharir. Handbook of discrete and computational geometry, chapter 40, page 733.

Crc Press, Inc., Boca Raton, FL, USA.

25

[39] P. Svestka. On probabilistic completeness and expected complexity of probabilistic

path planning, 1996.

26