AUTOMATYKA 2010 Tom 14 Zeszyt 3/1 Dominik Sankowski*, S³awomir Je ewski*, Adam Wulkiewicz* Uogólniony model dwuwymiarowego czujnika pomiaru odleg³oœci dla systemu operacyjnego robota mobilnego 1. System operacyjny robota mobilnego System operacyjny jest to oprogramowanie zarz¹dzaj¹ce sprzêtem komputerowym, tworz¹ce œrodowisko do uruchamiania i kontroli zadañ u ytkownika. Stanowi on swego rodzaju warstwê abstrakcji. System operacyjny proponowany przez nas posiada strukturê warstwow¹. W kontekœcie modelu czujnika wa ne s¹ dwie warstwy systemu operacyjnego (rys. 1), druga i trzecia. Rys. 1. Ogólny schemat drugiej i trzeciej warstwy systemu operacyjnego robota mobilnego * Katedra Informatyki Stosowanej, Politechnika ódzka 515
516 Dominik Sankowski, S³awomir Je ewski, Adam Wulkiewicz W warstwie drugiej znajduj¹ siê mechanizmy obs³ugi ró nego rodzaju czujników odleg³oœci, po³o enia i orientacji. Dane z czujników integrowane s¹ w warstwie trzeciej w celu budowy mapy otoczenia, która mo e zostaæ wykorzystana przez wy sze warstwy systemu operacyjnego na przyk³ad przez algorytm wyznaczania œcie ki ruchu robota. 2. Uogólniony model dwuwymiarowego czujnika pomiaru odleg³oœci dla systemu operacyjnego robota mobilnego Dane pomiarowe zbierane s¹ z czujników ró nego rodzaju. Czujniki te maj¹ czêsto ró ne charakterystyki oraz w³aœciwoœci. Nastêpnie dane te musz¹ zostaæ zintegrowane w mapie otoczenia. Jest ona struktur¹ danych nazywan¹ siatk¹ zajêtoœci. Posiada ona strukturê siatki i przechowuje prawdopodobieñstwa wystêpowania obiektów w przestrzeni. Poniewa celem jest wykorzystanie czujników o ró nej charakterystyce nale a³o opracowaæ matematyczny model czujnika uniwersalnego oraz algorytm integracji danych w przestrzeni percepcyjnej. 2.1. Dwuwymiarowe czujniki odleg³oœci stosowane w robotyce Do najczêœciej stosowanych w robotyce dwuwymiarowych czujników odleg³oœci nale- ¹ czujniki ultradÿwiêkowe, œwietlne, w tym podczerwone i skanery laserowe. Zasada dzia³ania czujników ultradÿwiêkowych (rys. 2) jest podobna do zasady dzia³ania radaru. Generuj¹ one falê ultradÿwiêkow¹ i badaj¹ czas powrotu echa odbitego od przeszkody. Wykorzystuje siê je równie w procesach przemys³owych do wykrywania obiektów na liniach produkcyjnych oraz w medycynie. Pomiary wykonane przez ten czujnik mog¹ ró niæ siê w zale noœci od rodzaju powierzchni obiektu, od którego odbi³a siê fala ultradÿwiêkowa. Rys. 2. Zbli eniowy czujnik ultradÿwiêkowy firmy SICK
Uogólniony model dwuwymiarowego czujnika pomiaru odleg³oœci... 517 Istnieje wiele rodzajów czujników œwietlnych maj¹cych ró ne zastosowanie. Najczêœciej stosowanymi w robotyce s¹ zbli eniowe czujniki podczerwone (rys. 3). Emituj¹ one œwiat³o podczerwone i badaj¹ jaka czêœæ tego œwiat³a do nich powróci³a. Rys. 3. Zbli eniowy czujnik podczerwony firmy Sharp Dalmierze laserowe s¹ to urz¹dzenia wykorzystuj¹ce wi¹zkê lasera w celu wyznaczenia odleg³oœci od obiektu. Odleg³oœæ mo e byæ obliczana na zasadzie zbadania czasu powrotu wygenerowanego impulsu lub wyznaczenia przesuniêcia fazowego wi¹zki powracaj¹cej do czujnika. Dwuwymiarowe skanery laserowe (rys. 4) rozszerzaj¹ funkcjonalnoœæ dalmierzy. Wykonuj¹ one pomiary w pewnym zakresie k¹tów typowo 90 o, 180 o, 270 o czy 360 o. Pomiary wykonywane s¹ co pewn¹ sta³¹ wartoœæ k¹ta. Jest to mo liwe poprzez obracanie lustra odbijaj¹cego wi¹zkê lasera w odpowiednim kierunku i wykonywanie pomiarów dla odpowiedniego ustawienia osi optycznej. 2.2. Model matematyczny Rys. 4. Dwuwymiarowy skaner laserowy firmy SICK Model matematyczny czujnika opisuje prawdopodobieñstwo wystêpowania obiektu w otoczeniu punktu pomiarowego. Na wartoœæ prawdopodobieñstwa ma wp³yw rodzaj
518 Dominik Sankowski, S³awomir Je ewski, Adam Wulkiewicz czujnika oraz b³êdy pomiaru, jakimi siê on charakteryzuje. Pierwszym rodzajem b³êdów pomiaru s¹ b³êdy, które nazwano poprzecznymi. W zale noœci od rodzaju czujnika mog¹ one zale eæ od k¹ta odchylenia od punktu pomiarowego lub od odleg³oœci od tego punktu. B³¹d poprzeczny zale ny od k¹ta jest charakterystyczny dla czujników ultradÿwiêkowych, gdzie prawdopodobieñstwo wyst¹pienia obiektu w przestrzeni maleje wraz z odchyleniem k¹towym od punktu pomiaru. W czujnikach laserowych, b³¹d poprzeczny ma charakter odleg³oœciowy. Drugim rodzajem b³êdów s¹ b³êdy pod³u ne, które mog¹ byæ wzglêdne lub bezwzglêdne, czyli odpowiednio zale ne lub niezale ne od odleg³oœci punktu pomiarowego. Rys. 5. Charakterystyka czujników: a) czujnik charakteryzuj¹cy siê poprzecznym b³êdem d c odleg³oœciowym; b) czujnik charakteryzuj¹cy siê poprzecznym b³êdem d c k¹towym Na rysunku 5 przedstawione s¹ dwa rodzaje czujników charakteryzuj¹ce siê ró nymi b³êdami poprzecznymi. Odleg³oœæ punktu pomiarowego P p od czujnika wynosi d s. Prawdopodobieñstwo wyst¹pienia obiektu obliczane jest dla punktu P(d l, d c ). Odleg³oœæ d l jest pod³u n¹ sk³adow¹ wektora odleg³oœci punktu P(d l, d c ) od punktu pomiarowego P p. Odleg³oœæ d c jest sk³adow¹ odleg³oœciow¹ poprzeczn¹ i w zale noœci od typu czujnika jej jednostk¹ jest jednostka d³ugoœci albo miara k¹ta. 2 2 dl dc 2 2 2δ 2 0, 5 l δ 0, 5 c + e e dla P, Cz < Pp, Cz Pd ( l, dc) = 2 2 dl dc 2 2 2δ 2 0, 5 0, 5e l δ + l dla P, Cz Pp, Cz (1) gdzie: d l, d c d³ugoœci sk³adowych pod³u nej i poprzecznej, δ l, δ c odchylenia standardowe pod³u ne i poprzeczne.
Uogólniony model dwuwymiarowego czujnika pomiaru odleg³oœci... 519 Model matematyczny czujnika przedstawia wzór (1). Po zastosowaniu centralnego twierdzenia granicznego funkcja prawdopodobieñstwa mo e byæ modelowana za pomoc¹ funkcji Gaussa. Zagadnienie zosta³o podzielone na dwie czêœci. Czêœæ przestrzeni, której punkty znajduj¹ siê dalej od czujnika ni punkt pomiarowy, oraz czêœæ przestrzeni, której punkty znajduj¹ siê bli ej. Co wiêcej, ³atwo mo na zauwa yæ, e wzory mo na rozbiæ na dwie czêœci sk³adowe, pod³u n¹ oraz poprzeczn¹. Obrazuje to rysunek 6 i rysunek 7. a) b) c) Rys. 6. Sk³adowe pod³u na i poprzeczna. Przesuniêcie w osi pod³u nej oznaczane jest jako x l, natomiast w osi poprzecznej jako x c, przy czym przesuniêcie liczone jest od pozycji próbki: a) sk³adowa pod³u na; b) sk³adowa poprzeczna dla x l = 0 (pozycja próbki); c) sk³adowa poprzeczna dla x l = Cz (pozycja czujnika) Rys. 7. Dwuwymiarowy wykres prawdopodobieñstwa punktu P(d l, d c ): a) czujnik charakteryzuj¹cy siê b³êdem odleg³oœciowym; b) czujnik charakteryzuj¹cy siê b³êdem k¹towym
520 Dominik Sankowski, S³awomir Je ewski, Adam Wulkiewicz 3. Algorytm integracji w siatce zajêtoœci Siatka zajêtoœci charakteryzuje siê tym, e przechowuje nie tylko dane opisuj¹ce wystêpowanie obiektów w przestrzeni, ale te dane opisuj¹ce przestrzeñ pust¹ oraz niezbadan¹. W czasie integracji modyfikowany jest obszar siatki, który pokrywa siê z pomiarem wykonanym przez czujnik. Prawdopodobieñstwa wszystkich punktów siatki w tym obszarze musz¹ zostaæ zmodyfikowane wed³ug wzoru (2). Wiêcej informacji mo na znaleÿæ w [1]. 1 1 Pnew( p)1 Pold ( p) P( p) = 1+ Pnew( p) Pold ( p) (2) gdzie: P(p) zmodyfikowane prawdopodobieñstwo punktu p, P new (p) prawdopodobieñstwo wyznaczone na podstawie aktualnego pomiaru, P old (p) znane prawdopodobieñstwo wystêpowania obiektu w punkcie p wyznaczone poprzez zintegrowanie wczeœniejszych pomiarów. Poni ej znajduje siê opis dwóch algorytmów integracji danych ze skanera laserowego. Pierwszy z nich, algorytm zorientowany na punkty, w którym prawdopodobieñstwa punktów w siatce obliczane s¹ w sposób nieuproszczony. Nie s¹ w nim stosowane adne uproszczenia modelu matematycznego. Algorytm ten stanowi punkt odniesienia. Mo na go zastosowaæ w celu integracji danych pochodz¹cych z dowolnego wy ej wymienionego czujnika. Równie z czujników wykonuj¹cych wiele pomiarów jednoczeœnie. Drugi algorytm, zorientowany na próbki, jest algorytmem uproszczonym. Uproszczony jest model matematyczny, a pozycja czujnika i próbek obliczana jest z dok³adnoœci¹ do odleg³oœci punktów siatki zajêtoœci. W tej formie ma on zastosowanie jedynie w przypadku czujników charakteryzuj¹cych siê poprzecznym b³êdem odleg³oœciowym, którego wartoœæ nie przekracza po³owy odleg³oœci miêdzy punktami siatki. Przyk³adem takiego czujnika jest skaner laserowy. 3.1. Algorytm zorientowany na punkty W trakcie dzia³ania algorytmu zorientowanego na punkty badane s¹ wszystkie punkty w obszarze pomiaru. Dla danego typu i wartoœci b³êdu poprzecznego, dla ka dego punktu obliczany jest zakres k¹tów próbek pomiarowych, których obszary dzia³ania zawieraj¹ w sobie punkt. Nastêpnie sprawdzana jest ka da próbka z tego zakresu. W zale noœci od rodzajów b³êdów badane jest wzajemne po³o enie pozycji próbki oraz punktu, odleg³oœciowe lub k¹towe (rys. 5) i na tej podstawie obliczane prawdopodobieñstwa ze wzoru (1).
Uogólniony model dwuwymiarowego czujnika pomiaru odleg³oœci... 521 Wybierana jest próbka, dla której prawdopodobieñstwo wystêpowania obiektu w punkcie siatki jest najwiêksze i wiêksze od 0,5. Jeœli takiej próbki nie ma, wybierana jest ta, dla której prawdopodobieñstwo jest najmniejsze. Nastêpnie dane mog¹ byæ zintegrowane w siatce zajêtoœci wed³ug wzoru (2). 3.2. Uproszczony algorytm zorientowany na próbki W trakcie dzia³ania algorytmu zorientowanego na próbki badane s¹ wszystkie próbki. Dla ka dej z nich wyznaczane s¹ punkty siatki, które s¹ najbli sze pozycji czujnika oraz pozycji próbki. Nastêpnie algorytm przechodzi po wszystkich punktach poœrednich. W tym celu wykorzystywany jest algorytm Bresenhama [4, 5]. Dla punktów najbli szych pozycji próbki, prawdopodobieñstwo ustawiane jest na maksymalne, dla pozosta³ych punktów prawdopodobieñstwo ustawiane jest na minimalne. 3.3. Wyniki Oba algorytmy by³y testowane na komputerze klasy PC z procesorem 2 GHz. Pomiary zosta³y wykonane w pomieszczeniach Katedry Informatyki Stosowanej (KIS) Politechniki ódzkiej. Do tego celu wykorzystano dwuwymiarowy skaner laserowy firmy SICK. Siatka zajêtoœci ma wielkoœæ 2000 na 2000 punktów i zawiera informacjê o przestrzeni 40 na 40 metrów. Daje to rozdzielczoœæ 50 punktów na metr lub 2 cm na punkt. Generalnie oba algorytmy da³y zbli one wyniki w postaci siatki zajêtoœci przechowuj¹cej dane o otaczaj¹cych obiektach i niezajêtej przestrzeni. Rysunek 8 przedstawia mapê pomieszczeñ KIS, w których zbierane by³y dane pomiarowe. Kolorem bia³ym oznaczona jest pusta przestrzeñ, kolorem czarnym wykryte obiekty, na przyk³ad œciany. Kolorem szarym oznaczona jest przestrzeñ nieodkryta. Oznaczona jest równie œcie ka, po której porusza³ siê robot. Po lewej stronie obu czêœci rysunku 8 widoczne s¹ okna. Wydaje siê i promienie lasera przez nie przechodz¹. W rzeczywistoœci s¹ to promienie odbite. Dostrzec mo na odbicie lustrzane czêœci przeciwleg³ych œcian. Na zbli eniu mapy (rys. 9 i 10) widoczne s¹ ró nice w dok³adnoœci mapy. Algorytm zorientowany na próbki generuje wiêkszy szum, a dla du ych odleg³oœci od czujnika widaæ poszczególne promienie. Jest to spowodowane przyjêciem uproszczeñ. Czasy integracji danych wykonanej za pomoc¹ obu testowanych algorytmów przedstawia rysunek 11. Najmniejsze modyfikowane obszary zawiera³y w przybli eniu 100 000 punktów. Odpowiada to œredniej d³ugoœci promienia wynosz¹cej 1,39 m. W przypadku algorytmu zorientowanego na punkty, modyfikacja takich obszarów trwa³a œrednio 87 ms, w przypadku algorytmu uproszczonego 8 ms. Najwiêksze modyfikowane obszary zawiera³y
522 Dominik Sankowski, S³awomir Je ewski, Adam Wulkiewicz w przybli eniu 562 856 punktów. Odpowiada to œredniej d³ugoœci promienia 3,02 m. W przypadku algorytmu zorientowanego na punkty, modyfikacja takich obszarów trwa³a œrednio 322 ms, w przypadku algorytmu uproszczonego 22 ms. Rys. 8. Wyniki dzia³ania algorytmów integracji: a) wynik dzia³ania algorytmu zorientowanego na punkty; b) wynik dzia³ania algorytmu zorientowanego na próbki Rys. 9. Wyniki dzia³ania algorytmów integracji: a) wynik dzia³ania algorytmu zorientowanego na punkty; b) wynik dzia³ania algorytmu zorientowanego na próbki
Uogólniony model dwuwymiarowego czujnika pomiaru odleg³oœci... 523 Rys. 10. Wyniki dzia³ania algorytmów integracji: a) wynik dzia³ania algorytmu zorientowanego na punkty; b) wynik dzia³ania algorytmu zorientowanego na próbki Rys. 11. Wykresy przedstawiaj¹ce œredni czas integracji próbek w zale noœci od liczby modyfikowanych punktów siatki: a) algorytm zorientowany na punkty; b) uproszczony algorytm zorientowany na promienie 4. Wnioski Przez zastosowanie uogólnionego modelu czujnika odleg³oœci mo liwe jest zbudowanie mapy otoczenia robota. Wyniki dzia³ania obu algorytmów s¹ zbli one, jednak algorytm uproszczony jest dziesiêciokrotnie szybszy, ale mniej elastyczny.
524 Dominik Sankowski, S³awomir Je ewski, Adam Wulkiewicz Nie bez znaczenia jest fakt, e budowana mapa jest dok³adna. W praktyce wystarczaj¹ca by³aby dok³adnoœæ rzêdu 20 cm. Zmniejszenie dok³adnoœci spowodowa³oby wzrost prêdkoœci integracji. Nale y nadmieniæ, i zastosowano algorytm budowania mapy ze znanymi pozami. W przypadku zastosowania dodatkowego algorytmu lokalizacji czas integracji pomiaru siê zwiêkszy. W takim przypadku mog³oby siê okazaæ, i integracja danych przez algorytm nieuproszczony trwa zbyt d³ugo. Literatura [1] Siliciano B., Khatib O., Groen F. (ed.), Robotic Mapping and Exploration. Springer-Verlag, Berlin, 2009. [2] Siliciano B., Khatib O., Groen F. (ed.), 3D Robotic Mapping. The Simultaneous Localization and Mapping Problem with Six Degrees Of Freedom. Springer-Verlag, Berlin, 2009. [3] Siliciano B., Khatib O., Groen F. (ed.), Spatial Representaion and Reasoning for Robot Mapping. A Shape-Based Approach. Springer-Verlag, Berlin, 2008. [4] Abrash M., Michael Abrash s Graphics Programming Black Book Special Edition. The Coriolis Group Inc., Scottsdale, Arizona, 1997, 654 678. [5] Glassner A.S. (ed), Graphics Gems. Academic Press Inc., USA, 1990, 98 106.