ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D

MODELOWANIE INŻYNIERSKIE nr 42, ISSN 1896-771X ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D Łukasz Mitka 1a, Michał Ciszewski 1b, Andrii Kudriashov 1c,Tomasz Buratowski 1d, Mariusz Giergiel 1e 1 Katedra Robotyki i Mechatroniki, Akademia Górniczo-Hutnicza a mitka@agh.edu.pl, b mcisz@agh.edu.pl, c andrii.kudriashov@gmail.com, d tburatow@agh.edu.pl, e giergiel@agh.edu.pl Streszczenie W pracy zaprezentowano system budowy dwuwymiarowej mapy otoczenia dla robota mobilnego kołowego poruszającego się po płaskiej powierzchni. Sterowanie robota i algorytmy mapujące zostały zaimplementowane z wykorzystaniem środowiska ROS. Testy systemu zostały przeprowadzone w środowisku symulacyjnym V-REP z wykorzystaniem wtyczek ROS. Walidacja działania systemu została przeprowadona w warunkach rzeczywistych na przygotowanym torze testowym. Słowa kluczowe: robot mobilny, SLAM, skaner laserowy. ROBOT WITH LASER SCANNER FOR 2D MAPPING Summary This paper presents a system for building of 2D environment map for a wheeled mobile robot, operating on flat surfaces. Robot control and mapping algorithms were implemented with use of ROS framework. System tests were conducted in V-REP simulation environment with use of ROS plugins. Validation of system operation was conducted in real conditions in a prepared test rig. Keywords: mobile robot, SLAM, laser scanner. 1. WSTĘP Zagadnienia związane z nawigacją w nieznanym środowisku są znane w robotyce pod nazwą SLAM (ang. Simultaneous Localization And Mapping - jednoczesna lokalizacja i budowanie mapy). Po raz pierwszy założenia tej techniki zostały sformułowane w roku 1996 [7]. W swoich założeniach SLAM jest ogólnym sposobem rozwiązywania problemu budowy mapy nieznanego środowiska, natomiast nie określa konkretnego algorytmu obliczeniowego, w związku z tym powstały liczne prace w zakresie implementacji algorytmów dostosowanych do określonych warunków [5,9,10]. Technika znajduje zastosowanie w różnych dziedzinach. Najnowsze badania dotyczą zastosowania SLAM w systemach poszukiwawczych i ratunkowych USAR (ang. Urban Search and Rescue) z wykorzystaniem algorytmu budowy dwuwymiarowej mapy wspomagany trójosiowym czujnikiem IMU [10]. Innym zastosowaniem jest system do planowania trajektorii w podziemnych kopalniach bazujący na połączeniu danych z różnych czujników sterowany za pomocą systemu ROS [11]. SLAM znajduje również zastosowanie w robotach inspekcyjnych, przykładem takiej implementacji może być technika obrazowania podczerwieni wykorzystana do detekcji gazu pozwalająca na lokalizowanie wycieków w pomieszczeniu eksplorowanym przez robota [2]. SLAM został wykorzystany do budowy narzędzia tworzącego trójwymiarowe mapy w trybie offline na podstawie danych zarejestrowanych podczas pracy robota [3]. Zostało również zaprezentowane środowisko symulacyjne oparte o system ROS z symulatorem Gazebo przeznaczone do testowania algorytmów SLAM [1]. 2. METODY NAWIGACJI I BUDOWY MAPY SLAM (simultaneous localization and mapping) jest metodą symultanicznej lokalizacji oraz mapowania, która rozwiązuje problem tworzenia mapy wtedy, kiedy 1

ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D podmiot jest ruchomy. Jest często wykorzystywana w robotyce oraz systemach autonomicznych. Migawkowe tworzenie mapy jest możliwe za pomocą dużej ilości systemów czujników oraz pomiarów, takich jak: ultradźwiękowe czujniki odległości, radary, lidary, systemy wizyjne etc. Wyniki jednego (rys. Rys. 3a) lub wyestymowanej serii (rys. Rys. 3b) pomiarów mogą być bardzo dokładne, ale są możliwe tylko wtedy gdy podmiot (robot) znajduje się w stanie statycznym. Estymacja całej trajektorii oraz mapy ( nazywa się "pełnym SLAM" (Full SLAM), co może być pokazane za pomocą modelu graficznego na rys. Rys. 7. 1a 2b Rys. 3, a jeden pomiar, b seria pomiarów Podczas ruchu aktualne pomiary oraz lokalizacja zmieniają się, więc aby możliwe było rozszerzenie mapy wymagana jest fuzja z pomiarami z poprzedniego stanu. Nie jest to jednak łatwe zadanie (rys. Rys. 6a, b). SLAM jest metodyką, która pomaga w rozwiązaniu danego problemu za pomocą dodania informacji o aktualnej lokalizacji oraz mapie. Z wykorzystaniem estymujących filtrów zbliżamy się do otrzymania precyzyjnego obrazu środowiska. 4a 5b Rys. 7. Model graficzny pełnego SLAM (Full SLAM) [13] W praktyce, najczęściej są używane ostatnie estymaty położenia oraz mapy. Dla danego przypadku wybierana jest metoda "Online SLAM" [12,13], której graficzna reprezentacja może być pokazana na rys. Rys. 8. 6 p( x t,m z 1 :t,u 1 : t )= x 0 p(x 0 :t,m x t 1 Rozwiązanie równania przeprowadzane jest zazwyczaj w sposób rekursywnego całkowania, jedno na raz [13]: 7 p( x t +1,m z 1: t+ 1,u 1:t +1 )= x 0 p(x 0: x t 1 Rys. 6, a porzucenie poprzedniego stanu, b scalanie z blędami Do zdefiniowania problemu SLAM wykorzystywane są wartości nadane (, ) oraz oczekiwane (, ): 1 2 3 1) Sterowanie robotem 2) Obserwacje 3) Mapa środowiska u 1 :t ={u 1, u 2,u 3,u t } z 1: t ={z 1, z 2,z 3,z t } m Rys. 8. Model graficzny Online SLAM [13] Bardzo ważnym dla problemu SLAM jest zdefiniowanie modeli ruchu oraz obserwacji. Są one pokazane na rys. Rys. 9. 4 4) Trajektoria ruchu robota x 0 :t ={x 0, x 1,x 2, x t } Prawdopodobna estymacja trajektorii ruchu robota oraz mapy są podane jak [6, 12]: 5 p(x 0 :t,m z 1 : t,u 1: t ) 2

MODELOWANIE INŻYNIERSKIE nr 42, ISSN 1896-771X takich jak: różnica średnic kół, odkształcenia, nie idealna powierzchnia (dywan etc); model odomertyczny działa poprawnie tylko w idealnych warunkach i wymaga dodatkowych estymatorów do kompensacji błędów. Do opisania związku między pomiarami a pozycją robota jest wykorzystywany model obserwacji, często nazywany modelem czujnikowym. Model obserwacji opisuje prawdopodobieństwo stworzenia obserwacji z t wtedy, kiedy lokalizacja pojazdu jest znana i jest Rys. 9. Modele ruchu oraz obserwacji na graficznym modelu SLAM [13] Model ruchu jest wykorzystywany dla opisu względnego ruchu robota. Model ruchu dla pojazdów można opisać za pomocą rozkładu prawdopodobieństwa w przejściach stanu za pomocą wzoru 8 p(x t x t 1, u t ) ogólnie opisany za pomocą wzoru: 11 gdzie: p(z t x t ) z t obserwacje / pomiary; x t położenie robota. Gdzie: x t nowa pozycja robota; Reprezentacja modelów obserwacji zależy od systemu czujników i może bardzo się różnić. x t 1 stara pozycja robota; u t sterowanie robotem. Często używanym przykładem modelu ruchu jest standardowy model odometrii, który jest oparty na sprzężeniu zwrotnym z systemem ruchu robota oraz wykorzystuje kinematykę robota wspólnie z geometrią środowiska. W rozwiązywaniu problemu SLAM niemal zawsze wykorzystuje się filtry estymacji, które wspomagają otrzymanie trajektorii bliskiej do rzeczywistej. Najczęściej one są oparte o filtry cząstkowe lub Kalmana. Bazą metodyki EKF SLAM - SLAM z wykorzystaniem rozszerzonego filtru Kalmana, jest opis ruchu pojazdu za pomocą wzoru [6, 12]: 12 P ( x t,m z t 1,u t ) x t =f ( x t 1,u t )+ gdzie: f model kinematyczny robota w k addytywne, o zerowej wartości oczekiwanej nieskorelowane Gaussowskie zakłócenia Rys. 10. Ruch robota w standardowym modelu odometrii Kiedy robot porusza się od ( x, ý, θ) do ruchu z kowariancją Q k [6]. Wtedy model obserwacji jest opisany jak: ( x ', ý ', θ ' ), informacja odometryczna może być opisana za pomocą wzoru: 9 u=(δ rot 1,δ trans 1, δ rot 2 ) 13 gdzie: P ( z t x t,m) z(t)=h( x t,m )+v t h obserwacja geometryczna v k addytywne, o zerowej wartości 10 δ trans = ( x ' x) 2 +( ý ' ý) 2 δ rot 1 =atan2( ý ' ý, x ' x ) θ δ rot 2 = θ ' θ δ rot 1 oczekiwanej nieskorelowane Gaussowskie błędy R obserwacji z kowariancją k [6]. Jednakże z powodu dużej ilości źródeł błędów ruchu 3

ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D Rys. 11. Schemat działania EKF SLAM dwoma kołami napędzanymi oraz jednym kołem podpierającym, nazywanego robotem 2-kołowym. W artykule oparto się na konstrukcji robota opisanego w [4]. Robot wyposażony jest w ramę aluminiową, do której zamontowano silniki szczotkowe prądu stałego, wraz z przekładniami planetarnymi o przełożeniu 66:1, na których osadzone są kola napędowe. Ogólną strukturę mechanizmu jezdnego robota przedstawiono na rysunku Rys. 14. Do pomiaru kąta obrotu silników zastosowano dwukanałowe enkodery optyczne o rozdzielczości 500 impulsów na obrót. W podstawowej wersji robota, do nawigacji w przestrzeni stosowane są czujniki podczerwienie i ultradźwiękowe oraz pomiary odometryczne otrzymywane przy użyciu modelu kinematyki robota, analogicznego do modeli robotów 2-kołowych przedstawionych w [8], oraz rozszerzonego filtra Kalmana. Dokładny opis pomiarów odometrycznych zaprezentowano w pracy [4]. Rys. 12. Obraz środowiska przygotowany z wykorzystaniem EKF SLAM Jedna z wielu implementacji procesu EKF SLAM jest pokazana na rys. Rys. 11, gdzie Rozszerzony filtr Kalmana jest wykorzystywany razem ze skanowaniem laserowym. Po otrzymaniu laserowych pomiarów środowiska tworzy się początkowa mapa, następnie metoda sprawdza za pomocą odometrii czy lokalizacja została zmieniona. W tym czasie nowe laserowe skany są dodawane do filtru Kalmana oraz porównywane ze sobą. Jeżeli odometria jest pusta - najbardziej intensywne punkty pomiarów wybierane są jako aktualna mapa i cały proces zaczyna się od początku. Gdy przychodzi nowa odometryczna informacja, algorytm jednocześnie dokonuje fuzji obecnej i poprzedniej lokalizacji wraz z pomiarami lasera, oraz aktualizuje globalny obraz środowiska (rys. Rys. 12). 3. MOBILNY ROBOT KOŁOWY Rys. 14 Struktura mechanizmu jezdnego robota kołowego oraz punkty charakterystyczne [4] Zastosowanie sensora laserowego LIDAR (LIght Detection And Ranging) pozwala na zbieranie zdecydowanie większej informacji o otoczeniu i pozwala przy użyciu techniki SLAM na dokładne mapowanie terenu. Do testów zastosowano platformę robota z zamontowanym czujnikiem LIDAR. Prototyp robota, przystosowany do mapowania 2D przedstawiony jest na rysunku Rys. 15. Robot wyposażony został w czujnik Hokuyo UTM-30LX-EW, o zasięgu 0.1 30 m, zakresie kątowym 270 oraz rozdzielczości kątowej 0.25. Nawigacja robotów mobilnych w pomieszczeniach wewnątrz budynków wymaga zastosowania dedykowanej platformy mobilnej. Podczas ruchu na powierzchniach płaskich, bez nierówności, najkorzystniejszym rozwiązaniem jest zastosowanie robota kołowego z Rys. 13. Struktura przepływu danych w symulatorze. 4

MODELOWANIE INŻYNIERSKIE nr 42, ISSN 1896-771X Rys. 17. Struktura przepływu danych w robocie. Rys. 15. Prototyp robota z czujnikiem laserowym LIDAR W celu zbadania algorytmów mapowania 2D, zostało stworzone środowisko testowe, w którym znajdowały się przeszkody o różnym kształcie. Środowisko przedstawione jest na rysunku Rys. 16. każdy z wątków może publikować lub subskrybować publikowane w nich wiadomości. Takie rozwiązanie pozwala na skonfigurowanie robota z wykorzystaniem dowolnych podzespołów. Graf przepływu danych w omawianym robocie przedstawiono na rysunku 1. Dzięki możliwości symulatora V-REP do połączenia z odpowiednimi tematami systemu ROS, możliwe było przeprowadzenie badań symulacyjnych systemu sterowania bez konieczności jego modyfikacji. Jedyną różnicą jest uruchomienie symulatora zamiast wątków obsługujących sterowniki skanera laserowego, czujnika IMU i kontrolera ruchu. Przepływ danych w konfiguracji z symulatorem zaprezentowano na rysunku Rys. 13. Porównując rysunki Rys. 17 i Rys. 13 można zauważyć, że zastosowanie symulatora nie miało wpływu na konfigurację algorytmów systemu nawigacji. Jako algorytm budujący mapę wykorzystana została implementacja filtra cząsteczkowego gmapping [9], która jako źródło obserwacji wykorzystuje dane ze skanera laserowego, natomiast jak źródło odometrii wykorzystuje algorytm Canonical Scan Matcher. Algorytm ten na podstawie następujących po sobie skanów laserowych, odczytów czujnika IMU i informacji z układu odometrii określa estymatę pozycji robota. 5. PRZEBIEG EKSPERYMENTU Rys. 16 Środowisko testowe do badania operacji mapowania 2D przy użyciu czujnika laserowego Analogiczne środowisko testowe zostało sporządzone w symulatorze V-REP. 4. STRUKTURA STEROWANIA ROBOTA Do kontroli robota wykorzystany został system operacyjny ROS, ze względu na ilość zintegrowanych narzędzi, dostępność gotowych modułów i łatwość implementacji nowych. Zgodnie z przyjętą konwencją, zarówno sterownik każdego urządzenia jak i algorytm sterujący wykonywane są jako osobny wątek, komunikacja odbywa się za pomocą tematów do których W celu weryfikacji poprawności opracowanego systemu przeprowadzaono testy symulacyjne oraz test na rzeczywistym robocie. Głównym celem testów symulacyjnych było sprawdzenie działania algorytmu budowania mapy. Mapa wzbogacana jest o informację z kazdego wykonanego skanu otoczenia, dlatego zakres przez nią obejmowany rozrasta się wraz z trajektorią wykonaną przez robota. Stan mapy na różnych etapach jej tworzenia zobrazowano na rysunkach Rys. 22a-Rys. 22d. Testy na rzeczywistym robocie potwierdziły wyniki symulacji, dodatkowo pozwoliły na określenie wpływu szumu czujnika i niedoskonałości powierzchni na jakość utworzonej mapy. Dokładność mapy można ocenić na podstawie grubości ścian rozpoznanych przeszkód lub 5

ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D dystrybucji punktów zajętych wokół przeszkód skupionych. Wysokie skupienie punktów wokół przeszkody oznacza małe odchylenie standardowe lokalizacji przeszkody, a co za tym idzie dobrą dokładność mapy. 18a 19b gładkość powierzchni. Drugą z testowanych właściwości algorytmu, to zdolność do określenia lokalizacji robota względem utworzonej mapy. W algorytmie budującym mapę jako estymata pozycji wykorzystane zostały dane z algorytmu CSM. Trajektoria robota względem mapy została porównana z estymatą trajektorii dla obu testów. 26a 27b 20c 21d Rys. 28. Trajektorie robota, a - w teście symulacyjnym, b - rzeczywistego. Biala linia trajektoria względem mapy, czarna linia estymata trajektorii. Rys. 22. a mapa zaraz po uruchomieniu systemu nawigacji w symulatorze, b, c mapa w trakcie działania systemu, d mapa po zakończeniu eksploracji 23 a 24b Rys. 25. a mapa uzyskana z testu symulacyjnego, b - mapa uzyskana z testu z rzeczywistym robotem Porównując mapy utworzone w dwóch eksperymentach, wyraźnie widać, że mapa uzyskana z symulatora znacznie dokładniej odzwierciedla otoczenie. Lepsze odzwierciedlenie w przypadku mapy z testu symulacyjnego wynika przede wszystkim z braku szumu pomiarowego czujników, a także z budowy modelu w którym założono całkowitą refleksyjność i idealną 6. WNIOSKI I PLANOWANE PRACE W artykule przedstawiono konstrukcję i testy mobilnego robota kołowego przeznaczonego do budowy dwuwymiarowej mapy środowiska w którym pracuje. Przeprowadzone testy symulacyjne w środowisku odzwierciedlającym rzeczywisty tor testowy wykazały wysoką dokładność algorytmu budujacego mapę. Porównanie z eksperymentem reczywistym wykazało, iż pomimo występowania szumów czujnika i zakłóceń pochodzących od niedoskonałości powierzchni, system jest zdolny do utworzenia mapy wystarczająco dokładnej do odzwierciedlenia środowiska pracy robota. Przyszłe prace będą obejmowały rozszerzenie systemu o moduł planowania trajektorii tak, aby robot przeszukiwał pomieszczenie aż do momentu zbadania wszystkich osiągalnych lokalizacji. Dodatkowym rozszerzeniem układu będzie montaż czujnka laserowego na sterowanym przegubie, co pozwoliłoby na budowę trójwymiarowej mapy. 6

Literatura 1 Afanasyev I., Sagitov A., Magid E.: ROS-based SLAM for a Gazebo-simulated mobile robot in image-based 3D model of indoor environment. "Advanced Concepts for Intelligent Vision Systems" 2015, s. 273 283. 2 Barber R., Rodriguez-conejo M. A., Melendez J., Garrido S.: Design of an Infrared Imaging System for Robotic Inspection of Gas Leaks in Industrial Environments. "International Journal of Advanced Robotic Systems" 2015, s. 12 23. 3 Będkowski J., Pełka M., Majek K., Fitri T., Naruniec J.: Open source robotic 3D mapping framework with ROS - Robot Operating System, PCL - Point Cloud Library and Cloud Compare. "International Conference on Electrical Engineering and Informatics", 2015, s. 644 649. 4 Buratowski T., Dąbrowski B., Uhl T., Banaszkiewicz M.: The Precise Odometry Navigation for the Group of Robots. "Schedae Informaticae" 2010, nr. 19, s. 99 111. 5 Carlone L., Du J., Kaouk M., Bona B., Indri M.: Active SLAM and Exploration with Particle Filters Using Kullback-Leibler Divergence. "Journal of Intelligent Robotic Systems" 2014, nr 2, s. 291 311. 6 Durrant-whyte H., Bailey T.: Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms. IEEE Robotics & Automation Magazine" 2006, nr 13, s. 99-108. 7 Durrant-whyte H., Rye D., Nebot E.: Localization of Autonomous Guided Vehicles. "Robotics Research" 1996, s. 613 625. 8 Giergiel M., Hendzel Z., Żylski W.: Modelowanie i sterowanie mobilnych robotów kołowych. Warszawa: Wydawnictwo Naukowe PWN, 2013. 9 Grisetti G., Stachniss C., Burgard W.: Improved techniques for grid mapping with Rao-Blackwellized particle filters. IEEE Transactions on Robotics" 2007, nr 23, s. 34 46. 10 Kohlbrecher S., Von Stryk O., Meyer J., Klingauf U.: A Flexible and Scalable SLAM System with Full 3D Motion Estimation. In International Symposium on Safety, Security, and Rescue Robotics" 2011, s. 155 160. 11 NeumannK., Berg J., Möllemann G., Nienhaus K.: Collision Avoidance System for the Underground Mining Industry. In 4th IFAC Workshop on Mining, Mineral and Metal Processing MMM" 2015, nr 48, s. 60 65. 12 Riisgaard S., Blas M. R.: SLAM for Dummies. A Tutorial Approach to Simultaneous Localization and Mapping. Cambridge, 2004. 13 Stachniss C.: Introduction to Robot Mapping. Freiburg, 2014. Proszę cytować ten artykuł jako: T. Buratowski, M. Ciszewski, M. Giergiel, A. Kudriashov, Ł. Mitka, Robot z laserowym czujnikiem odległości do budowy map 2D. MODELOWANIE INŻYNIERSKIE nr 42, 2012, Doi: 10.1016/j.xxx.2012.08.007