The Ariadne's Clew Algorithm

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.

Extracted Text

Journal of Artificial Intelligence Research 9 {1998} 295-316 Submitted 9/97;
published 11/98
 The Ariadne's Clew Algorithm
 Emmanuel Mazer
 Laboratoire GRAVIR,
 INRIA 665 Avenue de L'Europe 38330 Montbonnot, France Juan Manuel Ahuactzin
 Depto. de Ing. en Sistemas Computationales
 Unversidad de las Americas
 Puebla, Cholula, Puebla 72820, Mexico
 Pierre Bessi022ere
 Laboratoire LEIBNIZ,
 46 Avenue Felix Viallet
 38000 Grenoble, France Abstract
 W e present a new approach to path planning, called the \Ariadne's clew
 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,
 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,
 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
 In this paper, we present a new approach to path planning, called the \Ariadne's
 ". 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
 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
 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
 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
 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
 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
 ; q
 } are known. Thus, for
 each pair {q
 ; q
 }, 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
 and ^q
 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
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
P { ^q
P { ^q
 }P { ^q
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,
 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
 f ree
 f ree
 a^ q
^ q
 f^ q
C B 2
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
 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
 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
 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
 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
^q ffi
^ q
 = {x
 ; y
^q `
Figure 3: A parameterized tra jectory {022
 ; d 1
 ; 022
 2 ; d
 ; :::022 l
 ; d
 l } and a starting point ^q
 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
 in the configuration space.
 For example, for a holonomic mobile robot the tra jectory {022
 ; d
 ; 022 2
 ; d
 2 ; :::022
 ; d l
 } can be
 interpreted as making a 022 1
 degree turn, moving straight d
 , making a 022
 degree turn and so on. Given the starting configuration ^q
 , 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
 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
 ; ^q
 } if the path is collision-free, and d{ ^q i
 ; ^q 
 } otherwise
 { ^q
 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
 The algorithm search, based on this very simple technique and a randomized
 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
 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
 to ^q
 , the purpose of explore
 is to compute an approximation of the region of the configuration space
accessible from ^q
 . 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
317,590.25)(5.75,0)(0,7.5436.75,589.5)(6.25,0)(0,7.5ABPSfrag replacements^q
^ 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
 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
 2. The function to maximize is d{ ^q
 ; 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
 and ^q
 . 303Mazer, Ahuactzin, & Bessi 022
 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
 after a bounce of segment 3 against obstacle 2. Finally, Figure {d} presents
 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
 {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
 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
 {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
 ; ^q
*  ; 032}
 begin i := 1;
 := ^q
 /* Initialize the set of landmarks with the initial position
 1 := f
 025 1
 g; "
 = +1;
 do while {"
 > 032};
 /* run search : look for the goal with a local method
 if {min
 d{ ^q
 ; ^q{l}) == 0}
 return; /* A path has been found ! else
 /* run explore : place a new landmark
 i := i + 1;
 := ^q : sup
 d{003 i0001
 ; ^q{l});
 003 i
 := 003
 [ f
 ^ 025
 g; "
 := d{003
 003 = 003
 i ;
 " = "
 return{"}; /* No path !
 endFigure 8: The Ariadne's Clew Algorithm
 305Mazer, Ahuactzin, & Bessi 022
 The algorithm is based on the following optimization problems: E X P LORE
 sup d{003
 ; ^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
*  \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
 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
 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
 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
 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
 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
 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
 of S . S
 contains 2
 elements. In practice, the cardinality of S
 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
 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
 Decide if the best element can serve as an acceptable solution; if yes,
 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
 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
 string to get the final string.
 307Mazer, Ahuactzin, & Bessi 022
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.
 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
 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
 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
 : {001
 ; 001
 ; :::; 001
 6 ; : : : ; 001
 2 1
 ; : : : ; 001 2
 } and the argument of the cost function used
 for explore is a vector in IN 002 IR
 12 : {i; 001
 ; 001
 ; :::; 001 1
 ; : : : ; 001
 ; : : : ; 001
 } 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
 and IN 002 IR
 , 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
 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
 is obtained by enclosing each element of the scene into a bounding rectangular
 The evaluation of a vector is performed as follows:
 For each 001
 in {001
 ; 001 1
 ; :::; 001 1
 ; : : : ; 001
 ; : : : ; 001
 Compute the limits on the motion for joint i.
 Compute 001 j
 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
 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
 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
 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
Range computationtype A et B
type C
psicFigure 11: A parallel implementation of the Ariadne's clew algorithm
 311Mazer, Ahuactzin, & Bessi 022
 68030Mega-Node128 TransputersSun 3EthernetSilicon Graphics{Unix}{Unix}Robot
IIKALISun 4{Unix}ACT server)(Mega-Nodeserver)(VxWorksBus VME)()( {VxWorks}
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
 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
 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
 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
 003 n
 where n is the number of faces, k the number of dof, and C a constant factor,
 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
 time {Faverjon & Tournassoud, 1987}. Second, we could preserve part of the
 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
 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
 robots {Scheuer & Fraichard, 1997}, a reorientation planner for an articulated
 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.
 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
 to Program and Control Cooperative Manipulators. In Pro c e e dings of the
 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
 for Path Planningin High-Dimensional Configuration Spaces. IEEE Tr ansactions
 Rob otics and Automation, 14 {4}, 566{580.
 Lawrence, D. {Ed.}. {1991}. Handbo ok of Genetic Algorithms. Van Nostrand
 315Mazer, Ahuactzin, & Bessi 022
 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
 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
 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
 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