Motion Planning Algorithms

Survey: Motion Planning Algorithms

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 algorithms with greater attention given to recent work with RRTs (Rapidly-Exploring Random 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), virtual 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 location 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 ﬁrst analyzed in 1979 by Reif (also referred to as the ”piano mover’s problem”). The problem required ﬁnding 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 Speciﬁcation 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 diﬀerential 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 diﬀerential geometry and control theory. Examples of non-holohomic motions are those achieved by pushing, pulling, rolling, bouncing and ﬂying.

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 ﬁnal 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 constraints 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 Conﬁguration Space Speciﬁcation

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 ﬁxed (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 conﬁguration 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 modiﬁcation to provide clearance between obstacles results in a path planning solution.

Point Robot Simpliﬁcation 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 conﬁguration of the robot or moving object.

Minkowski Sums are used in motion planning to dilate the geometric conﬁguration of obstacles in the C-space to account for the robot’s footprint and degrees of freedom when working with the point robot simpliﬁcation.

2.4 State Space Speciﬁcation

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 speciﬁes 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, Artiﬁcial 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 speciﬁc to the particular problem.

Research in robotic path planning has been studied since the early 1970’s [18]. The ﬁrst 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 ﬁnd a minimized collision-free path through polygonal obstacles [18].

In the early 1980’s, working with the concept of conﬁguration 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-P´erez 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 ﬁrst 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 ﬁnish 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 0s. The complexity analysis simulated the dof 0s 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 conﬁguration space has shown the problem to be doubly exponential in the number of dof 0s [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, including 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 speciﬁc 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 conﬁguration spaces modeled as a regular grid over which a search is conducted for the goal state. One such approach uses potential ﬁelds, 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 Classiﬁcations

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 planning 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 approach to motion planning is prohibitively expensive to compute for any but the simplest C-space representations. The ”curse of dimensionality”, [Bellman61] refers to the exponential 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 conﬁguration 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 conﬁguration space

• Collision Detection – probes the conﬁguration 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 diﬀerent

8 motion planning domains is simpliﬁed. 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 conﬁguration spaces containing stationary obstacles. The planner consists of a two-stage algorithm:

Step One – Preprocessing From [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 suﬃcient 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 ﬁxed 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 – Query From [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 insuﬃcient 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 diﬃcult conﬁguration 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 conﬁgurations.

10 The RRT is more recent and appears suitable for a larger variety of motion planning problems, 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 conﬁguration space, C. The conﬁguration 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 conﬁguration 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 conﬁguration, qrand, in C. Alternatively, one could replace RAND CONF with RAND FREE CONF, and sample conﬁgurations in C-free (by using a collision detection algorithm to reject samples in C-obs).

It is assumed that a metric ρ has been deﬁned 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 conﬁguration, qnew, by moving from qnear an

incremental distance, 4q, in the direction of qrand. This assumes that motion in any direction is possible. If diﬀerential constraints exist, then inputs are applied to the corresponding control system,

and new conﬁgurations 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 k not more than p , 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 eﬃciency of both the Nearest-Neighbor Algorithm used in the integrated system and also the eﬃciency 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 m m from 1.2 s to 1.7 s . Additionally, replacing the pre-existing reactive control with the ERRT was not difficult.

• Resolution Complete RRTs (RC-RRT) Resolution completeness refers to ﬁnding a motion planning solution after a ﬁnite number 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- ﬁguration space through a collision detector, and incrementally explore conﬁguration 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 development and improvement of sampling-based planners is discussed. The authors suggest that future investigation of deterministic sampling might provide a better understanding 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 deﬁne an eﬃciency 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. F (C) Then, O(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 conﬁguration is halted. Experimental runs of 240 trials using two diﬀerent 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 Artiﬁcial 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 conﬁguration specﬁcis.

• 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 speciﬁc simple, basic geometric models for evaluating motion planning algorithms.

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

• Classify eﬀective sampling strategies based upon motion constraints and conﬁguration 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 3D http://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 Library http://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 ﬁle format for polygonal meshes. • MPK - Motion Planning Kit http://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 Software http://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 Dimensions http://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 diﬀerent 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 Library http://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 Algorithm http://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-Collide http://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-Clip http://www.merl.com/projects/vclip/

The Voronoi Clip, or V-Clip, algorithm is a low-level collision detection algorithm for polyhedral objects. The V-Clip library is a C++ implementation of this algorithm, with facilities for constructing and manipulating geometries. • UNC Collision Detection/Proximity Query Packages http://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-CD http://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 speciﬁed 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/

• Kuﬀner – 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. Eﬃcient 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 planning with moving obstacles, 2000.

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

[12] L. Kavraki. Random Networks in Conﬁguration 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. Kuﬀner and S. M. LaValle. Rrt-connect: An eﬃcient 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 planning in high dimensional conﬁguration 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. Kuﬀner. 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 randomized 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.