Anytime Predictive Navigation of an Autonomous Robot

Transcription

1 Anytme Predctve Navgaton of an Autonomous Robot Shu Yun Chung Department of Mechancal Engneerng Natonal Tawan Unversty Tape, Tawan Emal Abstract To acheve fully autonomous moble robot n dynamc crowded envronment, an effcent and real-tme moton plannng s necessary. In ths paper, a new A*-based predctve moton planner s presented for the navgaton task. By predctng pedestran moton n the future, t can drastcally decrease the probablty of collson n the publc place. In order to obtan the pedestran moton model, a goal-drected navgaton functon (NF) based moton model s traned and utlzed n our plannng algorthm. At the same tme, the potental goals of movng objects are automatcally searched by GVG-skeleton of map. Moreover, a generalzed trackng and predcton framework s also ntroduced n ths paper. Correspondng recursve Bayesan formula presented as DBNs (Dynamc Bayesan Networks) s derved for real tme operaton. Fnally, the smulaton results are also shown and valdated the dea of ths paper. Index Terms Navgaton, Moton Plannng, Moble Robot. B I. INTRODUCTION Y the efforts of robotc researchers, there has been a great progress n robotc technques. Robots are no longer only operated n laboratores and factores. On the contrary, lots of novel robots were desgned and developed to work n the populated envronment n the last two decades. Dfferent knds of servce robots provde assstance to people n hosptals [3, ], home envronment[8], offce buldngs, museums[3], and exhbtons. In crowded and populated envronment, real tme moton plannng of moble robots s an mportant and crucal constrant. In dynamc envronment, snce the obstacle motons are usually uncertan, confguraton space wll be hghly transtory. Tradtonal moton plannng whch queres a complete path from current poston to the goal often takes too much tme to satsfy the real tme requrement. Although reactve moton plannng can rapdly calculate the approprate next moton, t stll easly gets blocked n such cases. In other words, one needs to be able to plan a path fast but stll keeps enough look-ahead. A look-ahead planner can effcently decrease the probablty of collson. However, t s requred a good Han Pang Huang Department of Mechancal Engneerng Natonal Tawan Unversty Tape, Tawan Emal: hanpang@ntu.edu.tw predctor to tell the robot the future scenes. Thus there are three man topcs dscussed n the paper. The frst part wll focus on pedestran moton model learnng. A navgaton functon based pedestran moton model s utlzed to predct the long term moton. Then n the second part, a generalzed trackng and predcton framework based on the Dynamc Bayesan Networks s ntroduced to acheve the robust trackng. Fnally, t descrbes the predctve anytme moton planner. Ths paper s organzed as follows. In secton II, we wll ntroduce the learnng method of pedestran model. A generalzed trackng and predcton framework wll be proposed n secton III. The recursve Bayesan formulaton s also derved for real tme operaton. In secton IV, predctve anytme A* planner s descrbed. Some smulaton results wll be presented to valdate the dea n secton V. Fnally, conclusons are summarzed n secton VI. II. PEDESTRIAN MODEL LEARNING To predct the pedestran future acton, one of the best ways s to obtan the pedestran moton model frst. In the short term predcton, we can easly utlze certan moton models, such as constant velocty or constant acceleraton, to predct the next moton of pedestrans. However, n the long term predcton, t s not so obvous to defne a moton pattern. Thus there were several prevous researches that dscussed the pedestran model learnng methods. Most of them emphaszed the goal-drected concept to predct the pedestran moton. By observng the potental goals that human may go forward, further predct the poston of human n the next few seconds. For example, Foka and Trahanas[7] proposed to predct the human moton by manually defnng the hot ponts where people may approach n daly lfe. Bennewtz [] used expectaton maxmum(em) to clusterng the collected pedestran trajectores and obtan the potental goals leadng the movement of human n the envronment. He further derved a Hdden Markov Model (HMM) appled to estmate the future moton of people. However, ths method s requred to recollect the pedestran trajectores whle the operated envronment s dfferent place. Yen [5] extracted the potental goals of envronment by clusterng the trajectores wth K-means. Then a grd-based

2 navgaton functon (NF) s used to provde suggestons on pedestran moton. The transton probabltes of gradent drecton based on NF are avalable wth statstcally analyzng the frequency of certan drectons leaded by NF. The method can effcently model the pedestran moton pattern n most places after locatng the potental goals. But t gnores the effect of repulsve force to the obstacles. The moton model estmated by NF sometmes generates the moton that s closed to the obstacles. The reason s that NF only consders the relatve dstance wth the goal, not the obstacles. However, t s not compatble wth realty. Here our man purpose s to learn a pedestran moton model that s sutable n most places. At the same tme, t does not need to recollect the new trajectores to extract the potental goals or re-tran the moton model whle the robot move nto dfferent places. Under these consderatons, we desgn the followng several processes to learn the pedestran model. A. Potental Goal Extracton To automatcally extract the potental goals of pedestrans, the envronment map s dvded nto ndvdual submaps. Each submap has several exts whch are regarded as the potental goals. The crterons of map dvson are based on the characterstcs of geometry. For example, n Fg. 2(b), the GVG (Generalzed Vorono Graph)[4] s extracted as the map skeleton at frst. Then searchng the crtcal ponts of the skeleton, whch mnmze the clearance locally. Fnally, the map s parttoned nto fve submaps by the crtcal ponts (see Fg. 2 (c)(d)). The smlar work was proposed by Thrun[2] whch focused on the topologcal map extracton. Moreover, after map dvson, the NF of every potental goal of ndvdual submap s calculated. The dstance map (DstMap) of each submap whch records the dstance to the closest obstacle s also evaluated. Fg. 2 (e)(f) show the NF and DstMap of the blue submap. The functon of NF and DstMap for moton predcton wll be ntroduced n the next secton. B. Pedestran Moton Model We adopted NF-based pedestran moton model whch s smlar to Yen [5]. 8 neghbors n grd space are consdered as the next potental drectons. NF wll generate at least one optmal drecton n each grd shown n Fg. (left). By the relatve angle to the optmal drecton, 5 drectonal lots are grouped for each 45 degrees dsplayed n Fg. (rght). C. Learnng the Moton Transton Probabltes of Pedestran Model In NF-based pedestran moton model, learnng of the moton transton probablty s to evaluate the probablty to ndvdual drectonal lot. We collect pedestran trajectores n the lobby and hallway of the Department of Mechancal Engneerng, NTU shown n Fg. 3. The robot s equaled wth a laser scanner whch s of 80 degree range vew and 0.5 degree resoluton. Fnally, 70 trajectores are gathered and utlzed to evaluate the moton transton probablty. To consder the repulsve force to the obstacles, we separately evaluate the moton probablty n dfferent dstance value whch s provded by DstMap. Currently t s dvded nto two categores wth the threshold 2 meter. 77 data ponts extracted from trajectores are classfed nto correspondng lot (see Fg. 4). In Fg. 4, the vertcal axs ndcates the amount of data ponts and horzontal axs denotes the groups of relatve angle to the optmal drecton. Thus the moton transton probablty can be calculated by eq.() ( ) p L where L denote lot (a) (c) # of L = (# of L ) () (b) (d) Fg. Pedestran moton model. (left) NF, the best drecton denoted by the long red arrow. (rght) 5 drectonal lots are grouped. (e) (f) Fg. 2 map dvson. (a) orgnal map (b) GVG structure of the map (c) crtcal ponts selecton (d) the envronment s dvded nto 5 submaps (e) the NF of potental goal 2 of the blue submap (f) the dstance map of the blue submap.

3 III. GENERALIZED TRACKING & PREDICTION The detecton and trackng of movng object problem has been extensvely studed for decades. Lots of methods were proposed and appled to dfferent knds of trackng problem such as pedestran trackng, vehcle trackng, and contour trackng etc.. To enhance trackng performance, t sometmes combnes wth pror moton models to predct the next moton of movng objects. Consderng the smplcty and effcency, Kalman flter s commonly used n trackng problem. Furthermore, to assst the robustness, trackng over probablty assocaton s also the popular soluton [2, 0] Here we represent a more generalzed trackng and predcton framework. The predcton s dvded nto short term and long term predcton. The short term predcton s followed by the general moton model predcton. Goaldrected pedestran moton predcton s utlzed n long term predcton. Moreover, n ths paper, the structure of MHT (Multple Hypothess Trackng) s used to acheve robust data assocaton. A. Formulaton of Trackng p o Z Trackng problem can be treated as the posteror ( k estmaton. Where o k s the status of a movng object at tme step k and Zk s defned as the data set of sequental observatons. The generalzed trackng problem can be represented as DBNs (Dynamc Bayesan Networks) shown n Fg. 5. Z {,, k z zk} (2) Fg. 3 Pedestran trajectory collecton n lobby (left) and hallway (rght) dst > 2 m dst <= 2 m Drectonal Sub-Optmalty ( n degrees ) Fg. 4 Classfed lots of sub-optmalty values. Fg. 5 DBN for generalzed trackng problem Accordng to the total probablty and Bayesan formulaton, the posteror can be factorzed nto eq. (3). G s the goal leadng the movng object moton. j p( zk G, sk, o s the lkelhood probablty at tme step k. The second term, ( j pok G, sk, Z k- ), s the predcton process. p( o Z ) k k m n = = j= m n = j= j j ( k, k, ( ( k p o G s Z p G Z p s Z j j j ( k, k, ( k, k, k ) ( ( k p z G s o p o G s Z p G Z p s Z Update Predcton Goal Weghtng Model Weghtng The thrd term ( k ) p G Z s represented as the goal weghtng. Correspondng factorzaton of Bayesan formula s shown n eq.(4). p( G Z ) (, ) ( k p zk G Zk p G Zk ) (4) In the smlar case, moton model weghtng j p( sk Z can also be factorzed nto eq.(5). p s j Z p z s j, Z p s j Z (5) ( k ( k k k ) ( k k ) If we consder the uncertanty of sensor measurement and data assocaton (trackng), the formula of goal weghtng (see eq.(4)) can be further dvded nto eq.(6). p G Z = η p z G, Z p G Z ( ( k k ) ( k ) a a η p( zk ok ) p( ok G, Zk ) p( G Zk ) a a a b b η p( zk ok ) p( ok G, ok ) p( ok zk ) p( G Zk ) a b Where a p ( zk ok ) s the lkelhood and ( a, b k k ) = = (3) (6) p o G o s the predcton model guded by goal G. Ths predcton model s avalable by the above traned NF-based pedestran moton model. B. Formulaton of Predcton In predcton process, we splt the problem nto short term and long term predcton. Short term predcton s to forecast the next status of the movng object whch s drectly nfluenced by moton models. Thus by the total probablty, the short term predcton p( o k+ o can be modeled by the combnaton of moton model ( p o, k+ s o and model p s o. weghtng ( k )

4 n ( k+ ( k+, ( p o o = p o s o p s o (7) = Predcton Model Weghtng In long term predcton, the future moton can be drected by the goal locaton. Consderng the uncertanty of predcton, the moton after n tme step s modeled by the combnaton of ndvdual goal models wth dfferent weghts. The formulaton of short term and long term predcton s derved at eq.(7)(8). ( k+ n = ( k+ n, ( p o Z p o G Z p G Z = j Predcton Goal Weghtng j j ( k+ n, ( k ( p o G o p o Z p G Z Trackng The term p ( o k+ n G, o p( ok+ n G, o a0 a0 = p( ok+ n G, ok+ n ) p( ok+ n G, o a0 a0 an = p( ok+ n G, ok+ n ) W p( ok+ G, o a0 (8) can be further factorzed nto eq.(9). af af Where W = p ( ok+ n f G, ok+ n ( f + ) ) f a f and f = ~ n 2 IV. ANYTIME PLANNING Real tme moton plannng s a very mportant and crucal constrant for moble robots n dynamc envronment. However, when the envronment s complex, t may not be possble to obtan the optmal soluton wthn the delberaton tme. Anytme algorthms always keep a current best soluton whatever the complete plannng fnshed. Thus anytme plannng wll be useful and approprate n such complex plannng problem. Due to these advantages, anytme plannng also becomes one of the most popular topcs n robotcs.[6, 9, 4]. In ths paper, we combne above predcton and trackng soluton wth an anytme planner. The results of predcton are easly ncorporated nto the confguraton tme space of robot. Moreover, a predctve anytme A* planner s developed to ncremental search the best trajectory n C-T Space. A. Confguraton Tme Space(C-T Space) To safely move n the crowded dynamc envronment, tme space nformaton s necessarly requred. It can effcently decrease the probablty of collson by consderng the state varaton of movng objects n tme doman. Thus our plannng algorthm ncorporates nformaton of movng objects wth above predcton module. Confguraton tme space of the robot s bult and real-tme updated. However, because of the huge computaton, t s mpossble to real-tme update entre C-T space. Thus t only updates the areas closed to the robot, called Actve Regon. Moreover, only the movng objects n the actve regon are consdered. (9) Fg. 6 shows a smple C-T space concept for collson detecton. Fg. 6 Confguraton tme space of pedestrans By predctng pedestran moton, t could detect the collson earler. B. Predctve Anytme A* Planner To effcently plan the trajectory n C-T space, an A* based planner s adopted. At the begnnng, an NF (Navgaton Functon) s calculated from the goal to entre envronment. NF can be avalable by Wave Front planner[5] and only needs to query once. It s a better ndcator to measure the relatve dstance from current poston to the goal, especally n the spral shape envronment. Thus NF s utlzed to be the heurstc term n our A* planner. Under anytme consderaton, the current best trajectory s returned after a lmted perod. The trajectory crterons are based on the total cost and heurstc term n the A*. The current best trajectory wll be the one that s closest to the goal and the total cost s stll under the threshold. The flowchart of our planner s shown n Fg. 7. Fg. 7 Predctve anytme A* planner V. SIMULATION RESULTS There are three dfferent smulatons to verfy the above proposed deas. The poston of robot and movng objects are gven n all smulatons. But the data assocaton of movng objects s unknown. In other words, the trackng module s requred n all the tests. The purposes of three tests are to demonstrate the performance of predcton, trackng, and anytme plannng.

5 A. Smulaton I The test envronment of smulaton I s dsplayed n Fg. 8(a). The red cones are the potental goals of the movng object. The movng object s movng from goal A to goal C. The smulated trajectory of the movng object s generated by tradtonal A* planner. Here TS and PTS of fgure capton means current tme step and predcted tme step. The orange surface represents the occuped probablty n predcted tme step. The hgher part of surface gets hgher occuped probablty and correspondng color wll be approachng golden yellow. Two tme steps TS = 3 and TS = 0 were recorded n smulaton I. In TS = 3, the estmated probabltes of potental goals from A to D are 0.6, 0.24, 0.35 and The probabltes are almost equal. Thus the predcted occuped probablty wll dvded nto 3 groups n Fg. 8(c)-(d). On the contrary, predcted occuped probablty wll focus on one group n TS = 0 (see Fg. 8(g)-(h)) snce estmated probabltes of potental goals from A to D are0.007, 0.04, 0.97 and B. Smulaton II The confguraton of smulaton II s shown n Fg. 9(a). The robot s movng from goal A to goal D. At the same tme, one movng object s movng n reverse drecton. In TS = 7, snce the probablty of goal A and B are relatvely hgh ( the predcted results are presented n Fg. 9(c)-(d)), the robot chooses the trajectory (red lne) passed through rght sde. The reason s that the trajectory of rght sde gets the lower probablty of collson even though t s longer than the left one. C. Smulaton III In smulaton III, we smulated a crowded envronment. Fve movng objects are movng smultaneously. The smulated envronment s dsplayed n Fg. 0(a). There are sx potental goals n ths envronment. The robot s assgned the task that safely moves from left sde to rght sde. At the same tme, the robot s requred to track all the movng object motons. One thng we should notce s that the robot chooses the trajectory of bottom sde n TS = 7 Fg. 0(d). The reason s obvously gven n Fg. 0(e). The bottom sde area of robot wll get the lower collson cost n the future. Moreover, the data assocaton of movng objects s shown as the trackng ID closed to the movng objects. The trackng results are compatble wth the ground truth. (a) (b) TS = 3, PTS = 3 (a) (b) TS = 7, PTS = 7 (c)ts = 3, PTS = 6 (d) TS = 3, PTS = 8 (c) TS = 7, PTS = 0 (d) TS = 7, PTS = 3 (e)ts = 0, PTS = 0 (f) TS = 0, PTS = (g) TS = 0, PTS = 4 (h) TS = 0, PTS = 5 Fg. 8 Smulaton I. where TS: tme step, PTS: predcted tme step (e) TS = 0, PTS = 5 (f) TS = 5 Fg. 9 Smulaton II

6 VI. CONCLUSIONS In ths paper, we proposed a predctve navgaton planner appled n the crowded dynamc envronment. A NF-based pedestran moton model s represented and traned by analyzng the collected pedestran trajectores. Moreover, by constructng the GVG skeleton of envronment, the potental goals can be automatcally extracted and appled for the moton predcton. A generalzed trackng and predcton framework s also ntroduced n ths paper. At the same tme, the recursve Bayesan formula s derved for real tme operaton. And t provdes useful nformaton to buld the confguraton tme space of robot. Fnally, a predctve anytme A* planner s proposed. The smulaton results show that t can effcently track and predct the future moton of movng objects. Besdes, anytme trajectory generaton s successfully appled n the complex dynamc envronment. In the future, we wll focus on the real moble robot mplement and further extended the deas to partally observable envronment. (a) (b) TS = 3, PTS = 3 (c)ts =5, PTS = 5 (d) TS = 7, PTS =7 (e) TS = 7, PTS = 0 (f)ts = 9, PTS = 9 REFERENCES [] M. Bennewtz, W. Burgard, G. Celnak, and S. Thrun, "Learnng Moton Patterns of People for Complant Robot Moton," The Internatonal Journal of Robotcs Research, vol. 24, pp. 3-48, [2] S. S. Blackman, "Multple Hypothess Trackng for multple target trackng," Aerospace and Electronc Systems,, vol. 9, pp. 5-8, [3] F. Carrera, T. Canas, A. Slva, and C. A. C. C. Cardera, "-Merc: A Moble Robot to Delver Meals nsde Health Servces," n Robotcs, Automaton and Mechatroncs, 2006 IEEE Conference on, pp. -8, [4] H. Choset and J. Burdck, "Sensor based plannng. I. The generalzed Vorono graph," n 995 IEEE Internatonal Conference on Robotcs and Automaton vol. 2, pp [5] H. Choset, K. M. Lynch, S. Hutchnson, G. Kantor, W. Burgard, L. E. Kavrak, and S. Thrun, Prncples of Robot Moton Theory, Algorthms, and Implementatons: The MIT Press, [6] D. Ferguson and A. Stentz, "Anytme RRTs," n Intellgent Robots and Systems, 2006 IEEE/RSJ Internatonal Conference on, pp , [7] A. F. Foka and P. E. Trahanas, "Predctve autonomous robot navgaton," n Intellgent Robots and System, IEEE/RSJ Internatonal Conference on, vol., pp , [8] B. Graf, M. Hans, and R. D. Schraft, "Care-O-bot II - Development of a Next Generaton Robotc Home Assstant," Autonomous Robots, vol. 6, pp , [9] E. A. Hansen and R. Zhou, "Anytme heurstc search," Journal of Artfcal Intellgence Research, vol. 28, pp , [0] D. Schulz, W. Burgard, D. Fox, and A. B. Cremers, "People Trackng wth Moble Robots Usng Sample-Based Jont Probablstc Data Assocaton Flters," Int. J. Robotcs Research, vol. 22, pp. 99-6, [] M. Y. Sheh, J. C. Hseh, and C. P. Cheng, "Desgn of an ntellgent hosptal servce robot and ts applcatons," n Systems, Man and Cybernetcs, 2004 IEEE Internatonal Conference on, vol. 5, pp [2] S. Thrun, "Learnng Metrc-Topologcal Maps for Indoor Moble Robot Navgaton,," Artfcal Intellgence, vol. 99, pp. 2-7, 998. [3] S. Thrun, M. Beetz, M. Bennewtz, W. Burgard, A. B. Cremers, F. Dellaert, D. Fox, D. Hahnel, C. Rosenberg, N. Roy, J. Schulte, and D. Schulz, "Probablstc algorthms and the nteractve museum tour-gude robot Mnerva," Internatonal Journal of Robotcs Research, vol. 9, pp , [4] J. P. van den Berg and M. H. Overmars, "Roadmap-based moton plannng n dynamc envronments," Robotcs, IEEE Transactons on [see also Robotcs and Automaton, IEEE Transactons on], vol. 2, pp , [5] H. C. Yen, "Path Plannng for Moble Robots n Dynamc Envronments," Master Thess, Department of Mechancal Engneerng, Natonal Tawan Unversty, (g) TS = 3, PTS = 3 (h) TS = 20, PTS = 20 Fg. 0 Smulaton III