W dzisiejszym wpisie przedstawię implementację Rozszerzonego Filtru Kalmana dla robota Micromouse. Aby dojść do tego momentu musiałem najpierw wyznaczyć model ruchu robota oraz sprawdzić działanie EKF w środowisku symulacyjnym. Można o tym poczytać we wcześniejszych wpisach.

Biblioteka do obliczeń na macierzach

Aby zaimplementować Filtr Kalmana na mikrokontrolerze, niezbędne będzie wykonanie pewnych obliczeń na macierzach. Mogłem porwać się na samodzielną implementację takiej biblioteki, ale ostatecznie zdecydowałem, że skorzystam z gotowego rozwiązania. Zanim zacząłem przeszukiwać odmęty GitHuba postanowiłem zebrać wymagania, jakie stawiam poszukiwanej bibliotece:

  • implementacja w języku C,
  • nie wykorzystuje dynamicznej alokacji pamięci,
  • operuje na liczbach float,
  • dodawanie i odejmowanie dwóch macierzy,
  • mnożenie dwóch macierzy,
  • transpozycja,
  • wyznaczanie macierzy odwrotnej.

Niestety nie udało mi się znaleźć żadnej bibliteki, która by spełniała wszystkie te warunki. Ostatecznie wybrałem bibliotekę libfixmatrix działającą na liczbach stałoprzecinkowych 16,16. Wymagała więc ona pewnych modyfikacji.

Dostosowanie biblioteki

Biblioteka libfixmatrix poza obliczeniami na macierzach implementuje również arytmetykę na wektorach 2d/3d i kwaternionach. Ja potrzebuję tylko obliczeń macierzowych, więc usunąłem niepotrzebne pliki fixquat i fixvector. Zostawiłem jedynie fixmatrix zawierający obliczenia macierzowe oraz fixarray zawierający ogólne operacje na wektorach takie jak normalizacja, czy iloczyn skalarny. Zostawiłem również w okrojonej formie moduł fixstring służący do printowania struktur.

Kolejną przeszkodą była implementacja na liczbach stałoprzecinkowych. Biblioteka libfixmatrix wykorzystuje w tym celu implementację liczb 16,16 z biblioteki libfixmath. Ja zamiast tego chciałem wykorzystać liczby typu float. Zdefiniowałem więc swój własny nagłówek fix16.h definiujący typ fix16_t jako float oraz definiujący podstawowe działania arytmetyczne, a także funkcje trygonometryczne:

#ifndef _FIX16_H_
#define _FIX16_H_

#include <stdint.h>
#include <math.h>

#define fix16_one   1

#define fix16_add(a, b) ((a) + (b))
#define fix16_sub(a, b) ((a) - (b))
#define fix16_mul(a, b) ((a) * (b))
#define fix16_div(a, b) ((a) / (b))

#define fix16_sqrt(a)   sqrtf(a)
#define fix16_sin(a)    sinf(a)
#define fix16_cos(a)    cosf(a)

typedef float fix16_t;

#endif /* _FIX16_H_ */

Następnym krokiem było dostosowanie samych funkcji biblioteki pod floaty. Sprowadzało się to głównie do usunięcia obsługi FIXMATRIX_OVERFLOW czyli wykrycia przekroczenia rozmiaru zmiennej fixed point. We floatach ten problem nas nie dotyczy.

Po modyfikacji biblioteki stworzyłem build testowy sprawdzający poprawność poszczególnych operacji macierzowych. Pozwolił on wykryć kilka dodatkowych miejsc, które musiałem jeszcze pozmieniać. Ostatecznie dostałem gotową bibliotekę na liczbach float. Ostateczne wersje pliku można znaleźć na GitHubie w projekcie micromouse’a.

Implementacja EKF na robocie

Mając działającą bibliotekę mogłem przejść do implementacji na robocie. Należało więc zamienić równania EKF na odpowiednie funkcje z biblioteki i zobaczyć, czy działa.

Jak łatwo się domyślić – na począktu nie działała. Musiałem przeanalizować działania macierzowe krok po kroku, aby wykryć błędy w implementacji równań EKF. Oczywiście okazało się, że przy przepisywaniu pojawiło się pełno literówek. A to nie takie macierze przekazane jako argumenty funkcji, a to funkcja dodawania zamiast mnożenia. W końcu jednak udało się uzyskać oszacowanie z grubsza przypominające rzeczywistą przebytą drogę.

Kiedy implementacja EKF już była w porządku wynikły z kolei wcześniej niezauważone błędy związane z interpretacją danych pomiarowych. Po pierwsze prędkość kątowa zwracana przez żyroskop rosła w kierunku odwrotnym niż kąt \alpha . Orientację żyroskopu można łatwo sprawdzić patrząc na płytkę IMU.

Kolejnym problemem była źle wyskalowana przebyta odległość. Obliczana estymata różniła się od rzeczywistej drogi mniej więcej dwukrotnie. Oznacza to, że gdzieś we wzorach przeliczania ticków enkodera na odległość w milimetrach brakuje mnożenia przez dwa. I faktycznie enkoder na silniku daje 16 ticków na obrót na dwóch kanałach (A i B), a timer zwiększa wartość zarówno na zboczach rosnących, jak i opadających. Daje to w sumie 64 ticki na obrót, a w kodzie miałem początkowo 32.

Trzeci błąd dotyczył wyznaczania prędkości kątowej na podstawie odczytów enkoderów. Obliczona w ten sposób prędkość kątowa jest wyrażona w radianach. Trzeba ją więc przekonwertować na stopnie.

Po poprawieniu powyższych błędów w końcu udało się otrzymać oszacowanie przebytej drogi z grubsza odpowiadające rzeczywistości. Poniżej filmik z testowego przejazdu robota oraz przebyta droga na podstawie EKF.

 

Dalsze kroki

Największym problemem w działaniu oszacowania pozycji aktualnie jest zmiana początkowej orientacji pomimo, iż robot stoi w miejscu. Przez to zanim uruchomię testowy przejazd, kąt zdąży zmienić wartość z zera na np. 20°. Możliwe przyczyny to bias żyroskopu albo stany przejściowe przy włączaniu układu.