Przemysław Maciąg. Algorytmy jednoczesnej lokalizacji i budowania map z wykorzystaniem rozszerzonego filtru Kalmana

Transkrypt

1 POLITECHNIKA WARSZAWSKA Wydział Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki Stosowanej Zespół Sterowania i Programowania Robotów Przemysław Maciąg Algorytmy jednoczesnej lokalizacji i budowania map z wykorzystaniem rozszerzonego filtru Kalmana Simultaneous localisation and mapping algorithms based on Extended Kalman Filter Praca magisterska wykonana pod kierunkiem dra Wojciecha Szynkiewicza Warszawa, 2008

2 Ukochanej Żonie oraz Rodzicom, za możliwość, cierpliwość i zrozumienie.

3 Streszczenie Podstawowym celem było opracowanie, implementacja oraz eksperymentalna weryfikacja działania wybranych algorytmów jednoczesnej lokalizacji i budowy mapy (ang. Simultanious Localistation And Mapping, w skrócie SLAM ) będących realizacjami filtru Kalmana. Do celów badawczych wybrane zostały dwa algorytmy, wykorzystujące filtr Kalmana. Filtr Kalmana został oryginalnie opracowany dla systemów liniowych, podczas gdy modele ruchu i obserwacji robota mobilnego są nieliniowe. Szczegółowo omówiono dwa spośród algorytmów wykorzystujących filtr Kalmana: EKFSLAM, historycznie pierwszy zaproponowany i zaimplementowany, oraz UKFSLAM, będący wciąż stosunkowo nowatorskim podejściem filtracji kalmanowskiej w systemach nieliniowych. Zadanie realizowane przez robota z wykorzystaniem tych algorytmów polegało na rekonstrukcji przebytej przez ścieżki w dyskretnych momentach czasowych, z wykorzystaniem niedokładnych odczytów bieżącego położenia robota oraz znacznie dokładniejszych pomiarów środowiska, przy jednoczesnym budowaniu mapy, w jakim się robot poruszał. Rekonstrukcja ścieżki została przeprowadzona za pomocą wyżej wymienionych algorytmów. Robot, działający pod kontrolą środowiska programistycznego Player/Stage, poruszał się po laboratorium oraz w korytarzu bezpośrednio przez nim, wyszukując charakterystycznych cech środowiska, które mogą zostać zinterpretowane jako znaczniki. Mapa budowana z użyciem algorytmów SLAM, wykorzystujących filtr Kalmana, tworzy numeryczną reprezentację świata, która nie jest faktyczną mapą terenu, lecz jedynie mapą znalezionych w środowisku znaczników. Znaczniki na mapie określone są jedynie przez swoje położenie, bez jakichkolwiek informacji o własnościach geometrycznych. W celu wizualizacji wszystkich uzyskanych odczytów oraz zbudowania rzeczywistej mapy środowiska, zastosowana została metoda mapy siatki zajętości, obecnie najpopularniejsza metoda tworzenia map przez roboty mobilne. iii

4 Abstract The main goal of this thesis was to develop, implement and experimentally verify two algorithms of the Simultaneous Localization And Mapping (SLAM), using the extended Kalman Filter for nonlinear systems. The SLAM algorithm addresses the problem of a robot with a known kinematic model, starting at an unknown location, moving through an unknown environment populated with artificial or natural landmarks. The objective is to localize the robot and at the same to time build an incremental map with the observed landmarks. The first SLAM algorithm called EKFSLAM is based on standard EKF, which is used to provide estimates of robot and landmarks locations. The second algorithm called UKFSLAM uses unscented transformation instead of linearizations performed in the original EKF. The map building by the SLAM algorithms, which use the Kalman filter, makes the numerical world representation, which is not the actual map of the area but only the map of founded landmarks in the environment. On the build map landmarks are specified as points, without any information about geometrical contraints. To visualize all obtained readouts and to build the real map of the environment occupancy grid maps were used. Occupancy grid maps are most popular type of maps, build by the mobile robots. iv

5 Spis treści 1. Wstęp Cel pracy Zakres pracy Układ pracy Lokalizacja i budowanie mapy Zadanie nawigacji robotem mobilnym Tworzenie mapy Samolokalizacja Jednoczesna lokalizacja i budowanie mapy Wprowadzenie do problematyki SLAM Definicja zadania SLAM Rekurencja w zadaniu SLAM Prace badawcze poświęcone tematyce SLAM Opis badanych algorytmów Filtr Kalmana Model systemu stosowany w filtrze Kalmana Algorytm filtru Kalmana Dostrojenie filtru Kalmana Wnioski Model systemu w zadaniu SLAM Znaczniki w zadaniu SLAM Model ruchu w zadaniu SLAM Model obserwacji w zadaniu SLAM Filtr Kalmana jako rozwiązanie zadania SLAM Rzeczywisty model systemu Rzeczywisty model ruchu robota Rzeczywisty model obserwacji Wykorzystywane macierze w zadaniu SLAM Macierz kowariancji stanu P k Macierz wzmocnienia K k v

6 Macierz kowariancji zakłóceń Q k Macierz kowariancji zakłóceń pomiaru R k Algorytm EKFSLAM Wstęp Algorytm rozszerzonego filtru Kalmana SLAM z użyciem EKF Wnioski Algorytm UKFSLAM Wstęp Transformata unscented Algorytm filtru UKF SLAM z użyciem UKF Wnioski Obserwacje i integracja znaczników Ponowna obserwacja znaczników Integracja nowych znaczników w wektorze stanu Asocjacja danych w zadaniu SLAM Wizualizacja z użyciem mapy siatki zajętości Wstęp Algorytm konstruowania mapy siatki zajętości Wnioski Środowisko uruchomieniowe Platforma sprzętowa Robot mobilny Protonek Dalmierz laserowy SICK LMS Oprogramowanie wykorzystane w pracy Środowisko Player/Stage Wtyczka miarn Biblioteka LODO Pakiet Boost Biblioteka Qt Realizacja zadania Wstęp Badane środowisko Odczyt danych o stanie robota Wykorzystane znaczniki terenowe Wykrywanie obiektów geometrycznych przez wtyczkę miarn Aproksymacja prostych w skanach vi

7 Odnajdywanie pozycji znaczników w postaci rogów Opis stworzonego oprogramowania Ogólna struktura programu Diagramy dziedziczenia klas badanych algorytmów Organizacja zajętości pamięci Strojenie filtrów Tworzenie mapy siatki zajętości Opis implementacji algorytmu budowy mapy siatki zajętości Zrealizowany algorytm siatki zajętości Diagramy klas Badania eksperymentalne Wstęp Porównanie ścieżek Graficzna reprezentacja macierzy kowariancji Testy dalmierza laserowego Wyznaczanie znaczników Powtarzalność wyników dalmierza Budowa mapy siatek zajętości Parametry odwrotnego modelu czujnika Wpływ niedokładności pozycji robota na mapę siatki zajętości Testy rekonstrukcji ścieżki Wstęp Odometria robota Protonek Wyniki symulacyjne Wyniki eksperymentalne Stabilność macierzy kowariancji podczas aktualizacji Czasy działania algorytmów Filtracja Kalmana to za mało Dodatkowe problemy zadania SLAM Efektywniejszy system rozpoznawania znaczników Zamknięcie pętli Zewnętrzny system weryfikacji położenia Podsumowanie 124 A Rozkład Cholesky ego - Banachowicza 126 B Zmiana układu współrzędnych 127 C Instrukcja użytkowania programu 129 vii

8 C1. Wprowadzenie C2. Lista opcji D Zawartość płyty CD 132 Bibliografia 137 viii

9 Wykaz algorytmów 3.1 Pseudokod algorytmu EKFSLAM Pseudokod algorytmu UKFSLAM Pseudokod algorytmu aproksymacji prostej dla zadanego zbioru punktów Pseudokod algorytmu odnajdywania rogów obiektów w środowisku z wykorzystaniem aproksymowanych linii Algorytm odwrotnego modelu czujnika odległości ix

10 Wykaz skrótów BLAS - ang. Basic Linear Algebra Subprograms EKF - ang. Extended Kalman Filter GPS - ang. Global Positioning System GUI - ang. Graphic User Interface IAV - ang. Inscribed Angle Variance SLAM - ang. Simultanious Localisation And Mapping SPA - ang. Sense-Plan-Act TOF - ang. time-fo-flight UKF - ang. Unscented Kalman Filter x

11 Spis rysunków 2.1 Przykłady map Docelowe środowiska autonomicznej pracy robotów Przykładowa sieć połączeń przebytej przez robota ścieżki oraz obserwacji Przykładowe znaczniki znalezione przez robota w terenie Ogólny schemat działania algorytmów SLAM wykorzystujących filtr Kalmana Schemat bazy jezdnej robota Przemieszczenie się pojazdu o różnicowej bazie jezdnej Obserwacja znaczników przez robota Podstawa działania transformaty unscented Przykłady budowania map siatek zajętości Robot laboratoryjny Protonek Dalmierz laserowy Sick LMS Zasada działania dalmierza laserowego Sick LMS Ogólny schemat oprogramowania wykorzystywanego w niniejszej pracy Środowisko testowe Przykładowe rogi widoczne w skanie Graficzny opis zasady odnajdywania okręgów w skanie terenu z wykorzystaniem wtyczki miarn Graficzny opis zasady odnajdywania linii w skanie terenu z wykorzystaniem wtyczki miarn Niepoprawna aproksymacja zbioru punktów z użyciem metody najmniejszych kwadratów Aproksymacja zbioru punktów linią. Przypadek z Rysunku Aproksymacja zbioru punktów linii skośnej Zdjęcia części terenu widocznego przez robota z zaznaczonymi liniami Skan środowiska pokazanego na Rysunku Struktura oprogramowania z wyszczególnieniem wątków Diagram dziedziczenia dla klasy SlamAlg xi

12 5.12 Aktualizowany obszar podczas budowaniu mapy siatki zajętości Widoczność komórki przez robota Graficzna reprezentacja parametru β Graficzna reprezentacja parametru α Diagram dziedziczenia dla klasy MapContainer Diagram współpracy dla klasy MapElem Przykładowe skany terenu, uzyskane w programie playerv, z rzeczywistych odczytów dalmierza, przy braku przemieszczenia robota Przykłady mapy siatek zajętości dla różnych wartości parametru β Przykłady mapy siatek zajętości dla różnych wartości parametru α Przykłady mapy siatek zajętości przy zaburzonych parametrach położenia robota Przebyta przez robota ścieżka test Przebieg orientacji bazy robota odometrii oraz LODO test Błędy wskazania odometrii test Mapa siatki zajętości zbudowana na podstawie wskazań sterownika LODO test odometrii Mapa siatki zajętości zbudowana na podstawie wskazań odometrii test odometrii Przebyta przez robota ścieżka test Przebieg orientacji bazy robota odometrii oraz LODO test Błędy wskazania odometrii test Mapa siatki zajętości zbudowana na podstawie wskazań sterownika LODO test odometrii Mapa siatki zajętości zbudowana na podstawie wskazań odometrii test odometrii Symulacyjne środowisko pracy robota Przebiegi położenia (a)) oraz orientacji (b)) bazy robota w kolejnych krokach działania algorytmów - symulacja Mapa siatki zajętości zbudowana na podstawie wskazań odometrii Mapa siatki zajętości uzyskana za pomocą algorytmu EKFSLAM Mapa siatki zajętości uzyskana za pomocą algorytmu UKFSLAM Środowisko eksperymentów 1 oraz Środowisko eksperymentów 1 oraz 2 - mapa CAD Przebieg położenia robota test Przebieg orientacji bazy robota test Błędy wskazania współrzędnych pozycji badanych algorytmów a) EK- FSLAM b) UKFSLAM test Mapa siatki zajętości utworzona ze wskazań odometrii test xii

13 6.26 Mapa siatki zajętości utworzona na podstawie wyników algorytmu EKFSLAM test Mapa siatki zajętości utworzona na podstawie wyników algorytmu UKFSLAM test Przebieg położenia robota test Przebieg orientacji bazy robota test Błędy wskazania współrzędnych pozycji badanych algorytmów a) EK- FSLAM b) UKFSLAM test Mapa siatki zajętości utworzona ze wskazań odometrii test Mapa siatki zajętości utworzona na podstawie wyników algorytmu EKFSLAM test Mapa siatki zajętości utworzona na podstawie wyników algorytmu UKFSLAM test Środowisko dla eksperymentu Środowisko eksperymentu 3 - mapa CAD Przebieg położenia robota test Przebieg orientacji bazy robota test Błędy wskazania współrzędnych pozycji badanych algorytmów a) EK- FSLAM b) UKFSLAM test Mapa siatki zajętości utworzona ze wskazań odometrii test Mapa siatki zajętości utworzona na podstawie wyników algorytmu EKFSLAM test Mapa siatki zajętości utworzona na podstawie wyników algorytmu UKFSLAM test B1 Układy odniesienia globalny i związany z robotem xiii

14 Rozdział 1. Wstęp Rozwiązanie zadania nawigacji stanowi niezbędny element autonomicznej pracy robotów mobilnych. Do podstawowych funkcji zadania nawigacji autonomicznego robota mobilnego należą: samolokalizacja, budowa modeli otoczenia oraz planowanie ruchu (planowanie ścieżki). Dopiero robot wyposażony tak w sprzęt, jak i metody niezbędne do rozwiązania zadania nawigacji, będzie w stanie sam realizować inne, specjalizowane zadania, jakie zostaną przed nim postawione, jak np. przeszukiwanie nieznanego terenu w celu znalezienia określonych przedmiotów Cel pracy Podstawowym celem było opracowanie, implementacja oraz eksperymentalna weryfikacja działania wybranych algorytmów jednoczesnej lokalizacji i budowy mapy (ang. Simultaneous Localisation And Mapping, w skrócie SLAM ) wykorzystujących filtr Kalmana. Wybrane zostały dwa algorytmy: EKFSLAM oraz UKFSLAM. EKFSLAM jest pierwszym historycznie zaproponowanym i opracowanym algorytmem rozwiązującym zadanie SLAM, opartym o rozszerzony filtr Kalmana (ang. Extended Kalman Filter, w skrócie EKF ). Pomimo, iż jest to najstarszy algorytm do dziś jest z powodzeniem stosowany, jak również stale trwają prace nad jego usprawnieniem. Drugi wykorzystuje stosunkowo nowy filtr UKF (ang. Unscented Kalman Filter), który w założeniach ma minimalizować problemy związane z linearyzacją, jakie występują w filtrze EKF Zakres pracy Niniejsza praca dotyczy zagadnienia nawigacji robotem mobilnym w środowisku statycznym, przy nieznanej asocjacji danych pomiarowych. Robot, działający pod kontrolą środowiska programistycznego Player/Stage, będzie poruszał się po labolatorium oraz w korytarzu przed nim. Rozwiązywane będzie zadanie rekonstrukcji 2

15 ścieżki w dyskretnych chwilach czasowych, z wykorzystaniem niedokładnych odczytów odometrycznych bieżącej pozycji robota oraz znacznie dokładniejszych pomiarów o badanym środowisku, pochodzących z dalmierza laserowego. Oprócz map budowanych z użyciem badanych algorytmów, budowana będzie także mapa siatki zajętości, znacznie lepiej obrazująca dostrzegany przez robota teren Układ pracy Praca składa się z siedmiu rozdziałów. Rozdział drugi zawiera omówienie zadania SLAM wraz z krótkim rysem historycznym opisującym kolejne podejścia do zagadnienia jednoczesnej lokalizacji i budowy mapy otoczenia. Rozdział trzeci zawiera szczegółowy opis badanych algorytmów oraz metodę budowy mapy siatki zajętości. W rozdziale czwartym znajduje się opis środowiska uruchomieniowego, czyli wykorzystanej platformy sprzętowej oraz użytego oprogramowania. Rozdział piąty zawiera szczegółowy opis realizacji zadania wraz z charakterystyką środowiska, przyjętymi założeniami oraz omówieniem implementacji algorytmów realizujących zadanie SLAM. Rozdział szósty zawiera wyniki porównawcze badanych algorytmów oraz wnioski dotyczące możliwych usprawnień filtrów. W ostatnim rozdziale znajduje się podsumowanie. 3

16 Rozdział 2. Lokalizacja i budowanie mapy 2.1. Zadanie nawigacji robotem mobilnym Zadanie nawigacji robota mobilnego można podzielić na trzy główne podzadania [28]: tworzenia mapy, samolokalizacji, planowania ścieżki. Realizacja zadania nawigacji jest wymagana dla wszystkich robotów autonomicznych i może dotyczyć całości lub wybranych z powyżej wymienionych czynności, co zależy od możliwości jakimi dysponuje robot oraz od zadań, jakie przed nim stoją. Poniżej zamieszczono ogólne omówienie dwóch elementów zadania nawigacji, które dotyczą problematyki zadania jednoczesnej lokalizacji oraz budowy mapy. Zadanie planowania ścieżki nie jest omawiane w niniejszej pracy, a przykładowe metody rozwiązania można znaleźć np. w [27, 28] Tworzenie mapy Budowanie mapy stanowi podstawę zadania nawigacji robotów mobilnych. Prawidłowo zbudowana mapa może zostać wykorzystana w kolejnych zadaniach realizowanych przez sterownik robota, jak np. lokalizacja, czy efektywne planowanie ścieżki. 4

17 Przystępując do wyboru algorytmu budowania mapy należy rozważyć kilka podstawowych problemów [22]: interpretacja odczytów czujników surowe dane pochodzące z czujników reprezentują wiedzę robota o środowisku. Właściwe przetworzenie danych umożliwia zamodelowanie środowiska, w szczególności określenie rozmieszczenia przeszkód i wyodrębnienie znaków charakterystycznych terenu, jakie znajdują się w zasięgu czujników. Nie można jednak zapomnieć, iż wszystkie odczyty są obarczone błędami pomiarowymi, które przekładają się na błędy w budowanej mapie; wielkość środowiska im więcej odczytów zbierze robot, tym większą część środowiska jest w stanie zamodelować. Jednak ilość zebranych danych przekłada się proporcjonalnie na rozmiar zajętej pamięci, potrzebnej do gromadzenia danych oraz na długość czasu obliczeń; dostępna moc obliczeniowa budowa mapy przez autonomiczne roboty mobilne, rzadko jest jedynym zadaniem, jakie wykonuje oprogramowanie, a nierzadko jest jednym z najbardziej pochłaniających moc obliczeniową komputera. Przyjmując określone założenia w celu rozwiązania przedstawionych powyżej problemów, należy uwzględnić dodatkowo typ mapy, jaka ma zostać zbudowana. Obecnie stosowane mapy dwuwymiarowe (tzw. mapy 2D) można podzielić następująco [22, 29]: mapy topologiczne mapy topologiczne są najczęściej przedstawiane jako grafy nieskierowane. Mapy te buduje się jako sieć połączeń pomiędzy pomieszczeniami, jakie zostały rozpoznane w terenie (pomieszczenia stają się wówczas wierzchołkami grafu). Każde pomieszczenie przedstawia fragment środowiska, jakie robot może identyfikować, jako wolne (np. pokój). Jeśli dwa pomieszczenia są połączone, oznacza to, że istnieje ścieżka pomiędzy nimi, którą robot jest w stanie się bezpiecznie przemieszczać. Mapy topologiczne są bardzo użyteczne dla celów planowania ścieżki, należy jednak pamiętać, że wybrana za ich pomocą ścieżka, może nie być najkrótszą, czy też najszybszą do przebycia dla robota. Najprostszy typ mapy topologicznej nie uwzględnia kosztu przejścia pomiędzy kolejnymi pomieszczeniami (wierzchołkami grafu). Można temu częściowo zaradzić wprowadzając stosowne wagi na łukach, które będą symbolizować koszt takiego przejścia. Ponadto przechodzenie po samych łukach nie uwzględnia rzeczywistej formy terenu, która może być także bardzo trudna do przebycia dla robota; 5

18 mapy metryczne mapy metryczne opisują środowisko dzieląc je na jeden z dwu sposobów: mapy geometryczne, które wykorzystują do podziału rozpoznane w terenie charakterystyczne cechy, jak np. figury geometryczne (linie, okręgi, wieloboki, itp.); mapy rastrowe, które dzielą przestrzeń wykorzystując stały podział, jak ma to miejsce np. w mapach siatki zajętości (mapy siatek zajętości będą szerzej omówione w punkcie 3.8.). Mapy geometryczne są łatwe w interpretacji i zawierają znacznie pełniejszy obraz środowiska, niż przy użyciu map topologicznych, w szczególności niezależnie od przyjętego podziału, zawierają informacje o bezwzględnych relacjach pomiędzy obiektami (jak np. wymiary i odległości). Ma to jednak cenę, bowiem zarówno zapotrzebowanie na pamięć, jak i moc obliczeniową są znacznie większe, niż w przypadku map topologicznych; mapy hybrydowe (mieszane) mapy hybrydowe łączą w sobie cechy dwu poprzednio wymienionych typów map. Przykładowe środowisko i możliwe do zbudowania na jego podstawie mapy, zostały przedstawione na Rysunku 2.1. Ta praca skupia się na stworzeniu mapy dwuwymiarowej, co jest całkowicie wystarczające dla rozwiązania zadania, jakie zostanie postawione przed robotem. Istnieją już udane implementacje map trójwymiarowych (nazywanych mapami 3D), jak np. [8, 33], jednak nie będą one rozpatrywane w niniejszej pracy Samolokalizacja Lokalizacja robota polega na określeniu jego pozycji w zadanym układzie odniesienia. Termin samolokalizacja odnosi się do procesu obliczania własnej pozycji, jaką przeprowadza robot z wykorzystaniem na bieżąco budowanej (lub uprzednio zadanej) mapy. W podejściach do lokalizacji możemy wyróżnić kilka kryteriów podziału [28]: Metody lokalne / globalne korzystając z metod globalnych, pozycja robota wyznaczana jest w odniesieniu do ustalonego, globalnego układu odniesienia. W metodach lokalnych pozycja wyznaczana jest za pomocą przyrostów względem pewnej pozycji początkowej, która jest znana a priori. 6

19 a) b) c) Rysunek 2.1: Przykłady map: a) model budynku z sześcioma pomieszczeniami i wspólnym korytarzem, b) przykładowy podział przestrzeni na stałej wielkości komórki, jak ma to miejsce np. w mapach siatek zajętości, c) przykład mapy topologicznej tego terenu. Źródło: [22]. Praca w środowisku statycznym / dynamicznym dla środowiska statycznego zakłada się, że jedynym obiektem, którego pozycja może zmieniać się, jest robot. W środowisku statycznym, robot może przeprowadzać nawigację w oparciu o odczyty wszystkich tych elementów środowiska, jakie jest w stanie rozpoznać za pomocą dostępnych czujników. W środowisku dynamicznym, przed przystąpieniem do interpretacji danych pomiarowych, wpierw muszą zostać odfiltrowane dane o obiektach zmiennych w czasie (jak np. poruszający się ludzie). Metody bierne / aktywne w lokalizacji biernej zakłada się, że robot nie musi wykonywać żadnych ruchów, w celu wyznaczenia swojej pozycji. Aktywna lokalizacja, w celu eliminacji niejednoznaczności określonej pozycji, zakłada konieczność wykonania ruchu przez robota. 7

20 Najważniejszym czynnikiem procesu lokalizacji robota jest użyta metoda pomiaru położenia. Rozróżniamy dwie grupy metod [28]: względną i bezwzględną. Do metod bezwzględnych zaliczamy lokalizację wykorzystującą: Aktywne latarnie kierunkowe pozycja robota wyznaczana jest w oparciu o latarnie kierunkowe (nawigacyjne). Do tej metody zaliczają się, w szczególności, te oparte o badanie kątów pod jakim widoczne są latarnie (wówczas mowa o triangulacji) oraz o odległości do latarni (trilateracja, do której zalicza się np. nawigacja z użyciem systemu GPS). Rozpoznawanie sztucznych lub naturalnych znaczników pozycja robota wyznaczana jest na podstawie rozpoznanych w terenie znaczników. Dopasowywanie modelu uzyskane z czujników dane są porównywane z posiadaną przez robota informacją o środowisku. Najczęściej wykorzystuje się to podejście przy zadanej uprzednio mapie. Wszystkie metody bezwzględnej lokalizacji zakładają posiadanie pewnej wiedzy o środowisku (jak w przypadku dopasowywania modelu) lub też uprzedniego przygotowania tego środowiska (jak przy użyciu latarni, czy sztucznych znaczników) przed przystąpieniem do lokalizacji robota. Metody te rzadko można zastosować podczas eksploracji nieznanego lub trudno dostępnego środowiska. Wówczas możliwe jest użycie metod względnych, do których zaliczane są: Metody odometryczne najczęściej wykorzystują pomiar kątów obrotów kół za pomocą enkoderów optycznych. Metoda ta jest powszechnie stosowana, jednakże jest obarczona wieloma źródłami błędów, zarówno systematycznych, jak i przypadkowych. Dokładniejsze omówienie typu błędów, jakie występują w metodzie bazującej na odometrii można znaleźć w [28]. Metody inercyjne mierzonymi wielkościami są prędkość oraz przyspieszenie robota za pomocą żyroskopów oraz akcelerometrów. Pozycja uzyskiwana jest po scałkowaniu uzyskanych wartości. Ze względu na możliwość pracy w środowisku nieznanym i niemożliwym do wcześniejszego przygotowania, lokalizacja lokalna ma znacznie większe zastosowanie praktyczne, niż globalna. Wcześniejsze przygotowanie terenu dla celów samolokalizacji robota mobilnego wcale nie gwarantuje, że taka lokalizacja będzie możliwa do przeprowadzenia. Przykładowo wyznaczanie pozycji z wykorzystaniem GPS zakłada widoczność satelit, a jeśli robot znajdzie się w budynku, czy tunelu, nawigacja satelitarna przestanie być możliwa do przeprowadzenia. 8

21 2.2. Jednoczesna lokalizacja i budowanie mapy Rozwiązanie zadania jednoczesnej lokalizacji i budowy mapy jest uważane obecnie za jedno z największych wyzwań, jakie stoi przed badaczami zajmującymi się problematyką autonomicznych robotów mobilnych. Jednym z głównych założeń autonomicznej pracy robotów mobilnych jest możliwość operowania w nieznanym środowisku, przy braku mapy, bez nadzoru człowieka i bez dostępu do globalnych metod pomiaru pozycji (jak np. nawigacja GPS). Rozwiązanie zadania ma pozwolić robotom na pracę w dowolnym środowisku, w szczególności w tych niedostępnych dla człowieka, jak głębiny morskie, tunele starych kopalń, czy powierzchnie innych planet (jak np. eksploracja Marsa). Są to więc środowiska, które dla człowieka są nadal niedostępne, zbyt niebezpieczne, czy zbyt odległe (Rysunek 2.2). a) głębiny morskie b) podziemne tunele c) inne planety Rysunek 2.2: Docelowe środowiska autonomicznej pracy robotów, które wciąż jeszcze są niedostępne dla człowieka. Źródło: [15] Wprowadzenie do problematyki SLAM W latach 60-tych i 70-tych, podejmowano próby opracowania metod nawigacji bazujących na budowie dokładnych modeli robota i środowiska, w jakim miał pracować. Wszystkie te próby skazane były na porażkę, ze względu na losowy charakter niektórych zdarzeń, jak np. losowe błędy w odometrii (co opisano w punkcie ), czy niemożliwe do przewidzenia, dynamiczne zmiany środowiska. Brak możliwości zbudowania dokładnych modeli wymusił na badaczach uwzględnienie błędów pomiarowych oraz niepewności stanu środowiska związanej z jego dynamicznym charakterem. 9

22 Poza modelowaniem, drugim problemem jaki został zidentyfikowany przez badaczy, jest nierozerwalny wpływ błędów występujących w zadaniach lokalizacji oraz budowy mapy, jaki wywierają na siebie nawzajem zadania: błędy, powstające przy wyznaczaniu przebytej przez robota ścieżki (jak np. błędy odometrii), powodują błędną lokalizację robota. Zinterpretowane odczyty czujników po nałożeniu na mapę zostaną więc skojarzone z niewłaściwymi miejscami, co prowadzi do błędów w budowanej mapie; z drugiej strony, błędnie zbudowana mapa, nie pozwala na przeprowadzenie prawidłowej lokalizacji w swoim obrębie. Błędy w zadaniach lokalizacji i budowy map, kumulują się z czasem działania, co w efekcie prowadzi do uzyskania rozwiązań, których użyteczność jest zazwyczaj bardzo słaba. Wzajemny charakter narastania błędów, część badaczy przyjęła nazywać problemem jaja i kury, przyznając, że nie jest oczywistym co powinno zostać wpierw rozwiązane. Rozwiązanie problemu nawigacji zaproponowali w 1986 roku Smith oraz Cheesman w swojej pracy [26], a pierwsza udana implementacja została zaprezentowana przez Moutarlier a oraz Chatila w roku 1989 [17, 16]. Rozwiązanie zaproponowane przez Smitha praz Cheesmana uznaje się za historycznie pierwszą, metodę jednoczesnej lokalizacji i budowania mapy Definicja zadania SLAM Zadanie SLAM odnosi się do sytuacji, gdy robot porusza się w nieznanym dla siebie środowisku. Za pomocą dostępnych czujników, robot zbiera dane o własnej pozycji (najczęściej względnej zmianie tej pozycji w kolejnych krokach) oraz obserwacje dotyczące środowiska, w jakim się znajduje. Wszystkie zbierane przez robota dane są obarczone błędami, stąd modele opisujące zarówno ruch, jak i obserwacje, są opisywane przez rozkłady prawdopodobieństwa. Wynikiem działania algorytmów SLAM są: estymata ścieżki przebytej przez robota oraz estymata mapy terenu po jakim poruszał się robot. Spośród zebranych ze środowiska danych, wyodrębnia się pewne cechy szczególne. Przykładem takich cech szczególnych są łatwo rozpoznawalne kształty, jak figury geometryczne (np. linie proste, okręgi, wieloboki). Z listy takich obiektów, wybrane zostają te, które są łatwo rozpoznawalne oraz łatwo identyfikowalne. Tak wybrane obiekty stają się znacznikami. 10

23 Przebyta przez robota ścieżka jest wyznaczona w sposób dyskretny, tzn. ścieżkę reprezentuje jedynie skończony zbiór pozycji. Punkty ścieżki oraz zaobserwowane znaczniki tworzą sieć połączeń. Przykład takiej sieci znajduje się na Rysunku 2.3, gdzie przez okręgi zaznaczone są kolejne pozycje robota, w których dokonywał obserwacji terenu, a gwiazdkami wyodrębnione z terenu znaczniki. W miarę zbierania dalszych informacji o otoczeniu, poprzednio znalezione znaczniki są ponownie obserwowane. Ponowne obserwacje pozwalają zmniejszyć błędy powstałe przy wyznaczaniu położenia tychże znaczników, a tym samym także w wyznaczaniu pozycji robota. Obserwacje stanowią rodzaj sprzężenia zwrotnego, jakie jest dostępne dla robota w procesie nawigacji. Rysunek 2.3: Przykładowa sieć połączeń, jaką tworzą kolejne pozycje ścieżki robota wraz z zaobserwowanymi znacznikami. Dyskretne punkty ścieżki przedstawione są przez okręgi, gwiazdki oznaczają położenie znaczników, a zielone kreski reprezentują wykonane obserwacje. Źródło: [15]. Z probabilistycznego punktu widzenia, wyróżnia się dwa podejścia do rozwiązania zadania SLAM [34]. Pierwsze z nich to tzw. online SLAM, w którym estymowana jest bieżąca pozycja robota wraz z budowaną mapą terenu, co można zapisać w nastepujący sposób: p(s k, Θ z 1:k, u 1:k, n 1:k ), (2.1) gdzie s k oznacza bieżącą pozycję robota, a Θ oznacza mapę. Mapa w tym znaczeniu jest zbiorem znalezionych przez robota znaczników Θ i : i = 1,..., n. Pozycja oraz mapa są warunkowane przez zbiory odczytów z czujników z 1:k, sterowań u 1:k oraz asocjacji danych n 1:k, od ustalonej chwili 1, do bieżącej k. Asocjacje danych n 1:k wyznaczają połączenia między zbiorem odczytów z 1:k, a znacznikami na mapie Θ. Podejście to nazywane jest online (czyli na bieżąco ), ze względu na wyznaczanie 11

24 jedynie estymaty dla chwili k. Najczęściej algorytmy online SLAM u są algorytmami inkrementacyjnymi, które po wykorzystaniu informacji pomiarowych usuwają je z pamięci. Drugie podejście nazywane jest full SLAM (czyli pełny SLAM), co odnosi się do poszukiwania estymaty nie tylko dla bieżącej chwili k, ale także dla wszystkich chwil poprzedzających, przy jednoczesnym budowaniu mapy, co można zapisać następująco: p(s 1:k, Θ z 1:k, u 1:k, n 1:k ) (2.2) Znaczną wadą tego podejścia jest zapotrzebowanie na moc obliczeniową, która wraz ze zwiększającą się chwilą k, stale musiałaby rosnąć, aby wykonać obliczenia w czasie rzeczywistym. Pomiędzy przedstawionymi podejściami można wyznaczyć związek, który w przypadku SLAMu online jest wynikiem włączenia wszystkich przeszłych pozycji, jakie występują w pełnym SLAM ie: p(s k, Θ z 1:k, u 1:k, n 1:k ) =... p(s 1:k, Θ z 1:k, u 1:k, n 1:k ) ds 1 ds 2... ds k 1 (2.3) W niniejszej pracy są rozpatrywane algorytmy online SLAMu. Algorytmy full SLAMu nie będą dalej rozpatrywane Rekurencja w zadaniu SLAM Zadanie SLAM, zadane rozkładem (2.1), można przepisać wykorzystując regułę Bayesa [15]: p(s k, Θ z 1:k, u 1:k, n 1:k ) = p(z k s k, Θ, z 1:k 1, u 1:k, n 1:k ) p(z k z 1:k 1, u 1:k, n 1:k ) p(s k, Θ z 1:k 1, u 1:k, n 1:k ) (2.4) Warto zwrócić uwagę, że realizacja filtru Bayesa, stanowi podstawę dla większości algorytmów stosowanych online algorytmów SLAM. Obserwacje, wykonywane za pomocą dostępnych czujników przez robota, można opisać za pomocą rozkładu prawdopodobieństwa. Obserwację z k definiuje się jako funkcję obserwowanych przez robota znaczników Θ nk mapy Θ, poprzez asocjację danych n k, kiedy jest on w położeniu s k. Stosując tę definicję do licznika w wyrażeniu po prawej stronie równości (2.4), można zapisać: p(z k s k, Θ, z 1:k 1, u 1:k, n 1:k ) = p(z k s k, Θ, n k ) (2.5) Wartość mianownika po prawej stronie równości wzoru (2.4), nie zależy ani od poprzednich obserwacji z 1:k 1, ani od żadnego ze sterowań u 1:k, ani także od samych 12

25 asocjacji n 1:k, gdy nie jest dostępna ani mapa, ani pozycja robota. Mianownik pełni rolę stałej normalizującej, co można zapisać: p(z k z 1:k 1, u 1:k, n 1:k ) = p(z k ) = 1 η (2.6) Do ostatniego wyrazu po prawej stronie równości (2.4), stosowane jest twierdzenie o prawdopodobieństwie całkowitym, w rezultacie czego otrzymuje się: p(s k, Θ z 1:k 1, u 1:k, n 1:k ) = p(s k, Θ s k 1, z 1:k 1, u 1:k, n 1:k ) p(s k 1 z 1:k 1, u 1:k, n 1:k ) ds k 1 (2.7) Pierwszy z wyrazów znajdujący się pod całką można rozwinąć zgodnie z twierdzeniem o prawdopodobieństwie warunkowym: p(s k, Θ s k 1, z 1:k 1, u 1:k, n 1:k ) = p(s k Θ, s k 1, z 1:k 1, u 1:k, n 1:k ) p(θ s k 1, z 1:k 1, u 1:k, n 1:k ) (2.8) Zadanie SLAM często opisuje się jako łańcuch Markowa [15]. Bieżąca pozycja robota s k może zostać wyznaczona, wykorzystując rozkład prawdopodobieństwa przejścia z poprzedniego położenia s k 1 przy sterowaniu u k. Stosując założenie Markowa do równości (2.8), możemy zapisać: p(s k Θ, s k 1, z 1:k 1, u 1:k, n 1:k ) = p(s k s k 1, u k ) (2.9) Stosując (2.8) i (2.9) do równości (2.7) otrzymujemy: p(s k, Θ z 1:k 1, u 1:k, n 1:k ) = p(s k s k 1, u k ) p(θ s k 1, z 1:k 1, u 1:k, n 1:k ) p(s k 1 z 1:k 1, u 1:k, n 1:k ) ds k 1 (2.10) Łącząc dwa skrajne prawe wyrazy całki w powyższej równości, uzyskujemy: p(s k, Θ z 1:k 1, u 1:k, n 1:k ) = p(s k s k 1, u k ) p(s k 1, Θ z 1:k 1, u 1:k, n 1:k ) ds k 1 (2.11) Przyglądając się drugiemu rozkładowi znajdującemu się pod całką można zauważyć, że ani sterowanie u k, ani asocjacja n k nie mają wpływu na pozycję robota s k 1, czy postać mapy Θ, gdy nie jest dostępna obserwacja z k, stąd: p(s k, Θ z 1:k 1, u 1:k, n 1:k ) = p(s k s k 1, u k ) p(s k 1, Θ z 1:k 1, u 1:k 1, n 1:k 1 ) ds k 1 (2.12) Wykorzystując (2.5), (2.6) i (2.12) do (2.4) otrzymujemy rekurencyjną formułę rozwiązania zadania SLAM: p(s k, Θ z 1:k, u 1:k, n 1:k ) = η p(z k s k, Θ, n k ) p(s k s k 1, u k ) p(s k 1, Θ z 1:k 1, u 1:k 1, n 1:k 1 ) ds k 1 (2.13) 13

26 Występująca w równości (2.13) całka, nie może być jednak obliczona w sposób bezpośredni, a wszystkie istniejące metody jej rozwiązania, stanowią różnego rodzaju aproksymacje. Przykładem takiego przybliżenia są algorytmy bazujące na filtrze Kalmana, jak te dwa zaprezentowane w kolejnym rozdziale Prace badawcze poświęcone tematyce SLAM Od czasu powstania metod rozwiązujących zadanie SLAM, liczba badaczy zajmujących się tą tematyką cały czas rośnie. Przez wielu z nich rozwiązanie zadania SLAM nazywane jest Grallem robotyki mobilnej. Wśród bieżących prac można wyznaczyć trzy główne nurty, jakimi kierują się badacze: metody wykorzystujące filtr Kalmana są to algorytmy bazujące na adaptacjach liniowego filtru Kalmana dla systemów nieliniowych. Przykładami takich algorytmów mogą być badane w niniejszej pracy algorytmy EKFSLAM, wykorzystujący rozszerzony filtr Kalmana, oraz UKFSLAM, bazujący na nowatorskiej metodzie filtru UKF. Tematyka prac obejmuje zarówno poszukiwanie nowych rozwiązań problemu oraz badanie i poprawianie już istniejących, metody Monte Carlo to obiecujące algorytmy wielohipotezowe. W odróżnieniu od metod wykorzystujących filtr Kalmana, gdzie wyznaczane są najbardziej prawdopodobne wartości położenia robota oraz mapy, metody te operują na zbiorze możliwych rozwiązań. Przykładem algorytmu wykorzystującego filtrację cząsteczkową jest algorytm FASTSLAM [15], metody eksperymentalne do metod eksperymentalnych należy zaliczyć nowe, wciąż rozważane podejścia. Przykładem takiego algorytmu jest RatSLAM, gdzie badacze próbują odtworzyć metodę budowy mapy, jakiej prawdopodobnie używają szczury [14]. Dokładniejsze omówienie do prac związanych z metodami estymacji stanu, jakie są w bieżącej chwili stosowane i rozwijane, można znaleźć w [?]. Niniejsza praca jest poświęcona tematyce SLAM w środowisku 2D. Istnieją już jednak pierwsze prace poświęcone tematyce SLAM w środowisku 3D. Wśród zainteresowań tej tematyki leży w szczególności autonomiczna praca robotów latających, jak np. bezzałogowych samolotów. Oprócz opracowywania nowych i poprawiania istniejących metod zajmujących się samą lokalizacją i budowaniem mapy, istnieją także prace związane z możliwością 14

27 wykorzystania algorytmów SLAM przy realizacji innych zadań. Przykładem mogą być prace poświęcone denseslam owi. Nazwą tą określa się opracowanie takiego typu mapy, która pozwoli przedstawić w zwięzły sposób więcej danych o terenie, który jest przez robota badany, jak np. wysokość terenu nad poziomem morza, wilgotność terenu, czy rodzaj gleby. Dobrym przykładem tego typu opracowań są mapy HYMM (skrót od ang. HYbrid Metric Maps), omówione w pracy [21]. 15

28 Rozdział 3. Opis badanych algorytmów 3.1. Filtr Kalmana W 1960 roku Rudolf Emil Kalman opublikował swoją słynną pracę na temat rekurencyjnego wyznaczania minimalno-wariancyjnej estymaty stanu dla dyskretnego modelu liniowego [12]. Od tamtego czasu filtr Kalmana stał się obiektem wielu badań, a zastosowanie znalazł w wielu dziedzinach nauki, między innymi w elektronice, robotyce, przetwarzaniu sygnałów, czy obróbce obrazu i dźwięku. Filtr Kalmana jest optymalnym obserwatorem stanu układu, czyli układem, który estymuje jego stan [47]. Filtr Kalmana jest stosowany, gdy stan systemu nie może być wyznaczony w sposób jednoznaczny lub gdy pomiar wartości stanu jest silnie zakłócony, ale jednocześnie dysponuje się innym, znacznie dokładniejszym pomiarem, którego wartość jest związana z wartością stanu. W odniesieniu do filtru Kalmana, pomiar ten nazywany jest obserwacją Model systemu stosowany w filtrze Kalmana Dany jest proces stochastyczny dyskretny, którego modelem są nastepujące równania: s k = A k s k 1 + B k u k + w k (3.1) z k = H k s k + v k, (3.2) gdzie: k - dyskretna chwila czasowa, s k - wektor stanu w chwili k, s k R n, A k - macierz przejścia, dim(a k ) = n n, B k - macierz wejściowa, dim(b k ) = n m, 16

29 u k - wektor sterowań, u k R m, z k - wektor obserwacji, z k R p, H k - macierz wyjściowa (tzw. macierz obserwacji), dim(h k ) = p n. Wektorowe zmienne losowe w k oraz v k w równaniach (3.10) oraz (3.10) reprezentują odpowiednio szumy procesowe oraz pomiarowe. Szumy są wykorzystywane do uwzględnienia niemodelowanego wpływu zakłóceń, które mają wpływ na dynamikę układu. Najczęściej przyjmuje się, że szumy są wzajemnie niezależne, białe, o zerowych wartościach oczekiwanych oraz o znanych rozkładach gaussowskich: p(w k ) N(0, Q k ) (3.3) p(v k ) N(0, R k ), (3.4) gdzie macierze Q k oraz R k oznaczają (odpowiednio) macierze kowariancji rozkładu normalnego zmiennych w k oraz v k. Zaprezentowana powyżej ogólna postać równań stanu stosowanych w algorytmie filtru Kalmana zakłada, że wartości wszystkich występujących w niej wektorów oraz macierzy mogą być zmienne w kolejnych iteracjach algorytmu. W przypadku, gdy jest to możliwe, stosuje się zazwyczaj prostszą postać, gdzie część z wektorów i/lub macierzy uznaje się za stałe, tj. niezmienne w czasie: s k = As k 1 + Bu k + w (3.5) z k = H k s k + v, (3.6) przy rozkładach szumów: p(w k ) N(0, Q) (3.7) p(v k ) N(0, R) (3.8) W dalszej części pracy rozważany będzie model z macierzami zmiennymi w czasie Algorytm filtru Kalmana Filtr Kalmana realizuje estymację stanu w kolejnych momentach czasowych, a następnie dokonuje korekty wyznaczonej estymaty wykorzystując obserwację. Obserwacje stanowią pomiary, które są znacznie dokładniejsze, niż wyznaczona estymata stanu, chociaż także obarczone błędami. Równania, składające się na filtr Kalmana, można podzielić na dwie grupy: fazę predykcji oraz fazę korekcji. Równania fazy predykcji wyznaczają zmianę stanu systemu w czasie, obliczając nowe wartości estymaty stanu oraz macierzy kowariancji stanu, wykorzystując dostępny model systemu. 17

30 Wektor szumu w k z równania stanu (3.6), który służy uwzględnieniu niemodelowanych zakłóceń, nie może być w żaden sposób wyznaczony dokładnie. Przyjmując, że wartość oczekiwana jest zerowa, możemy przepisać równania modelu w nastepujący sposób: ŝ k = A k ŝ k 1 + B k u k + w k (3.9) z k = H k ŝ k + v k, (3.10) gdzie symbol ŝ k (stan s k z daszkiem ) oznacza estymatę stanu, a nie dokładny stan. Oczywiście wymiary dla wektora stanu i jego estymaty są identyczne. Przyjmując oznaczenia: ŝ k dla estymaty stanu s k a priori, po przeprowadzeniu przez równanie stanu, ŝ k dla estymaty stanu s k a posteriori, po wprowadzeniu ewentualnej korekty do wartości estymaty, wykorzystując obserwację z k kroku k, możemy zdefiniować błędy estymat [43]: ɛ k = s k ŝ k ɛ k = s k ŝ k Za pomocą wyżej zdefiniowanych błędów, definiowane są macierze kowariancji stanu (odpowiednio a priori oraz a posteriori): P k = E[ɛ k ɛ T k ] (3.11) P k = E[ɛ k ɛ T k ] (3.12) Wykorzystując zdefiniowane estymaty stanów oraz macierze kowariancji równania algorytmu Kalmana można przedstawić następująco: Faza predykcji: wykorzystując model zadany równaniami (3.6) oraz (3.6), wyznaczana jest początkowa estymata ŝ k stanu s k wraz z estymatą Pk macierzy kowariancji P k : ŝ k = A k ŝ k 1 + B k u k (3.13) Pk = A k P k 1 A T k + Q k (3.14) Faza korekcji: rozpoczyna się od badania wartości obserwowanej. Dokonywany jest nowy pomiar z r k wartości badanej oraz obliczana jest spodziewana wartość obserwacji z k, zgodnie z równaniem (3.6). Na podstawie obserwacji 18

31 są obliczane wektor innowacji ν k (mówiący o rozminięciu się wartości obserwowanej uzyskanej w pomiarach, od spodziewanej) oraz macierz kowariancji innowacji S k : ν k = z r k z k = z r k H k ŝ k (3.15) S k = H k P k HT k + R k (3.16) Następnie obliczane jest wzmocnienie Kalmana K k, wykorzystywane do aktualizacji wektora estymaty stanu oraz macierzy kowariancji, zgodnie z równaniami: K k = P k HT k S 1 k (3.17) ŝ k = ŝ k + K kν k (3.18) P k = (I K k H k )P k (3.19) Wartość wzmocnienia Kalmana decyduje o wpływie, jaki na wartość estymaty stanu a posteriori ma wektor innowacji. Równanie (3.18) po rozwinięciu, można zapisać w następujący sposób: K k = P k HT k (H k P k HT k + R k ) 1 = P k HT k H k P k HT k + R k (3.20) Patrząc na równanie (3.20) można łatwo zauważyć, że gdy macierz kowariancji błędu R k jest równa macierzy zerowej, wartość wzmocnienia K k osiąga swoje maksimum: lim K k = Hk 1 (3.21) R k 0 Przy takiej wartości wzmocnienia, wartość stanu systemu będzie estymowana jedynie z użyciem wartości pochodzących z obserwacji. Z kolei, gdy macierz kowariancji P k będzie macierzą zerową, wzmocnienie także przyjmie zerową wartość: lim K k = 0 (3.22) P k 0 Wówczas wartość estymaty stanu a posteriori jest równa wartości estymaty a priori. Fazy predykcji i korekcji są wykonywane cyklicznie, w każdej iteracji algorytmu, a nowo obliczone wartości stają się wejściowymi w następnej iteracji. Ukazuje to rekurencyjną naturę filtru Kalmana. Jak większość algorytmów rekurencyjnych, filtr Kalmana wymaga zadania danych inicjalizacyjnych. Z równań (3.13) - (3.19) widać, że wielkościami, jakie muszą zostać zainicjowane, są początkowy wektor wartości stanu układu s oraz macierz kowariancji P. 19

32 Dostrojenie filtru Kalmana Poprawianie działania filtru powinno być przeprowadzane poza działającym systemem i może być realizowane za pomocą dwóch, niezależnych metod: dobór początkowych wartości wektora stanu s oraz macierzy kowariancji P ; dobór macierzy Q oraz R. Ich właściwe dopasowanie stanowi największą trudność w konstrukcji rozwiązania zadania dla danego procesu z użyciem filtru Kalmana. W przypadku, gdy wartości macierzy są za małe w stosunku do błędów pomiarowych, filtr może z łatwością się zdestabilizować i produkować błędne wyniki. Z kolei przy zbyt dużych wartościach, zbieżność filtru będzie bardzo powolna (o ile w ogóle zbieżność zostanie osiągnięta w skończonym czasie) Wnioski Rekurencyjna natura filtru Kalmana odpowiada założeniom zadania SLAM. Najczęściej stosowaną metodą lokalizacji lokalnej w robotach mobilnych, jest odometria, której wskazania są bardzo niedokładne, co stanowi analogię do silnie zakłóconej estymaty stanu układu. Jednocześnie roboty są wyposażane najczęściej w bardzo dokładne czujniki do pomiaru środowiska otaczającego robota, jak np. dalmierze laserowe. W odniesieniu do filtru Kalmana, dane pochodzące z czujników zewnętrznych, można uznać za znacznie dokładniejsze i za ich pomocą realizować obserwacje. Analogia pomiędzy filtrem Kalmana, a zadaniem SLAM, nie jest jednak całkowita. W kolejnych podrozdziałach przedstawiono adaptacje filtru Kalmana dla zadania SLAM oraz rzeczywisty model stanu systemu, który, jak zostanie to pokazane, jest systemem nieliniowym. Dlatego w celu wykorzystania filtru Kalmana do rozwiązania zadania SLAM, niezbędne jest rozszerzenie algorytmu na potrzeby systemów nieliniowych. W dalszej części pracy zaprezentowano dwa przykłady takich rozszerzeń Model systemu w zadaniu SLAM Niniejszy podrozdział zawiera omówienie znaczników oraz modelu procesu, jaki jest używany w zadaniu SLAM. Model ten zawiera istotną modyfikację w stosunku do oryginalnego modelu używanego w filtrze Kalmana. Następnie omówiono stosowany w SLAMie model obserwacji. 20

33 Znaczniki w zadaniu SLAM Znacznikami terenowymi (ang. landmarks), w skrócie znacznikami, są nazywane te obiekty środowiska, które za pomocą dostępnych czujników, robot jest w stanie wykryć i rozróżnić w swym otoczeniu. Po dokonaniu skanu terenu i jego analizie, jeśli zostaną znalezione znaczniki, są one porównywane z poprzednio znalezionymi. Nowe znaczniki są dodawane do listy uprzednio znalezionych w terenie, a ponowna obserwacja znanego już znacznika jest wykorzystywana w celach weryfikacji bieżącego położenia robota. Rysunek 3.1: Przykładowe znaczniki znalezione przez robota w terenie. Źródło: [20]. Ze względu na swoją naturę, znaczniki możemy podzielić na dwie podstawowe grupy: aktywne oraz pasywne. Znacznikami aktywnymi nazywane są takie, które potrafią wysyłać sygnał, jaki robot byłby w stanie jednoznacznie przetworzyć w celu uzyskania informacji o swojej bieżącej pozycji. W celu wykorzystania informacji pochodzących ze znaczników aktywnych, robot musi znać ich położenie przed przystąpieniem do pracy, stąd system oparty o ten typ znaczników, nie nadaje się do ogólnego zastosowania. Znaczniki, które nie emitują żadnych informacji o swoim położeniu, nazywamy znacznikami pasywnymi. W celu ich odnalezienia, robot musi przemieścić się w terenie. Wyszukiwanie i rozpoznawanie takich znaczników jest uzależnione od możliwości czujników, w jakie jest wyposażony robot. Wśród znaczników pasywnych rozróżniamy ich dwa podtypy: 21

34 znaczniki sztuczne celowo umieszczone w środowisku w znanych pozycjach, w celu łatwej ich identyfikacji przez robota; znaczniki naturalne które znajdują się w środowisku bez jego wcześniejszego przygotowania, jak np. drzwi i okna w pomieszczeniach lub drogi i drzewa w środowiskach pozalaboratoryjnych. W odniesieniu do algorytmów SLAM wykorzystujących filtr Kalmana, obserwacja polega na ponownym odnalezieniu w terenie znacznika. Prawidłowe rozpoznawanie oraz identyfikacja znacznika dokonana przy użyciu pomiaru, stanowią podstawę prawidłowego działania tych algorytmów. Zadanie SLAM jest adresowane badaniu nieznanego środowiska, stąd robot w przeważającej większości sytuacji może korzystać jedynie ze znaczników naturalnych, jakie znajdują się w środowisku Model ruchu w zadaniu SLAM Stan systemu w zadaniu SLAM jest reprezentowany przez położenie oraz orientację robota, wraz z położeniami wszystkich znanych znaczników [20]. Oznaczając stan robota w chwili k, przez s r,k, możemy zapisać: s r,k = x k y k θ k, (3.23) a dla przypadku liniowego modelu ruchu robota, zmianę jego pozycji można zapisać w następujący sposób: Równanie (3.24) jest analogiczne do równania (3.6). s r,k = A r s r,k 1 + B r u r,k + w r,k (3.24) Wektor p i,k reprezentuje położenie i-tego znacznika w chwili k-tej, w przyjętym układzie odniesienia: p i,k = x i,k y i,k (3.25) W algorytmach SLAM, wykorzystujących filtr Kalmana, zakłada się, że znaczniki są stacjonarne, tj. ich położenie nie zmienia się w czasie. Można wówczas zapisać: p i,k = p i,k 1 = p i = x i y i (3.26) Zebrane razem znaczniki są zapisywane w postaci pojedynczego wektora. Przyjmując, że liczba znanych znaczników wynosi N: p = 22 p 1. p N (3.27)

35 Pełny wektor stanu układu może teraz zostać zapisany w następujący sposób: s r,k p s k = 1 (3.28). p N wraz z odpowiadającym mu liniowym modelem procesu: s r,k p 1. p N lub w równoważnym, zwartym zapisie: A r 0 0 s r,k 1 0 Ip = 1 0 p I pn p N B r 0 0 u r,k w r,k p p pn 0 pn (3.29) s k = As k 1 + Bu k + w k (3.30) Równanie (3.30) jest analogiczne do równania (3.24) z jedyną różnicą wynikającą z dołączenia położenia znaczników do wektora stanu. W równaniu (3.29) symbol I pi oznacza macierz jednostkową o wymiarach dim(p i ) dim(p i ), a symbol 0 pi oznacza wektor zerowy o wymiarze dim(p i ) Model obserwacji w zadaniu SLAM Ze względu na zmianę modelu wektora stanu układu w zadaniu SLAM, model obserwacji zadany poprzednio równaniem (3.6), także musi zostać zmieniony. Przy użyciu dostępnych czujników, poruszając się w terenie, robot dokonuje obserwacji polegającej na ponownym odnalezieniu w terenie znacznika. Dla liniowego procesu obserwacji, model obserwacji i-tego znacznika można zapisać w następujący sposób: z i,k = H i s k + v i,k (3.31) = H pi p H r s r,k + v i,k (3.32) Macierz H i w równaniu (3.31) jest macierzą obserwacji, podobnie jak to miało miejsce w (3.6). Podczas obserwacji i-tego znacznika, łączy ona dane pochodzące z czujników, z bieżącym wektorem stanu układu. Dla i-tego znacznika, macierz H można zapisać jako: H i = [ H r, 0 0, H pi, 0 0] (3.33) 23

36 Macierz H i odzwierciedla fakt, że obserwacje są wykonywane względem bieżącej pozycji robota. Podobnie jak w (3.6), w powyższych wzorach symbol v i,k reprezentuje zmienną losową w postaci szumu białego, o zerowej wartości oczekiwanej oraz macierzy kowariancji R i,k. Jeśli wszystkie obserwacje są dokonywane z użyciem jednego czujnika, to najczęściej jest przyjmowana stała, jednakowa dla wszystkich znaczników i niezmienna w czasie wartość macierzy wariancji, co symbolicznie można zapisać, jako: R i,k = R k = R (3.34) Filtr Kalmana jako rozwiązanie zadania SLAM Przedstawiony w punkcie rozkład gęstości prawdopodobieństwa określający dokładne rozwiązanie zadania SLAM, rzadko jest funkcją, która pozwala na analityczne wyznaczenie wyrażenia całkowego z równania (2.13). W pracy Smitha i Cheesmana [26] zaproponowana została aproksymacja wykorzystująca filtr Kalmana, którą można zapisać w następujący sposób [15]: p(s k, Θ z k, u k, n k ) = N (s SLAM k ; ŝ SLAM k, P k ), (3.35) gdzie symbol s SLAM k po prawej stronie równości, oznacza wektor rzeczywistych wartości pozycji robota oraz położeń znaczników, ŝ SLAM k jest estymatą pozycji (robota oraz znaczników), a P k jest macierzą kowariancji. W dalszej części pracy pod symbolem s (z indeksem k lub bez) oznaczony został wektor s SLAM. Na Rysunku 3.2 został przedstawiony schemat działania algorytmów SLAM, bazujących na filtrze Kalmana. Często schemat blokowy zawiera dodatkową pętlę dla badania obserwacji w danym kroku k. Wówczas w fazie korekcji, korekta estymaty stanu jest przeprowadzana dla każdej z obserwacji. Na schemacie z Rysunku 3.2 nie zaznaczono warunku STOPu. Jest to spowodowane faktem, że algorytmy SLAM przewidują działanie ciągłe, a nie wykonanie jedynie ustalonej liczby przebiegów. W dalszej części tego rozdziału zostało ukazane, że różnica pomiędzy kolejnymi algorytmami wykorzystującymi filtr Kalmana w systemach nielinowych, polega na odmiennych podejściach do wyznaczenia estymaty stanu i macierzy kowariancji Rzeczywisty model systemu Poprzednio omówiony został liniowy model systemu, wprowadzony na potrzeby rozwiązania zadania SLAM z użyciem filtru Kalmana. W niniejszym podrozdziale przedstawiony zostanie rzeczywisty, nieliniowy, model ruchu robota mobilnego. 24

37 Rysunek 3.2: Ogólny schemat działania algorytmów SLAM wykorzystujących filtr Kalmana Rzeczywisty model ruchu robota Przedstawiony w punkcie 3.2. model układu dla zadania SLAM jest liniowy. Taki model w praktyce nie zawsze może być przyjęty. Dla robota laboratoryjnego, który posłuży do weryfikacji tez zawartych w niniejszej pracy, takiego modelu przyjąć nie można (porównaj punkt ). Baza jezdna robota jest wyposażona w sześć kół, zamocowanych na sztywno. Silniki zamocowane obok kół środkowych, poprzez zastosowanie przekładni, przekazują napęd do wszystkich kół po swojej stronie. Konstrukcja taka przypomina te stosowane w czołgach, co obrazuje Rysunek 3.3. Dobrym przybliżeniem powyższej bazy jest różnicowa baza jezdna. Szczegóły na ten temat tej i innych baz można znaleźć w [28]. 25

38 Rysunek 3.3: Schemat bazy jezdnej robota. Rysunek 3.4: Przemieszczenie się pojazdu o różnicowej bazie jezdnej. Robot jest wyposażony w enkodery pozwalające na zliczenie kąta obrotu koła oraz badania kierunku, w jakim odbywa się ruch. Przykład zmiany położenia przez robota o różnicowej bazie jezdnej znajduje się na Rysunku 3.4. Przyjmując za l lk drogę przebytą przez lewe koło oraz analogicznie l pk dla drogi przebytej przez koło prawe, wyznaczane są długość łuku l k przebyta przez robota oraz zmiana kąta orientacji bazy θ k : l k = l l k + l pk 2 (3.36) θ k = l p k l lk, (3.37) d gdzie d jest odległością między kołami. Za pomocą stosownych przyrostów zmiennych stanu oraz wektora szumu, równanie zmiany pozycji robota można zapisać w następujący sposób: x k y k θ k = x k 1 y k 1 θ k l k cos θ k 1 l k sin θ k 1 θ k (3.38)

39 Wektor szumu w k, przedstawiający wpływ zakłoceń, można zamodelować za pomocą pojedynczej stałej q, mnożąc ją przez przyrosty zmiennych: x k 1 w k = q y k 1 (3.39) θ k 1 Pełny model ruchu będzie wówczas wyglądał następująco: lub w zwartej postaci: l k cos θ k 1 l k sin θ k 1 θ k = x k 1 l k cos θ k 1 y k 1 + (1 + q) l k sin θ k 1 θ k 1 θ k (3.40) s k = f(s k 1, u k, w k ) (3.41) = s k 1 + g(θ k 1, l k, θ k, q) (3.42) gdzie f oraz g oznaczają nieliniowe funkcje przejścia, wektor sterowań u k 1 zadany jest przyrostami z równań (3.36) oraz (3.37), a wartość q symbolizuje niepewność zakładaną w modelu. Model zadany powyżej obrazuje nieliniowość funkcji zmiany położenia robota. Alternatywną postać modelu można zaprezentować wykorzystując fakt, że: x k = l k cos θ k 1 (3.43) y k = l k sin θ k 1 (3.44) Wówczas: oraz w skróconej postaci: x k y k θ k = x k 1 y k 1 + (1 + q) θ k 1 x k y k θ k (3.45) s k = f(s k 1, u k, w k ) (3.46) = s k 1 + g( x k, y k, θ k, q) (3.47) Wyrażony za pomocą przyrostów model zadany równaniem (3.47) znacznie łatwiej wykorzystać w praktycznych implementacjach. Po obliczeniu nowej pozycji zgodnie z równaniem (3.40), ostatnim krokiem wyznaczenia pozycji jest normalizacja kąta θ k. Występujące w równaniu funkcje sinus oraz cosinus są co prawda okresowe, jednak przy braku normalizacji może dojść 27

40 do przekłamania wartości w związku ze skończoną dokładnością reprezentacji liczb zmiennoprzecinkowych w pamięci komputera. Jedną z metod normalizacji może być użycie poniższego wzoru: θ k = ( ( θ k + π ) mod 2π ) π (3.48) Tak znormalizowaną wartość zapisujemy następnie, jako nowe θ k. Wartość po normalizacji jest z zakresu [ π; π) Rzeczywisty model obserwacji Obserwacje są przeprowadzane względem jego bieżącej pozycji. W zależności od doboru układu współrzędnych, położenie odnalezionego w terenie znacznika, może być jednoznacznie określone jako wektor o jednej z dwu postaci: wektor współrzędnych kartezjańskich (x i, y i ), wektor współrzędnych biegunowych (r i, β i ). Laboratoryjny robot jest wyposażony w dalmierz laserowy, który dostarcza odczytów odległości do obiektów w otoczeniu robota w postaci serii współrzędnych biegunowych. Seria danych składa się z uporządkowanych par liczb, z których pierwsza wyznacza kąt pod jakim został dokonany skan, a druga zarejestrowaną odległość do obiektu w terenie (lub maksymalną odległość na jaką działa dalmierz, jeśli nie zarejestrował żadnego odczytu). Różnica kąta skanowania kolejnych dwu uporządkowanych par serii jest stała (dokładniejszy opis działania dalmierza znajduje się w podrozdziale ). Jest to najczęściej spotykany typ danych z czujników odległości. Rysunek 3.5 przedstawia przykład odnajdywania przez robota znacznika w terenie. W globalnym układzie odniesienia, w jakim porusza się robot, jego pozycję wyznacza uporządkowana trójka liczb [x r, y r, θ r ] T, wyznaczających odpowiednio współrzędne położenia na osi X r oraz Y r oraz orientację θ bazy jezdnej, względem układu OX r Y r. Znaleziony znacznik i jest widziany przez robota pod kątem β w odległości r, względem układu OX h Y h. Przyjmując [x i, y i ] T dla oznaczenia położenia i-tego znacznika w globalnym układzie odniesienia współrzędnych, rzeczywisty model obserwacji, rozpatrywany we współrzędnych biegunowych, przyjmie postać [20]: z i = h(s r, p i ) = zi r z i β = (x i x r ) 2 + (y i y r ) 2 atan ( y i y r x i x r ) θr + γ h (3.49) W równaniu (3.49) wyraz γ h wyraża niepewność zadaną w postaci szumu białego (identycznie, jak to było to wprowadzone w równaniu (3.31)). 28

41 Rysunek 3.5: Obserwacja znaczników przez robota 3.4. Wykorzystywane macierze w zadaniu SLAM W niniejszym podrozdziale przedstawiona została główna część macierzy stosowanych w zadaniu SLAM, które występują w większości stosowanych algorytmów wykorzystujących filtr Kalmana. Opis macierzy wyprzedza zdefiniowanie samych algorytmów, jednak pozwoli na sprawniejsze ich przedstawienie w kolejnych podrozdziałach Macierz kowariancji stanu P k Macierz kowariancji jest centralną macierzą dla algorytmów wykorzystujących filtrację Kalmana. Macierz można podzielić na kilka podmacierzy, jak na poniższym schemacie: P k = Pk r P r,1 k P r,n k ( ) P r,1 T k Pk 1 P 1,N k ( P r,n k ) T ( P 1,N k ) T P N k (3.50) Symbolem P r k oznaczona jest macierz kowariancji pozycji robota o rozmiarach 3 3 (co odpowiada trzem współrzędnym stanu). Przez P i k dla i = 1,..., N oznaczone są kowariancje położenia znaczników terenowych, które leżą na diagonali macierzy kowariancji; wymiary macierzy wynoszą 2 2. Kolejne macierze P r,i k o wymiarach 3 2 to macierze kowariancji pomiędzy pozycją robota, a położeniem znaczników. Ostatnimi elementami macierzy kowariancji P k są macierze kowariancji krzyżowych 29

42 pomiędzy dwoma różnymi znacznikami, oznaczone symbolami P i,j k, gdzie i j. Wymiary macierzy kowariancji krzyżowych wynoszą 2 2. Zgodnie z definicją macierz kowariancji jest symetryczna i dodatnio określona, stąd we wzorze (3.50) znajdujące się transpozycje. W momencie inicjalizacji algorytmu SLAM, wektor stanu robota nie zawiera pozycji żadnych znaczników, a macierz kowariancji dla tego stanu składa się jedynie z macierzy oznaczonej symbolem Pk r. Dobrze jest przyjąć [4, 24, 46] niezerowe wartości początkowe na diagonali tej macierzy. Niezerowa diagonala przede wszystkim zadaje podstawową niepewność współrzędnych pozycji robota. Ponadto dla diagonali z wartościami dodatnimi, macierz kowariancji staje się dodatnio określona Macierz wzmocnienia K k Macierz wzmocnienia Kalmana jest macierzą o rozmiarze 2 dim(s k ). Za pomocą macierzy wzmocnień jest przedstawiana wiara w poprawność położenia znaczników i ich wpływu na wektor stanu. Macierz wzmocnień można przedstawić w następującej postaci (dla jasności zapisu we wzorze (3.51) pominięte zostały indeksy dolne chwili czasowej k we wszystkich wartościach macierzy): x r x β y r y β θ r θ β x 1,r x 1,β K k = y 1,r y 1,β (3.51).... x N,r x N,β y N,r y N,β W wyniku mnożenia macierzy wzmocnień przez wektor innowacji otrzymujemy poprawkę do wektora stanu. Wielkość poprawki jest zależna od zaufania do informacji pochodzących z odometrii i wyznaczonego położenia znacznika. Wartości macierzy wzmocnień są większe, gdy wiara w wyznaczenie położenia znaczników jest większa, niż w dane odometryczne. Odwrotnie gdy czujnik i/lub metoda wyznaczenia położenia znaczników daje wyniki gorsze, niż uzyskane z odometrią, wartości macierzy są mniejsze. Pierwsza kolumna macierzy wzmocnień (3.51) przedstawia jak duży wpływ na poprawkę wektora stanu robota, ma pierwsza składowa wektora innowacji, tj. różnica pomiędzy uzyskaną z obserwacji, a wyznaczoną przewidywaną odległością od 30

43 robota do znacznika. Podobnie druga kolumna odnosi się do wpływu części wektora innowacji obrazującego niedokładność w wyznaczeniu kąta, pod jakim jest widziany znacznik, na wektor stanu Macierz kowariancji zakłóceń Q k Dla procesu SLAM przyjmuje się zakłócenie gaussowskie proporcjonalne do przyrostów zmiennych wektora stanu robota. Zakłócenie jest modelowane przez macierz o wymiarach 3 3. W literaturze poświęconej zagadnieniu SLAM można znaleźć kilka definicji macierzy kowariancji zakłóceń Q, stosowanej w kroku predykcji. W niniejszej pracy używana jest przedstawiona powyżej postać macierzy, którą można zastosować dla obu badanych algorytmów SLAM. Macierz Q wyznacza się ze wzoru: Q = s c s T, (3.52) gdzie s = [ t cos θ t sin θ θ] T, a c jest stałą, której wartość musi zostać dobrana eksperymentalnie i charakteryzuje jakość odometrii. Korzystając z przyrostów zmiennych wektora stanu, po wymnożeniu macierz Q w chwili k-tej przyjmie postać: Q k = c( x) 2 c x y c x θ c x y c( y) 2 c y θ c x θ c y θ c( θ) 2 (3.53) Macierz kowariancji zakłóceń pomiaru R k Macierz kowariancji zakłóceń pomiaru R k jest macierzą o wymiarach 2 2. Na diagonali macierzy są wartości proporcjonalne do wartości błędów używanego urządzenia oraz metody stosowanej do wyznaczenia położenia znaczników: R k = rc 0 0 bd (3.54) Wartość r określa zmierzoną odległość do znacznika i mnożona jest przez stałą c, której wartość należy wyznaczyć eksperymentalnie. Początkową wartość parametru c można przyjąć zgodnie z danymi dotyczącymi błędu stosowanego urządzenia. Iloczyn bd znajdujący się w macierzy R k powinien przyjąć wartość błędu dla wskazania kąta pod jakim widoczny jest znacznik. Typowo jest przyjmowana wartość 1, którą także należy potem dostroić. 31

44 3.5. Algorytm EKFSLAM Wstęp Filtr Kalmana, w swojej oryginalnej postaci, nie może być zastosowany w systemach, gdzie model estymacji stanu lub model obserwacji są nieliniowe. Pierwsza historycznie zaproponowaną i wdrożoną adaptacją filtru Kalmana dla modeli nieliniowych polegała na linearyzacji równań systemu wokół estymaty stanu oraz linearyzacji macierzy kowariancji. Metoda ta nazywana jest rozszerzonym filtrem Kalmana (ang. Extended Kalman Filter, w skrócie EKF). W rozszerzonym filtrze Kalmana przeprowadza się linearyzację, przez rozwinięcie funkcji nieliniowych w szereg Taylora. Wykorzystuje się jedynie pierwszy wyraz rozwinięcia w szereg. Takie rozwiązanie znalazło wiele zastosowań praktycznych, głównie przez wzgląd na szybkość działania, jednak rozwinięcie jedynie do pierwszego wyrazu dla funkcji silnie nieliniowych, w ogólności jest niewystarczające. Metoda rozszerzonego filtru Kalmana stała się także podstawą pierwszego algorytmu SLAM, jaki zaproponowali Smith i Cheesman [26] Algorytm rozszerzonego filtru Kalmana Algorytm EKF dla nieliniowego systemu, którego model można przedstawić w następującej postaci: s k = f(s k 1, u k, w k ) (3.55) z k = h(s k, v k ), (3.56) gdzie, jak poprzednio, s k oznacza stan systemu w chwili k-tej, przez u k oznaczone jest sterowanie, a z k oznacza przeprowadzane w systemie obserwacje. Symbole w k oraz v k opisują błędy w wyznaczeniu wartości stanu oraz obserwacji w postaci białych szumów, przy zerowych wartościach oczekiwanych, oraz rozkładach normalnych, odpowiednio, Q oraz R. Funkcje f oraz h, występujące w równaniach (3.55) oraz (3.56), modelują nieliniowość modelu. W praktyce nie ma możliwości wyznaczenia szumów dla kolejnych chwil czasowych, stąd aproksymację nieliniowych modeli rozpoczyna się od zadania zer dla wektorów szumów [43]: ŝ k = f(ŝ k 1, u k, 0) (3.57) zk = h(ŝ k, 0) (3.58) 32

45 Wyprowadzenie wzorów procesu estymacji stanu systemu, należy zacząć od zdefiniowania macierzy Jacobiego. Macierz Jacobiego A jest definiowana, jako pochodne cząstkowe funkcji f po zmiennych stanu s: A [i, j] = f [i] s [j] (ŝ k 1, u k, 0), (3.59) Macierz Jacobiego W jest definiowana, jako pochodne cząstkowe funkcji f względem elementów wektora szumu pomiarowego w: W [i, j] = f [i] w [j] (ŝ k 1, u k, 0), (3.60) Macierz Jacobiego H jest definiowana, jako pochodne cząstkowe funkcji h po zmiennych stanu s: H [i, j] = h [i] s [j] (ŝ k 1, 0), (3.61) Macierz Jacobiego V jest definiowana, jako pochodne cząstkowe funkcji h względem elementów wektora szumu pomiarowego v: V [i, j] = h [i] v [j] (ŝ k 1, 0), (3.62) Macierze Jacobiego, przedstawione w równaniach (3.59) - (3.62), nie są stałe w czasie. Stosowny indeks dolny k, określający chwilę czasu, został pominięty jedynie dla przejrzystości zapisu. Metoda EKF bezpośrednio wywodzi się z oryginalnego filtru Kalmana. Pomiędzy stosownymi wzorami zachodzi tutaj oczywista analogia, z dokładnością do niezbędnej linearyzacji modelu. Poniżej podano wszystkie równania stosowane w algorytmie rozszerzonego filtru Kalmana, przy przeprowadzaniu pojedynczej obserwacji: Faza predykcji: predykcja stanu wykonywana jest z użyciem modelu nieliniowego danego równaniem (3.57), a predykcja macierzy kowariancji wykorzystuje wyprowadzone poprzednio macierze Jacobiego, co wprowadza do rozwiązania wspomnianą linearyzację: ŝ k = f(ŝ k 1, u k, 0) (3.63) P k = A k P k 1 A T k + W k QW T k (3.64) Faza korekcji: podobnie jak poprzednio wyliczane są wektor innowacji, tym razem z wykorzystaniem nieliniowego modelu obserwacji (3.58), oraz macierz kowariancji stanu: ν k = z r k z k = zr k h(ŝ k, 0) (3.65) S k = H k P k HT k + V k RV T k (3.66) 33

46 następnie wyliczane są wzmocnienie Kalmana dla układu nieliniowego oraz przeprowadzana jest aktualizacja estymat z poprzedniej fazy: K k = P k HT k S 1 k (3.67) ŝ k = ŝ k + K kν k (3.68) P k = (I K k H k )P k (3.69) Związek pomiędzy równaniami (3.63) - (3.69), a przedstawionymi poprzednio (3.13) - (3.19), jest oczywisty. W istocie metoda EKF stanowi jedynie rozwinięcie filtru Kalmana dla systemów nieliniowych. Zastosowanie EKF do systemów liniowych daje identyczny przebieg wyznaczania estymaty stanu układu, co w oryginalnej wersji filtru Kalmana SLAM z użyciem EKF Wektor stanu systemu s k, czyli stanu robota, w zadaniu SLAM jest rozszerzony o położenie znaczników terenowych (patrz równanie (3.24)). Równanie przejścia zadane wzorem (3.45) zmienia jedynie pozycję robota, a nie zmienia położenia znaczników, które w algorytmach SLAM wykorzystujących filtr Kalmana uznawane są za stałe. Równania stosowane w fazie predykcji algorytmu EKF mają więc zastosowanie jedynie do części wektora stanu s k oraz macierzy kowariancji P k, które odnoszą się do pozycji robota. Małej zmianie muszą więc ulec równania fazy predykcji: Przede wszystkim funkcja f ŝ r,k = f(ŝ r,k 1, u k, 0) (3.70) P r, k = A k P r k 1A T k + Q k (3.71) opisuje zmianę stanu jedynie pierwszych trzech zmiennych wektora stanu s k, które wyznaczają bieżącą estymatę pozycji robota. Następnie aktualizowana jest część macierzy kowariancji odnosząca się do pozycji robota (patrz równość (3.50)). Wyraz W QW T występujący w algorytmie EKF zastąpiony został przez macierz Q k zdefiniowaną przez równość (3.53). Dodatkowym krokiem fazy predykcji staje się ponadto aktualizacja kowariancji krzyżowej pomiędzy pozycją robota, a położeniem znaczników, która obliczana jest według wzoru: P r,i, k = A k P r,i k 1 (3.72) 34

47 Analizując postać macierzy V (3.62) można zauważyć, że w zadaniu SLAM jest to macierz jednostkowa o wymiarze 2 2. Mnożenie przez macierz jednostkową nie zmieni wartości macierzy R k, można więc macierz V bezpiecznie pominąć w równaniach zadania SLAM. Korzystając z definicji macierzy (3.59) - (3.62) dla rzeczywistych modeli ruchu oraz obserwacji (patrz (3.40) oraz (3.49)), macierze stosowane w równaniach przyjmują następującą postać: macierz przejścia A: A = 1 0 y k 0 1 x k (3.73) macierz H wygodnie jest przedstawić wykorzystując dodatkowe podstawienie: x = x i,k x r,k y = y i,k y r,k = ( x ) 2 + ( y ) 2 Wówczas postać macierzy H można zapisać następująco: H = x y 2 y x x y y x (3.74) Algorytm 3.1 przedstawia zapis pseudokodu dla algorytmu EKFSLAM. 35

48 k 0 s 0 s pocz P 0 P pocz while true do k k + 1 pobierz nowe dane dla kroku k if zmiana pozycji then else s r,k f(s r,k 1, u k 1, 0) P r, k A k Pk 1A r T k + Q k P r,i, k = A k P r,i k 1 12 s k s k end P k P k 1 if nowa obserwacja then ν k z r k z k = zr k h(s k, 0) S k K k s k H k Pk HT k + R k Pk HT k Sk 1 s k + K kν k else P k s k end end P k (I K k H k )Pk s k Pk Algorytm 3.1: Pseudokod algorytmu EKFSLAM Wnioski Co podkreślono kilkukrotnie, algorytm filtru Kalmana w zadaniu SLAM musi być dostosowany do systemu nieliniowego. Linearyzacja modeli ruchu oraz obserwacji robota, wokół poprzedniej pozycji, pozwala zastosować algorytm rozszerzonego filtru Kalmana do zadania SLAM, jednak dzieje się to kosztem dokładności otrzymywanych wyników. Pomimo dobrych rezultatów, jakie można uzyskać z wykorzystaniem tej metody, EKF ma kilka istotnych ograniczeń. Po pierwsze, pojedyncza aktualizacja estymaty stanu wymaga czasu proporcjonalnego do kwadratu liczby rozpoznanych 36

49 znaczników, co czyni tę metodę trudną do zastosowania w czasie rzeczywistym, w bardzo dużych środowiskach. Ponadto, rzadko mamy do czynienia z dokładnymi modelami opisującymi odczyty z czujników, czy model ruchu. Dowiedzione zostało, że w przypadku naruszenia któregoś z tych założeń, algorytm EKF może nie być zbieżny [2]. Innym ograniczeniem tego podejścia jest niemożność reprezentacji wielomodalnego rozkładu pozycji robota, które często pojawia się w praktycznych sytuacjach z zastosowaniem filtrów cząsteczkowych. Wiele z bieżących badań zostało poświęconych próbom rozwiązania problemów związanych z algorytmem EKF. Przykładowe prace nad powyższymi problemami to np. dzielenie mapy na mniejsze podmapy [13] oraz przechowywanie stanu systemu w drzewie dla przyspieszenia obliczeń [23] Algorytm UKFSLAM Wstęp W rezultacie badań nad problemami algorytmu EKF, Julier i Ulhman, stworzyli algorytm UKF (ang. unscented Kalman filter) [37], którego podstawą działania jest transformata unscented. Jest to nowatorska metoda obliczania estymaty stanu nieliniowego systemu, której podstawową było spostrzeżenie autorów, że łatwiej jest aproksymować dystrybuantę rozkładu Gaussa, niż nieliniową funkcję [36] Transformata unscented Transformata unscented stanowi podstawę działania algorytmu UKF, będącego adaptacją filtru Kalmana dla systemów nieliniowych opisanych równaniami (3.55) oraz (3.56). Działanie transformaty modeluje Rysunek 3.6. Rysunek 3.6: Podstawa działania transformaty unscented. Źródło: [37]. 37

50 W pierwszej fazie wybrany zostaje reprezentatywny zbiór zmiennych losowych {X i }, nazywanych punktami sigma, które mają wspólną bazową wartość oczekiwaną, równą estymacie stanu s oraz macierz kowariancji P. Następnie punkt ze zbioru, przeprowadzany jest do nowego układu przy użyciu nieliniowego modelu stanu procesu. Z uzyskanych transformat uzyskiwana jest nowa wartość oczekiwana stanu systemu y wraz z macierzą kowariancji P y. Wybór punktów sigma przypomina działanie metod Monte Carlo, stosowanych w filtrach cząsteczkowych, jednakże pomiędzy tymi dwoma metodami istnieje istotna różnica. W miejsce losowego wyboru punktów stanowiących cząsteczki w algorytmach Monte Carlo, stosowana jest deterministyczna procedura wyboru punktów sigma. Poniżej przedstawiono algorytm doboru zmiennych losowych sigma X i, w podstawowej postaci. Wektor s jest stanem procesu o wymiarze dim(s) = n, a P jest odpowiadającą mu macierzą kowariancji. X 0 = s X i = s + X i = s ( ) (n + κ)p, dla i = 1,..., n (3.75) i ( ) (n + κ)p, dla i = n + 1,..., 2n, i n gdzie κ R jest czynnikiem skalującym określającym, jak daleko dany punkt sigma jest odsunięty od wartości oczekiwanej, a P jest i-tym wierszem lub kolumną zdekomponowanej macierzy kowariancji P. Zmiennym losowym ze zbioru {X i }, przypisywane są wagi, wyznaczające wpływ tych zmiennych na odtworzenie wartości stanu w późniejszej części. Wagi są znormalizowane, tak by spełniona była równość 2n i=0 W i = 1 i można je wyznaczyć w nastepujący sposób: W 0 = W i = κ n + κ 1 2(n + κ), dla i = 1,..., 2n (3.76) Przy powyższym wyborze punktów sigma, algorytm transformaty jest następujący: 1. Każda zmienna losowa sigma ze zbioru reprezentatywnego, podlega przekształceniu zgodnie z funkcją zmiany stanu, tworząc nowy zbiór punktów {Y i }: Y i = f(x i ) 38

51 2. Obliczana jest nowa wartość oczekiwana stanu systemu y, zgodnie ze wzorem: y = 2n i=0 W i Y i 3. Na koniec obliczana jest nowa macierz kowariancji: P y = 2n i=0 W i (Y i y)(y i y) T Wśród szczególnych właściwości algorytmu należy wymienić: Obliczane metodą transformaty unscented wartość estymaty stanu oraz macierzy kowariancji zawierają informacje z rozwinięcia w szereg Taylora do drugiego rzędu włącznie (linearyzacja metody EKF zawiera jedynie informacje pierwszego rzędu). W przypadku szczególnych postaci funkcji f transformata unscented może zachowywać informacje wyższych rzędów. Transformata unscented nie linearyzuje funkcji, nie ma więc potrzeby wyznaczania macierzy Jacobiego, które dla szczególnych typów funkcji mogą nie istnieć. W algorytmie są wykorzystywane jedynie proste operacje na wektorach oraz macierzach. Do wyznaczenia pierwiastka macierzy kowariancji, tj. (n + κ)p, można wykorzystać algorytmy stabilne numerczynie, jak np. faktoryzację Cholesky ego [30]. Dekompozycja macierzy P wykonywana jest z wykorzystaniem algorytmu LL T, została przedstawiona w Dodatku A. Występujący w równaniach transformaty parametr κ może posłużyć do dodatkowego dostrojenia algorytmu, w celu zrównoważenia błędów modelu stanu systemu. Chociaż κ może przyjąć dowolną wartość, wartości ujemne nie są pożądane, gdyż mogą doprowadzić do sytuacji, w której estymowana macierz P y przestanie być dodatnio określona, co wprowadza dodatkowe problemy przy wyznaczaniu jej pierwiastka. Szczegółowe właściwości algorytmu transformaty unscented, w tym dowody poprawności, można znaleźć w [38] oraz [36] Algorytm filtru UKF Stosowana w algorytmie UKF transformata unscented wykonywana jest w każdym kroku algorymu, dla każdej z jego faz, tj. dla fazy predykcji oraz fazy aktualizacji. Narzut obliczeniowy tej metody jest więc znacznie większy, niż dla algorytmu EKF. Identycznie, jak w przypadku algorytmu EKF, dla szumów procesowego oraz pomiarowego przyjmowane są wartości zerowe w trakcie przeprowadzania obliczeń. 39

52 Poniżej przedstawiony został, podzielony na dwie fazy (podobnie jak algorytm EKF) algorytm filtru Kalmana z transformatą unscented [10, 35]: Faza predykcji rozpoczyna się od wyznaczenia reprezentatywnego zbioru punktów sigma {X i,k 1 : i = 0,..., 2n}: X 0,k 1 = ŝ k 1 X i,k 1 = ŝ k 1 + X i,k 1 = ŝ k 1 ( (n + κ)p k 1 ) ( (n + κ)p k 1 ) wraz z odpowiadającymi im wagami: W 0,k = W i,k = κ n + κ 1 2(n + κ) i i n, dla i = 1,..., n (3.77), dla i = n + 1,..., 2n, dla i = 1,..., 2n (3.78) Dla każdej zmiennej losowej sigma obliczane jest przejście przez funkcję zmiany stanu systemu f, zgodnie z równaniem (3.55), tworząc nowy zbiór {X i,k k 1 }: X i,k k 1 = f(x i,k 1, u k, 0), dla i = 0,..., 2n (3.79) Estymowany stan systemu jest obliczany zgodnie ze wzorem: ŝ k = 2n i=0 W i X i,k k 1 (3.80) Na koniec wyznaczana jest macierz kowariancji z wykorzystaniem punktów sigma: P k = 2n i=0 W i [X i,k k 1 ŝ k ][X i,k k 1 ŝ k ]T + Q k, (3.81) gdzie macierz Q k jest macierzą kowariancji błędu zmiany stanu. Faza korekcji podobnie jak faza predykcji rozpoczyna się od wyznaczenia zbioru punktów sigma: X 0,k k 1 = ŝ k ) X i,k k 1 = ŝ k ( (n + + κ)pk ) X i,k k 1 = ŝ k ( (n + κ)pk i i n, dla i = 1,..., n (3.82), dla i = n + 1,..., 2n a następnie wyznaczenia predykcji obserwacji dla wykonanego pomiaru: Y i,k k 1 = h(x i,k k 1, 0) (3.83) zk = 40 2n i=0 W i Y i,k k 1 (3.84)

53 Wektor innowacji oraz stosowna macierz kowariancji innowacji obliczane są za pomocą wzorów: ν k = zk r zk (3.85) S k = 2n i=0 W i [Y i,k k 1 z k ][Y i,k k 1 z k ]T + R k (3.86) Następnie w algorytmie UKF wyznaczana jest macierz kowariancji krzyżowej, pomiędzy stanem układu, a obserwacją: P x y,k = 2n i=0 W i [X i,k k 1 s k ][Y i,k k 1 z k ]T (3.87) Końcowa aktualizacja estymaty stanu oraz macierzy kowariancji wykonywana jest zgodnie z poniższymi wzorami: K k = P x y,k S 1 k (3.88) ŝ k = ŝ k + K kν k (3.89) P k = P k K ks k K T k (3.90) SLAM z użyciem UKF Zastosowanie algorytmu UKF do zadania SLAM nie wymaga przeprowadzenia żadnych wstępnych kroków (jak np. wyznaczenie macierzy pochodnych, jak ma to miejsce w algorytmie EKF), a jedyne wzory nie uwzględnione w poprzednim podpunkcie to modele ruchu robota oraz obserwacji, które znajdują się w części 3.3. Dla macierzy kowariancji błędów Q k oraz R k użytych w równaniach algorytmu UKF w rozwiązaniu zadaniu SLAM przyjęte zostały wartości proporcjonalne (odpowiednio) do wykonanego przesunięcia oraz dokonanego pomiaru (zgodnie z definicjami zadanymi równościami (3.53) oraz (3.54). Pseudokod algorytmu UKFSLAM znajduje się w listingu Wnioski W algorytmie UKF nie stosuje się linearyzacji, lecz korzysta się bezpośrednio z nieliniowej funkcji zmiany stanu. Udowodniono [42], że transformata unscented dla modeli silnie nieliniowych, jak np. modele ruchu i obserwacji robota w zadaniu SLAM, są przybliżane przez wartości z dokładnością do drugiego wyrazu rozwinięcia w szereg Taylora. W literaturze znajdują się liczne przykłady poprawy działania filtru Kalmana, wykorzystującego transformatę unscented, zamiast linearyzacji, jak ma to miejsce w przypadku rozszerzonego filtru Kalmana. 41

54 k 0 s 0 s pocz P 0 P pocz while true do k k + 1 konstruuj zbiór punktów sigma X i,k 1 (i = 0,..., 2n) wyznacz wagi dla punktów sigma pobierz nowe dane dla kroku k if zmiana pozycji then else for i = 0; i <= 2n; i + + do end s k P k X i,k k 1 f(x i,k 1, u k 1 ) 2n i=0 W i X i,k k 1 2n i=0 W i [X i,k k 1 s k ][X i,k k 1 s k ]T for i = 0; i <= 2n; i + + do end X i,k k 1 = X i,k 1 19 s k s k end P k P k 1 if nowa obserwacja then else for i = 0; i <= 2n; i + + do end z k ν k S k P x y,k K k s k Y k k 1 h(x i,k k 1 0) 2n i=0 W i Y i,k k 1 zk r zk 2n i=0 W i [Y i,k k 1 zk ][Y i,k k 1 zk ]T 2n i=0 W i [X i,k k 1 s k ][Y i,k k 1 zk ]T P x y,k Sk 1 s k + K kν k P k P k K ks k K T k end end s k P k s k Pk Algorytm 3.2: Pseudokod algorytmu UKFSLAM. 42

55 Niewątpliwym minusem, w stosunku do algorytmu EKF, jest nakład obliczeniowy, jakiego wymaga transformata unscented. Konstrukcja zbioru punktów sigma i przeprowadzenie ich wszystkich przez model ruchu, wymaga dodatkowych cykli obliczeniowych procesora. Algorym UKF w zastosowaniu do zadania SLAM jest wciąż badany, a liczba prac na ten temat stale rośnie. Z algorytmem tym wiąże się spore nadzieje szczególnie w tych miejscach, gdzie EKF jest zawodny Obserwacje i integracja znaczników Ponowna obserwacja znaczników Obserwacja i-tego znacznika p i polega na wyznaczeniu jego współrzędnych, w większości przypadków we współrzędnych kartezjańskich w układzie związanym z robotem, co możemy oznaczyć przez z r i (bez indeksu dolnego chwili czasowej k dla jasności zapisu). Rzeczywisty model obserwacji zdefiniowany równaniem (3.49) operuje na współrzędnych w układzie biegunowym. Przejście pomiędzy układem kartezjańskim, a biegunowym, można wykonać wykorzystując proste przekształcenie: z r i = l z r i θ z r i (x r i ) 2 + (yi r ) 2 ( = y r ) atan i, (3.91) x r i gdzie x r i oraz y r i oznaczają współrzędne badanego znacznika we współrzędnych kartezjańskich. Wyznaczony za pomocą równości (3.91) wektor obserwacji we współrzędnych biegunowych nadaje się do zastosowania w przedstawionych poprzednio równaniach zadania SLAM. W przedstawionych do tej pory równaniach obserwacji, występowało niejawne założenie o możliwości obserwacji pojedynczego znacznika w danej chwili czasowej. W rzeczywistości w każdym kroku ilość zaobserwowanych znaczników, może być znacznie większa, niż jeden. W celu przeprowadzenia fazy aktualizacji dla kilku znaczników niezbędne staje się wyprowadzenie rozszerzonych równań. 43

56 Dla n zaobserwowanych znaczników w kroku k, stosowne wektory i macierze przyjmują nastepującą postać: z k = h k = R k = z k 1 z 2 k (3.92). zk n h 1 k h 2 k (3.93). h n k R k R 2 k , (3.94). 0 0 Rk n gdzie indeksy górne od 1 do n odpowiadają kolejnym odnalezionym znacznikom, o numeracji niekoniecznie zgodnej z tą z wektora stanu s k. Dla algorytmu EKFSLAM zmianie ulega także macierz Jacobiego obserwacji H k zadana równością (3.74), w której pojawiają się kolejne pary wierszy odnoszące się do kolejnych znaczników; wynika to bezpośrednio z powyższej definicji rozszerzonego wektora h k. Powiększona zostanie jeszcze macierz kowariancji innowacji S k, jednak wynika to bezpośrednio z metody jej obliczania i nie wymaga wprowadzania dodatkowych definicji. Dalsza część równań algorytmu EKFSLAM pozostaje bez zmian. W algorytmie UKFSLAM zmianie ulegnie liczba tworzonych wektorów obserwacji dla punktów sigma zdefiniowanych wzorem (3.84), muszą zostać wyznaczone dla każdego odnalezionego znacznika. Łączna liczba wektorów obserwacji jest więc iloczynem liczby odnalezionych znaczników oraz liczby punktów sigma. Obok liczby wektorów obserwacji, zmianie muszą ulec dwie macierze: kowariancji innowacji S k oraz kowariancji skośnej P x, y. Macierz innowacji przyjmie postać: S k i S j S k = k , (3.95). 0 0 Sk l gdzie kolejne podmacierze Sk x dla x = i, j,..., l, są obliczane dla odnalezionego znacznika x, zgodnie ze wzorem (3.86). Z kolei macierz kowariancji skośnej otrzymujemy 44

57 poprzez złączenie macierzy kowariancji skośnych dla kolejnych odnalezionych znaczników: P x y,k = [ ] Px i y,k P j x y,k Px l y,k (3.96) Pozostałe macierze albo nie zmieniają swojego rozmiaru, albo jest on w pełni zależny od powyższych zależności Integracja nowych znaczników w wektorze stanu Dodanie nowego znacznika p N+1 do wektora stanu, powiększa wymiar nie tylko samego wektor s, ale również macierzy kowariancji P [39]. W pierwszym kroku wyznaczana jest pozycja znacznika w globalnym układzie odniesienia. Korzystając z wyznaczonej pozycji znacznika we współrzędnych kartezjańskich w układzie robota, pozycja znacznika może zostać wyznaczona z zależności: p N+1 = m(s r, zi r ) = x r + x r i cos(θ r ) yi r sin(θ r ), (3.97) y r + x r i sin(θ r ) + yi r cos(θ r ) gdzie s r = [x r y r θ r ] T oznacza pozycję robota w chwili dokonania obserwacji znacznika z r i = [x r i y r i ] T. Wyznaczona w ten sposób pozycja znacznika w globalnym układzie odniesienia, czyli tym, z którym związany jest robot, zostaje dodana na koniec wektora stanu s: ŝ = ŝ p N+1 (3.98) W celu zwiększenia macierzy kowariancji P zdefiniowane muszą zostać dwie specyficzne dla zadania macierze, które nie występowały we wcześniejszych rozważaniach dotyczących algorytmów wykorzystujących filtr Kalmana. Pierwszą z nich jest macierz J xr, która przedstawia predykcję wyznaczenia położenia znacznika: J xr = 1 0 xr i sin(θ r ) yi r cos(θ r ) (3.99) 0 1 x r i cos(θ r ) yi r sin(θ r ) Drugą specyficzną macierzą stosowaną przy integracji nowego znacznika jest macierz J z : J z = cos(θ r) sin(θ r ) sin(θ r ) cos(θ r ) (3.100) Macierze J xr oraz J z to macierze Jacobiego funkcji m wyznaczenia pozycji znacznika zdefiniowanej we wzorze (3.97) ze względu na wektory stanu robota oraz obserwacji. Następnie macierz kowariancji P jest poszerzana do wymiaru 3 + 2(N + 1). Kolejne podmacierze (porównaj punkt ) zgodnie z poniższymi równaniami: P N+1 = J xr P r Jx T r + J z RJz T, (3.101) 45

58 P r,n+1 = ( P N+1,r) T = Jxr P r, (3.102) P N+1,i = ( P i,n+1) T = Jxr P r,i. (3.103) Asocjacja danych w zadaniu SLAM Problem asocjacji (kojarzenia) danych polega na dopasowaniu uzyskanych za pomocą czujnika wskazań o znacznikach z istniejącą bazą znaczników. Asocjacja danych jest więc elementem ponownej obserwacji znacznika. W praktyce w rozwiązaniu zadania asocjacji danych mogą wystąpić następujące problemy [24]: możliwe jest, że znacznik choć powinien nie zostanie zaobserwowany; możliwa jest obserwacja znacznika, który nigdy więcej już nie zostanie zaobserwowany; możliwe jest błędne przypisanie obserwacji do niewłaściwego znacznika z bazy. Problemy te mogą wynikać z kilku przyczyn, m.in. słabej jakości czujnika oraz błędnie wybranych znaczników. Dobrze dobrane znaczniki powinny być łatwo identyfikowalne i rozróżnialne między sobą. W przypadku błędnej asocjacji danych, robot wykona fazę aktualizacji algorytmu SLAM i obliczy poprawkę do pozycji, która może zostać uznana za poprawną. Oczywiście wielkość obliczonej korekty zależy od stopnia zaufania do czujnika i metody wyznaczania znaczników, jednak dla względnie dobrej odometrii i słabego pomiaru, algorytm stoi w sprzeczności z założeniami stosowalności filtru Kalmana. W badanich zrealizowana została asocjacja pomiaru do znacznika wykorzystując podejście bramkujące (ang. gating approach) w przestrzeni wektora innowacji [3, 46], które łączy niepewność w wyznaczeniu znacznika z niepewnością stanu robota. Macierz innowacji S i definiowana jest wzorem [46]: S i = H xi P r P r,i P i,r P i Hx T i + R i, (3.104) gdzie macierz kowariancji odpowiadająca przestrzeni robota, składa się z tych elementów pełnej macierz kowariancji P k, które dotyczą pozycji robota oraz badanego znacznika. Podobnie macierz Jacobiego H xi w powyższym wzorze jest obliczana dla wektora stanu obciętego jedynie do zmiennych odnoszących się do pozycji robota oraz badanego znacznika: H xi = h i([x r x i ] [x r x i ] 46 (3.105)

59 Wykorzystając wzory (3.104) oraz (3.105) dla wektora innowacji ν k, bramkowanie pomiaru definiowane jest wzorem: ν k S 1 i ν k λ (3.106) Dla systemu z dwoma stopniami swobody, wartość λ = 9.0 wyznacza obszar o minimalnym rozmiarze, w którym pomiar może zostać dopasowany do znacznika z prawdopodobieństwem 98,9% [46]. Przedstawiona procedura walidacji bramkującej definiuje obszar, w którym powinna znaleźć się obserwacja. Najprostszą metodą inicjalizacji nowego znacznika, znajdującą zastosowanie w praktycznych rozwiązaniach, jest metoda najbliższego sąsiada (ang. nearest neighbour), po raz pierwszy przedstawiona w pracy doktorskiej [7]. Po wyznaczeniu asocjacji danych, pomiary, które nie zostały przypisane do żadnego ze znanych znaczników, stanowią potencjalne nowe znaczniki. W kolejnych krokach dopasowywania pomiarów do znaczników, sprawdzane jest czy potencjalny znacznik został ponownie zaobserwowany. Kiedy ilość obserwacji dla potencjalnego znacznika przekroczy wartość β N, jest on dodawany do wektora stanu, zgodnie z procedurą opisaną we wcześniejszej części niniejszego podrozdziału. Dopasowanie pomiarów do potencjalnego znacznika odbywa się przy pomocy bramkowania, zadanego nierównością (3.106). W przypadku, gdy potencjalny znacznik nie został zaobserwowany ponownie przez ilość d kroków pomiarowych, jest on zapominany. Procedura dodawania nowego znacznika do wektora stanu, dopiero w momencie gdy został on sprawdzony pozwala nie powiększać bez potrzeby wielkości stosowanych wektorów oraz macierzy kowariancji. Jest o tyle istotne, że w algorytmach SLAM wykorzystujących filtr Kalmana, usuwanie znaczników nie powinno być przeprowadzane, chociaż istnieją techniki, które próbują obejść ten problem, jak np. opisane w [4, 6, 46] Wizualizacja z użyciem mapy siatki zajętości W definicji zadania SLAM, opisanej wzorem (2.1), symbolem Θ oznaczona jest mapa, budowana w trakcie działania algorytmów. W przypadku metod wykorzystujących filtr Kalmana, termin mapa odnosi się jedynie do mapy znaczników, jakie zostały znalezione w środowisku. Oczywistym jest, że informacja jedynie o położeniu znaczników (i dodatkowo o jakichś ich charakterystycznych) nie pozwala na dokładną rekonstrukcję środowiska, jak również czyni taką mapę mało użyteczną do wykorzystania w większości kolejnych warstw systemu, jak np. w planowaniu ścieżki, czy unikaniu kolizji. 47

60 W celu uzyskania znacznie dokładniejszej, wiarygodnej mapy terenu, niezbędne staje się użycie dodatkowych metod, które pozwalają na pełniejsze odwzorowanie badanego terenu. Najczęściej stosowaną techniką jest budowa map siatek zajętości, którym poświęcony został niniejszy podrozdział Wstęp W ciągu ostatnich dwudziestu lat, mapy siatki zajętości stały się dominującym podejściem modelowania środowiska w robotyce mobilnej [32]. Za pomocą siatek zajętości środowisko jest przedstawione w postaci zdyskretyzowanych obszarów, zwanych komórkami. Za pojedynczy obszar najczęściej przyjmuje się stałej wielkości kwadrat terenu, co w znacznej mierze upraszcza obliczenia. Każdemu spośród kwadratów przypisywana jest wartość, oznaczająca prawdopodobieństwo zajętości danej części terenu. W najczęstszej postaci mapy siatki zajętości przedstawiają środowisko w postaci mapy dwuwymiarowej, podzielonej na jednakowe komórki. Taki typ reprezentacji jest całkowicie wystarczający, jeśli robot porusza się po powierzchni płaskiej. Budowa mapy siatki zajętości jest procesem kosztownym obliczeniowo. W swojej podstawowej postaci, algorytm w każdej iteracji działa na zbiorze wszystkich punktów mapy, co niepotrzebnie wydłuża jego działanie. Prosta optymalizacja liczby punktów podlegających aktualizacji, polegająca na zawężaniu zbioru punktów jedynie do tych spośród nich, które mogą być w danym momencie widoczne przez robota (co wynika bezpośrednio z parametrów technicznych zastosowanego czujnika), może znacznie zredukować czas działania algorytmu. Ważną kwestią, także z punktu widzenia złożoności obliczeniowej, jest nierozerwalność procesów budowy mapy, od działania algorytmu SLAM. Mapy siatki zajętości są bowiem czułe, na przeprowadzane przez algorytmy SLAM korekty, jakich mogą one dokonywać w krokach poprzednich. Wówczas mapa musi zostać w całości przebudowana, co znacznie wykorzystuje zasoby obliczeniowe komputera. Problem można częściowo rozwiązać, stosując przechowywanie mniejszych, lokalnych map, związanych z danym punktem ścieżki i łączenie, takich mniejszych map, w jedną większą. Mapy pośrednie nie podlegają wówczas aktualizacji, zmienia się jedynie ich punkt zaczepienia na mapie globalnej. Mapa będąca rezultatem działania algorytmu budowania jest ponownie sklejana z map pośrednich. Należy jednak zauważyć, że podejście to odznacza się znacznie większym zapotrzebowaniem na pamięć, jaka jest potrzebna w celach przechowywania każdej lokalnej mapy oddzielnie. 48

61 a) b) Rysunek 3.7: Przykłady budowania map siatek zajętości. Po lewej mapa a) zbudowana w oparciu o bezpośrednie dane, pochodzące z czujników. Po prawej mapa b) zrekonstruowana z użyciem algorytmu SLAM. Źródło: [15]. Przykład budowy mapy terenu bez korzystania z wyników algorytmów SLAM oraz z ich pomocą, można pokazać za pomocą prostych Rysunków (3.7). Rysunek po lewej stronie prezentuje mapę siatki zajętości, zbudowaną w oparciu o dane pochodzące bezpośrednio z czujników, w jakie wyposażony był robot: z dalmierza laserowego oraz enkoderów optycznych. Te same dane, przefiltrowane przez algorytm SLAM, pozwoliły otrzymać mapę, która znajduje się na prawym rysunku. Oczywistym staje się, iż mapa budowana jedynie na podstawie danych pochodzących z czujników robota jest bardzo niedokładna, co w znacznym stopniu wynika z niedokładności enkoderów Algorytm konstruowania mapy siatki zajętości W celu rozróżnienia mapy budowanej przez algorytm SLAM, dla map siatek zajętości przyjęte będzie oznaczenie m. Wszystkie algorytmy budowy mapy siatki zajętości bazują na obliczeniu prawdopodobieństwa zajętości komórek mapy m. Symbolicznie można to zapisać w następujący sposób [34]: p(m z 1:k, s 1:k ), gdzie przez z 1:k oznaczone są wszystkie obserwacje, aż do chwili k-tej włącznie, jakich dokonano w stosownych punktach przebytej ścieżki s 1:t. W powyższym zapisie nie występuje zbiór sterowań robota u 1:k, jako że nie mają one bezpośredniego wpływu na kształt budowanej mapy. Przyjmując dodatkowo, że każda obserwacja z j jest ściśle związana z pozycją robota s j, funkcje prawdopodobieństwa można uprościć do postaci [32]: p(m z 1:k ) 49

62 Mapa m jest następnie dyskretyzowana na wiele małych komórek: m = {m i } Każda komórka oznaczona jako m i, reprezentuje ściśle określoną część terenu. Komórkom jest przypisywana wartość prawdopodobieństwa, oznaczająca przypuszczalną zajętość obszaru. Obszar terenu odpowiadający komórce m i, jest uznawany za na pewno zajęty, gdy p(m i ) = 1, oraz, adekwatnie, za na pewno wolny, gdy p(m i ) = 0. Wykorzystując rozkłady prawdopodobieństwa poszczególnych komórek, mapę wyznacza się za pomocą równości: p(m z 1:k ) = i p(m i z 1:k ) (3.107) Ze względów obliczeniowych, prawdopodobieństwa zajętości komórek zwyczajowo przechowuje się w postaci logarytmicznej: l k i = ln p(m i z 1:k ) 1 p(m i z 1:k ) (3.108) Ponownie ze względów obliczeniowych, zajętość komórki możemy wyznaczyć korzystając z rekurencyjnej zależności danej wzorem [32]: l k i = ln p(m i z k ) 1 p(m i z k ) l0 i + l k 1 i, (3.109) gdzie pierwszy czynnik po prawej stronie wyznaczany jest najczęściej wykorzystując odwrotny model czujnika (ang. inverse sensor model), przez li 0 oznaczana jest wartość bazowego prawdopodobieństwa zajętości komórki, w postaci logarytmicznej: l 0 i = ln p(m i ) 1 p(m i ) (3.110) Wartość początkowej zajętości komórki jest przyjmowana najczęściej z przedziału [0, 2; 0, 5]. Symbol li k 1 w równości (3.109) oznacza wartość uzyskaną w kroku poprzedzającym. Psuedokod algorytmu wyznaczania wartości odwrotnego modelu czujnika został podany w punkcie 5.7. Szukaną wartość prawdopodobieństwa z postaci logarytmicznej, uzyskuje się poprzez proste przekształcenie: p(m i z k 1 ) = 1 + [ ] 1 (3.111) e lk i Wzór (3.109) pokazuje, że przedstawiony algorytm budowy mapy siatki zajętości jest algorytmem rekurencyjnym, co jest zgodne z naturą większości algorytmów zadania SLAM. Szczegółowe wyprowadzenie wzorów stosowanych przy wyznaczaniu siatki zajętości i ich analizę można odnaleźć w [32]. 50

63 Wnioski Przy faktoryzacji mapy zadanej równaniem (3.107), zajętość jednej komórki, nie wpływa w żaden sposób na informację o zajętości innej komórki. Sytuacja taka nie zawsze jest prawdziwa. Nasuwa się bezpośrednie skojarzenie do gry w statki, gdzie poprzez informację o znalezieniu jednej z części okrętu, w kratkach leżących w bezpośrednim sąsiedztwie, poszukiwane były następne części tego okrętu. Podobnie możemy patrzeć na zdyskretyzowane środowisko robota. Przy dostatecznie małych rozmiarach komórek, istnieje duża szansa, iż dla zajętej komórki, komórka znajdująca się w bezpośrednim sąsiedztwie także będzie zajęta. Uwzględnienie takiej informacji prowadziłoby jednak do znacznej komplikacji obliczeń, już i tak czasochłonnych. W mapach siatek zajętości ten problem jest pomijany. Drugim ograniczeniem wynikającym z faktoryzacji zadania budowy mapy, jest brak możliwości integracji danych pochodzących z różnych czujników, o różnych parametrach, jak np. z dalmierza laserowego oraz czujników zbliżeniowych podczerwieni. Problem ten przedstawił, i zaproponował jego rozwiązanie, Thrun w [32]. W publikacjach znaleźć można także opisy budowy map siatek zajętości o zmiennej wielkości komórek. Rozmiary komórek są mniejsze w pobliżu zajętych części badanego terenu, co pozwala na znacznie dokładniejszą reprezentację środowiska. W związku z dużym nakładem obliczeniowym, jakiego wymaga algorytm budowy mapy siatki zajętości, proces ten może być efektywnie realizowany jedynie w wyższych warstwach aplikacji sterujących robotem, a najczęściej przeprowadzany jest w trybie off-line. Dokładna mapa terenu jednak, służy głównie zadaniom warstw wyższych, które z definicji są także procesami realizowanymi w trybie off-line, więc taka realizacja budowy siatki zajętości, nie wprowadza dodatkowych problemów, dla zadania nawigacji robotem mobilnym. 51

64 Rozdział 4. Środowisko uruchomieniowe 4.1. Platforma sprzętowa Roboty mobilne, z definicji, są tworzone jako pojazdy mechaniczne mogące przemieszczać się w terenie. Możliwości przemieszczania mogą być ograniczone konstrukcją robota oraz charakterystyką terenu, np. robot mający kształt małego samochodu nie jest w stanie podjechać na wysokie stopnie schodów. Konstrukcja robotów mobilnych najczęściej przypomina budową samochody. Jednym z parametrów, jaki charakteryzuje takie roboty jest typ bazy jezdnej, w jaką wyposażony został robot. Szczegółowe przedstawienie różnych typów baz można odnaleźć w [28] Robot mobilny Protonek Robot Protonek, którego użyto do weryfikacji badanych w tej pracy algorytmów posiada bazę jezdną podobną do tej stosowanej w czołgach (Rysunek 3.3). Koła robota nie są jednak połączone gąsienicami. Napęd jest przekazywany do wszystkich kół przez zakryte w obudowie pasy. Charakterystyka takiej bazy może być dość dobrze przybliżona przez różnicową bazę jezdną. Na środkowych kołach robota zamocowane są enkodery optyczne, służące do pomiaru kątów obrotu kół. Rozdzielczość zainstalowanych enkoderów pozwala badać obrót koła z dokładnością do 4000 impulsów na obrót. Stosowne wzory na obliczanie przebytej drogi, przy użyciu enkoderów optycznych można odnaleźć w [28]. Szczegółowy opis robota Protonek można znaleźć w [41]. 52

65 Rysunek 4.1: Robot laboratoryjny Protonek Dalmierz laserowy SICK LMS200 Jednym z najważniejszych elementów wyposażenia robota są czujniki odległości. Dokładność oraz możliwości (jak np. zmiana pozycji skanowania) czujników decydują jakie zadania, robot jest w stanie zrealizować. Do najczęściej obecnie stosowanych czujników należą dalmierze laserowe, które zapewniają zdecydowanie większą dokładność przy dużej odległości, niż większość innych dostępnych czujników. Obecnie rozwijane są dwa typy dalmierzy laserowych: 2D LADAR (LADAR jest akronimem od ang. Laser Detection and Ranging) oraz 3D LADAR. Pierwszy typ stanowią najliczniejsze obecnie dalmierze przeznaczone do skanowania przestrzeni dwuwymiarowej. Drugi typ to zdecydowanie droższe urządzenia, które realizują trójwymiarowe skanowanie terenu; do głównych zastosowań tak rozbudowanych skanerów, należy np. skanowanie środowiska podczas pracy na urządzeniach latających jak samoloty czy helikoptery. Laboratoryjny robot opisany poprzednio wyposażony jest w dalmierz laserowy Sick LMS200. Pomiar dokonywany przy użyciu tego czujnika realizowany jest na zasadzie czasu powrotu wiązki podczas skanowania (ang. time-fo-flight, w skrócie TOF). Zasadę działania przedstawia Rysunek 4.3. Nadajnik emituje pulsacyjnie 53

66 Rysunek 4.2: Dalmierz laserowy Sick LMS200. Źródło: [45]. wiązki laserowe. Po odbiciu się od obiektu wiązka wraca padając na lustro obrotowe. Znajdujący się nad lustrem odbiornik po wykryciu wiązki, dokonuje obliczenia czasu, po jakim wiązka wróciła do skanera po wyemitowaniu. Czas ten z kolei przeliczany jest na odległość do znajdującej się w terenie przeszkody. Wiązka lasera zmienia kąt skanowania terenu, poprzez przestawienie obrotowego lustra. Rysunek 4.3: Zasada działania dalmierza laserowego Sick LMS200. Źródło: [45]. Kąt skanowania lasera wynosi 180 stopni. Rozdzielczość kątowa skanowania może być jedną z następujących wartości: 1, 0,5 lub 0,25 stopnia, jednakże rzeczywisty krok skanowania wynosi zawsze 1 stopień, a bardziej dokładne wyniki uzyskiwane są z wykorzystaniem skanowania z przeplotem (odpowiednio za pomocą 2 lub 4 takich skanowań). Liczba wartości zwracanych podczas skanowania w trybie 180 stopni, wynosi 181. Efektywny zasięg dalmierza 8 metrów, przy błędzie rzędu 5 milimetrów. Wśród prac poświęconych badaniu właściwości tego dalmierza wyróżnić można [45]. Autorzy dokonali serii badań dokładności dalmierza, potwierdzając wartość błędu zadaną w specyfikacji. W pracy zawarty został dokładny opis przeprowadzonych badań oraz opis metody kalibracji dalmierza. 54

67 4.2. Oprogramowanie wykorzystane w pracy Oprogramowanie stworzone do weryfikacji tez zawartych w niniejszej pracy, korzysta z kilku projektów dostępnych w Internecie. Ogólna struktura stworzonego oprogramowania wygląda następująco: Rysunek 4.4: Ogólny schemat oprogramowania wykorzystywanego w niniejszej pracy. W niniejszym podrozdziale przedstawione zostanie ta część oprogramowania, która nie została stworzona przez autora niniejszej pracy. Opisowi programu robota (znajduje się w centralnej części na Rysunku 5.10) wraz z opisem implementacyjnym, zostanie poświęcony następny rozdział Środowisko Player/Stage Projekt Player/Stage jest projektem otwartym (ang. open source) zapewniającym narzędzia ułatwiające tworzenie aplikacji do sterowania robotem, czy też agentem wielorobotowym. Całe środowisko, dokumentacja oraz dodatkowe oprogramowanie dostępne jest na stronie projektu [52], a dystrybuowane jest na zasadach określonych w publicznej licencji GNU (ang. GNU GPL), której treść można znaleźć w źródłach projektu oraz na stronach projektu GNU [51]. Dwa główne programy tego pakietu, od których pochodzi nazwa projektu, to: Player - serwer zarządzający urządzeniami robota, Stage - symulator wielorobotowy. 55

68 Oprócz wyżej wymienionych, w plikach źródłowych znajdują się ponadto wszystkie niezbędne biblioteki. Uruchomiony na robocie, Player dostarcza prostego i przejrzystego interfejsu programistycznego dla wszystkich czujników i efektorów, jakie są dla niego dostępne. Programy klienckie, np. aplikacje sterujące ruchem, do porozumiewania się z Playerem wykorzystują protokół internetowy gniazd TCP/IP. W podstawowej postaci Player dostarcza niezbędne sterowniki dla wielu popularnych robotów, jakie są obecnie używane. Spójny interfejs programowy (ang. Aplication Programming Interface, w skrócie API) zapewnia możliwość łatwego dodania obsługi nowych urządzeń, jakie mogą być dostępne. Dostęp do danych robota uzyskuje się dzięki interfejsom programowym. Wszystkie interfejsy zrewalizowane są, jako wtyczki do programu Playera. Wśród standradowych interfejsów, w niniejszej pracy wykorzystane zostały następujące: player odwołuje się do serwera Player uruchomionego w komputerze robota. W każdej instancji serwera Player, może się znajdować tylko jeden interfejs takiego urządzenia i zawsze ma indeks o numerze 0; position interfejs ten zapewnia możliwość sterowania robotem mobilnym. Sterowanie może być przeprowadzane poprzez zadawanie prędkości kół lub pozycji docelowej, jaką ma osiągnąć robot. Interfejs zapewnia możliwość zapisu do urządzenia (sterowania) oraz odczytu wskazań odometrii; laser poprzez ten interfejs uzyskuje się dostęp do danych pochodzących z dalmierza laserowego, jak np. SICK LMS200. Poprzez interfejs zwracana jest seria danych z urządzenia lasera. Serie składają się z uporządkowanych par liczb (r, φ), gdzie r oznacza zmierzoną odległość, a φ kąt skanowania. Kąt na wprost lasera wynosi 0. Program Stage jest symulatorem dwuwymiarowego środowiska, za pomocą którego możliwe jest jednoczesne sterowanie wieloma robotami. Urządzenia robota(ów), jak czujniki, czy układy sterowania, dostępne są poprzez program Player w jednakowy sposób, niezależnie czy odnoszą się do urządzeń rzeczywistych, czy obiektów symulatora. Środowisko Player/Stage działa na wielu platformach UNIX-owych (jak np. Linux, FreeBSD, czy Mac OSX) oraz w systemach Microsoft Windows. Biblioteki dystrybuowane razem ze środowiskiem pozwalają na programowanie przy użyciu wielu popularnych języków programowania, jak np. C, C++, Tcl, Python oraz Java. 56

69 Omówiony poprzednio robot Protonek, wykorzystuje serwer Player jako swoje bazowe oprogramowanie. Program Stage użyty został, podczas przeprowadzania pierwszych testów tworzonego oprogramowania, jeszcze poza robotem Wtyczka miarn Większość złożonych zadań stawianych robotom wymagają mechanizmu rozpoznawania różnorodnych obiektów w terenie, jakie znajdują się w środowisku otoczającym robota. Systemy pomiaru terenu, jak omówiony poprzednio dalmierz laserowy, są czujnikami, które nie przeprowadzają analizy zbieranych danych. Pakiet miarn jest wtyczką (ang. plugin) działającą w serwerze Player, dostarczającą procedury rozróżniania różnego typu podstawowych obiektów w skanach środowiska, np. pochodzących z dalmierza laserowego. Za pomocą wtyczki możliwe staje się wyodrębnianie w terenie podstawowych kształtów: linii, łuków, okręgów oraz nóg poruszających się w pobliżu ludzi. Linie oraz łuki są najczęściej stosowanymi obiektami w nawigacji, w szczególności dla celu budowania mapy. Metoda rozpoznawania nóg poruszających się ludzi, może wspomóc techniki jednoczesnej lokalizacji i budowania mapy, poprzez usuwanie takich obiektów ze skanów. Wtyczka działa w ramach serwera Player, udostępniając interfejs danych fiducial, podobny do interfejsu lasera, opisanego poprzednio. Dla aplikacji klienckich, które chcą skorzystać z danych przetworzonych przez wtyczkę, interfejs jest dostępny pod nazwą fiducial, jako jedno z urządzeń robota. Algorytm wyznaczania obiektów wraz z zastosowaniem zostanie omówiony w części 5.4. Wtyczka miarn znalazła swoje zastosowanie, w tworzonym na potrzeby niniejszej pracy oprogramowaniu, przy wyznaczaniu oraz rozpoznawaniu znaczników terenowych Biblioteka LODO Biblioteka LODO realizuje stabilizację wskazań odometrii za pomocą dalmierza laserowego SICK LMS 200. W skład biblioteki wchodzi między innymi wtyczka realizująca pracę sterownika typu pozycji (ang. position) serwera Player. 57

70 Sterownik LODO pracujący w ramach serwer Player, wymaga istnienia dwu typu interfejsów: pozycji oraz dalmierza, z których pobierane są dane wejściowe. Jako dane wyjściowe otrzymywana jest poprawiona pozycja. Algorytm stabilizcacji odometrii, realizowany przez bibliotekę LODO, jest rodzajem algorytmu SLAM. Wskazania otrzymane z dalmierza porółnywane są w kolejnych krokach w celu uzyskania dopasowania obrazu widzianego z urządzenia. W ten sposób wyznaczana jest korekta dla pozycji robota. Szczegóły dotyczące metody można odnaleźć w [11]. Dane uzyskane za pomocą sterownika LODO, autor pracy, wykorzystał w celach porównawczych dla sprawdzenia badanych algorytmów. Ponadto, ze względu na brak innej metody wyznaczenia pozycji robota, dane otrzymane za pomocą sterownika posłużyły do strojenia algorytmów. Opis wykorzystania sterownika został zawarty w rozdziale Pakiet Boost Pakiet Boost jest kolekcją bibliotek rozszerzających możliwości języka C++. Wszystkie biblioteki wchodzące w skład pakietu są intensywnie testowane oraz badane pod kątem szybkości działania oraz zajętości pamięci, zanim wejdą w skład dystrybuowanych paczek. Duży nacisk w pakiecie położono na mechanizm szablonów (ang. templates), by możliwe było łatwe powtórne użycie kodu (ang. code re-use) oraz wprowadzanie kolejnych rozszerzeń. Pakiet dostępny jest dla większości platform programistycznych. W skład pakietu wchodzi biblioteka przekształceń algebraicznych o nazwie ublas. ublas dostarcza naturalnego dla C++ mechanizmu klas obsługującego wektory oraz macierze. Obliczenia realizowane są przez bibliotekę przy użyciu pakietu BLAS (ang. Basic Linear Algebra Subprograms). Pakiet boost jest wydawany na licencji Boost Software License, która zezwala na używanie go w dowolnych projektach, zarówno otwartych jak i zamkniętych. Źródła bibliotek oraz dokumentacja dostępne są w Internecie [50]. Biblioteka ublas, z pakietu Boost, została wykorzystana w tworzonym oprogramowaniu dla realizacji obliczeń wektorowych oraz macierzowych. 58

71 Biblioteka Qt Biblioteka Qt w znacznej części składa się z klas obsługi interfejsu graficznego programów. Dedykowana jest do wykorzystania w języku C++, ale istnieją także rozszerzenia dla innych języków obiektowych, jak np. python. Dostępna jest na wiele różnych platform, nie tylko środowiskowych, ale także sprzętowych. Biblioteka Qt rozwijana jest przez firmę Trolltech. Istotnymi cechami tej biblioteki są mechanizm sygnałów i slotów, zapewniająca komunikacje między obiektami (podobną do międzyprocesowego mechanizmu ipc) oraz hierarchiczny system zdarzeń. Licencja na użycie biblioteki jest podwójna: dla aplikacji wydawanych na licencji GPL, Qt jest dystrybuowana na licencji QPL, zezwalającej na jej swobodne użycie; dla aplikacji komercyjnych dostępna jest jedynie wersja komercyjna, płatna. Pełną wersję na licencji QPL oraz wersję komercyjną (jedynie na zasadach terminowego testowania) można pobrać z witryny internetowej firmy [49]. W tworzonym na potrzeby niniejszej pracy oprogramowaniu, biblioteka Qt zapewnia graficzny interfejs stosowany przy wyświetlaniu mapy siatki zajętości. 59

72 Rozdział 5. Realizacja zadania 5.1. Wstęp Zadanie polega na estymacji przebytej ścieżki w dyskretynych chwilach czasowych z jednoczesnym zbudowaniem mapy znaczników, przy wykorzystaniu algorytmu SLAM. Dla celów porównawczych zostały zaimplementowane i eksperymentalnie zbadane dwa typy algorytmów, wykorzystujące filtr Kalmana. Dla algorytmów wykorzystujących filtr Kalmana istotnym założeniem jest statyczność środowiska. W celu zapewnienia prawidłowej pracy algorytmów, w przypadku środowisk zmiennych w czasie, opracowana musi zostać metoda filtracji danych o obiektach zmiennych w czasie. Dla przykładu, robot poruszający się pośród ludzi nie może przyjąć ich pozycji, jako znaczników, bo doprowadziłoby to do błędnej korekty położenia robota w terenie w następnych krokach, przy aktualizacji pomiaru. Z tego też powodu zakłada się, że środowisko pracy robota jest statyczne. Brak stosownego sprzętu uniemożliwił przeprowadzenie kalibracji dalmierza laserowego (przykładowy opis kalibracji dalmierza można znaleźć w [45]). Błąd wskazań dalmierza oszacowano z wykorzystaniem danych producenta, tj. reprezentowany przez szum biały o zerowej wartości oczekiwanej oraz maksymalnym błędzie wskazania 5mm Badane środowisko Do testów wybrano środowisko laboratoryjne, jako spełniające warunek stacjonarności. Robot poruszał się w typowym otoczeniu składającym się z szaf, ławek oraz krzeseł. Podłoga laboratorium wykonana z płytek ceramicznych, zapewniała dość 60

73 dobrą przyczepność kół robota. Na Rysunku 5.1 znajdują się zdjęcia przygotowanego do testów środowiska. a) b) c) d) Rysunek 5.1: Środowisko testowe. Jako znaczniki w badaniach wykorzystano łatwo rozpoznawalne oraz identyfikowalne ze skanów obiekty. Do tego typu obiektów bardzo dobrze nadają się rogi ścian, szaf, kartonów, itp., które występują w większości pomieszczeń. Rozpoznawalność rogów w skanach dalmierza zapewnia ułożenie dwu przecinających się linii. Dodatkowo rogi mogą być rozróżniane poprzez sprawdzenie wypukłości odnalezionego rogu. Rozpoznawanie charakterystycznych obiektów w skanach należy do problemów jakimi zajmują się techniki rozpoznawania obrazów. Z punktu widzenia robotyki mobilnej, niezwykle ważny jest czas przetwarzania skanu, jakim charakteryzują się istniejące algorytmy. W niniejszej pracy wykorzystano nowy algorytm IAV (ang. Inscribed Angle Variance), który jego twórcy [44] zaimplementowali jako moduł, opisanej w punkcie wtyczki miarn. Algorytm ten cechuje się liniową złożonością O(n), a najbardziej czasochłonne obliczenia dotyczą wyznaczenia 61

74 odchyleń średniego oraz standardowego. Wykrywanie znaczników w otoczeniu robota jest przeprowadzane dwuetapowo. W etapie pierwszym jest wykorzystywany algorytm IAV, który z surowych danych dostarczonych z dalmierza, wyznacza zbiory punktów skanu, jakie tworzą linie (przy zadanej wartości błędu). W drugim etapie tak znalezione zbiory są aproksymowane liniami prostymi, a następnie sprwdza się czy znalezione linie mają przecięcia, które można identyfikować jako rogi Odczyt danych o stanie robota Wszystkie dane są odczytywane poprzez interfejs sieciowy programu Player, które po zarejestrowaniu mogą służyć do późniejszych symulacji off-line. Wśród danych nie można otrzymać surowych informacji o sterowaniach, jakie zostały zadane na silniki, a jedynie rezultaty ich działań. Tym samym zmiana pozycji robota (równanie zmiany pozycji (3.42)), nie mogą być wykonane przez tworzone oprogramowanie, a jedynie odczytywane są ich rezultaty z programu Player. Brak dostępu do aktualnych wartości sterowań, nie sprawia jednak znacznych problemów. Przebyta przez robota droga l k może być wyznaczona korzystając z wartości przyrostów stosownych zmiennych stanu. Przyrosty wyznaczamy wykorzystując informację o bieżącej i poprzedniej pozycji: i wówczas drogę można wyznaczyć korzystając ze wzoru: x k = x k x k 1 (5.1) y k = y k y k 1 (5.2) θ k = θ k θ k 1 (5.3) l k = ( x) 2 + ( y) 2 (5.4) Zmiana pozycji robota jest zawsze odczytywana z serwera Player. W przypadku algorytmu UKFSLAM problem braku dostępu do aktualnych wartości sterowań może wydawać się bardziej dotkliwy, ze względu na potrzebę przekształcenia wszystkich punktów sigma ze zbioru {X i } (porównaj punkt ), zgodnie z równaniem stanu zadanym wzorem (3.79). Z brakiem sterowań można sobie jednak poradzić bardzo prosto wykorzystując wzory (3.82), (3.79) oraz (3.42). Prze- 62

75 kształcenie punktów sigma można zapisać następująco: X i,k k 1 = f(x i,k 1, u k 1 ) = X i,k 1 + g( l k, θ k, δl k, δθ k ) (5.5) = s k 1 + < UKF > i + g( l k, θ k, δl k, δθ k ) = s k + < UKF > i gdzie przez < UKF > i oznaczony jest pierwiastek zdekomponowanej macierzy kowariancji P (porównaj wzór (3.76)). W powyższym wyprowadzeniu widać, że do przeprowadzenia punktów sigma przez równanie stanu nie jest potrzebna informacja o wartości sterowań. Ze względu na powyższy sposób wyznaczenia punktów sigma, jakie zastosowano w pracy, w algorytmie UKFSLAM nie ma potrzeby wyliczenia punktu stanu układu ŝ k w równaniu (3.80). Można to łatwo zauważyć, bowiem czynniki < UKF > i z powyższego wzoru będą się znosić (porównaj z równaniem tworzenia punktów sigma (3.82)). Własność ta wynika z przyjętego modelu ruchu robota Wykorzystane znaczniki terenowe Wykrywanie znaczników terenowych jest istotną kwestią dla algorytmów znacznikowych rozwiązujących zadanie SLAM. Im stosowana metoda wykrywania jest dokładniejsza, tym dokładniejsza staje się faza korekcji algorytmów. I odwrotnie jeśli zastosowana metoda jest słaba, wyniki uzyskane za pomocą algorytmów stają się mało wiarygodne. Wszystkie algorytmy wykorzystujące filtr Kalmana są algorytmami znacznikowymi. Liczną grupę znaczników w środowisku zamkniętym, stanowią rogi, np. rogi ścian, czy mebli. Identyfikacja rogów w skanach polega na odnalezieniu przecięcia dwu linii (w rzeczywistości punktu styczności dwu odcinków). Rogi posiadają jedną cechę charakterystyczną, za pomocą której możemy podzielić je na dwie grupy: wklęsłość / wypukłość. Róg przynależy do grupy wklęsłych, kiedy punkt przecięcia linii jest dalej od robota, niż dwa końcowe punkty kształtujące w skanie odcinek. Podobnie róg należy do grupy wypukłych, kiedy odległość punktu przecięcia jest bliżej robota, niż pozostałe dwa końce. Wklęsłość i wypukłość zależą bezpośrednio od strony, z której patrzymy na róg, jednak praktyce bardzo rzadko róg może być widoczny z obu stron. Zaproponowana metoda odnajdywania rogów w skanach składa się z dwu etapów. W pierwszym etapie są wyznaczane zbiory punktów tworzących linie przy 63

76 Rysunek 5.2: Przykładowe rogi widoczne w skanie. wykorzystaniu wtyczki miarn. W drugiem etapie aplikacja odczytująca dane z serwera Player aproksymuje proste uzyskanych zbiorów punktów i przeprowadza kilka testów sprawdzających czy uzyskane dwie kolejne linie przecinają się tworząc róg. Zaimplementowany algorytm za róg uznaje nie tylko te, dla których aproksymowane proste przecinają się pod kątem ok. 90 stopni. Każde załamanie np. ścian pod kątem nie mniejszym, niż 10 stopni, jest uznane za róg. Powiększa to znacznie liczbę znaczników, jakich może użyć do celów nawigacyjnych Wykrywanie obiektów geometrycznych przez wtyczkę miarn Wtyczka miarn, jak już wspomniano, pozwala na odnajdywanie w skanach obiektów o postaci linii, łuków/okręgów oraz nóg poruszających się ludzi. Dla celów niniejszej pracy wykorzystane zostało jedynie odnajdywanie linii w skanach. Wykrycie linii oraz łuków realizowane jest przez ten sam algorytm. Algorytm IAV, wykorzystuje trygonometryczną własność łuków, która mówi, że kąt wpisany jest dwa razy mniejszy od kąta środkowego opartego na tym samym łuku, a wszystkie kąty wpisane oparte na tym samym łuku są równe [48]. 64

77 Rysunek 5.3: Graficzny opis zasady odnajdywania okręgów w skanie terenu z wykorzystaniem wtyczki miarn. Źródło: [44]. Rysunek 5.3 przedstawia własność powyższego twierdzenia. Jeśli przyjmiemy, że punkty P 1 oraz P 4 są granicznymi punktami łuku okręgu o środku w punkcie O, wówczas zachodzą następujące związki między kątami: P 1 P 2 P 4 = P 1 P 3 P 4 = P 1OP 4 2 (5.6) Wykrycie okręgu w skanie następuje po obliczeniu odchyleń średniego i standardowego. Okręg uznaje się za znaleziony, kiedy odchylenie standardowe jest mniejsze, niż 8, 6, przy średniej wartości kąta pomiędzy 90, a 135 [44]. Wykrycie linii oparte jest na metodzie wykrywania łuku, przyjmując w powyższym wzrorze średnią wartość kąta równą 180. Obrazuje to Rysunek 5.4. Rysunek 5.4: Graficzny opis zasady odnajdywania linii w skanie terenu z wykorzystaniem wtyczki miarn. Źródło: [44]. W pierwszym kroku algorytmu wykrycia linii wyodrębniana jest badana grupa punktów. Następnie dla każdego z punktów, znajdującego się w skanie pomiędzy wybraną dwójką, obliczane są średnia wartość kąta opartego na łuku oraz odpowiadające kolejnym punktom odchylenie standardowe. Pozytywne wykrycie linii jest uzyskiwane, kiedy średnia wartość kąta opartego na łuku oraz odchyleń dla wszystkich punktów, mają wartości z przedziału dopuszczalnych. Zgodnie z testami opisanymi w [44] bezpieczne wartości, jakich można użyć, to 8.6 dla odchylenia standardowego przy wartości kąta równej

78 Aproksymacja prostych w skanach Wśród wyników dostarczanych przez wtyczkę miarn podczas odnajdywania linii, zawarte są indeksy początkowy oraz końcowy punktów skanu, które wyznaczają zbiór punktów tworzących odnalezioną linię. Wykorzystując zbiór odnalezionych punktów, linia jest wyznaczana za pomocą aproksymacji linii prostej. W pierwszych testach wykorzystano metodę aproksymacji liniowej (tj. wielomianowej stopnia m = 1), danej wzorem: f(x) = a 0 + a 1 x (5.7) Za pomocą metody najmniejszych kwadratów, współczynniki wielomianu mogą zostać wyznaczone za pomocą poniższych zależności: a 0 = t ts 2 t 1 S 1 S 0 S 2 S 2 1 a 1 = t 1S 0 t 0 S 1 S 0 S 2 S 2 1 (5.8) (5.9) gdzie: S k = t 0 = t 1 = n x k i, k = 0, 1, 2 (5.10) i=0 n y i (5.11) i=0 n x i y i (5.12) i=0 W powyższych równaniach niewiadome x i oraz y i to współrzędne aproksymowanych punktów, których liczba wynosi (n + 1). Niestety za pomocą funkcji danej wzorem (5.7) nie może zostać zapisana prosta o wykresie pionowym (tj. prosta x = const). Kilka pierwszych testów ujawniło także, że jeśli prosta aproksymacji jest prawie pionowa, wynik również nie należy do akceptowalnych (przykład znajduje się na Rysunku 5.5). Po analizie wyników uzyskanych z wtyczki miarn, autor postanowił wyznaczyć linię przyjmując, że: zbiór punktów został wcześniej wyznaczony i jest dobrze określony ; odrzucane są skrajne punkty zbioru, ponieważ mogą wprowadzać największe błędy do wyznaczonej prostej. 66

79 Rysunek 5.5: Niepoprawna aproksymacja zbioru punktów z użyciem metody najmniejszych kwadratów. Algorytm aproksymacji został przedstawiony w postaci pseudokodu z wykorzystaniem notacji C++ na schemacie S zbiór indeksów punktów tworzących linię {S 1, S 2 } S odrzuć indeks pierwszego punktu(s 1 ) odrzuć indeks ostatniego punktu(s 2 ) p 1 wyznacz punkt średni(s 1 ) p 2 wyznacz punkt średni(s 2 ) if abs(p 1 [x] p 2 [x]) < 10) then wyznacz prostą pionową(p 1, p 2 ) end else wyznacz prostą przecinającą punkty średnie(p 1, p 2 ) end Algorytm 5.1: Pseudokod algorytmu aproksymacji prostej dla zadanego zbioru punktów. Jedyną daną wejściową zrealizowanego algorytmu aproksymacji prostej jest zbiór indeksów punktów skanu, które tworzą prostą. Zbiór idneksów dzielony jest na dwa 67

80 rozłączne podzbiory, możliwie równoliczne (z dokładnością do jednego punktu), co odpowiada krokowi 2-emu schematu. W kroku 3-im oraz 4-ym odrzucane są indeksy punktów skrajnych. Dla pozostałych punktów w podzbiorach są wyznaczane punkty średnie, tj. obie współrzędne takich punktów są średnimi arytmetycznymi współrzędnych wszystkich punktów należących do podzbioru, co jest realizowane w kroku 5-ym oraz 6-ym na schemacie. W kroku 7-ym algorytmu jest sprawdzane, czy punkty średnie mogą opisywać linię pionową. Pamiętając, że błąd dalmierza wynosi ok. 5mm, jeśli wartości rzędnych punktów p 1 oraz p 2 różnią się nie więcej, niż 10mm, jest wyznaczana funkcja opisująca linię pionową, o wartości równej średniej obu punktów: x = x 1 + x 2 2 (5.13) co odpowiada krokowi 8-emu algorytmu. Jeśli różnica rzędnych obu punktów jest nie mniejsza, niż 10 mm, wyznaczana jest prosta, jako linia przechodząca przez dwa wyznaczone punkty średnie: y = ax + b (5.14) a = y 1 y 2 x 1 x 2 (5.15) b = y 2x 1 y 1 x 2 x 1 x 2 (5.16) co odpowiada krokowi 11-emu przedstawionego algorytmu. Dla przypadku z Rysunku 5.5, przedstawiony powyżej algorytm wyznaczył proą aproksymującą, której wykres znajduje się na Rysunku 5.6. Co łatwo zaobserwować, wyznaczona linia zdecydowanie lepiej aproksymuje zbiór punktów. Na Rysunku 5.7 jest przedstawiona przykładowa aproksymacja linii skośnej, wykorzystująca powyższy algorytm. 68

81 Rysunek 5.6: Aproksymacja zbioru punktów linią. Przypadek z Rysunku 5.5. Rysunek 5.7: Aproksymacja zbioru punktów linii skośnej. 69

82 Odnajdywanie pozycji znaczników w postaci rogów W środowisku laboratoryjnym (jak również innych zamkniętych), rogi ścian, szaf, biurek, itp. dobrze charakteryzują otoczenie i dają się stosunkowo prosto wyznaczyć. W skanie rogi mogą zostać wyznaczone dzięki przecięciu się dwu odnalezionych linii. Na Rysunku 5.8 znajdują się zdjęcia środowiska widocznego przez robota. Linie czerwone oraz bordowe (naprzemiennie) przedstawiają linie widoczne w skanie dalmierza. Na łączeniach dwu kolorów linii można łatwo zauważyć rogi, jakie może wykorzystać robot, jako potencjane znaczniki. Rysunek 5.8: Zdjęcia części terenu widocznego przez robota z zaznaczonymi liniami. Rysunek 5.9: Skan środowiska pokazanego na Rysunku 5.8. Punkty zaznaczone kolorem różowym, jako strzałki z owalem opisują miejsce odnalezienia linii przez wtyczkę miarn. 70

83 Algorytm odnajdywania rogów w skanie wykorzystuje aproksymacji prostej opisaną w poprzednim podpunkcie. Algorytm zapisany w postaci pseudokodu z wykorzystaniem notacji języka C++ przedstawiono na schemacie 5.2. Jako dane wejściowe algorytm otrzymuje zbiór indeksów tworzących kolejne linie. Linie badane są parami, więc dla n znalezionych linii wykonywane jest n-1 przebiegów pętli n n k // liczba zbiorów indeksów k 0 // zeruj liczbę znalezionych rogów for i = 0; i < (n 1); + + i do /* linie badane są parami */ if liczba punktów zbioru mniejsza niz cztery(i) then continue; else if liczba punktów zbioru mniejsza niz cztery(i+1) then continue; else if różnica indeksów kolejnych linii w zbiorze wieksza niz jeden(i) then continue; end if różne indeksy początkowy końcowy(i) then /* jeśli linie nie mają wspólnego numeru indeksu punktu w skanie */ if! odległość pomiędzy liniami dopuszczalna(i) then /* badana odległość pomiędzy punktami końco9wym i-tej linii oraz początkowym i + 1-ej */ continue; end end if! aproksymuj linie(i) then continue; else if! aproksymuj linie(i + 1) then continue; else if kąt pomiędzy liniami mniejszy niz 10(i,i + 1) then /* linie muszą się przecinać pod kątem nie mniejszym, niż 10 stopni */ continue; end ROGI(k) = wyznacz rog jako przeciecie linii(i,i + 1); zbadaj czy rog wklesly(rogi(k)); k + +; end Algorytm 5.2: Pseudokod algorytmu odnajdywania rogów obiektów w środowisku z wykorzystaniem aproksymowanych linii. 71

84 Ze względu na przyjętą metodę aproksymacji, analizie poddawane są jedynie te zbiory, które mają minimalnie 4 punkty. Test liczby punktów należących do zbiorów jest realizowany w krokach czwartym (dla i-tej linii) oraz szóstym (dla ((i + 1)-ej linii). Wtyczka miarn potrafi w wynikach dać linię dla zbioru dwupunktowego, który nie wydaje się być dostatecznym do aproksymacji prostej w skanie terenu. W kroku ósmym algorytmu badana jest różnica indeksów punktów dla ostatniego punktu i-tej linii oraz początkowym (i + 1)-ej. Kiedy indeksy te są równe - (i + 1)-sza linia zaczyna się tam, gdzie kończy się i-ta. Sytuacja taka choć występuje w praktyce, to rzadko promień dalmierza pada dokładnie w róg, czy też jego bezpośrednie sąsiedztwo (im dalej dalmierz znajduje się od rogu, tym mniejsze prawdopodobieństwo, że zajdzie taka sytuacja). Znacznie częściej występuje sytuacja, kiedy zbiory indeksów są rozdzielne, a indeksy końcowy i początkowy są kolejnymi dla rogów. Dla zbiorów indeksów oddzielonych dodatkowymi punktami skanu, przyjmowane jest, że nie tworzą dwu prostych przylegających do siebie. Dla przypadku zbiorów dwu linii bez wspólnego punktu dodatkowo sprawdzana jest odległość między punktami o najbliższych indeksach. Test wykonywany jest w kroku 12-tym algorytmu. W ten sposób eliminowane są przypadki kiedy jeden obiekt zasłaniał drugi, znacznie od niego oddalony. Maksymalna dopuszczalna odległość pomiędzy nimi, może być wyznaczona jako funkcja liniowa odległości dowolnego z punktów od dalmierza. W krokach 16-ym oraz 18-ym wykonywana jest aproksymacja zgodnie opisana algorytmem 5.1. Aproksymacja prostej jest niewątpliwie algorytmem numerycznie bezpiecznym (żaden z mianowników nie powinien przyjąć wartości zera), jednakże ze względu na skończoną dokładność numeryczną istnieje możliwość, że otrzymane współczynniki nie dadzą się wyrazić przez liczbę z dopuszczalnego zakresu. W kroku 20-tym wykonywany jest ostatni test badający, czy aproksymowane proste przecinają się w sposób, jaki umożliwia wykrycie rogu. Przyjęcie wartości minimalnego kąta pomiędzy prostymi na 10 pozwoliło odfiltrować wyniki sytuacji, kiedy linia widoczna w skanie, dzielona była przez wtyczkę miarn na dwie rozłączne. Na koniec wyznaczane jest przecięcie aproksymowanych linii, które dodawane jest do listy odnalezionych w skanie rogów. W kroku 24-ym algorytmu dodatkowo badane jest jeszcze, czy róg został zidentyfikowany jako wklęsły, czy wypukły. 72

85 5.5. Opis stworzonego oprogramowania Stworzone oprogramowanie miało na celu umożliwić przebadanie algorytmów SLAM oraz przygotowanie klas i procedur, do utworzenia biblioteki możliwej do wykorzystania w innych programach. Implementacja badanych algorytmów została zrealizowana w postaci klas języka C++ oraz zbioru procedur wykorzystujących je. Klasy pozwoliły w prosty sposób stworzyć wspólny interfejs dla wszystkich badanych algorytmów, w tym także integracji wyników odometrii. Rozszerzenia możliwości stworzonego oprogramowania można dokonać dziedzicząc z odpowiednich klas. W źródłach dołączonych na płycie do niniejszej pracy, nazwy plików klas zaczynają się dużą literą, a plików procedur małą. Oprogramowanie było testowane w systemie Linuks (dystrybucjach Gentoo oraz Debian). Wykorzystany mechanizm tworzenia i uruchamiania wątków powinien jednak pozwolić uruchomić się także na innych systemach, zgodnych ze standardem POSIX (jak np. rodzina systemów *BSD) Ogólna struktura programu Ogólny schemat napisanego oprogramowania znajduje się na rysunku Główny wątek programu, odpowiada za odbieranie nowych danych o pozycji robota oraz skanów otoczenia oraz przekazaniu ich do procedury rekonstrukcji ścieżki. Dane wyjściowe algorytmu SLAM są zapisywane w pamięci do wykorzystania w budowie mapy siatki zajętości. Głównym zadaniem realizowanym przez wątek GUI oprogramowania jest prezentacja obrazu mapy. Ze względu na czasochłoność obliczeń aktualizacji mapy, powoływany jest dodatkowy trzeci wątek, którego jedynym celem jest cykliczne włączania kolejnych skanów. Realizacja obliczeń w dodatkowym wątku sprawia, że wątek GUI nie musi być blokowany na ten czas i może cały czas przyjmować zlecenia od użytkownika Diagramy dziedziczenia klas badanych algorytmów Klasa SlamAlg jest bazową klasą wszystkich zaimplementowanych klas badanych algorytmów. Jej jedyne dwie składowe to rpath, w której jest zapisana przebyta przez robota ścieżka wraz ze wskazaniami z dalmierza oraz składowa curpose, w której jest zapisywana bieżąca pozycja robota. Klasa jest klasą czysto wirtualną (ang. pure virtual) ze względu na definicję pustej metody wirtualnej DoSlam. 73

86 Rysunek 5.10: Struktura oprogramowania z wyszczególnieniem wątków. Rysunek 5.11 zawiera diagram dziedziczenia klasy bazowej SlamAlg. Klasę OdometryNoSlam stworzono jedynie w celu przedstawienia jakości działania odometrii robota Protonek. Klasa SlamKalman, podobnie jak SlamAlg, jest klasą czysto wirtualną. Zawiera implementacje metody DoSlam, wspólną dla wszystkich algorytmów wykorzystujących filtr Kalmana, ale w jej miejsce definiuje trzy inne metody czysto wirtualne UpdateLoop, UpdateObservation oraz Observe. W klasie zawarte zostały także deklaracje metod: incorporatelandmark służącej do włączenia nowego znacznika do wektora stanu oraz macierzy kowariancji; inforcespd, która sprawdza czy macierz kowariancji po fazie aktualizacji nadal jest dodatnio określona oraz symetryczna; generatellt, która przeprowadza rozkład macierzy za pomocą metody Cholesky ego. Składowymi klasy są wszystkie macierze oraz wektory wspólne dla algorytmów wykorzystujących filtr Kalmana. 74

87 Rysunek 5.11: Diagram dziedziczenia dla klasy SlamAlg 75

88 Klasy EkfSlam oraz UkfSlam są klasami finalnymi, tj. deklarują wszystkie metody czysto wirtualne klasy SlamKalman, a ich składowymi są elementy charakterystyczne dla tych klas. W klasie UkfSlam dodatkowo jest zawarta metoda constructsigmapoints, której zadaniem jest konstruowanie zbioru punktów sigma z bieżących wartości wektora stanu oraz macierzy kowariancji Organizacja zajętości pamięci W każdym kroku działania aplikacji robota, po wykonaniu metody DoSlam(), do składowej rpath klasy SlamAlg, jest dodawana nowowyznaczona pozycja wraz ze wskazaniami pochodzącymi z dalmierza. W każdym kroku w pamięci odkładane są: trzy liczby typu double, przechowujące pozycję robota; 181 par dla wskazań dalmierza, gdzie każda para składa się z dwu liczb typu całkowitego; macierz kowariancji stanu robota. Wskazania dalmierza są przechowywane tylko do czasu, gdy wątek tworzenia wizualnej reprezentacji mapy nie przetworzy odczytów na mapę. Po wykorzystaniu wskazań, pamięć jest zwalniana, a tworzona jest nowa mapa cząstkowa (szczegóły dotyczące tworzenia map znajdują się w punkcie 5.7.) Strojenie filtrów Przedstawione nieliniowe filtry Kalmana zostały dostrojone ręcznie z wykorzystaniem uprzednio zebranych danych. Ciekawy przykład automatycznego strojenia algorytmów nieliniowej filtracji Kalmana w zastosowaniu do SLAM, można odnaleźć w publikacji [1]. Metoda ta jednak wymaga zewnętrznego pomiaru pozycji robota, w celu wyznaczenia możliwie dokładnej jej wartości. W przypadku obu algorytmów dostrajane muszą być następujące parametry: stała c macierzy kowariancji zakłóceń Q k (3.53); stała c oraz iloczyn bd macierzy kowariancji zakłóceń R k (3.54). Dodatkowo algorytm UKF można dostrajać dobierając stałą κ, która definiuje odległość punktów sigma od predykowanego wektora stanu. Strojenie obu algorytmów wykonano kolejno dla każdego z parametrów. Przyjmując wartości początkowe dla wszystkich parametrów, w każdym kroku strojenia był wybierany jeden, dla którego poszukiwano lepszego rozwiązania. Strojenie 76

89 każdego z parametrów wymagało przejrzenia zadanego dyskretnego zbioru wartości. Po wyznaczeniu najlepszej wartości, dobrany parametr był wykorzystywany do wyznaczenia kolejnego z poszukiwanych parametrów. W trakcie strojenia jko rzeczywiste wartości pozycji robota przyjęto wskazania uzyskane za pomocą sterownika LODO. Kryterium poprawności strojenia, był najmniejszy moduł różnicy pomiędzy końcowymi kątami orientacji bazy robota. Kryterium nie zawiera położenia robota, jednak błędy położenia są znacznie mniejsze, niż błędy kąta orientacji bazy robota. Proces strojenia parametrów przeprowadzany był cyklicznie. Już w czwartej iteracji, uzyskane wyniki różniły się nieznacznie od wyników trzeciej iteracji. W piątej iteracji wartości parametrów ustałały się i w kolejnych iteracjach nie było już zauważalnej poprawy Tworzenie mapy siatki zajętości Budowa mapy siatki zajętości nie jest wykonywana na bieżąco. Wizualizacja wyników budowy mapy wymaga użycia komputera z graficznym środowiskiem pracy. Dla napisanego programu istnieje możliwość wykorzystania przełącznika, która pozwala włączyć i/lub wyłączyć budowę mapy (lista wszystkich przełączników programu stworzonego dla weryfikacji tez niniejszej pracy znajduje się w Dodatku C). Zrealizowany algorytm budowania mapy siatki zajętości zakładał podział map na dwa rodzaje map: mapy cząstkowe - mapy lokalne związane z daną pozycją robota; mapę globalną - pojedynczą mapą terenu, która jest tworzona na bazie stworzonych map lokalnych, dołączanych względem pozycji robota, w której zostały utworzone. Taka realizacja pozwala szybko przebudować globalną mapę, jeśli zmianie uległyby pozycje robota w chwilach przeszłych. Komórki w tworzonych siatkach zajętości są kwadratami o bokach długości 10cm. Uzyskiwane mapy są dostatecznie dokładne, by możliwe było ich użycie w metodach planowania ścieżki. Mapa siatki zajętości zastosowana została jako jedna z metod wizualnej weryfikacyji działania badanych algorytmów. Oczywiście, weryfikacja taka niewiele mówi o faktycznej efektywności badanych algorytmów. 77

90 Opis implementacji algorytmu budowy mapy siatki zajętości Podstawowa wersja algorytmu budowania map siatek zajętości zakłada aktualizację wszystkich komórek mapy, podczas dołączania danych pochodzących z nowych skanów, jednak w praktyce liczbę aktualizowanych komórek można znacznie ograniczyć. Na Rysunku 5.12 a) półokrąg koloru niebieskiego określa część badanego terenu, jaka jest widoczna dla skanera laserowego. Jeśli w terenie znajduje się przeszkoda, niektóre z komórek przestaną być widziana, co widać na rysunku b). Ograniczając liczbę komórek, jakie będą podlegały aktualizacji, można zauważyć, że skan zawsze można zamknąć w prostokącie (rysunki c) - f)). Tak określony prostokąt ogranicza liczbę komórek, jaka w danym kroku powinna podlegać aktualizacji. a) b) c) d) e) f) Rysunek 5.12: Aktualizowany obszar podczas budowaniu mapy siatki zajętości 78

91 Przyjmując, że robot w ustalonej chwili znajduje się w pozycji opisanej przez uporządkowaną trójkę (x, y, θ), a zasięg dalmierza jest określony półokręgiem o promieniu z max, wartości współrzędnych przeciwległych rogów prostokąta (lewego dolnego oraz prawego górnego) można wyznaczyć korzystając z poniższych wzorów: dla θ [0; π 2 ]: dla θ [ π 2 ; π]: dla θ [π; 3π 2 ]: x 1 = x z max sin θ y 1 = y z max x 2 = x + z max cos θ y 2 = y + z max x 1 = x z max y 1 = y + z max cos θ x 2 = x + z max sin θ y 2 = y + z max x 1 = x z max y 1 = y + z max sin θ x 2 = x + z max y 2 = y z max cos θ dla θ [ 3π 2 ; 2π]: x 1 = x + z max sin θ y 1 = y z max x 2 = x + z max y 2 = y + z max cos θ Tylko komórki siatki zajętości, które należą do obszaru terenu ograniczonego punktami rogów prostokąta (x 1, y 1 ) (x 2, y 2 ) będą aktualizowane. Punkty mapy wybrane do aktualizacji, przez wprowadzony powyżej prostokąt, mogą być położone dalej, niż wynosi zasięg lasera, lub też mogą być zasłonięte przez inne obiekty rozmieszczone w terenie. Wykrycie czy dany wycinek terenu jest widziany przez skaner jest realizowane przez algorytm odwrotnego modelu czujnika wg opisu podanego w [34], razem z wyznaczeniem prawdopodobieństwa zajętości dla danej komórki opisującej teren. Obliczenie prawdopodobieństwa zajętości pojedynczej komórki m i w chwili k dla obserwacji otoczenia z k dokonanej w pozycji s k, można dokonać za pomocą algorytmu

92 (x i, y i ) oznacza środek komórki m i (x r, y r, θ r ) oznacza bieżącą pozycją robota (z j,sens, θ j,sens ) oznacza j-tą wartość uzyskaną przez czujnik w bieżącej chwili r (x i x r ) 2 + (y i y r ) 2 ( ) yi y r φ arctg θ r x i x r k argmin j φ θ j,sens if r > min(z max, z k,sens + α/2) OR φ θ k,sens > β/2 then return l 0 end if z k,sens < z max AND r z k,sens < α/2 then return l occ end if r < z k,sens then return l free end Algorytm 5.3: Algorytm odwrotnego modelu czujnika odległości. W kroku 4 i 5 algorytmu są wyznaczane współrzędne komórki w układzie biegunowym związanym ze środkiem robota. W kroku 6 jest wyznaczany indeks k próbki skanu terenu, którego kąt skanowania jest najbardziej zbliżony do kąta φ. W kroku 7 następuje sprawdzenie, czy dana komórka jest widziana przez skaner. Komórka uznawana jest za niewidoczną, jeśli zachodzi jedna z możliwości: komórka jest położona dalej, niż wynosi zasięg lasera; w pobliżu komórki znajduje się obiekt, który ją przesłania, przy czym odległość do środka komórki jest mniejsza niż ustalona wartość α/2; różnica pomiędzy kątem skanowania, a kątem φ, jest, co do modułu większa, niż ustalona wartość β/2. Kiedy komórka znajduje się w polu widzenia dalmierza, sprawdza się czy jest ona zajęta, co dane jest warunkiem przedstawionym w kroku 10. Zajętość jest potwierdzana, gdy odległość zmierzona do obiektu w k-tej próbce jest mniejsza, niż przyjęty zasięg dalmierza oraz różnica, co do modułu, pomiędzy zmierzoną odległością z k,sens a obliczoną odległością do komórki r jest mniejsza, niż α/2. W przeciwnym razie jeśli odległość r jest mniejsza niż z k,sens komórka jest uznawana za wolną. 80

93 Algorytm zwraca jedną z trzech możliwych wartości: l 0 - gdy dana komórka m i nie znajduje się w zasięgu widzenia czujnika; l occ - gdy komórka uznawana jest za prawopodobnie zajętą; l free - gdy komórka uznawana jest za prawdopodobnie wolną. Graficzna reprezentacja parametrów znajduje się na Rysunkach 5.15 oraz 5.14, które obejmują część terenu badanej komórki z Rysunku 5.13 Rysunek 5.13 przedstawia przykład wyznaczania zajętości komórki na podstawie skanu dalmierza laserowego. Pomarańczowy pięciokąt na rysunjku reprezentuje robota, a zielona kropka na szarym kwadracie wyznacza badaną komórkę przestrzeni. Widoczne niebieskie linie (oznaczone jako d i oraz d j ) wyznaczają kolejne dwie wiązki ze skanu dalmierza, między którymi znajduje się półprosta o początku w środku pięciokąta i przechodząca przez badaną komórkę. Półprosta łącząca robota oraz komórkę została oznaczona czerwoną, przerywaną linią. Kolorem czarnym oraz szarym oznaczono obiekty znajdujące się w środowisku. Rysunek 5.13: Widoczność komórki przez robota. Parametr β określa kąt pomiędzy dwoma półprostymi o początku w środku robota i otaczającymi badaną komórkę z obu stron (na Rysunku 5.14 półproste zaznaczone są jasnoniebieskimi, przerywanymi liniami). Komórka może zostać uznana za widzianą, jeśli pomiędzy tak określonymi półprostymi znajduje się próbka o indeksie k z dalmierza (indeks wyznaczany jest w kroku 6tym algorytmu 5.3). Innymi słowy komórka jest widoczna, jeśli kąt pomiędzy promieniem wodzącym od robota do komórki oraz promieniem badanej próbki dalmierza jest mniejszy, niż β/2. 81

94 Rysunek 5.14: Graficzna reprezentacja parametru β. Na Rysunku 5.15 znajduje się graficzna reprezentacja parametru α. Parametr ten określa długość boku kwadratu, którego środek pokrywa się ze środkiem komórki. Komórka może być uznana za widzianą, jeśli różnica co do modułu między odległością robota do komórki, a wskazaniem odległości dla k-tej próbki skanu dalmierza jest mniejsza, niż ustalona wartość α/2. Rysunek 5.15: Graficzna reprezentacja parametru α. 82

95 Zrealizowany algorytm siatki zajętości Przedstawiony powyżej proces budowy mapy siatki zajętości, można podzielić stosunkowo prosto na dwa etapy. W pierwszym jest wyznaczana mapa cząstkowa, zależna od pozycji robota, w której wykonywany był skan terenu. Mapie przypisywany jest pozycja robota z momentu przeprowadzenia skanu, a komórki w niej zawarte są budowane względem lokalnego układu odniesienia, ze środkiem w punkcie (0, 0). Wielkość terenu opisywana przez mapy cząstkowe nigdy nie będzię większa, niż kwadrat o boku dwukrotnej długości promienia lasera, co wynika ze wzorów (5.17) - (5.17). Dodatkowo dla tak utworzonej mapy można zwolnić pamięć w wierszach i/lub kolumnach macierzy, w których żadna komórka nie zmieniła bazowej wartości prawdopodobieństwa zajętości. W drugim etapie mapy cząstkowe są łączone w mapę globalną. Komórki kolejnych map cząstkowych są kolejno włączane do ogólnej mapy, za pomocą algorytmu opisanego powyżej. W rzeczywistości mapy lokalne są więc zbiorami wartości l i,j prawdopodobieństwa zajętości kolejnych komórek dla pojedynczego skanu. Przedstawione mapy lokalne nie są metodą dyskretyzacji zadania SLAM, którego opis można znaleźć np. w [20]. Zaprezentowane mapy lokalne mogą być zastosowane w większości praktycznych aplikacji, także w algorytmach wielohipotezowych, czy przy wykorzystaniu technik zamykania pętli - ewentualne zmiany pozycji robota wpływają wówczas jedynie na wektor przesunięcia samej mapy Diagramy klas Na Rysunku 5.16 przedstawiono diagramy klas obsługujących mapy siatki zajętości. Klasa MapContainer zawiera implementację kontenera mapy. Kontener przechowuje dwuwymiarową, rozszerzalną tablicę elementów klasy MapElem. Dla celów optymalizacji klasa MapElem przechowuje prawdopodobieństwo zajętości zarówno w postaci standardowej (składowa occupancy), jak również logarytmicznej (składowa ltcell). Mapa globalna została zrealizowana z użyciem wzorca typu Singleton [18, 19]. 83

96 Rysunek 5.16: Diagram dziedziczenia dla klasy MapContainer Rysunek 5.17: Diagram współpracy dla klasy MapElem 84

97 Rozdział 6. Badania eksperymentalne 6.1. Wstęp Wykonane testy obejmują eksperymentalną weryfikację poprawności działania opisanych algorytmów SLAM wykorzystujących filtr Kalmana oraz zbadanie podstawowych własności algorytmów. Algorytmy zostały przetestowane w środowiskach symulacyjnym oraz labolatoryjnym. Do podstawowych własności algorytmów, jakie zostały zbadane należą: zajętość pamięci; czasy działania. Dla testów przeprowadzanych z wykorzystaniem symulatora Stage, dostępne były idealne wskazania odometrii oraz skanów dalmierza laserowego. Zaimplementowany został dodatkowo mechanizm zaburzania wskazań odometrii, w celu weryfikacji działania zaimplementowanych algorytmów. W badaniach eksperymentalnych wykorzystano odczyty rzeczywistych czujników. Odczyty te były obarczone błędami pomiarowymi. Oprócz testów identycznych do przeprowadzanych z wykorzystaniem symulatora, zbadano wykrywanie znaczników terenowych. Wyniki testów przedstawiono w postaci: porównania przebytych ścieżek, wyznaczonych na podstawie odometrii oraz estymowanych za pomocą badanych algorytmów; graficznej reprezentacji macierzy kowariancji; mapy siatki zajętości zrealizowane podczas eksploracji terenu. 85

98 Porównanie ścieżek Badane algorytmy nieliniowej filtracji Kalmana wykorzystano do estymacji ścieżek przebytych przez robota. Do otrzymanych wyników dołączono wyniki uzyskane z odometrii robota oraz wyniki otrzymane za pomocą sterownika LODO. Sterownik LODO, realizaujący stabilizację odometrii za pomocą wskazań dalmierza laserowego, został wykorzystany zamiast zewnętrznego systemu pomiarowego do mierzenia dokładnego położenia robota. Istotnym jest to, że wyniki uzyskane ze sterownika LODO, posłużyły do strojenia badanych algorytmów; oznacza to, że dostrojone algorytmy mogą być co najwyżej tak dokładne, jak metoda estymacji pozycji zaimplmentowana w sterowniku LODO Graficzna reprezentacja macierzy kowariancji Macierz kowariancji błędu P k można przestawić graficznie w postaci elipsy. W literaturze najczęściej elipsa nazywana jest elipsą błędu (ang. error ellipse). W niniejszej pracy wykorzystano elipsę błędu w celu reprezentacji niepewności wyznaczonych wartości położenia robota (tj. współrzędnych x oraz y). Na wykresach map siatek zajętości badanych algorytmów przedstawionych w dalszej części niniejszego rozdziału, zielona elipsa przedstawia elipsę błędu utworzoną na podstawie macierzy kowariancji. Środek elipsy błędu jest umieszczany na wykresie w punkcie przedstawiającym rzeczywistą wartość estymowanej wartości, a pole elipsy wyznacza obszar w jakim powinna znaleźć się estymata z prawdopodobieństwem 39,4% (obliczenia dla tej i pozostałych wartości dotyczących elipsy błędu można znaleźć w [31]). Dokładna wartość estymowanej zmiennej losowej rzadko jest dostępna w rzeczywistej sytuacji, dlatego dla środka elipsy przyjmuje się wartość estymaty. Część obszaru zajęta przez elipsę, przedstawia zbiór wartości wśród których z określonym prawdpodobieństwem znajduje się wartość rzeczywista. Prawdopobieństwo, że elipsa zawiera wartość rzeczywistą, można zwiększyć dzięki użyciu współczynnika, stosowanego przy wyznaczaniu długości osi elipsy (patrz równanie (6.5)). Przykładowe wartości współczynników znajdują się w tabeli 6.1. W praktyce najczęściej wykorzystywana jest elipsa dla wartości prawdopodobieństwa 95,0% [5]. prawdopodobieństwo 39,4% 86,5% 95,0% 98,9% współczynnik σ 1,000 2,000 2,447 3,000 Tablica 6.1: Przykładowe wartości współczynników dla zadanych wartości prawdopodobieństwa wyznaczanych przez obszar elipsy. Źródło: [5]. 86

99 W pierwszym kroku obliczania elipsy, są wyznaczane wartości własne macierzy P xy o wymiarze 2 2, będącej podmiacierzą macierzy kowariancji P k. Wartości własne macierzy P xy odszukujemy przyrównując wyznacznik macierzy P xy λi do zera i rozwiązując powstałe równanie kwadratowe ze względu na zmienną λ. Przyjmując oznaczenia dla macierzy P xy : A = a b ze wzorów dla trójmianu kwadratowego otrzymujemy: λ 1 = a + d + (a + d) 2 4(ad b 2 ) 2 λ 2 = a + d (a + d) 2 4(ad b 2 ) 2 b d (6.1) (6.2) (6.3) Pierwiastki trójmianu λ 1 oraz λ 2 są wartościami własnymi (skróconej) macierzy kowariancji P xy. W drugim kroku jest wyznaczane nachylenie elipsy zgodnie z zależnością [25]: φ = 1 ( ) 2b 2 atan a 2 d 2 Równanie elipsy o środku w punkcie (0, 0) ma postać: (6.4) x 2 λ 1 + y2 λ 2 = σ (6.5) Wartość σ jest dobierana zgodnie z współczynnikiem podanym w tabeli 6.1. Długości osi elipsy przyjmują więc odpowiednio wartości: e a = σλ 1 (6.6) e b = σλ 2 (6.7) 6.2. Testy dalmierza laserowego Działanie dalmierza laserowego SICK LMS 200 nie jest identyczne w kolejnych chwilach czasowych. Przy nieruchomym robocie odczyty w kolejnych skanach nieznacznie się od siebie różnią. Różnice, choć niewielkie, mają wpływ na działanie algorytmów rozpoznawania obiektów w obrazach, jak np. dla wykorzystywej wtyczki miarn, a przez to na obliczone położenie znaczników. 87

100 Wyznaczanie znaczników Testy wyznaczenia położenia znaczników polegały na odnalezieniu pozycji tego samego znacznika w kolejnych chwilach czasowych przy nieruchomym robocie. Ponownie, ze względu na brak zewnętrznej metody weryfikacyjnej, możliwe było jedynie zgrubne oszacowanie dokładności wyznaczania położenia znaczników dla różnych pozycji robota. X Y 2006,24 712, ,99 711, ,69 712, ,5 713, ,96 712, ,85 712, ,43 713, ,53 712, ,94 711, ,82 711, ,52 713, ,53 712, ,11 711,081 Tablica 6.2: Położenie znacznika w postaci rogu, odnalezione w kolejnych skanach dalmierza. X Y minimalna średnia maksymalna minimalna średnia maksymalna 2003, , ,85 711, , ,377 Tablica 6.3: Minimalne, średnie oraz maksymalne wartości współrzędnych położenia znacznika. Wyniki uzyskane w kolejnych chwilach czasowych dla środowiska symulacyjnego, są stałe i niezmienne, tj. znacznik zawsze jest odnajdywany w tym samym położeniu. Z kolei dla środowiska rzeczywistego, wyniki w kolejnych krokach różnią się. Analiza odczytów uzyskanych z dalmierza pozwoliła jednoznacznie stwierdzić, że dane wejściowe wtyczki miarn, tj. skan terenu, nie daje w kolejnych krokach tych samych rezultatów. W tabeli 6.3 podano minimalne, średnie oraz maksymalne wartości położenia znaczników w układzie biegunowym, związanym z robotem. 88

101 Na rysunku 6.1 pokazano skany terenu wykonane z danych testu w sytuacji rzeczywistej, uzyskane w programie playerv. Strzałki koloru różowego przedstawiają środki odnalezionych w skanie linii (a dokładniej: środki odnalezionych odcinków, należących do linii). Podobnie jak poprzednio, test był wykonywany przy nieruchomym robocie. Jak widać, w kolejnych krokach, przy stałej pozycji, odnalezione zostały różne liczby znaczników. a) b) c) d) Rysunek 6.1: Przykładowe skany terenu, uzyskane w programie playerv, z rzeczywistych odczytów dalmierza, przy braku przemieszczenia robota. 89

102 Zarówno w testach środowiska symulacyjnego oraz rzeczywistego wykorzystano ten sam algorytm odnajdywania znacznika w skanie, przy jednakowej konfiguracji, stąd wniosek, iż zaobserwowany błąd nie powstał przez błędne działanie pakietu miarn. Badane środowisko było stacjonarne Powtarzalność wyników dalmierza Analiza surowych odczytów ze skanera, pozwoliła stwierdzić, iż problem leży w działaniu dalmierza, który nie dostarcza identycznych danych w kolejnych skanach. W tabeli 6.4 przedstawiono kilka próbek uzyskanych za pomocą dalmierza w kolejnych krokach skanowania. Ze względu na swoją objętość wyniki zostały skrócone jedynie do kilku wybranych (skan dalmierza dostarcza 181 próbek w każdym kroku). l.p. kąt β zmierzona odległość w [mm] odległości w [mm] (maxpróbki w [ ] k = 1 k = 2 k = 3 k = 4 min max średnia min) , , , ,25 51 Tablica 6.4: Wyniki skanowania terenu przez dalmierz laserowy SICK LMS200 dla wybranych próbek w kolejnych czterech krokach. Liczba porządkowa przedstawia indeks z tabeli wyników skanu (liczony od 1). Każdy wiersz wyników skanu zawiera wartość kąta skanowania β (wartość jest stała dla przyjętej rozdzielczości dalmierza) oraz zmierzonej odległości do do obiektu znajdującym się w środowisku otaczającym robota. Kąt β przedstawionych znajduje się w kolumnie drugiej tabeli. W kolumnach od trzeciej do szóstej znajdują się odległości do obiektów, jakie odczytano z dalmierza w kolejnych chwilach czasowych. W kolejnych trzech kolumnach dla każdej z próbek znajdują się wartości minimalna, maksymalna oraz średnia zmierzonych odległości. Wartość zapisana w ostatniej kolumnie określa różnicę między maksymalną, a minimalną odległością uzyskaną podczas skanowania. Różnica między wartościami najmniejszymi, a największymi oscyluje wokół wartości 10 milimetrów i jedynie dla dwu próbek znacznie ją przekracza (osiągając 30 i 51 milimetrów). Prawdopodobne wydaje się, że błąd wskazania pojedynczej wiązki dalmierza jest zgodny ze specyfikacją urądzenia (5 mm). Błąd ten jednak 90

103 nie uwzględnia niedokładności obrotu lustra, wykonywanego podczas dokonywania skanu (porównaj z Rysunkiem 4.3), w którym autor pracy widzi najbardziej prawdopodobną przyczynę uzyskania takich danych z lasera Budowa mapy siatek zajętości Parametry odwrotnego modelu czujnika Zgodnie z algorytmem budowy map siatek zajętości opisanym w podrozdziale 5.7., model odwrotny czujnika wymaga dostrojenia poprzez dobór dwóch parametrów: α oraz β. Parametr β występujący w odwrotnym modelu czujnika wyznacza maksymalną odległość kątową, jaka może dzielić prostą przechodzącą przez środek czujnika i środek komórki m i oraz najbliższą niej wiązkę skanowania z czujnika. Eksperymentalny dobór wartości tego parametru, polegał na utworzeniu mapy z dostępnych wskazań dalmierza laserowego, przy braku przesnięcia się robota w ustalonej pozycji. Oczywistym jest, że wartość parametru jest zależna od wielkości terenu, jaką pokrywają komórki mapy. Im mniejszy obszar reprezentują komórki, tym wartość parametru β musi być większa, aby pokryć cały obszar. Eksperymentalne wyniki zostały przedstawione w postaci przykładowych map na Rysunku 6.2. Na Rysunku 6.2 a) pokazano mapę dla parametru β = 0, Dla tej wartości, wszystkie komórki znajdujące się w zasięgu widoczności dalmierza mają swoje odwzorowanie w tworzonej mapie. Na Rysunku 6.2 b) znajduje się mapa zbudowana dla parametru β = 0, 01. Jak widać wybór komórek układa się w charakterystyczne wzory łukowe i nie pokrywa wszystkich komórek, jakie znajdują się w zasięgu widoczności dalmierza. Na Rysunku 6.2 c) przedstawiono mapę zbudowaną dla β = 0, 001. Jedynie nieliczne komórki zostały uznane za widoczne przez laser. Zwiększanie parametru β ponad wyznaczoną wartość dla przypadku z Rysunku 6.2 a) nie wnosi żadnych nowych informacji do mapy. 91

104 a) β = 0, b) β = 0, 01 c) β = 0, 001 Rysunek 6.2: Przykłady mapy siatek zajętości dla różnych wartości parametru β. Wpływ parametru α, na odwzorowanie obszaru w budowanej mapie jest mniej widoczny, niż w przypadku parametru β. Wartość α obrazuje jak daleko od środka komórki m i musi znaleźć się przeszkoda, aby komórka mogła być uznana za zajętą. W praktyce parametr α może mieć zatem duże znaczenie, kiedy z wyznaczonej mapy korzystają metody planowania ścieżki, bowiem ma bezpośredni wpływ na to czy komórkę można uznać za zajętą, czy wolną. 92

105 a) α = 100 b) α = 10 c) α = 200 Rysunek 6.3: Przykłady mapy siatek zajętości dla różnych wartości parametru α. Na Rysunku 6.3 znajdują się mapy zbudowane z tych samych wskazań dalmierza dla wartości parametru α równego dla a) 100 mm, b) 10 mm, c) 200 mm. Wielkość komórek budowanej mapy, ustalona została na kwadraty o wielkości 100 mm. Kolorem niebieskim wyszczególniono na rysunku trzy fragmenty zbudowanej mapy, gdzie różnica w mapach jest najlepiej widoczna. Jak widać mapa z przypadku b) praktycznie nie zawiera komórek koloru czarnego, oznaczających zajętość danego obszaru. Dla przypadków a) oraz c) komórki koloru czarnego są na mapach dobrze widoczne. Z praktycznego punkt widzenia jednak, przy wartości α większej niż rozmiar komórki, zajętość jednej komórki, byłaby definiowana przez zajętość komórki sąsiadującej z nią. Wartość równa wielkości komórki pozwala uzyskać mapy dobrze obrazujące środowisko. 93

106 Wpływ niedokładności pozycji robota na mapę siatki zajętości Mapy siatek zajętości są bardzo wrażliwe na dokładność wyznaczonej pozycji robota. Przy dowolnie małych odchyleniach od rzeczywistej pozycji, budowane mapy stają się znacznie mniej czytelne, a kolejne odczyty charakteryzujące zajętość / nie zajętość obszaru terenu zaczynają na siebie nachodzić i w rezultacie utworzona mapa jest błędna. a) b) c) Rysunek 6.4: Przykłady mapy siatek zajętości przy zaburzonych parametrach położenia robota. 94

107 Na Rysunku 6.4 przedstawione zostały mapy siatek zajętości zbudowane z tych samych pomiarów, jakich użyto przy przedstawieniu wpływu parametrów α oraz β. W przypadku 6.4 a) mapa powstała dla zaburzonych wskazań położenia robota o wartość losową z przedziału [ 50; 50] zadaną z milimetrach. Dla przypadku 6.4 b) wartość losowa pochodziła z przdziału [ 150; 150]. Jak widać dokładność mapy przy zaledwie 40 zinterpretowanych odczytach znacznie osłabła. Na Rysunku 6.4 c) przedstawiona jest mapa, która została utworzona dla położenia robota zaburzanej o wartość z przedziału [ 50; 50] oraz przy zaburzonym kącie orientacji z przedziału [ 0.05; 0.05] w radianach Testy rekonstrukcji ścieżki Wstęp Wszystkie wykonane testy miały charakter porównawczy. Za wskazanie dokładne przyjęto wyniki uzyskane za pomocą sterownika LODO. Zamieszczone wykresy przedstawiają: położenie robota; kąt orientacji bazy robota; mapy siatki zajętości. Na wykresach położenia oraz orientacji bazy umieszczono jednocześnie wyniki uzyskane za pomocą odometrii, sterownika LODO oraz badanych algorytmów. Wykres orientacji bazy należy czytać od wewnątrz na zewnątrz okręgu, gdzie kolejne wartości przebiegu, wyznaczają orientację bazy w kolejnych kroków algorytmu Odometria robota Protonek Dane odometryczne uzyskiwane za pomocą enkoderów optycznych są dość dokładne dla ruchu prostoliniowego. W tym przypadku odczyty z czujnika nie odbiegają znacznie od wyników uzyskiwanych za pomocą sterownika LODO, co można zaobserwować na wykresach Rysunków

108 Rysunek 6.5: Przebyta przez robota ścieżka test 1. Rysunek 6.6: Przebieg orientacji bazy robota odometrii oraz LODO test 1. 96

109 Na Rysunku 6.7 znajdują się wykresy błędów pozycji wskazania odometrii w stosunku do wskazania otrzymanego ze sterownika LODO, które uznane zostały za rzeczywiste (co jest konsekwencją braku zewnętrznego systemu weryfikacji pozycji robota). Rysunek 6.7: Błędy wskazania odometrii test 1. Na Rysunkach znajdują się mapy siatek zajętości zbudowane w oparciu o wyniki przedstawione powyżej. Na rysunkach map siatek zajętości znajduje się kilka charakterystycznych elementów. Seledynowy trójkąt przedstawia pozycję początkową robota, a bordowy pozycję końcową. Niebieskie punkty przedstawiają dyskretną ścieżkę przebytą przez robota. Mapy siatek zajętości przedstawiają prawdopodobieństwo zajętości poszczególnych komórek, co zostało szerzej opisane w podrozdziale 3.8. Kolor biały charakteryzuje pewność, że komórka jest wolna. Podobnie kolor czarny oznacza część przestrzeni zajętej (przez dowolny obiekt). Pośrednie barwy szarości przedstawiają prawdopodobieństwo zajętości przestrzeni, przy czym im barwa jest jaśniejsza, tym większe prawdopodobieństwo, iż komórka należy do wolnej przestrzeni. Wskazania odometryczne zaczynają odbiegać od uzyskanych za pomocą sterownika LODO, kiedy robot skręca, co widać na wykresach Rysunków Wśród przyczyn błędów, obok standardowych problemów odometrii, można także dodać niedokładność modelu ruchu. Wykorzystywany dla robota Protonek model jest modelem ruchu dla bazy jezdnej z napędem różnicowym. 97

110 Rysunek 6.8: Mapa siatki zajętości zbudowana na podstawie wskazań sterownika LODO test odometrii 1. Rysunek 6.9: Mapa siatki zajętości zbudowana na podstawie wskazań odometrii test odometrii 1. 98

111 Rysunek 6.10: Przebyta przez robota ścieżka test 2. Rysunek 6.11: Przebieg orientacji bazy robota odometrii oraz LODO test 2. 99

112 Rysunek 6.12: Błędy wskazania odometrii test 2. Co można łatwo zaobserwować na poniższych mapach siatek zajętości , pozycja robota uzyskana za pomocą wskazań odometrycznych, raczej nie nadaje się do zastosowania przez algorytmy wyższych warstw, jak np. planowania ścieżki. Rysunek 6.13: Mapa siatki zajętości zbudowana na podstawie wskazań sterownika LODO test odometrii

113 Rysunek 6.14: Mapa siatki zajętości zbudowana na podstawie wskazań odometrii test odometrii Wyniki symulacyjne Wyniki symulacyjne działania algorytmów uzyskano dla środowiska przedstawionego na rysunku Znaczniki terenowe w postaci rogów są dobrze widoczne. Rysunek 6.15: Symulacyjne środowisko pracy robota. 101

114 a) b) Rysunek 6.16: Przebiegi położenia (a)) oraz orientacji (b)) bazy robota w kolejnych krokach działania algorytmów - symulacja. 102

115 Rysunek 6.17: Mapa siatki zajętości zbudowana na podstawie wskazań odometrii. Rysunek 6.18: Mapa siatki zajętości uzyskana za pomocą algorytmu EKFSLAM. 103

116 Rysunek 6.19: Mapa siatki zajętości uzyskana za pomocą algorytmu UKFSLAM. Jak można zaobserwować na rysunkach uzyskane mapy siatek zajętości praktycznie się nie różniły, co jest konsekwencją braku znacznych różnic w położeniu, czy orientacji bazy robota obliczonych za pomocą każdego z algorytmów Wyniki eksperymentalne Dwa pierwsze testy eksperymentalne wykonany zostały w laboratorium, trzeci w korytarzu przed nim. Pierwszy test miał na celu jedynie wykazać działanie badanych algorytmów w czasie wykonywania skrętu przez robota. W drugim teście robot [wpierw poruszał się po łuku, po czym przebył pewien odcinek drogi prostej. W ostatinim, trzecim teście robot poruszał się torze przypominającym spiralę. test 1 W pierwszym teście sprawdzono jakość rekonstrukcji ścieżki w sytuacji, gdy robot wykonuje skręt, czyli wówczas kiedy odometria robota dostarcza błędne odczyty. Robot został ustawiony naprzeciw ściany, na której są ustawione szafy, co zapewniło widoczność znaczników w postaci rogów. Środowisko w jakim poruszał się robot pokazano na Rysunku 6.20, oraz na uproszczonym modelu typu CAD na Rysunku Widoczna na rysunku modelu CAD podziałka ma wymiar metrów. 104

117 Rysunek 6.20: Środowisko eksperymentów 1 oraz 2. Rysunek 6.21: Środowisko eksperymentów 1 oraz 2 - mapa CAD. 105

118 Rysunek 6.22: Przebieg położenia robota test 1. Rysunek 6.23: Przebieg orientacji bazy robota test

119 Jak łatwo zauważyć, na wykresach położenia z Rysunku 6.22 oraz orientacji bazy Rysunek 6.23 robota, zrekonstruowane ścieżki znacznie odbiegają od wyników uzyskanych na podstawie odometrii. Przyjmując wskazania sterownika LODO za wskazania rzeczywiste, na Rysunku 6.24 znalazły się wykresy błędów położenia oraz orientacji. Błędy współrzędnych położenia dla badanych algorytmów są niewielkie i jedynie dla pojedynczych chwil czasowych przyjmują wartości bliskie 100 [mm]. Błąd orientacji orientacji bazy robota jedynie w pojedynczych momentach przekaracza wartość 0.05 [rad], a zawsze jest mniejsza, niz 0.1 [rad]. Na Rysunkach pokazano mapy siatek zajętości przedstawiające rekonstrukcje środowiska, utworzone w oparciu o wskazania odometrii oraz badane algorytmy. Na mapach zbudowanych przez badane algorytmy pomarańczowe punkty przedstawiają pozycję odnalezionych w terenie znaczników, a jasnozielone elipsy są graficzną reprezentacją macierzy kowariancji P k dla bieżących chwil działania algorytmów. W przypadku mapy zbudowanej dla wskazań odometrii dobrze widoczna jest niedokładność pozycji robota podczas dołączania kolejnych skanów. Badane algorytmy poprawnie zrekonstruowały ścieżkę. a) b) Rysunek 6.24: Błędy wskazania współrzędnych pozycji badanych algorytmów a) EKFSLAM b) UKFSLAM test

120 Rysunek 6.25: Mapa siatki zajętości utworzona ze wskazań odometrii test 1. Rysunek 6.26: Mapa siatki zajętości utworzona na podstawie wyników algorytmu EKFSLAM test

121 Rysunek 6.27: Mapa siatki zajętości utworzona na podstawie wyników algorytmu UKFSLAM test 1. W trakcie eksperymentu zaobserwowano dziwne działanie algotmu UKFSLAM. W macierzy kowariancji po wyznaczeniu kilku korekt, praktycznie zerowane były kowariancje skrośne pomiędzy współrzędnymi pozycji robota, a współrzędnymi położenia znaczników. Jednocześnie elementy pozycji robota, rosły wraz ze wzrostem niepewności wyznaczenia położenia (na co bezpośredni wpływ miała macierz kowariancji błędu Q k ). W tym samym momemcie elementy kowariancji położenia znaczników dążyły do zera. Autor pracy próbował kilku wariantów strojenia algorytmu UKFSLAM, jednak nie przyniosło to znacznej poprawy. test 2 W tym teście robot rozpoczynał ruch od narożnika pomieszczenia, gdzie po wykonaniu skrętu poruszał się w pobliżu ściany. wystartował z końca labolatorium i poruszał się wzdłuż niego. Początkowo w zasięgu robota znajdowały się 3 znaczniki, które jednak po stosunkowo krótkim czasie (kilkanastu krokach) przestały być widziane przez skaner. Środowisko testowe było identyczne z wykorzystanym w poprzednim eksperymencie. 109

122 Rysunek 6.28: Przebieg położenia robota test 2. Rysunek 6.29: Przebieg orientacji bazy robota test

123 a) b) Rysunek 6.30: Błędy wskazania współrzędnych pozycji badanych algorytmów a) EKFSLAM b) UKFSLAM test 2. Rysunek 6.31: Mapa siatki zajętości utworzona ze wskazań odometrii test

124 Rysunek 6.32: Mapa siatki zajętości utworzona na podstawie wyników algorytmu EKFSLAM test 2. Rysunek 6.33: Mapa siatki zajętości utworzona na podstawie wyników algorytmu UKFSLAM test

125 W eksperymencie można zaobserwować niedokładności mapy siatki zajętości uzyskanej na podstawie odometrii, które częściowo zostały poprawione w wyniku działania badanych algorytmów. Mapy siatki zajętości pokazano na Rysunkach Na wykresach rysunku 6.30 widoczne są różnice, pomiędzy wartościami pozycji ze sterownika LODO oraz badanymi algorytmami, w kolejnych krokach działania. W tym teście ujawnił się problem w pracy algorytmu UKFSLAM. W fazie aktualizacji algorytmu, realizowanym dla więcej niż jednego znacznika, obliczona macierz kowariancji nie posiadała własności dodatniej określoności. Brak dodatniej określoności uniemożliwia wyznaczenie rozkładu LL T niezbędnego w obliczeniach. Kilka prób modyfikacji algorytmu jak np. dzielenie macierzy wzmocnienia Kalmana przez liczbę odnalezionych znaczników, czy ponowna generacja zbioru punktów sigma po każdej aktualizacji nie wniosły istotnej poprawy do działania algorytmu. Ostatecznie obliczenia fazy aktualizacji algorytmu UKFSLAM ograniczono do korekty z wykorzystaniem tylko jednego znacznika. Przyjęte zostało, że wybierany był pierwszy ze znalezionych znaczników, ale przeprowadzone badania wykazały, że nie miało to istotnego znaczenia, który ze znaczników został wybrany. Ograniczenie dotyczy wyników testów drugiego oraz trzeciego z wykorzystaniem algorytmu UKFSLAM. test 3 W tym eksperymencie robot poruszał się po korytarzu. Tor ruchu był zbliżony do spirali. Na Rysunku 6.34 są widoczne kolumny, których narożniki mogą stać się znacznikami. Na Rysunku 6.35 zamieszczono uproszczony model typu CAD badanego terenu. 113

126 Rysunek 6.34: Środowisko dla eksperymentu 3. Rysunek 6.35: Środowisko eksperymentu 3 - mapa CAD. 114

127 Rysunek 6.36: Przebieg położenia robota test 3. Rysunek 6.37: Przebieg orientacji bazy robota test

128 Na wykresach położenia (Rysunek 6.36) oraz orientacji bazy (Rysunek 6.37) widać dużą różnicę pomiędzy pozycją uzyskaną ze wskazań odometrii oraz pozycją obliczaną w algorytmach. Największą różnicę widać dla kąta orientacji bazy. Rysunek 6.38 przedstawia wykresy różnic wartości pozycji między pozycją odczytaną ze wskazań sterownika LODO, a pozycją uzyskaną z algorytmamów. Na wykresach jest widoczne niewielkie błędy kąta orientacji bazy robota, stale kompensowane w czasie. Różnica wartości dla położenia jedynie dla pojedynczych kroków przekracza 100 mm, można więc stwierdzić, że jest stosunkowo niewielka. Różnica kąta orientacji bazy jedynie dla pojedynczych chwil przekracza wartość 0,05 radiana. Na Rysunkach pokazano mapy siatki zajętości, zbudowane dla wskazań odometrii oraz badanych algorytmów. Jak widać różnica pomiędzy badanymi algorytmami, a odometrią jest znaczna. a) b) Rysunek 6.38: Błędy wskazania współrzędnych pozycji badanych algorytmów a) EKFSLAM b) UKFSLAM test

129 Rysunek 6.39: Mapa siatki zajętości utworzona ze wskazań odometrii test 3. Rysunek 6.40: Mapa siatki zajętości utworzona na podstawie wyników algorytmu EKFSLAM test