dr Barbara Siemiątkowska
IPPT PAN, Wydziaá Mechatroniki PW dr Jacek Szklarski
Instytut Podstawowych Problemów Techniki mgr inĪ. Jan SyrczyĔski
Wydziaá Mechatroniki PW mgr inĪ. Piotr WĊclewski Wydziaá Mechatroniki PW dr Michaá Gnatowski
Instytut Podstawowych Problemów Techniki
WYKORZYSTANIE FILTRÓW CZĄSTECZKOWYCH W PROCESIE
LOKALIZACJI ROBOTA MOBILNEGO
W pracy omówiono metody okreĞlenia zmian poáoĪenia robota mobilnego w Ğro-dowisku 3D. Przedstawiono dwie implementacje filtru cząsteczkowego, których celem jest przyĞpieszenie obliczeĔ. Omówiono metodĊ, w której zmiana poáoĪenia i orientacji odbywa siĊ oddzielnie oraz metodĊ wykorzystującą procesory graficz-ne w algorytmie lokalizacji
THE APPLICTION OF PARTICLE FILTERS IN MOBILE
ROBOT LOCALIZATION
In the article the overview of localization methods is presented. Two modifications of particle filter algorithm are described. In the first approach the position and orientation of the mobile robot are determined separately. In second method parallel processing units are used. Both methods allow us to speedup the process of localization.
1.WSTĉP
RobotykĊ moĪna postrzegaü jako naukĊ traktującą o percepcji i modyfikacji Ğwiata rzeczywi-stego przez urz ądzenia sterowane kom puterem. Z zagadnieniem tym związana jest niepew-noĞü informacji o aktualnym stanie Ğwiata. Niepewno Ğü ta jest spowodowana nieprzewidy-walną zmiennoĞcią Ğrodowiska oraz niedok áadnoĞcią sensorów robota. Obraz z kam ery, dal-mierza laserowego, czy przetwornika zliczającego kąt obroty kóá – są to wszystko informacje obarczone jakimĞ báĊdem. W robotyce m obilnej jednym z kluczowych pyta Ĕ na które robot musi sobie ,,odpowiedzieü'' brzmi: ,,gdzie jestem?'' [1, 2, 3, 4, 5].
Metody lokalizacji dzielim y w zale ĪnoĞci od konkretnego zastosowania. Mo Īemy wyróĪniü:
x LokalizacjĊ lokaln ą – robot zna swoj ą pozycj Ċ pocz ątkową z dobr ą dok áadnoĞcią i przemieszcza siĊ lokalnie, tzn. o stosunkowo m aáy odcinek drogi, a inform acje o przemieszczeniu pobiera np. z odometrii.
x LokalizacjĊ globalną – początkowa pozycja jest nieznana i robot na podstawie pom ia-rów sam musi okreĞliü swoje po áoĪenie. Gdy ju Ī to zrobi, np. poprzez zaobserwowa-nie jakiego Ğ znacznika, m oĪe t ą informacjĊ wykorzystywa ü przy kolejnych ocenach pozycji.
x Problem porwanego robota – podobnie jak lokalizacja globalna, ale tu robot m usi li-czyü si Ċ z tym , Īe w dowolnej chwili zostanie przeniesiony w inne m iejsce. Jest to znacznie trudniejszy problem od zwykáej lokalizacji globalnej.
Metody lokalizacji zaleĪą takĪe od rodzaju Ğrodowiska. MoĪemy wyróĪniü:
x ĝrodowisko typu wn Ċtrze pomieszczenia, w którym robot przem ieszcza siĊ po p áasz-czyĨnie, a ukáad Ğcian okreĞla gáówne kierunki rozmieszczenia przeszkód.
x ĝrodowisko zewnĊtrzne, zwykle o duĪych nierównoĞciach terenu i jednoczeĞnie z bra-kiem wyróĪnionych kierunków gáównych.
x ĝrodowisko statyczne – w którym jedynym poruszaj ącym si Ċ obiektem jest robot mobilny, a reszta otoczenia nie zmienia siĊ.
x ĝrodowisko dynam iczne – otoczenie robota zm ienia si Ċ (na dodatek niezale Īnie od akcji robota). Oczywi Ğcie wszystkie typowe Ğrodowiska, gdzie obecni s ą ludzie (jak np. wnĊtrze budynku czy autostrada) są dynamiczne.
Z punktu widzenia lokalizacji, pom iary dokonywane przez robota m obilnego m oĪemy po-dzieliü na dwie kategorie: wzglĊdne i bezwzglĊdne. Do pierwszych zaliczyü moĪemy enkode-ry, a do drugiej grupy GPS.
a) b)
Rys. 1. Mapa rastrowa otoczenia robota, do której wykorzystano pomiary z dalmierzy laserowych oraz pozycje, wyznaczona bez korekcji za pomocą obserwacji
OkreĞlanie pozycji m etodą pozycji zliczonej to najstarszy sposób lokalizacji. Polega on na wyznaczaniu bie Īącej pozycji na podstawie ostatnio obserwowanej pozycji oraz prze-suniĊcia wyznaczonego za pom ocą znanej prĊdkoĞci, kierunku ruchu i m inionego czasu. Lo-kalizacja zliczeniowa (na podstawie odom etrii) jest moĪliwa jedynie w sytuacji, w której ro-bot przemieszcza siĊ na stosunkowo krótkim odcinku.
Na rys. 1a przedstawiono przyk áadową trasĊ przebytą przez robota. Punkt A wskazuje poáoĪenie pocz ątkowe, punkt B po áoĪenie ko Ĕcowe. Kolorem Īóátym zaznaczono elips Ċ niepewnoĞci po áoĪenia robota. Niepewno Ğü wzrasta wraz z d áugoĞcią trasy przebytej przez robota. Na rys. 1b przedstawiono m apĊ rastrową otoczenia utworzon ą w sytuacji, gdy infor-macja o otoczeniu jest okre Ğlana jedynie na podstawie odom etrii. WyraĨnie widaü, Īe mapa jest nieprzydatna.
W 1960 powsta áa m etoda filtru Kalm ana [4], która przez dziesi Ċciolecia dominowaáa w dziedzinie filtrów adaptacyjnych. W robotyce mobilnej najczĊĞciej jest stosowany do okre-Ğlenia poáoĪenia robota (wektora stanu) na podstawie obserwacji. W algorytmie tym zakáada siĊ, Īe rozkáad szumu pomiarowego ma charakter gaussowski. W filtrze Kalm ana estymacja stanu procesu jest liniow ą kombinacją predykcji stanu i wa Īonej róĪnicy miĊdzy wektorem obserwacji, a predykcj ą obserwacji. Metoda posiada nast Ċpujące cechy: za áoĪono m odel liniowy, záoĪonoĞü obliczeniowa m etody wzrasta proporcjonalnie do kwadratu liczby obser-wacji, konieczne jest jednoznaczne okreĞlenie początkowego poáoĪenia robota, przyjĊto gaus-sowski rozkáad szumu, co nie zawsze dobrze opisuje rzeczywistoĞü.
W czasie II wojny Ğwiatowej J. Neum ann opracowa á me todĊ Monte-Carlo, która jest zwana takĪe metodą quasi-bayesowską. Metody Monte-Carlo zostaáy niezaleĪnie opracowane w wielu dziedzinach nauki np. fizyce, statystyce, technice. Od lat 90-tych XX wieku zacz Ċáy zastĊpowaü filtr Kalmana w lokalizacji robotów mobilnych [6].
2. FILTR CZĄSTECZKOWY
W metodzie tej poáoĪenie robota w chwili t jest opisywane przez zbiór cząsteczek St:
St = { t i x , wit} i = 1, ..., N, (1) t i
x – okreĞla poáoĪenie robota , wit – jest wagą, która zostaáa przypisana i-tej cząsteczce.
Ka Īda cząstka reprezentuje pewne m oĪliwe poáoĪenie robota w chwili t i wag Ċ wit
okreĞlającą przekonanie, Īe robot m oĪe si Ċ w danym punkcie znajdowa ü. Rozk áad cz ąstek odzwierciedla rozkáad funkcji gĊstoĞü prawdopodobieĔstwa, Īe robot znajduje siĊ w pewnym obszarze przestrzeni. Rys. 2 przedstawia ideĊ opisywanej metody.
W kolejnych krokach na podstawie wskazaĔ sensorów odometrycznych ut, dla kaĪdej
cząstki okreĞlane są przybliĪone wartoĞci xit1 w chwili t+1. NastĊpnie dla ka Īdej wartoĞci 1
t i
x obliczany jest wspóáczynnik dopasowania wit1. Wspóáczynnik ten okreĞla, na ile
prze-widywane poáoĪenie cech otoczenia ró Īni siĊ od po áoĪenia obserwowanego w danym punk-cie, w kolejnych krokach:
x wagi są normalizowane, x okreĞlany jest nowy rozkáad cząsteczek.
Rys. 2. Rozkáad cząsteczek
Do podjĊcia decyzji związanych z realizacją trasy robota konieczna jest jednoznaczna informacja o po áoĪeniu pojazdu, których m echanizm filtrów cz ąsteczkowych nie daje. W takim wypadku stosuje si Ċ jedną z metod oszacowania aktualnej pozycji: Ğrednia waĪona, najlepsza cząsteczka, Ğrednia waĪona w bliskim otoczeniu najlepszej cząsteczki.
3. MODYFIKACJE FILTRU CZĄSTECZKOWEGO
Liczba wygenerowanych cząsteczek jest silnie zale Īna od niedokáadnoĞci wskazaĔ odometrii oraz liczby param etrów, które m usimy estymowaü. PoáoĪenie robota przem ieszczającego siĊ w przestrzeni 3D okreĞlane jest przez 6 parametrów: poáoĪenie [x, y, z]T, oraz kąty nachylenia do osi OX, OY, OZ: [ Į, ȕ, Ȗ]T . Przy du Īej niedokáadnoĞci wskazaĔ sensorów odom etrycz-nych potrzebujemy miliony cząsteczek, aby uzyskaü zadowalającą dokáadnoĞü. Przy tak duĪej liczbie cząsteczek niemoĪliwe jest okreĞlanie przemieszczenia robota w czasie rzeczywistym.
W prowadzonych badaniach testowano dwie m etody umoĪliwiające przyspieszenie czasu obliczeĔ:
x oddzielne korygowanie zmian orientacji i przesuniĊcia, x wykonywanie obliczeĔ w sposób równolegáy.
Eksperymenty przeprowadzono przy pom ocy robota Elektron, który by á wyposa Īony w dalm ierz laserowy SICK [7]. Dane odebrane z dalm ierza m ają posta ü ci ągu o d áugoĞci równej liczbie odebranych punktów. SICK LMS 200 m a rozdzielczoĞü skanowania 1o, 0,5o lub 0,25o i dane odczytuje w zakresie 0–180 o. Zamontowanie takiego lasera na g áowicy
ob-rotowej umoĪliwia otrzymanie informacji trójwymiarowej o otoczeniu, dane te zapisane s ą w postaci chm ury punktów. Dla om awianego sposobu skanowania charakterystyczne jest za-gĊszczenie punktów przy osi obrotu g áowicy. Na rys. 3 przedstawiono chm urĊ punktów otrzymaną, gdy robot zosta á umieszczony w korytarzu wydzia áu Mechatroniki PW. W Ğrodo-wisku typu wnĊtrze pomieszczenia trzy wzajemnie prostopadáe kierunki gáówne wyznaczone są przez pod áogĊ (sufit), uk áad Ğcian oraz elem entów prostopadáych do Ğcian. Ukáad ten wy-znacza wersory globalnego uk áadu wspóárzĊdnych. Obserwując zmiany kierunków gáównych moĪemy okreĞliü obrót robota wokóá kaĪdej z osi.
Rys. 3. Chmura punktów [8]
3.1 OkreĞlenie zmiany orientacji i przesuniĊcia
Lokalizacja robota skáada siĊ z dwóch procesów: na pocz ątku wyznaczana jest zmiana orien-tacji robota, a w kolejnym kroku przesuniĊcie robota wzdáuĪ kaĪdej z osi. Ka Īdy z tych pro-cesów realizowany jest przy pomocy filtru cząsteczkowego.
Chmura punktów nie jest wygodna w procesie lokalizacji i zwykle jest ona zapisywana w innej formie. Jedną z nich jest okreĞlenie zbioru wektorów normalnych [9, 10].
a) b) c)
Rys. 4. Zbiór wektorów normalnych: a) chmura punktów, b) wektory normalne, c) histogram kierun-ków normalnych [8]
Na rys. 4b przedstawiono zbiór wektorów normalnych utworzony na podstawie chmu-ry punktów przedstawionej na chmu-rys 4a. Rys. 4c prezentuje histogram kierunków gáównych. Im bardziej popularny jest dany kierunek, tym wyĪszy jest odpowiadający mu wierzchoáek. Sto-sując filtr cz ąsteczkowy, moĪemy okreĞliü zmianĊ orientacji robota. W prowadzonych bada-niach zmiany orientacji robota okreĞla cząsteczka o najwiĊkszej wadze.
Znając orientacj Ċ robota w globalnym uk áadzie wspó árzĊdnych, m oĪemy wyznaczy ü przesuniĊcie robota wzd áuĪ ka Īdej osi. Przyj Ċta m etoda um oĪliwia znaczne zredukowanie
liczby cząsteczek. W nastĊpnym rozdziale opisane zostan ą wyniki przeprowadzonych ekspe-rymentów.
Metoda ta nie m oĪe byü stosowana w Ğrodowisku nieuporządkowanym np. na zewn ątrz budynków. Dlatego rozpocz Ċto badania dotycz ące wykorzystania procesorów graficznych (technologii CUDA), które umoĪliwiają wykonywanie obliczeĔ w sposób równolegáy.
3.2 Obliczenia równolegáe
Procesory graficzne um oĪliwiają przetwarzanie inform acji w sposób równoleg áy [11, 12]. RównolegáoĞü obliczeĔ na platformie CUDA otrzymywana jest poprzez wywoáanie tzw. jądra jako zbioru w ątków. Wątki pogrupowane s ą w bloki, te natom iast w tablice. Model wyko-nawczy technologii CUDA prezentuje rys. 5.
Rys. 5. Model wykonawczy CUDA (zródáo: NVidia.com)
Zarówno bloki, jak i tablice m ogą byü jedno-, dwu- lub trójwymiarowe. KaĪdy wątek, blok i tablica posiada unikalny identyfikator, który m oĪna wykorzystaü do wyznaczania po-zycji przetwarzanego elem entu w pam iĊci. Architektura CUDA zak áada caákowitą niezaleĪ-noĞü przetwarzania w obr Ċbie bloku, co utrudnia zastosowanie platform y do oblicze Ĕ o cha-rakterze ogólnym. W filtrze cz ąsteczkowym, kaĪda z cz ąsteczek jest niezale Īna od pozosta-áych wiĊc obliczenia wspóáczynników dopasowania mogą odbywaü siĊ wiĊc niezaleĪnie. Wy-niki wstĊpnych eksperymentów zostaną opisane w rozdz. 5.
4. Eksperymenty
Opisane w tym rozdziale wyniki eksperym entów zosta áy zaczerpni Ċte z [8]. Testowano wpáyw szeregu parametrów na dokáadnoĞü lokalizacji.
Rys. 6. Wpáyw zmiany orientacji robota na dokáadnoĞü otrzymanych wyników
Na rys. 6 przedstawiono wp áyw rzeczywistego obrotu robota (wzgl Ċdem osi OZ) na dokáadnoĞü otrzymanych wyników. Obliczenia prowadzono dla 20 000 cz ąsteczek. Przy ob-rocie powyĪej 60o báąd gwaátownie roĞnie. Cecha ta wynika ze sposobu skanowania (rys. 3).
Kolejnym istotnym parametrem jest wp áyw pola obserwowanych powierzchni g áów-nych na dokáadnoĞü okreĞlenia przemieszczenia robota. Analogicznie jak poprzednio oblicze-nia przeprowadzono dla 20 000 cząsteczek.
Tab. 1. Wpáyw powierzchni páaszczyzn dominujących na dokáadnoĞü lokalizacji
OkreĞlenie przem ieszczenia robota by áo wykonywane co 3 m. Zgodnie z intuicj ą, w sytuacji, gdy obserwowany jest niewielki fragment powierzchni báąd okreĞlania przemiesz-czenia jest duĪy (powyĪej 10 %). Wyniki przedstawiono w tab. 1.
Tab. 2 zawiera wyniki badania wp áywu przesuniĊcia robota na dokáadnoĞü lokalizacji. Báąd roĞnie, jeĞli robot przejechaá powyĪej 4 m. Wynika to z tego, Īe z pola widzenia robota znikają páaszczyzny, które moĪna dopasowaü.
Kolejnym parametrem jest okreĞlenie wpáywu liczby cząsteczek na dokáadnoĞü lokali-zacji. Stwierdzono, Īe powy Īej 100 000 cz ąsteczek i za áoĪonym 10 % b áĊdzie odom etrii dokáadnoĞü lokalizacji nie wzrasta.
Tab. 2. Wpáyw przesuniĊcia robota na dokáadnoĞü lokalizacji
Na rys. 7 przedstawiono m apĊ korytarza utworzon ą na podstawie danych pochodz ą-cych ze skanera laserowego. Kropkam i zaznaczono m iejsca zbierania pom iarów. Obliczenia przeprowadzono dla 20 000 cz ąsteczek okre Ğlających zm ianĊ orientacji robota i 20 000 cząsteczek okreĞlających przesuniĊcie.
Oprócz oblicze Ĕ sekwencyjnych sprawdzono m oĪliwoĞü stosowania technologii CUDA w procesie lokalizacji. Na rys. 8 przedstawiono zaleĪnoĞü miĊdzy czasem obliczeĔ na komputerze z procesorem Intel Centrino (2 x 2,0 GHz) (algorytm sekwencyjny) i kart ą gra-ficzną NVidia GeForce 9300MG (im plementacja równolegáa). Na rys. 8 przedstawiono sto-sunek czasu oblicze Ĕ CPU do GPU dla ró Īnej liczby cz ąsteczek. Stosowanie GPU przyspie-sza proces lokalizacji w sytuacji, gdy liczba cząsteczek przekracza 10 000.
Rys. 8. Stosunek czasu obliczeĔ CPU do GPU dla róĪnej liczby cząsteczek
5. WNIOSKI
W powyĪszym artykule przestawiono dwie im plementacje filtru cz ąsteczkowego. W pierw-szej metodzie proces obliczeĔ odbywa siĊ sekwencyjnie. Orientacja i przem ieszczenie robota okreĞlane są oddzielnie, dziĊki czemu moĪliwe jest zmniejszenie liczby cząsteczek. W drugiej metodzie stosowana jest technologia CUDA. W stĊpne wyniki eksperym entów wykazaáy, Īe stosowanie GPU ma sens wtedy, gdy liczba cz ąsteczek przekracza 10 000. Nasze dalsze pra-ce bĊdą dotyczyáy zastosowania GPU w nawigacji robotów mobilnych.
6. BIBLIOGRAFIA
[1] J. Borenstein, H. Everett, L. Feng, Where am I? Sensors and Methods for Mobile Robot Positioning. Univeristy of Michigan, 1996.
[1] R. E. Kalman, A new approach to linear filtering and prediction problem s. Transactions of the ASME–Journal of Basic Engineering 82 (Series D), 1960, str. 35–45.
[2] P. Skrzypczynski, Metody Analizy i Redukcji Niepewno Ğci Percepcji w System ie
Nawigacji Robota Mobilnego, Vol. 407 Rozprawy. W ydawnictwo Politechniki PoznaĔ-skiej, 2007.
[3] R.C. Smith, P. Cheesem an (1987), On the representation and estim ation of spatial un-certainly. Int. J. Rob. Res. 5(4), 56–68, 1987.
[4] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. The MIT Press, 2005.
[5] Zhang, Z., Iterative point matching for registration of free-form curves. Rapports de Re-cherche 1658, Institut National de ReRe-cherche en Informatique et en Automatique, 1992. [6] I. Rekleitis, A Particle Filter Tutorial for Mobile Robot Localization, Technical Report
TR-CIM-04-02, Center for Intelligent Machines, McGill University.
[7] Chojecki. R, Olszewski M., A Mobile Robot for Laboratory Purposes and Its Applica-tions. PAK, 3 (2009), 55, 190–193.
[8] J. Syrczy Ĕski, Lokalizacja robota m obilnego w nieznanym otoczeniu m etodą filtrów cząsteczkowych, Praca magisterska PW, 2009.
[9] B. Siem iątkowska, J. Szklarski, M. Gnatowski, A. Zychewicz, Budowa hybrydowej semantyczno-rastrowej reprezentacji otoczenia robota mobilnego na podstawie wskazaĔ dalmierza laserowego 3D, PAK, 3:278–282, 2010.
[10] A. Borkowski, B. Siemiątkowska, J. Szklarski, Towards Semantic Navigation In Mobile Robotics. In: G. Engles, C. Lewerenz, W . Schafer, A. Schurr, B. W estfechtel (Eds.): Graph Transformations and Model Driven Engineering – Essays Dedicated to Manfred Nafle, LNCS 5765, Springer.
[11] S. H. Seyed, Parallel processing and parallel algorithm s: theory and com putation. Springer, 2000.
[12] Harris, Mark, Optim izing Parallel Reduction in CUDA, NVIDIA Developer Technology, http://developer.download.nvidia.com.
