Jednym z najważniejszych celów stawianych robotom mobilnym jest autonomiczność. Roboty powinny wykonywać swoje zadania przy minimalnym udziale człowieka. Aktualnie jest to możliwe w bardzo ograniczony sposób. Środowisko pracy robota musi być specjalnie przygotowane i się nie zmieniać, a czynności muszą być powtarzalne. Te założenia są spełnione na przykład w halach produkcyjnych, gdzie roboty zastępują człowieka od wielu lat przy żmudnych i powtarzalnych czynnościach. Marzy nam się jednak zwiększenie użyteczności robotów również w innych dziedzinach życia. Dla robotów mobilnych jednym z największych ograniczeń jest poruszanie się w nieznanym terenie. Występują tutaj dwa wzajemnie na siebie wpływające procesy. Określenie swojej pozycji w przestrzeni, czyli lokalizacja oraz rozpoznawanie i zapamiętywanie otoczenia, czyli mapowanie.

Człowiek wykonuje te zadania intuicyjnie posługując się głównie zmysłem wzroku. Robotowi sprawia to dużo większą trudność, ponieważ potrzebuje dokładnej pozycji wyrażonej liczbowo, a do dyspozycji zwykle ma dużo uboższe czujniki odległości. Co więcej błędy mapowania i lokalizacji się kumulują. Błędne oszacowanie pozycji przekłada się na błędne umiejscowienie przeszkód na mapie. Natomiast błędne określenie pozycji przeszkód przeszkadza w określeniu zmiany własnej pozycji. Narastanie tych błędów może przyczynić się do kompletnego rozjechania się mapy w pamięci robota z rzeczywistością. Problem jednoczesnego określania własnej pozycji i zbierania informacji o otoczeniu nosi nazwę SLAM – Simultaneous Localisation and Mapping i to właśnie nim zajmę się w tym wpisie.

SLAM w codziennym życiu

Temat SLAM jest zgłębiany od lat osiemdziesiątych, ale  algorytmy nie są jeszcze wystarczająco rozwinięte, aby mobilne roboty weszły do wielu dziedzin życia. Autonomia takich robotów w dalszym ciągu jest raczej dosyć prymitywna, a roboty są stosunkowo drogie. Dlatego w większości przypadków zamiast w pełni autonomicznych robotów stosuje się pojazdy zdalnie sterowane przez operatora. Są one wykorzystywane na przykład przez saperów do rozbrajania bomb, czy przez NASA podczas misji na marsie. Najlepszą aplikacją SLAM dostępną dla zwykłych ludzi (i jak widać na zdjęciu nie tylko ludzi) są roboty sprzątające takie jak iRobot Roomba.

Co ciekawe Roomba nie była pierwszym tego typu urządzeniem. Wcześniej był Electrolux Trilobite.

Punkty orientacyjne

Człowiek do poruszania się w terenie wykorzystuje punkty orientacyjne (po angielsku landmarks). Może to być na przykład wieża kościoła, stare drzewo, czy skrzyżowanie. Nawet jeżeli nie rozpoznajemy innych elementów otoczenia, dzięki punktom orientacyjnym wiemy gdzie jesteśmy. Dobry punkt orientacyjny jest łatwy do rozpoznania, nie zmienia swojej pozycji i jest dobrze widoczny.

Podobne podejście wykorzystują także roboty. Jednak z racji tego, że zwykle posługują się one prostymi czujnikami odległości o dużo mniejszych możliwościach niż ludzkie oko, punkty orientacyjne dla robotów muszą być prostsze. Dobrym punktem orientacyjnym jest na przykład róg pokoju, w którym schodzą się dwie ściany. W przestrzeni 2D ma on postać punktu, więc można przypisać mu współrzędne dokładne współrzędne x i y. Jest on również łatwy do wykrycia analizując odległości od ścian. Róg pokoju jest dużo lepszym punktem orientacyjnym niż sama ściana, ponieważ ona posiada określoną długość i samo zidentyfikowanie ściany i poznanie naszej odległości od niej nie wystarczy do określenia dokładnej pozycji.

Możliwości czujników odległości w wykrywaniu punktów orientacyjnych są bardzo ograniczone. Duże pole do popisu mają tutaj systemy wizyjne i machine learning. Za ich pomocą możnaby stworzyć system identyfikowania charakterystycznych obiektów i określania ich pozycji podobny do ludzkiego.

Mapowanie

W robotach mobilnych istnieją dwa główne podejścia do tworzenia map terenu:

  • Landmark map – mapa punktów orientacyjnych.
  • Occupancy grid map – siatka zajętości.

Pierwsze z nich opiera się na opisanych wyżej punktach orientacyjnych. Robot przechowuje w pamięci bazę punktów orientacyjnych z ich współrzędnymi oraz dodatkowymi danymi umożliwiającymi ich identyfikację. Dzięki nim później widząc punkt orientacyjny z innej pozycji jest w stanie powiązać go z wcześniejszym wpisem w bazie.  Zaletą takiej mapy jest fakt, że zajmuje mało miejsca w pamięci – zapisujemy tylko te informacje, które są potrzebne. Wada natomiast jest taka, że dla człowieka nie ma ona żadnej wartości. Przykładowa mapa tego typu została przestawiona poniżej. Jest to jedynie zbiór punktów w przestrzeni.

Drugie podejście – occupancy grid map – polega na podzieleniu przestrzeni wokół robota na małe pola o stałym rozmiarze. Dla każdego pola niezależnie określana jest zajętość. W praktyce stosuje się tutaj prawdopodobieństwo zajętości. Czyli 1 oznacza, że jesteśmy pewni, że komórka jest zajęta, a 0, że pusta. Wszystkie stany pośrednie są wyrażone wartościami ułamkowymi, a 0.5 oznacza, że nie znamy stanu komórki. Prawdopodobieństwa dla poszczególnych pól są uzupełniane po każdym odczycie z czujnika odległości. Jeżeli na przykład wykryliśmy przeszkodę w odległości 1 m, wszystkie pola na drodze czujnika o odległości mniejszej niż 1 m uznajemy za puste, pole odległe o 1 m za zajęte, a o dalszych polach nie mamy żadnej informacji, więc nie zmieniamy ich prawdopodobieństwa zajętości. Oczywiście możemy tu uwzględnić charakterystykę czujnika – szerokość wiązki skanującej i dokładność pomiaru. Ta metoda dobrze się sprawdza w ograniczonej, zamkniętej przestrzeni. Tworzone w ten sposób mapy są wartościowe dla człowieka. Ze względu na duże zużycie pamięci nie sprawdzi się ona w otwartym terenie.

Ruchome przeszkody

Mapowanie terenu nie zawierającego poruszającego obiektów jest stosunkowo proste. Jeżeli wcześniej wykryliśmy jakąś przeszkodę, możemy założyć, że przy następnym pomiarze ona dalej jest w tym samym miejscu. Możemy więc wykorzystać ją do nawigacji, albo zaplanować drogę, aby ją ominąć. Niestety rzeczywistość jest bardziej skomplikowana.

Robot może wykryć ludzi, zwierzęta, czy jakieś poruszające się urządzenia. Poza tym obiekty takie jak drzwi, czy krzesła mogą zmieniać swoje położenie. Stworzono więc klasyfikację obiektów:

  1. Statyczne nieruchome – nigdy nie zmieniają położenia np. ściana.
  2. Statyczne ruchome – mogą zmienić położenie np. krzesło.
  3. Dynamiczne – poruszają się np. człowiek.

Robot przeznaczony do pracy w środowisku, gdzie występują obiekty klas 2 i 3 musi radzić sobie z niespodziewanymi zmianami. Musi więc potrafić na przykład rozpoznać lokalizację widząc tylko część punktów orientacyjnych,  czy spodziewać się przeszkód w przeskanowanych wcześniej obszarach. Stosując siatkę zajętości musi więc uaktualniać stan przeskanowanych ponownie komórek i zwiększać niepewność dla obszarów dawno nie skanowanych. Z pomocą może przyjść tutaj klasyfikacja wykrytych przeszkód, którą najlepiej zrealizuje system wizyjny.

Lokalizacja

Własna pozycja robota jest określana na podstawie zapisanego stanu oraz dwóch wejść – odczytów z odometrii oraz czujników odległości. Odometria mówi nam o ile jej zdaniem robot się przesunął, natomiast czujniki odległości niosą informację o pozycji względem punktów orientacyjnych. Stan natomiast zawiera informacje o ostatnim oszacowaniu pozycji robota oraz o mapie otoczenia. Do określania pozycji wykorzystuje się najczęściej jedno z dwóch podejść:

  • Rózne odmiany Filtru Kalmana – KF, EKF, UKF.
  • Filtr cząsteczkowy (Particle filter, nie mylić z filtrem cząstek stałych DPF w samochodach).

O Filtrze Kalmana oraz EKF już trochę napisałem w kontekście Micromouse:

EKF – Rozszerzony filtr Kalmana

Do użycia w SLAM Filtr Kalmana musi być specjalnie dostosowany. Wektor stanu poza położeniem robota musi również zawierać mapę punktów orientacyjnych. Macierz kowariancji zostaje wtedy rozszerzona o zależności między pozycją robota a poszczególnymi punktami orientacyjnymi oraz o zależności wzajemne między punktami orientacyjnymi. Zależności te są uaktualniane w każdej iteracji filtru. Dzięki temu poprawa oszacowania pozycji robota związana z jednym punktem orientacyjnym wpływa pozytywnie na oszacowania wszystkich pozostałych punktów na mapie.

Filtr Kalmana zakłada, że estymowany stan to zmienne losowe o rozkładzie Gaussa. Jego oszacowanie jest więc wartością oczekiwaną i przechowuje również macierz kowariancji. W filtrze cząsteczkowym zastosowano inne podejście. Zamiast całego rozkładu przedstawionego za pomocą macierzy kowariancji, przechowywany jest pewien reprezentatywny zbiór realizacji zmiennej losowej. Każda cząsteczka filtru jest więc jednym możliwym rozwiązaniem, a za pomocą metod statystycznych określa się wyjściową wartość całego filtru. Odpowiednio dobrane próbki są więc pewnym przybliżeniem rozkładu prawdopodobieństwa. Możemy więc stosować filtr cząsteczkowy do zmiennych o dowolnym rozkładzie, niekoniecznie musi to być rozkład Gaussa.

Filtr cząsteczkowy ma przewagę nad EKF, jeżeli punktów orientacyjnych zrobi się bardzo dużo np. 50000. Macierz kowariancji EKF rozrośnie się wtedy do ogromnych rozmiarów mocno spowalniając obliczenia. Filtr cząsteczkowy nie musi przechowywać wzajemnych zależności między punktami orientacyjnymi, tylko kilkaset cząsteczek. Dzięki temu lepiej poradzi sobie z dużą ilością danych. Z kolei dla małej ilości punktów orientacyjnych, zbliżonej do ilości cząsteczek, EKF nie będzie się dłużej obliczał, a zapewni lepszą dokładność oszacowań. Nie będę tutaj wyprowadzał wszystkich wzorów na EKF i filtr cząsteczkowy. Po więcej informacji odsyłam do materiałów [1] i [2] na końcu artykułu.

Podsumowanie

Zadanie jednoczesnego mapowania i nawigacji można rozbić na następujące elementy:

  • Landmark extraction – znalezienie punktów orientacyjnych w odczytach z czujnika odległości.
  • Data association – dopasowanie znalezionych punktów do wcześniej odkrytych znajdujących się w bazie.
  • State estimation – określenie pozycji robota na podstawie wcześniejszego stanu i danych z odometrii.
  • State update – aktualizacja stanu robota w oparciu o punkty orientacyjne.
  • Landmark update – aktualizacja pozycji punktów orientacyjnych.

Część z nich może być realizowanych jednocześnie. Realizacja tych zadań jest ciekawym, ale jednocześnie trudnym problemem wymagającym sporej wiedzy z teorii sterowania, fizyki, sensoryki, czy programowania. Istniejące aktualnie rozwiązania problemu SLAM wyraźnie ustępują możliwościom człowieka.

 

Materiały dodatkowe

[1] Søren Riisgaard, Morten Rufus Blas  – “SLAM for Dummies”

[2] Bradley Hiebert-Treuer – “An Introduction to Robot SLAM (Simultaneous Localization And Mapping)”

[3] Paul Michael Newman – “C4B – Mobile Robotics”