Zaloguj
Systemy HMD szybko stały się standardem i nadal umożliwiają pilotowi podglądanie informacji na ekranach wbudowanych w hełm lotniczy, a oprócz tego dają też możliwość sterowania wybranymi funkcjami maszyny. Znamy systemy sterowania karabinem podczepionym do śmigłowca Apache za pomocą ruchów głowy pilota, gdzie cel jest namierzany bez konieczności odrywania uwagi od pilotażu maszyny.
Kolejnym etapem rozwoju tej technologii jest świat "Artificial Reality" (AR), który możemy obserwować poprzez specjalne okulary, jak np. Oculus Rift, lub na ekranach telefonów komórkowych. Użytkownicy tych urządzeń wiedzą, iż efekt jest uzyskiwany dzięki wykorzystaniu pozycjonowania i żyroskopu, który mierzy kąty pochylenia w danej pozycji. Niewielu z nich jednak wie jak to w istocie działa.
Dzięki miniaturyzacji i stosunkowo niskiej cenie technologii MEMS możemy zbudować podobny system we własnym warsztacie. Rekomendacje: urządzenie przyda się nie tylko do modelu, ale również do innych zastosowań, takich jak: monitoring obiektu, robotyka, urządzenia dla osób niepełnosprawnych i innych.
Filtr Kalmana
Wyobraźmy sobie, że stoimy na ulicy i trzymamy w ręku GPS. Mimo że nasza pozycja jest niezmienna, to odczyty w każdej sekundzie są różne, ale mieszczą się w pewnym obszarze (np. w okręgu o promieniu 5 metrów). Chcielibyśmy "uśrednić" odczyty z GPS w taki sposób, aby uzyskać właściwą pozycję.
Aby tego dokonać musimy na podstawie wskazań z GPS przewidzieć faktyczne miejsce postoju. Dokładnie na tym polega filtrowanie Kalmana - na estymowaniu najbardziej prawdopodobnego (z minimalną wariancją) stanu układu (wyniku pomiaru). Szukamy estymacji pozycji 
|
gdzie:
- xk oznacza stan układu w chwili k, który jest zależny od poprzednio zarejestrowanego stanu, sygnału sterującego uk oraz szumu przetwarzania wk.
- A jest macierzą przejść (systemu),
- B jest macierzą sterowania (wejścia).
Drugie równanie przedstawia obserwowany pomiar zk jako zależność pomiędzy faktycznym stanem systemu xk i wektorem szumu pomiarowego vk. Teoretycznie, faktyczny stan systemu xk nie jest znany w żadnym momencie. Możemy go jedynie obserwować poprzez pomiary zk, które są niedokładne.
H jest macierzą wyjścia, wykorzystywaną w wypadku zamiany (mapowania) wyliczanych wartości na ich odpowiedniki w układzie obserwowanym (realnym). Przyjmuje się, że szumy wk i vk są o rozkładzie normalnym, o kowariancjach Qk i Rk. Zakładamy także liniową zależność pojawiających się po sobie pomiarów. Algorytm iteracyjnie wylicza estymatę stanu w chwilina podstawie wartości w chwili k-1, co jest charakterystyczne dla modelu dyskretnego.
Filtrowanie Kalmana przebiega dwuetapowo. Pierwszym etapem jest faza predykcji (aktualizacji czasu):
|
W pierwszym równaniu przewidywana jest estymata stanu 
|
W tej fazie obliczane jest wzmocnienie Kalmana Kk. Następnie, na jego podstawie dokonuje się aktualizacji estymaty stanu i kowariancji predykcji, które zostaną użyte w kolejnej iteracji algorytmu. Rk jest to estymacja kowariancji szumów wzmocnienia K.
Na pierwszy rzut oka teoria wydaje się bardzo zawiła, jednak staje się zrozumiała, gdy wykorzystamy ją w praktycznych doświadczeniach. W najprostszym wypadku, jeśli chcemy filtrować pojedynczą skalarną wartość pomiaru (np. czas kanału PPM) wzory ulegają znacznemu uproszczeniu. Macierz przejść A staje się liczbą 1, pomijamy sterowanie układem, a zatem uk=0. Faza predykcji przybiera postać:
|
Macierz wyjścia H również przyjmuje wartość 1, ponieważ interpretacja pomiarów (czasy kanału PPM) jest jednakowa w modelu matematycznym, jak i w obserwacjach.
Pozostały nam tylko 2 zmienne skalarne Q i R, za których pomocą możemy w łatwy filtrowania. Poprzez odpowiednie dobranie parametrów nasz filtr będzie w mniejszym reagował na szumy pomiarowe. Implementacja powyższego filtru zawiera się w kilku liniach kodu:
|
double filtrKalmana(double zk, double *xk, double *Pk) {
//double xk_1 = *xk;
//double Pk_1 = *Pk;
//Faza predykcji
//*Xk = Xk_1;
*Pk = *Pk + (double)Q;
//Faza korekcji double Kk = *Pk / (*Pk + (double)R);
*xk = *xk + Kk*(zk - *xk);
*Pk = (1 - Kk)*(*Pk);
//Zwrot nowej estymaty return *xk;
}
Sposób wykorzystania w programie jest następujący:
estymata = filtrKalmana(pomiar, &xk, &Pk);
Funkcja zwraca nową estymatę stanu. W parametrach przekazujemy aktualny pomiar, poprzednią estymatę xk i kowariancję Pk. Jako że xk i Pk biorą udział w następujących po sobie iteracjach, ich wartości muszą być zapamiętywane w każdym wywołaniu funkcji, stąd przekazanie przez wskaźnik.
Liczby Q i R (kowariancje szumów) zostały zdefiniowano poza ciałem funkcji. Tym sposobem za pomocą niewielkiego kodu i 2 zmiennych dostajemy do dyspozycji jedno z najlepszych narzędzi do "wygładzania" wszelkich przebiegów linowych. Z powodzeniem możemy nim zastąpić powszechnie używaną metodę uśredniania wyników.
Wracając do headtrackera - poprzez dodatkowe filtrowanie czasów kanałów PPM, uzyskujemy efekt płynnych ruchów kamery, co ma wpływ na dłuższą żywotność serwomechanizmów oraz może się także okazać przydatne przy filmowaniu z powietrza.
Rysunek 14. Filtrowanie odczytów z akcelerometru, Kalman vs Średnia
Na rysunku 14 pokazano przykładową serię szybko zmieniających się pomiarów z akcelerometru. Czerwona linia to efekt działania filtru Kalmana, natomiast kolorem czarnym pokazano średnią ruchomą rzędu 20. Jak widać czasy reakcji na zmiany pomiarów, a także estymaty stanów zostały poprawione.
Teoria Kalmana ma znacznie większe zastosowania, począwszy od powyższych, poprzez systemy telemetryczne w Nascar czy Formule 1, po systemy sterowania w przestrzeni kosmicznej. W Internecie dostępnych jest wiele publikacji opisujących różne zastosowania filtru.
Wśród nich jest także rozwiązanie, które możemy wykorzystać zamiast filtru komplementarnego. Opisane zostało na stronach [1] oraz [2] w wymienionych poniżej w źródłach. Zaproponowane algorytmy z powodzeniem zastosowałem także w projekcie headtrackera z równie dobrym rezultatem.
Zawierają one w sobie element sterowania układem uk, który eliminując ruchy liniowe, wpływa na lepszą stabilność heatrackera. Można to porównać do przedstawionego wyżej filtrowania niskich prędkości obrotowych.
Generowanie ramki PPM
Zgodnie z opisem ramki PPM przedstawionym na początku, zakodowane kanały posiadają określoną długość, która pośrednio reprezentuje kąt obrotu kamery w samolocie. Pozostaje zamienić kąty gyroAngleX oraz gyroAngleZ na czasy wyrażone w ms:
int calcLenPPM(double gyroAngle, double scale) {
//Nowy czas kanału PPM int len = CH_LEN_ CNT + (gyroAngle*scale/ SERVO_MAX_ANLGE)*CH_LEN_CHG;
//Sprawdzenie marginesów
if (len < CH_LEN_MIN) return CH_LEN_MIN;
else if (len > CH_LEN_MAX) return CH_LEN_MAX;
else return len;
}
gdzie:
CH_LEN_CNT - czas kanału dla pozycji centralnej (1200µs),
CH_LEN_CHG - maksymalny czas zmiany długości kanału (512µs),
CH_LEN_MIN - minimalny czas kanału (1200µs-512µs),
CH_LEN_MAX - maksymalny czas kanału (1200µs+512µs),
gyroAngle - kąt gyroAngleX i gyroAngleZ,
SERVO_MAX_ANLGE - dopuszczalny kąt wychylenia serwomechanizmu w stopniach (90°),
scale - współczynnik służący do sterowania czułością headtrackera, im wyższy tym szybszy obrót headtrackera.
Mamy już wszystkie informacje niezbędne do wygenerowania ramki PPM, która zostanie wysłana do nadajnika RC. Generowanie odbywa się w przerwaniu 16-bitowego timera, dzięki czemu jest niezakłócona przebiegiem pętli głównej:
ISR(TIMER1_COMPA_vect) {
static uint16_t ch_len_isr[CH_ NUMBER]; //Czasy poszczególnych kanałów PPM w przerwaniu
static uint8_t ch_num=0; // Numer aktualnie generowanego kanału w przerwaniu
static uint8_t ch_dspl=0; //0 - generacja przerwy pomiędzy kanałami; 1 - generacja kanału static uint16_t ch_fill=0; // Czas pozostały do wypełnienia PPM_LEN
if (ch_dspl==0) {
OCR1A = PPM_DELAY;
if (ch_num==0) {
ch_fill = 0;
//Przepisanie nowych długości kanałów
for (uint8_t i=0; i<CH_NUMBER; i++)
ch_len_isr[i]=ch_len[i];
}
ch_dspl = 1;
TCCR1A |= (1<<COM1A0); //
Następny stan wysoki ch_fill += PPM_DELAY;
}
else {
if (ch_num==CH_NUMBER) {
//Wypełnienie OCR1A = PPM_LEN-ch_fill;
ch_num = 0;
}
else {
//Jeden z kanałów OCR1A = ch_len_isr[ch_num++];
ch_fill += ch_len_isr[ch_num];
}
ch_dspl = 0;
TCCR1A &= ~(1<<COM1A0); //Następny stan niski
}
}
Sygnał PPM generowany jest na pinie procesora OC1A ( PB1), który zmienia stan na przeciwny w każdym przerwaniu. W kodzie przerwania sterujemy rejestrem OCR1A, do którego zapisujemy czas opóźnienia do kolejnego przerwania.
Zmiana stanu odbywa się poprzez sterowanie bitem COM1A0 rejestru TCCR1A, dzięki któremu możliwe jest generowanie ramki o polaryzacji dodatniej lub ujemnej (PPM Positive lub PPM Negative). Za pomocą dodatkowej zmiennej rozpoznajemy czy w aktualnym przerwaniu będzie generowany kanał lub pauza.
Rysunek 15. Algorytm wyznaczania kątów obrotu i sygnału PPM
W każdym przerwaniu zapamiętujemy czasy poszczególnych kanałów po to, by w ostatnim przebiegu odpowiednio wysterować czas wypełnienia ramki (sygnał synchronizacji). Natomiast czasy kanałów są ustawiane w kodzie w ściśle określonym momencie, dzięki czemu nie mogą ulec zmianie w trakcie generowania ramki PPM. Aby dodatkowo upewnić się, że sygnał wysyłany do nadajnika jest spójny w każdym momencie, zmiany długości kanałów dokonujemy w bloku atomowym:
ATOMIC_BLOCK(ATOMIC_FORCEON) {
ch_len[CH_ROLL] = newLenX;
ch_len[CH_YAW] = newLenZ;
}
gdzie:
ch_len[] - tabela zawierająca długości kanałów,
CH_ROLL - numer kanału dla obrotu wokół osi X,
CH_YAW - numer kanału dla obrotu wokół osi Z,
newLen* - nowe czasy kanałów.
Podsumowując, algorytm obliczania i filtrowania kątów został przedstawiony na rysunku 15.
Wykaz elementówWariant do montażu przewlekanego Rezystory: Kondensatory: Półprzewodniki: Inne: Wariant do montażu SMD Rezystory: (SMD 0603) Kondensatory: (SMD 0603) Półprzewodniki: Inne: |
Zerowanie położenia centralnego
Po uruchomieniu headtrackera następuje zapamiętanie aktualnych kątów kierunku i przechyłu IMU, względem których wykonywane są obroty kamery. Dzięki temu w sytuacji awaryjnej, na przykład po samoczynnym resecie procesora przez watchdog lub chwilowej utracie zasilania, kamera zawsze zostanie ustawiona w pozycji startowej (zostanie skierowana na dziób samolotu).
Pozycja startowa zapisana w zmiennych programowych może zostać w każdej chwili skorygowana przez naciśnięcie przycisku. Okazuje się jednak, że po włączeniu zasilania, układ IMU zaczyna przesyłać poprawne dane z pewnym opóźnieniem. Z tego powodu reset położenia centralnego wykonywany jest w pętli przez określony czas regulowany za pomocą programowego timera.
if (BTN_PRESSED)
timer1=RESET_TIME;
(...)
if (timer1>0) {
accXStart = accAngleX;
magZStart = magAngleZ;
gyroAngleX=gyroAngleZ = 0.0;
newLenX = newLenZ = CH_LEN_CNT;
}
Rysunek 16. Schemat montażowy HT w wersji przewlekanej
Po naciśnięciu przycisku zerowanie jest wielokrotnie powtarzany przez czas 200 ms. Okres ten jest wystarczająco długi, aby odczytać i obliczyć kąty z IMU i jednocześnie wystarczająco krótki, aby nie wprowadzać opóźnień. Wielokrotne powtarzanie wstępnych obliczeń jest także istotne ze względu na konieczność "wyżarzenia" filtru Kalmana, który w początkowych iteracjach zwraca wartości o niskiej jakości.
Należy zwrócić uwagę, iż w procedurze resetującej nie następuje zerowanie zmiennych filtru Kalmana, dzięki czemu przejście od położenia poprzedniego do centralnego zostanie wykonane w sposób płynny. Operacja sygnalizowana jest przez zapalenie diody.
Montaż
Rysunek 17. Schemat ideowy HT w wersji SMD z mikrokontrolerem ATmega168
Prototyp wykonano w wersji SMD i z przeznaczeniem dla mniej wprawnych osób do montażu przewlekanego. Dodatkowo, tę druga płytkę można wykonać samodzielnie, w warunkach domowych, na podstawie dokumentacji zawartej w materiałach dodatkowych.
Schemat ideowy wariantu THT opublikowano w poprzednim odcinku (rys. 4), natomiast na rysunku 16 pokazano jest schemat montażowy. Wśród użytych elementów znajdują się stabilizatory 7805T i 317T, kilka elementów filtrujących napięcie, przycisk, dioda świecąca, podstawka CPU (28 pinów) i gniazda goldpin.
Zastosowano podciągnięcie wyprowadzenia reset poprzez rezystor 10 kV zapobiegające przypadkowemu restartowaniu się urządzenia. Zarówno wejścia, jak i wyjścia układów scalonych są filtrowane za pomocą par kondensatorów, elektrolitycznego i ceramicznego, co chroni przed skokami napięcia spowodowanymi pracą CPU, zapewnia stabilną pracę układu, a także przyczynia się do ograniczenia emisji ciepła przez stabilizatory.
Przycisk jest wykorzystywany na dwa sposoby. Poprzez krótkie naciśnięcie jest zapamiętywane aktualne położenie IMU jako pozycja "centralna", względem której obliczane są względne kąty przechyłu i kierunku. Dłuższe przytrzymanie przycisku uruchamia procedurę kalibrowania żyroskopu.
Headtracker może być zasilany napięciem z zakresu 7...25 V, a więc do jego pracy wystarczy bateria LiPo 2S lub 3S.
Rysunek 18. Schemat montażowy HT w wersji SMD z mikrokontrolerem ATmega168
Na rysunku 17 pokazano schemat ideowy HT w wersji SMD, a na rysunku 18 jego schemat montażowy. Różnice pomiędzy wersją THT a SMD to między innymi zastosowanie Atmega168 w obudowie TQFP, zewnętrznego rezonatora 16 MHz oraz dwukrotne zmniejszenie wymiarów.
W projekcie SMD założeniem było rozdzielenie funkcji zasilacza od modułu, stąd do uruchomienia i poprawnej pracy jest niezbędny zasilacz +5 V i +3,3 V. W tej wersji konwerter poziomów logicznych umieszczono na płytce. Zmianie uległ także przycisk - na płytce w wersji SMD utworzono piny umożliwiające dołączenie zewnętrznego przycisku, który może zostać umieszczony na w innym dogodnym miejscu.
W obu wersjach użyto tego samego układu IMU - modułu z LSM9DS0 firmy SparkFun. Płytka zawiera także pady niezbędne do przyłączenia programatora ISP oraz kondensatory filtrujące zasilanie.
Podsumowanie
Headtracker poprawnie działa już przy taktowaniu zegarem 8 MHz. Wersja oparta na mikrokontrolerze Atmega8 jest w pełni funkcjonalna, jednak ze względu na ograniczoną wielkość pamięć Flash trudno jest oprogramować dodatkową funkcjonalność.
To ograniczenie staje się nieaktualne przy zastosowaniu Atmega168 wykorzystanego w wersji SMD. W tej wersji program może zostać rozbudowany o możliwość półautomatycznej kalibracji poprzez terminal PC, natomiast taktowanie 16 MHz sprawia, iż system działa z większą dokładnością.
Płynność ruchów kamery jest zapewniona dzięki wykorzystaniu flirtowania Kalmana. Naturalny dryf żyroskopowy został zniwelowany prostym, ale bardzo skutecznym filtrowaniem komplementarnym. W połączeniu z dodatkowym algorytmem eliminującym drgania akcelerometru przy niskich prędkościach obrotowych żyroskopu uzyskujemy stabilny i wygodny w użytkowaniu system. Pozostaje zająć miejsce na "fotelu pilota" i podziwiać widoki.
Arkadiusz Witczak
arwi@go2.pl
Bibliografia:
https://goo.gl/ChsnSG
https://goo.gl/avDkl6
http://goo.gl/tfvgqc
http://goo.gl/p6GwqA
http://goo.gl/E3mbGO
http://goo.gl/sJXwCX
http://goo.gl/zBkmqU
http://goo.gl/OHYNPm
http://goo.gl/1ESj4T
http://goo.gl/GC2VdD
http://goo.gl/NVX50M
http://goo.gl/LYUDiO
