Reflective Navigation

Transcription

1 Reflectve Navgaton Bors Kluge InMach Intellgente Maschnen GmbH Helmholtzstr. 16, Ulm, Germany Erwn Prassler Bonn-Rhen-Seg Unversty of Appled Scence Grantham-Allee 20, Sankt Augustn, Germany 1 Introducton Moton plannng for a robot n an envronment contanng obstacles s a fundamental problem n robotcs. For the task of navgatng a moble robot among movng obstacles, numerous approaches have been proposed. However, movng obstacles are most commonly assumed to be travelng wthout havng any percepton or moton goals (.e. collson avodance or goal postons) of ther own. In the expandng doman of moble servce robots deployed n natural, everyday envronments, ths assumpton does not hold, snce humans (whch are the movng obstacles n ths context) do perceve the robot and ts moton and adapt ther own moton accordngly. Therefore, reflectve navgaton approaches whch nclude reasonng about other agents navgatonal decson processes become ncreasngly nterestng. In ths paper an approach to reflectve navgaton s presented whch extends the velocty obstacle navgaton scheme to ncorporate reasonng about other objects percepton and moton goals. 1.1 Related Work Predctve navgaton s a doman where a predcton of the future moton of the obstacles s used to yeld more successful moton (wth respect to travel tme or collson avodance), see for example the work of Foka and Trahanas [3] and Mura and Shra [5]. However, reflectve navgaton approaches are an extenson of ths concept, snce they nclude further reasonng about percepton and navgatonal processes of movng obstacles. The velocty obstacle paradgm, whch belongs to the class of predctve navgaton schemes, has been presented by Forn and Shller [2] for obstacles movng on straght lnes, and has been extended by Shller et al. [6] for obstacles movng on arbtrary (but known) trajectores. 1

2 Modelng other agents decson makng smlar to the own agent s decson makng s used by the recursve agent modelng approach [4], where the own agent bases ts decsons not only on ts models of other agents decson makng processes, but also on ts models of the other agents models of ts own decson makng, and so on (hence the label recursve). 1.2 Overvew Assume a robot B uses determnstc velocty obstacles for ts navgaton. Then, there s some freedom n choce of avodng veloctes. That s, a unque velocty v B R 2 cannot be an adequate predcton of the future velocty of B. Therefore, f velocty obstacles are used n a recursve manner, they have to be extended n a way whch allows to express uncertanty about the velocty of the obstacles,.e. by usng (possbly multmodal) probablty dstrbutons. Such a probablstc extenson of the velocty obstacle approach s presented n Secton 2. Beng able to cope wth uncertan obstacle veloctes, Secton 3 descrbes how to apply the velocty obstacle scheme recursvely n order to create a reflectve navgaton behavor. The proposed method s evaluated for a collecton of smulated n Secton 4, and fnally concluded after dscussng the presented work. 2 Probablstc Velocty Obstacles Let B and B j be crcular objects wth rad r and r j, placed at postons c R 2 and c j R 2, as n the determnstc velocty obstacle approach. However, now we wll consder uncertanty n shape and velocty of the objects. Ths allows to reflect the lmtatons of real sensors and object trackng technques. 2.1 Shape Uncertanty A frst source of uncertanty s the actual shape of the obstacles. Wth real sensors, there wll always be measurement errors, whch should be reflected by the navgaton approach. It turns out that defnng the noton of an uncertan shape s not straghtforward. One dea mght be to model uncertanty about the actual shape of a rgd body B by a functon PB : R 2 [0, 1] (1) where PB(p) s nterpreted as the probablty of pont p belongng to B. However, we would have to specfy dependences between the ponts, too, whch wll become clear 2

3 after the followng defnton of a smple class of probablstc objects. Defnton 2.1 (Dsc wth Uncertan Radus) A dsc wth uncertan radus D(a, b) s a dsc centered at the orgn whose radus s a varate R wth range [a, b] and 0, f r < a, r a P(R r) = f r [a, b], and (2) b a 1, f r > b. For the sake of brevty, we may call a dsc wth uncertan radus probablstc dsc or p-dsc, too. Dscs wth uncertan radus cannot be represented by a mappng PB : R 2 [0, 1] as above alone, snce for example the events (0, r) B and (r, 0) B for r R are not ndependent f B s a dsc wth uncertan radus. Therefore we wll focus on p-dscs as probablstc objects n the followng. Ths s not a severe restrcton, snce the remander of ths paper remans vald after changng the defnton of probablstc objects, provded that the defnton of a probablstc collson cone s adapted accordngly. Defnton 2.2 (Placed Dsc wth Uncertan Radus) The ordered par (D(a, b), c) of a p- dsc D(a, b) and a poston c R 2 s called a placed dsc wth uncertan radus. The ordered trple (D(a, b), c, v) of a p-dsc D(a, b), a poston c R 2, and a velocty v R 2 s called a movng dsc wth uncertan radus, and the pont c + v t s called ts poston at tme t. Property 2.3 (Collson of Dscs wth Uncertan Radus) Let (D(a, b ), c ) and (D(a j, b j ), c j ) be placed p-dscs wth varates R and R j representng ther rad. Then, the placed p-dscs are colldng f R + R j c c j. In the determnstc velocty obstacle approach, the collson cone of an ordered par of movng objects s a set of relatve veloctes whch lead to a collson. If the shapes of the objects are uncertan, e.g. the radus of a crcular objects s only known up to some error, all we can expect as a probablstc collson cone s a mappng whch assgns collson probabltes to relatve veloctes. Defnton 2.4 (Probablstc Collson Cone) The probablstc collson cone of an ordered par of placed dscs (D(a, b ), c ) and (D(a j, b j ), c j ) wth uncertan rad s a mappng PCC j : R 2 [0, 1] wth ( ) PCC j : v j P X + X j mn c + v j t c j t R + 0, (3) that s, n words, PCC j (v j ) s the probablty of (D(a, b ), c +v j t) colldng wth (D(a j, b j ), c j ) for some t R

4 b + b j PCC j (v j ) = 1 a + a j c j c 0 < PCC j (v j ) < 1 v j PCC j (v j ) = 0 0 Fgure 1: Probablstc collson cone of two dscs (D(a, b ), c ) and (D(a j, b j ), c j ) wth uncertan rad 4

5 As an example, Fgure 1 shows the probablstc collson cone of two dscs wth uncertan rad. 2.2 Velocty Uncertanty Another source of uncertanty s the moton of the obstacles. In fact, we are confronted wth two types of uncertanty here, one whch stems from the measurement errors of the sensor system, and another one whch stems from unpredctable changes of the moton of the obstacles. Therefore we represent the uncertan velocty of object B j as a probablty densty functon V j : R 2 R + 0. (4) Gven such an blurred velocty V j of a placed p-dsc D j = (D(a j, b j ), c j ), we may ask for the collson probablty wth respect to a movng p-dsc D = (D(a, b ), c, v ) representng the robot, whch leads us to a probablstc formulaton of a velocty obstacle as a functon PVO j : R 2 [0, 1] (5) whch maps absolute veloctes v of B to the accordng probablty of colldng wth D j. Assume D j moves wth velocty v j R 2. Then, the probablty of a collson between D and D j s PCC j (v v j ). Snce the velocty of D j s uncertan, we have to wegh that collson probablty wth V j (v j ), the probablty densty at v j. Integratng over all possble veloctes v j of D j delvers PVO j (v ) = V j (v j )PCC j (v v j )d 2 v j, (6) R 2 whch s equvalent to where denotes the convoluton of two functon. PVO j = V j PCC j (7) When a movng p-dsc D s confronted wth a set of movng p-dscs B = {D j 1 j n, j}, the probablty of D colldng wth any other obstacle D j B equals the probablty of not avodng collsons wth any other movng obstacle. Therefore, the functon PVO : R 2 [0, 1] wth PVO (v ) = 1 (1 PVO j (v )). (8) j, D j B assgns to a velocty v of D the probablty of colldng wth any other p-dsc from B. That s, PVO s the probablstc counterpart of the composte velocty obstacle. 5

6 2.3 Navgatng wth Probablstc Velocty Obstacles In the determnstc case, navgatng s rather easy snce we consder only collson free veloctes and can choose a velocty whch s optmal for reachng the goal. But here, we have to balance two objectves: reachng a goal and mnmzng the probablty of a collson. Let U : R 2 [0, 1] be a functon representng the utlty of veloctes v for the moton goal of D. However, the full utlty of a velocty v s only attaned f (a) v s reachable, and (b) v s collson free. Therefore we defne the relatve utlty functon RU = U α R β (1 PVO ) γ, (9) where R : R 2 [0, 1] descrbes the reachablty of a new velocty,.e. t corresponds to the set RV of reachable veloctes n the determnstc velocty obstacle approach. The exponents α, β, γ R + are weghts for the three factors of the relatve utlty. A smple navgaton scheme for p-dsc D based on probablstc velocty obstacles can be obtaned by perodcally choosng a velocty v whch maxmzes the relatve utlty RU. In order to mplement ths approach, the use of contnuous functons has to be replaced by dscretzed verson, and explctly represented functons have to be restrcted to a fnte sze Dscretzaton Step functons s : R 2 R wth s(x, y) = s(x, y ) for κ x, x < ( + 1)κ and jκ y, y < (j + 1)κ are used for dscretzaton of contnuous (n the sense of non-dscrete) functons. In other words we use functons whch are pecewse constant on squares of sze κ κ, where κ s a predefned constant. For a pont p = (x, y) R 2, ts dscretzaton s ( x y dscr(p) = p =, Z κ κ ) 2. (10) Conversely, for a dscretzed pont p = (z 1, z 2 ) Z 2 we defne ts cell as cell(z 1, z 2 ) = {p R 2 dscr(p) = (z 1, z 2 )} = [z 1 κ, (z 1 + 1)κ) [z 2 κ, (z 2 + 1)κ). (11) For any functon F : R 2 [0, 1] we defne the dscretzaton of F to be the functon F : Z 2 [0, 1] wth F(z 1, z 2 ) = 1 F(x, y) dx dy, (12) κ 2 cell(z 1,z 2 ) 6

7 .e. F(z 1, z 2 ) s the average of F on cell(z 1, z 2 ). However, n practse we wll only requre F(z 1, z 2 ) F(cell(z 1, z 2 )) for F to be called a dscretzaton of F, snce the computaton of the ntegral s expensve and not neglgble. A smple extenson to overcome potental dffcultes would be to draw a constant number n of random ponts p from cell(p) and use the average value 1 n F(p ) as value for F(p), approachng the exact value for n. A more thorough treatment of ths problem nvolves samplng theory,.e. an analyss of the spectrum of F and the selecton of κ accordng to Shannon s samplng theorem, and goes beyond the scope of ths thess. We wll call a functon F a strct dscretzaton of F, f t fulflls Equaton 12, and otherwse assume that the value of κ s adequate for F. Fnally, for a dscretzed functon F : Z 2 R + 0 the set σ(f) = {(x, y) Z 2 F(x, y) > 0} (13) s called the supportng set of F, whch s the set of cells on whch the dscretzed functon does not vansh. The property σ(fg) = σ(f) σ(g) (14) s easly shown. Furthermore, F(p)κ 2 = F(p)d 2 p (15) p Z 2 R 2 holds for strct dscretzatons Restrcton Now we dscuss the restrcton problem n the context of navgatng a p-dsc D. Assumng that the velocty of any other p-dsc D j s bounded or s known wth bounded error, the supportng set σ(v j ) s fnte. Therefore, PVO j (v ) can be computed for any v by usng PVO j (v ) = V j (v j )PCC j (v v j )κ 2, (16) v j σ(v j ) whch s the dscrete verson of Equaton 6. The unbounded probablstc collson cones PCC j have to be represented mplctly by a subroutne whch computes the respectve collson probabltes on demand. Furthermore, for any real (.e. physcal) p-dsc D, the set σ(r ) descrbng reachable veloctes s fnte, as any bounded acceleraton appled to a body of non-zero mass for a bounded perod of tme results n a bounded change of velocty. Snce only veloctes from σ(ru ) wll be consdered for navgatng D, and snce σ(ru ) σ(r ), we can restrct veloctes to the fnte doman σ(ru ). 7

8 Algorthm 1 RELATIVE UTILITY 1: nput: a set of placed p-dscs B = {D = (D(a, b ), c ) = 1, 2,..., n} 2: nput: uncertan veloctes V : R 2 [0, 1] for each p-dsc D B 3: nput: a functon U : R 2 [0, 1] descrbng utlty of veloctes 4: nput: a functon R : R 2 [0, 1] descrbng reachable veloctes 5: nput: a functon PCC : N N Z 2 [0, 1] wth PCC(, j, v j ) = PCC j (v j ) 6: nput: the ndex of the p-dsc representng the robot 7: for v σ(r ) do 8: RU (v ) U α (v ) R β (v ) 9: for j {1, 2,..., n} {} do 10: PVO j (v ) 0 11: for v j σ(v j ) do 12: PVO j (v ) PVO j (v ) + V j (v j ) PCC j (v v j ) κ 2 13: end for 14: RU (v ) RU (v ) (1 PVO j (v )) γ 15: end for 16: end for 17: return RU Algorthm Combnng the results from the prevous subsectons, we get PVO = 1 j (1 PVO j ) (17) and further RU (v ) = U (v )R (v )(1 PVO (v )) = U (v )R (v ) j (1 PVO j (v )) (18) for any v σ(ru ). Ths observaton s summarzed n Algorthm 1, too. Assumng that PCC j (v ), R (v ), and U (v ) can be computed n tme O(1) for v Z 2, we can compute PVO j (v ) n tme O( σ(v j ) ) (accordng to Equaton 16 and lnes n Algorthm 1), and RU (v ) n tme O( j σ(v j) ) (accordng to Equaton 18 and lnes 8 15 n Algorthm 1). Fnally, a dscrete velocty v maxmzng RU can be found n tme ( O σ(r ) ) σ(v j ), (19) ntegratng the search nto the loop from lne 7 to lne 16 n Algorthm 1. That s, the dependence of the runnng tme on the number of obstacles s only lnear, but the dependence on the dscretzaton s O(1/κ 4 ). 8 j

9 3 Recursve Probablstc Velocty Obstacles Tradtonally, when navgatng a moble robot among movng obstacles (lke humans), these obstacles abltes to avod collsons and ther resultng moton behavors are not taken nto account. In contrast to ths plan obstacle modelng, recursve modelng approaches presume the opponents (or more generally, the nteracton partners) to apply decson makng processes for navgaton smlar or equvalent to the own process. In the gven context of moble robot navgaton, ths means to put the robot nto the poston of ts obstacles, let t reason about ther decsons and then ntegrate the resultng nsght nto ts own decson makng. We wll call such ntellgent movng obstacles (or, obstacles whch are consdered ntellgent) agents. Furthermore, we wll consder a fnte set of agents B = {D = (D(a, b ), c ) = 1, 2,..., n} wth uncertan velocty V for each D B for the remander of ths secton. 3.1 Agent Modelng Agents are assumed to perceve ther envronment and deduce accordng reactons, the reasonng process beng smlar to that of the robot. That s, any agent D j s assumed to take actons maxmzng ts relatve utlty functon RU j. Therefore, n order to predct the acton of agent D j, we need to know ts current utlty functon U j, reachable veloctes R j, and velocty obstacle PVO j. The utlty of veloctes can be nferred by recognton of the current moton goal of the movng obstacle. For example, Bennewtz et al. [1] learn and recognze typcal moton patterns of humans. If no global moton goal s avalable through recognton, one can stll assume that there exsts such a goal whch the agent strves to approach, expectng t to be wllng to keep ts current speed and headng. By contnuous observaton of a movng agent t s also possble to deduce a model of ts dynamcs, whch descrbes feasble acceleratons dependng on ts current speed and headng. These two problems are beyond the scope of ths thess and wll not be addressed n detal n the followng. The remanng problem s the computaton of a probablstc velocty obstacle for an agent D j, and ths requres to presume assumptons on the veloctes of the other movng agents D k, k j, to agent D j. In prncple, we can base assumptons on the future veloctes of an agent on ts probablstc velocty obstacle agan and agan. Ths s a recursve descrpton, hence these probablstc velocty obstacles wll be called recursve probablstc velocty obstacles, and wll be abbrevated as RPVO. However, at some pont the recurson has to be termnated,.e. the velocty obstacle must be based on perceved veloctes. Therefore, we may dstngush dfferent levels or depths of recurson, denoted by superscrpt d as n PVO (d) for agent D, such that PVO (1) s based on perceved veloctes of the other agents, and PVO (d) for d > 1 s based on veloctes of the other agents deduced usng probablstc velocty obstacles of recursve 9

10 depth d 1. Of course ths recursve modelng s not restrcted to any constant depth of recurson by a matter of prncple. However, computatonal demands wll ncrease wth the depth of the recurson, and ntutvely, one does not expect recurson depths of more than three or four to be of broad practcal value, snce such deeper modelng s not observed when we are walkng as human bengs among other humans. Note that accurate recursve models of movng agents are prerequste for more sophstcated reflectve navgaton approaches n order to be able to deceve and fent partcularly malevolent agents lke delberate obstructors. However beng dreams of the future, such potental abltes ndcate the mportance of reflectve navgaton approaches and ther nvestgaton. 3.2 Formal Representaton In order to nteract wth ther surroundngs, ntellgent agents create models of ther envronment. If ths envronment contans other agents, these can become part of the model, as well as these agents models of the envronment and so forth. Ths secton presents a formal representaton of recursve models n the gven context, whch serves as a bass for the mplementaton later on. Defnton 3.1 (Models of Functons by Agents, Interpretaton of Models) Let F be the symbol of a functon from R 2 to [0, 1]. Then, the symbol µ [F] denotes a functon from R 2 to [0, 1] and s verbalzed as model of F by agent. An nterpretaton I assgns functons to symbols µ [F], that s, I(µ [F]) : R 2 [0, 1]. Informally, we denote by µ [F] the current knowledge of agent about an entty F. For example, f R : R 2 [0, 1] s the functon whch specfes the reachablty of veloctes for an agent, we wll denote by µ j [R ] the functon specfyng the reachablty of veloctes as attrbuted to agent by agent j. Usng these symbols, we can now express the basc prncple of recursve agent modelng n the context of probablstc velocty obstacle navgaton as follows. Each agent assumes that the others wll choose ther velocty accordng to ther relatve utlty functon, that s µ [V (d) j ] = { 1 w µ [RU (d) j ] f d > 0 and w := RU (d) j d 2 v > 0, µ [V j ] else. (20) Note that V (d) j s a probablty densty, that s R 2 V j (v j ) (d) d 2 v j = 1, 10

11 but RU (d) j s a [0, 1]-valued functon wth bounded support, that s 0 w := RU (d) j (v j ) d 2 v j <. R 2 Ths s the reason for the scalng factor 1 n the frst case of Equaton 20, and the second w case n that equaton termnates the recurson for d = 0 or s a fallback poston for w = 0. For a recursve depth d = 0, no reflecton about the other agents moton s assumed, and therefore the relatve utlty RU (0) wll depend only on the utlty U of reachable veloctes as ndcated by R. For a recursve depth d > 0, the relatve utlty RU (0) of an agent depends on ts probablstc velocty obstacle PVO (d), too. Together, we have RU (d) = { U α R β ( ) γ f d = 0, U α Rβ 1 PVO (d) else, (21) wth weghts α, β, γ R +. The actual reflecton appears n the specfcaton of the recursve probablstc velocty obstacle PVO (d) : R 2 [0, 1] of depth d for agent, snce ths entty depends on the (recursve) model of other agents veloctes µ [V (d 1) j ] and s defned as ( ) 1 µ [V (d 1) j ] PCC j, (22) PVO (d) = 1 j whch completes our specfcaton of RPVO. Before any utlty RU (d) (v) for v R 2 can be computed, we have to specfy an nterpretaton of symbols µ [F] for functon symbols F, whch wll be gven n a recursve way by a set of rules, and two sets of rules wll dstngushed. Motvaton for the frst set stems from the gven context of reflectve navgaton. The second set of rules stems from our assumptons on the way how the agents acqure nformaton about each other, and s more or less specfc to a certan mplementaton. The frst set of nterpretaton rules s defned as follows. Defnton 3.2 (Interpretaton of RPVO Functon Models) Let F be a symbol for a func- 11

12 ton from R 2 to [0, 1]. Then, F wll be nterpreted as follows I(µ [G]) f F = µ [µ [G]], I(µ [G]) op I(µ [H]) f F = µ [G op H] wth op {+,, }, I(µ [G]) α f F = µ [G α ] wth α R, I(C) f F = µ [C] and C symbolzes a constant functon, I(F) = U f F = µ [U ], R f F = µ [R ], V f F = µ [V ], PCC j f F = µ [PCC j ], and F f F s not of the shape µ [G], (23) for, j {1, 2,..., n}. The frst rule, I(µ [µ [G]]) = I(µ [G]), s motvated by the assumpton that an agent knows what t knows,.e. ts model of ts model of an entty s the model of that entty. The second and the thrd rule are motvated by the assumpton that all agents use the same approach for decson makng,.e. they perform the same operatons to compute a certan functon. The remanng rules termnate the nterpretaton, ether for a symbol of a constant functon (e.g. 1 ), or when an agent models tself, snce we assume that each agent has accurate nformaton about tself, or when no modelng s nvolved. For d > 1, and w j := RU (d 1) j d 2 v > 0 for j, we get RU (d) = U α R β j ( 1 1 w j µ [RU (d 1) j ] PCC j ) γ, (24) from Equatons 20 22, and wth the rules from 3.2 follows µ [RU (d) j ] = µ [U α j R β j ( k j = µ [U j ] α µ [R j ] β k j 1 1 µ j [RU (d) k w ] PCC jk k ) γ ] ( 1 1 w k µ [µ j [RU (d) k ]] µ [PCC jk ]) γ, (25) that s, modelng s propagated towards the prmtve (.e. not composed) functons U, R, V, and PCC j. Furthermore, the number of models µ 1 [... µ d [F]... ] of prmtve functons occurrng n a full expanson of RU (d) may ncrease exponentally wth the recursve depth d, dependng on ther nterpretaton. 12

13 3.2.1 Interpretaton under Equal Informaton As seen above, we must specfy nterpretatons of (recursve) models of the functons U j, R j, V j and PCC jk n order to evaluate a relatve utlty RU (d). That s, we must say what agents assume or know about other agents percepton, ntenton, and reachable veloctes. As a frst smple approach, we wll assume equal nformaton among the agents. That s, no agent knows more or has a more accurate model of an entty than an other agent. Ths s expressed techncally n the followng defnton. Defnton 3.3 (Interpretaton under Equal Informaton) We say all agents have equal nformaton, ff I(µ [F]) = I(µ j [F]) (26) for agents and j, and F symbolzng a functon from R 2 to [0, 1]. If all agents have equal nformaton, any recursve model µ 1 [... µ k [F]... ] collapses to a smple model µ [F] for any 1,..., k, : I(µ 1 [µ 2 [... µ k [F]... ]]) = I(µ 2 [µ 2 [... µ k [F]... ]]) = I(µ 2 [... µ k [F]... ])... = I(µ k [F]) = I(µ [F]), (27) and wth Defnton 3.2 we have I(µ 1 [... µ k [F]... ]) = F, for F {U, R, V, PCC j } (28) and any agent 1,..., k,, j. Consequently, when agents have equal nformaton, we do not reason about mutual percepton but on relatve postons and velocty selectons only. Furthermore, relatve utltes RU (d) (v ) of veloctes v for an agent at a recursve depth d > 0 can now be computed effcently usng dynamc programmng. 3.3 Implementaton For the mplementaton we assume equal nformaton among the agents as defned above. Wth ths smplfcaton, the dependence of the complexty on the recurson depth s reduced to lnear, snce the number of models to be computed s equal on each level of recurson. Algorthm 2 gves the detals of the used dynamc programmng approach 13

14 Algorthm 2 RECURSIVE RELATIVE UTILITY 1: nput: a set of placed p-dscs B = {D = (D(a, b ), c ) = 1, 2,..., n} 2: nput: uncertan veloctes V : R 2 [0, 1] for each D B 3: nput: functons U : R 2 [0, 1] descrbng utlty of veloctes for each D B 4: nput: functons R : R 2 [0, 1] descrbng reachable veloctes for each D B 5: nput: a functon PCC : N N Z 2 [0, 1] wth PCC(, j, v j ) = PCC j (v j ) 6: nput: the desred recursve depth r N 7: for = 1,..., n do 8: V (0) dscr(v ) 9: RU (0) dscr(u R ) 10: end for 11: for d = 1,..., r do 12: for = 1,..., n do 13: RU (d) RELATIVE UTILITY as n Algorthm 1 for p-dscs B, uncertan veloctes V (d 1) j for each D j B, functons U j and R j for each D j B, the functon PCC, and consderng D as the robot. 14: w κ 2 ( ) RU (d) (v) v σ 15: f w > 0 then 16: V (d) 1 w RU(d) RU (d) 17: else 18: V (d) V (0) 19: end f 20: end for 21: end for 22: output: relatve utltes RU (d) 23: output: uncertan veloctes V (d) for each D B and d = 0, 1,..., r for each D B and d = 0, 1,..., r 14

15 3.3.1 Complexty We begn the complexty assessment by measurng the szes of the supportng sets of the dscretzed functons used n Algorthm 2, where lne 9 mples σ(ru (0) ) σ(r ), (29) and from lne 13 follows for d > 0. Lne 8 mples and from lnes 16 and 18 follows σ(ru (d) ) σ(r ) (30) σ(v (0) ) = σ(v ), (31) for d > 0, usng the three precedng Equatons. σ(v (d) ) σ(r ) σ(v ) (32) Now we count the numbers of operatons used n the algorthm, whch we wrte down usng N := σ(r ) σ(v ) as an abbrevaton. Lne 13 requres O(N j N j) operaton (cf. Equaton 19). Lnes 14, 16, and 18 requre O(N ) operatons each, and are thus domnated by lne 13. Therefore the loop from lne 12 to lne 20 requres operatons, and the loop from lne 11 to lne 21 requres O( O(r n (N N j )) (33) =1 j n (N N j )) (34) =1 operatons. The complexty of the loop from lne 11 to 21 clearly domnates the complexty of the ntalzaton loop from lne 7 to 10. Therefore Equaton 34 gves an upper bound of the overall tme complexty of our mplementaton. That s, the dependence on the maxmum recursve depth s lnear, the dependence on the number of objects s O(n 2 ), and the dependence on the dscretzaton remans O(1/κ 4 ). j 4 Results The approach has been evaluated n dfferent smulated stuatons, ncludng (a) two objects on a collson course, (b) a faster object approachng and overtakng a slower object, and (c) two objects encounterng each other close to a statc obstacle, see Fgure 2. 15

16 (a) Collson course (b) Overtakng (c) Statc obstacle Fgure 2: Stuatons for RPVO smulaton fnal poston of B ntal poston of B B A ntal poston of A (a) Resultng moton fnal poston of A object recursve depth maxmum relatve utlty v y RU (0) A 0 v x RU (0) B RU (1) A RU (1) B RU (2) A RU (2) B selected veloctes (b) Relatve utltes and velocty selecton Fgure 3: Legend for smulaton results 16

17 For each stuaton, varyng values for the recursve depth for each movng object have been used. The results for each stuaton and selected recursve depths are presented n the followng. For each experment, the entre resultng moton s depcted as n Fgure 3(a), where one dsc s drawn per four teraton steps. For selected ponts n tme the relatve utltes for the nvolved agents are depcted as n Fgure 3(b). Hgher values of relatve utlty are ndcated by darker shades of grey. For better vsblty, maxmum values are emphaszed n black. 4.1 Collson Course In ths stuaton, two agents are nvolved whch face each other ntally. Ther desred veloctes are conflctng,.e. they are drected aganst each other. Both agents have the same maxmum veloctes and acceleratons. Fgure 4 shows the collson course experment wth two agents A and B, where agent A from the left uses recursve depth 1 and agent B from the rght uses recursve depth 2. That s, agent B models agent A correctly and assumes that A s able to perceve ts envronment and to avod collsons. Therefore agent B s movng more aggressvely and wth less devaton from ts optmal path than agent A. Smlarly, Fgure 5 shows the encounter of agent A from the left and agent B from the rght, but now agent A uses recursve depth d = 3, and agent B uses depth d = 2 as before. Depth 3 means that agent A assumes agent B to move accordng to depth 2,.e. n a somewhat self-confdent way, so agent A chooses rather defensve veloctes for ts moton, and devates more decdedly and wth hgher velocty from ts optmal path than above. Ths becomes vsble when comparng the veloctes of A wth maxmum relatve utlty for recursve depths d = 1 and d = 3 n Fgure 5(c). Furthermore, the dstance between agent A and agent B when they meet s smaller when agent A uses recursve depth 3, compare Fgures 4(a) and 5(a). Fnally, an agent whch uses recursve depth d = 2,.e. assumng the other agents to avod collsons, s stll able to avod collsons wth movng obstacles whch are oblvous to other agents, as shown n Fgure Overtakng In ths stuaton, two agents are movng n the same drecton, agent A behnd agent B, whereby agent A desres a much hgher velocty than agent B. Ths creates a conflct that the two agents have to solve. Fgure 7 shows the result when agent A uses recursve depth 1 and agent B uses recursve depth 2. Agent B does not leave ts optmal path as much as agent A does, whch 17

18 B A (a) Resultng moton (b) Step 1 (c) Step 2 (d) Step 5 (e) Step 10 Fgure 4: Collson course, object A at depth 1 versus object B at depth 2 18

19 B A (a) Resultng moton A, d=3 B, d=3 A, d=3 B, d=3 (b) Step 1 (c) Step 2 A, d=3 B, d=3 A, d=3 B, d=3 (d) Step 5 (e) Step 10 Fgure 5: Collson course, object A at depth 3 versus object B at depth 2 19

20 B A (a) Resultng moton (b) Step 1 (c) Step 2 (d) Step 5 (e) Step 10 Fgure 6: Collson course, object A at depth 2 versus object B at depth 0 20

21 B A (a) Resultng moton (b) Step 1 (c) Step 2 (d) Step 5 (e) Step 10 Fgure 7: Overtakng, object A at depth 1 versus object B at depth 2 21

22 s what one expects for chosen par of depths. When agent A uses recursve depth 3 nstead of 1, ts downward velocty component s slghtly larger than n the prevous experment, whch s vsble when comparng ts relatve utltes and selected veloctes at step 10 between Fgure 7 and Fgure 8. Furthermore, agent B starts to move horzontally agan earler when agent A uses depth 3, whch can be seen when comparng Fgure 7(a) to Fgure 8(a). All ths ndcates that n the latter experment agent A passes by faster than n the former experment. In the overtakng examples untl now, we had a slow agent B usng recursve depth 2. Now we wll consder examples where the fast agent A uses depth 2 and encounters a slow agent B at depth 1 or 3. We start wth agent B usng depth 1. Havng seen the experments above, we would expect the fast agent at depth 2 to force the slow agent to leave ts optmal path. Ths s not the case, as can be seen n Fgure 9. The reason for ths s smple: agent B cannot move fast enough out of agent A s path. In the frst step, agent B chooses an avodng velocty whle agent A moves straght ahead, as depcted n Fgure 9(b). In the next step, agent B has moved a lttle downward, and therefore agent A starts to move upward, allowng agent A a faster moton n ts desred drecton (.e. to the rght). If agent B uses recursve depth 3, t assumes that agent A expects t to avod collsons, and therefore starts movng out of the way more quckly. As a result, the vertcal component of agent A s velocty s smaller n ths case, whch can be seen when comparng the relatve utlty (whch s centered at the current velocty) of agent A for step 10 n both cases. Anyhow, the avodance maneuver of agent B s more promnent when usng depth 3 than when usng depth 1, whch becomes obvous when comparng Fgures 9(a) and 10(a). Fnally we wll consder overtakng examples where both agents use the same recursve depth. We wll start wth both agents usng depth d = 2, see Fgure 11. Due to the symmetry, none of the agents consders devatng from ts optmum path, and the ntally slower agent B accelerates to avod a collson. But f both agents use recursve depth d = 3, the conflct s solved n a more ntellgent way. In a frst step, both agents devate n the same drecton n order to avod the pendng collson, see Fgure 12. In the next step, agent B stll chooses a velocty wth a small devatng component, whle agent A decdes to move horzontally. Ths asymmetry s amplfed durng the subsequent steps, such that both agents avod the collson cooperatvely. 4.3 Statc Obstacle The last type of experments whch we wll consder nvolves two agents movng n opposte drectons wth an encounter close to a statc obstacle. 22

23 B A (a) Resultng moton A, d=3 B, d=3 A, d=3 B, d=3 (b) Step 1 (c) Step 2 A, d=3 B, d=3 A, d=3 B, d=3 (d) Step 5 (e) Step 10 Fgure 8: Overtakng, object A at depth 3 versus object B at depth 2 23

24 A B (a) Resultng moton (b) Step 1 (c) Step 2 (d) Step 5 (e) Step 10 Fgure 9: Overtakng, object A at depth 2 versus object B at depth 1 24

25 A B (a) Resultng moton A, d=3 B, d=3 A, d=3 B, d=3 (b) Step 1 (c) Step 2 A, d=3 B, d=3 A, d=3 B, d=3 (d) Step 5 (e) Step 10 Fgure 10: Overtakng, object A at depth 2 versus object B at depth 3 25

26 A B (a) Resultng moton (b) Step 1 (c) Step 2 (d) Step 5 (e) Step 10 Fgure 11: Overtakng, object A at depth 2 versus object B at depth 2 26

27 A B (a) Resultng moton A, d=3 B, d=3 A, d=3 B, d=3 (b) Step 1 (c) Step 2 A, d=3 B, d=3 A, d=3 B, d=3 (d) Step 3 (e) Step 4 Fgure 12: Overtakng, object A at depth 3 versus object B at depth 3 27

28 C A B (a) Resultng moton (b) Step 1 (c) Step 5 Fgure 13: Statc obstacle, object A at depth 2 versus object B at depth 1 28

29 In the frst example of ths type, agent A uses depth 2 and agent B uses depth 1, see Fgure 13. Havng seen the examples above, the result of ths experment s not surprsng, snce agent A usng depth 2 s able to explot the collson avodng behavor of agent B and succeeds n movng on the shorter path, closer to the statc obstacle C. Smlarly, when agent B uses depth d = 3 nstead of depth d = 1, agent A succeeds n movng on the shorter path, too, see Fgure 14. Fnally, Fgure 15 demonstrates that the defensve behavor of agent B at depth 3 allows agent A to move on ts desred path even when usng depth 1. Note that n no case the agents decded to pass by obstacle C on dfferent sdes. The reason s the way a velocty wth maxmum relatve utlty s selected. A smple approach s to accept the frst velocty wth that property, when (dscrete) veloctes are consdered n ther lexcal order, resultng n the observed velocty selecton. Another approach s to select one velocty from the optmal (dscrete) veloctes by random, whch wll at least remove artfacts whch stem from some veloctes beng systematcally preferred to others. 5 Dscusson To navgate a moble robot B usng depth-d recursve probablstc velocty obstacles, we repeatedly choose a velocty v maxmzng RU (d). For d = 0, we get a behavor that only obeys the robot s utlty functon U and ts dynamc capabltes D, but completely gnores other obstacles. For d = 1, we get the plan probablstc velocty obstacle behavor as descrbed n Secton 2. Somethng new happens for d > 1, when the robot starts modelng the obstacles as perceptve and decson makng. Agents navgatng at depth d = 2 appear to move more aggressvely than agents navgatng at depths d = 1 or d = 3, whereby especally depth d = 3 appears to result n rather defensve behavors, and may become an an attractve opton for consderate servce robots. Fndng good models of another agent s dynamc capabltes R j and utlty functons U j s a problem beyond the scope of ths thess. When the acton to be taken s consdered the frst step of a longer sequence, computng the utlty functon may nvolve moton plannng, or even game-tree search, f reactons of other objects are taken nto account. Due to the recursve nature of the approach, such a procedure would have to be appled for any object at any recursve level. Ths renders such enhancements of utlty functons rather nfeasble, snce already sngle applcatons of such procedures are computatonally expensve. The role of the weghts α, β, and γ of the three factors of relatve utlty s largely unexplored. Some frst experments ndcated that they n fact do nfluence the results, but not n a ground-breakng manner. Ths mght change when the uncertanty about shapes and veloctes s ncreased. Durng the experments presented above, 29

30 C A B (a) Resultng moton A, d=3 B, d=3 A, d=3 B, d=3 (b) Step 1 (c) Step 5 Fgure 14: Statc obstacle, object A at depth 2 versus object B at depth 3 30

31 C A B (a) Resultng moton A, d=3 B, d=3 A, d=3 B, d=3 (b) Step 1 (c) Step 5 Fgure 15: Statc obstacle, object A at depth 1 versus object B at depth 3 31

32 weghts α = β = γ = 1 were used. Note that each agent mght use a dfferent set of weghts. Oscllatons may appear n models for successve depths. Reconsder the collson course example wth both agents facng each other. Assume at depth d, both objects avod a collson by devatng to the left or to the rght. Then at depth d + 1, none of the objects wll perform an avodance maneuver, snce each object s depth-d model of the other object predcts that other object to avod the collson. Subsequently, n depth d + 2, both objects wll perform collson avodance maneuvers agan, an so on. When drvng a car on a hghway, reasonng smlar to the presented approach arses. Cars n front have to be avoded, and when they are already drvng on the rghtmost lane, they expect faster cars from behnd to perform all maneuvers necessary for overtakng wthout further collaboraton. That s, cars from behnd are to be modeled wth depth 1 or depth 3, and cars n front are to be modeled wth depth 0 or depth 2. But the stuaton s dfferent for emergency cars from behnd. They expect any other car to gve way to them, and therefore need to be modeled wth depth 2. In the context of pedestran traffc, a rather dfferent aspect of the presented recursve modelng scheme s that t can serve as a bass for an approach to reasonng about the objects n the envronment. One could compare the observed moton of the objects to the moton that was predcted by recursve modelng, possbly dscoverng relatonshps among the objects. An example for such a relatonshp s delberate obstructon, when one object obtrusvely refrans from collson avodance. Fnally, more accurate models of the nteracton partners are requred for effectvely generatng unexpected actons. If µ j [U ] dffers notably from U, but µ [µ j [U ]] s rather close to µ j [U ], agent can detect the dfference between µ [µ j [U ]] and U, and explot ths stuaton by dong somethng that s unexpected, and therefore unobstructed by agent j. 5.1 Concluson An approach to coordnated moton n dynamc envronments has been presented, whch reflects the peculartes of natural, populated envronments: obstacles are not only movng, but also percevng and makng decsons based on ther percepton. Ths percepton and decson makng of the ntellgent obstacles s taken nto account,.e. t s modeled and ntegrated nto the robot s own decson makng. The approach can be seen as a twofold extenson of the velocty obstacle framework. Frstly, object veloctes and shapes may be known and processed wth respect to some uncertanty (by means of a probablstc extenson). Secondly, the percepton and decson makng of other objects s modeled and ncluded n the own decson makng process (by means of a recursve extenson). 32

33 6 Acknowledgments Ths work was supported by the German Department for Educaton and Research (BMB+F) under grant no. 01 IL 902 F6 as part of the project MORPHA. References [1] M. Bennewtz, W. Burgard, and S. Thrun. Learnng moton patterns of persons for moble servce robots. In Proceedngs of the Internatonal Conference on Robotcs and Automaton (ICRA), [2] P. Forn and Z. Shller. Moton plannng n dynamc envronments usng velocty obstacles. Internatonal Journal of Robotcs Research, 17(7): , July [3] A. F. Foka and P. E. Trahanas. Predctve autonomous robot navgaton. In Proceedngs of the 2002 IEEE/RSJ Intl. Conference on Intellgent Robots and Systems, pages , EPFL, Lausanne, Swtzerland, Oct [4] P. J. Gmytrasewcz. A Decson-Theoretc Model of Coordnaton and Communcaton n Autonomous Systems (Reasonng Systems). PhD thess, Unversty of Mchgan, [5] J. Mura and Y. Shra. Modelng moton uncertanty of movng obstacles for robot moton plannng. In Proc. of Int. Conf. on Robotcs and Automaton (ICRA), [6] Z. Shller, F. Large, and S. Sekhavat. Moton plannng n dynamc envronments: Obstacles movng along arbtrary trajectores. In Proceedngs of the 2001 IEEE Internatonal Conference on Robotcs and Automaton, pages , Seoul, Korea, May