Algorytm FastSLAM jednoczesnej lokalizacji i budowy mapy otoczenia przez robota mobilnego

Transkrypt

1 Politechnika Warszawska Wydzial Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki Stosowanej Rok akademicki 2010/2011 Praca Dyplomowa Magisterska Tomasz Zaborowski Algorytm FastSLAM jednoczesnej lokalizacji i budowy mapy otoczenia przez robota mobilnego Praca wykonana pod kierunkiem: dr inż. Wojciecha Szynkiewicza

2 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.

3 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 Fast- SLAM 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.

4 Notacja: t chwila czasowa x, y współrzędne położenia robota w kartezjańskim układzie odniesienia α orientacja robota s t = [x, y, α] T pozycja robota w chwili t u t wektor sterowań robota w chwili t z t wektor obserwacji robota w chwili t s 1:t = {s 1,..., s t } - ścieżka robota składająca się z ciągu kolejnych pozycji m = {m 1,..., m i } - mapa w postaci zbioru położeń znaczników z 1:t = {z 1,..., z t } - wyniki pomiarów położeń znaczników u 1:t = {u 1,..., u t } - ciąg wartości sterowań robota w kolejnych chwilach ruchu n 1:t = {n 1,..., n t } - zbiór skojarzeń obserwacji ze znacznikami K - liczba cząstek w filtrze cząsteczkowym N t - 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

5 Spis treści 1 Wstęp Cel pracy Przyjęte założenia Zakres pracy Zawartość pracy Jednoczesna lokalizacja i budowa mapy Wprowadzenie do zadania SLAM Budowa mapy Samolokalizacja Podejścia do rozwiązania zadania SLAM Metody parametryczne Metody nieparametryczne Metody hybrydowe Algorytm FastSLAM Wprowadzenie Filtr Bayesa Modele systemu Model ruchu Model obserwacji Opis algorytmu FastSLAM Generacja nowych cząstek Uaktualnienie estymat położeń znaczników. 21 i

6 3.4.3 Obliczenie wag cząstek Ponowne próbkowanie Wykrywanie znaczników Asocjacja danych Środowisko badawcze Robot Czujniki odometryczne Dalmierz laserowy Platforma Player/Stage Player Stage Biblioteka Miarn Implementacja algorytmu FastSLAM Struktura programu Opis klas Klasa FastSlamDriver Klasa Fastslam Klasa Ekf Klasa My Math Badania symulacyjne i eksperymentalne Badania symulacyjne Liczba cząstek w filtrze cząsteczkowym Jazda w linii prostej Wykonanie skrętu Eksperymenty w środowisku rzeczywistym Test algorytmu wykrywania znaczników Jazda po linii prostej Wykonanie skrętu Jazda po linii prostej oraz wykonanie skrętu 67 7 Podsumowanie 73

7 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 pomieszczenia 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

8 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

9 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

10 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ą s t 1, danymi pobranymi Z czujników odometrycznych u t, obserwacjami z t 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 s t, która zależy jedynie od pozycji Z poprzedniej chwili oraz danych Z czujników odometrycznych. Jednocześnie robot dokonuje obserwacji z t. 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(s 1:t, m z 1:t, u 1:t ), (2.1) 4

11 Rysunek 2.1: Dynamiczna sieć Bayes a opisująca probabilistyczne zależności między stanem, obserwacjami i cechami [17]. gdzie: m - mapa środowiska s 1:t = {s 1,..., s t } - ciąg dyskretnych pozycji robota (ścieżka) z 1:t = {z 1,..., z t } - wyniki pomiarów położeń znaczników u 1:t = {u 1,..., u t } - 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(s t, m z 1:t, u 1:t ) (2.2) Do rozwiązania powyższych postaci zadań SLAM zwykle wykorzystuje się różne implementacje filtru Bayesa [11]. 5

12 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

13 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

14 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, 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

15 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. Pierw- 9

16 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 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

17 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 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

18 12

19 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

20 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(s 1:t, m z 1:t, u 1:t, n 1:t ), (3.1) gdzie: s 1:t = {s 1,..., s t } - ciąg dyskretnych pozycji robota, m = {m 1,..., m i } - mapa w postaci zbioru znaczników, z 1:t = {z 1,..., z t } - wyniki pomiarów położeń znaczników, u 1:t = {u 1,..., u t } - wartości sterowań robotem w kolejnych chwilach, n 1:t = {n 1,..., n t } - 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(s t, m z 1:t, u 1:t, n 1:t ) = p(z t s t, m, z 1:t 1, u 1:t, n 1:t )p(s t, m z 1:t 1, u 1:t, n 1:t ) p(z t z 1:t 1, u 1:t, n 1:t ) (3.2) Bieżąca obserwacja z t zależy od aktualnego stanu robota s t, mapy m oraz asocjacji n t. Stosując taką definicję licznik wyrażnia (3.2) zapisuje się jako: p(z t s t, m, z 1:t 1, u 1:t, n 1:t ) = p(z t s t, m, n t ) (3.3) 14

21 Wartość mianownika wyrażenia (3.2) nie zależy od poprzednich obserwacji z 1:t 1, sterowań u 1:t ani asocjacji gdy nie jest znana pozycja robota ani mapa. Mianownik pełni rolę współczynnika normalizującego. p(z t z 1:t 1, u 1:t, n 1:t ) = 1 η (3.4) Stosując twierdzenie o prawdopodobieństwie całkowitym drugi człon licznika (3.2) można zapisać jako: p(s t, m z 1:t 1, u 1:t, n 1:t ) = p(s t, m s t 1, z 1:t 1, u 1:t, n 1:t )p(s t 1 z 1:t 1, u 1:t, n 1:t )ds t 1 (3.5) Pierwszy wyraz znajdujący się pod całką rozwija się korzystając z twierdzenia o prawdopodobieństwie warunkowym: p(s t, m s t 1, z 1:t 1, u 1:t, n 1:t ) = p(s t m, s t 1, z 1:t 1, u 1:t, n 1:t )p(m s t 1, z 1:t 1, u 1:t, n 1:t ) (3.6) Rozwiązując zadanie SLAM zakłada się, że stan procesu w chwili t zależy bezpośrednio od poprzedniej pozycji s t 1 oraz sterowania u t czyli spełnia warunki łańcucha Markowa [24]. Pozyższe założenie pozwala zapisać: p(s t m, s t 1, z 1:t 1, u 1:t, n 1:t ) = p(s t s t 1 u t ) (3.7) Wzór (3.4) można więc zapisać jako: p(s t, m z 1:t 1, u 1:t, n 1:t ) = p(s t s t 1, u t )p(m s t 1, z 1:t 1, u 1:t, n 1:t )p(s t 1 z 1:t 1, u 1:t, n 1:t )ds t 1 (3.8) Co dalej można przedstawić: p(s t, m z 1:t 1, u 1:t, n 1:t ) = p(s t s t 1, u t )p(s t 1, m z 1:t 1, u 1:t, n 1:t )ds t 1 (3.9) Podstawiając powyższe wyrażenie do wzoru (3.2) uzyskuje się formułę opisującą zadanie SLAM. 15

22 p(s t, m z 1:t, u 1:t, n 1:t ) = ηp(z t s t, m, n t ) p(s t s t 1, u t )p(s t 1, m z 1:t 1, u 1:t 1, n 1:t 1 )ds t 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 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 s t = [x t, y t, α 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((y t y t 1 )/(x t x t 1 )), (3.12) σ trans = (x t x t 1 ) 2 + (y t y t 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), (3.15) 16

23 ω trans N(0, R), (3.16) σ rot1 = σ rot1 + ω rot, (3.17) σ trans = σ trans + ω trans, (3.18) σ rot2 = σ rot2 + ω rot. (3.19) Rzeczywiste współrzędne pozycji robota uzyskuje się poprzez złożenie rotacji σ rot1, translacji σ trans oraz rotacji σ rot2. x t = x t 1 + σ trans cos(α t 1 + σ rot1 ), (3.20) y t = y t 1 + σ trans sin(α t 1 + σ rot1 ), (3.21) α t = α t 1 + σ rot1 + σ 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

24 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 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(s t, m nt ). Zakładając, że pozycja robota w chwili t oznaczona jest s t = [x, y, α] T, a aktualne współrzędne położenia znacznika m nt oznaczono jako 18

25 [u,v], funkcję g(s t, m nt ) można zapisać w następujący sposób: g(s t, m nt ) = [r, φ] T (3.23) r(s t, m nt ) = (x u) 2 + (v y) 2 + ω r (3.24) φ(s t, m nt ) = arctan ((v y)/(x u)) α + ω φ (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(s 1:t, m z 1:t, u 1:t, n 1:t ) = p(s 1:t z 1:t, u 1:t, n 1:t ) N p(m i s 1:t, z 1:t, u 1:t, n 1: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: S [i] t =< s [i] 1:t, µ [i] 1,t, Σ [i] 1,t,..., µ [i] N,t, Σ [i] N,t >, (3.27) gdzie i - indeks cząstki s [i] 1: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

26 Σ [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 Generacja nowych cząstek Zbiór cząsteczek S t w chwili t jest generowany na podstawie zbioru S t 1 z chwili t-1, obserwacji z t i sterowań u t. Ponieważ nie można bezpośrednio wygenerować nowych cząstek zgodnych z założonym probabilistycznym modelem p(s 1:t, m z 1:t, u 1:t, n 1: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ą s t p(s t s t 1, u t ). Zakładając, że zbiór cząstek w poprzedniej chwili S t 1 jest zgodny z rozkładem s 1:t 1 p(s 1:t 1 z 1:t 1, u 1:t 1 ), można stwierdzić, że wygenerowane cząstki są rozłożone zgodnie z rozkładem s 1:t p(s 1:t z 1:t 1, u 1:t ). W celu wygenerowania nowych cząstek korzysta się z modelu 20

27 ruchu opisanego w sekcji 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(m nt s 1:t, z 1:t, u 1:t, n 1:t ) = ηp(z t m nt, s t, n t )p(m nt s 1:t 1, z 1:t 1, u 1:t 1, n 1:t 1 ) (3.28) Aby skorzystać z algorytmu filtru Kalmana nieliniowy model obserwacji g(s t, m nt ) 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 R t. ẑ t = g(s [k] t, µ nt,t 1) (3.29) G mnt = mnt g(s t, m nt ) st =s [k] t ;m nt =µ [k] n t,t 1 (3.30) g(s [k] t, µ nt ) = ẑ t + G m (m nt µ [k] n t,t 1) (3.31) 21

28 Pierwszy czynnik wyrażenia (3.26) jest więc postaci: p(z t m nt, s t, n t ) N(z t ; ẑ t + G m (m nt µ [k] n t,t 1)), R t ) (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. G mnt ẑ t = g(s [k] t, µ nt,t 1) (3.33) = mnt g(s t, m nt ) st =s [k] t ;m nt =µ [k] n t,t 1 (3.34) Z nt = G mnt Σ [k] n t,t 1G T m nt + R t (3.35) K t = Σ [k] n t,t 1G T m nt Z 1 n,t (3.36) µ [k] n t,t = µ [k] n t,t 1 + K t (z t ẑ t ) (3.37) Σ [k] n t,t = (I K t G mnt )Σ [k] n t,t 1 (3.38) Oznaczając pozcyję robota s t = [x t, y t, α t ] oraz położenie znacznika z t = [u t, v t ], jakobian G mnt gdzie G mnt = oblicza się ze wzorów: u t q x t v t y t q v t y t q u t x t q q = (u t x t ) 2 + (v t y t ) 2 (3.39) Obliczenie wag cząstek Cząstki w pierwszym kroku algorytmu wygenerowane zostały zgodnie z proponowanym rozkładem prawdopodobieństwa s t p(s 1:t z 1:t 1, u 1:t, n 1:t 1 ), a więc nie spełniają założenia zadania SLAM, czyli nie są zgodne z docelowym rozkładem prawdopodobieństwa s t p(s 1:t z 1:t, u 1:t, n 1: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. w [k] t = p(s[k] p(s [k] 1:t z 1:t, u 1:t, n 1:t ) 1:t z 1:t 1, u 1:t, n 1:t 1 ) 22 (3.40)

29 Korzystając z reguły Bayesa licznik wyrażenia (3.37) można zapisać: w [k] t = p(z[k] t s [k] 1:t, z 1:t 1, u 1:t, n 1:t )p(s [k] 1:t z 1:t 1, u 1:t, n 1:t ) p(s [k] 1:t z 1:t 1, u 1:t, n 1: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ąć. w [k] t = p(z[k] t s 1:t, z 1:t 1, u 1:t, n 1:t )p(s [k] 1:t z 1:t 1, u 1:t, n 1:t 1 ) p(s [k] 1:t z 1:t 1, u 1:t, n 1:t 1 ) = p(z t s [k] 1:t, z 1:t 1, u 1:t, n 1: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 Z nt,t. Prawdopodobieństwo obserwacji z t jest równe prawdopodobieństwu innowacji z t ẑ t wygenerowanemu przez ten rozkład [2]. Obliczając wagi każdej z cząstek korzysta się ze wzoru: w [k] t = (1/ 2πZ nt,t ) exp{ 0.5(z t ẑ nt,t) T [Z nt,t] 1 (z t ẑ nt,t)} (3.43) 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

30 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 P 1, P 2, P 3...P i, z których każdy jest postaci P i = [x i, y i ]. Na początku przeprowadza się segmentację danych wejściowych podczas której wyznaczane są zbiory punktów tworzące segmenty. Dwa punkty P i i P j należą do jednego segmentu jeśli ich wzajemna odległość nie jest większa od ustalonego progu L max czyli spełniają warunek: (P1x P 2x ) 2 + (P 1y P 2y ) 2 < L max (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

31 Mając zbiory punktów tworzące segmenty, następnie dla każdego segmentu, który można zapisać w postaci {P 1, P 2, P 3,..., P i } 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 P 1 i P i. Wyznaczając równanie prostej w postaci y = ax + b wykorzystuje się wzory: a = y 1 y i x 1 x i, (3.45) b = y 1 y 1 y i x 1 x i x 1. (3.46) 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: d = ax y + b a2 + b 2, (3.47) a następnie wybrany zostaje punkt P dmax, dla którego obliczona wartość jest największa. Krok III. Jeśli wyznaczona odległość d jest większa od zadanego progu d max, podziel zbiór punktów tworzących segment na 2 podzbiory {P 1,..., P dmax } oraz {P dmax,..., P i }, a następnie dla każdego z nich powtórz procedurę. Jeśli odległość d jest mniejsza od progu d max, 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

32 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 {P i,..., P j } oraz {P k,..., P l } uznano za sąsiadujące, jeśli posiadają jeden punkt wspólny to znaczy P j = P k, 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 {P i,..., P j } można podzielić na dwa podzbiory {P i,..., P n } oraz {P n+1...p j }, 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 {P i,..., P n } korzysta się ze wzorów: n m=i x m x = (3.48) n i n y m m=i y = (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 usta- 26

33 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 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

34 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 L 1,..., L i to położenia znaczników rozpoznanych do aktualnej chwili, natomiast drugi O 1,..., O j 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: d kl = (L kx O lx ) 2 + (L ky O ly ) 2 (3.50) Krok 2. Wyznacz minimalną wartość obliczoną w kroku pierwszym, d = argmax(d kl ). Jeśli d < d max, 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 d max, 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. Ry- 28

35 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

36 Poniżej przedstawiono pseudokod algorytmu FastSLAM: F astslam(s t 1, z t, u t, R t ) S t = S tmp = for k=1 to K do pobierz k-tą cząstkę < s [k] t 1, µ[k] 1,t 1, Σ[k] 1,t 1,..., µ[k] N,t 1, Σ[k] N,t 1 > ze zbioru S t 1 losuj s [k] t p(s [k] t for n=1 to N [k] t 1 do s [k] t 1, u t) G mnt,t = mnt g(s t, m nt ) st=s [k] t ;m nt =µ [k] n t,t 1 ẑ t = g(s [k] t, µ [k] n ) t,t 1 Z nt,t = G mnt Σ [k] n t,t 1 GT m nt + R t p [k] n t = (1/ 2πZ nt,t ) exp{ 0.5(z t ẑ nt,t) T Zn 1 t,t(z t ẑ nt,t) end for p [k] = p N [k] t 1 +1,t 0 n t = argmaxp k n t,t if n t = N [k] N [k] t t then = N [k] t µ [k] n t,t = g 1 (s [k] t, µ nt,t ) Σ [k] n t,t = (GT m n, n t R 1 G mn, n t ) 1 else N [k] t = N [k] t 1 K nt,t = Σ[k] n GT t,t 1 m nt Zn 1 t,t µ [k] = n µ[k] + t,t n K n t,t 1 t (z t ẑ t ) Σ [k] n t,t = (I K nt Gm n t,t 1 end if for n=1 to n=n do if n n t then µ [k] m n,t = µ [k] m n,t 1 Σ [k] m n,t = Σ [k] m n,t 1 end if end for w [k] t = p [k] n t,t dodaj (s [k] t 1, µ[k] end for for k=1 to K do 1,t, Σ[k] 1,t,..., µ[k] )Σ [k] n t,t 1 N,t, Σ[k] N,t, w t) do zbioru tymczasowego S tmp losuj czastke z S tmp ustalajac prawdopodobienstwo proporcjonalne do jej wagi dodaj czastke do zbioru S t end for return S t 30

37 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

38 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 = (l l + l p )/2 (4.1) α = (l l + l p )/d, (4.2) gdzie: l l - droga przebyta przez koła na lewej strony l p - droga przebyta przez koła na prawej strony 32

39 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: x t = l cos α t 1 (4.3) y t = 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 [ ] 33

40 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 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 sicklms

41 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 Protonek- Driver 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

42 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

43 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

44 38

45 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 doty- 39

46 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 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

47 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

48 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

49 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

50 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 Klasa Fastslam Klasa Fastslam zawiera główną logikę sterownika, udostępnia funkcje realizujące kolejne kroki implementowanego algorytmu. Dostępne publicznie funkcje to: 44

51 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

52 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 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::vector P oint 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 recursive- LineFitting(), która wyznacza zbiory punktów tworzące linie proste. Opis działania algorytmu znajduje się w rozdziale 3. int getnearestobservationindex(std::vector Ekf &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

53 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

54 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 matrix float invertmatrix(matrix float &input) - zwraca macierz będącą odwrotnością podanej macierzy public matrix f loat sqrtmatrix(matrix f loat &input) - zwraca macierz będącą pierwiastkiem podanej macierzy 48

55 public float countdistance(point p1,point p2) - oblicza odległość euklidesową pomiędzy punktami p1 oraz p2. 49

56 50

57 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

58 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

59 Rysunek 6.1: Mapa środowisko wykorzystanego podczas testów algorytmu na symulatorze Stage 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 ms ms ms ms ms Tabela 6.1: Czasy wykonania pojedynczej iteracji algorytmu dla różnych liczb cząstek 53

60 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 ms. 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 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

61 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

62 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 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

63 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

64 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

65 6.2 Eksperymenty w środowisku rzeczywistym Środowisko działania robota przedstawiono na rysunku 6.6. Mapa ś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 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

66 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 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

67 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

68 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

69 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

70 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

71 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 przypad- 65

72 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

73 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 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 Fast- SLAM przedstawiono na rysunkach 6.15 oraz

74 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ęk- 68