WYKORZYSTANIE FILTRÓW CZ STECZKOWYCH W PROCESIE LOKALIZACJI ROBOTA MOBILNEGO THE APPLICTION OF PARTICLE FILTERS IN MOBILE ROBOT LOCALIZATION

Transkrypt

1 dr Barbara Sem tkowska IPPT PAN, Wydza Mechatronk PW dr Jacek Szklarsk Instytut Podstawowych Problemów Technk mgr n. Jan Syrczy sk Wydza Mechatronk PW mgr n. Potr W clewsk Wydza Mechatronk PW dr Mcha Gnatowsk Instytut Podstawowych Problemów Technk WYKORZYSTANIE FILTRÓW CZ STECZKOWYCH W PROCESIE LOKALIZACJI ROBOTA MOBILNEGO W pracy omówono metody okre lena zman po o ena robota moblnego w rodowsku 3D. Przedstawono dwe mplementacje fltru cz steczkowego, których celem jest przy peszene oblcze. Omówono metod, w której zmana po o ena orentacj odbywa s oddzelne oraz metod wykorzystuj c procesory grafczne w algorytme lokalzacj THE APPLICTION OF PARTICLE FILTERS IN MOBILE ROBOT LOCALIZATION In the artcle the overvew of localzaton methods s presented. Two modfcatons of partcle flter algorthm are descrbed. In the frst approach the poston and orentaton of the moble robot are determned separately. In second method parallel processng unts are used. Both methods allow us to speedup the process of localzaton. 1.WST P Robotyk mo na postrzega jako nauk traktuj c o percepcj modyfkacj wata rzeczywstego przez urz dzena sterowane kom puterem. Z zagadnenem tym zw zana jest nepewno nformacj o aktualnym stane wata. Nepewno ta jest spowodowana neprzewdywaln zmenno c rodowska oraz nedok adno c sensorów robota. Obraz z kam ery, dalmerza laserowego, czy przetwornka zlczaj cego k t obroty kó s to wszystko nformacje obarczone jakm b dem. W robotyce m oblnej jednym z kluczowych pyta na które robot mus sobe,,odpowedze '' brzm:,,gdze jestem?'' [1, 2, 3, 4, 5]. 344 Pomary Automatyka Robotyka 2/2011

2 Metody lokalzacj dzelm y w zale no c od konkretnego zastosowana. Mo emy wyró n : Lokalzacj lokaln robot zna swoj pozycj pocz tkow z dobr dok adno c przemeszcza s lokalne, tzn. o stosunkowo m a y odcnek drog, a nform acje o przemeszczenu pobera np. z odometr. Lokalzacj globaln pocz tkowa pozycja jest neznana robot na podstawe pom arów sam mus okre l swoje po o ene. Gdy ju to zrob, np. poprzez zaobserwowane jakego znacznka, m o e t nform acj wykorzystywa przy kolejnych ocenach pozycj. Problem porwanego robota podobne jak lokalzacja globalna, ale tu robot m us lczy s z tym, e w dowolnej chwl zostane przenesony w nne m ejsce. Jest to znaczne trudnejszy problem od zwyk ej lokalzacj globalnej. Metody lokalzacj zale tak e od rodzaju rodowska. Mo emy wyró n : rodowsko typu wn trze pomeszczena, w którym robot przem eszcza s po p aszczy ne, a uk ad can okre la g ówne kerunk rozmeszczena przeszkód. rodowsko zewn trzne, zwykle o du ych nerówno cach terenu jednocze ne z brakem wyró nonych kerunków g ównych. rodowsko statyczne w którym jedynym poruszaj cym s obektem jest robot moblny, a reszta otoczena ne zmena s. rodowsko dynam czne otoczene robota zm ena s (na dodatek nezale ne od akcj robota). Oczyw ce wszystke typowe rodowska, gdze obecn s ludze (jak np. wn trze budynku czy autostrada) s dynamczne. Z punktu wdzena lokalzacj, pom ary dokonywane przez robota m oblnego m o emy podzel na dwe kategore: wzgl dne bezwzgl dne. Do perwszych zalczy mo emy enkodery, a do drugej grupy GPS. a) b) Rys. 1. Mapa rastrowa otoczena robota, do której wykorzystano pomary z dalmerzy laserowych oraz pozycje, wyznaczona bez korekcj za pomoc obserwacj 2/2011 Pomary Automatyka Robotyka 345

3 Okre lane pozycj m etod pozycj zlczonej to najstarszy sposób lokalzacj. Polega on na wyznaczanu be cej pozycj na podstawe ostatno obserwowanej pozycj oraz przesun ca wyznaczonego za pom oc znanej pr dko c, kerunku ruchu m nonego czasu. Lokalzacja zlczenowa (na podstawe odom etr) jest mo lwa jedyne w sytuacj, w której robot przemeszcza s na stosunkowo krótkm odcnku. Na rys. 1a przedstawono przyk adow tras przebyt przez robota. Punkt A wskazuje po o ene pocz tkowe, punkt B po o ene ko cowe. Kolorem ó tym zaznaczono elps nepewno c po o ena robota. Nepewno wzrasta wraz z d ugo c trasy przebytej przez robota. Na rys. 1b przedstawono m ap rastrow otoczena utworzon w sytuacj, gdy nformacja o otoczenu jest okre lana jedyne na podstawe odom etr. Wyra ne wda, e mapa jest neprzydatna. W 1960 powsta a m etoda fltru Kalm ana [4], która przez dzes coleca dom nowa a w dzedzne fltrów adaptacyjnych. W robotyce moblnej najcz cej jest stosowany do okre- lena po o ena robota (wektora stanu) na podstawe obserwacj. W algorytme tym zak ada s, e rozk ad szumu pomarowego ma charakter gaussowsk. W fltrze Kalm ana estymacja stanu procesu jest lnow kombnacj predykcj stanu wa onej ró ncy m dzy wektorem obserwacj, a predykcj obserwacj. Metoda posada nast puj ce cechy: za o ono m odel lnowy, z o ono oblczenowa m etody wzrasta proporcjonalne do kwadratu lczby obserwacj, koneczne jest jednoznaczne okre lene pocz tkowego po o ena robota, przyj to gaussowsk rozk ad szumu, co ne zawsze dobrze opsuje rzeczywsto. W czase II wojny watowej J. Neum ann opracowa me tod Monte-Carlo, która jest zwana tak e metod quas-bayesowsk. Metody Monte-Carlo zosta y nezale ne opracowane w welu dzedznach nauk np. fzyce, statystyce, technce. Od lat 90-tych XX weku zacz y zast powa fltr Kalmana w lokalzacj robotów moblnych [6]. 2. FILTR CZ STECZKOWY W metodze tej po o ene robota w chwl t jest opsywane przez zbór cz steczek S t : x t okre la po o ene robota, S t = { x t, w t } = 1,..., N, (1) w t jest wag, która zosta a przypsana -tej cz steczce. Ka da cz stka reprezentuje pewne m o lwe po o ene robota w chwl t wag w t okre laj c przekonane, e robot m o e s w danym punkce znajdowa. Rozk ad cz stek odzwercedla rozk ad funkcj g sto prawdopodobe stwa, e robot znajduje s w pewnym obszarze przestrzen. Rys. 2 przedstawa de opsywanej metody. W kolejnych krokach na podstawe wskaza sensorów odometrycznych u t, dla ka dej cz stk okre lane s przybl one warto c x t 1 w chwl t+1. Nast pne dla ka dej warto c x t 1 oblczany jest wspó czynnk dopasowana w t 1. Wspó czynnk ten okre la, na le przewdywane po o ene cech otoczena ró n s od po o ena obserwowanego w danym punkce, w kolejnych krokach: wag s normalzowane, okre lany jest nowy rozk ad cz steczek. 346 Pomary Automatyka Robotyka 2/2011

4 Rys. 2. Rozk ad cz steczek Do podj ca decyzj zw zanych z realzacj trasy robota koneczna jest jednoznaczna nformacja o po o enu pojazdu, których m echanzm fltrów cz steczkowych ne daje. W takm wypadku stosuje s jedn z metod oszacowana aktualnej pozycj: redna wa ona, najlepsza cz steczka, redna wa ona w blskm otoczenu najlepszej cz steczk. 3. MODYFIKACJE FILTRU CZ STECZKOWEGO Lczba wygenerowanych cz steczek jest slne zale na od nedok adno c wskaza odometr oraz lczby param etrów, które m usmy estymowa. Po o ene robota przem eszczaj cego s w przestrzen 3D okre lane jest przez 6 parametrów: po o ene [x, y, z] T, oraz k ty nachylena do os OX, OY, OZ: [,, ] T. Przy du ej nedok adno c wskaza sensorów odom etrycznych potrzebujemy mlony cz steczek, aby uzyska zadowalaj c dok adno. Przy tak du ej lczbe cz steczek nemo lwe jest okre lane przemeszczena robota w czase rzeczywstym. W prowadzonych badanach testowano dwe m etody umo lwaj ce przyspeszene czasu oblcze : oddzelne korygowane zman orentacj przesun ca, wykonywane oblcze w sposób równoleg y. Eksperymenty przeprowadzono przy pom ocy robota Elektron, który by wyposa ony w dalm erz laserowy SICK [7]. Dane odebrane z dalm erza m aj posta c gu o d ugo c równej lczbe odebranych punktów. SICK LMS 200 m a rozdzelczo skanowana 1 o, 0,5 o lub 0,25 o dane odczytuje w zakrese o. Zamontowane takego lasera na g owcy obrotowej umo lwa otrzymane nformacj trójwymarowej o otoczenu, dane te zapsane s w postac chm ury punktów. Dla om awanego sposobu skanowana charakterystyczne jest zag szczene punktów przy os obrotu g owcy. Na rys. 3 przedstawono chm ur punktów otrzyman, gdy robot zosta umeszczony w korytarzu wydza u Mechatronk PW. W rodowsku typu wn trze pomeszczena trzy wzajemne prostopad e kerunk g ówne wyznaczone s przez pod og (suft), uk ad can oraz elem entów prostopad ych do can. Uk ad ten wyznacza wersory globalnego uk adu wspó rz dnych. Obserwuj c zmany kerunków g ównych mo emy okre l obrót robota wokó ka dej z os. 2/2011 Pomary Automatyka Robotyka 347

5 Rys. 3. Chmura punktów [8] 3.1 Okre lene zmany orentacj przesun ca Lokalzacja robota sk ada s z dwóch procesów: na pocz tku wyznaczana jest zmana orentacj robota, a w kolejnym kroku przesun ce robota wzd u ka dej z os. Ka dy z tych procesów realzowany jest przy pomocy fltru cz steczkowego. Chmura punktów ne jest wygodna w procese lokalzacj zwykle jest ona zapsywana w nnej forme. Jedn z nch jest okre lene zboru wektorów normalnych [9, 10]. a) b) c) Rys. 4. Zbór wektorów normalnych: a) chmura punktów, b) wektory normalne, c) hstogram kerunków normalnych [8] Na rys. 4b przedstawono zbór wektorów normalnych utworzony na podstawe chmury punktów przedstawonej na rys 4a. Rys. 4c prezentuje hstogram kerunków g ównych. Im bardzej popularny jest dany kerunek, tym wy szy jest odpowadaj cy mu werzcho ek. Stosuj c fltr cz steczkowy, mo emy okre l zman orentacj robota. W prowadzonych badanach zmany orentacj robota okre la cz steczka o najw kszej wadze. Znaj c orentacj robota w globalnym uk adze wspó rz dnych, m o emy wyznaczy przesun ce robota wzd u ka dej os. Przyj ta m etoda um o lwa znaczne zredukowane 348 Pomary Automatyka Robotyka 2/2011

6 lczby cz steczek. W nast pnym rozdzale opsane zostan wynk przeprowadzonych eksperymentów. Metoda ta ne m o e by stosowana w rodowsku neuporz dkowanym np. na zewn trz budynków. Dlatego rozpocz to badana dotycz ce wykorzystana procesorów grafcznych (technolog CUDA), które umo lwaj wykonywane oblcze w sposób równoleg y. 3.2 Oblczena równoleg e Procesory grafczne um o lwaj przetwarzane nform acj w sposób równoleg y [11, 12]. Równoleg o oblcze na platforme CUDA otrzymywana jest poprzez wywo ane tzw. j dra jako zboru w tków. W tk pogrupowane s w blok, te natom ast w tablce. Model wykonawczy technolog CUDA prezentuje rys. 5. Rys. 5. Model wykonawczy CUDA (zród o: NVda.com) Zarówno blok, jak tablce m og by jedno-, dwu- lub trójwymarowe. Ka dy w tek, blok tablca posada unkalny dentyfkator, który m o na wykorzysta do wyznaczana pozycj przetwarzanego elem entu w pam c. Archtektura CUDA zak ada ca kowt nezale no przetwarzana w obr be bloku, co utrudna zastosowane platform y do oblcze o charakterze ogólnym. W fltrze cz steczkowym, ka da z cz steczek jest nezale na od pozosta- ych w c oblczena wspó czynnków dopasowana mog odbywa s w c nezale ne. Wynk wst pnych eksperymentów zostan opsane w rozdz. 5. 2/2011 Pomary Automatyka Robotyka 349

7 4. Eksperymenty Opsane w tym rozdzale wynk eksperym entów zosta y zaczerpn te z [8]. Testowano wp yw szeregu parametrów na dok adno lokalzacj. Rys. 6. Wp yw zmany orentacj robota na dok adno otrzymanych wynków Na rys. 6 przedstawono wp yw rzeczywstego obrotu robota (wzgl dem os OZ) na dok adno otrzymanych wynków. Oblczena prowadzono dla cz steczek. Przy obroce powy ej 60 o b d gwa towne ro ne. Cecha ta wynka ze sposobu skanowana (rys. 3). Kolejnym stotnym parametrem jest wp yw pola obserwowanych powerzchn g ównych na dok adno okre lena przemeszczena robota. Analogczne jak poprzedno oblczena przeprowadzono dla cz steczek. Tab. 1. Wp yw powerzchn p aszczyzn domnuj cych na dok adno lokalzacj Okre lene przem eszczena robota by o wykonywane co 3 m. Zgodne z ntucj, w sytuacj, gdy obserwowany jest newelk fragment powerzchn b d okre lana przemeszczena jest du y (powy ej 10 %). Wynk przedstawono w tab Pomary Automatyka Robotyka 2/2011

8 Tab. 2 zawera wynk badana wp ywu przesun ca robota na dok adno lokalzacj. B d ro ne, je l robot przejecha powy ej 4 m. Wynka to z tego, e z pola wdzena robota znkaj p aszczyzny, które mo na dopasowa. Kolejnym parametrem jest okre lene wp ywu lczby cz steczek na dok adno lokalzacj. Stwerdzono, e powy ej cz steczek za o onym 10 % b dze odom etr dok adno lokalzacj ne wzrasta. Tab. 2. Wp yw przesun ca robota na dok adno lokalzacj Na rys. 7 przedstawono m ap korytarza utworzon na podstawe danych pochodz cych ze skanera laserowego. Kropkam zaznaczono m ejsca zberana pom arów. Oblczena przeprowadzono dla cz steczek okre laj cych zm an orentacj robota cz steczek okre laj cych przesun ce. Rys. 7. Mapa korytarza 2/2011 Pomary Automatyka Robotyka 351

9 Oprócz oblcze sekwencyjnych sprawdzono m o lwo stosowana technolog CUDA w procese lokalzacj. Na rys. 8 przedstawono zale no m dzy czasem oblcze na komputerze z procesorem Intel Centrno (2 x 2,0 GHz) (algorytm sekwencyjny) kart grafczn NVda GeForce 9300MG (m plementacja równoleg a). Na rys. 8 przedstawono stosunek czasu oblcze CPU do GPU dla ró nej lczby cz steczek. Stosowane GPU przyspesza proces lokalzacj w sytuacj, gdy lczba cz steczek przekracza Rys. 8. Stosunek czasu oblcze CPU do GPU dla ró nej lczby cz steczek 5. WNIOSKI W powy szym artykule przestawono dwe m plementacje fltru cz steczkowego. W perwszej metodze proces oblcze odbywa s sekwencyjne. Orentacja przem eszczene robota okre lane s oddzelne, dz k czemu mo lwe jest zmnejszene lczby cz steczek. W drugej metodze stosowana jest technologa CUDA. W st pne wynk eksperym entów wykaza y, e stosowane GPU ma sens wtedy, gdy lczba cz steczek przekracza Nasze dalsze prace b d dotyczy y zastosowana GPU w nawgacj robotów moblnych. 6. BIBLIOGRAFIA [1] J. Borensten, H. Everett, L. Feng, Where am I? Sensors and Methods for Moble Robot Postonng. Unversty of Mchgan, [1] R. E. Kalman, A new approach to lnear flterng and predcton problem s. Transactons of the ASME Journal of Basc Engneerng 82 (Seres D), 1960, str [2] P. Skrzypczynsk, Metody Analzy Redukcj Nepewno c Percepcj w System e Nawgacj Robota Moblnego, Vol. 407 Rozprawy. W ydawnctwo Poltechnk Pozna skej, Pomary Automatyka Robotyka 2/2011

10 [3] R.C. Smth, P. Cheesem an (1987), On the representaton and estm aton of spatal uncertanly. Int. J. Rob. Res. 5(4), 56 68, [4] S. Thrun, W. Burgard, and D. Fox, Probablstc Robotcs. The MIT Press, [5] Zhang, Z., Iteratve pont matchng for regstraton of free-form curves. Rapports de Recherche 1658, Insttut Natonal de Recherche en Informatque et en Automatque, [6] I. Reklets, A Partcle Flter Tutoral for Moble Robot Localzaton, Techncal Report TR-CIM-04-02, Center for Intellgent Machnes, McGll Unversty. [7] Chojeck. R, Olszewsk M., A Moble Robot for Laboratory Purposes and Its Applcatons. PAK, 3 (2009), 55, [8] J. Syrczy sk, Lokalzacja robota m oblnego w neznanym otoczenu m etod fltrów cz steczkowych, Praca magsterska PW, [9] B. Sem tkowska, J. Szklarsk, M. Gnatowsk, A. Zychewcz, Budowa hybrydowej semantyczno-rastrowej reprezentacj otoczena robota moblnego na podstawe wskaza dalmerza laserowego 3D, PAK, 3: , [10] A. Borkowsk, B. Sem tkowska, J. Szklarsk, Towards Semantc Navgaton In Moble Robotcs. In: G. Engles, C. Lewerenz, W. Schafer, A. Schurr, B. W estfechtel (Eds.): Graph Transformatons and Model Drven Engneerng Essays Dedcated to Manfred Nafle, LNCS 5765, Sprnger. [11] S. H. Seyed, Parallel processng and parallel algorthm s: theory and com putaton. Sprnger, [12] Harrs, Mark, Optm zng Parallel Reducton n CUDA, NVIDIA Developer Technology, 2/2011 Pomary Automatyka Robotyka 353