Ostatnio mocno pogłębiłem wiedzę dotyczącą algorytmów SLAM. Krótkoterminowym celem jest oczywiście inspiracja przy tworzeniu jak najlepszego sposobu lokalizacji i nawigacji w robocie Micromouse. W dzisiejszym wpisie przedstawię różnice między Micromouse, a typowym problemem SLAM oraz pomysły na implementację będące konsekwencją tych różnic.
Wykorzystanie wiedzy o labiryncie
Labirynt, w którym porusza się robot, jest zbudowany według określonych zasad spisanych w regulaminie zawodów. Możemy wykorzystać tą wiedzę do uproszczenia algorytmów lokalizacji i nawigacji. Przydatne informacje to między innymi:
- Stały, ściśle określony rozmiar ścian.
- Ściany mogą występować tylko na krawędziach pól.
- Pola mają stały, ściśle określony rozmiar.
- Labirynt to kwadrat składający się ze znanej ilości pól.
Z tej wiedzy korzystamy już na etapie projektowania hardware. Zamiast skomplikowanych i drogich dalmierzy laserowych, skanujących cała przestrzeń wokół robota, używamy prostych par dioda IR – fototranzystor skierowanych na stałe w wybranym kierunku i charakteryzujących się niewielkim, ale wystarczającym zasięgiem. Są one wystarczające do wykrycia najbliższej ściany z przodu i po bokach. Nie musimy jak w przypadku SLAM robić skanowania całej przestrzeni dookoła.
Mapa zna możliwe położenia ścian
Znane pozycje ścian umożliwiają implementację mapy w postaci siatki zajętości ze z góry wiadomymi miejscami, gdzie mogą znajdować się ściany. Właśnie takie rozwiązanie opisałem ostatnio przy okazji pierwszej implementacji mapy.
Jest jednak pewna różnica od standardowej grid mapy. Tutaj są duże pola, a ewentualne ściany na ich granicach są atrybutami pola. W prawdziwej grid mapie granulacja pól jest dużo większa tak, żeby każda ściana i słupek mogły zostać opisane za pomocą takich pól. Wtedy na jedno pole labiryntu składałoby się dużo więcej pól siatki. Rozwiązanie, które zastosowałem jest właśnie wykorzystaniem wiedzy o środowisku działania robota. Może w przyszłości przetestuje rozwiązanie z prawdziwą grid mapą.
Wykorzystanie siatki i landmarków jednocześnie
Znamy również rozmiar ścian i słupków oraz potencjalne miejsca występowania ich krawędzi – mogą one się znajdować jedynie na rogach pól. Takie miejsca idealnie nadają się na punkty orientacyjne służące do korekty pozycji. Są to łatwe do wykrycia dla czujników robota punkty znajdujące się na przecięciach 4 sąsiadujących ze sobą pól.
Mój pomysł na nawigację polega na połączeniu właściwości dwóch rodzajów map – siatki i punktów orientacyjnych. Siatka będzie służyła do planowania ścieżek, a punkty orientacyjne do korekty pozycji. Przy okazji stosując EKF z landmarkami nie będę musiał uwzględniać niepewności ich położenia. Jedyna niepewność może wynikać z dokładności wykonania elementów labiryntu i można ją pominąć. Punkty orientacyjne są od siebie na tyle odległe, że nie można ich pomylić jeżeli oszacowanie pozycji działa. Musielibyśmy pomylić się o cały rozmiar pola, a wtedy i tak robot pewnie w końcu pomyli drogę.
Oznacza to, że obliczenia EKF powinny być dosyć proste. Macierze stanu i kowariancji będą małe i co najważniejsze, ich rozmiar nie będzie zmieniał się w czasie. Najbliższe aktualnej pozycji robota punkty orientacyjne będą na bieżąco odczytywane z zapisanej mapy i pomogą w korekcji pozycji obliczonej przez odometrię.
Muszę się jeszcze zastanowić nad typami punktów orientacyjnych, sposobem ich wykrywania i ich wpływie na oszacowanie pozycji. Idealnie by było, gdyby udało się ich wpływ zaszyć w równaniu filtru Kalmana. Wtedy aktualizacja odbywałaby się automatycznie bez jakiś skomplikowanych szczególnych przypadków.
Problemem może być fakt, że przy odbiciach od końca ściany, albo od słupka nie mogę korzystać z konkretnej odległości zwracanej przez czujnik. Powierzchnia odbicia światła IR będzie tu mniejsza, co zaniży pomiar. Tak więc landmarki będą mogły korygować pozycję tylko w jednej osi.
Inne potencjalne landmarki to ściana z przodu widziana przez oba czujniki frontowe, albo ściany po obu stronach wykryte przez czujniki boczne i diagonalne. Roboty micromouse bardzo często wykorzystują te sytuacje do korekty swojej pozycji. Możemy wtedy porównać wartości odczytywane z czujników po obu stronach i wywnioskować na tej podstawie, czy robot nie jest trochę przekrzywiony. Fajne informacje na ten temat można znaleźć w prezentacji Ng Beng Kiat, konstruktora jednych z najszybszych Micromouse na świecie. Prezentacja zawiera również informacje o mechanice robota, czy sterowaniu. Mam wrażenie, że kiedyś można było ze strony autora ściągnąć również kod źródłowy jednego z jego robotów jako materiały edukacyjne dla studentów, ale coś nie mogę tego znaleźć.
Czym to podejście się wyróżnia?
Punkty orientacyjne takie jak końce ścian już od dawna są wykorzystywane w istniejących Micromouse’ach, można o tym przeczytać np. tutaj:
Metoda z tego przykładu, tak samo jak w większości innych rozwiązań, jest dosyć prymitywna. Wykrywamy odpowiedni sygnał z czujników za pomocą instrukcji warunkowych i robimy dla nich specjalną procedurę sterowania. Ja postaram się pójść w stronę ładniejszego od strony matematycznej i bardziej ogólnego rozwiązania. Celem projektu jest dla mnie głównie zdobywanie wiedzy, więc bardziej naukowe podejście, które później można by wykorzystać poza Micromouse jest bardzo kuszące.
Podsumowanie
To tyle jeżeli chodzi o koncepcję. W najbliższym czasie przejdę do implementacji. Przyszła już nowa wersja płytek i dokupione części elektroniczne. Udało się już również wszystko polutować i sprawdzić ważniejsze komponenty. Teraz na pierwszy ogień pójdą czujniki odległości. Muszę sprawdzić, czy wszystkie są tak samo dobrze skalibrowane, napisać procedurę obsługującą je sekwencyjnie i może jeszcze zakleić hotglue, aby się nie ruszały. Następnie będę mógł już w pełni zająć się lokalizacją i nawigacją. Bardzo prawdopodobne, że nie będę od razu implementował algorytmu z prawdziwego zdarzenia, tylko postaram się jak najszybciej dojść do miejsca, gdzie robot będzie potrafił samodzielnie przejechać labirynt. Dopiero od tego momentu pomyślę nad usprawnieniami. Mam nadzieję, że nie skończy się na stwierdzeniu „close enough” i odłożeniu robota w kąt jak tylko zacznie rozwiązywać labirynt.
Dodaj komentarz