SYMULACJA I WERYFIKACJA SYSTEMU NAWIGACJI SLAM DLA

Transkrypt

1 SYMULACJA I WERYFIKACJA SYSTEMU NAWIGACJI SLAM DLA PLATFORMY MOBILNEJ ŁUKASZ MITKA 1, TOMASZ BURATOWSKI 1 1 Katedra Inżynierii Mechanicznej i Robotyki, Akademia Górniczo-Hutnicza mitka@agh.edu.pl, tburatow@agh.edu.pl STRESZCZENIE Artykuł przedstawia testy symulacyjne i weryfikację różnych konfiguracji systemu nawigacji dla dwukołowego robota mobilnego wyposażonego w skaner laserowy, kontrolowany przez Robot Operating System. Symulacja została przeprowadzona przy pomocy środowiska V-REP z rozszerzeniami ROS, które pozwalają systemowi ROS na taką samą pracę jak z rzeczywistym robotem. 1. Wprowadzenie W artykule przedstawiono symulację i rzeczywiste testy systemu nawigacji dla dwukołowego robota z napędem różnicowym. Praca została skupiona na porównaniu różnych metod określania położenia robota. Główna część algorytmów wykonywana jest przez odpowiednie wątki w systemie ROS, natomiast symulacja wykonywana jest w środowisku V-REP połączonym z ROS. Po ukończeniu testów symulacyjnych, przeprowadzono weryfikację z wykorzystaniem rzeczywistego robota. Platforma wykorzystana do testów to robot z dwoma kołami napędowymi i jednym podpierającym. V-REP (Virtual Robot Experimental Platform) jest szeroko stosowanym narzędziem do symulacji robotów przystosowanym zarówno do aplikacji mobilnych jak i robotów przemysłowych. Posiada wszechstronny zakres zastosowań i interfejsy komunikacyjne dla licznych urządzeń i aplikacji. W ostatnim czasie były prowadzone badania w zakresie tematyki SLAM w których rozpatrywano różne podejścia do aproksymacji położenia robota. Carlone i in. [1] przedstawił zastosowanie dywergencji Kullbacka-Leiblera do budowy aproksymatora SLAM opartego na filtrze cząsteczkowym. Kohlbrecher i in. [2] przedstawił system dla scenariuszy USAR (Urban Search and Rescue, ang. poszukiwania i ratownictwo w terenie miejskim) z wykorzystaniem dwuwymiarowego algorytmu budującego mapę wspomaganego podsystemem

2 trójwymiarowej nawigacji IMU. Nienhaus i in. [3] przedstawił system SLAM dla zastosowań podziemnych oparty na syntezie czujników pod kontrolą systemu ROS. Barber i in. [4] przedstawił projekt techniki obrazowania podczerwieni w zastosowaniu do wykrywania gazu z wykorzystaniem SLAM do lokalizacji wycieków gazu względem mapy utworzonej przez robota pod kontrolą systemu ROS. Będkowski i in. [5] przedstawił ogólnodostępny zestaw narzędzi do akwizycji danych i budowy trójwymiarowej mapy w trybie offline w oparciu o algorytmy SLAM. Afanasyev i in. [6] przedstawił środowisko do testowania algorytmów SLAM oparte na systemie ROS i symulatorze Gazebo. 2. Przegląd systemu ROS (Robot Operating System) jest zestawem narzędzi do rozwoju oprogramowania sterującego robotów. Upraszcza proces programowania poprzez dostarczenie bibliotek, narzędzi i konwencji. Posiada również szeroki zakres gotowych paczek oprogramowania gotowych do zastosowania na urządzeniu. Struktura ROS jest oparta na rozproszonej sieci połączeń klient-klient pomiędzy procesami określanymi jako węzły. Zastosowanie węzła może być zróżnicowane, począwszy od niskopoziomowych sterowników urządzeń do zaawansowanych algorytmów planowania trajektorii. Komunikacja pomiędzy węzłami może odbywać się na dwa sposoby. Pierwszy to model publikacji/subskrypcji, w którym węzeł publikuje informacje w danym temacie poprzez wysyłanie wiadomości do niego. Pojedynczy temat zawsze posiada unikalną nazwę dla całego systemu, niezależnie od ilości urządzeń znajdujących się w nim. Gdy inny węzeł potrzebuje określonego rodzaju informacji, może subskrybować temat aby otrzymać wiadomości wysyłane do niego. Nie ma ograniczeń co do ilości węzłów podłączonych do tematu lub ilości tematów obsługiwanych przez jeden węzeł. Do wad tej metody komunikacji można zaliczyć fakt, że jest tylko jednokierunkowa, co oznacza, że węzeł nie może jednocześnie publikować i subskrybować tego samego tematu. Drugą z wad jest fakt, że węzeł nie może otrzymać informacji o tym jakie inne węzły publikują lub subskrybują w dany temat. Drugi model bazuje na usługach, pozwala na komunikację zapytanie/odpowiedź. W tym modelu jeden węzeł oferuje usługę pod zarejestrowaną nazwą z określonym typem wiadomości wejściowej. Inne węzły, w tym modelu nazywane klientami mogą uzyskać dostęp do usługi poprzez wywołanie jej z odpowiednią wiadomością i oczekując na odpowiedź. ROS jest wyposażony w narzędzi rosbag które pozwala na przechowywanie w pliku i odtworzenie przebiegu wiadomości publikowanych w tematach. Narzędzie pozwala na określenie czy wszystkie tematy mają być nagrywany lub tylko wybrane, a także można filtrować już istniejące nagrania [7]. V-REP, jak wspomniano wcześniej, jest wszechstronnym narzędziem przeznaczonym do symulacji. Jedną z najważniejszych cech decydujących o jego funkcjonalności jest rozproszona architektura, oznacza to, że każdy element symulowanego otoczenia może być kontrolowany osobno i z wykorzystaniem różnych technik. Można do nich zaliczyć wbudowane skrypty w

3 języku LUA, regularne API z bibliotekami dla LUA i C/C++, zdalne API obsługujące Lua, C/C++, Python, Java, MATLAB/Octave i Urbi lub wtyczki dla systemu ROS które pozwalają na komunikację z tematami i usługami [8]. Ostatnia metoda oferuje wysoki poziom abstrakcji, a co za tym idzie, może subskrybować temat lub publikować bezpośrednio z symulowanego obiektu. W większości przypadków pozwala to na szybkie nawiązanie połączenia pomiędzy obiektami V- REP a wątkami ROS. Ta metoda została wykorzystana w projekcie, ponieważ w systemie ROS traktowana jest niemal identycznie jak jak robot, dzięki czemu ta sama konfiguracja węzłów może zostać wykorzystana niezależnie od tego czy pracują z symulatorem czy rzeczywistym robotem. Rys. 1. Schemat przepływu danych w pluginie ROS dla V-REP [9]. 3. Akwizycja i przetwarzanie danych Do testu symulacyjnego przygotowano scenę V-REP z labiryntem do pokonania przez robota. W Części weryfikacyjnej przestrzeń laboratoryjna została wykorzystana jako tor testowy. Podczas pokonywania toru, dane ze skanera laserowego, systemu odometrii i określonej prędkości zostały nagrane z wykorzystaniem węzła rosbag. W przypadku symulacji, zamiast określania pozycji na podstawie obrotu kół, wykorzystana została pozycja absolutna względem układu współrzędnych symulatora. Przepływy danych podczas rejestracji zostały zaprezentowane na rysunkach 3a i 3b. Jest na nich

4 zobrazowany wpływ symulatora na tematy ROS w porównaniu do rozwiązania opartego wyłącznie na systemie ROS. W wykresie symulacji, węzeł V-REP pełni tą samą rolę co węzły skanera laserowego i kontrolera robota w systemie rzeczywistym. a) Rys. 2. Główne okno symulatora V-REP z przygotowanym torem testowym. b) Fig. 3. Przepływ danych podczas rejestracji,prostokąty prezentują tematy, elipsy prezentują węzły: a robot, b - symulator. Po jednokrotnym przebyciu labiryntu, dane zostały odtworzone kilkukrotnie z różnymi konfiguracjami algorytmu SLAM. Pierwsza konfiguracja łączy odometrię ze skanami laserowymi przy użyciu filtra cząsteczkowego z modyfikacją Rao- Blackwella. Podstawą działania tego filtra w zastosowaniu SLAM jest określenie estymaty p(x 1:t, m z 1:t, u 1:t 1 ) dotyczącej mapy m i trajektorii robota x 1:t = x 1,, x t. Estymata obliczana jest za pomocą równania: p(x 1:t, m z 1:t, u 1:t 1 ) = p(m x 1:t, z 1:t ) p(x 1:t z 1:t, u 1:t 1 ) (1)

5 gdzie z 1:t = z 1,, z t to skany laserowe i u 1:t 1 = u 1,, u t 1 to informacja z układu odometrii robota. Estymata p(x 1:t z 1:t, u 1:t 1 ) może zostać określona przy użyciu filtra cząsteczkowego, gdzie każda cząsteczka reprezentuje potencjalną trajektorię robota. Ponadto osobna mapa jest powiązana z każdą cząsteczką. Filtrowanie cząsteczkowe przeprowadzane jest w następujących krokach: Próbkowanie: Generacja cząsteczek {x t (i) } jest otrzymywana z (i) generacji {x t 1} przez próbkowanie z dystrybucji π, która jest probabilistycznym modelem ruchu opartym na odometrii. (i) Określanie wag: każda cząsteczka otrzymuje przypisaną wagę w t p(x (i) (i) 1:t z 1:t, u 1:t 1 ) w t = π(x (i) 1:t z 1:t, u 1:t 1 ) (2) Ponownie próbkowanie: Cząsteczki są zastępowane nowymi proporcjonalnie do przypisanych im wag. Estymacja mapy: Dla każdej cząsteczki, estymata powiązanej mapy p (m (i) x (i) (i) 1:t, z 1:t ) jest obliczana w oparciu o trajektorię x 1:t danej próbki i historię skanów z 1:t [10]. Kolejne dwie konfiguracje wykorzystują algorytm Iterative Closest Points do wyznaczania pozycja zamiast odometrii. Algorytm ICP służy do dopasowywania powierzchni: min q p i q {S ref 2, p i q} i gdzie S ref jest powierzchnią odniesienia, {p i } jest zestawem punktów i q = (t, θ) jest roto-translacją. ICP minimalizuje równanie (4) iteracyjnie, począwszy od pierwszej próby q 0. Projekcje punktów na S ref są obliczane zgodnie z poprzednią próbą q k, a rozwiązanie jest znajdywane dla nowej próby q k+1. Dla każdego kroku k jest to opisane przez: min p q i q k+1 {S ref, p i q k } k+1 i ICP może pracować bez początkowej próby q k lub ze wstępną roto-translacją, np. bazującą na zadanej prędkości robota, przy założeniu, że pomiędzy wykonaniem skanów robot poruszył się o odległość określoną na podstawie prędkości [11]. 2 (3) (5)

6 4. Porównanie wyników Wynikiem przetwarzanie danych są mapy w postaci siatki zajętości, przemieszczenia robota względem pozycji początkowej i pozycja robota względem mapy. Utworzone siatki zajętości wyróżniają trzy możliwe stany dla przestrzeni: przeszkoda oznaczona na czarno, wolna przestrzeń oznaczona na biało i teren niedostępny oznaczony na szaro. Teren może zostać oznaczony jako niedostępny w przypadku gdy jest otoczony przeszkodami lub nie został jeszcze zbadany. Rozważane parametry mapy to odwzorowanie badanej przestrzeni i wariacja rozmieszczenia przeszkód. W teście symulacyjnym, ze względu na fakt, że źródłem odometrii była pozycja absolutna, można oczekiwać, że mapa utworzona na podstawie odometrii będzie najdokładniejsza. Wyniki symulacji są zgodne z tymi oczekiwaniami, mapa utworzona na podstawie odometrii cechuje się najmniejszą dystorsją. Ponadto niedokładność lokalizacji przeszkód, widoczna jako dyspersja czarnych punktów wokół przeszkody jest większa w przypadku map utworzonych z algorytmem dopasowującym skany, w szczególności w wariancie z modelem predykcyjnym stałej prędkości. a b c Rys. 4. Fragmenty map toru testowego w symulatorze V-REP utworzone z wykorzystaniem: a odometrii, b dopasowania skanów, c dopasowania skanów z predykcją prędkości. a b c Rys. 5. Fragmenty mapy laboratorium utworzone z wykorzystaniem: a odometrii, b dopasowania skanów, c dopasowania skanów z predykcją prędkości.. Celem weryfikacji było sprawdzenie czy mapy utworzone na podstawie odometrii robota w warunkach rzeczywistych, przy występowaniu zakłóceń, również będą

7 dokładniejsze niż mapy utworzone z wykorzystaniem dopasowywania skanów. Źródła zakłóceń występujące w warunkach rzeczywistych, które nie zostały uwzględnione w symulacji to nierówności terenu, poślizg kół i nieodbijająca powierzchnia przeszkód. Aby zminimalizować wpływ zakłóceń, zostały wykorzystane przeszkody z powierzchnią dobrze odbijającą promienie laserowe a robot poruszał się po twardej i gładkiej powierzchni. Uzyskane mapy potwierdziły wyniki symulacji. Mapa utworzona na podstawie odometrii robota jest dokładniejsza, natomiast niedokładność lokalizacji przeszkód, widoczna jako grubość obrysu przeszkody, jest wyższa dla map utworzonych na podstawie dopasowania skanów. Rys. 6. Porównanie pozycji robota podczas symulacji. Rys. 7. Porównanie pozycji robota podczas weryfikacji.

8 Pozycje robota w trakcie symulacji określone przez algorytm dopasowujący skany, dowodzą, że algorytm jest podatny na błędy przemieszczeń generowane w miejscach gdzie z czujnika odbierana jest niewystarczająca ilość danych. Z drugiej strony błąd rotacji wygenerowany przez algorytm, zarówno dla braku predykcji jaki i modelu stałej prędkości, jest mniejszy niż 0.01%. Część weryfikacyjna wykazała, że pozycja określona na podstawie odometrii i algorytmu dopasowującego skany różnią się o około 3% w przemieszczeniu i rotacji, podczas gdy różnice między pozycją określoną przez oba warianty algorytmu dopasowującego skany różni się nie więcej niż 0.01%. Filtr cząsteczkowy wykorzystany do budowy mapy jest odporny na niedokładności lokalizacji do pewnego stopnia. Objawia się to przez dopasowanie pozycji robota względem mapy. W implementacji ROS to dopasowanie osiągnięte jest poprzez określenie położenia początku trajektorii robota względem mapy. Nie może zostać zaprezentowane bezpośrednio jako pozycja robota względem mapy, ponieważ ROS nie pozwala aby pojedynczy obiekt posiadał dwa nadrzędne, zasada ta pozwala na uniknięcie nieścisłości w strukturze układów współrzędnych. Testy symulacyjne wykazały, że wykorzystany filtr cząsteczkowy jest zdolny do eliminacji błędów powstałych w procesie lokalizacji, skutkując dokładną pozycją robota względem mapy. Celem części weryfikacyjnej było sprawdzenie czy błędy algorytmu w połączeniu z zakłóceniami są wystarczająco małe, aby filtr cząsteczkowy mógł je pokonać. Pozycja robota względem mapy określana jest jako złożenie punktu początkowego trajektorii względem mapy i pozycji robota względem punktu początkowego. Weryfikacja wykazała, że pozycje uzyskane w ten sposób są zbieżne dla wszystkich typów nawigacji. Obserwacje towarzyszące testom pozwalają wnioskować, iż jeżeli w otoczeniu występują powtarzalne wzorce, może to zaburzyć odporność algorytmu na niedokładności lokalizacji. a b Rys. 8. a Struktura układów współrzędnych, b Wyznaczone pozycje mapy.

9 Podczas analizy zebranych danych, zmierzone zostało obciążenie procesora w celu oceny czy wykorzystywane algorytmy mogłyby znaleźć zastosowanie w komputerach wbudowanych o niskim poborze mocy i jednocześnie niewielkiej mocy obliczeniowej. Pomiary były przeprowadzane na komputerze wyposażonym w procesor 2.1GHz i rezultaty pomiarów zebrano w tabeli 1, wartości prezentują procentowe wykorzystanie procesora. Wykorzystanie procesora przez filtr cząsteczkowy jest na tym samym poziomie w wszystkich przypadkach, natomiast warto omówić zapotrzebowanie algorytmu dopasowującego skany. W przypadku symulatora i robota różnią się dziesięciokrotnie, jest to spowodowane ilością skanów otrzymywanych przez algorytm. Wykorzystany skaner laserowy to Hokuyo UTM- 30LX-EW które może wykonać do 40 skanów na sekundę. Model skanera wykorzystany w symulacji odzwierciedla rzeczywisty skaner, jednak jego częstotliwość pracy zależy od czynników takich jak model dynamiczny symulatora, krok symulacji czy opcje wizualizacji. Podczas symulacji ta wartość oscylowała w granicy 5 skanów na sekundę. Ze względu na dużo mniejszą ilość danych do analizy, algorytm wykazywał mniejsze zapotrzebowanie na moc obliczeniową. Tabela 1. Procentowe wykorzystanie procesora przez warianty algorytmu SLAM. Rodzaj lokalizacji Odometria Dopasowanie skanów Dopasowanie skanów z predykcją prędkości Nawigacja (V-REP) Filtr cząsteczkowy (V-REP) Nawigacja (Robot) Filtr cząsteczkowy (Robot) Wnioski i przyszłe prace Badania zaprezentowane w artykule skupione były na demonstracji możliwości symulatora V-REP w porównaniu do robota rzeczywistego. Rozważane aspekty to łatwość tworzenia środowiska testowego i złożoność rekonfiguracji algorytmów ROS między pracą z symulatorem a rzeczywistym robotem. Przyszłe prace będą polegać na fuzji czujników przez zastosowanie czujników IMU lub GPS na platformie mobilnej a także implementacja ich modeli w środowisku symulacyjnym. Kolejnym krokiem będzie rozszerzenie możliwości systemu do budowy map trójwymiarowych. Rezultaty porównania wariantów SLAM prowadzą do wniosku, że możliwa jest budowa mobilnej platformy wyposażonej w skaner laserowy jako jedyny czujnik do lokalizacji i badania otoczenia. Może to zostać wykorzystane gdy dane z odometrii nie są dostępne z powodu konstrukcji robota lub nieodpowiedniego terenu.

10 6. Bibliografia [1] L. Carlone, J. Du, M. Kaouk, B. Bona, and M. Indri, Active SLAM and Exploration with Particle Filters Using Kullback-Leibler Divergence, J. Intell. Robot. Syst., vol. 75, no. 2, pp , [2] S. Kohlbrecher, O. Von Stryk, J. Meyer, and U. Klingauf, A Flexible and Scalable SLAM System with Full 3D Motion Estimation, in International Symposium on Safety, Security, and Rescue Robotics, 2011, pp [3] K. Neumann, J. Berg, G. Möllemann, and K. Nienhaus, Collision Avoidance System for the Underground Mining Industry, in 4th IFAC Workshop on Mining, Mineral and Metal Processing MMM, 2015, vol. 48, pp [4] R. Barber, M. A. Rodriguez-conejo, J. Melendez, and S. Garrido, Design of an Infrared Imaging System for Robotic Inspection of Gas Leaks in Industrial Environments Regular Paper, Int. J. Adv. Robot. Syst., pp , [5] J. Będkowski, M. Pełka, K. Majek, T. Fitri, and J. Naruniec, 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, no. August, pp [6] I. Afanasyev, A. Sagitov, and E. Magid, ROS-based SLAM for a Gazebo-simulated mobile robot in image-based 3D model of indoor environment, in Advanced Concepts for Intelligent Vision Systems, Springer International Publishing, 2015, pp [7] ROS, [Online]. Available: [Accessed: 22-May-2016]. [8] E. Rohmer, S. P. N. Singh, and M. Freese, V-REP : a Versatile and Scalable Robot Simulation Framework, pp [9] V-REP, [Online]. Available: [Accessed: 22-May-2016]. [10] G. Grisetti, C. Stachniss, and W. Burgard, Improved techniques for grid mapping with Rao-Blackwellized particle filters, IEEE Trans. Robot., vol. 23, no. 1, pp , [11] A. Censi, An ICP variant using a point-to-line metric, Proc. - IEEE Int. Conf. Robot. Autom., pp , 2008.