Jak rośnie błąd trajektorii? Ackermann‑predict w praktyce

Rachunek niepewności i propagacja błędu w modelu ruchu (Ackermann‑predict)

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:

\[\begin{aligned} x_k &= x_{k-1} + V_k\,\Delta T \cos \Theta_{k-1} \end{aligned}\] \[\begin{aligned} y_k &= y_{k-1} + V_k\,\Delta T \sin \Theta_{k-1} \end{aligned}\] \[\begin{aligned} \Theta_k &= \Theta_{k-1} + \frac{2 V_k\,\Delta T}{L}\,\tan \delta_k \end{aligned}\]

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:

\[\begin{aligned} P_0 = \mathrm{diag}\!\left( (\sigma_x)^2,\, (\sigma_y)^2,\, (\sigma_\Theta)^2 \right) \end{aligned}\]

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

\[\begin{aligned} Q_0 = \mathrm{diag}\!\left( (\sigma_V)^2,\, (\sigma_\delta)^2 \right) \end{aligned}\]

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:

\[\begin{aligned} x_k \approx f(x^-,\,u^-) + F_k\,(x_{k-1}-x^-) + G_k\,(u_k-u^-) \end{aligned}\]

gdzie wektor stanu ma postać:

\[\begin{aligned} x_k = \begin{bmatrix} x_k & y_k & \Theta_k \end{bmatrix}^\top \end{aligned}\]

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)$:

\[\begin{aligned} F_k = \left. \frac{\partial f}{\partial x} \right|_{k-1} \end{aligned}\]

co dla rozważanego modelu prowadzi do postaci:

\[\begin{aligned} F_k = \begin{bmatrix} 1 & 0 & -V_k\,\Delta T\,\sin \Theta_{k-1} \\ 0 & 1 & \ \,V_k\,\Delta T\,\cos \Theta_{k-1} \\ 0 & 0 & 1 \end{bmatrix} \end{aligned}\]

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

\[\begin{aligned} G_k = \begin{bmatrix} \Delta T \cos \Theta_{k-1} & 0 \\ \Delta T \sin \Theta_{k-1} & 0 \\ \dfrac{2\,\Delta T}{L}\,\tan \delta_k & \dfrac{2\,V_k\,\Delta T}{L}\,\sec^2 \delta_k \end{bmatrix} \end{aligned}\]

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ć:

\[\begin{aligned} \hat{x}_k^- &= f\!\left(\hat{x}_{k-1},\, z_k\right) \end{aligned}\] \[\begin{aligned} P_k^- &= F_k\,P_{k-1}\,F_k^\top + G_k\,Q_k\,G_k^\top \end{aligned}\]

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