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

Transkrypt

1 MODELOWANIE INŻYNIERSKIE 2017 nr 61, ISSN X 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. Walidację działania systemu przeprowadzono 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 (ang. Inertial Measurement Unit) [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 (ang. Robot Operating System) [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]. Innym zastosowaniem techniki SLAM jest narzędzie tworzące trójwymiarowe mapy w trybie offline na podstawie danych zarejestrowanych podczas pracy robota [3] lub środowisko oparte na systemie ROS z symulatorem Gazebo przeznaczone do testowania algorytmów SLAM [1]. W artykule zaprezentowano testy symulacyjne i weryfikację w warunkach rzeczywistych systemu przeznaczonego dla robotów mobilnych realizującego zadanie SLAM. 27

2 ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D 2. METODY NAWIGACJI I BUDOWY MAPY SLAM jest metodą symultanicznej mapowania, która rozwiązuje problem wtedy, kiedy podmiot jest ruchomy. Jest często wykoautonomicznych. rzystywana w robotyce oraz systemach 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. 1a) lub wyestymowanej serii (rys. 1b) pomiarów mogą być bardzo dokładne, ale ich otrzymanie jest możliwe tylko wtedy, gdy podmiot (robot) znajduje się w stanie sta- tycznym. 1a Rys. 1. Wizualizacja skanów laserowych: a) jeden pomiar, b) seria pomiarów 1b lokalizacji oraz tworzenia mapy Prawdopodobna estymacja trajektorii ruchu robota oraz mapy są podane jak [6, 12]: 5 :, :, : (5) Estymacja całej trajektorii oraz mapy (5 nazywa się "pełnym SLAM" (ang. Full SLAM), co może być pokazane za pomocą modelu graficznego na rys. 3. Rys. 3. 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" (6) [12,13], której przykłagraficzna reprezentacja pokazana jest na rys. dowa 4. 6, :, : :, : (6) : Rozwiązanie równania (6) przeprowadza się zazwyczaj w sposób rekursywnego całkowania, jedno na raz [13]: 7, :, : :, :, : (7) 2a 2b Rys. 4. Model graficzny Online SLAM [13] Bardzo ważne dla problemu SLAM jest zdefiniowanie modeli ruchu (8) oraz obserwacji (9). Są one pokazane na rys. 5. Podczas ruchu aktualne pomiary oraz lokalizacja zmie- mapy, niają się, więc aby możliwe było rozszerzenie wymagana jest fuzja z pomiarami z poprzedniego stanu. Nie jest to jednak łatwe zadanie (rys. 2a, b). SLAM jest metodyką, która pomaga w rozwiązaniu danego proble- lokalizacji mu za pomocą dodania informacji o aktualnej oraz mapie. Wykorzystanie filtrów estymujących, pozwa- obrazu środo- la zbliżyć się do otrzymania precyzyjnego wiska. Rys. 2. Metody tworzenia mapy: a) porzucenie poprzedniego stanu, b) scalanie z błędami Do zdefiniowania problemu SLAM wykorzystywane są wartości nadane (1, 2) oraz oczekiwane (3, 4): 1) sterowanie robotem 1 :,,, 2) obserwacje 2 :,,, 3) mapa środowiska 3 4) trajektoria ruchu robota 4 :,,, (1) (2) (3) (4) Rys. 5. Modele ruchu oraz obserwacji na graficznym modelu SLAM [13] Model ruchu jest wykorzystywany do opisu względnego ruchu robota. Model ruchu pojazdów można opisać za pomocą rozkładu prawdopodobieństwa w przejściach stanu za pomocą wzoru: 28

3 Łukasz Mitka, Michał Ciszewski, Andrii Kudriashov, Tomasz Buratowski, Mariusz Giergiel filtru Kalmana) jest opis ruchu pojazdu za pomocą wzoru [6, 12]: 12 0,, 12 3, *4 (12) 3) model kinematycznyy robota 4 5 addytywne, o zerowej wartości oczekiwa- zakłócenia ruchu z nej nieskorelowane Gaussowskie kowariancją 6 5 [6]. Wtedy model obserwacji jest opisany jak: 13 0,12,7,*8 (13)(13) 7 obserwacja geometryczna 8 5 addytywne, o zerowej wartości oczekiwa- błędy obserwacji z nej nieskorelowane Gaussowskie kowariancją 9 5 [6]. 8, (8) nowa pozycja robota, poprzednia pozycja robota, sterowanie robotem. Często używanym przykładem modelu ruchu jest stan- na sprzęże- dardowy model odometrii, który jest oparty niu zwrotnym z systemem ruchu robota oraz wykorzy- środowi- stuje kinematykę robota, wspólnie z geometrią ska. Rys. 6. Ruch robota w standardowym modelu odometrii Kiedy robot porusza się od,, do (,(, (, infor- za pomocą macja odometryczna może być opisana wzoru: 9! "#,! "$%&,! "# (9) 10! "$%& '( ) *( )! +,+-2( ),( "# ))! "# ) )! "# (10) Jednakże z powodu dużej ilości źródeł błędów ruchu, takich jak: różnica średnic kół, odkształcenia, nieidealna 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 wtedy, kiedy lokalizacja pojazdu jest znana i jest ogólnie opisany za pomocą wzoru: 11 (11) obserwacje / pomiary; położenie robota. Reprezentacja modeli obserwacji zależy od systemu czujników i może bardzo się różnić. W rozwiązywaniu problemu SLAM niemal zawsze wykorzystuje się filtry estymacji, które wspomagają otrzymanie trajektorii bliskiej do rzeczywistej. Najczę- lub ściej są one oparte na filtrach cząsteczkowych Kalmana. Bazą metodyki EKF SLAM (ang. Extended Kalman Filter SLAM - SLAM z wykorzystaniem rozszerzonego Rys. 7. Schemat działania EKF SLAM Rys. 8. Obraz środowiska przygotowany z wykorzystaniem EKF SLAM Jedna z wielu implementacji procesu EKF SLAM jest pokazana na rys. 7, gdzie EKF 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ą. Gdy przychodzi nowa odome- dokonuje fuzji tryczna informacja, algorytm jednocześnie obecnej i poprzedniej lokalizacji wraz z pomiarami laserowymi oraz aktualizuje globalny obraz środowiska (rys. 8). 29

4 ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D 3. MOBILNY ROBOT KOŁOWY 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 użycie robota kołowego z 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 rys. 9. Do pomiaru kąta obrotu silników zastosowano dwukanałowe enkodery optyczne o rozdzielczości 500 impulsów na obrót. W celu zbadania algorytmów mapowania 2D stworzono środowisko testowe (rys. 11), w którym znajdowały się przeszkody o różnym kształcie. Rys. 11. Środowisko testowe do badania operacji mapowania 2D przy użyciu czujnika laserowego Analogiczne środowisko testowe zostało sporządzone w symulatorze V-REP (rys. 12). W podstawowej wersji robota do nawigacji w przestrzeni stosowane są czujniki podczerwieni 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. Srodowisko testowe w symulatorze V-REP 4. STRUKTURA STEROWANIA ROBOTA Rys. 9. Struktura mechanizmu jezdnego robota kołowego oraz punkty charakterystyczne [4] Rys. 10. Prototyp robota z czujnikiem laserowym LIDAR Zastosowanie sensora laserowego LIDAR (ang. 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 przedstawiono na rys. 10. Robot wyposażony został w czujnik Hokuyo UTM-30LX-EW o zasięgu m, zakresie kątowym 270 oraz rozdzielczości kątowej Do kontroli robota wykorzystano system operacyjny ROS ze względu na liczbę 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 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 rys 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 rys. 13. Porównując rysunki 14 i 13, można zauważyć, że zastosowanie symulatora nie miało wpływu na konfigurację algorytmów systemu nawigacji. 30

5 Łukasz Mitka, Michał Ciszewski, Andrii Kudriashov, Tomasz Buratowski, Mariusz Giergiel Rys. 13. Struktura przepływu danych w symulatorze Rys. 14. Struktura przepływu danych w robocie Jako algorytm budujący mapę wykorzystano implementację filtra cząsteczkowego gmapping [9], która jako źródło obserwacji wykorzystuje dane ze skanera lasero- wykorzystuje wego, natomiast jako źródło odometrii algorytm CSM (ang. Canonical Scan Matcher). Algopo sobie skanów rytm ten na podstawie następujących laserowych, odczytów czujnika IMU i informacji z ukła- du odometrii określa estymatę pozycji robota. 5. PRZEBIEG EKSPERYMENTU 15a 15c 15b 15d W celu weryfikacji poprawności opracowanego systemu przeprowadzono testy symulacyjne oraz test na rzeczy- wistym robocie. Głównym celem testów symulacyjnych było sprawdzenie działania algorytmu budowania mapy. Mapa wzbogacana jest o informację z każdego wykona- nią obejmo- nego skanu otoczenia, dlatego zakres przez wany rozrasta się wraz z trajektorią wykonaną przez robota. Stan mapy na różnych etapach jej tworzenia zobrazowano na rysunkach 15a-15d. Testy na rzeczywi- dodatkowo stym robocie potwierdziły wyniki symulacji, pozwoliły na określenie wpływu szumu czujnika i niedo- mapy. skonałości powierzchni na jakość utworzonej Dokładność mapy można ocenić na podstawie grubości ścian rozpoznanych przeszkód lub 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. Rys. 15. Etapy procesu tworzenia mapy: a) zaraz po uruchomieniu systemu nawigacji w symulatorze, b, c) w trakcie działania systemu, d) po zakończeniu eksploracji Porównanie map utworzonych w dwóch eksperymentach pozwala zauważyć, że mapa uzyskana z symulatora znacznie dokładniej odzwierciedla otoczenie (rys. Rys. 16. Porównanie map z różnych eksperymentów: a) test symulacyjny, b) test z rzeczywistym robotem). 31

6 ROBOT Z LASEROWYM CZUJNIKIEM ODLEGŁOŚCI DO BUDOWY MAP 2D 16a 16b czarna linia estymata trajektorii Trajektoria robota względem mapy została porównana z estymatą trajektorii dla obu testów (rys. Rys. 17. Trajektorie robota, a - w teście symulacyjnym, b) rzeczywistego. Biała linia trajektoria względem mapy, czarna linia estymata trajektorii). 6. WNIOSKI I PLANOWANE PRACE Rys. 16. Porównanie map z różnych eksperymentów: a) test symulacyjny, b) test z rzeczywistym robotem 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ą gładkość powierzchni. Drugą z testowanych właściwości algorytmu jest zdolność do określenia lokalizacji robota względem utworzonej mapy. W algorytmie budującym mapę jako estyma tę pozycji wykorzystano dane z algorytmu CSM. 17a 17b 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 budującego mapę. Porównanie z eksperymentem rzeczywistym 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ż czujnika laserowego na sterowanym przegubie, co pozwoliłoby na budowę trójwymiarowej mapy. Rys. 17. Trajektorie robota, a - w teście symulacyjnym, b) rzeczywistego. Biała linia trajektoria względem mapy, 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, p 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, No. 12, p 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. In: International Conference on Electrical Engineering and Informatics 2015, p Buratowski T., Dąbrowski B., Uhl T., Banaszkiewicz M.: The precise odometry navigation for the group of robots. "Schedae Informaticae" 2010, nr 19, p 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, No. 2, p Durrant-Whyte H., Bailey T.: Simultaneous localisation and mapping (SLAM): Part I : The essential algorithms. IEEE Robotics & Automation Magazine" 2006, No. 13, p Durrant-Whyte H., Rye D., Nebot E.: Localization of autonomous guided vehicles. In: The Seventh International Symposium of Robotics Research 1996, p Giergiel M., Hendzel Z., Żylski W.: Modelowanie i sterowanie mobilnych robotów kołowych. Warszawa: Wyd. Nauk. PWN, Grisetti G., Stachniss C., Burgard W.: Improved techniques for grid mapping with Rao-Blackwellized particle filters. IEEE Transactions on Robotics" 2007, No. 23, p 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, p

7 Łukasz Mitka, Michał Ciszewski, Andrii Kudriashov, Tomasz Buratowski, Mariusz Giergiel 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, No. 48, p Riisgaard S., Blas M. R.: SLAM for dummies: a tutorial approach to simultaneous localization and mapping. Cambridge, Stachniss C.: Introduction to robot mapping. Freiburg, 2014, [dostęp freiburg.de/teaching/ws12/mapping/pdf/slam01-intro.pdf ] Artykuł dostępny na podstawie licencji Creative Commons Uznanie autorstwa 3.0 Polska. 33