OPTIMIZATION TECHNIQUES FOR PROBABILISTIC ROADMAPS A Dissertation by LUCIA KATHRYN DALE Submitted to Texas A&M University in partial fulfillment of th

Transcription

1 OPTIMIZATION TECHNIQUES FOR PROBABILISTIC ROADMAPS A Dissertation by LUCIA KATHRYN DALE Submitted to the Office of Graduate Studies of Texas A&M University in partial fulfillment of the requirements for the degree of DOCTOR OF PHILOSOPHY December 2000 Major Subject: Computer Science

2 OPTIMIZATION TECHNIQUES FOR PROBABILISTIC ROADMAPS A Dissertation by LUCIA KATHRYN DALE Submitted to Texas A&M University in partial fulfillment of the requirements for the degree of DOCTOR OF PHILOSOPHY Approved as to style and content by: Nancy Amato (Chair of Committee) Donald House (Member) Karan Watson (Member) Jennifer Welch (Member) Wei Zhao (Head of Department) December 2000 Major Subject: Computer Science

3 iii ABSTRACT Optimization Techniques for Probabilistic Roadmaps. (December 2000) Lucia Kathryn Dale, B.S., Texas A&M University; M.C.S., Texas A&M University Chair of Advisory Committee: Dr. Nancy Amato Recently, a new class of randomized path planning methods, known as Probabilistic Roadmap Methods (PRMs) have shown great potential for solving complicated high-dimensional problems. PRMs use randomization (usually during preprocessing) to construct a graph (a roadmap) of representative paths in the robot's configuration space. Vertices correspond to collision-free configurations of the robot. An edge exists between two vertices if a path between the two corresponding configurations can be found by a local planning method. PRMs solve many high degree of freedom (dof) motion planning problems. Unfortunately, for some problems running times may still be unacceptably large and solutions sub-optimal. We provide speed and quality optimization strategies applicable in cluttered 3-dimensional workspaces. Speed improvements for roadmap construction are accomplished by parallel processing and faster failure detection techniques. Quality improvements for the roadmap constructed are accomplished by new roadmap building methods which result in roadmaps that better represent the connectivity of the free configuration space. Quality is also improved by building roadmaps iteratively using information gained at each iteration to control and drive following iterations. Since there is no single generally accepted way of judging roadmap quality, several measures are considered.

4 To My Mother, Barbara J. Dale iv

5 v ACKNOWLEDGMENTS First and foremost, I would like to thank my mother who has supported and encouraged my education from the first grade onward. That's bound to have been several years longer than she expected. Iwould like to thank my advisor Nancy Amato who provided a seemingly endless supply of enthusiasm, help and guidance in all areas. Iwould also like to thank committee members Jennifer Welch whose technical and artistic assistance was invaluable and Karan Watson who first suggested I could and should get a doctorate. I'm grateful for the camaraderie of fellow students and post-docs in the robotics group; Li Han, Daniel Vallejo, and Sooyong Lee, especially. Iwould like to thank the Computer Science Department, General Electric Foundations (Faculty of the Future) and U.S. Department of Education (GAANN) for their financial support. I would like to thank all the creative people I met through the the university craft center as well as the athletic ones I met playing volleyball.

6 vi TABLE OF CONTENTS CHAPTER Page I INTRODUCTION : : : : : : : : : : : : : : : : : : : : : : : : : : 1 A. Contributions of This Dissertation B. Outline II BACKGROUND AND TERMINOLOGY : : : : : : : : : : : : : 7 A. Degrees of Freedom B. Motion Planning Complexity C. Configuration Space III PREVIOUS WORK : : : : : : : : : : : : : : : : : : : : : : : : 12 A. Roadmap Methods B. Probabilistic Roadmap Methods C. Basic PRM Analysis D. Variants on Basic PRM Gaussian Filter Lazy PRM Visibility Roadmaps Random Neighborhood Graphs E. Addressing the Narrow Passage Problem Obstacle Based PRM Geometric Node Adding Medial Axis PRM Dilated Space Haptic Gathered Nodes F. Building Connected Components Ariadne's Clew Rapidly Exploring Random Tree G. Applying PRMs to Special" Robots Closed Kinematic Chains Flexible Surfaces Folding Molecule Docking

7 vii CHAPTER Page H. Parallel Motion Planning IV EXPERIMENTAL PLATFORM : : : : : : : : : : : : : : : : : : 30 A. Code B. Platform V SPEED OPTIMIZATIONS : : : : : : : : : : : : : : : : : : : : : 35 A. Parallelization Divide and Conquer Experimental Results Discussion B. Faster Failure Detection Greedy Roadmap Edges Clearance Checking Increasing Resolution Line Segment Approximation Experimental Results Discussion VI QUALITY OPTIMIZATIONS : : : : : : : : : : : : : : : : : : : 52 A. Implicit Connections Environments and Measures Experimental Results Discussion B. Connect Components With and Without Expansion Environments and Measures Environmental Results Discussion C. VR Variant Environments and Measures Environmental Results Discussion VII CONCLUSION & FUTURE WORK : : : : : : : : : : : : : : : 82 REFERENCES : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : 84 VITA : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : 93

8 viii LIST OF TABLES TABLE Page I Parallel Processing Results. : : : : : : : : : : : : : : : : : : : : : : : 40 II ClosestVE Results. : : : : : : : : : : : : : : : : : : : : : : : : : : : : 62 III Connect-Components and Expand-CC Results. : : : : : : : : : : : : : 72 IV VR-Variant Results. : : : : : : : : : : : : : : : : : : : : : : : : : : : 77

9 ix LIST OF FIGURES FIGURE Page 1 Easy for Human Visualization. : : : : : : : : : : : : : : : : : : : : : 4 2 Difficult for Human Visualization. : : : : : : : : : : : : : : : : : : : : 4 3 Robots Vary in DOF : : : : : : : : : : : : : : : : : : : : : : : : : : : 8 4 Robots With Same DOF. : : : : : : : : : : : : : : : : : : : : : : : : 8 5 Workspace vs. C-Space. : : : : : : : : : : : : : : : : : : : : : : : : : 11 6 Randomly Generate Nodes. : : : : : : : : : : : : : : : : : : : : : : : 17 7 Discard Nodes in Collision. : : : : : : : : : : : : : : : : : : : : : : : 17 8 Attempt Inter-Node Connections. : : : : : : : : : : : : : : : : : : : : 17 9 Discard Paths with Collision. : : : : : : : : : : : : : : : : : : : : : : Narrow Passage in 2D. : : : : : : : : : : : : : : : : : : : : : : : : : : Narrow Passage in 3D. : : : : : : : : : : : : : : : : : : : : : : : : : : Closed Chain Robot. : : : : : : : : : : : : : : : : : : : : : : : : : : : Closed Chain Constraint. : : : : : : : : : : : : : : : : : : : : : : : : The Sandwich Environment. : : : : : : : : : : : : : : : : : : : : : : : Parallel Results. : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Line Segment Approximation. : : : : : : : : : : : : : : : : : : : : : : Clearance and Resolution Results. : : : : : : : : : : : : : : : : : : : Line Segment Approximation Results. : : : : : : : : : : : : : : : : : Typical Two Phase PRM. : : : : : : : : : : : : : : : : : : : : : : : : 53

10 x FIGURE Page 20 Optimizing PRM Quality. : : : : : : : : : : : : : : : : : : : : : : : : ClosestVE Considering Implicit Graph Nodes. : : : : : : : : : : : : : The Comb Environment, Pass II - OBPRM. : : : : : : : : : : : : : : The Comb Environment, Pass II - BasicPRM. : : : : : : : : : : : : : The Comb Environment, Pass II - GaussPRM. : : : : : : : : : : : : Connect-Components Attempts Pairwise Connections. : : : : : : : : Expand-CC Keeps Partial Paths. : : : : : : : : : : : : : : : : : : : : The Tunnel Environment. : : : : : : : : : : : : : : : : : : : : : : : : Connect-Components - Input Nodes. : : : : : : : : : : : : : : : : : : Connect-Components - Map Edges : : : : : : : : : : : : : : : : : : : Expand-CC - Dark Nodes are New. : : : : : : : : : : : : : : : : : : : Expand-CC - Map Edges. : : : : : : : : : : : : : : : : : : : : : : : : Visibility Roadmap (Visibility Roadmap) Method. : : : : : : : : : : : VR-Variant - Generalized Method. : : : : : : : : : : : : : : : : : : : Alpha Puzzle Results. : : : : : : : : : : : : : : : : : : : : : : : : : : VR-Variant - Results : : : : : : : : : : : : : : : : : : : : : : : : : : : 80

11 1 CHAPTER I INTRODUCTION In addition to robotics, motion planning applications occur in many areas in fact, any area where it is impossible (molecule docking in drug design, protein folding, space/remote exploration, virtual reality), impractical (assembly, computer aided design, virtual prototyping, surgical training, medical robots) or dangerous (mine fields, nuclear/waste material management) for human beings to act directly. The basic motion planning problem is the following. Given a robot, which can be any moving object, a start and goal, and a complete description of a static workspace, findavalid (e.g., collision free) path for the robot through the workspace from start to goal. The robot must avoid collision with itself as well as obstacles in the workspace environment. There are many variations to the basic problem. Static obstacles, moving obstacles, movable obstacles, multiple robots, deformable robots, deformable obstacles, energy and/or motion constraints all represent variations of interest. To be generally applicable, motion planning algorithms must take this infinite variation into account. The reason is simple. The same workspace presents a different motion planning problem to every robot. Small robots can navigate where large ones are blocked. Highly articulated robots may be able to maneuver through narrow twisted passages which are impossible for rigid robots of the same size. Robots can vary from simplistic to incredibly complex. There are simple spherical robots such as NASA's Autonomous Extra-vehicular Robotic Camera(AERcam, [25]) and highly articulated robots approximating human degrees of freedom such as the The journal model is IEEE Transactions on Automatic Control.

12 2 HONDA Humanoid Robot [31]. Motion planning can be applied to moving bodies not usually envisioned as `robots' by the layman. Protein folding, where the task is to fold the protein into a stable low energy configuration while avoiding high energy conformations, is in many ways quite similar to box folding where the box must fold so as to avoid self-collision. Each is an example of a very high degree of freedom robot, in the number of bonds and the number of folds, respectively. The environment can also vary. At the very least there is typically interaction with the supporting surface (`floor') to consider. Often there are other bodies with which collision must be avoided. Some of these other bodies (`obstacles') are stationary while others are dynamic with either predictable or random trajectories of their own. Sometimes the obstacles are other active agents and the autonomous robot agent must modify its own behavior with respect to them. Possibilities include: fleeing them, escaping detection by them, moving them, cooperating with them, or simply avoiding collision with them. Unpredictable obstacle motion and complicated interaction requirements demand ever more sophistication from motion planners. Many, if not most, of the variations to the basic motion planning problem add to its complexity. However, an autonomous agent navigating in an environment with only stationary obstacles, as in the basic problem formulation, can be prohibitively complex and involves many as yet unsolved problems. An algorithm which yields a complete solution to the motion planning problem is one which provides a correct solution if one exists or indicates that there is no solution. An algorithm which is probabilistically complete, is one which if allowed to run long enough (e.g. infinitely long) will produce a complete solution. Because there exists strong evidence that a complete motion planning solution requires exponential time in the number of degrees of freedom (dof) of the robot [37, 46], considerable re-

13 3 cent attention has focussed on probabilistic (randomized) roadmap methods (PRMs). Probabilistically complete, these methods have shown promise for solving high dimensional problems [2, 3, 4, 7, 16, 36, 44, 50, 67]. However, even with PRMs, prohibitively long running times can occur and the resulting roadmap may not be of sufficiently high quality to perform well with some queries. That is, some queries which are solvable in the workspace are not represented in the roadmap. Unfortunately, in general, determining the quality of a roadmap is no less difficult than building it. Quality is measured by how accurately a roadmap reflects the connectivity of the space in which the robot is free to move. We will discuss the applicability ofvarious readily available roadmap statistics which can be used to approximate the `quality' of a roadmap. In some of the examples used for illustrative purposes in this dissertation and others, the connectivity of the free space is easy for the human observer to see. This is not the general case. Consider Figure 1 versus Figure 2. While the limited passage from left to right is easy to identify in Figure 1, Figure 2 shows a complicated engine assembly with many small closely positioned parts whose insertion or removal from the assembly present a more difficult task. Often human operators cannot easily provide motion plans except in relatively simple or contrived examples. Motion planning is most crucial in cases where the workspace is too complex for reliable human comprehension or when no human is available to assist the robot. A. Contributions of This Dissertation There are two criticisms that have been leveled at PRMs. One, roadmap building takes too long. Two, the quality of roadmaps is often inadequate. The quality of a

14 4 start goal Fig. 1. Easy for Human Visualization. Move the point robot from the start to the goal position without contacting the black boundary areas. Fig. 2. Difficult for Human Visualization. Engine assembly with many small closely positioned parts whose removal/insertion paths may be difficult for human visualization.

15 5 roadmap is determined by how exactly it represents the connectivity of the spaces in the workspace where the robot is free to maneuver. This research provides methods for ffl decreasing roadmap construction time and ffl improving the quality ofroadmaps for difficult environments. Speed of construction is increased by two methods. One method is dividing the work among multiple processors during all phases of roadmap construction. We present parallel descriptions of current PRMs, which we consider to be `embarrassingly' parallel [5], suitable for execution on parallel platforms. The second method is to more efficiently implement current PRMs. Part of the slowness of current PRMs is often the numerous calls to collision detection necessary during local planning where proposed connections are checked for failure. We present faster failure detection techniques for local planning. Quality of roadmaps is a more complicated issue. The construction of roadmaps typically takes place in two phases. In the generation phase, collision free robot positions (nodes) are identified while in the connection phase collision free local path planning between nodes takes place. Because the connection phase of probabilistic roadmaps typically accounts for over 98% of the total running time, an important question to ask is why is connection using such a huge percentage of the running time? The answer is complicated. Part of the answer is often that the preliminary step of generation failed to generate nodes ofatype crucial to the success of the connection step. We present new methods to address this failure and re-examine the traditional separation of the generation and connection phases of roadmap construction. Finally, part of the answer is simply that smarter generation and connection methods are needed. We present new roadmap building methods as well as ways of building

16 6 roadmaps iteratively using information gained at each iteration to control and drive following iterations. We provide ways of optimizing probabilistic motion planners with the twin goals of providing better motion plans faster. We summarize our work reported in [4, 5] and report on recent related research and results. B. Outline The dissertation is organized as follows. Some useful background for the motion planning problem in general and PRMs, in particular, is reviewed in Chapter II. In Chapter III previous research into PRMs and related approaches is presented. Chapter IV describes our hardware and software resources. In Chapter V speed optimizations and in Chapter VI quality optimizations are described. With each optimization strategy, we provide details of experimental performance results along with a discussion of performance criteria applicable. Finally, conclusions and plans for future work are in Chapter VII.

17 7 CHAPTER II BACKGROUND AND TERMINOLOGY Before continuing we define some preliminary terms and concepts. A. Degrees of Freedom Because the navigable areas in a workspace vary from robot to robot, it is very useful to have a way in which to fully and succinctly describe the position of every point in the robot at any given moment. See Figure 3 for examples of robots with varying complexity. When the robot is a point, as is sometimes the case in theoretical discussions, it can be completely described by its translational coordinates, (x; y; z) as shown in the far left part of the figure. When the robot is a rigid body moving freely in 3D space as in the cube shown in Figure 3, six parameters (x; y; z; ff; fi; fl), a coordinate in each of three axis as well as the rotation about each, are needed to fully describe the position of every point in the robot's body. Each parameter, or coordinate, necessary to give the full description of a robot is called a degree of freedom (dof). The seven dof robot shown in Figure 3 is a spanning wrench. It has the the same six dof as the cube robot plus a seventh dof, the value of which represents the width of the tool jaw. The far right n dof robot in Figure 3 demonstrates in caricature that the number of dof can become extremely large as robots become more and more complex. Not only the number, but the interpretation of each dof is necessary to fully understand the robot's position and pose. Figure 4 shows six dof are necessary to describe a rigid body moving freely in 3D. Six parameters are also necessary to describe a planar robot with a fixed base and six serial links. Although the same size, the coordinate vectors of each robot are interpreted differently.

18 8 Fig. 3. Robots Vary in DOF. The `7D' figure is a spanning wrench where d is the jaw width. θ 4 θ 6 θ 2 θ 5 θ 3 θ 1 3D Workspace 6D ( x, y, z, pitch, roll, yaw ) 2D Workspace 6D ( θ1, θ2, θ3, θ4, θ5, θ6 ) Fig. 4. Robots With Same DOF. The robot on the left is free flying in a 3D workspace. The one on the right, which has a fixed base and six serial links, is limited to a 2D workspace. Both have 6 degrees of freedom.

19 9 B. Motion Planning Complexity In [37] Hwang and Ahuja provide a general survey of the motion planning area including a nice review of the applicable complexity results. Discussed in some detail are the findings of various researchers who have shown for various robots and under various assumptions that the motion planning problem is PSPACE-hard [8, 32, 45, 60, 64] and the upper bound appears to be exponential in the degrees of freedom (dof) of the robot [18, 19]. An algorithm which yields a complete solution is one which provides a correct solution if one exists or indicates that there is no solution. The complexity results which exist for motion planning explain why complete solutions for high dof robots are infeasible. As an aside we note that all known complete solutions are actually resolutionally" complete. That is, they guarantee the solution at the discretized resolution used by the algorithm. In a slight abuse of terminology, it is common usage to refer to such solutions as simply `complete' [24]. C. Configuration Space Configuration-space (C-space) provides a useful and well-known abstraction of the motion planning problem. An excellent explanation of C-space is provided by Latombe [46]. Briefly, the motion planning problem is expressed in a higher dimensional space where the robot configuration all the necessary dof's for fully describing the robot's position and pose is a point. The C-space consists of ALL possible points, both those of valid configurations corresponding to the robot in free space and invalid configurations corresponding to the robot in collision with one or more obstacles or itself. In a 3D workspace, the path between any given start and goal configuration is a swept volume. In C-space, the same path is a one-dimensional curve traced by the C-space point representing the robot moving from start to goal. Only when the

20 10 robot is really a point robot (as in theoretical discussions) are the workspace and the robot's C-space the same. Figure 5 compares one robot's workspace with that of its C-space. The figure shows a triangular robot operating in a planar workspace containing four rectangular obstacles. Because the triangle is free to translate and rotate in the plane, its C-space is three dimensional as shown on the right. In C-space, the robot is always represented as a point. Paths which are swept areas in the workspace correspond to one dimensional curves in C-space. While C-space provides an abstraction useful for developing and describing motion planning algorithms, it does not reduce the complexity ofthe problem. As Figure 5 demonstrates, although the robot is represented by a simple point in the higher dimensional space, the obstacles are similarly transformed into high dimensional volumes. Every point in those higher dimensional obstacle volumes represents a position and pose of the robot in the workspace which is in collision with the corresponding workspace obstacle. Fairly simple 2D or 3D workspace obstacles are transformed to higher dimensional C-space obstacles, C-obstacles, often with complex surface topologies. In Figure 5, the triangular robot shown on the left is capable of spinning completely about its center of rotation. The rotation parameter wraps around" from 0 to 360. In the C-space shown on the right, the vertical dimension represents this rotation. Because the tops" of the C-obstacles shown are the same as their bottoms", they can be visualized as repeating and extending infinitely up and down. An alternative visualization of this wrap around" is to picture the C-space as a torus. With either visualization, infinite C-obstacles or torus C-space, the more rotational degrees of freedom the robot possesses, the more difficult the visualization becomes. To complicate matters further, C-obstacles may intersect. Intersecting volumes correspond to a locus of configurations where the robot is in collision with more than

21 11 Fig. 5. Workspace vs. C-Space. one obstacle at the same time. Because the dimension of C-space equals the number of dof of the robot, computing C-obstacles (and intersections) is computationally infeasible for all but the lowest dof robots.

22 12 CHAPTER III PREVIOUS WORK Roadmap methods break the motion planning problem into two distinct stages: roadmap construction (preprocessing) and roadmap query (runtime). Roadmap construction time varies according to the problem, but can be quite significant (ranging from a few seconds to many hours). It is important to understand that the preprocessing time necessary to build an adequate roadmap for any particular problem is justified by the ability to later quickly satisfy (typically within a few seconds) a large number of different queries to the same roadmap. It is not assumed that any crucial start and goal configuration are known a priori but rather that the entire workspace must be explored and mapped before the robot can efficiently operate there. The process is analogous to that of cartographers making a map of a country's road system. When they finish, many drivers can all obtain the same map and use it for their individual navigation purposes. There is no single start to goal trip; rather all possible routes must be explored and charted. A. Roadmap Methods Extensive reviews of motion planning and roadmap methods can be found in Latombe's book [46] and Hwang and Ahuja's survey [37]. Both agree, with only minor differences in taxonomy, that roadmap methods are some variation of one of the following. ffl Visibility graphs [9, 26, 27, 51, 66] use features of the C-obstacles to build line of sight type roadmaps. ffl Retraction Methods [17, 20, 57] use regions and features of the C-obstacles to build retraction (greatest clearance) minimalistic roadmaps (most often Voronoi

23 13 diagrams [10]). ffl The Silhouette Method [18] is the only known complete algorithm which is only singly exponential in the number of degrees of freedom of the robot. It is often cited as evidence that no computationally simpler algorithm is likely to exist for motion planning. By sweeping one dimension at a time through the C-space, a series of critical points are connected to the silhouette curves of C- obstacles. Map edges are algebraic curve segments and nodes are the endpoints. A simplified version has been implemented in [21]. Clearly all three approaches depend upon the computation of C-obstacles. Hwang and Ahuja list no less than seven ways in which C-obstacles can be computed: point evaluation, Minkowski set difference, boundary equation, needle, sweep volume, template, and a Jacobian-based method. See [37] for a summary of each. All are feasible only for low degree of freedom problems. While Latombe separates (by chapter) [46] the above `Roadmap Methods' from those of cell decomposition and potential field methods, the product of the latter two methods can also be used as roadmaps representing the connectivity of free space. In cell decomposition methods, C-space (or C-free) is decomposed into cells which are of variable or fixed shape (hypercube). Since C-space dimensionality can be arbitrarily large, this method, in both its exact (complete) and approximate versions, is useful only for low degree of freedom problems. Potential field methods, which primarily proceed by gradient descent from start (repulsive field) to goal (attractive field) among obstacles (repulsive fields), cannot guarantee the avoidance of local minima. The Randomized Path Planner (RPP) [11], a potential field method and precursor to current PRMs, uses random walks to escape from local minima.

24 14 B. Probabilistic Roadmap Methods A class of motion planning methods, known as probabilistic roadmap methods (PRMs), have made large recent gains in popularity [2, 3, 4, 7, 16, 36, 44, 50, 67]. Briefly, PRMs use randomization to construct a graph (a roadmap) in configuration-space (C-space). PRMs provide heuristics for sampling C-space and C-obstacles without explicitly calculating either. When PRM maps are built, roadmap nodes correspond to collision-free configurations of the robot, i.e. points in the free C-space (C-free). Two nodes are connected by an edge if a collision-free path between the two corresponding configurations can be found by a `local planning' method. Local planners take as input a pair of configurations and check the path (edge) between them for collision. As output they declare the path valid (collision-free) or invalid. Because of the large number of calls made to local planners, their design often sacrifices sophistication for speed. When local planners [3] are deterministic, the edges do not need to be saved and the roadmap can be stored as an adjacency graph. PRM methods may include one or more `enhancement' steps in which some areas are sampled more heavily either before or after the connection phase. If after, the connection phase is also repeated. Queries are processed by connecting the initial and goal configurations to the roadmap, and then searching for a path in the roadmap between these two connection points. What distinguishes a PRM roadmap is that only a sample of all potential feasible paths are included in the roadmap rather than all of them. The following pseudo-code summarizes the high level algorithmic steps for both roadmap construction and usage (query processing). Lower level resources include distance metrics for evaluating the promise of various edges, local planners for validating proposed edges, and collision detectors for distinguishing between valid (collision

25 15 free) and invalid (in collision) robot configurations. PRMs: Probabilistic Roadmap Methods I. Preprocessing: Roadmap Construction 1. Node Generation (find collision-free configurations) 2. Connection (connect nodes to form roadmap) (repeat as desired) II. On Line: Query Processing 1. Connect start/goal to roadmap 2. Find roadmap path between connection nodes (repeat for all start/goal pairs of interest) When constructing a PRM roadmap, the limiting primitive operation is collision detection. In the generation phase, robot configurations are checked for collision before being added to the roadmap. In the connection phase, pairs of configurations are selected and submitted to a local planner which must verify that every point along the path between the two members of a pair is collision-free before `ok'ing the edge for addition to the roadmap. Because it is impossible to check every point between two configurations, this is done only to some discrete resolution. Although both generation and connection phases of roadmap construction are dependent on collision detection, node connection typically accounts for over 98% of the calls to a collision checker as it considers which edges can and cannot be added to the roadmap. Some terms are used interchangeably: ffl Roadmap and graph ffl Vertex, configuration and node all refer to a complete (every dof) description of the robot. Vertex and node within context of roadmap as graph. Configuration within context of C-space (see Section C of this chapter). ffl Edge and path. Edge within context of roadmap as graph. Path, depending on the context, will sometimes refer to a single edge of the roadmap graph and at others a series of connected edges through the graph.

26 16 Latombe's book [46] provides a reference for state of the art motion planning as of 1991, including a chapter on roadmap methods. Probabilistic methods and variations began to appear a few years later as randomized algorithms became more popular. C. Basic PRM The authors of [33] describe very similar ideas to PRMs although they do not name their work a PRM. Their motion planning method generates random configurations which they attempt to connect to near neighbors just as in the other basic PRM methods described in detail in the following paragraphs. Connections between connected components are then attempted using random reflections at C-obstacle surfaces. A roadmap of the workspace is built off-line without calculating complete C-obstacles. The method is applied to a mobile robot in a dynamic environment. We are aware of no follow up to this work. Figures 6 through 9 demonstrate the basic steps of PRM roadmap construction. As described by [42, 44, 58, 59] the basic PRM uses uniform sampling to generate, uniformly at random, configurations (a.k.a. nodes) of the robot in the workspace. Configurations which are in collision are discarded. For those collision free nodes remaining, pair-wise connections are attempted using a local planner. Collision-free connections are retained and the result is a basic PRM. Attempting exhaustive pairwise connections is uncommon. Instead, for each node, the remaining nodes are sorted by distance [3] and connections are attempted for a user selected small constant number k of them. The connection method is referred to as k-closest.

27 17 free configuration configuration in collision c-obstacle Fig. 6. Randomly Generate Fig. 8. Attempt Inter-Node Nodes. path w/ collision Connections. collision-free path Fig. 7. Discard Nodes in Collision. Fig. 9. Discard Paths with Collision.

28 18 1. Analysis An algorithm which yields a complete solution is one which provides a correct solution if one exists or indicates that there is no solution. A probabilistically complete algorithm is one which, if allowed to run long enough (e.g. infinitely long), will produce a complete solution. Usually only probabilistically complete [43], PRMs trade complete solutions for faster ones. Analyzing PRMs and characterizing C-spaces and roadmaps has proven remarkably difficult in all but the simplest cases. Nevertheless, some results are available. For very simple problems, [43] presents the property of ffl-goodness for determining how well a probabilistic roadmap `covers' a free space. Properties of expansiveness and path clearance are used to analyze simple roadmaps in [36] and [40], respectively. PRMs perform well in relatively open environments. Since the collision free space is, by assumption, a large part of the entire space, random sampling will likely result in many configurations there. In the connection phase, connections between configurations can be made which correspond to a reasonable representation of the connectivity of the free space. Figure 10 and Figure 11 each give an example of the `narrow passage problem' [13, 35, 59, 67, 68]. In Figure 10, a two dimensional space is shown. All white areas are collision free for the point robot while all dark areas represent collision with either an obstacle or the workspace boundary. Random sampling in this workspace will easily discover the two large white areas. However, it is unlikely that the small narrow passage between the two open areas will be sampled for discovery. A roadmap built using a basic PRM will not be able to provide a path for queries like the one shown, with the start in one area and the goal in the other. Figure 11 shows a 3D workspace example of the narrow passage problem. There are two identical tubes.

29 19 start goal Fig. 10. Narrow Passage in 2D. Fig. 11. Narrow Passage in 3D. (Separate the two intertwined tubes.) One is the obstacle and the other is the robot. Queries which start with the two tubes interconnected as shown and have as a goal completely separating them, can only be satisfied by a roadmap which has recorded the very narrow passage between the connected configurations and the unconnected ones. Traversal of the narrow passage in this example requires simultaneous translation and rotation of the robot tube in a very specific manner. Again, random sampling is very unlikely to discover this passage. PRMs do not perform well in the presence of crucial narrow passages in the configuration space. By crucial we mean that without discovering and finding paths which traverse these corridors, there are problems of interest which cannot be solved for the environment. D. Variants on Basic PRM There have been several interesting variations on the basic PRM strategy. We list some of the more interesting recent ones.

30 20 1. Gaussian Filter In one variant a Gaussian filter is applied as nodes are generated [16]. Filtering, thereby reducing in number, the nodes added to the roadmap tends to reduce the work done in the connection phase. In any case, pairs of nodes are generated as follows. The first node is generated at random. The second node is generated in a random direction at some fixed distance, d, away from the first. Only if one of the nodes is in free space and the other is not, is the free node added to the roadmap. This method ensures that only nodes no further than d away from some obstacle surface are retained. Generally many nodes must be generated in order to obtain an acceptable distribution of nodes on/near obstacle surfaces. This method is no more likely than the basic PRM to generate nodes in narrow passages. It does, however, act as a filter on the nodes which are generated. The authors present avariant specifically designed to filter for nodes in narrow passages as long as those passages are created by two distinct obstacles. Triples are generated rather than pairs: if two nodes are in collision with two distinct obstacles and the third is in C-free, all no more than d away from each other, the node is added to the graph. 2. Lazy PRM In [15], `Lazy PRM' is the usual PRM built with all collision checking postponed until a query is presented. At that time a path through the roadmap is found and only then is collision checking performed from the outside nodes (start and goal) towards the middle at increasingly finer resolution. After each query, the roadmap is updated. This represents a possibly useful variation in cases where queries are very similar to one another. The authors specifically state this work is not intended to address the narrow passage problem. Implementation results are presented for up to 6 dof.

31 21 3. Visibility Roadmaps In the Visibility Roadmap method [48], roadmap nodes are generated randomly just as in a Basic-PRM but connection is done as they are generated in the following way. For each node generated, if it can be connected to more than one currently existing connected component or to no currently existing connected component, it is retained and the connecting edges (if any) are added the roadmap. If the node can be connected to only one of the existing connected components, it is discarded. Multiply connectable nodes are assumed to yield important information about the connectivity of free space. Isolated nodes are assumed to point to unexplored areas of free space. Singly connectable nodes are assumed to represent another sample in an already explored region and are discarded rather than allowing them to increase the size of the roadmap. Because nodes are generated randomly in the basic PRM manner, this method is no better than a basic PRM at handling narrow passages. 4. Random Neighborhood Graphs In the Random Neighborhood Graph (RNG) of [70], the collision free subset of C- space is probabilistically filled with randomly generated spheres (or cylinders) of maximum radius. Each sphere, termed a `neighborhood', is a vertex in a RNG and has a collision free potential function defined upon it. The largest possible radius of each neighborhood is found using clearance information from the collision detection routines. Overlapping neighborhoods have an edge between them. Once a query is presented, a potential function with no local minima other than the goal is created on the fly. Using the results of [55] as an underlying technique, the authors claim their method is not dependent on dof but rather on the number of spheres, n. No satisfactory way ofkeeping n low is presented. As the authors note, this method does

32 22 not address the narrow passage problem. Implementation results are presented for up to 5 dof. E. Addressing the Narrow Passage Problem There have been several variants proposed to the basic PRM method which do address the `narrow passage problem'. Workspaces are difficult when they are `cluttered'. In general, the clutter is made up of closely positioned workspace obstacles. Two of the methods, described in Sections 1 and 2 of this chapter, use descriptions of workspace obstacles in order to increase the likelihood of sampling narrow passages and difficult regions. Although precisely how to identify `difficult' regions is a topic of some debate, when these regions can be identified, additional sampling there can enhance a roadmap. Naturally, many of the nodes generated in a difficult area will be in collision with whatever obstacles are making that area difficult. Some researchers, not wishing to waste computation invested in generating nodes discovered to be in collision, are considering how to `salvage' such nodes by transforming them in various ways until they become collision free. This is the inspiration behind the methods described in Sections 3, 4 and 5 of this chapter. 1. Obstacle Based PRM In [7], we introduced the obstacle based PRM (OBPRM). With this planner, we sample the surfaces of obstacle surfaces without explicitly calculating those surfaces. An OBPRM generation strategy provides information which makes possible a connection strategy where every node generated can be considered for connection with its k closest neighbors on each obstacle in the environment. OBPRM shows marked success in discovering and navigating narrow passages in C space.

33 23 We evaluate various sampling and connection strategies within the context of OBPRM in [4]. Amulti stage strategy for connecting roadmap nodes where different local planners are used at different stages is shown to enhance the connectivity of the resulting roadmap significantly. The most common local planner used by PRM methods is a straight line in C space. We evaluate distance metrics and local planning methods in [3] where we propose the rotate-at-s local planner. Rotate-at-s divides the usual straight line path into three straight line segments to be tested for collision. The first and final segments consist of pure translation while the intervening segment is pure rotation. Where the intervening segment lies, depends on the user specified value for s 2 [0; 1]. 2. Geometric Node Adding Inavariant called geometric node adding [59], configurations of non-articulated robots near workspace obstacle boundaries are generated using the geometry of both robot and obstacles. For the geometric axis" of the robot, the perpendicular width of the robot is minimized. The robot is positioned with its geometric axis perpendicular, with some small clearance value, to the outward normal of the edges and vertices of workspace obstacles. Which edges and vertices are selected randomly. Because this method depends on the description of the workspace obstacles it will sample more in areas of greater descriptive complexity. 3. Medial Axis PRM In medial axis PRM (MAPRM) [67, 68], clearance information for collision free nodes and penetration information for nodes in collision, provided by some collision detection library packages, is used to retract randomly generated nodes to the medial axis using known properties of the generalized Voronoi diagram. This is done without

34 24 having to calculate the axis itself. This work, although preliminary, is appealing due to its strong theoretical foundation. 4. Dilated Space In [35], the free space is dilated during first phase node generation and then in a second phase, nodes in the dilated free space but not in the real free space are pushed towards free space. Although this method widens narrow corridors so they are probabilistically more likely to be discovered by random sampling, it also potentially alters the topology of the free space, e.g., by creating corridors where none actually exist. 5. Haptic Gathered Nodes In [13], haptic devices are used to collect nodes suggested by a user. The algorithm then attempts to push these nodes to C-free. The important contribution is the incorporation of user input. F. Building Connected Components There are other probabilistic approaches that never allow a grossly disconnected roadmap but rather grow exploratory roadmaps from `seeds' which are known to be collision free. 1. Ariadne's Clew The Ariadne's Clew" algorithm [14] does not sample C-space, but instead grows a connected component. A parallel genetic algorithm is used to build a roadmap using two local strategies, search and explore. Explore generates configurations which are connected to, but as far as possible from, previously generated ones. Search tries

35 25 to connect query (start & goal) configurations to the closest map nodes. Dynamic obstacles are considered and Manhattan distances are used. 2. Rapidly Exploring Random Tree The Rapidly Exploring Random Tree (RRT) [49] starts from a single collision free node and grows out from it in fixed-size randomly directed steps until the unexplored space is filled. The steps are small and those which are successful represent a short collision free path (roadmap edge) to the next node (roadmap vertex). No node, other than the initial seed, is ever isolated. Because the roadmap built is a tree, query paths may be longer than necessary. This method has been successfully applied to robots with simple non-holonomic constraints. The turning radius of a car is an example of a non-holonomic constraint. Non-holonomic constraints reduce the space of possible differential motions without reducing the dimension of C-space. For more information on non-holonomic constraints, refer to [46]. G. Applying PRMs to Special" Robots PRMs have been applied to many different types of robots. In addition to simple rigid body robots and articulated robots (serpentine as well as more general compositions), there has also been some preliminary investigation of closed chain robots and flexible body robots. 1. Closed Kinematic Chains Figures 12 and 13 show closed kinematic chain motion planning applications. A closed kinematic chain represents a type of constrained robot. In Figure 12, the robot itself is a closed chain. In Figure 13, two cooperating robots must maintain

36 26 robot block robot 2 robot 1 Fig. 12. Closed Chain Robot. Robot itself is a closed chain. Start is upper left configuration and goal is lower right. Fig. 13. Closed Chain Constraint. Two cooperating robots maintaining a closed chain constraint in order to perform a task. Start is lower right figure and goal is upper left. The task is transporting and stacking rectangular blocks. a closed chain connection in order to perform a cooperative task. A closed chain constraint is one which demands a certain type of contact be maintained. Robot configurations generated at random cannot be expected to adhere to closed chain" constraints. In [50], map nodes are generated by ignoring kinematic (closed chain) constraints and then using randomized gradient descent to force satisfaction of the closure constraints. Connections are recognized for addition to the roadmap by executing a randomized traversal of the constraint surface between a pair of nodes. The gradient descent and traversal are subject to local minima traps which, while they do not effect the resulting maps, being simply discarded when recognized, do represent wasted computation. The method suffers primarily because it cannot get valid nodes

37 27 by ignoring the closure constraints. Closure configurations are a measure zero set. In [30] a preliminary preprocessing step generates a set of poses with a nominal base configuration. Each pose satisfies the kinematic constraints of the closed chain robot. Transformations between the poses are attempted without taking into consideration the workspace obstacles. This creates a mini map of poses which are known to be valid. In the next step, instances of this valid set of robot poses are randomly distributed about the environment's free space and as much of the mini map as is collision free is also added at each position. In the connection stage, connections are attempted between nodes which have identical poses. If there are no collisions, these connections can be accomplished with rigid body motion of the robot frozen in a specific pose. This method, which never considers configurations which violate the fundamental kinematic constraints of a closed chain robot, is more efficient than that of [50] because inverse kinematics are used to enforce closure constraints instead of gradient descent search. 2. Flexible Surfaces PRM methods are applied to a robot which is a thin elastic sheet in [41]. The robot is represented using a 3x3 Bézier surface although the authors claim their approach is not limited to any specific geometric representation. Work in this area is quite complex and the treatment in this paper is interesting but thus far preliminary. 3. Folding In [63], foldable objects (paper and proteins) are modeled as tree-like multi-link objects with many degrees of freedom. PRM techniques are used to determine if one object can be folded (or unfolded) into another object and also to provide a tool for investigating the dynamic folding process itself. Protein folding, where the task is

38 28 to fold the protein into a stable low energy configuration while avoiding high energy conformations, is in many ways quite similar to box folding where the box must fold so as to avoid self-collision. Each is an example of a very high degree of freedom robot, in the number of bonds and the number of folds, respectively. The authors present preliminary experimental results with traditional paper crafts (e.g., box folding) and small proteins (approximately 60 residues). 4. Molecule Docking In [62], the authors apply PRM methods to ligand binding, which is of interest in drug design. To facilitate connection, randomly generated ligand conformations are allowed to go through higher potentials in the connection phase. The PRM approach provides awayto efficiently cover a large portion of the planning space, in this case, the conformation space provides a way to study various initial conformations. H. Parallel Motion Planning We are aware of few studies of parallel methods which can be applied in higher dimensional C-space. Lozano-Perez and O'Donnell [52] give a parallel algorithm for computing a discretized C-space for the first three links of a six degree of freedom manipulator. In parallel, the method pre-computes the free space for each arm configuration that will be considered by a sequential search algorithm. Challou et al. [22, 23] formulated a parallel version of the randomized path planner (RPP) of Barraquand and Latombe [12]. Basically, in the parallel version, each processor independently runs the sequential RPP algorithm. When a processor finds a solution, it notifies all other processors they can stop their search. Significant speedups were reported. Although this is the only work we are aware of specifically relating to motion planning,

39 29 there have been some parallel methods proposed for collision detection [54, 53] and related geometric problems (see, e.g., [1, 6]). These results illustrate that significant reductions in planning time can be obtained with parallel methods. We note, however, that none of the above mentioned motion planning methods is truly parallel. The former [52] does not parallelize the search itself, while the latter [22] cannot be expected to yield scalable speedups. That is, the expected running time is not inversely proportional to the number of processors used, since the fastest solution one can hope for is the time required by a fast sequential RPP execution. Thus, in order to consistently obtain scalable speedups we need to use different approaches that actually partition the work to be performed over the set of available processors, e.g., parallel divide and conquer.

40 30 CHAPTER IV EXPERIMENTAL PLATFORM As discussed in Section B of Chapter II. motion planning is an intractable problem. The lower bound is PSPACE-hard while the upper bound is likely to be exponential in the number of degrees of freedom the robot possesses. Because of these complexity results, our work is experimental in approach. Our group has developed a rather comprehensive coded implementation of our interpretations of many published and unpublished PRM strategies and variants. When conducting our experiments, we work within and modify this development platform. This chapter provides a brief summary of each of the algorithms which were used in this study. Details of each will be provided in later chapters as appropriate. This chapter also provides an overview of the entire development platform. A. Code Our code is written in C++. It implements a 3D motion planning testbed for rigid bodies as well as general articulated linkages. Work is on going for additional types of robot bodies. We have implemented (our interpretations of) various PRM generation and connection methods. Generation Methods: ffl Basic-PRM [44] Map nodes are generated uniformly at random. ffl OBPRM [7] Map nodes are generated randomly on or near obstacle surfaces.

41 31 ffl Gauss-PRM [16] Map nodes are generated uniformly at random and then filtered to retain only those near obstacle surfaces. Connection Methods (Some also generate nodes.): ffl Closest-K [44] A connection attempt is made between every map node and each of its k closest neighbors, where k is a user input. Neighbors are other map nodes. ffl ObstacleBased [7] A connection attempt is made between every map node and each of its k closest neighboring nodes on the surface of each of the m environment obstacles. For every node, k Λ m connections are attempted. ffl ClosestVE [this dissertation] A connection attempt is made between every map node and each of its k closest neighbors, where k is a user input. Neighbors are other map nodes and implicit nodes on map edges. ffl Connect-Components [4] Pair-wise connection attempts are made between all connected components. ffl Expand-CC [4] Pair-wise connection attempts are made between all connected components. Partial results from all failed connection attempts are retained as graph nodes and associated shorter (than attempted) edges. ffl VR-Variant [48], [this dissertation] Nodes are generated as user specifies. As each is generated, connection attempts

42 32 are made between it and all connected components. The node is retained unless it was connectable to exactly one connected component and does not expand that connected component more than r times the connected component's radius, where r is a user input. Local Planners (for moving from one configuration, c 1, to another, c 2 ): ffl straightline [44] Follows a straight line in C-space when moving from c 1 to c 2 by interpolating all dof along the way. ffl rotate-at-s, 0» s» 1 [3] Translates from c 1 to an intermediate configuration c 0, rotates to a second intermediate configuration c 00, and finally translates to c 2. The parameter s represents the fractional part of the translational distance between c 1 and c 2 that the robot travels from c 1 to c 0. The straightline planner is used to plan each of the segments, that is, between (c 1 ;c 0 ), between (c 0 ;c 00 ), and between (c 00 ;c 2 ). ffl A Λ -like planners [3, 24, 28, 38, 39, 65] An A Λ algorithm is guaranteed to return a path of minimum cost whenever a path exists. It is an example of a best-first searching strategy. The A Λ -like planners we have implemented vary according to: (1) number of neighboring configurations explored in each iteration, (2) the evaluation function used to select among the neighbors, and (3) the number of iterations they are allowed to run. The two evaluation functions used are (i) minimum distance from the goal, and (ii) maximum clearance from a workspace obstacle; both these functions are approximated using the center of mass of the relevant objects. Note these planners are not truly A Λ methods, but rather employ A Λ -like strategies.

43 33 We have installed three different collision detection libraries which vary in speed and sophistication. Collision detectors require, as input, descriptions of the workspace obstacles and the robot. In general, faster collision detection can be done where less information is calculated and returned. Some of our methods require the more sophisticated implementations while others do not. Therefore, for flexibility, we have installed the following. ffl C-Space Toolkit (CSTK) [69] is valid for arbitrary polyhedral objects and returns a closest (witness) pair of points when there is no collision. ffl V-Clip [56] is valid for convex objects and returns a witness pair and a clearance or penetration distance. ffl RAPID [29] is valid for arbitrary polyhedral objects and returns only a true/false answer. B. Platform The parallel machine used in this study was an SGI Origin2000 [61]. The Origin2000 is a ccnuma machine (cache-coherent, non-uniform memory access), where sets of processors are connected to form nodes, and sets of nodes are connected in a hierarchical fashion by an interconnection fabric built of dynamically-allocated switch-connected links. Each processor was a 195 MHz R10000 microprocessor. The Origin2000 supports a single global address space although the memory is physically distributed among the processors. The shared address space allowed us to use a shared memory programming paradigm, which greatly simplified our programming task (i.e., we did not have to deal with the complications of message passing).

44 34 a PC. For the experiments not requiring a parallel machine, we used Linux running on To date our code has been ported to the following platforms. ffl Linux smp, i686 processor ffl SunOS 5.7, SPARCstation-20 ffl HP-UX B D, 64-user license ffl SGI, IRIX 6.5

45 35 CHAPTER V SPEED OPTIMIZATIONS We propose two general strategies for speeding up roadmap construction: parallelization and faster failure detection during local planning. Parallelization divides the work among multiple processors. Faster failure detection during local planning can be approached in various ways. A. Parallelization The basic components of a successful parallelization effort are (i) load balancing among the processors so all do approximately the same percentage of the total work and (ii) reducing the overhead of communication among the processors. Our parallel formulation of several PRMs satisfies both criteria and demonstrates the power of combining these randomized algorithms with parallel processing. The general strategy used is divide-and-conquer. We want to divide all the collision detection calls, the limiting primitive for PRMs (Chapter III B), among multiple processors. This method applies to both generation and connection phases of traditional PRMs. The promise of parallel implementations has been acknowledged in the literature [34], although to our knowledge no previous experimental results exist. We summarize our results from [5] where we show that significant, scalable speedups can be obtained with relatively little effort on the part of the developer. Our conclusions are not limited to PRMs. In particular, we outline general techniques for parallelizing types of computations commonly performed in motion planning algorithms, and identify potential difficulties that might befaced in other efforts to parallelize sequential motion planning methods [5].

46 36 1. Divide and Conquer Even though node generation represents only a relatively small percentage of the roadmap construction time (1-2%), efforts spent parallelizing it are not wasted. Since it does take such a relatively small amount of time, we could afford to spend more time generating nodes of better" quality before turning things over to the connection phase. In addition, the programmer effort is minimal, while the speed up is substantial. Let n be the number of configuration nodes requested, m be the number of obstacles, p be the number of processors and pid be the processor identification number, all of which are unique. The parallel formulations are as follows. ffl Parallelizing the node generation phase of the basic PRM (Basic-PRM) method [44] is trivial. If n nodes are desired, we simply ask each of the p processors to generate n=p of them. The load balancing is excellent and the communication overhead is non-existent. ffl For transforming obstacle based PRM (OBPRM [4]) more than one way exists for partitioning the work of this generation method. Letting n obst be the number of nodes requested per obstacle, the method we chose is to have each of the p processors generate n obst =p nodes per obstacle rather than the sequential n obst. Again, the load balancing is excellent and the communication overhead is non-existent. Another way to parallelize this method is to assign obstacles to processors and let one and only one processor generate nodes on the surface of each obstacle. However, this partitioning is extremely problem dependent (e.g. on the number, size and complexity of obstacles) and, therefore, chronically prone to load imbalance.

47 37 ffl To transform the Closest-K connection method, we chose to divide the vertex list of configurations among the p processors and have each processor perform the usual Closest-K connection algorithm on its own set. Because the closest k neighbors of each processor's input set are not necessarily members of that processors input set, each processor must have read access to the full roadmap. Unlike the node generation phase, it is possible, and indeed likely, that some of the processors might generate the same connections. That is, our simplistic parallelization strategy causes some duplication of work. An edge connection can be attempted by two processors at most. Also all processors will read all nodes when computing distances. Again, the load balancing is excellent and the communication overhead is non-existent. Since the time required to add nodes and edges to the roadmap graph data structure was insignificant as compared to the rest of the computation, we elected to serialize access to that data structure. In particular, during both the node generation and connection phases, each processor collected a list of all the additions it would like to make to the evolving roadmap graph. Then, after the parallel portion of the phase was finished, a single processor added all nodes or edges to the graph. As is seen in our experimental results, this did not have a measurable impact on the speedups obtained (see Section 2). An additional benefit of serializing roadmap access was that the graph trivially remained uncorrupted which is always an important concern when modifying shared data structures in parallel codes. Finally, wenote that all of our parallel algorithms adhere to what is commonly known in the parallel computing literature as the concurrent read exclusive write (CREW) model.

48 38 2. Experimental Results Our measurements were made on heavily utilized multi-user systems, in interactive mode because that was the system most readily available to us. Nevertheless, we consistently obtained measurable speedups if the problem size was large enough. Measurements of small tasks (e.g., less than seconds) were unstable. Recall that node generation typically represents only 1-2% of the total roadmap construction time. For this reason, we elected to measure the node generation and the connection phases on different problem sizes. All algorithms were run on 1; 2; 4; 8; and 16 processors. Each experiment was run several times, each time with the random number generators identically seeded. The results reported are the best times obtained. The best times on an interactive system represent those times when our experiment suffered the least interference from competing processes. A summary of the results is reported in Table I. The motion planning environment we considered, see Figure 14, was a threedimensional environment containing two identical parallel blocks, each of dimension The `robot' is a smaller block (1 2 2) that can barely fit in the space between the parallel blocks. Although this environment is fairly simple, we note that similar speedups can be expected for any environment. That is, since our parallelization strategy is independent of the problem instance, our conclusions are also not dependent upon the particular problem instance. Two node generation methods, Basic-PRM and OBPRM,were selected for parallelization. Basic-PRM was selected as the most common representative of randomized roadmap methods. Nodes are generated and added to the roadmap independent of any consideration other than collision. OBPRM provides an example of a PRM variant which addresses the narrow passage problem. For node generation, the results

49 39 (a) (b) Fig. 14. The Sandwich Environment. (a) Side View. (b) Top View.

50 40 Table I. Parallel Processing Results. Preprocessing computation times and statistics for Generation and Connection phases of roadmap building. For Generation, we always requested PRM to attempt to generate 2000K nodes and OBPRM to attempt to generate 200K nodes. For Connection, we always worked problems which had previously requested 1500 nodes to be generated. Note that when some number of nodes was requested, some number less than that number was produced due to nodes generated but discarded as being invalid (in-collision). Architecture: SGI Origin 2000 Generation Phase Connection Phase PRM OBPRM Closest K= 5 Proc sec nodes sec nodes sec edges cc size reported are for problem instances in which we asked the methods (Basic-PRM and OBPRM) to generate 2000K and 200K, respectively, nodes. It was necessary to use such large problems to obtain running times that were large enough to be measured stably on the system used. No special significance is attached to each being a multiple of two or two hundred. Note that even when a certain number of nodes are requested, it is unlikely that exactly that many will be added to the roadmap. In the presence of obstacles, some percentage of the generated nodes wil be discarded as invalid (in-collision). The results for both Basic-PRM and OBPRM generation are shown in Figure 15. As can be seen, very good speedups were obtained even on such a heavily utilized system. Table I gives more detailed results, including the number of nodes successfully generated.

51 41 Fig. 15. Parallel Results. For node connection, the results reported are for problem instances in which we asked the node generation phase to generate 1500 nodes, which yielded approximately 1300 nodes for the connection phase. The connection algorithm used k = 5, so that connections were attempted between a node and the 5 other nodes closest to it. The Closest-K connection method is rarely used with values of k greater than 20 and most typically a value between 5 and 10 is selected. For these experiments 5 was selected as a representative value. Neither the parallelization nor the speedup results depend on any particular value of k having been selected. In Figure 15, all generation and connection methods show similar results. As the work is divided among more processors, there is a measurable speedup. Table I gives more detailed results, including statistics regarding the roadmap generated such as the number of connected components in the resulting roadmap, and the size of the largest connected component. These statistics are offered as reference for the connection progress made by Closest-K = 5. Figure 15 is a graphical depiction of the numerical data provided in Table I.

52 42 3. Discussion Our sequential planners use available collision detection libraries [56, 69]. Unfortunately, some difficulties were encountered when we tried to use these collision detection libraries in parallel because these libraries are optimized for performance on sequential machines. For example, they use global auxiliary data structures to exploit spatial coherence between subsequent collision detection calls and use static (register) variables for frequently accessed values. Since multiple processors will be executing collision detection calls at the same time, both of the above optimizations are problematic for parallel implementations. The solution we adopted was to make a separate copy of the data that was modified during the collision detection call for each active processor. This was relatively simple in V-Clip [56], since the processors only needed separate copies of the hash tables they used to record the closest features between object pairs on previous collision detection calls. The modifications that would have been required in the C-Space Toolkit [69] would have been more extensive. We note that wholesale replication of data, as was done with the V-Clip parallelization, is not always a good solution, as it may prove overly costly in terms of memory requirements. Our experience leads us to recommend that authors of collision detection libraries provide implementations capable of taking advantage of parallel platforms. Not only do we believe specialized parallel platforms will, in the future, become more widely available, but desktop PC's and workstations even now typically contain more than one processor. In summary, based on our experiments and experience parallelizing probabilistic roadmap motion planning methods, we have shown that significant, scalable speedups can be obtained with relatively little developer effort. The PRM methods have mas-

53 43 sive inherent parallelism which is easily exploited even by relatively simplistic parallelizing strategies. B. Faster Failure Detection Parallelization is one way in which to decrease roadmap construction time, this section presents another, which is by no means mutually exclusive. The general strategy of this section is to find ways of detecting failure faster during the connection phase of roadmap construction. The connection phase calls a local planner to decide whether or not to add an edge to the roadmap. The local planner, depending on the length of the edge it is passed, typically makes many collision detection calls before reporting whether the edge is collision free. Remember each edge represents a collision free path between configuration nodes and that collision detection is the limiting primitive for PRMs (Chapter III B). Only after exhaustively checking the path for collision can the path be reliably reported collision free. However, the path can be reported not collision free when the first collision is discovered. While there is no way to reduce work for successful edges, faster failure detection for non-successful edges is possible. Faster failure detection during the generation phase is rarely possible because methods already typically make very few collision detection calls per node before deciding whether or not to keep a configuration (e.g., for Basic-PRM only one). Therefore the methods in this section apply primarily to the connection phase of roadmap construction. One of the simplest optimizations we make is to greedily take the first edge connecting a node to any connected component rather than searching for or considering better possibilities. The roadmap components that we build are therefore trees. In terms of shortest paths between nodes, the resulting graph is unlikely to be optimal.

54 44 This non-optimal graph results in lower construction costs than would be incurred if cycles were permitted in the graph or if longer edges were replaced by shorter ones. To replace one edge with another, both must be validated as collision free by a local planner. Recall that collision detection calls are the limiting primitive in roadmap construction (Chapter III B). This greedy edge heuristic, while the same in spirit as the others in our faster failure detection category, is unique in that it completely avoids collision detection calls on an edge rather than attempting to detect failures (collisions) on that edge more quickly. Clearance-Checking, Increasing-Resolution and Line-Segment-Approximation are three heuristics we have implemented for attempting to detect failures on an edge more quickly. They represent more intelligent ways of invoking and utilizing existing off-the-shelf collision detection packages. 1. Greedy Roadmap Edges GreedyCC is a simple heuristic which always takes the first path found between nodes. Before trying to add a candidate edge to the roadmap, a simple check is first made to determine whether or not the two vertex configuration nodes are already part of the same connected component. If they are, the connection algorithm discards the edge and proceeds to the next candidate edge. We suspect that many roadmap motion planning implementations do this but are aware of no documentation. The consequence of implementing this connected component check is a roadmap without cycles. Connected components are trees. Once a path between two nodes is discovered, there is never an attempt to replace it with a shorter one. Graph optimizations of the roadmap are postponed to a post processing step. We find this simple optimization to be very effective and use it throughout the study unless otherwise noted.

55 45 2. Clearance Checking Some of the collision detection packages currently available return the distance to the nearest obstacle in addition to a collision/no-collision response. For convex objects, V-Clip [56] is one of these. V-Clip uses negative distances to indicate penetration by the numerical value while positive distances indicate a clearance. Because it is redundant tocheck for collision within a known clearance distance, the local planner can make faster progress by skipping the clearance distance away from the current position before making another call to the collision checker. The Clearance-Checking heuristic requires trivial overhead but it can only be used when a clearance distance is available from the collision detection package. Also, what is meant by `clearance' must be well defined. Care must be taken when used with rotation. The direction of rotation as well as the center of rotation can have minor or profound effects. The effect can be demonstrated with articulated bodies where a small rotation at one joint can cause large movements of connected masses several joint segments away. The robot complexity directly impacts the interpretation of `large' and `small' distances (clearance). When the robot is other than a rigid body, simple Euclidean distance may nolonger suffice. 3. Increasing Resolution Most local planner implementions, when presented with a pair of nodes (configurations) and asked to validate the edges (path) between them, start from one node and step serially through the collision checks which must be done until the distance is covered and the second node is reached or a preemptive collision is discovered. The Increasing-Resolution heuristic gradually increases the resolution distance between steps. That is, first the midpoint of the edge is checked; then the midpoint of the

56 46 two halves; and so on in a binary search pattern of checks rather than a sequential one. By iteratively incrementing the sampling resolution when validating an edge, we increase the chance, on average, of detecting collisions sooner. There is a slight overhead associated with calculating the recursive midpoints to check. As noted previously (Chapter III D 2), Lazy PRM [15] uses an increasing resolution strategy on entire paths (multiple edges) at a time. We are aware of no further documentation of this heuristic although it is likely that other researchers with mature development platforms have also considered it. 4. Line Segment Approximation No matter how the local planner patterns the order of exhaustive collision checks along an edge, the possibility always exists that the final collision check is the one which fails. In the worst case, this could always be true due to the discrete nature of the collision checking. In the case of gross collision however, there is a way to discover the collision with only one collision detection call. (See Figure 16 for an example of a `gross' collision along an edge.) In Line-Segment-Approximation the local planner will first perform a gross approximation to checking the entire path for collision. Only if the input nodes seem connectable" by the gross approximation method will the path be thoroughly checked for collision. Recall that the path of the robot in a 3D (2D) workspace traveling from one node to another sweeps out a volume (area) in the workspace. Referring to Figure 16, we see on the left the swept area of the small square robot as well as the straight line sketched out by its center of rotation as it travels the edge from the upper left to the lower right configuration. The solid black horizontal bar is a workspace obstacle which will block the proposed edge. We can replace the robot's swept volume (area) with a representative subset. For example, if the robot's center of rotation is a point

57 47 Fig. 16. Line Segment Approximation. The solid black horizontal bar is an obstacle grossly in collision with the proposed path. on the robot, then that point sweeps out a line within the swept volume (area) of the entire robot. If the local planner is a straight line (in C-space) local planner, then the line swept out by the center of rotation will be a straight line in the workspace as well. The entire swept line can be presented to the collision checker as a single pseudo-robot configuration. Only when this single pseudo-robot configuration is NOT in collision does the local planner need to thoroughly check the path for collision. This heuristic has a much larger overhead than the others. Forming the pseudorobot is quite expensive and a unique operation for every edge checked. The operation is unique because every edge will be represented by a pseudo-robot with the appropriate corresponding length. To address the overhead issue, we suggest a tolerance be used with this heuristic. Only if the submitted edge length is greater than the tolerance, should the local planner deploy the line segment approximation. The heuristic as implemented is restricted to straight line local planners and robots which have their center of rotation inside the robot but the method is not theoretically so restricted.

58 48 5. Experimental Results In the results of this section, the speed of roadmap construction time is measured in the number of collision detection calls. Collision detection is the limiting primitive operation for PRMs (Chapter III B). For our faster failure detection experiments we use two environments. In the sandwich, the robot is a block ( ) shown in Figure 17 sandwiched between two larger flat rectangular blocks ( ) which are separated by a distance of 2 units. The flange comes from an engine assembly design. It consists of a fixed rectangular part with a circular opening (the obstacle, 990 triangles) and a curved pipe (the robot, 5306 triangles) that must be inserted into the opening in the obstacle. The fitting between the two parts is very close (see Figure 17). Many modeling systems describe objects as polygonal surfaces. Polygons are typically decomposed into triangles. The number of triangles used to describe an object gives the size of the data structure needed to store it and which must be manipulated by the collision detection routines. Clearance-Checking and Increasing-Resolution were both employed when gathering the results shown in Figure 17. It is natural to use the two methods together as they are quite complimentary. Having clearance information available while gradually increasing the resolution of collision checking of an edge often allows the edge to be completely checked for collision faster. Clearance checking speeds sequential as well as other patterns of collision checks on an edge by returning enough information to avoid some calls. As can be seen in Figure 17, for some problems the combined method strategy can have a significant impact. For the sandwich problem, many costly collision detection calls were avoided by the usage of Clearance-Checking and Increasing-Resolution. For the flange, while there is some improvement, it is much less dramatic. The tightness of the fit between the robot (curved pipe) and the ob-

59 49 Fig. 17. Clearance and Resolution Results. stacle (rectangular part with circular opening) dictates failures (collision detections) occur very quickly without any need to speed up their detection. An edge can only be validated after ensuring that each and every resolutionally small increment (a.k.a. `tick') lying on it is collision free. Since the resolution size is fixed, longer edges have more ticks, resolutionally small subdivisions of its length, and shorter ones have fewer. Line-Segment-Approximation results are shown in Figure 18. The bar to the far left of each set of results (sandwich and flange) shows the number of collision detection calls made without employing Line-Segment-Approximation. Each of the other three bars shows the results for the tolerance value set to 2, 20 or 200 ticks, respectively, from left to right. Assume the tolerance is set to 20. If when dividing the length of an edge by the resolution, the result is some number of ticks greater than 20, the Line-Segment-Approximation will be used to perform a preliminary gross resolution collision check before calling a finer grain check. When the result is 20 or fewer ticks, only the fine grain check will be used. Notice in the sandwich results, that for some threshold value between 20 and 200, the improvement is lost. This is because most edges generated contained fewer

60 50 Fig. 18. Line Segment Approximation Results. Various r. than 200 ticks and so the Line-Segment-Approximation method was never being used. When the tolerance is set too high (200 for the sandwich) there is no benefit from the method. When the tolerance is set too low (2 for the flange) the overhead of running the method is actually detrimental to the roadmap construction time. Tolerance settings are problem dependent, as these results demonstrate. The results we obtained from using the Line-Segment-Approximation method on the relatively open environment ofthesandwich were quite good. Once again we see no real improvement onthe more difficult flange environment. 6. Discussion In a PRM local planner, speed is valued above sophistication. However, as most of the heuristics in this category demonstrate, there may be a middle ground where some overhead for sophistication is justified. In crowded workspaces where failure is the rule rather than the exception, faster failure detection is essential. Naive PRM strategies for generation and connection are fine for uncluttered environments. Exactly where basic PRMs start to break down is where the heuristics and strategies of this section

61 51 begin to pay off. We believe Line-Segment-Approximation is a valuable tool to have available for some problems. However, for the problems studied here, the Clearance-Checking and Increasing-Resolution combination demonstrates faster failure detection. Although outside the scope of this study, see [3] for discussions of the impact of choosing good distance metrics and local planners.

62 52 CHAPTER VI QUALITY OPTIMIZATIONS All the heuristics discussed in Chapter V are aimed towards improving the speed of roadmap construction using existing PRMs strategies. The same roadmaps will be built; but they will be built faster. While speed is one part of the optimization package we offer, quality is another. To improve the quality of the roadmaps constructed we must discover new ways to explore environments. As discussed, PRMs are a valuable tool for handling high dof motion planning problems and they perform well up to a point. Now we explore beyond that point. All the methods in this chapter are designed to make a powerful iteration beyond the simple first pass of the standard PRMs discussed. Figure 19 shows the architecture of the initial PRMs. However, it is common in recently proposed PRMs to include one or more improvement iterations as shown in Figure 20. The first pass through the generation and connection phases provides a gross approximation of the free space connectivity. The second and successive passes should refine that approximation. Roadmap quality is measured by how accurately a roadmap reflects the connectivity of the space in which the robot is free to move, the free C-space. Each pass should ideally provide an enhancement to roadmap quality. Each of the heuristics in this chapter, when used properly, is designed to do just that. ClosestVE considers the implicit nodes on every existing edge when connecting new nodes. Connect-Components is the only method in this chapter which does not add new nodes to the roadmap graph, only new edges, as it tries to make connections between the connected components. Expand-CC adds new nodes which are the partial failures of the Connect-Components method. VR-Variant generates and adds new nodes based on a user supplied measure of progress. Other researchers have used a

63 53 robot workspace robot Generate Nodes workspace Enhancement Local Planner edges Generate Nodes Connect Nodes no Local Planner edges Motion Planner Connect Nodes Distance Metric Collision Detection Motion Planner stopping criteria met? yes Distance Metric Collision Detection roadmap roadmap Fig. 19. Typical Two Phase PRM. Fig. 20. Optimizing PRM Quality. specific version of VR-Variant [48] as a stand alone PRM. See III D 3 for review. We use all the heuristics in this section as resource methods in our endeavor to build a better roadmap. A. Implicit Connections In the generation phase of PRMs, nodes are added to the roadmap. In the connection phase, pairs of nodes are selected and each pair is presented to a local planner as a candidate edge for the roadmap graph. Edges are validated to some predefined resolution. The local planner can only validate an edge after ensuring that each and every resolutionally small increment (a.k.a. `tick') lying on the path between two nodes in a pair is collision free. Because most local planners are deterministic, the ticks (implicit nodes along the edge) do not need to be saved and the roadmap can be stored as an adjacency graph. Assuming at least one edge has been added to the roadmap, there will be many implicitly stored graph nodes. Typically, shorter edges are considered more likely to be connectable by a local

64 54 planner. Connection strategies use a distance metric, commonly Euclidean distance or some variant, to sort candidate edges. See [3] for more information on the selection of good distance metrics and local planners. In Figure 21 existing map nodes are shown as the larger solid black circles. One existing roadmap edge is shown as a solid black line. The unfilled circles are new nodes which are not yet connected to the roadmap. In order for the the existing map edge shown to have been added to the graph, it must be collision free. That is, the edge must have been checked for collision at resolutionally fine increments along its length. Each of these increments, or ticks, represents an implicit roadmap node. They are shown in Figure 21 as smaller black circles on the roadmap edge. The nodes are not explicitly stored in the typical adjacency graph representation of the roadmap. The implicit connection" strategy can be employed to connect a set of newly generated nodes like those represented by the unfilled circles in Figure 21. We call this strategy ClosestVE because it tries to connect new nodes to the closest existing vertices (explicit nodes - large black circles) or edges (implicit nodes - smaller black circles). In particular, for each new node, we check edges which if valid (collision free) would connect that node to an existing node or the closest implicit node on an existing edge. Using ClosestVE, nodes which have been checked for collision, but which are not explicitly present in the roadmap, are considered when choosing promising candidate edges to send to the local planner. Because ClosestVE has all nodes in the roadmap to chose from, both explicit and implicit, this heuristic will consider connections which are not considered by other heuristics. There is an overhead associated with calculating the closest point on a line l to a given point p in C-space where p is the new node we wish to add to the graph and l is an edge of the graph. For this reason, ClosestVE, like the other methods in Chapter VI, should be used with discrimination and usually only after simpler

65 Fig. 21. ClosestVE Considering Implicit Graph Nodes. Larger filled circles represent explicit nodes. Smaller filled circles represent implicit nodes. Unfilled circles are new nodes which ClosestVE can connect (dashed lines) to the implicit graph nodes (smaller filled circles) on the existing graph edge (solid line). The Basic-PRM and variants only consider explicit nodes (larger filled circles) when attempting connections. In this example, no straightline local planner, the most common in PRMs, will succeed in connecting new nodes (unfilled circles) to the existing map nodes (larger filled circles).

66 56 methods have already done the `easy' work. For example in Figure 1, any basic PRM or PRM variant used to make a `first pass' at mapping the workspace will quickly discover the two large open areas. However, the narrow passage connecting them will likely be missed with simple methods. ClosestVE, like others presented in Chapter VI, is a more sophisticated heuristic and should be used as part of a second or later enhancement step (see Figure 20). The pseudo code below shows the sequence of events. Nodes are generated and connected into a roadmap during Pass I. Pass II continues to build the roadmap with more nodes and more connection attempts. We show one iteration in the pseudo code, but in principle Pass II could be repeated as many times as proved advantageous. Note, n 1 is the number of nodes generated in Pass I and n 2 is the number of nodes generated in Pass II. Pass I 1. Generation of n 1 nodes 2. Connection - Simple Method Pass II (repeat as desired) 3. Generation of n 2 nodes 4. Connection - Sophisticated Method Our implementation of ClosestVE sorts the closest k node-node or node-edge pairs only once for each new node where k is a small constant value which can be specified by the user. New edges added to the roadmap by ClosestVE are not used to update these pairings. For this reason, in the procedural script above, if Pass Iis omitted, ClosestVE is nothing more than the familiar Closest-K connection strategy typically used by PRMs. Initially, we did update the pairings with the new edges but prohibitively large running times quickly convinced us to abandon this practice. The connection phase of roadmap construction is computationally expensive and limiting the number of connections attempted to those which seem most promising is highly desirable. With relatively few nodes and edges, Pass I is expected to create a gross

67 57 approximation of the connectivity of C-free. In Pass II, nodes are generated more densely or in different ways or concentrated in specific areas in hopes of enhancing the roadmap. The goal of ClosestVE in the enhancement process is to connect the new nodes to the existing roadmap. While it is possible that by not updating our pairings as new edges are discovered we may miss some helpful connection possibilities, if we update a large number of pairs, the method becomes much slower. 1. Environments and Measures In the results of this section we compare the quality of roadmaps constructed using number and size of connected components as well as the number of isolated nodes (components of size one). Connection time is also a concern but a secondary one. Our interest in the number and size of connected components lies in how well these measures serve to reflect the roadmap quality. If a certain robot's workspace induces a free space which is all connected, then a roadmap of this workspace should ideally have only one connected component. Given two roadmaps with identical map nodes and a differing number of connected components, it seems likely that the roadmap with the smaller number of connected components will be superior. This is not an absolute conclusion, as we must also consider the composition of the connected components. However, number and size of connected components is a frequently used comparison measure between roadmaps. Fewer connected components indicate a better roadmap. Isolated nodes, connected components of size one, are those which could not be connected to the roadmap. The fewer of them which exist in the roadmap, the better, by the same reasoning. In the comb environment shown in Figures 22-24, the robot is a block measuring :5 :25 :25. Obstacles are a series of 10 parallel wall blocks measuring 4 4 :25. Spacing between walls is 1:25. The environment bounding box dimensions are

68 58 (a) (b) (c) (d) Fig. 22. The Comb Environment, Pass II - OBPRM. Pass I created a roadmap with two nodes and an edge connecting them. Pass II used OBPRM to generate 79 new nodes and a connection method of (a) Closest-K 5 (b) ObstacleBased (c) ClosestVE (d) ClosestVE - identical to (c) but visually depicts `implicit' nodes on the roadmap edge which were used to connect new nodes.

69 59 (a) (b) (c) Fig. 23. The Comb Environment, Pass II - BasicPRM. Pass I created a roadmap with two nodes and an edge connecting them. Pass II used Basic-PRM to generate 86 new nodes and a connection method of (a) Closest-K 5 (b) ClosestVE (c) ClosestVE - identical to (b) but visually depicts `implicit' nodes on the roadmap edge which were used to connect new nodes.

70 60 (a) (b) (c) (d) Fig. 24. The Comb Environment, Pass II - GaussPRM. Pass I created a roadmap with two nodes and an edge connecting them. Pass II used Gauss-PRM to generate 86 new nodes and a connection method of (a) Closest-K 5 (b) ObstacleBased (c) ClosestVE (d) ClosestVE - identical to (c) but visually depicts `implicit' nodes on the roadmap edge which were used to connect new nodes.

71 As shown in Figures 22-24, the bounding box `bottom' and `sides' coincide with those of the walls, leaving an open area above the walls. In the figures, the small blocks depict the workspace position and orientation of various robot configurations which are map nodes present in the various roadmaps. The lines connecting them represent edges produced by a straightline local planner. The lines show the straight line travel of the robot's center of rotation moving from one map node to another. The robot will rotate about the center of rotation as it translates from one endpoint of an edge to the other. 2. Experimental Results To obtain the results shown in Table II, and Figures 22-24, we first built a simple preliminary roadmap. The preliminary roadmap consists of two configurations located at the two extreme ends of the bounding box in the elongated free area which lies above the domino like arrangement ofwalls. The nodes are connected by a single straightline edge that traverses the distance between them. This one edge gives us many implicitly stored configurations which are located directly above the slots defined by the walls below. The experiments begin with the next pass. We use three different generation methods: Basic-PRM, OBPRM and Gauss-PRM.We combine them with three different connection methods: Closest-K, ObstacleBased and ClosestVE. All combinations of generation and connection methods are made with the exception of Basic-PRM- ObstacleBased. Due to the manner in which they are generated, Basic-PRM nodes do not possess the obstacle based information required by the ObstacleBased connection method. When the connection method in the combination is ClosestVE, there is one additional row of data. In Table II the three rows (one for each generation method)

72 62 Table II. ClosestVE Results. Preprocessing computation times and statistics for comparing ClosestVE heuristic against Closest-K and ObstacleBased (except for PRM nodes which have no obstacle information). The values in parenthesis are the number of implicit nodes used by ClosestVE. The `#CC' values indicate the number of connected components in the final roadmap while those labeled by numbers give the size in nodes for each of the largest three connected components. `#iso' values are the number of isolated nodes which could not be connected to any other node. Enhancement: ClosestVE (PC-Linux) Connection usec #nodes #edges #CC #iso Generation:Basic-PRM Closest k= ClosestVE 0.85 (69) (69) * Closest k= (69) (69) Generation:OBPRM Closest k= ObstBased ClosestVE 0.69 (47) (47) * Closest k= (47) (47) Generation:Gauss-PRM Closest k= ObstBased ClosestVE 0.86 (70) (70) * Closest k= (69) (69)

73 63 which have a label beginning with `*', contain this additional data. The results in these three rows show what happens after a second connection method is applied to the roadmap built by ClosestVE. Table II shows, for each of the generation and connection combinations, the connection time in microseconds, the number of roadmap nodes and edges, the number of connected components and the sizes in nodes of each of the largest three connected components. The values reported in parenthesis are the number of implicit nodes used to make connections. We also report the number of isolated nodes in the graph. Isolated nodes are those which could not be connected to any other node in the graph by the connection methods applied. As can be seen in Table II, each generation method created either 81 or 88 map nodes. After the ClosestVE connection is applied, there is always one large connected component containing most of the map nodes and isolated nodes. It was this result which prompted us to run the Closest-K strategy after ClosestVE. Doing so greatly reduces the number of isolated nodes as can be observed on rows labelled `*'. Figures show in a most striking manner the differences between the connection methods and why ClosestVE does so well. These figures show for each of the generation methods, that only ClosestVE made the kinds of connections necessary to build a roadmap which accurately reflects the connectivity of the free space. The initial figures in each set (Figures 22(a), 23(a) and 24(a)) show the roadmap after Closest-K has been used to connect the roadmaps. Recall that the top horizontal edge and its end point configurations were created in a preliminary step. From these figures we see that the connections made by Closest-K were to nearby nodes as intended. However, none of these connections was able to escape the small free space between the enclosing walls. OBPRM and Gauss-PRM generated nodes, again by intent, are located close to the walls which means that many times the closest k

74 64 pairs were on opposite sides of an intervening wall. The second figures in the OBPRM and Gauss-PRM generation sets (Figures 22(b) and 24(b)) show the roadmap after ObstacleBased heuristic has been used to connect the roadmaps. Recall that the top horizontal edge and its end point configurations were created in a preliminary step. From these figures we see that the connections made by ObstacleBased were also unable to escape the small free space between the enclosing walls. In this case, nodes not associated with obstacles (the two in the preliminary map) were not considered for connection. Figures 22(d), 23(c) and 24(d) are identical in most respects to the figures immediately above them, Figures 22(c), 23(b) and 24(c), respectively. The difference is the explicit visual depiction of the `implicit' nodes on the roadmap edge which were used to connect new nodes to the roadmap using ClosestVE. In these figures we see most of the configurations making connections that will allow movement between differing wall separated free spaces. 3. Discussion Only ClosestVE made the kind of connections necessary to correctly represent the connectivity of the free space in the comb environment. Refer to Figures Furthermore, running yet another connection phase (Closest-K) after ClosestVE eliminated most of the isolated nodes. Isolated nodes, although they may reside in truly disconnected areas of C-free, more often indicate inadequately explored areas of C- free. Their presence will frequently necessitate greater computation on the part of enhancement algorithms to distinguish between these cases. In every experiment, ClosestVE built a single large connected component with very few isolated nodes remaining. Isolated nodes are those which could not be connected to any other node in the graph by any of the connection methods applied.

75 65 Their presence in a roadmap can indicate a disjoint area of free space, a region that is insufficiently explored, or a narrow passage. In all cases they are of interest. In this case the free space is contrived to be connected so the presence of isolated nodes in the roadmap is not necessary. The relatively small numbers of isolated nodes remaining after ClosestVE connection is applied to this problem is a measure of its merit as a new connection strategy. Apparently avery simple roadmap of the easy parts of the free space should be as heavily utilized as possible, both explicit and implicit parts of it. We note here that applying ObstacleBased connection to nodes generated with Gauss-PRM is our own work. Although obstacle based information for each map node is available to them, neither in [16] nor in any subsequent publication are we aware of the authors utilizing obstacle based connection methods. B. Connect Components With and Without Expansion Connect-Components is the only method described in Chapter VI that does not add any new nodes to the graph. Not adding new nodes can be a strength or weakness. With the number of map nodes held static, the connection task is computationally confined. Other methods we discuss in Chapter VI add some number of nodes. Thus the connection task, with its complexity related to the number of nodes, may become more time consuming. It is preferable to `cover' a workspace with as small a map as possible. Roadmaps with many redundant nodes and edges are more expensive to build (preprocessing), search (run-time), and maintain (re-use). Nodes and edges represent redundant information when they lie in areas of C-free which are already well covered by other nodes and edges in the graph. The Connect-Components connection method attempts to connect all pairs of

76 66 connected components in the graph. See Figure 25. Connect-Components is usually applied after another connection method has created some connected components of size greater than one. If all connected components are of size one, (e.g. if no previous connection method has been applied), Connect-Components degenerates into an exhaustive (all nodes) Closest-K connection attempt. The goal of Connect-Components is to connect all pairs of connected components. If both components in a pair are small (less than some user specified number of nodes), then we try to connect all pairs of nodes. If at least one of the components is large, we try to connect the k pairs closest pairs of nodes, where k pairs may also be user supplied. The Expand-CC connection method operates in exactly the same fashion as Connect-Components with one exception. See Figure 25 versus Figure 26. Where the Connect-Components method discards failed connection attempts, Expand-CC retains them. The last free node of each `failed' edge is added to the graph as a new node along with the connecting partial edge. In the best case, this method, by keeping partial attempts, makes valuable incremental progress towards discovering the connection between connectable free areas. In the worst case, connected components may become very large while redundant information is added to the roadmap. 1. Environments and Measures In the tunnel environment there are two large open areas separated by a tunnel (Figure 27). Bounding box dimensions are The tunnel has a rectangular cross-section 1 1 8:5 which makes it a fairly tight fit for a :5 :5 1 block robot using a straightline local planner. Two open areas measuring 4 4 4:25 lie at either end of the tunnel. Because all the collision packages installed use surface intersections to determine collision, the tunnel environment was constructed by placing many identical

77 67 Fig. 25. Connect-Components Attempts Pairwise Connections. The dashed line is a new edge connecting two previously separate connected components of 4 nodes and 2 nodes into a larger 6 node connected component. Fig. 26. Expand-CC Keeps Partial Paths. New nodes (small unfilled circles) are added by method via new edges (dashed lines). Pre-existing nodes are shown as small filled circles connected by pre-existing edges shown as solid lines.

78 68 (a) (b) (c) Fig. 27. The Tunnel Environment. (a) Shows the aligned walls and the tunnel opening. (b) Shows the same figure with the walls rendered in wire frame. The tunnel is shown as a solid mass for visual clarity. (c) Shows only the tunnel outline sketched out in wire frame.

79 69 thin walls in juxtaposition. Each wall has an identical tunnnel opening and the walls are aligned. In Figure 27, (a) shows the aligned walls and the tunnel opening and (b) shows the same figure with the walls rendered in wire frame. The tunnel is shown as a solid mass for visual clarity. Finally, in (c) only the tunnel outline is sketched out in wire frame. In successive figures the environment will be presented as in (c) to focus attention on roadmap edges and nodes. Robot configurations may or may not be rendered in the figures. Edges are produced by a straightline local planner. The lines show the straight line travel of the robot's center of rotation moving from one map node to another. The robot will rotate about the center of rotation as it moves from the orientation of the map node at one end point of an edge to the map node at the other end point. To study the roadmaps, we look closely at the number and size of connected components. The number of connected components is a reasonable measure while the size of them is included to demonstrate the different methodologies of Connect- Components and Expand-CC. Only one round of Expand-CC is employed. The connected components are identified before the round begins and not re-evaluated until it ends. The number of collision detection calls made will correspond closely to the connection time of the methods and is therefore another appropriate measure to use. 2. Environmental Results The Connect-Components method, without generating any new nodes, seeks the free space connections between connected components in the roadmap. The roadmap will grow only in the number of edges. Therefore, roadmap size is limited. Even without restricting our roadmaps to be forests, the roadmap could only grow byo(jnodesj 2 ). Figure 28 shows the map nodes generated by Basic-PRM. Out of over 100 nodes generated, one was located inside the tunnel. Figure 29 shows the connections made

80 70 Fig. 28. Connect-Components - Input Nodes. Fig. 29. Connect-Components - Map Edges by Connect-Components. Notice that the single node located inside the tunnel was connected to the large connected component on the right. The Expand-CC method does the same connectivity work as Connect-Components while adding all partial results. The roadmap can grow in both nodes and edges. This growth can be observed in Figures 30 and 31. In Figure 30, the dark nodes are added to the roadmap by Expand-CC. In Figure 31 it is possible to observe the progress that expansion has made possible towards joining the connected component located inside the tunnel to the connected components mapping the larger free areas at either end of the tunnel. Referring to Table III, after an initial connection method of Closest-K for k =3 was applied, there were five connected components in the roadmap. In Figure 28 we can see that as luck would have it, Basic-PRM initially generated one node ap-

81 71 Fig. 30. Expand-CC - Dark Nodes are New. Light Nodes are Pre-Existing. Fig. 31. Expand-CC - Map Edges. Table III. Connect-Components and Expand-CC Results. Preprocessing computation times and statistics for Connect-Components and Expand-CC. The `#CC' values indicate the number of connected components in the final roadmap while columns labeled by numbers give the size in nodes for each of the largest three connected components. `#iso' values are the number ofnodes which could not be connected to any other node - isolated. Column `#LPs' is the number of local planning attempts. Enhancement: Connect-Components & Expand-CC (PC-Linux) Connection usec #nodes #edges #CC #iso #LPs Closest K= ConnectC ExpandCC

82 72 proximately mid-way along the tunnel. Applying Connect-Components as the next connection method brings the total number of connected components down to two. The single node in the tunnel was connectable to one of them. This did not have to be the case and in fact was quite fortunate. As noted in the previous subsection, manuverability using a straightline local planner is quite limited in the tunnel environment given the robot's size and shape. By this we can be sure that the node in the larger free area at the right to which the tunnel node was connected must have a very similar orientation. A great deal of rotation would not have been possible along that path edge. Table III shows that 18 nodes were added to the roadmap by Expand-CC. By referring as well to Figures 30 and 31 it is clear that many of these were added in the tunnel. Some were also added in the well-explored open areas, however. These nodes are surely redundant. One of these redundant nodes is clearly visible in the right hand free area of Figure 30. Recall that the lighter colored nodes are those which were already in the roadmap before Expand-CC connection was applied. The dark nodes are those which Expand-CC added to the graph as a by product of its connection attempts. 3. Discussion The two methods, Connect-Components and Expand-CC, while similar in implementation, are quite different in philosophy. The advantage of Connect-Components is that roadmaps remain as small as possible and the computation task is contained. The disadvantage lies in whatever useful work and information may be discarded when roadmap components cannot be connected. There could easily be cases where this method will work very hard and accomplish very little or even nothing. In the tunnel example studied, nothing very interesting would have happened as a result of

83 73 applying the Connect-Components connection method if Basic-PRM had not generated the very useful node inside the tunnel or if the tunnel had not been straight. The Connect-Components method maintains a minimal roadmap but can only be as good as the map nodes with which it must work. The advantage of Expand-CC is that it is not limited in this way. It can generate its own nodes as a direct by product of its component level connection attempts. Keeping the successful parts of otherwise failed edges will make it a straightforward task for the method to walk itself out of the narrow tunnel during successive applications of the method. The disadvantage of the method is that it does add nodes to the graph. The method has no way of determining whether the nodes it is adding are helpful or not. They are added without any sort of filtering process. C. VR Variant As noted previously (Chapter III D 3), the visibility roadmap method [48] generates nodes using Basic-PRM. Refer to Figure 32. As each node is generated, the method will attempt to connect it to all the currently existing connected components in the roadmap. Nodes connectable to only one connected component are discarded, while all others are added to the roadmap. Our VR-Variant method keeps the same nodes as the visibility roadmap method, but it does not discard the same nodes. Refer to Figure 33. Rather than discarding all singly connectable nodes, we filter them. Those which expand a connected component in some way are added to the roadmap. Specifically, in VR-Variant, n nodes are generated in whatever manner the user specifies. Each time a new node is generated an attempt is made to connect it to all known connected components. The connection method used is Closest-K where the user may specify k. A node is kept if it can be

84 74 Fig. 32. Visibility Roadmap (Visibility Roadmap) Method. As described by Laumond. A node is kept if it can be connected to multiple connected components or none. r Fig. 33. VR-Variant - Generalized Method. A node is kept if it can be connected to multiple connected components or none. If the node is connectable to exactly one connected component, it will be retained if it expands a connected component in terms of distance from its center. It is our understanding that Laumond's originally proposed method [47], is identical to ours when r = 1.

85 75 connected to multiple connected components or none. If the node is connectable to exactly one connected component, it will be retained if its distance from the center of the connected component is greater than a user specified r times the current radius of that connected component. That is, a node will be kept only when it expands a connected component in terms of distance from its center. As each new node is added to the graph, the VR-Variant updates its connected components list. It is our understanding that Laumond's originally proposed method [47], is identical to ours when r = 1 and the generation method is always via Basic-PRM (completely random). See Figure 32 versus Figure 33. In our method the distance between a newly generated node and the center of the (one) connected component to which it was connectable is compared to the current radius of the component. If the distance is greater than the radius times some user specified expansion factor, the node is added to the graph with its connecting edge. Those which are less, are discarded. A connected component's center is calculated by treating each configuration in the component as a vector so that a vector sum can be taken over them. The sum, divided by the component's configuration count, is the `center'. The `radius' is obtained by summing the distances of all component configurations from the center and dividing by the configuration count of the component. X center =(1=jCCj) c X c2cc radius =(1=jCCj) Distance(c; center) Where c2cc CC = all configurations in a particular connected component

86 76 Fig. 34. Alpha Puzzle Results. Pass I consisted of a Basic-PRM and OBPRM generated mix of nodes connected using a Closest-K method. Pass II results are shown with `Components' (Connect-Components), Expand-CC and VR-Variant with rfactor=2. In other words, the average configuration in a connected component is taken to be the center of that connected component and the average distance from that center is taken as the radius. 1. Environments and Measures Our experimental setup is identical to that of Section B of this chapter. Again we utilize the tunnel environment shown in Figure 27. We report the same types of statistics for the same reasons. Another set of experiments were run on a more difficult and less well-understood problem the alpha puzzle. The alpha puzzle consists of two identical twisted, intertwined tubes (one the obstacle and one the robot, 1008 triangles each); the objective of the puzzle is to separate the tubes (see Figure 34).

87 77 2. Environmental Results Table IV shows for four different values (0,.5, 1, and 2) of VR-Variant's radius multiplication factor what happens when a varying number of nodes are generated and added to the roadmap by the algorithm. For all factors chosen, the roadmap nodes can be joined into a single connected component. Since the same input set of map nodes is used as in Section B of this chapter, we again initially have approximately half the nodes on either side of the tunnel and exactly one node inside the tunnel. See Figure 28. Therefore, when all the nodes are joined in one connected component we have a good roadmap for this particular problem environment. Figure 35, which shows the roadmap and nodes added by VR-Variant for 5 generated nodes and a radius multiplier of 2, assures us that this is indeed the case. Notice that in all cases when a single connected component was created, the number of collision detection calls made by the local planners (`cdlp') was pretty much the same. (The only exception is for 5 generated nodes with radius of 2 and there the number is rather than ) This indicates that the crucial connection nodes to be generated were some distance (a factor of 2) away from the connected component centers. When the r factor is set lower than 2, more newly generated nodes are necessary to discover connections between the existing connected components. For the alpha puzzle in Figure 34, we apply not only the methods of this section but also those of Section B of this chapter. In the figure, VR-Variant results are with rf actor = 2. All of the methods reduce the number of connected components. Expand-CC adds many nodes to the roadmap and has by far the largest single connected component as measured by number of nodes. Unfortunately, our studies have led us to suspect that many of these nodes are redundant. The nodes are redundant if they are generated in areas of the free space which are already well represented

88 78 Table IV. VR-Variant Results. Preprocessing computation times and statistics. The `CC' values indicate the number of connected components in the final roadmap while those labeled by numbers give the size in nodes for each of the largest three connected components. `iso' values are the number of nodes which could not be connected to any other node - isolated. Column `CDs' is the total number of collision detection calls. Column `LPs' is the number of local planning attempts. Column `cdlps' is the number of collision detection calls made by the local planner. Enhancement: VR-Variant (PC-Linux) Connection usec nodes edges CC 1 2 iso CDs LPs cdlp Radius = 0.0 VR-Var, 5 nodes VR-Var, 10 nodes VR-Var, 20 nodes VR-Var, 30 nodes VR-Var, 40 nodes VR-Var, 50 nodes VR-Var, 60 nodes Radius = 0.5 VR-Var, 5 nodes VR-Var, 10 nodes VR-Var, 20 nodes VR-Var, 30 nodes VR-Var, 40 nodes VR-Var, 50 nodes VR-Var, 60 nodes Radius = 1.0 VR-Var, 5 nodes VR-Var, 10 nodes VR-Var, 20 nodes VR-Var, 30 nodes Radius = 2.0 VR-Var, 5 nodes

89 79 (a) (b) (c) (d) Fig. 35. VR-Variant - Results (a) Shows the 105 input nodes generated by Basic-PRM. (b) Shows the addition of darker nodes added by VR-Variant. (c) Shows only the darker nodes added by VR-Variant to the roadmap. (d) Shows the roadmap constructed by VR-Variant adding only 3 with 5 requested nodes to generate and r =2.