# ÀÝ Ö ÅÓØ ÓÒ ÈÐ ÒÒ Ò ÓÓÖ Ò Ø Ò ÌÛÓ ÅÓÚ Ò ÑÓÒ ÈÓÐÝ ÓÒ Ð Ç Ø Ð Ò Ø ÈÐ Ò Ë À Ö Ò Ò À ÐÔ Ö Ò Ë ÓÓÐ Ó ÓÑÔÙØ Ö Ë Ò Ì Ð Ú Ú ÍÒ Ú Ö ØÝ Ì Ð Ú Ú Á Ö Ð Ò ºØ Ùº º

## Full text

(1)

Polygonal Obsta les in the Plane ?

ShaiHirs handDanHalperin

S hoolofComputerS ien e,TelAvivUniversity,TelAviv,Israel69978

fshaihi,danhag s.tau.a .il

Abstra t. Thebasi motion-planning problemis to plan a ollision-freemotion

for an obje t movingamong obsta les between free initial and goal positions, or

to determinethat nosu hmotionexists. Thebasi problemas wellas numerous

variants of ithave beenintensively studied overthe past two de ades yielding a

wealthofresultsandte hniques,boththeoreti alandpra ti al.Inthispaper, we

propose a novel approa h to motion planning, hybrid motion planning, in whi h

in order to ombine their strengths and o set their weaknesses. We in orporate

robusttools,that havenotbeenavailable before,inordertoimplementthe

om-pletesolutions.Weexemplifyourapproa hinthe ase oftwodis smovingamong

polygonal obsta les inthe plane. The planner we present easily solves problems

whereanarrowpassage intheworkspa e anbearbitrarilysmall.Ourplanneris

also apableofproviding orre tnontrivial \no"answers,namelyit an,for some

queries,dete tthesituationwherenosolutionexists.Weenvisionourplannernot

asatotalsolutionbutratherasanewtoolthat ooperateswithexistingplanners.

results.

1 Introdu tion

In this paper we study the motion-planning problem for two dis s moving

among polygonalobsta les intheplane. Thisis oneof themanyextensions

to thebasi motionplanningproblemin whi h onlyonerobot is on erned.

Givenaninitial andgoal on gurations,theplannerhastoplan a

ollision-free path betweenthese on gurations,ortodeterminethatnosu hmotion

exists.Wefollowthestandardpra ti etostudythemotion-planningproblem

in terms of the on guration spa e (C-spa e), thespa e of allpossible

on- gurationsoftherobot.Workspa eobsta lestransformintoobsta lesinthe

?

WorkreportedinthispaperhasbeensupportedinpartbytheISTProgramme

oftheEUas aShared- ostRTD(FETOpen)Proje tunderContra tNo

IST-2000-26473(ECG{E e tiveComputationalGeometryforCurvesandSurfa es),

byTheIsraelS ien eFoundationfoundedbytheIsraelA ademyofS ien esand

Humanities(Centerfor Geometri ComputinganditsAppli ations),andbythe

(2)

on guration spa e, or C-obsta les. Inter-robot ollisionsindu e additional

C-obsta les. We refer to inter-robot ollisions as oordination ollisions to

di erentiatethem fromobsta le-robot ollisions.Amotionof therobot

sys-tem orrespondstoa urveinC-spa e.Amotionis ollision-freeifthe

orre-sponding urvedoesnotinterse t anyC-obsta le,that is, itlies ompletely

within the free part of C-spa e. Motion planning has been proven a hard

problem(see,e.g.,[21℄)andthereisstrongeviden ethatitssolutionrequires

exponentialtime in the numberof dofs (degrees of freedom, the dimension

ofC-spa e).See[18℄ formoreinformationonmotionplanning.

1.1 CompleteSolutions: Theoryand Pra ti e

In robot motion planning we onsider an algorithm to be omplete for a

problemifitisguaranteed,forallinstan esoftheproblem,to ndasolution

when oneexists andto returnfailure otherwise.General ompletesolutions

are exponentialin thenumber k of dofs [6℄. A lot of e ort wasinvested in

givingworst- asenear-optimalsolutionstomotion-planningproblemswitha

smallnumberofdofs[23℄.

Until re ently, omplete algorithms have rarely been implemented and

some were implemented over a de ade ago (e.g., [2℄). The task is far from

trivialevenfor motion-planningproblems withasmallnumberofdofs.

No-ti e however,that a genuinely omplete implementation, namely an

imple-mentationthat an orre tly opewithallinstan esoftheproblem,requires

theusage ofspe ial arithmeti in orderto dealwith arbitraryinput andin

parti ulartohandletightornarrowpassagesfortherobotintheworkspa e.

We will referto a omplete implementation of a omplete motion-planning

algorithm asa omplete solution. ThediÆ ulties in implementing omplete

solutionsarisein theimplementationofgeometri algorithmsinmanyother

areasaswell.Thishasledtoamajore ortbyseveralresear hgroupsin

om-putationalgeometrytodeveloptheinfrastru tureforimplementationofsu h

algorithms.Onesu he ortistheCgallibraryofgeometri datastru tures

andalgorithms[7℄.

WithinCgaltherearemeansto onstru tandmanipulatearrangements

of urvesintheplane[11℄,namelythesubdivisionoftheplaneintoverti es,

edges,andtwo-dimensionalfa esindu edbya olle tionof urves.

Arrange-ments anserveasakeyingredientin ompletesolutionstomotion-planning

problems with twodofs (this onne tionas well asthe underlying software

support foritaredes ribedin [13℄).Ontopof thearrangementpa kagewe

havebuilta ompletesolutiontothepolygonaltranslationproblem[10℄and

(3)

Inthefa eoftheprohibitive omplexityof ompletegeneralplannersandthe

diÆ ultyinimplementingthemotherresear hdire tionsweresought.Among

these,theintrodu tionofprobabilisti solutions[3,17℄provedasanimportant

amountof ompleteness againstagainin omputing eÆ ien y.In addition,

they are mu h simpler than omplete solutions, easier to implement, and

easily adaptable to di erent settings, in luding problems with many dofs.

The form of ompletenessthat des ribesthis familyof solutionsis referred

to as probabilisti ompleteness: if asolution path exists, the planner nds

oneinboundedtimewithhighprobability.

A lass of probabilisti solutions that has been intensively studied and

applied is probabilisti roadmaps (PRMs), whi h onsists of sampling the

on guration spa e at random, retaining the free samples as graphnodes,

and onne tingthesenodesbya\lo al"plannertoforma onne tivitygraph

(the roadmap).In PRMs, theprohibitive omputationof an expli it

free on guration spa e is impli itly provided by some lo al fun tion, e.g.,

ollisiondete tionordistan emeasurement.Themaindrawba kofPRMsis

theirdiÆ ultyto solve aseswhere the on gurationspa e ontainsnarrow

passages. Many variants of the basi PRM s heme have been proposed in

ordertoover omethishurdle[1,5,16℄.LaValleandBrani kyhavedeveloped

deterministi variantsofthePRMthat usequasi-randomizedsampling[19℄.

They showthat these variantso er performan e advantages in omparison

with ommon PRMs. All advan ed variants work well on ertain types of

problemsbut unfortunatelyallfail onothers.Asynthesisis proposedin [8℄,

where a meta-planner breaks down the originalproblem into subproblems,

isreferredtoa omparativestudy ofPRMs[12℄.Weremarkthat nomatter

howsu essful PRMsare, whensu h aplannerfails to ndapath,itisnot

sampled thefree on gurationsspa e.It isalsopossiblethatthe timelimit

to whi haPRMisbeing on nedisinsuÆ ient.

1.3 Multi-Robot MotionPlanning

Anaturalextensionofthebasi motion-planningproblemisthemulti-robot

variant,whi hrangesfromthetwo-robot asetos enarioswithgiant eetsof

movingobje ts.In entralizedmotionplanners,planningisperformed

on ur-rentlyforalltherobotsina omposite on gurationspa e,o ering omplete

solutions,whi hare howeverexponentialin thedimensionofthe omposite

on gurationspa e. De oupled solutionsavoidthis high omplexity by rst

(4)

robots. Sin e deadlo k situations are possible su h methods are inherently

in omplete.See[18℄ fordetails.

Sharir and Sifrony [24℄ des ribed a omplete O(n 2

) solutionfor two

in-dependentrobots movingamong polygonalobsta lesin theplane that is a

spe ial aseofthe ompletealgorithmtothegeneralmotionplanningproblem

des ribedin[22℄.Oursolutionisbasedinpartonideasfromthisalgorithm,

ideasthatarebrie ypresentedinSe t. 2.



goastepfurtherandredu e thatsizeofthegraphthat representsthe

oor-dinationoftherobots,bydistinguishingbetweensituationswheretherobots

arelo atednearea hother,andsituationswherethisisnotthe ase.Inthis

paperwe apply similar ideas.Our planner identi es subregions of the free

on gurationspa e wheremutual ollisionareguaranteed notto exist,and

oordinationishen enotrequired,evenpriorto sampling.

1.4 OurResults

Even as PRMs be ome more sophisti ated it is lear that between these

methods and ompletesolutionsthere is still awidegap.This suggeststhe

possibility of a further synergy between the two approa hes. Su h a

om-bination has been hard to a hieve until re ently be ause, as explained in

Se t. 1.1, omplete solutions require spe ial arithmeti along with robust

geometri tools for them to be widely used in pra ti e. As su h tools are

nowavailable,e.g.,as partofCgal [7℄,webelievethat there isapla e for

a newtype of planners that tryto integrate omplete solutions with PRM

te hniques.Inotherwords,thereis awhole spe trumof potentialsolutions

thatin orporatetheseeminglytwoextremeapproa hes,solutionsthatwould

ombine thestrengthsofbothapproa hesando settheirweaknesses.

Thehybridapproa hwesuggestistobreakanexistingprobleminto

sim-pler subproblems(e.g., with lessdofs orwith simpler geometry), su h that

a omposition of solutionsto the subproblems is asolution to the original

one. We present a spe i example of this hybridapproa h in the form of

aplanner forthe oordinationoftwodis s movingamong polygonal

obsta- les in the pla e. We referto thehybrid motionplanner asHyMP. Instead

of havinganexpli itrepresentationof C

free

ornoneatall, HyMP omputes

twoexpli itrepresentations,C

free for asubset of C free andC + free for a

super-set thereof (whi h is smaller than the entire on gurationspa e C). These

expli it partial representations, aswerefer to them, provide HyMP with a

omplete solutionfor an important subset of the possible queries. We

em-phasizethatwedonotgivea ompletesolutionofthetwo-dis problem,as

weavoid themost omplexstage of omplete solutions, namely omputing

an expli itrepresentationof C

free

. Inorder to answerthe full setof queries

HyMPin orporatesaPRMwithin C +

free nC

free

(5)

it is aided byinformation derivedfrom theexpli it partial representations.

Theprobabilisti partofHyMPisinterestinginitsownrightand,for larity,

wewilldenote itby hPRM.UnlikePRMs,a omplete solutionfor asubset

ofthequeriesenablesourplanner toresultwith orre t nontrivial\no"

an-swers(in fa t,dis onne tion proofs, atermweborrowfrom [4℄)ratherthan

\failure"forasubsetofthequeries.Wealsoshowexperimental resultsthat

demonstratehowHyMPeasilysolvespreviouslyhardproblems.

HyMP is not a total solution that repla es existing ones. Beyond its

Se t. 4. We onsider HyMP as a novel tool in the a umulating

motion-planning toolbox and we envision it aspart of a meta-planner framework,

liketheonepresentedin[8℄.

Therestofthepaperisorganizedasfollows.Webeginbypresentingthe

setting of ourstudy in Se t. 2. In Se t. 3,we give ataxonomy of

on gu-rations that leads to the introdu tion of the general s heme of the hybrid

disadvantages of the planner along with experimental results aredes ribed

in Se t.4.Suggestionsforfuture resear hdire tionsaregiveninSe t.5.

2 Preliminaries

2.1 The Setting

The moving obje ts (robots) onsidered in this paperare two planar dis s

D 1 and D 2 of radii r 1 and r 2

, respe tively. The dis s are free to move in

a planar re tangular workspa eW, whi h ontains N polygonal obsta les,

P

1

, ..., P

N

,withatotalofnedges,aslongasthedis sdonot ollidewith

theobsta les(the omplementoftheboundingre tangleofWisregardedas

anotherobsta le)orwithea hother.A on gurationforwhi harobot

inte-rioroverlapstheinteriorofanobsta le,ordis interiorsoverlapisforbidden.

All non-forbidden on gurations are semi-free, but only semi-free

on gu-rationsin whi hnorobot tou hesanobsta les andtherobots donottou h

oneanotherare onsideredfree.Semi-free on gurationsthatarenotfreeare

boundary on gurations.Inthispaperwedonotassumegeneralpositionand,

asarule,allowboundary on gurationssaveforasmallfra tionthereof.We

ex lude boundary on gurationsforwhi h at least onerobot tou hesmore

than one obsta le edge or vertex simultaneously. In other words, we avoid

obsta le-robottightpassages.This onstraintservestosimplify boththe

de-s ription of the planner as well as its implementation. The tools we used

themselvesallowthein lusionoftightpassages(asimplementedforonedis

in[15℄).Werepresent on gurationsusingfour-tuples(x

1 ,y 1 ,x 2 ,y 2 ),where p i =(x i ;y i

),i=1;2isthepla ementofthe enterofthedis D

i inW.

The on guration spa e C is four-dimensional. We let C

free

denote the

( losed)subsetof( ollision-)free on gurationsandletC

forb

(6)

inthetrapezoidalde ompositionofa on gurationsspa e;

for larity theobsta les(bla kpolygons) andthedis are

shown.

2.2 CompleteSolution forOne orTwoDis s

Oursolutionreliesona ompletesolutionfortheproblemofonedis moving

among polygonalobsta les,and borrowsideasfrom a omplete solutionfor

the oordinationoftwodis s.Webrie ysket hea hofthesesolutions.

CompleteSolutionforOneDis Amotionplannerforapolygonalrobot

among polygonalobsta lesispresentedin [9℄. Weuseanadaptationof this

Theplanner beginswith omputingarepresentationofthe2Dfree

on- guration spa e C

free

of the robot. The boundary of C

free

onsists of line

segmentsand ir ularar s.Theplanner ontinuesbyverti allyde omposing

C

free

.Theresultisa olle tionof(pairwiseopenlydisjoint)simple onne ted

ells. Ea h ell hasa trapezoid-likeshapewith two x-monotonebases,any

ofwhi hmaybeeitherastraight-linesegmentora ir ularar .Wereferto

these ellsastrapezoids.Iftheupperorlowerbaseofatrapezoidisa ir ular

ar thenthetrapezoidisnon- onvex,be ausethe onvexsideofa ir ularar

isalwayspartofaC-obsta le.Oneoftheverti alatta hmentsofatrapezoid

alongaverti alatta hment.

Next,theplanner onstru tsaroadmapwhi hre e tsthe onne tivityof

C

free

trapezoids are onne ted with an edge. This roadmap is embedded in the

of thedis robot is toensurethat this ruleholds fornon- onvextrapezoids

[15℄.Given apair of query on gurations, there is apath onne tingthem

in C

free

ifand onlyifthenodes oftheir ontainingtrapezoidsbelongto the

Complete Solution for Two Dis s An eÆ ient omplete algorithm for

twodis srunninginO(n 2

)timeisdes ribedin [24℄.Thealgorithmisbased

on de omposing C

free

into a olle tion of simple onne ted 4D ells, su h

that ea h ellhasO(1) omplexity.Then, ea h ellis repartitionedbyO(1)

ombinatorial representation of C

free

(7)

Fig.2. Con gurationspa epartition: Theinterior

of the dashedellipse is free spa e. The dark gray

areaisC

forb

andthewhiteareaisC

free

,bothwhose

representation is expli it. The light gray annulus,

whi hstandsforC 

free [C



forb

,isthedomainofthe

probabilisti planner.

Theinitial ell de omposition is based onthe verti alde omposition of

the free2D on gurationspa e ofea h dis . Asmentioned above,software

toolsarenowavailabletomaintainthearrangementsrepresentingthese

un-derlying 2D on gurationspa es. Inthis paperweperformonly theinitial

ellde ompositionbutavoidthe ostly omputationof riti alfa ets.The

ini-tialde ompositionsuppliesourplannerwithusefulalbeitpartialinformation

regardingthe onne tivityofC

free .

3 The Hybrid Planner

3.1 Taxonomy ofCon gurations

Wedi erentiatebetweenobsta le ollisions,whereoneoftherobots ollides

with someobsta leand oordination ollisions, where ea h robot isdisjoint

from the obsta les but the robots overlap. We refer to on gurations that

representsu h ollisionsasobsta le-forbidden and oordination-forbidden,

re-spe tively.(Obsta le-forbidden on gurationsmayrepresent aseswherethe

robots overlap,in addition to theobsta le ollision,but wedonot onsider

these asesas oordination ollisions.) LetC

forb and C



forb

denotethesets of

obsta le-forbidden on gurationsand oordination-forbidden on gurations,

respe tively. Let us partition C free as well. Consider C forb , the omplement of C forb . C forb equalsC free [C  forb

,i.e,itin ludes on gurationsthatareeitherfreeor

oordination-forbidden.C

forb

isasupersetofC

free

andweusetheshorthand

C +

free

to refer to it. Let C

1

; :::; C

M

be a disjoint nite partition of C +

free

into onne ted ells. (We will shortly propose su h a partition.) Some of

these ells may ontain only free on gurations, while others may ontain

oordination-forbidden on gurationsaswell.We denoteby C

free

theunion

of allthe ellsC

i

that ontainonlyfree on gurations.If q

init and q

goal are

bothin thesame onne ted omponentofC

free

, thenitis evidentthere isa

solutiontothemotion-planningproblem,althoughsolutionsmaybepossible

evenifthequery on gurationsareindi erent onne ted omponentofC

free .

We denoteby C 

free

the rest ofthe ellsC

i , namelyC free nC free . Theresult of

the partitioningis that thefull on guration spa eC of ourproblem is the

disjointunionC free [C  free [C forb [C  forb

(8)

3.2 General S heme

HyMP omputes expli it representations of C +

free and C

free

, but it does not

ompute an expli it representation of C

free

. The planner begins with

om-puting C 1 free and C 2 free , where C i free

is the two-dimensional spa e of all free

pla ements of D

i

, ignoring the presen e of the other dis . Let f 1 1 ,..., 1 m 1 g and f 2 1 ,..., 2 m 2

g be the trapezoids in the verti al de ompositions for C 1 free andC 2 free ,respe tively.Forea h1im 1 and1jm 2 , letC i;j denote

theCartesianprodu t 1 i  2 j . Obviously, C i;j

is thefree on guration spa e

ofthe ombinedsystemD

1 andD

2

whenmovinginthepresen eofonlyO(1)

obsta les,or,morepre isely,subje ttoonlyO(1) ollision onstraints,

ignor-ing oordination ollisions. Ea hC

i;j

is a onne tedfour-dimensionalregion

thatwe alla4D ell,or ell forshort.Byde nition,weobtainthefollowing

lemma. Lemma1. C + free = S m 1 i=1 S m 2 j=1 C i;j . C + free

is the result of \gluing" all these 4D ells together. A pair of 4D

ellsC

i;j andC

k ;l

are adja ent if andonly ifone ofthe following onditions

hold: (1) 1 i = 1 k and 2 j and 2 l areadja ent. (2) 2 j = 2 l and 1 i and 1 k are adja ent. (3) both 1 i and 1 k and 2 j and 2 l

are adja ent in theirrespe tive

2D on gurationspa e.The rsttwoentries des ribe aseswhere D

1 (resp.

D

2

) moves within the same trapezoid, while D

2

(resp. D

1

) rosses over a

verti alatta hmentfrom onetrapezoidto anadja enttrapezoid. Thethird

entrydes ribesthe asewherebothdis s rossoverfromonetrapezoidtoan

Let us look at a 4D ell C

i;j

, that des ribes all possible on gurations

where D 1 (resp. D 2 ) is in 1 i (resp. 2 j

). Assume that we embed the

two-dimensional on gurationspa eofea hdis intheworkspa eintheobvious

manner (i.e., a on gurationismapped to thelo ation of the enter of the

dis ). Let X Y denote the Minkowski sum of the sets X and Y, that is

X Y = fx+y j x 2 X; y 2 Yg. The followinglemma givesa suÆ ient

onditionfora4D ellto ontainonlyfree on gurations.

Lemma2. C

i;j

ontains only free on gurations if 1 i D 1 and 2 j D 2 are disjoint. We de ne C free

to be the union of 4D ells that ful ll the ondition of

Lemma2andhen e ontainonlyfree on gurations.

HyMP omputestwo(undire ted) onne tivitygraphs.The onne tivity

graphG +

isbuilttore e tthe onne tivityofC +

free

.ThenodesofG +

arethe

4D ells.The graph ontainsanedge onne ting apairof 4D ellsifthese

Within C

free

no oordination ollisions are possible, so we apply the exa t

(9)

independently.Wethen perform whatis, in general,aCartesianprodu t of

the twoindependent roadmaps. hPRM, the PRM within HyMP ompletes

the roadmap G by sampling and onne ting on gurations within C  free [ C  forb

. In the ourse of the algorithm the two parts of G are glued to one

another.WenextpresenthPRM andthisgluingpro ess.

Sampling S heme The default sampling s heme is simple: We sample a

onstantnumberk

1

of on gurationsinevery4D ellofC  free [C  forb .Clearly,

itisnotpossibleforasampled on gurationtobeobsta le-forbidden.

CollisionDete tionand Simple Lo al PlanningGivena on guration

qinC  free [C  forb

, ollisiondete tionamountstoaninvo ationofapredi ate,

that answerstrue if and only if the dis s overlap.Given a pair of

on gu-rationsp;qin C  free [C  forb

,thesimplest lo al plannertries to onne tthese

on gurationsalongastraightlinepq.WithinhPRM,weinvokethisprimitive

only forpairsp;q ontained in thesame4D ell.More omplex onne tion

attempts are redu ed to a set of onse utive invo ations of this primitive.

Sin e a4D ellhasO(1) omplexity, and sin eitsshapeissimple andwell

hara terized, wegain an instantaneous onne tion primitive. In turn, this

enablesustoinvokethisprimitivemanytimeswithoutsa ri ingeÆ ien y.

In the ase of two dis s the straight-line lo al planner may prove too

rude.Weexaminethedi erenttypesof on gurationstore neit.Thepath

pq may be invalid be ause it ontains obsta le-forbidden or

oordination-forbidden on gurations.Theexpli itrepresentationsofC 1 free andC 2 free allow

ustobreakthe onne tionattemptbetweenpandqintothreequestionsvia

twosimplerpredi ates.Forea hdis weindependently he kifthemovement

1 andD

2

donotoverlapalong

the path. Thanks to theexpli it representations of C 1 free and C 2 free , a single

omputation is suÆ ient to answer whether the path of ea h of the dis s

is free in its 2D on guration spa e in omparison with the need to he k

many onse utive on gurationatsomeresolutionalongthepathin ommon

PRMs.Iftherearenoobsta le-forbidden on gurationsalongpqbutthedis s

overlapalongit,wefurther he kifitispossibleto oordinatetherobotsso

that onewaits totheotherandonlythen moves.

Roadmap Conne tion Similar to OBPRM [1℄, there are three general

stages in our PRM onne tion strategy, where ea h stage is omposed of

sub-stages of itsown. The rst stage (In-Cell Conne tion) attempts to

onne t on gurations sampled within a 4D ell into a lo al onne tivity

graph. The se ond stage (Inter-Cell Conne tion) attempts to onne t

lo al graphs of adja ent 4D ells. The third stage (Stit hing) improves

the onne tivity of the roadmap by attempting to join di erent onne ted

1.In-Cell Conne tion:Thisstep(alongwiththesamplingstage)isaset

of simpli edmotion-planning problemswithin every4D ell.Ea h ellis of

(10)

on gura-samplings hemeandalo alplanneraresuÆ ienttograsp the onne tivity

ofa ellinmost ases.Thisisba kedupbyourexperimentalresults.LetG

i;j

denote the onne tivity graphof the ell C

i;j

. (The planner an befurther

enhan edby applyingsophisti ated samplings hemeswithin 4D ells,e.g.,

OBPRM-likesampling[1℄,orquasi-randomizedsampling[19℄.)

2. Inter-Cell Conne tion:These ond stagegoesoverallpairsof

i;j andC

k ;l

andattemptsto onne tthe onne ted omponents

of G

i;j

with the onne ted omponents of G

k ;l

. As in Stage 1, theproblem

at this stage isalso simple.Theadditional diÆ ultyis due to thefa t that

pairsof adja ent4D ells anform more ompli ated shapesthan one ell.

Giventwo on gurationsp2C

i;j

and q2C

k ;l

,we ndan intermediatefree

on gurationr onthe ommonboundary(fa et) betweenthetwo ellsand

wetryto onne tboth pandq to r.Ea h of the onne tingpathsin these

attempts lie entirely within the losure of a 4D ell and therefore we an

usethe samesimplelo alplanningprimitiveasin in- ell onne tion. Ifone

of the 4D ells C i;j is in C free then onne ting G i;j to the on guration on

the ommon boundary is easily a hieved deterministi allyby applying the

solutionofonerobotforea hofthedis s.

3.Stit hing:Atea hiterationofthisstage,theplannertakespairsof

on-ne ted omponents V

i and V

j

of the roadmap onstru ted so far and tries

to \stit h"theminto asingle onne ted omponentin orderto improvethe

overall onne tivityoftheroadmap.Themotivation isto nddis repan ies

betweenthe onne tivityof C

free

and thepossiblemotionsasrepresentedin

G.Havingnoexpli itrepresentationofC

free

C +

free

topredi tsu hpossibledis repan ies.Forea hpairof onne ted

om-ponentstheplanner pi ksupdi erent pairsof representative on gurations

p2V

i ,q2V

j

.Next,theplanner ndsthe4D ellC

p

(resp.C

q

)that ontains

p(resp.q).Theplanner tries to ndapath in G +

that onne tsC

p andC

q

and orrespondstoa\ hannel"inC +

free

withpandqonitsoppositeends.The

existen eofsu hapath(there anbeone,many,ornone)doesnotguarantee

that afreemotionispossiblebetweenpandqalongthis\ hannel,"sin eit

may ontain oordination-forbidden on gurations, whi h may blo k robot

movements. It may still however be possible that other paths between the

ellsofthe samepair ofrepresentativesorpathsthat onne t ellsofother

pairsofrepresentatives,willenableafreemotionfromone onne ted

ompo-nentstotheother.(Weleavethestudyofsophisti atedstrategiesof nding

qualitativelydi erentpathsin on gurationspa estofuture resear h.)

GivenapathinG +

,its ellsaretraversedin orderto ndpairsof

repre-sentativesofV

i andV

j

su h thattheserepresentativesarebothinthesame

ellorin onse utive ells.Ifsu hapairisfoundthena omplexprobabilisti

lo alplannerisinvokedinanattemptto ndapaththat onne tsthese

rep-resentatives. Thetraversal terminates upon onne tionsu ess or when no

morepairsof representativesarefound.Theplannerskips ellsandpairsof

(11)

determinis-Alg. 1Prepro essing 1: Constru tC i free exa tly,i=1;2 2: Verti allyde omposeC i free into i 1 ... i m i ,i=1;2 3: LetC i;j := 1 i  2 j ,i=1:::m 1 ,j=1:::m 2

4: forall4D ellsCi;j do

5: Constru tG

i;j

6: forallpairsofadja ent4D ellsCi;j,Ck ;l do

7: Conne t onne ted omponentsofG

i;j ,G

k ;l

8: LetV1:::Vm bethe onne ted omponentsoftheinitialroadmapG

9: forallpairsVi;Vj;i6=jdo

10: Stit hV

i ;V

j

ti allyandare ertainnotto auseanydis repan y.Onefailedtraversalmay

notpreventtherelevant onne ted omponentsfromgettingtransitively

on-ne tedastheymaybelongtoasequen eof onne ted omponents,forwhi h

ea hand every onse utivepairalong thesequen ewasdire tly onne ted.

Wesummarizetheprepro essingstage ofHyMPinAlgorithm1.

Query Pro essingGiven amotion-planning query,i.e., apairoffree

on- gurations q init andq goal ,the4D ellsC init and C goal that ontainq init and q goal

,respe tively,arefound.IftheG + nodesofC init andC goal areindi erent onne ted omponentsofG +

thenitis learthatnosolutionispossibleand

init andq

goal

andattemptsto onne tthese

nodestothesame onne ted omponentofG.Forea h onne ted omponent

of G theplannerlooks forrepresentativenodeswhoseunderlying

on gura-tionsliewithinC

init

(resp.C

goal

)andappliesalo alplannerinanattemptto

onne tq

init

(resp.q

goal

)andtotheseunderlying on gurations.Ifa

onne -tionisfound,then asimplegraph-sear hingalgorithmis appliedto retrieve

apathbetweenthenodesofq

init andq

goal

.Otherwisetheplannerresponses

with\failure".

3.3 ImplementationDetails

HyMPwasimplementedin C++.WeusedtheCgal library[7℄ asthe

geo-metri alengineofourimplementation.TheCgalkernelprovidedtheset of

primitiveobje ts(points,linesegments, ir les,et .)andpredi atesrequired.

The planar map and arrangementpa kage was used for the representation

and manipulationof the2D on gurationspa esand provedeasyto usein

orderto perform omplexoperationssu hastrapezoidalde omposition and

Minkowskisums[11,14℄.4D ellsofC +

free

wererepresentedwithpairsof

trape-zoidsfrom C 1 free andC 2 free

.Ourworkreliedonourpreviously-developedtools

to ompute Minkowski sums of polygons and dis s as well as to solve the

(12)

(a)Hshape (b)Mazeswap ( )Annulus (d)Jagged boundary

Fig.3.Workspa esaretheunitsquare[0;1℄ 2

Hen e,the orrespondingC-spa esare on nedtothe4Dunit ube[0;1℄ 4

.

abilitytomanipulate oni urvesexa tly[27℄.Thisrequiresanexa tnumber

typethat anrepresentalgebrai numbers(irrationalsinparti ular).Weused

theLedarealnumbertypeforthispurpose[20℄.Finally,weusedthegraph

pa kage of Leda to represent, manipulate, and query onne tivity graphs.

As additionallevels of ltering, whi h are ru ial to expedite exa t

ompu-tations, we were usingSturm sequen esand keepingtra k ofthe historyof

omputations.Seethefullversionofthepaperformoredetails.

4 Features of HyMP and Experimental Results

HyMPstops shortof omputinganexpli itrepresentationofC

free

(doingso

wouldprovideus witha ompletesolution)butitdoessuÆ ient

prepro ess-ing so that despite some disadvantages it enjoys distin tive advantages. In

this se tionwepresentthesefeatures andprovideexperimentalresultsthat

demonstratethem.Alargersetofexperimentsisavailablein thefullpaper.

plan-nerand the ombination thereof.These are (1)thebreaking ofthe original

problemintosubproblems,(2)theappli ationof ompletesolutionstosome

of the subproblems, and (3) the ell de omposition s heme. These

andleaveas littleworkaspossibletoaspe ializedprobabilisti partandas

The ompletesolutionsofea hofthefree2D on gurationspa es,C 1

free

and C 2

free

, provide theplanner with theexpli it representationof C + free . The ell de omposition of C 1 free and C 2 free

, whi h is used to de ne and onstru t

the ell de omposition of C +

free

, also provides the planner with the expli it

representationofC

free

.Thisisa hievedbyidentifying4D ellsthatful llthe

onditionof Lemma 2.Inwhat followswe showhowthis informationleads

themeanstoover omeit.

(13)

ityandmaybehard toimplement.This isnotthe asehere.The omplete

is very eÆ ient. In addition, the expli it representations of C 1

free and C

2

free

indeed demand an intri ate implementation that requires exa tand rather

involveddata stru tures in omparison withthe straightforward

implemen-tations of PRMs, but the means to onstru t and manipulate the required

data stru tures(mainlyarrangementsof urvesintheplane)arenow

avail-ableand anbeused asbla kboxes.Moreover,relevant2Dalgorithms and

the ellde ompositionarefarless omplexthanthe ompletesolutionofthe

originaltwo-dis problem[24℄.

Spe ialized PRM hPRM is a spe ialized PRM and for various reasons.

First,itssamplingdomain,C 

free [C



forb

issmallerthantheentire

on gura-tion spa e C, oftenmu h smaller. This allows hPRM to sample (far)fewer

on gurations in order to obtain athreshold of free on gurationsthan it

shouldhaveifitweretosampleallofC.Inturn,thisallowshPRMto

itsdomain,ingeneral,andnarrowpassages,inparti ular,in reases.Se ond,

this subspa e ontainsonlyfreeor oordination-forbidden on gurationsso

ollision dete tion amounts to an overlappingpredi ate of the dis s whi h

is trivial. Third,be ause ofthe ell de omposition within this domain, the

robotslo allyoperatewithinsimplewell- hara terized4D ellsandthe

on-ne tivity of these ells is known from their adja en y relation.As a result,

thelo al planner weapply anbeverysimpleandeÆ ient,aspresentedin

Se .3.2.Fourth, ellde ompositiongivesadditionalweightto on gurations

within narrowpassagesin thepro ess ofsampling.Narrowpassagestendto

result in smaller 4D ells be ause their width is smaller along at least one

axis,but thesamenumberof on gurationis sampledwithin these ellsas

inbigger ellsandmoreifneeded.Finally,theexpli itrepresentationofC +

free

alongwiththe ellde omposition providesagood predi tionof the

onne -tivityofC free sin eC + free equalsC free [C  forb

.Thisenablestheplannertolook

for possibledis repan iesin the onne tivityof theroadmapin omparison

of the onne tivity of C

free

and apply the stit hing stage to attempt and

over omesu h dis repan ies.An experimentdes ribedin thefullversionof

thepaperdemonstratesthe ontribution of thestit hing stage,whi h often

proves riti altothe nal onne tivityoftheroadmap.Weshowthat

stit h-ing an over omes enarioswherewe(arti ially)for etheplannertoavoid

anyprior onne tionattempts.

Dis onne tion ProofsThe expli it representationof C +

free

enables HyMP

tore ognizequerieswheretheinitial on gurationisinadi erent onne ted

omponentof C +

free

from the goal on guration. As C

free C

+

free

, these

on- gurations are in di erent onne ted omponents of C

free

. For su h queries

(14)

Workspa e Narrow Passages It is lear that omplete solutions are in

prin ipalindi erenttonarrowpassages.4D ellsofC

free

that represent

nar-rowpassagesdonot ontain oordination-forbidden on guration.These

re-gions are deterministi allyrepresentedin the nal roadmapbe ausewithin

forea hrobot.ThismakesHyMPalmostinsensitivetonarrowpassageswhere

no oordinationisinvolved.

Sensitivity to the Complexity of C

free

HyMP as des ribed above has

the following obviousdisadvantagewhi h is we an easily over ome. If the

omplexity ofthe obsta lesisn, then there are(n 2 )4D ellsin C + free and atotalof(n 2

onne tivitygraphforeveryoneofthe (n 2

)4D ellsandtries to onne t

onne ted omponentsbetweeneveryoneofthe(n 2

ells.Insimplemotion-planningproblems,su hasdepi tedinFig.3(d),this

may prove an e ort far beyond what is a tually needed. Fortunately, this

thesolutionis easy.We envisionHyMP aspartofmeta-plannerframework

[8℄ratherthanatotalsolutiononits own.Su haframework isexpe tedto

queries,andreserve omplexsolutionsfor omplexproblems.Anexperiment

presentedin thefullpaperdemonstratesthissensitivity.

Experiments Theworkspa eused are illustratedin Fig.3and theresults

are presented in Table 1 along with additional details. Despite the use of

exa tarithmati ofalgebrai numbers(anoverkillforpolygonalrobots),

pre-pro essing took nolongerthan aminutein theexperimentspresented.The

rsttwoexperimentsdemonstratethelowsensitivityofthehybridplannerto

thewidth ofworkspa enarrowpassages(in the rst)and theirquantity(in

these ond).Forthe rstexperiment(\H

1

") weuseanH-shapedworkspa e

environmentwhereea hof theverti alslabsofthe shape(\slabs")spansa

thirdofthewidthoftheworkspa e(Fig.3(a)).Thisprovidestherobotswith

suÆ ientspa eto oordinatetheirmovements.Thequery on gurationsare

hosen sothat both robots haveto su eed in rossing thenarrow passage

for theplanningto besu essful. Theheight ofthe orridoris 0.05and we

inC i

free

arethewidthof0.0002.Ten on gurationsweresampledinea h4D

ell of C +

free nC

free

. The resultsshowthat there is no problem for thehybrid

plannertosample on gurationsthatrepresentrobotpla ementswithinthe

orridor,to onne tthese on gurationstooneanother,andto onne tthem

to on gurationsinadja ent ells.Alltenrunsweresu essfulandallended

For the se ond experiment (\maze

1

") we used a maze-like workspa e

(Fig. 3(b)). All the orridors, the long horizontal ones and the short

(15)

oordination hallengeofmedium diÆ ultyby on ning theareawhere the

robots an bypass oneanotherto the the uppermost horizontal bar, whose

width is 0.1. The robots are both pla ed at the bottom-left orner of the

maze andhaveto swappositions. Theinitialand goal on gurationsarein

the farthest regions in C

free

asfar as the shortest path distan e is

onsid-ered.Topologi ally,C

free

islikea\ hannel"withtheseregionsonitsopposite

ends.Inorderfortherobotstosu eed,theyhavetogofromoneendofthe

\ hannel"totheother,i.e.,ea hrobothastogothroughall possiblenarrow

passages twi e and they have to su eed in bypassingone another aswell.

pas-sages.This anonlybepossibleifthe on gurationssampledwithinashort

verti alpassagehavevisibilitythat oversbothin identlonghorizontal

pas-sages.There weresevensu essful runsoutoften. Thereasonforfailurein

thesethree asesisthattheplannerdidnot ndawayfortherobotstoswap

in thetoppartoftheworkspa e.Webelievethatamoresophisti atedlo al

plannershouldover omethisdiÆ ulty.

The third experiment (\annulus

1

") presents how material the expli it

representationofC

free

anbetotherepresentationofthe onne tivityofthe

C

free

. The workspa e environment is similar to themaze but di ers in two

major aspe ts (Fig. 3( )). First, nowhere in the workspa e an the robots

lo ally swap positions. Se ond, topologi ally,the free on gurationspa e is

anannulus.Inorderfortherobotstoswappositions (likein themaze)one

of therobots should remainin pla e,while theother should go alltheway

through the orridors and rea h the lower left orner from the other side

of the stationary robot. All ten runs were su essful. Noti e that most of

theroadmap,i.e. 85%ofitsverti esand95%of itsedges, were determined

deterministi ally.Totakethispointfurtherwerantheplanneragainonthis

input, but preventing it from sampling so the planner ould use only the

free

ells(\annulus

2

the initial and goal on guration are positioned in oordination-free ells

so that the planner ould onne t them to the roadmap. This experiment

stressesthe fa t that in many s enarioswe an avoid sampling andstill be

abletosolvediÆ ultandinterestingproblems.

5 Future Resear h

Obviousfutureappli ationofthehybrid-motion-planningapproa hisin ases

of morethan twobodies in the planebut alsoin 3-Spa e, where, e.g.,

the keyingredientsof a omplete solutionfor the problem of translating a

single polyhedron among polyhedra in 3-spa e [25℄ (polyhedral translation

problem forshort). Webelievehowever,that ahybridapproa h analsobe

(16)

Table1.Experimentalresults.Thetablepresentsthenumberofnodes(V),edges

(E),and onne ted omponents(# )inthe nalroadmap.Itpresentswhatisthe

partofthe nalroadmapwhi hwas onstru teddeterministi allyfor ellsinC

free

intermsofverti es(V ) andedges (E ).Information abouthPRMin ludesthe

numberofsampled on gurations(s )andthefreeportionthereof(f ),aswellas

thenumberofsu essful onne tionattempts(s a)outoftotalattempts( a).The

dataonthe rstthreerows ofthetablearetheaverageover10runs.

verti es,sampling edges,lo alplanner

exper. V V V =V f s f /s E E E =E s a a s a/ a #

H 1 176 32 18% 23093712 60% 303 108 36% 195 441 45% 1 maze1 1602713560 85% 33635633 59% 56889 108 94% 32734357 75% 1.2 annulus 1 1869515952 85% 27434873 56% 6713563448 95% 36874810 77% 1 annulus2 1595215952 100% 0 0 { 6344863448 100% 0 0 { 1

forthepolyhedraltranslationprobleminordertosolvethefullrigid

motion-planningproblemforapolyhedronamongpolyhedra(thatis,allowing

trans-lationandrotation).Onewaytodothat istousea\sli ing"method,where

webuilda oarsegridinthethree-dimensionalrotationspa e. Forea h

ver-tex in this grid(whi h xes the rotational dofs of therobot) we onstru t

an expli it representation of the free spa e (we all these representations

omplete ross-se tions). Wethen use PRM te hniquesto onne t between

the omplete ross-se tions. How to e e tivelymakethese onne tionsis a

non-trivial hallenge.Along di erentlines, one oulduse aPRMto solvea

motion-planning problem and resort to omplete solutionsin only sele ted

regions ofthe on gurationspa e where it hasfailed to make a onne tion

andthereissomeindi ationthata onne tion ouldbefound.

Referen es

1. N.Amato,O.Bayazit,L.Dale,C.Jones,andD.Vallejo.OBPRM:An

obsta le-basedPRMfor3Dworkspa es.InPro .Int.WorkshoponAlg.Found.ofRob.,

pages155{168,1998.

2. J. Ba~non. Implementation and extension of the ladder algorithm. In Pro .

IEEE Conf.onRob.andAuto.,pages1548{1553,1990.

3. J. Barraquand, L. E. Kavraki, J.-C. Latombe, T.-Y. Li, R. Motwani, and

P. Raghavan. A random sampling s heme for path planning. Internat. J.

Robot.Res.,16(6):759{774,1997.

4. J.Bas h,L.Guibas,D.Hsu,andA.Nguyen.Dis onne tionproofsformotion

planning. InPro .IEEEInt.Conf.onRob.andAuto.,pages1765{1772,2001.

5. V.Boor,M.Overmars,andA.vanderStappen.TheGaussiansampling

Auto.,pages1018{1023,1999.

6. J. Canny. The Complexity of Robot Motion Planning. ACM { MIT Press

Do toralDissertationAwardSeries. MITPress,Cambridge,MA,1987.

7. CGAL. TheComputationalGeometryAlgorithmsLibrary. www. gal.org.

(17)

9. M.deBerg,M.vanKreveld,M.Overmars,andO.S hwarzkopf.Computational

Geometry:AlgorithmsandAppli ations.Springer-Verlag,Berlin,Germany,2nd

edition,2000.

10. E.Flato.RobustandeÆ ient onstru tionofplanarMinkowskisums.Master's

thesis,Dept.Comput.S i.,Tel-AvivUniv.,2000.

11. E.Flato,D.Halperin,I.Hanniel,O.Ne hushtan,andE.Ezra.Thedesignand

implementationofplanar mapsinCGAL. TheACM Journal of Exper. Alg.,

5,2000. AlsoinLNCSVol.1668(WAE'99),Springer,pp.154{168.

12. R.GeraertsandM. Overmars. A omparativestudyofprobabilisti roadmap

planners. Thesepro eedings.

13. D.Halperin. Robustgeometri omputinginmotion. InternationalJournalof

Roboti sResear h,21(3):219{232,2002.

pointlo ationforparametri urves.InPro .4thWorkshoponAlg.Eng.,2000.

15. S.Hirs handE.Leiserowitz.Exa t onstru tionofMinkowskisumsofpolygons

and adis withappli ationto motion planning. Te hni al Report

ECG-TR-181205-01, Tel-AvivUniversity,2002.

16. D.Hsu,L.E.Kavraki,J.-C.Latombe,R.Motwani,andS.Sorkin.On nding

onAlg.Found.Rob.,Wellesley,MA,1998. A.K.Peters.

17. L. E. Kavraki, P. 

Svestka, J.-C. Latombe, and M. Overmars. Probabilisti

roadmapsfor pathplanning inhigh dimensional on gurationspa es. IEEE

Trans.Robot.Autom.,12:566{580,1996.

1991.

19. S.LaValleandM.Brani ky. Ontherelationshipbetween lassi algridsear h

20. K.Mehlhorn andS.Naher. The LEDA Platformof Combinatorial and

Geo-metri Computing. CambridgeUniversityPress, 1999.

21. J. H.Reif. Complexity ofthe mover'sproblemand generalizations. InPro .

20thAnnu.IEEESympos.Found. Comput.S i.,pages421{427,1979.

22. J. T. S hwartzand M. Sharir. On the \piano movers" problem II: General

te hniques for omputing topologi al properties of real algebrai manifolds.

23. M.Sharir. Algorithmi motionplanning. InJ.E.GoodmanandJ.O'Rourke,

editors,Handbook ofDis reteandComput.Geom., hapter40,pages733{754.

CRCPressLLC,Bo aRaton,FL,1997.

24. M. Sharir and S.Sifrony. Coordinatedmotion planning for two independent

robots. Ann.Math.Artif.Intell.,3:107{130,1991.

25. H.ShaulandD.Halperin.Improved onstru tionofverti alde ompositionsof

threedimensionalarrangements. InPro .18thAnnu.ACMSympos.Comput.

Geom.,pages283{292,2002.

26. P. 

SvestkaandM. Overmars. Coordinatedpathplanningformultiplerobots.

Roboti sandAutonomousSystems, 23:125{152,1998.

27. R. Wein. High level ltering for arrangements of oni ar s. In Pro . 10th

EuropeanSymposiumonAlgorithms,volume2461ofLe tureNotesinComputer

Updating...

## References

Related subjects :