The Ariadne's Clew Algorithm

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.


Introduction
The path planning problem is of major interest for industrial robotics.A basic version of this problem consists of nding a sequence of motions for a robot from a start con guration to a given goal con guration 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, has been 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, nding a path for a robot with many dof (more than ve) in an environment with several obstacles is, indeed, a very di cult 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 1998 AI Access Foundation and Morgan Kaufmann Publishers.All rights reserved.
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 nd paths for robots with many dof in dynamic environments.
The ultimate goal of a planner is to nd 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 ne 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 algorithm 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 e cient and general: 1.The algorithm is e cient 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 signi cant 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 e ort 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 elds 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 principle of the Ariadne's clew algorithm.Section 4 describes the application of the algorithm to a six dof arm in a dynamic environment.Finally, Section 5 concludes the paper with a discussion of the contributions of our approach, the main di culties involved, and possible improvements of our method.
1.According to Greek legend, Ariadne, the daughter of Minos, King of Crete, helped Theseus kill the Minotaur, a monster living in the Labyrinth, a huge maze built by Daedalus.The main di culty faced by Theseus was to nd 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 nd his path back.

The Path Planning Problem
Many versions of the path planning problem exist.An exhaustive classi cation 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 nal position of the robot arm, the problem is to nd a set of motions that will lead the robot to move between the two positions without colliding with the obstacles.
To drive the robot amidst the obstacles, early methods (Brooks, 1983) directly used the 3d cad models of the robot and of the obstacles to nd a solution, i.e., they considered the \operational 3d space".In this space, the path planning problem consists of nding the movements of a complex 3d structure (the robot) in a cluttered 3d space.
A major advance was to express the problem in another space known as the con guration space, denoted by C (Lozano-P erez, 1987).In this space, the position (or con guration) 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, and are called C-obstacles.In the con guration space, the path planning problem consists of nding a continuous curve (representing a path for a single geometrical point) that (i) connects the points representing the initial and the nal con guration of the robot, and (ii) does not intersect any C-obstacles.This method trades a simpli cation of the path planning problem (it searches a path for a single point) against a higher-dimensional search space (the dimension of 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 qa and qf one will also de ne 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 eld is to consider the \trajectory space" (Ferbach, 1996) where a whole path is represented by a single point.The coordinates of this point are the values of the parameters de ning the successive movements of the robot.For instance, the list of successive commands 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 simpli cation of the path planning problem (searching for a point) against a higher dimension of the search space (the dimension of the trajectory space is the number of parameters needed to specify completely a whole path).For example, in Figure 2, the path between qb and qd can be represented by a point in a seven-dimensional space simply by considering the length of its seven segments.

PSfrag replacements
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

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 con guration space.In the decomposition methods, one tries to characterize the regions of the con guration 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 nd a path if one exists and will still terminate in a nite 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 limited number (three or four) of dof.In addition, they are slow and can only be used o -line: the planner is invoked with a model of the 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.

Path Planning with Local Planners
One way to combat the complexity of the problem is to trade completeness against performance.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 environment where the method will get trapped in a local minimum.However, these methods are useful in many industrial applications because they can deal with complex robots and it is divided into two regions C f ree qa and C f ree qb 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.

Path Planning with Randomized Techniques and Landmarks
The stochastic or random approach was rst 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 build a graph in the con guration space.The graph is obtained 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 con guration has been reached from one of these intermediary positions by a gradient descent motion.These algorithms work with a discretized representation of the con guration space.They are known to be probabilistically 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 in nity.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 number 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 fail to reach a goal, new subgoals are generated until the problem is easy enough to be solved.In their approach, rst a local planner is used to reach the nal position: should the local planner fail, the con guration 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 nal position.sandros has been shown to be particularly well adapted to nd paths for manipulators.It has been implemented and tested for planning paths for Puma and Adept robots.

Path Planning in the Trajectory Space
The previous methods were essentially based on the con guration space: the retraction, the decomposition, or the optimization is made in this space.An alternative is to consider the \trajectory space".For example, in his method vdp, Ferbach (1996) starts by considering the straight line segment joining the initial and the nal con guration in C.This path is progressively modi ed 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 length across the forbidden region as the cost function in order to minimize.The search results in a new trajectory whose intersection with the forbidden regions is smaller than the original trajectory.The process is repeated until an admissible trajectory is found.As in the previous sections, it is also possible to design simple deceptive environments that will make this 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, Bessi ere, & Talbi, 1992), genetic algorithms are used to carry out optimization in the trajectory space.Trajectories 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 trajectory and the forbidden region crossed.The standard operators of the genetic algorithms have been modi ed and later extended to produce a large variety of paths (Xiao, Michalewicz, & Zhang, 1996).The number of intermediary via-points is xed 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.

Principle of the Ariadne's Clew Algorithm
As we have seen in the previous section, the computation of the con guration space C is a very time-consuming task.The main idea behind the Ariadne's clew algorithm is to avoid this computation.In order to do this, the algorithm searches directly for a feasible path in the trajectory space.The con guration space C is never explicitly computed.
As will be shown, in the trajectory 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.

PSfrag replacements
x y qF igure 3: A parameterized trajectory ( 1 ; d 1 ; 2 ; d 2 ; ::: l ; d l ) and a starting point q0 implicitly de ne a path (in the operational space) for a holonomic mobile robot.

Path Planning as an Optimization Problem: search
Given a robot with k dof, a trajectory of length l may be parameterized as a sequence of n = k l successive movements.A starting point q along with such a parameterized trajectory implicitly de ne a path and a nal con guration ql in the con guration space.
For example, for a holonomic mobile robot the trajectory ( 1 ; d 1 ; 2 ; d 2 ; ::: l ; d l ) can be interpreted as making a 1 degree turn, moving straight d 1 , making a 2 degree turn and so on.Given the starting con guration q , this trajectory leads to the nal con guration ql (see Figure 3).
Given a distance function d on the con guration space, if we nd a trajectory such that it does not collide with any obstacles and such that the distance between ql 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 trajectories, the trajectory 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 rst collision point). 2 2. 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 rst 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.
The algorithm search, based on this very simple technique and a randomized optimization 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 con guration 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 e cient but it is not complete, since it may fail to nd a path even if one exists for two di erent reasons: 1. Due to the optimization-based formulation, search can get trapped by local minima of the objective function, which in turn may place the robot far away from the goal (see Figure 5).
2. The length l of the trajectories considered may be too short to reach all the accessible regions of the con guration space.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 to q , the purpose of explore is to compute an approximation of the region of the con guration space accessible from q .
The explore algorithm builds an approximation of the accessible space by placing landmarks in the con guration space C in such a way that a path from the initial position q 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: 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 nd 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.

A Major Improvement: Bouncing on C-Obstacles
A typical di culty 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 version of the Ariadne's clew algorithm, presented above.The problem is that very few trajectories encode such paths and therefore they are very di cult to nd.Most trajectories 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 trajectory, a collision is detected along the corresponding path, then we simply consider transforming that trajectory so that it encodes a new path, one that is found by bouncing o the obstacle at the collision point (see Figure 7).Note that this construction is applied recursively until the entire trajectory corresponds to a collision-free path.
Using this technique, all trajectories 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 trajectory space, it is as if the obstacles have simply vanished.This method is especially e cient for narrow corridors in the con guration space.Without bouncing, the mapping of a corridor in the con guration space to the trajectory space is a set of very few points.With bouncing, every single trajectory going through a part of the corridor is \folded" into the corridor (see Figure 7).The resultant mapping of the corridor in the trajectory space is consequently a much larger set of points, and therefore it is much easier to nd a member of this set.This empirical improvement has a major practical impact because it makes the proposed algorithm faster ( fteen times) in the problem considered below.

The Algorithm
We can now give a nal version of the Ariadne's clew algorithm.It has three inputs: q (the initial position), q (the goal position), and (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.
ALGORITHM ARIADNE(q ; q ; ) begin i := 1; ^ 1 := q ; /* Initialize the set of landmarks with the initial position 1 := f ^ 1 g; " 1 = +1; do while (" i > ); /* 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; ^ i := q : sup l2IR `d( i 1 ; q(l)); i := i 1 f ^ i g;  Talbi, Ahuactzin, & Mazer, 1996), are very easy to implement on parallel architectures.We have previously developed a parallel genetic algorithm (pga) and we have already had signi cant 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 & Bessi ere, 1996).

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 & Bou ouix, 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.

Principle of Genetic Algorithm
The goal of the algorithm is to nd a point reaching a \good" value of a given function F over a search space S. First, a quantization step is de ned 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 de ne 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 elements produced in step 3.
Many genetic operators (Davidor, 1989) are available.However, the more commonly used are the mutation and the cross-over operators.The mutation operator consists of randomly ipping some bits of an element of the population.The cross-over operator consists of rst 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 the initial pair 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 nal string.There are many parallel versions of genetic algorithms: the standard parallel version (Robertson, 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 population 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 parallel all the individuals.2. Selection: Select in parallel, among the neighbors, the mate with the best evaluation.
3. Reproduction: Reproduce in parallel with the chosen mate.4. Replacement: Replace in parallel the parents by the o spring.
On the Meganode, we implemented the pga on a torus of processors where each individual has four neighbors (see Figure 10)

Parallel Evaluation of the Cost Function
The evaluation functions used in search and explore are very similar: they both compute the nal position of the arm given a Manhattan path of a xed order.In our implementation, based on experience, we chose to use Manhattan paths of order 2. Order 2 appeared to be 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 in R 12 : ( 1 1 ; 1 2 ; :::; 1 6 ; : : : ; 2 1 ; : : : ; 2 6 ) and the argument of the cost function used for explore is a vector in IN IR 12 : (i; 1 1 ; 1 2 ; :::; 1 6 ; : : : ; 2 1 ; : : : ; 2 6 ) where i codes the landmark used as a starting point for the path.In both cases the functions are de ned only on a bounded subset of IR 12 and IN IR 12 , whose limits are xed by the mechanical stops of the robot and the maximum number of landmarks.A discretization step is chosen for these two subsets by de ning the resolution at which each elementary motion is discretized.

INDIVIDUALS "HOST" "ROOT"
Figure 10: A torus with sixteen processors.One individual is placed on each processor.
Each individual has four neighbors.
In our case, each 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 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 simpli ed model of the environment.This model is obtained by enclosing each element of the scene into a bounding rectangular box.
Compute 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 when joint 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 the range (Lozano-P erez, 1987).
In our parallel implementation, we distributed the geometric computations among several processors.Each processor is dedicated to the computation of a single type of interaction.

Parallel Implementation of the Ariadne's Clew Algorithm
Finally, the Ariadne's clew algorithm is implemented in parallel with three levels of parallelism.
1. Obviously, a rst level of parallelization can be obtained by running search and explore at the same time on two sets of processors.While search is checking whether a path exists between the last placed landmark and the goal, explore is generating the next landmark.
2. The second level of parallelism corresponds to a parallel implementation of both genetic algorithms employed by search and explore to treat their respective optimization 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 algorithm 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 are under the control of kali (Hayward, Daneshmend, & Hayati, 1988).First, a simpli ed 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 con guration of robot B to the Meganode as well as the desired nal con guration 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.

Go to 1.
This sequence allows us to test our algorithm extensively in real situations by having to deal with many di erent environments.Of course, the most interesting gure 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) would reduce 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 indeed possible (with the Ariadne's clew algorithm) to plan collision-free paths for a real robot with many dof in a dynamic realistic environment.

Conclusion: Contributions, Di culties, and Perspectives
As mentioned in the Introduction, the Ariadne's clew algorithm has two main qualities: e ciency, and generality.Let us, in conclusion, explain and discuss these two qualities.Figure 11: A parallel implementation of the 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 speci c. 2. The three practical reasons are: (a) Obviously, the rst requirement for such a comparison is that di erent algorithms run on the same machines with the same available memory.This may seem simple but it is a main di culty in our case because our algorithm has been designed to run on rather speci c 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 di cult in practice.(b) Many known path planning algorithms rst compute the con guration space (or an approximation of it) o -line, and then e ciently solve the path planning problem on-line.As we saw, in order to deal with a dynamic environment, the Ariadne's clew algorithm adopts a completely di erent approach.(c) For practical reasons, many test problems are toy problems (2d, few obstacles, few faces, simulated robots) and the performance results using these kinds of problems are very di cult 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 including six di erent machines (1 meganode, 2 68030, 2 sun 4, and 1 silicon graphics), two mechanical arms, and running seven di erent 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 ve xed obstacles and a six dof arm moving independently.
We 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 ve dof (Kavraki et al., 1996).Despite the fact that nding a general purpose planning technique for real industrial application is a very di cult problem, we believe that the Ariadne's clew algorithm provides an e ective approach to such problems.
The number of range computations for a Manhattan motion of order 1 is C k 2 +k 2 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 di culty for the implementation of the Ariadne's clew algorithm described so far.To speed up the computation we use a number of geometric lters 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 time (Faverjon & Tournassoud, 1987).Second, we could preserve part of the landmark graph when the environment is changing (McLean & Mazon, 1996).

Generality
The Ariadne's clew algorithm is general in the sense that it may be used for numerous and very di erent 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 ne motion planner (De la Rosa, Laugier, & Najera, 1996), two motion planners for holonomic and non-holonomic mobile robots (Scheuer & Fraichard, 1997), a reorientation planner for an articulated hand (Gupta, 1995), a planner 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, clearly a 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 elds other than robotics.Although it may be su cient 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 nding a path without collisions but rather nding a path best satisfying the di erent constraints.Such a planner has been developed for a naval application where the problem was to nd a path for a boat with various constraints on the trajectory.This opens numerous perspectives of applications for applying the Ariadne's clew algorithm in a broader eld than pure robotics.

Figure 2 :
Figure 2: The con guration space corresponding to Figure 1.Note: (1) C is a torus, (2)it is divided into two regions C f ree qa and C f ree qb that cannot be connected by a continuous path, and (3) there is not a C-obstacle for B 1 because it does not

Figure 4 :
Figure 4: Reactive replanning in a changing environment

Figure 7 :
Figure 7: Bouncing against C-obstacles.Figure (a) presents the original path in the conguration 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 nal bounce of segment 4 against obstacle 2.