Dzisiaj zaprezentuję jak wyeliminowałem wpływ dryftu żyroskopu z pomiaru prędkości kątowej wykorzystując Rozszerzony Filtr Kalmana. Będzie miejsce na teoretyczny opis problemu i implementację najpierw w środowisku symulacyjnym, a następnie w robocie Micromouse.

Definicja dryftu

Częstą wadą żyroskopu jest dryft, czyli błąd systematyczny pomiaru polegający na dodaniu do niego pewnej wartości (offsetu). Ten rodzaj błędu często określany jest jako bias. Offset może lekko zmieniać się w czasie np. pod wpływem zmiany temperatury. Wartość jaką otrzymujemy z żyroskopu zawiera więc kilka składowych:

\omega_g(t) = \omega(t) + \omega_b(t) + n(t)

gdzie \omega_g s=2$ to wartość odczytana z żyroskopu, \omega s=2$ to rzeczywista prędkość kątowa, \omega_b s=2$ to wartość dryftu, a n to biały szum.

Metoda usuwania zakłócenia

Filtr Kalmana jest najlepszym możliwym narzędziem (statystycznie optymalnym) do usuwania wpływu białego szumu. Niestety z biasem nie jest w stanie sobie poradzić sam. Najlepszym sposobem eliminacji takich deterministycznych zakłóceń (do tej grupy należy też np. zakłócenie sinusoidalne) jest ich estymacja. W tym celu nasz model procesu w przestrzeni stanu musimy rozszerzyć o stan zakłócenia. Dzięki temu Filtr Kalmana będzie potrafił usuwać zakłócenie.

Dysponując samym pomiarem z żyroskopu nie da się stwierdzić jaka część otrzymanego pomiaru to wartość rzeczywista, a jaka to dryft. Aby móc to określić, potrzebny jest inny, niezależny pomiar tej samej wartości. Może on być dużo bardziej zaszumiony, ale nie powinien być obarczony biasem. Wtedy Filtr Kalmana znając wariancje szumów na pewno sobie poradzi. Dlatego w parze z żyroskopem często korzysta się z akcelerometru, czy magnetometru. Ja natomiast użyję odczytów z enkoderów (w przyszłości może dodam też akcelerometr, czy magnetometr, w końcu mam je na module IMU).

Rozszerzanie modelu robota

Wiemy już co mamy robić. Zaczniemy więc od rozszerzenia modelu stanowego o bias (wyprowadzenie modelu dostępne tutaj i tutaj):

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

Musimy również uaktualnić macierze A i C. Macierz A zmieni rozmiar na 6×6 i dojdzie kolejna 1 na głównej przekątnej. Macierz C natomiast przyjmie postać:

C = \begin{bmatrix} 0 & 0 & 0 & dt & 0 & 0 \\ 0 & 0 & 0 & 0 & dt & 0 \\ 0 & 0 & 0 & 0 & 1 & 1 \end{bmatrix}

Należy również uaktualnić macierz kowariancji szumu procesowego V. Na głównej przekątnej  dojdzie więc wariancja biasu.

Testy w środowisku symulacyjnym

Teraz pora sprawdzić, jak rozszerzony model sprawuje się w symulacji. Mogę tutaj poprawić ewentualne błędy i dostroić poszczególne wariancje i wartości początkowe. Kod symulacji w MATLAB dostępny jest na GitHubie. Od wersji użytej we wpisie o EKF zrobiłem kilka fixów. Przede wszystkim odwzorowałem kierunek obrotu żyroskopu oraz obrotu na bazie enkoderów tak jak w prawdziwym robocie. Poza tym poprawiłem buga z przeliczaniem prędkości kątowej z enkoderów – nie było skalowania przez dt ani dla pomiaru ani dla wariancji i przede wszystkim nie było konwersji z radianów na stopnie.

Jak wspomniałem wcześniej, bias może powoli zmieniać się w czasie, dlatego jego wariancja powinna więc być dosyć mała. Za to początkowe oszacowanie biasu (wektor x0) może znacząco odbiegać od rzeczywistości. Dlatego wariancja oszacowania początkowego (macierz P0) powinna być stosunkowo duża. Jeżeli jednak ustawimy w P0 zbyt dużą wartość, KF zignoruje oszacowanie początkowe i będzie bazował jedynie na pomiarach.

Ostateczne odchylenie standardowe biasu ustawiłem na 0.5, a wartość w P0 na 40. Rzeczywisty bias na 2.9, a poczatkowe oszacowanie w x0 na 5. Wyniki są bardzo ładne:

Oszacowanie pozycji w dalszym ciągu z czasem coraz bardziej się rozjeżdża, ale w stosunku do wcześniejszych wykresów jest znacząca poprawa.

Testy na robocie

Po udanych symulacjach przyszła pora na implementację w rzeczywistym układzie. Kod robota również musiałem nieco dostosować. W bibliotece do macierzy maksymalny rozmiar musiałem zmienić na 6×6. Poza tym trzeba było dostosować poszczególne zmienne do obliczania EKF do nowych rozmiarów macierzy. Udało się nanieść te zmiany dosyć szybko i zebrać dane pomiarowe.

Wartość początkową dryftu ustawiłem na 0.9° będącą bliską do rzeczywistych wartości, które wcześniej zmierzyłem. Wynik działania EKF z usuwaniem dryftu jest satysfakcjonujący. Przede wszystkim oszacowanie kąta orientacji nie odpływa od razu po włączeniu. Wcześniej od momentu włączenia zasilania do uruchomienia logowania i startu robota (czyli jakieś 15 sekund), oszacowanie kąta mogło wskazywać już nawet 20°. Przy estymacji dryftu ten efekt praktycznie nie występuje.

Podsumowanie

Osiągnięte wyniki działania EKF na tą chwilę są dla mnie satysfakcjonujące. Możliwe są jeszcze poprawki, ale aktualnie szkoda mi na nie czasu. Mając ładne oszacowania stanu robota mogę wykorzystać te wartości w innych modułach np. prędkość kątową z EKF można podać na regulatory silników zamiast wartości z żyroskopu. Teraz mogę zająć się czujnikami ścian. Jak już je skalibruję, będę mógł wrócić do EKF, aby informacje o ścianach również wpływały na oszacowanie pozycji.