Dr Barbara Semtkowska Instytut Podstawowych Problemów Technk PAN HYBRYDOWA REPREZENTACJA OTOCZENIA ROBOTA MOBILNEGO W ponszym artykule przedstawone zostane zastosowane hybrydowej reprezentacj otoczena do jednoczesnego tworzena mapy lokalzacj robota moblnego na podstawe wskaza dalmerza laserowego. W procese lokalzacj zastosowane s fltry czsteczkowe oraz metody badana przesunca hstogramów ktowych. Połczene obu metod umolw dokonywane lokalzacj w czase rzeczywstym. A HYBRID MAP FOR MOBILE ROBOTS NAVIGATION In ths paper a method of Monte-Carlo localzaton s presented. A D grd map s bult. The map contans the features of an envronment. The robot poston s presented as a set of partcles. The observaton model s used to evaluate the partcles. Expermenst preformed n a real envronment are performed n order to demonstarte that the sytem s able to track the poston of a moble robot equpped wth laser range fnder. 1. WSTP Podstawowe problemy jake wystpuj podczas jednoczesnego tworzena mapy otoczena lokalzacj robota moblnego to wybór odpowednej metody reprezentacj otoczena. W praktycznych zastosowanach spotykamy nastpujce sposoby przedstawana map: reprezentacja rastrowa, mapa cech, topologczna reprezentacja sceny, reprezentacja hybrydowa rastrowo topologczna. W przypadku reprezentacj rastrowej mapa otoczena jest reprezentowana jako tablca komórek, z których kada reprezentuje pewen newelk fragment otoczena robota. Wartoc przechowywane w komórkach s lczbam z przedzału [0,1] okrelaj stope potwerdzena hpotezy e odpowadajcy obszar jest zajty przez przeszkody. Rastrowa reprezentacja sceny umolwa szybk agregacj danych pochodzcych z rónych sensorów. Najczcej stosowana jest probablstyczna metoda agregacj [1]. W lteraturze opsywane s 1
take metody wykorzystujce teor zborów rozmytych Shafera-Dempstera []. Reprezentacja rastrowa jest wygodna w procese planowana bezkolzyjnej trasy. Podstawowym problemem w przypadku stosowana map rastrowych jest to, e dokładno metod lokalzacj jest cle zwzana z rozdzelczoc mapy. Dua rozdzelczo mapy poprawa jako metod lokalzacj, ale jednoczene czas oblcze ulega znacznemu wydłuenu. W przypadku mapy cech otoczene robota jest reprezentowane jako zbór cech (krawdze, naroa, walce, obekty o wyrónajcym s kolorze) z których kady jest opsywany parametryczne[3][4]. Lokalzacja robota na podstawe mapy cech polega na okrelenu zboru cech danego odczytu porównanu go ze zborem cech zapamtanych w mape. Aby wybrane obekty mogły by znacznkam, pownny by obserwowane przez ruchomy pojazd przez pewen czas by odporne na przypadkowe szumy, np. nne poruszajce s obekty. Wybrane cechy pownny by nezmenncze tzn. nezalene od połoena orentacj robota. Ich wybór jest cle zwzany z rodzajem sensorów, w jake pojazd jest wyposaony. W przypadku, gdy robot obserwuje otoczene przy pomocy kamery, bdze to obekt o unkalnym kolorze lub kształce. W przypadku czujnków aktywnych - sonarów lub dalmerzy laserowych znacznkam mog by cany, naroa, wnk lub drzw. Jel pojazd wyposaony w dalmerz laserowy porusza s wewntrz budynku to w wkszoc systemów jako obekty charakterystyczne przyjmuje s fragmenty can. Mapa cech jest bardzo efektywnym sposobem opsu sceny, gdy zapamtywane s nformacje jedyne o fragmentach przeszkód. W przypadku gdy scena zawera wele obektów zbór cech jest duy, a ch dopasowane staje s zadanem netrywalnym. W mapach topologcznych otoczene jest reprezentowane jako graf. Wzły grafu opsuj mejsce w którym robot moe s znale. Przechowywana jest nformacja o pewnych cechach otoczena, które mog by w danym mejscu obserwowane. Dwa wzły grafu s ze sob połczone, jel stneje bezkolzyjna trasa łczca opsywane mejsca. Problem planowana trasy jest traktowany jak problem poszukwana najkrótszej cek w grafe. Lokalzacja robota bdze polegała na znalezenu wzła grafu dla którego zapamtane cechy otoczena s najbardzej podobne do cech obserwowanych. Reprezentacja topologczna umolwa zapamtane w sposób efektywny nformacj o otoczenu robota planowane tras w przypadku, gdy pojazd porusza s po bardzo duych obszarach. Przemeszczane s robota mdzy wzłam grafu wymaga jednak, dodatkowych nformacj o otoczenu. Typowe metody odruchowe np. jazda wzdłu cany mog by ne wystarczajce w otoczenu zmenajcym s dynamczne. Kolejn wad jest to, e trasa mus przebega przez wzły grafu, jest to bardzo slne załoene w przypadku, gdy wzły grafu s od sebe znaczne oddalone. Jel robot posługuje s map topologczn reprezentowan przy pomocy grafu, to ne stneje molwo sprawdzena w którym wle aktualne s znajduje robot, dlatego w kadym wle umeszczana jest nformacja metryczna. Informacja ta zawera najczcej ops otoczena, które w danym wle jest obserwowane. Przykład takego opsu moemy znale w pracy Menegattego [4]. W
kadym wle przechowywanych jest 10 składowych transformaty Fourera obrazu z omnkamery otrzymanego w danym punkce otoczena. Jel robot zbudował lokaln globaln map otoczena to porównujc nformacj w mapach pownen okrel swoje połoene. Metody okrelena połoena pojazdu dzelmy na trzy grupy: korelacja danych przechowywanych w mape lokalnej globalnej, fltr Kalmana, metody typu Monte-Carlo z których najbardzej znane s fltry czsteczkowe.. SPRZT W prowadzonych eksperymentach wykorzystywany był robot Elektron wyposaony w dalmerz laserowy SICK LMS 00. Zdjce robota przedstawono na rysunku 1. Rysunek 1: Robot Elektron Robot jest wyposaony w czujnk odometryczne, ale błd okrelena zmany orentacj pojazdu w welu wypadkach przekracza 10 o.. nezbdne jest wc zastosowane systemu który umozlw dokładn lokalzacj pojazdu w przestrzen. Na podstawe danych z lasera mona okrel połoene wystpujcych w otoczenu pojazdu przeszkód. Na rysunku przedstawono zdjce przykładowej sceny wynk wskaza dalmerza. W prowadzonych eksperymentach rodzelczo skanowana wynosła 1 o. 3
Rysunek : Wynk pomarów skanera dla zadanego otoczena 3. REPREZENTACJA OTOCZENIA W prowadzonych badanach przyjto hybrydow: rastrowo-obektow reprezentacj sceny. Otoczene robota dzelone jest na elementarne podobszary klatk o wymarach 10 x10cm. Kadej komórce przyporzdkowujemy lczb z przedzału [-1,N], która okrela numer obektu, który w danej komórce został wykryty. Jel komórka jest wolna od przeszkód to przypsuje s jej warto 1. Obekty, wykrywane na postawe wskaza dalmerza laserowego to: fragmenty can, które reprezentowane s jako odcnk opsywane w postac cgu parametrów (d, α, dl, x s,, y s, σ d, σ α), gdze α jest ktem nachylena normalnej do os OX, d- jest odległoc pocztku układu współrzdnych od prostej, ( x s,, y s ) współrzdne rodka wykrytego odcnka, dl jest długoc wykrytego odcnka), σ d, σ α - warancje parametrów d α. Fragmenty kolumn, które s opsywane jako okrg s reprezentowane przez nastpujce parametry: (d, α, R, σ d, σ α, σ R), gdze d odległo rodka okrgu od pocztku układu współrzdnych,, α- nachylene odcnka łczcego rodek okrgu z pocztkem 4
układu współrzdnych, R prome okrgu, a wartoc σ d, σ α, σ R to warancje parametrów d, α, R. Naroa s reprezentowane jako para prostych Obekty neregularne s reprezentowane jako łamane Sposób wykrywana obektów został przedstawony w artykule[10]. Po wykrycu nowego obektu przypsywany jest mu numer. Identyfkator jest przechowywany we wszystkch komórkach mapy rastrowej w których fragmenty wykrytego obektu s znajduj. Tak sposób reprezentacj otoczena umolwa proste dopasowane cech, mapa zawera te stotne nformacje o kerunkach wystpowana przeszkód. W przypadku otoczena przedstawonego na rysunku wykryto 3 obekty: fragmenty dwóch can oraz kolumn. Połoene robota w chwl t jest opsywane przez wektor [ x t, y t, θ t ]. Wartoc [ x t, y t, θ t ] s wartocam zaburzonym. Nedokładno zaley od czasu przejazdu, błdów wskaza czujnków odometrycznych błdów nesystematycznych. W najstarszych systemach nawgacyjnych robotów moblnych do okrelena zman połoena robota moblnego stosowany był fltr Kalmana, a nepewno połoena robota okrelana była przez podane elpsy nepewnoc [7]. Fltr Kalmana generuje bardzo due błdy w sytuacj, gdy modele s slne nelnowe, zakłada Gaussowsk rozkład błdów. W przypadku obserwowana welu cech macerze kowarjancj s bardzo due, a to wpływa negatywne na efektywno algorytmu. Od 000r rozwjane s metody wykorzystujce łacuch Markowa metody optymalzacj Monte-Carlo. Najbardzej znane s tzw. fltry czstkowe [7][8]. W opsywanym systeme zastosowana została zmodyfkowana metoda przedstawona w pracy [8]. 4. FILTR CZSTKOWY Nepewno połoena robota opsuje wzór: px t z 1..t,u 0,..,t 1=pz t x t xt 1 px t x t 1,u t 1px t 1 z 1:t 1,u 0:t dx t 1 (1) u 0,.., t 1 - jest cagem sterowa do chwl t-1, z 0,.., t - jest cgem obserwacj, px t x t 1,u t 1 jest modelem przemeszczana se robota, okrela prawdopodbestwo tego, e robot znajdujcy s w chwl t-1 w punkce x t 1 przy sterowanach u t 1 znajdze s w chwl t w połoenu x t. W przypadku fltrów czstkowych połoene robota w chwl t jest opsywane przez zbór czstek S t ={ x t, w t } =1,..,N. Kada czstka reprezentuje molwe połoena robota wag okrelajc przekonane, e robot moe s w danym punkce znajdowa. 5
Algorytm okrelana połoena robota składa s z nastpujcych etapów: Wartoc x t s molwym połoenam robota. Rozkład punktów odzwercedla rozkład nepewnoc połoena robota. Wag w t okrelaj stope potwerdzena hpotezy, e robot w chwl t jest w danym punkce. W kolejnych krokach na podstawe wskaza czujnków odometrycznych wartoc, dla kadej czstk okrelane s przyblone warto x t1. Nastpne dla kadej wartoc x t1 oblczany jest współczynnk dopasowana w t1. Współczynnk ten okrela na le przewdywane połoene cech otoczena rón s od połoena obserwowanego w danym punkce. Jel cecha opsywana jest przez parametry (d, α, dl, σ d, σ α) to przyjmujemy, e w t1 okrelony jest wzorem: w t1 = d d 1 p e d 1 d p e () Nowa waga okrelona jest wzorem w t1 = w t w t1 Nastpne wag s normalzowane: (3) w t1 = w t1 M j= 1 w t1 j (4) Ze zboru czstek usuwane s te dla których wag s blske zeru. W welu systemach z kad czstk wzana jest mapa otoczena robota. W pracy \cte{} przedstawono metod optymalnego zapamtywana map. W prowadzonych badanach zdecydowałam s na reprezentowana otoczena robota przy pomocy jednej mapy. W procese uaktualnana mapy przyjłam, e połoene robota jest okrelane przez kombnacj wypukł 10 czstek o najwekszych wagach 5. MODYFIKACJE METODY 6
Lczba generowanych czstek jest w duym stopnu zwzana z nepewnoc okrelena połoena robota na podstawe czujnków odometrycznych. W przypadku robota Elektron błd okrelena przemeszczena robota był newelk, ale błd okrelena zmany orentacj był bardzo duy w welu wypadkach przekraczał 10 o, bezporedne stosowane fltru czsteczkowego w takej sytuacj jest czasochłonne. Jel robot porusza s w otoczenu typu wntrze to w wekszoc wypadków przeszkody umeszczone s wzdłu wyrónonych kerunków. Kerunk te moemy w sposób jednoznaczny okrel na podstawe parametrów opsujcych obekty. W prowadzonych badanach do okrelane kerunków głównych wykorzystywana jest jedyne nformacja o połoenu segmentów. Algorytm okrelana przyblonej wartoc orentacj robota jego warancj jest nastepujcy: Na podstawe wskaza lasera generowany jest zbór obektów. Równana wygenerowanych segmentów okrelaj kt połozena can, dla kadege segmentu okrelana jest te warancja parametru α. Nastpne budowany jest hstogram ktowy, dla kadego kta okrelana jest długo segmentów wskazywanych przez parametr α Hstogram oznaczamy symbolem h l (α). W procese budowana hstogramu wykorzystujemy nformacj o warancj tzn. Jel dany segment głosuje na kerunek α to głosuje te na wszystke kerunk β take, e, (α β) < σ α Dla kadej czstk okrelany jest wycnek mapy globalnej, obserwowany jel robot znajduje s w danej czstce. Na podstawe nformacj o obektach, która jest przechowywana w komórkach mapy rastrowej okrelany jest hstogram ktowy, który oznaczamy symbolem h(α). Warto dα dla którego funkcja okrelona wzorem osaga maksmum jest przyblon wartoc orentacj robota w globalnym układze współrzdnych. N u= h = 0 h l (5) Na rysunku 3a) przedstawono hstogramy katowe dla mapy przedstawonej na rysunku. Czerwonym kolorem zaznaczono hstogram wygenerowany na podstawe nformacj przechowywanej w mape globalnej. Mapa została utworzona w chwl t=0. Kolorem zelonym zaznaczono hstogram, który został utworzony na podstawe wskaza lasera, po przejechanu przez robota 0cm, robot zmenł orentacj ok o. Na rysunku 3b) przedstawono wartoc funkcj splotu, okrelonej równanem (5). Funkcja przyjmuje maksmum dla dα = o. Aby 7
móc korygowa oblczon warto orentacj robota naley zna warancj parametru dα. W przypadku, gdy robot porusza s w otoczenu typu wntrze funkcja splotu u(dα), przyjmuje wartoc nezerowe w penym przedzale [a,b]. W opsywanym przypadku [1.3,.6]. Jel okrela waro dla której funkcja splotu ossga maksmum to jako nepewno okrelana orentacj przyjmujemy warto ε okrelon wzorem: = max a, b (6) 6. EKSPERYMENTY Rysunek 3: Hstogramy ktowe funkcja splotu Eksperymenty przeprowadzono w otoczenu o wyrane zaznaczonych kerunkach głównych, które zostało przedstawone na rysunku 1. Rysunek 4a) przedstawa map otoczena, która została utworzona w sytuacj, gdy połoene pojazdu było okrelane jedyne na podstawe wskaza czujnków odometrycznych. Rysunek 4b) przedstawa map w sytuacj, gdy zastosowano fltry czstkowe metod badana hstogramów. Przeprowadzano take badana metody w przypadku, gdy ne wystepuj wyrane kerunk główne. Rysunek 5 przedstawa zdjce otoczena wynk pomarów. Nawet w takm otoczenu zastosowane badana hstogramów umozlwa zawene przestrzen poszukwa 7. WNIOSKI W artykule przedstawono zastosowane metody fltrów czstkowych do lokalzacj robota moblnego. Aby przyspeszy dzałane algorytmu oprócz nformacj pochodzcej z sensorów odometrycznych wykorzystywana jest te nformacja o kerunkach głównych wystepujcych 8
w otoczenu robota. Orentacja czstek opsujacych połoene robota jest okrelana na podstawe analzy hstogramów ktowych. Umolwa to znaczne zmnejszene loc czstek, które musz by przechowywane. Metoda badana hstogramów została zrealzowana w ramach grantu MeN 3 T11C 038 9. LITERATURA 1 H.P. Moravec, Sensor fuson n certanty grds for moble robots, AI Magazne, str. 61 74, 1988 P. L Bogler, Shafer-Dempster Reasonng wth Applcatons to Multsensor Target Identfcaton Systems, IEEE Transactons on Systems, Man, and Cybernetcs, vol. 17, str. 968-977, 1987 3 W. Burgard and B. Armn and S. Thrun, Poston Estmaton for Moble Robot n Dynamc Envronment, AAAI 98, 1998 4 G. Dudek. and C. Zhang, Vson-based robot localzaton wthout explct object models, Proc. Int. Conf. Robotcs and Automaton, 1996 5 E. Menegatt and M. Zoccarato and E. Pagello and H. Ishguro, Moble Robots wth Monte-Carlo Localsaton, Proc. of 1st European Conference on Moble Robots ECMR'03, 003 6 B. Sematkowska, Zastosowane transformacj Hougha do tworzena map lokalzacj robota moblnego, Postpy Robotyk, IX Krajowa Konferencja Robotyk, str. 371-380, 006. 7 M.S. Grewal and A.P. Andrews, Kalman Flterng: Theory and Practce Usng MATLAB, John Wley and Sons, 001 8 author = "I.M. Reklets", A partcle flter tutoral for moble robot localzaton, Raport, Unverty Montreal, 004 9 S. Thrun and D. Fox and W. Burgard and F. Dellaert, Robust Monte Carlo localzaton for moble robots, Artfcal Intellgence, str. 99 141, 001 10 9