Algorytm FastSLAM jednoczesnej lokalizacji i budowy mapy

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