Neural Network Local Navigation of Mobile Robots in a Moving Obstacles Environment

Copy igh @ IF AC In elligen Componen s and Ins umen s o Con ol Applica ions, Budapes , Hunga y, 1994 NEURAL NETWORK LOCAL NAVIGATION OF MOBILE ROBOTS IN A MOVING OBSTACLES ENVIRONMENT J. GOMEZ-ORTEGA, E. F. CAMACHO and J. QUERO Dp o . Ing. de Sis emas y Au omc:i ica, Uni . de Se illa, A d. Reina Me cedes sin, Spain. Fax: +34-5-4556849, E-mail:[email p o ec ed] Abs ac .This pape p esen s a local na iga ion me hod based on gene alized p edic i e con ol. A modi ied cos unc ion o a oid mo ing and s a ic obs acles is p esen ed. An Ex ended Kalman Fil e is p oposed o p edic he mo ions o he obs acles. A Neu al Ne wo k implemen a ion o his me hod is analysed. Simula ion esul s a e shown. Key Wo ds- Mobile obo s; guidance sys em; obs acle a oidance; neu al Ne wo k; p edic i e eal- ime con ol; ex ended Kalman il e . 1. INTRODUCTION One o he mos impo an issues in he design and de elopmen o in elligen mobile obo s is he na iga ion p oblem. This consis s o he abili y o a ehicle o plan and execu e collision- ee mo ions wi hin i s en i onmen . This p oblem can be di ided in o wo hie a chical le els. The highe le el, called global na iga ion o pa h planning, is conce ned wi h he gene a- ion o a ajec o y (space- ime) om an ini ial con igu a ion o a goal con igu a ion, a oiding he known s a ic and mobile obs acles in he en i on- men . A his le el, only hose obs acles whose si - ua ion and mo ion a e p e iously known a e aken in o accoun . Al hough some wo ks p esen ed in he li e a u e conside he kinema ic and dynamic models o he ehicle (Shille and Gwo 1991), mos o hem only conside he geome ic app oach o he p oblem. Thei compu a ion ime is no ac- cep able o eal- ime con ol o mobile obo s. Some well known solu ions a e p oposed by: Fu- jimu a and Same (1989), based on including ime as one o he dimensions o he model wo ld. This allow hem o ega d he mo ing obs acles as be- ing s a iona y in he ex ended wo ld; Kan and Zucke (1986) p oposed a solu ion based on he decomposi ion o he ajec o y planning p oblem (TPP) in o wo subp oblems: he pa h planning p oblem (PPP), which is conce ned wi h planning he pa h o a oid s a iona y obs acles, and he eloci y planning p oblem (VPP) , which is con- ce ned wi h planning he eloci ies along he pa h o a oid mo ing obs acles. Al hough his educes he complexi y o he global p oblem, his solu ion 247 does no change he p ede ined pa h and i canno a oid mo ing obs acles wi h colinea ajec o ies o he obo 's. E dmann and Lozano-Pe ez (1987) p oposed a solu ion based on a planne o mo ing objec s ha cons uc s a con igu a ion space each ime an objec in he scene changes i s eloci y. The lowe le el, called local na iga ion o guid- ance, is conce ned wi h d i ing he ehicle h ough he ajec o y gene a ed by he global planne , now a oiding he unexpec ed obs acles (s a ic o mo ing), and compensa ing he unce - ain y o he con igu a ions and mo ions da a used by he global planne , using he eal- ime en i- omen in o ma ion p o ided by he senso sys- em. Usually, a his le el, he ehicle kinema - ics and/o dynamics, and kinema ics cons ain s (such as maximum eloci y o accele a ion) a e conside ed by he sys em. Kan and Zucke (1988) ha e p oposed a modi ica ion o hei algo i hm, adding a low con ol module, which compensa es he unce ain y in he eloci ies o he mo ing ob- s acles conside ed by he planne . The solu ion p oposed by Ka hib (1989) is based on he a i i- cial po en ial ield (APF) app oach, which is one o he mos popula me hods o eal- ime s a ic obs acle a oidance. Ano he app oach based on APF has been p oposed by Bo ens ein and Ko en (1989). They use he concep o he ce ain y g id o ob ain he epulsi e o ces om he da a o he senso s. G iswold and Eem (1990) ,based on Kan and Zucke s app oach, p opose a solu ion o un- expec ed mo ing objec s. The unce ain y in e - loci y and di ec ion o mo ing obs acles a e con- side ed simply as "noise". This noise sugges s ha speed and di ec ion angles o mo ing obs acles, ela i e o he obo , should be conside ed an- dom a iables, wi h a p ede e mined dis ibu ion , a a ixed ime. Wang and Tsai (1991) use a mod- i ied leas -mean-squa ed-e o classi ica ion algo- i hm (used in pa e n ecogni ion) o compu e a local collision- ee na iga ion pa h among mo ing obs acles wi h no a p io i posi ion in o ma ion, in an indoo co ido en i onmen . The ajec o ies o mo ing obs acles a e p edic ed by a eal- ime LMSE es ima ion algo i hm, and he speed alue o he obo is de e mined by he manoeu e ing boa d echniqu e used o nau ical na iga ion. Pa- pageo giou and S einkogle (1993) ha e p oposed an op imal con ol app oach o he p oblem o mo ing ehicles in changing en i onmen s. This app oach is he mos simila o he p oposed one, al hough hey gi e a nume ical solu ion o he minimiza ion p oblem while he e, a neu al ne - wo k (NN) solu ion is p esen ed. Papageo giou and S einkogle gi e imes o less han a second o sol e he op imal p oblem in some examples, using a e y simple kinema ic model (heading is no conside ed). A mo e complex model, which akes in o accoun he heading o he obo and he eloci ies o bo h d i ing wheels, is conside ed he e. Wi h his model, he nume ical solu ion e- qui es oo much compu a ion o eal- ime. 2. GENERALIZED PREDICTIVE CONTROL The Gene alzzed P edic i e Con ol (GPC) p o- posed by Cla ke e al. (1987) is an op imal con ol echnique, ha has inspi ed much esea ch wo k in h e ecen yea s. The obje i e o he GPC is o d i e u u e sys em ou pu s (in ou case he obo posi ion and o ien a ion) close o hei desi able alues in some sense, bea ing in mind he con ol ac i i y equi ed o do so. This is done using a e- ceding ho izon app oach o which, a each sample ins an , using a p edic ion model o gene a e a se o p edic ed ou pu s , some app op ia e quad a ic unc ion J o he u u e e o s and con ols is min- imized, assuming ha a e some con ol ho izon H u he inc emen s in con ol a e ze o. Only h e i s con ol is applied, esul ing in a con ol law ha belongs o he class known as Open-Loop- Feedback- Op imal con ol. The cos unc ion J can be o he o m: H J(H , ~ V) = 2)X( + i) -Xd( + i) ;=1 H + LA[~V( +i-1W ;=1 whe e X is he ec o o p edic ed ou pu s , Xd is he ec o o desi ed alues o X, V is he con ol a iables ec o , and A is a weigh ing ac o . Some 248 esea ch has been done aiming o apply his ech- nique o he pa h acking p oblem (Olle o and Amidi 1991). This pape p oposes a modi ica ion o he cos unc ion J o include a e m ha penalizes he p oximi y o any obs acle (s a ic o mo ing) in any o he H nex sample ins an s . This leads o an obs acle a oidance wi h low con ol cos . The idea is ha i i is no iced ha a collision may be p oduced in he u u e, i will begin o a oid his si ua ion wi h smoo h con ol ac ions . The p oblem can be de ined as ollows: gi en a a- jec o y (space- ime), d i e he obo o ollow i using he on-line senso da a, a oiding he unex- pec ed obs acles ound in he en i onmen , and compensa ing he unce ain ies in he da a (posi- ions o he obs acles) used by he global planne . The new cos unc ion J is (see Fig. 1): H J(H , ~ V) = L[X( + i) -Xd( + i)]2 ;=1 H + L(Al([~V ( + i -IW + [~VI( + i -1)]2) ;=1 NMO H ~1 + j; (~ PI ( + i)[dis (X( + i), X MOi ( + i)))2) NSO H 6 + j; (~ Pz( + i)[dis (X( + i), X SOi ( + i))]2) FIG. 1. Obje i e Func ion J(H, :.V) o H=l and one s a ic obs acle whe e XCi) = {xCi), y(i), B(i)} is he posi ion and o ien a ion ec o o he obo in he sample in- s an i, Xd(i) = {xd(i), Yd(i), Bd(i)} is he de- si ed posi ion and o ien a ion ec o o he con- ol ho izon, and X M Oi (i) = {xmoi (i), ymoi (i)} and X SOi (i) = {xsoi (i), ysoi (i)} a e he posi- ions o he mo ing and s a ic obs acles in he sample ins an i. V and VI a e he igh and le eloci ies o he wo d i ing wheels, which a e he con ol a iables. N M 0 and N SO a e he num- be o mo ing and s a ic obs acles espec i ly, and A1, A2 , 6 and 6 a e weigh ing ac o s. P1 and P2 a e he co a iance ma ices o he p edic ions o he u u e posi ions o he obs acles, and dis is he euclidean dis ance be ween he obo and he obs acles. Fo his o mula ion a model is needed o p edic he u u e posi ions and o ien a ions o he obo and a model o p edic he posi ions o he mo ing obs acles. The ollowing kinema ic model (which co esponds o a di e en ial-d i e ehicle) is used o he i s issue: {}(k + 1) = (}(k) + AT x(k + 1) = x(k) + ~(sin({}(k) + AT) -sin({}(k))) V y(k + 1) = y(k) -A (cos({}(k) + AT) -cos({}(k))) Y y FIG. 2. Re e ence F ame whe e x, y, {} a e he posi ion and o ien a ion o he obo in a ixed e e ence ame, A = w , and V = V V ,. T is he sample ime and W is he dis ance be ween wheels (see Fig. 2) . 3. MOBILE OBSTACLE MOTIONS PREDICTION To p edic he posi ions o he mo ing obs acles in he u u e, when he eloci y o he obs acle is as- sumed o be cons an be ween sampling in e als, he ollowing non linea model can be conside ed: xo(k + 1) = xo(k) + V(k + l)cos({}o(k + 1)) yo(k + 1) = yo(k) + V(k + l)sin({}o(k + 1)) V(k) = ((xo(k) -xo(k -1))2+ (Yo(k) -yo(k - 1)2)1/2 yo(k) -yo(k - 1) (}o(k) = a c an(xo(k) _ xo(k -1)) 249 Yn(lc+J) >;,(lc) ....................... ....... .~?~! .... i yo(k·2) ,,<, (k·l) ,,<, (k) FIG. 3. Obs acle mo ion pa ame e s whe e Xo , Yo a e he posi ion o he mobile obs a- cle, V is he lineal eloci y be ween wo posi ions and {}o is he angle o he eloci y ec o in espec o he ho izon al x axis (see Fig. 3). Now , he ollowing hypo hesis a e made: V(k + 1) = V(k) ~(}o(k + 1) = ~(}o(k) = (}o(k) - (}o(k - 1) The Ex ended K alman Fil e app oach is applied o his model o p edic he u u e posi ions o he mobile obs acle and hei co a iance ma ices P1(k + jlk). The esul o he p opaga ion cycle o H pe iods o p edic ion a e he ollowing equa- ions: j-1 xo(k + jlk) = xo(klk) + V(k)TL cos(/1-(i)) ;=0 j-1 yo(k + jlk) = yo(klk) + V(k)TLs i n(/1-(i)) ;=0 ( ')-2 (yo(k+ilk)-yo(k+i-llk)) /1- 1 - a c an xo(k + ilk) _ xo(k + i-Ilk) _ a c an ( .=...Yo:..,:(_k _+_i_-----!II--'k ):--.......::..,Yo:....:,(_k _+_i_-_2-.!I--.:..k) ) xo(k + i-Ilk) -xo(k + i - 21k) Wi h he ac ualiza ion cycle only xo(k + Ilk + 1), Yo(k + Ilk + 1) and he ac ualized co a iance ma ix P1 (k + 11 k + 1) a e calcula ed, because only measu es o he k + 1 ins an a e a ailable; bu hese educe he co a iance ma ix o he nex p edic ions. To ake in o accoun he p edic ion unce ain ies, he dis ances be ween he obo and he obs a- cles a e penalized wi h he co a iance ma ices ob ained om he Kalman Fil e equa ions. In he compu a ion o he obo -obs acles dis ances, i will be assumed ha he obs acles ha e a ci - cula o m. 4. THE NEURAL NETWORK APPROACH The minimiza ion o he cos unc ion J canno be ob ained in eal ime wi h nume ical me h- ods. So, a Neu al Ne wo k solu ion is p oposed, which gua an ees eal ime o he obo con ol. The Neu al Ne wo k app oach o obo guidance has been p oposed by o he esea ches (Pome lau 1990), (Meng and Pic on 1992). The a chi ec u e o he NN con olle consis o a single hidden laye backp opaga ion ne wo k. V (K·I) VI (I . I) OBSTAC. NEURAL V (k) PARAM. NETWORK VI (I ) DESIRED TRA iC. PARAM. FIG. 4. Neu al Ne wo k Scheme The inpu laye consis s o h ee modules (see ig . 4). The i s one includes wo ne wo k in- pu s associa ed wi h he con ol alues in he las sample ins an . The second module co e- sponds o he obs acle pa ame e s: wo inpu s o each s a ic obs acle (dis ance and o ien a- ion), and i e inpu s o each mo ing obs acle (xo(k), Yo(k) , V(k + 1), Bo(k + 1) , ~£qk + 1 )). The hi d module includes en ne wo k inpu s which co espond o he pa ame iza ion o he desi ed ajec o y in he nex H sample ins an s. These pa ame e s consis o he loca ion and o ien a ion o he i s poin o he local ajec o y, he cu a- u e o he nex H poin s and an a e age desi ed eloci y. The ou pu laye consis s o wo nodes which co espond o he con ol command: he le and igh wheels eloci ies. The aining module consis s o a backp opaga- ion scheme, shown in ig . 5, whe e he aining pa e ns a e sol ed by a GPC module which uses a nume ical me hod o gene a e he ou pu s . The aining pa e ns a e selec ed p ope ly o ep e- sen all he possible si ua ions o d i ing among obs acles. Also, a local e e ence sys em is used o educe he pa ame e s ange o a ia ion. The NN app oach will be as ollows: • A ime k, ob ain he obs acles posi ion pa- ame e s om he senso sys em. P edic he u u e posi ions o he mo ing obs acles. 250 GP C MODULE NEURAL NETWORK MODULE GPC BACKPROPAGATION MODULE FIG . 5. Neu al Ne wo k aining Scheme • Using he NN , p edic he u u e H posi ions o he obo as i no obs acles whe e encoun- e ed. • De ec i a collision may be p oduced in he nex H sample ins an . • i no , apply he i s NN con ol ou pu s o he ehicle. • i a collision may be p oduced, hen use he p edic ed posi ions and o ien a ions o he obo as he new desi ed ajec o y, and com- pu e he NN again, now conside ing he ob- s acles. The use o he p edic ed posi ions o he obo as a new desi ed ajec o y is jus i ied by he educ- ion o he numbe o aining pa e ns necessa y o ob ain a good pe o mance because a syme y analysis has can be made. In his analysis i has been supposed ha he obo is always on he de- si ed pa h. Finally, i is impo an o no ice ha his me hod sol es, a he same ime, he p oblems o ind- ing a oidance local ajec o ies and he con ol one. This gua an ees ha he ajec o y ollowed by he obo is con inuous in cu a u e, a oiding discon inui ies in he wheels eloci ies, which p o- duces e o s be ween he p edic ed and he eal posi ion o he ehicle. 5. RESULTS The p oposed con ol s uc u e has been es ed by simula ion wi h a model o he Labma e mobile obo (T. R. C. 1989). The NN used consis ed o se en een inpu neu ons co esponding o he pas con ol ac ions, pas mobile obs acles posi- ions and u u e e e nce ajec o y. The hidden laye was composed o 35 neu ons and he ou pu laye consis ed o wo neu ons co esponding o he le and igh wheel eloci ies o he mobile obo . The NN was ained in a supe ised manne as de- sc ibed p e iously. The con ol ho izon choosen o he G PC was made equal o six. And he weigh ing ac o s we e gi en he ollowing alues: ~I = 3,6 = 3, Al = 63, A2 = 10. Whe e Al and A2 co espond o he weigh s o he module o he eloci y and o he angula eloci y espec i ely. The high alue o Al is o make su e ha he GPC will no choose he easy solu ion o s opping he obo and wai o he mobile objec o pass. The ela i ely high alue o A2 ensu es a smoo h a- jec o y. Fig. 6 shows he e olu ion o he e - o unc ion E(i) = E '::I(Od(i) -O(i))2 o he aining phase, whe e N is he numbe o aining pa e ns, and Od(i) and O(i) a e he desi ed and ne wo k ou pu o he i e a ion i. The simula ed esul s ob ained o h ee si ua- ions, di e en om he aining cases, can be seen in ig. 7. The ajec o ies shown on he le hand side o ig. 7 co espond o he solu ion~ ob ained when applying he GPC con olle while he ajec o ies shown on he igh hand side co - espond o he solu ions ob ained wi h he NN. Fig. 7.1 co esponds o an obs acle which is com- ing owa ds he mobile obo , while igu es 7.2 and 7.3 co espond o obs acle ajec o ies c oss- ing he mobile obo ajec o y. The ajec o ies shown in ig. 7.3 co espond o a case whe e he ini ial posi ion o he mobile obo is no si ing on he desi ed pa h. In all cases, he GPC and NN make he mobile obo ake he necessa y e a- si e con ol ac ions. As expec ed, he NN ep o- duces he beha iou o he GPC con ol qui e well and akes only small ac ion o he compu a ion ime equi ed o sol ing he GPC which has o be sol ed using a nume ical op imiza ion algo i hm (powell me hod has been used he e). 3.0 -----------~-----_, c 2.0 0 U c .2 e w 1.0 0.0 L---.::::::=========:::I::===------,-J 0.0 100 .0 200 .0 300 .0 Numbe o i e a ions (x 4000) FIG. 6. E olu ion o he e o unc ion 6. CONCLUSIONS A local na iga ion me hod based on he GPC con- ol algo i hm has been p oposed. A new cos unc ion which ' penalizes he in e se o he dis- 251 ance be ween he obo and he mo ing and s a ic obs acles has been p esen ed. A p edic- ion me hod o he u u e posi ions o he mo- bile obs acles has been s udied. Finally a Neu al Ne wo k implemen a ion o he GPC me hod has been p oposed o ob ain eal- ime pe o mance. Simula ion esul s ha e been p esen ed. 7. ACKNOWLEDGEMENT The au ho s would like o acknowledge he CI- CYT o unding his wo k unde g an s TAP93- 0408 and TAP93-0581. REFERENCES Bo ens ein, J. and Y. Ko en (1989). Real- Time Obs acle A oidance o Fas Mobile Robo s. IEEE ans. on Sys em , Man , and Cybe ne ics, o!. 19, nO 5, pp 1179- 1187. T . R. C. (1989). Labma e e e ence manual. (T ansi ions Resea ch Co po a ion) . Cla ke, D. W., C. Moh adi and P. S. Tu s (1987). Gene - alized P edic i e Con ol - Pa I. The Basic Algo- i hm . Au oma ica, ol 23, no 2, pp 137-148. E dmann, M. and T. Lozano-Pe ez (1987). On mul iple mo - ing objec s. Algo i hmica, o!. 2, no. 4 pp 477-521. Fujimu a, K. and H. Sa ne (1989). A Hie a chical S a egy o Pa h Planning Among Mo ing Obs acles .. IEEE ans. on obo ics and au oma ion, o!. 5, no. 1, pp 61-69. G iswold, N. C. and J . Eem (1990). Con ol o mobile Robo s in he P esence o Mo ing Objec s. IEEE ans . on Robo ics and Au oma ion , ol 6, no 2, pp 263-268. Kan , K. and S. W. Zucke (1986). Towa d E icien T a- jec o y Planning: The Pa h- Veloci y Decomposi ion .. The In . jou nal o Robo ics Resea ch, o!. 5, no. 3, pp 72-89. Kan , K. and S. W. Zucke (1988). Planning Collision-F ee T ajec o ies in Time- a ying en i onmen s : A Two- Le el hie a chy .. P oc. IEEE In . Con . on Robo ics and Au oma ion, pp 1644-1649 . Ka hib, O. (1989). Real- ime Obs acle A oidance o Ma- nipula o s and Mobile Robo s. The In . Jou nal o Robo ics Resea ch, o!'5, no. 1, pp 90-98 . Meng, H. and P. D. Pic on (1992). Obs acle A oidance Using a Neu al Ne wo k Con olle and Visual Feedback. P oc. SICICA 92, pp 563-568. Olle o, A. and O. Amidi (1991). P edic i e Pa h T acJ.:ing o Mobile Robo s. Aplica ions o he CMU Na lab .. P oc. IEEE Fi h In . Con . on Ad anced Robo ics (Pisa), ol 2, pp 1081-1086. Papageo giou, M. and A. S einkogle (1993). Real- Time Op- imal Con ol o Mo ing Vehicles in Changing En i- onmen s. P oc . o he 12 h IFAC Wo ld Cong ess (Sydney), ol 3, pp 107-112. Pome lau, D. A. (1990). Neu al Ne wo k Based Au onomous Na iga ion, (Vision and Na iga ion. The Ca negie Me/Ion Na lab). C.E. Tho pe Edi o ., Kluwe Aca- demic Publishe s, pp 83-93. Sha ma, R. (1992). Locally E icien Pa h Planning in an Unce ain, Dynamic En i onmen Using a P o ba- bilis ic Model. IEEE ans . on Roboics and Au oma- ion, ol8, no 1, pp 105-110. Shille , Z. and Y. Gwo (1991). Dynamic Mo ion Planning o A u onomous Vehicles. T ans . IEEE on Robo ics and Au oma ion , ol 7, no 2, pp 241-249. Wang, L. and W. Tsai (1991). Collision A oidance by a Modi ied Leas -Mean-Squa e-E o Scheme o In- doo Au onomous Land Vehicle Na iga ion . In . Jou nal o Robo ics Sys ems , ol 8, no 5, pp 677-698 . M i >- . . 10 .0 --~--~--~--_--~-- ---, 8.0 6.0 4.0 2.0 ~I.O .... ++-H~1.12 1 .1:l~ . '( I . I .. I , . i. ~I.' "+-1- '--- 1·'1"~1 1.1 1 +--HoIIowed T ajec o y ><-->< Desi ed T ajec10 y ., .. .. •. • -. Obs acle ajec o y 1.0 0.0 L---::,:--:":--~:---::',.--~:---~---,J 1.0 2.0 10 4.0 5.0 6.0 7.0 8.0 1 (a) X (moIe s) 9 .0 ---- ---_-- ....... --....,----,.-----, 8.0 ~--<. 7.0 pO .. ·"'- -·- · -·-· ~X' · ~·~ ·-· )' " >- . .! ,.e ' , 5.0 4.0 , ' ~. , , , , 3.0 L-_--:~ _ __:~-__:~--~-- ~----,J 0.0 1.0 2.0 3.0 4.0 5.0 6.0 2 (a) X (moIo s) 10 .0 ---~--_---_--~--_--__, 1.0 ........ 1.0 8.0 I 6.0 >- 4.0 2.0 L- __ -'- __ ~ __ ~ __ ~ __ ~ __ --' 1.0 2.0 3.0 4.0 5.0 6.0 7.0 3 (a) X (mo "") .. ;;; I 8.0 6.0 >- 4.0 2.0 0.0 L-_-::,:_:--:":-_-:,::--_::',.-_ -::-_-:-:-_-:, 1.0 2.0 10 4.0 5.0 6.0 7.0 8.0 1 (b) X (moIe s) 9.0 ---- ---..,.--- ....... --....,-----,----, 8.0 7.0 5.0 4.0 10~--~---~--~--~:---~ ~-~ 0.0 1.0 2.0 10 4.0 5.0 6.0 2 (b) x (mol ... ) 10 .0 ,---_ _--..,.--- ....... -- _,.----,----, 8.0 -; I 6.0 >- 4.0 2.0 ':----:7"----:7"----:':----:7"----:7"---:' 1.0 2.0 10 4.0 5.0 6.0 7.0 3 (b) X (me e s) FIG. 7. Resul s . The le igu es co espond o he nume ical sol ed GPC. The igh igu es co espond o he Neu al Ne wo k solu ion. 252