Mając gotowe regulatory prędkości silników mogę przystąpić do modułu estymacji pozycji. W tym wpisie umieszczę garść ogólnych informacji dotyczących pracy estymatora, a także wyprowadzenie modelu matematycznego robota, który będzie potrzebny do wewnętrznych obliczeń.

Moduł estymacji pozycji

Moduł estymacji pozycji ma za zadanie oszacować aktualne położenie robota. Estymacja jest wykonywana na podstawie pomiarów z czujników takich jak IMU i enkodery Pomiary te są obarczone błędem, dlatego rzeczywista pozycja nie jest znana – jedynie czym dysponujemy to oszacowaniem obarczonym pewną niepewnością. Moduł estymacji wykorzystuje równania ruchu robota, aby obliczyć zmianę pozycji robota znając dane z czujników. Obliczenia położenia wykonywane są iteracyjnie bazując na oszacowaniu z poprzedniej chwili czasu. Błędy z poszczególnych iteracji kumulują się.

Do obliczeń potrzebna jest również pozycja początkowa robota, która także obarczona jest pewną niepewnością. Zasady konkurencji micromouse mówią, że robot startuje z określonego pola. Jednak jest tam umieszczany ręcznie przez człowieka, więc może być ustawiony centymetr w bok, albo nie skierowany idealnie prosto.

Jak widać całkiem sporo tych oszacowań, niepewności i błędów. Moduł estymacji potrzebuje równań ruchu robota oraz jakiegoś sposobu radzenia sobie z tymi niepewnościami.

Równania ruchu

Pozycję robota można opisać za pomocą trzech parametrów – współrzędnych kartezjańskich x,y oraz orientacji \alpha . Natomiast zmiana pozycji jest opisana przez prędkość liniową v w kierunku wskazywanym przez orientację, oraz prędkość kątową \omega .

Równania ruchu, czyli wzór na wyznaczenie pozycji w chwili t+1 znając pozycję z chwili t, można zapisać jako:

x(t+1) = x(t) + v(t) \cdot dt \cdot cos \alpha

y(t+1) = y(t) + v(t) \cdot dt \cdot sin \alpha

\alpha(t+1) = \alpha (t) + \omega (t) \cdot dt

Symulacja MATLAB

Do badania algorytmów estymacji pozycji wykorzystam symulację w MATLABie. W najbliższym czasie będę rozwijał ją o kolejne funkcjonalności. Do jej zadań będzie należało:

  1. Generowanie trasy przebytej przez robota na podstawie równań ruchu.
  2. Generowanie odczytów z czujników obarczonych błędem.
  3. Generowanie wyników działania algorytmów estymacji działających na danych z czujników.
  4. Wizualizowanie danych na wykresach.

Symulacji postanowiłem nie dołączać do źródeł projektu, w najbliższym czasie może założę na nią oddzielny projekt na GitHubie. Mając gotowe równania stanu mogę przejść do punktu 1. Wygenerowałem trasę robota dla zadanych wartości prędkości postępowej i kątowej.

 

 

Generowanie danych z czujników

Mając rzeczywistą pozycję robota możemy zająć się symulowaniem danych, jakie będzie odbierał nasz robot. W przypadku żyroskopu sytuacja jest prosta – do prędkości obrotowej wystarczy dodać biały szum. Wykres dla odczytów z żyroskopu znajduje się poniżej. Nie jest to dokładne odwzorowanie błędu żyroskopu, ponieważ charakteryzuje się on dryftem, czyli pewnym offsetem zwiększającym się w czasie. Jednak niedoskonałość modelu nie powinna być problemem.

Z symulacją enkoderów jest trochę więcej zabawy. Odczytem z enkodera jest dystans przejechany w trakcie jednej iteracji przez koło. Koła mają pewien rozstaw osi, więc nie znajdują się w tym samym punkcie co środek robota. Aby obliczyć położenie kół należy znać położenie środka i odległość koła od środka robota (albo szerokość robota L):

x_L(t) = x(t) + \frac{L}{2} \cdot cos(\alpha + 90^{\circ}) 

y_L(t) = y(t) + \frac{L}{2} \cdot sin(\alpha + 90^{\circ}) 

x_R(t) = x(t) + \frac{L}{2} \cdot cos(\alpha - 90^{\circ}) 

y_R(t) = y(t) + \frac{L}{2} \cdot sin(\alpha - 90^{\circ}) 

Znając aktualne i poprzednie położenie koła możemy policzyć dystans pomiędzy tymi punktami:

D = \sqrt{(x(t) - x(t - 1))^2 + (y(t) - y(t - 1))^2}

To jeszcze nie koniec – koło może kręcić się do przodu albo do tyłu. Aby wykryć kierunek obrotu trzeba wiedzieć, czy różnica pomiędzy aktualną i poprzednią pozycją jest dodatnia, czy ujemna. Kierunek jest zależny od orientacji robota:

  • Dla 45^{\circ} < \alpha < 135^{\circ}  robot jedzie do przodu, jeśli y(t) > y(t - 1) .
  • Dla 45^{\circ} < \alpha < 225^{\circ}  robot jedzie do przodu, jeśli x(t-  1) > x(t) .
  • Dla 225^{\circ} < \alpha < 315^{\circ}  robot jedzie do przodu, jeśli y(t - 1) > y(t) .
  • Dla \alpha > 315^{\circ} lub \alpha < 45^{\circ} robot jedzie do przodu, jeśli x(t) > x(t - 1) .

Po wprowadzeniu powyższych równań do symulacji, udało mi się otrzymać  pozycje kół i wartości z enkoderów takie jak na poniższych wykresach.

Filtr Kalmana

Dysponując zaszumionymi pomiarami mogę przejść do algorytmu estymacji radzącego sobie z danymi obarczonymi błędem. Tym algorytmem jest Filtr Kalmana. Jest to algorytm wyznaczający oszacowanie wektora stanu obserwowanego obiektu, które posiada minimalną wariancję. Innymi słowy jest to optymalna estymata stanu dla pomiarów obarczonych błędem w postaci białego szumu. Algorytm ten został stworzony na potrzeby misji Apollo i został tam zastosowany w systemie nawigacji. Od tego czasu znaleziono dla niego zastosowanie w wielu dziedzinach, również bardzo odległych od początkowego zastosowania. Należą do nich odszumianie sygnałów, sztuczna inteligencja, procesy chemiczne, a nawet ekonomia i prognozy pogody. Algorytm ten miał wpływ na rozwój tylu dziedzin, że Rudolf Kalman za jego stworzenie mógłby dostać nagrodę Nobla. O filtrach Kalmana można poczytać w serii artykułów, jakie kiedyś napisałem dla portalu Forbot.pl.

Jest jednak pewien szczegół, który uniemożliwia użycie filtru Kalmana w robocie. Otóż równania ruchu są nieliniowe – zawierają funkcje trygonometryczne. Z pomocą przychodzi nam tutaj EKF (Extended Kalman Filter), czyli wersja filtru dla układów nieliniowych. Polega on na wykonaniu linearyzacji modelu wokół punktu pracy będącego aktualną estymatą stanu i wykonanie algorytmu na zlinearyzowanym modelu.

Model stanowy – podejście pierwsze

Aby mieć co linearyzować, najpierw musimy wyprowadzić nasz model. Wektor stanu będzie zawierał pozycję robota i  ma postać:

\mathbf{x}(t) = [x(t) \> y(t) \> \alpha(t)]^T 

Wektor wejścia:

\mathbf{t}(t) = [v(t) \> \omega(t)]^T 

Równania stanu:

x_1(t + 1) = x_1(t) + dt \cdot u_1(t) \cdot cos x_3

x_2(t + 1) = x_2(t) + dt \cdot u_1(t) \cdot sin x_3

x_3(t + 1) = x_3(t) + dt \cdot u_2(t)

Teraz pora na równania wyjścia i tutaj zaczyna się dopiero zabawa. Odczyty z enkoderów dają prędkość chwilową poszczególnych kół, czego nie da się w prosty sposób wyrazić za pomocą zmiennych stanu. Trzeba by było obliczać pozycję na bazie prędkości i poprzedniego stanu. Stan z kolei byłby uzyskiwany na bazie odczytów z czujników, które od tego stanu zależą. Od razu widać, że coś tu jest nie tak. Model z takimi wewnętrznymi zależnościami nie będzie działał dobrze. Potrzebny jest inny.

Model stanowy – podejście drugie

Problemem było wyrażenie odczytów prędkości za pomocą zmiennych stanu w równaniach wyjścia. Aby rozwiązać ten problem, możemy zrobić z prędkości dodatkowe zmienne stanu, a wektor wejść będzie wtedy zerowy:

\mathbf{x}(t) = [x(t) \> y(t) \> \alpha (t) \> v(t) \> \omega (t)]^T 

Równania stanu przyjmą wtedy postać:

x_1(t + 1) = x_1(t) + dt \cdot x_4(t) \cdot cos x_3

x_2(t + 1) = x_2(t) + dt \cdot x_4(t) \cdot sin x_3

x_3(t + 1) = x_3(t) + dt \cdot x_5(t)

x_4(t + 1) = x_4(t)

x_5(t + 1) = x_5(t)

Zastosowałem tutaj zabieg częsty w takich sytuacjach. Nasz model zakłada, że prędkość postępowa i obrotowa są stałe. Czynnikiem, który w modelu będzie wpływał na ich zmianę jest szum procesowy, którym obarczone są równania stanu.

W tym modelu wektor wyjścia ma postać:

\mathbf{y}(t) = [v_{enc}(t) \> \omega_{enc} (t) \> \omega_{gyro} (t)]^T 

Równania wyjścia możemy wtedy zapisać w prosty sposób:

y_1(t) = x_4(t)

y_2(t) = x_5(t)

y_3(t) = x_5(t)

Tak sformułowany model stanowy nadaje się do linearyzacji i użycia w EKF. Jednak o tym następnym razem.