Randomized Kinodynamic Motion Planning with Moving Obstacles

Transcription

1 Randomized Kinodynamic Moion Planning wih Moving Obsacles David Hsu Rober Kindel Jean-Claude Laombe Sephen Rock Deparmen of Compuer Science Deparmen of Aeronauics & Asronauics Sanford Universiy Sanford, CA 94305, U.S.A. Absrac This paper presens a novel randomized moion planner for robos ha mus achieve a specified goal under kinemaic and/or dynamic moion consrains while avoiding collision wih moving obsacles wih known rajecories. The planner encodes he moion consrains on he robo wih a conrol sysem and samples he robo s sae ime space by picking conrol inpus a random and inegraing is equaions of moion. The resul is a probabilisic roadmap of sampled sae ime poins, called milesones, conneced by shor admissible rajecories. The planner does no precompue he roadmap; insead, for each planning query, i generaes a new roadmap o connec an iniial and a goal sae ime poin. The paper presens a deailed analysis of he planner s convergence rae. I shows ha, if he sae ime space saisfies a geomeric propery called expansiveness, hen a slighly idealized version of our implemened planner is guaraneed o find a rajecory when one exiss, wih probabiliy quickly converging o 1, as he number of of milesones increases. Our planner was esed exensively no only in simulaed environmens, bu also on a real robo. In he laer case, a vision module esimaes obsacle moions jus before planning sars. The planner is hen allocaed a small, fixed amoun of ime o compue a rajecory. If a change in he expeced moion of he obsacles is deeced while he robo execues he planned rajecory, he planner recompues a rajecory on he fly. Experimens on he real robo led o several exensions of he planner in order o deal wih ime delays and uncerainies ha are inheren o an inegraed roboic sysem ineracing wih he physical world. 1 Inroducion In is simples form, moion planning is a purely geomeric problem: given he geomery of a robo and saic obsacles, compue a collision-free pah of he robo beween wo given configuraions. This formulaion ignores several key aspecs of he physical world. In paricular, robo moions are ofen subjec o kinemaic and dynamic consrains (kinodynamic consrains [DXCR93]) ha canno be ignored. Unlike obsrucion by obsacles, such consrains canno be represened as forbidden regions in he configuraion space. Moreover, he environmen may conain moving obsacles, requiring ha compued pahs be paramerized by ime o indicae when he robo is o 1

2 Figure 1: Robo esbed consising of an air-cushioned robo among moving obsacles. achieve a paricular sae. In his paper, we consider moion planning problems wih boh kinodynamic consrains and moving obsacles, and propose an efficien algorihm for his class of problems. In pracice, we also need o consider numerous oher issues (e.g., uncerainy abou he environmen), some of which will be examined here. Our work exends he probabilisic roadmap (PRM) framework originally developed for planning collision-free geomeric pahs [Kav94, KŠLO96, Šve97]. A PRM planner samples he robo s configuraion space a random and reains he collision-free samples as milesones. I hen ries o connec pairs of milesones wih pahs of predefined shape (ypically sraigh-line segmens in configuraion space) and reains he collision-free connecions as local pahs. The resul is an undireced graph, called a probabilisic roadmap, whose nodes are he milesones and he edges are he local pahs. Muli-query PRM planners precompue he roadmap (e.g., [KŠLO96]), while singlequery planners compue a new roadmap for each query (e.g., [HLM97]). I has been proven ha, under reasonable assumpions abou he geomery of he robo s configuraion space, a relaively small number of milesones picked uniformly a random are sufficien o capure he conneciviy of he configuraion space wih high probabiliy [HLM97, KLMR95]. The planner proposed in his paper represens kinodynamic consrains by a conrol sysem, which is a se of differenial equaions ha describes all he possible local moions of a robo. For each query, he planner builds a new roadmap in he collision-free subse of he robo s sae ime space, where a sae ypically encodes boh he configuraion and he velociy of he robo. To sample a new milesone, i firs selecs a conrol inpu a random in he se of admissible conrols and hen inegraes he conrol sysem wih his inpu over a shor duraion of ime, from a previously generaed milesone. By consrucion, he local rajecory hus obained auomaically saisfies he kinodynamic consrains. If his rajecory does no collide wih he obsacles, is endpoin is added o he roadmap as a new milesone. This ieraive incremenal procedure produces a ree-shaped roadmap rooed a he iniial sae ime poin and oriened along he ime axis. I erminaes when a milesone falls in an endgame region from which i is known how o reach he goal. This endgame region may be specifically defined for a given robo. I may also be generaed by he planner by consrucing a second ree of milesones rooed a he goal and inegraing he equaions of moion backwards in ime.

3 Our planner explois he synergy of previously proposed ideas (see Secion ). I makes wo key conribuions, one heoreical and one experimenal: We provide an in-deph analysis of he planner s convergence rae. I shows ha, if he sae ime space saisfies a geomeric propery called expansiveness, hen under suiable assumpions, he probabiliy ha he planner fails o find a rajecory, when one exiss, quickly goes o 0, as he number of milesones increases. The expansiveness propery defined here generalizes a similar noion inroduced in [HLM97] for holonomic robos in saic environmens. The proof of convergence, however, is differen from he one in [HLM97]. The earlier proof assumes ha local moions of he robo are oally unconsrained. I also criically uses he symmery of he conneciviy relaionship in configuraion space if a poin is reachable from a poin, hen is also reachable from. This symmeric relaionship no longer holds when he robo has an asymmeric conrol sysem (e.g., a car-like robo ha can only move forward) or when obsacles are moving. Currenly we do no know how o esimae a priori he degree of expansiveness for a given sae ime space. Hence, our analysis is only one sep oward undersanding he convergence of randomized moion planners. However, we believe ha expansiveness is a very useful concep for characerizing he spaces in which randomized planners are likely o work well (or no so well). I may also help in designing beer sampling sraegies. We also describes our experiences in inegraing he planner ino a hardware robo esbed (Figure 1). In his inegraed sysem, a vision module esimaes obsacle moions jus before planning sars. The planner is hen allocaed a small, fixed amoun of ime (a fracion of a second) o compue a rajecory. If a change in he expeced moion of he obsacles is deeced while he robo execues he planned rajecory, he planner recompues a rajecory on he fly. Experimens on he real robo led o several exensions of he planner o deal wih ime delays and uncerainies ha are inheren o an inegraed roboic sysem ineracing wih he physical world. This is paricularly imporan because kinodynamic consrains are nooriously difficul o model accuraely. Even more difficul is o build an accurae model for predicing fuure obsacle moion. Our experimenal work demonsraes ha a fas planner can reliably handle dynamic environmens, even wih uncerainy in he fuure moions of he obsacles. The res of he paper is organized as follows. Secion reviews previous work. Secion 3 describes he planning algorihm. Secion 4 develops he heoreical analysis of he planner s convergence. Secions 5 hrough 7 describe our experimens wih he planner on a nonholonomic robo and on a dynamically-consrained robo developed o invesigae space roboics asks. In simulaion (Secions 5 and 6), we verified ha he planner can reliably solve ricky problems. In he hardware robo esbed (Secion 7), we verified ha he planner can operae effecively despie various unconrollable uncerainies and ime delays. This paper combines and exends he resuls previously repored in [HKLR00, KHLR00]. For more deails, see [Hsu00, Kin01]. 3

4 Previous Work.1 Moion planning by random sampling Sampling-based moion planning is a classic concep in moion planning (e.g., see [Don87]). Originally, he approach was proposed o boh avoid difficulies encounered in implemening complee planners (e.g., floaing-poin approximaions) and faciliae he incorporaion of search heurisics (e.g., poenial fields). Samples are organized ino regular grids or hierarchical ones (e.g., quadrees in -D configuraion spaces). These grids provide an explici represenaion of he robo s free space and help he search algorihm o remember he poins already visied. Their size, however, grows exponenially wih he dimensionaliy of he configuraion space, i.e., he number of degrees of freedom (dofs) of he robo. Moreover, explicily compuing he geomery of he free subse of a configuraion space wih dimension greaer han four or five urns ou o have a prohibiively high cos. Random sampling more specifically, PRM mehods was inroduced o solve (geomeric) pah planning problems for robos wih many dofs [ABD 98, BK00, BKL 97, BL91, BOvdS99, HLM99, HST94, Hsu00, Kav94, KŠLO96, Kuf99, LH00, SLL01, Šve97]. The cosly compuaion of an explici represenaion of he free space is replaced by a collision es on every randomly picked sample and connecion beween samples. This, of course, can be done wih regular and hierarchical grids, oo. More ineresingly, random sampling provides an incremenal planning scheme which does no arificially depend on he dimensionaliy of he configuraion space. The analysis of he convergence rae of several PRM algorihms reveals he rue value of random sampling [Hsu00, HLM97, KKL98, KLMR95, Šve97]: each new milesone added o a probabilisic roadmap refines he he conneciviy relaionship capured in and reduces he probabiliy ha he planner fails o find a soluion pah, when one exiss (see Secion.3). Various applicaions of randomized planners are reviewed in [La99], including roboics, design for manufacuring and servicing, graphic animaion of digial acors, surgical planning, and predicion of molecular moion. Oher planning approaches (e.g., [Ahu94, HXCW98]) aemp o capure he global conneciviy of a robo s free space by combining exploraion and search in a manner similar o graph search in arificial inelligence.. Sampling sraegies Proposed PRM echniques differ in heir sampling sraegies. An imporan disincion exiss beween muli-query sraegies (e.g., [KŠLO96]) and single-query ones (e.g., [HLM97]). A muli-query planner precompues a roadmap for a given robo and workspace and hen uses his roadmap o process muliple queries. In general, he query configuraions are no known in advance. So he sampling sraegy mus disribue he milesones over he enire free space. In conras, a singlequery planner compues a new roadmap for each query. Here he goal is o find a collision-free pah beween he wo query configuraions by exploring as lile space as possible. Muli-query sraegies are appropriae when he cos of precompuing a roadmap can be amorized over a large number of queries. Single-query ones are appropriae when he number of queries in a given space is small. Inermediae sraegies, which precompue parial roadmaps and complee hem o pro- 4

5 cess specific queries, have also been proposed [BK00, SMA01]. The planner proposed in his paper follows he single-query sampling paradigm. Single-query sraegies ofen build a new roadmap for each query by growing rees of sampled milesones rooed a he iniial and/or goal configuraions [AG99, HLM97, Hsu00, Kuf99, LK99], bu hey differ in he way hey sample he milesones ha form he nodes of he rees. Similar o he planner in [HLM97], our algorihm selecs a milesone in a ree o expand a random, wih probabiliy inverse proporional o he curren densiy of milesones around (see Secion 3.). A new milesone is hen picked by sampling he neighborhood of a random. This guaranees ha he roadmap evenually diffuses hrough he componen(s) of he free space reachable from he query configuraions and ha he milesone disribuion over hese componens converges o a uniform one. This condiion is needed for he analysis of he planner s convergence developed in Secion 4. An alernaive is o firs pick a configuraion in he configuraion space a random, selec o be he milesone in he ree closes o, and hen pick a new milesone along he line connecing o [LK99]. This echnique is slighly simpler o implemen han ours and works well when he query admis a soluion ha does no require long deours. However, his sampling sraegy biases he disribuion of milesones oward hose regions in he configuraion space wih large obsacles. This may be undesirable and severely slow down he rae of convergence of he planner. Anoher possibiliy is o grow he search ree by picking each new milesone as far away as possible from he curren milesones [AG99]. Oher echniques or refinemens of hese echniques are clearly possible. Our experience is ha, alhough one may improve he performance of a PRM planner on some examples by biaising he disribuion of milesones, a sampling sraegy ha yields a uniform disribuion of milesones over he reachable free space avoids pahological cases and gives he bes resuls on he average..3 Probabilisic compleeness A complee moion planner is one ha reurns a soluion whenever one exiss and indicaes ha no such pah exiss oherwise. However, as was shown in [Rei79], pah planning is PSPACE-hard, which is srong indicaion ha any complee planner is likely o be exponenial in he number of dofs of a robo. Adding kinodynamic consrains and moving obsacles furher increases he complexiy of he problem [DXCR93, RS85]. A planner based on random sampling canno be complee. However, a weaker noion of compleeness, called probabilisic compleeness, was inroduced in [BL91]: a planner is probabilisically complee if he probabiliy ha i reurns a correc answer goes o 1 as he running ime increases. Suppose ha a randomized planner reurns a soluion pah as soon as i finds one, and indicaes ha no such pah exiss if i canno found one afer a given amoun of ime. If he planner reurns a pah, he answer mus be correc. If i repors ha no pah exiss, he answer may be someimes wrong. I has been shown ha he probabiliy ha he randomized poenial field planner fails o find a soluion pah when one exiss goes o 0 as he running ime increases, hence proving ha he planner is probabilisically (resoluion) complee [BL91]. Several oher randomized planners have also been proven o be probabilisically complee [AG99, LK01, LL96, Šve97]. Probabilisic compleeness, however, is a weak concep, as i says nohing abou a planner s rae of convergence. In order o undersand why PRM planners work well in pracice and idenify he cases where hey may no work well, we need o show ha hey have a fas convergence rae. 5

6 This requires us o develop a characerizaion of he complexiy of he inpu geomery ha is suiable for random sampling. This characerizaion should no depend on he dimensionaliy of he configuraion space in an arificial way. Afer all, is i really more difficul o sample an empy -dimensional hypercube han o sample an empy square? Along hese lines, i has been shown ha, under suiable assumpions, cerain idealized versions of muli-query PRM pah planners have a convergence rae exponenial in he number of sampled milesones [HLM97, HLMK99, KKL98, KLMR95, Šve97, ŠO98]. More specifically, he noion of -goodness was inroduced o characerize he complexiy of a robo s configuraion space [KLMR95, BKL 97]. If a space is -good, hen wih some limied help from a complee planner, a muli-query PRM planner ha samples milesones uniformly a random from he configuraion space converges a an exponenial rae wih respec o he number of sampled milesones. The proof of his resul relaes PRM mehods o he issue of visibiliy ses sudied in compuaional geomery, in paricular, he ar-gallery problem [O R97]: each milesone is regarded as a guard ha sees a subse of he robo s free space, he milesone s visibiliy region [KLMR95]. This insigh was recenly exploied o generae smaller roadmaps [SLL01]. To remove he need for a complee planner in he proof presened in [BKL 97], expansiveness was inroduced as a more refined characerizaion of he robo s free space. While he compuaional complexiy of a complee planner is usually expressed as a funcion of he number of dofs and he number and he degree of polynomials describing he boundary surface of a robo and obsacles, he rae of convergence of a PRM planner is expressed as a funcion of parameers measuring he degree o which a robo s free space is expansive. Imporanly, he expansiveness of a free space capures he narrow passage issue sudied in [HKL 98]. I reveals he rue narrowness of a passage and is a beer measure han he widh of he passage o capure he difficuly of sampling in a muli-dimensional narrow passage [HLM99]. In his paper, we furher generalize he noion of expansiveness and exend i o sae ime space. We prove ha if he sae ime space is expansive, hen under suiable assumpions, our new randomized planner for kinodynamic planning wih moving obsacles is probabilisically complee wih a convergence rae exponenial in he number of sampled milesones..4 Geomeric complexiy One ene of he PRM approach o moion planning is ha a fas algorihm exiss o check sampled configuraions and connecions beween hem for collision. When boh he robo and he obsacles have simple geomeric shapes, which is he case of mos examples in his paper, his assumpion is clearly saisfied. However, does his remain rue when he robo and he obsacles are complex 3D objecs described by 100,000 riangles or more? During he pas decade, a number of efficien collision checking and disance compuaion algorihms have been developed. The mos popular ones are hierarchical decomposiion algorihms, which precompue a muli-level bounding approximaion of every objec in an environmen, using primiive volumes such as spheres, axis-aligned bounding boxes, or oriened bounding boxes [CLMP95, GLM96, Hub96, KHM 98, KPLM98, Qui94]. Experimens repored in [SA01] indicae ha he PQP package [GLM96] ess wo objecs, described by 500,000 riangles each, in imes ranging beween and seconds (on an Inel Penium III 1GHz processor), depending on he acual disance beween he wo objecs. 6

7 The use of efficien collision checkers in PRM planners has been repored in [BK00, CL95, HLM97, SA01, SLL01]. These planners are capable of efficienly and reliably processing planning queries wih geomeric models conaining hundreds of housands of riangles..5 Moving obsacles When obsacles are moving, he planner mus compue a rajecory paramerized by ime, insead of simply a geomeric pah. This problem has been proven o be compuaionally difficul even for robos wih few dofs [RS85]. A number of heurisic algorihms (e.g., [FS96, Fuj95, KZ86]) have been proposed. The echnique in [KZ86] is a wo-sage approach: in he firs sage, i ignores he moving obsacles and compues a collision-free pah of he robo among he saic obsacles; in he second sage, i unes he robo s velociy along his pah o avoid colliding wih moving obsacles. The resuling planner is clearly incomplee, bu i ofen gives good resuls when he number of moving obsacles is small and/or he workspace is no oo cluered. The planner in [Fuj95] ries o reduce he incompleeness by generaing a nework of pahs. The planner in [FS96] inroduces he noion of a velociy obsacle, defined as he se of velociies ha will cause he robo o collide wih an obsacle a a fuure ime. Velociy obsacles are used o generae an iniial feasible rajecories for he robo, which is laer opimized. The planner can handle acuaor consrains such as bounded acceleraion. The noion of a configuraion ime space was inroduced in [ELP87] o coordinae he moion of muliple robos. I was laer exended in [Fra93] o ha of a sae ime space, where a sae encodes a robo s configuraion and velociy, o plan robo moions wih boh moving obsacles and kinodynamic consrains..6 Kinemaic and dynamic consrains Kinodynamic moion planning refers o problems in which he robo s moion mus saisfy nonholonomic and/or dynamic consrains. Planning for nonholonomic robos has araced considerable ineres (e.g., [BL89, Lau86, LCH89, LJTM94, LM96, ŠO94, SŠLO97]). One approach [Lau86, LJTM94] is o firs generae a collisionfree pah, ignoring he nonholonomic consrains, and hen break his pah ino small pieces and replace hem by admissible canonical pahs (e.g., Reeds and Shepp curves [RS90]). An exension is o perform successive pah ransformaions of various ypes [Fer98, SL98]. A relaed approach [SŠLO97, ŠO94] uses a muli-query PRM algorihm ha connecs milesones by canonical pah segmens such as Reeds and Shepp curves. All hese mehods require he robos o be locally conrollable [BL93, HK77, LCH89, LM96]. An alernaive approach, inroduced in [BL89, BL93], is o generae a ree of sampled configuraions rooed a he iniial configuraion. A each ieraion, a sample is seleced from he ree and expanded o produce new samples, by inegraing he robo s equaions of moion over a shor duraion of ime wih deerminisically picked conrols. A space pariioning scheme regulaes he densiy of samples in any region of he configuraion space. This approach works well for car-like robos and racor-railor robos wih wo o four dofs and is applicable o robos ha are no locally conrollable. Our new planner akes a similar approach, bu picks conrols a random. Neiher he planner nor he analysis of is convergence rae requires he robo o be locally conrollable. Compared o he planner in [BL93] and he planner presened in 7

8 his paper, he wo-sep approach of [Lau86, LJTM94] has he advanage ha i can reach he goal configuraion exacly, which eliminaes he need o define an endgame region, bu i is applicable only o locally conrollable robos. Algorihms for dealing wih dynamic consrains are comparable o hose developed for nonholonomic consrains. In [BDG85, SD91], a collision-free pah is firs compued, ignoring he dynamic consrains; a variaional echnique hen deforms his pah ino a rajecory ha boh conforms o he dynamic consrains and opimizes a crierion such as minimal execuion ime. These mehods work well on many pracical examples; however, no formal guaranee of performance has been esablished for hem. In fac, i is no always possible o ransform he pah generaed in he firs phase ino an admissible rajecory, due o limis on he acuaor forces or orques. The approach in [DXCR93] places a regular grid over he robo s sae space and direcly searches he grid for an admissible rajecory using dynamic programming. I offers provable performance guaranees (resoluion compleeness and an asympoic bound on he compuaion ime), bu i is only applicable o robos wih few dofs (ypically, wo or hree), as he size of he grid grows exponenially wih he number of dofs. The planner in [Fra93] uses a similar approach in he sae ime space of he robo and deals wih moving obsacles as well. Boh our planner and he one in [Kuf99, LK99, LK01] have many similariies wih he approach aken in [BL93, DXCR93, Fra93]. Our planner discreizes he sae ime space via random sampling, insead of placing a regular grid over i. This makes i possible o deal wih robos wih many more dofs. On he oher hand, our planner does no achieve resoluion compleeness as he one in [DXCR93]. Insead, under suiable assumpions, i achieves probabilisic compleeness wih an exponenial convergence rae (Secion 4). The represenaion and he algorihm used in our planner build upon several exising ideas, in paricular: single-query random sampling of configuraion space [HLM97], sae ime space formulaion [BL93, DXCR93, ELP86, Fra93], and represenaion of kinodynamic consrains wih a conrol sysem [BL93, DXCR93, Fra93]. The mos salien conribuions of his work are he generalizaion of expansiveness o sae ime space, he heoreical analysis of he planner s convergence rae, and he inegraion and experimens of he planner on a real robo. 3 Planning framework Our algorihm builds a probabilisic roadmap in he collision-free subse of he robo. The roadmap is compued in he conneced componen of iniial sae ime poin. of he sae ime space ha conains he robo s 3.1 Sae-space formulaion Moion consrains We consider a robo whose moion is governed by an equaion of he form (1) where "! is he robo s sae, is is derivaive relaive o ime, and "$# is he conrol inpu. The ses! and # are he robo s sae space and conrol space, respecively. We assume ha! and 8

9 O ( ) * + \ & ' % Figure : A simple model of a car-like robo. # are bounded manifolds of dimensions and wih -,. By defining appropriae chars, we can rea! and # as subses of R. and R/. Eq. (1) can represen boh nonholonomic and dynamic consrains. The moion of a nonholonomic robo is consrained by 0 independen, non-inegrable scalar equaions of he form , 7 89:;=<><=<? 0, where and denoe he robo s configuraion and velociy, respecively. Define he robo s sae o be $. I is shown in [BL93] ha, under appropriae condiions, he consrains ABC5D 7 E89:;><=<=<F 0 are equivalen o Eq. (1) in which is a vecor in R/ R.HGJI. In paricular, Eq. (1) can be rewrien as 0 LK independen equaions of he form 13 9 M N5. Dynamic consrains are closely relaed o nonholonomic consrains. In Lagrangian mechanics, dynamics equaions are of he form DO P-5, where,, and are he robo s configuraion, velociy, and acceleraion, respecively. Defining he robo s sae as LQ, we can rewrie he dynamics equaions in he form 1R 9 AST5, which, as in he nonholonomic case, is equivalen o Eq. (1). Robo moions may also be consrained by inequaliies of he forms 1U, 5 and ;O, 5. These-consrains resric he ses of admissible saes and conrols o subses of R. and R/. Examples These noions are illusraed below wih wo examples ha will also be useful laer in he paper: Nonholonomic car navigaion. Consider he car example in Figure. Le WVXYZ be he posiion of he midpoin [ beween he rear wheels of he robo and \ be he orienaion of he rear wheels wih respec o he V -axis. Define he car s sae o be WVX]Y^ \ U R_. The nonholonomic consrain `bahc \ YDd V is equivalen o he sysem V egf>h9i \ Y egij c \ We;dlkm `balcon < This reformulaion corresponds o defining he car s sae o be is configuraion WVXYp \ and choosing he conrol inpu o be he vecor qer n, where e and n are he car s speed and seering angle. 9

10 G Bounds on svx]y^ \ and qer n can be used o resric! and # o subses of R_ and R, respecively. For insance, if he maximum speed of he car is 1, hen we have u e uv, 8. Poin-mass robo wih dynamics. For a poin-mass robo w moving on a horizonal plane, we ypically wan o conrol he forces applied o w. This leads us o define he sae of w as WVXYp]eMy9e{z> x, where svx]yd and qemy]emz> are he posiion and he velociy of w. The conrol inpus are chosen o be he forces applied o w in he V - and Y -direcion. Hence he equaions of moion are V emy emy ryad Y emz e{z rz>d () where is he mass of w and W^y9rz> is he applied force. The velociy qely9]e{z= and force q yl] z> are resriced o subses of R due o limis on he maximum velociy and force. Planning query Le!U} denoe he sae ime space! ~ 5; ƒ. Obsacles in he robo s workspace are mapped ino his space as forbidden regions. The free space -! } is he se of all collision-free poins 9. A collision-free rajecory ˆŠ ~ >ŒF Ž ˆ W ocjs F is admissible if i is induced by a funcion M~ =Œb p # hrough Eq. (1). A planning query is specified by an iniial sae ] and a goal sae ime M M?. A soluion o he query is eiher a funcion M~ # ha induces a collision-free rajecory ˆŠ L ~ ] Ž ˆ s š@vw, such ha Js œq=, vw?œq>, or an indicaion ha no admissible rajecory exiss ] M?. This formulaion can be exended o allow o be any insan in some given ime inerval, or o be he earlies possible arrival ime. In he following, we consider piecewise-consan funcions W only. 3. The planning algorihm Our planning algorihm is an exension of he algorihm presened in [HLM97]. I ieraively builds a ree-shaped roadmap rooed a L = b. A each ieraion, i firs picks a random a from, a ime ž wih ž,, and a conrol funcion A~ ž Ÿ #. I hen compues he rajecory induced by by inegraing Eq. (1) If his rajecory lies in, is ž is added o as a new milesone; a direced edge is creaed ž ž, and is sored wih his edge. The kinodynamic consrains are hus naurally enforced in all rajecories represened in. The planner exis wih success when he newly generaed milesone falls in an endgame region ha M?. Milesone selecion The planner assigns a weigh o each milesone in. The weigh is he number of oher milesones lying in he neighborhood of. So indicaes how of densely he neighborhood of has already been sampled. A each ieraion, he planner picks an exising milesone in a random wih probabiliy inversely proporional o. Hence, a milesone lying in a sparsely sampled region has a greaer chance of being seleced han a milesone lying in an already densely sampled region. This echnique avoids oversampling any paricular region of. Conrol selecion Le be he se of all piecewise-consan conrol funcions wih a mos admis a finie pariion ª «N ]Œ «<=<>< «E such ha # s over he ime inerval W Œ, for 7 89:;><=<=<F. We also require K consan pieces. So every is a consan 10

11 ± G G Œ, ±²X³@, where ±²X³@ is a consan. Our algorihm picks a conrol µ U, for some prespecified and ±F²X³@, by sampling each consan piece of independenly. For each piece, and K Œ are seleced uniformly a random from # and ~ 5D ±>²X³@, respecively. The specific choices of he parameers and ±F²X³@ will be discussed in Secion 4.5. In he acual implemenaion of he algorihm, however, one may chose 8, because any rajecory passing hrough several consecuive milesones in he ree is obained by applying a sequence of consan conrols. Endgame connecion Unlike some oher planning echniques (e.g., [Lau86, LJTM94]), he above conrol-driven sampling echnique does no allow us o reach he goal 9 M? exacly. We need o expand he goal ino an endgame region ha he sampling algorihm will evenually aain wih high probabiliy. There are several ways of creaing such a region: In [BL93], he endgame region is defined o be a ball of small radius cenered a he goal. Any poin in his ball is considered o be a sufficienly good approximaion of he specified goal. This echnique is pracical only in spaces of small dimensionaliy, as he relaive volume of a ball of small fixed radius goes oward 0 as he dimensionaliy increases. We could neverheless adap his echnique by seing he parameer ±M²X³@ proporional o he disance beween he milesone picked from and he goal, allowing he densiy of milesones o increase in he goal s viciniy, and erminaing wih success when he planner samples a new milesone close enough o he goal. For some robos, i is possible o analyically compue one or several canonical conrol funcions ha exacly connec wo given poins while obeying he kinodynamic consrains. An example is he Reeds and Shepp curves [RS90] developed for nonholonomic car-like robos. If such conrol funcions are available, one can es if a milesone belongs o he engame region by checking wheher a canonical conrol funcion generaes a collision-free rajecory from M?. A more general mehod is o build a secondary ree ž of milesones from he goal in he same way as ha for he primary ree, excep ha Eq. (1) is inegraed backwards in ime. Le =ž ž¹ be a new milesone obained by inegraing backwards from an exising milesone in ž. By consrucion, if he ime goes forward, he conrol funcion drives he robo from sae ž a ime ž o sae a ime (Figure 3). Thus here is a known rajecory from every milesone in ž o he goal. The sampling process erminaes wih success when a milesone is in he neighborhood of a milesone ž ž. In his case, he endgame region is he union of he neighborhoods of milesones in ž. To generae he final rajecory, we simply follow he appropriae edges of and ž ; however, here is a small gap beween and ž. The gap can ofen be deal wih in pracice. For example, beyond, one can use a PD conroller o rack he rajecory exraced from ž. Consrucing endgame regions by backward inegraion is a very general echnique and can be applied o any sysem described by (1). In Secions 5 7, we will presen implemenaions of he planner, using he las wo echniques described above. Endgame region can also be used when he goal does no have a unique configuraion. For example, in [AG99], he goal region is defined o be he subse of configuraions of a redundan robo such ha he end-effecor achieves a given posure. 11

12 Ô Ô» º» ÐFÑ ÍFÄFÄÂÆÄÇ ÁÂÆÅX¼3ÇÃ>ÂÆÄÇ Ò]Í] Ó ËZÍbÊÏÎ ¼¾½¼ À Á Ã=Ä ÁÂÆÅX¼ ÇbÃ>ÂÆÄÇ ÈÉÃFÊÌË Í]ÊÏÎ º Figure 3: Building a secondary ree of milesones by inegraing backwards in ime. Algorihm in pseudo-code The planning algorihm is summarized in he following pseudo-code. Algorihm 1 Conrol-driven randomized expansion. 1. Inser ino ; 7 Õ 8.. repea 3. Pick a milesone from wih probabiliy Ö. 4. Pick a conrol funcion from uniformly a random. 5. ž Õ PROPAGATE ]p. 6. if ž ÙØ ÚsÛ hen 7. Add ž o ; 7 Õ 7 Ü8. 8. Creae an edge Ý from o ž ; sore wih Ý. 9. if ž^ ENDGAME hen exi wih SUCCESS. 10. if 7 ßÞ hen exi wih FAILURE. In line 5, PROPAGATE ]p inegraes he equaions of moion from wih conrol. I reurns a new milesone ž if he compued rajecory is admissible; oherwise i reurns nil. If here exiss no admissible rajecory from ] o = M?, he algorihm canno deec i. Therefore, in line 10, we bound he maximum number of milesones o be sampled by a consan Þ. The oucome FAILURE may be inerpreed as here exiss no soluion rajecory, bu his answer may be incorrec. The above algorihm can poenially benefi from more sophisicaed sampling sraegies, bu hese sraegies considerably complicae he following formal analysis. Moreover, he sampling sraegy in Algorihm 1 gave very saisfacory experimenal resuls (see Secions 5 7). 1

13 å à á Figure 4: A free space wih a narrow passage 4 Analysis of he Planner The experimens o be described in Secions 5 7 demonsrae ha Algorihm 1 provides an efficien soluion for difficul kinodynamic moion planning problems. Neverheless some imporan quesions canno be answered by experimens alone. Wha is he probabiliy ã ha he planner fails o find a rajecory when one exiss? Does ã converge o 5 as he number of milesones increases? If so, how fas? In his secion, we generalize he noion of expansiveness, originally proposed in [HLM97] for (geomeric) pah planning. We show ha in an expansive space, he failure probabiliy ã decreases exponenially wih he number of sampled milesones. Hence, wih high probabiliy, a relaively small number of milesones are sufficien o capure he conneciviy of he free space and answer he query correcly. 4.1 Expansive sae ä ime space Expansiveness ries o characerize how difficul i is o capure he conneciviy of he free space by random sampling. To be concree, consider he simple example shown in Figure 4. Assume ha here are no kinodynamic consrains and a poin robo can move freely in he space shown. Le us say ha wo poins in he free space see each oher equivalenly, are muually visible if he sraigh line segmen beween hem lies enirely in. The free space in Figure 4 consiss of wo subses å Œ and å conneced by a narrow passage. Few poins in å Œ see a large fracion of å. Recall ha a classic PRM planner samples uniformly a random and ries o connec pairs of milesones ha see each oher. Le he lookou of å Œ be he subse of all poins in å Œ ha sees a large fracion of å. If he lookou of å Œ were large, i would be easy for he planner o sample a milesone in å Œ and anoher in å ha see each oher. However, in our example, å Œ has a small lookou due o he narrow passage beween å Œ and å : few poins in å Œ see a large fracion of. Thus i is difficul for he planner o generae a connecion beween å Œ and å. This example suggess ha we can characerize he complexiy of a free space for random sampling by he size of lookou ses. In [HLM97], a free space is said o be expansive if every subse å æ has a large lookou. I has been shown ha in an expansive space, a classic PRM planner wih uniform random sampling converges a an exponenial rae as he number of sampled milesones increases. When kinodynamic consrains are presen, he basic issues remain he same, bu he noion of visibiliy (connecing milesones wih sraigh-line pahs) is inadequae. Algorihm 1 generaes a differen kind of roadmaps, in which rajecories beween milesones may be neiher sraigh, nor 13

14 é ë ç è édê ë ê Figure 5: The lookou of a se å. reversible. This leads us o generalize he noion of visibiliy o ha of reachabiliy. Given wo poins 9 ž in ž¹ is reachable from if here exiss a conrol funcion A~ ž Ö # ha induces an admissible rajecory o ž ž. ž ž remains reachable from 9 by using î, a piecewise-consan conrol wih a mos consan pieces as defined in Secion 3., hen we say ha ž ž is locally reachable, or -reachable, Le ï and ïœ denoe he se of poins reachable and -reachable from some poin, respecively; we call hem he reachabiliy and he -reachabiliy se of. For any subse åü ð, he reachabiliy ( -reachabiliy) se of å is he union of he reachabiliy ( -reachabiliy) ses of all he poins in å : ï å òñ ó=ô=õ We define he lookou of a se åø ƒ ï alczö ïp å R ñ ó=ôaõ ïp F< as he subse of all poins in å whose -reachabiliy ses overlap significanly wih he reachabiliy se of å ha is ouside å (Figure 5): Definiion 1 Le ù be a consan in q5d=8. The ù -lookou of a se åú is ù -LOOKOUT å û åüumü ï4 Šý å þ where ü denoe he volume of a se. ùÿü ï å Öý å v The free space is expansive if for every poin, every subse å ï has a large lookou: Definiion Le and ù be wo consans in q5d=8. For any, he se ï is ù - expansive if for every conneced subse å ï, ü ù -LOOKOUT å Uþ Sü å < The free space is ù -expansive if for every, ï is ù -expansive. To beer grasp hese definiions, hink of in Definiion as he iniial ] as he -reachabiliy se of a se of milesones sampled by Algorihm 1. If and ù are and å 14

15 G boh reasonably large, hen Algorihm 1 has a good chance o sample a new milesone whose - reachabiliy se adds significanly o he size of å. In fac, we show below ha wih high probabiliy, he -reachabiliy se of he sampled milesones expands quickly o cover mos of ï ; hence, if he M? lies in ï, hen he planner will quickly find an admissible rajecory wih high probabiliy. 4. Ideal sampling To simplify our presenaion and focus on he mos imporan aspecs of our planner, le us assume for now ha we have an ideal sampler IDEAL-SAMPLE ha picks a poin uniformly a random from he -reachabiliy se of exising milesones. If i is successful, IDEAL-SAMPLE reurns a new milesone ž and a rajecory from an exising milesone planning algorihm can be resaed as follows: o ž. Wih ideal sampling, he Algorihm Randomized expansion wih IDEAL-SAMPLE. 1. Inser ª ino a ree ; [ ª ÕòïP ª.. repea 3. Invoke IDEAL-SAMPLE [, which samples a new milesone ž and reurns a rajecory from an exising milesone o ž if he rajecory is admissible. 4. if ž nil hen 5. Inser ž ino. 6. Creae a direced edge Ý from o ž, and sore he rajecory wih Ý. 7. [ Œ Õ [ ïp ž¹ ; 7 Õš7 ß8. 8. if ž^ ENDGAME hen exi wih SUCCESS. This algorihm is he same as Algorihm 1, excep ha he use of IDEAL-SAMPLE replaces lines 3 5 in Algorihm 1. We will discuss how o approximae IDEAL-SAMPLE in Secion Bounding he number of milesones Le ï be he se of all poins reachable from under piecewise-consan conrols. Algorihm 1 deermines wheher he goal lies in by sampling a se of milesones; i erminaes as soon as a milesone falls in he endgame region. The running ime of he planner is hus proporional o he number of sampled milesones. In his subsecion, we give a bound on he number of milesones needed in order o guaranee a milesone in he endgame region wih high probabiliy, assuming he inersecion of he endgame region and is non-empy. Le ªA ŒF =<=<=< be a sequence of milesones generaed by Algorihm, and le denoe he firs 7 milesones in. A milesone is called a lookou poin if i lies in he ù -lookou of ï Œ. Lemma 1 below saes ha he -reachabiliy se of spans a large volume if i conains enough lookou poins, and Lemma gives an esimae of he probabiliy of his happening. Togeher hey imply ha wih high probabiliy, he -reachabiliy se of a relaively small number of milesones spans a large volume in. The following resuls assume ha is ( ù )-expansive. For convenience, le us scale up all he volumes so ha ü RÙ8. 15

16 I G I I G ' I G Œ G G G I G G G G G 8 Figure 6: A sequence of sampled milesones. Lemma 1 If a sequence of milesones conains 0 lookou poins, hen ü ïx þ 8 K Ý G>I. Proof. Le =<=<=<? be he subsequence of lookou poins in, where 7 ªA 7 Œ 7 =<=<=< give he indices of he lookou poins in he sequence we have Thus ü ïœ ü ïp where is a lookou poin, we ge Le e e þƒe Noe 7 Seing m ü ï ŒÖ ü ïp Uþ ü ïp "! for all 7 þ$#, in paricular, ªA Œ ü ïp ŒŠ ù 8 K e K 8Bþ 7 I?G ðe ü ïp =<=<=<? æ ª= ŒF Öý ïp ><=<=<. For any 7 Ù89:;=<><=<, Œ F< (3) ü ïp þ ü ïp F (4). Using (3) wih 7 7 in combinaion wih he fac ha þ ü ïp. Since ü ý ïp Œ Ö Œ, which can be rewrien as ùÿü ý ïp Œ ü mk ü ïp e þƒe &% ù 8 K e &% Ö 8 K ù?qe Œ (Figure 6) and hus e leads o he recurrence þù 8 K ù I ª ù Œ K Œ F< Œ C8 K e Œ, we have Œ K e &% F< (5) e &% þü5. I follows from (5) ha e þƒe &% ù 8 K e &% F< IFG!)( ª þ ŒÖ IFG ù 8 K I?G Œ, wih he soluion 8 K ù! 8 Kß 8 K ù I 8 K ª< Since * ª,+.- and / G, we ge * I + /1056 G=I. Combined wih (4), i yields ü ï þ 8 K Ý G=I < Lemma A sequence of 9 milesones conains 0 lookou poins wih probabiliy a leas 8 K 0DÝ G;:=<?>¾I. Proof. Le be a sequence of 9 milesones, and k be he even ha conains 0 lookou poins. he even ha he ih subsequence conains a leas one lookou poin. Since he probabiliy of having We divide M ino 0 subsequences of 9 d 0 consecuive milesones each. Denoe by k 16

17 G G I I I ã : < 8 0 lookou poins is greaer han he probabiliy of every subsequence having a leas one lookou poin, we have which implies Pr km þ Pr kg, Pr kgœ k Pr kmœa@ k <=<=< k <=<=<B@îk, ' ( ª Pr k < Since each milesone picked by IDEAL-SAMPLE has probabiliy of being a lookou poin, he probabiliy Pr k of having no lookou poin in he ih subsequence is a mos 8 K <?>¾I. Hence Pr km 8 K Pr kg þ 8 K 0 8 K <?>¾I < Noe ha 8 K <?>¾I,ßÝ G;:=<?>¾I. So we have Pr km þ 8 K 0DÝ G;:=<C>¾I. 8 The main resul, saed in he heorem below, esablishes a bound on he number of milesones needed in order o guaranee a milesone in he endgame region wih high probabiliy. Theorem 1 Le DFE 5 be he volume of he endgame region G in and ã be a consan in 5;=8. A sequence of 9 milesones conains a milesone in G wih probabiliy a leas 8K ã, if 9 þ 0 d H c : 0 d ã Š ð@:9d D H c :9d ã, where 0 æ 8Ad ù H D. ino wo subsequences ž and ž ž such ha ž Proof. Le us divide ª= ŒF ><=<=<> < conains he firs 9 ž milesones and ž ž conains he nex 9 ž žd 9 K 9 ž milesones. By Lemma, ž conains 0 lookou poins wih probabiliy a leas 8oK 0 8 K <JIK>¾I. If here are 0 lookou poins in ž, hen by Lemma 1, ï ž has volume a leas 8oK D dl:, provided ha 0 þ 8Md ù H D < As a resul, ïp ž has a non-empy inersecion L wih he endgame region of volume a leas D d9:, and so does ïœ, for 7 þ 9 ž. The procedure IDEAL-SAMPLE picks a milesone uniformly a random from he -reachabiliy se of he exising milesones, and herefore every milesone in ž ž falls in L wih probabiliy 8 for all 7, and he milesones are sampled indepen- >. fails o conain a milesone in he endgame region G, hen eiher he -reachabiliy se D d9:ld ü ïp Œ. Since ü ïp Œ, denly, ž ž conains a milesone in L wih probabiliy a leas 8 KÜ 8 K D d9:9 < II þ 8 K Ý G< II If of ž does no have a large enough inersecion wih G (even M ), or no milesone of ž ž lands in he inersecion (even N ). From he preceding discussion, We know ha Pr M, ã d9: if 9 žrþt 0 d H c : 0 d ã and Pr N, ã d9: if 9 ž žrþ :9d D H ã. Choosing 9 þt 0 d H c : 0 d D H ã guaranees ha Pr M ino he inequaliy bounding 9, we ge he final resul 9 þ N, Pr M 3 Pr N,ã. Subsiuing 0 µ 8Md ù H c :9d D H D ù H c :OH D ùxã, or he algorihm has failed o find one. The laer even, which corresponds o reurning an incorrec If he planner reurns FAILURE, eiher he query admis no soluion, M? 17 : D H c

18 answer o he query, has probabiliy less han ã. Since he bound in Theorem 1 conains only logarihmic erms of ã, he probabiliy of an incorrec answer converges o 0 exponenially in he number of milesones. The bound given by Theorem 1 also depends on he expansiveness parameers, ù and he volume D of he endgame region. The greaer, ù, and D, he smaller he bound. In pracice, i is ofen possible o esablish a lower bound for D. However, and ù are difficul o esimae, excep for every simple cases. This prevens us from deermining he parameer Þ, he maximal number of milesones needed for Algorihm 1 a priori. Neverheless hese resuls are imporan. Firs, hey ell us ha he failure probabiliy of our planner decreases exponenially wih he number of milesones sampled. Second, he number of milesones needed increases only moderaely when and ù decrease, i.e., when he space becomes less expansive. 4.4 Approximaing IDEAL-SAMPLE The above analysis assumes he use of IDEAL-SAMPLE, which picks a new milesone uniformly a random from he -reachabiliy se of he exising milesones. One way o implemen IDEAL-SAMPLE would be rejecion sampling [KW86], which hrows away a fracion of samples in regions ha are more densely sampled han ohers. However, rejecion sampling is no efficien: many poenial candidaes are hrown away in order o achieve he uniform disribuion. So insead, our implemened planners ry o approximae he ideal sampler. The approximaion is much faser o compue, bu generaes a slighly less uniform disribuion. Recall ha o sample a new milesone ž, we firs choose a milesone from he exising milesones and hen sample in he neighborhood of. Every new milesone ž hus creaed ends o be relaively close o. If we seleced uniformly among he exising milesones, he resuling disribuion would be very uneven; wih high probabiliy, we would pick a milesone in an already densely sampled region and obain a new milesone in ha same region. Therefore he disribuion of milesones ends o cluser around he iniial sae ime poin. To avoid his problem, we associae wih every milesone a weigh, which is he number of milesones in a small neighborhood of, and pick an exising milesone o expand wih probabiliy inversely proporional o. So i is more likely o sample a region conaining a smaller number of milesones. The disribuion QP 8Md conribues o he diffusion of milesones over he free space and avoids oversampling any paricular region. In general, mainaining he weighs as he roadmap is being buil incurs a much smaller compuaional cos han performing rejecion sampling. There is also a slighly greaer chance of generaing a new milesone in an area where he -reachabiliy ses of several exising milesones overlap. However, milesones wih overlapping -reachabiliy ses are more likely o be close o one anoher han milesones wih no such overlapping. Thus i is reasonable o expec ha using X Pæ8Md s keeps he problem from worsening as he number of milesones grows. If here are no kinodynamic consrains on he robo s moion, hen oher han he wo issues menioned above, Theorem 1 gives an asympoic bound ha closely characerizes he amoun of work ha he planner mus do in order o guaranee finding a rajecory wih high probabiliy whenever one exiss. In paricular, he resul applies o (geomeric) pah planning problems. There is, however, one more issue o consider when kinodynamic consrains are presen. Alhough line 4 of Algorihm 1 selecs uniformly a random from, he disribuion of ž in 18

19 ïp is no uniform in general, because he mapping from U o ïp may no be linear. In some cases, one may precompue a disribuion SR such ha picking from wih probabiliy TR qp yields a uniform disribuion of ž in ïp. In oher cases, rejecion sampling can be used locally. Firs pick several conrol funcions 7 8l:;=<=<>< and compue he corresponding ž he endpoin of he rajecory induced by disribuion among he remaining ž s, and pick a remaining ž a random. 4.5 Choosing suiable conrol funcions,. Then hrow away some of hem o achieve a uniform To sample new milesones, Algorihm 1 picks a random a piecewise-consan conrol funcion from. Every has a mos consan pieces, each of which lass for a ime duraion less han ±F²X³@. The parameers and ±F²X³@ are chosen according o specific properies of each robo. In heory, mus be large enough so ha for any ï, ïp has he same dimension as ï. Oherwise, ïœ has zero volume relaive o ï, and ï canno be expansive even for arbirarily small values of and ù. This can only happen when some dimensions of ï are no spanned direcly by basis vecors in he conrol space #, bu hese dimensions can hen be generaed by combining several conrols in # using Lie-brackes [BL93]. The mahemaical definiion of a Lie bracke can be inerpreed as an infiniesimal maneuver involving wo conrols. by Spanning all he dimensions of ï may require combining more han wo conrols of imbricaing muliple Lie brackes. A mos KP: Lie brackes are needed, where is he dimension of he sae space. Hence i is sufficien o choose ÜLKú:. In general, he larger is, he greaer and ù end o be. So according o our analysis, fewer milesones are needed; on he oher hand, he cos of inegraion and collision checking during he generaion of a new milesone becomes more expensive. The choice of ±A²X³@ is somewha relaed. A larger ± y may resul in greaer and ù, bu also lead he planner o inegrae longer rajecories /VU ha are more likely o be inadmissible. Experimens show ha and ±l²x³@ can be seleced in relaively wide inervals wihou significan impac on he performance of he planner. However, if he values for and ±F²X³@ are oo large, he approximaion o IDEAL-SAMPLE becomes very poor. 5 Nonholonomic robos We implemened Algorihm 1 for wo differen robo sysems. One consiss of wo nonholonomic cars conneced by a elescopic link and moving among saic obsacles. The oher is an air-cushioned robo ha is acuaed by air hrusers and operaes among moving obsacles on a fla able. The air-cushioned robo is subjec o sric dynamic consrains. In his secion, we discuss he implemenaion of Algorihm 1 for he nonholonomic cars. In he nex wo secions, we will do he same for he air-cushioned robo. 5.1 Robo descripion Wheeled mobile robos are a classical example for nonholonomic moion planning. The robo considered here is a new variaion on his heme. I consiss of wo independenly-acuaed cars moving on a fla surface (Figure 7). Each car obeys a nonholonomic consrain and has nonzero minimum urning radius. In addiion, he wo cars are conneced by a elescopic link whose 19

20 Y V Y \ n n \ < WJ F Figure 7: Two-car nonholonomic robos. XWJ Cooperaive mobile manipulaors. F Two wheeled nonholonomic robos ha mainain a direc line of sigh and a disance range. lengh is lower and upper bounded. This sysem is inspired by wo scenarios. One is he mobile manipulaion projec a he Universiy of Pennsylvania s GRASP Laboraory [DK99]; he wo cars are each mouned wih a manipulaor arm and mus remain wihin a cerain disance range so ha he wo arms can cooperaively manipulae an objec (Figure 7 W ). The manipulaion area beween he wo cars mus be free of obsacles. In he oher scenario, wo cars parolling an indoor environmen mus mainain a direc line of sigh and say wihin a cerain disance range, in order o allow visual conac or simple direcional wireless communicaion (Figure 7 ). We projec he geomery of he cars and he obsacles ono he horizonal plane. For 7 89:, be he midpoin beween he rear wheels of he 7 h car, 1 be he midpoin beween he fron wheels, and k be he disance beween [ and 13. We define he sae of he sysem by S WVŠŒF]YvŒ \ Œ V ]Y \, where WV ]Y are he coordinaes of [, and \ is he orienaion of he rear wheels of 7 h car relaive o he V -axis (Figure ). To mainain a disance range beween he wo car, we require Y9²[Z]\,_^ svšœk V ðqyvœ K,`Y9²X³@ for some consans Yv²[Z]\ and Y9²X³@. Each car has wo scalar conrols, and n, where, and n is he seering le [ angle. The equaions of moion for he sysem are VŠŒQ ŠŒJf>h9i \ Œ YJŒ ŠŒJij c \ Œ \ ŒQ qšœ dhk Œ `balcon Œ The conrol space is resriced by u uš, and seering angles. 5. Implemenaion deails ²X³@ and u uš, is he speed of [ W f>hli \ ij c dlk `balcon (6) ²X³@, which bound he cars velociies We assume ha all obsacles are saionary. So he planner builds a roadmap in he robo s 6-D sae space (wihou he ime dimension). Compuing he weighs To compue he weigh of a milesone, we define he neighborhood of o be a small ball of fixed radius cenered a. The curren implemenaion uses a 0

21 WJ F Figure 8: Compued examples for nonholonomic carl-like robos. naive mehod ha checks every new milesone ž agains all he milesones currenly in. Thus, for every new milesone, updaing akes linear ime in he number of milesones in. More efficien range search echniques [Aga97] would cerainly improve he planner s running ime for problems requiring very large roadmaps. Implemening PROPAGATE Given a milesone and a conrol funcion, PROPAGATE uses he Euler mehod wih a fixed sep size o inegrae (6) from and compues a rajecory a of he sysem under he conrol. More sophisicaed inegraion mehods, e.g., fourh-order Runge- Kua or exrapolaion mehod [PTVP9], can improve he accuracy of inegraion, bu a a higher compuaional cos. We hen discreize a ino a sequence of saes and reurns nil if any of hese saes is in collision. For each car, we precompue a 3-D bimap ha represens he collision-free configuraions of he car prior o planning. I hen akes consan ime o check wheher a given configuraion is in collision. A well-known disadvanage of his mehod is ha if he resoluion of he bimap is no fine enough, we may ge wrong answers. In he experimens repored below, we used an 8M:cb 8A:cb edcf bimap, which was adequae for our es cases. Endgame region We obain he endgame region by generaing a secondary ree ž of milesones from he goal A. 5.3 Experimenal resuls We experimened wih he planner in many workspaces. Each one is a 10 m 10 m square region wih saic obsacles. The wo cars are idenical, each represened by a polygon conained in a circle wih diameer 0.8 m, and k Œoæk æ5d<hg m. The speed of he cars ranges from Kji m/s o i m/s, and is seering angle n varies beween Kji95k and i95lk. The allowable disance beween [ Œ and [ ranges from 89< f m o id<hi m. Figure 8 shows hree compued examples. Environmen XWJ is a maze; he robo cars mus navigae from one side of i o he oher. Environmen F conains wo large obsacles separaed by a narrow passage. The wo cars, which are iniially parallel o one anoher, change formaion 1

22 Scene Time (sec.) monqp rqsu m1vw p m1xy z mean sd mean sd {} B~ {?~ { &~ Table 1: Performance saisics of he planner on he nonholonomic robo. and proceed in a single file hrough he passage, before becoming parallel again. Environmen consiss of wo rooms cluered wih obsacles and conneced by a hallway. The cars need o move from he room a he boom o he one a he op. The maximum seering angles and he size of he circular obsacles conspire o increase he number of required maneuvers. We ran he planner for several differen queries in each workspace shown in Figure 8. For every query, we ran he planner 30 imes independenly wih differen random seeds. The resuls are shown in Table 1. Our planner was wrien in C++, and he running imes repored were obained on an SGI Indigo worksaion wih a 195 Mhz R10000 processor. Every row of he able corresponds o a paricular query. Columns 5 lis he average running ime, he average number of collision checks, and heir sandard deviaions. Columns 6 7 give he oal number of milesones sampled and he number of calls o PROPAGATE. The running imes range from less han a second o a few seconds. The firs query in environmen akes longer because he cars mus perform several maneuvers in he hallway before reaching he goal (see he example in Figure 8 ). The sandard deviaions in Table 1 are larger han wha we would like. In Figure 9, we show a hisogram of more han 100 independen runs for a paricular query. In mos runs, he running ime is well under he mean or slighly above. This indicaes ha our planner performs well mos of he ime. The large deviaion is caused by a few runs ha ake as long as four imes he mean. The long and hin ail of he disribuion is ypical of he ess ha we have performed. 6 Air-cushioned robos 6.1 Robo descripion Our algorihm has also been implemened and evaluaed on a second sysem, which was developed a he Sanford Aerospace Roboics Laboraory for esing space roboics echnology. This aircushioned robo (Figure 1) moves fricionlessly on a fla granie able among moving obsacles. Eigh air hrusers provides omni-direcional moion capabiliy, bu he hrus available is small compared o he robo s mass, resuling in igh acceleraion consrains. We define he sae of he robo o be svx]yp VX Y;, where WVX]YD are he coordinaes of he robo s

23 running ime (seconds) Figure 9: Hisogram of planning imes for more han 100 runs on a paricular query. The average ime is 1.4 sec, and he four quariles are 0.6, 1.1, 1.9, and 4.9 seconds. cener, and VX where Y; is he velociy. The equaions of moion are V O f>h9i \ alczö YŸ O is he robo s mass, and and \ are he magniude and direcion of he force generaed by he hrusers. We have 5, pd E, 5;<Ì5:cg m/s and 5lk,Ü\, i d 5lk. The maximum speed of he robo is 0.18 m/s. For planning purposes, he workspace is represened by a 3 m i j c \ 4 m recangle, he robo by a disc of radius 0.5 m, and he obsacles by discs of radii beween 0.1 and 0.15 m. The planner assumes ha he obsacle moves along a sraigh-line pah a consan speed beween 5 and 5D<Ì: m/s (more complex rajecories will be considered in Secion 7.4). When an obsacle reaches he workspace s boundary, i leaves he workspace and is no longer considered a hrea o he robo. 6. Implemenaion deails The planner builds a roadmap in he robo s 5-D sae ime space, I is given an iniial sae ]]5 and a goal sae ime M M?, where is any ime less han a given ²X³@. In addiion, he planner is given he obsacle rajecories. Unlike he experimens wih he real robo in he nex secion, planning ime is no limied here. This is equivalen o assuming ha he world is frozen unil he planner reurns a rajecory. Compuing he weighs The 3-D configuraion ime space of he robo is pariioned ino an array of idenically sized recangular boxes called bins. When a milesone is insered in, he planner adds i o he lis of milesones associaed wih he bin in which i falls. To implemen line 3 of Algorihm 1, he planner firs picks a random a bin conaining a leas one milesone and hen a milesone from wihin his bin. Boh choices are made uniformly a random. This corresponds o picking a milesone wih probabiliy approximaely proporional o he inverse of he densiy of samples in he robo s configuraion ime space (raher han is 5-D sae ime space). We did some experimens wih bins in sae ime space, bu he resuls did no differ significanly. 3

24 Scene Time (sec) movw p mean sd mean sd {} B~ {?~ {} &~ Table : Performance saisics of he planner on he air-cushioned robo. Implemening PROPAGATE The simpliciy of he equaions of moion makes i possible o compue rajecories analyically. The rajecories are hen discreized, and a each discreized sae ime poin, he robo is checked for collision agains every obsacle. This naive echnique works reasonably well when he number of obsacles is small, bu can be easily improved o handle a large number of obsacles. Endgame region The endgame region is generaed wih specialized curves, specifically, hirdorder splines. Whenever a new milesone is added o, i is checked for connecion wih 0 goal poins A M?, for some pre-defined consan 0. Each of he 0 values of is chosen uniformly a random from he inerval ~ ²[Z]\ ²X³@, where ²[Z]\ is an esimae of he earlies ime when he robo may reach >, given is maximum velociy. For each value of ], he planner compues he hirdorder spline beween M?. I hen verifies ha he spline is collision free and saisfies he velociy and acceleraion bounds. If all he ess succeed, hen lies in he endgame region. In all he experimens repored below, 0 is se o Experimenal resuls We performed experimens in more han one hundred simulaed environmens. To simplify he simulaion, collisions among obsacles are ignored. So wo obsacles may overlap emporarily wihou changing courses. In a small number of queries, he planner failed o reurn a rajecory, bu in none of hese cases were we able o deermine wheher an admissible rajecory acually exised. On he oher hand, he planner successfully solved several queries for which we iniially hough here was no soluion. Three examples compued by he planner are shown in Figure 10. For each example, we display five snapshos labeled by ime. The large gray disc indicaes he robo; he smaller black discs indicae he obsacles. The solid and doed lines mark he rajecories of he robo and he obsacles, respecively. For each of he hree queries, we ran he planner 100 imes independenly wih differen random seeds. The planner successfully reurned a rajecory in all runs. Table liss he means and sandard deviaions of he planning imes and he number of sampled milesones for each query. The repored imes were obained from a planner wrien in C and running on a Penium-III PC wih a 550 Mhz processor and 18 MB of memory. In he firs wo examples, he moving obsacles creae narrow passages hrough which he robo mus pass in order o reach he goal. Ye planning ime remains much under one second. The fac ha he planner never failed in 100 runs esifies o is reliabiliy. To poin ou he difficuly of hese queries, we show in Figure 11 he configuraion ime space for example F. In he configuraion ime space, he robo maps o a poin svx]y^. Since he obsacles are assumed 4

25 T = 0.0 secs T = 11. secs T =.4 secs T = 33.7 secs T = 44.9 secs WJ T = 0.0 secs T = 9.0 secs T = 0.0 secs T = 30.0 secs T = 39. secs F T = 0.0 secs T = 8.0 secs T = 16.1 secs T = 4.1 secs T = 3.1 secs Figure 10: Compued examples for he air-cushioned robo. o move wih consan linear velociy, hey map ino cylinders. The velociy and acceleraion consrains require every soluion rajecory o pass hrough a small gap beween he cylinders. Example is much simpler. There are wo saionary obsacles obsrucing he middle of he workspace and hree moving obsacles. Planning ime is well below 0.01 second, wih an average of 0.00 second. The number of milesones is also small, confirming he resul of Theorem 1 ha when he space is expansive, Algorihm 1 is very efficien. As in he experimens on nonholonomic robo cars, he running ime disribuion of he planner ends o have a long and hin ail due o long execuion ime in a small number of runs, bu overall he planner is very fas. 5

26 ;ƒ l Figure 11: Configuraion space for he example in Figure Experimens wih he real robo To furher es he performance of he planner, we conneced he planner described in he previous secion o he air-cushioned robo in Figure 1. In hese ess, we examined he behavior of Algorihm 1 running in real-ime mode on a sysem inegraing conrol and sensing modules over a disribued archiecure and operaing in a physical environmen wih uncerainies and ime delays. 7.1 Tesbed descripion The robo shown in Figure 1 is unehered and moves fricionlessly on an air bearing on a 3 m 4 m able. Gas anks provide compressed air for boh he air-bearing and hrusers. An onboard Moorola ppc604 compuer performs moion conrol a 60 Hz. Obsacles are also on air-bearings, bu have no hrusers. They are iniially propelled by hand from various locaions and hen move fricionlessly on he able a roughly consan speed unil hey reach he boundary of he able, where hey sop due o he lack of air bearing. An overhead vision sysem esimaes he posiions of he robo and he obsacles a 60 Hz by deecing LEDs placed on he moving objecs. The measuremen is accurae o 5 mm. Velociy esimaes are derived from posiion daa. Our planner runs offboard on a 333 Mhz Sun Sparc 10. The planner, he robo, and he vision module communicae over he radio Eherne. 7. Sysem inegraion Implemening he planner on he hardware esbed raises several new challenges. Time delays Various compuaions and daa exchanges occurring a differen pars of he sysem lead o delays beween he insan when he vision module measures he moion of he robo and he obsacles and he insan when he robo sars execuing he planned rajecory. These delays, 6

27 if ignored, would cause he robo o begin execuing he planned rajecory behind he sar ime assumed by he planner. The robo may no hen be able o cach up wih he planned rajecory before a collision occurs. To deal wih his issue, he planner compues a rajecory assuming ha he robo will sar execuing i 0.4 second ino he fuure. I also assumes ha he obsacles move a consan velociies, as measured by he vision module, and exrapolaes heir posiions accordingly. The 0.4 second includes all he delays in he sysem, in paricular, he ime needed for planning. This ime could be furher reduced by implemening he planner more carefully and running i on a machine faser han he relaively slow Sun Sparc 10 currenly being used. Sensing errors Alhough he planner assumes ha he obsacles move along sraigh lines a consan velociies measured by he vision module, he acual rajecories are slighly differen due o asymmery in air-bearings and inaccuracy in he measuremens. The planner deals wih hese errors by growing he obsacles. As ime elapses, he radius of each moving obsacle is increased by, where is a fixed consan, is he measured velociy of he obsacle, and is he ime. So he planner can avoid erroneously assering ha a sae ime poin is collision-free when i is acually no. Trajecory racking The robo receives from he planner a rajecory ha specifies he posiion, velociy, and acceleraion of he robo a all imes. A PD-conroller wih feedforward is used o rack his rajecory. The maximum racking errors for he posiion and velociy are 0.05 m and 0.0 m/s, respecively. As a resul, we increase he size of he disc modeling he robo by 0.05 m during he planning o guaranee ha he compued rajecory is collision-free. Trajecory opimizaion Since he planner is very efficien in general, he 0.4 second allocaed is ofen more han wha is needed for finding a firs soluion. So he planner explois he exra ime o generae addiional milesones and keeps rack of he bes rajecory found so far. The cos funcion for comparing rajecories is LI, where 0 is he number of segmens in he rajecory, fixed consan, and ± ( Œ q ð F ± is he magniude of he force exered by he hrusers along he 7 h segmen, is a is he duraion of he 7 h segmen. The cos funcion akes ino accoun boh fuel consumpion and execuion ime. A larger yields faser moion, while a smaller yields less fuel consumpion. In our experimens, he cos of rajecories was reduced, on he average, by 14% wih his simple improvemen. Safe-mode planning If he planner fails o find a rajecory o he goal wihin he allocaed ime, we found i useful o compue an escape rajecory. The endgame region G ˆ Š for he escape rajecory Žˆ Š for some ime ˆ Š. An escape rajecory corresponds o any acceleraion-bounded, collision-free moion in he workspace for a small duraion of ime. In general, G ˆ Š is very large, and so generaing an escape rajecory ofen akes lile ime. To ensure collision-free moion beyond Sˆ Š, a new escape rajecory mus be compued long before he end of he curren escape rajecory so ha he robo can escape collision despie he acceleraion consrains. We modified he planner o compue simulaneously a normal and an escape rajecory. The modificaion increased he running ime of he planner by abou % in our experimens, bu i leads o a sysem ha is much more useful pracically. consiss of all he reachable, collision-free ŒF?Œ wih &Œ þ 7

28 Figure 1: Snapshos of he robo execuing a rajecory. 7.3 Experimenal resuls The planner successfully produced complex maneuvers of he robo among saic and moving obsacles in various siuaions, including obsacles moving direcly oward he robo or perpendicular o he line connecing is iniial and goal posiions. The ess also demonsraed he abiliy of he sysem o wai for an opening o occur when confroned wih moving obsacles in he robo s desired direcion of movemen and o pass hrough openings ha are less han 10 cm larger han he robo. In almos all he rials, a rajecory was compued wihin he allocaed ime. Figure 1 shows snapshos of he robo during one of he rials, in which he robo maneuvers among hree incoming obsacles o reach he goal a he fron corner of he able. Several problems limied he complexiy of he planning problems which we could ry in his esbed. Two are relaed o he esbed iself. Firs he acceleraions provided by he robo s air hrusers are quie limied. Second he size of he able is small relaive ha of he robo and he obsacles, which limis he available space for he robo o maneuver. The hird problem resuls from he design of our sysem. The planner assumes ha obsacles move a consan linear velociies and do no collide wih one oher, an assumpion which is likely o fail in pracice. To address his las and imporan issue, we inroduce on-he-fly replanning. 7.4 On-he-fly replanning An obsacle may deviae from is prediced rajecory, because eiher he error in he measuremens is larger han expeced, or he obsacle s direcion of moion has suddenly changed due o a collision wih oher obsacles. Whenever he vision module deecs his, i alers he planner. The planner hen recompues a rajecory on he fly wihin he same allocaed ime limi, by projecing he sae of he world 0.4 second ino he fuure. On-he-fly replanning allows much more complex 8

29 T =.1 secs T = 14.6 secs T = 19.8 secs x [meers] x [meers] x [meers] x 1 [meers] x 1 [meers] x 1 [meers] T = 33. secs T = 50. secs T = 75.0 secs x [meers] x [meers] x [meers] x 1 [meers] x 1 [meers] x 1 [meers] Figure 13: A compued example wih replanning in a simulaed environmen. experimens o be performed. We show wo examples below, one in simulaion and one on he real robo. In he example shows in Figure 13, eigh replanning operaions occurred over he enire course (75 seconds) of he experimen. Iniially he robo moves o he lef o reach he goal a he boom middle (snapsho 1). Then he upper-lef obsacle changes is moion and blocks he robo s way, resuling in a replan (snapsho ). Soon afer, he moion of he upper-righ obsacle also changes, forcing he robo o reverse he direcion and approach he goal from he oher side of he workspace (snapsho 3). In he remaining ime, new changes in obsacle moion cause he robo o pause (see he sharp urn in snapsho 5) unil a direc approach o he goal is possible (snapsho 6). The efficacy of he replanning procedure on he real robo is demonsraed by he example in Figure 14. The robo s goal is o move from he back lef of he able o he fron middle. Iniially he obsacle in he middle is saionary, and he oher wo obsacles are moving oward he robo (snapsho 1). The robo dodges he faser-moving obsacle from he lef and proceeds oward he goal (snapsho ). The obsacle is hen redireced wice (in snapshos 3 and 5) o block he rajecory of he robo, causing i o slow down and say behind he obsacle o avoid collision (snapshos 3 6). Righ before snapsho 7, he righmos obsacle is direced back oward he robo. The robo wais for he obsacle o pass (snapsho 8) and finally aains he goal (snapsho 9). The enire moion lass abou 40 seconds. Throughou his experimen, oher replanning operaions (no shown) occurred as a resul of errors in he measuremen of he obsacle moions. However, none resuled in a major redirecion of he robo. 9

30 Figure 14: An example wih he real robo using on-he-fly replanning. 8 Conclusion We have presened a simple, efficien randomized planner for kinodynamic moion planning in he presence of moving obsacles. Our algorihm represens he moion consrains by an equaion of he form and consrucs a roadmap of sampled milesones in he sae ime space of a robo. I samples new milesones by firs picking a random a poin in he space of admissible conrol funcions and hen mapping he poin ino he sae space by inegraing he equaions of moion. Thus he moion consrains are naurally enforced during he consrucion of he roadmap. The algorihm is general and can be applied o a wide class of sysems, including ones ha are no locally conrollable. The performance of he algorihm has been evaluaed hrough boh heoreical analysis and exensive experimens. We have generalized he noion of expansiveness, originally proposed in [HLM97] for (geomeric) pah planning. The main purpose of he generalizaion is o address he complicaions inroduced by kinemaic and dynamic consrains. Using he expansiveness o characerize he complexiy of he sae space, we have proven ha, under suiable assumpions, he failure prob- 30