ESKÉ VYSOKÉ U ENÍ TECHNICKÉ V PRAZE Fakulta elektrotechnická BAKALÁ SKÁ PRÁCE Michal Burian

Transkript

1 ESKÉ VYSOKÉ UENÍ TECHNICKÉ V PRAZE Fakulta elektrotechnická BAKALÁSKÁ PRÁCE 29 Michal Burian

2 ESKÉ VYSOKÉ UENÍ TECHNICKÉ V PRAZE Fakulta elektrotechnická Katedra mení Zpracování signál naviganí jednotky Vedoucí práce Ing. Michal Reinštein Autor Michal Burian Praha 29

3 Prohlášení Prohlašuji, že jsem svou bakaláskou práci vypracoval samostatn a použil jsem pouze podklady (literaturu, projekty, SW atd.) uvedené v piloženém seznamu. Nemám závažný dvod proti užití tohoto školního díla ve smyslu 6 Zákona.121/2 Sb., o právu autorském, o právech souvisejících s právem autorským a o zmn nkterých zákon (autorský zákon). V Praze dne.. podpis

4 Podkování Chtl bych pedevším podkovat svému vedoucímu bakaláské práce Ing. Michalu Reinšteinovi, který m seznámil s problematikou inerciální navigace a pomohl mi s realizací a testováním mého algoritmu mechanizace.

5

6 Anotace Práce se zabývá návrhem a realizací algoritmu v prostedí Matlab, který provede mechanizaci inerciálního naviganího systému (INS). Algoritmus vypoítá polohu, rychlost a úhly náklonu navigovaného objektu jak v zemské, tak i v naviganí souadné soustav. Souástí práce je ovení algoritmu na simulovaných a namených datech. Mení je provedeno pomocí naviganí jednotky 3DM-GX2 (MicroStrain) v laboratoi. Annotation This project deals with the design and realization of algorithm in Matlab, which implement mechanization inertial navigation system (INS). Algorithm calculates the position, speed and attitude of object in the earth, and in the navigation coordinate system. Part of this project is the verification algorithm on simulated and measured data. Measurement is performed by using the navigation unit 3DM-GX2 (MicroStrain) in the laboratory. Keywords: mechanization INS, position, speed, attitude, earth coordinate system, navigation coordinate system

7 Obsah 1 Úvod Teorie Navigace Inerciální navigace Akcelerometry Senzory úhlových rychlostí Senzory úhlové rychlosti na principu Coriolisovy síly Obecný princip gyroskop MEMS Coriolisova síla a její projevy Píklad provedení samotného MEMS snímae íslicové filtry Popisy stav a vztah používané v inerciální navigaci Souadné soustavy a jejich transformace Inerciální souadná soustava Zemská souadná soustava Naviganí souadná soustava Souadná soustava micí jednotky (IMU) Mechanizace INS Výstupy ze senzor Mení Naviganí jednotky DM-GX2 (MicroStrain) AHRS-M3 (Inalabs) Simulace mení Rotace Zem Nehybnost jednotky vzhledem k NED Pohyb jednotky v jednom smru bez natoení Pohyb jednotky ve dvou smrech bez natoení Pohyb zatáející jednotky Pohyb jednotky do spirály Laboratorní mení Zpracování namených dat Úprava namených dat Bias senzoru Filtry Implementace mechanizace INS Poátení hodnoty Aktualizace vektoru rychlostí Aktualizace polohy Aktualizace polohových úhl Výstupy implementace INS Výstup jednotky v ECEF Výstup jednotky v NED Závr Literatura Pílohy Seznam obrázk Tabulky... 45

8

9 1 Úvod Lidstvo se potýká s navigací už od dávných vk, kdy orientace a navigace byla velmi nepesná a spoívala nanejvýše v nalezení cesty z msta A do msta B podle rozeznávání vizuáln výrazných orientaních bod jako jsou eky nebo hory. V dnešní dob je již navigace na daleko vyšší úrovni a již se k navigaci používají sofistikované systémy, které umí nejen najít cestu z pesn ureného bodu A do pesn ureného bodu B, ale i urovat pesnou polohu objektu v prostoru nebo pesný pohyb objektu vzhledem k Zemi nebo k jiné vztažné soustav. Jedním z takových systému je i inerciální naviganí systém, kterým se zabývá tato práce. Hlavním cílem této práce je implementace algoritmu, který z výstup inerciálního systému, tvoeného soustavou akcelerometr a senzor úhlových rychlostí, vypoítá aktuální polohu, rychlost a úhly natoení navigovaného objektu. Díky tomuto algoritmu je možné sledovat pesný pohyb navigovaného objektu, což se dá v praxi využít jak pro sledování objekt velkých rozmr jako jsou auta nebo letadla, ale také pro sledování automatických ramen robot ve výrobních linkách pro pesné umísování souástek. 1

10 2 Teorie Tato kapitola shrnuje význam slova navigace a její druhy. Zamená je pedevším na inerciální navigaci. Dále popisuje, co je to akcelerometr a senzor úhlové rychlosti a zpsob jejich mení. Dále popisuje íslicové filtry a jejich vlastnosti a v poslední ásti se zabývá mechanizací inerciálního naviganího systému (INS), souadnými soustavami a výstupy ze senzoru. 2.1 Navigace Navigace je souhrnný název pro postupy, jimiž lze kdekoliv na Zemi, moi i obecn v njakém prostoru stanovit svou polohu nebo polohu jiného pemisovaného objektu. Termín je odvozen z latinského slova navis znamenajícího lo. Pvodn slovo znamenalo plavbu po moi, ale význam se metonymicky penesl na zjišování polohy a smru i volbu trasy. Metaforicky se rozšíil na další druhy dopravy a další innosti (1). Základní metody navigace jsou: Navigace podle orientaních bod Navigace podle hvzd Navigace výpotem Radiová navigace Družicová navigace Inerciální navigace Inerciální navigace Inerciální navigace je zpsob navigace, kdy se používají pouze inerciální senzory, jako jsou akcelerometry a gyroskopy (resp. senzory úhlových rychlostí) pro mení pohybu jednotky. Akcelerometry mí okamžité zrychlení a gyroskopy okamžitou úhlovou rychlost navigovaného objektu. Z tchto hodnot se dá vypoítat aktuální pozice. Princip inerciální navigace spoívá ve výpotu souasné rychlosti a pozice jednotky ze známé poátení polohy a zaznamenaných zmn zrychlení ve všech ortogonálních osách. Tento princip je celkem jednoduchý, ale praktická realizace už tak jednoduchá není, protože pi integraci nameného signálu se integruje i pípadná chyba senzoru (1). Pro inerciální navigaci se používají ti gyroskopy ve tech osách pro urení náklonu v každé ose a ti akcelerometry ve tech osách pro urení zrychlení v každé ose. Výhodou inerciální navigace je její sobstanost. To znamená, že pro plnou funkci navigace nejsou poteba žádné další zdroje informací, jako napíklad u GPS, kde je poteba GPS pijíma, ale také GPS satelity. Nevýhodou je zatím stále nedostatená pesnost inerciálních senzor (1). 2.2 Akcelerometry Akcelerometr je inerciální senzor, který využívá setrvanosti hmoty pro mení rozdílu mezi kinematickým zrychlením a gravitaním zrychlení. Dnes se pevážn využívají akcelerometry typu MEMS (mikro-elektromechanické 2

11 akcelerometry), které pracují na principu mechanického kmitavého systému. Tento systém je tvoen vetknutým nosníkem k a hmotností m. Deformaci nosníku pak snímají tenzometry umístné poblíž místa vetknutí nosníku jak je znázornno na Obr. 2.1 (2). Obr. 2.1: Akcelerometr s kmitajícím nosníkem (2 str. 39) Akcelerometr typu MEMS v integrované podob je zobrazen na Obr Základem takového akcelerometru je destika z polykrystalického kemíku, tvarovaná do dvou pružných ttiv, zakotvených na monokrystalickém kemíkovémm substrátu. Tyto ttivy pedstavují tuhostt k mechanického oscilátoru a jsou spojeny hebínkem, který pedstavuje seismickou hmotnost m. Každý zub hebínku pedstavuje stední pohyblivou elektrodu X soustavy diferenciálních kapacitních senzor. Jako pevné elektrody slouží systém nosník Y a Z. Kapacita mezi elektrodami X a Z se zvýší a mezi elektrodami X a Y sníží pi psobení horizontálního zrychlení ve smru doprava ve smyslu Obr Pro opané psobení zrychlení se kapacita mní obrácen. Tato MEMS technologie dnes nahrazuje vtšinu klasických mechanických senzor, protože akcelerometry tohoto typu mají mnohem menší rozmry, nižší energetickou spotebu a podstatn nižší cenu (2). Dnes je možné vybrat z nkolika možných princip mení zrychlení pomocí akcelerometr a to kapacitní, piezorezistorové a piezoelektrické mení. Kapacitní mení pohybu má na rozdíl od piezorezistorového a piezoelektrického nkolik výhod a to jeho teplotní stabilitu, opakovatelnost mení, CMOS obvodovou kompatibilitu a schopnost mení zrychlení o nízké frekvenci (3). Z a Obr. 2.2: Akcelerometr MEMS (2 str. 4) tchto dvod akcelerometry vyrábné technologii bulk MM (slepení senzor elektronických obvod) používají kapacitní mení. U tchto akcelerometr 3

12 došlo ke zlepšení mnoha parametr, pesto mezi limitující faktory stále patí omezení jen jedné osy snímání, nízká rezonanní frekvence a vysoká cena (3). Daleko lepších parametr a pijatelné ceny dosáhly až akcelerometry kombinující kapacitní mení a MM (mikromechanická) technologie. Výroba takovéto kombinace se dá realizovat dvouipovou metodou nebo jednoipovou metodou. Dvouipová metoda spoívá ve výrob samotného senzoru technologií MM a jeho zapouzdení a spojení s ipem pro úpravu signálu. Jednoipová metoda spoívá v integraci senzoru i obvod pro úpravu signálu na jednom ipu. Tato technologie je nazývána imems (integrované mikroelektromechanické systémy) (3). Pinov kompatibilní digitální akcelerometry mají pibližn stejné parametry i cenu a díky tomu, že fungují na zcela odlišném principu, dosahují mnohonásobn vtší odolnosti proti nárazm (až 5. g). Jsou ureny pro mení zrychlení s frekvencí až do 4 Hz. Tyto akcelerometry nevyužívají pohyblivé ásti, resp. seismickou hmotnost m, ale pouze odporový materiál ve stedu akcelerometru, který ohívá vzduch, jenž ve svém okolí vytváí teplotní gradient. V ustáleném stavu akcelerometru, tedy pokud na nj nepsobí zrychlení, symetricky rozmístná teplotní idla zaznamenávají kolem odporového materiálu stejnou teplotu. Pi psobení zrychlení dojde k namení diferenní teploty mezi jednotlivými idly, která je úmrná zrychlení (3). Mezi další mnohem dražší akcelerometry patí nap. elektromechanické a rezonanní akcelerometry. Krom vysoké ceny mají elektromechanické akcelerometry navíc píliš velké rozmry (3). 2.3 Senzory úhlových rychlostí Úhlová rychlost je údaj o tom jak se mený objekt rychle otáí v jednotkách radián za sekundu (rad/s). Pohyb, zmnu polohy nebo natoení a otáení je možné mit senzory úhlových rychlostí tzv. gyroskopy, které jsou již dlouhou dobu využívány pro mení a urování zmny polohy nebo natoení jakéhokoli objektu, ke kterému jsou pipevnné. Díve se však používalo jen mechanické provedení nebo provedení s využitím svtla, tedy pomocí optických vláken. V dnešní dob je ale už možné díky technologii MEMS využívat i v integrované podob klasických souástek, které obsahují nejen samotný senzor, ale i vyhodnocovací obvody vetn logiky. Výstup, který získáme, mže být potom jak analogový tak digitální, a proto je možno využívat senzory úhlových rychlostí nejen ve vd a výzkumu, ale i v bžných aplikacích (4). Gyroskopy mají mnoho využití napíklad pi: Navádní a ízení raket, letadel, robot apod. Detekci a mení rotaního pohybu Stabilizaci ve stabilizaních jízdní systémech automobil Zpesování pozice systém GPS Stabilizaci obrazu a pedmt Zjišování zmny polohy, detekci pohybu Mení setrvanosti Mení náklonu Detekci pevrácení, nap. automobilu 4

13 2.3.1 Senzory úhlové rychlosti na principu Coriolisovy síly Princip mení pomocí Coriolosovy síly je vidt na Obr. 2.3, kde horní ramena vidlice jsou rozkmitávána elektrostaticky v rovin plochy senzoru a radiální rychlost jejich pohybu má amplitudu. Kolem hlavní osy se tyto vidlice otáejí menou úhlovou rychlostí.. Tím na ramena kmitající v protifázi psobí Coriolisova síla dle rovnice ( 2.1 ) a na spojnici obou ramen vznikne krouticí moment, úmrný hodnot úhlové rychlosti. Tento krouticí moment vybudí kmity dolní vidlice. Amplituda tchto kmit dolní vidlice mže být snímána napíklad kapacitn a je úmrná mené úhlové rychlosti. Na podobném principu jsou založeny MEMS gyroskopy (2). Obr. 2.3: Senzor úhlové rychlosti na principu Coriolisovy síly (2 str. 43) Obecný princip gyroskop MEMS Mit rotaci lze vzhledem k jedné ze tí os x, y, z. Na Obr. 2.4 jsou oznaeny jako podélná osa (roll axis), svislá osa (yaw axis) a píná osa (pitch axis). V této práci využívané gyroskopy, pracující na principu Coriolisovy síly, jsou vyrábné jako integrované MEMS obvody, které umjí mit pouze v jednom smru a to v kolmém na plochu ipu (yaw axis). Aby mohly být využívány pro jiné smry je nutné správné natoení a umístní souástky. Proto se využívají ti gyroskopy umístné tak, aby jejich osy citlivosti byly vzájemn ortogonální (4). Obr. 2..4: Mení natoení a rotace (4) 5

14 2.3.3 Coriolisova síla a její projevy Pro zaátek je dležité upesnit pojem Coriolisova síla pro lepší porozumní principu v práci využívaných senzor gyroskop. Coriolisova síla je virtuální síla, která psobí na libovolný hmotný pedmt i objekt, který se pohybuje rychlostí v v soustav rotující kolem osy rotace úhlovou rychlostí. Coriolisova síla se dá vyjádit dle rovnice ( 2.1 ), kde m je hmotnost pohybujícího se objektu, v je rychlost objektu a je úhlová rychlost rotující soustavy (4). ( 2.1 ) Názorný píklad vysvtluje Obr. 2.5, kde se slena pohybuje uritou rychlostí v od stedu rotujícího kruhu k jeho okraji (oranžová šipka). Na slenu pitom psobí vzrstající Coriolisova síla (modrá šipka), která se zvtšuje smrem k okrajm, kde dosahuje maximální velikosti. V globálním hledisku Coriolisova síla psobí na veškeré hmotné objekty na Zemi, protože práv naše planeta je typickým píkladem soustavy otáející se pravideln kolem své osy. Na severní polokouli tak psobí podle vzorce ve smru hodinových ruiek (stáí pedmt v tomto smru), na jižní polokouli pak v protismru. Na pólech je síla maximální, zatímco na rovníku nulová. V praxi mžeme Coriolisovu sílu pozorovat pi toení vír pi odtoku vody v umyvadle nebo u tornáda, dále u vymletých pravých beh ek nebo více opotebovaných pravých kolejnic na jednosmrných železniních tratích (4). Obr. 2.5: Píklad psobení Coriolisovy síly (4) U mechanických gyroskop se využívá stejného úinku, jak znázoruje Obr. 2.6, kde vlevo se mechanický gyroskop pohybuje k okraji kruhu a vpravo ke stedu kruhu. Coriolisova síla psobí pi pohybu objektu, který je upevnn na pružinách uvnit rámu, smrem doleva, pi opaném smru pohybu objektu zase doprava. Tento systém lze úspšn využívat pro mení velikosti a smru této síly, protože jak velikost, tak smr síly je úmrný jak velikosti úhlové rychlosti, tak i smru otáení (4). 6

15 Obr. 2.6: Psobení Coriolisovy síly na mechanický gyroskop (4) Píklad provedení samotného MEMS snímae V integrovaných gyroskopech na principu Coriolisovy síly se využívá technologie MEMS. Samotný senzor je pak tvoen na ipu elektronickými obvody i mechanickými mikrosouástkami. V praxi existuje nkolik odlišných struktur, avšak princip je všude podobný a dá se znázornit pomocíobr. 2.7(4) Obr. 2.7: Zjednodušená struktura snímae MEMS gyroskopu (4) Základem je rezonující struktura, která je upevnná ve vnitním. Ta se pod vlivem vlastní mechanické rezonance, zde reprezentované pružinami, pohybuje v uvedeném smru, který je kolmý na smr otáení dle Obr Zde vzniká Coriolisova síla, která je úmrná úhlové rychlosti otáení, a která stlaí vnjší pružiny rámu a tak zpsobí vzájemný posuv micích plošek, fungujících jako elektrody vzduchových kondenzátor. Zmna kapacity je úmrná úhlové rychlosti otáení rad/s, která je výstupním signálem (4). 7

16 2.4 íslicové filtry Obr. 2.8: Píklad funkce strukturyry snímae gyroskopu pi rotaci (4) íslicový filtr je obvod nebo algoritmus (program), který mní požadovaným zpsobem spektrum vstupního signálu. Pomocí filtr lze docílit potlaení rušivých vliv nebo frekvenn analyzovat vstupní signál. Výhodou íslicových filtr je oproti analogovým filtrm snadná zmna vzorkovací frekvence nebo zvýšení ádu filtru pomocí zptné vazby. To vše je možné i pro vstupní signály s frekvencí nižší než 1 Hz (5). Filtry se dlí dle své struktury na filtry nerekursivní (NRDF), které neobsahují ve svém blokovém schématu zptnou vazbu, a filtry rekursivní (RDF), které zptnou vazbu ve své struktue mají. Podle délky impulsní odezvy se íslicové filtry dlí na filtry s konenou impulsní odezvou (FIR filtry) a filtry s nekonenou impulsní odezvou (IIR filtry). Filtry NRDF jsou vždy typu FIR. IIR filtry jsou vždy RDF. Vlastnosti filtr FIR a IIR se liší, a liší se také metody návrhu tchto dvou typ filtr (5). FIR filtry jsou vždy stabilní, mají v praxi vždy pesn lineární fázi, jejich ád je obvykle vysoký a proto výraznji zpožují výstupní signál proti signálu vstupnímu. Mohou být užity pro návrh filtru s libovolnou frekvenní charakteristikou (nejen dolní propust, horní propust, pásmová propust a pásmová zádrž, ale také nap. filtr s nkolika propustnými pásmy). Klasická metoda návrhu FIR filtr je metoda okénkování, dležitou metodou návrhu je návrh filtr s konstantním zvlnním frekvenní charakteristiky, který využívá ebyševových funkcí (5). Návrh íslicových filtr, ili dosažení požadovaného tvaru frekvenní charakteristiky filtru, je založen na toleranním diagramu. Frekvenní charakteristika (frekvenní odezva) filtr FIR je vždy polynom zatím co frekvenní charakteristika IIR filtr je podíl dvou polynom. Frekvenní charakteristiky íslicových filtr jsou periodické s periodou rovnou vzorkovací frekvenci (5). 8

17 2.5 Popisy stav a vztah používané v inerciální navigaci Všechny rovnice jsou použity dle literatury (6). V inerciální navigaci se pro popisy stav a vztah mezi soustavami používají matice smrových kosin (DCM), kvadriky (quaternion), rotaní vektory a rychlosti otáení. Transformace souadné soustavy b do souadné soustavy a je popsána smrovou maticí kosin, kvadrikou, rotaním vektorem. Rychlost otáení soustavy b vzhledem k soustav a vyjádena v soustav c je popsána vektorem rychlosti otáení. Kvadrika je složená ze skalární ásti s a tí-dimenzionálního vektoru v dle rovnice ( 2.2 ). Na rozdíl od rotaního vektoru lze kvadriky kombinovat pro skládání pohyb nebo souet rotací dle rovnic ( 2.3 ). ( 2.2 ) ( 2.3 ) Matice smrových kosin neboli rotaní matice je reálná ortogonální matice 3x3 korespondující s kvadrikou. Matice smrových kosin se dají jednoduše kombinovat jednoduchým maticovým násobením dle rovnice ( 2.4 ). ( 2.4 ) Korespondenci mezi kvadrikou a maticí smrových kosin popisuje pevod z kvadriky na matici smrových kosin dle rovnice ( 2.5 ) nebo obrácený pevod matice smrových kosin na kvadriku dle rovnic ( 2.8 ), kde jsou hodnoty získané pomocí rovnic ( 2.6 ). V tchto rovnicích je použita diagonální suma matice oznaená, kterou lze vypoítat dle rovnice ( 2.7 ). ( 2.5 ) ( 2.6 ) ( 2.7 ) 1 Zkratka tr je z anglického slova trace, tedy stopa matice 9

18 ( 2.8 ) Rotaní vektor definuje osy rotace a velikost rotace okolo rotaního vektoru. Na Obr. 2.9 je soustava b zarovnaná se soustavou a a pi rotaci okolo docílí finální polohy soustavy b, což je ekvivalent rotace soustavy a do soustavy b. Kombinace dvou rotaních vektor není možná, proto musíme nejdív rotaní vektor transformovat na kvadriku nebo matici smrových kosin. Ty kombinovat a výsledek zptn transformovat na rotaní vektor. Obr. 2.9: Rotaní vektor (6 str. 11) Z rotaního vektoru ze soustavy a do soustavy b mžeme vypoítat kvadriku ze soustavy a do soustavy b dle rovnice ( 2.9 ) nebo ze soustavy b do soustavy a dle rovnice ( 2.1 ), kde zápis oznauje Eukleidovskou normu trojrozmrného vektoru dle rovnice ( 2.11 ) (6). 1

19 ( 2.9 ) ( 2.1 ) ( 2.11 ) 2.6 Souadné soustavy a jejich transformace V INS mechanizaci a analýze se používá inerciální souadná soustava, zemská souadná soustava, naviganí souadná soustava a souadná soustava navigované jednotky. Vektory popsané v jedné soustav se transformují do jiné soustavy. Namené zrychlení a úhlové rychlosti jsou vztažené k souadné soustav micí jednotky (IMU). Pro výpoet naviganích ukazatel jako je zempisná poloha v zemské souadné soustav, rychlost a úhly náklonu navigovaného objektu, se musí tyto namené hodnoty transformovat do naviganí souadné soustavy Inerciální souadná soustava Inerciální soustava (IF) je ideální soustava, kde mají ideální akcelerometry a gyroskopy sesouhlasené s IF nulové výstupy. Pesnou IF je velmi obtížné sestrojit, proto se v praxi používá takzvaná kvazi-inerciální soustava. Ta má poátek ve stedu Zem a její osy nerotují závisle na vzdálených galaxiích. Osa z je paralelní s osou otáení Zem, osa x smuje na bod jarní rovnodennosti a osa y dopluje ortogonální souadný systém dle pravidla pravé ruky viz Obr Bod jarní rovnodennosti (Vernal equinox) je dán prseíkem nebeského rovníku (Celestial Equator), což je kružnice na obloze vytvoená prmtem svtového rovníku, a ekliptiky (Ecliptic), což je kružnice na obloze vytvoená prseíkem roviny dráhy Zem okolo Slunce s oblohou (6). Obr. 2.1: Inerciální souadná soustava (6 str. 19) 11

20 2.6.2 Zemská souadná soustava Zemská souadná soustava (ECEF) je soustava, která má poátek ve stedu Zem a její osy jsou pevn svázány s osami Zem. Osa x smuje na Greenwichský poledník, osa z je paralelní s osou otáení Zem a osa y dopluje ortogonální souadný systém dle pravidla pravé ruky. Vektor rychlosti otáení ECEF vzhledem k IF zobrazený v ECEF je popsaný dle rovnice ( 2.12 ), kde je rotaní rychlost Zem (6). ( 2.12 ) Polohový vektor ECEF je popsaný dle rovnice ( 2.13 ), kde je geodetická zempisná šíka, je geodetická zempisná délka, h je nadmoská výška, e je excentricita referenního elipsoidu a je polomr kivosti normály (6). ( 2.13 ) Vztah mezi ECEF a svtovými souadnicemi viz Obr Obr. 2.11: Vztah ECEF a geodetických souadnic (6 str. 21) Polomr Zem Protože Zem není homogenní koule, nedá se brát její polomr jako konstanta. Pi pohybu po povrchu Zem, tedy pi zmn zempisné délky a šíky, se bude mnit i polomr Zem. Aktuální hodnotu polomru Zem lze definovat jako polomr kivosti poledníku nebo polomr kivosti normály. Polomr kivosti popisuje zmnu polomru Zem vzhledem k mnicí se zempisné 12

21 šíce a vzhledem k mnicí se zempisné délce. Aktuální polomry a se dají vypoítat dle upravených rovnic ( 2.14 ). Výpoet je proveden pomocí rovníkovéhoo polomru Zem, tedy vzdálenosti stedu zem k rovníku, která je, pomocí polárního polomru Zem, tedy vzdálenosti stedu Zem ksevernímu a jižnímu pólu, která je a pomocí excentricity neboli výstednosti Zem, kterou popisuje rovnice ( 2.15 ) (7). ( 2.14 ) ( 2.15 ) Naviganí souadná soustava Naviganí souadná soustava (NED) je lokální geodetická soustava, která má poátek stejný jako soustava senzor, ale její osy jsou pevn dány. Osa x smuje na geodetický sever, osa z smuje ortogonáln dol od referenního elipsoidu do stedu Zem a osa y dopluje ortogonální souadný systém dle pravidla pravé ruky viz Obr (6). Obr. 2.12: Zemská a naviganí souadná soustava (6 str. 23) Matici smrových kosin (DCM) z NED do ECEF lze vypoítat geodetické zempisné šíky a délky dle rovnice ( 2.16 ) (6). pomocí ( 2.16 ) 13

22 Rychlost otáení Zem v NED popisuje rovnice ( 2.17 ). ( 2.17 ) Rychlost otáení NED vzhledem k ECEF se nazývá transportní rychlost a lze ji popsat dle rovnice ( 2.18 ), kde a jsou rychlosti pohybu NED ve východním (E) a severním (N) smru a je polomr kivosti poledníku (6). ( 2.18 ) Souadná soustava micí jednotky (IMU) Souadná soustava micí jednotky (BF) je soustava pevn svázaná s jednotkou. Její poátek je v míst prseíku micích os akcelerometr a gyroskop, a její osy x, y a z jsou soubžné s micími osami akcelerometr a gyroskop, takže výstupní hodnoty senzor jsou vždy v této soustav, a musí se nejprve transformovat do NED (6). 2.7 Mechanizace INS Existují dva druhy mechanizace INS a to pímá (forward) a zptná (backward), což je metoda post procesingu. Pomocí zptné mechanizace se dají vypoítat z naviganích ukazatel ve dvou okamžicích za sebou zmny rychlostí a úhlových rychlostí. Zatímco pímou mechanizací mžeme vypoítat ze zmn rychlostí a úhlových rychlostí naviganí ukazatele. V této práci se využívá pouze pímá mechanizace, protože výstupní hodnoty z naviganí jednotky jsou aktuální zrychlení a aktuální úhlová rychlost. A tyto veliiny se integrují práv na zmny rychlostí a zmny úhl (6). Pímá mechanizace je výsledkem dvacetiletého vývoje inerciálních naviganích algoritm. Každý diskrétní algoritmus mechanizace musí vycházet ze svého spojitého protjšku, který lze popsat tymi rovnicemi ( 2.19 ), kde je rychlost NED ve smru do stedu Zem (6). Pevedení do diskrétního asu, zjednodušení a implementace do kódu je popsána v odstavci 4.2, kde jsou tyto rovnice rozdleny do tí kapitol a to na ást výpot rychlostí, ást výpot polohy a ást výpot polohových úhl. ( 2.19 ) 14

23 2.8 Výstupy ze senzor K dosažení pesné digitalizace mených zrychlení a úhlových rychlostí se používá pesná analogová integrace jako ást digitalizace. Výstup takových systém je pak zmna úhl a zmna rychlosti. Nicmén vtšina levnjších jednotek obvykle má jako výstup pímo úhlové rychlosti a zrychlení. V takových systémech se musí tato integrace provést dodaten a to dle rovnic ( 2.2 ) (6). ( 2.2 ) 15

24 3 Mení V této kapitole jsou popsány naviganí jednotky 3DM-GX2 (MicroStrain) a AHRS-M3 (Inalabs), které mly být pi mení použity. Dále jsou popsána simulovaná data pro testování algoritmu a prbh a zpsob reálného mení na jednotce v laboratoi. 3.1 Naviganí jednotky Mení mlo být provedeno dle zadání práce na dvou naviganích jednotkách a to 3DM-GX2 (MicroStrain) a AHRS-M3 (Inalabs) DM-GX2 (MicroStrain) 3DM-GX2 je gyroskopický senzor orientace, který využívá technologii miniaturních MEMS sníma. Kombinuje trojosý akcelerometr, trojosý gyroskop, trojosý magnetometr, teplotní senzor a vestavný procesor. Jako výstupní data je možné zvolit zrychlení, úhlovou rychlost, magnetické pole, vektory zmny úhl a rychlostí nebo vypotenou orientaci (Pitch a Roll) nebo rotaní matici. Všechny tyto hodnoty jsou z kalibrovaného inerciálního mení, teplotn kompenzované v celém micímm rozsahu a korigované senzorem vyosení. Úhlová rychlost je ješt korigována o citlivost na gravitaní sílu a nelinearitu stupnice (8). Komunikaní hardware je umístn v oddlitelném modulu, a proto ho lze snadno pizpsobit. V souasné dob je k dispozici rozhraní pro bezdrátový vysíla, USB 2., RS232 a RS422. Rychlost penosu dat mže nastavit uživatel od 1až do 25Hz. Pro mení bylo použito komunikaní rozhraní USB 2. a vzorkovací frekvence. Bloková struktura jednotky dle Obr. 3.1 (8). Obr. 3.1: 3DM-GX2 struktura (8) Architektura celého systému je navržena tak, aby výrazn eliminovala spolené zdroje chyb, jako je napíklad hysterezee vyvolaná zmnou teploty a citlivost na kolísání napájecího naptí. Používá šest nezávislých Delta-Sigmaa A / D 16

25 pevodník (jeden pro každé idlo), což zajišuje, že všechna idla jsou vzorkována souasn, a tím dosáhne nejlepších výsledk integrace. 3DM-GX2 zaleuje tíosý magnetometr, který je volitelný a mže být oddlen od akcelerometr a gyroskop, a tím snížit rušení, zpsobené železnými materiály (8). Tato jednotka je velmi malá a lehká. Díky tomu je vhodná pro aplikace, kde je poteba šetit místo nebo se musí pemísovat. Fotografie jednotky viz Obr Obr. 3.2: Jednotka 3DM-GX2 Technické parametry jednotky viz Tabulka 5 v pílohách AHRS-M3 (Inalabs) Innalabs Attitude and Heading Reference System (AHRS) je vysoce výkonný strapdown systém, který v reálném ase uruje pesné polohové úhly (Roll, Pitch, Heading) v 3D prostoru. Výstupní data vysílá po RS232 nebo USB 2.. Bloková struktura jednotky dle Obr. 3.3 (9). Obr. 3.3: AHRS struktura (9) Jednotka AHRS se skládá ze tí akcelerometr, tí gyroskop a velmi pesného trojosého magnetometru. Všechny namené hodnoty jsou teplotn 17

26 kompenzované v celém micím rozsahu. Gyroskopy se používají k mení absolutní úhlové rychlosti jednotky. Polohové úhly jednotky (Heading, Pitch, Roll) jsou poítány pomocí integrace výstupu z gyroskop. Akcelerometry jsou použity ke stanovení poáteních polohových úhl jednotky a upravení drift gyroskop v polohových úhlech Pitch a Roll v provozním režimu. Magnetometr je použit ke stanovení poáteního zarovnání jednotky v azimutu a k upravení driftu gyroskopu v polohovém úhlu Heading v provozním režimu (9). Tato jednotka je velmi malá, lehká a odolná proti nárazm a vibracím. Díky tomu je vhodná pro aplikace, kde je poteba šetit místo nebo se musí pemísovat. Fotografie jednotky viz Obr Obr. 3.4: Jednotka AHRS-M3 Technické parametry jednotky viz Tabulka 6 v pílohách. 3.2 Simulace mení Algoritmus byl ovován pomocí rotace Zem a šesti rzných pohyb jednotky a to od jednoduché nehybnosti jednotky vzhledem k NED až po složitý spirálovitý pohyb jednotky. Pro každý takový pohyb jednotky byla vytvoena Matice reprezentující simulované signály z jednotky a uložená do souboru sim(íslo simulace).mat, které jsou piloženy na CD. Tyto matice byly vytvoeny pomocí funkce generator viz Tabulka Rotace Zem Signál pro simulaci mení rotace Zem byl simulován pouze konstantním gravitaním zrychlením -1 g psobící na jednotku ve smru osy z. Ob zbývající zrychlení ve smru osy x a y byly nulové a všechny ti úhlové rychlosti byly také nulové. Poátení poloha byla nastavena na prseík rovníku a Greenwich poledníku s nulovou nadmoskou výškou. 18

27 Natoceni ( ) Obr. 3.5: Zmna úhlu natoení Roll Na Obr. 3.5 je vidt, že i pi nulových úhlových rychlostech se jednotka natáí a to vlivem otáení Zem. Natoení Zem se vypote dle rovnice ( 3.1 ), kde je rychlost otáení Zem a t je doba otáení. ( 3.1 ) Pi simulaci trvající 25 s vyjde úhel a dle rovnice ( 3.1 ) vyjde. Z tchto dvou úhl, které se liší ádov ve stotisícinách stupn což je zanedbatelný rozdíl, je vidt, že algoritmus správn koriguje otáení jednotky zpsobené rotací Zem. Psobení rotace Zem závisí na zempisné šíce, protože na rovníku je v ose x maximální a v ose z nulová a na pólech obrácen. Tato závislost je popsána rovnicí ( 2.17 ) Nehybnost jednotky vzhledem k NED Signál pro nehybnost jednotky byl simulován pomocí: konstantního gravitaního zrychlení -1 g psobící na jednotku ve smru osy z konstantní úhlové rychlosti okolo osy x a osy z zpsobené otáením Zem ovenou v pedchozí podkapitole Jako poátení poloha byla nastavena zempisná poloha a nadmoská výška letecké laboratoe s151 v Dejvicích. 19

28 Vzdalenost (um) Vzdalenost (um) Vzdalenost (um).5 Smer sever Smer vychod Zmena nadmorské vysky Obr. 3.6: Pohyb jednotky v NED Na Obr. 3.6 je vidt, že jednotka se i pes nehybnost vi NED pohnula v ádech mikrometr. Tento pohyb je z hlediska mení zanedbatelný a je zpsoben nepesnou simulací gravitaního zrychlení a otáení Zem Pohyb jednotky v jednom smru bez natoení Signál pro pohyb ve východním smru o jeden metr bez natoení byl simulován pomocí: konstantního gravitaního zrychlení ve smru osy z zrychlení 1,194 g v t = 1 s a zrychlením -1,194 g v t = 2 s ve smru osy y konstantní úhlové rychlosti jednotky okolo osy x a osy z zpsobené otáením Zem Na Obr. 3.7 je vidt prbh simulovaného signálu. 2

29 Zrychleni (g) Zrychleni (g) Zrychleni (g) Obr. 3.7: Simulovaný signál zrychlení v ose y Vzdalenost (m) Obr. 3.8: Pohyb ve východním smru 21

30 Na Obr. 3.8 je vidt, že vypotený pohyb jednotky odpovídá simulovanému zadání. Vypotený pohyb vyšel 997,86 mm, odchylka je tedy v milimetrech, což je zpsobeno nepesnou simulací Pohyb jednotky ve dvou smrech bez natoení Signál pro pohyb ve východním smru o pl metru a v severním smru o pl metru bez natoení byl simulován pomocí: konstantního gravitaního zrychlení ve smru osy z zrychlení,597 g v t = 1 s a zrychlením -,597 g v t = 2 s ve smru osy x a y konstantní úhlové rychlosti jednotky okolo osy x a osy z zpsobené otáením Zem Na Obr. 3.9 je vidt prbh simulovaných signál v ose x a y. 1 Osa x Zrychleni (g) Osa y 1 Zrychleni (g) Obr. 3.9: Simulované signály zrychlení v ose x a y 22

31 .6 Smer sever Vzdalenost (m) Smer vychod.8 Vzdalenost (m) Obr. 3.1: Pohyb jednotky v NED.5.45 Vzdalenost ve vychodnim smeru (m) Vzdalenost v severnim smeru (m) Obr. 3.11: Horizontální dráha jednotky 23

32 Na Obr. 3.1 je vidt, že vypotený pohyb v obou smrech odpovídá simulovanému zadání, tedy pohybu o pl metru v obou smrech. Na Obr je zobrazena horizontální dráha, která odpovídá pohybu po úhlopíce tverce o velikosti strany pl metru. Vypotený pohyb v severním smru vyšel 499,717 mm a ve východním smru 499,517 mm, odchylka je tedy v desetinách milimetru Pohyb zatáející jednotky Signál pro zatáející pohyb, tedy pro pohyb jednotky v jednom smru o jeden metr s prbžným natáením okolo osy z celkem o 9. Takový signál byl simulován pomocí: konstantního gravitaního zrychlení v ose z konstantního zrychlením,2 g od t = 1 s ve smru osy x konstantní úhlové rychlosti,1571 rad/s okolo osy z od t = 1 s, která je pitena k úhlové rychlosti v ose z zpsobené rotací Zem úhlovou rychlostí zpsobenou rotací Zem, která již v tomto pípad není okolo osy x konstantní, ale mní se s natoením v ose z a to dle rovnice ( 3.2 ) vzniklé úhlové rychlosti v ose y dle rovnice ( 3.3 ) Na Obr je vidt simulovaný prbh zrychlení v ose x a na Obr jsou zobrazeny simulované prbhy úhlový rychlostí ve všech osách. ( 3.2 ) ( 3.3 ) 24

33 2 x Zrychleni (g) Obr. 3.12: Simulovaný signál zrychlení v ose x Uhlova rychlost (rad/s) Uhlova rychlost (rad/s) Uhlova rychlost (rad/s) 5 x 1-5 Osa x x 1-5 Osa y Osa z Obr. 3.13: Simulované signály úhlových rychlostí 25

34 1 Smer sever Vzdalenost (m) Smer vychod.2 Vzdalenost (m) Obr. 3.14: Pohyb jednotky v NED.5 Vzdalenost ve vychodnim smeru (m) Vzdalenost v severnim smeru (m) Obr. 3.15: Horizontální dráha jednotky 26

35 Natoceni ( ) Obr. 3.16: Zmna úhlu natoení Heading Na Obr je vidt vypotený pohyb v obou osách. Vypotená vzdálenost v severním smru je 824,95 mm a ve východním smru 44,553 mm. Úhlopíka tohoto obdélníku vyšla 934,462 mm, což odpovídá dráze jednoho metru se zakivením ze zadání simulace. Na Obr je vidt vypotená horizontální dráha, kterou jednotka urazila a na Obr je vidt prbh natoení jednotky v ase. Vypotený výsledný úhel je 89,131, což tém odpovídá zadání simulace Pohyb jednotky do spirály Signál pro spirálovitý pohyb jednotky s polomrem 8 m, stoupáním 1 m a celkovým otoením o 9, tedy s oto ením jednotky o dv a pl kružnice, byl simulován pomocí: konstantního gravitaního zrychlení, ke kterému bylo v ase t = 5 s piteno aktuální zrychlení,597 g v ose z aktuálního zrychlení 65 g v ase t = 5 s v ose y konstantního zrychlení,5 g od t = 5 s v ose x konstantní úhlové rychlosti,7854 rad/s od t = 5 s v ose z vypotených úhlových rychlostí v osách x a y dle rovnic ( 3.2 ) a ( 3.3 ) Na obrázku Obr jsou vidt prbhy simulovaných zrychlení a na Obr prbhy simulovaných úhlových rychlostí. 27

36 Zrychleni (g) Zrychleni (g) Zrychleni (g).5 Osa x Osa y Osa z Obr. 3.17: Simulované signály zrychlení Uhlova rychlost (rad/s) Uhlova rychlost (rad/s) Uhlova rychlost (rad/s) 5 x 1-5 Osa x x 1-5 Osa y Osa z Obr. 3.18: Simulované signály úhlových rychlostí 28

37 Vzdalenost (m) Vzdalenost (m) Vzdalenost (m) 2 Smer sever Smer vychod Zmena nadmorské vysky Obr. 3.19: Vypotené pohyby jednotky v NED 12 1 Vzdalenost ve vychodnim smeru (m) Vzdalenost v severnim smeru (m) Obr. 3.2: Vypotená horizontální dráha jednotky 29

38 1.2 Vzdalenost ve smeru nahoru (m) Vzdalenost v severnim smeru (m) Vzdalenost ve vychodnim smeru (m) Obr. 3.21: Vypotená 3D dráha jednotky Natoceni ( ) Obr. 3.22: Vypotené natoení jednotky v ose z 3

39 Na Obr jsou vidt vypotené pohyby ve všech osách a na Obr. 3.2 je vidt horizontální pohyb jednotky, který zhruba opisuje zadanou kružnici spirály. Odchylka od pesné kružnice je zpsobena nepesnou simulací. Na Obr je vidt pohyb jednotky v prostoru tedy i docílení stoupání o jeden metr. A na Obr je vidt natoení jednotky v ase, které odpovídá otoení o 9, tedy otoení o dv a pl kružnice. 3.3 Laboratorní mení V laboratoi byl odmen pohyb jednotky v jednom smru o pibližn jeden metr bez rotace a s desetivteinovým klidovým stavem v poátku pro výpoet poáteního natoení jednotky. Na Obr jsou vidt namené hodnoty z akcelerometr ovlivnné biasy, tedy systematickou chybu projevující se nadhodnocením nebo podhodnocením skutené hodnoty, a na Obr namené hodnoty z gyroskop. Je vidt, že tyto hodnoty jsou ovlivnny šumem, proto je nutné je ped samotným výpotem filtrovat pomocí filtr popsaných v kapitole Na Obr je vidt rozdíl filtrovaného a nefiltrovaného signálu z akcelerometru v ose x pomocí prvního navrženého filtru a na Obr je vidt rozdíl filtrovaného a nefiltrovaného signálu z gyroskopu v ose y pomocí druhého navrženého filtru. Filtrovány byly signály ve všech osách z akcelerometr i gyroskop. Zrychleni (g) Zrychleni (g) Zrychleni (g).2 Osa x Osa y Osa z Obr. 3.23: Namené signály zrychlení 31

40 Uhlova rychlost (rad/s) Uhlova rychlost (rad/s) Uhlova rychlost (rad/s).5 Osa x Osa y Osa z Obr. 3.24: Namené signály úhlových rychlostí.2.15 Nefiltrovaný Filtrovaný.1 Zrychleni (g) Obr. 3.25: Porovnání signál z akcelerometru pi filtraci 32

41 .25.2 Nefiltrovaný Filtrovaný.15 Uhlova rychlost (rad/s) Obr. 3.26: Porovnání signál z gyroskopu pi filtraci Bohužel namená data z naviganí jednotky jsou tak ovlivnna šumem, který je zpsoben vlivem neortogonalit a chyb SF, že vypotená poloha má velkou odchylku od skutené polohy naviganí jednotky na konci mení. Ani pi použití navržených filtru není výpoet dostaten pesný. Srovnání vypotených dat viz Tabulka 1. Pro pesnjší výpoet by musela být jednotka kalibrována nebo na namená data použita nap. vlnková transformace, což je nad rámec této bakaláské práce. Tabulka 1: Srovnání vypotených poloh naviganí jednotky 33

42 4 Zpracování namených dat Kapitola popisuje vlastní implementaci algoritmu mechanizace inerciálního naviganího systému v Matlabu. Pro zjednodušení je rozdlena na ti hlavní kapitoly a jejich podkapitoly, a to na úpravu namených dat z naviganí jednotky s jejich korekcí, implementaci mechanizace INS, která je ješt dále rozdlena do podkapitol výpot poáteních hodnot, aktualizace rychlostí, aktualizace polohy a aktualizace polohových úhl, a pevody vypotených hodnot mezi souadnými soustavami. 4.1 Úprava namených dat Pi výpotu polohy, rychlosti a úhl natoení jednotky pímo z namených dat vychází velké odchylky od skutených hodnot polohy, rychlosti a úhlu natoení jednotky. To je zpsobeno tím, že Low-Cost jednotka 3DM-GX2 je ovlivnna šumem a její senzory zrychlení mají bias, tedy systematickou chybu projevující se nadhodnocením nebo podhodnocením skutené hodnoty. Proto je nutné namená data nejdíve upravit odetením biasu a filtrací šumu Bias senzoru Korekce bias akcelerometr jednotky 3DM-GX2 byly získány na základ kalibrace, která nebyla souástí této bakaláské práce. Jejich hodnoty jsou,2 g v ose x, -,4 g v ose y a,17 g v ose z. Tyto biasy lze odeíst jednoduchým cyklem, který v každém kroku odete bias akcelerometru v dané ose od hodnoty namené akcelerometrem v dané ose, a tímto rozdílem pepíše pvodní namenou hodnotu Filtry Pomocí Matlab funkce filter viz Tabulka 1 je možné filtrovat signál, kde jako vstupní parametry jsou namená data z naviganí jednotky a data filtru. Data filtru byla získána pomocí nástroje Matlab fdatool, pomocí kterého lze navrhovat filtry a analyzovat filtry typu FIR a IIR. Pi použití filtru vzniká zpoždní úmrné ádu filtru. Proto je nutné pi použití filtru posunout poátek výpotu o toto zpoždní. Pro filtry použité v této práci je zpoždní 2 s. Pro filtrování namených dat byly použity dva filtry. Pro filtraci šumu na signálech z akcelerometr byl použit FIR filtr typu dolní propust s ádem 1, s metodou filtrování constreined equiripple, se zlomovou frekvencí a frekvenní charakteristikou dle Obr Data tohoto filtru jsou uložena v souboru mfilter_acc.mat. 34

43 Magnitude (db) Frequency (Hz) Obr. 4.1: Frekvenní charakteristika filtru pro akcelerometry Pro filtraci šumu na signálech z gyroskop byl použit FIR filtr typu dolní propust s ádem 25, s metodou filtrování equiripple, se zlomovou frekvencí a frekvenní charakteristikou dle Obr Data toho filtru jsou uložena v souboru mfilter_gyro.mat Magnitude (db) Frequency (Hz) Obr. 4.2: Frekvenní charakteristika filtru pro gyroskopy 35

44 4.2 Implementace mechanizace INS Implementace mechanizace INS v diskrétním ase je v Matlabu provedena cyklem pro všechny namené hodnoty. V každém bhu tohoto cyklu se vypote aktuální vektor rychlostí, aktuální poloha a aktuální polohové úhly. Tyto hodnoty se ukládají do polí. Z tchto polí lze vypoítat a vykreslit celkový pohyb jednotky. Výpoet všech tchto hodnot je pro vtší pehlednost rozdlen do ty podkapitol výpoet poáteních hodnot, aktualizace vektoru rychlostí, aktualizace polohy a aktualizace polohových úhl Poátení hodnoty Poátení hodnota vektoru rychlosti je vektor rychlostí, které má naviganí jednotka na zaátku mení ve smrech os NED. Tyto hodnoty jsou nulové, protože na zaátku mení se naviganí jednotka nepohybuje. Poátení hodnota polohy je místo, kde se nachází naviganí jednotka vzhledem ke stedu Zem na zaátku mení, a to popisuje vztah mezi ECEF a NED popsaný kvadrikou a nadmoskou výškou h. Hodnotu kvadriky lze vypoítat dle zempisné šíky a délky místa zaátku mení, které jsou získány napíklad z GPS. Kvadriku lze vypoíst dle rovnice ( 4.1 ). Hodnotu výšky h na zaátku mení získáme napíklad z vrstevnicové mapy. ( 4.1 ) Poátení hodnoty polohových úhl, tedy natoení jednotky v prostoru popisuje vztah mezi NED a BF popsaný kvadrikou. Tu lze vypoítat pomocí funkce Aerospace Toolbox euler2quat viz Tabulka 2, která z Eulerových úhl vypote kvadriku. Eulerovy úhly (Roll) a (Pitch) jsou vypoteny z poáteních hodnot zrychlení dle rovnic ( 4.2 ). Úhel (Heading), tedy úhel mezi osou x jednotky a severním smrem se musí urit napíklad kompasem. ( 4.2 ) Pro implementaci algoritmu mechanizace jsou poteba ješt další poátení hodnoty, a to poátení zmna vektoru rychlostí v NED, zmna vektoru rychlostí zpsobená gravitaní a Coriolisovou silou, rychlost otáení Zem popsané v NED a rychlost rotace NED vzhledem k ECEF. Zmna vektoru rychlostí v NED je nulová, protože na zaátku mení se naviganí jednotka nepohybuje. Zmna vektoru rychlostí zpsobená gravitaní a Coriolisovou silou se vypote z vektoru poáteního gravitaního zrychlení v NED a asové konstanty t dle rovnice ( 4.3 ). 36

45 ( 4.3 ) Vektor gravitaního zrychlení v NED se vypoítá pomocí funkce comp_gravity 2 viz Tabulka 2, kde jako vstupní parametry dáme poátení nadmoskou výšku h, zempisnou šíku a délku. Rychlost otáení Zem popsaná v NED je dána pomocí rovnice ( 2.17 ) a rychlost rotace NED vzhledem k ECEF dle rovnice ( 2.18 ) Aktualizace vektoru rychlostí Aktuální vektor rychlostí jednotky v NED je dán jako souet vektoru rychlostí v NED v pedchozím cyklu, aktuálního vektoru zmny rychlostí v NED a aktuálního vektoru zmny rychlostí zpsobených gravitaní a Coriolisovou silou v NED dle rovnice ( 4.4 ). ( 4.4 ) Aktuální vektor zmny rychlostí v NED Pro získání aktuálního vektoru zmny rychlostí v NED v rovnici ( 4.4 ) je nutné nejprve vypoítat vektor zmny rychlostí jednotky v BF dle rovnice ( 4.5 ). Hodnota aktuální zmny rychlostí a zmny úhl jsou získány z naviganí jednotky dle rovnic ( 2.2 ). ( 4.5 ) Pro vektorový souin byla použita Matlab funkce cross viz Tabulka 2, která vektorov vynásobí dva vstupní vektory. Dále musí být vypotena prostední hodnota rotaního vektoru NED, kterou popisuje rovnice ( 4.6 ) a prostední hodnota rotaního vektoru BF, kterou popisuje rovnice( 4.7 ). Prostední hodnota je hodnota vektoru uprosted mezi hodnotami vektoru v ase a v ase. Rychlost otáení Zem v NED je získána dle rovnice ( 2.17 ) v pedchozím cyklu, rychlost otáení NED vzhledem k ECEF je získána dle rovnice ( 2.18 ) v pedchozím cyklu a vektor rychlosti otáení ECEF vzhledem k IF zobrazený v ECEF je konstantní a popisuje ho rovnice ( 2.12 ). ( 4.6 ) ( 4.7 ) 2 Tato funkce je od vedoucího práce Ing. Michala Reinšteina dle literatury (7) 37

46 Z tchto dvou rotaních vektor lze vypoítat prostední kvadriky rotace NED dle rovnice ( 2.9 ) a ECEF dle rovnice ( 2.1 ), díky kterým je dle rovnice ( 4.8 ) možné vypoítat prostední kvadriku rotace NED vzhledem k ECEF. Tu je možné pomocí funkce Aerospace Toolbox quad2dcm viz Tabulka 2 pevést na matici smrových kosin. Z té lze vypoítat prostední hodnotu zempisné šíky dle rovnice ( 4.9 ). ( 4.8 ) ( 4.9 ) Prostední hodnotu nadmoské výšky lze vypoítat z hodnoty nadmoské výšky v pedchozím cyklu a rychlosti navigované jednotky ve smru do stedu Zem z vektoru rychlostí jednotky v NED v pedchozím cyklu dle rovnice ( 4.1 ). ( 4.1 ) Pomocí prostední hodnoty zempisné šíky a prostední hodnoty nadmoské výšky lze vypoítat prostední rychlost otáení Zem v NED dle rovnice ( 2.17 ) a prostední rychlost otáení NED vzhledem k ECEF dle rovnice ( 2.18 ). Z tchto dvou rychlostí lze vypoítat aktuální hodnotu rotaního vektoru NED dle rovnice ( 4.11 ). ( 4.11 ) Te je již možné vypoítat aktuální vektor zmny rychlostí v NED dle rovnice ( 4.12 ), kde je jednotková matice velikosti 3x3 a je matice smrových kosin, která popisuje vztah mezi BF a NED v pedchozím cyklu. ( 4.12 ) Pro tvorbu tzv. skew symetrické matice rotaního vektoru je použita funkce vek_cp viz Tabulka 2, která rozšíí rotaní vektor dle rovnice ( 4.13 ). ( 4.13 ) 38

47 Aktuální vektor zmny rychlostí zpsobených gravitaní a Coriolisovou silou v NED Pro získání aktuálního vektoru zmny rychlostí zpsobených gravitaní a Coriolisovou silou v NED v rovnici ( 4.4 ) je nutné nejprve vypoítat vektor prostedních hodnot rychlostí naviganí jednotky v NED. Ten je soutem vektoru rychlostí navigované jednotky v NED v pedchozím cyklu, vektoru zmny rychlostí v NED v pedchozím cyklu a vektoru zmny rychlostí zpsobené gravitaní a Coriolisovou silou v pedchozím cyklu dle rovnice ( 4.14 ). ( 4.14 ) Pomocí funkce comp_gravity, kde jako vstup je prostední hodnota zempisné šíky, která byla vypotena dle rovnice ( 4.9 ), prostední hodnota zempisné délky, která byla vypotena dle rovnice ( 4.15 ) a prostední hodnota nadmoské výšky h, která byla vypotena dle rovnice ( 4.1 ), získáme stední hodnotu vektoru gravitaního zrychlení v NED. ( 4.15 ) Prostední rychlost otáení Zem v NED, popsaná rovnicí ( 2.17 ), a prostední rychlost otáení NED vzhledem k ECEF, popsaná rovnicí ( 2.18 ) jsou již vypotené, takže je možné vypoítat aktuální vektor zmny rychlostí zpsobených gravitaní a Coriolisovou silou v NED dle rovnice ( 4.16 ) Aktualizace polohy ( 4.16 ) Aktuální poloha naviganí jednotky je popsána zempisnou šíkou a délkou a nadmoskou výškou h. Informaci o zempisné šíce a délce obsahuje kvadrika popisující vztah mezi NED a ECEF Zempisná šíka a délka Pro výpoet této kvadriky je nutné nejdíve získat rotaní vektor NED, který je již vypotený z aktualizace rychlosti dle rovnice ( 4.11 ), a rotaní vektor ECEF, který popsán dle rovnice ( 4.17 ) z konstantního vektoru rychlosti otáení ECEF vzhledem k IF zobrazeného v ECEF, který je konstantní a vypoítá se dle rovnice ( 2.12 ). ( 4.17 ) 39

48 Z rotaních vektor NED a ECEF lze vypoítat kvadriky rotace NED dle rovnice ( 2.9 ) a rotace ECEF dle rovnice ( 2.1 ), díky kterým lze dle rovnice ( 4.18 ) vypoítat aktuální kvadriku rotace NED vzhledem k ECEF. Tu pomocí funkce Aerospace Toolbox quad2dcm viz Tabulka 2 je možné pevést na matici smrových kosin, ze které je již možné urit aktuální zempisnou šíku a zempisnou délku. ( 4.18 ) Nadmoská výška Nadmoskou výšku h lze vypoítat dle rovnice ( 4.19 ), kde je rychlost ve smru do stedu Zem z prostedního vektoru rychlostí NED. Prostední vektor rychlostí v NED je vypoten z vektoru rychlostí v NED v pedchozím cyklu a aktuálního vektoru rychlostí dle rovnice ( 4.2 ). ( 4.19 ) Aktualizace polohových úhl ( 4.2 ) Aktuální polohové úhly (Roll, Pitch, Heading) popisují polohu naviganí jednotky v prostoru. Informaci o polohových úhlech obsahuje kvadrika popisující vztah mezi BF a NED. Pro výpoet této kvadriky je nutné nejdíve získat rotaní vektor NED, který je již vypotený z aktualizace rychlosti dle rovnice ( 4.11 ), a rotaní vektor BF, který lze vypoítat z vektoru zmny úhl v pedchozím cyklu a aktuálního vektoru zmny úhl dle rovnice ( 4.21 ). ( 4.21 ) Z rotaních vektor NED a BF lze vypoítat kvadriky rotace NED dle rovnice ( 2.1 ) a BF dle rovnice ( 2.9 ), díky tmto kvadrikam lze dle rovnice ( 4.22 ) vypoítat aktuální kvadriku rotace BF vzhledem k NED. Tu je možné pomocí funkce Aerospace Toolbox quad2dcm viz Tabulka 2 pevést na matici smrových kosin, ze které je již možné urit aktuální polohové úhly pomocí funkce Aerospace Toolbox dcm2euler viz Tabulka 2. 4

49 ( 4.22 ) Pokud se BF vzhledem k NED neotáí, Euklidova norma rotaního vektoru je nulová. Pi této situaci dochází v rovnici ( 2.9 ) k dlení nulou. Proto je pro pípad nulového rotaního vektoru v algoritmu nutné zavést podmínku, která tuto situaci rozpozná a kvadriku nastaví na kvadriku rovnosti soustav, tedy. 4.3 Výstupy implementace INS Z implementace mechanizace INS je znám vektor rychlostí v NED, transportní kvadrika, nadmoská výška h a kvadrika natoení Výstup jednotky v ECEF Pro popis pohybu jednotky v ECEF je nutné v každém cyklu pevádt transportní kvadriku na aktuální zempisné souadnice. Pomocí funkce Aerospace Toolbox quat2dcm viz Tabulka 2 lze pevést kvadriku dle rovnice ( 2.5 ) na matici smrových kosin a z této matice pomocí funkce Aerospace Toolbox dcm2latlon viz Tabulka 2 pevést na aktuální zempisnou šíku a aktuální zempisnou délku. Pro doplnní souadného soustavy ECEF zbývá jen nadmoská výška h, která je vypotena pímo. Vykreslením prbžných hodnot nadmoské výšky a zempisné šíky a délky je možné získat dráhu jednotky v ECEF Výstup jednotky v NED Pro popis pohybu jednotky v NED je nutné v každém cyklu vypoítat vzdálenost mezi jednotlivými ECEF souadnicemi v tomto a pedchozím cyklu, a to jak v severním, tak i ve východním smru. To lze získat pomocí funkce ll_dist viz Tabulka 2, kde jsou jako vstupní parametry souadnice v pedchozím cyklu a aktuální souadnice. Pokud bude zempisná šíka konstantní, funkce vrátí hodnotu vzdálenosti ve východním smru, a pokud bude zempisná délka konstantní, funkce vrátí hodnotu vzdálenosti v severním smru. Pro doplnní souadné soustavy NED zbývá jen vzdálenost ve smru do stedu Zem, která se vypote pomocí rovnice ( 4.23 ) z poátení nadmoské výšky a pímo vypotené aktuální nadmoské výšky. Vykreslením prbžných hodnot vzdáleností v severním a východním smru a smru do stedu Zem je možné získat dráhu jednotky v NED. ( 4.23 ) 41

50 5 Závr Cílem této práce bylo navrhnout a v prostedí Matlab realizovat algoritmus mechanizace inerciálního naviganího systému (INS) a jeho ovení na reálných datech namených jednotkami 3DM-GX2 a AHRS-M3. Návrh a realizace tohoto algoritmu byla velmi obtížná a asov nároná, ale pesto se podailo tento algoritmus úspšn implementovat. Ani samotná tvorba simulovaných dat nebyla jednoduchá, protože jednotlivé simulované signály spolu souvisí a bez pesného sladní všech signál nelze získat pesný výsledek. Pes všechny tyto problémy byly simulace velmi úspšné a jejich odchylky od požadovaných hodnot polohy byly pi simulaci nehybnosti jednotky v ádech mikrometr, pi simulaci pohybu jednotky v jednom smru o jeden metr byla odchylka 2,14 mm, pi simulaci pohybu ve dvou smrech o pl metru bez natoení byla odchylka v severním smru,283 mm a ve východním smru,483 mm a pi simulaci pohybu zatáející jednotky byla odchylka natoení v ose z,869. Mení reálných dat bylo provedeno pouze pomocí jednotky 3DM-GX2. Mení pomocí jednotky AHRS-M3 a statická mení pomocí náklonné kalibraní plošiny a pi automobilové navigaci bude provedeno v pokraování této práce. Pi mení pomocí jednotky 3DM-GX2 bylo zjištno, že namené hodnoty jsou velmi ovlivnny šumem a pi jejich zpracování dochází k velkým odchylkám vypotené hodnoty polohy od skutené polohy jednotky pi mení. Šum na namených hodnotách byl filtrován pomocí FIR filtr typu dolní propust dle podkapitoly 4.1.2, což odchylku snížilo zhruba o polovinu viz Tabulka 1, ale i taková odchylka je píliš velká pro reálné použití tohoto algoritmu. Pro pesnjší výpoet polohy by musela být použita bu kvalitnjší naviganí jednotka, která by byla ale daleko dražší než jednotky 3DM-GX nebo AHRS-M3, nebo by musely být jednotky kalibrovány nebo na namená data použita nap. vlnková transformace, což je nad rámec této bakaláské práce. V této práci bych chtl pokraovat a realizovat pokroilejší metody zpracování signálu na namená data a realizovat algoritmus v reálném ase na datech získaných z automobilové nebo letecké navigace. asová náronost algoritmu s FIR filtrací je prmrn 3,492 s na výpoet 25 s mení, realizace v reálném ase je tedy možná a zatím ješt i s asovou rezervou. Navržený algoritmus najde využití ve všech oblastech, kde se využívá zpracování signálu z inerciálních senzor. 42

51 6 Literatura 1. Syrovátka, Viktor. Úvod do systém inerciální navigace. Praha : Czech Space Office, 25. ervenec Ripka, Pavel, a další. Senzory a pevodníky. Praha : Vydavatelství VUT, RobotWiki. Akcelerometr. RobotWiki. [Online] [Citace: 5. Duben 29.] 4. Vojáek, Antonín. Gyroskopy v integrovaném provedení MEMS. [Online] [Citace: 5. Duben 29.] 5. Hlavá, Václav a Sedláek, Miloš. Zpracování signál a obraz. Praha : Vydavatelství VUT, Shin, E.H. Estimation Techniques for Low-Cost Inertial Navigation. PhD Thesis. Calgary, Canada : University of Calgary, Soták, Miloš, a další. Integrácia naviganých systémov. Košice : Róbert Bréda, Inc., MicroStrain. 3DM-GX2 Datasheet Innalabs. AHRS M3 Datasheet

52 7 Pílohy 7.1 Seznam obrázk Obr. 2.1: Akcelerometr s kmitajícím nosníkem (2 str. 39)... 3 Obr. 2.2: Akcelerometr MEMS (2 str. 4)... 3 Obr. 2.3: Senzor úhlové rychlosti na principu Coriolisovy síly (2 str. 43)... 5 Obr. 2.4: Mení natoení a rotace (4)... 5 Obr. 2.5: Píklad psobení Coriolisovy síly (4)... 6 Obr. 2.6: Psobení Coriolisovy síly na mechanický gyroskop (4)... 7 Obr. 2.7: Zjednodušená struktura snímae MEMS gyroskopu (4)... 7 Obr. 2.8: Píklad funkce struktury snímae gyroskopu pi rotaci (4)... 8 Obr. 2.9: Rotaní vektor (6 str. 11)... 1 Obr. 2.1: Inerciální souadná soustava (6 str. 19) Obr. 2.11: Vztah ECEF a geodetických souadnic (6 str. 21) Obr. 2.12: Zemská a naviganí souadná soustava (6 str. 23) Obr. 3.1: 3DM-GX2 struktura (8) Obr. 3.2: Jednotka 3DM-GX Obr. 3.3: AHRS struktura (9) Obr. 3.4: Jednotka AHRS-M Obr. 3.5: Zmna úhlu natoení Roll Obr. 3.6: Pohyb jednotky v NED... 2 Obr. 3.7: Simulovaný signál zrychlení v ose y Obr. 3.8: Pohyb ve východním smru Obr. 3.9: Simulované signály zrychlení v ose x a y Obr. 3.1: Pohyb jednotky v NED Obr. 3.11: Horizontální dráha jednotky Obr. 3.12: Simulovaný signál zrychlení v ose x Obr. 3.13: Simulované signály úhlových rychlostí Obr. 3.14: Pohyb jednotky v NED Obr. 3.15: Horizontální dráha jednotky Obr. 3.16: Zmna úhlu natoení Heading Obr. 3.17: Simulované signály zrychlení Obr. 3.18: Simulované signály úhlových rychlostí Obr. 3.19: Vypotené pohyby jednotky v NED Obr. 3.2: Vypotená horizontální dráha jednotky Obr. 3.21: Vypotená 3D dráha jednotky... 3 Obr. 3.22: Vypotené natoení jednotky v ose z... 3 Obr. 3.23: Namené signály zrychlení Obr. 3.24: Namené signály úhlových rychlostí Obr. 3.25: Porovnání signál z akcelerometru pi filtraci Obr. 3.26: Porovnání signál z gyroskopu pi filtraci Obr. 4.1: Frekvenní charakteristika filtru pro akcelerometry Obr. 4.2: Frekvenní charakteristika filtru pro gyroskopy

53 7.2 Tabulky Tabulka 2: Seznam použitých funkcí 45

54 Tabulka 3: Použité zkratky Tabulka 4: Obsah piloženého CD 46

55 Tabulka 5: Technické parametry jednotky 3DM-GX2 (8) 47

56 Tabulka 6: Technické parametry jednotky AHRS-M3 (9) 48