E. Mazer, J. M. Ahuactzin and P. Bessiere

Volume 9, 1998

**Links to Full Text:**

We present a new approach to path planning, called the ``Ariadne's clew algorithm''. It is designed to find paths in high-dimensional continuous spaces and applies to robots with many degrees of freedom in static, as well as dynamic environments --- ones where obstacles may move. The Ariadne's clew algorithm comprises two sub-algorithms, called SEARCH and EXPLORE, applied in an interleaved manner. EXPLORE builds a representation of the accessible space while SEARCH looks for the target. Both are posed as optimization problems. We describe a real implementation of the algorithm to plan paths for a six degrees of freedom arm in a dynamic environment where another six degrees of freedom arm is used as a moving obstacle. Experimental results show that a path is found in about one second without any pre-processing.

Journal of Artificial Intelligence Research 9 {1998} 295-316 Submitted 9/97; published 11/98 The Ariadne's Clew Algorithm Emmanuel Mazer Emmanuel.Mazer@imag.fr Laboratoire GRAVIR, INRIA 665 Avenue de L'Europe 38330 Montbonnot, France Juan Manuel Ahuactzin jmal@mail.udlap.mx Depto. de Ing. en Sistemas Computationales Unversidad de las Americas Puebla, Cholula, Puebla 72820, Mexico Pierre Bessi022ere Pierre.Bessiere@imag.fr Laboratoire LEIBNIZ, 46 Avenue Felix Viallet 38000 Grenoble, France Abstract W e present a new approach to path planning, called the \Ariadne's clew algorithm". It is designed to find paths in high-dimensional continuous spaces and applies to robots with many degrees of freedom in static, as well as dynamic environments - ones where obstacles may move. The Ariadne's clew algorithm comprises two sub-algorithms, called search and explore, applied in an interleaved manner. explore builds a representation of the accessible space while search looks for the target. Both are posed as optimization problems. We describe a real implementation of the algorithm to plan paths for a six degrees of freedom arm in a dynamic environment where another six degrees of freedom arm is used as a moving obstacle. Experimental results show that a path is found in about one second without any pre-processing. 1. Introduction The path planning problem is of ma jor interest for industrial robotics. A basic version of this problem consists of finding a sequence of motions for a robot from a start configuration to a given goal configuration while avoiding collisions with any obstacles in the environment. A simple version of the problem, that of planning the motion of a point robot among 3-dimensional polyhedral obstacles, hasbeen proved to be np-complete {Canny, 1988}. Generally speaking, the complexity of the problem is exponential in the number of degrees of freedom {dof} of the robot, and polynomial in the number of obstacles in the environment. Consequently, findinga path for a robot with many dof {more than five} in an environment with several obstacles is, indeed, a very difficult problem. Unfortunately, many realistic industrial problems deal with robots of at least six dof and hundreds of obstacles. Even worse, often the environment is dynamic in the sense that some of the obstacles may move, thereby further requiring that new paths be found in very short computing times. c fl1998 AI Access Foundation and Morgan Kaufmann Publishers. All rights reserved.Mazer, Ahuactzin, & Bessi 022 ere In this paper, we present a new approach to path planning, called the \Ariadne's clew algorithm 1 ". The approach is completely general and applies to a broad range of path planning problems. However, it is particularly designed to find paths for robots with many dof in dynamic environments. The ultimate goal of a planner is to find a path from the initial position to the target. However, while searching for this path, the algorithm may consider collecting information about the free space and about the set of possible paths that lie in that free space. The Ariadne's clew algorithm tries to do both at the same time: a sub-algorithm called explore collects information about the free space with increasingly fine resolution, while, in parallel, an algorithm called search opportunistically checks whether the target can be reached. The explore algorithm works by placing landmarks in the searched space in such a way that a path from the initial position to any landmark is known. In order to learn as much as possible about the free space, the explore algorithm tries to spread the landmarks uniformly all over the space. To do this, it places the landmarks as far as possible from one another. For each new landmark produced by the explore algorithm, the search algo- rithm checks {with a local method} whether the target can be reached from that landmark. Both the explore and search algorithms are posed as optimization problems. The Ariadne's clew algorithm is efficient and general: 1. The algorithm is efficient in two senses: {a} Experiments show that the algorithm is able to solve path planning problems fast enough to move a six dof arm in a realistic and dynamic environment where another six dof robot is used as a moving obstacle. {b} It is well suited for parallel implementation and shows significant speed-up when the number of processors increases. 2. The algorithm is general in two senses: {a} It may be used for a wide range of applications in robotics with little additional effort to adapt it. {b} Finally, the algorithm is general in that it may be adapted for a large range of search problems in continuous spaces that arise in fields that are not related to robotics. The paper is organized as follows. Section 2 presents the path planning problem and discusses related work. Section 3 presents the principleof the Ariadne's clew algorithm. Section 4 describes the application of the algorithm to a six dof arm in a dynamic envi- ronment. Finally, Section 5 concludes the paper with a discussion of the contributions of our approach, the main difficulties involved, and possible improvements of our method.1. According to Greek legend, Ariadne, the daughter of Minos, King of Crete, helpedTheseus kill the Minotaur, a monster living in the Labyrinth, a huge maze built by Daedalus. The main difficulty faced by Theseus was to find his way through the Labyrinth. Ariadne brilliantly solved the problem by giving him a thread {or a clew} that he could unwind in order to find his path back. 296The Ariadne's clew algorithm 2. The Path Planning Problem Many versions of the path planning problem exist. An exhaustive classification of these problems and of the methods developed to solve them can be found in a survey by Hwang and Ahuja {1992}. We choose to illustrate our discussion with a particular case. A robot arm is placed among a set of obstacles. Given an initial and a final position of the robot arm, the problem is to find a set of motions that will lead the robot to move between the two positionswithout colliding with the obstacles. To drive the robot amidst the obstacles, early methods {Brooks, 1983} directlyused the 3d cad models of the robot and of the obstacles to find a solution, i.e., they considered the \operational 3d space". In this space, the path planning problem consists of finding the movements of a complex 3d structure {the robot} in a cluttered 3d space. A ma jor advance was to express the problem in another space known as the configuration space, denoted by C {Lozano-Perez, 1987}. In this space, theposition {or configuration} of a robot is completely determined by a single point having n independent parameters as coordinates. The positions that are not physically legal {because of a collision} are represented by particular regions of C , andare called C -obstacles. In the configuration space, the path planning problem consists of finding a continuous curve {representing a path for a single geometrical point} that {i} connects the points representing the initial and the final configuration of the robot, and {ii} does not intersect any C -obstacles. This method trades a simplificationof the path planning problem {it searches a path for a single point} against a higher-dimensionalsearch space {the dimensionof C is the number dof of the robot}and against more complex shapes of obstacles {very simple physical obstacles may result in very complex C -obstacles}. For example, let us consider the planar arm of Figure 1. Its position among the obstacles is totally known once the values of the angles between its links {q 0 ; q 1 } are known. Thus, for each pair {q 0 ; q 1 }, it is possible to decide whether the robot collides with the surrounding obstacles. This is what we did in Figure 2 to represent the mapping between the physical obstacles in the operational space and the C -obstacles. Now, by moving a point along the curve joining ^q a and ^q f one will also define a collision-free motion for the planar arm between the corresponding positions P { ^q a } and P { ^q f } in the operational space. This curve is one solution to this particular path planning problem. A recent trend in the field is to consider the \tra jectory space" {Ferbach, 1996} where a whole path is represented by a single point. The coordinates of this point are the values of the parameters defining the successive movements of the robot. For instance, the list of successivecommands sent to the robot controller indeed encode an entire path of the robot. In this space, the path planning problem is reduced to the search for a single point. Once again, we trade a simplificationof the path planning problem {searching for a point} against a higher dimension of the search space {the dimension of the tra jectory space is the number of parameters needed to specify completely a whole path}. For example, in Figure 2, the path between ^q b and ^q d can be represented by a point in a seven-dimensional space simply by considering the length of its seven segments. 297Mazer, Ahuactzin, & Bessi 022 ere B0B1B2B3AaAbAcx0x1Emmanuel Mazer26,75.25)(11.25,0)(0,6.75186.25,39.25)(10.5,0)(0,6.75131.25,197.75)(11.25,0)(0,6.7511.5,209.5)(12,0)(0,6.519.75,179.75)(11,0)(0,6.7546.75,109.5)(11.5,0)(0,6.7582.75,87)(8.75,0)(0,6.75109.75,150)(9.25,0)(0,6.75163.75,159)(11.25,0)(0,6.75B2B1B0AbB3Aax1x0AcPSfrag replacementsB 0 B 1 B 2 B 3 P { ^q a } P { ^q b }P { ^q f } q 0 q 1 Figure 1: A two dof arm placed among obstacles in the operational space 2.1 Global Approaches Global approaches are classically divided into two main classes: {i} retraction methods, and {ii} decomposition methods. In the retraction methods, one tries to reduce the dimension of the initial problem by recursively considering sub-manifolds of the configuration space. In the decomposition methods, one tries to characterize the regions of the configuration space that are free of obstacles. Both methods end up with a classical graph search over a discrete space. In principle, these methods are complete because they will find a path if one exists and will still terminate in a finite time if a path does not exist. Unfortunately, computing the retraction or the decomposition graph is an np-complete problem: the complexity of this task grows exponentially as the number of dof increases {Canny, 1988}. Consequently, these planners are used only for robots having a limitednumber {three or four} of dof. In addition, they are slow and can only be used off-line: the planner is invoked with a model ofthe environment, it produces a plan that is passed to the robot controller which, in turn, executes it. In general, the time necessary to achieve this is not short enough to allow the robot to move in a dynamic environment. 2.2 Path Planning with Local Planners One way to combat the complexity of the problem is to trade completeness against perfor- mance. To do this, the local planners are guided by the gradient of a cost function {usually the Euclidean distance to the goal} and take into account the constraints introduced by the obstacles to avoid them {Faverjon & Tournassoud, 1987}. Since the path planning problem is np-complete, knowing the cost function, it is always possible to design a deceptive envi- ronment where the method will get trappedin a local minimum. However, these methods are useful in many industrialapplications because they can deal with complex robots and 298The Ariadne's clew algorithmC0C1qaqbqcqdCB0CB2CB3x0x12pioEmmanuel Mazer86.25,209)(11.25,0)(0,6.75234.75,130.5)(9.5,0)(0,8.7527.75,2)(8.75,0)(0,6.75338.25,362)(9.25,0)(0,6.750.25,76.5)(13.25,0)(0,8.75306.25,378)(13.25,0)(0,8.759.75,375.5)(4,0)(0,4.2577.25,11)(18.5,0)(0,6.75149.25,11)(18.5,0)(0,6.75221.25,11)(18,0)(0,6.7572.75,306)(9,0)(0,6.550.25,216)(8.75,0)(0,6.5207.75,344)(10.5,0)(0,6.75225.75,297)(9.75,0)(0,8.75C0qbx1x02pi2pioCB0CB2CB3qaqcC1qdPSfrag replacementsC f ree ^q a C f ree ^q b ^q a^ q b ^ q f^ q d C B 0 C B 2 C B 3 q 0 q 1 2031 o Figure 2: The configuration space corresponding to Figure 1. Note: {1} C is a torus, {2} it is divided into two regions C f ree ^q a and C f ree ^q b that cannot be connected by a continuous path, and {3} there is not a C -obstacle for B 1 because it does not interfere with the arm. environment models having thousands of faces, that are often too time-consuming for global methods. 2.3 Path Planning with Randomized Techniques and Landmarks The stochastic or random approach was first introduced by Barraquand and Latombe {1990}, and later used by Overmars {1992}, and more recently by Kavraki {1996}. The main idea behind these algorithms is to builda graph in the configuration space. The graph is ob- tained incrementally as follows: a local planner is used to try to reach the goal. Should the motion stop at a local minimum, a new node {or landmark} is created by generating a random motion starting from that local minimum. The method iterates these two steps until the goal configuration has been reached from one of these intermediary positions by a gradient descent motion. These algorithms work with a discretized representation of the configuration space. They are known to be prob abilistic al ly complete because the probability of terminating with a solution {a path has been found or no path exists} converges to one as the allowed time increase towards infinity. As in the previous section, it is also possible to design simple deceptive environments that will make this kind of algorithm slower than a pure random approach. However, they have been tested for robots with a high num- ber of dof and they have been shown to work quickly in relatively complex and natural environments. Other methods using landmarks have been devised. For example, sandros, introduced by Chen and Hwang{1992}, makes use of landmarks to approximate the free space. This approach is similar to the \hierarchical planning" approach used in ai: should the method 299Mazer, Ahuactzin, & Bessi 022 ere fail to reach a goal, newsubgoals are generated until the problem is easy enough to be solved. In their approach, first a local planner is used to reach the final position: should the local planner fail, the configuration space is divided into two subspaces, one containing the goal and the other a new sub-goal. The problem is therefore divided into two sub-problems: {i} going from the initial position to the subgoal, and {ii} going from the subgoal to the final position. sandros has been shown to be particularly well adapted to find paths for manipulators. It has been implemented and tested for planning paths for Puma and Adept robots. 2.4 Path Planning in the Tra jectory Space The previous methods were essentially based on the configuration space: the retraction, the decomposition, or the optimization is made in this space. An alternative is to consider the \tra jectory space". For example, in his method vdp, Ferbach {1996} starts by considering the straight line segment joining the initial and the final configuration in C . This path is progressively modified in such a manner that the forbidden regions it crosses are reduced. At each iteration, a sub-manifold of C containing the current path is randomly generated. It is then discretized and explored using a dynamic programming method that uses the lengthacross the forbiddenregion as the cost function in order to minimize. The search results in a new tra jectory whose intersection with the forbidden regions is smaller than the original tra jectory. The process is repeated until an admissibletra jectory is found. As in the previous sections, it is also possible to design simple deceptive environments that will makethis kind of algorithm slower than a pure random approach. The work of Lin, Xiao, and Michalewicz {1994} is similar to our approach. As in an early version of our algorithm {Ahuactzin, Mazer, Bessi022ere, & Talbi, 1992}, genetic algorithms are used to carry out optimization in the tra jectory space. Tra jectories are parameterized using the coordinates of intermediary via-points. An evolutionary algorithm is used to optimize a cost function based on the length of the tra jectory and the forbidden region crossed. The standard operators of the genetic algorithms have been modified and later extended to produce a large variety of paths {Xiao, Michalewicz, & Zhang, 1996}. The number of intermediary via-points is fixed and chosen using an heuristic. Given this number, nothing prevents to design a deceptive problem which solution will require more intermediary points, leading the algorithm to fail while one solution exists. 3. Principle of the Ariadne's Clew Algorithm As we have seen in the previous section, the computation of the configuration space C is a very time-consuming task. The main idea behind the Ariadne's clew algorithm is to avoid thiscomputation. In order to do this, the algorithm searches directly for a feasible path in the tra jectory space. The configuration space C is never explicitly computed. As will be shown, in the tra jectory space, path planning may be seen as an optimization problem and solved as such by an algorithm called search. It is possible to build an approximation of free space by another algorithm called explore that is also posed as an optimization problem. The Ariadne's clew algorithm is the result of the interleaved execution of search and explore. 300The Ariadne's clew algorithmt1t2t3l1l2l3QIQFxyqlEmmanuel Mazer198,425)(6.75,0)(0,6.5114.25,350.25)(7.5,0)(0,6.7567.5,369)(7,0)(0,6.7550.75,315)(6.75,0)(0,6.75106.5,375.5)(7,0)(0,6.5155,419.5)(6.5,0)(0,6.75185.75,466.25)(10.25,0)(0,8.5355.75,73)(12.75,0)(0,8.519.25,278)(7.25,0)(0,8.7510.25,12.5)(4.25,0)(0,6.5491.75,482.5)(4,0)(0,4.2564.25,469)(35.5,0)(0,6.75t1t2t3l3l2l1QIQFqlyxROBOTPSfrag replacements022 1 022 2 022 3 d 1 d 2 d 3 ^q ffi ^ q * = {x f ; y f } x y ^q ` Figure 3: A parameterized tra jectory {022 1 ; d 1 ; 022 2 ; d 2 ; :::022 l ; d l } and a starting point ^q 0 implic- itly define a path {in the operational space} for a holonomic mobile robot. 3.1 Path Planning as an Optimization Problem: search Given a robot with k dof, a tra jectory of length l may be parameterized as a sequence of n = k 003 l successive movements. A starting point ^q ffi along with such a parameterized tra jectory implicitly define a path and a final configuration ^q l in the configuration space. For example, for a holonomic mobile robot the tra jectory {022 1 ; d 1 ; 022 2 ; d 2 ; :::022 l ; d l } can be interpreted as making a 022 1 degree turn, moving straight d 1 , making a 022 2 degree turn and so on. Given the starting configuration ^q ffi , this tra jectory leads to the final configuration ^q l {see Figure 3}. Given a distance function d on the configuration space, if we find a tra jectory such that it does not collide with any obstacles and such that the distance between ^q l and the goal ^q * is zero, then we have a solution to our path planning problem. Therefore, the path planning problem may be seen as a minimization problem where: 1. The search space is a space of suitably parameterized tra jectories, the tra jectory space. 2. The function to minimize is d{ ^q l ; ^q * } if the path is collision-free, and d{ ^q i ; ^q * } otherwise { ^q i being the first collision point}. 22. Another possible choice would be to give the +1 value to the distance when a collision occurs. However, this is less informative than the chosen function because the first part of a colliding path could be a good start toward the goal and should not be discarded. Note that the cost function does not include any optimality criteria such as the length of the trajectory or the amount of energy used. 301Mazer, Ahuactzin, & Bessi 022 ere The algorithm search, based on this very simple technique and a randomized optimiza- tion method, is already able to solve quite complex problems of robot motion planning. For example, Figure 4 represents the two paths found for the holonomic mobile robot. Each path was computed on a standard workstation {sparc 5} in less than 0.5 second without using any pre-computation of the configuration space. Thus, it is possible, albeit slowly, to get a planner that can be used in a dynamic environment {where the obstacles may move} by \dropping" a new world into the system every 0.5 second. search is very efficient but it is not complete, since it may fail to find a path even if one exists for two different reasons: 1. Due to the optimization-based formulation, search can get trapped by local minima of the ob jective function, which in turn may place the robot far away from the goal {see Figure 5}. 2. The length l of the tra jectories considered may be too short to reach all the accessible regions of the configuration space.J. Manuel AHUACTZINxwdumpiscale=1 means 1/300 inch per pixel width/8J. Manuel AHUACTZINxwdumpiscale=1 means 1/300 inch per pixel width/8Figure 4: Reactive replanningin a changing environment 3.2 Exploring as an Optimization Problem: explore In order to build a complete planner, we propose a second algorithm called explore. While the purpose of search was to look directly for a path from ^q ffi to ^q * , the purpose of explore is to compute an approximation of the region of the configuration space accessible from ^q ffi . The explore algorithm builds an approximation of the accessible space by placing landmarks in the configuration space C in such a way that a path from the initialposition ^q ffi to any landmark is known. In order to learn as much as possible about the free space, the explore algorithm tries to spread the landmarks uniformly over the space {see Figure 6}. To do this, it tries to put the landmarks as far as possible from one another by maximizing the distances between them. Therefore, explore may be seen as a maximization problem where: 302The Ariadne's clew algorithmA B 317,590.25)(5.75,0)(0,7.5436.75,589.5)(6.25,0)(0,7.5ABPSfrag replacements^q ffi ^ q * Figure 5: A problem leading to a local minimum. In such a case, a solution path has first to move away from the goal. The goal's \attraction" based on the minimization of the Euclidean distance prevents search from finding such a path.Figure 6: The first picture represents the initial position and the first landmark. The sub- sequent landmarks are then uniformly spread over the search space while the method keeps track of all paths joining the landmarks to the initial position. The algorithm is named after Ariadne because by placing landmarks, explore unwinds as if it were using a thread as Theseus did. 1. The search space is the set of all paths starting from one of the previouslyplaced landmarks. 2. The function to maximize is d{ ^q l ; 003}, where 003 is the set of landmarks already placed. 3.3 The Ariadne's Clew Algorithm: explore + search In order to have a planner that is both complete and efficient, we combined the two previous algorithms search and explore to obtain the Ariadne's clew algorithm. The principle of the Ariadne's clew algorithm is very simple: 1. Use the search algorithm to find whether a \simple" path exists between ^q ffi and ^q * . 303Mazer, Ahuactzin, & Bessi 022 ere abcdFigure 7: Bouncing against C -obstacles. Figure {a} presents the original path in the con- figuration space. Figure {b} shows the same path after two bounces along the second segment on obstacle 2 and on obstacle 1. Figure {c} is the result obtained after a bounce of segment 3 against obstacle 2. Finally, Figure {d} presents a valid path obtained after a final bounce of segment 4 against obstacle 2. 2. If no \simple" path is found by step 1, then continue until a path is found. {a} Use explore to generate a new landmark. {b} Use search to look for a \simple" path from that landmark to ^q * . The Ariadne's clew algorithm will find a path if one exists. In an overwhelming number of cases, just a few landmarks are necessary for the Ariadne's clew algorithm to reach the target and stop. 3.4 A Ma jor Improvement: Bouncing on C -Obstacles A typical difficulty for a path planning algorithm is to find a collision-free path through a small corridor in the configuration space. This is also the case for the basic version of the Ariadne's clew algorithm, presented above. The problem is that very few tra jectories encode such paths and therefore they are very difficult to find. Most tra jectories collide with the obstacles. We propose a very simple idea to deal with this problem: going backwards at each collision point. If, for a given tra jectory, a collision is detected along the corresponding path, then we simplyconsider transforming that tra jectory so that it encodes a new path, one that is found by bouncing off the obstacle at the collision point {see Figure 7}. Note that this construction is applied recursively until the entire tra jectory corresponds to a collision-free path. 304The Ariadne's clew algorithm Using this technique, all tra jectories are so transformed that they encode valid paths. This improved version of the Ariadne's clew algorithm no longer cares about obstacles. From the point of view of a search in the tra jectory space, it is as if the obstacles have simply vanished. This method is especially efficient for narrow corridors in the configuration space. Without bouncing, the mapping of a corridor in the configuration space to the tra jectory space is a set of very few points. With bouncing, every single tra jectory going through a part of the corridor is \folded" into the corridor {see Figure 7}. The resultant mapping of the corridor in the tra jectory space is consequently a much larger set of points, and therefore it is much easier to find a member of this set. This empirical improvement has a ma jor practical impact because it makes the proposed algorithm faster {fifteen times} in the problem considered below. 3.5 The Algorithm We can now give a final version of the Ariadne's clew algorithm. It has three inputs: ^q ffi {the initial position}, ^q * {the goal position}, and 032 {the maximum allowed distance for a path to the C -obstacles}. It returns a legal path or terminates if no path exists at the given resolution.ALGORI T H MARI ADN E { ^q ffi ; ^q * ; 032} begin i := 1; ^ 025 1 := ^q ffi ; /* Initialize the set of landmarks with the initial position 003 1 := f ^ 025 1 g; " 1 = +1; do while {" i > 032}; /* run search : look for the goal with a local method if {min l2IR ` d{ ^q * ; ^q{l}) == 0} return; /* A path has been found ! else /* run explore : place a new landmark i := i + 1; ^ 025 i := ^q : sup l2IR ` d{003 i0001 ; ^q{l}); 003 i := 003 i0001 [ f ^ 025 i g; " i := d{003 i0001 ; ^ 025 i }; endif enddo 003 = 003 i ; " = " i ; return{"}; /* No path ! endFigure 8: The Ariadne's Clew Algorithm 305Mazer, Ahuactzin, & Bessi 022 ere The algorithm is based on the following optimization problems: E X P LORE : { sup d{003 i0001 ; ^q{l}) l 2 IR ` S E ARC H : { min d{ ^q{l}; ^q * } l 2 IR ` ^q{l} denotes the extremity of a legal path parameterized with ` real parameters and starting either from each of the previously placed landmarks {explore} or from the latest placed landmark {search}. The algorithm is resolution-complete under the following assumptions: * \Space filling completeness": The global maximum distance can be found by the optimization algorithm used in explore; the configuration space is a compact set. * \032 completeness": The optimization procedure used in search always find a complete path {or returns 0} when the starting and the goal positions are located within a ball of radius 032 of the free space. In practice, the first condition cannot be met with a randomized optimization algorithm in a boundedtime, andonly local maxima are found. However, the landmarks placed according to the new algorithm are better distributedover the free space than landmarks placedrandomly, leading to better performances. The goal of the next section is to justify this claim, experimentally. 4. Path Planning for a Six dof Arm in a Dynamic Environment In order to demonstrate the feasibility and qualities of the Ariadne's clew algorithm, we have developed a realistic application of the algorithm. We selected a problem where we want to have a path planner for a six dof robot arm in a dynamic environment where another arm is used as a mobile obstacle. The robot {robot A} is under the control of the Ariadne's clew algorithm. It shares its workspace with a second robot {robot B} that is moving under the control of a random motion generator. The Ariadne's clew algorithm must be able to compute paths for A in \real time" {here, real time means fast enough to ensure that robot A will never collide with robot B}. In order to reach such a level of performance, we chose to implement the Ariadne's clew algorithm on a massively parallel machine {Meganode with128 T800 Transputers}. F urthermore, we selected a genetic algorithm as our optimization technique. The reasons forthis choice are: 1. Genetic algorithms are well suited for problems where the search space is huge but where there are many acceptable solutions. This is exactly the case here. The tra- jectory space is huge but there are, barring exceptional cases, numerousacceptable paths going from ^q ffi to ^q * without collision. 306The Ariadne's clew algorithm 2. Genetic algorithms, unlike a number of the other optimization techniques {Bessi022ere, Talbi, Ahuactzin, & Mazer, 1996}, are very easy to implement on parallel architec- tures. We have previously developed a parallel genetic algorithm {pga} and we have already had significant experience using it {Talbi, 1993}. 3. pga, unlike most parallel programs, shows linear speed-up {when you double the number of processors you reduce the computation time by half } and even super-linear speed-up under certain circumstances {Talbi & Bessi022ere, 1996}. 4.1 Parallel Genetic Algorithm Genetic algorithms are stochastic optimization techniques introduced by Holland {1975} twenty years ago. They are used in a large variety of domains including robotics {Ahuactzin et al., 1992; Lawrence, 1991; Falkenauer & Bouffouix, 1991; Falkenauer & Delchambre, 1992; Meygret & Levine, 1992} because they are easy to implement and do not require algebraic expression for the function to be optimized. 4.1.1 Principle of Genetic Algorithm The goal of the algorithm is to find a point reaching a \good" value of a given function F overa search space S . First, a quantization step is defined for S and the search is conducted over a discrete subset, S d of S . S d contains 2 N elements. In practice, the cardinality of S d can be extremely large. For example, in our implementation of explore, N = 116. Thus, a continuous domain is discretized with a given resolution. During an initialization phase a small subset of S d is drawn at random. This subset is called a population. Each element of this population is coded by a string of N bits. The genetic algorithm iterates the following four steps until a solution is found. 1. Evaluation: Rank the population according to the value of F for each element of S d . Decide if the best element can serve as an acceptable solution; if yes, exit. 2. Selection: Use the function F to define a probability distribution over the population. Select a pair of elements randomly according to this probability distribution. 3. Reproduction:Produce a new element from each pair using \genetic" operators. 4. Replacement: Replace the elements of the starting population by better new ele- ments produced in step 3. Many genetic operators {Davidor, 1989} are available. However, themore commonly used are the mutation and the cross-over operators. The mutation operator consists of randomly flipping some bits of an element of the population. The cross-over operator consists of first randomly choosing a place where to cut the two strings of bits, and then building two new elements from this pair by simply gluing the right and the left parts of theinitialpair of strings {see Figure 9}. We use both operators to produce new elements. First, we use the cross-over operator to get an intermediate string. Then, the mutation operator is used on this intermediate string to get the final string. 307Mazer, Ahuactzin, & Bessi 022 ere kPARENTSCross-overNEW ELEMENTSFigure 9: The cross-over operation. 4.1.2 Principle of the Parallel Genetic Algorithm {pga} There are many parallel versions of genetic algorithms: the standard parallel version {Robert- son, 1987}, the decomposition version {Tanese, 1987} and the massively parallel version {Talbi, 1993}. We chose this last method. The main idea is to allocate one element of the popula- tion for each processor so that steps 1, 3, and 4 can be executed in parallel. Furthermore, the selection step {step 2} is carried out locally, in that each individual may mate only with the individuals placed on processors physically connected to it. This ensures that the communication overhead does not increase as the number of processors increases. This is the reason why pga shows linear speed-up. The parallel genetic algorithm iterates the following four steps until a solution is found. 1. Evaluation: Evaluate in par al lel allthe individuals. 2. Selection: Select in par al lel, amongthe neighbors, the mate with the best evaluation. 3. Reproduction:Reproduce in par al lel withthe chosen mate. 4. Replacement: Replace in par al lel the parents by the offspring. On the Meganode, we implemented the pga on a torus of processors where each indi- vidual has four neighbors {see Figure 10} 4.2 Parallel Evaluation of the Cost Function The evaluation functions used in search and explore are very similar: they both compute the final position of the arm given a Manhattan path of a fixed order. In our implementation, based on experience, we chose to use Manhattan paths of order 2. Order 2 appeared tobe a good compromise between the number of landmarks needed {increases as order decreases}and the computing time necessary for the optimization functions {increases as order increases}. Since our robot has six dof, the argument of the cost function in search is a vector inR 12 : {001 1 1 ; 001 1 2 ; :::; 001 1 6 ; : : : ; 001 2 1 ; : : : ; 001 2 6 } and the argument of the cost function used for explore is a vector in IN 002 IR 12 : {i; 001 1 1 ; 001 1 2 ; :::; 001 1 6 ; : : : ; 001 2 1 ; : : : ; 001 2 6 } where i codes the landmark used as a starting point for the path. In both cases the functions are defined only on a bounded subset of IR 12 and IN 002 IR 12 , whose limits are fixed by the mechanical stops of the robot and the maximum number of landmarks. A discretization step is chosen for these two subsets by defining the resolution at which each elementary motion is discretized. 308The Ariadne's clew algorithmINDIVIDUALS "HOST" "ROOT"Figure 10: A torus with sixteen processors. One individualis placed on each processor. Each individual has four neighbors. In our case, each 001 j i is discretized with 9 bits and the number of landmarks is limited to 256. Thus, given a binary string of 116 = 8 + 12 002 9 bits, we can convert it into a vector {as an argument} for the cost function of search, or explore, respectively. Manhattan paths are evaluated in a simplifiedmodel of the environment. This model is obtained by enclosing each element of the scene into a bounding rectangular box. The evaluation of a vector is performed as follows: For each 001 j i in {001 1 1 ; 001 1 2 ; :::; 001 1 6 ; : : : ; 001 2 1 ; : : : ; 001 2 6 } Compute the limits on the motion for joint i. Compute 001 j i 0 by bouncing on these limits {see Section 3.4}. Update the position of the robot. The limits on the motion of joint i are obtained by merging the legal ranges of motion of all the links that move whenjoint i moves, and all the obstacles. To obtain a legal range of motion between a link and an obstacle, we consider the two enclosing parallelepipeds and express their coordinates in the joint frame. Then, we use a classical method to compute therange {Lozano-Perez, 1987}. Inour parallel implementation, we distributed the geometric computations among sev- eral processors. Each processor is dedicated to the computation of a single type of interac- tion. 4.3 Parallel Implementation of the Ariadne's Clew Algorithm Finally, the Ariadne's clew algorithm is implemented in parallel with three levels of paral- lelism. 1. Obviously, a first level of parallelization can be obtained by running search and explore at the same time on two sets of processors. While search is checking 309Mazer, Ahuactzin, & Bessi 022 ere whether a path exists between the last placed landmark and the goal, exploreis generating the next landmark. 2. The second level of parallelism corresponds to a parallelimplementation of both ge- netic algorithms employed by search and explore to treat their respective opti- mization problems. 3. The third level corresponds to a parallelization of the collision checking function and range computation. We completed a full implementation of these three levels on a Meganode with 128 t800 transputers. Figure 11 represents our parallel implementation of the Ariadne's clew algo- rithm and Figure 12 shows how we have embedded this architecture into our experimental setup. A cad system {act} is used to model the scene with the two robots. The robots areunder the control of kali {Hayward, Daneshmend, & Hayati, 1988}. First, a simplified geometric model of the scene is downloaded into the memory of the transputers. Then, a Silicon Graphics workstation works as a global controller and loops over the following steps: 1. Generate and execute a legal random motion for robot B. 2. Send the new configuration of robot B to the Meganode as well as the desired final configuration for robot A. 3. Get the planned path for robot A from the Meganode and execute it. 4. Wait for a random time and stop robot A. 5. Go to 1. This sequence allows us to test our algorithm extensively in real situations by having to deal with many different environments. Of course, the most interesting figure we can obtain from this experiment is the mean time necessary to compute one path given a new environment. For this experimental setup this mean time is 1.421 seconds. Using the same architecture with more up-to-date processors {t9000} wouldreduce this time by a factor of ten. The same computation on a single processor {sparc 5} would take three times longer than the current implementation. In summary, we have achieved our main goal by proving that it is indee d possible {with the Ariadne's clew algorithm} to plan col lision-fre e paths for a re al rob ot with many dof in a dynamic re alistic environment. 5. Conclusion: Contributions, Difficulties, and Perspectives As mentioned in the Introduction, the Ariadne's clew algorithm has two main qualities: efficiency, and generality. Let us, in conclusion, explain and discuss these two qualities. 310The Ariadne's clew algorithm12345HSEGAGPABPCpsabpscjuan-manuel ahuactzin (these mazer)330.5,1031.5)(2.5,0)(0,6.5149.25,761.5)(4.5,0)(0,6.5136.25,509.5)(3.5,0)(0,6.532.5,199)(4,0)(0,6.5383.5,194.5)(4,0)(0,6.5604,199)(3.75,0)(0,6.5500.75,509.5)(3.5,0)(0,6.5504.75,757)(4.5,0)(0,6.5328.25,977.5)(6.5,0)(0,6.5392,878.5)(5.5,0)(0,6.5267.25,879.25)(4.25,0)(0,6.75286.75,508.5)(14,0)(0,6.75140.25,446.5)(6.5,0)(0,6.5503,446.5)(6.25,0)(0,6.584.25,334.75)(19.5,0)(0,6.75192.5,330.25)(11.75,0)(0,6.75446.75,332)(19.75,0)(0,6.75556,331.25)(11.75,0)(0,6.75108.75,145.5)(18.75,0)(0,8.75130.25,28.5)(13.5,0)(0,6.5481.25,151)(19,0)(0,8.75500.25,34)(13.5,0)(0,6.5249.5,197)(3.75,0)(0,6.7512344532HESGAGGPABPCPABPCpsabpscpsabpsc5PSfrag replacementsAriadne's clew algorithm pga Range computationtype A et B type C Master Search ExploreAGGammaPsiAB PsiCpsiab psicFigure 11: A parallel implementation of the Ariadne's clew algorithm 311Mazer, Ahuactzin, & Bessi 022 ere 68030Mega-Node128 TransputersSun 3EthernetSilicon Graphics{Unix}{Unix}Robot IIKALISun 4{Unix}ACT server)(Mega-Nodeserver)(VxWorksBus VME)()( {VxWorks} 68030Bus VME)()( {VxWorks}KALIRobot IGENETIC ALGORITHM)()( {SEARCH}GENETIC ALGORITHM)()( {EXPLORE} CAD SYSTEM Figure 12: The experimental setup 5.1 Performance Comparing the performance of this kind of algorithm is a very delicate sub ject. Performance may be a matter of computing time, efforts needed to program, or ease of application to different problems {see Section 5.2}. Evaluating the performance in terms of computing time is very difficult for one fundamental and three practical reasons: 312The Ariadne's clew algorithm 1. The fundamental reason is, once again, the np-completeness of the path planning problem. As deceptive cases may always be designed, the only performance results one may reasonably present are always specific. 2. The three practical reasons are: {a} Obviously, the first requirement for such a comparison is that different algorithms run on the same machines with the same available memory. This may seem simple but it is a main difficulty in our case because our algorithm has been designed to run on rather specific kinds of machines, namely, massively parallel ones. It could also be implemented on non-parallel machines, but then it may lose part of its interest. A fair comparison would be to compare the algorithms on both types of machines. This would imply programming other algorithms in parallel, which is very difficult in practice. {b} Many known path planning algorithms first compute the configuration space {or an approximation of it} off-line, and then efficiently solve the path planning problem on-line. As we saw, in order to deal with a dynamic environment, the Ariadne'sclew algorithm adopts a completely different approach. {c} For practical reasons, many test problems are toy problems{2d, few obstacles, few faces, simulatedrobots} and the performance results using these kinds of problems are very difficult to generalize to realistic industrial problems {3d, tens of obstacles, hundreds of faces, real robots}. Considering all these reasons, we tested our algorithm by implementing a realistic robotic application to the very end. To achieve this goal, we assembled a complex experimental setup includingsix different machines {1 meganode, 2 68030, 2 sun 4, and 1 silicon graphics}, two mechanical arms, and running seven different cooperative programs {2 kali, 1 act, 2 vxworks, 1 parx, and 1 Ariadne's clew algorithm}. Our challenge was to be able to solve the path planning problem fast enough to drive a real six dof arm in a dynamic environment. The Ariadne's clew algorithm indeed achieved this goal in our experiments where the environment is composed of five fixed obstacles and a six dof arm moving independently. W e are not aware of any other methods capable of such performance. To the best of our knowledge, currently implemented planners would take a number of seconds {ten} to place a set of landmarks on a 2d example for a robot with five dof {Kavraki et al., 1996}. Despite the fact that finding a general purpose planning technique for real industrial application is a very difficultproblem, we believe that the Ariadne's clew algorithm provides an effective approach to such problems. The number of range computations for a Manhattan motion of order 1 is C k 2 +k2 003 n where n is the number of faces, k the number of dof, and C a constant factor, depending on the number of parts used to model the robot. Obviously, such a number of faces may be a severe difficulty for the implementation of the Ariadne's clew algorithm described so far. To speed up the computation we use a number of geometric filters that reduce the number of pairs of entities to be analyzed. However, it was possible to follow two research tracks in combination. First, we could use collision checking methods that allow access to the pairs in collision in a logarithmic 313Mazer, Ahuactzin, & Bessi 022 ere time {Faverjon & Tournassoud, 1987}. Second, we could preserve part of the landmark graph when the environment is changing {McLean & Mazon, 1996}. 5.2 Generality The Ariadne's clew algorithm is general in the sense that it may be used for numerous and very different applications in robotics. Basically, the main thing that needs to be changed in the algorithm is the distance d used in the evaluation functions of the two optimization problems. Several planners have been implemented in this way: a fine motion planner {De la Rosa, Laugier, & Na jera, 1996}, two motion planners for holonomic and non-holonomic mobile robots {Scheuer & Fraichard, 1997}, a reorientation planner for an articulated hand{Gupta, 1995}, aplanner for grasping and regrasping {Ahuactzin, Gupta, & Mazer, 1998}, and a planner for a robotic arm placed in the steam generator of a nuclear plant {McLean & Mazon, 1996}. Adapting the algorithm to a new application is, therefore, clearlya very easy task. For instance, the application to path planning for the non-holonomic trailer was developed in three days. The Ariadne's clew algorithm is also general in the sense that it may be used for any kind of path planning problem in a continuous space, in fields other than robotics. Although it may be sufficient to change the distance function d, one may also consider changing the form of the function d, or even the nature of the searched spaces. For instance, the concept of obstacles may be reconsidered. Instead of \hard" obstacles, one could replace them by zones of constraints. In that case, the path planning problem does not consist of finding a path without collisions but rather finding a path best satisfying the different constraints. Such a planner has been developed for a naval application where the problem was to find a path for a boat with various constraints on the tra jectory. This opens numerous perspectives of applications for applying the Ariadne's clew algorithm in a broader field than pure robotics. Acknowledgments The authors are greatly indebted to Dr. Kamal Gupta from Simon Fraser University who carefully read the paper and suggested valuable corrections that greatly improve the quality of the final paper. This work has been made possible by: Le Centre National de la Recherche Scientifique {France}, Consejo Nacional de Ciencia y Tecnologia {Mexico} and esprit 2, P2528 {eec}. References Ahuactzin, J., Gupta, K., &Mazer, E. {1998}. Manipulation Planning for Redundant Robots: A Practical Approach. The International Journal of Rob otics Rese ar ch, 17 {7}, 731{747. Ahuactzin, J., Mazer, E., Bessi022ere, P., & Talbi, E. {1992}. Using Genetic Algorithms for Robot Motion Planning. In Pro c e e dings of the 1992 Europ e an Conferenc e on Artificial Intel ligence, pp. 671{675. 314The Ariadne's clew algorithm Barraquand, J., & Latombe, J. {1990}. A Monte Carlo Algorithm for Path Planning with Many Degrees of Freedom. In Pro c e e dings of the 1990 IEEE International Conferenc e on Rob otics and Automation, pp. 1712{1717. Bessi022ere, P., Talbi, E., Ahuactzin, J., &Mazer, E.{1996}. Un Algorithme Gen etique Parall022 elle pour l'Optimisation. Te chnique et Science Informatique, 15 {8}, 1105{1130. Brooks, R. {1983}. Solving the Find-Path Problem by Good Representation of the Free Space. IEEE Tr ansactions on System, Man and Cybernetics, 13 {4}, 190{197. Canny, J. {1988}. The Complexity of Rob ot Motion Planning. MIT Press. Davidor, Y. {1989}. Analogous Crossover. In Pro c e e dings of the Third International Con- ferenc e on Genetic Algorithms, pp. 98{103. De la Rosa, F., Laugier, C., & Na jera, J. {1996}. Robust Path Planning in the Plane. IEEE Tr ansactions on Rob otics and Automation, 12 {3}, 347{352. Falkenauer, E., & Bouffouix, S. {1991}. A Genetic Algorithm for Job Shop. In Pro c e e dings of the 1991 IEEE International Conferenc e on Rob otics and Automation, pp. 824{829. Falkenauer, E., & Delchambre, A. {1992}. A Genetic Algorithm for Bin Packing and Line Balancing. In Pro c e e dings of the 1992 IEEE International Conferenc e on Rob otics and Automation, pp. 1186{1192. Faverjon, B.,& Tournassoud, P. {1987}. A Local Based Approach for Path Planning of Manipulators with a High Number of Degrees of Freedom. In Pro c e e dings of the 1987 IEEE International Conferenc e on Rob otics and Automation, pp. 1152{1159. Ferbach, P. {1996}. Contribution 022a la Planification de Tra jectoires. Rapport de Recherche EDF-DER HP-28/96/026, Direction des Etudes et Recherches EDF. Gupta, K. {1995}. Motion Planning for Re-Orientation Using Finger Tracking: Landmarks in S O{3} 002 !. In Pro c e e dings of the 1995 IEEE International Conferenc e on Rob otics and Automation, pp. 446{451. Hayward, V., Daneshmend, L., & Hayati, S.{1988}. An Overview of KALI: A System to Program and Control Cooperative Manipulators. In Pro c e e dings of the Fourth International Conferenc e on Advanc e d Rob otics, pp. 236{240. Holland, J. {1975}. Adaptation in Natural and Artificial Systems. University of Michigan Press. Hwang, Y., & Ahuja, N. {1992}. Gross Motion Planning: A Survey. ACM Computing Surveys, 24 {3}. Kavraki, L., Svestka, P., Latombe, J., & Overmars, M. {1996}. Probabilistic Roadmaps for Path Planningin High-Dimensional Configuration Spaces. IEEE Tr ansactions on Rob otics and Automation, 14 {4}, 566{580. Lawrence, D. {Ed.}. {1991}. Handbo ok of Genetic Algorithms. Van Nostrand Reinhold. 315Mazer, Ahuactzin, & Bessi 022 ere Lin, H., Xiao, J., & Michalewicz, Z. {1994}. Evolutionary Navigator for a Mobile Robot. In Pro c e e dings of the 1994 IEEE International Conferenc e on Rob otics and Automation, pp. 2199{2004. Lozano-Perez, T. {1987}. A Simple Motion-Planning Algorithm for General Robot Manip- ulators. IEEE Tr ansactions on Rob otics and Automation, 3 {3}, 224{238. McLean, A., & Mazon, T. {1996}. Incremental Roadmaps and Global Path Planning in Evolving Industrial Environments. In Pro c e e dings of the 1996 IEEE International Conferenc e on Rob otics and Automation, pp. 101{106. Meygret, A., & Levine, M. {1992}. Extraction de Primitives Geom etriques: Utilisation d'un Algorithme Gen etique. Rapport Annuel, Center for Intelligent Machines, McGill University, Montreal. Overmars, M. {1992}. A Random Approach to Motion Planning. Technical Report RUU- CS-92-32, Department of Computer Science, Utrecht University. Robertson, G. {1987}. Parallel Implementation of Genetic Algorithms in a Classifier System. In Davis, L. {Ed.}, Genetic Algorithms and Simulated Annealing. Morgan Kaufmann Publishers. Scheuer, A., & Fraichard, T. {1997}. Continuous-Curvature Path Planning for Car-Like Vehicles. In Pro c e e dings of the IEEE/RSJ International Conferenc e on Intel ligent Rob ots and Systems, pp. 997{1003. Talbi, E. {1993}. Al loc ation de Pro c essus sur les Archite ctur es Paral l022eles 022a Memoir Dis- tribuee. Ph.D. thesis, Institut National Polytechnique de Grenoble - France. T albi, E., & Bessi022ere, P. {1996}. A Parallel Genetic Algorithm Applied to the Mapping Problem. In Astfalk, G. {Ed.}, Applications on Advanc e d Archite ctur e Computers. SIAM. Tanese, R. {1987}. Parallel Genetic Algorithm for a Hypercube. InPro c e e dings of the Sec ond International Conferenc e on Genetic Algorithms, pp. 177{183. Xiao, J., Michalewicz, Z., & Zhang, L. {1996}. Evolutionary Planner/Navigator: Operator Performance and Self-Tuning. In Pro c e e dings of 1996 IEEE International Conferenc e on Evolutionary Computation, pp. 336{371. 316