Algorytm FastSLAM jednoczesnej lokalizacji i budowy mapy
Transkrypt
Algorytm FastSLAM jednoczesnej lokalizacji i budowy mapy
Rok akademicki 2010/2011 Politechnika Warszawska Wydzial Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki Stosowanej Praca Dyplomowa Magisterska Tomasz Zaborowski Algorytm FastSLAM jednoczesnej lokalizacji i budowy mapy otoczenia przez robota mobilnego Praca wykonana pod kierunkiem: dr inż. Wojciecha Szynkiewicza Algorytm FastSLAM jednoczesnej lokalizacji i budowy mapy otoczenia przez robota mobilnego Streszczenie Praca dotyczy zagadnienia jednoczesnej lokalizacji i budowy mapy otoczenia (SLAM) przez robota mobilnego. Zadanie SLAM stanowi podstawę działania autonomicznych robotów mobilnych. Bez możliwości precyzyjnego określenia własnej pozycji względem otoczenia, wiele dalszych celów stawianych robotom staje się niemożliwe do wykonania. Z tego powodu zadanie SLAM stało się w ostatnich latach obszarem intensywnych badań. W tym czasie zaproponowano liczne rozwiązania, z których większość wykorzystywała algorytmy filtru Kalmana lub filtrów cząsteczkowych. Alternatywne podejście to algorytm FastSLAM. Jest to algorytm hybrydowy łączący elementy dwóch wyżej wymienionych podejść. W niniejszej pracy została szczegółowo zaprezentowana zasada działania algorytmu FastSLAM, opisano jego konkretną realizację, oraz przedstawiono rezultaty jego działania. Algorytm zaimplementowano dla robota mobilnego wyposażonego w czujniki odometryczne oraz dalmierz laserowy. Robot, na którym uruchomion był serwer Player poruszał się w statycznym środowiku wewnętrznym korytarzy i holi. Celem robota było zbudowanie mapy występujących w środowisku naturalnych znaczników oraz wyznaczenie względem nich własnej ścieżki. Algorithm FastSLAM of Simultaneus Localization And Mapping By Mobile Robot Abstract This thesis concerns the problem of Simultaneus Localization and Mapping (SLAM) by a mobile robot. SLAM problem is essential for autonomous mobile robots. Without possibility of localizing robot in the environment lots of further tasks become impossible to solve. This is the reason why the SLAM problem has become the area of intensive research in robotics. In the last years numerous approaches to SLAM problem have been proposed. Most of them based on Kalman Filters or Particle Filters. Alternative method to SLAM problem, FastSLAM algorithm, was proposed by Montemerlo. This is a hybrid algorithm that contains elements of two mentioned approaches. The main goal of this thesis was to develop, implement FastSLAM algorithm and verify it with the mobile robot in the real environment. The algorithm has been implemented for the mobile robot that was equipped with odometric sensors and range-sensor. The robot moved under the control of Player/Stage environment in the static environment of corridors and halls. The goal of the robot was to build map of natural landmarks that existed in environment and to estimate its path on built map. Notacja: t – chwila czasowa x, y – współrzędne położenia robota w kartezjańskim układzie odniesienia α – orientacja robota st = [x, y, α]T – pozycja robota w chwili t ut – wektor sterowań robota w chwili t zt – wektor obserwacji robota w chwili t s1:t = {s1 , ..., st } - ścieżka robota składająca się z ciągu kolejnych pozycji m = {m1 , ..., mi } - mapa w postaci zbioru położeń znaczników z1:t = {z1 , ..., zt } - wyniki pomiarów położeń znaczników u1:t = {u1 , ..., ut } - ciąg wartości sterowań robota w kolejnych chwilach ruchu n1:t = {n1 , ..., nt } - zbiór skojarzeń obserwacji ze znacznikami K - liczba cząstek w filtrze cząsteczkowym Nt - liczba znaczników wykryta do chwili t µn,t - wartość oczekiwana rozkładu normalnego reprezentującego położenie n-tego znacznika Σn,t - macierz kowariancji rozkładu normalnego reprezentującego położenie n-tego znacznika Spis treści 1 Wstęp 1 1.1 Cel pracy . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Przyjęte założenia . . . . . . . . . . . . . . . . . . . 1 1.3 Zakres pracy . . . . . . . . . . . . . . . . . . . . . . 2 1.4 Zawartość pracy . . . . . . . . . . . . . . . . . . . . 2 2 Jednoczesna lokalizacja i budowa mapy 2.1 2.2 3 Wprowadzenie do zadania SLAM. . . . . . . . . . . 3 2.1.1 Budowa mapy . . . . . . . . . . . . . . . . . 6 2.1.2 Samolokalizacja . . . . . . . . . . . . . . . . 7 Podejścia do rozwiązania zadania SLAM . . . . . . 8 2.2.1 Metody parametryczne . . . . . . . . . . . . 8 2.2.2 Metody nieparametryczne . . . . . . . . . . 10 2.2.3 Metody hybrydowe . . . . . . . . . . . . . . 11 3 Algorytm FastSLAM 13 3.1 Wprowadzenie . . . . . . . . . . . . . . . . . . . . . 13 3.2 Filtr Bayesa . . . . . . . . . . . . . . . . . . . . . . 14 3.3 Modele systemu . . . . . . . . . . . . . . . . . . . . 16 3.3.1 Model ruchu . . . . . . . . . . . . . . . . . . 16 3.3.2 Model obserwacji . . . . . . . . . . . . . . . 18 Opis algorytmu FastSLAM . . . . . . . . . . . . . . 19 3.4.1 Generacja nowych cząstek . . . . . . . . . . 20 3.4.2 Uaktualnienie estymat położeń znaczników . 21 3.4 i 3.4.3 Obliczenie wag cząstek . . . . . . . . . . . . 22 3.4.4 3.4.5 Ponowne próbkowanie . . . . . . . . . . . . Wykrywanie znaczników . . . . . . . . . . . 23 24 3.4.6 Asocjacja danych . . . . . . . . . . . . . . . 27 4 Środowisko badawcze 4.1 31 Robot . . . . . . . . . . . . . . . . . . . . . . . . . 31 4.1.1 4.1.2 Czujniki odometryczne . . . . . . . . . . . . Dalmierz laserowy . . . . . . . . . . . . . . 32 33 Platforma Player/Stage . . . . . . . . . . . . . . . . 4.2.1 Player . . . . . . . . . . . . . . . . . . . . . 34 34 4.2.2 4.2.3 Stage . . . . . . . . . . . . . . . . . . . . . . Biblioteka Miarn . . . . . . . . . . . . . . . 36 37 5 Implementacja algorytmu FastSLAM 5.1 Struktura programu . . . . . . . . . . . . . . . . . . 39 39 4.2 5.2 Opis klas . . . . . . . . . . . . . . . . . . . . . . . . 5.2.1 Klasa FastSlamDriver . . . . . . . . . . . . 40 40 5.2.2 5.2.3 Klasa Fastslam . . . . . . . . . . . . . . . . Klasa Ekf . . . . . . . . . . . . . . . . . . . 44 47 5.2.4 Klasa My Math . . . . . . . . . . . . . . . . 48 6 Badania symulacyjne i eksperymentalne 6.1 Badania symulacyjne . . . . . . . . . . . . . . . . . 51 52 6.1.1 6.1.2 6.2 Liczba cząstek w filtrze cząsteczkowym . . . Jazda w linii prostej . . . . . . . . . . . . . 53 54 6.1.3 Wykonanie skrętu . . . . . . . . . . . . . . . Eksperymenty w środowisku rzeczywistym . . . . . 56 59 6.2.1 6.2.2 Test algorytmu wykrywania znaczników . . Jazda po linii prostej . . . . . . . . . . . . . 59 60 6.2.3 6.2.4 Wykonanie skrętu . . . . . . . . . . . . . . . Jazda po linii prostej oraz wykonanie skrętu 64 67 7 Podsumowanie 73 Rozdział 1 Wstęp 1.1 Cel pracy Celem pracy było opracowanie, implementacja a następnie eksperymentalna weryfikacja działania algorytmu rozwiązującego zadanie jednoczesnej lokalizacji i budowy mapy otoczenia (ang. Simultaneus Localization And Mapping - SLAM ) w przypadku rzeczywistego robota mobilnego poruszającego się w nieznanym sobie środowisku. Do realizacji został wybrany algorytm FastSLAM. Jest to algorytm hybrydowy, który łączy w sobie elementy algorytmów rozszerzonego filtru Kalmana oraz filtrów cząsteczkowych. 1.2 Przyjęte założenia Jako docelowe środowisko działania robota wybrane zostały po- mieszczenia zamknięte, takie jak korytarze lub hole. Założono, że robot w chwili początkowej nie ma żadnej informacji na temat otoczenia. Robot jest wyposażony w czujniki odometryczne oraz dalmierz laserowy. Eksperymenty wykonano przy założeniu statyczności środowiska, bez obecności obiektów ruchomych, oraz przy nieznanych położeniach znaczników. Za znaczniki uznano naturalne charakterystyczne obiekty w postaci rogów tworzonych przez sąsiadujące ściany budynku lub innych obiektów. 1 1.3 Zakres pracy Praca dotyczy zagadnienia jednoczesnej lokalizacji i budowy mapy otoczenia przez robota mobilnego. W zakres pracy wchodzi opracowanie i implementacja algorytmu FastSLAM oraz badania symulacyjne i eksperymentalne z użyciem robota w środowisku wewnętrznym. 1.4 Zawartość pracy Drugi rozdział pracy zawiera omówienie zadania jednoczesnej lokalizacji i oraz przedstawienie typowych podejść do jego rozwiązania. W rozdziale trzecim szczegółowo przedstawiono zasadę działania hybrydowego algorytmu FastSLAM, który został zaimplementowany na potrzeby niniejszej pracy oraz opisano algorytmy wykorzystane w celu realizacji kolejnych kroków algorytmu. W rozdziale czwartym znajduje się opis docelowego stanowiska badawczego czyli mobilnego robota laboratoryjnego Elektron wraz z jego czujnikami. Następnie, w rozdziale piątym przedstawiono szczegółowy opis implementacji algorytmu FastSLAM jako sterownika do serwera Player. Rozdział szósty zawiera rezultaty badań przeprowadzonych podczas prac na symulatorze Stage oraz eksperymentów na rzeczywistym robocie Elektron poruszającym się w środowisku wewnętrznym. W podsumowaniu przedstawiono najważniejsze osiągnięcia uzyskane w pracy oraz sformułowano wnioski na podstawie otrzymanych wyników. 2 Rozdział 2 Jednoczesna lokalizacja i budowa mapy 2.1 Wprowadzenie do zadania SLAM. Zadanie jednoczesnej lokalizacji i budowy mapy otoczenia stanowi podstawę działania autonomicznych robotów mobilnych [1]. Bez możliwości precyzyjnego określenia własnej pozycji względem otoczenia oraz bez dostępności dobrego modelu, wiele dalszych celów stawianych robotom również staje się niemożliwe do wykonania. Z tego powodu zadanie SLAM stało się w ostatnich latach obszarem intensywnych badań. Zazwyczaj nie ma dokładnej mapy środowiska, w którym działa robot. Mając dokładną mapę określenie pozycji robota jest stosunkowo proste, jeśli dysponujemy odpowiednio wiarygodnymi pomiarami. I odwrotnie, znając dokładnie pozycje robota można łatwo zbudować dokładną mapę. Niestety zazwyczaj nie mamy dokładnej mapy i nie potrafimy dokładnie określić pozycji robota. Dodatkowo trzeba zaznaczyć, że błędy wyznaczania pozycji robota są skorelowane Z błędami czujników. Robot, który błędnie wyznaczył swoją aktualną pozycją, błędnie odtworzy model środowiska, w którym się znajduje [12]. Wykonanie całości zadania wymaga rozwiązania dwóch głównych 3 problemów. Pierwszy polega na określeniu własnej pozycji w danym układzie odniesienia, zaś drugi to budowa mapy lub uzupełnienie jej nowymi danymi. Dla poprawnego działania robota oba wymienione wyżej zadania muszą być wykonywane jednocześnie. Ponadto pojedyncza iteracja algorytmu musi trwać na tyle krótko, aby robot nadążał przetwarzać pobierane dane. Rozwiązując postawione zadania trzeba wziąć pod uwagę nieuchronne występowanie błędów przy odczytach Z czujników robota, które mogą w znacznym stopniu wpływać na ostateczne rozwiązanie. Robot wykonując zadanie SLAM korzysta Z dostępnych sobie danych pomiarowych, aby stworzyć spójną mapę otoczenia oraz wyznaczyć na niej dotychczasową ścieżkę. Ścieżka robota jest opisana jako dyskretny zbiór punktów, które są pośrednimi pozycjami robota wyznaczonymi na postawie danych dostarczonych Z czujników w danej chwili t oraz danych Z poprzedniej chwili t-1. Zależności pomiędzy poprzednią pozycją st−1 , danymi pobranymi Z czujników odometrycznych ut , obserwacjami zt oraz mapą m, a stanem robota wykonującego zadanie SLAM w chwili t można przedstawić w postaci dynamicznej sieci Bayesa pokazanej na rysunku 2.1. w chwili t robot znajduje się w pozycji st , która zależy jedynie od pozycji Z poprzedniej chwili oraz danych Z czujników odometrycznych. Jednocześnie robot dokonuje obserwacji zt . w kolejnych chwilach robot zachowuje sie analogicznie. W literaturze można spotkać dwie postaci zadania SLAM [15]: • Full SLAM - w tym zadaniu estymuje się całą dotychczasową ścieżkę robota oraz mapę jako łączny rozkład prawdopodobieństwa a posteriori ścieżki robota i mapy pod warunkiem wszystkich obserwacji i sterowań. Można to zapisać w następujący sposób: p(s1:t , m | z1:t , u1:t ), 4 (2.1) Rysunek 2.1: Dynamiczna sieć Bayes’a opisująca probabilistyczne zależności między stanem, obserwacjami i cechami [17]. gdzie: m - mapa środowiska s1:t = {s1 , ..., st } - ciąg dyskretnych pozycji robota (ścieżka) z1:t = {z1 , ..., zt } - wyniki pomiarów położeń znaczników u1:t = {u1 , ..., ut } - wartości sterowań robota w kolejnych chwilach Zadanie full SLAM polega na wyznaczaniu łącznego rozkładu prawdopodobieństwa ścieżki robota i mapy na podstawie dostępnych danych Z czujników. • SLAM online - w tym zadaniu oprócz mapy estymuje się aktualną pozycję robota zamiast całej ścieżki, co można zapisać następujący sposób: p(st , m | z1:t , u1:t ) (2.2) Do rozwiązania powyższych postaci zadań SLAM zwykle wykorzystuje się różne implementacje filtru Bayesa [11]. 5 2.1.1 Budowa mapy Jak zostało wspomniane w poprzedniej sekcji, jednym Z dwóch głównych problemów zadania SLAM jest budowa spójnej mapy otoczenia. Mapę rozumie się jako reprezentację wewnętrzną środowiska zewnętrznego. Tworzona jest ona na podstawie danych pobranych Z czujników robota. Spójna mapa środowiska umożliwia robotowi wykonanie wielu zadań jak samolokalizacja lub nawigacja. Podstawowe rodzaje map to [17]: • Mapy metryczne - opisują relacje geometryczne między obiektami - rastrowe - w postaci regularnych siatek zajętości - geometryczne - zbudowane Z obiektów geometrycznych • Mapy topologiczne - w postaci grafów, których węzły odpowiadają obserwowanych cechom, obiektom, a łuki opisują zależności między tymi cechami • Mapy hybrydowe - połączenie map topologicznych i geometrycznych • Mapy kognitywne - poza opisem geometrycznym lub topologicznym środowiska, zawierają dodatkowe informacje o obiektach Głównymi problemami, które występują podczas budowy mapy są: • szumy pomiarowe. Błędy sumują się w czasie i mają wpływ na dalsze pomiary. • złożoność zadania budowy mapy. • problem kojarzenia danych Z fizycznymi obiektami. • zmienne w czasie środowisko 6 2.1.2 Samolokalizacja Drugim Z problemów napotykanych przez robota podczas wykonywania zadania SLAM jest samolokalizacja w czasie ruchu. Odczytując dane Z czujników odometrycznych robota, należy mieć świadomość błędów pomiarowych jakie towarzyszą tym pomiarom. Możliwe przyczyny występowania niedokładności w odczytach danych odometrycznych to [17]: • różne średnice kół, • rzeczywisty rozstaw kół różny od wartości nominalnych, • niewspółosiowość kół, • ślizganie się kół po podłożu, • nierówne podłoże, • skończona rozdzielczość oraz częstotliwość próbkowania enkodera. Mając na uwadze błędy jakie towarzyszą czujnikom odometrycznym, konieczne jest wykorzystanie innej metody weryfikacji pozycji robota. Do wyznaczenia bezwzględnej pozycji robota korzysta się Z następujących technik: • Aktywne latarnie kierunkowe - na podstawie kierunku promieniowania 3 lub więcej nadajników o znanch położeniach oblicza się absolutne położenie . • Sztuczne znaczniki - w środowisku, w którym porusza się robot umieszcza się charakterystyczne obiekty. Na podstawie co najmniej 3 znaczników znajdujących się w polu widzenia robota można wyznaczyć jego pozycję. • Naturalne znaczniki - do wyznacznenia pozycji robota wykorzystuje się charakterystyczne obiekty znajdujące się w środowisku, w którym porusza się robot. 7 • Dopasowanie modelu - lokalną mapę zbudowaną na podstawie pomiarów robota porównuje się ze znaną mapą otoczenia i na ich podstawie wyznacza się pozycję robota 2.2 Podejścia do rozwiązania zadania SLAM W trakcie prac badawczych dotyczących zadania SLAM powstały liczne metody wykorzystujące rozkłady prawdopodobieństwa umożliwiające jednoczesną budowę mapy otoczenia i lokalizację robota. Mozną wyróżnić trzy podstawowe grupy metod: • metody wykorzystujące parametrczne rozkłady prawdopodobieństwa, • metody wykorzystujące nieparametrczne rozkłady prawdopodobieństwa, • metody hybrydowe, łączące elementy obu wymienionych wyżej grup metod, 2.2.1 Metody parametryczne Najczęściej spotykanymi w literaturze algorytmami parametrycznymi stosowanymi w zadaniu SLAM są różne adaptacje filtru Kalmana. Oryginalny algorytm filtru Kalmana został zaproponowany w 1960 roku przez Rudolfa Emila Kalmana w pracy [6] jako rekurencyjny sposób wyznaczania minimalno-wariancyjnej estymaty stanu procesu. Filtry Kalmana są stosowane w wielu dziedzinach nauki takich jak: elektronika, robotyka, przetwarzanie sygnałów, obrazów. Filtr znalazł zastosowanie w przypadkach, gdy stan procesu nie może być jednoznacznie wyznaczony lub kiedy do pomiaru stanu systemu używane są dwa źródła pomiarów, Z których jeden jest obarczony dużym błędem pomiarowym, natomiast drugi jest znacznie dokładniejszy. w podejściu tym przechowywany i uaktualniany jest stan systemu. 8 Algorytm składa się Z fazy predykcji i korekcji. w przypadku robota wyposażonego w czujniki odometryczne oraz dalmierz laserowy, mniej dokładnym źródłem danych o otoczeniu są czujniki odometryczne, które służą do wyznaczenia stanu systemu w pierwszym kroku algorytmu. Dane pobierane Z dalmierza, które dostarczają znacznie dokładniejszych pomiarów, służą do korekcji wyznaczonego stanu. Schemat postępowania robota działającego według algorytmu filtru Kalmana widoczny jest na rysunku 2.2. Rysunek 2.2: Schemat działań algorytmu filtru Kalmana wykorzystywanego przez robota mobilnego wyposażonego w czujniki odometryczne i dalmierz laserowy[17]. Oryginalny algorytm filtru Kalmana wykorzystuje się niestety tylko w przypadku systemów opisanych funkcjami liniowymi. Pierw9 szą historycznie zaproponowaną adaptacją algorytmu filtru Kalmana dla systemów nieliniowych był EKF (rozszerzony filtr Kalmana) [16]. w rozszerzonym filtrze Kalmana funkcje tranzycji stanu są linearyzowane poprzez pierwszy wyraz ich rowinięcia w szereg Taylora [16]. Kolejną adaptacją filtru Kalmana jest UKF (Unscented Kalman Filter) [20], który do wyznaczania aktualnego stanu systemu wykorzystuje transformatę unscented. w literaturze można też spotkać inne algorytmy adaptujące filtr Kalmana dla zadania SLAM, między innymi: CEKF (Compressed Kalman Filter) [5] oraz Hybrid Kalman Filter [3]. Wady metod bazujących na algorytmie filtru Kalmana to: założenie o normalnym rozkładzie szumów czujników (zwykle nie jest to prawdą), fakt iż błędna asocjacja znaczników wchodzi na stałe do stanu procesu oraz brak możliwości śledzenia wielu hipotez lokalizacji. 2.2.2 Metody nieparametryczne Do grupy metod nieparametrycznych należą metody korzystające Z filtrów cząsteczkowych. Metody bazujące na filtracji cząsteczkowej rozwiązały w przeszłości kilka problemów Z zakresu robotyki m. in. problem lokalizacji robota przy założeniu znanej mapy. Filtry cząsteczkowe początkowo wykorzystywane były w zagadnieniach mniej skomplikowanych, lecz sukcesy jakie dzięki nim osiągnięto, spowodowały dalsze ich badania i rozwój, a obecnie często wykorzystywane są przy próbach rozwiązania zadania SLAM [9]. w przypadku metod bazujących na filtrach cząsteczkowych wykorzystuje się skończony zbiór cząstek do opisu rozkładu prawdopodobieństwa pozycji robota. w każdej iteracji generuje się zbiór próbek według ustalonego rozkładu prawdopodobieństwa, a następnie na podstawie obserwacji wybiera się cząstki najbardziej dopasowane do budowanej mapy środowiska. Przykładowy zbiór cząsteczek wygenerowanych zgodnie Z rozkładem Gaussa przedstawiony został 10 na rysunku 2.3. Największe skupienie cząstek znajduje sie w pobliżu punktu o współrzędnych obliczonych Z modelu ruchu, zaś im dalej od wyznaczonego punktu, cząsteczek jest coraz mniej. Rysunek 2.3: Przykładowy zbiór cząstek wygenerowany w I kroku algorytmu [12]. Do algorytmów wykorzystujących filtry cząsteczkowe zaliczają się między innymi: DP-SLAM [4] lub GridSLAM [26]. Główną wadą filtrów cząsteczkowych jest konieczność generacji dużej liczby cząstek, a co za tym idzie duża złożoność obliczeniowa. 2.2.3 Metody hybrydowe Łączą w sobie cechy dwóch powyższych podejść, przykładem algorytmu należącego do tej grupy jest algorytm FastSLAM [12], któremu poświęcona została niniejsza praca. Algorytm FastSLAM wykorzystuje algorytm filtru cząsteczkowego do estymacji pozycji oraz rozszerzony filtr Kalmana do estymacji położeń znaczników. 11 12 Rozdział 3 Algorytm FastSLAM 3.1 Wprowadzenie Do poprawnego rozwiązania zadania SLAM bardzo ważne jest opracowanie algorytmu, który zapewni uzyskanie odpowiednio dokładnej i spójnej mapy otoczenia. Niedokładne lub niespójne mapy często uniemożliwiają w pełni autonomiczną pracę robotów. Tak jak wcześniej wspomniano w trakcie badań nad problemem SLAM, powstawały liczne algorytmy, które do estymacji pozycji robota wykorzystywały głównie różne wersje filtru Kalmana lub filtrację cząsteczkową. Alternatywne podejście do rozwiązania zadania SLAM, algorytm FastSLAM, zaproponował Montemerlo w [12]. FastSLAM jest algorytmem hybrydowym, który wykorzystuje filtr cząsteczkowy do estymacji ścieżki robota i rozszerzony filtr Kalmana do estymacji położeń znaczników. Dzięki takiemu rozwiązaniu możliwe jest śledzenie wielu możliwości lokalizacji robota, a co za tym idzie wielu możliwości reprezentacji środowiska, a także nie ma potrzeby linearyzacji modelu ruchu robota. Algorytm FastSLAM polega na wyznaczaniu ścieżki robota na podstawie pomiarów położeń znaczników (charakterystycznych cech środowiska) w kolejnych krokach ruchu robota. W algorytmie FastSLAM ścieżka oraz mapa w kolejnych chwilach 13 t są przybliżane na podstawie serii sterowań robota, położeń znaczników oraz skojarzeń obserwacji ze znacznikami. Probabilistyczny model systemu opisuje łączny rozkład warunkowy a posteriori ścieżki i mapy: p(s1:t , m | z1:t , u1:t , n1:t ), (3.1) gdzie: s1:t = {s1 , ..., st } - ciąg dyskretnych pozycji robota, m = {m1 , ..., mi } - mapa w postaci zbioru znaczników, z1:t = {z1 , ..., zt } - wyniki pomiarów położeń znaczników, u1:t = {u1 , ..., ut } - wartości sterowań robotem w kolejnych chwilach, n1:t = {n1 , ..., nt } - skojarzenia obserwacji ze znacznikami w kolejnch chwilach. 3.2 Filtr Bayesa Nie znając dokładnego stanu systemu w danej chwili t, zwykle modeluje się go za pomocą rozkładu prawdopodobieństwa wykorzystując filtr Bayesa [11], który na podstawie znanego ciągu poprzednich pozycji, sterowań i obserwacji określa rozkład prawdopodobieństwa warunkowego aktualnej pozycji. Wykorzystując regułę Bayesa prawdopodobieństwo znajdowania się robota w danej pozycji można zapisać: p(st , m | z1:t , u1:t , n1:t ) = p(zt | st , m, z1:t−1 , u1:t , n1:t )p(st , m | z1:t−1 , u1:t , n1:t ) p(zt | z1:t−1 , u1:t , n1:t ) (3.2) Bieżąca obserwacja zt zależy od aktualnego stanu robota st , mapy m oraz asocjacji nt . Stosując taką definicję licznik wyrażnia (3.2) zapisuje się jako: p(zt | st , m, z1:t−1 , u1:t , n1:t ) = p(zt | st , m, nt ) 14 (3.3) Wartość mianownika wyrażenia (3.2) nie zależy od poprzednich obserwacji z1:t−1 , sterowań u1:t ani asocjacji gdy nie jest znana pozycja robota ani mapa. Mianownik pełni rolę współczynnika normalizującego. p(zt | z1:t−1 , u1:t , n1:t ) = 1 η (3.4) Stosując twierdzenie o prawdopodobieństwie całkowitym drugi człon licznika (3.2) można zapisać jako: p(st , m | z1:t−1 , u1:t , n1:t ) = Z p(st , m | st−1 , z1:t−1 , u1:t , n1:t )p(st−1 | z1:t−1 , u1:t , n1:t )dst−1 (3.5) Pierwszy wyraz znajdujący się pod całką rozwija się korzystając z twierdzenia o prawdopodobieństwie warunkowym: p(st , m | st−1 , z1:t−1 , u1:t , n1:t ) = p(st | m, st−1 , z1:t−1 , u1:t , n1:t )p(m | st−1 , z1:t−1 , u1:t , n1:t ) (3.6) Rozwiązując zadanie SLAM zakłada się, że stan procesu w chwili t zależy bezpośrednio od poprzedniej pozycji st−1 oraz sterowania ut czyli spełnia warunki łańcucha Markowa [24]. Pozyższe założenie pozwala zapisać: p(st | m, st−1 , z1:t−1 , u1:t , n1:t ) = p(st | st−1 ut ) (3.7) Wzór (3.4) można więc zapisać jako: p(st , m | z1:t−1 , u1:t , n1:t ) = Z p(st | st−1 , ut )p(m | st−1 , z1:t−1 , u1:t , n1:t )p(st−1 | z1:t−1 , u1:t , n1:t )dst−1 (3.8) Co dalej można przedstawić: p(st , m | z1:t−1 , u1:t , n1:t ) = Z p(st | st−1 , ut )p(st−1 , m | z1:t−1 , u1:t , n1:t )dst−1 (3.9) Podstawiając powyższe wyrażenie do wzoru (3.2) uzyskuje się formułę opisującą zadanie SLAM. 15 p(st , m | z1:t , u1:t , n1:t ) = ηp(zt | st , m, nt ) Z p(st | st−1 , ut )p(st−1 , m | z1:t−1 , u1:t−1 , n1:t−1 )dst−1 (3.10) Powyższe wyrażenie nie może być obliczone bezpośrednio, dlatego stosuje się różnego rodzaju aproksymacje. 3.3 Modele systemu Tak jak wspomniano w poprzednim rozdziale rozwiązując zadanie SLAM korzysta się z dwóch matematycznych modeli: modelu ruchu oraz modelu obserwacji. Poniżej opisano modele wykorzystywane w implementowanym algorytmie FastSLAM. 3.3.1 Model ruchu Korzystając z czujników odometrycznych mamy możliwość odczytu współrzędnych pozycji robota w kolejnych dyskretnych chwilach. Stan robota w chwili t oznaczono jako st = [xt , yt , αt ]T (3.11) Zmianę współrzędnych pozycji robota pomiędzy dwoma kolejnymi chwilami t-1 oraz t można zapisać jako złożenie pierwszej rotacji (σrot1 ), translacji σtrans oraz drugiej rotacji σrot2 (rys. 3.1). σrot1 = arctan((yt − yt−1 )/(xt − xt−1 )), q (3.12) σtrans = (xt − xt−1 )2 + (yt − yt−1 )2 , (3.13) σrot2 = αt − αt−1 − σrot1 . (3.14) Zakłada się, że rzeczywiste wartości rotacji i translacji zakłócone są przez addytywny szum pomiarowy o rozkładzie normalnym, oznaczony w poniższych równaniach jako ωrot i ωtrans . ωrot1 ∼ N (0, Q), 16 (3.15) ωtrans ∼ N (0, R), (3.16) σb rot1 = σrot1 + ωrot , (3.17) σb trans = σtrans + ωtrans , (3.18) σb rot2 = σrot2 + ωrot . (3.19) Rzeczywiste współrzędne pozycji robota uzyskuje się poprzez złożenie rotacji σb rot1 , translacji σb trans oraz rotacji σb rot2 . xt = xt−1 + σb trans cos(αt−1 + σb rot1 ), (3.20) yt = yt−1 + σb trans sin(αt−1 + σb rot1 ), (3.21) αt = αt−1 + σb rot1 + σb rot2 . (3.22) Rysunek 3.1: Zmiana pozycji robota podczas jednego kroku algorytmu. Przykładowe zbiory próbek wylosowane w przypadkach różnych wartości ωrot i ωtrans przedstawiono na rysunku 3.2. 17 Rysunek 3.2: Zbiory cząstek przy różnych ustalonych wartościach szumu rotacji oraz translacji. Po lewej przedstawiono przypadek, gdy większą wartość przyjmuje szum towarzyszący rotacji, natomiast po prawej większą wartość ma szum towarzyszący translacji. 3.3.2 Model obserwacji W kolejnych chwilach t obserwacje przeprowadza się względem aktualnej pozycji robota. Dane pobrane z czujnika przedstawione są we współrzędnych biegunowych (r, φ). Rysunek 3.3: Robot obserwujący znacznik pod kątem φ, znajdujący się w odległości r. Oznaczmy funkcję opisującą model obserwacji jako g(st , mnt ). Zakładając, że pozycja robota w chwili t oznaczona jest st = [x, y, α]T , a aktualne współrzędne położenia znacznika mnt oznaczono jako 18 [u,v], funkcję g(st , mnt ) można zapisać w następujący sposób: g(st , mnt ) = [r, φ]T (3.23) q r(st , mnt ) = (x − u)2 + (v − y)2 + ωr φ(st , mnt ) = arctan ((v − y)/(x − u)) − α + ωφ (3.24) (3.25) gdzie ωr oraz ωφ są szumami pomiarowymi. 3.4 Opis algorytmu FastSLAM W algorytmie FastSLAM zakłada się że znamy dokładną ścieżkę robota i na jej podstawie możemy niezależnie określić położenie każdego ze znaczników. Pozwala to na faktoryzację Rao-Blackwella [12] rozkładu prawdopodobieństwa (3.1). p(s1:t , m | z1:t , u1:t , n1:t ) = p(s1:t | z1:t , u1:t , n1:t ) N Y p(mi | s1:t , z1:t , u1:t , n1:t ) (3.26) i=1 Pierwsza część wyrażenia opisuje rozkład prawdopodobieństwa ścieżki, natomiast druga część to iloczyn rozkładów prawdopodobieństwa położeń N znaczników na podstawie obliczonej ścieżki [12]. Estymata ścieżki jest wyznaczana za pomocą filtru cząsteczkowego, zaś estymaty położeń znaczników są obliczane z użyciem rozszerzonych filtrów Kalmana. Pojedyncza cząsteczka z filtru cząsteczkowego jest postaci: [i] [i] [i] [i] [i] [i] St =< s1:t , µ1,t , Σ1,t , ..., µN,t , ΣN,t >, (3.27) gdzie i - indeks cząstki [i] s1:t - estymata ścieżki dla i -tej cząstki [i] µn,t - wartość oczekiwana rozkładu normalnego reprezentującego położenie n-tego znacznika 19 [i] Σn,t - macierz kowariancji rozkładu normalnego reprezentującego położenie n-tego znacznika W każdej kolejnej chwili t zostaje wygenerowany nowy zbiór cząstek na podstawie zbioru z poprzedniej chwili t-1, dla każdej cząstki wyznaczane są współrzędne obserwowanych znaczników, następnie cząstkom przypisywane są wagi i następuje ponowne próbkowanie. Pojedynczą iterację algorytmu powtarzaną dla każdej kolejnej chwili t można podzielić na 4 etapy. Etap I Generacja nowych cząstek Etap II Uaktualnienie estymat położeń znaczników Etap III Obliczanie wag cząstek Etap IV Ponowne próbkowanie Poniżej opisano kolejne kroki algorytmu. 3.4.1 Generacja nowych cząstek Zbiór cząsteczek St w chwili t jest generowany na podstawie zbioru St−1 z chwili t-1, obserwacji zt i sterowań ut . Ponieważ nie można bezpośrednio wygenerować nowych cząstek zgodnych z założonym probabilistycznym modelem p(s1:t , m | z1:t , u1:t , n1:t ), początkowo cząstki są generowane według prostszego rozkładu, w którym nowa pozycja jest zależna od poprzedniej chwili oraz aktualnych sterowań, a w dalszych krokach algorytmu tak uzyskane wyniki są weryfikowane na podstawie aktualnych obserwacji, a cząstkom przypisywane są odpowiednie wagi [12]. Wynikiem pierwszego kroku algorytmu jest tymczasowy zbiór cząstek wygenerowanych zgodnie z probabilistycznym modelem ruchu opisanym zależnością st ∼ p(st | st−1 , ut ). Zakładając, że zbiór cząstek w poprzedniej chwili St−1 jest zgodny z rozkładem s1:t−1 ∼ p(s1:t−1 | z1:t−1 , u1:t−1 ), można stwierdzić, że wygenerowane cząstki są rozłożone zgodnie z rozkładem s1:t ∼ p(s1:t | z1:t−1 , u1:t ). W celu wygenerowania nowych cząstek korzysta się z modelu 20 ruchu opisanego w sekcji 3.3.1. 3.4.2 Uaktualnienie estymat położeń znaczników W drugim kroku algorytmu, na podstawie aktualnych obserwacji korygowane są mapy każdej cząstki. Załóżmy, że robot w danej chwili rozpoznał N znaczników. Pewne z nich znajdują się już na liście cząstki, z kolei inne robot obserwuje po raz pierwszy. Dodatkowo załóżmy, że znane są skojarzenia znaczników czyli można stwierdzić, który aktualnie obserwowany znacznik należy do listy znaczników cząstki, który jest widoczny dla robota po raz pierwszy, a które ze znaczników z listy nie są wcale widoczne w danej chwili. Dane tych ostatnich obiektów są pozostawiane bez zmian. Nowe obiekty zostają dodane do listy znaczników z nowymi współrzędnymi, zależnymi od położenia k − tej cząstki z filtru cząsteczkowego oraz danych z lasera. Natomiast dla każdego znanego wcześniej i obserwowanego aktualnie znacznika, jego współrzędne są korygowane, biorąc pod uwagę dane z poprzednich iteracji algorytmu oraz wyznaczoną dla danej cząstki pozycję znacznika. Zależność tę przedstawia poniższe wyrażenie: p(mnt | s1:t , z1:t , u1:t , n1:t ) = ηp(zt | mnt , st , nt )p(mnt | s1:t−1 , z1:t−1 , u1:t−1 , n1:t−1 ) (3.28) Aby skorzystać z algorytmu filtru Kalmana nieliniowy model obserwacji g(st , mnt ) jest przybliżany pierwszym wyrazem rozwinięcia funkcji w szereg Taylora. Zakłada się, że zakłócenia pomiarowe mają postać szumu o rozkładzie normalnym i macierzy kowariancji Rt . [k] zbt = g(st , µnt ,t−1 ) Gmnt = 5mnt g(st , mnt ) |s =s[k] ;m t t (3.29) [k] nt =µnt ,t−1 [k] [k] g(st , µnt ) = zbt + Gm (mnt − µnt ,t−1 ) 21 (3.30) (3.31) Pierwszy czynnik wyrażenia (3.26) jest więc postaci: [k] p(zt | mnt , st , nt ) ∼ N (zt ; zbt + Gm (mnt − µnt ,t−1 )), Rt ) (3.32) W celu obliczenia nowej wartości oczekiwanej oraz kowariancji dla pojedynczego znacznika należy skorzystać ze standardowych równań wykorzystywanych w algorytmie filtru Kalmana. [k] zbt = g(st , µnt ,t−1 ) (3.33) Gmnt = 5mnt g(st , mnt ) |s =s[k] ;m t [k] nt =µnt ,t−1 t (3.34) [k] (3.35) −1 Kt = Σnt ,t−1 GTmnt Zn,t (3.36) Znt = Gmnt Σnt ,t−1 GTmnt + Rt [k] [k] [k] µnt ,t = µnt ,t−1 + Kt (zt − zbt ) [k] [k] Σnt ,t = (I − Kt Gmnt )Σnt ,t−1 (3.37) (3.38) Oznaczając pozcyję robota st = [xt , yt , αt ] oraz położenie znacznika zt = [ut , vt ], jakobian Gmnt oblicza się ze wzorów: Gmnt = ut√ −xt q vt −yt q vt√ −yt q ut −xt q gdzie q q = (ut − xt )2 + (vt − yt )2 3.4.3 (3.39) Obliczenie wag cząstek Cząstki w pierwszym kroku algorytmu wygenerowane zostały zgodnie z proponowanym rozkładem prawdopodobieństwa st ∼ p(s1:t | z1:t−1 , u1:t , n1:t−1 ), a więc nie spełniają założenia zadania SLAM, czyli nie są zgodne z docelowym rozkładem prawdopodobieństwa st ∼ p(s1:t | z1:t , u1:t , n1:t ). w następnym kroku należy wyznaczyć wagi wygenerowanych cząstek. w algorytmie FastSLAM waga każdej cząstki wyznaczona zostaje jako stosunek oczekiwanej pozycji robota do pozycji wyznaczonej w danej chwili t. [k] [k] wt = p(s1:t | z1:t , u1:t , n1:t ) [k] p(s1:t | z1:t−1 , u1:t , n1:t−1 ) 22 (3.40) Korzystając z reguły Bayesa licznik wyrażenia (3.37) można zapisać: [k] [k] wt = [k] [k] p(zt | s1:t , z1:t−1 , u1:t , n1:t )p(s1:t | z1:t−1 , u1:t , n1:t ) [k] p(s1:t | z1:t−1 , u1:t , n1:t−1 ) (3.41) W powyższym wyrażeniu pominięta została stała normalizująca, gdyż wagi będą normalizowane w kolejnym kroku algorytmu. Ostatni wyraz wyrażenia (3.38) nie zależy od obserwacji z ostatniej chwili t więc nie zależy również od asocjacji z ostatniej chwili dlatego można go bezpiecznie pominąć. [k] [k] p(zt | s1:t , z1:t−1 , u1:t , n1:t )p(s1:t | z1:t−1 , u1:t , n1:t−1 ) [k] wt = [k] p(s1:t | z1:t−1 , u1:t , n1:t−1 ) [k] = p(zt | s1:t , z1:t−1 , u1:t , n1:t ) (3.42) Powyższe prawdopodobieństwo oblicza się jako innowację lub różnicę pomiędzy rzeczywistymi współrzędnymi obserwacji z aktualnej chwili t, a przewidywanych współrzędnych obserwacji. Innowacje w EKF są rozłożone według rozkładu normalnego o wartości oczekiwanej 0 i kowariancji Znt ,t . Prawdopodobieństwo obserwacji zt jest równe prawdopodobieństwu innowacji zt − zbt wygenerowanemu przez ten rozkład [2]. Obliczając wagi każdej z cząstek korzysta się ze wzoru: [k] q wt = (1/ | 2πZnt ,t |) exp{−0.5(zt − zbnt ,t )T [Znt ,t ]−1 (zt − zbnt ,t )} (3.43) 3.4.4 Ponowne próbkowanie W ostatnim kroku algorytmu, wykonywane jest ponowne próbkowanie. Po obliczeniu wag wszystkich cząstek, z wygenerowanego tymczasowego zbioru cząstek wylosowany zostaje nowy zbiór ustalając prawdopodobieństwa wylosowania każdej cząstki wprost proporcjonalne do jej wagi obliczonej w poprzednim kroku. 23 3.4.5 Wykrywanie znaczników W przedstawionej realizacji algorytmu korzysta się z naturalnych znaczników w postaci rogów ścian lub innch obiektów. Proces wykrywania znaczników składa się z dwóch etapów. w pierwszym etapie, danymi wejściowymi jest zbiór punktów z dalmierza laserowego P1 , P2 , P3 ...Pi , z których każdy jest postaci Pi = [xi , yi ] . Na początku przeprowadza się segmentację danych wejściowych podczas której wyznaczane są zbiory punktów tworzące segmenty. Dwa punkty Pi i Pj należą do jednego segmentu jeśli ich wzajemna odległość nie jest większa od ustalonego progu Lmax czyli spełniają warunek: q (P1x − P2x )2 + (P1y − P2y )2 < Lmax (3.44) Efekt działania algorytmu segmentacji przedstawiono na rysunku 3.3. Rysunek 3.4: Zbiór punktów pobranych z dalmierza, podzielony na dwa segmenty. Kolorem czerwonym zaznaczono wyznaczone segmenty. 24 Mając zbiory punktów tworzące segmenty, następnie dla każdego segmentu, który można zapisać w postaci {P1 , P2 , P3 , ..., Pi } stosuje się algorytm Rescursive Line Fitting [21] składający się z 3 kroków, które można opisać w następujący sposób: Krok I. Wyznacz równanie prostej przechodzącej przez 2 skrajne punkty segmentu P1 i Pi . Wyznaczając równanie prostej w postaci y = ax + b wykorzystuje się wzory: y1 − yi , (3.45) a= x1 − xi y1 − yi b = y1 − x1 . (3.46) x1 − xi Krok II. Wyznacz punkt ze zbioru, którego odległość od prostej jest największa. w tym celu dla każdego punktu P = [x, y] ze zbioru oblicza się odległość według wzoru: |ax − y + b| d= √ 2 , (3.47) a + b2 a następnie wybrany zostaje punkt Pdmax , dla którego obliczona wartość jest największa. Krok III. Jeśli wyznaczona odległość d jest większa od zadanego progu dmax , podziel zbiór punktów tworzących segment na 2 podzbiory {P1 , ..., Pdmax } oraz {Pdmax , ..., Pi }, a następnie dla każdego z nich powtórz procedurę. Jeśli odległość d jest mniejsza od progu dmax , uznaje się że badany zbiór punktów tworzy prostą. Algorytm korzysta z trzech parametrów, których wartości wpływają na efektywność jego działania. Są to: • minimalna liczba punktów, jakie musi zawierać zbiór tworzący prostą, • maksymalna odległość pomiędzy skrajnymi punktami zbioru, • maksymalna odległość najbardziej oddalonego punktu ze zbioru od prostej poprowadzonej przez dwa skrajne punkty. Efektem działania algorytmu są zbiory uporządkowanych punktów. Każdy ze zbiorów zawiera punkty należące do innej prostej 25 Schemat działania algorytmu przedstawiono na rysunku 3.5. Na rysunku 3.5a widać prostą poprowadzoną przez skrajne punkty segmentu, oraz punkt najbardziej oddalony od tej prostej. Na rysunku 3.5b, zbiór punktów został podzielony na dwa podzbiory w punkcie najbardziej oddalonym od prostej. Następnie dla każdego podzbioru procedura jest powtarzana. Na rysunku 3.5c, widać, że jeden z podzbiorów uznany został za prosty odcinek, zaś drugi jest ponownie dzielony w identyczny sposób, aż na rysunku 3.5d, wszystkie zbiory punktów tworzą odcinki. Mając zbiory punktów tworzących linie, w drugim etapie następuje aproksymacja par sąsiadujących prostych. Dwie kolejne proste składające się odpowiednio z punktów {Pi , ..., Pj } oraz {Pk , ..., Pl } uznano za sąsiadujące, jeśli posiadają jeden punkt wspólny to znaczy Pj = Pk , lub gdy różnica indeksów dwóch punktów skrajnych, z których każdy należy do innej prostej nie jest większa niż 1 czyli |k − j| ¬ 1. w celu aproksymacji prostej dokonuje się podziału zbioru punktów należacych do prostej na 2 możliwie równoliczne podzbiory. Prostą składającą się z punktów {Pi , ..., Pj } można podzielić na dwa podzbiory {Pi , ..., Pn } oraz {Pn+1 ...Pj }, gdzie j − n = m − n − 1. Następnie oblicza się współrzędne punktów średnich w każdym z podzbiorów. w przypadku zbioru składającego się z punktów {Pi , ..., Pn } korzysta się ze wzorów: n P x= m=i n−i n P y= xm m=i (3.48) ym (3.49) n−i Na podstawie współrzędnych punktów z obu równań wyznacza się równanie prostej przechodzącej przez oba punkty. Służy do tego wzór (3.42) oraz (3.43). Mając wyznaczone równania sąsiadujących prostych należy wyznaczyć współrzędne punktu przecięcia jeśli taki punkt istnieje. Jeśli odległość obliczonych współrzędnych od punktów krańcowych należących do obu prostych jest mniejsza od usta26 Rysunek 3.5: Przykład działania algorytmu wyznaczającego proste na podstawie zbioru punktów tworzących segment. Kolorem zielonym zaznaczono proste poprowadzone przez dwa skrajne punkty badanego zbioru, kolorem czerwonym odleglości do najbardziej oddalonego punktu od prostej, kolorem niebieskim zaznaczono znalezione zbiory punktów tworzące proste odcinki. lonego progu, uznaje się że znaleziono znacznik. 3.4.6 Asocjacja danych W poprzednich sekcjach założonoo, że skojarzenia znaczników są znane. Niestety najczęściej nie jest to prawdą i należy wykorzystać jedną z metod, które takie skojarzenia wyznaczają. Niezwykle 27 ważne jest dobranie odpowiedniego algorytmu, gdyż każde błędne skojarzenie znaczników może wprowadzać błędy do modelu systemu i uniemożliwiać zbudowanie dokładnej mapy. Algorytm FastSLAM dzięki możliwości śledzenia wielu hipotez w pewnym stopniu zapobiega efektowi degeneracji modelu, jaki może mieć miejsce w przypadku rozszerzonego filtru Kalmana. Jednak mimo to błędne skojarzenia znaczników mogą stanowić znaczną przeszkodę w budowaniu mapy oraz samolokalizacji. Mając wyznaczone współrzędne obserwacji z aktualnej chwili t, konieczne jest ich skojarzenie z obiektami wyznaczonymi w poprzednich iteracjach algorytmu. W zaproponowanej realizacji algorytmu wykorzystuje się opisaną poniżej metodę asocjacji znaczników. Na wejściu algorytmu wyznaczającego skojarzenia danych są 2 tymczasowe zbiory punktów. Pierwszy zbiór L1 , ..., Li to położenia znaczników rozpoznanych do aktualnej chwili, natomiast drugi O1 , ..., Oj to obserwacje z aktualnej chwili. Kolejne kroki algorytmu opisano poniżej. Krok 1. Oblicz odległości pomiędzy wszystkimi parami punktów, z których jeden należy do zbioru obserwacji, a drugi do zbioru znacznikow według wzoru: q dkl = (Lkx − Olx )2 + (Lky − Oly )2 (3.50) Krok 2. Wyznacz minimalną wartość obliczoną w kroku pierwszym, d = argmax(dkl ). Jeśli d < dmax , uznaj że reszta obserwacji to nieznane do tej pory znaczniki, dodaj pozostałe obserwacje do zbioru znaczników cząstki i zakończyć algorytm. W przeciwnym przypadku przejdź do kroku 3. Krok 3. Jeśli odległość z kroku 2 jest mniejsza od dmax , należy zapamiętać skojarzenie obserwacji ze znacznikiem, a następnie usunąć oba obiekty z odpowiednich zbiorów Krok 4. Wróć do kroku 1. Przykład działania algorytmu przedstawiono na rysunku 3.6. Ry28 sunek 3.6a przedstawia znane znaczniki (kolor czarny) oraz obserwacje (kolor czerwony). Na rysunku 3.6b zaznaczono wyznaczone odległości pomiędzy każdą oserwacją a najbliższym znacznikiem. w dwóch przypadkach odległości są poniżej ustalonego progu, natomiast trzecia odległość jest większa od tego progu i nie można skojarzyć tej obserwacji z żadnym znanym znacznikiem. Na rysunku 3.6c widać znalezione skojarzenia pomiędzy obserwacjami oraz znacznikami. Rysunek 3.6: Przykład działania algorytmu wyznaczającego asocjacje pomiędzy znanymi znacznikami oraz obserwacjami. Czarne punkty to znane znaczniki, czerwone punkty to obserwacje, na niebiesko zaznaczono odległości pomiędzy obserwacjami, a najbliższymi znacznikami. Pary znacznik - obserwacja połączone w kole oznaczają asocjacje. 29 Poniżej przedstawiono pseudokod algorytmu FastSLAM: F astSLAM (St−1 , zt , ut , Rt ) St = Stmp = ∅ for k=1 to K do [k] [k] [k] [k] [k] pobierz k-tą cząstkę < st−1 , µ1,t−1 , Σ1,t−1 , ..., µN,t−1 , ΣN,t−1 > ze zbioru St−1 [k] [k] [k] losuj st ∼ p(st | st−1 , ut ) [k] for n=1 to Nt−1 do Gmnt ,t = 5mnt g(st , mnt ) |st =s[k] ;mn t [k] t =µnt ,t−1 [k] [k] zbt = g(st , µnt ,t−1 ) [k] Znt ,t = Gmnt Σnt ,t−1 GTmnt + Rt q [k] pnt = (1/ | 2πZnt ,t |) exp{−0.5(zt − zbnt ,t )T Zn−1 (zt − zbnt ,t ) t ,t end for [k] p = p0 [k] Nt−1 +1,t b t = argmaxpknt ,t n [k] b t = Nt−1 + 1 then if n [k] [k] Nt = Nt−1 + 1 [k] [k] µbn ,t = g −1 (st , µbnt ,t ) t [k] Σbn ,t = (GTm ,bn R−1 Gmn ,bnt )−1 n t t else [k] [k] Nt = Nt−1 [k] Kbnt ,t = Σbn ,t−1 GTmnt Zn−1 t ,t t [k] [k] µbn ,t = µbn ,t−1 + Kbnt (zt − zbt ) t t [k] [k] Σnt ,t = (I − Kbnt Gm )Σnt ,t−1 end if for n=1 to n=N do b t then if n 6= n [k] bnt ,t−1 [k] µmn ,t = µmn ,t−1 [k] [k] Σmn ,t = Σmn ,t−1 end if end for [k] [k] wt = pbn ,t t [k] [k] [k] [k] [k] dodaj (st−1 , µ1,t , Σ1,t , ..., µN,t , ΣN,t , wt ) do zbioru tymczasowego Stmp end for for k=1 to K do losuj czastke z Stmp ustalajac prawdopodobienstwo proporcjonalne do jej wagi dodaj czastke do zbioru St end for return St 30 Rozdział 4 Środowisko badawcze 4.1 Robot Algorytm FastSLAM został zrealizowany dla robota laboratoryjnego o nazwie Elektron widocznego na rysunku 4.1. Poniżej znajduje się opis wyposażenia robota. Rysunek 4.1: Robot mobilny Elektron. 31 4.1.1 Czujniki odometryczne Baza jezdna robota składa się z 6 kół napędzanych dwoma silnikami. Budowa bazy jezdnej zbliżona jest do budowy bazy jezdnej czołgów. Robot dokonuje skrętów poprzez wyłączenie jednego z silników napędzających koła znajdujące się po jednej stronie. Można ją traktować jako różnicową bazę jezdną [18]. Robot jest wyposażony w enkodery optyczne pozwalające określić kąt obrotu kół w 2 osiach, co z kolei pozwala obliczyć pokonaną drogę, zmianę orientacji bazy i nową pozycję robota. Rysunek 4.2: Zmiana pozycji robota z napędem różnicowym. W celu wyznaczenia nowej pozycji robota należy obliczyć przebytą przez robota drogę oraz zmianę kąta orientacji bazy. Dla różnicowej bazy jezdnej służą do tego wzory: ∆l = (ll + lp )/2 (4.1) ∆α = (ll + lp )/d, (4.2) gdzie: ll - droga przebyta przez koła na lewej strony lp - droga przebyta przez koła na prawej strony 32 Znając powyższe wartości, możemy w prosty sposób obliczyć zmianę współrzędnch robota w chwili t względem chwili t-1 korzystając z zależności: 4.1.2 ∆xt = ∆l cos αt−1 (4.3) ∆yt = ∆l sin αt−1 (4.4) Dalmierz laserowy Robot jest wyposażony również w dalmierz laserowy Sick LMS100. Czujniki laserowe w ostatnich latach zyskały dużą popularność dzięki odporności na zakłócenia zewnętrzne oraz znacznie większej niż inne urządzenia dokładności, w przypadku pomiarów na dużą odległość. Sick LMS100 to czujnik typu 2D, został stworzony głównie do pracy w pomieszczeniach zamkniętych. Cechuje się dużą dokładnością i szybkością działania, błąd statystyczny określony w specyfikacji wynosi 12 milimetrów, a maksymalna częstotliwość odświeżania to 50 Hz [22]. Rysunek 4.3: Dalmierz laserowy SICK LMS 100. Czujnik posiada możliwość ustawień: • zasięg - dowolna wartość z zakresu [0.5-20m] • maksymalnego kąta widzenia - dowolna wartość z zakresu [90◦ 270◦ ] 33 • rozdzielczości: 0.25◦ lub 0.5◦ Działanie dalmierza polega na wysłaniu przez nadajnik wiązki laserowej, która po napotkaniu obiektu odbija się od niego i po pewnym czasie zostaje przechwycona przez lustro obrotowe. Znajdujący się nad lustrem odbiornik po wykryciu powracającej wiązki oblicza czas przelotu drogi, a następnie na jego podstawie obliczona zostaje droga przebyta przez wiązkę czyli podwojona odległość obiektu od czujnika. Bezpośrednio z dalmierza odczytać można kąt pod jakim widziany jest obiekt oraz jego odległość od lasera. Są to wartości zapisane we współrzędnych biegunowych. 4.2 Platforma Player/Stage Podczas implementacji algorytmu wykorzystane zostało oprogramowanie Player/Stage [27]. Jest to projekt otwarty, dystrybuowany na zasadach licencji GNU. Udostępnia narzędzia ułatwiające tworzenie aplikacji do sterowania robotem. Składa się z dwóch modułów: Player oraz Stage. 4.2.1 Player Serwer zarządzający robotem, który dostarcza przejrzysty interfejs dla czujników robota i efektorów. Aplikacje klienckie sterujące ruchem robota do komunikacji z Player’em korzystają z protokołu TCP/IP. Player został stworzony jako program niezależny od platformy uruchomieniowej oraz języka programowania. Podczas implementacji algorytmu wykorzystane zostaną pewne już istniejące interfejsy oraz sterowniki. • Za komunikację z dalmierzem laserowym odpowiedzialny jest interfejs laser. Obsługę interfejsu zapewnia sterownik sicklms100. 34 Rysunek 4.4: Architektura serwera Player. Sterownik udostępnia również możliwość ustawień parametrów czujnika, oraz pobieranie aktualnych danych. • W celu komunikacji z czujnikami odometrycznymi robota laboratoryjnego Elektron, wykorzystany został sterownik ProtonekDriver zaimplementowany przez członków Zespołu Robotyki w Instytucie Automatyki i Informatyki Stosowanej. Sterownik wykorzystuje interfejs position2d umożliwiający pobieranie danych z czujnika. • W trakcie prowadzenia badań wygodne sterowanie robotem rzeczywistym umożliwia dostępny w laboratorium joystick, z którym komunikacja jest możliwa poprzez interfejs joystick wykorzystywany przez sterownik linuxjoystick. 35 4.2.2 Stage Symulator wielu robotów, czujników i obiektów w dwuwymiarowym środowisku. Moduł stworzony w celu wsparcia badań nad wieloagentowymi autonomicznymi systemami. Udostępnia modele wielu urządzeń, do których dostęp za pomocą serwera Player jest identyczny jak w przypadku urządzeń rzeczywistych. Rysunek 4.5: Symulator Stage. W niniejszej pracy moduł Stage był wykorzystywany głównie w początkowych etapach implementacji algorytmu, w późniejszych etapach prace skupione były na przystosowaniu algorytmu do działania na rzeczywistym roboci. 36 4.2.3 Biblioteka Miarn W większości algorytmów mających na celu rozwiązanie zadania SLAM wykorzystuje się charakterystyczne obiekty występujące w środowisku. Biblioteka Miarn dedykowana do współpracy ze środowiskiem Player/Stage udostępnia zestaw funkcji wykorzystywanych do identyfikacji obiektów na podstawie danych z czujników laserowych w systemach czasu rzeczywistego [11]. Za pomocą biblioteki możliwe jest wykrycie kształtów jak linie, łuki, okręgi lub nogi poruszających się ludzi. W zaproponowanej implementacji algorytmu FastSLAM wykorzystywane będą funkcje realizujące algorytm IAV służący do wyznaczania linii na podstawie danych z dalmierza laserowego. Zasadę działania algorytmu IAV opisano w rozdziale 3. 37 38 Rozdział 5 Implementacja algorytmu FastSLAM 5.1 Struktura programu Algorytm FastSLAM został zaimplementowany w języku C++ jako sterownik do środowiska Player/Stage. Rozwiązanie takie jest elastyczne i umożliwia wykorzystanie sterownika podczas przyszłych badań dotyczących sterowania robotem mobilnym. Sterownik korzysta z pliku konfiguracyjnego przechowującego parametry algorytmu. Pozwala to w prosty sposób zmienić konfigurację jeśli zaistnieje taka potrzeba. Biblioteka sterownika zawiera 4 klasy: główną klasę sterownika FastSlamDriver, klasę Fastslam odpowiedzialną za logikę algorytmu, klasę Ekf udostępniającą funkcje umożliwiające wykorzystanie filtru Kalmana, oraz klasę pomocniczą My Math udostępniającą funkcje matematyczne. Ponadto zdefiniowane zostały następujące struktury danych, które przechowują informacje o stanie systemu: • Vector3d, • Point, • Particle, Pierwsza z wymienionych wyżej struktur przechowuje dane doty39 czące pozycji robota, druga wykorzystywana jest do przechowywania współrzędnych punktów zarówno w środowisku rzeczywistym jak i na budowanej mapie, zaś trzecia jest reprezentacją pojedynczej cząsteczki przechowywanej w filtrze cząsteczkowym. Poniżej znajduje się diagram zaimplementowanych na potrzeby pracy klas i struktur. Rysunek 5.1: Ogólny diagram zaimplementowanych klas i struktur. Dodatkowo sterownik korzysta z funkcji biblioteki Miarn, oraz klasy MAP dostarczonej wraz ze środowiskiem Player/Stage. 5.2 5.2.1 Opis klas Klasa FastSlamDriver Główna klasa sterownika FastSlamDriver jest klasą pochodną klasy ThreadedDriver znajdującej się bibliotece pakietu Player. Klasa udostępnia standardowe funkcje umożliwiające komunikację pomiędzy serwerem Player a sterownikiem. Ponadto klasa podstawowa definiuje typowe metody dla sterowników serwera Player, które 40 implementuje klasa FastSlamDriver. Rysunek 5.2: Klasa FastSlamDriver. Przy inicjalizacji sterownika, w konstruktorze z pliku konfiguracyjnego pobierane są parametry oraz informacje na temat dostępnych dla robota interfejsów urządzeń. w przypadku braku zdefiniowanych wartości dla parametrów przypisywane są wartości domyślne zapisane w kodzie źródłowym sterownika. Następnie jest wywoływana funkcja MainSetup() odpowiedzialna za prawidłową inicjalizację urządzeń przed pierwszym pobraniem danych. Następuje w niej otwarcie portów do komunikacji z każdym urządzeniem a także uruchomienie głównego wątku. Główny wątek sterownika odpowiedzialny za „zachowanie” robota znajduje się w funkcji Main(), w której jest wykonywana pętla. Tutaj następuje pobranie danych z czujników robota, dokonywane są niezbędne obliczenia oraz kolejne kroki algorytmu FastSLAM, a 41 na podstawie otrzymanych wyników generowana jest mapa odpowiadająca cząstce o największej wadze. Diagram sekwencji przedstawiający współpracę klasy FastSlamDriver z klasami FastSlam oraz MAP widoczny jest na rysunku 5.3 Rysunek 5.3: Diagram sekwencji przedstawiający współpracę klasy FastSlamDriver z innymi klasami. Jednocześnie z głównym wątkiem, do sterownika mogą trafiać żądania połączenia lub rozłączenia z udostępnianymi interfejsami. Za obsługę tego typu żądań odpowiedzialne są funkcje Subscribe() oraz Unsubscribe(). W głównej pętli okresowo następuje pobranie oczekujących w kolejce wiadomości dostarczonych z czujników. Obsługę nadchodzących wiadomości realizuje funkcja ProcessMessage(), w której następuje rozróżnienie typu wiadomości, a następnie wykonywane są odpowiednie operacje. Po zakończeniu pracy sterownika wywoływany jest destruktor, gdzie następuje zwolnienie wykorzystywanych zasobów. Schemat blokowy życia sterownika pokazano na rysunku 5.4. 42 Rysunek 5.4: Działanie sterownika. W klasie FastSlamDriver zaimplementowane zostały metody odpowiedzialne za budowę mapy znaczników oraz mapy w postaci siatki zajętości odpowiednio buildLandmarksMap(Particle p) oraz buildMap(Particle p). Obie metody wywoływane są po zakończeniu każdej iteracji algorytmu dla cząstki z filtru cząsteczkowego o największej wadze. w celu optymalizacji pracy algorytmu w przypadku siatki zajętości, aktualizowana jest jedynie część mapy znajdująca się w zasięgu widzenia dalmierza laserowego. Mapy przechowywane są w pamieci jako obiekty klasy MAP. Podczas aktualizacji mapy w postaci siatki zajętości dla każdego kąta pod jakim czujnik dokonuje skanu pobrana zostaje odległość od widzianego obiektu oraz wyznaczone zostaje rzeczywiste położenie tego obiektu. Po przeskalowaniu otrzymanych danych do 43 rozmiarów umożliwiających ich naniesienie na mapę, wszystkie piksele znajdujące się pomiędzy robotem a wyznaczonymi punktami zostają uznane za puste, natomiast w miejscu wystąpienia obiektu piksele uznane zostają za zajęte. w przypadku gdy odległości pobrane z dalmierza są równe zasięgowi widzenia dalmierza, mapa dla tych danych nie jest uaktualniana. Powodem są sytuacje, gdy dalmierz wysyła wiązkę, ale pomimo istnienia obiektów, ginie ona i nie wraca do dalmierza. Takie sytuacje mogłyby zamazywać mapę i powodować, że jest mniej czytelna. Przykład mapy zbudowanej dla nieruchomego robota przedstawiono na rysunku 4.6. Rysunek 5.5: Przykładowa mapa zbudowana przez robota. Mapa odpowiada fragmentowi środowiska przedstawionego na rysunku 4.5. 5.2.2 Klasa Fastslam Klasa Fastslam zawiera główną logikę sterownika, udostępnia funkcje realizujące kolejne kroki implementowanego algorytmu. Dostępne publicznie funkcje to: 44 Rysunek 5.6: Struktura klasy Fastslam. • initialize(), • doFastSlam(), • getBestParticle(). Pierwsza z wymienionych funkcji wywoływana jednokrotnie w czasie życia sterownika, zaraz po jego starcie, inicjalizuje niezbędne parametry dla prawidłowej pracy sterownika. Funkcja doFastSlam() jest wywoływana jednokrotnie w każdej iteracji głównej pętli sterownika i jest odpowiedzialna za wykonanie 4 kroków zaimplementowanego algorytmu. Po wykonaniu powyższej funkcji, zawsze następuje wywołanie funkcji getBestParticle(), która zwraca cząstkę o największej wadze. Dla tej cząstki generowane są mapy. Wewnątrz klasy Fastslam zaimplementowane zostały funkcje prywatne realizujące kolejne kroki algorytmu: 45 • Particle getNewParticle(Vector3d &prevPose) - funkcja jest wywoływana dla każdej cząstki z filtru cząsteczkowego. Na podstawie poprzedniej pozycji i aktualnych sterowań generuje nową pozycję robota zgodnie z modelem systemu opisanym w sekcji 3.3.1. Funkcja wykorzystuje metodę gauss() z klasy My Math. • void generateParticles() - funkcja służy do generowania nowego, tymczasowego zbioru cząstek, na podstawie zbioru cząstek z poprzedniej iteracji algorytmu. Dla każdej cząstki ze zbioru wywoływana jest funkcja getNewParticle(). • std::vectorhP ointi getLandmarks(Vector3d pose) – pobiera listę obserwacji na podstawie danych pochodzących z lasera i oblicza ich współrzędne względem przekazanej w parametrze pozycji robota. Wewnątrz funkcji wykorzytywana jest biblioteka Miarn. Wywoływane są funkcje Segmentation(), która jest odpowiedzialna za wyznaczanie segmentów sąsiadujących punktów na podstawie danych z dalmierza oraz funkcja recursiveLineFitting(), która wyznacza zbiory punktów tworzące linie proste. Opis działania algorytmu znajduje się w rozdziale 3. • int getNearestObservationIndex(std::vectorhEkf i &ekfs, Point &o) - zwraca indeks skojarzonego znacznika, w przypadku braku skojarzenia, zwracana jest wartość -1. Funkcja jest wykorzystywana na etapie asocjacji obserwowanych obiektów z rozpoznanymi już znacznikami. • void updateLandmarks() - na podstawie odczytów dalmierza uaktualnia dane dotyczące znaczników na liście znaczników każdej cząstki, lub dodaje do tej listy nowe obiekty. • void calculateWeights() - oblicza wagi cząstek według algorytmu opisanego w rozdziale trzecim. • void resample() - wykonuje ponowne próbkowanie według algorytmu opisanego w rozdziale trzecim. 46 5.2.3 Klasa Ekf Dodatkowo zaimplementowana została klasa Ekf przechowująca dane na temat pojedynczego znacznika oraz udostępniająca metody umożliwiające tworzenie i uaktualnianie tych danych. Rysunek 5.7: Klasa EKF. Główne funkcje publicznie dostępne to: • Ekf(Vector3d pose, Point observation) - tworzy nowy obiekt klasy • updateObs(Vector3d pose, Point observation) - dokonuje aktualizacji danych filtru zgodnie ze standardowymi równaniami algorytmu filtru Kalmana opisanymi w rozdziale trzecim Obie funkcje jako argumenty przyjmują aktualną pozycję opakowaną w obiekt strukturalny typu Vector3d oraz położenie obserwowanego obiektu jako obiekt Point. Położenie obiektu wyznaczone jest względem pozycji robota. Dodatkowo klasa Ekf implementuje prywatne metody pomocnicze wykorzystywane wewnątrz wymienionych wyżej funkcji. 47 5.2.4 Klasa My Math Klasa My Math to klasa pomocnicza udostępniająca zestaw funkcji wykonujących obliczenia matematyczne wykorzystywane przez pozostałe klasy. Rysunek 5.8: Klasa My Math. • private float draw() - funkcja pomocnicza, zwraca liczbę pseudolosową zgodnie z rozkładem jednostajnym zakresu [0,1] • public float gauss(float mean, float cov) - zwraca liczbę pseudolosową zgodnie z rozkładem Gaussa. Parametrami przekazywanymi do metody jest wartość oczekiwana oraz odchylenie standardowe rozkładu. Funkcja korzysta z algorytmu Boxa-Mullera [28]. • public float radToDeg(float radians) - zwraca wartość kąta w stopniach, jako argument funkcja przyjmuje wartość kąta podaną w radianach. • public float degToRad(float degrees) - zwraca wartość kąta w radianach, jako argument funkcja przyjmuje wartość kąta podaną w stopniach. • public matrixhf loati invertMatrix(matrixhf loati &input) - zwraca macierz będącą odwrotnością podanej macierzy • public matrixhf loati sqrtMatrix(matrixhf loati &input) - zwraca macierz będącą pierwiastkiem podanej macierzy 48 • public float countDistance(Point p1,Point p2) - oblicza odległość euklidesową pomiędzy punktami p1 oraz p2. 49 50 Rozdział 6 Badania symulacyjne i eksperymentalne W poniższym rozdziale zostały opisane badania, które miały na celu zweryfikowanie działania zaimplementowanego algorytmu. Eksperymenty wykonywane były przy założeniu statyczności środowiska, w którym poruszał się robot. W przeciwnym przypadku konieczne byłoby zaimplementowanie dodatkowego mechanizmu, pozwalającego odfiltrować elementy dynamiczne, tak aby robot nie uznał ich za znaczniki. Uznanie obiektów takich jak ludzkie nogi za znaczniki miałoby zgubne skutki dla pracy całości zaimplementowanego algorytmu FastSLAM. Podczas kolejnych skanów tego samego obszaru, wspomnianych powyżej obiektów prawdopodobnie nie byłoby już w tym samym miejscu, co wprowadzałoby niezgodność odebranych z czujników danych w stosunku do przechowywanych danych na temat środowiska. Wszystkie badania przeprowadzone zostały na komputerze PC wyposażonym w procesor Intel Core 2 Duo 2.20GHz oraz pamięć RAM 3GB. Testy wykonane były przy różnych konfiguracjach parametrów wejściowych algorytmu. W trakcie weryfikacji działania algorytmu szczególną uwagę zwrócono na wyznaczone przez algorytm pozycje znaczników oraz ścieżkę robota w układzie odniesienia stworzonym przez obserwowane 51 znaczniki. 6.1 Badania symulacyjne Pierwsze testy zaimplementowanego algorytmu przeprowadzano wykorzystując symulator środowiska robotów Stage. Działanie dostępnego w symulatorze modelu robota i czujników zbliżone jest do działania robota rzeczywistego co umożliwia ocenę działania algorytmu. Podczas badań na symulatorze do odczytów danych z czujników wykorzystano te same sterowniki oraz interfejsy, jakie później używano do komunikacji z robotem rzeczywistym. Symulator Stage co prawda symuluje szumy jakie towarzyszą odczytom danych o środowisku, jednak w przypadku symulatora szumy pomiarowe czujników odometrycznych są dużo mniejsze niż te dla rzeczywistego robota Elektron. W celu zbliżenia działania symulatora do działania rzeczywistego robota zasymulowano dodatkowe zakłócenia. Doświadczenia przeprowadzone zostały wykorzystując stworzoną na potrzeby symulacji mapę odzwierciedlającą rzeczywiste środowisko korytarzy budynku Wydziału Elektroniki i Technik Informacyjnych, w którym będzie poruszał się robot mobilny. Mapa jest przedstawiona na rysunku 6.1. 52 Rysunek 6.1: Mapa środowisko wykorzystanego podczas testów algorytmu na symulatorze Stage. 6.1.1 Liczba cząstek w filtrze cząsteczkowym Dobór odpowiedniej liczby cząsteczek może mieć znaczący wpływ na działanie algorytmu. Większa liczba cząstek w filtrze cząsteczkowym powoduje śledzonie większej liczby hipotez. Jednak, z drugiej strony, duża liczba cząstek wydłuża czas pojedynczej iteracji i może uniemożliwiać efektywną pracę robota w czasie rzeczywistym. Dla różnych liczby cząsteczek przbliżone czasy pojedynczej iteracji algorytmu przedstawiono w tabeli 6.1. Liczba cząsteczek Czas pojedynczej iteracji 50 250ms 100 300ms 200 500ms 500 700ms 1000 1500ms Tabela 6.1: Czasy wykonania pojedynczej iteracji algorytmu dla różnych liczb cząstek 53 W przypadku systemów czasu rzeczywistego, pojedyncza iteracja nie może trwać zbyt długo. W rozważanym przypadku akceptowalne są wartości rzędu 200-300ms. Biorąc pod uwagę wartości z tabeli 6.1, wszystkie kolejne badania zostały przeprowadzone przy liczbie cząsteczek równej 100. Taka wartość umożliwia wydajną pracę algorytmu w czasie rzeczywistym. 6.1.2 Jazda w linii prostej Podczas pierwszego eksperymentu robot jechał korytarzem po torze będącym linią prostą. Pozycję początkową i końcową robota przedstawiono na rysunku 6.2. Rysunek 6.2: Pozycja początkowa (A) i końcowa (B) robota podczas testu, w którym zasymulowano jazdę na wprost. Po wykonaniu ok. 50 iteracji algorytmu, robot stworzył mapę widoczną na rysunku 6.3. 54 Rysunek 6.3: Mapa stworzona gdy zasymulowano jazdę robota po linii prostej. W przypadku jazdy po torze prostym zbudowano spójną mapę otoczenia. Ściany korytarza są wyraźne i bez problemu można zrekonstruować w odwzorowanym środowisku ścieżkę robota. W czasie jazdy robot rozpoznał 4 znaczniki. W tabeli 6.2 przedstawiono zestawienie współrzędnych odczytanych z mapy oraz współrzędnych wyznaczonych za pomocą algorytmu FastSLAM. Wyznaczone współrzędne w przybliżeniu odzwierciedlają położenia rogów ścian na wykorzystanej mapie. Największy błąd wystąZnacznik Odczyt z mapy [m] FastSLAM [m] A (-3,33;7,75) (-3,55;7,62) B (0,17;3,16) (-0,14;3,12) C (-1,73;3,33) (-1,80;3,21) D (2,01;7,89) (1,85;7,96) Tabela 6.2: Współrzędne rozpoznanych znaczników podczas testu, w którym zasymulowano jazdę robota na wprost. 55 pił w przypadku znacznika A. W tym przypadku odległość pomiędzy współrzędnymi odczytanymi z mapy oraz wyznaczonymi przez algorytm FastSLAM wynosi ok. 25cm. Znacznik A jest położony w największej odległości ścieżki robota, a więc odczyty z dalmierza w tym przypadku były mniej dokładne niż w przypadku pozostałych znaczników. Dodatkowo znacznik był stosunkowo późno wykryty przez algorytm, a więc liczba iteracji, w których robot posiadał dane o jego współrzędnych była stosunkowo mała. Co za tym idzie, nie było możliwości skorygowania danych dotyczących znacznika. 6.1.3 Wykonanie skrętu Podczas drugiego testu przeprowadzonego na symulatorze Stage robot dokonał skrętów przejeżdżając pomiędzy filarami. Pozycje początkową oraz końcową przedstawiono na rysunku 6.4. Rysunek 6.4: Pozycja początkowa i końcowa robota podczas testu, w którym zasymulowano wykonanie przez robota skrętu. 56 Po ok. 60 iteracji rozpoznano 6 znaczników oraz zbudowano mapę widoczną na rysunku 6.5. Rysunek 6.5: Mapa stworzona po 60 iteracjach algorytmu w przypadku gdy zasymulowano wykonywanie przez robota skrętów. W tabeli 6.3 przedstawiono współrzędne położeń znaczników wyznaczone przy pomocy algorytmu oraz współrzędne odczytane z wykorzystanej mapy. 57 Znacznik Odczyt z mapy [m] FastSLAM [m] A (0,5;-5,62) (0,56;-5,76) B (-0,5;-5,62) (-0,55;-5,75) C (2,18;-7,09) (2,17;-7,14) D (-1,05;-2,29) (-1,15;-2,41) E (-1,13;-4,55) (-1,09;-4,64) F (0,97;-2,26) (0,85;-2,36) Tabela 6.3: Współrzędne rozpoznanych znaczników podczas testu, w którym zasymulowano wykonanie przez robota skrętu. W tym przypadku odległości pomiędzy współrzędnymi znaczników odczytanymi z mapy oraz tymi wyznaczonymi przez algorytm wahają się od 5 do 15cm. W przeciwieństwie do poprzedniego eksperymentu błędy we współrzędnych znaczników wyznaczonych przez algorytm FastSLAM są bardziej wyrównane, nie występuje znacznik, którego współrzedne znacznie różnią się od współrzędnych odczytanych z mapy. Może to być spowodowane tym, że znaczniki położone są bliżej siebie oraz w niedużej odległości od ścieżki robota. Robot w każdej iteracji obserwował co najmniej jeden znacznik, dzięki temu w kolejnych iteracjach algorytm, wykorzystywał współrzędne znaczników do wyznaczania pozycji robota. Podczas wykonwania skrętu stworzono spójną mapę otoczenia. Położenia zaobserwowanych znaczników odzwierciedlają położenia rogów ścian na mapie wykorzystanej podczas eksperymentów. Również ścieżka robota odzwierciedla rzeczywistą trasę robota pomiędzy wykrytymi znacznikami. Dalsze eksperymenty prowadzone były w środowisku rzeczywistym. 58 6.2 Eksperymenty w środowisku rzeczywistym Środowisko działania robota przedstawiono na rysunku 6.6. Ma- pa środowiska została przeze mnie wykorzystana podczas wcześniejszych eksperymentów przeprowadzonych z wykorzystaniem symulatora Stage i jest widoczny na rysunku 6.1. Rysunek 6.6: Środowisko, działania robota 6.2.1 Test algorytmu wykrywania znaczników Pierwszy eksperyment miał na celu weryfikację działania algorytmu wyznaczania znaczników na podstawie rzeczywistych danych z pomiarowych dalmierza laserowego. Sprawdzone zostało czy są wykrywane istniejące w środowisku rzeczywistym znaczniki, a także czy dane pobrane z dalmierza laserowego są powtarzalne w przypadku gdy czujnik jest nieruchomy. Robot stojąc nieruchomo dokonywał skanów środowiska, w którym znajdowały się możliwe do zaobserwowania znaczniki. Pozycja robota na mapie środowiska widoczna jest na rysunku 6.7. 59 Rysunek 6.7: Pozycja robota podczas testu algorytmu wyznaczania znaczników Dla danej pozycji robota wykryto 2 znaczniki. Wyznaczone współrzędne wyznaczone podczas kolejnych pobrań danych z czujnika oraz wykonania algorytmu wykrywania znaczników przedstawiono w tabeli 6.4. 6.2.2 Jazda po linii prostej Podczas drugiego eksperymentu przeprowadzonego w środowisku rzeczywistym robot przejechał polinii prostej na odcinku ok. 5,5m. Podczas eksperymentu przyjęta została liczba cząstek w filtrze cząsteczkowym równa 100. Pozycje początkową, końcową oraz trasę robota przedstawiono na rysunku 6.8. W czasie ruchu robot wykonał 42 iteracje algorytmu, podczas których rozpoznał 6 znaczników. Na rysunku 6.9 widoczna jest mapa zbudowana bezpośrednio na podstawie danych z czujników robota natomiast na rysunku 6.10 przedstawiona została mapa uzyskana 60 Iteracja Znacznik 1 Znacznik 2 1 (0,7765;-2,6534) (3,0535;-2,5949) 2 (0,7746;-2,6547) (3,0517;-2,5977) 3 (0,7744;-2,6549) (3,0519;-2,5982) 4 (0,7780;-2,6554) (3,0532;-2,5967) 5 (0,7774;-2,6557) (3,0526;-2,5973) 6 (0,7788;-2,6537) (3,0515;-2,5968) 7 (0,7736;-2,6583) (3,0558;-2,5991) 8 (0,7747;-2,6589) (3,0569;-2,5988) 9 (0,7755;-2,6591) (3,0568;-2,5981) 10 (0,7879;-2,6563) (3,0532;-2,5967) Tabela 6.4: Współrzędne rozpoznanych znaczników wyznaczone na podstawie danych pobieranych z nieruchomego dalmierza. Rysunek 6.8: Pozycje początkowa (A), końcowa (B) oraz ścieżka robota podczas testu działania algorytmu w przypadku wykonywania przez robota skrętu. 61 za pomocą algorytmu FastSLAM. Rysunek 6.9: Mapa stworzona bezpośrednio na podstawie danych z czujników robota podczas jazdy na wprost. Rysunek 6.10: Mapa stworzona przez robota po 42 iteracjach algorytmu, gdy robot jchał po linii prostej.a 62 Przyjmując, że robot rozpoczynał pracę w pozycji [0,0,90] w kartezjańskim układzie odniesienia, współrzędne odnalezionych znaczników przedstawiono w tabeli 6.5. Dla porównania podano również współrzędne rzeczywiste znaczników względem początkowej pozycji robota. Znacznik Pomiar [m] FastSLAM [m] A (-2,93;0,85) (-2,93;0,93) B (-0,7;0,85) (-0,71;0,77) C (-0,7;5,91) (-0,45;6,17) D (-0,7;11,25) (-0,49;11,24) E (-3,73;11,25) (-3,44;11,51) F (1,32;9,67) (1,53;9,98) Tabela 6.5: Współrzędne rozpoznanych znaczników podczas testu, w którym rzeczywisty robot jechał na wprost. Można zauważyć, że zarówno ścieżka robota jak i ściany korytarza zostały dokładniej odtworzone w przypadku wykorzystania algorytmu FastSLAM. W przypadku mapy zbudowanej bezpośrednio na podstawie danych z czujników linie odwzorowujące ścieżkę robota oraz ściany korytarza nie są prostymi, lecz widoczne są wyraźne załamania. W rzeczywistości, a także na mapie stworzonej przy wykorzystaniu algorytmu FastSLAM, załamania te nie występują, lub są dużo mniejsze. Można stwierdzić, że mapa stworzona za pomocą algorytmu FastSLAM znacznie lepiej odwzorowuje środowisko oraz zrealizowaną przez robota ścieżkę. 63 6.2.3 Wykonanie skrętu Kolejne doświadczenie miało na celu sprawdzenie czy poprawnie zostanie wyznaczona ścieżka w przypadku wykonania przez robota skrętu. Błędy wskazań czujników odometrycznych w przypadku zmiany orientacji bazy rzeczywistego robota okazują się dużo większe niż błędy translacji podczas ruchu po prostej. Poniżej przedstawiono pozycje początkową i końcową robota wraz ze ścieżką przebytą w czasie ruchu. Rysunek 6.11: Pozycje początkowa (A), końcowa (B) oraz ścieżka robota podczas testu w którym rzeczywisty robot wykonał skręt. Robot wystartował z pozycji o współrzędnych [0,0,45], a następnie manewrując pomiędzy filarami wykonywał skręty. Na rysunkach 6.12 oraz 6.13 przedstawiono, dla porównania, odpowiednio mapę stworzoną przy bezpośrednim wykorzystaniu danych pobranych z czujników oraz mapę zbudowaną używając algorytm FastSLAM. 64 W czasie ruchu robot zaobserwował 5 znaczników, którch współrzędne przedstawiono w tabeli 6.6. Znacznik Pomiar [m] FastSLAM [m] A (0;0,8) (0,04;0,9) B (1,36;1,2) (1,4;1,12) C (3,82;-1,7) (3,65;-1,58) D (2,32;3,9) (2,23;4,23) E (-0,91;3,9) (-1,14;3,98) Tabela 6.6: Współrzędne rozpoznanych znaczników podczas testu, w którym rzeczywisty robot wykonywał skręty. Rysunek 6.12: Mapa stworzona bezpośrednio na podstawie danych z czujników robota Z mapy stworzonej na podstawie „surowych” danych z czujników robota trudno jest odtworzyć rzeczywiste środowisko, a co za tym idzie ścieżkę robota w tym środowisku. Obraz jest zniekształcony z powodu znacznych błędów czujników odometrycznych w przypad65 ku zmiany orientacji bazy robota. Rysunek 6.13: Mapa stworzona przez robota po 50 iteracjach algorytmu, cienka czarna linia to ściany pomieszczeń, grubsza czarna linia to wyznaczona ścieżka robota, czerwone punkty to rozpoznane znaczniki Można zauważyć, że ścieżka robota została wyznaczona znacznie dokładniej za pomocą algorytmu FastSLAM. Stworzona mapa nie jest idealna, ponieważ w kolejnych chwilach ruchu była budowana dla cząstki o najwyższej wadze, a niekoniecznie ta sama cząstka miała najwyższą wagę podczas całego czasu pracy algorytmu. Jednakże cząstki najmniej odpowiadające mapie tworzonej w poprzednich iteracjach były odrzucane, co pozwoliło uniknąć zniekształcenia mapy jakie miało miejsce w przpadku mapy stworzonej bezpośrednio na podstawie danych z czujników. Również położenia znaczników dobrze odzwierciedlają rzeczywiste położenia rogów ścian. Ich dane były uaktualniane w kolejnych iteracjach algorytmu 66 na podstawie danych z poprzedniej chwili oraz stosunkowo dokładnych danych z dalmierze laserowego. Pomimo niedokładności, ściany korytarzy na mapie zbudowanej przy wykorzystaniu algorytmu FastSLAM są wyraźne oraz w przybliżeniu tworzą proste odcinki. Wzajemne położenia ścian są również znacznie lepiej wyznaczone za pomocą algorytmu FastSLAM niż wykorzystując jedynie dane z czujników. Tak jak jest w rzeczywistości rogi sąsiadujących ścian tworzą w przybliżeniu kąty proste. 6.2.4 Jazda po linii prostej oraz wykonanie skrętu Podczas tego doświadczenia robot początkowo wykonał skręt, nastepnie jechał po linii prostej a na końcu dokonał kolejnego skrętu. Pozycja początkowa i końcowa zostały przedstawione na poniższm rysunku. Rysunek 6.14: Pozycja początkowa (A), końcowa (B) podczas testu, w którym rzeczywisty robot jechał na wprost oraz wykonywał skręty. Mapy zbudowane przez robota w przypadku wykorzystania bezpośrednio danych z czujników oraz przy pomocy algorytmu FastSLAM przedstawiono na rysunkach 6.15 oraz 6.16. 67 Rysunek 6.15: Mapa stworzona bezpośrednio na podstawie danych z czujników robota. Rysunek 6.16: Mapa stworzona przez robota po ponad 100 iteracjach algorytmu. Największa różnica w mapach jest widoczna w miejscu najwięk68 szego skrętu, na początku ruchu robota. W przypadku mapy zbudowanej za pomocą algorytmu FastSLAM, kąt pomiędzy dwoma korytarzami jest bliższy rzeczywistemu, który wynosi 90 stopni. Błąd jest spowodowany dużym błędem rotacji towarzyszącym ruchowi robota. W przypadku wykorzystania algorytmu FastSLAM zminimalizowano go dzięki danym, pobranym z dalmierza laserowego. W tym przypadku robot rozpoznał 10 znaczników. W tabeli 6.7 przedstawiono ich współrzędne. Znacznik Pomiar FastSLAM A (-0,4;0,3) (-0,46;0,34) B (1,23;1,37) (1,3;1,54) C (2,11;6,3) (2,78;7,12) D (-2,12;3,33) (-2,55;3,67) E (-3,72;2,92) (-3,32;2,63) F (-7,53;4,14) (-7,67;4,57) G (-9,54;4,14) (-9,73;4,55) H (-10,31;3,98) (-9,86;4,34) I (-13,12;5,48) (-13,76;5,87) J (-13,12;2,02) (-13,56;2,71) Tabela 6.7: Współrzędne rozpoznanych znaczników podczas testu, w którym rzeczywisty robot jechał na wprost oraz wykonywał skręty. Podczas tego doświadczenia, różnice między współrzędnymi wyznaczonymi przez algorytm oraz odczytanymi z mapy były większe niż podczas poprzednich eksperymentów. Spowodowane jest to większą odległością jaką przebył robot, a co za tym idzie większymi niedokładnościami wyznaczonej pozycji w ostatnich fazach ruchu robota. Podczas ruchu, błędy pomiarowe nie zostają całkowicie wyeliminowane, a w przy kolejnych odczytach z czujników 69 odometrycznych błędy kumulują się. Z tego powodu niedokładności w wyznaczonych współrzędnych znaczników są tym większe im dłuższa jest ścieżka. Należy zauważyć również, że przez czas kilkunastu iteracji algorytmu robot nie obserwował żadnych znaczników przez co nie miał możliwości skorygowania własnej pozycji. Mimo niedokładności mapa stworzona za pomocą algorytmu FastSLAM jest dokładniejsza od mapy zbudowanej tylko na podstawie danych odometrycznych. Podzcas testów stworzono spójną mapę otoczenia, zarówno w przypadku gdy robot jechał po linii prostej, jak i wtedy gdy wykonywał skręty. W przypadku jazdy na wprost, błędy czujników odometrycznych są znacznie mniejsze niż w przypadku zmiany orientacji bazy, jednak ich występowanie jest nieuniknione. Mapa zbudowana w tym przypadku jest spójna, jednak można zauważyć, że przez długą część ruchu robot nie obserwował żadnych znaczników, a to utrudnia proces samolokalizacji. Aby algorytm działał poprawnie, dane o położeniach znaczników muszą być dostarczane regularnie, w przeciwnym przypadku, zarówno ścieżka robota, jak i położenia kolejnych znaczników mogą być błędne. W przypadku wykonywania skrętów bazy jeznej robota błędy czujników odometrycznych są znacznie większe. Bez dostarczenia danych na temat znaczników robot nie ma możliwości skorygować swojej pozycji, a więc niezbędne jest ciągłe obserwowanie charakterystycznych obiektów. W zaproponowanej realizacji algorytmu wykorzystano naturalne znaczniki w postaci rogów ścian. Głównym źródłem błędów w algorytmie FastSLAM są błędy odczytów czujników odometrycznych w przypadku zmiany bazy robota. Dla dużych zmian orientacji bazy błędy odometrii są większe. Jeśli błędy są zbyt duże mogą uniemożliwiać wykorzystanie algo70 rytmu FastSLAM. W sytuacji gdy w pobliżu robota występuje kilka znaczników niezbyt oddalonych od siebie, a błędy odometrii są znaczne, obiekty te mogą zostać błędnie skojarzone, a w konsekwencji proces samolokalizacji zostanie zaburzony, gdyż robot wykona lokalizację względem błędnie określonych znaczników. 71 72 Rozdział 7 Podsumowanie Praca dotyczyła problemu jednoczesnej lokalizacji i budowy mapy otoczenia przez robota mobilnego. Do realizacji zadania został wybrany algorytm FastSLAM. W ramach pracy opracowano, zaimplementowano a następnie dokonano weryfikacji eksperymentalnej algorytmu, który mógłby być wykorzystany dla rzeczywistego robota. Praca obejmuje opis działania realizowanej wersji algorytmu FastSLAM wraz z opisami algorytmów pomocniczych służących m. in. do wykrywania czy asocjacji znaczników, z których korzysta algorytm FastSLAM. Przedstawiono również docelowe środowisko działania wraz z modelami ruchu i obserwacji. Na potrzeby pracy stworzono sterownik do serwera Player w wersji 3.1.0. Oprogramowanie zaimplementowane zostało w języku C++, wykorzystując istniejące już i udostępnione wraz z oprogramowaniem Player/Stage sterowniki oraz biblioteki. W celu weryfikacji działania algorytmu przeprowadzone zostały badania symulacyjne z wykorzystaniem symulatora Stage oraz badania experymentalne w środowisku rzeczywistym z wykorzystaniem robota mobilnego Elektron. Badania symulacyjne miały na celu zweryfikować poprawność zaimplementowanego algorytmu, natomiast celem badań eksperymentalnych w środowisku rzeczywistym było sprawdzenie działania 73 algortmu w przypadku rzeczywistego robota mobilnego poruszającego się w statycznym środowisku wewnętrznym korytarzy i holi. Przy pomocy zaimplementowanego algorytmu FastSLAM zbudowano spójną mapę otoczenia, na której można odnaleźć charakterystyczne obiekty w postaci rogów ścian, a także wyznaczyć przebytą przez robota ścieżkę w układzie odniesienia tworzonym przez te obiekty. Błędy towarzyszące wyznaczaniu współrzędnych znaczników, a co za tym idzie ścieżki robota są nieuniknione, jednak zaimplemplementowany algorytm w kolejnych iteracjach korygował zarówno swoją pozycje jak i położenia znaczników, co umożliwiło zminimalizowanie błędów. Efekty działania algorytmu w znacznym stopniu są uzależnione od występowania w otoczeniu łatwych do identyfikacji znaczników, oraz wykorzystania efektywnej metody ich wykrywania i asocjacji. Zaproponowane w pracy algorytmy realizujące te zadania, poradziły sobie w przypadku, gdy znaczniki były oddalone od siebie. W środowiskach, gdzie rozpoznawalne obiekty znajdują się w mniejszych odległościach, konieczne wydaje się wykorzystanie większej liczby cech obiektów, za pomocą których można je identyfikować i kojarzyć. 74 Bibliografia [1] T. Bailey, H. Durrant-Whyte, Simultaneous Localization and Mapping: Part I. IEEE Robotics & Automation Magazine, s. 99-108, June 2006. [2] J. L. B. Claraco, Development of Scientific Applications with the Mobile Robot Programming Toolkit. October 25, 2010. [3] E. Einhorn, C. Schroter, H. Bohme, H. Gross, A Hybrid Kalman Filter Based Algorithm for Real-time Visual Obstacle Detection. Neuroinformatics and Cognitive Robotics Lab. Ilmenau Technical University. [4] A. I. Eliazar, R. Parr, DP-SLAM 2.0 Department of Computer Science Duke University Durham. [5] J. E. Guivant, Efficient Simultaneous Localisation and Mapping in Large Environments. PhD thesis, ACFR, University of Sydney, Australia, May 2002. [6] R. E. Kalman, A new approach to linear filtering and prediction problems. Transaction of the ASME. Journal of Basic Engineering, 1960, s. 35–45. [7] K. Krzak, Zadanie jednoczesnej lokalizacji i budowania mapy przez autonomiczne roboty mobilne. Praca dyplomowa magisterska. Instytut Automatyki i Informatyki Stosowanej, Wydział Elektroniki i Technik Informacyjnych, Politechnika Warszawska, 2006. 75 [8] P. Maciag, Algorytmy jednoczesnej lokalizacji i budowania map z wykorzystaniem rozszerzonego filtru Kalmana. Praca dyplomowa magisterska. Instytut Automatyki i Informatyki Stosowanej, Wydział Elektroniki i Technik Informacyjnych, Politechnika Warszawska, 2008. [9] M. Meila, C. Kwok, D. Fox, Real–time particle filters. In Advances in Neural Information Processing Systems, volume 15, 2004. [10] M. Montemerlo, S. Thrun, FastSLAM: A Scalable Method for the Simultaneous Localization and Mapping Problem in Robotics. Springer Tracts in Advanced Robotics, Volume 27, 2007. [11] M. Montemerlo, S. Thrun, FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem. Proceedings of the AAAI National Conference on Artificial Intelligence, 2003. [12] M. Montemerlo, S. Thrun, FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges. Proceedings of IJCAI, 2003. [13] J. Nieto, J. Guivant, Eduardo Nebot, S. Thrun, Real Time Data Association for FastSLAM. Proceedings of the IEEE Conference on Robotics and Autonomation, 2003. [14] B. Petersen, J. Fonseca, Writing Player/Stage Drivers , a howto for ERSP Player Driver Source Package. Player/Stage - Player driver implementation for ERSP , Department of Computer Science , University of Copenhagen, Winter 2006. [15] Bruno Siciliano, Oussama Khatib Springer handbook of robotics chapter 37, Springer, 2008. 76 [16] R. C. Smith, P. Cheeseman, On the representation and estimation of spatial uncertainty. Proceedings of International Journal of Robotics Research 1986, s. 56–68. [17] W. Szynkiewicz, Wykład z przedmiotu Inteligentne systemy robotyczne. Skrypt elektroniczny dostępny dla słuchaczy przedmiotu. [18] W. Szynkiewicz, Wykład z przedmiotu Wstęp do Robotyki Skrypt elektroniczny dostępny dla słuchaczy przedmiotu. [19] P. Trojanek, Środowisko programowania robotów mobilnych z wykorzystaniem pakietu Player/Stage Praca dyplomowa magisterska. Instytut Automatyki i Informatyki Stosowanej, Wydział Elektroniki i Technik Informacyjnych, Politechnika Warszawska, 2006. [20] J. K. Ulhman, S. J. Julier, A new extension of the kalman filter to nonlinear systems. Proceedings of SPIE - The International Society for Optical Engineering, 1997. [21] J. Xavier, M. Pacheco, D. Castro, A. Ruano, U. Nunes, Fast Line, Arc/Circle and Leg Detection from Laser Scan Data in a Player Driver. Portugal. [22] Homepage Sebastian Thrun http://robots.stanford.edu [23] Laser Measurement Technology https://www.mysick.com/ [24] Łańcuch Markowa http://pl.wikipedia.org/wiki/ [25] OpenSLAM https://www.openslam.org/ [26] Strona domowa firmy SICK http://www.sick.com [27] Strona domowa projektu Player/Stage http://playerstage.sourceforge.net/ , 2007. [28] W. H. Press, Saul A. Teukolsky, W. T. Vetterling, B. P. Flannery Numerical Recipes 3rd Edition: The Art of Scien77 tific Computing Chapter 7, s. 364-365, Cambridge University Press, 2007. 78 Zawartość płyty CD Na dołączonej do pracy płycie CD znajdują się katalogi: • praca - Praca magisterska w postaci dokumentu pdf • my driver - Źródła zaimplementowanego na potrzeby pracy programu, oraz przykładowy plik konfiguracyjny • player stage - Źródła programu, wraz z wykorzystanymi w pracy bibliotekami 79