Głównym zadaniem robota Micromouse jest znajdowanie drogi w labiryncie. Może więc wydawać się dziwne, że siadłem do tego tematu dopiero teraz – po roku od rozpoczęcia prac. Jednak do stworzenia mapy potrzebna jest dobrze działająca lokalizacja i czujniki ścian. Te zagadnienia mam już jako tako opanowane, można więc przejść do wyznaczania trasy. Zrobiłem już pierwszą implementację, która okazała się zaskakująco prosta. Kod oczywiście dostępny jest na GitHubie.
Koncepcja systemu
Celem robota micromouse jest dotarcie do środka labiryntu startując z jego rogu. W tym celu musi poznać położenie ścian i wyznaczyć trasę, która je omija. Aby zrealizować to zadanie powinien:
- Korzystać z założeń konkurencji opisanych w regulaminie.
- Wykrywać ściany znajdujące się w labiryncie.
- Zapisywać rozkład ścian w pamięci.
- Znajdować drogę do celu dla danego rozkładu ścian.
Za wykrywanie ścian odpowiadają czujniki, o których pisałem już w dwóch wcześniejszych wpisach.
Czujnik ścian odpowiada za samo wykrycie ściany, jednak jego pomiary mogą być zakłócone. Dlatego zanim wykryta ściana zostanie dodana do mapy, jej obecność musi być potwierdzona w kolejnych pomiarach. Istnieją różne sposoby podejścia do problemu np. próg ilości wykryć, czy wykorzystanie probabilistyki. Tym zagadnieniem nie będę się dzisiaj zajmował, wrócę do niego w najbliższym czasie.
Moduł opisany w poprzednim akapicie, kiedy już ma pewność wykrycia ściany, przekazuje tą informację do modułu zarządzającego mapą, żeby zapisać ją w pamięci. Moduł mapy labiryntu powinien przechowywać informacje o wszystkich komórkach, które potem mogą zostać wykorzystane przez moduł wyznaczania trasy, albo moduł określający aktualne położenie robota w labiryncie.
Informacje o rozmieszczeniu ścian służą modułowi wyznaczania trasy. Moduł ten musi potrafić nie tylko wyznaczyć trasę znając wszystkie ściany w labiryncie. Powinien też odpowiadać za planowanie trasy w fazie eksploracji, gdzie robot nie zna położenia wszystkich ścian i dopiero odkrywa optymalną ścieżkę. W dalszej części artykułu opiszę właśnie moje podejście do tworzenia mapy i wyznaczania trasy.
Mapa labiryntu
Mapa labiryntu powinna korzystać z informacji przedstawionych w regulaminie zawodów Micromouse. Regulaminy często różnią się pomiędzy zawodami, ale ogólne założenia zwykle pozostają takie same:
- Labirynt to kwadrat 16 x 16 pól o znanych wymiarach (czasem stosuje się mniejszy labirynt 9×9, który można rozwiązać bez zmiany programu robota).
- Labirynt z zewnątrz cały jest otoczony ścianą.
- Pole startowe znajduje się w rogu i jest otoczone ścianami z trzech stron.
- Miejsce docelowe znajduje się na 4 środkowych polach tworzących kwadrat nie przedzielony żadnymi ścianami, do którego wejście jest tylko z jednej strony.
Zanim zaczniemy implementację, warto zastanowić się jeszcze nad orientacją mapy. Które pole otrzymuje indeks 0? W którą stronę numerujemy dalsze pola? Ja przyjąłem, że pole startowe ma indeks 0 i znajduje się w lewym górnym rogu. Jedyne otwarte przejście prowadzi w prawą stronę do pola o indeksie 1. Kolejne indeksy rosną dalej w prawą stronę, a po dojściu do końca w rzędzie niżej znowu od lewej.
Dla takiej orientacji labiryntu ściany w każdej komórce nazwałem kolejno zgodnie z ruchem zegara top, right, bottom, left. Później wyczytałem, że ogólnie przyjęta konwencja to kierunki geograficzne: N E S W (North, East, South, West). Możliwe, że w przyszłości zmienię nazewnictwo.
Reprezentacja mapy w pamięci
Logicznym rozwiązaniem jest przechowywanie mapy w tablicy o rozmiarze równym ilości pól (16 x 16 = 256). Najpopularniejszym sposobem reprezentacji mapy jest tablica, gdzie każde pole jest reprezentowane przez jeden bajt. Maski bitowe określają zajętość poszczególnych ścian. W ten sposób najefektywniej wykorzystujemy pamięć.
#define NORTH 0x01 #define EAST 0x02 #define SOUTH 0x04 #define WEST 0x08 static uint8_t labirynth_map[MAP_SIZE];
Ja zdecydowałem się jednak na inne rozwiązanie. Każde pole jest reprezentowane przez strukturę, a każda ściana jest polem tej struktury.
/** Type for wall presence state. */
typedef int32_t map_wall_state_t;
/** State of a single cell in labirynth map. */
struct map_cell
{
map_wall_state_t left; /**< State of the left wall. */
map_wall_state_t right; /**< State of the right wall. */
map_wall_state_t top; /**< State of the top wall. */
map_wall_state_t bottom; /**< State of the bottom wall. */
};
/** Labirynth map containing all cells. */
static struct map_cell labirynth_map[MAP_SIZE];
Na tym etapie jeszcze nie jestem w stanie stwierdzić, czy będę przechowywał jakieś informacje o ścianie poza prostą informacją wolny/zajęty. Może to być na przykład prawdopodobieństwo zajętości. Wybrana przeze mnie implementacja jest łatwiejsza do późniejszej edycji. Tym bardziej, że RAMu mi nie brakuje. Nie ma sensu więc przedwcześnie optymalizować.
Takie rozwiązanie ma jeszcze jedną zaletę. Zmienna typu map_wall_state_t może przyjmować więcej wartości niż dwie. Mogę więc wyróżnić więcej stanów:
enum
{
MAP_WALL_UNKNOWN = 0,
MAP_WALL_PRESENT = 1,
MAP_WALL_ABSENT = 2,
MAP_WALL_ERROR = 0xFF,
};
Początkowo wszystkie ściany mają stan UNKNOWN, a dopiero po eksploracji zmieniam stan na PRESENT, albo ABSENT. Na jednym bicie nieznane komórki musiałbym traktować jako brak ściany. Można pozostałe 4 bity wykorzystać na określenie zajętości, ale ściana może być widoczna z dwóch stron, co dodatkowo komplikuje implementację.
Implementacja mapy
Moduł mapy napisałem korzystając z TDD. Implementacja wyszła zaskakująco prosta, zajęła mi też dużo mniej czasu niż się spodziewałem. Moduł mapy posiada następujące funkcje API:
void map_init(void); map_wall_state_t map_wall_top_get(int32_t cell_id); void map_add_top_no_wall(int32_t cell_id); void map_add_top_wall(int32_t cell_id);
Dolne 3 funkcje mają swoje wersje również dla trzech pozostałych kierunków. Funkcja map_init wypełnia mapę domyślnymi wartościami. Wszystkie ściany mają wartość UNKNOWN poza ścianami określonymi przez regulamin. Czyli zewnętrzny obrys labiryntu posiada ściany oraz pole startowe jest ogrodzone ścianami z trzech stron. Kwadrat z czterech środkowych pól labiryntu nie jest odgrodzony żadnymi ścianami.
Funkcje map_wall_xxx_get zwracają stan ściany (UNKNOWN, PRESENT, ABSENT). Z kolei funkcje map_add_xxx_wall / _no_wall zmieniają stan ściany odpowiednio na PRESENT i ABSENT. Zmiany stanu można dokonać tylko, jeśli aktualny stan to UNKNOWN. Podczas zawodów rozkład ścian nie będzie zmieniał się dynamicznie. Dodając ścianę należy pamiętać, że jest ona widoczna z dwóch komórek. Ściana prawa dla komórki o indeksie 0 jest jednocześnie ścianą lewą dla komórki o indeksie 1.
Mazetool
Przed zaimplementowaniem algorytmu wyznaczania ścieżek musiałem zastanowić się nad sposobem sprawdzania jego poprawności. Trafiłem na GitHubie na projekt micromouse_maze_tool autorstwa Petera Harrisona, twórcy strony micromouseonline zawierającej masę praktycznych informacji dotyczących tworzenia robotów Micromouse.
Zawiera on sporą bazę labiryntów wykorzystywanych w zawodach. Każdy labirynt jest zapisany w trzech formatach: binarnym, tekstowym i tablicy w C. Wykorzystałem tą bazę i napisałem parser labiryntów z mazetoola, który przenosi układ ścian do mojego modułu map. W celu walidacji napisałem również funkcje drukujące labirynt w obu formach, żeby móc sprawdzić, czy są takie same. Output wygląda tak:
Maze layout from file: o---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---o | | o---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo o o---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo o | || || | o oo---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo oo o o oo---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo oo o | || || || || || || || | o oo oo---oo---oo oo oo oo oo oo---oo---oo---oo oo oo oo o o oo oo---oo---oo oo oo oo oo oo---oo---oo---oo oo oo oo o | || || || || || || || || || | o oo oo oo oo---oo---oo---oo oo---oo---oo oo---oo oo oo oo o o oo oo oo oo---oo---oo---oo oo---oo---oo oo---oo oo oo oo o | || || || || || || || || | o oo oo oo---oo---oo oo oo oo oo---oo---oo oo---oo---oo oo o o oo oo oo---oo---oo oo oo oo oo---oo---oo oo---oo---oo oo o | || || || || || || || || || | o oo oo---oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o o oo oo---oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o | || || || || || || || || | o oo oo oo---oo---oo---oo oo---oo---oo oo oo---oo oo---oo oo o o oo oo oo---oo---oo---oo oo---oo---oo oo oo---oo oo---oo oo o | || || || || || || || || || || || | o oo oo oo oo oo oo oo oo oo oo oo oo---oo oo oo o o oo oo oo oo oo oo oo oo oo oo oo oo---oo oo oo o | || || || || || || || || || | o oo oo---oo---oo---oo oo---oo---oo---oo---oo---oo---oo oo oo oo o o oo oo---oo---oo---oo oo---oo---oo---oo---oo---oo---oo oo oo oo o | || || || || || || || || | o oo oo oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o o oo oo oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o | || || || || || || || || | o oo oo oo---oo oo---oo---oo oo oo---oo---oo---oo---oo---oo oo o o oo oo oo---oo oo---oo---oo oo oo---oo---oo---oo---oo---oo oo o | || || || || || || || || || | o oo oo---oo oo---oo oo---oo---oo oo oo oo oo oo oo oo o o oo oo---oo oo---oo oo---oo---oo oo oo oo oo oo oo oo o | || || || || || || || || || | o oo oo oo---oo oo---oo oo---oo---oo---oo---oo---oo---oo oo oo o o oo oo oo---oo oo---oo oo---oo---oo---oo---oo---oo---oo oo oo o | || || || || | o oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o o oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o | || || | o oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o o oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o | | o---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---o Maze map: o---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---o | | o---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo o o---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo o | || || | o oo---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo oo o o oo---oo---oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo oo o | || || || || || || || | o oo oo---oo---oo oo oo oo oo oo---oo---oo---oo oo oo oo o o oo oo---oo---oo oo oo oo oo oo---oo---oo---oo oo oo oo o | || || || || || || || || || | o oo oo oo oo---oo---oo---oo oo---oo---oo oo---oo oo oo oo o o oo oo oo oo---oo---oo---oo oo---oo---oo oo---oo oo oo oo o | || || || || || || || || | o oo oo oo---oo---oo oo oo oo oo---oo---oo oo---oo---oo oo o o oo oo oo---oo---oo oo oo oo oo---oo---oo oo---oo---oo oo o | || || || || || || || || || | o oo oo---oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o o oo oo---oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o | || || || || || || || || | o oo oo oo---oo---oo---oo oo---oo---oo oo oo---oo oo---oo oo o o oo oo oo---oo---oo---oo oo---oo---oo oo oo---oo oo---oo oo o | || || || || || || || || || || || | o oo oo oo oo oo oo oo oo oo oo oo oo---oo oo oo o o oo oo oo oo oo oo oo oo oo oo oo oo---oo oo oo o | || || || || || || || || || | o oo oo---oo---oo---oo oo---oo---oo---oo---oo---oo---oo oo oo oo o o oo oo---oo---oo---oo oo---oo---oo---oo---oo---oo---oo oo oo oo o | || || || || || || || || | o oo oo oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o o oo oo oo oo---oo---oo oo oo oo oo---oo---oo---oo oo oo o | || || || || || || || || | o oo oo oo---oo oo---oo---oo oo oo---oo---oo---oo---oo---oo oo o o oo oo oo---oo oo---oo---oo oo oo---oo---oo---oo---oo---oo oo o | || || || || || || || || || | o oo oo---oo oo---oo oo---oo---oo oo oo oo oo oo oo oo o o oo oo---oo oo---oo oo---oo---oo oo oo oo oo oo oo oo o | || || || || || || || || || | o oo oo oo---oo oo---oo oo---oo---oo---oo---oo---oo---oo oo oo o o oo oo oo---oo oo---oo oo---oo---oo---oo---oo---oo---oo oo oo o | || || || || | o oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o o oo oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o | || || | o oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o o oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo oo o | | o---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---oo---o
Każda komórka składa się z trzech linii po 5 znaków. W rogach znaki o symbolizują słupki, a kreski oznaczają ściany. Każda ściana poza brzegowymi jest drukowana podwójnie – po razie dla każdej komórki. Dzięki temu łatwiej zauważyć przekłamanie danych.
Początkowo projekt mi się nie kompilował, ponieważ niektóre nazwy labiryntów zaczynają się od liczb i nie są poprawnymi nazwami zmiennych w C. Postanowiłem więc takie labirynty po prostu usunąć. Nawet bez nich baza jest wystarczająco duża.
Algorytm wyznaczania trasy
Mając zestaw danych testowych mogłem przystąpić do implementacji algorytmu wyznaczania ścieżek. Mój wybór padł na popularny w micromouse algorytm Floodfill zwany potocznie laniem wody. Jest to uproszczona wersja algorytmu Bellmana-Forda. Największą zaletą jest jego prostota. Działanie algorytmu można zobrazować jako wylewanie wody na dane pole w labiryncie i patrzenie, jak zalewa ona kolejne komórki docierając w końcu do miejsca docelowego. Inna nazwa tego algorytmu to propagacja fali.
Co to oznacza w praktyce? Pole startowe algorytmu otrzymuje wagę 0. Wszystkie pola z nim graniczące, które nie są oddzielone ścianą, otrzymują wagę 1. Pola graniczące z tym o wadze 1 otrzymają wagę 2 itd. Jeżeli docieramy do pola, które już wcześniej otrzymało niższą wagę, nie zmieniamy jej. Kończymy po dotarciu do pola docelowego albo po wypełnieniu całego labiryntu. Drogę z pola docelowego do startowego wyznaczamy idąc zawsze na sąsiadujące pole o najniższej wadze. Najlepiej jako pole startowe przyjąć środek labiryntu, a aktualną pozycję robota jako pole docelowe. Dzięki temu od razu po wypełnieniu pól wagami otrzymujemy drogę, nie musimy jej dodatkowo wyznaczać.
Okazuje się, że najkrótsza ścieżka nie zawsze musi być najszybsza. Czasem opłaca się wybrać dłuższą, ale mniej krętą drogę. Robot przebędzie ją szybciej, bo na prostej może osiągnąć wyższą prędkość. W tym celu konieczna jest modyfikacja algorytmu. Aby to osiągnąć należy ustalić inne wagi za skręt i za jazdę prosto. Możemy również premiować dodatkowo kolejne pola w linii prostej symulując przyspieszanie robota. Można więc powiedzieć, że wagi wtedy reprezentują nie odległość, a czas. Konkretne wartości wag najlepiej wyznaczyć eksperymentalnie znając osiągi swojego robota. Inną możliwą modyfikacją jest dostosowanie algorytmu do jazdy na skos.
Dobrym wprowadzeniem do algorytmu floodfill jest artykuł na Forbocie:
Wikipedia mówi, że Floodfill jest wykorzystywany w programach graficznych do wypełniania obszarów kolorem, ale nie jest zbyt efektywny. W micromouse mamy do czynienia jedynie z obszarem 16 x 16 i algorytm wykonuje się wystarczająco szybko.
Implementacja floodfilla również okazała się prostsza niż zakładałem. Tym razem nie korzystałem z TDD, ponieważ algorytm wykonuję w całości w jednej funkcji i z zewnątrz jest dostęp tylko do wyniku. Ciężko tu sprawdzać jakieś kroki pośrednie, a testy by bardzo mocno wchodziły w szczegóły implementacji i łamały enkapsulację. Testowanie rozwiązania pozostawia wiele do życzenia – po prostu oglądam wydrukowane na konsolę rozwiązanie labiryntu. Dla labiryntu z mazetoola, dla którego output z konsoli przedstawiałem wyżej, wagi wyznaczone dla każdego pola wyglądają następująco:
Floodfill values: 107 106 105 104 103 102 101 100 99 98 97 96 95 94 93 92 104 105 106 71 70 69 68 67 66 65 64 63 62 61 62 91 103 74 73 72 11 12 15 16 51 52 53 54 55 60 63 90 102 73 10 9 10 13 14 17 50 49 48 57 56 59 64 89 101 72 11 8 7 6 5 18 19 20 47 46 57 58 65 88 100 71 12 13 8 7 4 19 20 21 22 45 44 43 66 87 99 70 15 14 15 4 3 20 21 22 23 24 41 42 67 86 98 69 16 19 20 23 2 0 0 23 24 25 40 39 68 85 97 68 17 18 21 22 1 0 0 24 25 26 27 38 69 84 96 67 54 53 24 23 24 27 28 31 30 29 28 37 70 83 95 66 55 52 51 26 25 26 29 32 33 34 35 36 71 82 94 65 56 57 50 49 28 27 28 31 32 33 36 37 72 81 93 64 63 58 59 48 47 30 29 30 31 34 35 38 73 80 92 63 62 61 60 61 46 45 44 43 42 41 40 39 74 79 91 64 65 66 67 68 69 70 71 72 73 74 75 76 75 78 90 89 88 87 86 85 84 83 82 81 80 79 78 77 76 77
Nie dodałem drukowania wag razem ze ścianami, na pewno zwiększyłoby to czytelność.
Podsumowanie
Mój robot potrafi już realizować swoje najważniejsze zadanie – wyznaczać drogę w labiryncie. Realizacja tego zadania okazała się nadspodziewanie prosta. Oczywiście nie jest to ostateczna wersja modułów mapowania i wyznaczania trasy. Muszę jeszcze dodać zasady określania, że ściana wystąpiła, a algorytm Floodfill mogę ulepszyć o minimalizację ilości zakrętów i jazdę na skos. Kolejnym ciekawym zagadnieniem, które już zacząłem zgłębiać jest wykorzystanie mapy labiryntu do korekcji pozycji przez filtr Kalmana.
16 kwietnia 2018 at 13:20
Świetny artykuł. Na jakiej zasadzie zrobiłeś mapowania labiryntu? Myślałem o zastosowaniu reguły prawej ręki ale ona sprawdza się tylko dla prostych labiryntów.
16 kwietnia 2018 at 17:49
Wykorzystałem algorytm floodfill. Polega on na tym, że pole startowe ma wagę 0, wszystkie z nim sąsiadujące 1 itd aż do celu. Mając taką mapę z wagami poruszasz się zawsze na pole o najniższej wadze. Więcej możesz poczytać tutaj:
https://forbot.pl/blog/roboty-micromouse-5-metod-przeszukiwania-labiryntu-id17354