Fakulta elektrotechnická. Odhad orientace UAV v prostoru

ˇ ´ vysoke ´ uc ˇen´ı technicke ´ v Praze Cesk e ´ Fakulta elektrotechnicka

´ PRACE ´ DIPLOMOVA Odhad orientace UAV v prostoru

Praha, 2010

ˇ an Autor: Michal Silh´

Podˇ ekov´ an´ı Dˇekuji pˇredevˇs´ım vedouc´ımu diplomové práce Ing. Martinu Hromˇc´ıkovi, Ph.D. za cenné pˇripom´ınky a rady pˇri ˇreˇsen´ı problém˚ u souvisej´ıc´ıch s diplomovou prac´ı. Dále bych ˇ rád podˇekoval Ing. Ondˇreji Spinkovi za poskytnut´ı dat a fakt˚ u d˚ uleˇzit´ ych pro tuto práci.

ii

Abstrakt Bezpilotn´ı vzduˇsné prostˇredky (UAV) a jejich pouˇzit´ı se v dneˇsn´ı dobˇe velmi rychle ˇ rozv´ıjej´ı. Casto vˇsak b´ yvaj´ı osazeny levn´ ymi senzorick´ ymi systémy, které neposkytuj´ı mˇeˇren´ı dostateˇcné pˇresnosti. Tato práce se zab´ yvá moˇznost´ı, jak pomoc´ı levn´ ych senzor˚ u co nejpˇresnˇeji odhadnout mˇeˇrenou veliˇcinu.

Konkrétnˇe se tato práce zab´ yvá integrac´ı dat z gyroskop˚ u, akcelerometr˚ u a magnetometr˚ u za u ´ˇcelem odhadu polohov´ ych u ´hl˚ u vzduˇsného prostˇredku v prostoru. Jako nástroj pro integraci dat je potom pouˇzit algoritmus Kalmanova filtru, jehoˇz odhad je zpˇresˇ nován pomoc´ı modelovan´ ych chyb senzor˚ u.

iii

Abstract Unmanned aerial vehicles (UAVs) are nowadays rapidly developing. They are usually equipped with low-cost sensor systems, which do not provide sufficient measurement accuracy. This work deals with methods for low-cost sensors to estimate measured quantity with good accuracy.

Specifically, this work deals with integration of data from rate gyros, accelerometers and magnetometers to estimate Euler angles of a UAV. As a tool for data integration, the Kalman filter algorithm with sensor error model is then used.

iv

vi

Obsah Seznam obr´ azk˚ u

xi

´ 1 Uvod

1

2 Obecnˇ e

3

2.1

Pohyb v prostoru . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

2.2

Souˇradné soustavy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

2.2.1

Inerciáln´ı souˇradná soustava . . . . . . . . . . . . . . . . . . . . .

4

2.2.2

Zemská souˇradná soustava . . . . . . . . . . . . . . . . . . . . . .

5

2.2.3

Navigaˇcn´ı souˇradná soustava . . . . . . . . . . . . . . . . . . . . .

6

2.2.4

Tˇelesová souˇradná soustava . . . . . . . . . . . . . . . . . . . . .

7

2.2.5

Transformace souˇradn´ ych soustav . . . . . . . . . . . . . . . . . .

7

2.2.6

Reprezentace orientace tˇelesa . . . . . . . . . . . . . . . . . . . .

9

2.2.6.1

Matice smˇerov´ ych kosin˚ u . . . . . . . . . . . . . . . . .

10

2.2.6.2

Eulerovy u ´hly . . . . . . . . . . . . . . . . . . . . . . . .

10

2.2.6.3

Quaterniony . . . . . . . . . . . . . . . . . . . . . . . . .

12

2.3

Kalman˚ uv filtr . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

14

2.4

Rozˇs´ıˇren´ y Kalman˚ uv filtr . . . . . . . . . . . . . . . . . . . . . . . . . . .

17

3 Sbˇ er dat 3.1

19

Senzory inerciáln´ı navigace . . . . . . . . . . . . . . . . . . . . . . . . . .

19

3.1.1

Gyroskopy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

20

3.1.1.1

Mechanické gyroskopy . . . . . . . . . . . . . . . . . . .

20

3.1.1.2

Optické gyroskopy . . . . . . . . . . . . . . . . . . . . .

21

vii

3.1.1.3

Elektrické gyroskopy . . . . . . . . . . . . . . . . . . . .

22

3.1.1.4

Jaderné gyroskopy . . . . . . . . . . . . . . . . . . . . .

23

3.1.1.5

Kvantové gyroskopy . . . . . . . . . . . . . . . . . . . .

23

Akcelerometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

23

3.2

Magnetometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

25

3.3

Chyby senzor˚ u . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

28

3.1.2

4 Integrace dat 4.1

31

Generátor dat . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

33

4.1.1

Vˇetev gyroskop˚ u . . . . . . . . . . . . . . . . . . . . . . . . . . .

33

4.1.2

Vˇetev akcelerometr˚ u . . . . . . . . . . . . . . . . . . . . . . . . .

34

4.1.3

Vˇetev magnetometr˚ u . . . . . . . . . . . . . . . . . . . . . . . . .

36

4.1.4

Kompletn´ı generátor dat . . . . . . . . . . . . . . . . . . . . . . .

38

4.2

Senzorick´ y model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

38

4.3

Kalman˚ uv filtr . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

43

5 V´ ysledky

51

5.1

Umˇele vytvoˇrená data . . . . . . . . . . . . . . . . . . . . . . . . . . . .

51

5.2

Reálná letová data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

57

6 Moˇ zn´ a zpˇ resnˇ en´ı odhadu

63

7 Z´ avˇ er

69

Literatura

71

A Skripty gener´ atoru dat

I

A.1 Transformace Eulerov´ ych u ´hl˚ u na u ´hlové rychlosti . . . . . . . . . . . . .

I

A.2 Transformace vektoru z navigaˇcn´ı do tˇelesové souˇradné soustavy . . . . .

I

B Skripty senzorick´ eho modelu

III

B.1 Misalignment error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

III

B.2 Temperature drift . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

III

viii

B.3 Bias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

IV

B.4 Chyba polohy akcelerometr˚ u mimo tˇeˇziˇstˇe . . . . . . . . . . . . . . . . .

IV

C Obsah pˇ riloˇ zen´ eho CD

V

ix

x

Seznam obr´ azk˚ u 2.1

Pohybové veliˇciny vzuˇsného prostˇredku . . . . . . . . . . . . . . . . . . .

4

2.2

Znázornˇen´ı inreciáln´ı (i ), zemské (e ) a navigaˇcn´ı (n ) souˇradné soustavy .

5

2.3

Znázornˇen´ı tˇelesové (b ) souˇradné soustavy . . . . . . . . . . . . . . . . .

6

2.4

Kladn´ y (+) a záporn´ y (-) smˇer rotace souˇradné soustavy . . . . . . . . .

7

2.5

Rotace souˇradn´ ych soustav . . . . . . . . . . . . . . . . . . . . . . . . . .

8

2.6

Algoritmus Kalmanova filtru . . . . . . . . . . . . . . . . . . . . . . . . .

17

2.7

Algoritmus rozˇs´ıˇreného Kalmanova filtru . . . . . . . . . . . . . . . . . .

18

3.1

Uspoˇrádán´ı sn´ımaˇce MEMS gyroskopu . . . . . . . . . . . . . . . . . . .

22

3.2

Dipólov´ y charakter magnetického pole Zemˇe (zdroj: www.cez.cz) . . . . .

26

3.3

Celková intenzita mag. pole na Zemi v nT (zdroj: www.geophysics.ou.edu)

27

3.4

Mag. deklinace na Zemi ve stupn´ıch z roku 1860 (zdroj: www.wikipedia.org) 27

4.1

Schéma mˇeˇren´ı ideáln´ımi gyroskpy . . . . . . . . . . . . . . . . . . . . .

34

4.2

Schéma mˇeˇren´ı ideáln´ımi akcelerometry ve funkci sklonomˇeru . . . . . . .

35

4.3

Schéma mˇeˇren´ı ideáln´ımi magnetometry . . . . . . . . . . . . . . . . . .

37

4.4

Schéma generátoru dat . . . . . . . . . . . . . . . . . . . . . . . . . . . .

39

4.5

Schéma senzorického modelu . . . . . . . . . . . . . . . . . . . . . . . . .

40

5.1

Vytvoˇrené pr˚ ubˇehy polohov´ ych u ´hl˚ u . . . . . . . . . . . . . . . . . . . .

52

5.2

Data senzor˚ u v ose z . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

54

5.3

Srovnán´ı skuteˇcn´ ych a odhadnut´ ych quaternion˚ u (vlevo) a odpov´ıdaj´ıc´ıch

5.4

polohov´ ych u ´hl˚ u (vpravo) . . . . . . . . . . . . . . . . . . . . . . . . . .

56

Srovnán´ı skuteˇcn´ ych a odhadnut´ ych aditivn´ıch chyb . . . . . . . . . . . .

56

xi

5.5

Pr˚ ubˇehy polohov´ ych u ´hl˚ u z reálného letu . . . . . . . . . . . . . . . . . .

58

5.6

Srovnán´ı mˇeˇren´ ych a modelovan´ ych dat . . . . . . . . . . . . . . . . . . .

60

5.7

Srovnán´ı mˇeˇren´ ych a odhadnut´ ych quaternion˚ u . . . . . . . . . . . . . .

61

5.8

Srovnán´ı mˇeˇren´ ych a odhadnut´ ych polohov´ ych u ´hl˚ u . . . . . . . . . . . .

62

6.1

Srovnán´ı polohov´ ych u ´hl˚ u skuteˇcn´ ych, odhadnut´ ych a stanoven´ ych dynamick´ ym modelem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

xii

67

Kapitola 1 ´ Uvod Letectv´ı lze v dneˇsn´ı dobˇe rozdˇelit do nˇekolika kategori´ı podle toho, kdo a jak´ ym zp˚ usobem letadla vyuˇz´ıvá. Jako dvˇe nejvetˇs´ı skupiny m˚ uˇzeme oznaˇcit civiln´ı a vojenské letectv´ı. Dnes se vˇsak stále v´ıce rozˇsiˇruje skupina letadel, která má oproti pˇredchoz´ım dvˇema nˇekolik nesporn´ ych v´ yhod. Jsou to bezpilotn´ı vzduˇsné prostˇredky (UAV), které d´ıky absenci posádky mohou b´ yt nasazovány do mis´ı, které by pro ˇclovˇeka byly extrémnˇe riskantn´ı, nebo tam, kde je pˇr´ıliˇs málo prostoru a velikost pouˇzitelného letadla je omezen. V dneˇsn´ı dobˇe se jiˇz bezpilotn´ı vzduˇsné prostˇredky vyuˇz´ıvaj´ı napˇr´ıklad pro meteorologické u ´ˇcely ˇci monitorován´ı Zemˇe. Jejich v´ yhoda v malé velikosti vˇsak m˚ uˇze b´ yt zároveˇ n i nev´ yhodou, jelikoˇz t´ım pádem leckdy nen´ı na zástavbu veˇsker´ ych potˇrebn´ ych zaˇr´ızen´ı a senzorick´ ych systém˚ u dostatek prostoru.

Dalˇs´ım omezuj´ıc´ım faktorem vˇsak mohou b´ yt i finance. Zat´ımco v pˇr´ıpadˇe vojenského nebo civiln´ıho letectv´ı se mnohdy napˇr´ıklad z d˚ uvodu bezpeˇcnosti na mnoˇzstv´ı vynaloˇzen´ ych finanˇcn´ıch prostˇredk˚ u nehled´ı, v pˇr´ıpadˇe bezpilotn´ıch prostˇredk˚ u, které mohou provozovat i drobn´ı modeláˇri nebo nadˇsenci, mohou b´ yt finance do znaˇcné m´ıry omezené. To je potom spojeno se zástavbou levnˇejˇs´ıch a ménˇe pˇresn´ ych senzorick´ ych systém˚ u, coˇz pouˇzitelnost tˇechto prostˇredk˚ u do znaˇcné m´ıry omezuje.

Konkrétnˇe se tato práce bude zab´ yvat odhadem orientace vzduˇsného prostˇredku v prostoru za pˇredpokladu pouˇzit´ı levn´ ych a ménˇe pˇresn´ ych senzor˚ u, které jsou zat´ıˇzeny mnoha 1

´ KAPITOLA 1. UVOD

2 chybami.

Práce je rozdˇelena následuj´ıc´ım zp˚ usobem. V kapitole 2 se seznám´ıme s obecn´ ymi informacemi o pohybov´ ych moˇznostech a reprezentaci polohy vzduˇsného prostˇredku v prostoru, dále je zde pak nast´ınˇen princip práce algortitmu Kalmanova filtru a jeho rozˇs´ıˇrené modifikace, která bude v této práci pouˇzita. Kapitola 3 potom obsahuje informace o senzorech pouˇz´ıvan´ ych v této práci, zejména o fyzikáln´ıch jevech a principech, na kter´ ych jsou mˇeˇren´ı tˇemito senzory zaloˇzena. Kapitola 4 dále pojednává o integraci dat ze senzor˚ u. Jsou zde uvedeny základn´ı informace o moˇznostech integrace a dále je zde podrobnˇe rozebrána integrace dat ze senzor˚ u pouˇz´ıvan´ ych v této práci za u ´ˇcelem odhadu polohov´ ych u ´hl˚ u vzduˇsného prostˇredku. V kapitole 5 jsou potom uvedeny v´ ysledky integrace pˇri pouˇzit´ı jak umˇele vytvoˇren´ ych dat, tak i dat z reálného letu. Kapitola 6 dále nastiˇ nuje moˇznosti dalˇs´ıho vylepˇsen´ı algoritmu. V kapitole 7 je nakonec tato práce shrnuta a podány závˇereˇcné informace.

Kapitola 2 Obecnˇ e 2.1

Pohyb v prostoru

Prvn´ı vˇec´ı, kterou je nutno udˇelat, je obecné definován´ı pohybov´ ych moˇznost´ı vzduˇsného prostˇredku a jejich znaˇcen´ı, které bude pouˇz´ıváno dále v této práci. Základn´ı pohybové veliˇciny jsou potom znázornˇeny na obrázku 2.1. Zde je zobrazeno schéma vrtuln´ıku, jehoˇz tˇelesové osy x, y a z procházej´ı tˇeˇziˇstˇem a dalˇs´ı pohybové veliˇciny jsou s tˇemito osami svázány. Veliˇciny u, v a w znaˇc´ı rychlosti pohybu ve smˇerech pˇr´ısluˇsn´ ych os, veliˇciny znaˇcené jako Φ, Θ a Ψ pˇredstavuj´ı u ´hly natoˇcen´ı vrtuln´ıku kolem pˇr´ısluˇsn´ ych os, ˇc´ımˇz ´ urˇcuj´ı jeho orientaci v prostoru. Uhel natoˇcen´ı Φ je také naz´ yván jako klopen´ı (angl. roll), ´hel Θ je potom nazáván jako klonˇen´ı (angl. pitch) a u u ´hel Ψ je naz´ yván jako zatáˇcen´ı (angl. yaw). Posledn´ımi zobrazen´ ymi veliˇcinami, které jsou oznaˇceny jako p, q a r, jsou potom u ´hlové rychlosti otáˇcen´ı kolem pˇr´ısluˇsn´ ych os.

2.2

Souˇ radn´ e soustavy

Pro správnou interpretaci dat z´ıskan´ ych ze senzor˚ u a t´ım i pro správné urˇcen´ı orientace tˇelesa v prostoru je nutné definovat nˇekolik druh˚ u souˇradn´ ych soustav (Titterton, D. H. a Weston, J. L., 2004). Kaˇzdá z nich vˇsak bude ortogonáln´ı (osy souˇradného systému jsou vzájemnˇe kolmé) a pravotoˇcivá, liˇsit se potom budou vztaˇzn´ ymi soustavami. 3

ˇ KAPITOLA 2. OBECNE

4

Obr´ azek 2.1: Pohybové veliˇciny vzuˇsného prostˇredku

Souˇradné soustavy se dˇel´ı na tˇri základn´ı skupiny. Prvn´ı skupinou jsou soustavy, jejichˇz poˇca´tek se umist’uje do stˇredu Zemˇe, a proto je naz´ yváme jako zemské souˇradné systémy. Druhou skupinou jsou soustavy, jejichˇz poˇca´tek se umist’uje s ohledem na navigaˇcn´ı systém a naz´ yváme je lokáln´ı souˇradné soustavy. Posledn´ı, tˇret´ı, skupinou jsou soustavy, jejichˇz poˇcátek b´ yvá nˇejak´ ym zp˚ usobem spojen s tˇelesem, proto se naz´ yvaj´ı tˇelesové souˇradné soustavy. Jednotlivé souˇradné soustavy jsou znázornˇeny na obrázc´ıch 2.2 a 2.3.

2.2.1

Inerci´ aln´ı souˇ radn´ a soustava

Inerciáln´ı souˇradná soustava spadá do prvn´ı skupiny definované v´ yˇse, jej´ı poˇca´tek je tedy um´ıstˇen do stˇredu Zemˇe. Osa z je shodná s osou rotace Zˇemˇe, která je v tomto pˇr´ıpadˇe povaˇzována za nemˇennou ve svém smˇeru, a procház´ı zemˇepisn´ ym severn´ım pólem. Zbylé

ˇ ´ SOUSTAVY 2.2. SOURADN E

5

Obr´ azek 2.2: Zn´ azornˇen´ı inreci´ aln´ı (i ), zemské (e ) a navigaˇcn´ı (n ) souˇradné soustavy

dvˇe osy x a y jsou potom fixovány na okoln´ı hvˇezdy. Celá tato souˇradná soustava tedy nerotuje spoleˇcnˇe se Zem´ı. Na obrázku 2.2 jsou osy této soustavy oznaˇceny jako xi , y i a zi.

2.2.2

Zemsk´ a souˇ radn´ a soustava

Stejnˇe jako inerciáln´ı souˇradná soustava spadá i zemská souˇradná soustava do prvn´ı skupiny s poˇca´tkem um´ıstˇen´ ym do stˇredu Zemˇe, avˇsak jej´ı osy jsou jiˇz pevnˇe spjaté se Zem´ı. Osa z je opˇet shodná s osou rotace Zemˇe a procház´ı zemˇepisn´ ym severn´ım pólem. Osa x potom leˇz´ı v pr˚ useˇcnici roviny rovn´ıku a poloroviny nultého poledn´ıku. Osa y se potom umist’uje tak, aby doplnila ortogonáln´ı pravotoˇcivou souˇradnou soustavu. Celá zemská souˇradná soustava potom vzhledem k inerciáln´ı souˇradné soustavˇe rotuje kolem


6

Obr´ azek 2.3: Zn´ azornˇen´ı tˇelesové (b ) souˇradné soustavy

osy z u ´hlovou rychlost´ı rotace Zemˇe. Na obrázku 2.2 jsou osy této soustavy oznaˇceny jako xe , y e a z e .

2.2.3

Navigaˇ cn´ı souˇ radn´ a soustava

Navigaˇcn´ı souˇradná soustava spadá do druhé skupiny definované v´ yˇse. Je to lokáln´ı zemˇepisná soustava, jej´ıˇz poˇca´tek je um´ıstˇen shodnˇe s poˇca´tkem soustavy os navigaˇcn´ıho systému. Osa z potom vˇzdy m´ıˇr´ı smˇerem k zemˇepisnému severu, osa y vˇzdy m´ıˇr´ı na ´ v´ ychod a osa z stále smˇeˇruje do stˇredu Zemˇe. Uhlov´ a rychlost a smˇer otáˇcen´ı navigaˇcn´ı souˇradné soustavy v˚ uˇci zemské souˇradné soustavˇe je dán rychlost´ı a smˇerem pohybu poˇca´tku navigaˇcn´ı souˇradné soustavy. Tato souˇradná soustava se pouˇz´ıvá pro navigaci. Na obrázku 2.2 jsou osy této soustavy oznaˇceny jako xn , y n a z n .


7

Obr´ azek 2.4: Kladn´ y (+) a z´ aporn´ y (-) smˇer rotace souˇradné soustavy

2.2.4

Tˇ elesov´ a souˇ radn´ a soustava

Tˇelesová souˇradná soustava spadá do tˇret´ı skupiny definované v´ yˇse. Poˇca´tek této soustavy umist’ujeme do tˇeˇziˇstˇe tˇelesa, osa z smˇeˇruje smˇerem dol˚ u a je shodná s osou zatáˇcen´ı telesa. Osa x smˇeˇruje dopˇredu a je shodná s osou klonˇen´ı tˇelesa. Osa y potom doplˇ nuje ortogonáln´ı pravotoˇciv´ y systém a je shodná s osou klopen´ı tˇelesa. Tato souˇradná soustava je znázornˇena na obrázku 2.3, kde jednotlivé osy této soustavy jsou oznaˇceny jako xb , y b a zb.

2.2.5

Transformace souˇ radn´ ych soustav

Jelikoˇz je bˇeˇzné, ˇze r˚ uzné senzory um´ıstˇené na tˇelese mˇeˇr´ı veliˇciny v r˚ uzn´ ych souˇradn´ ych soustavách, je nutné tyto veliˇciny mezi jednotliv´ ymi soustavami pˇrevádˇet. Jednotlivé soustavy se transformuj´ı jedna do druhé pomoc´ı rotac´ı. Pro tento u ´ˇcel definujeme kladn´ y a záporn´ y smˇer rotace. Za kladn´ y smˇer rotace kolem jednotliv´ ych os budeme povaˇzovat takov´ y smˇer, kdy pˇri pohledu ve smˇeru osy rotace rotuje soustava ve smˇeru hodinov´ ych ruˇciˇcek. Za záporn´ y smˇer rotace se potom povaˇzuje rotace ve smˇeru opaˇcném, tedy proti smˇeru hodinov´ ych ruˇciˇcek. Kladn´ y a záporn´ y smˇer rotace je znázornˇen na obrázku 2.4.


8

Obr´ azek 2.5: Rotace souˇradn´ ych soustav


9

Z obrázku 2.4 je také patrné, ˇze orientaci tˇelesa m˚ uˇzeme vyjádˇrit pomoc´ı série rotac´ı kolem jednotliv´ ych os. Je vˇsak velice d˚ uleˇzité si uvˇedomit, ˇze zmˇena orientace tˇelesa zp˚ usobená takovouto rotac´ı nen´ı funkc´ı pouze velikost´ı u ´hl˚ u rotace kolem jednotliv´ ych os, ale také poˇrad´ı, ve kterém jsou jednotlivé rotace provádˇeny. Tato skuteˇcnost je zˇrejmá z obrázku 2.5. Zde jsou v jednotliv´ ych sloupc´ıch znázornˇeny dvˇe série rotac´ı souˇradného systému (xb ,y b ,z b ) se shodnou v´ ychoz´ı polohou (xe ,y e ,z e ). V obou séri´ıch jsou potom provádˇeny rotace stejné velikosti ve stejném smˇeru dle konvence znázornˇené na obrázku 2.4, liˇs´ı se vˇsak poˇrad´ım, v jakém jsou jednotlivé rotace provádˇeny. V levém sloupci jsou rotace provádˇeny v poˇrad´ı yaw → roll → pitch, v pravém sloupci je to potom sekvence pitch → roll → yaw. Vˇsechny rotace maj´ı velikost 90◦ v kladném smˇeru. V posledn´ım ˇra´dku obrázku potom vid´ıme, ˇze aˇc mˇely rotace kolem jednotliv´ ych os stejn´ y smˇer i velikost, v´ ysledky se d´ıky rozd´ılnému poˇrad´ı rotac´ı podstatnˇe liˇs´ı.

2.2.6

Reprezentace orientace tˇ elesa

K vyjádˇren´ı orientace tˇelesa vzhledem pˇr´ısluˇsné souˇradné soustavˇe m˚ uˇzeme vyuˇz´ıt nˇekolik r˚ uzn´ ych matematick´ ych metod. Kaˇzdé metodˇe potom pˇr´ısluˇs´ı nˇekolik kl´ıˇcov´ ych parametr˚ u. Ze vˇsech r˚ uzn´ ych metod reprezentace se nejv´ıce pouˇz´ıvaj´ı následuj´ıc´ı tˇri, které budou podrobnˇeji popsány dále. 1. Matice smˇ erov´ ych kosin˚ u. Je to matice 3 × 3, jej´ıˇz sloupce reprezentuj´ı jednotkové vektory v tˇelesové souˇradné soustavˇe transformované podél navigaˇcn´ı souˇradné soustavy. 2. Eulerovy u ´ hly. V tomto pˇr´ıpadˇe se transformace z jedné souˇradné soustavy do druhé definuje pomoc´ı tˇr´ı po sobˇe jdouc´ıch rotac´ı kolem jednotliv´ ych os. Reprezentace pomoc´ı Eulerov´ ych u ´hl˚ u je pravdˇepodobnˇe jedna z nejjednoduˇsˇs´ıch metod vyjádˇren´ı orientace tˇelesa. 3. Quaterniony. Reprezentace orientace tˇelesa pomoc´ı quaternion˚ u umoˇzn ˇuje transformaci z jedné souˇradné soustavy do druhé pomoc´ı jediné rotace kolem vektoru definovaného v referenˇcn´ı souˇradné soustavˇe. Quaternion je vektor o ˇctyˇrech prvc´ıch,


10

které jsou funkcemi orientace tohoto vektoru a velikosti rotace. 2.2.6.1

Matice smˇ erov´ ych kosin˚ u

Matici smˇerov´ ych kosin˚ u, která má rozmˇer 3 × 3, ⎡ c c ⎢ 11 12 ⎢ Cbn = ⎢ c21 c22 ⎣ c31 c32

m˚ uˇzeme zapsat následovnˇe. ⎤ c13 ⎥ ⎥ c23 ⎥ ⎦ c33

Jak jiˇz bylo ˇreˇceno v´ yˇse, jednotlivé sloupce matice Cbn reprezentuj´ı jednotkové vektory v tˇelesové souˇradné soustavˇe transformované podél navigaˇcn´ı souˇradné soustavy. Prvek v i-tém ˇra´dku a j-tém sloupci pˇredstavuje kosinus u ´hlu mezi i-tou osou navigaˇcn´ı souˇradné soustavy a j-tou osou tˇelesové souˇradné soustavy.

Transformace vektoru vyjádˇreného v jedné souˇradné soustavˇe do souˇradné soustavy druhé se potom vykoná pˇrenásoben´ım tohoto vektoru matic´ı smˇerov´ ych kosin˚ u. Pokud tedy napˇr´ıklad budeme cht´ıt vektor rn vyjádˇren´ y v navigaˇcn´ı souˇradné soustavˇe transformovat do tˇelesové souˇradné soustavy, m˚ uˇzeme to provést dle následuj´ıc´ıho pˇredpisu. rb = Cbn rn

(2.1)

Podrobnˇejˇs´ı v´ yklad t´ ykaj´ıc´ı se matice smˇerov´ ych kosin˚ u je moˇzno nalézt v literatuˇre (Titterton, D. H. a Weston, J. L., 2004). 2.2.6.2

Eulerovy u ´hly

Transformace z jedné souˇradné soustavy do drhuhé, jak jiˇz bylo ˇreˇceno v´ yˇse, m˚ uˇzeme také provést pomoc´ı tˇr´ı po sobˇe jdouc´ıch rotac´ı kolem jednotliv´ ych os. Napˇr´ıklad m˚ uˇzeme uvaˇzovat situaci z levého sloupce obrázku 2.5. Zde je transformace z v´ ychoz´ıho do nového souˇradného systému vyjádˇrena následovnˇe. 1. rotace kolem osy z v´ ychoz´ıho souˇradného systému o u ´hel Ψ v kladném smˇeru,


11

2. rotace kolem osy x nového souˇradného systému o u ´hel Φ v kladném smˇeru, 3. rotace kolem osy y nového souˇradného systému o u ´hel Θ v kladném smˇeru. ´ Uhly Φ, Θ a Ψ jsou oznaˇcovány právˇe jako Eulerovy u ´hly. Reprezentace pomoc´ı Eulerov´ ych u ´hl˚ u je obl´ıbená hlavnˇe z d˚ uvodu jednoduchého mˇeˇren´ı pomoc´ı senzor˚ uu ´hlové rychlosti a také z d˚ uvodu jednoduchosti na pˇredstavu.

Jednotlivé rotace m˚ uˇzeme také vyjádˇrit pomoc´ı tˇr´ı matic smˇerov´ ych kosin˚ u jako ⎡

⎤ cos(Ψ)

sin(Ψ) 0

⎢ ⎢ C1 = ⎢ − sin(Ψ) cos(Ψ) 0 ⎣ 0 0 1 ⎡ cos(Θ) 0 − sin(Θ) ⎢ ⎢ C2 = ⎢ 0 1 0 ⎣ sin(Θ) 0 cos(Θ) ⎡ 1 0 0 ⎢ ⎢ C3 = ⎢ 0 cos(Φ) sin(Φ) ⎣ 0 − sin(Φ) cos(Φ)

⎥ ⎥ ⎥, ⎦ ⎤ ⎥ ⎥ ⎥, ⎦ ⎤ ⎥ ⎥ ⎥, ⎦

´hel Ψ kolem osy z, C2 pˇredstavuje rotaci o u ´hel Θ kolem kde C1 pˇredstavuje rotaci o u osy y a C3 pˇredstavuje rotaci o u ´hel Φ kolem osy x. Pokud bychom napˇr´ıklad chtˇeli pomoc´ı tˇechto tˇr´ı matic vyjádˇrit transformaˇcn´ı matici vedouc´ı k transformaci z navigaˇcn´ı do tˇelesové souˇradné soustavy, kde tato matice bude pˇredstavovat rotaci o jednotlivé u ´hly kolem jednotliv´ ych os v poˇrad´ı yaw → roll → pitch, staˇc´ı tyto tˇri matice smˇerov´ ych kosin˚ u ve správném poˇrad´ı vynásobit. V tomto pˇr´ıpadˇe tedy Cbn = C2 C3 C1 .

(2.2)

Podobnˇe m˚ uˇzeme vyjádˇrit i matici inverzn´ı transformace, tedy transformace z tˇelesové do navigaˇcn´ı soustavy. T T T Cnb = CbT n = C1 C3 C2


12

˙ Θ, ˙ Ψ ˙ Vztah mezi u ´hlov´ ymi rychlostmi p, q, r a rychlost´ı zmˇeny Eulerov´ ych u ´hl˚ u Φ, potom vypadá následovnˇe. ⎡

p

⎤

⎡

Φ˙

⎡

⎤

⎤

⎡

0

⎤ 0

⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎢ ⎢ ˙ ⎥ ⎢ ⎥ ⎥ ⎢ q ⎥ = ⎢ 0 ⎥ + C3 ⎢ Θ ⎥ + C3 C2 ⎢ 0 ⎥ ⎣ ⎦ ⎣ ⎣ ⎦ ⎣ ⎦ ⎦ ˙ r 0 Ψ 0

(2.3)

Roznásoben´ım a pˇreuspoˇrádán´ım rovnice (2.3) dostneme ⎡

Φ˙

⎤

⎤⎡

⎡ 1 sin(Φ) tan(Θ) cos(Φ) tan(Θ)

⎢ ⎥ ⎢ ⎢ ˙ ⎥ ⎢ ⎢ Θ ⎥=⎢ 0 ⎣ ⎦ ⎣ ˙ 0 Ψ

cos(Φ)

− sin(Φ)

sin(Φ) cos(Θ)

cos(Φ) cos(Θ)

p

⎤

⎥⎢ ⎥ ⎥⎢ ⎥ ⎥⎢ q ⎥. ⎦⎣ ⎦ r

(2.4)

Jednotlivé Eulerovy u ´hly m˚ uˇzeme z rovnice (2.4) z´ıskat pomoc´ı integrace pˇri znalosti poˇca´teˇcn´ı oriantace tˇelesa. Tento vztah je vˇsak omezen t´ım, ˇze nab´ yvá neurˇcit´ ych hodnot pokud se Θ rovná ±90◦ . Tyto situace ˇreˇs´ı reprezentace orientace tˇelesa pomoc´ı quaternion˚ u.

2.2.6.3

Quaterniony

Jako jiˇz bylo ˇreˇceno v´ yˇse, reprezentace polohy pomoc´ı quternion˚ u je zaloˇzena na myˇslence, ˇze m˚ uˇzeme provést transformaci z jedné douˇradné soustavy do druhé pomoc´ı jediné rotace kolem vektoru definovaného v referenˇcn´ı souˇradné soustavˇe. Tento vektor budeme znaˇcit μ. Quaternion, kter´ y ze budeme znaˇcit q, je vektor o ˇctyˇrech prvc´ıch, které jsou funkcemi orientace tohoto vektoru a velikosti rotace. Quaternion tedy definujeme jako ⎡

q0

⎤

⎡

⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ q1 ⎥ ⎢ ⎢ ⎥=⎢ q=⎢ ⎥ ⎢ ⎢ q2 ⎥ ⎢ ⎣ ⎦ ⎣ q3

cos( μ2 ) μx μ μy μ μz μ

⎤

⎥ ⎥ ) ⎥ ⎥, ⎥ μ sin( 2 ) ⎥ ⎦ μ sin( 2 ) sin( μ2

kde μx , μy , μz jsou prvky u ´hlového vektoru μ a μ je jeho velikost. Vztah mezi u ´hlov´ ymi rychlostmi p, q, r a rychlost´ı zmˇeny quaternion˚ u q˙0 , q˙1 , q˙2 , q˙3

ˇ ´ SOUSTAVY 2.2. SOURADN E potom vypadá následovnˇe. ⎡

⎤

13

⎡

q˙ ⎢ 0 ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ q˙1 ⎥ 1 ⎢ ⎢ ⎥ ⎢ q˙ = ⎢ ⎥= 2 ⎢ ⎢ q˙2 ⎥ ⎢ ⎣ ⎦ ⎣ q˙3

0 −p −q −r

⎤⎡

⎤

q ⎥⎢ 0 ⎥ ⎥ ⎥⎢ p 0 r −q ⎥ ⎢ q1 ⎥ ⎥ ⎥⎢ ⎥ ⎥⎢ q −r 0 p ⎥ ⎢ q2 ⎥ ⎦ ⎦⎣ q3 r q −p 0

Dále si m˚ uˇzeme pomoc´ı quaternion˚ u vyjádˇrit transformaˇcn´ı matici. ⎡ 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) q 2 + q12 − q22 − q32 ⎢ 0 ⎢ C = ⎢ 2(q1 q2 − q0 q3 ) q02 − q12 + q22 − q32 2(q2 q3 + q0 q1 ) ⎣ 2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) q02 − q12 − q22 + q32

(2.5)

⎤ ⎥ ⎥ ⎥ ⎦

(2.6)

Transformace vektoru vyjádˇreného v jedné souˇradné soustavˇe do souˇradné soustavy druhé se potom, stejnˇe jako je to v pˇr´ıpadˇe matice smˇerov´ ych kosin˚ u, vykoná pˇrenásoben´ım tohoto vektoru transformaˇcn´ı matic´ı. Pokud tedy napˇr´ıklad budeme opˇet cht´ıt vektor rn vyjádˇren´ y v navigaˇcn´ı souˇradné soustavˇe transformovat do tˇelesové souˇradné soustavy, m˚ uˇzeme to provést dle pˇredpisu rb = Crn ,

(2.7)

kde pˇri porovnán´ı s rovnic´ı (2.1) vid´ıme, ˇze C odpov´ıdá matici smˇerov´ ych kosin˚ u Cbn . Quaterniony se daj´ı dále vyjádˇrit pomoc´ı Eulerov´ ych u ´hl˚ u dle následuj´ıc´ıho pˇrepisu. Θ Ψ Φ Θ Ψ Φ ) cos( ) cos( ) + sin( ) sin( ) sin( ) 2 2 2 2 2 2 Θ Ψ Φ Θ Ψ Φ q1 = sin( ) cos( ) cos( ) − cos( ) sin( ) sin( ) 2 2 2 2 2 2 Θ Ψ Φ Θ Ψ Φ q2 = cos( ) sin( ) cos( ) + sin( ) cos( ) sin( ) 2 2 2 2 2 2 Θ Ψ Φ Θ Ψ Φ q3 = cos( ) cos( ) sin( ) − sin( ) sin( ) cos( ) 2 2 2 2 2 2 q0 = cos(

(2.8) (2.9) (2.10) (2.11)

D˚ uleˇzitá podm´ınka pouˇzit´ı quaternion˚ u je, ˇze velikost vektoru q mus´ı b´ yt vˇzdy 1, tedy mus´ı b´ yt splnˇeny následuj´ıc´ı rovnice. q02 + q12 + q22 + q32 = 1 ⇒ q02 + q12 + q22 + q32 = 1

(2.12)


14

Podrobnˇejˇs´ı v´ yklad, kter´ y se zab´ yvá quaterniony, je dále moˇzno nalézt v literatuˇre (Titterton, D. H. a Weston, J. L., 2004).

2.3

Kalman˚ uv filtr

ˇ Kalman˚ uv filtr (Havlena, V. a Stecha, J., 2000) je filtr zaloˇzen´ y na metodˇe nejmenˇs´ıch ˇctverc˚ u chyb a poskytuje velice efektivn´ı v´ ypoˇcetn´ı rekurzivn´ı ˇreˇsen´ı pro odhad signálu, kter´ y je zkreslen Gaussovsk´ ym ˇsumem. Je to algoritmus, kter´ y optimálnˇe vyuˇz´ıvá nepˇresná data lineárn´ıho nebo témˇeˇr lineárn´ıho systému s Gaussovsk´ ymi chybami tak, aby stále dosahoval co nejlepˇs´ıho odhadu aktuáln´ıho stavu systému.

Kalman˚ uv filtr je zaloˇzen na stavovém pˇr´ıstupu, kde stavové rovnice modeluj´ı dynamiku procesu generován´ı signálu a rovnice pozorován´ı (mˇeˇren´ı) modeluj´ı zaˇsumˇen´ y a zkreslen´ y pozorovan´ y (mˇeˇren´ y) signál. Bez ztráty na obecnosti budeme v této kapitole uvaˇzovat systém bez vstup˚ u. Pro signál x (k) a zaˇsumˇené pozorován´ı y (k) jsou pak definovány rovnice popisuj´ıc´ı model procesu a model pozorován´ı jako x (k + 1) = Ax (k) + w (k),

(2.13)

y (k) = Hx (k) + n(k),

(2.14)

kde x (k) je P -rozmˇern´ y vektor signál˚ u neboli také stavov´ y vektor v ˇcase k, A je P × P rozmˇerná pˇrechodová matice dávaj´ıc´ı v souvislost stav systému v ˇcase k a stav systému v ˇcase k + 1, w (k) neboli ˇsum procesu je P -rozmˇern´ y nekorelovan´ y vstupn´ı bud´ıc´ı vektor stavové rovnice. Pˇredpokládáme, ˇze w (k) je normáln´ı (Gaussovsk´ y) proces neboli p(w (k)) ∼ N (0, Q), kde Q je P × P -rozmˇerná kovarianˇcn´ı matice ˇsumu w (k) nebo také kovarianˇcn´ı matice ˇsumu procesu. y (k) je M -rozmˇern´ y zaˇsumˇen´ y vektor pozorován´ı, H je M × P -rozmˇerná matice a n(k) je M -rozmˇern´ y vektor ˇsumu nebo také ˇsum mˇeˇren´ı. Opˇet pˇredpokládáme, ˇze n(k) má normáln´ı rozdˇelen´ı neboli p(n(k)) ∼ N (0, R),

˚ FILTR 2.3. KALMANUV

15

kde R je M × M -rozmˇerná kovarianˇcn´ı matice ˇsumu n(k) nebo také kovarianˇcn´ı matice ˇsumu mˇeˇren´ı.

Algoritmus Kalmanova filtru pracuje se dvˇema druhy odhad˚ u stavu. Prvn´ım z nich je tzv. apriorn´ı odhad, kter´ y je uˇcinˇen´ y v ˇcase k za u ´ˇcelem odhadu stavu v ˇcase k + 1. Tento odhad se také naz´ yvá predikce, jelikoˇz odhaduje budouc´ı stav na základˇe znalosti stav˚ u aktuáln´ıch. Budeme jej znaˇcit jako ˆ x (k + 1|k). Druh´ ym je tzv. aposteriorn´ı odhad, kter´ y je uˇcinˇen v ˇcase k na základˇe mˇeˇren´ı y (k), tedy vyuˇz´ıvá aktuálnˇe zmˇeˇren´ ych hodnot k odhadu aktuáln´ıho stavu. Tento odhad budeme znaˇcit jako ˆ x (k|k). Chyba apriorn´ıho odhadu stavu je potom definována jako e(k|k − 1) = x (k) − ˆ x (k|k − 1) a chyba aposteriorn´ıho odhadu stavu jako e(k|k) = x (k) − ˆ x (k|k). Kovarianˇcn´ı matici chyby apriorn´ıho odhadu stavu potom vyjádˇr´ıme jako

P(k|k − 1) = E e(k|k − 1)e T (k|k − 1) a kovarianˇcn´ı matici chyby aposteriorn´ıho odhadu stavu jako

P(k) = E e(k)e T (k) .

Algoritmus Kalmanova filtru pracuje ve dvou hlavn´ıch kroc´ıch. V prvn´ım kroku (predikce) provede apriorn´ı odhad stavu systému x (k + 1|k) a jeho kovarianˇcn´ı matice chyby P(k + 1|k). Tento krok lze vyjádˇrit rovnicemi ˆ x (k + 1|k) = Aˆ x (k|k), P(k + 1|k) = AP(k|k)AT + Q. V druhém kroku se provád´ı aktualizace stavu predikovaného v prvn´ım kroku a jeho


16

kovarianˇcn´ı matice chyby pomoc´ı informace z´ıskané z mˇeˇren´ı. Tento krok vyjadˇruj´ı rovnice K (k) = P(k|k − 1)H T (HP(k|k − 1)H T + R)−1 ,

(2.15)

ˆ x (k|k) = ˆ x (k|k − 1) + K (k)(y (k) − Hˆ x (k|k − 1)), P(k|k) = (I − K (k)HP(k|k − 1)), kde I je jednotková matice, K (k) je P × M -rozmˇerná matice vyjadˇruj´ıc´ı tzv. Kalmanovo zes´ılen´ı. Tento ˇclen minimalizuje kovarianˇcn´ı matici chyby apostreiorn´ıho odhadu a má funkci váhy“, která urˇcuje, do jaké m´ıry informace z´ıskaná mˇeˇren´ım ovlivn´ı aposteriorn´ı ” odhad stavu systému. Rozd´ıl y (k) − Hˆ x (k|k − 1) se naz´ yvá inovace mˇeˇren´ı a vyjadˇruje neshodu mezi predikovanou hodnotou a aktuáln´ım mˇeˇren´ım. Pokud se pod´ıváme na rovnici (2.15), vid´ıme, ˇze pokud se bude kovarianˇcn´ı matice ˇsumu mˇeˇren´ı R bl´ıˇzit k nule, potom Kalmanovo zes´ılen´ı K (k) váhuje inovaci mˇeˇren´ı vyˇsˇs´ı mˇerou, konkrétnˇe lim K (k) = H −1 .

R→0

Pokud se vˇsak k nule bude bl´ıˇzit kovarianˇcn´ı matice chyby apriorn´ıho odhadu P(k|k − 1), potom Kalmanovo zes´ılen´ı K (k) váhuje inovaci mˇeˇren´ı menˇs´ı mˇerou, konkrétnˇe lim

P(k|k−1)→0

K (k) = 0.

Pro snaˇzˇs´ı pochopen´ı si m˚ uˇzeme ˇr´ıci, ˇze pokud se bude kovarianˇcn´ı matice ˇsumu mˇeˇren´ı R bl´ıˇzit k nule, budeme aktuáln´ımu mˇeˇren´ı vˇeˇrit v´ıce, zat´ımco predikovan´ ym hodnotám mˇeˇren´ı budeme vˇeˇrit ménˇe. Na druhou stranu pokud se k nule bude bl´ıˇzit kovarianˇcn´ı matice chyby apriorn´ıho odhadu P(k|k − 1), aktuáln´ımu mˇeˇren´ı budeme vˇeˇrit ménˇe, zat´ımco predikovan´ ym hodnotám mˇeˇren´ı budeme vˇeˇrit v´ıce.

Pro shrnut´ı si m˚ uˇzeme ˇr´ıc´ı, ˇze algoritmus Kalmanova filtru pracuje jako zpˇetnovazebn´ı systém tak, ˇze v urˇcitém ˇcase odhadne stav procesu a poté obdrˇz´ı zpˇetnovazebn´ı informaci ve formˇe (zaˇsumˇeného) mˇeˇren´ı, dle které uprav´ı odhad stavu. Pro uveden´ı algoritmu do chodu je vˇsak nutné zadat poˇca´teˇcn´ı odhad stavu ˆ x (k|k − 1) a kovarianˇcn´ı matici chyby apriorn´ıho odhadu stavu P(k|k − 1). Schéma algoritmu Kalmanova filtru m˚ uˇzeme vidˇet na obrázku 2.6.

ˇ Y ´ KALMANUV ˚ FILTR 2.4. ROZSˇÍREN

17

Obr´ azek 2.6: Algoritmus Kalmanova filtru

2.4

Rozˇ s´ıˇ ren´ y Kalman˚ uv filtr

Ne vˇzdy vˇsak m˚ uˇzeme dynamick´ y systém popsat lineárn´ımi rovnicemi (2.13) a (2.14). ˇ Casto je dynamika systému popsána rovnicemi nelineárn´ımi, které m˚ uˇzeme obecnˇe zapsat jako x (k + 1) = f (x (k)) + w (k),

(2.16)

y (k) = h(x (k)) + n(k),

(2.17)

kde f (x (k)) je nelineárn´ı stavová funkce a h(x (k)) je nelineárn´ı v´ ystupn´ı funkce.

Algoritmus Kalmanova filtru popsan´ y v sekci 2.3 je moˇzné rozˇs´ıˇrit i na nelineárn´ı systémy popsané rovnicemi (2.16) a (2.17). Predikˇcn´ı krok algoritmu rozˇs´ıˇreného Kalmanova filtru potom bude vypadat následovnˇe. ˆ x (k + 1|k) = f (ˆ x (k)) A(k) =

∂f (x ) ∂x x =ˆx (k|k)

P(k + 1|k) = A(k)P(k|k)AT (k) + Q

(2.18) (2.19) (2.20)

Zde je vidˇet, ˇze predikce stavu se poˇc´ıtá pomoc´ı nelineárn´ıch rovnic, zat´ımco pro v´ ypoˇcet kovarianˇcn´ı matice chyby P se vyuˇzije matice A vypoˇctené jako jakobián funkce f (x ),


18

Obr´ azek 2.7: Algoritmus rozˇs´ıˇreného Kalmanova filtru

která je tak linearizována v okol´ı aktuáln´ıho pracovn´ıho bodu. Podobnˇe potom vypadaj´ı rovnice kroku aktualizace. H (k) =

∂h(x ) ∂x x =ˆx (k|k−1)

K (k) = P(k|k − 1)H T (k)(H (k)P(k|k − 1)H T (k) + R)−1

(2.21) (2.22)

ˆ x (k|k) = ˆ x (k|k − 1) + K (k)(y (k) − h(ˆ x (k|k − 1)))

(2.23)

P(k|k) = P(k|k − 1) − K (k)(H (k)P(k|k − 1)H T (k) + R)K T (k)

(2.24)

Matice H (k) je zde jakobián funkce h(x ), která je tak opˇet linearizována kolem aktuáln´ıho pracovn´ıho bodu. Schéma práce algoritmu rozˇs´ıˇreného Kalmanova filtru m˚ uˇzeme vidˇet na obrázku 2.7.

Podrobnˇejˇs´ı v´ yklad t´ ykaj´ıc´ı se Kalmanova filtru a jeho modifikac´ı je moˇzno nalézt ˇ v literatuˇre (Havlena, V. a Stecha, J., 2000).

Kapitola 3 Sbˇ er dat D˚ uleˇzitou kapitolou této práce jsou senzory zaloˇzené na nejr˚ uznˇejˇs´ıch fyzikáln´ıch principech a jevech, které poskytuj´ı d˚ uleˇzitá letová data, která m˚ uˇzeme dále zpracovávat. Pro u ´ˇcel odhadu orientace v prostoru budou v této práci pouˇzity zejména tˇri druhy senzor˚ u, a to gyroskopy, akcelerometry (tyto dva spadaj´ı do skupiny senzor˚ u inerciáln´ı navigace) a magnetometry. S kaˇzd´ ym z tˇechto senzor˚ u se dále seznám´ıme bl´ıˇze, abychom si pˇribl´ıˇzili principy, na jejichˇz základˇe tyto senzory mˇeˇr´ı, a t´ım pádem byli schopni jejich v´ ystupn´ı data co nejefektivnˇeji vyuˇz´ıt.

3.1

Senzory inerci´ aln´ı navigace

Inerciáln´ı navigace je proces, pˇri kterém se pomoc´ı inerciáln´ıch senzor˚ u, jako jsou gyroskopy a akcelerometry, provád´ı mˇeˇren´ı, jehoˇz hodnoty slouˇz´ı pro v´ ypoˇcet postupného pohybu objektu.

Inerciáln´ı navigaˇcn´ı systém se skládá z mˇeˇr´ıc´ı jednotky, která obsahuje akcelerometry a gyroskopy, a z navigaˇcn´ıho poˇc´ıtaˇce, kter´ y vyhodnocuje data z mˇeˇr´ıc´ıch zaˇr´ızen´ı a urˇcuje aktuáln´ı polohu objektu. Toto uspoˇrádán´ı má potom velkou v´ yhodu v sobˇestaˇcnosti, jelikoˇz pro svou ˇcinnost nepotˇrebuje ˇzádná vnˇejˇs´ı zaˇr´ızen´ı nebo objekty. Dalˇs´ı v´ yhodou je odolnost v˚ uˇci vnˇejˇs´ım vliv˚ um, jako je poˇcas´ı, magnetické poruchy atd. 19

ˇ DAT KAPITOLA 3. SBER

20

Základn´ı myˇslenka tohoto uspoˇra´dán´ı je zaloˇzena na Newtonovˇe pohybovém zákonu (Feynman, R. P. et al., 2000), kter´ y ˇr´ıká, ˇze vyprodukovaná s´ıla je pˇr´ımo u ´mˇerná zrychlen´ı tˇelˇesa. Dle této souvislosti m˚ uˇzeme vypoˇc´ıtat zmˇenu rychlosti a t´ım i pozici v ˇcase. Tuto informaci potom doplˇ nuj´ı gyroskopy, které ˇr´ıkaj´ı, jak je ve chv´ıli p˚ usoben´ı zrychlen´ı tˇelˇeso orientováno.

V této práci vˇsak nebudou tyto senzory pouˇzity pˇr´ımo k navigaci, ale data z tˇechto senzor˚ u se budou pouˇz´ıvat separátnˇe za u ´ˇcelem co nejlepˇs´ıho odhadu orientace objektu v prostoru nad zem´ı.

3.1.1

Gyroskopy

Gyroskopy jsou zaˇr´ızen´ı vyuˇz´ıvaná ke stanoven´ı u ´hlové rychlosti otáˇcen´ı objektu nebo ke stanoven´ı jeho natoˇcen´ı. Tˇechto informac´ı se vyuˇz´ıvá zejména v navigaci, a to nejen napˇr´ıklad letadel a lod´ı, ale i balistick´ ych raket a torpéd.

Dle fyzikáln´ıho principu, na kterém jsou zaloˇzeny, m˚ uˇzeme gyroskopy rozdˇelit do pˇeti základn´ıch skupin (Lachnit, Z., 2007): mechanické, optické, elektrické, jaderné, kvantové.

3.1.1.1

Mechanick´ e gyroskopy

Mechanick´ y gyroskop je v podstatˇe tˇeˇzk´ y setrvaˇcn´ık ve tvaru prstence nebo kotouˇce, kter´ y rotuje kolem osy na nˇej kolmé. Rotuj´ıc´ı setrvaˇcn´ık má moment hybnosti, kter´ y je dán

´ Í NAVIGACE 3.1. SENZORY INERCIALN

21

vztahem MH = Jω, kde J je moment setrvaˇcnosti tohoto setrvaˇcn´ıku k ose rotace a ω je u ´hlová rychlost rotace k této ose. D´ıky nenulovému momentu hybnosti udrˇzuje osa rotuj´ıc´ıho setrvaˇcn´ıku stále stejn´ y smˇer, pokud nen´ı ovlivˇ nován vnˇejˇs´ımi silami. Tohoto efektu se potom vyuˇz´ıvá k urˇcován´ı orientace objekt˚ u v prostoru - gyroskop si stále uchovává v´ ychoz´ı orientaci, kdeˇzto objekt pohybuj´ıc´ı se v prostoru se v˚ uˇci gyroskopu otáˇc´ı. Aby byl setrvaˇcn´ık ovlivˇ nován vnˇejˇs´ımi silami co moˇzná nejménˇe, umist’uje se do tzv. Cardanova závˇesu, kter´ y má tˇri stupnˇe volnosti.

Dalˇs´ı zaj´ımavou vlastnost´ı mechnického gyroskopu je to, ˇze pokud na rotuj´ıc´ı setrvaˇcn´ık p˚ usob´ı vnˇejˇs´ı s´ıly, které vyvolaj´ı otáˇcen´ı kolem jiné osy, neˇz je osa rotace setrvaˇcn´ıku, dojde k precesn´ımu pohybu, pˇri nˇemˇz se osa rotace setrvaˇcn´ıku bude snaˇzit po co nejkratˇs´ı dráze sesouhlasit s osou rotace vyvolané vnˇejˇs´ımi silami. Takto se dá vhodnˇe uloˇzen´ y gyroskop pouˇz´ıt jako kompas, kdy vnˇeˇs´ı p˚ usob´ıc´ı silou je rotace Zemˇe. Osa rotace gyroskopu se po urˇcité dobˇe sesouhlas´ı s osou rotace Zemˇe a osa gyroskopu tedy ukazuje na sever.

3.1.1.2

Optick´ e gyroskopy

Optické gyroskopy jsou zaloˇzeny na principu Sagnacova jevu, coˇz znamená, ˇze pˇri rotaci kruhového vlnovodu u ´hlovou rychlost´ı Ω, ve kterém proti sobˇe ob´ıhaj´ı dva svˇetelné svazky, je obvodová rychlost svazku ob´ıhaj´ıc´ıho ve smˇeru rotace vlnovodu zvyˇsována o hodnotu v = Ω · R, kde R je polomˇer rotace vlnovodu, zat´ımco obvodová rychlost svazku ob´ıhaj´ıc´ıho proti smˇeru rotace vlnovodu je o stejnou hodnotu sniˇzována.

Optické gyroskopy lze rozdˇelit na dva druhy: laserov´ e, kde jsou svˇetelné svazky vedeny pomoc´ı soustavy zrcadel k detektoru, vl´ aknov´ e, kde jsou svˇetelné svazky vedeny k detektoru pomoc´ı optického vlnovodu.


22

Obr´ azek 3.1: Uspoˇra´d´ an´ı sn´ımaˇce MEMS gyroskopu

3.1.1.3

Elektrick´ e gyroskopy

Elektrické gyroskopy se vyrábˇej´ı v proveden´ı MEMS1 a mimo samotného sn´ımaˇce obsahuj´ı i spoustu vyhodnocovac´ıch a ˇr´ıdic´ıch obvod˚ u a obvod˚ u logiky.

Gyroskopy vyrábˇené jako integrované obvody MEMS pracuj´ı na principu Coriolisovy s´ıly, coˇz je takzvaná virtuáln´ı s´ıla, která p˚ usob´ı na kaˇzd´ y hmotn´ y objekt, kter´ y se pohybuje rychlost´ı v v soustavˇe, která rotuje kolem své osy rotace u ´hlovou rychlost´ı ω. Tato s´ıla tedy p˚ usob´ı na kaˇzd´ y hmotn´ y objekt na Zemi. Coriolisovu s´ılu popisuje následuj´ıc´ı vztah. FC = m · v × ω, kde m je hmotnost pohybuj´ıc´ıho se objektu, v je vektor rychlosti pohybuj´ıc´ıho se objektu a ω je vektor u ´hlové rychlosti rotace soustavy.

Uspoˇra´dán´ı sn´ımaˇce MEMS gyroskopu je vidˇet na obrázku 3.1. Základ tvoˇr´ı rezonuj´ıc´ı struktura upevnˇená v rámu, která se vlivem vlastn´ı mechanické rezonance, která 1

Micro-Electro-Mechanical Systems

´ Í NAVIGACE 3.1. SENZORY INERCIALN

23

je zde reprezentována pruˇzinami, pohybuje ve smˇeru kolmém na smˇer rotace. Pˇri tomto pohybu vzniká Coriolisova s´ıla, jej´ıˇz velikost je u ´mˇerná u ´hlové rychlosti rotace. Tato s´ıla p˚ usob´ı na vnˇejˇs´ı pruˇziny, ˇc´ımˇz se mˇen´ı vzájemná poloha mˇeˇr´ıc´ıch ploˇsek, které mohou m´ıt napˇr´ıklad funkci elektrod vzduchov´ ych kondenzátor˚ u.

3.1.1.4

Jadern´ e gyroskopy

Jaderné gyroskopy (Lachnit, Z., 2007) vyuˇz´ıvaj´ı principu jaderného paramagnetismu látek, jako je voda, organické roztoky, helium, pára rtuti atd. Atomy nebo molekuly tˇechto látek maj´ı v základn´ım stavu magnetick´ y moment dan´ y spiny jader (vlastn´ı moment hybnosti). Pokud orientujeme magnetické momenty jader magnetick´ ym polem, a potom pole zruˇs´ıme, pak pokud nep˚ usob´ı jiné magnetické pole, zachová si v´ ysledn´ y magnetick´ y moment po jistou dobu svou prostorovou orientaci, a to nezávisle na zmˇenách orientace objektu obsahuj´ıc´ı tuto látku. Nicménˇe hodnota v´ ysledného magnetického pole bude v d˚ usledku relaxace postupnˇe klesat. Proto se pro tyto druhy gyroskop˚ u vol´ı látky s velk´ ymi relaxaˇcn´ımi ˇcasy. 3.1.1.5

Kvantov´ e gyroskopy

Kvantov´ y gyroskop (Lachnit, Z., 2007) je svou funkc´ı podobn´ y mechanickému gyroskopu, jen nevyuˇz´ıvá vlastnost´ı setrvaˇcné hmoty, ale vlastnost´ı atomov´ ych jader.

3.1.2

Akcelerometry

Akcelerometry jsou v dneˇsn´ı dobˇe velmi rozˇs´ıˇrené a velmi pouˇz´ıvané sn´ımaˇce. Primárn´ı veliˇcinou, kterou sn´ımaj´ı, je s´ıla, která je vyvozena pˇri zrychlen´ı tˇelesa. Akcelerometry jsou schopny mˇeˇrit zrychlen´ı jak statické, coˇz je napˇr´ıklad t´ıhové zrychlen´ı pˇr´ıtomné nad povrchem Zemˇe, tak dynamické, které je dáno skuteˇcn´ ym zrychlen´ım objektu.

Hodnota zrychlen´ı se bˇeˇznˇe udává v jednotkách g, coˇz udává násobek t´ıhového zrychlen´ı na Zemi. T´ıhové zrychlen´ı na Zemi je v´ ysledkem sloˇzen´ı gravitaˇcn´ıho zrychlen´ı


24

a odstˇredivého zrychlen´ı, které vzniká v d˚ usledku rotace Zemˇe. Jednotkou t´ıhového zrychlen´ı je m · s−2 . Zde jeˇstˇe stoj´ı za zm´ınku, ˇze hodnota gravitaˇcn´ıho a odstˇredivého zrychlen´ı v bl´ızkosti Zemˇe nen´ı vˇzdy stejná, ale závis´ı i na vzdálenosti od stˇredu Zemˇe. M´ıstn´ı t´ıhové zrychlen´ı dále závis´ı na zemˇepisné ˇs´ıˇrce a nadmoˇrské v´ yˇsce. Za pˇredpokladu, ˇze je v´ yˇska vzhledem k zemskému pr˚ umˇeru malá, se na jeden metr v´ yˇsky sniˇzuje g o 3 · 10−6 m · s−2 . Dohodnutá stˇredn´ı hodnota t´ıhového zrychlen´ı, tzv. normáln´ı t´ıhové zrychlen´ı, je g = 9, 80665 m · s−2 . Senzory mohou mˇeˇrit od velmi n´ızk´ ych hodnot g, ale dokáˇz´ı nárazovˇe vydrˇzet i hodnoty v okol´ı tis´ıcinásobku g.

Akcelerometry jsou v dneˇsn´ı dobˇe rozˇs´ıˇreny ve velkém mnoˇzstv´ı nejr˚ uznˇejˇs´ıch oblast´ı: navigace, elektronické libely, detekce p´ adu, detekce n´ arazu, stabilizace obrazu, monitorov´ an´ı seismické aktivity, rehabilitaˇcn´ı pˇr´ıstroje, pedometry, zaˇr´ızen´ı pro sportovn´ı lékaˇrstv´ı a mnoho dalˇs´ıch.

Pro kaˇzdou oblast vyuˇzit´ı je pak vhodn´ y jin´ y typ akcelerometru. Akcelerometry lze rozdˇelit podle nˇekolika hledisek (Lachnit, Z., 2007). M˚ uˇze to b´ yt napˇr´ıklad podle poˇctu os, ve kter´ ych je akcelerometr schopen mˇeˇrit, a to na akcelerometry jednoosé, dvouosé a tˇr´ıosé. Dále m˚ uˇzeme akcelerometry dˇelit na akcelerometry se seismickou hmotou a MEMS akcelerometry s promˇennou kapacitou. Z akcelerometr˚ u se seismickou hmotou stoj´ı v dneˇsn´ı dobˇe za zm´ınku piezoelektrické akcelerometry, které vyuˇz´ıvaj´ı piezoelektrick´ y krystal generuj´ıc´ı náboj u ´mˇern´ y s´ıle (zrychlen´ı) p˚ usob´ıc´ı na objekt, a piezo-

3.2. MAGNETOMETRY

25

rezistivn´ı akcelerometry, které vyuˇz´ıvaj´ı mikrokˇrem´ıkovou mechanickou strukturu, kde zrychlen´ı odpov´ıdá zmˇenˇe odporu.

3.2

Magnetometry

Název tohoto senzoru je odvozen od vˇedn´ı discipl´ıny, která se jmenuje magnetometrie (EnviWeb s.r.o., 2008). Magnetometrie se zab´ yvá mˇeˇren´ım indukce magnetického pole, která se mˇeˇr´ı bud’ v jednotkách Tesla (znaˇceno T), nebo Gauss (znaˇceno G) a plat´ı 1 T = 10 000 G. Magnetická indukce je vektorová veliˇcina a má tedy velikost a smˇer.

Magnetometry maj´ı velice ˇsiroké moˇznosti vyuˇzit´ı: kompas (buzola), navigaˇcn´ı systémy, labolatorn´ı n´ astroje, medic´ınské n´ astroje, geologické pr˚ uzkumy, archeologické pr˚ uzkumy, potravin´ aˇrsk´ y pr˚ umysl, zemˇedˇelsk´ y pr˚ umysl a mnoho dalˇs´ıch.

V kaˇzdé oblasti se vˇsak magnetické pole m˚ uˇze vyuˇz´ıvat r˚ uznˇe. V nˇekter´ ych pˇr´ıpadech staˇc´ı pouze informace o jeho pˇr´ıtomnosti, jinde zase m˚ uˇzeme potˇrebovat informaci o intenzitˇe, smˇeru ˇci zmˇenˇe tohoto pole. Senzor˚ u, které nám tyto informace mohou poskytnout, exisˇmec, M., 2005). tuje celá ˇrada (Ne


26

Obr´ azek 3.2: Dip´ olov´ y

charakter

magnetického

pole

Zemˇe

(zdroj:

www.cez.cz)

V této práci se budeme zab´ yvat v´ yhradnˇe magnetick´ ym polem Zemˇe, jelikoˇz nám jde o urˇcován´ı orientace objektu v prostoru nad jej´ım povrchem.

Magnetické pole Zemˇe s nejvˇetˇs´ı pravdˇepodobnost´ı vzniká pohyby v jej´ım tekutém jádru a mˇen´ı se v ˇcase i v prostoru. Rozloˇzen´ı celkové intenzity magnetického pole Zemˇe je potom vidˇet na obrázku 3.3. Z tohoto obrázku je také vidˇet, ˇze pro Stˇredn´ı Evropu nab´ yvá tato hodnota velikosti pˇribliˇznˇe 45000 nT. Tato hodnota vˇsak závis´ı i na aktivitách jádra Zemˇe, na aktivitˇe Slunce a na magnetick´ ych bouˇr´ıch v ionosféˇre, dále na materiálu zemské k˚ ury a podobnˇe.

Magnetické pole Zemˇe má dipólov´ y charakter, jak je vidˇet na obrázku 3.2. Severn´ı magnetick´ y pól dnes leˇz´ı v severn´ıch oblastech Kanady a jiˇzn´ı magnetick´ y pól se nacház´ı v Antarktidˇe. Zde je zˇreba si uvˇedomit, ˇze poloha magnetick´ ych a zemˇepisn´ ych pól˚ u se liˇs´ı, a tedy napˇr´ıklad stˇrelka kompasu neukazuje pˇresnˇe na zemˇepisn´ y sever, ale je m´ırnˇe ´ vych´ ylena. Uhel mezi smˇerem na sever (poledn´ık) a smˇerem, kam ukazuje kompas, se naz´ yvá magnetická deklinace. Tento u ´hel je na r˚ uzn´ ych m´ıstech Zemˇe r˚ uznˇe velk´ y, coˇz je ˇ znázornˇeno na obrázku 3.4. V dneˇsn´ı dobˇe je hodnota magnetické deklinace v Cesku asi 2 stupnˇe a 53 minut na v´ ychod a posouvá se kaˇzd´ y rok asi o 6 minut v´ ychodn´ım smˇerem. Vektor intenzity magnetického pole Zemˇe se vˇsak neprojevuje pouze v horizontáln´ı rovinˇe, ale má svou sloˇzku i v rovinˇe veritkán´ı. Sklon tohoto vektoru od horizontáln´ı roviny se ˇ ´, J., naz´ yvá magnetická inklinace a v Cesku má hodnotu asi 65 stupˇ n˚ u a 7 minut (Perny 2006). Jelikoˇz je tento sklon smˇeˇrován k Zemi, tedy smˇerem dol˚ u, je inklinace kladná.

3.2. MAGNETOMETRY

Obr´ azek 3.3: Celková intenzita mag. pole na Zemi v nT (zdroj: www.geophysics.ou.edu)

Obr´ azek 3.4: Mag. deklinace na Zemi ve stupn´ıch z roku 1860 (zdroj: www.wikipedia.org)

27


28

3.3

Chyby senzor˚ u

I pˇres velké pokroky v technologi´ıch v´ yroby senzor˚ u je stále tˇreba brát v u ´vahu jejich nedokonalost. Tato nedokonalost potom zp˚ usobuje chyby v jejich mˇeˇren´ı, coˇz m˚ uˇze b´ yt zp˚ usobeno technologi´ı v´ yroby, ale i samotn´ ym fyzikáln´ım principem, na kterém je dan´ y senzor zaloˇzen.

Chyby senzor˚ u m˚ uˇzeme dˇelit na tˇri základn´ı skupiny podle toho, jak se vyv´ıjej´ı v ˇcase. Prvn´ı skupinou jsou chyby, které se v ˇcase nijak nevyv´ıjej´ı a maj´ı tedy stejné hodnoty v pr˚ ubˇehu celého mˇeˇren´ı. Druhou skupinou jsou chyby, které se v ˇcase mˇen´ı, ale kolem správné hodnoty bud’ osciluj´ı, nebo se od n´ı odchyluj´ı pouze na omezenou dobu. Posledn´ı skupinou jsou chyby, které se v ˇcase vyv´ıjej´ı, avˇsak maj´ı sp´ıˇse kumulativn´ı charakter. To znamená, ˇze chyba v ˇcase stále nar˚ ustá a je tedy potˇreba s t´ım poˇc´ıtat.

Dle své povahy m˚ uˇzeme základn´ı chyby dále dˇelit na následuj´ıc´ı: Bias. Tato chyba m´ıv´ a ˇcasto dvˇe sloˇzky. Prvn´ı z nich se v ˇcase nevyv´ıj´ı a jedná se

tedy o nenulovou hodnotu v´ ystupu pˇri nulovém vstupu senzoru po zapnut´ı senzoru. Tato sloˇzka se v pr˚ ubˇehu mˇeˇren´ı nemˇen´ı, avˇsak m˚ uˇze nab´ yvat rozd´ıln´ ych hodnot pˇri kaˇzdém zapnut´ı senzoru. Druhou sloˇzkou této chyby je sloˇzka ˇcasovˇe promˇenná, která m˚ uˇze m´ıt i kumulativn´ı charakter a je zp˚ usobena napˇr´ıklad zmˇenami teploty nebo fyzikáln´ım principem, na kterém je seznor zaloˇzen. Tento druh chyby se v r˚ uzné m´ıˇre vyskytuje u vˇsech senzor˚ u. Scale factor error. Tato chyba vyjadˇruje nest´ alost pomˇeru zmˇeny v´ ystupn´ıho

signálu ke zmˇenˇe vstupn´ıho signálu senzoru. Tato chyba se m˚ uˇze bˇehem mˇeˇren´ı m´ırnˇe mˇenit, ale m´ıvá také konstantn´ı sloˇzku, která m˚ uˇze b´ yt zp˚ usobena napˇr´ıklad stárnut´ım senzoru nebo tolerancemi pˇri jeho v´ yrobˇe. Misalignment error. Tato chyba je zp˚ usobena nepˇresn´ ym um´ıstˇen´ım senzor˚ u.

Pokud chceme mˇeˇrit veliˇcinu ve v´ıce osách objektu, ˇcasto nestaˇc´ı jedin´ y senzor, ale potˇrebujeme jich v´ıce na sebe kolm´ ych. Ne vˇzdy se vˇsak pˇri v´ yrobˇe podaˇr´ı

˚ 3.3. CHYBY SENZORU

29

tyto senzory um´ıstit pˇresnˇe kolmo na sebe, nav´ıc se ˇcasto nepodaˇr´ı tuto soustavu senzor˚ u um´ıstit tak, aby osy mˇeˇren´ı tˇechto senzor˚ u souhlasily s osami soustavy, ve které chceme mˇeˇrit. Toto se potom projev´ı tak, ˇze se do jednotliv´ ych os mˇeˇren´ı senzor˚ u v r˚ uzné m´ıˇre prom´ıtnou i sloˇzky z os ostatn´ıch, které by tyto jednotlivé senzory teoreticky v˚ ubec nemˇely mˇeˇrit, a naopak sloˇzky, které by tˇemito senzory mˇely b´ yt mˇeˇreny, jsou t´ımto zeslabeny. Citlivost senzor˚ u v jednotliv´ ych osách potom tedy nen´ı ideáln´ı. ˇ Sum. Tato chyba vyjadˇruje zat´ıˇzen´ı v´ ystupn´ıho signálu senzoru ruˇsiv´ ym signálem. To se potom projev´ı oscilac´ı v´ ystupn´ı hodnoty senzoru kolem hodnoty skuteˇcné. Tento druh chyby m˚ uˇze m´ıt celou ˇskálu r˚ uzn´ ych pˇr´ıˇcin. M˚ uˇze b´ yt napˇr´ıklad zp˚ usoben elektronick´ ymi systémy samotného senzoru, ale i okoln´ı elektronikou. Dle fyzikáln´ıho principu, na kterém jsou jednotlivé senzory zaloˇzeny, mohou b´ yt pˇr´ıˇcinou této chyby i r˚ uzné vnˇejˇs´ı vlivy, napˇr´ıklad ruˇsivé magnetické pole v pˇr´ıpadˇe mˇeˇren´ı magnetometry.

Pokud chceme u ´daje z´ıskané senzory co nejefektivnˇeji vyuˇz´ıt, je tˇreba s chybami v jimi namˇeˇren´ ych hodnotách poˇc´ıtat. Rovnici mˇeˇren´ı, která respektuje jednotlivé chyby uvedené v´ yˇse, m˚ uˇzeme zapsat jako f m = Sff + M ff + Bf + nf,

(3.1)

kde f m je vektor hodnot mˇeˇrené veliˇciny zmˇeˇren´ ych senzory v jednotliv´ ych osách, f je vektor jejich skuteˇcn´ ych hodnot, S f je diagonáln´ı matice (m˚ uˇze to vˇsak b´ yt i polynomiáln´ı funkce mˇeˇrené veliˇciny) vyjadˇruj´ıc´ı scale factor error, M f je matice vyjadˇruj´ıc´ı misalignment error, B f je vektor vyjadˇruj´ıc´ı bias senzoru a n f je vektor ˇsumu senzoru.

30


Kapitola 4 Integrace dat V této sekci se pod´ıváme na to, jak data z´ıskaná ze senzor˚ u co nejefektivnˇeji vyuˇz´ıt. Integrace dat se m˚ uˇze provádˇet hned za nˇekolika r˚ uzn´ ymi u ´ˇcely.

Jedn´ım z nich je napˇr´ıklad pˇr´ıpad, kde máme nˇekolik r˚ uzn´ ych senzor˚ u, jejichˇz informace se navzájem doplˇ nuj´ı, a my jsme potom schopni z tˇechto dat z´ıskat informace nové. Pˇr´ıkladem m˚ uˇze b´ yt palubn´ı poˇc´ıtaˇc automobilu, kter´ y je schopen nám napˇr´ıklad vypoˇc´ıtat pr˚ umˇernou spotˇrebu paliva na základˇe informac´ı o ujeté vzdálenosti a mnoˇzstv´ı spotˇrebovaného paliva.

Dalˇs´ım pˇr´ıpadem je situace, kdy máme k dispozici nˇekolik totoˇzn´ ych senzor˚ u, d´ıky ˇcemuˇz jsme schopni z´ıkat informaci, kterou bychom pomoc´ı jediného senzoru z´ıskat nemohli. Pˇr´ıkladem mohou b´ yt dvˇe kamery um´ıstˇené vedle sebe zamˇeˇruj´ıc´ı stejn´ y bod. Kombinac´ı takto poˇr´ızen´ ych dvourozmˇern´ ych obrázk˚ u jsme potom schopni vypoˇc´ıtat vzdálenost zamˇeˇreného objektu a tedy dvourozmˇernou informaci z jednotliv´ ych kamer doplnit o tˇret´ı rozmˇer, tedy hloubku.

Pˇr´ıpadem, kter´ ym se budeme zab´ yvat v této práci, je situace, kde máme nˇekolik r˚ uzn´ ych senzor˚ u zaloˇzen´ ych na rozd´ıln´ ych fyzikáln´ıch principech a jevech, avˇsak vzhledem k veliˇcinˇe, kterou chceme mˇeˇrit, jsou informace z nich do jisté m´ıry redundatn´ı. Pokud by vˇsak tyto senzory mˇeˇrily dokonale pˇresnˇe a jejich v´ ystup by nebyl nijak zat´ıˇzen 31

KAPITOLA 4. INTEGRACE DAT

32

chybami, byla by tato redundance zbyteˇcná a k urˇcen´ı mˇeˇrené veliˇciny by staˇcil jedin´ y senzor, kter´ y by byl schopen tuto veliˇcinu mˇeˇrit. V dneˇsn´ı dobˇe vˇsak senzory dokonalé rozhodnˇe nejsou, coˇz jeˇstˇe m˚ uˇze b´ yt umocnˇeno pouˇzit´ım levn´ ych senzor˚ u, které maj´ı chyby obecnˇe v´ yznamnˇejˇs´ı.

V této práci budeme k urˇcen´ı orientace objektu v prostoru uvaˇzovat tˇri druhy senzor˚ u, jejichˇz funkce a principy byly nast´ınˇeny v kapitole 3. Prvn´ım z nich jsou gyroskopy, které mˇeˇr´ı u ´hlovou rychlost otáˇcen´ı tohoto objektu. Jelikoˇz v´ıme, ˇze vztah mezi u ´hlovou rychlost´ı otáˇcen´ı a zmˇenou polohového u ´hlu je obecnˇe dán vztahem ω=

dα = α, ˙ dt

(4.1)

kde ω je vektor u ´hlové rychlosti otáˇcen´ı a α je polohov´ yu ´hel, bude informace o u ´hlov´ ych rychlostech otáˇcen´ı kolem jednotliv´ ych tˇelesov´ ych os velmi uˇziteˇcná.

Dalˇs´ımi senzory, jejichˇz informace vyuˇzijeme, jsou akcelerometry. V tomto pˇr´ıpadˇe nám vˇsak bude uˇziteˇcná pouze sloˇzka gravitaˇcn´ıho zrychlen´ı, kterou akcelerometry mˇeˇr´ı jako statické zrychlen´ı, a akcelerometry tak vyuˇzijeme jako libelu. Je vˇsak tˇreba si uvˇedomit, ˇze zde také existuj´ı sloˇzky zrychlen´ı zp˚ usobené zmˇenou rychlosti pohybu objektu, které jsou pro nás potom ˇskodlivou informac´ı, a to nesm´ı b´ yt opom´ıjeno.

Posledn´ımi senzory jsou magnetometry, které budou vyuˇzity podobnˇe jako akcelerometry s t´ım rozd´ılem, ˇze vektor intenzity magnetického pole Zemˇe, kter´ y je magnetometry sn´ımán, má jin´ y smˇer a velikost neˇz vektor gravitaˇcn´ıho zrychlen´ı. V´ yhodou oproti akcelerometr˚ um je v tom, ˇze zde mˇeˇrená veliˇcina nen´ı negativnˇe ovlivˇ nována pˇr´ımo pohybem objektu. Nev´ yhoda vˇsak m˚ uˇze spoˇc´ıvat v moˇznosti zkreslen´ı vektrou intenzity magnetického pole Zemˇe r˚ uzn´ ymi elektronick´ ymi pˇr´ıstoji a dalˇs´ımi vnˇejˇs´ımi vlivy, jako jsou veden´ı vysokého napˇet´ı, magnetické horniny atd.

Máme tedy r˚ uzné senzory zat´ıˇzené r˚ uzn´ ymi chybami a naˇs´ım c´ılem je, aby tyto senzory své chyby vzájemnˇe potlaˇcovaly.

´ 4.1. GENERATOR DAT

4.1

33

Gener´ ator dat

Nutnou sloˇzkou systému efektivn´ı integrace dat v této práci je generátor, kter´ y zachycuje ˇcinnost jednotliv´ ych senzor˚ u popsan´ ych v sekc´ıch 3.1 a 3.2 ve vztahu k mˇeˇren´ı polohov´ ych u ´hl˚ u.

4.1.1

Vˇ etev gyroskop˚ u

Jak jiˇz bylo ˇreˇceno v sekci 3.1.1, gyroskopy jsou primárnˇe urˇceny k mˇeˇren´ı u ´hlové rychlosti rotace objektu, na kterém jsou um´ıstˇeny. Zde si tedy vytvoˇr´ıme model, kter´ y bude znázorˇ novat funkci tˇechto senzor˚ u v ideáln´ım pˇr´ıpadˇe, tedy zat´ım bez zat´ıˇzen´ı mˇeˇren´ı chybami. Tento model bude tedy vyjadˇrovat vztah mezi polohov´ ymi u ´hly a u ´hlovou rychlost´ı rotace objektu. Schéma tohoto modelu je znázornˇeno na obrázku 4.1.

Vztah mezi polohov´ ym u ´hlem a u ´hlovou rychlost´ı rotace je obecnˇe dán vztahem (4.1). Zde je vˇsak tˇreba si uvˇedomit, ˇze nen´ı moˇzné tento vztah pouˇz´ıt na jednotlivé osy separátnˇe, jelikoˇz zmˇeny polohov´ ych u ´hl˚ u jsou vzájemnˇe provázány. Napˇr´ıklad v pˇr´ıpadˇe nenulového u ´hlu klopen´ı Θ nezp˚ usob´ı otáˇcen´ı kolem tˇelesové osy z pouze zmˇenu u ´hlu natoˇcen´ı Ψ, ale zároveˇ n se budou mˇenit zbylé dva polohové u ´hly Φ a Θ. Nicménˇe i pˇres to, ˇze se v takovém pˇr´ıpadˇe mˇen´ı vˇsechny polohové u ´hly, gyroskopy zaznamenaj´ı u ´hlovou rotaci pouze kolem tˇelesové osy z. Tato skuteˇcnost je dána t´ım, ˇze gyroskopy mˇeˇr´ı u ´hlovou rychlost rotace pˇr´ımo v tˇelesové souˇradné soustavˇe, zat´ımco jednotlivé polohové u ´hly Φ, Θ a Ψ jsou urˇcovány jako natoˇcen´ı tˇelesové souˇradné soustavy v˚ uˇci soustavˇe navigaˇcn´ı. Zjiˇstˇen´ım derivac´ı jednotliv´ ych polohov´ ych u ´hl˚ u (bloky Discrete Derivative“ na obrázu ” 4.1) tedy nez´ıskáme informace pˇr´ımo o u ´hlov´ ych rychlostech rotac´ı p, q a r. Informaci o rychlostech zmˇen jednotliv´ ych u ´hl˚ u, které jsou mˇeˇreny v navigaˇcn´ı souˇradné soustavˇe, je tedy tˇreba transformovat na u ´daje poskytované gyroskopy, jeˇz mˇeˇr´ı u ´hlové rychlosti v tˇelesové souˇradné soustavˇe. Tuto transformaci provedeme dle vztahu ⎡ ⎤ ⎡ ⎤⎡ ⎤ p 1 0 − sin(Θ) Φ˙ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ˙ ⎥ = ⎢ q ⎥ ⎢ 0 cos(Φ) sin(Φ) cos(Θ) ⎥ ⎢ Θ ⎥ , ⎣ ⎦ ⎣ ⎦⎣ ⎦ ˙ r 0 − sin(Φ) cos(Φ) cos(Θ) Ψ


34

1 Phi

2 Theta

3 Psi

K (z−1) Ts z Discrete Derivative


MATLAB Function eul2pqr

1 p,q,r


Obr´ azek 4.1: Schéma mˇeˇren´ı ide´ aln´ımi gyroskpy

kter´ y je z´ıskán dosazen´ım do vztahu (2.3). Na obrázku 4.1 tuto transformaci zajiˇst’uje blok eul2pqr“ obsahuj´ıc´ı skript uveden´ y v pˇr´ıloze A.1. T´ım jsme z´ıskali model mˇeˇren´ı ” ideáln´ıch gyroskop˚ u.

4.1.2

Vˇ etev akcelerometr˚ u

Funkce akcelerometr˚ u jiˇz byla nast´ınˇena v sekci 3.1.2. Jak jiˇz bylo ˇreˇceno, akcelerometry jsou urˇceny k mˇeˇren´ı zrychlen´ı, a to jak statického, tak dynamického. Statické zrychlen´ı je dáno pˇr´ıtomnost´ı hmotn´ ych tˇeles. V naˇsem pˇr´ıpadˇe, jelikoˇz se zab´ yváme pohybem nad povrchem Zemˇe, je t´ımto tˇelesem právˇe Zemˇe. Dynamické zrychlen´ı je pak dáno dynamikou pohybu objektu, na nˇemˇz jsou akcelerometry um´ıstˇeny.

Protoˇze se tato práce zab´ yvá urˇcován´ım polohov´ ych u ´hl˚ u, vyuˇzijeme tˇr´ıos´ y akceleroˇ a ć ˇ, M., 2008). V takovém pˇr´ıpadˇe je pro nás uˇziteˇcná pouze metr jako sklonomˇer (Rez sloˇzka statického zrychlen´ı, jej´ıˇz vektor je moˇzné nad povrchem Zemˇe uvaˇzovat jako konstantn´ı. Zmˇeny závislé na nadmoˇrské v´ yˇsce a zemˇepisné ˇs´ıˇrce, které byly zm´ınˇeny v sekci


35

1 Phi [0, 0, −9.81] Gravity constants 2 Theta

MATLAB Function geo2bdy

1 ax,ay,az

3 Psi

Obr´ azek 4.2: Schéma mˇeˇren´ı ide´ aln´ımi akcelerometry ve funkci sklonomˇeru

3.1.2, je moˇzné zanedbat i proto, ˇze se zde vˇzdy budou vyskytovat i dalˇs´ı ruˇsivé sloˇzky, jejichˇz vliv je mnohem v´ yznamˇejˇs´ı. Nejv´ yznamˇejˇs´ı ruˇsivou sloˇzkou zde bude p˚ usoben´ı dynamické sloˇzky zrychlen´ı, dále to pak mohou b´ yt zrychlen´ı zp˚ usobená vibracemi pohonn´ ych jednotek atd.

Vektor t´ıhového zrychlen´ı p˚ usob´ı smˇeˇrem dol˚ u, coˇz je v naˇs´ı navigaˇcn´ı soustavˇe kladn´ y smˇer. Zde je vˇsak tˇreba si uvˇedomit, ˇze akcelerometr nemˇeˇr´ı pˇr´ımo t´ıhové zrychlen´ı, ale mˇeˇren´ı zrychlen´ı obejktu, na kterém je um´ıstˇen. A jelikoˇz tento objekt mus´ı t´ıhové zrychlen´ı kompenzovat, namˇeˇr´ı akcelerometr právˇe toto kompenzaˇcn´ı zrychlen´ı“ vyvozované ” objektem, které má opaˇcn´ y smˇer neˇz t´ıhové zrychlen´ı.

Zde tedy vytvoˇr´ıme model, kter´ y bude znázorˇ novat mˇeˇren´ı ideáln´ımi akcelerometry ve funkci sklonomˇeru. Schéma tohoto modelu je znázornˇeno na obrázku 4.2. Zde se vektor zrychlen´ı [0; 0; −9, 81], kter´ y má v navigaˇcn´ı soustavˇe konstantn´ı velikost i smˇer, transformuje na základˇe znalosti polohov´ ych u ´hl˚ u do soustavy tˇelesové, ve které mˇeˇr´ı soustava akcelerometr˚ u. Tuto transformaci m˚ uˇzeme provést bud’ pomoc´ı transformaˇcn´ı matice smˇerov´ ych kosin˚ u dle vztahu (2.1), kde se transformaˇcn´ı matice Cbn urˇc´ı dle vztahu (2.2), nebo pomoc´ı transformaˇcn´ı matice quaternion˚ u dle vztahu (2.7), kde se transformaˇcn´ı


36

matice C urˇc´ı dle vztahu (2.6). V této práci je zvolena transformace pomoc´ı quaternion˚ u. V souladu se vztahy (2.7) a (2.6) potom bude transformace vypadat dle následuj´ıc´ıho vztahu. ⎡ ⎤ ⎡ 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) a q 2 + q12 − q22 − q32 ⎢ x ⎥ ⎢ 0 ⎢ ⎥ ⎢ ⎢ ay ⎥ = ⎢ 2(q1 q2 − q0 q3 ) q02 − q12 + q22 − q32 2(q2 q3 + q0 q1 ) ⎣ ⎦ ⎣ 2(q1 q3 + q0 q2 ) az 2(q2 q3 − q0 q1 ) q02 − q12 − q22 + q32

⎤

⎤⎡ ⎥⎢ ⎥⎢ ⎥⎢ ⎦⎣

0 0 −9, 81

⎥ ⎥ ⎥ (4.2) ⎦

Zde se jednotlivé quaterniony z polohov´ ych u ´hl˚ u urˇc´ı dle vztah˚ u (2.8) aˇz (2.11). Tato transformace je ve schématu na obrázku 4.3 realizována pomoc´ı bloku geo2bdy“, kter´ y ” obsahuje skript uveden´ y v pˇr´ıloze A.2. Takto jsme z´ıskali model mˇeˇren´ı ideáln´ıch akcelerometr˚ u ve funkci sklonomˇeru.

4.1.3

Vˇ etev magnetometr˚ u

Jak jiˇz bylo ˇreˇceno v sekci 3.2, magnetometry jsou urˇceny k mˇeˇren´ı intenzity magnetického pole. Jejich funkce v této práci bude velmi podobná funkci akcelerometr˚ u, kde se tˇr´ıos´ y akcelerometr pouˇz´ıval jako sklonomˇer, neboli mˇeˇril intenzitu t´ıhového pole Zemˇe ve tˇrech na sebe kolm´ ych osách. Zde budou magnetometry pouˇzity stejnˇe, liˇsit se budou pouze vektorem, na kterém je toto mˇeˇren´ı zaloˇzeno. Zat´ımco vektor t´ıhového zrychlen´ı, kter´ y je mˇeˇren akcelerometry, je moˇzné nad cel´ ym povrchem Zˇemˇe uvaˇzovat jako konstantn´ı, vektor intenzity magnetického pole Zemˇe mˇen´ı sv˚ uj smˇer i velikost v závislosti na horizontáln´ı poloze nad Zem´ı, jak to ukazuj´ı obrázky 3.3 a 3.4. Z tohoto d˚ uvodu je nutné urˇcit pro kaˇzdou oblast, nad kterou se má operovat, tento vektor zvláˇst’. ˇ Pro Cesko se vektor intenzity magnetického pole Zemˇe urˇc´ı následovnˇe. Jak bylo ˇ ˇreˇceno v sekci 3.2, velikost tohoto vektoru je v Cesku pˇribliˇznˇe 45000 nT. Inklinace tohoto vektoru je zde asi 65 stupˇ n˚ u a 7 minut. Rozloˇzen´ı tohoto vektoru do horizontáln´ı sloˇzky Mh a vertikáln´ı sloˇzky Mv , která bude zároveˇ n sloˇzkou vektoru v ose z, se potom urˇc´ı jako Mh = 45000 cos(65◦ 7 ) = 18939, 49 nT, Mv = M z = 45000 sin(65◦ 7 ) = 40820, 29 nT.


37

1 Phi (Mx

My

Mz)

Magnetic constants 2 Theta

MATLAB Function geo2bdy

1 Hx,Hy,Hz

3 Psi

Obr´ azek 4.3: Schéma mˇeˇren´ı ide´ aln´ımi magnetometry

ˇ Horizontáln´ı sloˇzku je dále nutné vlivem deklinace, jej´ıˇz velikost je v Cesku nyn´ı asi 2 stupnˇe a 53 minut v´ ychodn´ım smˇerem, rozdˇelit na sloˇzku v ose x a sloˇzku v ose y.

Mx = Mh cos(2◦ 53 ) = 18915, 57 nT My = Mh sin(2◦ 53 ) = 951, 60 nT

V´ yhodou oproti mˇeˇren´ı akcelerometry je nezávislost na zrychlen´ı objektu, nicménˇe se zde ale mohou projevovat r˚ uzné vnˇejˇs´ı magnetické vlivy.

Opˇet vytvoˇr´ıme model, kter´ y bude znázorˇ novat mˇeˇren´ı ideáln´ımi magnetometry ve funkci sklonomˇeru. Schéma tohoto modelu je znázornˇeno na obrázku 4.3. Zde se vektor intenzity magnetického pole [Mx ; My ; Mz ], kter´ y je moˇzné v urˇcité oblasti uvaˇzovat jako konstantn´ı v navigaˇcn´ı soustavˇe, opˇet transformuje na základˇe znalosti polohov´ ych u ´hl˚ u do soustavy tˇelesové, ve které mˇeˇr´ı soustava magnetometr˚ u. Tuto transformaci m˚ uˇzeme stejnˇe jako v pˇr´ıpadˇe akcelerometr˚ u provést bud’ pomoc´ı transformaˇcn´ı matice smˇerov´ ych kosin˚ u, nebo pomoc´ı transformaˇcn´ı matice quaternion˚ u. Stále se vˇsak budeme drˇzet transformace pomoc´ı quaternion˚ u. V souladu se vztahy (2.7) a (2.6) potom bude transformace


38 vypadat dle vztahu ⎡

⎤

⎡

H ⎢ x ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Hy ⎥ = ⎢ ⎣ ⎦ ⎣ Hz

q02

+

q12

−

q22

−

q32

2(q1 q2 + q0 q3 )

2(q1 q2 − q0 q3 )

q02 − q12 + q22 − q32

2(q1 q3 + q0 q2 )

2(q2 q3 − q0 q1 )

2(q1 q3 − q0 q2 )

⎤⎡

⎤

M ⎥⎢ x ⎥ ⎥⎢ ⎥ 2(q2 q3 + q0 q1 ) ⎥ ⎢ My ⎥ , (4.3) ⎦⎣ ⎦ 2 2 2 2 q 0 − q1 − q 2 + q3 Mz

kde jednotlivé quaterniony se z polohov´ ych u ´hl˚ u urˇc´ı opˇet dle vztah˚ u (2.8) aˇz (2.11). Stejnˇe jako v pˇr´ıpadˇe akcelerometr˚ u je tato transformace ve schématu na obrázku 4.3 realizována pomoc´ı bloku geo2bdy“, kter´ y obsahuje skript uveden´ y v pˇr´ıloze A.2. Takto ” jsme z´ıskali model mˇeˇren´ı ideáln´ıch magnetometr˚ u.

4.1.4

Kompletn´ı gener´ ator dat

Spojen´ım model˚ u popsan´ ych v sekc´ıch 4.1.1 aˇz 4.1.3 z´ıskáme kompletn´ı model generátoru dat, kter´ y v závislosti na polohov´ ych u ´hlech objektu, na kterém jsou senzory um´ıstˇeny, reprezentuje mˇeˇren´ı ideáln´ımi gyroskopy, akcelerometry a magnetometry . Schéma tohoto modelu je na obrázku 4.4. Pomoc´ı tohoto generátoru m˚ uˇzeme na základˇe znalosti pr˚ ubˇehu polohov´ ych u ´hl˚ u objektu z´ıskat informaci o tom, jaké pr˚ ubˇehy hodnot by mˇely b´ yt namˇeˇreny jednotliv´ ymi senzory v ideáln´ım pˇr´ıpadˇe.

4.2

Senzorick´ y model

V pˇredchoz´ı sekci byl vytvoˇren model generátoru dat, kter´ y bere v potaz fyzikáln´ı principy jednotliv´ ych senzor˚ u a poskytuje tak u ´daje, které by mˇely b´ yt v ideáln´ım pˇr´ıpadˇe namˇeˇreny jednotliv´ ymi senzory v závislosti na polohov´ ych u ´hlech objektu. V dneˇsn´ı dobˇe vˇsak ˇzádn´ y takov´ y senzor neexistuje, proto chceme-li se v´ıce pˇribl´ıˇzit realitˇe a ˇcinnost senzor˚ u namodelovat tak, aby se v´ ystup modelu realitˇe co nejv´ıce pˇribliˇzoval, mus´ıme vz´ıt v u ´vahu chyby vyskytuj´ıc´ı se v mˇeˇren´ı tˇemito senzory a co nejpˇresnˇeji je namodelovat (Kumar, N. S. a Tann, T., 2004).

´ MODEL 4.2. SENZORICKY

1 Phi

2 Theta

39

K (z−1) Ts z

MATLAB Function

Discrete Derivative

eul2pqr

[0, 0, −9.81]

K (z−1) Ts z

MATLAB Function

Gravity constants

Discrete Derivative

geo2bdy

3 Psi

1 p,q,r

(Mx


My

2 ax,ay,az

Mz) MATLAB Function

Magnetic constants

geo2bdy

3 Hx,Hy,Hz

Obr´ azek 4.4: Schéma generátoru dat

V této sekci se vˇsak budeme zab´ yvat pouze chybami, které se daj´ı deterministicky urˇcit. To jsou napˇr´ıklad chyby zp˚ usobené nepˇresnostmi pˇri v´ yrobˇe, teplotn´ı závislost´ı materiál˚ u, ze kter´ ych jsou senzory vyrobeny, a dalˇs´ımi fyzikáln´ımi závislostmi. Schéma modelu, kter´ y tyto chyby zachycuje, je znázornˇeno na obrázku 4.5.

Prvn´ım typem chyby, kter´ ym jsou zat´ıˇzeny vˇsehny tˇri uvaˇzované typy senzor˚ u (kaˇzd´ y vˇsak v jiné m´ıˇre), je tzv. misalignment error. Jak jiˇz bylo ˇreˇceno v sekci 3.3, je to chyba zp˚ usobená nepˇresn´ ym uloˇzen´ım jednotliv´ ych senzor˚ u z pˇr´ısluˇsné trojice do os, ve kter´ ych maj´ı tyto senzory mˇeˇrit. Tuto ⎡ f ⎢ mx ⎢ ⎢ fmy ⎣ fmz

chybu vyjadˇruje vztah ⎤ ⎡ m12 m13 m ⎥ ⎢ 11 ⎥ ⎢ ⎥ = ⎢ m21 m22 m23 ⎦ ⎣ m31 m32 m33

⎤⎡

⎤

f ⎥⎢ x ⎥ ⎥⎢ ⎥ ⎥ ⎢ fy ⎥ , ⎦⎣ ⎦ fz

(4.4)

kde fmx , fmy a fmz jsou hodnoty namˇeˇrené senzory, prvky m11 aˇz m33 v matici jsou koeficienty vyjadˇruj´ıc´ı kˇr´ıˇzové vazby v mˇeˇren´ı senzory v jednotliv´ ych osách a tedy zároveˇ n m´ıru nepˇresnoti uloˇzen´ı jednotliv´ ych senzor˚ u v pˇr´ısluˇsn´ ych osách a fx , fy a fz jsou skuteˇcné hodnoty mˇeˇren´ ych veliˇcin. Tento typ chyby je na obrázku 4.5 realizován blokem Misa”


40

MATLAB

1

Function

p, q, r MATLAB

Misalignment

Function CG Offset

Function

ax, ay, az

Misalignment

3

MATLAB Function

Temperature drift

Sensor bias

MATLAB

MATLAB

1 p, q, r

MATLAB

2

Hx, Hy, Hz

MATLAB Function

Function

Function

Temperatue drift

Sensor bias

2 ax, ay, az

MATLAB Function Misalignment

MATLAB

MATLAB

Function Temperatue drift

3

Function

Hx, Hy, Hz

Sensor bias

4 T

Obr´ azek 4.5: Schéma senzorického modelu

lignment“, kter´ y vykonává skript uveden´ y v pˇr´ıloze B.1.

Dalˇs´ım typem chyby je tzv. scale factor error. Jak jiˇz bylo ˇreˇceno v sekci 3.3, je to chyba zp˚ usobená nestálost´ı zes´ılen´ı senzoru. Zde budeme uvaˇzovat pouze konstantn´ı sloˇzku této chyby. Potom v souladu s rovnic´ı (3.1) m˚ uˇzeme tuto chybu zahrnout do chyby misalignment. Rovnice ⎡ f ⎢ mx ⎢ ⎢ fmy ⎣ fmz

(4.4) potom pˇrejde ve tvar ⎤ ⎡ m12 m13 s + m11 ⎥ ⎢ x ⎥ ⎢ ⎥ = ⎢ m21 sy + m22 m23 ⎦ ⎣ m31 m32 sz + m33

⎤⎡

fx

⎤

⎥⎢ ⎥ ⎥⎢ ⎥ ⎥ ⎢ fy ⎥ , ⎦⎣ ⎦ fz

(4.5)

kde sx , sy a sz jsou jednotlivé sloˇzky chyby scale factor.

Dalˇs´ım zdrojem chyb je závislost v´ ystupn´ı hodnoty na teplotˇe. Této závoslosti ˇr´ıkáme teplotn´ı drift a je to hodnota, která se pˇriˇc´ıtá ke skuteˇcné v´ ystupn´ı hodnotˇe senzoru v závislosti na jeho teplotˇe. Tato závislost se bˇeˇznˇe popisuje polynomem jako funkce teploty. Tuto chybu v pˇr´ıpadˇe namodelován´ı polynomem tˇret´ıho ˇrádu vyjadˇruje vztah ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 3 2 f f t T + t12 T + t13 T + t14 ⎢ mx ⎥ ⎢ x ⎥ ⎢ 11 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ (4.6) ⎢ fmy ⎥ = ⎢ fy ⎥ + ⎢ t21 T 3 + t22 T 2 + t23 T + t24 ⎥ , ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ fmz fz t31 T 3 + t32 T 2 + t33 T + t34

´ MODEL 4.2. SENZORICKY

41

kde fmx , fmy a fmz jsou hodnoty namˇeˇrené senzory, fx , fy a fz jsou skuteˇcné hodnoty mˇeˇren´ ych veliˇcin, T je teplota senzoru a prvky t11 aˇz t34 jsou koeficienty polynomu vyjadˇruj´ıc´ıho teplotn´ı závislost. Teplotu T je moˇzné z´ıskat bud’ pomoc´ı samostaného senzoru, nebo se dá v jist´ ych pˇr´ıpadech modelovat pˇri znalosti vertikáln´ıho teplotn´ıho profilu atmosféry. Dle modelu standardn´ı atmosféry potom teplota klesá o 6, 5◦ na kaˇzd´ y kilometr v´ yˇsky. Zde je vˇsak tˇreba si uvˇedomit, ˇze samotn´ y senzor, jehoˇz teplotn´ı závislost modelujeme, nemus´ı b´ yt ovlivnˇen pouze teplotou okoln´ıho vzduchu, ale m˚ uˇze b´ yt ovlivˇ nován okoln´ımi zastavˇen´ ymi zaˇr´ızen´ımi, jako je napˇr´ıklad spalovac´ı motor. Proto samostatn´ y senzor teploty je jistˇe mnohem lepˇs´ı volba.

Tento typ chyby je na obrázku 4.5 realizován blokem Temperature drift“, kter´ y vy” konává skript uveden´ y v pˇr´ıloze B.2.

Dalˇs´ım typem chyby, kter´ y ovlivˇ nuje funkci vˇsech tˇr´ı typ˚ u senzor˚ u, je tzv. bias. Podobnˇe jako chyba zp˚ usobená teplotn´ım driftem je i toto chyba aditivn´ı a tedy se ke skuteˇcné v´ ystupn´ı hodnotˇe senzoru pˇriˇc´ıtá. V modelu na obrázku 4.5 je tento typ chyby realizován blokem Sensor bias“. Tento blok vykonává skript uveden´ y v pˇr´ıloze B.3. ” Jak jiˇz bylo ˇreˇceno v sekci 3.3, projevy této chyby jsou velmi rozmanité. V modelu na obrázku 4.5 je vˇsak zahrnuta pouze konstatntn´ı sloˇzka této chyby. Tuto sloˇzku m˚ uˇzeme naz´ yvat také chybou nuly senzoru, která je dána nenulov´ ym v´ ystupem senzoru pˇri jeho nulovém vstupu. Tuto chybu potom vyjadˇruje vztah ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ fmx fx Bx ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ fmy ⎥ = ⎢ fy ⎥ + ⎢ By ⎥ , ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ fmz fz Bz

(4.7)

kde opˇet fmx , fmy a fmz jsou hodnoty namˇeˇrené senzory, fx , fy a fz jsou skuteˇcné hodnoty mˇeˇren´ ych veliˇcin a prvky Bx , By a Bz jsou honoty udávaj´ıc´ı chybu nuly senzoru.


42

Pˇrestoˇze v tomto modelu je funkce chyby biasu omezena pouze na konstantn´ı hodnotu, pozdˇeji uvid´ıme, ˇze tato sloˇzka chyby má mnohem rozsáhlejˇs´ı v´ yznam pro odhad polohov´ ych u ´hl˚ u objektu.

Posledn´ı chybou zahrnutou ve zde popisovaném modelu je chyba zp˚ usobená nepˇresn´ ym um´ıstˇen´ım akcelerometr˚ u na objektu. V ideáln´ım pˇr´ıpadˇe by soustava akcelerometr˚ u mˇela b´ yt um´ıstˇena v tˇeˇziˇsti objektu. Pokud vˇsak budou akcelerometry um´ıstˇené jinde neˇz v tˇeˇziˇsti objektu, bude pˇri jeho otáˇcen´ı vznikat dostˇredivé zrychlen´ı, které akcelerometry budou také mˇeˇrit. Tato informace je pro nás vˇsak ruˇsivá a budeme se ji tedy snaˇzit kompenzovat.

Na kaˇzd´ y ze tˇr´ı akcelerometr˚ u bude p˚ usobit aditivn´ı zrychlen´ı dané vztahem ad = ω 2 R,

(4.8)

´ kde ω je u ´hlová rychlost otáˇcen´ı a R je polomˇer tohoto otáˇcen´ı. Uhlov´ a rychlost otáˇcen´ı se potom pro kaˇzd´ y akcelerometr bude skládat ze dvou sloˇzek. Pro akcelerometr um´ıstnˇen´ y tak, aby mˇeˇril zrychlen´ı v tˇelesové ose x, to budou sloˇzky dané otáˇcen´ım objektu kolem tˇelesov´ ych os y a z, pro akcelerometr um´ıstˇen´ y pro mˇeˇren´ı zrychlen´ı v tˇelesové ose y to potom budou sloˇzky otáˇcen´ı objektu kolem tˇelesov´ ych os x a z a nakonec pro akcelerometr mˇeˇr´ıc´ı zrychlen´ı v tˇelesové ose z to budou sloˇzky otáˇcen´ı kolem tˇelesov´ ych os x a y. Jednotlivé dvojice sloˇzek otáˇcen´ı pˇr´ısluˇsej´ıc´ı jednotliv´ ym akcelerometr˚ u je tˇreba sloˇzit, coˇz se napˇr´ıklad pro akcelerometr mˇeˇr´ıc´ı v tˇelesové ose x provede dle vztahu ωx =

q 2 + r2 ,

kde q je u ´hlová rychlost otáˇcen´ı kolem tˇelesové osy y a r je u ´hlová rychlost otáˇcen´ı kolem tˇelesové osy z. Po dosazen´ı tohoto vztahu do vztahu (4.8) dostaneme adx = (q 2 + r2 )Rx , kde Rx je vzdálenost um´ıstˇen´ı akcelerometr˚ u od tˇeˇziˇstˇe v ose x. Analogicky potom do-

˚ FILTR 4.3. KALMANUV staneme vztah pro vˇsechny tˇri akcelerometry. Tento vztah m˚ uˇzeme napsat jako ⎡ ⎤ ⎡ ⎤ adx (q 2 + r2 )Rx ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ 2 ⎥ 2 = ⎢ ady ⎥ ⎢ (p + r )Ry ⎥ , ⎣ ⎦ ⎣ ⎦ adz (p2 + q 2 )Rz

43

(4.9)

kde q, q a r jsou u ´hlové rychlosti otáˇcen´ı kolem jednotliv´ ych tˇelesov´ ych os a Rx , Ry a Rz jsou vzdálenosti um´ıstˇen´ı akcelerometr˚ u od tˇeˇziˇstˇe v jednotliv´ ych osách. Jednotlivá zrychlen´ı adx , ady a adz se potom pˇriˇc´ıtaj´ı ke skuteˇcn´ ym hodnotám zrychlen´ı mˇeˇren´ ym jednotliv´ ymi akcelerometry. Jelikoˇz se vˇsak jedná o zrychlen´ı dostˇredivé, je jeho smˇer pˇri kladn´ ych hodnotách vzdálenost´ı um´ıstˇen´ı akcelerometr˚ u od tˇeˇziˇstˇe záporn´ y, jelikoˇz toto zrychlen´ı smˇeˇruje do stˇredu otáˇcen´ı. Princip urˇcován´ı smˇeru p˚ usoben´ı zrychlen´ı je zde velice podobn´ y jako pˇri urˇcován´ı vektoru zrychlen´ı v sekci 4.1.2 a je tedy moˇzné si to pˇredstavit i takto.

Tato chyba je na obrázku 4.5 realizována blokem CG Offset“, kter´ y vykonává skript ” uveden´ y v pˇr´ıloze B.4.

4.3

Kalman˚ uv filtr

Nyn´ı, kdyˇz jiˇz máme pˇripraven model mˇeˇren´ı jednotliv´ ymi senzory, m˚ uˇzeme pˇristoupit k jejich integraci. Velice vhodn´ ym nástrojem pro takovou integraci se ukázal b´ yt Kalman˚ uv filtr, kter´ y byl jiˇz popsán v sekci 2.3.

Jako prvn´ı tedy setav´ıme stavové rovnice, které jsou obecnˇe popsány rovnicemi (2.13) ˇci (2.16). K tomu vyuˇzijeme právˇe informace ze sestaveného modelu ˇcinnosti senzor˚ u. Jelikoˇz jsou stavové rovnice obecnˇe popsány pomoc´ı derivac´ı nebo diferenc´ı, nab´ız´ı se k sestaven´ı stavov´ ych rovnic pouˇz´ıt model mˇeˇren´ı pomoc´ı gyroskop˚ u, jelikoˇz naˇr´ıklad na obrázku 4.1 vid´ıme, ˇze zde jsou derivace (diference) obsaˇzeny. Pˇri sestavován´ı stavov´ ych rovnic budeme postupovat od konce modelu mˇeˇren´ı k jeho zaˇca´tku, neboli u ´hlové rychlosti zmˇeˇrené pomoc´ı gyroskop˚ u postupnˇe oprav´ıme pomoc´ı informac´ı z modelu mˇeˇren´ı


44

a nakonec je transformujeme v quaterniony, které reprezentuj´ı polohové u ´hly.

Prvn´ım krokem tedy bude dle obrázku 4.5 odstranˇen´ı chyby biasu. To se dle rovnice (4.7) provede jako ⎡

p1

⎤

⎡

pm

⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ q 1 ⎥ = ⎢ qm ⎣ ⎦ ⎣ r1 rm

⎤

⎡

Bp

⎤

⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ − ⎢ Bq ⎥ , ⎦ ⎣ ⎦ Br

´hlové rychlosti. kde pm , qm a rm jsou zmˇeˇrené u

Dále následuje odstranˇen´ı teplotn´ıho ⎡ ⎤ ⎡ ⎤ ⎡ p p ⎢ 2 ⎥ ⎢ 1 ⎥ ⎢ ⎢ ⎥ ⎢ ⎥ ⎢ ⎢ q2 ⎥ = ⎢ q1 ⎥ − ⎢ ⎣ ⎦ ⎣ ⎦ ⎣ r2 r1

driftu. To se dle rovnice (4.6) provede jako ⎤ 3 2 tp1 T + tp2 T + tp3 T + tp4 ⎥ ⎥ tq1 T 3 + tq2 T 2 + tq3 T + tq4 ⎥ . ⎦ 3 2 tr1 T + tr2 T + tr3 T + tr4

Nyn´ı odstran´ıme kombinaci chyb misalignment a scale factor. To se dle rovnice (4.5) provede jako ⎡

⎤

⎡

mp2 mp3 p s + mp1 ⎢ o ⎥ ⎢ p ⎥ ⎢ ⎢ ⎢ qo ⎥ = ⎢ mq1 sq + mq2 mq3 ⎦ ⎣ ⎣ ro mr1 mr2 sr + mr3

⎤−1 ⎡ ⎥ ⎥ ⎥ ⎦

⎤

p ⎢ 2 ⎥ ⎢ ⎥ ⎢ q2 ⎥ . ⎣ ⎦ r2

ych Z mˇeˇren´ ych u ´hlov´ ych rychlost´ı pm , qm a rm jsme tedy opraven´ım dle namodelovan´ chyb z´ıskali u ´hlové rychlosti po , qo a ro . Posledn´ım krokem k vytvoˇren´ı stavov´ ych rovnic je transformace u ´hlov´ ych rychlost´ı na quaterniony. To lze provést dle rovnice (2.5). Tedy ⎡ ⎤ ⎤ ⎤⎡ ⎡ 0 −po −qo −ro q0 q˙0 ⎢ ⎥ ⎥ ⎥⎢ ⎢ ⎢ ⎥ ⎥ ⎥⎢ ⎢ ⎢ q˙1 ⎥ 1 ⎢ po 0 ro −qo ⎥ ⎢ q1 ⎥ ⎥= ⎢ ⎥. ⎥⎢ (4.10) q˙ = ⎢ ⎢ ⎥ ⎥ ⎥⎢ ⎢ ⎢ q˙2 ⎥ 2 ⎢ qo −ro 0 p o ⎥ ⎢ q2 ⎥ ⎣ ⎦ ⎦ ⎦⎣ ⎣ q˙3 ro qo −po 0 q3


45

T´ım jsme z´ıskali ˇctyˇri stavové rovnice, ke kter´ ym dále pˇridáme dalˇs´ıch devˇet stav˚ u, které vyjadˇruj´ı chybu biasu vˇsech tˇr´ı typ˚ u pouˇzit´ ych senzor˚ u. Kromˇe polohov´ ych u ´hl˚ u tedy budeme jeˇstˇe odhadovat aditivn´ı chyby jednotliv´ ych senzor˚ u, coˇz dále zpˇresn´ı odhad quaternion˚ u (polohov´ ych u ´hl˚ u). Chyby biasu budeme v tomto kroku modelovat jako konstanty, ˇc´ımˇz ˇr´ıkáme, ˇze se tyto stavy nebudou v predikˇcn´ım kroku algoritmu Kalmanova filtru mˇenit, ale budou se aktualizovat pouze v aktualizaˇcn´ım kroku. Vˇsech tˇrináct stavov´ ych rovnic potom bude vypadat následovnˇe. 1 (−po q1 − qo q2 − ro q3 ) 2 1 q˙1 = (po q0 + ro q2 − qo q3 ) 2 1 q˙2 = (qo q0 − ro q1 + po q3 ) 2 1 q˙3 = (ro q0 + qo q1 − po q2 ) 2 B˙ p = 0 q˙0 =

B˙ q = 0 B˙ r = 0 B˙ ax = 0 B˙ ay = 0 B˙ az = 0 B˙ Hx = 0 B˙ Hy = 0 B˙ Hz = 0 Vˇsechny tyto stavové rovnice jsou spojité, Kalman˚ uv filtr vˇsak pracuje v diskrétn´ıch kroc´ıch. Je tedy tˇreba tyto rovnice diskretizovat. Diskretizaci m˚ uˇzeme provést napˇr´ıklad pomoc´ı Eulerovy diskretizaˇcn´ı metody, která ˇr´ıká, ˇze ˙ x(t) ≈

x(t + Ts ) − x (t) , Ts

kde Ts je vzorkovac´ı perioda. Odtud plyne, ˇze ˙ x(t + Ts ) ≈ x (t) + Ts x(t).

46


Diskretizované stavové rovnice potom pˇrejdou v následuj´ıc´ı tvar. ⎡ ⎡ ⎤ 1 q (−po q1 − qo q2 − ro q3 ) ⎢ 2 ⎢ 0 ⎥ ⎢ 1 ⎢ ⎥ ⎢ 2 (po q0 + ro q2 − qo q3 ) ⎢ q1 ⎥ ⎢ ⎢ ⎥ ⎢ 1 ⎢ ⎥ ⎢ 2 (qo q0 − ro q1 + po q3 ) ⎢ q2 ⎥ ⎢ ⎢ ⎥ ⎢ 1 ⎢ ⎥ ⎢ 2 (ro q0 + qo q1 − po q2 ) ⎢ q3 ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Bp ⎥ 0 ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Bq ⎥ 0 ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ x(k + 1) = ⎢ Br ⎥ + Ts ⎢ 0 ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Bax ⎥ 0 ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Bay ⎥ 0 ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Baz ⎥ 0 ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ BHx ⎥ 0 ⎢ ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ BHy ⎥ 0 ⎣ ⎣ ⎦ 0 BHz

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(4.11)

Dále sestav´ıme rovnice mˇeˇren´ı. K tomu vyuˇzijeme zbylé dvˇe senzorické vˇetve z modelu ˇcinnosti senzor˚ u, tedy vˇetev akcelerometr˚ u a vˇetev magnetometr˚ u. Podle tˇechto rovnic se jiˇz vˇsak nebude provádˇet odhad, ale budou pouˇzity ke korekci stav˚ u popsan´ ych rovnic´ı (4.11). Z toho d˚ uvodu budeme pˇri sestavován´ı rovnic mˇeˇren´ı postupovat od zaˇcátku modelu ke konci, narozd´ıl od sestavován´ı stavov´ ych rovnic, kde jsme postupovali naopak. Nejprve tedy vytvoˇr´ıme rovnice popisuj´ıc´ı ˇcinnost akcelerometr˚ u. Z obrázku 4.2 vid´ıme, ˇze nejprve mus´ıme provést transformaci vektrou zrychlen´ı do tˇelesové souˇradné soustavy. Tato transformace je dána vztahem (4.2), tedy ⎡ ⎤ ⎡ a q 2 + q12 − q22 − q32 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) ⎢ x1 ⎥ ⎢ 0 ⎢ ⎥ ⎢ ⎢ ay1 ⎥ = ⎢ 2(q1 q2 − q0 q3 ) q02 − q12 + q22 − q32 2(q2 q3 + q0 q1 ) ⎣ ⎦ ⎣ 2(q1 q3 + q0 q2 ) az1 2(q2 q3 − q0 q1 ) q02 − q12 − q22 + q32

⎤

⎤⎡ ⎥⎢ ⎥⎢ ⎥⎢ ⎦⎣

0 0 −9, 81

⎥ ⎥ ⎥. ⎦

Dále budeme postupovat dle obrázku 4.5. Dalˇs´ım krokem je tedy zapoˇcten´ı chyby zp˚ usobené nepˇresn´ ym um´ıstˇen´ım akcelerometr˚ u. Tato chyba je popsána rovnic´ı (4.9),


47

tedy ⎡

ax2

⎢ ⎢ ⎢ ay2 ⎣ az2

⎤

⎡

ax1

⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ ay1 ⎦ ⎣ az1

⎤

⎡

(qo2 + ro2 )Rx

⎥ ⎢ ⎥ ⎢ 2 ⎥ − ⎢ (po + ro2 )Ry ⎦ ⎣ (p2o + qo2 )Rz

⎤ ⎥ ⎥ ⎥. ⎦

Dále zapoˇc´ıtáme kombinaci chyb misalignment a scale factor dle rovnice (4.5). Tedy ⎡

ax3

⎢ ⎢ ⎢ ay3 ⎣ az3

⎤

⎡

⎥ ⎢ ⎥ ⎢ ⎥=⎢ ⎦ ⎣

sax + m ax 1

m ax 2

m ax 3

m ay 1

say + m ay 1

m ay 3

m az 1

m az 2

saz + m az 3

⎤⎡

ax2

⎥⎢ ⎥⎢ ⎥ ⎢ ay2 ⎦⎣ az2

⎤ ⎥ ⎥ ⎥. ⎦

Dalˇs´ım krokem je zapoˇcten´ı chyby teplotn´ıho driftu dle rovnice (4.6). Tedy ⎡

axe

⎢ ⎢ ⎢ aye ⎣ aze

⎤

⎡

ax3

⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ ay3 ⎦ ⎣ az3

⎤

⎡

tax 1 T 3 + tax 2 T 2 + tax 3 T + tax 4

⎥ ⎢ ⎥ ⎢ ⎥ + ⎢ tay 1 T 3 + tay 2 T 2 + tay 3 T + tay 4 ⎦ ⎣ taz 1 T 3 + taz 2 T 2 + taz 3 T + taz 4

⎤ ⎥ ⎥ ⎥. ⎦

Po pˇriˇcten´ı posledn´ı modelované chyby, tedy chyby biasu, z´ıskáme prvn´ı tˇri rovnice mˇeˇren´ı. ⎡

axm

⎢ ⎢ ⎢ aym ⎣ azm

⎤

⎡

axe

⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ aye ⎦ ⎣ aze

⎤

⎡

Bax

⎥ ⎢ ⎥ ⎢ ⎥ + ⎢ Bay ⎦ ⎣ Baz

⎤ ⎥ ⎥ ⎥ ⎦

Dalˇs´ı tˇri rovnice mˇeˇren´ı z´ıskáme z vˇetve modelu, která popisuje ˇcinnost magnetometr˚ u. Z obrázku 4.3 vid´ıme, ˇze nejprve mus´ıme opˇet provést transformaci pˇr´ısluˇsného vektoru magnetického pole Zemˇe do tˇelesové souˇradné soustavy. Tato transformace je


48

dána vztahem (4.3), tedy ⎡ ⎤ ⎡ 2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 ) H q 2 + q12 − q22 − q32 ⎢ x1 ⎥ ⎢ 0 ⎢ ⎥ ⎢ ⎢ Hy1 ⎥ = ⎢ 2(q1 q2 − q0 q3 ) q02 − q12 + q22 − q32 2(q2 q3 + q0 q1 ) ⎣ ⎦ ⎣ 2(q1 q3 + q0 q2 ) Hz1 2(q2 q3 − q0 q1 ) q02 − q12 − q22 + q32

⎤⎡

⎤

M ⎥⎢ x ⎥ ⎥⎢ ⎥ ⎥ ⎢ My ⎥ . ⎦⎣ ⎦ Mz

Dalˇs´ı kroky budou opˇet vycházet z obrázku 4.5. Nyn´ı tedy zapoˇc´ıtáme kombinaci chyb misalignment a scale factor dle rovnice (4.5). Tedy ⎤ ⎡ ⎡ mHx 2 mHx 3 H s + mHx 1 ⎢ x2 ⎥ ⎢ Hx ⎥ ⎢ ⎢ ⎢ Hy2 ⎥ = ⎢ mHy 1 sHy + mHy 1 mHy 3 ⎦ ⎣ ⎣ Hz2 mHz 1 mHz 2 sHz + mHz 3

Dále zapoˇc´ıtáme ⎡ H ⎢ xe ⎢ ⎢ Hye ⎣ Hze

⎤⎡

H ⎥ ⎢ x1 ⎥⎢ ⎥ ⎢ Hy1 ⎦⎣ Hz1

chybu teplotn´ıho driftu dle rovnice (4.6). Tedy ⎤ ⎡ ⎤ ⎡ H t T 3 + tHx 2 T 2 + tHx 3 T + tHx 4 ⎥ ⎢ x2 ⎥ ⎢ Hx 1 ⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ Hy2 ⎥ + ⎢ tHy 1 T 3 + tHy 2 T 2 + tHy 3 T + tHy 4 ⎦ ⎣ ⎦ ⎣ Hz2 tHz 1 T 3 + tHz 2 T 2 + tHz 3 T + tHz 4

⎤ ⎥ ⎥ ⎥. ⎦

⎤ ⎥ ⎥ ⎥. ⎦

Nakonec opˇet pˇriˇcteme chybu biasu, ˇc´ımˇz z´ıskáme druhou trojici rovnic mˇeˇren´ı. ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ Hxm Hxe BHx ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ Hym ⎥ = ⎢ Hye ⎥ + ⎢ BHy ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ Hzm Hze BHz

Nyn´ı pˇridáme jeˇstˇe jednu rovnici mˇeˇren´ı, která bude zajiˇst’ovat, ˇze souˇcet kvadrát˚ u jednotliv´ ych quaternion˚ u bude roven jedné, jak to poˇzaduje rovnice (2.12). Tedy 1 = q02 + q12 + q22 + q32

˚ FILTR 4.3. KALMANUV Kompletn´ı skupina sedmi rovnic mˇeˇren´ı potom vypadá ⎡ ⎤ ⎡ 1 q 2 + q12 + q22 + q32 ⎢ ⎥ ⎢ 0 ⎢ ⎥ ⎢ ⎢ axm ⎥ ⎢ axe + Bax ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ aym ⎥ ⎢ aye + Bay ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ azm ⎥ = ⎢ aze + Baz ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Hxm ⎥ ⎢ Hxe + BHx ⎢ ⎥ ⎢ ⎢ ⎥ ⎢ ⎢ Hym ⎥ ⎢ Hye + BHy ⎣ ⎦ ⎣ Hze + BHz Hzm

49 následovnˇe. ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(4.12)

Jelikoˇz jsou jak stavové rovnice, tak rovnice mˇeˇren´ı nelineárn´ı, budou se zpracovávat algoritmem rozˇs´ıˇreného Kalmanova filtru. V kaˇzdém kroku se potom provede v´ ypoˇcet dle rovnic (2.18) aˇz (2.24), tedy následovnˇe. ˆ x (k + 1|k) = f (ˆ x (k)) A(k) =

∂f (x ) ∂x x =ˆx (k|k)

P(k + 1|k) = A(k)P(k|k)AT (k) + Q H (k) =

∂h(x ) ∂x x =ˆx (k|k−1)

K (k) = P(k|k − 1)H T (k)(H (k)P(k|k − 1)H T (k) + R)−1 ˆ x (k|k) = ˆ x (k|k − 1) + K (k)(y (k) − h(ˆ x (k|k − 1))) P(k|k) = P(k|k − 1) − K (k)(H (k)P(k|k − 1)H T (k) + R)K T (k) Názornˇe je algoritmus znázornˇen na obrázku 2.7. Aby vˇsak tento algoritmus mohl fungovat, je nutné nejprve inicializovat poˇcáteˇcn´ı odhad stavu x (k), jeho kovarianˇcn´ı matici chyby P(k|k), kovarianˇcn´ı matici ˇsumu procesu Q a kovarianˇcn´ı matici ˇsumu mˇeˇren´ı R. Vˇsechny tyto hodnoty se budou liˇsit pˇr´ıpad od pˇr´ıpadu a budou pro kaˇzd´ y pˇr´ıpad pouˇzit´ı ˇsity na m´ıru“. ”

50


Kapitola 5 V´ ysledky V pˇredchoz´ı kapitole byly vytvoˇreny nástroje pro integraci dat ze senzor˚ u k urˇcován´ı polohov´ ych u ´hl˚ u. V této kapitole se pod´ıváme na jejich funkˇcnost, a to jak na datech vytvoˇren´ ych umˇele, tak na skuteˇcn´ ych letov´ ych datech bezpilotn´ıho vzduˇsného prostˇredku.

5.1

Umˇ ele vytvoˇ ren´ a data

Prvn´ım krokem zde bude vytvoˇren´ı pr˚ ubˇeh˚ u jednotliv´ ych polohov´ ych u ´hl˚ u, které budou reprezentovat skuteˇcné hodnoty a ze kter´ ych budeme vycházet. K tomu poslouˇz´ı Simulinkové bloky Signal Builder“, ve kter´ ych je moˇzné ruˇcnˇe namodelovat jakékoliv pr˚ ubˇehy ” signál˚ u. Vytvoˇrené pr˚ ubˇehy polohov´ ych u ´hl˚ u jsou potom vidˇet na obrázku 5.1.

Nyn´ı pomoc´ı generátoru vytvoˇreného v sekc´ıch 4.1 a 4.2 budeme schopni z´ıskat data, která odpov´ıdaj´ı mˇeˇren´ı jednotliv´ ymi senzory. Nejprve je vˇsak nutné do tohoto modelu zanést parametry jednotliv´ ych modelovan´ ych chyb. Zvoleny byly následuj´ıc´ı hodnoty jednotliv´ ych parametr˚ u. 51

´ KAPITOLA 5. VYSLEDKY

52

Φ uhel [rad]

0.5

0

−0.5 0

20

40

60 cas [s] Θ

80

100

120

20

40

60 cas [s] Ψ

80

100

120

20

40

60 cas [s]

80

100

120

uhel [rad]

0.5

0

−0.5 0

uhel [rad]

20

10

0 0

Obr´ azek 5.1: Vytvoˇrené pr˚ ubˇehy polohov´ ych u ´hl˚ u

Matice chyb kombinuj´ıc´ı misalignment a scale factor jednotliv´ ych senzor˚ u:

⎡ 1, 1000

⎢ ⎢ M pqr = ⎢ −0, 1000 ⎣ 0, 2000 ⎡ 1, 1500 ⎢ ⎢ M a = ⎢ −0, 0900 ⎣ 0, 1500 ⎡ 0, 9000 ⎢ ⎢ M H = ⎢ 0, 0000 ⎣ −0, 0500

0, 1000

−0, 0500

⎤

⎥ ⎥ 0, 9000 0, 1000 ⎥ ⎦ −0, 1000 0, 9500 ⎤ 0, 0500 0, 0100 ⎥ ⎥ 1, 0500 0, 0200 ⎥ ⎦ −0, 1000 1, 0100 ⎤ 0, 0000 0, 0000 ⎥ ⎥ 0, 8000 0, 0000 ⎥ ⎦ 0, 1000 1, 0000

ˇ ˇ A ´ DATA 5.1. UMELE VYTVOREN

53

Koeficienty rovnice chyby teplotn´ıho driftu:

tp3 = 0, 1

tp4 = 0, 4

tq3 = 0, 02

tq4 = −0, 5

tr3 = 0, 05

tr4 = −0, 2

tax 3 = −0, 01

tax 4 = 0, 3

tay 3 = −0, 02

tay 4 = 0, 5

taz 3 = 0, 05

taz 4 = −1

tHx 3 = −0, 005 tHx 4 = 0, 003 tHy 3 = −0, 002 tHy 4 = 0, 004 tHz 3 = 0, 005

tHz 4 = −0, 001

Vˇsechny ostatn´ı koeficienty rovnic teplotn´ıho driftu byly zvoleny jako nulové. Konstantn´ı sloˇzky chyb biasu:

Bp = 0, 01 rad/s Bq = 0, 02 rad/s

Br = −0, 01 rad/s

Bax = 0, 2 m/s2

Bay = −0, 1 m/s2 Baz = 0, 06 m/s2

BHx = 0, 02 T

BHy = −0, 04 T

BHz = −0, 06 T

Vzd´ alenosti um´ıstˇen´ı akcelerometr˚ u od tˇeˇziˇstˇe v jednotliv´ ych osách:

Rx = 0, 01 m Ry = 0, 03 m Rz = 0, 02 m Teplota:

T = 23, 7◦ C (odpov´ıdá teplotˇe ve v´ yˇsce 200 m pˇri pˇr´ızemn´ı teplotˇe 25◦ C dle modelu standardn´ı atmosféry) Vzorkovac´ı frekvence:

fs = 64 Hz


54

uhlova rychlost [rad/s]

r 4

poskozena data data realnych senzoru data idealnich senzoru

2

0 0

10

20

30

40

50

60

70

80

90

100

110

60

70

80

90

100

110

60

70

80

90

100

110

zrychleni [m/s2]

cas [s] az −7 −8 −9 −10

intenzita mag. pole [T]

−11 0

10

20

30

40

50 cas [s] Hz

0.1 0.08 0.06 0.04 0.02 0

10

20

30

40

50 cas [s]

Obr´ azek 5.2: Data senzor˚ u v ose z

Aby se takto z´ıskaná data co nejv´ıce bl´ıˇzila reálnému mˇeˇren´ı, je tˇreba v´ ystupn´ı data jeˇstˇe zat´ıˇzit dalˇs´ımi chybami, které se vˇsak jiˇz nedaj´ı dobˇre modelovat. Nejprve je to aditivn´ı ˇsum, kterému se nelze vyhnout pˇri ˇza´dném mˇeˇren´ı. Dále byla data gyroskop˚ u a akcelerometr˚ u zat´ıˇzena ˇcasov´ ym driftem a data magnetometr˚ u byla zat´ıˇzena skokov´ ymi zmˇenami intenzit magnetického pole, coˇz simuluje poruchy dané lokáln´ımi vlivy jin´ ych magnetick´ ych pol´ı.

Pˇr´ıklad v´ ystupn´ıch dat jednotliv´ ych senzor˚ u v ose z je vidˇet na obrázku 5.2. Zde je také vidˇet rozd´ıl mezi daty z´ıskan´ ymi pouhou transformac´ı polohov´ ych u ´hl˚ u (tedy daty, která by byla namˇeˇrena ideáln´ımi senzory), daty z´ıskan´ ymi pomoc´ı senzorického modelu (tedy daty z´ıskan´ ymi mˇeˇren´ım reáln´ ymi senzory bez dalˇs´ıch poruch) a daty poˇskozen´ ymi vˇsemi chybami (tedy v´ ystupn´ımi daty senzor˚ u pˇri mˇeˇren´ı, které se v´ıce bl´ıˇz´ı realitˇe).

ˇ ˇ A ´ DATA 5.1. UMELE VYTVOREN

55

Nyn´ı jiˇz máme vˇsechny potˇrebné informace k pouˇzit´ı algoritmu rozˇs´ıˇreného Kalmanova filtru. Nejprve je tedy tˇreba provést inicializaci algoritmu. Jako prvn´ı urˇc´ıme poˇcáteˇcn´ı stav, kter´ y bude vypadat následovnˇe.

x (0) =

T

1 0 0 0 Bp Bq Br Bax Bay Baz BHx BHy BHz

Prvn´ı ˇctyˇri stavy byly urˇceny tak, aby byla zachována podm´ınka, ˇze souˇcet kvadrát˚ u quaternon˚ u mus´ı b´ yt roven jedné. Zbylé stavy odpov´ıdaj´ı konstantn´ım sloˇzkám chyb biasu pouˇz´ıvan´ ym v senzorickém modelu.

Dále je tˇreba urˇcit poˇcáteˇcn´ı hodnotu kovarianˇcn´ı matice chyby apriorn´ıho odhadu stavu P(0). Tu urˇc´ıme jednoduˇse jako jednotkovou matici o velikosti 13 × 13.

Jako posledn´ı urˇc´ıme kovarianˇcn´ı matice ˇsumu procesu Q a ˇsumu mˇeˇren´ı R. Cyklickou optimalizac´ı se jako nejlepˇs´ı ukázalo následuj´ıc´ı nastaven´ı. Obˇe matice jsou diagonáln´ı.

Q = diag R = diag

2

1

2

1

2

1

2

10

2

1, 4

2

1, 4

2

1, 4

2

2, 5

2

2, 5

0.000012 10002 10002 10002 102 102 102

2

2, 5

2

0, 6

2

0, 6

2

0, 6

Algoritmus bude dále pracovat dle obrázku 2.7, kde vyuˇz´ıvá rovnic (2.18) aˇz (2.24). V´ ysledek ˇcinnosti algoritmu je potom vidˇet na obrázc´ıch 5.3 a 5.4. Zde je vidˇet, ˇze algoritmus dokázal velmi efektivnˇe zkombinovat poˇskozená data ze senzor˚ u a odhadnout polohové u ´hly. Odhadnuté hodnoty se potom jen velmi málo liˇs´ı od tˇech skuteˇcn´ ych. Jak je vidˇet z obrázku 5.4, algoritmus dokázal také velmi dobˇre odhadnout pr˚ ubˇeh aditivn´ıch chyb, které byly k dat˚ um pˇridány za u ´ˇcelem lepˇs´ıho pˇribl´ıˇzen´ı k realitˇe. Dokázal velmi dobˇre odhadnout jak chyby ˇcasov´ ych drift˚ u, tak skokové chyby u mˇeˇren´ı magnetometry.


56

Φ

uhel [rad]

10

20

30

40

50 60 cas [s] q1

70

80

90

100

110

skutecnost

0

−0.5 0

odhad

10

20

30

40

0.5 0

50 60 cas [s] Θ

70

80

90

20

30

40

50 60 cas [s] q2

70

80

90

100

uhel [rad]

10

110

100

110

skutecnost odhad

1

−0.5 0

0

0.5 −1 0

0 20

30

40

50 60 cas [s] q3

70

80

90

100

110

1 0 −1 0

10

20

30

40

50 60 cas [s]

70

80

90

100

10

20

30

40

50 60 cas [s] Ψ

70

80

90

100

110

10

20

30

40

50 60 cas [s]

70

80

90

100

110

5 uhel [rad]

10

0

−5 0

110

Obr´ azek 5.3: Srovn´ an´ı skuteˇcn´ ych a odhadnut´ ych quaternion˚ u (vlevo) a

velikost [rad/s]

Bp 0.2 0.1 0 40

60 cas [s] Br

80

100

0.1 0 −0.1 0

20

40

60 cas [s] Bay

80

100

0 −0.1 −0.2 0

20

40

60 cas [s] BHx

80

100

velikost [m/s2]

20

velikost [m/s2]

0

velikost [T]

velikost [rad/s]

velikost [rad/s]

odpov´ıdaj´ıc´ıch polohov´ ych u ´hl˚ u (vpravo)

velikost [m/s2]

velikost [−]

−0.5 0

velikost [T]

velikost [−]

0.5

0 −1 0

0.04 0.02 0 0

velikost [T]

velikost [−]

velikost [−]

q0 1

20

40

60 cas [s] BHz

80

100

Bq 0.2 0.1 0 0

20

40

60 cas [s] Bax

80

100

0

20

40

60 cas [s] Baz

80

100

0

20

40

60 cas [s] BHy

80

100

20

40

60 cas [s]

80

100

0.3 0.2 0.1

0.2 0.1 0

−0.02 −0.04 −0.06 0

−0.04 skutecnost −0.06 −0.08 0

odhad 20

40

60 cas [s]

80

100

Obr´ azek 5.4: Srovn´ an´ı skuteˇcn´ ych a odhadnut´ ych aditivn´ıch chyb

´ A ´ LETOVA ´ DATA 5.2. REALN

5.2

57

Re´ aln´ a letov´ a data

V pˇredchoz´ı sekci byla demonstrována funkce algoritmu v pˇr´ıpadˇe integrace poˇskozen´ ych dat z r˚ uzn´ ych senzor˚ u. Bylo ukázáno, ˇze koneˇcn´ y odhad polohov´ ych u ´hl˚ u pomoc´ı takto poˇskozen´ ych dat byl velmi dobr´ y a od reality se pˇr´ıliˇs neliˇsil. Proto nyn´ı pˇristoup´ıme k ovˇeˇren´ı funkce algoritmu na reáln´ ych datech nasb´ıranych bˇehem letu bezpilotn´ıho prostˇredku.

Data, která zde budou pouˇzita, pocházej´ı z bezpilotn´ıho vrtuln´ıku Hirobo sst-eagle ˇ Freya, kter´ y je souˇcást´ı projektu RAMA (Spinka, O. et al., 2010), na jehoˇz domovsk´ ych stránkách je moˇzno se o tomto vrtuln´ıku dozvˇedˇet dalˇs´ı informace. Kromˇe mnoha jin´ ych je tento vrtuln´ık vybaven i senzory vhodn´ ymi pro pouˇzit´ı v této práci, tedy tˇr´ıos´ ym gyroskopem, tˇr´ıos´ ym akcelerometrem a tˇr´ıos´ ym magnetometrem. Veˇskerá letová data jsou bˇehem letu ukládána do matice, ze které je následnˇe moˇzno vybrat poˇzadovaná data.

Vystává zde vˇsak problém, jak´ ym zp˚ usobem urˇcit skuteˇcné polohové u ´hly vrtuln´ıku bˇehem letu. Mezi mˇeˇren´ ymi daty jsou právˇe i polohové u ´hly, ty vˇsak mohou b´ yt zat´ıˇzeny chybami, které byly popsány v pˇredchoz´ıch kapitolách. Mus´ıme tedy uˇcinit pˇredpoklad, ˇze tˇemto dat˚ um se dá do urˇcité m´ıry v malém ˇcasovém mˇeˇr´ıtku vˇeˇrit, jelikoˇz se zde ve ´ velké m´ıˇre neprojev´ı chyby driftu apod. Usek signál˚ u polohov´ ych u ´hl˚ u vrtuln´ıku, které budou pouˇzity k ladˇen´ı senzorického modelu jsou potom vidˇet na obrázku 5.5.

Nyn´ı pomoc´ı generátoru vytvoˇreného v sekc´ıch 4.1 a 4.2 budeme schopni z´ıskat data, která by odpov´ıdala mˇeˇren´ı jednotliv´ ymi senzory, pˇriˇcemˇz v´ ystupn´ı data budou záviset na konkrétn´ım nastaven´ı parametr˚ u jednotliv´ ych modelovan´ ych chyb. Takto z´ıskaná v´ ystupn´ı data je potom tˇreba porovnávat s odpov´ıdaj´ıc´ımi skuteˇcn´ ymi letov´ ymi daty tak, aby se modelovaná v´ ystupn´ı data co nejv´ıce bl´ıˇzila dat˚ um mˇeˇren´ ym. T´ım z´ıskáme parametry urˇcuj´ıc´ı chyby jednotliv´ ych senzor˚ u pouˇz´ıvan´ ych na vrtuln´ıku. Nejlepˇs´ı shody bylo potom cyklickou optimalizac´ı dosaˇzeno pˇri následuj´ıc´ım nastaven´ı parametr˚ u a shoda modelovan´ ych a reáln´ ych dat je potom vidˇet na obrázku 5.6.


58

Φ

uhel [rad]

0.2

0

−0.2 0

10

20

30

40 cas [s] Θ

50

60

70

80

10

20

30

40 cas [s] Ψ

50

60

70

80

10

20

30

40 cas [s]

50

60

70

80

uhel [rad]

0.5

0

−0.5 0

uhel [rad]

6 5.5 5 4.5 0

Obr´ azek 5.5: Pr˚ ubˇehy polohov´ ych u ´hl˚ u z reálného letu

Matice chyb kombinuj´ıc´ı misalignment a scale factor jednotliv´ ych senzor˚ u:

⎡

1, 1000 0, 1000 −0, 1000 ⎢ ⎢ M pqr = ⎢ −0, 1000 1, 0000 0, 0000 ⎣ 0, 0300 −0, 1000 0, 3000 ⎤ ⎡ 0, 1000 0, 0000 0, 0000 ⎥ ⎢ ⎥ ⎢ M a = ⎢ 0, 0000 0, 2000 0, 0000 ⎥ ⎦ ⎣ −0, 1000 0, 0000 1, 0000 ⎡ ⎤ 0, 6000 0, 0000 0, 0000 ⎢ ⎥ ⎢ ⎥ M H = ⎢ 0, 0000 0, 8000 0, 0000 ⎥ ⎣ ⎦ 0, 0000 0, 0000 0, 8000 Koeficienty rovnice chyby teplotn´ıho driftu:

⎤ ⎥ ⎥ ⎥ ⎦


59

Chyba teplotn´ıho driftu zde modelována nebude, jelikoˇz urˇcen´ı teploty senzor˚ u by bylo velice obt´ıˇzné. Nejsou zde totiˇz k dispozici samostatné teplotn´ı senzory a pˇr´ıpadné urˇcen´ı teploty dle modelu standardn´ı atmosféry také nen´ı moˇzné, jelikoˇz hlavn´ım prvkem urˇcuj´ıc´ım teplotu je spalovac´ı motor. Z tohoto d˚ uvodu budou vˇsechny koeficienty nulové a konstantn´ı sloˇzky teplotn´ıho driftu budou zahrnuty v chybách biasu. Konstantn´ı sloˇzky chyb biasu:

Bp = 0 rad/s

Bq = 0 rad/s

Br = 0 rad/s

Bax = −0, 8 m/s2 Bay = 0, 2 m/s2 Baz = 0 m/s2 BHx = 0, 002 T

BHy = 0, 003 T

BHz = 0, 015 T

Vzd´ alenosti um´ıstˇen´ı akcelerometr˚ u od tˇeˇziˇstˇe v jednotliv´ ych osách:

Rx = 0, 05 m Ry = −0, 003 m Rz = 0, 02 m Teplota:

Z d˚ uvodu neuvaˇzován´ı chyby teplotn´ıho driftu nebude teplota modelováva. Vzorkovac´ı frekvence:

fs = 64 Hz (odpov´ıdá vzorkovac´ı frekvenci nasb´ıran´ ych letov´ ych dat)

Z obrázku 5.6 je potom vidˇet, ˇze nejobt´ıˇznˇejˇs´ı bylo modelován´ı ˇcinnosti akcelerometr˚ u, u kterého se nepodaˇrilo dosáhnout tak dobr´ ych v´ ysledk˚ u, jako u ostatn´ıch senzor˚ u. To m˚ uˇze b´ yt zp˚ usobeno velk´ ym poˇskozen´ım v´ ystupn´ıch dat senzor˚ u napˇr´ıklad vibracemi spalovac´ıho motru. Pˇri urˇcován´ı kovarianˇcn´ıch matic algoritmu rozˇs´ıˇreného Kalmanova filtru na tuto skuteˇcnost potom mus´ı b´ yt pamatováno.

´ KAPITOLA 5. VYSLEDKY p

ax

mereni

0.5

zrychleni [m/s2]

model 0

−0.5 35

36

37

38

39

40 cas [s]

41

42

43

44

model

−1

−2 0

45

0.5

10

20

30

40 cas [s] ay

50

60

70

80

−0.5 35

10

20

30

40 cas [s] az

50

60

70

80

10

20

30

40 cas [s]

50

60

70

80

1

zrychleni [m/s2]

0

36

37

38

39

40 cas [s]

41

42

43

44

0

−1 0

45

r zrychleni [m/s2]

1

0

36

37

38

39

40 cas [s]

41

43

44

−12 0

45

0.02

0 mereni −0.02 0

intenzita mag. pole [T] intenzita mag. pole [T]

42

−8 −10

Hx

intenzita mag. pole [T]

−1 35

mereni

0

q




60

model 10

20

30

40 cas [s] Hy

50

60

70

80

0

10

20

30

40 cas [s] Hz

50

60

70

80

0

10

20

30

40 cas [s]

50

60

70

80

0.03

0.02

0.01

0.05

0.045

0.04

Obr´ azek 5.6: Srovn´ an´ı mˇeˇren´ ych a modelovan´ ych dat

Nyn´ı jiˇz opˇet máme vˇsechny potˇrebné informace k pouˇzit´ı algoritmu rozˇs´ıˇreného Kalmanova filtru. Nejprve znovu provedeme inicializaci algoritmu. Jako prvn´ı urˇc´ıme poˇca´teˇcn´ı stav, kter´ y bude vypadat stejnˇe jako v pˇr´ıpadˇe pouˇzit´ı umˇele vytvoˇren´ ych dat, tedy x (0) =

1 0 0 0 Bp Bq Br Bax Bay Baz BHx BHy BHz

T

.

Dále je tˇreba urˇcit poˇcáteˇcn´ı hodnotu kovarianˇcn´ı matice chyby apriorn´ıho odhadu stavu P(0). Tu opˇet urˇc´ıme jednoduˇse jako jednotkovou matici o velikosti 13 × 13. Posledn´ım krokem inicializace je urˇcen´ı kovarianˇcn´ı matice ˇsumu procesu Q a ˇsumu mˇeˇren´ı R. Cyklickou optimalizac´ı se jako nejlepˇs´ı ukázalo následuj´ıc´ı nastaven´ı. Obˇe


61

−1 −1.5 0

20

40

60

80 cas [s] q1

100

120

140

160 mereni odhad

0.5 0 −0.5 0

20

40

60

80 cas [s] q2

100

120

140

160

20

40

60

80 cas [s] q3

100

120

140

160

20

40

60

80 cas [s]

100

120

140

160

0.2 0 −0.2 0

velikost [−]

velikost [−]

velikost [−]

velikost [−]

q0 −0.5

1 0 −1 0

Obr´ azek 5.7: Srovn´ an´ı mˇeˇren´ ych a odhadnut´ ych quaternion˚ u

matice jsou diagonáln´ı.

Q = diag R = diag

2

15

2

10

2

10

2

50

2

0, 8

2

0, 3

2

0, 6

2

0, 9

2

0, 5

0.000012 100002 100002 100002 502 502 102

2

7

2

0, 2

2

0, 2

2

0, 2

Algoritmus bude dále opˇet pracovat dle obrázku 2.7, kde vyuˇz´ıvá rovnic (2.18) aˇz (2.24). V´ ysledek ˇcinnosti algoritmu je potom vidˇet na obrázc´ıch 5.7 a 5.8. Zde je vidˇet minimálnˇe jedno zlepˇsen´ı odhadnut´ ych dat oproti dat˚ um mˇeˇren´ ym. V pˇr´ıpadˇe odhadován´ı polohov´ ych u ´hl˚ u Φ a Θ totiˇz vid´ıme, ˇze p˚ uvodn´ı mˇeˇrená data driftovala, zat´ımco odhadnuté polohové u ´hly driftem netrp´ı. Uˇcinit v´ıce závˇer˚ u by bylo velico obt´ıˇzné, jelikoˇz nemáme k dispozici data správná.


62

Φ mereni

1 uhel [rad]

odhad 0.5 0 −0.5 0

20

40

60

80 cas [s] Θ

100

120

140

160

20

40

60

80 cas [s] Ψ

100

120

140

160

20

40

60

80 cas [s]

100

120

140

160

uhel [rad]

1 0.5 0 −0.5 0

uhel [rad]

1 0 −1 −2 0

Obr´ azek 5.8: Srovn´ an´ı mˇeˇren´ ych a odhadnut´ ych polohov´ ych u ´hl˚ u

Kapitola 6 Moˇ zn´ a zpˇ resnˇ en´ı odhadu V této kapitole budou nast´ınˇeny moˇznosti, které by mohly vést k dalˇs´ımu vylepˇsen´ı algoritmu, coˇz by vedlo i ke zpˇresnˇen´ı odhadu polohov´ ych u ´hl˚ u.

Jednou z moˇznost´ı, jak takového zlepˇsen´ı dosáhnout, je pouˇzit´ı dynamického modelu samotného letadla. Ne vˇzdy je vˇsak moˇzné takov´ y model z´ıskat. Nicménˇe i v pˇr´ıpadˇe, ˇze je model k dispozici, nemus´ı m´ıt dostateˇcnou pˇresnost na to, aby ho bylo moˇzné pouˇz´ıt. Zde tedy budeme uvaˇzovat situaci, kdy dostateˇcnˇe pˇresn´ y model letadla máme. Tento model m˚ uˇze vypadat napˇr´ıklad následovnˇe.

⎡ 0, 9982

−0, 00848

0, 00504

−2, 589 · 10−5

⎢ ⎢ ⎢ −0, 003726 0, 9651 1, 594 · 10−5 xpod (k + 1) = ⎢ ⎢ ⎢ 2, 428 · 10−5 −0, 0009618 1 ⎣ 0, 00314 −0, 1206 −1, 319 · 10−5 ⎡ ⎤ −5 0, 01669 1, 054 · 10 ⎢ ⎥⎡ ⎤ ⎢ ⎥ ⎢ −3, 138 · 10−5 0, 006059 ⎥ δm (k) ⎥⎣ ⎦ +⎢ ⎢ ⎥ −7 ⎢ 1, 345 · 10 0, 001376 ⎥ δv (k) ⎣ ⎦ 2, 596 · 10−5 0, 1735 63

0, 01468 0, 01494 0, 9137

⎤⎡

v(k)

⎤

⎥⎢ ⎥ ⎥⎢ ⎥ ⎥ ⎢ α(k) ⎥ ⎥⎢ ⎥+ ⎥⎢ ⎥ ⎥ ⎢ Θ(k) ⎥ ⎦⎣ ⎦ ˙ Θ(k)

(6.1)

ˇ A ´ ZPRESN ˇ ˇ Í ODHADU KAPITOLA 6. MOZN EN

64 ⎡ 0, 992

−0, 000278

⎤⎡ 0, 0155

0, 0009279

⎢ ⎢ ⎢ −0, 6629 0, 9204 −0, 02899 −0, 000314 ⎢ ⎢ xstran (k + 1) = ⎢ −0, 3018 −0, 004838 0, 9889 −0, 0001412 ⎢ ⎢ ⎢ −0, 005229 0, 01499 −0, 0004724 1 ⎣ −0, 002369 −3, 849 · 10−5 0, 01554 −7, 37 · 10−7 ⎤ ⎡ 0, 0002151 0, 001344 ⎥ ⎢ ⎥ ⎢ ⎤ ⎢ 0, 6876 0, 1561 ⎥ ⎡ ⎥ δ (k) ⎢ k ⎥ ⎢ ⎦ + ⎢ 0, 02964 0, 08988 ⎥ ⎣ ⎥ δ (k) ⎢ s ⎥ ⎢ ⎢ 0, 005443 0, 001229 ⎥ ⎦ ⎣ 0, 0002365 0, 0007048

0

⎥⎢ ⎥⎢ 0 ⎥⎢ ⎥⎢ ⎥⎢ 0 ⎥⎢ ⎥⎢ ⎥⎢ 0 ⎥⎢ ⎦⎣ 1

β(k)

⎤

⎥ ⎥ ˙ Φ(k) ⎥ ⎥ ⎥ ˙ Ψ(k) ⎥ + ⎥ ⎥ Φ(k) ⎥ ⎦ Ψ(k)

(6.2)

Vid´ıme, ˇze celkov´ y model je sloˇzen ze dvou d´ılˇc´ıch. Prvn´ı z nich popisuje podéln´ y pohyb letadla. Zde stavová veliˇcina v znaˇc´ı rychlost letu (nab´ıhaj´ıc´ıho vzduchu) letadla a α znaˇc´ı u ´hel nábˇehu letadla, vstupn´ı veliˇcina δm odpov´ıdá ovládán´ı pˇr´ıpusti motoru a δv odpov´ıdá nastaven´ı v´ yˇskového kormidla letadla. Druh´ y d´ılˇc´ı model popisuje stranov´ y pohyb letadla. Zde stavová veliˇcina β znaˇc´ı u ´hel vyboˇcen´ı letadla, vstupn´ı veliˇcina δk odpov´ıdá nastaven´ı kˇridélek a δs odpov´ıdá nastaven´ı smˇerového kormidla letadla. Oba modely jsou diskretizované s vzorkovac´ı frekvenc´ı 64 Hz.

Jelikoˇz stavy v modelu popsaném rovnicemi (6.1) a (6.2) jsou mimo jiné i polohové u ´hly, nab´ız´ı se jejich zaˇclenˇen´ı do stavov´ ych rovnic (4.11) v algoritmu Kalmanova filtru nam´ısto rovnic, které obsahuj´ı mˇeˇren´ı gyroskopy. Rovnice mˇeˇren´ı (4.12) potom mohou z˚ ustat nezmˇenˇeny a mohou b´ yt opˇet pouˇzity ke korekci odhadu z´ıskaného pomoc´ı dynamického modelu.

Vyvstává zde vˇsak problém, ˇze v modelu popsaném rovnicemi (6.1) a (6.2) je poloha letadla urˇcována v podobˇe Eulerov´ ych u ´hl˚ u, kdeˇzto rovnice mˇeˇren´ı (4.12) algoritmu Kalmanova filtru pouˇz´ıvané v této práci pracuj´ı s polohou, která je urˇcena pomoc´ı quaternion˚ u. Pro správnou funkˇcnost algoritmu Kalmanova filtru je tedy nuté zajistit konzistenci stavov´ ych rovnic s rovnicemi mˇeˇren´ı. Jednoduˇse to lze provést tak, ˇze simulace modelu se bude provádˇet jeˇstˇe pˇred zpracováván´ım stavov´ ych rovnic v algoritmu Kal-

65 manova filtru a následnˇe se Eulerovy u ´hly, které jsou v´ ystupem simulace modelu letadla, pˇrevedou na quaterniony. Ty se potom zahrnou do novˇe vytvoˇren´ ych stavov´ ych rovnic algoritmu Kalmanova filtru. Takto vytvoˇrené stavové rovnice potom budou vypadat následovnˇe. ⎡

⎤

q + B0 ⎢ 0e ⎥ ⎢ ⎥ ⎢ q1e + B1 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ q2e + B2 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ q3e + B3 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ B0 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ B1 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ B2 ⎥ x(k + 1) = ⎢ ⎢ ⎥ ⎢ ⎥ B3 ⎢ ⎥ ⎢ ⎥ ⎢ Bax ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ Bay ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ Baz ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ BHx ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ BHy ⎥ ⎣ ⎦ BHz Zde q0e aˇz q4e jsou quaterniony z´ıskané konverz´ı Eulerov´ ych u ´hl˚ u, které jsou v´ ystupy simulace dynamického modelu letadla. B0 aˇz B4 vyjadˇruj´ı aditivn´ı chyby, kter´ ymi algoritmus m˚ uˇze podchytit r˚ uzné ruˇsivé vlivy, se kter´ ymi dynamick´ y model letadla nepoˇc´ıtá, jako jsou napˇr´ıklad poryvy vˇetru.

Nyn´ı pˇristoup´ıme k ovˇeˇren´ı funkce takto navrˇzeného algoritmu. Chyby akcelerometr˚ u a magnetometr˚ u a dalˇs´ı ruˇsivé vlivy, vˇcetnˇe jejich nastaven´ı a velikost´ı, budeme uvaˇzovat stejné, jako v sekci 5.1. Jako jedin´ y ruˇsiv´ y vliv t´ ykaj´ıc´ı se dynamického modelu zde budeme pro jednoduchost uvaˇzovat aditivn´ı chybu, která pˇredstavuje chybné urˇcen´ı polohov´ ych u ´hl˚ u, coˇz m˚ uˇze b´ yt napˇr´ıklad d˚ usledkem poryvu vˇetru. Tato chyba je patrná na obrázku 6.1, kde se vˇsak o poryv vˇetru nejdená, ale jedná se sp´ıˇse o vnitˇrn´ı chybu modelu ˇci o jeho chybné vstupy.


66

Jako prvn´ı je tˇreba inicializovat algoritmus Kamlanova filtru. Poˇcáteˇcn´ı stav bude tedy vypadat následovnˇe.

T x (0) = 1 0 0 0 0 0 0 0 Bax Bay Baz BHx BHy BHz

Dále je tˇreba urˇcit poˇcáteˇcn´ı hodnotu kovarianˇcn´ı matice chyby apriorn´ıho odhadu stavu P(0). Tu urˇc´ıme jednoduˇse jako jednotkovou matici o velikosti 14 × 14. Jako posledn´ı urˇc´ıme kovarianˇcn´ı matice ˇsumu procesu Q a ˇsumu mˇeˇren´ı R. Cyklickou optimalizac´ı se jako nejlepˇs´ı ukázalo následuj´ıc´ı nastaven´ı. Obˇe matice jsou diagonáln´ı.

2 2 2 2 2 2 2 2 2 2 2 2 2 2 Q = diag 10 10 10 10 3 3 3 3 5 5 5 2 2 2

2 2 2 2 2 2 2 R = diag 0.00001 100 100 100 100 100 100

Algoritmus bude dále pracovat dle obrázku 2.7, kde vyuˇz´ıvá rovnic (2.18) aˇz (2.24). Skuteˇcné hodnoty polohov´ ych u ´hl˚ u, vˇcetnˇe pˇr´ısluˇsn´ ych vstup˚ u dynamického modelu letadla, byly vytvoˇreny pˇredem. V´ ysledek ˇcinnosti algoritmu je potom vidˇet na obrázku 6.1. Zde je vidˇet, ˇze algoritmus dokázal efektivnˇe zkombinovat poˇskozená data ze senzor˚ u a v´ ystupn´ı data dynamického modelu letadla. V odhadnut´ ych hodnotách se potom témˇeˇr neprojevuj´ı chyby akcelerometr˚ u a magnetometr˚ u, stejnˇe tak se zde neprojevuje konstantn´ı chyba v´ ystupn´ıch dat dynamického modelu. Odhadnuté hodnoty se potom od skuteˇcn´ ych pˇr´ıliˇs neliˇs´ı, pouze je zde patrnˇejˇs´ı ˇsum, kter´ y se algoritmem nepodaˇrilo odstranit.

Závˇerem této kapitoly je jeˇstˇe nutno dodat, ˇze metoda uvedená v této kapitole pouze nastiˇ nuje dalˇs´ı moˇznost odhadu polohov´ ych u ´hl˚ u pomoc´ı algoritmu Kalmanova filtru. Tato metoda nebyla testována na ˇza´dn´ ych reáln´ ych datech nebo v reálném provozu a oproti metodˇe pouˇz´ıvané v pˇredchoz´ıch kapitolách pˇredstavuje vyˇsˇs´ı v´ ypoˇcetn´ı nároˇcnost a dosaˇzené v´ ysledky nejsou pˇri porovnán´ı s v´ ysledky dosaˇzen´ ymi v sekci 5.1 lepˇs´ı.

67 Φ

uhel [rad]

0.5 0 −0.5 −1 0

10

20

30 cas [s] Θ

40

vystup dynamickeho modelu 50 60 odhad skutecnost

uhel [rad]

0.5

0

−0.5 0

10

20

30 cas [s] Ψ

40

50

60

10

20

30 cas [s]

40

50

60

uhel [rad]

2

1.5

1 0

Obr´ azek 6.1: Srovn´ an´ı polohov´ ych u ´hl˚ u skuteˇcn´ ych, odhadnut´ ych a stanoven´ ych dynamick´ ym modelem

Nicménˇe tato metoda má jistˇe velké moˇznosti dalˇs´ıho vylepˇsován´ı. Napˇr´ıklad pokud by byl k dispozici model letadla pracuj´ıc´ı s quaterniony nebo pokud by rovnice mˇeˇren´ı algoritmu Kalmanova filtru byly v´ıce konzistentn´ı se stavy modelu letadla, mohly by b´ yt stavové rovnice algoritmu Kalmanova filtru sestaveny pˇr´ımo bez nutnosti separátn´ı simulace, coˇz by pˇrineslo i pˇresnˇeˇs´ı v´ ypoˇcty uvnitˇr algoritmu.

68


Kapitola 7 Z´ avˇ er C´ılem této práce bylo navrhnout vhodn´ y algoritmus pro odhad polohov´ ych u ´hl˚ u bezpilotn´ıho vzduˇsného prostˇredku. Velice vhodn´ ym nástojem v tomto pˇr´ıpadˇe se ukázal b´ yt Kalman˚ uv filtr, respektive rozˇs´ıˇren´ y Kalman˚ uv filtr, kter´ y pracuje s nelineárn´ım popisem systému. Do popisu systému, kter´ y vyuˇz´ıval algoritmus rozˇs´ıˇreného Kalmanova filtru, byl zahrnut i model ˇcinnosti jednotliv´ ych senzor˚ u, které byly vyuˇzity pro urˇcován´ı polohov´ ych u ´hl˚ u. Tˇemito senzory jsou gyroskopy meˇr´ıc´ı u ´hlové rychlosti, akcelerometry ve funkci sklonomˇeru a magnetometry mˇeˇr´ıc´ı vektor intenzity magnetického pole Zemˇe ve vˇsech tˇrech osách.

Odhad polohov´ ych u ´hl˚ u prob´ıhá ve formˇe quaternion˚ u, jelikoˇz quaterniony nab´ızej´ı v´ ypoˇcetn´ı v´ yhody oproti pˇr´ımému odhadu polohov´ ych u ´hl˚ u. V dneˇsn´ı dobˇe jiˇz nen´ı v pˇr´ıpadˇe potˇreby problém pˇrevody mezi quaterniony a polohov´ ymi u ´hly provádˇet za bˇehu.

Na umˇele vytvoˇren´ ych datech bylo ukázáno, ˇze navrˇzen´ y algoritmus rozˇs´ıˇreného Kalmnanova filtru dokázal s pomoc´ı modelu ˇcinnosti senzor˚ u velmi efektivnˇe zkombinovat poˇskozené informace z jednotliv´ ych senzor˚ u a pomˇernˇe pˇresnˇe dokázal zrekonstruovat polohové u ´hly (ve formˇe quternion˚ u) a stejnˇe tak dokázal velmi pˇresnˇe odhadnout aditivn´ı chyby senzor˚ u.

69

´ ER ˇ KAPITOLA 7. ZAV

70

Algoritmus byl dále otestován na reáln´ ych letov´ ych datech. V´ ysledek odhadu algoritmu rozˇs´ıˇreného Kalmanova filtru byl poté porovnán s polohov´ ymi u ´hly, které byly mˇeˇreny bˇehem letu. V tomto pˇr´ıpadˇe bylo ukázáno zlepˇsen´ı ve formˇe odstranˇen´ı ˇcasového driftu, kter´ ym byla zat´ıˇzena mˇeˇrená data.

Na závˇer byla jˇeˇstˇe otestována moˇznost zaˇclenˇen´ı dynamického modelu do algoritmu rozˇs´ıˇreného Kalmanova filtru. Zde algoritmus dokázal efektivnˇe zkombinovat chybami zat´ıˇzená data z akcelerometr˚ u a magnetometr˚ u s daty poskytnut´ ymi dynamick´ ym modelem letadla zat´ıˇzen´ ymi aditivn´ımi chybami.

Algoritmy uvedené na pˇriloˇzeném CD byly konstruovány s ohledem na jejich pˇr´ıpadnou implementaci, proto obsahuj´ı minimum sloˇzit´ ych funkc´ı prostˇred´ı MATLAB a veˇskeré v´ ypoˇcty, vˇcetnˇe jakobián˚ u, jsou psány manuálnˇe.

Literatura EnviWeb

s.r.o.

(2008),

EnviWeb

[online].

Posledn´ı revize

2008-07-24,

http://www.enviweb.cz/. Feynman, R. P., Leighton, R. B. a Sands, M. (2000), Feynmanovy pˇredn´ aˇsky z fyziky, Havl´ıˇck˚ uv Brod: FRAGMENT. ISBN 80-7200-405-0. ˇ ˇ Havlena, V. a Stecha, J. (2000), Modern´ı teorie ˇr´ızen´ı, Praha: Vydavatelstv´ı CVUT. ISBN 80-01-02095-9. Kumar, N. S. a Tann, T. (2004), Estimation of attitudes from a low-cost miniaturized inertial platform using Kalman Filter-based sensor fusion algorithm. Lachnit, Z. (2007), Inerci´ aln´ı sn´ımaˇce pro zpˇresˇ nován´ı odometrie mobiln´ıch robot˚ u – Bakaláˇrsk´ a pr´ ace, Brno. ˇmec, M. (2005), Mˇeˇren´ı stejnosmˇerných a stˇr´ıdavých magnetických pol´ı – Bakaláˇrsk´ Ne a pr´ ace, Plzeˇ n. ´, J. (2006), Mˇeˇren´ı magnetického pole Zemˇe. Perny Titterton, D. H. a Weston, J. L. (2004), Strapdown inertial navigation technology, Stevenage: Institution of Electrical Engineers. ISBN 0-86341-358-7. ˇ ˇ a Hanza ´lek, Z. (2010), RAMA UAV Control System [online]. Spinka, O., Kroupa, S. Posledn´ı revize 07/01/2010, http://rtime.felk.cvut.cz/helicopter/. ˇ a ć ˇ, M. (2008), N´ Rez avrh ˇr´ızen´ı pro systém stabilizace optické osy kamerového systému pro bezpilotn´ı letoun – Diplomová pr´ ace, Praha.

71

72

LITERATURA

Pˇ r´ıloha A Skripty gener´ atoru dat A.1

Transformace Eulerov´ ych u ´ hl˚ u na u ´ hlov´ e rychlosti

function [pqr] = eul2pqr(dPhi, dTheta, dPsi, Phi, Theta) %EUL2PQR − euler angles to angular rates p, q, r conversion %eul2pqr(dPhi, dTheta, dPsi, Phi, Theta) R = [1 0 −sin(Theta); 0 cos(Phi) sin(Phi)*cos(Theta); 0 −sin(Phi) cos(Phi)*cos(Theta)];

pqr = R*[dPhi; dTheta; dPsi];

end

A.2

Transformace vektoru z navigaˇ cn´ı do tˇ elesov´ e souˇ radn´ e soustavy

function [AxAyAz] = geo2bdy(Phi, Theta, Psi, x, y, z)

I

II

ˇ ÍLOHA A. SKRIPTY GENERATORU ´ PR DAT

%GEO2BDY converts vector [x;y;z] between two frames %

geo2bdy( Phi, Theta, Psi, x, y, z )

Q = [cos(Phi/2)*cos(Theta/2)*cos(Psi/2)+sin(Phi/2)*sin(Theta/2)*sin(Psi/2) sin(Phi/2)*cos(Theta/2)*cos(Psi/2)−cos(Phi/2)*sin(Theta/2)*sin(Psi/2) cos(Phi/2)*sin(Theta/2)*cos(Psi/2)+sin(Phi/2)*cos(Theta/2)*sin(Psi/2) cos(Phi/2)*cos(Theta/2)*sin(Psi/2)−sin(Phi/2)*sin(Theta/2)*cos(Psi/2)];

AxAyAz = [Q(1)ˆ2+Q(2)ˆ2−Q(3)ˆ2−Q(4)ˆ2, 2*(Q(1)*Q(4)+Q(2)*Q(3)), 2*(Q(2)*Q(4)−Q(1)*Q(3)); 2*(Q(2)*Q(3)−Q(1)*Q(4)), Q(1)ˆ2−Q(2)ˆ2+Q(3)ˆ2−Q(4)ˆ2, 2*(Q(1)*Q(2)+Q(3)*Q(4)); 2*(Q(1)*Q(3)+Q(2)*Q(4)), 2*(Q(3)*Q(4)−Q(1)*Q(2)), Q(1)ˆ2−Q(2)ˆ2−Q(3)ˆ2+Q(4)ˆ2] * [x; y; z];

end

Pˇ r´ıloha B Skripty senzorick´ eho modelu B.1

Misalignment error

function [ out ] = misalig( fx, fy, fz, Mf ) % misalig( ax, ay, az, Maxyz ) %

misalignment of sensors

out = Mf*[fx; fy; fz];

end

B.2

Temperature drift

function [ out ] = Temp drift( fx, fy, fz, T, t11, t12, t13, t14, t21, t22, t23, t24, t31, t32, t33, t34 ) % Temp drift pqr( fx, fy, fz, T, t11, t12, t13, t14, % %

t21, t22, t23, t24, t31, t32, t33, t34 ) Temperature drift of sensors

out = [fx; fy; fz] + [t11*Tˆ3+t12*Tˆ2+t13*T+t14; t21*Tˆ3+t22*Tˆ2+t23*T+t24;

III

ˇ ÍLOHA B. SKRIPTY SENZORICKEHO ´ PR MODELU

IV

t31*Tˆ3+t32*Tˆ2+t33*T+t34];

end

B.3

Bias

function [ out ] = Sensor bias( fx, fy, fz, Bx, By, Bz ) % Sensor bias pqr( fx, fy, fz, Bx, By, Bz ) %

Sensor bias

out = [fx; fy; fz] + [Bx; By; Bz];

end

B.4

Chyba polohy akcelerometr˚ u mimo tˇ eˇ ziˇ stˇ e

function [ out ] = CG Offset( p, q, r, Xax, Yay, Zaz ) % Center of gravity offset %

CG Offset( p, q, r, Xax, Yay, Zaz )

out = [(qˆ2+rˆ2)*Xax; (pˆ2+rˆ2)*Yay; (pˆ2+qˆ2)*Zaz];

end

Pˇ r´ıloha C Obsah pˇ riloˇ zen´ eho CD K této práci je pˇriloˇzeno CD, na kterém jsou uloˇzeny následuj´ıc´ı soubory: tento dokument ve form´ atu PDF, n´ asleduj´ıc´ı skripty programu MATLAB a modely prostˇred´ı Simulink pouˇz´ıvané v

této práci: – kalman angles.m vykonávaj´ıc´ı algoritmus Kalmanova filtru, – kalman angles model.m vykonávaj´ıc´ı algoritmus Kalmanova filtru zahrnuj´ıc´ı dynamick´ y model letadla, – test generator.mdl obsahuj´ıc´ı generátor dat vˇcetnˇe senzorického modelu, – dalˇs´ı podp˚ urné skritpy, modely a datové soubory.

V

Fakulta elektrotechnická. Odhad orientace UAV v prostoru

Recommend Documents