ESKÉ VYSOKÉ UENÍ TECHNICKÉ V PRAZE Fakulta elektrotechnická DIPLOMOVÁ PRÁCE 211 Autor : Bc. Petr Gazdík
ESKÉ VYSOKÉ UENÍ TECHNICKÉ V PRAZE Fakulta elektrotechnická Katedra mení Micí jednotka naviganího systému pro bezpilotní prostedky Vedoucí práce: Ing. Jan Rohá, Ph.D. Autor: Bc. Petr Gazdík Praha 211
Prohlášení Prohlašuji, že jsem svou diplomovou 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
Podkování Na tomto míst bych chtl podkovat pedevším své rodin za podporu, kterou mi poskytovali po celou dobu studia. Dále bych chtl podkovat vedoucímu diplomové práce Ing. Janu Roháovi, Ph.D. za jeho pipomínky a návrhy. Na závr bych chtl podkovat firm MESIT Pístroje spol. s r.o. za pomoc pi pájení senzor.
Anotace Cílem této práce je navržení micí jednotky naviganího systému pro bezpilotní prostedky. Jednotka se má skládat ze senzor úhlových rychlostí a akcelerometr tvoících klasickou soustavu a dále má být použito modifikované soustavy akcelerometr. Výstupem jednotky mají být polohové úhly podélný sklon a píný náklon. Jednotka musí být schopna komunikace s jiným systémem pomocí sbrnice CAN a RS232. Na závr má být jednotka otestována a zhodnocena její pesnost. Annotation The aim of this thesis is to design an inertial measurement unit for an unmanned aircraft system. The unit should consist of angular rate sensors and accelerometers which form the classical system and then use the modified system of accelerometers. The output of the unit should come to positional angles PITCH and ROLL. The unit must be able to communicate with another system via CAN bus and RS232. At the end the unit should be tested and its accuracy evaluated.
Obsah 1. Úvod... 2 2. Teoretický rozbor...3 2.1. Inerciální navigace... 3 2.1.1. Základy inerciální navigace... 3 2.1.2. Souadnicové soustavy... 5 2.1.3. Princip výpotu orientace a polohy... 7 2.2. Senzory pro inerciální navigaci... 8 2.2.1. Akcelerometry... 8 2.2.2. Senzory úhlové rychlosti... 11 2.2.3. Analýza šumových vlastností výstupních signál senzor... 14 2.3. Kalmanova filtrace... 17 3. Praktické ešení... 2 3.1. Popis IMU jednotky... 21 3.1.1. Procesorová deska... 23 3.1.2. Senzorová deska... 24 3.1.3. Kompletace IMU jednotky... 25 3.2. Klasická a modifikovaná soustava... 26 3.3. Komunikaní sbrnice... 28 3.3.1. Komunikace se senzory pomocí SPI... 28 3.3.2. Komunikace po RS232... 31 3.3.3. Komunikace po CAN... 32 3.4. innost mikroprocesoru... 35 3.5. Vizualizace mených dat... 36 4. Základní mení na IMU jednotce... 38 4.1. Šumová analýza senzor... 38 4.1.1. Výsledky výkonové spektrální hustoty a histogramu... 38 4.1.2. Výsledky analýzy Allan Variance... 39 4.2. Kalibrace jednotky... 42 4.2.1. Postup kalibrace akcelerometr IMU jednotky ADIS16355... 42 4.2.2. Postup kalibrace dvojice dvouosých akcelerometr ADIS1629... 45 4.2.3. Srovnání klasické a modifikované soustavy akcelerometr... 47 5. Zpracování dat...49 5.1. Výpoet polohových úhl... 49 5.1.1. Výpoet polohových úhl ze senzor úhlových rychlostí... 49 5.1.2. Výpoet polohových úhl z akceleromet... 52 5.2. Fúze dat... 53 5.3. Filtrace vibrací a šumu... 58 5.4. Návrh Kalmanova Filtru... 64 6. Testování IMU jednotky... 67 6.1. Statická mení... 67 6.2. Dynamická mení... 69 7. Závr... 76 8. Literatura... 77 Obsah piloženého CD... 78-1 -
1. Úvod Pojem navigace je v dnešní dob asto skloovaný termín a získává nových rozmr v nejrznjších odvtvích, nap. v automobilovém prmyslu, stavebnictví, zemdlství, zdravotnictví, atd. V automobilových aplikacích je princip vyhodnocování polohy založen na píjmu signálu z družicového naviganího systému (GPS, GLONASS, ). V místech, kde není tento signál k dispozici (nap. v budovách, husté zástavb, v podzemí, ), je nutné pro navigaci využít jiného systému, který bude urovat polohu odlišným zpsobem. ešením je použití autonomního zaízení, kterým je inerciální naviganí systém (INS). Jádrem INS je soustava senzor spojena s výpoetní jednotkou, která na základ jejich výstup poítá orientaci, rychlost a polohu. V letectví je INS využíván jako primární zdroj informací týkajících se navigace letadla. V souasné dob zstávají i pes rychle rozvíjející se technologie nejpesnjší stále ty naviganí systémy, které využívají laserových gyroskop s pevnou optickou základnou RLG. Bohužel cena tchto senzor je velmi vysoká, a proto se používají zejména u velkých dopravních letadel. Pro menší letadla se snaží výrobci o nahrazení tchto drahých senzor jinými levnými senzory, kterými jsou nap. senzory vyrobené technologií MEMS. Cílem této práce je navržení micí jednotky INS s modifikovanou soustavou akcelerometr s použitím senzor MEMS, kterými budou ti senzory úhlové rychlosti a ti akcelerometry tvoící klasickou soustavu a tyi akcelerometry tvoící modifikovanou soustavu. Výstupem jednotky mají být polohové úhly podélný sklon a píný náklon. Dalším cílem práce je ukázat, zda použití modifikované soustavy akcelerometr povede ke zvýšení pesnosti systému. Diplomová práce je lenna do sedmi kapitol a je doplnna CD se zdrojovými kódy a veškerou dokumentací. V teoretickém rozboru jsou popsány principy inerciální navigace a používané senzory. Tetí kapitolu tvoí praktická realizace micí jednotky. tvrtá kapitola se zabývá základním kalibrací jednotky a analýzou šumových vlastností senzor. Pátá kapitola popisuje zpracování dat, v šesté kapitole jsou uvedeny namené výsledky a v sedmé kapitole je závr. - 2 -
2. Teoretický rozbor 2.1. Inerciální navigace 2.1.1. Základy inerciální navigace Základem pro inerciální navigaci je první a druhý Newtonv zákon. 1. Newtonv zákon tleso setrvává ve stavu klidu nebo rovnomrného pímoarého pohybu, není-li vnjšími silami nuceno svj stav zmnit. 2. Newtonv zákon (2.1) jestliže na tleso psobí síla, pak se tleso pohybuje se zrychlením, které je pímo úmrné psobící síle a nepímo úmrné hmotnosti tlesa. F = m a, (2.1) kde F vektor síly psobící na letadlo, a odpovídá vektoru zrychlení, m hmotnost tlesa. Klasické ešení inerciálního naviganího systému je založeno na mení zrychlení a úhlových rychlostí v hlavních osách letadla, tj. v podélné a píné ose a ve smru vertikály (viz obr.2.1). Zrychlení jsou následn transformována do referenní soustavy, ve které se navigace provádí, a to pomocí transformaní matice. To krom jiného poskytuje možnost provést korekce tíhového a Coriolisova zrychlení, která by ve výpotech polohy zpsobovala chybu. Tíhové a Coriolisovo zrychlení je vyvoláno psobením silových úink Zem a rotující zemskou soustavou, tudíž se s náklonem letadla mní i jejich rozložení v letadlové souadnicové soustav. V transformaní matici vystupují údaje o orientaci letadla vi referenní soustav v podob Eulerových úhl, kvaternion nebo smrových kosin. Po transformaci zrychlení do referenní vztažné soustavy a následném provedení korekce, je dvojitou integrací získána poloha letadla v dané referenní soustav. Dležitou roli ve výpotech hraje pesné urení polohových úhl. Ty zárove poskytují pilotovi informace o náklonu a sklonu letadla, plní funkci umlého horizontu. Pi nepesnostech tchto úhl dostává pilot jednak chybnou informaci o orientaci letadla a jednak vznikají chyby ve výpotech korekcí tíhového a Coriolisova zrychlení a chyby v urení polohy. V dnešní dob jsou jako nejpesnjší senzory používány gyroskopy s pevnou optickou základnou RLG s rozlišovací schopností 6 1 /s a mechanické akcelerometry s rozlišovací schopností jednotek g. Pi použití senzor s tmito parametry nedosahují chyby v urení polohy - 3 -
velkých hodnot ani pi dlouhých letech. V pípad použití senzor s horšími parametry je nutné provádt korekci polohy letadla nap. pomocí družicové navigace GPS, radiomaják VOR, atp. Inerciální naviganí systémy (INS) se v zásad dlí na dva typy. Prvním (starším) typem je INS s pohyblivou základnou a druhým (modernjším typem) je tzv. strap-down INS. INS s pohyblivou základnou Senzory jsou umístné na mechanicky stabilizované platform, jejíž zarovnání je shodné s referenní soustavou. Platforma je stabilizována v nemnné poloze na základ vyhodnocení údaj ze senzor úhlových rychlostí. Jinými slovy, platforma zachovává konstantní orientaci bez ohledu na pohyb letadla. Zrychlení je tak meno pímo v referenní soustav. Po jeho integraci je získána rychlost a poloha. Výpoet probíhá v naviganím poítai. Tento typ se používá v aplikacích, kde nehraje roli hmotnost a kde je nutné velmi pesné urení polohy (nap. lod a ponorky). Strap-down INS Tento typ je modernjší a klade draz na odstranní mechanických ástí z platformy. Systém Strap-down se od pedchozího odlišuje tím, že senzory jsou pevn spojeny s letadlem (viz obr.2.1). Zrychlení je meno v tlesové (letadlové) soustav. Pro výpoet polohy se musí nejprve zrychlení z tlesové (letadlové) soustavy transformovat do referenní soustavy, kde probíhá navigace. To má za následek vyšší výpoetní nároky, ale jeho výhodou je snížení náklad, snížení hmotnosti, menší rozmry a vtší spolehlivost. senzory úhlové rychlosti y boná osa x podélná osa akcelerometry z osa ve smru vertikály Obr. 2.1: Umístní senzor na letadle - 4 -
Strap-down systémy se dají dále rozdlit do následujících skupin: IMU (Inertial Measurement Unit) inerciální micí jednotka, skládající se ze soustavy tí akcelerometr a tí senzor úhlové rychlosti, jejími výstupy jsou pouze hodnoty zrychlení a úhlových rychlostí. AHRS (Attitude & Heading Reference System) systém, jehož výstupem je orientace v prostoru, tzn. polohové úhly náklon, sklon a kurz. Jedná se o rozšíení IMU o blok výpotu orientace v prostoru. Souástí tohoto systému asto bývá magnetometr nebo GPS. INS (Inertial Navigation System) inerciální naviganí systém, rozšíení AHRS o blok výpotu polohy v prostoru. Jedná se o jednotku, která poítá polohu objektu v prostoru. 2.1.2. Souadnicové soustavy Stavy naviganích systém poloha, rychlost a orientace jsou definovány vzhledem k urité souadnicové soustav. Tyto souadnicové soustavy jsou navrženy tak, že informace mohou být mezi soustavami propojovány, tzn. že soustavy jsou ortogonální a pravotoivé. [1] Inerciální soustava (Inertial frame) má poátek ve stedu Zem a osy, které nerotují se Zemí. Osa Z i je totožná se zemskou osou. Zbývající dv osy jsou orientovány dle daných hvzd, které nemní svoji polohu vi Zemi. Zemská soustava (Earth Fixed Earth Centered Frame) má poátek ve stedu Zem a osy, které rotují se Zemí. Existují ti možnosti znaení os, piemž nejpoužívanjší je následující: osa Z e je totožná se zemskou osou. Osa X e je tvoena prnikem rovin rovníku a Greenwichského poledníku. Osa Y e dodefinovává ortogonální systém. Naviganí soustava (Navigation Frame) má poátek v umístní naviganího systému. Existují rzné typy, piemž nejpoužívanjší je soustava, která má osy smující na sever (N), východ (E) a ve smru vertikály (D) Všechny výše uvedené souadnicové soustavy jsou znázornny na obr.2.2. - 5 -
Obr. 2.2: Souadnicové soustavy [7] Tlesová soustava (Body Frame) - je pevn spojena s letadlem. V praxi se vyskytují dv definice letadlových (tlesových) souadnicových systém, a to starší systém dle normy GOST a novjší systém dle ISO (viz obr.2.3). V této práci je uvažována norma ISO. Orientaci tlesové souadnicové soustavy oproti referenní souadnicové soustav lze popsat pomocí tí úhl. Tyto úhly kurz (ψ - YAW), podélný sklon (θ - PITCH) a píný náklon (φ - ROLL) se nazývají Eulerovy úhly. Kladný smysl otáení je dán pravidlem pravé ruky (palec ukazuje ve smru osy a zahnuté prsty ukazují kladný smysl otáení). X NÁKLON (ROLL) Y KURZ (YAW) SKLON (PITCH) Z X NÁKLON (ROLL) SKLON (PITCH) Y KURZ (YAW) Z a) systém GOST b) systém ISO Obr. 2.3: Letadlové souadnicové systémy - 6 -
2.1.3. Princip výpotu orientace a polohy Obr.2.4 popisuje blokové schéma strap-down systému. Je využito 3 akcelerometr a 3 senzor úhlové rychlosti. Ze senzor úhlových rychlostí je poítána orientace (polohové úhly). Na základ znalosti tchto úhl je provádna transformace zrychlení z tlesové soustavy do referenní soustavy. K tomu slouží transformaní matice n C b. Po transformaci zrychlení do referenní soustavy je provedena korekce gravitaního a Coriolisova zrychlení. Takto získaná zrychlení se integrují za úelem získání rychlosti a druhou integrací je získána poloha. Obr. 2.4: Blokové schéma strap-down systému [7] Transformace z tlesové soustavy do naviganí soustavy Transformaci z tlesové soustavy do naviganí popisuje rovnice (2.2). Transformaní matice slouží pro pevod údaj z akcelerometr mících zrychlení v tlesové soustav do požadované referenní soustavy. Jelikož se jedná o navigaci v trojrozmrném prostoru, má transformaní matice velikost 3x3. Prvky matice jsou tvoeny funkcemi sinus a kosinus polohových úhl. Podoba matice mže mít více tvar, piemž nejpoužívanjší je tvar (2.3) [1]. Dalším typem transformace je použití kvaternion i smrových kosin. Transformaní rovnice n n b r = Cb r, (2.2) kde n r vektor veliin v referenní soustav, b r vektor veliin v tlesové soustav, n C b transformaní matice pro pevod z tlesové do naviganí soustavy, - 7 -
cθcψ cφsψ + sφsθcψ sφsψ + cφsθcψ C n = b cθcψ cφcψ + sφsθsψ sφcψ + cφsθsψ, (2.3) sθ sφcθ cφcθ kde s odpovídá funkci sinus a c odpovídá funkci kosinus. 2.2. Senzory pro inerciální navigaci V systémech inerciální navigace se jak primární senzory využívají akcelerometry a senzory úhlových rychlostí. V této kapitole je uvedeno základní dlení a principy jednotlivých senzor. 2.2.1. Akcelerometry Akcelerometry jsou senzory, které mí zrychlení, což je podle druhého Newtonova zákona výsledek psobení síly na tleso o dané hmotnosti. Princip akcelerometru je založen na seismické hmot, která je pomocí pružin spojena s pouzdrem. Výchylka seismické hmoty nebo rychlost jejího pohybu je úmrná psobícímu zrychlení. Podle zpsobu vyhodnocení zmny polohy seismické hmoty je možné rozdlit akcelerometry do dvou skupin. Do první skupiny patí akcelerometry využívající nepímého mení zrychlení pomocí výchylky seismické hmoty z rovnovážné polohy. Bývají oznaovány jako mechanické. Do druhé skupiny se adí akcelerometry, u kterých je pohyb seismické hmoty pevádn na jinou veliinu. Tyto akcelerometry využívají pro mení zrychlení nap. vibraních struktur, optických vláken a fotocitlivých prvk, tenzometr nebo piezokrystal a jsou oznaovány jako akcelerometry s pevnou strukturou. V INS se používají výhradn akcelerometry, které mí zrychlení v rozsahu od Hz. Pro strunost jsou dále uvedeny pouze nkteré z nich. Detailnjší popis a rozdlení lze nají v literatue. [2] Mechanické akcelerometry Tyto akcelerometry je možné rozdlit na dv kategorie. Rozlišují se akcelerometry bez zptné vazby a akcelerometry se zptnou vazbou. Akcelerometry bez zptné vazby mají výstupní zrychlení úmrné výchylce seismické hmoty oproti pouzdru senzoru (viz.obr.2.5a). U akcelerometr se zptnou vazbou je pružina doplnna o elektromagnet. Pi psobení zrychlení je detekována výchylka seismické hmoty z nulové pozice snímacím vinutím. Pro udržení seismické hmoty v nulové pozici se vyvodí elektromagnetem síla, která až na - 8 -
znaménko odpovídá menému zrychlení (viz.obr.2.5b). Výhodou zptnovazebních akcelerometr je vtší citlivost a stabilita. a) akcelerometr bez zptné vazby b) akcelerometr se zptnou vazbou Obr. 2.5: Mechanický akcelerometr [2] Akcelerometry s pevnou strukturou Tyto akcelerometry se oproti mechanickým liší v tom, že mají menší hmotnost, rozmry a jsou odolnjší proti mechanickému poškození. Jak již bylo zmínno výše, u tchto akcelerometr je vliv pohybu seismické hmoty pevádn na jinou veliinu. Provedení existuje celá ada [2], proto jsou dále uvedeny jen dva typy. Vibraní akcelerometry Senzory využívají dvojice kemíkových vláken upevnných symetricky v pouzde (viz obr.2.6). V klidovém stavu, tzn. bez psobení zrychlení kmitají ob vlákna stejnou frekvencí. Pi psobení zrychlení je jedno vlákno namáháno na tah a druhé na tlak, ímž dojde ke zmn frekvence kmitání. Rozdíl frekvencí je potom úmrný psobícímu zrychlení. Obr. 2.6: Vibraní akcelerometr[2] - 9 -
MEMS akcelerometry V posledních letech se rozvinula technologie MEMS (Micro-Electro-Mechanical-System). Jedná se o technologii chemického leptání struktur do kemíkového substrátu. Lze vytvoit libovolnou strukturu s pesností ádu 6 1 m. Na obr.2.7 je uveden jeden z princip MEMS akcelerometr - kapacitní. Skládá se ze seismické hmoty ve tvaru hebene, jejíž jednotlivé zuby jsou pohyblivé elektrody. Celá seismická hmota je pipevnna pomocí pružných úchyt k pouzdru senzoru. V pouzdru umístny pevné elektrody, které tvoí spolu s pohyblivými elektrodami miniaturní diferenní kondenzátory. Pi psobení zrychlení dojde k vychýlení seismické hmoty, a tím ke zmn kapacity. Paralelním zapojením diferenních kondenzátor se zvyšuje citlivost senzor. Obr. 2.7: MEMS kapacitní akcelerometr[1] Další princip MEMS akcelerometr je tenzometrický. Zde je seismická hmota pipevnna na vetknutém nosníku, na kterém jsou nalepeny z obou stran tenzometry (tenzometr je rezistor, jehož odpor je závislý na mechanickém namáhání) viz obr.2.8. Pi psobení zrychlení je nosník vychýlen a tenzometry na jedné stran jsou namáhány na tah, druhé na tlak. Zmna odporu je pak úmrná psobícímu zrychlení. Tento systém se používá zejména v automobilovém prmyslu. Obr. 2.8: MEMS tenzometrický akcelerometr [1] - 1 -
V následující tabulce jsou uvedeny typické parametry akcelerometr [2] Mech. se zp. vaz Vibraní Kemíkový Rozsah 1 g 2 g 1 g Nelinearita,5%,5%,1-,4% Rozlišení 1 g 1 g 1-1 g Šíka pásma 4 Hz 1 Hz 4 Hz Tab. 2.1: Parametry akcelerometr 2.2.2. Senzory úhlové rychlosti Senzory úhlových rychlostí se využívají v rzných aplikacích jako prostedek pro sledování rotaního pohybu rzných zaízení (plošiny, v automobilech ve stabilizaním systému ESP, manipulátory ), k vyhodnocování polohových úhl nebo ke stabilizaci daného zaízení. Mená úhlová rychlost je vztažena k ose citlivosti použitého senzoru. Senzory se používají nejastji: - v systémech stabilizace, - v systémech automatického ízení letu, - v systémech autonomní navigace, - v robotice. Senzory úhlové rychlosti je možné rozdlit do následujících skupin [14]: - mechanické gyroskopy o RIG (Rate Integrating Gyro) integraní gyroskop o DTG (Dynamically Tuned Gyro) dynamicky ladný gyroskop - rezonanní (vibraní) gyroskopy o HRG (Hemispherical Resonant Gyro) o VSG (Vibrating Silicon Gyro) - optické gyroskopy o RLG (Ring Laser Gyro) gyroskop s pevnou optickou základnou o FOG (Fiber Optic Gyro) vláknový gyroskop Dynamicky ladný gyroskop Jedná se o rotaní zaízení s kardanovým závsem, jehož uspoádání je uvedeno na obr.2.9. Setrvaník je spojen pomocí torzních závs s rámem (v praxi se vyskytují uložení se dvma nebo temi stupni volnosti). Tato ást (setrvaník se závsem) je mechanicky spojena s osou motoru, který roztáí celý systém. - 11 -
Obr. 2.9: Setrvaník DTG [2] Pi zmn úhlu náklonu se zmní poloha rámu zaízení, zatímco setrvaník si zachovává polohu konstantní (viz obr.2.1). Odchylka mezi pouzdrem zaízení a setrvaníkem je snímána pomocí tveice cívek a je vedena do vyhodnocovacího obvodu. Odtud je získána informace o úhlu a zárove je vypotena zptná vazba pro korekní cívky, které sesouhlasí osu setrvaníku s osu motoru. Obr. 2.1: Principiální schéma DTG gyroskopu [8] Kemíkové vibraní gyroskopy Princip vibraních gyroskop je založen na tom, že je použit rezonátor, který vibruje uritou frekvencí. Pi psobení úhlové rychlosti kolem osy citlivosti, vznikne Coriolisova síla, která ovlivní charakter vibrování rezonátoru. Charakter vibrování je snímán bu kapacitn nebo piezoelektricky. Provední existuje nkolik, viz literatura [2]. Zde je popsán pouze princip vibraního MEMS gyroskopu, jehož uspoádání je na obr.2.11. Základem je periodicky se - 12 -
pohybující seismická hmota spojena s vnitním rámem. Smr pohybu seismické hmoty musí být kolmý ke smru mené úhlové rychlosti. Za této podmínky psobí na pohybující se seismickou hmotu Coriolisova síla, jejíž velikost je úmrná úhlové rychlosti otáení. Ta zpsobí vychýlení vnitního rámu (viz obr.2.12) a vzájemný posuv elektrod tvoících elementární diferenní kondenzátory. Výsledná kapacita je úmrná mené úhlové rychlosti otáení. Obr. 2.11: Uspoádání vibraního gyroskopu [2] Obr. 2.12: Chování struktury pi nenulové úhlové rychlosti [2] Optické gyroskopy V systémech inerciální navigace se využívá také optických gyroskop, které patí do skupiny pesných senzor úhlových rychlostí. Rozlišují se dva typy: - s pevnou optickou základnou (RLG) - vláknové (FOG) Princip vyhodnocení úhlových rychlostí je u obou typ podobný a je založen na zmn délek drah dvou protibžných paprsk (A,B) o stejné vlnové délce. Délka dutiny L musí být - 13 -
celistvým násobkem vlnové délky. V klidovém stavu ( ω = ), jsou délky obou paprsk stejné. V pípad ω, se délky drah paprsk liší o Δ L (viz obr.2.13). To odpovídá urité zmn fáze mezi jednotlivými paprsky. Tato zmna je detekována pomocí fotocitlivých diod. Obr. 2.13: Princip optických gyroskop V pípad RLG oba paprsky v míst detekce interferují a vytváejí tzv. Fressnelovy interferenní proužky. V klidovém stavu, tj. ω = se proužky nepohybují. V pípad ω je rychlost pohybu proužk úmrná fázovému rozdílu paprsk, a tedy i úhlové rychlosti otáení. U FOG se v míst detekce vyhodnocuje intenzita záení, která je úmrná fázovému posunu obou paprsk, a tedy i úhlové rychlosti otáení. Detailní popis lze nalézt v literatue [2] V následující tabulce jsou uvedeny typické parametry nkterých senzor úhlových rychlostí. DTG Vibraní RLG Rozsah 1 /s 3 /s tisíce /s Nelinearita,1-,1 %,3-,3 % - Drift,5-1 /h,1-1 /s,1-1 /h Šíka pásma < 1 Hz 6 5 Hz >2 Hz Tab. 2.2: Parametry senzor úhlových rychlostí 2.2.3. Analýza šumových vlastností výstupních signál senzor Vzhledem k tomu, že každý výstupní signál senzoru je zatížen šumem, je nutné pro pesné urení mené veliiny znát parametry tohoto šumu. V oblasti odhadování chyb inerciálních senzor existuje nkolik metod, napíklad výkonová spektrální hustota (PSD Power Spectral Density) a autokorelaní funkce (ACF Auto Correlation Function), které jsou pímoaré, ale neumožují rozlišit rzné typy šumu ve výstupním signálu senzoru bez znalosti jeho matematického modelu (stavového popisu). Analýza Allan Variance naopak nepotebuje pro urení parametr šumu znalost matematického modelu senzoru. Norma IEEE definuje pro inerciální senzory pt základních typ šumu [6]. - 14 -
Výkonová spektrální hustota Výkonová spektrální hustota je analýza signálu ve frekvenní oblasti. Charakterizuje rozdlení výkonu signálu v závislosti na kmitotu jeho dílích složek. Pomocí Fourierovy transformace je definován odhad výkonové spektrální hustoty (2.4). Z tvaru spektra je možno usoudit, o jaký typ šumu se jedná (úzkopásmový nebo širokopásmový), ale odhad dalších parametr jako nestabilita prmrné hodnoty (bias instability), šum typu náhodná procházka (random walk) a kvantizaní šum (quantization noise) je komplikovaný [4]. Rovnice (2.4) popisuje výpoet výkonové spektrální hustoty pro spojitý signál P XX ( f ) R ( τ ) 2 π j f τ = XX e dτ RXX. (2.4) Pro diskrétní signál R XX je výpoet výkonové spektrální hustoty provádn pomocí (2.5) P XX 1 2 π j f m / f XX e f S m= ( f ) = R ( m) S. (2.5) Analýza Allan Variance Allan Variance (AVAR) je analýza signálu v asové oblasti. AVAR zohleduje rozptyl stedních hodnot dané veliiny vypotených z rzných délek asových interval τ (2.7) napí celým záznamem dat. Pro ádnou AVAR je nutné použít dlouhé záznamy dat ád hodin. Existují ti základní typy AVAR. Jedná se o základní AVAR, dále s pekryvem interval a modifikovaná AVAR. Výpoet AVAR lze definovat jako ( ) M 1 1 ( M 1) i= 1 ( ) 2 AVAR τ = y i + 1 y i, (2.6) 2 τ = m T S, (2.7) kde m poet vzork v asovém intervalu, M poet asových interval délky τ v datovém souboru, y stední hodnoty signálu urené v jednotlivých asových intervalech. 2 V pípad krátkých interval τ je k dispozici vtší poet daných interval, ze kterých se poítá stední hodnota, což vede k malým chybám ve výpotu a vtší vypovídací schopnosti. V pípad dlouhých interval τ je poet daných interval malý, což vede k vtším chybám ve - 15 -
výpotu a menší vrohodnosti. Vrohodnost výpotu AVAR odpovídá odhadu chyby definované v [6] 1 δ AVAR ( τ ) =, N (2.8) 2 1 m kde všechny promnné jsou oznaeny u (2.7). Typický prbh Allan Variance je znázornn na obr.2.14. Z prbhu je možné identifikovat jednotlivé typy šum a podle tab.2.3 urit jejich parametry. Obr. 2.14: Typický prbh AVAR [4] Typ šumu Zkratka Sklon kivky Hodnota koeficientu Quantization noise Q -1 Q = σ ( 3) Angular/velocity random walk Flicker noise/bias instability Rate/acceleration random walk ARW -1/2 N = σ () 1 BIN B = σ min /, 664 RRW +1/2 K = σ () 3 Rate ramp noise RR +1 ( 2) R = σ Tab. 2.3: Shrnutí zdroj chyb a jejich parametry [6] Vzhledem k tomu, že jednotlivé složky šumu jsou považovány za nekorelované, lze výslednou hodnotu rozptylu šumu považovat za souet jednotlivých nezávislých šum. Celkový rozptyl lze tedy vyjádit jako σ = σ + σ + σ + σ + σ. (2.9) 2 total 2 Q 2 ARW 2 BIM 2 RRW 2 RR - 16 -
2.3. Kalmanova filtrace Kalmanova filtrace se adí mezi adaptivní typy filtrace. Je založena na znalosti stavového modelu systému. Kalmanv filtr je v podstat pozorovatel stavu, který se na základ vytvoeného modelu systému a pozorování výstup fyzického systému snaží odhadnout jeho stavy. Princip Kalmanovy filtrace (KF) vychází ze základ statistického potu a pro jeho realizaci je použito pti vztah. Nejdležitjší ást KF je výpoet Kalmanova zesílení K. Podrobné odvození KF je možné nalézt v literatue [2], [3]. Popis diskrétního lineárního systému je dán rovnicemi ( k ) = Ax( k) + Bu( k ) v( k ) x + 1 +, (2.1) ( k) Cx( k) + Du( k ) w( k) kde x ( k) stavový vektor, u ( k) vektor vstupních veliin, ( k) v ( k), w( k) poruchové veliiny, y = +, (2.11) y vektor výstupních veliin, A, B, C, D matice systému, k asový krok. Na následujícím obrázku je uvedeno blokové schéma systému, popsaného (2.1) a (2.11). Poruchové veliiny ( k) w( k) Obr. 2.15: Blokové schéma dynamického systému v, odpovídající šumu procesu a šumu mení jsou považovány za nekorelované bílé šumy s nulovou stední hodnotou a kovarianními maticemi Q a R. Následn lze definovat kovarianní matici chyb odhadu jako - 17 -
( k ) Q S [ ( ) ( )] = ( ) v k w k S R v P( k ) = E, T (2.12) w k která vypovídá o tom, jak úspšný byl odhad stavového vektoru. Pro nekorelované šumy je matice S nulová. Cílem KF je odhad stavového vektoru x ( k +1) a kovarianní matice chyb odhadu ( k + 1) P, k emuž se využívá kritérium LMMSE. Algoritmus filtrace se skládá ze dvou krok. asový krok predikce budoucího stavu na základ znalosti aktuálního stavu, dále na základ odhadu stavu se urí výstupní vektor a kovarianní matice chyb odhadu. Datový krok zde je provedena korekce odhadnutého stavu na základ odmru nových dat. Postup výpotu v KF je dán následujícími pti rovnicemi T [ C + R ] 1 T ( ) C C P ( ) K, (2.13) k = Pk k k k k k ( ) = x ( ) + K [ y C x ( ) ] k + k k k k k x, (2.14) ( ) = [ I K C ] P ( ) k + k k k P, (2.15) ( ) = Ax ( + ) x 1, (2.16) k + k T ( ) = AP ( + ) A Q Pk +1 k +, (2.17) kde rovnice (2.14), (2.15) odpovídají datovému kroku a rovnice (2.16), (2.17) odpovídají asovému kroku. Oznaení ( ) odpovídá hodnotám ped aktualizací dat, oznaení ( + ) odpovídá hodnotám po aktualizaci. Na obr.2.16 je uveden zpsob propojení KF s fyzickým systémem. Kalmanv filtr Obr. 2.16: Spojení KF se systémem [2] - 18 -
Na obr.2.17 je schematicky znázornn postup výpotu KF. V asovém kroku je odhadnut stav systému x k ( ) a kovarianní matice chyb odhadu P k ( ). V okamžiku, kdy je k dispozici vektor mených hodnot y ( k), je provedena korekce odhadnutého stavu a kovarianní matice chyb odhadu, ímž je získáno x ( + ) a P ( + ) (datový krok). Dále se tento postup opakuje. k k Obr. 2.17: Schematické znázornní výpotu KF[3] Vzhledem k tomu, že vtšina systém je nelineárních, je nutné pro použití KF v tomto pípad tyto systémy linearizovat v okolí pracovního bodu. Zpsob provedení linearizace je nkolik a jejich detailní popis je uveden v literatue [3]. Jedním ze zpsob linearizace je tzv. rozšíený Kalmanv filtr (Extended Kalman Filter - EKF). Stavový popis nelineárního systému vyjadují následující rovnice ( k 1 ) = f ( x( k), u( k) v( k )) x +,, (2.18) ( k 1 ) = h( x( k), u( k) w( k) ) y +,. (2.19) Algoritmus EKF je shodný s algoritmem lineárního KF s tím rozdílem, že matice systému A, C jsou definovány pomocí parciálních derivací nelineárních funkcí podle jednotlivých stavových promnných [3]. f ( x) A = x h( x) C = x x x (2.2) (2.21) - 19 -
3. Praktické ešení Cílem práce bylo vytvoení micí jednotky umlého horizontu s použitím klasické konfigurace senzor úhlové rychlosti a klasické a modifikované soustavy akcelerometr. Jednotka vyítá data ze senzor, na základ kterých poítá polohové úhly. Tyto úhly, vetn mených veliin, jsou posílány do PC, kde v prostedí MATLAB probíhá vizualizace vypotených polohových úhl na displeji umlého horizontu a uložení mených veliin pro pozdjší zpracování. Jednotka je dále schopna komunikovat s nadazeným systémem pomocí sbrnice CAN. Blokové schéma realizace micího systému je uvedeno na obr.3.1. 3 x senzor úhlové rychlosti 3 x akcelerometr klasická soustava SPI mikroprocesor RS232 CAN PC (MATLAB) Nadazený systém 4 x akcelerometr modifikovaná soustava IMU jednotka Obr. 3.1: Blokové schéma realizovaného systému - 2 -
3.1. Popis IMU jednotky Realizovaná IMU jednotka je uvedena na obr.3.2. Skládá se z univerzální procesorové desky a ze senzorové desky. Na senzorové desce je umístn tíosý inerciální senzor ADIS16355 a dále jsou k ní pipojeny dv desky s dvouosými akcelerometry ADIS1629. Komunikace procesoru se senzory je realizována pomocí sbrnice SPI (Serial Peripheral Interface). Pro komunikaci jednotky s PC je použito sériové linky RS232 a pro komunikaci s nadazeným systémem je použito sbrnice CAN. deska AY ADIS1629 deska AX ADIS1629 senzorová deska ADIS16355 procesorová deska Obr. 3.2: Uspoádání realizované IMU jednotky V následující tabulce 3.1 jsou uvedeny základní parametry použitých senzor a na obr.3.3 je jejich blokové schéma. Senzor ADIS16355 ADIS1629 Veliina úhlová rychlost zrychlení Zrychlení Rozsah ± 75 /s, ± 15 /s, ± 3 /s ± 1g ± 1,7g Rozlišení,1832 /s,,3663 /s,,7326 /s 2,522 mg,244 mg Poet bit 14 14 14 Kódování dvojkový doplnk dvojkový doplnk dvojkový doplnk Šum [rms],17 /s,,35 /s,,6 /s 35 mg 1,7 mg Napájení [V] 5 5 3,3 Tab. 3.1: Parametry použitých senzor Na obr.3.4 je uvedeno uspoádání os citlivostí jednotlivých senzor. V tab.3.2 jsou uvedeny názvy mených veliin a jejich oznaení, které bude používáno ve zbylé ásti textu. - 21 -