Metody wyznaczania kątów z żyroskopów dla filtru komplementarnego na potrzeby określania orientacji IMU

Transkrypt

1 Rafał RYIEL, Robert BIEDA, Konrad WOJCIECHOWSKI 2 Politechnia Śląsa w liwicach (), Polso-Japońsa Wyższa Szoła echni Komputerowych (2) Metody wyznaczania ątów z żyrosopów dla filtru omplementarnego na potrzeby oreślania orientacji IMU Streszczenie. W pracy zaprezentowano sposób estymacji orientacji obietu w przestrzeni trójwymiarowej z wyorzystaniem filtru omplementarnego. Wyorzystano onstrucję filtru omplementarnego dla ażdego z trzech ątów Eulera. Fuzja danych w tej filtracji polega na poprawie ątów wyznaczonych z uładu acelerometrów i magnetometrów oraz ątów z żyrosopów, załadając rozdzielność częstotliwościową załóceń w tych dwóch anałach pomiarowych. Celem opracowania jest analiza metod wyznaczania ątów z żyrosopu na potrzeby filtru omplementarnego i ich wpływu na doładność estymacji orientacji. Zaproponowano metodę niezależnego całowania pomiarów z żyrosopów, całowania prędości ątowych po uprzednim przeliczeniu ich do uładu nawigacyjnego oraz macierzowego całowania równania rotacji. Analizę wyonano na danych z symulatora czujnia IMU oraz dla danych pomiarowych z rzeczywistego sensora IMU. Abstract. In the paper complementary filter is presented for estimating 3D orientation. he main aim of this paper is to analyze methods of determining the angles from gyroscopes. Angles from gyro are calculated by integration measurements directly in sensor frame, by integration angles velocity after transformation to navigation frame and by strapdown integration. Analysis of the complementary filter is presented for data from real IMU sensor and for data from IMU signal simulator. (Angles from gyroscope to complementary filter in IMU) Słowa luczowe: orientacja 3D, IMU, filtr omplementarny, całowanie żyrosopów, całowanie bezardanowe. Keywords: 3D orientation, IMU, complementary filter, gyroscope integration, strapdown integration. doi:0.295/pe Wprowadzenie W pracy zaprezentowano sposób wyznaczania orientacji obietu w przestrzeni wyorzystujący filtr omplementarny w jej podstawowej formie []. Klasyczną, jednowymiarową postać filtru omplementarnego stosuje się do estymacji sygnału z dwóch niezależnych anałów pomiarowych, charateryzujących się wzajemnym dopełnianiem załóceń w dziedzinie częstotliwości. Nieliniowy opis orientacji w przestrzeni 3D teoretycznie wylucza bezpośrednie zastosowanie trzech jednowymiarowych tego typu filtrów. Eliminacje załóceń uwzględniającą ich omplementarność można zastosować do poprawy oreślania orientacji w przestrzeni trójwymiarowej jednaże wymaga to właściwego opisu matematycznego [3, 4, 5] lub odpowiedniego przygotowania sygnałów pomiarowych, co jest celem niniejszego opracowania. Zaprezentowano zatem, różne podejścia do estymacji sygnałów wejściowych dla trójosiowego filtru omplementarnego, ze szczególnym uwzględnieniem onstrucji bazującej na wyorzystaniu założeń podobnych ja dla naiwnego lasyfiatora bayesowsiego [6, 7]. Ponao, zaprezentowano ideę uogólnionego filtru omplementarnego. Znajomość orientacji w przestrzeni, zdefiniowanej jao ąty Eulera w odpowiednim uładzie odniesienia, jest niezbędna do sterowania obietami a taże jao źródło informacji o ich atualnym stanie. Informacja o orientacji wyorzystywana jest w bardzo różnych gałęziach naui, przemysłu i urządzeniach życia codziennego. Pomocna jest ona przy analizie chodu ludziego [2], oreślaniu zaresu i ruchomości ończyn oraz wizualizacji segmentów ciała ludziego [9, 0, ]. Znajomość orientacji szczególnie pożądana jest w lotnictwie a przede wszystim w bardzo popularnym w ostatnim czasie sterowaniu i nawigacji obietami typu UAV [4, 2, 3, 4]. Orientacja wyorzystywana jest taże w smartfonach [8]. Miniaturyzacja czujniów wyonanych w technologii MEMS używanych do budowy IMU (ang. Inertial Measurement Unit) sprawia wręcz nieograniczone możliwości implementacji tego typu uładów w rozwiązaniach technicznych, biologicznych czy nawet sportowych [5]. Podstawowy uład czujniów IMU do oreślenia pełnej orientacji w przestrzeni stanowi zestaw trzech trójosiowych czujniów przyspieszenia liniowego (acelerometry), prędości ątowej (żyrosopy) oraz pola magnetycznego (magnetometry). Z odpowiednio przygotowanych sygnałów pomiarowych należy oreślić orientację (ąty). W literaturze znaleźć można rożne algorytmy oferujące wyznaczenie orientacji w postaci ątów Eulera,, nazywanych odpowiednio ątem przechylenia, pochylenia oraz odchylenia. Rozwiązania te wyorzystują rachune waternionów, filtry omplementarne oraz filtry Kalmana, taże w wersjach nieliniowych, ale przede wszystim ich wzajemne hybrydy [3, 4, 6, 7, 8]. Filtr Kalmana wymaga zapisania dla procesu jego modelu, tóry w przeważających przypadach bazuje na nieliniowych równaniach inematyi ruchu obrotowego bryły sztywnej. Kwaterniony oraz filtry omplementarne wymagają zazwyczaj przeształceń macierzowych. W pracy przedstawiono sposób estymacji ątów Eulera wyorzystujący pierwotną ideę filtracji omplementarnej []. Przy taim sposobie przetwarzania sygnałów, poszuiwane ąty wyznaczane są na podstawie filtracji wysooczęstotliwościowej ątów z acelerometrów i magnetometrów oraz filtracji dolnoprzepustowej ątów z żyrosopów. W metodzie tej istotny jest sposób wyznaczania ątów z żyrosopów na drodze odpowiedniego całowania prędości ątowej. Jest to główny aspet poruszany w niniejszym opracowaniu. W rezultacie analizie poddano metody z całowaniem naiwnym [9], z całowaniem w uładzie nawigacyjnym oraz z bezardanowym całowaniem macierzowym [20, 2, 25]. Do oceny suteczności działania filtru omplementarnego wyorzystano średni masymalny błąd MAE (ang. Mean Absolute Error) oraz przebiegi czasowe. Badania przeprowadzono dla danych wygenerowanych z symulatora wsazań czujniów oraz dla danych z rzeczywistego czujnia IMU ja w [9]. Stworzony na potrzeby analizy ilościowej z wyorzystaniem wsaźnia MAE symulator dla zadanych ątów Eulera wyznacza odpowiadające im wsazania acelerometrów, żyrosopów i magnetometrów. Uład nawigacyjny (referencyjny) W problemach wyznaczania orientacji obietu w przestrzeni 3D wyorzystywane są różne ułady odniesienia związane z przestrzenią, w tórej odbywa się ruch. Definiuje się ułady bazowe oraz związane z PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/204 27

2 obietem. Uład referencyjny (bazowy) definiowany jest jao nieruchomy uład przestrzeni nawigacyjnej (ang. n- frame; navigation-frame) [20,2] np.: ECI, ECEF, LP w tym: ENU oraz NED [22,20]. Druga grupa uładów są to ułady związane z obietem (ang. b-frame; body-frame) [2,23] lub uładem pomiarowym umieszczonym na nim (ang. s-frame; sensor-frame) [9]. W analizowanym problemie jao uład referencyjny (ang. n-frame) zaproponowana została onstrucja o struturze (ang. North-West-Up) (rys. 3). Jao uład przestrzeni ruchomej, dla tórej wyznaczana jest orientacja względem uładu referencyjnego, przyjęta została postać umożliwiająca oreślenie ąta obrotu woół trzech osi R-P- Y (ang. Roll-Pitch-Yaw). Przyjęto też, że uład związany z obietem (ang. b-frame) jest tożsamy z uładem pomiarowym sensorów IMU (ang. s-frame). W badaniach na potrzeby oreślenia orientacji obietu estymuje się ąty przechylenia (ang. Roll), pochylenia (ang. Pitch) oraz odchylenia (ang. Yaw) oznaczane w pracy odpowiednio jao,,. Związe pomiędzy uładem bazowym (referencyjnym) a uładem pomiarowym opisuje macierz R oreślająca transformację obrotu w przestrzeni trójwymiarowej jednego uładu względem drugiego. W nawigacji inercyjnej, szczególnie w lotnictwie, macierz przeształcenia jednorodnego () odpowiadająca obrotom o ąty Y-P-R [24, 25] (odpowiednio o ąty R-P-Y względem pierwotnego/bazowego uładu odniesienia ) może zostać zdefiniowana z wyorzystaniem elementarnych macierzy obrotów woół poszczególnych osi [9, 20, 26, 27]. Macierz rotacji oreślająca transformację obrotu uładu nieruchomego do uładu ulegającego rotacji RPY zdefiniowana jest następująco [20, 26, 2, 25]: () cc ssc cs csc ss RRPY cs sss cc css sc s sc cc c cos, sin s Pozwala ona na oreślenie orientacji obietu w uładzie bazowym na podstawie znajomości wartości ątów R- P-Y oraz informacji o obiecie wyrażonej w uładzie pomiarowym IMU. Natomiast przeształcenie odwrotne rotacji, w notacji ątów Eulera XYZ (RPY), można zdefiniować jao obroty w przeciwnym (odwrotnym) ierunu woół osi Z-Y-X uładu pierwotnego/ruchomego RPY związanego z uładem pomiarowym IMU: (2) cc cs s RPY R s sc cs sss cc sc csc ss css sc cc c cos, sin s Filtr omplementarny Zasada omplementarności polega na wzajemnym uzupełnianiu lub dopełnianiu. Zasadę tę wyorzystuje się w onstrucji filtrów, tóre integrują informacje pochodzące z różnych, anałów i czujniów pomiarowych. Celem tej filtracji jest wyorzystanie najistotniejszej informacji, tórą transportuje dany sygnał pomiarowy, przy jednoczesnym pominięci części będącej jego załóceniem. Filtracja ta umożliwia połącznie pomiarów z różnych czujniów i uzysanie informacji o lepszych własnościach. Czujnii powinny zatem cechować się różną charaterystyą częstotliwościową błędów pomiarowych. Idea filtracji omplementarnej przedstawiona została na rysunu. Awizycja oraz przetwarzanie sygnału pomiarowego w celu wyodrębnieniu z niego informacji użytecznej polega na wyorzystaniu różnego rodzaju filtrów: dolnoprzepustowych, górnoprzepustowych lub pasmowo przepustowych. x x 2 x N Rys.. Ogólna idea filtru omplementarnego Konstrucja taiego filtru powinna spełniać ogólną zależność: (4) Fi s N i W pracy, do poprawnego oreślenia orientacji w przestrzeni wyorzystuje się pomiary z acelerometrów oraz magnetometrów, załócone sygnałem wysooczęstotliwościowym (szumy) oraz pomiary z żyrosopów sażone głównie sygnałem wolnozmiennym (błędy zera). Do wyeliminowania załóceń szybozmiennych wyorzystano filtry dolnoprzepustowe (ang. low-pass filter - LPF) natomiast do usunięcia wolnozmiennego dryftu zera użyto filtru górnoprzepustowego (ang. high-pass filter - HPF). Filtry te można zrealizować w technice ciągłej (np. czwórnii typu RLC) lub cyfrowej (o sończonej lub niesończonej odpowiedzi impulsowej). Wyorzystano zatem filtr omplementarny o schemacie bloowym przedstawionym na rysunu 2 z tórego wynia, iż estymowany ąt ˆ jest sumą dwóch ocen: ąta z żyrosopu ˆ oraz ąta z acelerometru i magnetometru ˆ. ˆx Powyższe transformacje () i (2) często oreślane są jao macierze osinusów ierunowych DCM (ang. Direction Cosine Matrix) [22, 28, 2, 25], powiązane są ze sobą w następującej zależności: ˆ ˆ (3) N RRPY R P Y W U RPY N W U R ˆ Rys. 2. Filtr omplementarny dla estymacji ąta z wyorzystaniem pomiarów z IMU 28 PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/204

3 W związu z tym, iż uład wyznaczający orientację zaimplementowano w technice cyfrowej, obliczenia przeprowadzone są w jednostce centralnej, a więc docelowo filtry zrealizowano w postaci algorytmu wyonywanego przez program omputerowy. Jednaże, syntezę filtru zapoczątowano w dziedzinie czasu ciągłego, mając na uwadze lasyczne jego ujęcia w postaci filtrów: Butterowrtha, Besela czy Chebyszewa. W pracy wyorzystano filtr dolnoprzepustowych w postaci inercji I rzędu: (5) LPF s gdzie jest stałą czasową elementu inercyjnego i oreśla dynamię filtru. a więc, zgodnie z zasadą omplementarności wyniającą z zależności (4), filtr górnoprzepustowy będzie miał postać: s (6) HPF LPF s W celu zaimplementowania algorytmu filtru omplementarnego o elementach dynamicznych (5) i (6) w uładzie miroprocesorowym, należy je zapisać w postaci równań różnicowych a więc zdysretyzować w dziedzinie czasu. W pracy wyorzystano przybliżenie operacji różniczowania w czasie schematem różnicy wstecznej (ang. bacward Euler method): df t f t f tt (7) s ˆ, t tóry prowadzi do uzysania podstawienia: (8) ˆ z s z t z t z gdzie jest operatorem opóźnienia o jeden ores próbowania t. Oznaczając jao: ˆ ˆ (9) LPF ; HPF oraz wstawiając do transmitancji filtrów (5) i (6) zależność (8) otrzymuje się dysretne transmitancje filtrów: (0) ˆ ˆ z t z t t z t t Uwzględniając równanie filtru omplementarnego: () otrzymuje się pełny jej opis: (2) t ˆ ˆ ˆ t z ˆ z ( t) z t ˆ t t t Zastosowanie podstawienia p t prowadzi do zależności: (3) ˆ ˆ p p p tórą przedstawić można w postaci wetorowej: (4) αˆ ˆ pα pα pα α gdzie αˆ, α, α są odpowiednio wetorami ątów estymowanych, wetorami ątów wyznaczonych z żyrosopów oraz wetorami ątów wyznaczonych z uładu acelerometry-magnetometry: (5) α ˆ ˆ ˆ ˆ (6) (7) α α. Orientacja obietu Uład pomiarowy IMU zbudowany został z wyorzystaniem technologii MEMS [20, 28]. Wyposażony został w trzy trójosiowe ułady pomiarowe zdefiniowane w onfiguracji uładu odniesienia związanego z uładem bazowym Ziemi. Acelerometry i magnetometry Model sygnału mierzonego przyspieszenia przez uład acelerometrów definiowany jest jao suma rzeczywistej wartości przyspieszenia obietu a, wetora grawitacji g oraz sładowej w a modelującej załócenie w postaci białego szumu gaussowsiego [9, 0]: (8) a a g wa Podobnie sygnał mierzony przez uład magnetometrów modelowany jest jao suma wetora natężenia pola magnetycznego Ziemi b oraz wetora w m modelującego szum pomiarowy [9]: (9) m b wm Rozpatrując przypade idealny (bez załóceń) uład trójosiowego acelerometru mierzy wartość przyspieszenia A a ax ay az z uwzględnieniem rzutów wetora przyspieszenia na osie uładu pomiarowego w przestrzeni RPY [24]. Również w przypadu zastosowanego w IMU trójosiowego uładu magnetometrów zwracana jest b informacja o rozładzie znormalizowanego b B wetora pola magnetycznego Ziemi b cos 0 sin w uładzie RPY obietu [0]. Kąt γ definiuje odchylenie (inlinację) wetora pola magnetycznego od płaszczyzny XY uładu odniesienia. rójosiowy magnetometr mierzy więc wartości rzutów M m mx my mz znormalizowanego wetora pola magnetycznego [24] w uładzie RPY [9]. Budowa czujniów przyspieszenia oraz magnetometrów uładu IMU jest PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/204 29

4 zgodna z onfiguracją uładu bazowego Ziemi (rys. 3). Wielości mierzone przez te ułady można zdefiniować poniższymi zależnościami: A RPY (20) a R a g (2) M gdzie a, RPY m R b g 0 0 g oraz b to odpowiednio wetor przyspieszenia obietu, wetor przyspieszenia ziemsiego (g 9,8[m/s 2 ]) oraz znormalizowany wetor pola magnetycznego Ziemi (rys. 3) wyrażone w uładzie odniesienia. Rys. 3. Położenie wetorów przyspieszenia ziemsiego g oraz pola magnetycznego Ziemi b w uładzie odniesienia Przyjmując, że wartość przyspieszenia związanego z ruchem obietu jest pomijalnie mała ( a 0 ) [27] można w przybliżeniu założyć, że uład acelerometrów mierzy wartości rzutów stałego wetora przyspieszenia ziemsiego wyrażonego w uładzie pomiarowym IMU. Założenie taie może zostać poczynione w przypadu gdy ruch obietu i związanego z nim czujnia IMU jest zbliżony do ruchu pozbawionego nagłych i gwałtownych zmian. Wyorzystując więc powyższe założenie oraz operator RPY rotacji R (2) można wetor przyspieszenia ziemsiego g wyrazić w uładzie RPY: RPY (22) s g a x scg ay ccg a z R g a Podobnie przeształcając równanie (2) otrzymano [29]: (23) M R R m R b Y X Z cm x ssm y csm z cc cm y sm z sc sm s x scm y ccm z gdzie K woół osi K X,Y,Z o ąt,, R to odpowiednio elementarna macierze rotacji. Wyorzystując zależność (22) oraz (23) można, w oparciu o uzysany pomiar przyspieszenia liniowego A A a [27, 30] oraz pomiar wetora pola magnetycznego M m [29, 30], wyznaczyć wartości ątów rotacji obietu, oraz : (24) (25) (26) ay arctan az a arcsin x g sm z cm y arctan cm x ssm y csm z Uwzględniając zares zmienności poszczególnych ątów zapewniający jednoznaczność rozwiązania [26] ( ~,, ~ ~ 2 ątów oraz ) w procesie wyznaczania wartości wyorzystana została funcja arctan2(y,x) zwracająca wartości ątów w zaresie ±π. Ze względu na pojawiające się załócenia w uładzie pomiarowym acelerometrów (8) oraz magnetometrów α (9) wartości wyznaczonych ątów rotacji charateryzują się gwałtownymi, szybozmiennymi oraz rótotrwałymi zmianami wartości w czasie. Z tego też powodu, w celu wyeliminowania załóceń wysoo- częstotliwościowych z sygnału α wyorzystano jeden z anałów w filtracji omplementarnej LPF. Żyrosopy rzecim uładem czujniów wyorzystywanym w IMU jest trójosiowy uład żyrosopów. Sygnał mierzony przez uład żyrosopów modelowany jest jao suma wetora prędości ątowych ω, wetora wartości podporowych (biasu) β oraz białego szumu gaussowsiego w [9, 24]: (27) ω ω βw W przypadu idealnym (bez biasu oraz załóceń) uład ten, sonfigurowany zgodnie z uładem bazowym, zwraca informację o zmianie ąta obrotu woół danej osi uładu obracanego RPY. W rezultacie sygnał z uładu żyrosopów można zdefiniować jao wetor prędości ątowych ω x y z poszczególnych osi uładu pomiarowego IMU. Sygnał ten można więc wyorzystać do wyznaczenia wartości ątów α rotacji w procesie całowania prędości ątowych ω. Całowanie naiwne W przypadu sygnałów salarnych (rotacji woół jednej osi) problem wyznaczenia ąta obrotu w oparciu o informację o prędości ątowej jest problemem trywialnie prostym. Związe pomiędzy zmianą ąta a prędością ontową można zdefiniować ja w zależności: d t t tt t (28) t t t Z równania (28) można wprost wyprowadzić zależność pozwalającą oreślić wartość ąta rotacji: t tt t t (29) 220 PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/204

5 ai sposób całowania w ogólnym przypadu wyznaczania orientacji w przestrzeni 3D jest niepoprawny. Jedna w prezentowanej pracy podejście przedstawione w (29) zostało rozszerzone na przypade trójwymiarowy. Rozszerzenie taie było możliwe przy założeniu o niezależności poszczególnych sładowych wetora prędości ątowych zwracanych przez uład żyrosopów podobnie ja w systemach lasyfiacji bayesowsiej [6, 7]. Definiuje się w nich onstrucje tzw. naiwnego lasyfiatora. Bazuje on na założeniu, że prawdopodobieństwo wystąpienia wetora zdarzeń (wielowymiarowej zmiennej losowej) w analizowanym procesie jest równe iloczynowi prawdopodobieństw wystąpienia zdarzeń elementarnych. Konsewencje te wyniają z założenia o niezależności zdarzeń elementarnych definiujących wielowymiarową zmienną losową. a więc, dla analizowanego przypadu poczynione zostało założenie, że wetor prędości ątowej ω modelujący zdarzenie losowe, może być analizowany jao sładający się z trzech zdarzeń niezależnych. Bazując na taim założeniu można powyższą zależność (29) rozszerzyć na przypade trójwymiarowy: (30) α α ω t tt t t Całowanie w uładzie nawigacyjnym W ogólnym przypadu 3D sposób całowania prędości ątowych przedstawiony w (29) jest niepoprawne ze względu na fat, że trójosiowy uład żyrosopów mierzy prędości ątowe w uładzie pomiarowym RPY sensora IMU. Kąty R-P-Y natomiast definiują rotację obietu względem uładu bazowego. Rozwiązaniem tego problemu jest więc przeliczenie prędości ątowych żyrosopów ω wyrażonych w uładzie sensora do uładu bazowego. ransformacji taiej można doonać według zależności [2, 3, 32, 33]: (3) ss c cs c x dα d 0 c s y 0 s c c c z W rezultacie otrzymuje się prędości ątowe oreślające zmianę ąta obrotu woół osi uładu bazowego. Dla ta zdefiniowanych sygnałów można już w sposób poprawny przeprowadzić operację całowania prędości ątowych w uładzie w trzech niezależnych anałach: d (32) α t α t t t α t Uzysane w ten sposób ąty obrotu będą wyrażały rotację obietu względem uładu referencyjnego. W przypadu brau załóceń otrzymane wartości ątów są tożsame z wyniami uzysiwanymi z rozwiązania równania różniczowego opisującego ruch obrotowy. Całowanie macierzowe (bezardanowe) Wartości ątów rotacji R-P-Y możliwe są do wyznaczenia z macierzy rotacji R RPY (3). Wyorzystując wetor prędości ątowych ω można go tratować jao pseudowetor rotacji [22, 25] woół tórego obrócony zostaje uład przestrzeni RPY związany z uładem IMU. W taim przypadu macierz rotacji R RPY oraz związane z nią wartości ątów,, można wyznaczyć rozwiązując poniższe równanie różniczowe generujące rotację [0, 25]: (33) d R RPY RRPY ω gdzie ω jest macierzą sośnie-symetryczną [34]. Załadając, że wetor pseudorotacji jest równy wetorowi prędości ątowych mierzonych przez uład żyrosopów ω, wówczas tensor sośnie-symetryczny o wetorze osiowym ω [34] zdefiniowany jest następująco [20, 22, 28, 35]: (34) ω 0 z y z 0 x y x 0 Analizując równanie różniczowe rotacji (33) można wyprowadzić iteracyjne równanie umożliwiające wyznaczanie macierzy R RPY t w olejnych chwilach czasu [9, 25, 30]: (35) RRPY t RRPY tt I 3x3 ω t gdzie Δt jest oresem próbowania w uładzie IMU. Wyorzystując informację o wyznaczonej macierzy przeształcenia jednorodnego R RPY t (35) można oreślić, z porównania odpowiednich elementów (wiersz,olumna) macierzy: cc ssc cs csc ss r r r 2 3 cs sss cc css sc r2 r22 r 23, s sc cc r3 r32 r33 wartości ątów rotacji obietu sposób: (36) 32 arctan r r 33 (37) arcsin r (38) α [20, 25] w następujący 3 2 arctan r r Podobnie ja w przypadu ątów (24)-(26) uzysiwanych z acelerometrów i magnetometrów uwzględniając zares zmienności poszczególnych ątów zapewniający jednoznaczność rozwiązania [26] w procesie wyznaczania wartości ąta oraz wyorzystać można funcję arctan2(y,x) zwracającą wartość ąta w zaresie ±π. Ze względu na występujący w pomiarach z żyrosopów błąd zera, zamodelowany w postaci sygnału β (27), na sute operacji całowania, w trzech wyżej opisanych wariantach, wyznaczone na jej podstawie ąty rotacji α wyazują silny efet powolnego płynięcia (dryftu) ich wartości. W celu wyeliminowania błędu zera wyorzystuje PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/204 22

6 się omplementarny anał filtru - HPF. Efet załóceń szybozmiennych jest w tym przypadu pomijalnie mały. Przyładowe esperymenty Na potrzeby przeprowadzenia testów zaprojetowany został symulator sensora IMU, tóry dla zdefiniowanych i zadanych przebiegów czasowych wartości ątów rotacji oreśla przebiegi sygnałów zwracanych przez ułady czujniów przyspieszenia liniowego ( a ), prędości ątowych (żyrosopów) ( ω ) oraz wetora pola M magnetycznego (magnetometrów) ( m ): A =0.[s]. W tabeli zaprezentowano wartości MAE dla wszystich metod całowania odczytów z żyrosopów oraz stałych czasowych, tóre potwierdzają tą obserwację. Na rysunu 5 zaprezentowano natomiast porównanie przebiegów czasowych z filtracji omplementarnej przy zastosowaniu podejścia naiwnego (30) z prawidłowym całowaniem macierzowym równania rotacji (35). Z przeprowadzonych doświadczeń wynia ponao, że przebiegi czasowe z całowaniem w uładzie nawigacyjnym (3) i (32) nie różnią się od tych uzysanych dla całowania macierzowego (35). Ze względów na czytelność rysunu nie zostały one na nich zamieszczone. A A A A a,, M M M M m, a a g w ω ω β w m b d w eneralnie symulator IMU realizuje zadanie oreślone zależnością: A M a, ω, m fimuα, a, β, d, σ sensor gdzie: α - wartości zadane ątów rotacji, a - przyspieszenie liniowe obietu w uładzie nawigacyjnym, A M σ sensor,, - zbiór wartości wariancji sensorów. Na potrzeby testów wygenerowane zostały przebiegi sygnałów z sensorów symulatora IMU przy założeniu o brau załócenia pola magnetycznego ( d 0 ) oraz przyspieszenia liniowego obietu ( a 0 ) przy jednocześnie występującym biasie na żyrosopach: β [ rad / s] Dla ilościowej analizy doładności estymacji zastosowano wsaźni MAE, a więc średni bezwzględny błąd oszacowania zdefiniowany, jao: Rys. 3. Przebiegi czasowe ątów Eulera,, estymowane z żyrosopów (), acelerometrów () i ąty referencyjne z symulatora (ref.) MAE n n ˆ gdzie ˆ to estymowana wartość ąta, a to referencyjna wartość ąta. W celu doładniejszej i przejrzystszej analizy symulacji przebiegi zaprezentowano w dwóch zaresach czasowych: 0-20 [s] oraz [s] esperymentu, slejając je w jeden wyres. Na rysunu 3 zaprezentowano przyładowy, zadany w symulatorze, schemat zmiany ątów Eulera,, wraz z ich estymacją na podstawie żyrosopów oraz na podstawie uładu acelerometry-magnetometry. Kąty z żyrosopów w zaresie [s] charateryzują się błędem wyniającym z sumowania błędu zera natomiast ąty z acelerometrów i magnetometrów sażone są w całym horyzoncie czasu błędami wysooczęstotliwościowymi. Wyorzystanie filtru omplementarnego (rys.(4)), w uładzie z naiwnym całowaniem prędości ątowych z żyrosopów, prowadzi do usunięcia błędu zera oraz pozbycia się szumów pomiarowych. Zaprezentowana na tym rysunu analiza jaości ze względu na stałą czasową filtru wsazuje, że jaościowo najlepsze przebiegi uzysano dla wartości Rys. 4. Przebiegi czasowe estymowanych atów dla całowania naiwnego prędości ątowej dla czterech wartości stałych czasowych filtru omplementarnego 222 PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/204

7 abela. Wartości średniego absolutnego błędu (MEA) dla estymacji atów Eulera z wyorzystaniem różnych metod całowania prędości ątowej. Metoda R MAE P - MAE Y - MAE 3,78 2,3067 5,7622 2,8030 8,8333 8,268 Całowanie naiwne =[s],2566 5,7283 6,4908 =0,[s],5809 2,273 2,7230 =0,0[s],8473,3685 3,348 =0,00[s] 2,904 2,073 5,258 Całowanie w uł. nawig. Całowanie macierzowe =[s] 4,9839 3,4766 4,7002 =0,[s],0452 0,7628,568 =0,0[s],8433,3447 3,3400 =0,00[s] 2,904 2,073 5,2585 =[s] 4,7655 3,3262 4,5254 =0,[s],024 0,7493,4937 =0,0[s],8430,3445 3,3394 =0,00[s] 2,904 2,072 5,2584 Proponowane rozwiązania filtru omplementarnego zastosowano taże do odczytów z rzeczywistych czujniów wbudowanych w IMU, co zostało zaprezentowane na rysunu 6. Prezentowane w pracy trzy metody wyznaczania ątów z żyrosopów prowadzą do podobnych wartości ątów ˆ,. ˆ, ˆ Rys. 5. Przebiegi czasowe ątów, dla, całowania naiwnego oraz macierzowego dla =0,[s] w odniesieniu do ątów prawidłowych z symulatora Podsumowując, wyorzystanie całowania naiwnego dla filtru omplementarnego, tóre jest implementacyjnie najprostszą metodą oszacowania ątów z żyrosopów, prowadzi do zadawalających wyniów wyznaczania orientacji w przestrzeni. Uzysane wynii nie ustępują jaościowo wyniom uzysanym przy wyorzystaniu poprawniejszej teoretycznie metody macierzowego całowania równania rotacji. Rys. 6. Przebiegi czasowe estymowanych atów z acelerometrów, żyrosopów oraz z wyorzystaniem całowania naiwnego oraz macierzowego dla stałej czasowej filtru =0,[s] dla danych pomiarowych rzeczywistych z IMU Podsumowanie W pracy zaprezentowano lasyczny filtr omplementarny do estymacji orientacji czujnia IMU w przestrzeni trójwymiarowej. Opis orientacji w bazowym uładzie odniesienia np. Ziemi bazuje na nieliniowych zależnościach opisujących rotacje woół osi tego uładu. Zastosowanie idei filtracji omplementarnej do oceny orientacji na podstawie pomiarów z żyrosopów oraz uładu acelerometrów i magnetometrów wymaga zatem wyorzystania nieliniowego opisu zjawisa oraz odpowiedniego aparatu matematycznego ja na przyład waternionów. Innym rozwiązaniem, rozważanym w niniejszej pracy jest odpowiednie przygotowanie sygnałów pomiarowych w celu wyorzystania lasycznego filtru omplementarnego w trzech niezależnych anałach wyznaczających poszuiwane ąty Eulera. W pracy zaprezentowano trzy sposoby wyznaczania atów Eulera z trójosiowego żyrosopu. Powołując się na założenia podobne ja w systemach lasyfiacji bayesowsich, zaproponowano metodę naiwnego (niezależnego) całowania prędości ątowych wyznaczonych w uładzie sensora. Metoda ta wydaje się być niepoprawna teoretycznie. Ponao, poazano prawidłowe metody całowania: prędości ątowych w uładzie bazowym po wcześniejszym przeliczeniu tych prędości do uładu nawigacyjnego oraz macierzowego równania rotacji. Z przeprowadzonych analiz, wyestymowane przebiegi czasowe ątów są bardzo zbliżone dla trzech prezentowanych metod całowania. Najistotniejszy wpływ na jaość estymacji ma odpowiedni dobór stałej czasowej filtru. Wniosi wyciągnięto na podstawie analizy serii pomiarów syntetycznych, wygenerowanych z symulatora działania IMU oraz rzeczywistych danych pomiarowych uzysanych z fizycznego czujnia IMU. Do analiz wyorzystano przebiegi czasowe zrealizowanych scenariuszy zmian orientacji oraz wartości wsaźnia MAE. PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/

8 Publiacja powstała w ramach projetu: Kostium do awizycji ruchu człowiea oparty na sensorach IMU z oprogramowaniem gromadzenia, wizualizacji oraz analizy danych. Projet dofinansowany w ramach I Programu Badań Stosowanych przez Narodowe Centrum Badań i Rozwoju.(projet ID ścieża A) LIERAURA [] Higgins W.., jr., A comparison of complementary and Kalman filtering, IEEE ranactions On Aerospace And Electronic Systems Vol. AES-, NO. 3 MAY 975 [2] Scapellato S., Cavallo F., Martelloni Ch., Sabatini A.M., In-use calibration of body-mounted gyroscopes for applications in gait analysis, Sensors and Actuators A: Physical, (2005) [3] Mahony R., Hamel., Pflimlin J.-M., Nonlinear Complementary Filters on the Special Orthogonal roup, IEEE ransactions On Automatic Control, VOL.53,NO.5, June 2008 [4] Euston E., Coote P., Mahony R., Kim J., and Hamel., A complementary filter for attitude estimation of a fixed-wing uav, 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, France, Sept, 22-26, [5] Yoo.S., Hong S.K., Yoon H.M. and Par S., ain-scheduled Complementary Filter Design for a MEMS Based Attitude and Heading Reference System, Sensors 20,, [6] heodoridis S., Koutroumbas K., Pattern Recognition, 3rd edition, Academic Press, Elsevier, (2006) [7] Koronaci J., Ćwi J., Statystyczne systemy uczące się, wydanie drugie, Aademica Oficyna Wydawnicza EXI, Warszawa, (2008) [8] Ayub S., Bahraminisaab A., Honary B., A sensor fusion method for smart phone orientation estimation, he 3th Annual Postraduate Symposium on he Convergence of elecommunications, Networing and Broadcasting, 25 th June 202 [9] Roetenberg D., Inertial and Magnetic Sensing of Human Motion, PhD hesis, University of wente, (2006) [0] Roetenberg D., Hen J. Luinge H.J., Chris. M. Baten Ch..M, Veltin P.H, Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation, IEEE ransactions On Neural Systems And Rehabilitation Engineering, 3, (2005), 3, [] Luinge H.J., Veltin P.H., Baten C..M., Ambulatory measurement of arm orientation, Journal of Biomechanics 40 (2007) 78 85, [2] Wendel J., Meister O., Schlaile Ch., rommer.f., An integrated PS/MEMS-IMU navigation system for an autonomous helicopter, Aerospace Science and echnology 0 (2006) [3] Hyo-Sung Ahn, Chang-Hee Won, DPS/IMU integrationbased geolocation system: Airborne experimental test Results, Aerospace Science and echnology 3 (2009) [4] Hang uo, Min Yu, Chengwu Zou, Wenwen Huang, Kalman filtering for PS/magnetometer integrated navigation system, Advances in Space Research 45 (200) [5] K. King, S.W. Yoon, N.C. Perins, K. Najafi, Wireless MEMS inertial sensor system for golf swing dynamics, Sensors and Actuators A 4 (2008) [6] E. Foxlin, Inertial head-tracer sensor fusion by a complementary separate-bias alman filter, In Proc. Virtual Reality Annual International Symposium the IEEE 996, str 85-94, 267, March 30 - April 3, 996. [7] Yun X., Bachmann E.R., Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion racing, IEEE ransactions on Robotics, vol.22, no.6, pp , (2006) [8] Sabatini A.M., Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensing, IEEE ran. Biomedical Eng., VOL. 53, NO. 7, 2006, [9] Bieda R., rygiel R., Wyznaczanie orientacji obietu w przestrzeni z wyorzystaniem naiwnego filtru Kalmana, Przegląd Eletrotechniczny, ISSN , R. 90 NR, (204), str [20] itterton D.H., Weston J.L., Strapdown Inertial Navigation echnology - 2nd Ed., he Institution of Electrical Engineers, (2004) [2] Pusa J., Strapdown inertial navigation system aiding with nonholonomic constraints using indirect Kalman filtering, MSc hesis, ampere University of echnology, (2009) [22] rewal M.S., Weill L.R., Andrews A.P, lobal Positioning Systems, Inertial Navigation, and Integration, John Wiley & Sons, (200) [23] J.S. Medith, Estymacja i sterowanie statystycznie optymalne w uładach liniowych, WN, Warszawa 975. [24] Fux S., Development of a planar low cost Inertial Measurement Unit for UAVs and MAVs, MSc hesis, Eidgenössische echnische Hochschule Zürich, (2008) [25] Bieda R., Wyznaczanie orientacji IMU w przestrzeni 3D z wyorzystaniem macierzy tensora rotacji oraz niestacjonarnego filtru Kalmana, Przegląd Eletrotechniczny, ISSN , R. 89 NR 2, (203), str [26] LaValle S.M, Planning algorithms, Cambridge University Press, (2006) [27] Pedley M., ilt Sensing Using a hree-axis Accelerometer, Freescale Semiconductor Application Note, (203), n.an346, rev.5 [28] ucma M., Montewa J., Podstawy morsiej nawigacji inercyjnej, Aademia Morsa w Szczecinie, Szczecin (2006) [29] Ozyagcilar., Implementing a ilt-compensated ecompass using Accelerometer and Magnetometer Sensors, Freescale Semiconductor Application Note, (202), n.an4248, rev.3 [30] Kim K., Par C.., A New Initial Alignment Algorithm for Strapdown Inertial Navigation System Using Sensor Output, Proceedings of the 7th World Congress he International Federation of Automatic Control (IFAC), (2008), [3] Kim J., Autonomous Navigation for Airborne Applications, PhD hesis, Department of Aerospace, Mechanical and Mechatronic Engineering, he University of Sydney, (2004) [32] Bachmann E.R., Inertial And Magnetic Angle racing Of Limb Segments For Inserting Humans Into Synthetic Environments, PhD hesis, Naval Postgraduate School Monterey, California, (2000) [33] Quoc Phuong N.H., Kang H.-J., Suh Y.S., Ro Y.-S., A DCM Based Orientation Estimation Algorithm with an Inertial Measurement Unit and a Magnetic Compass, Journal of Universal Computer Science, vol. 5, no. 4 (2009), [34] Sadłowsi P., Parametryzacje rotacji i algorytmy rozwiązywania równań dynamii z rotacyjnymi stopniami swobody, Praca dotorsa, Polsa Aademia Nau, (2007) [35] Woodman O.J., An introduction to inertial navigation, echnical Report 696, University of Cambridge, (2007) Autorzy: dr inż. Rafał rygiel, dr inż. Robert Bieda, Instytut Automatyi, Politechnia Śląsa w liwicach ul. Aademica 2A, liwice, Polsa, robert.bieda@polsl.pl rafal.grygiel@polsl.pl. 224 PRZELĄD ELEKROECHNICZNY, ISSN , R. 90 NR 9/204