The Ariadnes Clew Algorithm

Journal of Articial In telligence Researc h 9 (1998) 295-316 Submitted 9/97; published 11/98 The Ariadne's Clew Algorithm Emman uel Mazer Emmanuel.Mazer@ima g.fr L ab or atoir e GRA VIR, INRIA 665 A venue de L'Eur op e 38330 Montb onnot, F r anc e Juan Man uel Ah uactzin jmal@mail.udlap.mx Depto. de Ing. en Sistemas Computationales Unversidad de las A meric as Puebla, Cholula, Puebla 72820, Mexic o Pierre Bessi  ere Pierre.Bessiere@ima g.fr L ab or atoir e LEIBNIZ, 46 A venue F elix Vial let 38000 Gr enoble, F r anc e Abstract W e presen t a new approac h to path planning, called the \Ariadne's clew algorithm". It is designed to nd paths in high-dimensional con tin uous spaces and applies to rob ots with man y degrees of freedom in static, as w ell as dynamic en vironmen ts | ones where obstacles ma y mo v e. The Ariadne's clew algorithm comprises t w o sub-algorithms, called sear ch and explore , applied in an in terlea v ed manner. explore builds a represen tation of the accessible space while sear ch lo oks for the target. Both are p osed as optimization problems. W e describ e a real implemen tation of the algorithm to plan paths for a six degrees of freedom arm in a dynamic en vironmen t where another six degrees of freedom arm is used as a mo ving obstacle. Exp erimen tal results sho w that a path is found in ab out one second without an y pre-pro cessing. 1. In tro duction The path planning problem is of ma jor in terest for industrial rob otics. A basic v ersion of this problem consists of nding a sequence of motions for a rob ot from a start conguration to a giv en goal conguration while a v oiding collisions with an y obstacles in the en vironmen t. A simple v ersion of the problem, that of planning the motion of a p oin t rob ot among 3-dimensional p olyhedral obstacles, has b een pro v ed to b e np -complete (Cann y , 1988). Generally sp eaking, the complexit y of the problem is exp onen tial in the n um b er of degrees of freedom ( dof ) of the rob ot, and p olynomial in the n um b er of obstacles in the en vironmen t. Consequen tly , nding a path for a rob ot with man y dof (more than v e) in an en vironmen t with sev eral obstacles is, indeed, a v ery dicult problem. Unfortunately , man y realistic industrial problems deal with rob ots of at least six dof and h undreds of obstacles. Ev en w orse, often the en vironmen t is dynamic in the sense that some of the obstacles ma y mo v e, thereb y further requiring that new paths b e found in v ery short computing times. c  1998 AI Access F oundation and Morgan Kaufmann Publishers. All righ ts reserv ed. Mazer, Ahua ctzin, & Bessi  ere In this pap er, w e presen t a new approac h to path planning, called the \Ariadne's clew algorithm 1 ". The approac h is completely general and applies to a broad range of path planning problems. Ho w ev er, it is particularly designed to nd paths for rob ots with man y dof in dynamic en vironmen ts. The ultimate goal of a planner is to nd a path from the initial p osition to the target. Ho w ev er, while searc hing for this path, the algorithm ma y consider collecting information ab out the free space and ab out the set of p ossible paths that lie in that free space. The Ariadne's clew algorithm tries to do b oth at the same time: a sub-algorithm called explore collects information ab out the free space with increasingly ne resolution, while, in parallel, an algorithm called sear ch opp ortunistically c hec ks whether the target can b e reac hed. The explore algorithm w orks b y placing landmarks in the searc hed space in suc h a w a y that a path from the initial p osition to an y landmark is kno wn. In order to learn as m uc h as p ossible ab out the free space, the explore algorithm tries to spread the landmarks uniformly all o v er the space. T o do this, it places the landmarks as far as p ossible from one another. F or eac h new landmark pro duced b y the explore algorithm, the sear ch algo- rithm c hec ks (with a lo cal metho d) whether the target can b e reac hed from that landmark. Both the explore and sear ch algorithms are p osed as optimization problems. The Ariadne's clew algorithm is ecient and gener al : 1. The algorithm is ecien t in t w o senses: (a) Exp erimen ts sho w that the algorithm is able to solv e path planning problems fast enough to mo v e a six dof arm in a realistic and dynamic en vironmen t where another six dof rob ot is used as a mo ving obstacle. (b) It is w ell suited for parallel implemen tation and sho ws signican t sp eed-up when the n um b er of pro cessors increases. 2. The algorithm is general in t w o senses: (a) It ma y b e used for a wide range of applications in rob otics with little additional eort to adapt it. (b) Finally , the algorithm is general in that it ma y b e adapted for a large range of searc h problems in con tin uous spaces that arise in elds that are not related to rob otics. The pap er is organized as follo ws. Section 2 presen ts the path planning problem and discusses related w ork. Section 3 presen ts the principle of the Ariadne's clew algorithm. Section 4 describ es the application of the algorithm to a six dof arm in a dynamic en vi- ronmen t. Finally , Section 5 concludes the pap er with a discussion of the con tributions of our approac h, the main diculties in v olv ed, and p ossible impro v emen ts of our metho d. 1. According to Greek legend, Ariadne, the daugh ter of Minos, King of Crete, help ed Theseus kill the Minotaur, a monster living in the Lab yrin th, a h uge maze built b y Daedalus. The main dicult y faced b y Theseus w as to nd his w a y through the Lab yrin th. Ariadne brillian tly solv ed the problem b y giving him a thread (or a clew) that he could un wind in order to nd his path bac k. 296 The Ariadne's clew algorithm 2. The P ath Planning Problem Man y v ersions of the path planning problem exist. An exhaustiv e classication of these problems and of the metho ds dev elop ed to solv e them can b e found in a surv ey b y Hw ang and Ah uja (1992). W e c ho ose to illustrate our discussion with a particular case. A rob ot arm is placed among a set of obstacles. Giv en an initial and a nal p osition of the rob ot arm, the problem is to nd a set of motions that will lead the rob ot to mo v e b et w een the t w o p ositions without colliding with the obstacles. T o driv e the rob ot amidst the obstacles, early metho ds (Bro oks, 1983) directly used the 3 d cad mo dels of the rob ot and of the obstacles to nd a solution, i.e., they considered the \op erational 3 d space". In this space, the path planning problem consists of nding the mo v emen ts of a complex 3 d structure (the rob ot) in a cluttered 3 d space. A ma jor adv ance w as to express the problem in another space kno wn as the conguration space, denoted b y C (Lozano-P  erez, 1987). In this space, the p osition (or conguration) of a rob ot is completely determined b y a single p oin t ha ving n indep enden t parameters as co ordinates. The p ositions that are not ph ysically legal (b ecause of a collision) are represen ted b y particular regions of C , and are called C -obstacles . In the conguration space, the path planning problem consists of nding a con tin uous curv e (represen ting a path for a single geometrical p oin t) that (i) connects the p oin ts represen ting the initial and the nal conguration of the rob ot, and (ii) do es not in tersect an y C -obstacles . This metho d trades a simplication of the path planning problem (it searc hes a path for a single p oin t) against a higher-dimensional searc h space (the dimension of C is the n um b er dof of the rob ot) and against more complex shap es of obstacles (v ery simple ph ysical obstacles ma y result in v ery complex C -obstacles ). F or example, let us consider the planar arm of Figure 1. Its p osition among the obstacles is totally kno wn once the v alues of the angles b et w een its links ( q 0 ; q 1 ) are kno wn. Th us, for eac h pair ( q 0 ; q 1 ), it is p ossible to decide whether the rob ot collides with the surrounding obstacles. This is what w e did in Figure 2 to represen t the mapping b et w een the ph ysical obstacles in the op erational space and the C -obstacles . No w, b y mo ving a p oin t along the curv e joining ^ q a and ^ q f one will also dene a collision-free motion for the planar arm b et w een the corresp onding p ositions P ( ^ q a ) and P ( ^ q f ) in the op erational space. This curv e is one solution to this particular path planning problem. A recen t trend in the eld is to consider the \tra jectory space" (F erbac h, 1996) where a whole path is represen ted b y a single p oin t. The co ordinates of this p oin t are the v alues of the parameters dening the successiv e mo v emen ts of the rob ot. F or instance, the list of successiv e commands sen t to the rob ot con troller indeed enco de an en tire path of the rob ot. In this space, the path planning problem is reduced to the searc h for a single p oin t. Once again, w e trade a simplication of the path planning problem (searc hing for a p oin t) against a higher dimension of the searc h space (the dimension of the tra jectory space is the n um b er of parameters needed to sp ecify completely a whole path). F or example, in Figure 2, the path b et w een ^ q b and ^ q d can b e represen ted b y a p oin t in a sev en-dimensional space simply b y considering the length of its sev en segmen ts. 297 Mazer, Ahua ctzin, & Bessi  ere PSfrag replacemen ts B 0 B 1 B 2 B 3 P ( ^ q a ) P ( ^ q b ) P ( ^ q f ) q 0 q 1 Figure 1: A t w o dof arm placed among obstacles in the op erational space 2.1 Global Approac hes Global approac hes are classically divided in to t w o main classes: (i) retraction metho ds, and (ii) decomp osition metho ds. In the retraction metho ds, one tries to reduce the dimension of the initial problem b y recursiv ely considering sub-manifolds of the conguration space. In the decomp osition metho ds, one tries to c haracterize the regions of the conguration space that are free of obstacles. Both metho ds end up with a classical graph searc h o v er a discrete space. In principle, these metho ds are c omplete b ecause they will nd a path if one exists and will still terminate in a nite time if a path do es not exist. Unfortunately , computing the retraction or the decomp osition graph is an np -complete problem: the complexit y of this task gro ws exp onen tially as the n um b er of dof increases (Cann y , 1988). Consequen tly , these planners are used only for rob ots ha ving a limited n um b er (three or four) of dof . In addition, they are slo w and can only b e used o-line: the planner is in v ok ed with a mo del of the en vironmen t, it pro duces a plan that is passed to the rob ot con troller whic h, in turn, executes it. In general, the time necessary to ac hiev e this is not short enough to allo w the rob ot to mo v e in a dynamic en vironmen t. 2.2 P ath Planning with Lo cal Planners One w a y to com bat the complexit y of the problem is to trade completeness against p erfor- mance. T o do this, the lo cal planners are guided b y the gradien t of a cost function (usually the Euclidean distance to the goal) and tak e in to accoun t the constrain ts in tro duced b y the obstacles to a v oid them (F a v erjon & T ournassoud, 1987). Since the path planning problem is np -complete, kno wing the cost function, it is alw a ys p ossible to design a deceptiv e en vi- ronmen t where the metho d will get trapp ed in a lo cal minim um. Ho w ev er, these metho ds are useful in man y industrial applications b ecause they can deal with complex rob ots and 298 The Ariadne's clew algorithm PSfrag replacemen ts C f r ee ^ q a C f r ee ^ q b ^ q a ^ q b ^ q f ^ q d C B 0 C B 2 C B 3 q 0 q 1 2  2  o Figure 2: The conguration space corresp onding to Figure 1. Note: (1) C is a torus, (2) it is divided in to t w o regions C f r ee ^ q a and C f r ee ^ q b that cannot b e connected b y a con tin uous path, and (3) there is not a C -obstacle for B 1 b ecause it do es not in terfere with the arm. en vironmen t mo dels ha ving thousands of faces, that are often to o time-consuming for global metho ds. 2.3 P ath Planning with Randomized T ec hniques and Landmarks The sto c hastic or random approac h w as rst in tro duced b y Barraquand and Latom b e (1990), and later used b y Ov ermars (1992), and more recen tly b y Ka vraki (1996). The main idea b ehind these algorithms is to build a graph in the conguration space. The graph is ob- tained incremen tally as follo ws: a lo cal planner is used to try to reac h the goal. Should the motion stop at a lo cal minim um, a new no de (or landmark) is created b y generating a random motion starting from that lo cal minim um. The metho d iterates these t w o steps un til the goal conguration has b een reac hed from one of these in termediary p ositions b y a gradien t descen t motion. These algorithms w ork with a discretized represen tation of the conguration space. They are kno wn to b e pr ob abilistic al ly c omplete b ecause the probabilit y of terminating with a solution (a path has b een found or no path exists) con v erges to one as the allo w ed time increase to w ards innit y . As in the previous section, it is also p ossible to design simple deceptiv e en vironmen ts that will mak e this kind of algorithm slo w er than a pure random approac h. Ho w ev er, they ha v e b een tested for rob ots with a high n um- b er of dof and they ha v e b een sho wn to w ork quic kly in relativ ely complex and natural en vironmen ts. Other metho ds using landmarks ha v e b een devised. F or example, sandr os , in tro duced b y Chen and Hw ang(1992), mak es use of landmarks to appro ximate the free space. This approac h is similar to the \hierarc hical planning" approac h used in ai : should the metho d 299 Mazer, Ahua ctzin, & Bessi  ere fail to reac h a goal, new subgoals are generated un til the problem is easy enough to b e solv ed. In their approac h, rst a lo cal planner is used to reac h the nal p osition: should the lo cal planner fail, the conguration space is divided in to t w o subspaces, one con taining the goal and the other a new sub-goal. The problem is therefore divided in to t w o sub-problems: (i) going from the initial p osition to the subgoal, and (ii) going from the subgoal to the nal p osition. sandr os has b een sho wn to b e particularly w ell adapted to nd paths for manipulators. It has b een implemen ted and tested for planning paths for Puma and Adept rob ots. 2.4 P ath Planning in the T ra jectory Space The previous metho ds w ere essen tially based on the conguration space: the retraction, the decomp osition, or the optimization is made in this space. An alternativ e is to consider the \tra jectory space". F or example, in his metho d vdp , F erbac h (1996) starts b y considering the straigh t line segmen t joining the initial and the nal conguration in C . This path is progressiv ely mo died in suc h a manner that the forbidden regions it crosses are reduced. A t eac h iteration, a sub-manifold of C con taining the curren t path is randomly generated. It is then discretized and explored using a dynamic programming metho d that uses the length across the forbidden region as the cost function in order to minimize. The searc h results in a new tra jectory whose in tersection with the forbidden regions is smaller than the original tra jectory . The pro cess is rep eated un til an admissible tra jectory is found. As in the previous sections, it is also p ossible to design simple deceptiv e en vironmen ts that will mak e this kind of algorithm slo w er than a pure random approac h. The w ork of Lin, Xiao, and Mic halewicz (1994) is similar to our approac h. As in an early v ersion of our algorithm (Ah uactzin, Mazer, Bessi  ere, & T albi, 1992), genetic algorithms are used to carry out optimization in the tra jectory space. T ra jectories are parameterized using the co ordinates of in termediary via-p oin ts. An ev olutionary algorithm is used to optimize a cost function based on the length of the tra jectory and the forbidden region crossed. The standard op erators of the genetic algorithms ha v e b een mo died and later extended to pro duce a large v ariet y of paths (Xiao, Mic halewicz, & Zhang, 1996). The n um b er of in termediary via-p oin ts is xed and c hosen using an heuristic. Giv en this n um b er, nothing prev en ts to design a deceptiv e problem whic h solution will require more in termediary p oin ts, leading the algorithm to fail while one solution exists. 3. Principle of the Ariadne's Clew Algorithm As w e ha v e seen in the previous section, the computation of the conguration space C is a v ery time-consuming task. The main idea b ehind the Ariadne's clew algorithm is to a v oid this computation. In order to do this, the algorithm searc hes directly for a feasible path in the tra jectory space. The conguration space C is nev er explicitly computed. As will b e sho wn, in the tra jectory space, path planning ma y b e seen as an optimization problem and solv ed as suc h b y an algorithm called sear ch . It is p ossible to build an appro ximation of free space b y another algorithm called explore that is also p osed as an optimization problem. The Ariadne's clew algorithm is the result of the in terlea v ed execution of sear ch and explore . 300 The Ariadne's clew algorithm ROBOT PSfrag replacemen ts  1  2  3 d 1 d 2 d 3 ^ q  ^ q  = ( x f ; y f ) x y ^ q ` Figure 3: A parameterized tra jectory (  1 ; d 1 ;  2 ; d 2 ; ::: l ; d l ) and a starting p oin t ^ q 0 implic- itly dene a path (in the op erational space) for a holonomic mobile rob ot. 3.1 P ath Planning as an Optimization Problem: sear ch Giv en a rob ot with k dof , a tra jectory of length l ma y b e parameterized as a sequence of n = k  l successiv e mo v emen ts. A starting p oin t ^ q  along with suc h a parameterized tra jectory implicitly dene a path and a nal conguration ^ q l in the conguration space. F or example, for a holonomic mobile rob ot the tra jectory (  1 ; d 1 ;  2 ; d 2 ; ::: l ; d l ) can b e in terpreted as making a  1 degree turn, mo ving straigh t d 1 , making a  2 degree turn and so on. Giv en the starting conguration ^ q  , this tra jectory leads to the nal conguration ^ q l (see Figure 3). Giv en a distance function d on the conguration space, if w e nd a tra jectory suc h that it do es not collide with an y obstacles and suc h that the distance b et w een ^ q l and the goal ^ q  is zero, then w e ha v e a solution to our path planning problem. Therefore, the path planning problem ma y b e seen as a minimization problem where: 1. The searc h 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 b eing the rst collision p oin t). 2 2. Another p ossible c hoice w ould b e to giv e the + 1 v alue to the distance when a collision o ccurs. Ho w ev er, this is less informativ e than the c hosen function b ecause the rst part of a colliding path could b e a go o d start to w ard the goal and should not b e discarded. Note that the cost function do es not include an y optimalit y criteria suc h as the length of the tra jectory or the amoun t of energy used. 301 Mazer, Ahua ctzin, & Bessi  ere The algorithm sear ch , based on this v ery simple tec hnique and a randomized optimiza- tion metho d, is already able to solv e quite complex problems of rob ot motion planning. F or example, Figure 4 represen ts the t w o paths found for the holonomic mobile rob ot. Eac h path w as computed on a standard w orkstation ( sp ar c 5 ) in less than 0.5 second without using an y pre-computation of the conguration space. Th us, it is p ossible, alb eit slo wly , to get a planner that can b e used in a dynamic en vironmen t (where the obstacles ma y mo v e) b y \dropping" a new w orld in to the system ev ery 0.5 second. sear ch is v ery ecien t but it is not complete, since it ma y fail to nd a path ev en if one exists for t w o dieren t reasons: 1. Due to the optimization-based form ulation, sear ch can get trapp ed b y lo cal minima of the ob jectiv e function, whic h in turn ma y place the rob ot far a w a y from the goal (see Figure 5). 2. The length l of the tra jectories considered ma y b e to o short to reac h all the accessible regions of the conguration space. Figure 4: Reactiv e replanning in a c hanging en vironmen t 3.2 Exploring as an Optimization Problem: explore In order to build a complete planner, w e prop ose a second algorithm called explore . While the purp ose of sear ch w as to lo ok directly for a path from ^ q  to ^ q  , the purp ose of explore is to compute an appro ximation of the region of the conguration space accessible from ^ q  . The explore algorithm builds an appro ximation of the accessible space b y placing landmarks in the conguration space C in suc h a w a y that a path from the initial p osition ^ q  to an y landmark is kno wn. In order to learn as m uc h as p ossible ab out the free space, the explore algorithm tries to spread the landmarks uniformly o v er the space (see Figure 6). T o do this, it tries to put the landmarks as far as p ossible from one another b y maximizing the distances b et w een them. Therefore, explore ma y b e seen as a maximization problem where: 302 The Ariadne's clew algorithm PSfrag replacemen ts ^ q  ^ q  Figure 5: A problem leading to a lo cal minim um. In suc h a case, a solution path has rst to mo v e a w a y from the goal. The goal's \attraction" based on the minimization of the Euclidean distance prev en ts sear ch from nding suc h a path. Figure 6: The rst picture represen ts the initial p osition and the rst landmark. The sub- sequen t landmarks are then uniformly spread o v er the searc h space while the metho d k eeps trac k of all paths joining the landmarks to the initial p osition. The algorithm is named after Ariadne b ecause b y placing landmarks, explore un winds as if it w ere using a thread as Theseus did. 1. The searc h space is the set of all paths starting from one of the previously placed landmarks. 2. The function to maximize is d ( ^ q l ; ), where  is the set of landmarks already placed. 3.3 The Ariadne's Clew Algorithm: explore + sear ch In order to ha v e a planner that is b oth complete and ecien t, w e com bined the t w o previous algorithms sear ch and explore to obtain the Ariadne's clew algorithm. The principle of the Ariadne's clew algorithm is v ery simple: 1. Use the sear ch algorithm to nd whether a \simple" path exists b et w een ^ q  and ^ q  . 303 Mazer, Ahua ctzin, & Bessi  ere a b c d Figure 7: Bouncing against C -obstacles. Figure (a) presen ts the original path in the con- guration space. Figure (b) sho ws the same path after t w o b ounces along the second segmen t on obstacle 2 and on obstacle 1. Figure (c) is the result obtained after a b ounce of segmen t 3 against obstacle 2. Finally , Figure (d) presen ts a v alid path obtained after a nal b ounce of segmen t 4 against obstacle 2. 2. If no \simple" path is found b y step 1, then con tin ue un til a path is found. (a) Use explore to generate a new landmark. (b) Use sear ch to lo ok for a \simple" path from that landmark to ^ q  . The Ariadne's clew algorithm will nd a path if one exists. In an o v erwhelming n um b er of cases, just a few landmarks are necessary for the Ariadne's clew algorithm to reac h the target and stop. 3.4 A Ma jor Impro v emen t: Bouncing on C -Obstacles A t ypical dicult y for a path planning algorithm is to nd a collision-free path through a small corridor in the conguration space. This is also the case for the basic v ersion of the Ariadne's clew algorithm, presen ted ab o v e. The problem is that v ery few tra jectories enco de suc h paths and therefore they are v ery dicult to nd. Most tra jectories collide with the obstacles. W e prop ose a v ery simple idea to deal with this problem: going bac kw ards at eac h collision p oin t. If, for a giv en tra jectory , a collision is detected along the corresp onding path, then w e simply consider transforming that tra jectory so that it enco des a new path, one that is found b y b ouncing o the obstacle at the collision p oin t (see Figure 7). Note that this construction is applied recursiv ely un til the en tire tra jectory corresp onds to a collision-free path. 304 The Ariadne's clew algorithm Using this tec hnique, all tra jectories are so transformed that they enco de v alid paths. This impro v ed v ersion of the Ariadne's clew algorithm no longer cares ab out obstacles. F rom the p oin t of view of a searc h in the tra jectory space, it is as if the obstacles ha v e simply v anished. This metho d is esp ecially ecien t for narro w corridors in the conguration space. Without b ouncing, the mapping of a corridor in the conguration space to the tra jectory space is a set of v ery few p oin ts. With b ouncing, ev ery single tra jectory going through a part of the corridor is \folded" in to the corridor (see Figure 7). The resultan t mapping of the corridor in the tra jectory space is consequen tly a m uc h larger set of p oin ts, and therefore it is m uc h easier to nd a mem b er of this set. This empirical impro v emen t has a ma jor practical impact b ecause it mak es the prop osed algorithm faster (fteen times) in the problem considered b elo w. 3.5 The Algorithm W e can no w giv e a nal v ersion of the Ariadne's clew algorithm. It has three inputs: ^ q  (the initial p osition), ^ q  (the goal p osition), and  (the maxim um allo w ed distance for a path to the C -obstacles ). It returns a legal path or terminates if no path exists at the giv en resolution. ALGO R I T H M AR I AD N E ( ^ q  ; ^ q  ;  ) b egin i := 1; ^  1 := ^ q  ; /* Initialize the set of landmarks with the initial p osition  1 := f ^  1 g ; " 1 = + 1 ; do while ( " i >  ); /* run sear ch : lo ok for the goal with a lo cal metho d if (min l 2 I R ` d ( ^ q  ; ^ q ( l )) == 0) return; /* A path has b een found ! else /* run explore : place a new landmark i := i + 1; ^  i := ^ q : sup l 2 I R ` d ( i  1 ; ^ q ( l ));  i :=  i  1 [ f ^  i g ; " i := d ( i  1 ; ^  i ); endif enddo  =  i ; " = " i ; return( " ); /* No path ! end Figure 8: The Ariadne's Clew Algorithm 305 Mazer, Ahua ctzin, & Bessi  ere The algorithm is based on the follo wing optimization problems: E X P LO R E : ( sup d ( i  1 ; ^ q ( l )) l 2 I R ` S E AR C H : ( min d ( ^ q ( l ) ; ^ q  ) l 2 I R ` ^ q ( l ) denotes the extremit y of a legal path parameterized with ` real parameters and starting either from eac h of the previously placed landmarks ( explore ) or from the latest placed landmark ( sear ch ). The algorithm is resolution-complete under the follo wing assumptions:  \Space lling completeness": The global maxim um distance can b e found b y the optimization algorithm used in explore ; the conguration space is a compact set.  \  completeness": The optimization pro cedure used in sear ch alw a ys nd a complete path (or returns 0) when the starting and the goal p ositions are lo cated within a ball of radius  of the free space. In practice, the rst condition cannot b e met with a randomized optimization algorithm in a b ounded time, and only lo cal maxima are found. Ho w ev er, the landmarks placed according to the new algorithm are b etter distributed o v er the free space than landmarks placed randomly , leading to b etter p erformances. The goal of the next section is to justify this claim, exp erimen tally . 4. P ath Planning for a Six dof Arm in a Dynamic En vironmen t In order to demonstrate the feasibilit y and qualities of the Ariadne's clew algorithm, w e ha v e dev elop ed a realistic application of the algorithm. W e selected a problem where w e w an t to ha v e a path planner for a six dof rob ot arm in a dynamic en vironmen t where another arm is used as a mobile obstacle. The rob ot (rob ot A) is under the con trol of the Ariadne's clew algorithm. It shares its w orkspace with a second rob ot (rob ot B) that is mo ving under the con trol of a random motion generator. The Ariadne's clew algorithm m ust b e able to compute paths for A in \real time" (here, real time means fast enough to ensure that rob ot A will nev er collide with rob ot B). In order to reac h suc h a lev el of p erformance, w e c hose to implemen t the Ariadne's clew algorithm on a massiv ely parallel mac hine (Megano de with 128 T800 T ransputers). F urthermore, w e selected a genetic algorithm as our optimization tec hnique. The reasons for this c hoice are: 1. Genetic algorithms are w ell suited for problems where the searc h space is h uge but where there are man y acceptable solutions. This is exactly the case here. The tra- jectory space is h uge but there are, barring exceptional cases, n umerous acceptable paths going from ^ q  to ^ q  without collision. 306 The Ariadne's clew algorithm 2. Genetic algorithms, unlik e a n um b er of the other optimization tec hniques (Bessi  ere, T albi, Ah uactzin, & Mazer, 1996), are v ery easy to implemen t on parallel arc hitec- tures. W e ha v e previously dev elop ed a parallel genetic algorithm ( pga ) and w e ha v e already had signican t exp erience using it (T albi, 1993). 3. pga , unlik e most parallel programs, sho ws linear sp eed-up (when y ou double the n um b er of pro cessors y ou reduce the computation time b y half ) and ev en sup er-linear sp eed-up under certain circumstances (T albi & Bessi  ere, 1996). 4.1 P arallel Genetic Algorithm Genetic algorithms are sto c hastic optimization tec hniques in tro duced b y Holland (1975) t w en t y y ears ago. They are used in a large v ariet y of domains including rob otics (Ah uactzin et al., 1992; La wrence, 1991; F alk enauer & Bououix, 1991; F alk enauer & Delc ham bre, 1992; Meygret & Levine, 1992) b ecause they are easy to implemen t and do not require algebraic expression for the function to b e optimized. 4.1.1 Principle of Genetic Algorithm The goal of the algorithm is to nd a p oin t reac hing a \go o d" v alue of a giv en function F o v er a searc h space S . First, a quan tization step is dened for S and the searc h is conducted o v er a discrete subset, S d of S . S d con tains 2 N elemen ts. In practice, the cardinalit y of S d can b e extr emely large. F or example, in our implemen tation of explore , N = 116. Th us, a con tin uous domain is discretized with a giv en resolution. During an initialization phase a small subset of S d is dra wn at random. This subset is called a p opulation . Eac h elemen t of this p opulation is co ded b y a string of N bits. The genetic algorithm iterates the follo wing four steps un til a solution is found. 1. Ev aluation : Rank the p opulation according to the v alue of F for eac h elemen t of S d . Decide if the b est elemen t can serv e as an acceptable solution; if y es, exit. 2. Selection : Use the function F to dene a probabilit y distribution o v er the p opulation. Select a pair of elemen ts randomly according to this probabilit y distribution. 3. Repro duction : Pro duce a new elemen t from eac h pair using \genetic" op erators. 4. Replacemen t : Replace the elemen ts of the starting p opulation b y b etter new ele- men ts pro duced in step 3. Man y genetic op erators (Da vidor, 1989) are a v ailable. Ho w ev er, the more commonly used are the mutation and the cr oss-over op erators. The m utation op erator consists of randomly ipping some bits of an elemen t of the p opulation. The cross-o v er op erator consists of rst randomly c ho osing a place where to cut the t w o strings of bits, and then building t w o new elemen ts from this pair b y simply gluing the righ t and the left parts of the initial pair of strings (see Figure 9). W e use b oth op erators to pro duce new elemen ts. First, w e use the cross-o v er op erator to get an in termediate string. Then, the m utation op erator is used on this in termediate string to get the nal string. 307 Mazer, Ahua ctzin, & Bessi  ere k PARENTS Cross−over NEW ELEMENTS Figure 9: The cross-o v er op eration. 4.1.2 Principle of the P arallel Genetic Algorithm (pga) There are man y parallel v ersions of genetic algorithms: the standard parallel v ersion (Rob ert- son, 1987), the decomp osition v ersion (T anese, 1987) and the massiv ely parallel v ersion (T albi, 1993). W e c hose this last metho d. The main idea is to allo cate one elemen t of the p opula- tion for eac h pro cessor so that steps 1, 3, and 4 can b e executed in parallel. F urthermore, the selection step (step 2) is carried out lo cally , in that eac h individual ma y mate only with the individuals placed on pro cessors ph ysically connected to it. This ensures that the comm unication o v erhead do es not increase as the n um b er of pro cessors increases. This is the reason wh y pga sho ws linear sp eed-up. The parallel genetic algorithm iterates the follo wing four steps un til a solution is found. 1. Ev aluation : Ev aluate in p ar al lel all the individuals. 2. Selection : Select in p ar al lel , among the neigh b ors, the mate with the b est ev aluation. 3. Repro duction : Repro duce in p ar al lel with the c hosen mate. 4. Replacemen t : Replace in p ar al lel the paren ts b y the ospring. On the Megano de, w e implemen ted the pga on a torus of pro cessors where eac h indi- vidual has four neigh b ors (see Figure 10) 4.2 P arallel Ev aluation of the Cost F unction The ev aluation functions used in sear ch and explore are v ery similar: they b oth compute the nal p osition of the arm giv en a Manhattan path of a xed order. In our implemen tation, based on exp erience, w e c hose to use Manhattan paths of order 2. Order 2 app eared to b e a go o d compromise b et w een the n um b er of landmarks needed (increases as order decreases) and the computing time necessary for the optimization functions (increases as order increases). Since our rob ot has six dof , the argumen t of the cost function in sear ch is a v ector in R 12 : ( 1 1 ;  1 2 ; :::;  1 6 ; : : : ;  2 1 ; : : : ;  2 6 ) and the argumen t of the cost function used for explore is a v ector in I N  I R 12 : ( i;  1 1 ;  1 2 ; :::;  1 6 ; : : : ;  2 1 ; : : : ;  2 6 ) where i co des the landmark used as a starting p oin t for the path. In b oth cases the functions are dened only on a b ounded subset of I R 12 and I N  I R 12 , whose limits are xed b y the mec hanical stops of the rob ot and the maxim um n um b er of landmarks. A discretization step is c hosen for these t w o subsets b y dening the resolution at whic h eac h elemen tary motion is discretized. 308 The Ariadne's clew algorithm INDIVIDUALS "HOST" "ROOT" Figure 10: A torus with sixteen pro cessors. One individual is placed on eac h pro cessor. Eac h individual has four neigh b ors. In our case, eac h  j i is discretized with 9 bits and the n um b er of landmarks is limited to 256. Th us, giv en a binary string of 116 = 8 + 12  9 bits, w e can con v ert it in to a v ector (as an argumen t) for the cost function of sear ch , or explore , resp ectiv ely . Manhattan paths are ev aluated in a simplied mo del of the en vironmen t. This mo del is obtained b y enclosing eac h elemen t of the scene in to a b ounding rectangular b o x. The ev aluation of a v ector is p erformed as follo ws: F or eac h  j i in ( 1 1 ;  1 2 ; :::;  1 6 ; : : : ;  2 1 ; : : : ;  2 6 ) Compute the limits on the motion for join t i . Compute  j i 0 b y b ouncing on these limits (see Section 3.4). Up date the p osition of the rob ot. The limits on the motion of join t i are obtained b y merging the legal ranges of motion of all the links that mo v e when join t i mo v es, and all the obstacles. T o obtain a legal range of motion b et w een a link and an obstacle, w e consider the t w o enclosing parallelepip eds and express their co ordinates in the join t frame. Then, w e use a classical metho d to compute the range (Lozano-P  erez, 1987). In our parallel implemen tation, w e distributed the geometric computations among sev- eral pro cessors. Eac h pro cessor is dedicated to the computation of a single t yp e of in terac- tion. 4.3 P arallel Implemen tation of the Ariadne's Clew Algorithm Finally , the Ariadne's clew algorithm is implemen ted in parallel with three lev els of paral- lelism. 1. Ob viously , a rst lev el of parallelization can b e obtained b y running sear ch and explore at the same time on t w o sets of pro cessors. While sear ch is c hec king 309 Mazer, Ahua ctzin, & Bessi  ere whether a path exists b et w een the last placed landmark and the goal, explore is generating the next landmark. 2. The second lev el of parallelism corresp onds to a parallel implemen tation of b oth ge- netic algorithms emplo y ed b y sear ch and explore to treat their resp ectiv e opti- mization problems. 3. The third lev el corresp onds to a parallelization of the collision c hec king function and range computation. W e completed a full implemen tation of these three lev els on a Megano de with 128 t 800 transputers. Figure 11 represen ts our parallel implemen tation of the Ariadne's clew algo- rithm and Figure 12 sho ws ho w w e ha v e em b edded this arc hitecture in to our exp erimen tal setup. A cad system (a ct) is used to mo del the scene with the t w o rob ots. The rob ots are under the con trol of kali (Ha yw ard, Daneshmend, & Ha y ati, 1988). First, a simplied geometric mo del of the scene is do wnloaded in to the memory of the transputers. Then, a Silicon Graphics w orkstation w orks as a global con troller and lo ops o v er the follo wing steps: 1. Generate and execute a legal random motion for rob ot B. 2. Send the new conguration of rob ot B to the Megano de as w ell as the desired nal conguration for rob ot A. 3. Get the planned path for rob ot A from the Megano de and execute it. 4. W ait for a random time and stop rob ot A. 5. Go to 1. This sequence allo ws us to test our algorithm extensiv ely in real situations b y ha ving to deal with man y dieren t en vironmen ts. Of course, the most in teresting gure w e can obtain from this exp erimen t is the mean time necessary to compute one path giv en a new en vironmen t. F or this exp erimen tal setup this mean time is 1.421 seconds. Using the same arc hitecture with more up-to-date pro cessors ( t 9000) w ould reduce this time b y a factor of ten. The same computation on a single pro cessor ( sp ar c 5 ) w ould tak e three times longer than the curren t implemen tation. In summary, we have achieve d our main go al by pr oving that it is inde e d p ossible (with the A riadne's clew algorithm) to plan c ol lision-fr e e p aths for a r e al r ob ot with many dof in a dynamic r e alistic envir onment . 5. Conclusion: Con tributions, Diculties, and P ersp ectiv es As men tioned in the In tro duction, the Ariadne's clew algorithm has t w o main qualities: eciency , and gener ality . Let us, in conclusion, explain and discuss these t w o qualities. 310 The Ariadne's clew algorithm PSfrag replacemen ts Ariadne's clew algorithm pga pga Range computation Range computation t yp e A et B t yp e A et B t yp e C t yp e C Master Searc h Explore A G Gamma Gamma Psi AB Psi AB Psi C Psi C psi ab psi ab psi c psi c Figure 11: A parallel implemen tation of the Ariadne's clew algorithm 311 Mazer, Ahua ctzin, & Bessi  ere 68030 Mega−Node 128 Transputers Sun 3 Ethernet Silicon Graphics (Unix) (Unix) Robot II KALI Sun 4 (Unix) ACT server Mega−Node server VxWorks Bus VME (VxWorks) 68030 Bus VME (VxWorks) KALI Robot I GENETIC ALGORITHM (SEARCH) GENETIC ALGORITHM (EXPLORE) CAD SYSTEM Figure 12: The exp erimen tal setup 5.1 P erformance Comparing the p erformance of this kind of algorithm is a v ery delicate sub ject. P erformance ma y b e a matter of computing time, eorts needed to program, or ease of application to dieren t problems (see Section 5.2). Ev aluating the p erformance in terms of computing time is v ery dicult for one fundamen tal and three practical reasons: 312 The Ariadne's clew algorithm 1. The fundamen tal reason is, once again, the np -completeness of the path planning problem. As deceptiv e cases ma y alw a ys b e designed, the only p erformance results one ma y reasonably presen t are alw a ys sp ecic. 2. The three practical reasons are: (a) Ob viously , the rst requiremen t for suc h a comparison is that dieren t algorithms run on the same mac hines with the same a v ailable memory . This ma y seem simple but it is a main dicult y in our case b ecause our algorithm has b een designed to run on rather sp ecic kinds of mac hines, namely , massiv ely parallel ones. It could also b e implemen ted on non-parallel mac hines, but then it ma y lose part of its in terest. A fair comparison w ould b e to compare the algorithms on b oth t yp es of mac hines. This w ould imply programming other algorithms in parallel, whic h is v ery dicult in practice. (b) Man y kno wn path planning algorithms rst compute the conguration space (or an appro ximation of it) o-line, and then ecien tly solv e the path planning problem on-line. As w e sa w, in order to deal with a dynamic en vironmen t, the Ariadne's clew algorithm adopts a completely dieren t approac h. (c) F or practical reasons, man y test problems are to y problems (2 d , few obstacles, few faces, sim ulated rob ots) and the p erformance results using these kinds of problems are v ery dicult to generalize to realistic industrial problems (3 d , tens of obstacles, h undreds of faces, real rob ots). Considering all these reasons, w e tested our algorithm b y implemen ting a realistic rob otic application to the v ery end. T o ac hiev e this goal, w e assem bled a complex exp erimen tal setup including six dieren t mac hines (1 meganode , 2 68030, 2 sun 4, and 1 silicon graphics ), t w o mec hanical arms, and running sev en dieren t co op erativ e programs (2 kali , 1 a ct , 2 vxw orks , 1 p arx , and 1 Ariadne's clew algorithm). Our c hallenge w as to b e able to solv e the path planning problem fast enough to driv e a real six dof arm in a dynamic en vironmen t. The Ariadne's clew algorithm indeed ac hiev ed this goal in our exp erimen ts where the en vironmen t is comp osed of v e xed obstacles and a six dof arm mo ving indep enden tly . W e are not a w are of an y other metho ds capable of suc h p erformance. T o the b est of our kno wledge, curren tly implemen ted planners w ould tak e a n um b er of seconds (ten) to place a set of landmarks on a 2 d example for a rob ot with v e dof (Ka vraki et al., 1996). Despite the fact that nding a general purp ose planning tec hnique for real industrial application is a v ery dicult problem, w e b eliev e that the Ariadne's clew algorithm pro vides an eectiv e approac h to suc h problems. The n um b er of range computations for a Manhattan motion of order 1 is C k 2 + k 2  n where n is the n um b er of faces, k the n um b er of dof , and C a constan t factor, dep ending on the n um b er of parts used to mo del the rob ot. Ob viously , suc h a n um b er of faces ma y b e a sev ere dicult y for the implemen tation of the Ariadne's clew algorithm describ ed so far. T o sp eed up the computation w e use a n um b er of geometric lters that reduce the n um b er of pairs of en tities to b e analyzed. Ho w ev er, it w as p ossible to follo w t w o researc h trac ks in com bination. First, w e could use collision c hec king metho ds that allo w access to the pairs in collision in a logarithmic 313 Mazer, Ahua ctzin, & Bessi  ere time (F a v erjon & T ournassoud, 1987). Second, w e could preserv e part of the landmark graph when the en vironmen t is c hanging (McLean & Mazon, 1996). 5.2 Generalit y The Ariadne's clew algorithm is general in the sense that it ma y b e used for n umerous and v ery dieren t applications in rob otics. Basically , the main thing that needs to b e c hanged in the algorithm is the distance d used in the ev aluation functions of the t w o optimization problems. Sev eral planners ha v e b een implemen ted in this w a y: a ne motion planner (De la Rosa, Laugier, & Na jera, 1996), t w o motion planners for holonomic and non-holonomic mobile rob ots (Sc heuer & F raic hard, 1997), a reorien tation planner for an articulated hand(Gupta, 1995), a planner for grasping and regrasping (Ah uactzin, Gupta, & Mazer, 1998), and a planner for a rob otic arm placed in the steam generator of a n uclear plan t (McLean & Mazon, 1996). Adapting the algorithm to a new application is, therefore, clearly a v ery easy task. F or instance, the application to path planning for the non-holonomic trailer w as dev elop ed in three da ys. The Ariadne's clew algorithm is also general in the sense that it ma y b e used for an y kind of path planning problem in a con tin uous space, in elds other than rob otics. Although it ma y b e sucien t to c hange the distance function d , one ma y also consider c hanging the form of the function d , or ev en the nature of the searc hed spaces. F or instance, the concept of obstacles ma y b e reconsidered. Instead of \hard" obstacles, one could replace them b y zones of constrain ts. In that case, the path planning problem do es not consist of nding a path without collisions but rather nding a path b est satisfying the dieren t constrain ts. Suc h a planner has b een dev elop ed for a na v al application where the problem w as to nd a path for a b oat with v arious constrain ts on the tra jectory . This op ens n umerous p ersp ectiv es of applications for applying the Ariadne's clew algorithm in a broader eld than pure rob otics. Ac kno wledgmen ts The authors are greatly indebted to Dr. Kamal Gupta from Simon F raser Univ ersit y who carefully read the pap er and suggested v aluable corrections that greatly impro v e the qualit y of the nal pap er. This w ork has b een made p ossible b y: Le Cen tre National de la Rec herc he Scien tique (F rance), Consejo Nacional de Ciencia y T ecnologia (Mexico) and esprit 2, P2528 ( eec ). References Ah uactzin, J., Gupta, K., & Mazer, E. (1998). Manipulation Planning for Redundan t Rob ots: A Practical Approac h. The International Journal of R ob otics R ese ar ch , 17 (7), 731{747. Ah uactzin, J., Mazer, E., Bessi  ere, P ., & T albi, E. (1992). Using Genetic Algorithms for Rob ot Motion Planning. In Pr o c e e dings of the 1992 Eur op e an Confer enc e on A rticial Intel ligenc e , pp. 671{675. 314 The Ariadne's clew algorithm Barraquand, J., & Latom b e, J. (1990). A Mon te Carlo Algorithm for P ath Planning with Man y Degrees of F reedom. In Pr o c e e dings of the 1990 IEEE International Confer enc e on R ob otics and A utomation , pp. 1712{1717. Bessi  ere, P ., T albi, E., Ah uactzin, J., & Mazer, E. (1996). Un Algorithme G  en  etique P arall  elle p our l'Optimisation. T e chnique et Scienc e Informatique , 15 (8), 1105{1130. Bro oks, R. (1983). Solving the Find-P ath Problem b y Go o d Represen tation of the F ree Space. IEEE T r ansactions on System, Man and Cyb ernetics , 13 (4), 190{197. Cann y , J. (1988). The Complexity of R ob ot Motion Planning . MIT Press. Da vidor, Y. (1989). Analogous Crosso v er. In Pr o c e e dings of the Thir d International Con- fer enc e on Genetic A lgorithms , pp. 98{103. De la Rosa, F., Laugier, C., & Na jera, J. (1996). Robust P ath Planning in the Plane. IEEE T r ansactions on R ob otics and A utomation , 12 (3), 347{352. F alk enauer, E., & Bououix, S. (1991). A Genetic Algorithm for Job Shop. In Pr o c e e dings of the 1991 IEEE International Confer enc e on R ob otics and A utomation , pp. 824{829. F alk enauer, E., & Delc ham bre, A. (1992). A Genetic Algorithm for Bin P ac king and Line Balancing. In Pr o c e e dings of the 1992 IEEE International Confer enc e on R ob otics and A utomation , pp. 1186{1192. F a v erjon, B., & T ournassoud, P . (1987). A Lo cal Based Approac h for P ath Planning of Manipulators with a High Num b er of Degrees of F reedom. In Pr o c e e dings of the 1987 IEEE International Confer enc e on R ob otics and A utomation , pp. 1152{1159. F erbac h, P . (1996). Con tribution  a la Planication de T ra jectoires. Rapp ort de Rec herc he EDF-DER HP-28/96/026, Direction des  Etudes et Rec herc hes EDF. Gupta, K. (1995). Motion Planning for Re-Orien tation Using Finger T rac king: Landmarks in S O (3)  ! . In Pr o c e e dings of the 1995 IEEE International Confer enc e on R ob otics and A utomation , pp. 446{451. Ha yw ard, V., Daneshmend, L., & Ha y ati, S. (1988). An Ov erview of KALI: A System to Program and Con trol Co op erativ e Manipulators. In Pr o c e e dings of the F ourth International Confer enc e on A dvanc e d R ob otics , pp. 236{240. Holland, J. (1975). A daptation in Natur al and A rticial Systems . Univ ersit y of Mic higan Press. Hw ang, Y., & Ah uja, N. (1992). Gross Motion Planning: A Surv ey. A CM Computing Surveys , 24 (3). Ka vraki, L., Sv estk a, P ., Latom b e, J., & Ov ermars, M. (1996). Probabilistic Roadmaps for P ath Planning in High-Dimensional Conguration Spaces. IEEE T r ansactions on R ob otics and A utomation , 14 (4), 566{580. La wrence, D. (Ed.). (1991). Handb o ok of Genetic A lgorithms . V an Nostrand Reinhold. 315 Mazer, Ahua ctzin, & Bessi  ere Lin, H., Xiao, J., & Mic halewicz, Z. (1994). Ev olutionary Na vigator for a Mobile Rob ot. In Pr o c e e dings of the 1994 IEEE International Confer enc e on R ob otics and A utomation , pp. 2199{2004. Lozano-P  erez, T. (1987). A Simple Motion-Planning Algorithm for General Rob ot Manip- ulators. IEEE T r ansactions on R ob otics and A utomation , 3 (3), 224{238. McLean, A., & Mazon, T. (1996). Incremen tal Roadmaps and Global P ath Planning in Ev olving Industrial En vironmen ts. In Pr o c e e dings of the 1996 IEEE International Confer enc e on R ob otics and A utomation , pp. 101{106. Meygret, A., & Levine, M. (1992). Extraction de Primitiv es G  eom  etriques: Utilisation d'un Algorithme G  en  etique. Rapp ort Ann uel, Cen ter for In telligen t Mac hines, McGill Univ ersit y , Mon tr  eal. Ov ermars, M. (1992). A Random Approac h to Motion Planning. T ec hnical Rep ort R UU- CS-92-32, Departmen t of Computer Science, Utrec h t Univ ersit y. Rob ertson, G. (1987). P arallel Implemen tation of Genetic Algorithms in a Classier System. In Da vis, L. (Ed.), Genetic A lgorithms and Simulate d A nne aling . Morgan Kaufmann Publishers. Sc heuer, A., & F raic hard, T. (1997). Con tin uous-Curv ature P ath Planning for Car-Lik e V ehicles. In Pr o c e e dings of the IEEE/RSJ International Confer enc e on Intel ligent R ob ots and Systems , pp. 997{1003. T albi, E. (1993). A l lo c ation de Pr o c essus sur les A r chite ctur es Par al l  eles  a M  emoir Dis- tribu  e e . Ph.D. thesis, Institut National P olytec hnique de Grenoble - F rance. T albi, E., & Bessi  ere, P . (1996). A P arallel Genetic Algorithm Applied to the Mapping Problem. In Astfalk, G. (Ed.), Applic ations on A dvanc e d A r chite ctur e Computers . SIAM. T anese, R. (1987). P arallel Genetic Algorithm for a Hyp ercub e. In Pr o c e e dings of the Se c ond International Confer enc e on Genetic A lgorithms , pp. 177{183. Xiao, J., Mic halewicz, Z., & Zhang, L. (1996). Ev olutionary Planner/Na vigator: Op erator P erformance and Self-T uning. In Pr o c e e dings of 1996 IEEE International Confer enc e on Evolutionary Computation , pp. 336{371. 316