Rozdział 8: SLAM i Mapowanie (LiDAR, Budowa Map, Algorytmy ICP)

Część 1: Przyjazna Encyklopedia (Dla Każdego)

Witamy w świecie profesjonalnej robotyki! Jeśli do tej pory jeździłeś autem "na ślepo" (tylko z kamerą FPV), przygotuj się na rewolucję. Gdy Twój model posiada na dachu LiDAR (skaner laserowy), przestaje polegać wyłącznie na Twoim wzroku. Zaczyna rozumieć przestrzeń, przeszkody i swoją dokładną pozycję z dokładnością do kilku centymetrów.

Dzięki technologii SLAM (Simultaneous Localization and Mapping), auto buduje mapę nieznanego otoczenia i jednocześnie potrafi odnaleźć się wewnątrz niej.

📡 1. LiDAR – Laserowy Wzrok Pojazdu

LiDAR to obracający się cylinder, który wysyła tysiące wiązek światła laserowego w ułamku sekundy. Odbijają się one od otoczenia i wracają do sensora, tworząc tzw. "Chmurę Punktów" (Point Cloud).

• Zalety: W przeciwieństwie do kamery, LiDAR działa w całkowitej ciemności! Potrafi też dostrzec obiekty, z którymi sztuczna inteligencja wizyjna ma problem (np. cienkie, szklane nogi od stołu, siatki ogrodzeniowe).

• Diagnostyka (Radar LiDAR): W GCS, w zakładce radaru, widzisz otoczenie jako okrąg pełen punktów. Jeśli zauważysz w nim "czarne dziury" lub czerwone sekcje, oznacza to dwie rzeczy: albo coś fizycznie zasłania sensor (np. przewód zasilający), albo laser padł na obiekt idealnie matowy i czarny (taki materiał pochłania światło i nie "odwija" go do sensora).

🗺️ 2. Procedura Budowania Mapy (Mapping)

Tworzenie mapy Twojego domu lub toru wyścigowego jest bardzo proste, ale wymaga cierpliwości.

• W zakładce Nawigacja uruchom mapowanie przyciskiem startu SLAM; GCS wywołuje SlamService.start_mapping() z konfiguracją monaco_slam.

• Prowadź płynnie: Przejedź całą trasę z niewielką, stałą prędkością. GCS zacznie łączyć ze sobą miliony skanów lasera w jedną, spójną mapę 2D z lotu ptaka.

• Złota zasada (Zamknięcie Pętli - Loop Closure): Kiedy zwiedzasz dom lub jedziesz po torze, wróć dokładnie do punktu startu! Kiedy LiDAR rozpozna miejsce, w którym zaczął mapowanie, uruchomi mechanizm Loop Closure. Zobaczysz, jak cała mapa nagle minimalnie się przesuwa i "wskakuje" idealnie na swoje miejsce, niwelując wszystkie drobne błędy, które narosły podczas jazdy.

[!WARNING] Krytyczna Uwaga: POŚLIZG KÓŁ NISZCZY MAPĘ! System SLAM w dużej mierze opiera się na liczeniu obrotów kół (odometrii). Jeśli podczas mapowania gwałtownie dodasz gazu i zrobisz tzw. "bączka" w miejscu, system zgłupieje. Pomyśli, że przejechałeś dystans (bo koła się kręciły), podczas gdy LiDAR pokaże, że stoisz w miejscu. To najszybsza droga do "rozjechania się" mapy i konieczności zaczynania od nowa. Unikaj agresywnego gazu w trakcie mapowania!

⚙️ 3. Optymalizacja Mapy (Siatka i Grube Ściany)

W zakładce Nawigacja ustawienia są podzielone na podzakładki: Ustawienia LiDAR, Ustawienia silnika SLAM, Ustawienia PWF, Ustawienia FTG, Ustawienia PP, Ustawienia AI Assist i Ustawienia Full AI.

• Resolution (Rozdzielczość): Standardowo to 0.05 m (5 centymetrów kwadratowych na piksel mapy). Zmniejszenie tej wartości da przepiękną, ostrą mapę, ale wymaga potężnej mocy obliczeniowej na Raspberry Pi.

• Inflation Layer (Dylatacja / Gruby Mur): To wirtualne "pogrubienie" ścian o kilkadziesiąt centymetrów. Dodanie tej warstwy to gwarancja, że algorytmy planujące trasę poprowadzą auto środkiem drogi, w bezpiecznej odległości od krawężników, chroniąc Twoje drogie zawieszenie.

• Apex Planner (Dla Ścigantów): moduły PlanningEngine, PathOptimizer i RacingEngine potrafią wygenerować oraz wygładzić linię przejazdu na podstawie mapy i inflacji ścian. Funkcja jest częścią potoku planowania SLAM; korzystaj z niej dopiero po stabilnym zbudowaniu mapy.

Część 2: Inżynieryjny Deep Dive (Dla Specjalistów)

Pod warstwą wizualnej mapy 2D kryje się potężny aparat matematyczny łączący probabilistykę, algebrę liniową i optymalizację grafową. Oto jak RCSIM przetwarza świat zewnętrzny na strukturę w pamięci operacyjnej robota.

1. Fuzja Danych i Skanowanie (Algorytm ICP)

Rdzeniem lokalizacji ciągłej na pokładzie Raspberry Pi jest algorytm ICP (Iterative Closest Point), który dopasowuje najnowszą chmurę punktów ($P$) do chmury referencyjnej z poprzedniego kroku ($Q$).

Aby uniknąć zapadnięcia się algorytmu w minimum lokalne, system wykorzystuje odometrię kinematyczną (pochodzącą z enkoderów kół / komend PWM przepustnicy i skrętu) jako weryfikację wstępną (Initial Guess) dla translacji i rotacji. Następnie ICP iteracyjnie minimalizuje funkcję kosztu (błąd średniokwadratowy):

$$E(R, t) = \sum_{i=1}^{N_{match}} || R \cdot p_i + t - q_i ||^2$$

Gdzie $R$ to macierz rotacji, $t$ to wektor translacji układu, a $(p_i, q_i)$ to pary najbliższych sąsiadów wyznaczone za pomocą drzewa KD (KD-Tree).

Odporność (Catastrophic Drift Guard): W wysoce dynamicznym środowisku (np. symulator Monaco z lagami sieci) ICP może zbiec do błędnego rozwiązania. GCS bezustannie weryfikuje wynik dopasowania. Jeśli transformacja przestrzenna $(R, t)$ zgłosi nienaturalny skok pozycji (np. $>50\text{ m}$ między ramkami), system przerywa aktualizację. Zamiast budować fałszywą mapę, wymuszony zostaje Graph Reset lub mechanizm Motion Recovery (gt_hint), który re-inicjuje pozycję robota ze znanego Ground Truth.

2. Reprezentacja Przestrzeni: Probabilistyczna Mapa Zajętości (Occupancy Grid)

Siatka zajętości nie przechowuje prostych wartości logicznych (Ściana/Pusto). Ze względu na szum sensora LiDAR, każda komórka mapy przechowuje prawdopodobieństwo obecności przeszkody w ujęciu ułamkowym, reprezentowanym za pomocą log-szans (Log-odds):

$$l(x) = \log\left(\frac{P(x)}{1 - P(x)}\right)$$

Z każdą nową wiązką LiDARu $z_t$, przecinającą komórkę $x$, stan mapy jest aktualizowany za pomocą filtra rekursywnego (Bayesian Update):

$$l_{t+1}(x) = l_t(x) + \text{inv_sensor_model}(x, z_t) - l_0$$

Dzięki temu sporadyczny błąd odbicia lasera (np. promień światła słonecznego) nie generuje trwałej "cegły" na środku drogi. Komórka musi zostać "potwierdzona" przez kilkanaście następujących po sobie skanów, aby wyjść z szarej strefy (stan nieznany) i zmienić się w trwałą przeszkodę.

3. Loop Closure i Graph Optimization (Grafy Poz)

Podczas eksploracji, błąd estymacji ICP i odometrii powoli narasta (Drift). Gdy pojazd powraca do znanej lokalizacji, system wykrywa tzw. "Zamknięcie Pętli" (Loop Closure). Wtedy RCSIM przestaje być prostym filtrem przestrzennym, a staje się Optymalizatorem Grafowym (Pose Graph Optimization).

Poszczególne lokalizacje w czasie (węzły) są połączone krawędziami reprezentującymi pomiary odometryczne i skany LiDAR. Gdy pojazd zamyka pętlę, dodawana jest nowa krawędź między węzłem obecnym a węzłem historycznym. Występuje nagłe przesztywnienie grafu. Algorytm optymalizacji nieliniowej (np. Gauss-Newton lub Levenberg-Marquardt) rozwiązuje globalny układ równań i błyskawicznie "rozprowadza" skumulowany błąd równomiernie wzdłuż całej trajektorii. Właśnie to zjawisko obserwujesz, gdy mapa gwałtownie "wskakuje" na swoje miejsce w interfejsie GCS!

4. Apex Planner: Trajektorie B-Spline dla Linii Wyścigowej

Gdy mapa jest domknięta i użyta do nawigacji, standardowy algorytm A* wyznacza trasę w przestrzeni konfiguracyjnej rozszerzonej o warstwę dystansu (Inflation Layer opartą o transformatę Minkowski Sum). Efektem jest ścieżka biegnąca dokładnie środkiem szerokości toru.

Dla modułu Monaco Racing, zaimplementowano heurystykę Apex Plannera. Bierze ona wyjściową ścieżkę A* (składającą się z odcinków prostych) i optymalizuje ją w celu zminimalizowania czasu przejazdu.

System wykorzystuje aproksymację krzywymi B-Spline stopnia $k=3$. Funkcja krzywej $S(t)$ jest definiowana przez wektor węzłów kontrolnych $P_i$:

$$S(t) = \sum_{i=0}^{n} P_i \cdot N_{i,k}(t)$$

Algorytm optymalizacyjny iteracyjnie przesuwa punkty kontrolne $P_i$ w kierunku wewnętrznej strony zakrętu (Apex), jednocześnie wyliczając karę (Penalty Function), jeśli krzywa zbliża się zbyt mocno do warstwy Inflation Layer. Rezultatem jest idealnie zaokrąglona trajektoria (Racing Line), pozwalająca pojazdowi na utrzymanie drastycznie wyższej prędkości dośrodkowej w zakręcie bez utraty stabilności.