– Wprowadzenie “Model pojazdu do badań nad autonomią”

– Opis konstrukcji “Etapy budowy platformy modelu”

– Modelowanie “Aktualizacja położenia w oparciu o model i metoda wyznaczania trajektorii ruchu”

– Pierwsze przejazdy “Surowe wyniki i wyznaczanie trajektorii w postprocesingu”

– Jak rośnie błąd trajektorii? Ackermann‑predict w praktyce czyli Rachunek niepewności i propagacja błędu w modelu ruchu

– Testy dokładności “Pierwsza seria ćwiczeń. Przejazdy po torze modułowym i weryfikacja szacowania pozycji”

– Przejazd z kamerą czyli “co możemy odczytać ze zdjęć”

– Korekta obrazu “Metody tworzenia bird’s eye view (BEV)”

– “Wykrywanie pasów ruchu w BEV (bird’s eye view)”

– Map fitting, co to jest i dlaczego to “nie działa”? - “Kamera jako czujnik położenia”

– Kiedy trajektoria wreszcie może się poprawić? “SLAM offline”

Jeśli mój projekt Cię zaciekawił …

możesz zajrzeć do repozytorium. Umieściłem tam cały kod używany w kolejnych etapach opisywanych na blogu: transformację obrazu z kamery (FEV → BEV), map fitting oraz SLAM offline (post‑processing na logach przejazdów), a także skrypty do synchronizacji logów, ekstrakcji obserwacji i diagnostyki wyników.

Repozytorium znajdziesz tutaj: https://github.com/dr-mako/samochod

W repo jest też krótki README.md, który prowadzi przez strukturę katalogów i opisuje, do czego służą poszczególne skrypty oraz jakie pliki są wejściem/wyjściem w pipeline. Jeśli chcesz odtworzyć eksperymenty krok po kroku, zacznij właśnie od tego pliku.

1) SLAM (Simultaneous Localization And Mapping)

W poprzednim wpisie („map fitting”) celowo „zamroziłem” trajektorię i potraktowałem ją jako prawdę. W efekcie pozwala to zobaczyć, że jeśli odometria dryfuje, to mapa musi się zdeformować, ponieważ algorytm jej nie poprawia. Teraz robię krok dalej. Wchodzimy w SLAM — etap, w którym trajektoria nie jest traktowana jako parametr stały. SLAM (Simultaneous Localization And Mapping) to metoda równoczesnego budowania mapy otoczenia i lokalizowania się na niej na podstawie obserwacji. Klasycznie robi się to „bez mapy wejściowej”: mapa dopiero powstaje. Z drugiej strony możemy ją też stosować do “lokalizacji” na gotowej mapie lub do kalibracji modelu ruchu. Obecny wpis dotyczy wersji pośredniej, ale bardzo praktycznej: SLAM offline, czyli postprocessing na podstawie zapisanych logów przejazdu. Nie ma presji czasu rzeczywistego, jest za to geometria, optymalizacja i możliwość zadania sobie trudnych pytań: co tak naprawdę jest niezgodne — odometria, kamera, a może oba naraz?

2) Problem: ten sam marker w kilku miejscach mapy

Eksperyment jest prosty: na podłodze leżą markery ArUco. Każdy ma unikalne ID, więc asocjacjia danych jest ułatwiona: „marker 7” to zawsze ten sam obiekt w świecie. W każdej klatce obrazu kamera widzi markery, a po przekształceniu do BEV (bird’s-eye view) potrafię odczytać ich położenia w pikselach. To kuszące, żeby traktować BEV jak „metryczną mapkę”, tylko że w praktyce łańcuch przekształceń zawsze ma błędy: trochę w skali, trochę w orientacji, trochę w modelu ruchu. Ta sytuację ilustruje poniższy rys., gdzie pokazano obrazy z kamery nałożone na mapę trajektorii odometrycznej (co odpowiada założeniom „map fitting”) w pobliżu ostrego zakrętu z widocznym markerem nr. 8. Te dwa obrazy były zarejestrowane w odstępie 30 klatek. Widoczne jest istotne przesunięcie pozy markera, chociaż oczekiwałbym, że będą w tym samym miejscu mapy. Co jest tu jeszcze bardzo istotne z punktu widzenia SLAM – pokazany przejazd zawiera dwie pętle. To warunek konieczny aby SLAM mógł prawidłowo uzgodnić położenie trajektorii i markerów na jednej mapie.

W map fittingu mogłem „ratować sytuację” deformując mapę. W SLAM-ie celem jest coś innego: zbudować mapę i trajektorię, które są ze sobą spójne

3) Minimalna teoria

W chwili $k$ pojazd ma pozę \(x_k=(x_k,y_k,\phi_k)\) w globalnym układzie świata (WORLD). Detektor zwraca marker (landmark) o ID $j$ w obrazie BEV jako punkt w pikselach \((x_{\text{pix}},y_{\text{pix}})\) Dalej zachodzi prosty łańcuch przekształceń: piksele $\to$ metry w BEV $\to$ stała korekta o ekstrynsykę kamery/BEV względem pojazdu (przesunięcie i stały offset yaw) $\to$ punkt względny w układzie pojazdu $\to$ projekcja do WORLD z użyciem aktualnej pozy pojazdu. Jeżeli chcę to zapisać jednym zdaniem matematycznym (tylko po to, żeby nie zgubić sensu), to „pozycja w świecie wynikająca z pomiaru” ma postać:

W tym zapisie \(z_{\text{veh},j,k}\in\mathbb{R}^2\) oznacza obserwację markera w układzie pojazdu (po przeliczeniu BEV na metry i po uwzględnieniu ekstrynsyki), a $R(\phi_k)$ jest macierzą obrotu 2D wynikającą z orientacji pojazdu $\phi_k$. Tymczasem $p_j\in\mathbb{R}^2$ to bieżąca estymata mapowej pozycji tego samego markera w WORLD. Sedno SLAM-u jest proste: chcę, żeby to, co wynika z pomiarów, zgadzało się z tym, co jest w mapie. Dlatego błąd dopasowania (residual) definiuję jako:

To jednak wciąż nie wszystko, bo trajektoria też nie jest stała. Odometria narzuca dodatkowe ograniczenie: kolejne pozy pojazdu powinny być zgodne z tym, co zmierzył model ruchu. Tę niezgodność opisuję residualem $r_{\text{odom},k}$ między klatkami $k$ i $k+1$. W efekcie algorytm szuka kompromisu minimalizującego łączny błąd:

gdzie $\mathcal{O}$ jest zbiorem wszystkich zaobserwowanych par „klatka–marker”. W map fittingu mogłem zmieniać tylko $p_j$ (mapę) przy zamrożonej trajektorii. W SLAM-ie zmieniają się jednocześnie $p_j$ oraz $x_k$ (trajektoria) — i to jest ta jedna różnica, która robi całą robotę.

4) Jak to działa intuicyjnie, czyli dwie sprzeczne „prawdy”

Najprościej myśleć o tym jak o kompromisie między dwiema informacjami:

• Odometria mówi: „między klatką $k$ i $k+1$ przesunąłem się mniej więcej tak”.
• Landmarki mówią: „marker o ID=7 jest jeden i stoi w świecie w jednym miejscu, więc jeśli widzę go ponownie, moja trajektoria musi się z tym zgodzić”.

SLAM offline bierze cały log przejazdu i próbuje znaleźć takie pozycje pojazdu w czasie oraz takie położenia markerów, żeby te dwie „prawdy” dało się pogodzić możliwie najlepiej. U mnie dodatkowo używam funkcji odpornej na outliery, żeby pojedyncze złe detekcje nie obciążały wyniku. Aby zapewnić możliwość porównywania wyników różnych metod obliczeniowych między sobą narzucam punkt odniesienia w miejscu pierwszego zarejestrowanego markera. W moim przypadku przejazd zaczynam od markera id=1 więc przyjmuję, że stanowi on początek globalnego „światowego” układu współrzędnych.

5) MEtap kalibracyjny: gdy SLAM uczy się błędów systematycznych

Zanim zacznę traktować wynik jak „lokalizację”, pozwalam SLAM-owi znaleźć również kilka prostych korekt systematycznych. To ważne, bo w praktyce większość problemów nie jest czystym szumem Gaussa, tylko konsekwencją drobnych, ale stałych błędów:

• skala px $\to$ m w BEV może być minimalnie zła,
• odometria może zaniżać/zawyżać narastanie kąta,
• w zakrętach może pojawić się systematyczny dryf boczny (poślizg, nieliniowość modelu skrętu, sprężystość opon itd.).

Na danych z przejazdu optymalizacja znalazła m.in. efektywną skalę około $0.000950$ m/px (około 5% różnicy względem nominalnego $0.001$ m/px) oraz niewielkie korekty parametrów związanych z yaw/krzywizną, plus prosty parametr bocznego „uciekania”. Najważniejszy fakt nie jest jednak w samych liczbach, tylko w konsekwencji: żeby złożyć spójny świat, trajektoria musiała się realnie przesunąć względem odometrii nawet o dziesiątki centymetrów. Tego map fitting nie mógł zrobić z definicji. Po tym etapie moja sytuacja wygląda jak na poniższym rys. Otrzymane parametry korekt zapisuję i traktuję jako stałe.

6) Etap „runtime” offline: biasy zamrożone, pracuje tylko mapa i trajektoria

W drugim kroku uruchamiam SLAM jeszcze raz, ale już bez „uczenia się” powyższych korekt. One są zamrożone, a optymalizacja poprawia tylko: trajektorię w czasie $\leftrightarrow$ mapę markerów. W praktyce wynik nadal wymagał korekty trajektorii względem surowej odometrii (średnio około 0.38 m, maksymalnie około 0.82 m). To jest spójne z intuicją z poprzedniego wpisu: problem najbardziej wychodzi w manewrach wymagających (zakręty), gdzie uproszczony model ruchu zaczyna rozmijać się z fizyką. Moje wyniki ukazują rys. poniżej. Porównanie wyznaczonych pozycji landmarków z ich prawdziwymi (nie znanymi przez algorytm) pozycjami Ground Truth nie wygląda imponująco. W stosunku do GT landmarki mapy wykazują następujące błędy położenia RMSE [m]: 0.355, Mean [m]: 0.317, Max [m]: 0.568.

7) Porównanie z Ground Truth: sztywne wyrównanie nie wystarcza, przekształcenie afiniczne ujawnia prawdę

Ponieważ układ markerów na makiecie mam zmierzony, mogłem porównać mapę uzyskaną ze SLAM z Ground Truth. I tu pojawia się rzecz, która na pierwszy rzut oka wygląda jak porażka, a w praktyce jest świetnym testem diagnostycznym: jakiego typu błędy dominu¬ją w całym pipeline’ie. Najpierw zrobiłem najprostsze możliwe wyrównanie: potraktowałem wynik SLAM jak „sztywny obiekt” i dopasowałem go do GT tylko przez przesunięcie i obrót. Gdyby problem polegał wyłącznie na tym, że SLAM ma inny punkt zerowy albo jest obrócony w inną stronę, to takie wyrównanie powinno niemal zlikwidować różnice. Tymczasem po tym kroku błąd nadal był rzędu dziesiątek centymetrów (RMSE około 0.36 m). To sugeruje, że nie chodzi tylko o wybór układu współrzędnych — w wyniku jest coś, czego nie da się skorygować samym „przesuń i obróć”. Potem dopuściłem wyrównanie afiniczne (rys.). To nadal jest proste dopasowanie globalne, ale daje dodatkową swobodę: poza przesunięciem i obrotem pozwala na inną skalę w osiach oraz lekki „skos” (czyli sytuację, w której osie w praktyce mieszają się ze sobą). Ważne jest jednak, co tak naprawdę robi tu „affine”: to nie jest kolejny etap SLAM-u, tylko czysta procedura dopasowania geometrii. Mamy dwie chmury punktów — landmarki ze SLAM oraz ich pozycje z Ground Truth — i zakładamy, że GT jest znane. Następnie szukamy takiej transformacji afinicznej, która możliwie najlepiej nałoży jedną chmurę na drugą (w sensie minimalizacji średniego błędu). Dopiero po takim dopasowaniu błąd spada do około 6.7 cm RMSE. Jeden marker nadal odstaje wyraźniej (około 15 cm), ale cała struktura układu robi się zaskakująco zgodna.

To prowadzi do mocnego, ale jednocześnie ostrożnego wniosku: „After affine alignment the landmark RMSE equals 6.7 cm, indicating high structural consistency of the SLAM reconstruction. The residual deformation suggests systematic bias in BEV metric scaling and/or vehicle kinematic model.” Po polsku: SLAM zrekonstruował układ markerów spójnie, tylko że ta spójność jest opisana w geometrii, która nie jest idealnie zgodna z „metryką z miarki”. To może oznaczać systematyczne odkształcenie po stronie obserwacji (np. BEV nie jest idealnie metryczny), ale nie byłbym tu kategoryczny. Największa pozostała niezgodność dotyczy markera 8, a ten leży dokładnie tam, gdzie przejazd zawierał ostry zakręt. W takich miejscach uproszczony model Ackermanna najczęściej zaczyna przegrywać z rzeczywistością: promień skrętu nie zgadza się z modelem, pojawia się poślizg boczny, a tor jazdy „płynie”. Tego nie da się w pełni zamknąć jedną stałą korektą. Dlatego na tym etapie najuczciwszy opis brzmi tak: rekonstrukcja SLAM jest strukturalnie spójna, ale resztkowa deformacja wskazuje na obecność błędów systematycznych — prawdopodobnie jednocześnie po stronie geometrii obserwacji (BEV) i po stronie modelu ruchu (szczególnie w ostrych zakrętach).

8) Co to zmienia w interpretacji „czy SLAM działa”?

Na tym etapie najważniejsza zmiana myślenia jest taka: • SLAM nie polega na tym, że „koszt spada i wykres wygląda ładnie”. • SLAM polega na tym, że potrafi ujawnić, gdzie świat przestaje pasować do modelu. U mnie wyszło jasno: SLAM domknął spójność landmarków i skorygował trajektorię, ale porównanie z GT pokazało, że resztka błędu ma charakter geometrycznej deformacji zgodnej z affine. To nie jest „błąd optymalizacji”. To jest informacja o modelu obserwacji (BEV) i/lub o uproszczeniach ruchu.

9) Co dalej: dwa kierunki, które mają sens

Na tym etapie SLAM wymusił spójność między trajektorią a landmarkami i pokazał, gdzie kończą się możliwości „wiary w odometrię”. Jednocześnie porównanie z Ground Truth ujawniło, że resztkowy błąd nie wygląda jak przypadkowy szum, tylko jak błąd systematyczny. I tu pojawia się najważniejsze pytanie na dalszą część projektu: czy dominującym źródłem tej resztki jest geometria obserwacji (metryczność BEV), czy dynamika ruchu (poślizg i odejście od idealnego Ackermanna w ostrych zakrętach), a najpewniej — jaka jest proporcja jednego i drugiego. Dalsza praca naturalnie dzieli się więc na dwa kierunki. Pierwszy to dopracowanie modelu pomiaru: zamiast zakładać, że BEV jest idealnie metryczny, trzeba sprawić, żeby faktycznie taki był (albo jawnie dopuścić w modelu prostą korektę, która kompensuje zniekształcenie). Drugi to dopracowanie modelu ruchu: skoro największy błąd pojawia się w miejscach o dużej krzywiźnie toru, to warto opisać poślizg jako zjawisko zależne od manewru, a nie jako stałą poprawkę. W praktyce można to zrobić bardzo „inżyniersko”: potraktować trajektorię ze SLAM (wyrównaną do GT) jako odniesienie i na jej podstawie wyznaczyć, jak odometria systematycznie myli się w funkcji skrętu i prędkości, a potem tę korektę włączyć z powrotem do odometrii. To jest moment, w którym SLAM przestaje być tylko algorytmem mapowania — staje się narzędziem do tego, żeby z danych wyciągnąć poprawki do modelu, który dotąd był tylko założeniem.

1) Problem

Zaczynam od obrazu przedstawionego na rys. 1, który na pierwszy rzut oka wygląda poprawnie. Widzimy trajektorię pojazdu i dwa fragmenty pasów ruchu przeniesione do wspólnego, globalnego układu odniesienia. Każda klatka kamery została przekształcona do widoku z lotu ptaka (BEV), a następnie “położona” na mapie w oparciu o odometrię. Intuicyjnie wszystko się zgadza: kamera widzi podłogę, znamy pozycję pojazdu, więc wystarczy te obserwacje przesuwać i obracać zgodnie z ruchem. Problem polega na tym, że to nie powinno działać. A jeśli działa — to tylko pozornie.

2) Od obrazu do świata

Kamera w każdej chwili obserwuje fragment rzeczywistości w swoim lokalnym układzie odniesienia. Po przekształceniu do BEV geometria staje się w przybliżeniu metryczna: odległości mają sens, kąty są zachowane, a perspektywa przestaje dominować. To bardzo kuszące — skoro mamy lokalny “plan podłogi”, to wystarczy go umieścić w układzie świata. I dokładnie w tym miejscu zaczynają się kłopoty. Jeżeli składamy kolejne klatki wyłącznie na podstawie surowej odometrii, każda drobna niedokładność modelu ruchu — minimalny błąd kąta, niewielka asymetria skrętu, makro poślizg koła — powoduje, że kolejny fragment pasa odkładany jest w nieco złym miejscu. Te błędy nie znikają. One się sumują. Po przejechaniu pętli mapa nie domyka się, a pas zaczyna “płynąć”. To nie jest błąd obrazu. To jest konsekwencja niespójności transformacji między lokalnym układem kamery a układem świata — wymuszonej przez błędy trajektorii. Rys. 2 przedstawia kilka wybranych klatek obrazów z całego przejazdu złożonych w opisany powyżej sposób. Pasy ruchu rozjeżdzają się - szczególnie w okolicy ostrych zakrętów. Wynik ten pokazuje, że przy użyciu samej odometrii nie da się z obrazów BEV prawidłowo złożyć mapy.

3) Landmarki jako punkty stałe

Żeby zrozumieć, gdzie leży problem, potrzebny jest punkt odniesienia. A najlepiej kilka — i to takich, które na pewno się nie poruszają. Dlatego pojawiają się landmarki. Na podłodze leży mata z markerami ArUco. Mata zastosowana w eksperymencie z rozłożonymi na niej pasami ruchu i markerami jest przedstawiona na rys. 3.

Każdy marker ma unikalne ID, jest jednoznacznie rozpoznawalny i daje precyzyjnie wyznaczoną pozycję w obrazie. W kolejnych klatkach, po transformacji do BEV, zapisujemy położenie markerów w lokalnym układzie kamery (albo w “lokalnym BEV” związanym z pojazdem). Każda klatka daje nam więc dwie rzeczy:

• fragment pasa ruchu,
• współrzędne punktu środkowego markera, który w rzeczywistości jest nieruchomy.

Pozycje markerów zapisuje w odpowiedniej tablicy csv. Jeżeli po przekształceniu do układu świata ten sam marker pojawia się w różnych miejscach, wiemy jedno: to nie podłoga się rusza. To model ruchu przestaje być zgodny z rzeczywistością. Na rysunku 5 pokazano globalne pozycje tych samych landmarków obserwowanych w różnych momentach przejazdu. Landmarki o tym samym ID tworzą wyraźne, rozdzielone skupiska. Każde skupisko odpowiada innemu fragmentowi trajektorii. Różnice między nimi nie są losowe — odzwierciedlają systematyczny dryf odometrii oraz błędne założenia co do położenia i orientacji kamery względem pojazdu.

4) Etap I – Offline Map Fitting

W tym miejscu warto jasno powiedzieć, co robimę. To nie jest jeszcze SLAM. Jestem w Etapie I – Offline Map Fitting, który ma bardzo konkretne założenia:

• trajektoria pojazdu jest zamrożona i traktowana jako prawda,
• nie wolno jej zmieniać,
• estymujemy tylko:
• • parametry systemowe (ekstrynsykę kamery),
• • położenia landmarków w jednej, wspólnej mapie.

Innymi słowy: szukamy najlepiej dopasowanej mapy świata, zakładając, że trajektoria jest idealna. To kluczowe założenie. I — jak się za chwilę okaże — bardzo silne.

5) Minimalna formalizacja (żeby nie zgubić sensu)

W dalszym opisie użyję dwóch pojęć:

• $C$ — “trajektoria” (kolejne pozycje pojazdu w czasie),
• $G$ — “mapa” (globalny układ odniesienia i pozycje landmarków w tym układzie).

W chwili $k$ znamy (z odometrii) pozę pojazdu: \(\mathbf{x}_k = (x_k, y_k, \psi_k).\) Z detekcji dostajemy pomiar landmarku: $\mathbf{z}_{k,j}$ w układzie lokalnym (BEV/kamery). Chcemy go przenieść do świata i porównać z globalną pozycją landmarku $\mathbf{p}_j$. W praktyce (w 2D) sprowadza się to do składania transformacji:

• najpierw “lokalny punkt z BEV”,
• potem ekstrynsyka (stała transformacja między kamerą/BEV a bazą pojazdu),
• potem poza pojazdu w świecie.

Można to zapisać zwięźle jako:

gdzie:

• $T_{G\leftarrow C}(\mathbf{x}_k)$ wynika z zamrożonej trajektorii,
• $T_{C}(\theta)$ to estymowana ekstrynsyka (np. przesunięcie i skręt kamery/BEV względem pojazdu),
• $\mathbf{z}_{k,j}$ to pomiar landmarku w układzie lokalnym.

A cel Etapu I to minimalizacja rozrzutu obserwacji tego samego landmarku w świecie, przy stałej trajektorii:

To są dwa wzory, które “spinają” cały tekst: pokazują, co liczymy i dlaczego. W wersji “programistycznej” (czytelniejszą dla osób z robotyki), to ten sam łańcuch można zapisać tak:

landmark_in_G = pose_G_from_traj[k] ∘ T_extrinsic ∘ z_kj

gdzie:

• pose_G_from_traj[k] – zamrożona trajektoria (odometria),
• T_extrinsic – estymowana ekstrynsyka,
• z_kj – pomiar landmarku w układzie lokalnym (kamera/BEV; analogicznie może to być lidar).

6) Kalibracja ekstrynsyki, czyli kiedy geometria przestaje wystarczać

W projekcie CAD samochodu przyjąłem, że punkt G (“kotwica” kamery tzn. początek lokalnego układu współrzędnych związanego z BEV) znajduje się w określonej odległości od środka modelu kinematycznego pojazdu. Znamy rozstaw osi, znamy promień kół, znamy przybliżone położenie kamery względem konstrukcji. To naturalny punkt startowy. Tyle że model nigdy nie jest rzeczywistością. Kamera może być minimalnie skręcona. Oś optyczna może nie być idealnie równoległa do osi pojazdu. A co ważniejsze — sam model ruchu jest kompromisem. Choć korzystamy z uproszczonego modelu Ackermanna, pojazd ma cztery koła skrętne sterowane parami. Koła wewnętrzne i zewnętrzne skręcają się pod tym samym kątem. Nie ma dyferencjału kątowego — jest tylko dyferencjał prędkościowy. W praktyce oznacza to jedno: rzeczywisty tor ruchu nie może idealnie odpowiadać modelowi. Pojawiają się poślizgi, sprężystość opon, asymetrie napędu. Tego nie da się „dokalibrować” jedną stałą. Dlatego zamiast wierzyć geometrii, stosujemy kryterium obiektywne: minimalizujemy rozrzut globalnych pozycji tych samych landmarków. Szukamy takich parametrów transformacji, dla których landmarki w układzie świata są możliwie najbardziej zwarte. Punkt G został wyznaczony projektowo jako L/2+R. Jednak analiza wariancji landmarków ujawniła, że rzeczywiste położenie układu BEV różni się o od tej wartości. Projekt przestał być założeniem — stał się hipotezą, którą można weryfikować danymi. To moment, w którym projekt mechaniczny staje się elementem systemu pomiarowego, który można oceniać statystycznie. Efekty minimalizacji rozrzutu grup landmarków poprzez zmiany współrzędnych punku G przedstawia rys. 6.

7) Algorytm “map fitting” działa świetnie. Mapa? – niekoniecznie

Po optymalizacji “map fitting” dzieje się coś pozornie idealnego. Koszt funkcji dopasowania grup landmarków do trajektorii gwałtownie spada. Landmarki układają się w zwarte skupiska. Algorytm nie widzi już sprzeczności — wszystko „pasuje”. Tylko, że gdy na tę mapę spojrzymy w odniesieniu do rzeczywistości, coś się nie zgadza. Landmarki są spójne względem trajektorii, ale nie względem świata. Fragmenty pasa ruchu są przesunięte. A różnice nie są losowe — pojawiają się dokładnie tam, gdzie ruch pojazdu był najbardziej wymagający. I to nie jest błąd optymalizacji. To jej logiczna konsekwencja. Dzieje się tak dlatego, że algorytm znajduje rozwiązanie matematycznie spójne — ale tylko względem przyjętej trajektorii (ponieważ ona z zalożenia jest “zamrożona” - prawdziwa i nie może być korygowana). Wynik działania “map fittigu” ukazuje rys. 7.

Zielone i różowe punkty na wykresach nie są trajektorią. One pokazują, gdzie w globalnym układzie lądują lokalne obserwacje landmarków, gdy przeniesiemy je przez zamrożoną trajektorię. Jeżeli trajektoria byłaby idealna, wszystkie obserwacje tego samego obiektu nałożyłyby się na siebie. Jeżeli nie jest — chmury zaczynają się wyginać, przesuwać i rozjeżdżać. Szczególnie wyraźnie widać to na ostrych zakrętach. W tych miejscach rzeczywisty promień skrętu jest większy niż wynikałoby to z modelu. Pojazd nie obraca się wokół jednego, idealnego środka chwilowego obrotu. Może to być efektem poślizgów. Model Ackermanna przestaje być dobrym przybliżeniem — a kamera bezlitośnie to ujawnia.

8) A jak to się ma do rzeczywistości?

Na rysunku 8 zestawiono trzy informacje: trajektorię odometryczną, globalne skupiska landmarków po map fittingu oraz rzeczywiste położenia markerów. Dla części landmarków (1–5) zgodność jest bardzo dobra, natomiast dla kolejnych (6–8) pojawiają się systematyczne przesunięcia. Od tego momentu cały układ zaczyna „odjeżdżać”. Różnice te korelują z ostrymi zakrętami, gdzie rzeczywisty ruch pojazdu odbiega od uproszczonego modelu kinematycznego.

Można się w tym miejscu zastanowić dlaczego map fitting „nie działa” i co z tego wynika?

Offline map fitting robi dokładnie to, co do niego należy:

• nie poprawia trajektorii,
• przesuwa landmarki tak, aby pasowały do przyjętego ruchu.

Jeżeli trajektoria jest błędna, mapa zostanie zdeformowana. Algorytm nie ma innego wyjścia. I właśnie dlatego ten etap jest tak ważny dydaktycznie. On nie służy do „naprawiania mapy”. On służy do zdiagnozowania, gdzie i dlaczego model ruchu przestaje być zgodny z rzeczywistością.

Na tym etapie pipeline jest spójny, ale ograniczony:

Algorytm znajduje rozwiązanie matematycznie poprawne, lecz fizycznie błędne. Koszt maleje nie dlatego, że świat został dobrze opisany, ale dlatego, że mapa została zdeformowana tak, aby zgadzała się z błędną trajektorią.

• kamera daje lokalne obserwacje w BEV,
• odometria zapewnia ciągłość ruchu,
• transformacja między układami jest skalibrowana,
• landmarki pozwalają mierzyć rozjazd pętli.

Ale jedna rzecz jest już jasna: nie da się zbudować poprawnej mapy świata, jeżeli nie pozwolimy trajektorii się zmieniać. I to jest dokładnie moment, w którym Etap I musi się skończyć. Można go podsymować następującym sformułowaniem: “Best-fit map of landmarks assuming the trajectory is ground truth” czyli “najlepiej dopasowana mapa punktów orientacyjnych, przy założeniu, że trajektoria jest zgodna z rzeczywistością.

9) co dalej

Dopiero kolejne kroki — korekta trajektorii, pełny SLAM i modelowanie poślizgu — mają sens. Ale to już osobna historia. Spróbuje to zrobić później. Na razie kamera zrobiła coś znacznie ważniejszego niż „zobaczenie pasa”. Stała się czujnikiem niespójności modelu ruchu. A to jest fundament każdego sensownego systemu lokalizacji.

1) Koncepcja

W poprzednim kroku sprowadziłem obraz z rybiego oka do układu drogi (bird’s eye view, BEV). To była decyzja czysto pragmatyczna: jeśli chcę sterować i składać mapę, potrzebuję geometrii, która zachowuje się możliwie „metrycznie”. W BEV jeden piksel odpowiada w przybliżeniu stałemu odcinkowi na podłodze, a pas przestaje być efektem perspektywy i dystorsji — staje się faktycznym kształtem toru. Ten wpis jest o następnym etapie: jak z takiego obrazu w BEV wyciągam pas w postaci punktów, które później mogę wykorzystać do sterowania i do składania mapy przejazdu. To, co opisuję, jest na razie rozwiązaniem „na bogato”: świetnym do przygotowań, strojenia parametrów i masowego przeglądania logów. Na komputerze PC taki pipeline sprawdza się dobrze przy obróbce nagrań, ale w tej formie jest zbyt złożony, żeby Jetson liczył go w czasie jazdy. Dlatego mapę będę składał po przejeździe, a na pokładzie zostanie uproszczona detekcja potrzebna do sterowania. Warto też doprecyzować, czym w tym projekcie są „pasy ruchu”. Fizycznie to cienkie, białe elementy drukowane 3D (około 2–3 mm grubości) łączone na klipsy jak w składanych torach. Dzięki temu są powtarzalne i mają wyraźną krawędź, ale potrafią też łapać refleksy w trudnym oświetleniu.

2) Segmentacja: „znajdź pas”, ale nie daj się oszukać oświetleniu

Na pierwszy rzut oka zadanie wydaje się proste: pas jest jasny, tło jest ciemniejsze. W praktyce problemem nie jest sam pas, tylko to, że jasność tła nie jest stała. Nawet na dość jednorodnej macie pojawiają się gradienty od okna, cienie, delikatne zmiany ekspozycji, a czasem lokalne refleksy. Jeśli zastosuję zwykłe progowanie jasności, to w jednym miejscu pas wyjdzie idealnie, a w innym zacznie się „wylewać” całe tło. Schemat kroków (BEV → ROI → tło → Otsu → morfologia → punkty) ukazany jest na rysunku:

Dlatego pierwszym krokiem po BEV nie jest progowanie, tylko wyrównanie tła. Intuicja jest taka: wolnozmienne zmiany jasności traktuję jako „oświetlenie”, a pas jako strukturę, którą chcę z tego oświetlenia wyciągnąć. Technicznie da się to zrobić na różne sposoby, ale sens jest zawsze ten sam: oszacować łagodne tło i je odjąć. Po takim zabiegu pas zostaje wyraźny, a podłoga przestaje udawać obiekt. Dopiero po wyrównaniu tła robię lekkie wygładzenie (żeby uspokoić fakturę maty), a potem globalne progowanie (np. metodą Otsu, z ewentualną drobną regulacją progu). To jest celowo proste: na tym etapie nie próbuję „zrozumieć obrazu”, tylko chcę dostać stabilną binarną maskę, która w większości klatek mówi: tu jest pas. Wynik po progowaniu prawie zawsze wymaga sprzątania. Wchodzą tu klasyczne narzędzia morfologii, czyli operacje „porządkujące kształty”: domykanie łączy przerwy i zasklepia drobne dziury, a filtr minimalnej powierzchni wyrzuca śmieci. Potem stosuję jeszcze jeden praktyczny trik: zostawiam tylko obiekty odpowiednio „długie”. Pas na torze jest strukturą wydłużoną, więc jeśli coś jest krótką plamą, to zwykle jest artefaktem. Na typowych klatkach działa to bardzo dobrze. W BEV dostaję czyste krawędzie pasa, a pierwsze dwa przykłady przejazdu (bez blików) pokazują ten „happy path”: segmentacja jest stabilna, a wykryty kształt jest spójny i gładki w kolejnych klatkach. Te „poprawne” sytuacje pokazuje poniższy rysunek:

— widać na nim kolejne etapy przetwarzania, od surowej klatki do punktów, które mogę dalej wykorzystać w sterowaniu i mapowaniu. W lewym górnym rogu znajduje się obraz wejściowy w skali szarości (jeszcze w geometrii rybiego oka). W prawym górnym rogu jest ten sam fragment sceny po przekształceniu do widoku z lotu ptaka (BEV): podłoga ma już „prawie metryczną” geometrię, a obszary poza mapą/ROI są wycięte (czarne trójkąty). Lewy dolny panel to wynik segmentacji binarnej (BW) po wyrównaniu tła, wygładzeniu, progowaniu oraz sprzątaniu morfologicznym. To jest moment, w którym algorytm mówi wprost: „to są piksele pasa”. W prawym dolnym rogu pokazuję tę samą detekcję w formie punktów obrysu — czyli piksele krawędzi wyciągnięte z maski (obrys). Taka reprezentacja jest wygodna w dalszych krokach: można na niej dopasowywać krzywe, wyznaczać oś pasa i odkładać punkty do wspólnej mapy przejazdu.

3) Obrys, szkielet i „punkty, które da się użyć”

Maska binarna jest dobra do podglądu, ale do dalszego przetwarzania wolę punkty. I tu są dwie sensowne reprezentacje, zależnie od tego, co chcę robić dalej: • obrys pasa — punkty na granicy wykrytego obiektu, • szkielet — jednopikselowa oś obiektu. Obrys dobrze oddaje geometrię krawędzi, co jest intuicyjne przy pasach ruchu. Szkielet bywa stabilniejszy, gdy segmentacja miejscami robi pas grubszy lub cieńszy, bo sprowadza obiekt do jednej krzywej. Ponieważ BEV jest metryczny, te punkty są od razu punktami w układzie drogi — i to jest dokładnie to, czego potrzebuję do mapowania. Na poniższym rys. widzimy, że nawet w bardzo zakrzywionej perspektywie możemy prawidłowo wyznaczyć punkty obrysu.

4) Największy wróg: bliki i niskie słońce

Następny obrazek jest dla mnie najcenniejszy, bo pokazuje realne ograniczenie tej metody. W bardzo słoneczny zimowy dzień, gdy słońce świeci pod małym kątem, potrafią pojawić się rozległe refleksy. Wtedy „jasne” przestaje znaczyć „pas”, tylko „cokolwiek, co akurat złapało blik”. Progowanie dostaje fałszywe, bardzo jasne regiony i maska może rozsypać się w spektakularny sposób. I tu dochodzę do ważnej decyzji projektowej. Na tym etapie nie próbuję agresywnie walczyć z blikami filtrami „anty-refleks”, bo bardzo łatwo jest przy okazji uszkodzić sam pas. Wolę mieć pipeline, który w trudnych warunkach jawnie zaczyna produkować błędy (i można takie klatki odsiać albo potraktować inaczej), niż pipeline, który pozornie działa, ale cicho gubi poprawne fragmenty. Co więcej, w tym konkretnym przypadku najlepsze „ulepszenie algorytmu” jest poza algorytmem: po prostu stabilizuję warunki pracy kamery. W praktyce oznacza to osłonięcie okna, żeby wyciąć niskie słońce i ograniczyć refleksy u źródła. Ewentualnie można zastosować oświetlenie drogi z pojazdu (np. LED). Rys. poniżej ukazuje sytuację krytyczną – przebieg pasa ruchu jest tu prawie niewidoczny.

5) Sterowanie: środek pasa, a co gdy widać tylko jedną krawędź?

Docelowo najwygodniejsza sytuacja jest wtedy, gdy widzę dwie krawędzie. Wtedy problem prowadzenia upraszcza się do geometrii: z lewej i prawej krawędzi wyznaczam oś pasa i steruję tak, żeby pojazd trzymał się środka. Wiem jednak, że to nie zawsze będzie możliwe. W zakręcie albo przy ograniczonym polu widzenia może się zdarzyć, że jedna krawędź „znika”. Tego jeszcze nie rozpracowałem w tej iteracji, ale naturalne są dwa kierunki: albo przejść w tryb jazdy „z offsetem” od jednej krawędzi (w BEV to jest proste, bo odległości są metryczne), albo wykorzystać mapę/pamięć trasy, żeby brakujący fragment domyślić z kontekstu. Ten rys pokazuje taką sytuację – algorytm rozpoznaje tutaj tylko jedną stronę pasa ruch.

W mojej wcześniejszej wersji sterowania bardzo dobrze sprawdziło się podejście “pościgowe”. Zamiast sterować bezpośrednio na krzywiznę, wybierałem „wirtualną kropkę” na środku toru w stałej odległości przed pojazdem i jechałem w jej kierunku — jakby ta kropka ciągnęła pojazd “na dyszlu”. Taki układ, lekko domknięty prostym regulatorem (PID), działał zaskakująco stabilnie. BEV jest do tego wręcz stworzone: „kropka” ma sensowną geometrię w układzie drogi, a nie w zdeformowanej perspektywie.

6) W następnym kroku mapa: mniej SLAM-u, więcej odometrii (i lekka korekta wizyjna)

Poprzednio budowałem mapę metodą DP SLAM bez odometrii, co było obliczeniowo kosztowne. Teraz sytuacja jest inna: mam odometrię, której ufam na tyle, że może być podstawą predykcji ruchu. To zmienia filozofię całego układu: odometria robi „ciężką robotę” w sensie przesunięć i obrotów między klatkami, a wizja nie musi wymyślać ruchu od zera — może pełnić rolę korekty, szczególnie przy poślizgach kół pojazdu w trudnym terenie. Naturalna funkcja celu w takim dopasowaniu to zgodność bieżącej obserwacji z tym, co już jest w mapie. W moich wcześniejszych eksperymentach dobrze sprawdzała się korelacja z wagami (środek obrazu ważniejszy niż brzegi), bo to, co jest najbliżej pojazdu i najbardziej „na wprost”, zwykle jest najbardziej informacyjne i najmniej podatne na artefakty. W poprzednich wpisach wspominałem też o RANSAC, ale tu trzeba być uczciwym: RASNAC ma sens dopiero wtedy, gdy mam co dopasowywać. Jeśli obraz jest prawie gładki i jedyną strukturą są dwie białe krawędzie, to RANSAC na klasycznych cechach może nie mieć „punktów zaczepienia”. Natomiast da się go użyć sprytniej: nie do dopasowania cech typu ORB/SIFT, tylko do dopasowania modelu do punktów pasa — czyli do dopasowania transformacji między chmurami punktów (bieżąca klatka kontra fragment mapy). To jest kierunek, który warto sprawdzić, bo potencjalnie mógłby korygować poślizgi nawet wtedy, gdy odometria chwilowo kłamie. I znów: nie obiecuję wykonywania mapy w czasie rzeczywistym na Jetsonie. Bardziej realistyczne jest, że mapowanie będzie procesem offline albo pół-online, a na pokładzie zostanie tylko ta część, która jest potrzebna do stabilnego prowadzenia.

1) Koncepcja

W projekcie korzystam z kamery typu rybie oko $170^\circ$. Powód jest prosty: kamera jest zamontowana nisko, a ja chcę widzieć możliwie dużo „podłogi” i toru jazdy tuż przed pojazdem. Przy klasycznym obiektywie z takiej pozycji szybko tracę pole widzenia, natomiast rybie oko daje szeroki kadr i dużo informacji o przebiegu pasa. Cena za ten komfort jest jednak wysoka: obraz z rybiego oka jest silnie zniekształcony. Linie, które w rzeczywistości są proste, w obrazie stają się krzywe, a skala obiektów mocno zależy od położenia w kadrze. Ten efekt najłatwiej zauważyć na szachownicy kalibracyjnej — rys. poniżej pokazuje ustawienie pojazdu podczas wykonywania zdjęcia oraz przykładowy obraz z kamery (rys. 1).

Rys. 1. Ustawienie pojazdu podczas wykonywania zdjęcia szachownicy oraz przykładowy obraz z kamery.

Dla człowieka „wygięta” szachownica nie stanowi problemu — mózg koryguje to automatycznie. Dla algorytmu sterowania, który ma wykrywać krawędzie pasa i utrzymywać pojazd w jego środku, jest to już realna przeszkoda. To samo miejsce na torze może wyglądać inaczej w kolejnych klatkach, a proste operacje geometryczne przestają działać stabilnie. Najlepsze własności geometryczne ma sytuacja, w której obserwujemy podłogę prostopadle z góry — „z lotu ptaka”. Taki obraz ma w przybliżeniu stałą skalę, a linie na podłodze pozostają liniami. Dokładnie tego chcę. Dlatego wprowadzam korektę obrazów do widoku z lotu ptaka (bird’s eye view, BEV). Zakładam przy tym, że interesuje mnie przede wszystkim płaszczyzna drogi (mata/tor), a wszystko, co jest „pionowe”, traktuję jako efekt uboczny. BEV nie jest rekonstrukcją 3D — to świadome uproszczenie: buduję przekształcenie 2D, które na podłodze daje obraz o bardziej stałej geometrii, wygodny do sterowania, składania mapy z wielu klatek oraz ewentualnego uczenia sieci. Przekształcenie (fisheye $\rightarrow$ bird’s eye / usunięcie dystorsji) musi się jednak „z czegoś wziąć” — potrzebuję informacji, jak piksele obrazu odpowiadają punktom na podłodze. Są dwa sensowne sposoby, żeby tę zależność wyznaczyć.

2) Metody przekształcania

Aby uzyskać widok z lotu ptaka, konieczne jest określenie zależności pomiędzy pikselami obrazu a punktami na podłodze. Problem ten można rozwiązać na kilka sposobów, które różnią się przede wszystkim tym, czy opisują odwzorowanie lokalnie, czy globalnie. Najbardziej bezpośrednim podejściem jest interpolacja oparta na punktach pomiarowych. W tym przypadku nie buduje się modelu kamery — zamiast tego wykorzystuje się zbiór znanych korespondencji pomiędzy obrazem a przestrzenią i konstruuje odwzorowanie lokalne. Metoda ta pozwala uzyskać bardzo wysoką dokładność tam, gdzie dostępne są dane, jednak jej działanie ogranicza się do obszaru pokrytego punktami. Na brzegach pojawiają się braki danych i niestabilność. Drugim podejściem jest wykorzystanie homografii, czyli globalnego przekształcenia projektowego. Zakłada ono, że scena jest płaska, a obraz został wcześniej skorygowany z dystorsji. Homografia zapewnia spójność całego obrazu, ale upraszcza rzeczywistą geometrię — szczególnie w przypadku obiektywu typu rybie oko, gdzie deformacje są znaczne. Trzecia możliwość polega na wykorzystaniu modelu kamery. W tym przypadku estymowane są parametry optyczne, a następnie używane do rzutowania promieni na płaszczyznę. Podejście to jest fizycznie poprawne, ale w praktyce okazuje się wrażliwe na błędy kalibracji, zwłaszcza w obszarach peryferyjnych. W efekcie żadna z metod nie jest idealna. Interpolacja daje wysoką dokładność lokalną, homografia zapewnia stabilność globalną, a model kamery oferuje spójny opis optyki, ale kosztem złożoności i wrażliwości na błędy. Wybór rozwiązania staje się więc kompromisem pomiędzy tymi cechami.

3) Plansza kalibracyjna $ChArUco$

Kluczowym elementem całego procesu jest sposób pozyskania danych. Zamiast ręcznego wskazywania punktów wykorzystuję planszę typu ChArUco, która łączy szachownicę z markerami ArUco. Planszę wykorzystaną w projekcie przedstawiono na rys. 2.

Rys. 2. Plansza kalibracyjna ChArUco wykorzystująca słownik markerów DICT_6X6_250.

Każdy marker posiada unikalny identyfikator, dzięki czemu możliwe jest jednoznaczne przypisanie wykrytych punktów do ich pozycji na planszy. W praktyce oznacza to, że nawet przy częściowym widoku planszy możliwe jest uzyskanie poprawnych korespondencji. Efektem działania algorytmu detekcji jest zbiór danych, w którym każdemu punktowi obrazu odpowiada punkt na planszy. Etapy procesu detekcji markerów przedstawiam na rys. 3: wykonanie zdjęcia, obraz z kamery, wynik detekcji markerów i szczegóły oznaczenia identyfikatorów narożników.

Rys. 3. Przykład detekcji markerów ChArUco: obraz wejściowy, wykryte markery oraz szczegóły (ID, narożniki, ramki).

Powstaje w ten sposób gęsta, metryczna tabela odwzorowania przestrzeni. W porównaniu do ręcznego klikania pozwala to uzyskać znacznie większą liczbę punktów, a jednocześnie eliminuje błędy wynikające z niejednoznaczności.

4) Model kamery

Niezależnie od sposobu pozyskiwania punktów korespondencji pomiędzy obrazem a podłogą — czy jest to metoda ręczna oparta na szachownicy, czy automatyczna detekcja z wykorzystaniem planszy ChArUco — możliwe jest zbudowanie modelu kamery. Biblioteka OpenCV udostępnia w tym celu gotowe narzędzia, które na podstawie wykrytych narożników estymują parametry optyczne układu. Warunkiem działania tej procedury jest wykonanie serii zdjęć planszy kalibracyjnej przy zachowaniu stałej geometrii układu, w szczególności niezmiennego położenia kamery względem podłogi. Przykładowy zestaw takich obrazów przedstawiono na rys. 4.

Rys. 4. Zestaw obrazów wykorzystanych do kalibracji kamery typu rybie oko.

W wyniku kalibracji otrzymywany jest model kamery, który można interpretować jako zestaw parametrów opisujących transformację pomiędzy przestrzenią trójwymiarową a obrazem. W jego skład wchodzą między innymi ogniskowa, położenie środka optycznego oraz współczynniki dystorsji, które modelują deformacje charakterystyczne dla obiektywów szerokokątnych. Model ten pozwala na „odwrócenie” zniekształceń i rzutowanie promieni kamery na płaszczyznę podłogi, co teoretycznie umożliwia uzyskanie widoku z lotu ptaka w sposób fizycznie poprawny. W praktyce pojawia się jednak istotny problem. Estymacja parametrów odbywa się na podstawie skończonej liczby punktów i zawsze obarczona jest błędem. Błąd ten jest niewielki w centralnej części obrazu, ale rośnie w jego obszarach peryferyjnych, gdzie dystorsja obiektywu jest największa. W rezultacie model kamery, choć spójny geometrycznie, nie odwzorowuje idealnie relacji pomiędzy obrazem a płaszczyzną podłogi. To ograniczenie ma bezpośredni wpływ na jakość obrazu BEV i stanowi jeden z powodów, dla których warto porównać to podejście z innymi metodami. Oznacza to, że sam model kamery nie jest wystarczający do uzyskania stabilnego i metrycznie poprawnego obrazu BEV.

5) Porównanie podejść

Dysponując zarówno danymi pomiarowymi, jak i modelem kamery, można przeanalizować zachowanie poszczególnych metod przekształcania obrazu do widoku z lotu ptaka. Interpolacja oparta na punktach pomiarowych zapewnia bardzo wysoką dokładność w obszarach, gdzie dane są dostępne. Jednocześnie jednak nie radzi sobie poza nimi — pojawiają się braki danych i artefakty. Metoda ta ma więc charakter lokalny. Homografia reprezentuje podejście przeciwne. Zapewnia spójność całego obrazu dzięki globalnemu modelowi przekształcenia, ale kosztem dokładności. Błąd ma charakter systematyczny i rośnie wraz z odległością od obszaru najlepszego dopasowania. Model kamery stanowi próbę fizycznie poprawnego opisu układu, jednak w praktyce jego dokładność ograniczona jest przez błędy kalibracji, szczególnie w obszarach oddalonych od środka obrazu. W efekcie mamy do czynienia z dwoma przeciwstawnymi podejściami: lokalnym i globalnym. Jedno zapewnia wysoką dokładność, drugie stabilność. Żadne z nich nie daje obu jednocześnie.

Rys. 5. Porównanie obrazu BEV uzyskanego metodą interpolacji (po prawej) oraz homografii (po lewej).

Różnice pomiędzy podejściem lokalnym i globalnym są dobrze widoczne na rys. 5. Interpolacja zachowuje wysoką dokładność w obszarach pokrytych punktami pomiarowymi, natomiast homografia zapewnia ciągłość obrazu kosztem deformacji geometrycznych, szczególnie na obrzeżach.

Rys. 6. Analiza różnic pomiędzy metodami: maska obszarów dodatkowych, mapa różnic oraz próba fuzji wyników.

Próba bezpośredniego połączenia obu metod prowadzi jedynie do poprawy wizualnej, ale nie tworzy jednego spójnego modelu odwzorowania. Interpolacja i homografia opisują przestrzeń w różny sposób — odpowiednio lokalny i globalny — dlatego ich wyniki nie są bezpośrednio kompatybilne (rys. 6).

6) Wybór rozwiązania: mapy $Xmap$ i $Ymap$

Zamiast wybierać jedną z metod w czystej postaci, stosuję podejście pośrednie oparte na jawnej mapie przekształcenia. Buduję dwie macierze: $Xmap$ oraz $Ymap$, które dla każdego punktu obrazu wynikowego wskazują odpowiadający mu punkt w obrazie źródłowym. W praktyce oznacza to, że całe przekształcenie sprowadza się do operacji remapowania obrazu. Rozwiązanie to nie wymaga ani idealnego modelu kamery, ani globalnego uproszczenia geometrii. Jednocześnie zachowuje wysoką dokładność lokalną, ponieważ opiera się bezpośrednio na danych pomiarowych, oraz zapewnia pełne pokrycie obrazu dzięki interpolacyjnemu wypełnieniu przestrzeni. Można więc powiedzieć, że jest to kompromis pomiędzy podejściem lokalnym i globalnym — wykorzystuje rzeczywiste dane, ale prowadzi do jednego, spójnego odwzorowania. Efektem jest stabilny, metryczny obraz podłogi, który nadaje się zarówno do sterowania pojazdem, jak i do budowy mapy z wielu klatek.

7) Ograniczenia

Przyjęte podejście ma jednak swoje ograniczenia. Przede wszystkim działa poprawnie tylko dla konkretnej konfiguracji kamery — jej wysokości, kąta nachylenia oraz pola widzenia. Zmiana któregokolwiek z tych parametrów wymaga ponownego wyznaczenia map. Drugim ograniczeniem jest założenie płaskości sceny. Obiekty wystające ponad powierzchnię podłogi są w widoku z lotu ptaka zniekształcone, co jest bezpośrednią konsekwencją przyjętego modelu. W praktyce oznacza to, że odwzorowanie jest poprawne tylko dla płaszczyzny drogi, natomiast elementy przestrzenne nie są reprezentowane w sposób geometrycznie zgodny z rzeczywistością.

7) Co dalej

W kolejnym kroku obraz w układzie drogi — w szczególności jego wariant krawędziowy — zostanie wykorzystany do składania wielu klatek w jedną spójną mapę przejazdu. Kluczową rolę odgrywa tutaj odometria pojazdu, która dostarcza przybliżonej informacji o przesunięciach i obrotach, natomiast korekcja wizyjna pozwala kompensować błędy wynikające z poślizgów oraz niedokładności modelu ruchu. W praktyce oznacza to połączenie dwóch źródeł informacji: predykcji ruchu wynikającej z odometrii oraz dopasowania obrazu, które stabilizuje i koryguje trajektorię. Dzięki temu możliwe jest stopniowe budowanie mapy otoczenia w sposób inkrementalny, bez konieczności stosowania dodatkowych czujników. Ostatecznie prowadzi to do uzyskania spójnej reprezentacji przestrzeni na podstawie pojedynczej kamery, co znacząco upraszcza cały system przy zachowaniu użytecznej dokładności.

1) Rejestruje obrazy z kamery

W systemie używam modułu „ArduCAM IMX219 Wide Angle Camera” z obiektywem typu rybie oko 170°. Wybrałem go, ponieważ jest wspierany przez Nvidię dla Jetsona Orin Nano, którego używam. Muszę zaznaczyć, że uruchomienie kamery było wyjątkowo trudne i czasochłonne — pierwszy raz spotkałem się z tyloma niuansami. Obecnie kamera pracuje w trybie zdjęciowym 1640×1232 przy 30 fps. Równolegle obrazy są streamowane na stronę WWW, co pozwala na podgląd „na żywo”.

2) Jak to teraz wygląda - czyli gdzie jestem z projektem

Na kolażu poniżej pokazuję aktualny stan:

• Kolumna lewa, góra: fotografia toru jazdy. Do testów używam maty z wyznaczonymi pasami.

• Kolumna lewa, dół: trajektoria i błędy szacowania pozycji wyznaczane wyłącznie z odometrii (model „Ackermann predict”). Widać bardzo dobre dopasowanie.

• Kolumna środkowa, góra: surowe logi z enkoderów i silników (podstawa do odometrii).

• Kolumna środkowa, dół: nowość — seria klatek z kamery na pojeździe, czyli „jak pojazd widzi trasę”.

• Kolumna prawa: dwa powiększone zrzuty z przejazdu.

3 Z jakim problemem się spotkałem i jakie to niesie konsekwencję

Używam taśmy CSI do kamery która ma długość 15 cm. Próba z taśmą 30 cm zakończyła się brakiem obrazu i nagrzewaniem taśmy, więc zostałem przy 15 cm. To wymusza niskie osadzenie kamery. Podczas ostrych zakrętów tracę widoczność zewnętrznych pasów, co utrudnia segmentację i późniejsze decyzje sterujące.

4 co dalej?

• Budowa mapy obrazów. Korekta perspektywy “fisheye” do widoku z lotu ptaka “bird’s eye view”. Do łączenia klatek rozważam RANSAC (detekcja i dopasowanie cech, odrzucanie outlierów), a następnie złożenie mozaiki.

• Fuzja map: spróbuję scalić mapę odometryczną i wizyjną metodą bayesowską (ważenie wiarygodności źródeł).

• Autonomia: planuję dotrenować sieć CNN „AlexNet” do autonomicznego prowadzenia po torze metodą transfer learning z uczeniem nadzorowanym. Etykiety to nastawy prędkości i kąta skrętu.

• Agent LLM: demonstrator sterowania urządzeniami peryferyjnymi przez lokalny model LLM na Jetsonie (np. Gemma 3 lub równoważny) — wydawanie komend prędkości/skrętu oraz analiza zdjęć z kamery.

Koncepcja. Środowisko do badań - składana mata i pasy ruchu

Ten post to propozycja zestawu ćwiczeń ilustrujących estymację trajektorii z przejazdu po makiecie drogi. Najpierw prezentuję samo środowisko. Makieta to czarna, składana mata oraz białe „klocki–szyny” udające pasy ruchu, wykonane w technice druku 3D. Elementy łączę na wpusty („na klik”). Przygotowałem dwa zestawy łuków o znanych promieniach $R_{\mathrm{ICR}}$: 250 mm i 500 mm (segmenty po 15°) oraz dwa zestawy prostych (długości 100 i 200 mm). Pozwala to szybko zbudować różne trasy.

Rysunek poniżej pokazuje jedną z konfiguracji toru:

Ponieważ mata składa się z płytek o stałych wymiarach, a łuki i proste mają zadaną geometrię, traktuję trasę jako skalibrowaną. Na tak przygotowanej makiecie sprawdzam funkcjonalność modelu pojazdu i przydatność modeli obliczeniowych. Na ostrych zakrętach jadę wolno, aby ograniczyć poślizgi (pojazd nie ma dyferencjału kątowego). Poniżej surowe serie: prędkości i prądy silników oraz kąty serw (FBK) przed obróbką — dane wejściowe do resamplingu 50 Hz.

Następnie, jak wcześniej, wyznaczam sygnały sterujące w punkcie środka pojazdu:

Otrzymana trajektoria:

Widać rozbieżność końcowej pozycji w kierunku osi (Y) (w (X) jej nie ma — pojazd rzeczywiście zatrzymał się nieco dalej). Różnica w (Y) wynosi około 10 cm. Jest to „znoszenie”, którego prawdopodobną przyczyną jest brak dyferencjału kątowego i wynikające z tego poślizgi na łukach; przy większej prędkości odchylenie byłoby zapewne większe. Na koniec prezentuję oszacowane błędy (przy założeniu normalnych rozkładów szumów):

Błędy narastają powoli i przewidywalnie wzdłuż trasy, co potwierdza stabilność odometrii przy niskich prędkościach i umiarkowanych kątach skrętu.

Zadanie wprowadzające. Sprawdzenie poprawności nastaw dyferencjału

Nastaw programowo kąt skrętu na jedną z wartości $\delta \in {5^\circ,10^\circ,\ldots,35^\circ}$ i wyłącz pojazd z takim ustawieniem kół. Wolno przepychaj pojazd po macie tak, aby środek pojazdu wykonał 1–2 pełne okręgi. Zlicz obroty kół: $N_{\mathrm{in}}$, $N_{\mathrm{out}}$.

Pomiary geometryczne (bez modelu):

• Zaznacz markerem punkt dokładnie pod środkiem pojazdu (pozycja startowa). W trakcie ruchu zaznacz co najmniej trzy kolejne pozycje środka pojazdu (najlepiej rozłożone kątowo o ok. 120°).

• Wyznacz promień okręgu przejazdu środka pojazdu z trzech punktów $A,B,C$:

  • policz długości boków trójkąta:

    $a=BC$

    $b=AC$

    $c=AB$

    ,

  • policz pole:

    $A_\triangle=\tfrac{1}{2}(B-A)\times(C-A)$

    ,

  • promień okręgu opisanego:
    $ \begin{aligned} R_{\mathrm{ICR}}=\frac{a\,b\,c}{4\,A_\triangle} \end{aligned} $

  • powtórz dla kilku trójek punktów i uśrednij wynik.
• Alternatywnie możesz użyć metody cięciwy i strzałki (dla jednej pary punktów oddalonych kątowo o co najmniej 120°):
  • dla długości cięciwy $c$ i strzałki $s$: $ \begin{aligned} R_{\mathrm{ICR}}=\frac{c^{2}}{8s}+\frac{s}{2} \end{aligned} $

Obliczenia z pomiarów:

• Oblicz stosunek obrotów: $ \begin{aligned} S_{\mathrm{meas}}=\dfrac{N_{\mathrm{out}}}{N_{\mathrm{in}}} \end{aligned} $

Oblicz przewidywne wartości teoretyczne z modelu Ackermana (idealna zbieżność):

• Dla zadanego $\delta$ policz wartości teoretyczne:

$ \begin{aligned} R_{\mathrm{ICR}}(\delta) &= \dfrac{L}{2\,\tan \delta}
\end{aligned} $

$ \begin{aligned} R_{\mathrm{in}}(\delta) &= \sqrt{\left(R_{\mathrm{ICR}}(\delta) - \dfrac{D}{2}\right)^{2} + \left(\dfrac{L}{2}\right)^{2}}
\end{aligned} $

$ \begin{aligned} R_{\mathrm{out}}(\delta) &= \sqrt{\left(R_{\mathrm{ICR}}(\delta) + \dfrac{D}{2}\right)^{2} + \left(\dfrac{L}{2}\right)^{2}}
\end{aligned} $

$ \begin{aligned} S_{\mathrm{model}}(\delta) &= \dfrac{R_{\mathrm{out}}(\delta)}{R_{\mathrm{in}}(\delta)} \end{aligned} $

Oceń wyniki i sformułuj wnioski:

• Zestaw w tabeli dla każdej wartości $\delta$: $R_{\mathrm{ICR}}$ (z pomiaru), $S_{\mathrm{meas}}$, $S_{\mathrm{model}}$.
• Oceń konieczność korekty wzorów dyferencjału: jeśli $S_{\mathrm{meas}}<S_{\mathrm{model}}$, wprowadź współczynnik redukcji $\kappa$ tak, aby

$ \begin{aligned} \frac{n_{\mathrm{out}}}{n_{\mathrm{in}}} =\frac{1-\kappa}{1+\kappa}\;S_{\mathrm{model}} \quad\Rightarrow\quad \kappa=\frac{S_{\mathrm{model}}-S_{\mathrm{meas}}}{S_{\mathrm{model}}+S_{\mathrm{meas}}} \end{aligned} $

i podaj proponowaną wartość $\kappa(\delta)$ z eksperymentu.

Cel pierwszej serii ćwiczeń

• Zastosować odometrię 4WS (Ackermann‑predict) na danych z przejazdów.
• Przećwiczyć obróbkę logów NDJSON: import, synchronizacja, konwersje jednostek.
• Wyznaczyć trajektorię w układzie globalnym i oszacować niepewność (elipsy 3‑sigma).
• Zweryfikować wyniki na prostej makiecie z pasów (mata + wydruki 3D).

Instrukcja do tej serii ćwiczeń (pdf) jest na stronie githuba.

Materiały wejściowe

• Pliki z logami NDJSON: out_00.txt, out_01.txt. Przygotowane wstępnie pliki będą się znajdowały na serwerze githuba.

• Parametry pojazdu: L = 260 mm, D = 153 mm, Rw = 37.25 mm,

  δ

  ≤ 35°.

• Skrypty startowe (MATLAB/Python): do parsowania, resamplingu, odometrii i rysowania wykresów.

1) Import, synchronizacja, jednostki

Zadanie:

• Wczytaj log NDJSON linia po linii, rozdziel rekordy MOTOR i SERVO.
• Przekonwertuj jednostki:
  • prędkości kół: spd [0.1 rpm] → v_wheel [m/s],
  • kąty serw: deg → rad (kanały servo_1, servo_2).
• Wyrównaj do 50 Hz (ΔT = 0.02 s) interpolacją liniową.
• Zdefiniuj sygnały modelu:
  • $V_k$ — średnia prędkość pojazdu,
  • $\delta_k$ — średnia z FBK po konwersji do rad.

Wyniki:

• wykresy $V(t)$, $\delta(t)$,
• krótki opis resamplingu (2–3 zdania).

2) Odometria 4WS (Ackermann‑predict)

Równania aktualizacji stanu:

Zadanie:

• Przyjmij $x_0 = 0, y_0 = 0, \Theta_0 = 0 $.
• Narysuj trajektorię (x, y) z zaznaczonym początkiem i końcem.

Wyniki:

• wykres trajektorii,
• komentarz: wpływ znaku $\delta$ na stronę skrętu i ICR.

3) Niepewność i elipsy 3‑sigma

Założenia:

Macierze Jacobiego:

Propagacja: $\hat x_k^{-} = f(\hat x_{k-1}, z_k)$ $P_k^{-} = F_k P_{k-1} F_k^\top + G_k Q_k G_k^\top$ $z_k = [V_k,\ \delta_k]^\top$

Zadanie:

• Przyjmij np. $\sigma_V = 0.02,\mathrm{m/s}$, $\sigma_\delta = 0.5^\circ$ (w rad).
• Narysuj elipsy 3‑sigma co 10 s wzdłuż trajektorii.

Wyniki:

• trajektoria z elipsami,
• krótki komentarz: gdzie niepewność rośnie szybciej i dlaczego.

4) Weryfikacja na makiecie: korytarz z pasów

Zadanie:

• Ułóż tor prosty i łagodny zakręt.
• Zrób przejazd prosto i po łagodnym łuku.
• Policz błąd boczny trajektorii względem osi korytarza.

Wyniki:

• wykres błędu bocznego vs. czas,
• interpretacja wpływu $V$ i $\delta$.

5) Pętla zamknięta (ósemka / pętla parkingowa)

Zadanie:

• Ułóż pętlę, wykonaj przejazd.

• Policz wektor domknięcia:

  $\Delta x$

  $\Delta y$

  $\Delta \Theta$

Wyniki:

• trajektoria + wektor domknięcia,
• komentarz: co dominuje w błędzie.

6) (Opcjonalnie) Dopasowanie łuku

Zadanie:

• Na segmencie stałego skrętu dopasuj okrąg LS i porównaj promień z $R_c = L/(2 \tan \bar{\delta})$.

Wyniki:

• promień z dopasowania vs. teoretyczny,
• błąd względny [%].

Kryteria oceniania

Poprawność techniczna

• Import NDJSON, rozdzielenie MOTOR/SERVO.
• Konwersje jednostek poprawne.
• Resamplowanie 50 Hz opisane i poprawne.
• Implementacja odometrii 4WS zgodna ze wzorami.
• $F_k, G_k$ policzone dobrze; $Q_k, P_0$ uzasadnione.

Jakość analizy

• Czytelne trajektorie z oznaczeniami.
• Elipsy 3‑sigma poprawnie zorientowane.
• Błąd boczny obliczony; wnioski sensowne.
• Wektor domknięcia policzony i omówiony.
• Wykrywanie anomalii i wpływ filtracji.

Prezentacja i wnioski

• Podpisane osie, jednostki, legendy.
• Zwięzłe komentarze pod wykresami.
• Wnioski: gdzie i dlaczego rośnie niepewność; kiedy potrzebna korekcja.

Punktacja sugerowana

• Zad. 1–2: 30%
• Zad. 3: 25%
• Zad. 4: 15%
• Zad. 5: 15%
• Zad. 6: 10%
• Prezentacja/wnioski: 5%

Ocena 3 - 51%, 3.5 - 61%, 4 - 71%, 4.5 - 81%, 5 ponad 91%.

Uwaga praktyczna

• W tej implementacji krok „predict” opiera się na pomiarach $V, \delta$ (FBK), nie na komendach, aby uniknąć błędów od opóźnień aktuatorów.

1) Koncepcja obliczania błędów

W poprzednim poście „Pierwsze jazdy” pokazałem wrażliwość wyznaczania trajektorii na stały offset kąta skrętu: nawet +0.5° systematycznie zniekształca tor ruchu. W niniejszym wpisie analizuję inny aspekt — wpływ błędów losowych na estymację trajektorii. Przyjmuję, że odchylenia od nieznanych wartości rzeczywistych wynikają z obecności szumu procesu oraz szumu pomiaru.

Obliczenia prowadzę iteracyjnie w czasie dyskretnym. Ruch pojazdu opisuję modelem kinematycznym 4WS w konfiguracji przeciwfazowej, w którym przemieszczenie realizowane jest po łuku okręgu, a obrót następuje wokół chwilowego środka obrotu (ICR). Punktem wyjścia są równania kroku predykcji stanu:

Powyższe równania opisują deterministyczną propagację stanu pomiędzy chwilami $k-1$ i $k$. Interesuje nas jednak, w jaki sposób w tym kroku narasta niepewność estymacji. Błąd stanu w chwili $k-1$ opisany jest macierzą kowariancji $P_{k-1}$. Celem dalszych rozważań jest wyznaczenie macierzy kowariancji stanu po predykcji, $P_k^-$.

2) Błędy stanu i sterowania

Przyjmuję, że rozkłady gęstości prawdopodobieństw błędów mają charakter normalny. Niepewność początkową stanu opisuję macierzą kowariancji:

Niepewność sterowania (wejść) opisuje macierz kowariancji:

gdzie $\sigma$ to odchylenie standardowe, a $\sigma^2$ wariancja.

3) Linearyzacja kroku (Ackermann‑predict)

Równania ruchu są nieliniowe, dlatego w celu analizy propagacji niepewności stosuję ich liniowe przybliżenie w otoczeniu punktu pracy $x^-$ oraz $u^-$. Umożliwia to zastosowanie prawa propagacji błędu dla transformacji liniowych. W rozważaniach przyjmuję dyskretyzację metodą Eulera w przód oraz stałe sterowanie w obrębie kroku czasowego.

Liniaryzację modelu ruchu można zapisać w postaci:

gdzie wektor stanu ma postać:

Macierz Jacobiego względem stanu Macierz $F_k$ jest Jacobianem funkcji przejścia stanu względem wektora stanu, wyznaczonym w punkcie $(x_{k-1}, u_k)$:

co dla rozważanego modelu prowadzi do postaci:

Macierz Jacobiego względem sterowania. Macierz (G_k) jest Jacobianem funkcji przejścia stanu względem wektora sterowania:

4) Krok predykcji oparty na pomiarach, nie na sterowaniach

W mojej implementacji nie korzystam z sygnałów sterujących jako wejść do kroku predykcji. Powód jest natury praktycznej: opóźnienia pomiędzy wydaniem komendy a rzeczywistą odpowiedzią serwomechanizmów i silników są istotne, zmienne w czasie i trudne do wiarygodnego modelowania. W konsekwencji wektor sterowania $u_k$ nie reprezentuje faktycznego ruchu pojazdu w danym kroku czasowym.

Takie sformułowanie kroku predykcji odpowiada podejściu znanemu w literaturze jako dead‑reckoning driven by measured inputs, w którym estymacja ruchu opiera się na mierzonych wielkościach kinematycznych zamiast sygnałów sterujących.

Zamiast tego krok predykcji opieram bezpośrednio na pomiarach prędkości $V_k$ oraz kąta skrętu $\delta_k$, pochodzących z enkoderów serwomechanizmów oraz czujników prędkości kół (po przeliczeniu do jednostek SI). Innymi słowy, wielkości, które w klasycznym ujęciu traktowane są jako sterowania, pełnią tutaj rolę obserwowanych zmiennych kinematycznych, uwzględniających już wewnętrzną dynamikę aktuatorów oraz ich opóźnienia.

W tej konwencji:

równania propagacji stanu pozostają bez zmian, jednak wielkości $V_k$ oraz $\delta_k$ traktowane są jako pomiary obarczone niepewnością, a nie sygnały sterujące,

• macierz kowariancji wejść $Q_k$ opisuje niepewność pomiarów prędkości i kąta skrętu (szum oraz rozrzut sygnałów FBK), a nie niepewność komend sterujących.

W efekcie propagacja stanu i niepewności w kroku predict przyjmuje postać:

gdzie \(\begin{aligned} z_k = \begin{bmatrix} V_k & \delta_k \end{bmatrix}^\top \end{aligned}\)

Dlaczego takie podejście?

Eliminuje ono błąd modelowania wynikający z nieznanych i zmiennych opóźnień wykonawczych. Predykcja opiera się na tym, co faktycznie wydarzyło się w pojeździe w danym kroku czasowym, a nie na intencji sterowania, która mogła zostać zrealizowana z opóźnieniem lub w zmienionej postaci.

5) O przyszłej fuzji

W kolejnych etapach projektu planuję dołożyć niezależne źródło informacji o położeniu z wizji (mapa wizualna / SLAM, w tym wariant monokularny). Po określeniu niepewności lokalizacji pochodzącej z mapy obrazów możliwe będzie połączenie „trajektorii kinematycznej” (z enkoderów) z „trajektorią wizualną” metodą bayesowską, z ważeniem obu źródeł zgodnie z ich wiarygodnością.

Na obecnym etapie wystarczające jest podkreślenie, że krok predict już uwzględnia niepewność pomiarów $V$ i $\delta$. Dodatkowe czujniki będą w przyszłości wprowadzane jako niezależne obserwacje tego samego stanu, realizowane w kroku korekcji filtru.

6) Przejazd 1

W celu wykonania obliczeń testujących, przyjmuje następujące wartości początkowe odchylenia standardowego położenia i orientacji: współrzędna x: $\sigma_x=0.05$ [m], współrzędna y: $\sigma_y=0.05$ [m], współrzędna $\theta$: $\sigma_\theta=1^\circ$. Błędy sterowania: prędkośc $v$: $\sigma_v = 5$ [obr/min], kąt skrętu kół $\delta$: $\sigma_\delta=1^\circ$. Wykres otrzymanego wyniku szcowania wartości błędu wzdłóż trajektorii przedstawia rys:

Krzyżykami zaznaczam wybrane punkty trajektorii, w których prezentuję wynik estymaty stanu. Błędy pozycji liczone są w lokalnym układzie pojazdu (w punkcie środkowym), a ich rozrzut w kierunku wzdłużnym i poprzecznym do osi pojazdu przedstawia elipsa 3‑sigma. Niepewność orientacji (kursu) ilustruje czerwona strzałka: im dłuższa i „grubsza”, tym większy błąd kąta. Na wykresie widać, że błąd z czasem rośnie, ale robi to powoli i w przewidywalny sposób. Nie „rozjeżdża się” szybko. Dzięki temu nasza wyznaczona trajektoria pozostaje użyteczna przez dłuższy czas, nawet bez dodatkowych poprawek z innych czujników.

7) Przejazd 2

Rysunek przedstawia wyniki szacowania błędów pozycjonowania i kursu dla przejazdu nr 2.

Analiza potwierdza wcześniejsze wnioski: niepewność rośnie wzdłuż trasy stopniowo, a wartości pozostają umiarkowane w badanych warunkach prędkości i kątów skrętu.

8) Wnioski

• Kinematyka 4WS (Ackermann‑predict oparta na pomiarach (V, δ) pozwala stabilnie wyznaczać trajektorię bez dodatkowych czujników.
• Niepewność położenia i kursu narasta w czasie stopniowo; przy małych prędkościach i umiarkowanych kątach skrętu pozostaje niewielka.
• Elipsy 3‑sigma i wektory błędu kąta potwierdzają przewidywalny kierunek wzrostu niepewności: bardziej wzdłuż kierunku jazdy i w orientacji.
• Oparcie predykcji na pomiarach (a nie na komendach sterujących) eliminuje problem zmiennych opóźnień aktuatorów i poprawia spójność estymacji.
• Dla dłuższych odcinków przydatna będzie dodatkowa korekcja z niezależnego źródła (np. wizja/SLAM), łączona bayesowsko z odometrią, aby ograniczyć kumulację błędu.

Cele dydaktyczne

• Zrozumieć, jak propagować niepewność stanu w kinematyce 4WS (Ackermann‑predict) za pomocą liniaryzacji i macierzy kowariancji.

• Wyprowadzić i zastosować macierze $F_k$, $G_k$ do kroku predykcji oraz poprawnie zdefiniować $Q_k$ dla pomiarów V i δ.

• Ocenić wpływ szumów procesu/pomiaru na wzrost błędu pozycji i orientacji w czasie (elipsy 3‑sigma, wektor błędu kąta).

• Porównać predykcję opartą na komendach sterujących vs na pomiarach i uzasadnić wybór „predict-from-measurements”.

• Interpretować wyniki na trajektorii (kierunki największej niepewności) i wskazać, kiedy potrzebna jest korekcja z dodatkowych czujników (np. SLAM).

Co dalej

W kolejnym wpisie omówie propozycje ćwiczeń dydaktycznych bazujących na przedstawionym dotychczas materiale

1) Surowe dane pomiarowe

Zmienne sterujące i obserwowane zapisuję do pliku „out.txt” w formacie NDJSON (Newline‑Delimited JSON). Każda linia to niezależny obiekt JSON, co umożliwia strumieniowe przetwarzanie bez ładowania całego pliku do pamięci. Dzięki temu łatwo scalać różne strumienie logów oraz wykonywać filtrację i synchronizację w czasie. Dane z napędów i serw rejestruję co 10 ms.

Poniżej fragment surowego logu:

{"type": "MOTOR", "time": "44.535256928", "data": {"T": 20010, "id": 1, "typ": 400, "spd": 222, "crt": 351, "act": 1, "tep": 19, "err": 0}}
{"type": "SERVO", "time": "44.535667104", "data": {"setAngle": "14.557342529296875", "servo_1": "12.74", "servo_2": "-12.83", "feedback_type": "FBK"}}
{"type": "MOTOR", "time": "44.544571104", "data": {"T": 20010, "id": 4, "typ": 400, "spd": 215, "crt": 249, "act": 1, "tep": 18, "err": 0}}
{"type": "MOTOR", "time": "44.556052736", "data": {"T": 20010, "id": 3, "typ": 400, "spd": -290, "crt": -651, "act": 1, "tep": 19, "err": 0}}
{"type": "MOTOR", "time": "44.56589152", "data": {"T": 20010, "id": 2, "typ": 400, "spd": -291, "crt": -717, "act": 1, "tep": 19, "err": 0}}
{"type": "SERVO", "time": "44.574356032", "data": {"setAngle": "14.557342529296875", "servo_1": "13.45", "servo_2": "-13.54", "feedback_type": "FBK"}}
{"type": "MOTOR", "time": "44.57628272", "data": {"T": 20010, "id": 1, "typ": 400, "spd": 213, "crt": 266, "act": 1, "tep": 19, "err": 0}}
{"type": "MOTOR", "time": "44.586096576", "data": {"T": 20010, "id": 4, "typ": 400, "spd": 217, "crt": 235, "act": 1, "tep": 18, "err": 0}}
{"type": "MOTOR", "time": "44.596907488", "data": {"T": 20010, "id": 3, "typ": 400, "spd": -274, "crt": -522, "act": 1, "tep": 19, "err": 0}}
{"type": "MOTOR", "time": "44.606897056", "data": {"T": 20010, "id": 2, "typ": 400, "spd": -271, "crt": -708, "act": 1, "tep": 19, "err": 0}}
{"type": "SERVO", "time": "44.614625568", "data": {"setAngle": "14.557342529296875", "servo_1": "14.15", "servo_2": "-14.15", "feedback_type": "FBK"}}

2) Struktura i znaczenie pól

Każdy rekord zawiera typ, znacznik czasu i sekcję danych. Pole type określa źródło pomiaru: „MOTOR” dla napędów kół (DDSM400) oraz „SERVO” dla układu skrętu (ST3215). Pole time to znacznik czasu w sekundach zapisany jako tekst; w analizie rzutuję go na liczbę zmiennoprzecinkową i używam do synchronizacji. Pole data zawiera właściwe wartości telemetryczne, których zestaw zależy od typu rekordu.

MOTOR: identyfikator napędu (id), tryb/typ ramki (typ), prędkość obrotowa (spd), prąd (crt), temperatura (tep), flaga aktywności (act), kod błędu (err).

SERVO: zadany kąt (setAngle) oraz dwa kanały sprzężenia zwrotnego z enkoderów (servo_1, servo_2), plus typ ramki (feedback_type).

W feedback_type rozróżniam:

• ACK — potwierdzenie przyjęcia komendy (po ustawieniu pozycji/prędkości/konfiguracji),

• FBK — ramka telemetryczna ze sprzężenia zwrotnego (bieżący stan serwa).

To właśnie FBK wykorzystuję w postprocessingu do wyznaczania kąta skrętu δ (konwersja ze stopni na radiany i ewentualna fuzja kanałów servo_1/servo_2).

3) Parsowanie i synchronizacja

Plik przetwarzam strumieniowo. Każdą linię parsuję jako JSON i — wg pola type — kieruję do dekodera MOTOR lub SERVO. Następnie:

• normalizuję jednostki (kąty: deg→rad; prędkości: na SI),

• łączę strumienie po czasie, otrzymując spójną serię do obliczeń trajektorii,

• w razie potrzeby filtruję anomalie (np. rozbieżności servo_1 vs servo_2), wygładzam szum i wykonuję interpolację,

• resampluję do interwału 20 ms (50 Hz), aby ujednolicić krok obliczeń.

4) Jednostki i wagi według dokumentacji. Obróbka danych

Skale wynikają z dokumentacji:

• DDSM400: prędkość spd w dziesiątych części rpm (0.1 rpm), prąd crt w mA.

• ST3215: kąty w stopniach.

Konwersje do SI:

• prędkość liniowa koła: $v = 2\pi R_w \cdot \mathrm{spd}/60$ (po uwzględnieniu skali 0.1 rpm),

• kąt skrętu: $\delta = \mathrm{deg}\cdot \pi/180$.

Model obliczeniowy wymaga jednej pary zmiennych sterujących {prędkość, kąt skrętu}. W logach mam cztery prędkości silników i dwa kąty skrętu. W tej fazie przyjmuję uśrednianie:

• prędkość pojazdu jako średnia z prędkości kół (ze znakiem i jednostkami po konwersji),

• kąt skrętu jako średnia z kanałów FBK (servo_1, servo_2) po konwersji do rad.

Tak przygotowany sygnał zasila model 4WS do wyznaczania trajektorii środka geometrycznego w globalnym układzie.

5) Przejazd 1

Pierwszy rysunek pokazuje wykresy surowych serii: prędkości i prądy silników oraz kąty skrętu serw — bez ingerencji, w jednostkach z plików.

Drugi rysunek przedstawia uśrednione wartości prędkości pojazdu i kątów skrętu wyrażone w jednostkach SI.

Trzeci rysunek to uzyskana trajektoria w globalnym układzie współrzędnych. W chwili początkowej pojazd znajdował się w punkcie [0, 0]. Po wykonaniu pętli wrócił w przybliżeniu w to samo miejsce.

Ponieważ trajektorię wyznaczam na podstawie modelu, ta metoda obliczeniowa daje dodatkową możliwość: można symulacyjnie sprawdzać wrażliwość estymacji położenia na zmianę parametrów. Łatwo pokazać, że modyfikacje rozstawu osi $L$ lub promienia koła $R_w$ istotnie wpływają na wynik integracji. W praktyce jednak oba te parametry są dobrze określone w projekcie (geometria) i w danych technicznych napędów, więc ich niepewność jest niewielka. Znacznie ważniejszy jest inny czynnik: ewentualny offset kąta skrętu lub luz w układzie sterowania. Nawet niewielkie przesunięcie nastawy o stałą wartość potrafi systematycznie „skrzywić” trajektorię. Obliczeniowo można wykazać, że już dodanie stałego offsetu $+0.5^\circ$ do kąta $\delta$ prowadzi do zauważalnego odchylenia toru — błąd narasta z długością przejazdu, ponieważ każdorazowo integrujemy nieco inną krzywiznę $\kappa = \tfrac{2}{L}\tan(\delta)$. W praktyce:

• Warto wprowadzić przejazdy przez punkty kontrolne o znanej pozycji (markery na macie). Porównanie pozycji z odometrii z pozycją referencyjną pozwala korygować offset kąta skrętu i oceniać dryft.
• Dobrym kierunkiem jest dodanie prostych czujników pozycjonowania, np. tagów RFID (Radio Frequency Identification) w wybranych punktach trasy (bramy kontrolne). Odczyt RFID daje „twardą” korektę położenia w konkretnych miejscach i ogranicza kumulację błędu odometrii.

6) Przejazd 2

Drugi przejazd był dłuższy — pojazd wykonał dodatkową pętlę w drugim pomieszczeniu. Jak poprzednio, wszystkie sygnały rejestrowałem. Poniżej surowe serie w jednostkach z plików.

Uśrednione wartości w jednostkach SI:

Trajektoria w układzie globalnym. Start z [0, 0]; po pętli powrót w pobliże punktu startu.

7) Wnioski

Podczas jazd pojazd poruszał się z minimalną prędkością około 0.15 m/s. Kąty skrętu były umiarkowane (±15°). W tych warunkach poślizgi boczne w łukach były niewielkie. Dzięki brakowi luzów w układzie kierowniczym pojazd poruszał się po płaszczyźnie drogi z wysoką powtarzalnością. Enkodery o rozdzielczości 4096 imp/obr zapewniły dokładne odczyty prędkości i kątów. Testy potwierdziły, że zakładane efekty modelowania i postprocessingu zostały osiągnięte.

Przeprowadziłem również dłuższe jazdy. W sesjach do 30 minut. W tych przejazdach, mikrokomputer Jetson ani razu się nie zawiesił podczas rejestracji danych, co potwierdza poprawność projektu pod względem: komunikacji i zasilania.

Dodatkowo potwierdziłem wrażliwość trajektorii na stały offset kąta skrętu: nawet +0.5° systematycznie zniekształca tor. Dlatego kluczowa jest kalibracja zer serw i okresowa kontrola luzów.

Cele dydaktyczne

• Od surowych logów do wglądu: nauczysz się zamieniać strumień NDJSON (MOTOR/SERVO) w czyste, zsynchronizowane dane w SI, gotowe do obliczeń.

• Ruch „widoczny” w liczbach: zbudujesz sygnały $V_k$ i $δ_k$ z pomiarów, sprawdzisz ich jakość (zgodność kanałów, zakresy) i zobaczysz, jak przekładają się na trajektorię.

• Nauczysz się stosować proste reguły kontroli jakości i resamplingu, by Twoje wykresy i wnioski były powtarzalne i wiarygodne.

8) Co dalej

W kolejnym wpisie pokażę rachunek niepewności i propagację błędu dla zastosowanego modelu, a następnie włączę obserwator (filtrację) do bieżącej estymacji trajektorii.

Parametry geometryczne prototypu

• Rozstaw kół: $D = 153\,\mathrm{mm}$
• Rozstaw osi: $L = 260\,\mathrm{mm}$
• Zakres skrętu mechaniczny: 0° - 90°, ograniczenie programowe: $35^\circ$
• Promień koła: $R_w = 37.25\,\mathrm{mm}$

1) Koncepcja

W tym poście, do opisu ruchu pojazdu zastosuję prosty model Ackermanna w ujęciu „rowerowym” (bicycle model dla konfiguracji 2WS przednia para kół skrętna), a następnie rozszerzę go do mojego przypadku 4WS (przód + tył skrętne, przeciwfazowo).

W modelu 2WS skrętna jest wyłącznie oś przednia, a oś tylna jest toczna. W idealizacji Ackermanna dopuszczamy różne kąty skrętu kół wewnętrznego i zewnętrznego na osi przedniej tak, aby przedłużenia ich płaszczyzn przecinały się w jednym punkcie ICR (środek obrotu — Instantaneous Center of Rotation). Dzięki temu ruch jest bezpoślizgowy: wszystkie koła poruszają się po współśrodkowych okręgach ze wspólnym środkiem obrotu ICR.

W modelu „rowerowym” zastępuje pary kół kołem ekwiwalentnym. Z przodu i z tyłu umieszczam po jednym „kole zastępczym” w środku osi, a skręt opisuję pojedynczym kątem $\delta$. Ponieważ w modelu 2WS łuk ruchu wyznacza oś tylna, stan pojazdu definiuję w środku osi tylnej i opisuje zmiennymi stanu $[x, y, \Theta]$. Traktuje je jako współrzędne położenia i orientacji pojazdu w globalnym układzie odniesienia. Promień wycinka łuku kołowego do środka ICR oznaczam $R_{\mathrm{ICR}}$.

Ten model pozwoli mi pokazać, że sterowanie pojazdem mogę realizować w lokalnym układzie odniesienia za pomocą pary zmiennych $[v, \delta]$ (prędkość i kąt skrętu), a jego efektem jest zmiana położenia i kursu w układzie globalnym, opisana przez zmienne $[x, y, \Theta]$ (współrzędne położenia x,y punktu środka osi tylnej w globalnym układzie odniesienia XOY oraz kąt zwrotu osi pojazdu względem osi OX). Na tej bazie w prosty sposób rozszerzę opis ruchu pojazdu do przypadku 4WS.

2) Model 2WS. Minimum wzorów do opisania zależności kinematycznych

Stosuję model 2WS „rowerowy”. Przyjmuję następujące założenia:

• Koła tylne są sztywne (oś tylna toczna, bez skrętu).
• Koła przednie są skrętne; dopuszczamy różne kąty kół wewnętrznego i zewnętrznego, w ten sposób że proste prostopadłe do powierzchni bocznych przecinają się w jednym punkcie — środku obrotu pojazdu ICR.

Otrzymuję związek krzywizny z kątem skrętu:

Długość przebytej drogi w kroku czasu $\Delta T$ w ruchu po wycinku łuku kołowego wynosi:

Oznaczenia:

$L$ — rozstaw osi [mm]
$D$ — rozstaw kół (szerokość toru) [mm]
$R_w$ — promień koła [mm]
$\delta$ — kąt skrętu w modelu „rowerowym” (ekwiwalent dla osi przedniej) [rad]
$R_{\mathrm{ICR}}$ — promień do chwilowego środka obrotu (ICR) mierzony od środka osi tylnej [mm]
$v$ — prędkość liniowa środka osi tylnej [mm/s]
$\lambda$ — długość przebytego łuku w kroku $\Delta T$ [mm]
$\Delta \theta$ — przyrost orientacji w kroku $\Delta T$ [rad]
$[x, y, \Theta]$ — zmienne stanu: współrzędne i orientacja w układzie globalnym

Równania ruchu (postać ciągła dla punktu środka osi tylnej, układ globalny) przyjmują postać:

gdzie oznaczono:

$x, y$ — położenie środka osi tylnej w układzie globalnym
$\Theta$ — orientacja pojazdu w układzie globalnym
$v$ — prędkość liniowa wzdłuż osi pojazdu (lokalnie)
$\delta$ — kąt skrętu w modelu „rowerowym” (ekwiwalent dla osi przedniej)
$L$ — rozstaw osi

3) Model geometrii samochodu 4WS — skręt przeciwfazowy

Pojazd obraca się wokół punktu przecięcia promieni skrętu obu osi (Instantaneous Center of Rotation, ICR). Dla skrętu przeciwfazowego (kąty skrętu pary kół przód i tył o tej samej wartości, lecz przeciwnie skierowanej) ICR leży na osi symetrii pojazdu, a promień toru środka pojazdu wynosi:

Długość przebytej drogi w kroku czasu $\Delta T$ w ruchu po wycinku łuku kołowego wynosi:

Równania ruchu (postać ciągła dla środka pojazdu, układ globalny) przyjmują postać:

Przyjmując ruch bez poślizgu (uproszczenie konieczne dla zachowania „czystej” kinematyki), promienie toru kół (względem tego samego ICR) określam następująco:

• wewnętrzny:

• zewnętrzny:

Dla sterowania „prędkością centralną” ($n_c \propto v$) otrzymuję dyferencjał prędkościowy (prędkości kół dobrane do promieni toru, aby unikać poślizgu):

gdzie $R_c$ to promień toru środka pojazdu (tu $R_c = R_{\mathrm{ICR}}$), $n_c$ to prędkość obrotowa kóła zastępczego w punkcie środka pojazdu wynikająca z prędkości v lub po prostu zadana prędkość obrotowa środka pojazdu.

4) Model 4WS — pary kół identycznie skrętne, różnica względem Ackermanna

W praktycznej realizacji mojego pojazdu przyjmuję równoległe ustawienie kół po lewej i prawej stronie tej samej osi (brak geometrii Ackermanna na osi).

To oznacza: kąty skrętu kół lewe/prawe na danej osi są identyczne, proste prostopadłe do płaszczyzn kół na tej osi są równoległe i nie przecinają się w jednym punkcie, nie istnieje jeden wspólny ICR dla wszystkich kół.

Na ukazanym rysunku koła ustawione są równolegle. Pojazd ma dwa “fikcyjne” ICR (chociaż w rzeczywistości może mieć tylko jeden) narzucane przez pary kół wewnętrznych i zewnętrznych. W konsekwencji pojawiają się poślizgi wiertne (lateral scrub) i dodatkowe momenty; rzeczywiste położenie „efektywnego” ICR oraz rozkład prędkości/poślizgów wynikają z sił kontaktowych opona–podłoże. Aby to ująć, potrzebny byłby model dynamiki opon (np. Pacejka/Magic Formula) lub przynajmniej quasi‑statyczny model sił bocznych.

Stosują w moim modelu zależności kinematyki Ackermanna zakładam idealną symetrię pojazdu i jeden wspólny ICR w punkcie wynikającym z idealnej geometrii (zaznaczonym na rysunku). Wtedy wszystkie płaszczyzny kół przecinają się w tym samym punkcie, co oznacza idealną zbieżność w ujęciu kinematycznym. W praktyce (po pierwszych przejazdach na macie, których wyniki opiszę później) można zauważyć efekt “przyduszania prędkości” kół zewnętrznych w skręcie. Wynika to prawdopodobnie z wyższej przyczepności podłoża maty, która ogranicza poślizg kompensujący nieidealną zbieżność. W związku z tym w sterowaniu dyferencjałem planuję zmniejszyć różnicę prędkości po stronach poprzez współczynnik redukcji:

gdzie $n_c$ to prędkość referencyjna (np. prędkość “środkowa”), $R_c$ — promień toru środka pojazdu, a $\kappa \in [0,1)$ jest regulowanym współczynnikiem redukcji różnicy prędkości. Wartość $\kappa$ wyznaczę eksperymentalnie na podstawie przejazdów po okręgu.

5) Obserwacja zmiennych stanu — problem do rozwiązania

Jak pokazałem wcześniej, ruch pojazdu opisany zmiennymi stanu $[x, y, \Theta]$ traktuję jako konsekwencję sterowania parami $[v, \delta]$. Teraz zaglądam „pod maskę” — chcę zobaczyć, co dzieje się, gdy pojawią się błędy i zakłócenia oraz jak wpływają one na proces sterowania (którego celem jest osiągnięcie pożądanego zachowania obiektu mimo zakłóceń).

Rysunek powyżej pokazuje, że szumy i zakłócenia (według miejsca powstawania w łańcuchu sterowania) można rozróżnić następująco:

• sterowania — dodają się do sygnałów sterujących (gaz/hamulec/skręt),
• procesowe — wpływają na sam obiekt (np. efektywny moment napędowy, kąt skrętu),
• pomiarowe — zanieczyszczają wynik pomiaru z czujników.

W moim układzie mogę bezpośrednio nastawiać i mierzyć prędkości obrotowe kół oraz kąty skrętu serw. Natomiast zmienne stanu opisujące ruch (położenie i kurs) nie są mierzalne wprost. Jeśli na nich mi zależy — a to jest właśnie ten przypadek — muszę je wyznaczać pośrednio na podstawie równań modelu. Problemem są tu zakłócenia i szumy. Tę funkcję przejmie obserwator zmiennych stanu.

Mój model, w którym umieszczam obserwator, ma dwie warstwy zmiennych: widzialną i ukrytą.

Warstwa widzialna:

• sterowania zadane $u$ (gaz/hamulec/skręt), obarczone szumem sterowania,
• pomiary $y$ (z czujników), obarczone szumem pomiarowym.

Warstwa ukryta:

• rzeczywisty stan $x$ (położenie, orientacja/kurs, prędkości, ewentualne poślizgi), podlegający niepewnościom: realizacyjnym, procesowym i pomiarowym.

Cel modelowania formułuję następująco: wyznaczyć najlepsze oszacowanie $\hat{x}$ na podstawie $u$ i $y$, mimo błędów modelu i szumów. Jest to problem probabilistyczny; do jego rozwiązania zastosuję prawo propagacji niepewności i filtrację.

Ujęcie równań modelu:

• dynamika:

— szum procesu $w_k$,

• pomiar:

— szum pomiaru $v_k$.

6) Prosta odometria modelu 4WS

Zanim zacznę modelowanie, ustalam sposób sterowania i efekt ruchu oraz wybór modelu. Pojazdem steruję, nastawiając prędkości silników i ustawiając kąty skrętu kół. Efektem jest ruch postępowy albo obrót po łuku wokół chwilowego środka obrotu ICR. Do opisu wystarczy najprostsza kinematyka — model Ackermanna w wariancie 4WS (przeciwfazowo), bez wchodzenia w dynamikę i poślizgi.

Stan pojazdu opisuję wektorem $x = [x, y, \Theta]^{\mathsf T}$, gdzie $x$ i $y$ to współrzędne w globalnym układzie odniesienia, a $\Theta$ to orientacja (kąt zwrotu) nadwozia względem osi OX. Sterowanie zbieram w wektorze $u = [v_{\mathrm{icr}}, \delta]^{\mathsf T}$. W praktyce wygodniej jest sterować prędkością liniową pojazdu $v$ (w środku pojazdu) i kątem skrętu $\delta$, ale w odometrii 4WS można myśleć równoważnie o prędkości „centralnej” związanej z ruchem po okręgu wokół ICR.

Równania aktualizacji stanu otrzymuję przez dyskretyzację równań ruchu. Stosuję prostą dyskretyzację explicite (Euler w przód) z kątem liczonym z poprzedniego kroku, bo w typowym sterowaniu wartości utrzymane są stałe przez cały krok czasu $\Delta T$. Wtedy:

gdzie: $\delta_k$ to zastępczy kąt skrętu w modelu 4WS przeciwfazowego, $\Theta_k$ to orientacja pojazdu w układzie globalnym, $V_k$ — prędkość liniowa (w środku pojazdu), $L$ — rozstaw osi, a $\Delta T$ — krok czasu. Ten schemat stanowi bazę do odometrii: integruję przebyte odcinki i przyrosty orientacji, korzystając z próbkowanych sygnałów sterujących i/lub z estymowanych prędkości. W praktycznej implementacji ograniczam kąt skrętu do dopuszczalnego zakresu, pilnuję wspólnego zegara dla wszystkich sygnałów oraz — gdy to potrzebne — stosuję korekty (np. filtrację) w celu redukcji dryftu wynikającego z szumów i błędów modelu.

Cele dydaktyczne

• Modelowanie do sterowania: zbudować i zrozumieć kinematykę 2WS/4WS (ICR, dΘ/dt, R_ICR), wskazać założenia i ograniczenia, świadomie dobrać parametry i dyferencjał prędkości (z korektą κ).

• Odometria: zaimplementować i zastosować dyskretną odometrię na bazie kinematyki (integracja [x, y, Θ]), ocenić błędy i niepewność (propagacja, podstawowa filtracja).

• Porównanie modeli: wyjaśnić różnice między Ackermannem a geometrią równoległą (scrub, brak wspólnego ICR) oraz dobrać model do zadania i warunków ruchu.

Co dalej

W następnej sekcji opiszę wyniki pierwszych jazd i poddam je krytycznej analizie.