wersja mobilna | kontakt z nami

FPV Headtracker inercyjny sterownik ruchu. cz. 2

Numer: Wrzesień/2015

Postęp technologiczny sprawił, iż wynalazki, które kilkadziesiąt lat temu budziły podziw i zachwyt, dziś są w normalnym użytkowaniu. Na przykład, w napięciu oglądało się filmy, w których pokazywane były zaawansowane "zachodnie technologie". Przykładem może być system Helmet-Mounted Display stosowany w lotnictwie wojskowym już w latach siedemdziesiątych.

Pobierz PDFMateriały dodatkowe

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  w chwili k na podstawie estymacji  oraz współczynnika wzmocnienia Kalmana Kk. Rozpoczynamy od zdefiniowania modelu matematycznego naszego układu za pomocą następujących równań:

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 drugim jest obliczana kowariancja zmian estymaty Pk, czyli "dokładność" estymaty Qk jest macierzą estymacji kowariancji błędów Pk. Możemy to także rozumieć, jako zaufanie do przewidzianej estymaty. Kolejny etap - faza korekcji - wygląda następująco:

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ów

Wariant do montażu przewlekanego

Rezystory:
R1, R6: 240 Ω
R2: 390 Ω
R5: 10 kΩ

Kondensatory:
C1: 220 µF/16 V
C2: 47 µF/16 V
C3: 10 µF/16 V
C4: 100 µF/16 V
C5...C7: 100 nF
C8: 22 µF/16 V

Półprzewodniki:
IC1: moduł z LSM9DS0 (SparkFun)
IC2: LM317T lub odpowiednik
IC3: LM7805TV lub odpowiednik
IC4: ATmega8
LED: dioda LED 3 mm

Inne:
Podstawka 28pin
Przycisk
Odpowiednie listwy i gniazda goldpin

Wariant do montażu SMD

Rezystory: (SMD 0603)
R1...R4: 4,7 kΩ
R5: 10 kΩ
R6: 470 Ω

Kondensatory: (SMD 0603)
C1, C2: 100 nF
C3, C4: 22 pF

Półprzewodniki:
IC1: moduł z LSM9DS0 (SparkFun)
IC2: ATmega168 TQFP
LED: dioda LED SMD
T_SDA, T_SCL: BSS138

Inne:
16MHz: rezonator kwarcowy 16 MHz
Odpowiednie listwy i gniazda goldpin

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

Pozostałe artykuły

Radar ultradźwiękowy

Numer: Sierpień/2016

Prezentowane urządzenie to radar wykorzystujący fale ultradźwiękowe. Reaguje, gdy w jego zasięgu pojawi się obiekt wykonany z dowolnego materiału odbijającego dźwięki. Wyróżnia się tym, że współpracuje z czujnikami ultradźwiękowymi stosowanymi w motoryzacji, do systemów wspomagających parkowanie. Czujniki te są estetyczne, hermetyczne i łatwe w montażu, dzięki czemu urządzenie znajdzie wiele zastosowań. Rekomendacje: czujnik ...

Stacjonarny odtwarzacz audio Media PI

Numer: Sierpień/2016

W artykule przedstawiono stacjonarny odtwarzacz plików audio oparty o Raspberry PI. Podstawowym założeniem przy projektowaniu układu była prostota użytkowania i całkowita samodzielność urządzenia. Dzięki zastosowaniu dystrybucji Openelec, uruchomienie odtwarzacza sprowadza się do zainstalowania obrazu systemu na karcie SD. Rekomendacje: estetyczny, funkcjonalny odtwarzacz audio, który może uatrakcyjnić niejeden ? zwłaszcza starszy ...

Sterownik z modułem komunikacyjnym GSM

Numer: Sierpień/2016

Nieskomplikowane w obsłudze urządzenie, którym można sterować wysyłając wiadomości SMS. Do dyspozycji są dwa wyjścia z możliwością załączenia czasowego oraz dwa wejścia z funkcją alarmu. Rekomendacje: urządzenie może być szczególnie przydatne w okresie wakacyjnym. Umożliwi symulowanie obecności domowników oraz może powiadamiać np. o wejściu do mieszkania.

Komputer samochodowy Mee 2.0 (2)

Numer: Sierpień/2016

W poprzedniej części omówiliśmy ważniejsze części oprogramowania, w tym obsługę wyświetlacza TFT. Najwyższy czas na opisanie budowy komputera, pokazanie schematów ideowych oraz dołączenia do instalacji samochodu i sposobu jego obsługi.

Stereofoniczna, cyfrowa linia opóźniająca

Numer: Lipiec/2016

Niegdyś linie opóźniające budowało się z zastosowaniem sprężyn i przetworników elektromechanicznych. Współcześnie do ich konstrukcji można zastosować układy cyfrowe, co daje nieporównywalnie większe możliwości. Można nie tylko opóźniać sygnał, ale również wprowadzać dodatkowe efekty akustyczne. Opisywane urządzenie to moduł linii opóźniającej, który przyda się przy tworzeniu własnych konstrukcji. Jest przy tym nieskomplikowany ...

Mobilna
Elektronika
Praktyczna

Elektronika Praktyczna

Sierpień 2017

PrenumerataePrenumerataKup w kiosku wysyłkowym

Elektronika Praktyczna Plus

lipiec - grudzień 2012

Kup w kiosku wysyłkowym