UsingSk eletonsforNonholonomicP athPlanningamongObstacles BrianMirtic JohnCann ComputerScienceDivision Universit yofCalifo rnia Berk eley CA Abstract This p ap er describ es a pr actic al p ath plann
144K - views

UsingSk eletonsforNonholonomicP athPlanningamongObstacles BrianMirtic JohnCann ComputerScienceDivision Universit yofCalifo rnia Berk eley CA Abstract This p ap er describ es a pr actic al p ath plann

The planner is b ase d on building a onedimensional maximal cle ar anc e skeleton thr ough the c on gur ation sp ac e of the r ob ot However r ather than using the Eu clide an metric to determine cle ar anc e a sp cial metric which c aptur es inform

Tags : The planner
Download Pdf

UsingSk eletonsforNonholonomicP athPlanningamongObstacles BrianMirtic JohnCann ComputerScienceDivision Universit yofCalifo rnia Berk eley CA Abstract This p ap er describ es a pr actic al p ath plann




Download Pdf - The PPT/PDF document "UsingSk eletonsforNonholonomicP athPlann..." is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.



Presentation on theme: "UsingSk eletonsforNonholonomicP athPlanningamongObstacles BrianMirtic JohnCann ComputerScienceDivision Universit yofCalifo rnia Berk eley CA Abstract This p ap er describ es a pr actic al p ath plann"— Presentation transcript:


Page 1
UsingSk eletonsforNonholonomicP athPlanningamongObstacles BrianMirtic JohnCann ComputerScienceDivision Universit yofCalifo rnia Berk eley ,CA94720 Abstract This p ap er describ es a pr actic al p ath planner for nonholonomic r ob ots in envir onments with obstacles. The planner is b ase d on building a one-dimensional, maximal cle ar anc e skeleton thr ough the c on gur ation sp ac e of the r ob ot. However r ather than using the Eu- clide an metric to determine cle ar anc e, a sp cial metric which c aptur es information ab out the nonholonomy of the r ob ot is use d. The r ob

ot navigates fr om start to go al states by lo osely fol lowing the skeleton; the r esult- ing p aths taken by the r ob ot ar e of low \c omplexity." We describ e how much of the c omputation c an b e done o -line onc e and for al l for a given r ob ot, making for an ecient planner. The fo cus is on p ath planning for mobile r ob ots, p articularly the planar two-axle c ar, but the underlying ide as ar e quite gener al and may b applie d to planners for other nonholonomic r ob ots. In troduction With an abundance of theoretical results and al- gorithms already existing for solving the

classical pi- ano mo er's problem, researc h has recen tly fo cused on solving more general v ersions of this problem. Promi- nen t among these generalizations is path planing for nonholonomic rob ots. A useful tec hnique in attac k- ing the piano mo er's problem is to transform it from ph ysical space to con guration space, replacing the rob ot with a single p oin t in con guration space and eac h obstacle with a corresp onding con guration ob- stacle. Planning for the p oin t rob ot in con guration space is equiv alen t to planning for the actual rob ot in ph ysical space [17 ]. An

underlying assumption in the classic v ersion of this problem is that (ignoring ob- stacles) the p oin t can lo cally mo ein an y direction of con guration space, that is it can mo eat an yv elo cit in the tangen t space of the manifold of con gurations. When planning for nonholonomic rob ots, this assump- tion m ust b e relaxed, greatly increasing the dicult of the problem. Supportedb yNSFGraduateF ello wship,Da vidandLucile ac ardF ello wshipandNationalScienceF oundationPresiden- tialY oungIn estigatorAw ard(#IRI-8958577). Supportedinpartb yDa vidandLucileP ac ardF ello wship

andNationalScienceF oundationPresiden tialY oungIn estiga- torAw ard(#IRI-8958577). 1.1 ath complexit Muc h recen t researc h has fo cused on imp ortan t the- oretical considerations suc h as exactly when a system is completely con trollable [1, 3]. A completely con- trollable rob ot can reac hev ery p oin t in free con gu- ration space within the same connected comp onen tof its curren t con guration. Assuming a completely con- trollable rob ot, the ob ject is to plan nice tra jectories bet een start and goal con gurations whic ha oid ob- stacles. \Nice" is of course a rather nebulous

concept; a somewhat b etter description w ould b e tra jectories of lo w complexit . This b egs the question of ho wto measure path complexit . A complexit y measure for general systems is discussed later; for no w consider a sp eci c example, the t o-axle car in the plane. It seems in tuitiv e that the \simplest" path b et een o con gurations migh t b e the shortest suc h path, so one ma y lo ok to the shortest paths for inspira- tion in de ning complexit . Reeds and Shepp ha pro en that in the absence of obstacles the shortest path for the car rob ot nev er con tains more than t rev ersals,

whic h are places where the car c hanges di- rection from forw ard to bac kw ard or vice-v ersa [20 ]. As an y practiced parallel park er can attest, ha ving to c hange directions man y times do es not mak e for a desirable path. Since it is also desirable to driv as little as p ossible, path complexit y for the car lik rob ot increases with path length and n um berofrev er- sals. Our planner tries to nd paths whic h minimi ze the path complexit , although no guaran tees on opti- malit y are made since complexit y remains a somewhat qualitativ e concept. o illustrate this in terpretation of

complexit , con- sider the follo wing metho d of planning feasible paths for the car (a feasible path is one whic h ob eys the nonholonomic constrain ts of the system). First a non- feasible path is planned using an y algorithm for the classic piano mo er's problem. This nonfeasible path ma y then b e appro ximated to arbitrary closeness b ya feasible path. The standard planner has no kno wledge of the nonholonomic constrain ts of the rob ot, and the path it generates ma y not b e a go o d one to appro x- imate with a feasible path. The left of gure 1 illus- trates this; the resulting feasible

path is quite complex in olving man y rev ersals. On the other hand, the righ of gure 1 sho ws a less complex path. Qualitativ ely this app ears to b e a m uc h b etter path.
Page 2
Figure 1: eft: Path of high c omplexity involving many eversals. R ight: Simpler (and shorter) p ath. 1.2 Ov erview of the algorithm The basic idea of the algorithm is as follo ws. F or a giv en en vironmen t of the rob ot, a one dimensional subset (called a sk eleton or roadmap) of con guration space is constructed. This subset is path-connected within connected comp onen ts of con guration space, and

the same sk eleton ma y b e used rep eatedly for solv- ing path planing problems in the giv en en vironmen t. o nd a path b et een start and goal con gurations and , paths are found whic h link and to the sk ele- ton. Then a path is found whic h connects the t o link p oin ts of the sk eleton. This latter path is computed b mo ving roughly \along" the sk eleton. Once the sk ele- ton has b een constructed, path planing is m uc h sim- pler since only a one dimensional set of con gurations is in olv ed. F urthermore, the metho d of constructing the sk eleton insures that the paths whic h follo w

it will b e of lo w complexit y if at all p ossible. or a giv en rob ot, a large part of the w ork can b e done o -line so that sk eletons for new en vironmen ts can b e com- puted more rapidly . The algorithm is curren tly b eing implemen ted for the planar t o-axle car. Relatedw ork Latom b e presen ts an excellen t surv ey of the prob- lems and algorithms of rob ot motion planning, includ- ing nonholonomic motion planning [12 ]. Laumond p erformed some the earliest w ork in the area of nonholonomic motion planning, planning for mobile rob ots sub ject to one nonholonomic constrain in the

presence of obstacles [14 ]. ortune and Wil- fong dev elop ed a decision algorithm for determining the existence of a feasible path under giv en condi- tions, although the algorithm do es not nd the ac- tual path [6]. Barraquand and Latom be ha e attac ed nonholonomic motion planing from a di eren t angle; their planner nds a path b y p erforming a systematic searc h through the the con guration space, using p o- ten tial eld metho ds to guide the searc h [1]. They nd paths for car and trailer-lik e rob ots (in the presence of obstacles) whic h minimi ze the n um b er of maneuv ers required,

and their planner is able to generate paths for rob ots with relativ ely man y degrees of freedom. Jacobs and Cann yha e giv en an algorithm for path planning for a car-lik e rob ot amidst obstacles based on reducing the set of a smo oth tra jectories to a su- cien t set of canonical tra jectories [7 , 8]. The building blo c ks for their tra jectories are \Dubins paths" while our tra jectories for the car-lik e rob ot are constructed from \Reeds-Shepp paths," the ma jor di erence b e- ing that the latter ma y con tain rev ersals along the path b et een t o p oin ts. Murra y and Sastry dev el-

op ed a metho d of steering systems with nonholonomic constrain ts b et een arbitrary con gurations using si- usoidal con trol inputs [19 ]. Although their metho d assumes no obstacles, it pro vides a simple and e- cien t means for generating paths and can certainly b e incorp orated in to a planner whic h tak e obstacles in to accoun t. Our w ork most closely parallels the recen tw ork of Jacobs, Laumond, and T aix. They ha e dev el- op ed a t o-stage planning strategy for the car-lik rob ot [9 ]. First a path is found for the asso ciated holonomic system (obtained b y remo ving the nonholo-

nomic constrain ts), and then this initial path is ap- pro ximated b y feasible path segmen ts. Our planner is also based on follo wing a nonfeasible path (a p ortion of the sk eleton). They also in tro duced the notion of a metric based on shortest paths and the corresp ond- ing \Reeds-Shepp ball," whic h serv ed as inspiration for our planner. Shortestfeasiblepath(SFP)metric 3.1 De nition of the SFP metric The idea of reducing con guration space to a smaller dimensional subset still complete for path planning is not new. Roadmaps [4] and V oronoi di- agrams ha e b een used previously to

simplify path planning. What is no el ab out our approac h is the particular sk eleton whic h is constructed and ho witis built. It is a maxima l clearance sk eleton based on a sp ecial metric that mak es it esp ecially suited to non- holonomic motion planning. The sk eleton is a set of p oin ts whic h are maxim all clear from t o or more obstacles. Using the car as an example, it is easy to see wh y the standard Euclidean metric giv es a somewhat distorted view of equidistance for nonholonomic systems (see gure 2). Under the Euclidean metric, the car is the same distance from eac h of the t o

con guration obstacles. Ho ev er, it is actually harder for the car to mo e in the \sidew ys" direction. When considering only feasible paths, the car m ust driv e a farther distance in order to reac obstacle than to reac h obstacle . So in some sense obstacle is further a y than obstacle . This is the essen tial idea b ehind our metric, the shortest feasible path (SFP) metric. De nition. Let b e the con guration space for a nonholonomic rob ot . The SFPmetric is a function whic h assigns to eac h pair of p oin ts ;p the (arc) length of the shortest fe asible path b et een the t o con

gurations. The real n um ber ;p ) is called the SFPdistance bet een p oin ts and
Page 3
Figure 2: When c onsidering only fe asible p aths obsta- cle is closer. 3.2 Justi cation for using SFP metric Giv en start and goal con gurations represen ted b p oin ts and in con guration space, a path b et een and is found b y concatenating three subpaths, all of whic hm ust of course a oid obstacles. First sub- paths are found from to and to , where and are p oin ts on the sk eleton. Then a subpath m ust b e found whic hmo es \along" the sk eleton, linking and . In general, this last p ortion

of the path forms the bulk of the complete path. In general it is imp ossible to mo ebet een p oin ts and without lea ving the sk eleton b ecause the sk ele- ton ma y sometimes lead in infeasible directions. Al- though the sk eleton lies completely in free space, when the rob ot steps o the sk eleton obstacle a oidance b e- comes an issue. De nition. Let b e the con guration space for a nonholonomic rob ot , and The shortestfeasiblepathballofradius cen tered at is de ned as SFP c; r )= c; q If Euc c; r ) is the standard Euclidean ball of radius ab out , then SFP c; r B Euc c; r ) since the

SFP distance b et een t o p oin ts is at least as large as the Euclidean distance. The basic idea is to co er the section of the sk eleton from to with SFP balls whic h lie completely in free space in order to mak e a series of \jumps" b et een the p oin ts ;p ;:::;p ;p on the sk eleton. If the rob ot is curren tly at con guration then the next jump p oin +1 is determined b y nding an SFP ball SFP ;r ) lying completely in free space, and then ho osing the p oin t in the ball furthest along the sk ele- ton path as +1 . The shortest path from to +1 has a length less than since +1 2B SFP ;r ). An

path with length smaller than oids all con gu- ration obstacles b ecause the set of all reac hable con- gurations SFP ;r ) lies completely in free space. urthermore, the path from to +1 is a shortest path and therefore of lo w complexit .F or example the shortest paths of the planar t o-axle car in olv no more than v e straigh t line or curv ed segmen ts, and no more than t o cusps (c hanges of directions). Since the complexit y of the path is xed for the path b et een successiv e jump p oin ts, the complexit of the nal path from to is prop ortional to the um b er of jump required. The n um b

er of jumps can b e minimi zed b y making the SFP balls SFP ;r )as large as p ossible. As an example, consider the planar /2 Figure 3: eft: Portion of a skeleton thr ough c on g- ur ation sp ac e with start and go al p oints. R ight: R e- sulting p ath thr ough physic al sp ac e when the skeleton is c over d with smal l SFP b al ls. o-axle car with a p ortion of a sk eleton sho wn in the left of gure 3, and supp ose the sk eleton segmen from to is co ered with small SFP balls. Then the jump p oin ts are closely spaced, and the resulting path in ph ysical space w ould lo ok lik e the one in the

righ t of the gure. Note that the paths b et een the successiv e jump p oin ts are simple, but it is necessary to concatenate so man y suc h paths together that the nal total path is quite complex. In con trast if the sk eleton path from to is co ered with larger balls less jumps are required resulting in nicer path suc has those sho wn in gure 4. The path on the left of gure 4in olv es t o jumps to bring the rob ot from to while the one on the righ t brings the rob ot from to in a single jump. This latter case w ould o ccur if the SFP ball at as large enough to con tain .F or the ph ysical

paths in gure 4 the rob ot's con guration deviates farther from the sk eleton, but this is of no consequence since the actual path is still guaran teed to a oid obstacles. Figure 4: esulting p aths when the skeleton is c over with lar ger SFP b al ls al lowing for fewer jumps inste ad of many smal ler ones. It should no w b e clear wh y the sk eleton built as a set of maxim al clearance p oin ts under the SFP met- ric is useful in planning paths. The SFP balls ab out p oin ts on the sk eleton are as large as p ossible, and few er jumps are required to mo e along the sk eleton resulting in less

complex nal paths. Bellaic he has used SFP balls to determine lo er b ounds on path complexities in the presence of obstacles [2]. Essen- tially , the sk eleton is designed so that the complexities of the resulting paths are as close as p ossible to the lo er b ounds.
Page 4
ComputationoftheSFPdistance Computing the SFP distance b et een the rob ot's curren t con guration and a con guration obstacle is an imp ortan t op eration used often in constructing the sk eleton. The prosp ect of implem en ting this op era- tion ecien tly initially app ears dubious, since nding shortest

feasible paths for ev en a simple nonholonomic rob ot lik e the t o-axle car in the plane is non-trivial. orse y et the shortest paths for some rob ots are un- kno wn, and some appro ximation or searc hb y brute force en umeration of paths is required. These facts suggest computing a lo okup table o -line for deter- mining the SFP distance b et een t o p oin ts in con- guration space. Sev eral prop erties of suc hasc heme are desirable: 1. F or eciency , computing the actual con guration obstacles should b e a oided. The calculation of the SFP distance b et een the rob ot's con gura- tion and

a con guration obstacle m ust only in- olv e the ph ysical obstacle in ph ysical space. 2. The table should b e computed only once for a particular rob ot. Placing the rob ot in a new en- vironmen t should not require a new table. 3. The dimension of the table should b e as small as p ossible. Since is a map from ,a naiv e approac hw ould in olv ea 2 -dimensional table where is the dimension of the con gura- tion space. This is to o large for an y practical rob ot. 4. There should b e a metho d of planning paths in en vironmen ts larger than the size of the table. Generally it is not practical

to build a table large enough to co er the largest exp ected en vironmen or to imp ose size constrain ts on the en vironmen t. Our metho d satis es all of these desired prop erties. In the follo wing discussion and represen t the ph ysi- cal and con guration space of the rob ot. 4.1 able reduction via rigid b o dy motions Supp ose the SFP distance b et een a start con gu- ration and a goal con guration is to b e found. If a rigid b o dy motion is applied to the t o rob ot con g- urations and to obtain new con gurations and , then s; g )= ;g ) (see gure 5) In fact ap- plying the rigid b o dy

motion to the the trace in of the shortest feasible path b et een con gurations and yields the trace in of the shortest feasible path bet een and The rigid b o dy motions for and are giv en b SE (2) and SE (3) SO (3) resp ec- tiv ely . The lo okup table is simpli ed if a rigid b o dy motion that brings the start con guration to some home p osition is alw ys applied prior to lo okup. Then the domain of the table need not b e but only C=SE )) where is the dimension of ph ysical space. (T ec hnically , the domain is , where is the natural em b edding of C=SE )in .) or example, with the t o-axle

car in the plane b oth and SE (2) are and so C=SE (2) is a single p oin t. Hence ev ery initial start con guration s g Figure 5: Applying a rigid b dy motion to the start and go al c on gur ations do es not change the SFP distanc etwe en the c on gur ations. can b e transformed via rigid b o dy motion to the same (arbitrary) home con guration, suc h as the car p osi- tioned at the origin and p oin ting along the p ositiv -axis. The domain of the lo okup table is isomorphic to , since w e only need to compute the SFP distance bet een this home con guration and an y other con- guration. An

analogous situation arises if the rob ot is an aircraft with since the space of rigid b o dy motions SE (3) is again equal to the con gura- tion space . If the rob ot is the planar car with trailers, a sligh tly di eren t situation arises. The con- guration of the rob ot ma y b e sp eci ed b y the p osition and orien tation of the lead car plus the orien tation of eac h trailer, so ::: +1 factors). Th us C=SE (2) = ::: factors), whic h is no longer a single p oin t. It is not p ossible to bring all starting con gurations to a single home con guration using rigid b o dy motions. Instead the

home p osition is actually a non-trivial subspace of con guration space. The car can not alw ys b e brough t to the origin with the trailers lined up along the negativ -axis, ho ev er it is alw ys p ossible to bring the lead car to the origin p oin ting along the p ositiv -axis, while lea ving the relativ e orien tations of the trailers unc hanged. The p oin t is that while applying a rigid b o dy motion to the start and goal con gurations do es not a ect the SFP distance, other motions suc h as straigh tening out the trailers do. 4.2 Computing SFP distances in ph ysical space ttac king the

problem of computing SFP distances in ph ysical space leads to a further reduction in the size of the lo okup table. Let b e an obstacle in ph ysical space and CO the corresp onding con gura- tion obstacle. The SFP distance b et een the rob ot at con guration and the con guration obstacle CO is de ned as SFP p; C O ) = min CO p; q Consider a single p oin on the ph ysical obstacle , and let b e the con guration obstacle of this single p oin t. Clearly CO
Page 5
No w de ne a map p; w ) = min 2C p; q The SFP distance b et een the rob ot's con guration and the con guration obstacle CO ma

y then b e de- ned as SFP p; C O ) = min p; w This is equiv alen t to the previous form ulation, ho w- ev er in the second form ulation the minim izati on is o er the p oin ts in the ph ysical obstacle , not the con gu- ration obstacle CO . The lo okup table need not store the v alue of er p oin ts in ( C=SE )) , but only the v alue of er p oin ts in ( C=SE )) . The size of the lo okup table is reduced and the SFP distance computations are p erformed using ph ysical obstacles rather than con guration obstacles. 4.3 Building the table o -line Eac h table en try corresp onds to a particular home

con guration and a particular p oin in ph ysical space, and con tains the length of the shortest feasible path whic hmo es the rob ot from con guration to a con guration in tersecting . Hence it is necessary to minim ize o er all goal con gurations for whic h the rob ot in tersects the p oin in ph ysical space. As an example, consider computing p; w ) for the o-axle car rob ot. F or this rob ot C=SE (2) is a single p oin t, so is the same for all table en tries (supp ose for example that is the con guration with the car at the origin p oin ting along the p ositiv -axis). F or a xed nal orien

tation , the shortest path m ust b e found from to a con guration at whic h the rob ot in tersects the p oin This problem can b e trans- formed b y gro wing the obstacle and shrinking the rob ot to a p oin t. Ho ev er since the obstacle itself is simply a p oin t, the gro wn obstacle is just the \nega- tiv e" of the shap e of the rob ot ( gure 6). The problem b ecomes one of nding the shortest feasible path that brings the p oin t rob ot to some p oin t on the edge of the gro wn obstacle. This pro cedure is p erformed for eac nal orien tation . Minimizing o er all nal orien ta- tions yields

the v alue to b e stored in the table, that is s; w ). Clearly the calculations do not in olv e the obstacles in the rob ot's en vironmen t, and th us ma ybe p erformed completely o -line. or more complex rob ots the minimi zation problem b ecomes higher dimensional, but the basic approac his the same. F or a car with one trailer the minim zation ould b e p erformed o er all pairs ( ; ) describing the nal orien tations of the car and trailer, rather than just o er a single orien tation parameter in the previ- ous example. Nonetheless these are still \one time" calculations for a giv en rob

ot. 4.4 Appro ximating SFP distances outside table The requiremen t that the table b e of nite size re- mains. By bringing the rob ot to the origin of ph ysical space (but not con guration space) via rigid b o dy mo- tions, the table can b e constructed o er ( C=SE )) where is some compact region con taining the origin. The nite table is then used for computing Figure 6: The shade dr gions ar e the r esult of gr owing the p oint obstacle and shrinking the r ob ot to a p oint. eft: =0 .R ight: = SFP distances b et een the rob ot and ne arby obstacles. Distan t obstacles fall outside the range

of the table, ho ev er the SFP distances to these ma y b e appro xi- mated b y a simpler metric. F or example the standard Euclidean metric is a v ery go o d estimate of the ac- tual SFP metric for the planar t o-axle car at large distances. Computing SFP deriv ativ es The sk eleton is built b y incremen tally extending a path through con guration space. During this pro cess it is often necessary to mo e directly a y from an ob- stacle or to mo e suc h that the distances to the t or more closest obstacles c hange b y the same amoun t. or suc h op erations it is useful to kno who w the dis-

tance b et een a giv en con guration and a con gu- ration obstacle CO hanges with resp ect to the co or- dinates parameterizing . Recall the distance function SFP CO! where CO is the set of con gu- ration obstacles. F or a giv en con guration and con guration obstacle CO 2CO SFP s; C O )isthe length of the shortest feasible path from to a con- guration CO .If is sp eci ed b y the co ordinate ariables ;x ;:::;x , what is desired are the v alues @x SFP s; C O ;i =1 :::n . As with the distance cal- culations it is desirable to compute these deriv ativ es using only the ph ysical obstacles rather

than com- puting the con guration obstacles. F or eciency the deriv ativ es should also b e computed from a lo okup ta- ble computed o -line once for a giv en rob ot. W eno analyze ho w this ma y b e done. Let b e the rob ot con guration, CO a con gura- tion obstacle, and CO the closest p oin tonthe con guration obstacle to under the SFP metric. If is displaced a distance the closest p oin ma ymo along the surface of CO .Ho ev er since is the clos- est p oin ton CO to , the rst order deriv ativ es of as mo es along an y curv e on the surface of CO are nondecreasing (the deriv ativ es are zero

along an curv e on the stratum con taining ). Th us the rst or- der deriv ativ es of SFP ma y b e computed assuming is xed on CO , that is @x SFP s; C O )= @x s; g The righ t hand side of the ab o e equation is easy
Page 6
to compute o -line when the distance table is gener- ated. Let b e the ph ysical obstacle corresp onding to CO and the ph ysical p oin ton corresp onding to 2C ) for some ). Clearly is the closest p oin ton )to since CO . The clos- est p oin tto on ) is kno wn at the time s; w is computed, so the deriv ativ es ma ybe ev aluated at this time. In most cases closed

form deriv ativ es are not a ailable and the deriv ativ es m ust b e ev aluated umerically . The deriv ativ es are stored in the same table as the distances, so eac h table en try actually consists of a distance and deriv ativ es where is the dimension of con guration space. Note that this sc heme allo ws computing the deriv a- tiv es of the SFP distance to a con guration obstacle in ph ysical space; the con guration obstacle is nev er actually computed. The deriv ativ es tak en from the table are deriv ativ es with resp ect to the con guration co ordinates when the rob ot is at some home con

guration. When a rigid b o dy motion is applied to the actual con guration bring it to a home con guration , the deriv ativ es computed at ust b e mapp ed bac k to the corre- sp onding deriv ativ es at . This is not a problem since the rigid b o dy transformation bringing to estab- lishes an isomorphism b et een their tangen t spaces. Outside of the range of the table the Euclidean met- ric is a go o d appro ximation of the SFP metric. There- fore outside of the range of the lo okup table the deriv a- tiv es of SFP s; C O ) are estimated b y their Euclidean analogues. ThePlanningAlgorithm Once

the metric to b e used for building the maxi- um clearance sk eleton and its implemen tation ha b een de ned, the planning algorithm b ecomes an ap- plication of Cann y's general roadmap algorithm [4 ]. eno w giv e a brief description of the algorithm. The roadmap algorithm is similar to a plane sw eep algorithm. One of the co ordinates of con guration space is c hosen as a sw eeping direction, and successiv slices of con guration space are tak en, eac h slice b e- ing a (h yp er) plane through con guration space with the sw eeping co ordinate held xed. F or example, the car is describ ed b y

three parameters, a p osition ( x; y in the plane and an orien tation .Ph ysical space is 2-dimensional. If is c hosen as the sw eeping direc- tion, slices tak en through con guration space w ould b e planes of constan alue. Another k ey concept of the roadmap algorithm is that of a silhouette curv e. De nition. Within a giv en slice of con guration space, let b e a p oin t of maxim al clearance from the obstacles, i.e. mo ving a y from in an y direc- tion within the slice decreases the SFP distance to the nearest obstacle. P oin ts corresp onding to ma ybe lo cated in successiv e slices as

the sw eeping co ordinate is v aried; this lo cus of p oin ts is the silhouettecurv passing through In tuitiv ely , a silhouette curv e is generated b y rst nding a lo cal maxima of the distanc etone ar est ob- stacle function within a slice, and then tracing this maxim a through the slices as the sw eeping co ordinate is v aried. There is also the notion of a critical slice where t o or more previously disconnected regions of free space are joined, or a single region splits in to dis- connected regions. Suc h slices in tersect more than one silhouette curv e. The o erall planning algorithm is

no w describ ed. 1. Compute the v alues of and the partial deriv a- tiv es of SFP er a compact region ab out the origin of ph ysical space; store these v alues in a lo okup table. 2. Giv en a start con guration and a goal con- guration nd curv es to p oin ts and on a sk eleton silhouette curv e. 3. T race the unexplored silhouette curv es in b oth directions. If a complete path along silhouette and linking curv es is found from to ,goto step 5. 4. In a critical slice lo cate p oin ts on unexplored sil- houette curv es and go to step 3. 5. Plan a path from to y jumping along the curv e linking

to , jumping along the con- structed sk eleton from to , and nally jumping along the curv e linking to The rst step is p erformed only once (o -line) for a particular rob ot. The nal step is p erformed after the necessary p ortions of the sk eleton ha e b een com- puted. Steps 2 through 4 are a v ersion of the roadmap algorithm; sk eleton construction o ccurs during these steps. These steps are no w describ ed using the planar o-axle car as an example. The car's con guration is parameterized b yapo- sition ( x; y ) and an orien tation ; assume that is hosen as the sw eeping direction. Giv en

an -slice through con guration space it is simple to nd a p oin in the slice lo cated on a silhouette curv e. Beginning at some initial p oin t in the slice, w e incremen tally mo ea y from the nearest obstacle or obstacles while remaining in the slice. Ev en tually a p oin t will b e reac hed from whic h it is imp ossible to mo e without mo ving closer to one of the nearest obstacles. Suc a p oin t is maxima ll y clear from the obstacles and is th us b y de nition on a silhouette curv e. This is the pro cedure that o ccurs in step 2 ab o e. Note that the deriv ativ es of the SFP function are

used in mo ving y from the nearest obstacle(s). In step 3 silhouette curv es are \traced." Once a p oin t on a silhouette curv e is lo cated, the silhouette curv e can b e traced b y taking successiv -slices sep- arated b y some small  . Supp ose =( x; y ;  )is a p oin t on a silhouette curv e in the curren -slice. T nd the p oin =( + x; y + y;  + ) on the silhouette curv e in the next slice,  and  ust be c hosen so that the distances to all of the nearest obstacles c hange b y the same amoun t. An initial ap- pro ximation to can b e obtained b yc ho osing  and  suc h that the inner

pro ducts of the v ector ( x; y; ) with the gradien ts of the distance func- tions to the nearest obstacles are all equal. Again the deriv ativ es of the SFP function are required. Silhou- ette curv es are traced in b oth directions b ysw eeping
Page 7
in b oth directions from the curren t slice, i.e. taking to b e p ositiv e and negativ e. If after tracing the silhouette curv es from and there is no path whic h links the t o p oin ts, new sil- houette curv es m ust b e found and traced. The searc for unexplored silhouette curv es o ccurs in a critical slice, and the pro cedure is

similar to that of step 2; w lo ok for new lo cal maxim a of the distanc etone ar est obstacle function within the critical slice. After p oin ts on unexplored silhouette curv es are found, the pro cess is rep eated starting at step 3. Steps 3 and 4 are re- p eated as long as necessary ,un til a complete path is found from to The ab o e ideas are only an o erview of the roadmap algorithm. F or a more complete description of the algorithm and ho wit ma y b e sp ecialized for computing a roadmap using only the ph ysical space obstacles w e refer the reader to [5 ]. The2-axlecar&Reeds-Shepppaths

The tec hniques describ ed in this pap er are curren tly b eing applied to the planar t o-axle car. Planning for this system exhibits the same basic diculties inher- en t in more complex nonholonomic systems, and the shortest paths for the planar car ha e b een studied extensiv ely . In fact Reeds and Shepp ha e completely haracterized the shortest paths b et een an yt o con- gurations of the planar car, and an algorithm exists for computing these paths [20 ]. Brie y , the shortest paths consist of v eorfew er path segmen ts, eac h seg- men t b eing a straigh t line or a turn with a radius of

curv ature equal to the minim um turning radius of the car. En umerating o er the v arious p ossibilities suc has turn direction and order of segmen ts pro duces fort y- eigh t p ossible path t yp es. Curren tly eac h of these path yp es m ust b e \tried" when searc hing for the shortest paths b et een con gurations and . Although not all fort y-eigh tt yp es can mo e the car b et een a giv en and , in general man y of them can and the only y to nd the shortest is to try (compute) them all. An in teresting problem is to try to initially reduce the set of fort y-eigh t paths to a smaller set of

candidates to a oid computing so man y paths, but this problem is not the fo cus of this researc h. or our planner the imp ortan t consequence of Reeds and Shepp's w ork is that the SFP metric is com- putable for the planar car. W e receiv ed a subroutine for computing Reeds-Shepp paths from Mic hel T aix, who wrote the C co de as part of a complete plan- ner for car-lik e rob ots [16 ]. Figure 7 sho ws the lev el curv es of the lo okup table for the car-lik e rob ot (in generating this table the car w as assumed to b e a p oin t, although this assumption is not necessary). In- tuitiv ely it

should b e easier for the car to mo e forw ard or bac kw ard (along the -axis) than sidew ys (along the -axis), and this in tuition is v eri ed b y the lev el curv es. Note the lev el curv es b ecome more and more circular as the distance increases, justifying the claim that at large distances the Euclidean distance closely appro ximates the SFP distance. Generalizingtoothersystems The planar t o-axle car is a fairly w ell understo o d nonholonomic system, at least as far as shortest paths Figure 7: evel curves of for the two-axle planar ar. The r gion shown is a 10 10 squar ec enter dat the

origin, with c ontours 0.5 units ap art. are concerned. urthermore there is a clear idea of what paths of lo w complexit y should lo ok lik e; they should b e short and also ha e as few rev ersals as p ossible. or more complex nonholonomic systems the shortest paths b et een con gurations ma y not b e kno wn, and the idea of what constitutes a simple path ma y also b e unclear. W eno w discuss generalizing our algorithm to suc h systems. 8.1 Relaxing the SFP assumption or an aircraft t yp e rob ot nding the shortest paths bet een t o con gurations (p ositions and orien tations in ) is still an

op en problem. Planning for suc systems using the sk eleton metho d is still p ossible with a sligh t mo di cation. Ev en when the shortest paths are not kno wn, a \library" of paths can b e found whic are able to steer the system (non-optimall y) b et een an yt o con gurations. F or example, the path library for the aircraft w ould include v arious lo oping paths for bringing the aircraft to the same p oin t in space but with a di eren t orien tation. Using this smaller set of library paths, an algorithm can b e constructed for nding a path b et een an yt con gurations. Using rigid b o dy

motions to bring the rob ot to a home con guration mak es it necessary to nd paths only b et een the home con guration(s) and p oin ts in a compact set ab out the origin in ph ysical space. Let b e a ball ab out the origin. Mo ving the rob ot from a home con guration to a p oin tin ma in olv e a path whic h temp orarily passes outside of Ho ev er, a larger ball can b e found suc h that a path from a home con guration to an y p oin tin nev er passes outside of . Note that the paths m ust come from the pre-de ned library of paths. If the library of paths consists of shortest paths, then the ball

coincides exactly with the ball In the more general setting when the library paths are not necessarily shortest paths is strictly larger than . The metric for constructing the sk eleton should b e based on the balls, since obstacle a oidance can
Page 8
only b e guaran teed b y insuring the ball lies com- pletely in free space. When mo ving along the sk eleton, the balls are used. A path b et een sk eleton jump p oin ts and +1 will a oid obstacles if the p oin +1 lies within the ball cen tered at p oin . All of this rests on the fact that the paths b et een jump p oin ts will come

from the same library of paths used to con- struct the distance table for the rob ot. An yc hoice of library paths is v alid, as long as the same ones are used in the table building and planning stages. Of course the planner will b e to o conserv ativ e if the balls are m uc h larger than the balls, k eeping the rob ot farther than necessary from the obstacles. judicious c hoice of library paths will result in smaller discrepancy b et een and balls, resulting in a b etter planner. A library consisting of shortest paths is optimal in this sense. 8.2 De ning path complexit y in general De ning

path complexit y for the planar car is fairly straigh tforw ard. In this case path complexit yisanin- creasing function of the arc length of the path and the um b er of rev ersals along the path. Ho ev er this ex- ample also sho ws that the concept of what mak es a desirable (simple) path is dep enden t on the particular system. In the case of the car, there are t o con trol inputs: the forw ard v elo cit y and the steering angle of the fron t wheels. Changing the sign of the forw ard elo cit y input results in a rev ersal for the car, whic is to b e a oided. Y et c hanging the sign of the

steer- ing angle only causes the car to mo e from (sa y) a left turn path segmen t to a righ t turn path segmen t. Qualitativ ely , this latter o ccurrence is not as unde- sirable as a c hange of direction. A path con taining sev eral left-righ t switc hes seems b etter than a path of the same length whic h con tains sev eral forw ard- bac kw ard switc hes. Hence it seems that path com- plexit y is closely tied to the sp eci c system at hand. Nonetheless, it is still p ossible to de ne some more general measure of path complexit . La erriere and Sussman ha e sho wn that a nilp oten tizable

system can b e steered b et een t o con gurations b y follo wing a nite n um b er of subpaths, where only one of the con- trol inputs is non-zero o er an y one subpath [11 ]. The um b er of subpaths is b ounded b y some constan for the particular system. F or a nonholonomic rob ot represen table as a nilp oten tizable system, one can th us ho ose the path library to comprise all paths whic are comp osed of or few er suc h path segmen ts. The um b er of subpaths b et een successiv e jump p oin ts on the sk eleton will then b e b ounded, and the algo- rithm will generate simple paths under this

notion of complexit Ac kno wledgemen ew ould lik e to thank Mic hael T aix for the co de to compute Reeds-Shepp aths. References [1] J. Barraquand and J-C. Latom b e. On nonholonomic mobile rob ots and optimal maneuv ering. In 4th International Sym- osium on Intel ligent Contr ol , Alban , NY, 1989. [2] A. Bellaic he. Lo er b ounds on path complexit y with obstacles. orkshop on nonholonomic planning, T oulouse, F rance, July 1991. P aris VI I. [3] A. Bellaic he, J-P . Laumond, and P . Jacobs. Con trollabilit yof car-lik e rob ots and complexit y of the motion planning problem with

non-holonomic constrain ts. In International Symp osium on Intel ligent c ontr ol , Bangalore, India, 1991. [4] J. F. Cann The Complexity of R ob ot Motion Planning .M. I. T. Press, Cam bridge, 1988. [5] John F. Cann y and Ming C. Lin. An opp ortunistic global path planner. In International Confer enc eonR ob otics and utomation , 1990. [6] S. F ortune and G. Wilfong. Planing constrained motion. In STOCS , pages 445{459, Chicago, IL, Ma y 1988. Asso ciation for Computing Mac hinery [7] P . Jacobs and J. Cann . Planning smo oth paths for mobile rob ots. In International Confer enc eon R ob

otics and A u- tomation , pages 2{7. IEEE, Ma y 1989. [8] P . Jacobs and J. Cann . Robust motion planning for mobile rob ots. In International Confer enc eon R ob otics and A u- tomation . IEEE, 1990. [9] P . Jacobs, J-P . Laumond, and M. T aix. A complete iterativ motion planner for a car-lik e rob ot. In Journe es Ge ometrie lgorithmique , INRIA, 1990. [10] P . Jacobs, J-P . Laumond, M. T aix, and R. Murra .F ast and exact tra jectory planning for mobile rob ots and other sys- tems with non-holonomic constrain ts. T ec hnical Rep ort 90318, LAAS/CNRS, T oulouse, F rance, Septem b er 1990.

[11] G. La erriere and H. J. Sussman. Motion planning for con trol- lable systems without drift: A preliminary rep ort. T ec hnical Rep ort SYSCON-90-04, Rutgers Cen ter for Systems and Con- trol, June 1990. [12] J-C. Latom be. ob ot Motion Planning . Klu er Academic Publishers, Boston, 1991. [13] J-P . Laumond. F easible tra jectories for mobile rob ots with kinematic and en vironmen t constrain ts. In Intel ligent A u- tonomous Systems . North Holland, 1987. [14] J-P . Laumond. Finding collision-free smo oth tra jectories for a non-holonomic mobile rob ot. In International Joint Confer- enc

eonA rti cial Intel ligenc , pages 1120{1123 , 1987. [15] J-P . Laumond. Nonholonomic motion planning v ersus con- trollabilit y via the m ultib o dy car system example. T ec hnical Rep ort ST AN-CS-90-1345, Departmen t of Computer Science, Stanford Univ ersit , Octob er 1990. (preprin t). [16] J-P . Laumond, M. T aix, and P . Jacobs. A motion planner for car-lik e rob ots based on a mixed global/lo cal approac h. In IEEE International Workshop on Intel ligent R ob ots and Systems , Japan, 1990. [17] T. Lozano-P erez and M. A. W esley . An algorithm for planning collision-free paths among p

olyhedral obstacles. Communic a- tions of the A CM , 22(10):560{57 0, 1979. [18] R. M. Murra y and S. S. Sastry . Grasping and manipulation using m ulti ngered rob ot hands. In R. W. Bro c ett, editor, ob otics: Pr dings of Symp osia in Applie d Mathematics, olume 41 , pages 91{128. American Mathematical So ciet 1990. [19] R. M. Murra y and S. S. Sastry . Steering nonholonomic systems using sin usoids. In IEEE Contr ol and De cision Confer enc 1990. [20] J. A. Reeds and L. A. Shepp. Optimal paths for a car that go es b oth forw ards and bac kw ards. Paci c Journal of Math- ematics , 145(2),

1990.