Robert BIEDA, Rafał RYIEL Politechnia Śląsa w liwicach Wyznaczanie orientacji obietu w przestrzeni z wyorzystaniem naiwnego filtru Kalmana Streszczenie. W pracy zaprezentowano sposób estymacji orientacji obietu, w postaci ątów Eulera, w przestrzeni 3D z wyorzystaniem naiwnego filtru Kalmana. Wyorzystując założenie z systemów lasyfiacji bayesowsiej, że wetor pomiarowy prędości ątowej modelujący pewne zdarzenie może być rozpatrywany jao zbiór trzech niezależnych zdarzeń, doonano syntezy niezależnych filtrów Kalmana dla ażdej z osi uładu współrzędnych. Zaprezentowano fuzję danych z rzeczywistego czujnia IMU integrującą dane pomiarowe z trójosiowych żyrosopów, acelerometrów oraz magnetometrów. Abstract. In the paper naive Kalman filter is presented for estimating orientation in 3D space. Using the assumption of Bayesian classification systems, the angular velocity vector is treated as three separate events. herefore, tree independent Kalman filter are used to estimate Euler angles for each coordinate system. Data fusion is presented for real IMU sensor which integrated data from triaxial gyroscope, accelerometer and magnetometer. (3D object orientation with naive Kalman filter) Słowa luczowe: orientacja 3D, IMU, filtr Kalmana. Keywords: 3D orientation, IMU, Kalman filter. doi:10.1915/pe.014.01.09 Wstęp Znajomość orientacji w przestrzeni jest niezbędną informacją podczas sterowania obietami doznającymi ruchu obrotowego, oceny ruchliwości ludzich ończyn czy wizualizacji położenia segmentów ciała ludziego w świecie wirtualnym. Orientacja zdefiniowana jest jao ąty Eulera: przechylenia, pochylenia oraz odchylenia (ang. R-roll, P- pitch i Y-yaw) woół osi związanych z obietem. Do estymacji ątów Eulera wyorzystuje się ideę fuzji sensorycznej, w tórej w jednym algorytmie integruje się dane pomiarowe z różnych czujniów. Kąty wyznaczyć można bowiem z żyrosopów, poprzez macierzowe całowanie prędości ątowych [5,11], oraz z acelerometrów i magnetometrów, wyorzystując zależności algebraiczne [7,19,0]. W literaturze do oceny orientacji wyorzystywane są różne metody oraz narzędzia matematyczne ja na przyład rachune waternionów, filtry omplementarne oraz filtr Kalmana. Filtr Kalman wymaga zapisania dla procesu jego modelu, tóry w przeważających przypadach bazuje na nieliniowych równaniach inematyi ruchu obrotowego bryły sztywnej. Wyorzystanie tego opisu wymaga zlinearyzowania tego modelu. Podejście to jest prawidłowe, jednaże bywa bardzo sompliowane pojęciowo, obliczeniowo oraz implementacyjnie. W pracach nad problemem wyznaczania orientacji autorzy poszuiwali ja najprostszego algorytmu realizującego wyznaczanie orientacji. Chcąc wyorzystać do tego celu lasyczny filtr Klaman poszuiwano liniowego opisu procesu. łówna teza pracy brzmi, iż można wyznaczać orientację (ąty Eulera) na drodze niezależnego przetwarzania w filtrach Kalmana wsazań żyrosopów w trzech niezależnych osiach uładu związanego z obietem. Jaolwie teza ta wydaje się błędna to prezentowane przyłady pratyczne potwierdzają słuszność rozwiązania. Na pewno jedna nie pozwalają odrzucić taiej hipotezy jao całowicie nieprawdziwej. W pracy zaprezentowano pełny opis uładu sensorów, sposób wyznaczania z nich ątów oraz dwa podejścia do modelowania fuzji sensorycznej. Poprawność proponowanego rozwiązania potwierdzają wyznaczone orientacje. Do przeprowadzonych esperymentów wyorzystane zostały pomiary uzysane z czujnia IMU (rys. 1) będącego wyniiem prac zespołu badawczego Instytutu Automatyi w Politechnice Śląsiej w liwicach. Rys. 1. Czujni IMU Uład odniesienia 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 obietem. Uład referencyjny (bazowy) definiowany jest jao nieruchomy uład przestrzeni nawigacyjnej (ang. n- frame; navigation-frame) [5,11] np.: ECI, ECEF, LP w tym: ENU oraz NED [6,5]. Druga grupa uładów są to ułady związane z obietem (ang. b-frame; body-frame) [1,11] lub uładem pomiarowym umieszczonym na nim (ang. s-frame; sensor-frame) [8]. W analizowanym problemie jao uład referencyjny (ang. n-frame) zaproponowana została onstrucja o struturze (ang. North-West-Up) (rys. ). Jao uład przestrzeni ruchomej, dla tórej wyznaczana jest orientacja względem uładu referencyjnego, przyjęta została postać często wyorzystywana w nawigacji i 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,,. Macierz transformacji Przeształcenie R definiuje macierz rotacji oreślającą 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 [15] 34 PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014
(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 (1) [4,5,7,8]. Macierz rotacji oreślająca transformację przejścia z uładu bazowego (nieruchomego) do uładu ulegającego rotacji zdefiniowana jest następująco [5,4,11]: (1) cc ssc cs csc ss R cs sss cc css sc s sc cc c cos, sin s A a ax ay az z uwzględnieniem rzutów wetora przyspieszenia na osie uładu pomiarowego w przestrzeni [15]: A (5) a R a g gdzie a oraz g 0 0 g to odpowiednio wetor przyspieszenia obietu oraz wetor przyspieszenia ziemsiego (g 9,81[m/s ]) (rys. ) wyrażone w uładzie odniesienia. 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 (), można zdefiniować jao obroty w przeciwnym (odwrotnym) ierunu woół osi Z-Y-X uładu pierwotnego/ruchomego związanego z uładem pomiarowym IMU: () cc cs s R s sc cs sss cc sc csc ss css sc cc c cos, sin s Powyższe transformacje (1) i () często oreślane są jao macierze osinusów ierunowych DCM (Direction Cosine Matrix) [6,3,11], powiązane są ze sobą w następującej zależności: cnr cnp cny 1 N R 1R 1P 1Y cwr cwp c WY 1W (3) c UR cup cuy 1U 1 1 1 1 R N W U gdzie 1 i to wersor i-tej osi uładu danej przestrzeni, natomiast współczynnii macierzy DCM cij 1i 1 j cos oreślają osinus ąta pomiędzy osią i uładu bazowego oraz osią j uładu pomiarowego. Orientacja obietu Uład pomiarowy IMU zbudowany został z wyorzystaniem technologii MEMS [5,3]. Wyposażony został w trzy trójosiowe ułady pomiarowe zdefiniowane w onfiguracji uładu odniesienia związanego z uładem bazowym Ziemi. Acelerometry 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 [8,10]: (4) a a g wa Rozpatrując przypade idealny (bez załóceń) uład trójosiowego acelerometru mierzy wartość przyspieszenia Rys.. 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 ) [7] 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. Jest to poprawne założenie często czynione w przypadu obietów o dużej masie i bezwładności, dla tórych gwałtowna zmiana prędości oraz duże i długotrwałe przyspieszenia linowe nie są możliwe. Oczywiście założenie taie jest niepoprawne w przypadach gwałtownych ruchów ja np. przy pomiarach orientacji ruchu ramienia ludziego w testach medycznych [8,10]. Uwzględnienie taich zmian wiąże się jedna z potrzebą estymacji wetora przyspieszenia i uwzględnienia tej wielości w wetorze pomiarowym uładu acelerometrów. Mimo, iż w prezentowanej pracy taimi problemami nie zajmowano się to proponowane rozwiązanie można w prosty sposób rozszerzyć uwzględniając wartość przyspieszenia liniowego obietu. Wyorzystując więc powyższe założenie oraz operator rotacji R można wetor przyspieszenia ziemsiego g wyrazić w uładzie : (6) s g a x scg ay ccg a z R g a Wyorzystując zależność (6) można, w oparciu o uzysany A pomiar a z czujniów przyspieszenia liniowego [7,9], wyznaczyć wartości ątów rotacji obietu (7) oraz (8): scg a (7) y a tan y a arctan y ccg az a z a z A PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014 35
ax ax x arcsin g g (8) s g a sin Uwzględniając zares zmienności poszczególnych ątów zapewniający jednoznaczność rozwiązania [4] ( ~,, ~ ~ ) w procesie wyznaczania wartości ąta wyorzystana została definicja funcji arctan(y,x) (arcustangens dwuargumentowy) zwracającej wartość ąta w zaresie ±π: a (9) arctan y ~ arctan ay, az a z Analiza zależności wartości zwracanych przez uład acelerometrów nie umożliwia wyznaczenia wszystich trzech wartości ątów rotacji. W celu wyznaczenia wartości ąta odchylenia wymagane jest zastosowane uładu ompasu magnetycznego realizowanego przez trójosiowy uład magnetometrów. Magnetometry 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 [8]: (10) m b wm Zastosowany w IMU trójosiowy uład magnetometrów mierzy rozład wetora pola magnetycznego Ziemi b Bcos 0 Bsin w uładzie obietu. Kąt γ definiuje odchylenie (inlinację) wetora pola magnetycznego od płaszczyzny XY uładu odniesienia. Wartość tego ąta zmienia się w zależności od szeroości geograficznej od 0 na równiu do 90 na biegunach magnetycznych. Podobnie wartość natężenia pola magnetycznego B zależna jest od szeroości geograficznej. W celu uniezależnienia uładu pomiarowego magnetometrów od natężenia pola magnetycznego Ziemi w procesie alibracji wartość natężenia wetora pola b magnetycznego została znormalizowana b 1 B [10]. Budowa czujnia magnetometrów uładu IMU jest zgodna z onfiguracją uładu bazowego Ziemi (rys. 1). rójosiowy magnetometr mierzy więc wartości M rzutów m mx my mz znormalizowanego wetora pola magnetycznego b cos 0 sin [15] w uładzie [8]: (11) M m R b Przeształcając powyższą zależność otrzymano [1]: M R R m R b Y X Z cm x ssm y csm z cc (1) cm y sm z sc sm s x scm y ccm z R to odpowiednio elementarna macierze rotacji gdzie K woół osi K X,Y,Z o ąt,,. Z przyrównania odpowiednich wartości sładowych wetorów wyniowych [9] otrzymano zależność na wartość ąta obrotu odchylenia [1]: cc cm ssm csm x y z sc cm y sm z (13) tan cm y sm z cm ssm csm x y z sm z cm y arctan cm x ssm y csm z Podobnie ja w przypadu zależności oreślającej wartość ąta przechylenia (7) w procesie wyznaczania wartości ąta odchylenia wyorzystana została funcja arcustangens dwuargumentowy: (14) sm z cm y arctan ~ cm x ssm y csm z arctan, sm z cm y cm x ssm y csm z Ze względu na pojawiające się załócenia w uładzie pomiarowym acelerometrów (4) oraz magnetometrów (10) wartości wyznaczonych ątów rotacji,, charateryzują się gwałtownymi, szybozmiennymi oraz rótotrwałymi zmianami wartości w czasie. Z tego powodu ta wyznaczone wartości ątów rotacji często są poddawane filtracji dolnoprzepustowej mającej na celu eliminację załóceń będących efetem pojawiającego się szumu w sygnałach mierzonych w uładzie IMU. Cechy tej nie posiadają wartości ątów rotacji wyznaczone w procesie całowania sygnału z żyrosopów. Ż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 [8,15]: (15) ω ω β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. 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. Wyorzystując informację o macierzy przeształcenia jednorodnego R (1): (16) cc ssc cs csc ss r11 r1 r13 cs sss cc css sc r1 r r 3 s sc cc r31 r3 r33 36 PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014
można wyznaczyć, z porównania odpowiednich elementów (wiersz,olumna) macierzy (16), wartości ątów rotacji obietu:,, [5]. Odpowiednio z porównania elementów (3,) oraz (3,3) otrzymano wartość ąta : sc r 3 r 3 r arctan 3 cc r 33 r33 r33 (17) tan Z porównania elementu (3,1) można wyznaczyć wartość ąta : (18) s r sin r arcsin r 31 31 31 Wartość ąta otrzymano z porównania elementów (1,1) oraz (,1): cs r r r cc r r r (19) tan 1 1 arctan 1 11 11 11 Uwzględniając, podobnie ja dla rozwiązania uzysiwanego z uładu acelerometrów i magnetometrów, zares zmienności poszczególnych ątów zapewniający jednoznaczność rozwiązania [4] w procesie wyznaczania wartości ąta oraz wyorzystana została funcja arctan(y,x) (arcustangens dwuargumentowy) zwracająca wartość ąta w zaresie ±π: r r33 3 33 r arctan 1 ~ arctan r11 r, r (0) arctan 3 ~ arctan r, r (1) 1 11 Równanie ruchu rotacji Powyższe zależności definiujące ąty możliwe są do wyznaczenia pod waruniem znajomości macierzy rotacji R (3). Wyorzystując wetor prędości ątowych ω, zwracanych przez uład pomiarowy trójosiowego żyrosopu, można go tratować jao pseudowetor rotacji [6], woół tórego obrócony zostaje uład przestrzeni związany z uładem IMU. W taim przypadu macierz rotacji R oraz związane z nią wartości ątów,, można wyznaczyć rozwiązując poniższe równanie różniczowe generujące rotację [10]: d () R R ω gdzie ω jest macierzą sośnie-symetryczną [13] (operatorem iloczynu wetorowego). Załadając, że wetor pseudorotacji jest równy wetorowi prędości ątowych mierzonych przez uład żyrosopów: ω x y z, wówczas tensor sośniesymetryczny o wetorze osiowym ω [13] zdefiniowany jest następująco [5,6,3,14]: (3) ω 0 z y z 0 x y x 0 Jedną z możliwości rozwiązania równania różniczowego () jest zdefiniowanie, w oparciu o różniczowe równanie generujące rotację, rozwiązania przybliżonego [14] bazującego na zastąpieniu operacji różniczowania operacją różnicową dla sończonego oresu czasu Δt [5,11]: d R (4) R t ttr t t Analizując zależność (4) można wyprowadzić iteracyjne równanie umożliwiające wyznaczanie macierzy R t w olejnych chwilach czasu: d t t t t t (5) R R R gdzie Δt jest oresem pomiędzy dysretnymi chwilami czasu (jest oresem próbowania w uładzie IMU). Dla małych wartości ątów rotacji {,, } można przyjąć, że sin( ) oraz cos( ) 1 [14]. W rezultacie macierz rotacji R (3) można przybliżyć w następujący sposób [5,1,11]: 1 (6) R t 1 1 W oparciu o przybliżoną postać macierzy rotacji (6), można równanie generujące rotację () przedstawić w następującej postaci [11]: d (7) R t R t 0 0 0 Wyorzystując powyższe przybliżenie (7), oraz uwzględniając zależność: d t tt t t (8) t t t można sładową uatualniającą rozwiązanie w równaniu (5) przedstawić w postaci [5]: (9) d R 0 t t R t 0 0 R ω t R A t t gdzie A jest macierzą sośnie-symetryczną: 0 z t y t A ω t z t 0 x t y t x t 0 (30) Wyorzystując powyższe zależności oraz informację pozysaną z uładu pomiarowego żyrosopów można PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014 37
zależność (5), definiującą macierz rotacji przedstawić w postaci [8]: (31) t t t gdzie: R R B (3) B I A 1 z t y t z t 1 x t y t x t 1 R, Na sute operacji całowania, w procesie wyznaczania macierzy R, wyznaczone na jej podstawie wartości ątów rotacji,, pozbawione są efetu gwałtownych zmian będących sutiem pojawiającego się szumu w w wetorze prędości ątowych ω (15). W odróżnieniu od wartości ątów rotacji wyznaczonych z sygnałów uładu acelerometrów i magnetometrów ąty wyznaczone z sygnałów uładu żyrosopów pozbawione są elementu nerwowości w przebiegach czasowych. Jedna ze względu na pojawiający się stały czynni (bias) β (15), w sygnale otrzymywanym z uładu żyrosopów, w procesie całowania wartość ta ulega sumulowaniu co objawia się powolną lecz ciągłą zmianą wartości wyznaczonych ątów rotacji. Efetem zaistniałego zjawisa jest dryft wartości wyznaczonych wielości. W rezultacie zarówno wartości ątów wyznaczone z pomiarów uładu żyrosopów ja i uładów acelerometrów i magnetometrów posiadają charaterystyczne, dla danych uładów pomiarowych, błędy. Jednym ze sposobów orygowania wyznaczonych wielości ątów rotacji jest wyorzystanie tzw. filtrów omplementarnych [15]. Często jedna proces orecji realizowany jest jao fuzja sygnałów niosących informacje o orientacji z różnych źródeł pomiarowych. Proces ten realizowany jest z wyorzystaniem filtru Kalmana, pozwalającego na estymowanie wielości niemierzalnych na podstawie dostępnych sygnałów oraz modelu procesu. Naiwny filtr Kalmana Fuzja sensoryczna integruje w spójną całość informacje pochodzące z różnych, na ogół fizycznie odseparowanych i niezależnych źródeł. W przypadu rozpatrywanej estymacji orientacji, fuzja dotyczy integracji informacji z acelerometrów, żyrosopów i magnetometrów. W pracy wyorzystano do tego celu algorytm filtru Kalmana, tóry wymaga znajomości dysretnego liniowego modelu procesu: (33) x Fx u Qw y Hx r 1 Załada się, że dane z czujniów (rys. 1) pobierane są w równo odległych chwilach czasu z oresem próbowania t. Należy zatem stworzyć model procesu wyznaczania orientacji w przestrzeni z trzech niezależnych anałów pomiarowych. Podobne modele znaleźć można w literaturze [1] jednaże dotyczą one opisu obrotu woół jednej osi. Inne podejścia uwzględniają nieliniowe równania inematyi ruchu obrotowego bryły sztywnej, tóre należy linearyzować. Pomiar prędości ątowej z żyrosopu obarczony jest błędem biasu (zera) oraz dodatowym załóceniem stochastycznym modelowanym w postaci dysretnego białego szumu gaussowsiego o zerowej wartości oczeiwanej oraz wariancji : (34) w, Błąd zera zaliczany jest do załócenia nisoczęstotliwościowego i można go zamodelować w postaci realizacji procesu Marowa pierwszego rzędu wraz z losowym białym szumem o rozładzie E 0;E : (35) 1 W celu wyznaczenia ąta ze zmierzonej prędości ątowej wyorzystano dysretne całowanie zgodnie z zależnością (8). Uwzględnienie zależności (35) w całowanym równaniu (34) prowadzi do modelu: (36) 1 w, t t ttw, Agregując opis modelu (36) oraz równania (35) do równań stanu (33) otrzymuje się następujące macierze i wetory: (37) 1 t t t 0 F ; ; 1 0 ; ; 0 1 0 H Q 0 1 x gdzie w, ; u ; y ; ; r r w r jest szumem pomiarowym Er 0;E r ąta wyznaczonego z wyorzystaniem uładu acelerometrów i magnetometrów. Model (33) zdefiniowany przez elementy (37) posiada wejście sterujące u(t) w postaci prędości ątowej. Podejście to wydaje się być niewłaściwe bo sygnał u(t), zgodnie z założeniami czynionymi podczas wyprowadzania filtru Klamana, powinien być sygnałem deterministycznym. Jednaże z puntu widzenia modelowania podejście to jest prawidłowe. W celu pozbycia się wejścia sterującego, z modelu wyznaczającego ąt z żyrosopu, prędość ątową włączono do wetora stanu. Proces można teraz zapisać następującymi macierzami: 1 t t t 0 1 0 0 F 0 1 0 ; ; 1 0 H ; 0 1 0 Q 0 0 1 0 1 (38) w, r x ; y ; ; w r r Model (33) opisany przez macierze (38) nie zawiera sygnału sterującego ale wyjście stanowią dwa sygnały: ąt oraz prędość ątowa. W proponowanym rozwiązaniu model zapisany dla jednego ąta (osi) został zastosowany do pozostałych dwóch ątów. Rozszerzenie proponowanego podejścia na wszystie trzy ąty obrotu było możliwe przy założeniu o niezależności poszczególnych sładowych wetora prędości ątowych podobnie ja w systemach lasyfiacji bayesowsiej [16,17]. Definiuje się w nich onstrucje tzw. naiwnego lasyfiatora. Bazuje on na założeniu, że 38 PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014
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ższe podejścia (37) oraz (38) rozszerzyć na przypade trójwymiarowy definiując model naiwnego filtru Kalmana. Syntetyzując model wyznaczania orientacji z fuzji sensorycznej dla filtru Kalmana rozważono dwa warianty. W pierwszym podejściu zastosowano model z wejściem sterującym 0 i otrzymano opis: (39) x Fx u Qw 1 α1 I33 t I3 3α t I3 3 ω β1 033 I3 3 β 03 3 t I 0 w y Hx r α I 33 33, 03 3 I3 3 ε α 33 03 3 r β gdzie wetory ątów, biasów w odpowiednich osiach, oraz prędości ątowych zdefiniowano następująco: (40) x, y, z, x, y, z, Macierze Inn, 0 nn to odpowiednio macierz jednostowa i zerowa o wymiarach n n. Wyjściami z modelu, w fazie orecji, będą ąty wyznaczone z acelerometrów oraz magnetometrów: (41) α =. Wariant drugi modelu z włączeniem prędości ątowej do wetora stanu prowadzi do opisu: (4) x Fx Qw 1 1 33 33 33 ω 1 03 3 I3 3 03 3 ω β 1 03 3 03 3 I 33 β t I3 3 03 3 w, I3 3 03 3 ε 0 33 I3 3 α I t I t I α α ω y Hx r I 0 33 33 33 ω 033 I3 3 03 3 r β α 0 r W modelu tym wetor wyjścia rozszerza się o pomiar z żyrosopu. Dla modelu opisanego uładem równań (33) filtr Kalmana [1] zdefiniowany jest etapem predycji (43) xˆ ˆ 1 Fx u oraz filtracji: P FP F QWQ 1 xˆ xˆ K y H xˆ K 1 P 1 H HP 1 H R P I K H P (44) 1 1 1 1 1 1 1 1 1 1 o następujących rozładach stochastycznych załócenia oraz szumu pomiarowego w modelu: E x ˆ ˆ 0 1x 0 1 X0, E w w W, E r r R. Przedstawiony opis modelu procesu w dwóch wariantach (39) i (4) może zostać zdeomponowany na trzy niezależne podproblemy, dla ażdej z osi (ąta). Ideę wyorzystania naiwnego filtru Kalmana, w trzech niezależnych anałach przedstawiono na rysunu 3. Deompozycja ta upraszcza implementację filtru Kalmana na urządzeniu docelowym. Rys. 3. Idea implementacji naiwnego filtru Kalmana. Przyładowe esperymenty W tracie analizy algorytmu naiwnego filtru Kalmana przeprowadzono badania suteczności tego rozwiązania w procesie estymacji orientacji obietu w przestrzeni. Jednym z głównych problemów jest oreślenie referencyjnej, doładnej orientacji obietu. W tym celu wyorzystać można inne rozwiązania AHRS ja na przyład bazujące na filtrach omplementarnych lub rozszerzonych (nieliniowych) filtrach Kalmana. Wyorzystać można taże inny czujni o oreślonej doładności na przyład rozwiązanie omercyjne w postaci modułu Xsens Mi. Jednaże, najprostszym rozwiązaniem, dla oreślenia orientacji w stanie ustalonym jest wyorzystanie acelerometrów i magnetometrów. W przyładach zaprezentowano taże orientacje wyznaczone z równania ruchu rotacji. Wyorzystując czujni IMU z rys. 1 przeprowadzono liczne esperymenty. Na rysunach 4-6 zaprezentowano przyładowe przebiegi czasowe. Jao parametr strojenia filtrów Kalmana przyjęto wariancje załócenia stanu W oraz wariancję szumu pomiarowego R w postaci macierzy jednostowej z odpowiednim mnożniiem. Należy podreślić iż oreślenie wariancji błędów, i nie jest jednoznaczne. W literaturze [18] proponuje się różne metody szacowania tych wielości. Korzystając z omercyjnych rozwiązań IMU można bazować na wartościach podanych w doumentacji technicznej. W pracy założono jednostową macierz owariancji szumu pomiarowego R oraz dobierano ta macierz owariancji 1 PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014 39
załócenia W aby zbadać wzajemny stosune tych wielości na jaość i doładność estymacji. Na rysunach 4-6 zaprezentowano ąty wyznaczone z wyorzystaniem: acelerometrów i magnetometrów (z zależności (8), (9) i (14) ); F.K.1 filtru Kalmana o modelu z wejściem sterującym (39); F.K. filtru Kalmana z modelem bez sterowania (4) oraz macierzy (1) i algorytmu całowania bezardanowego (ang. strapdown integration) (31) za pomocą wzorów (18), (0) i (1). Z przeprowadzonych badań wynia iż filtry Kalmana z liniowymi modelami (39) i (4) prawidłowo estymują ąty R-P-Y realizując zadanie filtracji oraz fuzji danych z niezależnych anałów. Ja wynia z rysunu 4 i rysunu 5 ąty wyznaczone z modelów procesów (39) i (4) pratycznie nie różnią się między sobą. W rezultacie przebiegi czasowe oznaczone jao F.K.1 i F.K. prawie porywają się. Estymaty ątów z acelerometrów i magnetometrów () charateryzują się więszą wariancją zmienności. Algorytm całowania (31) macierzy rotacji (przebieg na rysunu 4b)) błędnie wylicza ąty a wynii obarczone są błędem w stanie ustalonym. Spowodowane jest to między innymi błędem biasu. Na jaość filtracji, wariancję otrzymywanego sygnału ąta, istotny wpływ ma wzajemny stosune wariancji załócenia, do szumu pomiarowego. W przypadu gdy różnica między wariancją załócenia a wariancją szumu pomiarowego jest znaczna wówczas przebiegi są bardziej wygładzone (rys. 7), jedna doładność wyznaczania ąta maleje. Ponao, dostrzega się wówczas pewne różnice w wartościach ątów uzysanych z modelowania zjawisa z wejściem sterującym (39) oraz bez wejścia sterującego (4). a) Rys. 5. Przebiegi czasowe estymowanych ątów Eulera przy W I 0,001 i R I w odniesieniu do ątów uzysanych z acelerometrów i magnetometrów (). b) Rys. 6. Przebiegi czasowe estymowanych ątów Eulera przy W I 0,0001 i R I w odniesieniu do ątów uzysanych z acelerometrów i magnetometrów (). Rys. 4. Przebiegi czasowe estymowanych ątów Eulera przy W I0,1 i R I w odniesieniu do ątów uzysanych z a) acelerometrów i magnetometrów () oraz b) całowania macierzy rotacji (). 40 PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014
a) b) Rys. 7. Przybliżenie przebiegu czasowego ąta dla a) W I 0,1 i b) W I0,0001 oraz R I w odniesieniu do ątów uzysanych z acelerometrów i magnetometrów (). Podsumowanie W pracy, orzystając z idei onstrucji tzw. naiwnego lasyfiatora zaproponowano algorytm estymacji orientacji w przestrzeni 3D wyorzystujący do tego celu trzy niezależne filtry Kalmana. Założono bowiem, że prędość ątowa ω oreśla pewien wetor sładający się z trzech niezależnych zdarzeń. Podejście to umożliwiło zbudowanie dla ażdej z osi R-P-Y niezależnego filtru Kalmana, tóry wyorzystując pomiary z żyrosopu oraz acelerometrów i magnetometrów wyznacza odpowiednie ąty Eulera. Pomimo zastosowania, wydawać by się mogło, błędnego podejścia z przeprowadzonych esperymentów można wniosować o poprawności zaproponowanego rozwiązania. Ponieważ filtry Kalmana syntetyzuje się niezależnie dla ażdego z ątów rotacji oraz wyorzystuje się bezpośrednio pomiary z żyrosopów problem fuzji można zdeomponować na trzy niezależne podproblemy. Ułatwia to implementację algorytmu w docelowym urządzeniu pomiarowym. Modelowanie procesu, w postaci dysretnego całowania, wyonać można wyorzystując model z wejściem lub bez wejścia sterującego. Zaproponowane filtry Kalmana poza orientacją wyznaczają taże błędy zera żyrosopów. Odpowiednie sparametryzowanie modelu pozwala uzysać przebiegi czasowe estymowanych ątów o pożądanej wariancji oraz precyzji i szybości uzysania stanu ustalonego. 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 178438 ścieża A) LIERAURA [1] J. S. Medith, Estymacja i sterowanie statystycznie optymalne w uładach liniowych, WN, Warszawa (1975). [] M. Haid, J. Breitenbach, Low cost inertial orientation tracing with Kalman filter, Applied Mathematics and Computation 153 (004) 567 575 [3] ucma M., Montewa J., Podstawy morsiej nawigacji inercyjnej, Aademia Morsa w Szczecinie, Szczecin (006) [4] LaValle S.M, Planning algorithms, Cambridge University Press, (006) [5] itterton D.H., Weston J.L., Strapdown Inertial Navigation echnology - nd Ed., he Institution of Electrical Engineers, (004) [6] rewal M.S., Weill L.R., Andrews A.P, lobal Positioning Systems, Inertial Navigation, and Integration, John Wiley & Sons, (001) [7] Pedley M., ilt Sensing Using a hree-axis Accelerometer, Freescale Semiconductor Application Note, (013), n.an3461 [8] Roetenberg D, Inertial and Magnetic Sensing of Human Motion, PhD hesis, University of wente, (006) [9] Kim K., Par C.., A New Initial Alignment Algorithm for Strapdown Inertial Navigation System Using Sensor Output, Proceedings of the 17th World Congress he International Federation of Automatic Control (IFAC), (008), 13034-13039 [10] 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, 13, (005), 3, 305-405 [11] Pusa J., Strapdown inertial navigation system aiding with nonholonomic constraints using indirect Kalman filtering, MSc hesis, ampere University of echnology, (009) [1] alat Ozyagcilar, Implementing a ilt-compensated ecompass using Accelerometer and Magnetometer Sensors, Freescale Semiconductor Application Note, (01), n.an448 [13] Sadłowsi P., Parametryzacje rotacji i algorytmy rozwiązywania równań dynamii z rotacyjnymi stopniami swobody, Praca dotorsa, Polsa Aademia Nau, (007) [14] Woodman O.J., An introduction to inertial navigation, echnical Report 696, University of Cambridge, (007) [15] Fux S., Development of a planar low cost Inertial Measurement Unit for UAVs and MAVs, MSc hesis, Eidgenössische echnische Hochschule Zürich, (008) [16] heodoridis S., Koutroumbas K., Pattern Recognition, 3rd edition, Academic Press, Elsevier, (006) [17] Koronaci J., Ćwi J., Statystyczne systemy uczące się, wydanie drugie, Aademica Oficyna Wydawnicza EXI, Warszawa, (008) [18] Peter Haywood King, Low Cost Localization Solution Using a Kalman Filter for Data Fusion, Praca magistersa, April 9, 008, Blacsburg, Virginia ech, (008) [19] Songlai Han and Jinling Wang A Novel Method to Integrate IMU and Magnetometers in Attitude and Heading Reference Systems. Journal of Navigation, 64, (011), pp 77-738. [0] Michael J. Caruso, Applications of Magnetoresistive Sensors in Navigation Systems, Honeywell Inc, (1998) Autorzy: Ph.D. Robert Bieda, Ph.D. Rafał rygiel, Instytut Automatyi, Politechnia Śląsa w liwicach ul. Aademica A, 44-100 liwice, Polsa, email: robert.bieda@polsl.pl rafal.grygiel@polsl.pl. PRZELĄD ELEKROECHNICZNY, ISSN 0033-097, R. 90 NR 1/014 41