Studie av integration mellan rategyron och magnetkompass

Studie av integration mellan rategyron och magnetkompass

Examensarbete utfört i Reglerteknik vid Linköpings tekniska högskola

av Sara Nilsson LITH-ISY-EX-3521-2004

Examinator: Anders Helmersson

Handledare: Mikael Axelsson

Hans Bohlin

Avdelning, Institution Division, Department Institutionen för systemteknik 581 83 LINKÖPING Datum Date 2004-10-11 Språk Language Rapporttyp Report category ISBN X Svenska/Swedish Engelska/English Licentiatavhandling

X Examensarbete ISRN LITH-ISY-EX-3521-2004

C-uppsats

D-uppsats Serietitel och serienummer

Title of series, numbering

ISSN

Övrig rapport ____

URL för elektronisk version

http://www.ep.liu.se/exjobb/isy/2004/3521/

Titel

Title

Studie av integration mellan rategyron och magnetkompass Study of sensor fusion of rategyros and magnetometers

Författare

Author

Sara Nilsson

Sammanfattning

Abstract

This master thesis is a study on how a rategyro triad, an accelerometer triad, and a magnetometer triad can be integrated into a navigation system, estimating a vehicle’s attitude, i.e. its roll, tipp, and heading angles.

When only a rategyro triad is used to estimate a vehicle’s attitude, a drift in the attitude occurs due to sensor errors. When an accelerometer triad and a magnetometer triad are used, an error in the vehicle’s heading, appearing as a sine curve, depending on the heading, occurs. By integrating these sensor triads, the sensor errors have been estimated with a filter to improve the estimated attitude’s accuracy.

To investigate and evaluate the navigation system, a simulation model has been developed in Simulink/Matlab. The implementation has been made using a Kalman filter where the sensor fusion takes place. Simulations for different scenarios have been made and the results from these simulations show that the drift in the vehicle’s attitude is avoided.

ABSTRACT

This master thesis is a study on how a rategyro triad, an accelerometer triad, and a magnetometer triad can be integrated into a navigation system, estimating a vehicle’s attitude, i.e. its roll, tipp, and heading angles.

When only a rategyro triad is used to estimate a vehicle’s attitude, a drift in the attitude occurs due to sensor errors. When an accelerometer triad and a magnetometer triad are used, an error in the vehicle’s heading, appearing as a sine curve, depending on the heading, occurs. By integrating these sensor triads, the sensor errors have been estimated with a filter to improve the estimated attitude’s accuracy.

To investigate and evaluate the navigation system, a simulation model has been developed in Simulink/Matlab. The implementation has been made using a Kalman filter where the sensor fusion takes place. Simulations for different scenarios have been made and the results from these simulations show that the drift in the vehicle’s attitude is avoided.

Keywords: Navigation, Estimation, Kalman filtering, Rategyro triad, Accelerometer triad, Magnetometer triad

SAMMANFATTNING

Examensarbetet studerar hur en rategyrotriad, en accelerometertriad och en magnetometertriad kan integreras för att utveckla ett navigeringssystem som estimerar ett fordons attityd, dvs. dess roll-, tipp- och kursvinkel.

Då endast en rategyrotriad används för att skatta fordonets attityd uppstår drift i denna på grund av sensorfel. När endast en accelerometer- och magnetometertriad används uppstår ett fel i fordonets kursvinkel som har formen av en sinuskurva som beror av kursen. Genom att integrera dessa sensorer kan sensorfelen estimeras med hjälp av ett filter för att förbättra noggrannheten i den skattade attityden.

För att undersöka och utvärdera detta navigeringssystem har en simuleringsmodell utvecklats i Simulink/Matlab. Implementeringen har gjorts med ett kalmanfilter där sensorfusion sker. Simuleringar har gjorts för olika scenarier och resultaten av dessa visar att attityddriften undviks.

Nyckelord: Navigering, Estimering, Kalmanfiltrering, Rategyrotriad,

INNEHÅLL

1 Inledning... 1 1.1 Bakgrund ... 1 1.2 Syfte... 1 1.3 Problemdefinition ... 1 1.4 Metod... 2 1.5 Avgränsningar ... 2 1.6 Rapportens disposition ... 2 2 Navigering ... 4

2.1 Definition av begreppet navigering ... 4

2.1.1 Navigering ... 4

2.1.2 Styrning ... 4

2.1.3 Guidance... 4

2.2 Jordmodeller ... 5

2.2.1 Jordens geometriska form... 5

2.2.2 Jordens rotation ... 9 2.2.3 Jordens gravitation... 10 2.2.4 Jordens magnetfält... 11 2.3 Omvärldsmodell ... 15 2.3.1 Terrängmodell ... 15 2.3.2 Färdväg ... 15 2.3.3 Utdata ... 16 3 Sensorer ... 17 3.1 Allmänt om sensorer... 17 3.1.1 Rategyron ... 17 3.1.2 Accelerometrar ... 17 3.1.3 Magnetometrar ... 18 3.1.4 Sensorfel ... 18 3.2 Rategyrotriaden ... 20

3.2.1 Modellering av fordonets vinkelhastighet ... 20

3.2.2 Modellering av rategyrotriaden ... 21

3.3 Accelerometertriaden... 22

3.3.1 Modellering av fordonets acceleration ... 22

3.3.2 Modellering av accelerometertriaden ... 22 3.4 Magnetometertriaden... 23 3.4.1 Magnetisk deviation ... 23 3.4.2 Modellering av magnetfält... 25 3.4.3 Modellering av magnetometertriaden... 26 4 Sensorfusion ... 27 4.1 Kapitelöversikt ... 27 4.2 Attitydberäkningar... 28 4.2.1 Rategyrotriadens attitydberäkningar... 28

4.2.2 Accelerometer- och magnetometertriadens attitydberäkningar. 30 4.3 Definition av attitydfel... 32 4.4 Felmodeller för attityd... 34 4.4.1 Rategyrotriaden ... 34 4.4.2 Accelerometertriaden... 36 4.4.3 Magnetometertriaden... 38 4.4.4 Slutlig tillståndsmodell ... 41 4.5 Kalmanfiltrering ... 44 4.5.1 Tillståndsmodell ... 44 4.5.2 Sampling av tillståndsmodellen... 45 4.5.3 Kalmanfiltret... 45 4.5.4 Extended kalmanfilter... 47 4.6 Felkompensering ... 48 4.6.1 Metod... 48 4.6.2 Gyrodrift ... 49 4.6.3 Korrigering av attityden... 52

5 Simuleringar och resultat... 54

5.1 Simuleringsmodell... 54

5.2 Simuleringar ... 56

5.2.1 MonteCarlo simulering... 56

5.2.2 Studerade felvariabler... 56

5.2.3 Scenarier ... 58

5.2.4 Sensorfel och initialvärden ... 60

5.3 Simuleringsresultat ... 60

5.3.1 Återkoppling och kompensering ... 61

5.3.2 Variering av rategyrobias och rategyrobrus ... 63

5.3.3 Variering av magnetometerbias... 68

5.3.4 Estimering av fordonets acceleration... 73

5.3.5 Simuleringsresultat från olika scenarier ... 79

6 Slutsatser ... 97

6.1 Resultat och slutsatser ... 97

6.1.1 Simuleringsmodell... 97

6.1.2 Resultat ... 97

6.2 Förslag på fortsatt arbete ... 98

6.2.1 Sensorfel ... 98

6.2.2 Transporthastighet ... 99

6.2.3 Magnetisk kurs ... 99

6.2.4 Scenarier ... 99

6.2.5 Kalmanfiltrering ... 100

A Notation och beteckningar... 103

A.1 Referenskoordinatsystem... 103

A.2 Fysikaliska konstanter ... 103

A.3 Kinematik ... 104

B Referenskoordinatsystem ... 106

B.1 Earth Centered Inertial Frame ... 106

B.2 Earth Centered Earth Fixed Frame ... 106

B.3 Geodetic Frame ... 107

B.4 Local Level Frame... 108

B.5 Chassi Frame ... 108 B.6 Sensorplattform ... 110 B.7 Rategyrotriad ... 110 B.8 Accelerometertriad ... 110 B.9 Magnetometertriad ... 111 C Vektortransformationer ... 112 C.1 Skevoperatorn... 112

1 INLEDNING

I detta kapitel beskrivs bakgrund och syfte med examensarbetet. Här återges även problemdefinition samt metod och avgränsningar. I slutet av kapitlet finns en beskrivning av rapportens disposition.

1.1 Bakgrund

På AerotechTelub AB i Linköping bedrivs en studie gällande enkla och robusta navigeringssystem för bland annat soldater och stridsfordon. Inom ramen för denna studie skall bland annat möjligheter och begränsningar vid integration mellan magnetometrar, accelerometrar och rategyron studeras. Tidigare har förinställningar av fordons navigeringssystem studerats men då detta ofta är en tidskrävande process finns ett behov av en bättre metod.

1.2 Syfte

Syftet med examensarbetet är att undersöka hur integrationen mellan nämnda sensorer kan göras, vilka tekniker som kan användas samt vilka resultat som kan uppnås. Resultaten av integrationen skall även jämföras med vilken prestanda som uppnås då endast en rategyrotriad respektive en accelerometer- och en magnetometertriad används.

1.3 Problemdefinition

Uppgiften syftar till att utveckla en simuleringsmodell och ett filter för applikationen navigering genom att integrera en rategyrotriad, en accelerometertriad samt en magnetometertriad.

För att kunna modellera detta system ska först en modell av jordens geometriska form specificeras och modelleras, varefter modeller av jordens rotation, gravitation och magnetfält kan studeras. Vidare ska en rategyrotriad, en accelerometertriad samt en magnetometertriad modelleras med sensorfel i form av vitt brus och bias.

Simuleringmodellen ska utvecklas i Matlab och Simulink. Efter simuleringsmodellen implementerats ska erhållen prestanda samt egenskaper för det framtagna filtret och mekaniseringsmodellen studeras genom simulering. Dessa resultat skall jämföras med användning av endast rategyron respektive magnetkompass.

1.4 Metod

Efter en litteraturstudie härleds de samband som används för att beräkna fordonets attityd med mätningarna från rategyrotriaden samt med accelerometer- och magnetometertriaden och därefter studeras de fel som är av intresse. Utifrån detta härleds felmodeller för hur felen fortplantas i systemet och en tillståndsmodell för detta ställs upp.

För att dämpa feltillväxten studeras även vilka fel som kan återkopplas för att reglera systemet. Felen estimeras med hjälp av ett extended kalmanfilter. Simulering och verifiering av modellen utförs slutligen i Matlab och Simulink.

1.5 Avgränsningar

Det här examensarbetet begränsas till att studera ett fordon som färdas på en liten yta i en platt värld, vilket medför att jordens transporthastighet kan försummas. Ingen uppdatering av fordonets position kommer att ske och en följd av detta är att jordens gravitation, jordens primära magnetfält samt jordens rotationshastighet på den aktuella positionen endast behöver beräknas vid initiering av systemet. Detta på grund av att dessa värdens förändringar är försumbara när fordonet färdas på en liten yta. Examensarbetet begränsas vidare till att endast studera en platt lokal värld, dvs. en värld där inga berg och dalar existerar utan ytan är platt vilket medför att fordonets roll- och tippvinkel alltid är noll.

En ytterligare begränsning är antagandet att magnetiskt norr sammanfaller med geografiskt norr, vilket medför att deklinationen är noll och således inte behöver tas hänsyn till. En modell för det jordmagnetiska fältet ska ändå implementeras men jordens magnetfält i y-led sätts därefter till noll.

I examensarbetet antas vidare att fordonets position och hastighet är givna observationer till exempel från en GPS-mottagare. Dessa behöver inte modelleras utan antas vara oberoende observationer kontaminerade av brus.

1.6 Rapportens

disposition

I kapitel 2 definieras navigering som är ett centralt begrepp i det här examensarbetet. Där definieras även de jordmodeller som används i examensarbetet, dvs. modeller av jordens geometriska form, jordens rotation, jordens gravitation samt jordens magnetfält. I slutet av kapitlet finns information om den omvärldsmodell som används vid simuleringarna.

I kapitel 3 beskrivs de sensorer som används i examensarbetet, dvs. rategyron, accelerometrar och magnetometrar. Där beskrivs även vilka signaler dessa sensorer mäter samt vilka typer av sensorfel dessa signaler är behäftade med. I första delen av kapitel 4 återfinns de attitydberäkningar som gjorts på utsignalerna från rategyrotriaden respektive accelerometer- och magnetometertriaden. Därefter definieras attitydfel och felmodeller för de båda beräknade attityderna härleds och sätts samman till en slutgiltig tillståndsmodell. I slutet av kapitlet beskrivs den allmänna tillståndsformen samt kalmanfiltret, extended kalmanfilter och observerbarhetskriterium för dessa båda.

I kapitel 5 beskrivs vilka simuleringar som gjorts. Där beskrivs även de olika scenarier som simulerats samt vilka värden på observationsbrus och processbrus i kalmanfiltret som är lämpliga. Även storleksordningen på sensorfelen återges här.

I kapitel 6 redovisas de resultat som uppnås samt vilka slutsatser som dragits utifrån dessa. I detta kapitel återfinns även förslag på fortsatt arbete.

I bilaga A beskrivs den notation samt de beteckningar som används i examensarbetet. Bilaga B definierar de koordinatsystem som används samt samband mellan dessa och i bilaga C beskrivs de vektortransformationer som behöver närmare förklaring.

2 NAVIGERING

I detta kapitel definieras navigering, som är ett grundläggande begrepp i det här examensarbetet. Därefter presenteras de jordmodeller som används och slutligen förklaras även den omvärldsmodell som används vid simuleringen.

2.1

Definition av begreppet navigering

Begreppen navigera och navigering innebär i dagligt tal förmågan att veta var man är och hur man ska röra sig för att komma till rätt plats. Det innebär bland annat att storheter som position, riktning och hastighet används. Vanligtvis bryts det allmänna begreppet navigering ner i tre nya begrepp - navigering, styrning och guidance. För mer information om navigering hänvisas till [1].

2.1.1 Navigering

Navigering innebär att endast det egna fordonets position, hastighet och attityd estimeras. Även storheter som acceleration och vinkelhastighet räknas som navigeringsstorheter.

2.1.2 Styrning

Styrning innebär att fordonet påverkas av yttre krafter på ett kontrollerat sätt för att det ska röra sig i en viss riktning eller anta ett nytt läge. Utan kunskap om farkostens navigeringsstorheter som position och hastighet är det omöjligt att styra farkosten.

2.1.3 Guidance

Guidance, som är ungefär detsamma som manövrering, innebär att alla metoder som kan användas för att komma fram till hur farkosten ska styras för att komma till önskad position används. För att åstadkomma detta måste både kunskaper om farkostens navigering och styrning användas. Storheter som avstånd och riktning till destination samt avstånd till önskat spår mellan två brytpunkter är exempel på guidancestorheter.

För att entydigt kunna ange och tolka navigations- och guidancestorheter behöver olika referenser att relatera till anges. Navigering av rymdfarkoster, så kallad celestial navigering, har normalt solsystemet som referens. Vid navigering av ett fordon på jorden, så kallad terrestrial navigering, används vanligtvis jorden som referens.

2.2 Jordmodeller

En modell av jorden innebär en matematisk modell av jordens geometri samt dess rotation, gravitationsfält och magnetfält. För att kunna specificera de tre sistnämnda är det nödvändigt att först specificera en modell av jordens geometriska form. Denna behövs även för att riktningar och positioner ska kunna beskrivas entydigt. Jordmodeller ligger alltid till grund för terrestial navigering.

2.2.1 Jordens

geometriska

form

Enligt [1] har människans syn på jordens form varit mycket varierande genom tiderna. För mycket länge sedan ansågs jorden vara platt, därefter sfärisk och idag finns mycket noggranna observationer av jordens form, en våggeodid med ojämn massfördelning. I tabell 2.1 nedan beskrivs de historiska modellerna av jordens geometriska form.

Tabell 2.1 Historiska modeller av jordens form, [1].

År Modell Person/Civilisation Utvecklingsmetod

> 1000

f.Kr. Platt förtida Babyloniern m.fl. Felaktig förståelse av dagliga observationer 900

f.Kr.

Konvex disk Babyloniern Observationer av

skepp som försvinner under horisonten 300

f.Kr. Sfäriskt klot Egypten Triangulering solen mot (Jorden ca. 15% för

stor) 1687 Ellipsoid Isaac Newton,

England Mätningar på gravitations potentialer mm (felaktig

avplattningsfaktor ca 20%)

1958 Svagt päronformad ellipsoid Ann Balie, USA Satellitmätningar

1960 Våggeodid Desmond G.

King-Hele

England m.fl.

Satellitmätningar

- 1997 Dynamisk våggeodid med ojämn massfördelning

Sam C. Bose, USA m.fl.

Satellitmätningar och flygmätningar

I det här examensarbetet används en ellipsoid modell av jordens form. Eftersom denna modell är en relativt grov förenkling av jordens form ger den upphov till relativt stora geometriska modellfel i vertikala positioner, vilket är en nackdel. En ellipsoidmodell modellerar jordens geometriska form som en något tillplattad sfär. Formen genereras genom att en ellips roteras kring den mindre axeln. Figur 2.1 nedan visar ett tvärsnitt av en ellipsoidisk jordmodell.

Ekvatorplanet Geografiska nordpolen Tangentplan Φ GEO Φ CEN 90ο_{+ Φ} GEO X Y (RA,0) (0,RB) (a,b) Ekvatorplanet Geografiska nordpolen Tangentplan Φ GEO Φ CEN 90ο_{+ Φ} GEO X Y (RA,0) (0,RB) (a,b) Φ GEO Φ CEN 90ο_{+ Φ} GEO X Y (RA,0) (0,RB) (a,b)

Figur 2.1 Tvärsnitt av en ellipsoidisk jordmodell, [1].

I figuren ovan betecknar RA ellipsoidens stora axel, som också representerar

ekvatorns radie, och RB ellipsoidens mindre axel vilket motsvarar avståndet

mellan polerna och ekvatorplanet.

Figur 2.1 visar även skillnaden mellan geodetisk latitud, ФGEO, och geocentrisk

latitud, ФCEN. Geocentrisk latitud är vinkeln mellan ekvatorplanet och en linje

från origo till positionen (a,b). Geodetisk latitud är den latitud som normalt anges då latituden för en position anges. Den definieras som vinkeln mellan ekvatorplanet och normalen till tangentplanet vid positionen (a,b). I fortsättningen kommer geodetisk latitud betecknas med Ф och benämnas med enbart latitud. Longitud betecknas λ.

Låt r vara en ortsvektor från jordens mittpunkt till en godtycklig punkt på ellipsoiden. Då gäller att alla r som tillhör referensellipsoiden uppfyller följande ekvation

1

R

r

R

r

R

r

2 A 2 z 2 A 2 y 2 B 2 x

₊

₊

₌

_{. (2.1)}

Genom att införa variablerna geocentrisk latitud, geodetisk latitud och longitud kan ekvationen skrivas på parametrisk form enligt följande

(2.2)

sinλi

cosΦ

R

sinλ

cosΦ

R

r

cosλ

cosΦ

R

cosλ

cosΦ

R

r

)sinΦ

e

(1

R

sinΦ

R

r

P CEN A z P CEN A y 2 P CEN B x

=

=

=

=

−

=

=

För en ellipsoidmodell som beskriver jordens geometri gäller följande samband

Avplattningsformfaktorn: A B A a

R

R

R

f

=

−

Excentricitetformfaktorn:

f

(2

f

)

R

R

R

e

2 a a A 2 B 2 A 2

₌

−

₌

₋

Principalkurvaturradien:

Φ

=

2 2 A P

sin

e

-1

R

R

Meridiankurvaturradien:

(

2 2

)

32 2 A M

Φ

sin

e

1

)

e

(1

R

R

−

−

=

.

Principalkurvaturradien är krökningsradien för en rörelse i öst- västlig riktning och meridiankurvaturradien är jordradien längs en viss longitud.

Det finns ett stort antal ellipsoidmodeller som beskriver jordens geometri. En av de mest använda modellerna är World Geodetic System 1984 (WGS84), som är en bra global anpassning. Problemet med denna modell är att lokalt kan ellipsoiden avvika med upp till flera hundra meter från geodiden. I Sverige brukar en modell som heter Rikets triangelnät 1990 (RT90) användas. Denna modell är en ellipsoid som lokalt ger bättre anslutning till geodiden och som skiljer sig endast några meter från den sanna geodiden inom Sveriges gränser. Skillnaden mellan lokalt och globalt anpassade ellipsoider illustreras nedan i figur 2.2.

Anpassningsområde Globalt anpassad ellipsoid Lokalt anpassad ellipsoid Geodid

Figur 2.2 Lokal anpassning av ellipsoid jordmodell, [1].

I det här examensarbetet används modellen WGS84. Den har följande parametrar:

0022...

0,00669438

e

22101...

1/298,2572

f

m

31414

,

6356752

R

m

00000

,

6378137

R

2 a B A

=

=

=

=

2.2.2 Jordens

rotation

Jorden roterar som bekant runt sin egen vertikala axel. Det tar ett dygn för jorden att rotera ett helt varv. Det motsvarar ungefär 360 grader på 24 timmar och därmed en vinkelhastighet på 15 grader per timme. Ωe betecknar jordens

rotationshastighet relativt tröghetsrymden och ges av

. (2.3)

rad/s

10

5

7,29211585

Ω

-5 e

=

×

För att uttrycka jordens rotationshastighet behöver två koordinatsystem, ECI (I) och ECEF (E), definieras. ECI och ECEF är båda koordinatsystem som har origo i jordens mittpunkt. Skillnaden mellan dem är att ECI är fix relativt avlägsna stjärnor medan ECEF är fix mot jordens yta. Detaljerad information om koordinatsystemen återfinns i bilaga B.1 och B.2.

För att definiera jordens rotationshastighet används DCM-sambandet mellan ECI och ECEF som definieras i bilaga B. Detta samband definieras som

. (2.4)

( )

( )

( )

( )

I z y x e e e e E z y x

r

r

r

t

Ω

cos

t

Ω

sin

0

t

Ω

sin

t

Ω

cos

0

0

0

1

r

r

r









































−

=





















4

4

4

4

3

4

4

4

4

2

1

E I C

Jordens rotationshastighet kan nu uttryckas som en vektor med hjälp av ECI och ECEF

. (2.5)

E IE e E IE e

0

0

Ω

0

0

Ω





















=

=

⇒





















=

=

I E I IE I e E IE E e

ω

Ω

ω

C

Ω

För mer detaljerad information om jordens rotation hänvisas till [1].

2.2.3 Jordens

gravitation

En modell av jordens gravitation behövs för att beskriva jordens gravitationskraft på olika ställen på jorden. Gravitationskraften som påverkar allt på jorden beror både på centripetalaccelerationen och på massattraktionen från jorden.

I [1] ges en enkel modell av jordens gravitation, som en funktion av latitud och altitud enligt följande

Gravitationen vid ekvatorn: 2

0

9

,

7804

m/s

g

≈

Modell för jordgravitationen:

_











+

−

=

Bsin

Φ

R

h

2A

1

g

g

2 A 0











=

G ,

g











g

0

0

Konstanter: A=1+fa +k,

k

f

a

2

5 −

=

B

,

GM

R

R

2 _B A 2 e

Ω

=

k

.

De övriga variablerna som ingår är:

fa Avplattningsformfaktorn, se avsnitt 2.2.1.

h Altitud eller höjd över referensellipsoiden, se figur a.2 i bilaga B.3. Ωe Jordens rotationshastighet, se avsnitt 2.2.2.

M Jordens massa, ≈ 5,977 ·1023 _kg

G Gravitationskonstanten, ≈ 6,670 · 10-11 _Nm2_/kg2

RA, RB Kurvaturradier, se avsnitt 2.2.1.

Ф Latitud, se avsnitt 2.2.1. G

g

betecknar gravitationen uttryckt i koordinatsystemet GEO, som finns närmare beskrivet i bilaga B.3.

Enligt gravitationsmodellen minskar gravitationen när altituden ökar och ökar när latituden ökar. Formeln är lämplig att använda på lägre altituder exempelvis för markgående fordon och flygplan, men gäller inte för en farkost långt från jorden, exempelvis en satellit.

2.2.4 Jordens

magnetfält

En modell av jordens magnetfält är en teoretisk modell som beskriver magnetfältet med avseende på geografisk position. Jordens magnetfält härrör enligt [2] från flera källor som samverkar på olika sätt. I stort sett kan magnetfältet delas upp i tre delar med olika ursprung. Den största delen av fältet, primärfältet, alstras av elektriska strömmar i jordens yttre kärna som består av flytande järn. De andra två källorna, av mindre styrka, är lokala störningar från strömmar och ferromagnetiska material i jordens mantel samt elektriska strömsystem i jonosfären och magnetosfären. I det här kapitlet ges en modell för jordens primära magnetfält.

Primärfältet, som utgör cirka 90 procent av jordens magnetfält, alstras av elektriska strömmar i jordens yttre kärna av flytande järn. Dessa strömmar fungerar som elektromagneter och producerar ett dipolliknande magnetfält runt jorden. Detta fält varierar sakta med tiden vilket gör att de magnetiska polerna förflyttas kontinuerligt relativt geografiskt norr. För att beskriva primärfältet och dess förändring finns två globala modeller, World Magnetic Model (WMM) och International Geomagnetic Reference Field (IGRF), som modelleras i det här examensarbetet och beskrivs nedan.

Under antagandet att atmosfären är icke magnetisk i en region nära jordens yta kan magnetfältet enligt [2] beskrivas som den negativa gradienten av den skalära potentialen V enligt

(

r,θ,λ,t

)

V −∇ =

som satisfierar Laplace ekvation

(

r,

θ,

λ,

t

)

0

V

2

₌

∇

(2.7)

som är en partiell differentialekvation.

(

r,θ,λ,t

)

är sfäriska koordinater och motsvarar avståndet från jordens mittpunkt, geocentrisk kolatitud (90° - latitud), longitud och tiden angiven i år. En lösning till Laplace ekvation i sfäriska koordinater är följande

(

)

(

( ) ( )

( ) ( )

)

(

)

( ) ( )

( ) ( )

(

q

t

cos

mλ

s

t

sin

mλ

)

P

(

cosθ

)

.

r

a

a

cosθ

P

mλ

sin

t

h

mλ

cos

t

g

r

a

a

t

λ,

θ,

r,

V

m n n 0 m m n m n n ne 1 n m n n 0 m m n m n 1 n ni 1 n max max

∑

∑

∑

∑

= = = + =

+













+

+

+













=

(2.8)

där a = 6371.2 är en referensradie och

P

m

(

cosθ

)

n är Schmidt seminormaliserade

associerade Legendrefunktioner av grad n och ordning m. För en närmare beskrivning av dessa funktioner hänvisas till [2]. Variablerna och är interna gausskoefficienter vid tiden t medan och är externa gausskoefficienter vid tiden t. Summeringsindexen ni och betecknar

antalet summationer för de interna respektive de externa gausskoefficienterna.

m n

g

m n

q

m n

s

max ne m n m n m n

h

m n

h

max

Enligt [2] är koefficienterna och

s

mycket mindre än koefficienterna

g

och och därför kan lösningen till Laplace ekvation approximeras enligt

q

m

n

(

)

(

g

( ) ( )

t

cos

mλ

h

( ) ( )

t

sin

mλ

)

P

(

cosθ

)

r

a

a

t

λ,

θ,

r,

V

m n n 0 m m n m n 1 n N 1 n

∑

∑

= + =

+













≈

.

(2.9)

Gausskoefficienterna

g

m

( )

t

och n

h

( )

t

m

n är funktioner av tiden och kan skrivas

som

( )

(

)

( )

(

0

)

m n m n m n 0 m n m n m n t t h h t h t t g g t g − + = − + =

&

&

5

t

t

5

t

t

0 0

+

≤

≤

t

t

0 0

≤

≤

+

_(2.10)

m n

g

m n

h

m n

&

där

och

är koefficienterna för primärfältet för det valda

tidsintervallet och g och

är den sekulära variationen för

nästföljande femårsperiod. Detta medför att ekvation (2.9) kan skrivas

som

m n

h&

(

)

(

( )

( )

)

(

)

(

) ( )

(

) ( )

(

g

cos

mλ

h

t

-

t

sin

mλ

)

P

(

cosθ

)

.

a

cosθ

P

mλ

sin

h

λ

g

λ

θ,

r,

V

m n n 0 m 0 m n m n 1 n 8 n m n n 0 m m n m n 1 n 13 n

∑

∑

∑

∑

= + = + =

+

+

+

&

&

t

-

t

m

cos

0

r

a

r

a

1 1 =

























a

t

,

+

=

(2.11)

IGRF-modellen för det primära magnetfältet trunkeras vid N = 13 och modellen för den sekulära variationen trunkeras vid N = 8. Gausskoefficienterna och och koefficienterna för den sekulära variationen

g

och återfinns i [3].

m n

g

m n

h

m n

&

m n

&h

Enligt (2.6) beskrivs magnetfältet som den negativa gradienten av den skalära potentialen V. Observationerna av jordens magnetfält kan då beräknas enligt följande

( )

( )

(

)

(

)

( )

( )

(

)

(

)

(

)

m

(

g

cos

( )

mλ

h

sin

( )

mλ

)

P

(

cosθ

)

r

a

1

n

r

V

B

m

cosθ

P

mλ

cos

h

mλ

sin

g

m

r

a

sinθ

1

λ

V

rsinθ

1

B

m

dθ

cosθ

dP

mλ

sin

h

mλ

cos

g

r

a

θ

V

r

1

B

m

m n n 0 m m n m n 2 n 13 1 n r z m n n 0 m m n m n 2 n 13 1 n λ y m n n 0 m m n m n 2 n 13 1 n θ x

∑

∑

∑

∑

∑

∑

= + = = + = = + =

+













+

−

=

∂

∂

=

−

=

−













=

∂

∂

=

−

=

+













=

∂

∂

=

−

=

(2.12)

( )

( )

(

)

(

)

( )

( )

(

)

(

)

(

)

m

(

g

cos

( )

mλ

h

sin

( )

mλ

)

P

(

cosθ

)

.

r

a

1

n

r

V

B

m

cosθ

P

mλ

cos

h

mλ

sin

g

m

r

a

sinθ

1

λ

V

rsinθ

1

B

m

dθ

cosθ

dP

mλ

sin

h

mλ

cos

g

r

a

θ

V

r

1

B

m

m n n 0 m m n m n 2 n 13 1 n r z m n n 0 m m n m n 2 n 13 1 n λ y m n n 0 m m n m n 2 n 13 1 n θ x

∑

∑

∑

∑

∑

∑

= + = = + = = + =

+













+

−

=

∂

∂

=

−

=

−













=

∂

∂

=

−

=

+













=

∂

∂

=

−

=

&

&

&

&

&

&

&

&

&

&

&

&

&

&

&

(2.13)

Magnetfältsvektorn ges då av

( )

(

)

( )

(

)

( )

t

m

m

(

t

t

)

.

m

t

t

m

m

t

m

t

t

m

m

t

m

0 z z z 0 y y y 0 x x x

−

+

=

−

+

=

−

+

=

&

&

&

(2.14)

Från magnetfältsvektorn kan den totala och horisontella intensiteten samt inklination och deklination beräknas

Horisontell intensitet:

H

m

( )

t

m

2

( )

t

y 2 x

+

=

Total intensitet:

F

H

m

2

( )

t

z 2

₊

=

Inklination:

( )













=

H

t

m

arctan

I

z Deklination:

( )

( )



_









=

t

m

t

m

arctan

D

x y _.

Deklination betecknar skillnaden mellan geografisk och magnetisk nordriktning och inklination betecknar magnetfältets vinkel mot horisontalplanet på en given plats.

)

λ

,′

θ

Eftersom modellerna baseras på sfäriska koordinater måste en transformation från de geodetiska koordinaterna göras. Transformationen av de geodetiska koordinaterna

(

h,

θ

, där h är höjden över havet, ′ är kolatituden och

λ

är longituden, till de sfäriska koordinaterna

(

r,θ,λ

)

beräknas enligt följande

θ

cos

R

θ

sin

R

θ

cos

R

θ

sin

R

θ

cos

R

θ

sin

R

θ

cos

R

θ

sin

R

θ

cos

R

θ

sin

R

2h

h

r

θ

tan

R

R

arctan

θ

tan

R

h

θ

cos

R

θ

sin

R

R

h

θ

cos

R

θ

sin

R

arctan

θ

2 2 B 2 2 A 2 4 B 2 4 A 2 2 B 2 2 A 2 4 B 2 4 A 2 2 B 2 2 A 2 2 2 B 2 A 2 B 2 2 B 2 2 A 2 A 2 2 B 2 2 A

′

+

′

′

+

′

=

′

+

′

′

+

′

+

′

+

′

+

=













_′

=

















′

+

′

+

′

+

′

+

′

=

(2.15)

där h = 0 i de sista leden eftersom endast ett markgående fordon modelleras i det här examensarbetet.

2.3 Omvärldsmodell

För att kunna studera ett fordons beteende vid körning i olika terräng måste en omvärldsmodell skapas. Det här avsnittet beskriver översiktligt hur detta går till med hjälp av redan implementerade program. För mer information om omvärldsmodellen hänvisas till [4].

2.3.1 Terrängmodell

Det första momentet när omvärldsmodellen skapas är att ta fram den yta som fordonet ska färdas på. Terrängmodellen använder spline-interpolation för att anpassa en tredimensionell yta till diskreta punkter som anges av användaren. För att denna yta, som representerar marken som fordonet åker på, ska vara kontinuerlig och kontinuerligt deriverbar två gånger används kubiska splines för tre dimensioner för interpolerationen.

2.3.2

Färdväg

För att definiera hur fordonet ska röra sig på den skapade ytan måste ett antal punkter som fordonet ska passera anges av användaren. För att skapa en kurva som fordonet ska färdas längs skapas först en kubisk splinefunktion i två dimensioner utifrån de angivna punkterna. Denna kurva projiceras sedan på den tredimensionella ytan som redan skapats.

Eftersom det är önskvärt att studera uppträdanden hos fordonet i olika hastigheter, accelerationer, vinkelhastigheter och attityder har kurvan eller vägen som fordonet rör sig längs ingen koppling till tiden. Istället anger användaren vilken hastighet fordonet ska ha i olika tidpunkter. Notera att hastigheten alltid måste vara noll vid simuleringens start och slut. För att undvika diskontinuerliga hastighetsförändringar anpassas även dessa punkter med en kubisk splinefunktion.

2.3.3 Utdata

Omvärldsmodellen genererar fordonets rörelse genom att vid varje tidpunkt returnera fordonets position, hastighet, acceleration, attityd och vinkelhastighet. Fordonets rörelse beskrivs med hjälp av koordinatsystemen GEO, LL och CHASSI (läs mer om dessa koordinatsystem i bilaga B).

Från Simulink får fordonets chassi dessa storheter i form av en vektor med 21 element som representerar följande utdata:

Element 1-3 Position i koordinatsystemet LL 4-6 Hastighet i koordinatsystemet GEO 7-9 Acceleration i koordinatsystemet GEO

10-18 Attityden, DCM-sambandet mellan GEO och CHASSI 19-21 Vinkelhastigheten i koordinatsystemet C

3 SENSORER

I det här kapitlet ges först en introduktion till de sensorer som används i examensarbetet. Därefter följer en närmare beskrivning av modelleringen av sensorerna och beräkningar av fordonets attityd med hjälp av utsignaler från dessa.

3.1 Allmänt

om

sensorer

De sensorer som används i examensarbetet är som tidigare nämns rategyron, accelerometrar och magnetometrar. I det här avsnittet ges en översiktlig beskrivning av dessa sensorer och de fel som dessa genererar då de används. För mer detaljerad beskrivning av rategyron och accelerometrar samt dess sensorfel hänvisas till [1] och [5].

3.1.1 Rategyron

Ett gyro är en sensor som mäter attityd eller vinkelhastighet (eng. rate). Vad som mäts beror på gyrots konstruktion och vilken typ av gyro det är. I det här examensarbetet används rategyron.

Utsignalen från ett rategyro är en summa av fordonets vinkelhastighet och jordens rotationshastighet samt de sensorfel rategyrot ger upphov till, i den riktning rategyrots känslighetsaxel är placerad.

De gyron som modelleras har endast en känslighetsaxel. Detta innebär att om rotationsvektorn är ortogonal mot gyrots känslighetsaxel kommer utsignalen att bli noll. För att mäta en godtycklig vinkelhastighetsvektor behövs därför tre gyron som placeras ortogonalt, en så kallad rategyrotriad.

3.1.2 Accelerometrar

En accelerometer är en sensor som mäter acceleration hos en kropp. Utsignalen från en accelerometer kallas för specifik kraft och den består av den verkliga accelerationen och jordens massattraktion eftersom dessa två inte går att skilja åt i ett slutet system, samt de sensorfel accelerometern ger upphov till.

Accelerometrar har generellt bara en känslighetsaxel och eftersom både acceleration och massattraktionskraft är definierade i tre dimensioner, behövs tre accelerometrar vars axlar är orienterade ortogonalt mot varandra, en så kallad accelerometertriad, för att mäta en specifik kraft.

3.1.3 Magnetometrar

En magnetometer är en sensor som mäter magnetfältet på en position. Utsignalen från magnetometern kommer att vara styrkan hos magnetfältet på positionen som i sin tur är en summa av olika magnetfält som samverkar. Dessa är först och främst jordens primära magnetfält, lokala anomalier och det magnetfält som genereras av fordonet. Även för en magnetometer kontamineras utsignalen med de sensorfel magnetometern ger upphov till.

Magnetometrar har generellt bara en känslighetsaxel och eftersom även magnetfält är definierat i tre dimensioner, behövs tre magnetometrar vars axlar är orienterade ortogonalt mot varandra, en så kallad magnetometertriad, för att mäta magnetfältet på en position.

3.1.4 Sensorfel

Utsignalen från de sensorer som används innehåller tre typer av fel, biasfel, skalfaktorfel och brus. Då tre sensorer används för att mäta signaler i olika riktningar kan ett fjärde typ av fel uppstå, ortogonalitetsfel.

Biasfel (som på svenska kallas nollfel) karaktäriseras av att utsignalen från en sensor är konstant och skiljd från noll då ingen vinkelhastighet, specifik kraft respektive magnetfält finns att mäta. Biasfelet för ett rategyro är alltså en konstant utsignal som är skiljd från noll då gyrot inte roterar. Gyrots bias kan mätas genom att orientera rategyrot med känslighetsaxeln i öst- västlig riktning där jordrotationen inte är mätbar. Biasfelet för en accelerometer är även den en konstant utsignal som är skild från noll då accelerometern inte utsätts för någon specifik kraft. Accelerometerns bias kan mätas genom att orientera accelerometern så att känslighetsaxeln är parallell med horisontalplanet där gravitationen inte är mätbar. Även en magnetometers utsignal har ett biasfel och även det en konstant utsignal som är skiljd från noll då magnetometern inte utsätts för något magnetfält.

Skalfaktorfel är en annan typ av fel som utsignalerna från sensorerna kontamineras med. Rategyrots skalfaktorfel kan kalibreras genom att rotera gyrot med en känd konstant rotationshastighet och därefter beräkna kvoten mellan utsignalen och den kända referensen. Ett annat sätt att göra detta på är att orientera gyrot i en riktning där det känner av en känd del av jordrotationen och därefter beräkna kvoten mellan utsignalen och den kända referensen. Skalfaktorfelet hos en accelerometer kan beräknas genom att utsätta accelerometern för en specifik kraft. Detta kan t.ex. göras genom att orientera accelerometern i vertikal riktning och utsätta den för en kraft och därefter vända på accelerometern och låta den mäta samma kraft i motsatt riktning.

Skalfaktorfelet kan därefter beräknas utgående från dessa mätningar. Även en magnetometers utsignal kommer att skalas med en skalfaktor.

Den tredje typen av fel som utsignalerna från sensorerna kommer att innehålla är brus. I stort sett alla sensorers utsignaler blir kontaminerade med brus och detta gäller för både rategyron och accelerometrar samt magnetometrar. Figur 3.1 nedan illustrerar hur de sensorfel som beskrivits, bias, skalfaktor samt vitt brus, påverkar en signal.

0 0.5 1 1.5 2 2.5 3 3.5 4 0 1 2 3 4 5 6 7 8 Sann utsignal Bias

Utsignal med biasfel

Utsignal med bias, skalfaktorfel och vitt brus

Figur 3.1 Illustration av hur sensorfel påverkar en signal.

Då tre sensorer används för att mäta signaler i olika riktningar kan som tidigare nämnt en fjärde typ av fel uppstå, ortogonalitetsfel. Detta fel uppstår om känslighetsaxlarna hos sensorerna inte är exakt ortogonala mot varandra. Mätningen av vinkelhastigheten, den specifika kraften respektive magnetfältet kommer då att bli felaktiga eftersom sensorerna antas vara ortogonalt placerade.

3.2 Rategyrotriaden

I det här avsnittet beskrivs först hur fordonets vinkelhastighet modelleras. Därefter följer en beskrivning av hur rategyrotriaden modelleras samt hur utsignalen från denna kommer att se ut.

3.2.1

Modellering av fordonets vinkelhastighet

Gemensamt för alla typer av gyron är att de har tröghetsrymden som referens. Detta innebär bland annat att om applikationen kräver en noggrannhet bättre än 15 grader per timme kommer jordrotationen att behöva kompenseras bort från utdata. Alternativt kan funktioner byggas in i gyrot och kompensera för detta internt. Även den så kallade transporthastigheten (eng. craftrate) kommer att påverka mätningarna. Transporthastigheten är den vinkelhastighet som observeras då ett fordon förflyttar sig över jordytan och den kan tecknas enligt följande

h

R

+

=

v

ω

(3.1)

där R är jordens radie, h är altituden och v är fordonets hastighet.

Transporthastigheten definieras som vinkelhastigheten hos koordinatsystemet GEO relativt ECEF och betecknas med

ω

G_EG. Den kan enligt [1] uttryckas som





































+

−

+

=





















−

=





















=

0

h

R

v

h

R

v

0

Φ

cosΦ

λ

ω

ω

ω

M G x p G y G EGz G EGy G EGx

&

&

G EG

ω

(3.2)

där och betecknar fordonets hastighet i x- respektive y-led uttryckt i koordinatsystemet GEO. G y v G x

v

Då transporthastigheten kommer att vara mycket liten jämfört med jordens rotation som i sin tur är mycket liten jämfört med fordonets vinkelhastighet, kan transporthastigheten approximeras till noll.

Den vinkelhastighet som mäts av rategyrotriaden är alltså en summa av fordonets egentliga vinkelhastighet, jordens rotationshastighet samt transporthastigheten

(3.3)

C GC C IE C GC C EG C IE C IC

ω

ω

ω

ω

ω

ω

=

+

+

≈

+

Jordens rotation beskrivs närmare i avsnitt 2.2.2.

3.2.2

Modellering av rategyrotriaden

Utsignal från rategyrotriaden är enligt föregående stycke en summa av fordonets egentliga vinkelhastighet, jordens rotation och transporthastigheten samt de sensorfel som rategyrotriaden själv ger upphov till.

Utsignalen från ett rategyro innehåller de sensorfel som beskrivs närmare i avsnitt 3.1.4. Den verkliga vinkelhastigheten skalas alltså med en skalfaktor krg

och därefter kommer vitt brus med medelvärde noll, ηrg, och bias, γrg, att

adderas till signalen. Även korrelerat brus adderas till utsignalen men denna typ av fel modelleras inte i examensarbetet. Utsignalen får då följande utseende

(

)

rg rg R IC rg R IC k 1 ω η γ ω~ = + + +

. (3.4)

Felet från gyrot blir då

rg rg R IC rg R IC k ω η γ ω ~ δ = + +

. (3.5)

Hittills har endast fel som uppstår vid användning av enstaka gyron studerats. Enligt avsnitt 3.1.4 uppstår även fel då de tre rategyrona som används inte är idealt placerade relativt varandra. Rategyrona är placerade i ett koordinatsystem RG enligt bilaga B.7. Rategyrotriaden är i sin tur placerad på en sensorplattform med koordinatsystemet PF enligt bilaga B.6. Eftersom antagandet att origo för koordinatsystemet RG och PF sammanfaller med origo för CHASSI modelleras endast två vridningsmatriser mellan dessa koordinatsystem. Rategyrona behöver enligt avsnitt 3.1.4 inte vara placerade ortogonalt och DCM-sambandet mellan PF och RG betecknar därför med

C

medan betecknar DCM-sambandet mellan koordinatsystemen CHASSI och PF. Sambandet mellan RG, PF och CHASSI modelleras då enligt följande

R P

/

P C

C

. (3.6)

C IC P C R P R IC

C

C

ω

ω

=

/

( ) ( )

(

(

)

rg rg

)

C IC P C R P rg -1 R P -1 P C C IC C C k 1C C ω γ η ω~ = / + / + +

. (3.7)

I examensarbetet antas dock gyrona vara ortogonalt placerade och

C

betecknar då en enhetsmatris.

R P

/

3.3 Accelerometertriaden

I det här avsnittet beskrivs först hur fordonets acceleration modelleras. Därefter följer en beskrivning av hur accelerometertriaden modelleras samt hur utsignalen från denna kommer att se ut.

3.3.1

Modellering av fordonets acceleration

Eftersom det i slutna system inte går att skilja på acceleration, a, och massattraktion, g, består utsignalen från accelerometern, som kallas specifik kraft och betecknas f, av båda dessa. För att mäta den verkliga accelerationen måste därför gravitationen kompenseras bort från utsignalen.

Den specifika kraft som är utsignal från accelerometertriaden är alltså en differens mellan fordonets egentliga acceleration och jordens gravitationskraft.

. (3.8)

G G

G

_a

_g

f

=

−

Jordens gravitation beskrivs närmare i avsnitt 2.2.3.

3.3.2

Modellering av accelerometertriaden

Utsignal från accelerometertriaden är enligt föregående stycke en differens mellan fordonets egentliga acceleration och jordens gravitationskraft samt de sensorfel som accelerometertriaden själv ger upphov till.

Utsignalen från en accelerometer innehåller de sensorfel som beskrivs närmare i avsnitt 3.1.4. Den verkliga accelerationen skalas alltså med en skalfaktor kacc

och därefter kommer vitt brus med medelvärde noll, ηacc, och bias, γacc, att

adderas till signalen. Även korrelerat brus adderas till utsignalen men denna typ av fel modelleras inte i examensarbetet. Utsignalen får då följande utseende

(

)

acc acc A acc A _{k 1f η γ} f ~ _{= + + +}

. (3.9)

Felet från accelerometern blir då

acc acc A acc A _{k f η γ} f ~ δ = + +

. (3.10)

A P

/

Hittills har endast fel som uppstår vid användning av enstaka accelerometrar studerats. Enligt avsnitt 3.1.4 uppstår även fel då de tre accelerometrarna som används inte är idealt placerade relativt varandra. Accelerometrarna är placerade i ett koordinatsystem ACC enligt bilaga B.8. Accelerometertriaden är i sin tur placerad på en sensorplattform med koordinatsystemet PF enligt bilaga B.6. Eftersom antagandet att origo för koordinatsystemet ACC och PF sammanfaller med origo för CHASSI modelleras endast två vridningsmatriser mellan dessa koordinatsystem. Accelerometrarna behöver enligt avsnitt 3.1.4 inte vara placerade ortogonalt och DCM-sambandet mellan PF och ACC betecknas därför med

C

medan betecknar DCM-sambandet mellan koordinatsystemen CHASSI och PF. Sambandet mellan ACC, PF och CHASSI modelleras då enligt följande

P C

C

. (3.11)

C P C A P A

_C

_C

_f

f

=

/

Detta medför att utsignalen från rategyrotriaden modelleras enligt följande

( ) ( )

(

(

)

acc acc accc

)

C P C A P acc -1 A P -1 P C C

_C

_C

_k

₁

_C

_C

_f

_γ

_η

_η

f

=

/

+

/

+

+

+

~

_{. (3.12)}

I examensarbetet antas dock accelerometrarna vara ortogonalt placerade

och

C betecknar då en enhetsmatris.

A

P

/

3.4 Magnetometertriaden

I det här avsnittet ges först en beskrivning av magnetisk deviation. Därefter följer en beskrivning av hur magnetometertriaden modelleras samt hur utsignalen från denna ser ut.

3.4.1 Magnetisk

deviation

Eftersom kursen beräknas utifrån det jordmagnetiska fältets komponenter är det av stor vikt att dessa kan detekteras utan störningar från närliggande magnetiska källor. Ferromagnetiska material i närheten av sensorerna kan alltså bidra till ett sämre resultat vid beräkningen av fordonets kurs. När ett ferromagnetiskt material placeras i ett likformigt magnetiskt fält kommer detta att störa det magnetiska fältet genom att fältet kröks. Figur 3.2 nedan illustrerar detta.

Figur 3.2 [7], Ett ferromagnetiskt material stör ett likformigt magnetfält genom att fältet kröks och därmed förstärks.

Det är enligt [6] huvudsakligen två typer av magnetisk deviation som orsakar störningar i det magnetfält som magnetometrarna detekterar, mjukjärns- och hårdjärnsdeviation. Hårdjärnsdeviation orsakas av permanenta magneter eller magnetiserat järn eller stål i närheten av sensorplattformen. Dessa störkällor är konstanta och placerade på ett fixt avstånd relativt magnetometrarna oavsett kursvinkel. Hårdjärnsdeviation orsakar ett fel i kursvinkeln med avseende på fordonets kurs i form av sinuskurva med en period. Detta visas nedan i figur 3.3. 0 50 100 150 200 250 300 350 400 -10 -5 0 5 10 K ur sf el gr ader

Kursfel med avseende på kurs, hårdjärnsdeviation

0 50 100 150 200 250 300 350 400 -4 -2 0 2 4 K ur sf el gr ade r Kursvinkel

Kursfel med avseende på kurs, mjukjärnsdeviation

Figur 3.3 Kursfel med avseende på fordonets kurs på grund av hårdjärnsdeviation (längst upp)

Magnetometrarnas sensorfel i form av bias klassificeras som hårdjärnsdeviation och orsakar därmed ett kursfel i form av en sinuskurva med en period. Eftersom fordonet själv består till stora delar av järn påverkar även det magnetfältet i form av hårdjärnsdeviation.

Mjukjärnsdeviation orsakas av att det jordmagnetiska fältet störs av omgivande mjuka material i närheten av magnetometrarna. På samma sätt som hårdjärnsdeviation kröker mjukjärnsdeviation det jordmagnetiska fältet. Skillnaden är att mjukjärnsdeviation resulterar i ett kursfel med avseende på fordonets kurs som har formen av en sinuskurva med två perioder enligt figur 3.3 ovan.

Magnetometrarnas sensorfel i form av skalfaktor klassificeras som mjukjärnsdeviation och orsakar därmed ett kursfel i form av en sinuskurva med två perioder.

Det finns egentligen ingen skarp gräns mellan hårdjärns- och mjukjärnsdeviation men i regel är hårdjärnsdeviationen betydligt större än mjukjärnsdeviationen. Hårdjärnsdeviationen är även lättare att korrigera och de vanligaste rekommendationerna är enligt [8] att hårdjärnsdeviationen ska korrigeras och i övrigt ska mjukjärnsegenskaperna försöka undvikas eftersom dessa inte är lika normalt förekommande. Det här examensarbetet begränsas till att estimera biasfelet i magnetometrarna eftersom dessa sensorfel är en av de största källorna till magnetisk deviation enligt resonemanget ovan.

Även geografiska störningar i det jordmagnetiska fältet påverkar beräkningen av fordonets kursvinkel. Exempel på denna typ av störningar är att på lokal nivå kan strömmar och ferromagnetiska material i jordens mantel ha mycket stor inverkan på det totala magnetfältet. I malmrika områden är lokala anomalier på 2-4° vanliga. Exempelvis är det totala magnetfältet över Kirunamalmen 75 000 nT vilket är 23 000 nT över det normala fältet i trakten på ungefär 52 000 nT enligt [8]. Ett annat exempel är att även solvinden kan ha en stor inverkan på det totala magnetfältet. Elektriska strömsystem i jonosfären och magnetosfären är ett resultat av samverkan mellan jordens primärfält och partikelstrålningen från solen. Ökad partikelstrålning från solen kan orsaka mycket stora förändringar i jordens magnetfält. Denna typ av störningar i det jordmagnetiska fältet är inget som tas hänsyn till i examensarbetet.

3.4.2

Modellering av magnetfält

Magnetfältet modelleras endast med den modell av jordens primära magnetfält som beskrivs närmare i avsnitt 2.2.4. De yttre störkällor som finns, till exempel lokala förändringar i magnetfältet och fordonets egen inverkan på magnetfältet, modelleras inte.

3.4.3

Modellering av magnetometertriaden

Utsignalen från magnetometertriaden är enligt föregående stycke en summa av jordens primära magnetfält samt de sensorfel som magnetometertriaden själv ger upphov till.

Utsignalen från en magnetometer innehåller de sensorfel som beskrivs närmare i avsnitt 3.1.4. Det verkliga magnetfältet skalas alltså med en skalfaktor kmagn och

därefter adderas vitt brus med medelvärde noll, ηmagn, och bias, γmagn, till

signalen. Även korrelerat brus adderas till utsignalen men denna typ av fel modelleras inte i examensarbetet. Utsignalen får då följande utseende

(

)

magn magn M

magn

M _{k 1m η γ}

m~ = + + +

. (3.13)

Felet från magnetometern blir då

magn magn M magn M _{k m η γ} m~ δ = + +

. (3.14)

Hittills har endast fel som uppstår vid användning av enstaka magnetometrar studerats. Enligt avsnitt 3.1.4 kommer även fel att uppstå då de tre magnetometrarna som används inte är idealt placerade relativt varandra. Magnetometrarna är placerade i ett koordinatsystem MAGN enligt bilaga B.9. Magnetometertriaden är i sin tur placerad på en sensorplattform med koordinatsystemet PF enligt bilaga B.6. Eftersom antagandet att origo för koordinatsystemet MAGN och PF sammanfaller med origo för CHASSI modelleras endast två vridningsmatriser mellan dessa koordinatsystem. Magnetometrarna behöver enligt avsnitt 3.1.4 inte vara placerade ortogonalt och DCM-sambandet mellan PF och MAGN betecknas därför med

C

medan

C

betecknar DCM-sambandet mellan koordinatsystemen CHASSI och PF. Sambandet mellan MAGN, PF och CHASSI modelleras då enligt följande

M P

/

P C

. (3.15)

C P C M P M

_C

_C

_m

m

=

/

Detta medför att utsignalen från magnetometertriaden modelleras som

( ) ( )

(

(

)

magn magn

)

C P C M P magn -1 M P -1 P C C _{C C k 1C C m γ η} m~ = / + / + +

. (3.16)

I examensarbetet antas dock magnetometrarna vara ortogonalt placerade och betecknar då en enhetsmatris.

M P

4 SENSORFUSION

I det här kapitlet härleds uttryck för den attityd som beräknas med utsignalerna från rategyrotriaden samt den attityd som beräknas med utsignalerna från accelerometer- och magnetometertriaden. Därefter definieras attitydfel och felmodeller för de båda beräknade attityderna härleds och sätts samman till en slutgiltig tillståndsmodell. I slutet av kapitlet beskrivs den allmänna tillståndsformen samt kalmanfiltret och extended kalmanfilter.

4.1 Kapitelöversikt

Det här avsnittet ger en översikt över de beräkningar och uttryck som härleds i det här kapitlet samt hur de byggs samman till en simuleringsmodell enligt figur 4.1 nedan.

Gyrotriad Acctriad M agntriad

Attitydberäkning C IC ω~ _f~C _m~C Attitydfel Gr C C Gm C C Kalmanfilter C Gm Gr CC C Attitydberäkning G Gr C C IC ωˆ δ Attitydkorrigering C G C + C IC ωˆ δ −

Gyrotriad Acctriad M agntriad

Attitydberäkning C IC ω~ _f~C _m~C Attitydfel Gr C C Gm C C Kalmanfilter C Gm Gr CC C Attitydberäkning G Gr C C IC ωˆ δ Attitydkorrigering C G C + C IC ωˆ δ −

De sensorer som används, en rategyrotriad, en accelerometertriad samt en magnetometertriad, vilka signaler de mäter, samt vilka fel dessa mätningar innehåller beskrivs i kapitel 3.

I avsnitt 4.2.1 härleds samband för att beräkna fordonets attityd med utdata från rategyrotriaden. Fordonets attityd beräknas även med utdata från accelerometertriaden samt magnetometertriaden och dessa samband finns beskrivna i avsnitt 4.2.2. Därefter beräknas skillnaden mellan dessa två attityder i avsnitt 4.3.

Skillnaden mellan de båda beräknade attityderna observeras av ett kalmanfilter som estimerar gyrobias och magnetometerbias. Tillståndsmodellen för detta kalmanfilter finns beskriven i avsnitt 4.4.4. I avsnitt 4.5 återfinns teori för kalmanfiltret samt extended kalmanfilter.

För att undvika gyrodrift återkopplas enligt avsnitt 4.6.2 den av kalmanfiltret estimerade gyrobiasen. Efter denna kompensering innehåller fordonets kurs-, roll och tippvinkel fortfarande stationärfel. För att även undvika dessa fel korrigeras fordonets attityd efter kalmanfiltreringen enligt avsnitt 4.6.3.

4.2 Attitydberäkningar

I det här avsnittet återfinns de beräkningar som gjorts på utsignalen från rategyrotriaden samt beräkningarna som gjorts på utsignalerna från accelerometer- och magnetometertriaden.

4.2.1 Rategyrotriadens

attitydberäkningar

Eftersom det är fordonets attityd,

C

, som ska beräknas kan följande differentialekvation lösas G C G C E IE G E C IC G C G C G EG G IE C IC G C C IG G C C IC G C C IG C IC G C C IC C GI G C C GC G C G C

C

ω

C

ω

C

C

ω

ω

ω

C

ω

C

ω

C

ω

ω

C

ω

ω

C

ω

C

C

− ≈ + − = − = − = + = =

&

(4.1)