W dzisiejszym wpisie prześledzimy całą drogę od pomiaru odległości przez czujnik ściany do ostatecznego dodania tej ściany do mapy labiryntu w pamięci robota. Na tej drodze znajduje się wiele akcji pośrednich takich, jak określenie współrzędnych wykrytej ściany, czy potwierdzenie wykrycia w kilku pomiarach.

Architektura rozwiązania

Elementy biorące udział w dodawaniu informacji o ścianach do mapy zostały przedstawione na poniższym rysunku:

Całość może wydawać się nadmiernie skomplikowana przez mnogość modułów. Tak naprawdę jednak skomplikowanie projektu na papierze przekłada się na uproszczenie implementacji. Dzięki dodaniu odpowiedniej ilości modułów pośrednich każdy z nich jest odpowiedzialny za jedno konkretne zadanie przez co logika w kodzie jest przejrzysta. Przyznam się, że nie planowałem z góry dokładnie takiej architektury. Miałem pewne ogólne wyobrażenie jak to powinno wyglądać, a reszta jest efektem zastosowania TDD.

Wall detection

Moduł wall_detection przede wszystkim określa, czy czujnik ściany w ostatnim pomiarze wykrył ścianę. W tym celu porównuje otrzymaną odległość z progiem detekcji aktualnie ustawionym na 130 mm. Drugą ważną rzeczą jaką wykonuje jest określenie współrzędnych sensora, który wykonał pomiar i znalezionego punktu na ścianie.

W tym celu potrzebna jest informacja o aktualnym położeniu robota z modułu estymacji pozycji. Otrzymany punkt to geometryczny środek robota. Każdy czujnik względem pozycji robota jest przesunięty o pewien offset i obrócony o pewien kąt. Pozycję i orientację czujnika obliczamy przez dodanie offsetu czujnika do pozycji robota.

Punkt wykrycia ściany to punkt oddalony od sensora o zmierzona odległość pod kątem, w który zwrócony jest sensor. Punkt wykrycia ściany jest obliczany za pomocą funkcji trygonometrycznych. Skorzystałem tutaj z wbudowanego w procesor STM32F4 moduł FPU przyspieszający operacje zmiennoprzecinkowe na liczbach pojedynczej precyzji (float). Wykorzystałem funkcje sinf i cosf. Operacje zmiennoprzecinkowe na tym mikrokontrolerze to temat na osobny wpis, który planuje opublikować w najbliższym czasie.

Obliczyć punkt wykrycia ściany możemy oczywiście jedynie gdy ściana została wykryta. A co w przeciwnym wypadku? Po prostu obliczamy pozycję najdalszego widzianego punktu nie przesłoniętego przez żadną ścianę. Taka informacja też jest wartościowa i wykorzystamy ją później do stwierdzenia braku ściany. Kod obliczający współrzędne punktu wykrycia ściany wygląda mniej więcej tak:

    if (NO_WALL_THRESHOLD < sensor_data)
    {
        sensor_after_threshold = (float)NO_WALL_THRESHOLD;
        wall_state = MAP_WALL_ABSENT;
    }
    else
    {
        sensor_after_threshold = (float)sensor_data;
        wall_state = MAP_WALL_PRESENT;
    }

    wall_pos->x = sensor_pos->x + sensor_after_threshold * cosf(sensor_pos->alpha);
    wall_pos->y = sensor_pos->y + sensor_after_threshold * sinf(sensor_pos->alpha);
    wall_pos->alpha = 0;

Map update

Regulamin konkurencji micromouse precyzuje, że ściany mogą znajdować się jedynie na bokach komórek. Grubość ściany wynosi 12 mm. Pojedyncza ściana jest widoczna z dwóch stron, dlatego zajmuje ona 6 mm z każdej strony. Dla każdej komórki nałożonej na układ współrzędnych możemy więc wyznaczyć obszary, gdzie spodziewamy się wykryć ścianę. Przedstawiłem je na poniższym rysunku.

Na żółto zaznaczone są progi wykrycia każdej ze ścian. Aktualnie ustawiłem je z pewnym zapasem na 20 mm. Ostateczne wartości prawdopodobnie wybiorę po testach polowych. Zaznaczyłem również na czerwono narożniki. Wykrycia z tamtych rejonów nie będą przeze mnie uwzględniane, ponieważ istnieje ryzyko wykrycia nie tej ściany którą chcemy. Aktualnie próg ustawiłem na 30 mm, prawdpodobnie docelowo będzie mniejszy.

Mając te obszary sprawdzenie poprawności wykrycia ściany jest proste. Wystarczy jedynie sprawdzić, czy punkt wykrycia leży w obszarze. Sytuacja jest nieco trudniejsza przy wykrywaniu braku ściany. Musimy wtedy mieć czujnik wewnątrz komórki przed obszarem i punkt braku wykrycia za obszarem, aby stwierdzić, że nie ma ściany pomiędzy nimi. Wykorzystuje tutaj te same progi. Sprawdzanie obszarów jest zaimplementowane na ifach:

static bool is_wall_left_present(int32_t cell_x, int32_t cell_y, struct coords *wall_pos)
{
    float l_x_min = wall_front_min(cell_x);
    float l_x_max = wall_front_max(cell_x);
    float l_y_min = wall_side_min(cell_y);
    float l_y_max = wall_side_max(cell_y);

    if ((l_x_min < wall_pos->x) && (l_x_max > wall_pos->x))
    {
        if ((l_y_min < wall_pos->y) && (l_y_max > wall_pos->y))
        {
            return true;
        }
    }

    return false;
}

static bool is_wall_left_absent(int32_t cell_x, int32_t cell_y, struct coords *sensor_pos, struct coords *wall_pos)
{
    float l_x_min = wall_front_min(cell_x);
    float l_x_max = wall_front_max(cell_x);
    float l_y_min = wall_side_min(cell_y);
    float l_y_max = wall_side_max(cell_y);

    if ((l_x_min > wall_pos->x) && (l_x_max < sensor_pos->x))
    {
        if ((l_y_min < wall_pos->y) && (l_y_min < sensor_pos->y))
        {
            if ((l_y_max > wall_pos->y) && (l_y_max > sensor_pos->y))
            {
                return true;
            }
        }
    }

    return false;
}

Map validate

Po potwierdzeniu wykrycia przechodzimy do walidacji. Każda ściana posiada swoje liczniki wykryć obecności i braku obecności. Po poprawnej detekcji ten licznik jest zwiększany i jeśli jego wartość przekracza próg – ściana jest dodawana do mapy na stałe. Jeżeli natomiast stan obu liczników dla tej samej ściany będzie większy od zera – oba są zerowane. Dzięki temu nie dodamy żadnej ściany dopóki nie mamy co do niej pewności.

Implementacja tablicy komórek

Poszczególne moduły mają swoje własne tablice komórek. Myślałem początkowo o jednej strukturze danych, do której odwołują się wszystkie moduły. Jednak dzięki rozdzieleniu tablic możliwe jest uproszczenie implementacji i pewne optymalizacje. Na przykład struktury z wartościami dla pojedynczej komórki, czy ściany możemy przekazać do funkcji, albo w solverze możemy zastosować podwójne buforowanie. Przechowywanie jednej tablicy jest praktyczne z punktu widzenia debugu i testowania. Jako zastępstwo możliwe więc, że zrobię odpowiednie funkcje loggera printujące zagregowane dane o komórce.

Podsumowanie

Zastosowane przeze mnie rozwiązanie jest dosyć rozbudowane. Zwykle w micromouse wykorzystuje się proste sprawdzenie wartości z czujnika i ewentualnie próg ilości wykryć. Nie są robione żadne obliczenia na pozycji robota i sensorów. Takie rozwiązanie działa zakładając, że błędy pozycjonowania nie są duże. Ja natomiast chciałem podejść do problemu bardziej naukowo. Zobaczymy podczas testów w labiryncie, czy to się opłaci.