Sensorfusion av GPS och IMU för en racingtillämpning

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Sensorfusion av GPS och IMU för en

racingtillämpning

Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping

av

Anders Blomfeldt Rasmus Haverstad

LITH-ISY-EX--08/4143--SE Linköping 2008

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

Sensorfusion av GPS och IMU för en

racingtillämpning

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Anders Blomfeldt Rasmus Haverstad

LITH-ISY-EX--08/4143--SE

Handledare: Johan Sjöberg

isy, Linköpings universitet

Anders Birgersson

Zetiq Development Examinator: Fredrik Gustafsson

isy, Linköpings universitet Linköping, 2 juli, 2008

Avdelning, Institution

Division, Department

Division of Automatic Control Department of Electrical Engineering Linköpings universitet

SE-581 83 Linköping, Sweden

Datum Date 2008-07-02 Språk Language  Svenska/Swedish  Engelska/English   Rapporttyp Report category  Licentiatavhandling  Examensarbete  C-uppsats  D-uppsats  Övrig rapport  

URL för elektronisk version

http://www.control.isy.liu.se http://www.ep.liu.se ISBN — ISRN LITH-ISY-EX--08/4143--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

Titel

Title

Sensorfusion av GPS och IMU för en racingtillämpning Sensor Fusion of GPS and IMU for a Racing Application

Författare

Author

Anders Blomfeldt Rasmus Haverstad

Sammanfattning

Abstract

AIMS AB is a Swedish company that develops and sells inertial sensor systems. A new product is under research which basically is an aided inertial navigation system where unwanted drift is eliminated. This can be done by integrating a GPS-reciever into the system and fuse measurement data from GPS and IMU. Such a product is useful in racing, where engineers always are searching for the optimal performance and the shortest lap times. Today the only feedback the engineers and mechanics get from their work is the driver’s comments and lap times. It would be of great help if it was possible to see the behaviour of the car directly in technical terms. The focus of this master thesis is fusing a GPS receiver and an IMU using an Extended Kalman Filter (EKF). The main task of the EKF is to estimate the errors of the intertial measurements and correct the measurements to eliminate drift in velocity and position. To verify the system, data collection was done togheter with the team G2 Racing and Tony Ring. The measurements were done in Tony’s Ferrari F430 Pista during two days at the Paul Ricard circuit in southern France. From the results possibilites to further estimate interesting entities were evaluated.

Nyckelord

Abstract

AIMS AB is a Swedish company that develops and sells inertial sensor systems. A new product is under research which basically is an aided inertial navigation system where unwanted drift is eliminated. This can be done by integrating a GPS-reciever into the system and fuse measurement data from GPS and IMU. Such a product is useful in racing, where engineers always are searching for the optimal performance and the shortest lap times. Today the only feedback the engineers and mechanics get from their work is the driver’s comments and lap times. It would be of great help if it was possible to see the behaviour of the car directly in technical terms. The focus of this master thesis is fusing a GPS receiver and an IMU using an Extended Kalman Filter (EKF). The main task of the EKF is to estimate the errors of the intertial measurements and correct the measurements to eliminate drift in velocity and position. To verify the system, data collection was done togheter with the team G2 Racing and Tony Ring. The measurements were done in Tony’s Ferrari F430 Pista during two days at the Paul Ricard circuit in southern France. From the results possibilites to further estimate interesting entities were evaluated.

Sammanfattning

AIMS AB är ett företag som tillverkar och säljer olika typer av tröghetsnavige-ringssystem. En ny produkt med ett tröghetsnavigeringssystem stöttat av GPS är under utveckling. Med sensorfusion kan GPS- och IMU-mätningar integreras genom användning av ett Extended Kalman Filter (EKF). Ett sådant system kan komma till användning inom racing där strävan efter optimal prestanda och korta varvtider är stor. Den information ingenjörerna får om bilens beteende är ofta en-bart förarens kommentarer. Förarens beskrivning kan ofta vara svår att översätta till tekniska termer som behövs för att enkelt justera inställningar och nå önskade egenskaper. Det skulle vara till stor hjälp om ingenjörerna direkt fick tillgång till teknisk information om fordonets beteende. I detta examensarbete ligger fokus på sensorfusion av GPS- och IMU-mätningar genom användning av ett Extended Kalman Filter (EKF). Huvuduppgiften för kalmanfiltret är att skatta fel i mät-ningar av accelerationer och vinkelhastigheter så att korrigering för dessa kan göras och därmed drift i position elimineras. För verifiering av systemet har datainsam-ling utförts tillsammans med teamet G2 Racing och Tony Ring. Mätningnarna är gjorda i Tonys Ferrari F430 Pista under två dagar på banan Paul Ricard i söd-ra Fsöd-rankrike. Ur resultatet undersöks även möjligheterna till vidare skattning av intressanta data som kan vara till hjälp för ingenjörerna i ett racingteam.

Tack

Vi vill tacka AIMS för att vi fått möjligheten att göra detta intressanta och lärorika examensarbete. Ett stort tack även till vår handledare Dr. Johan Sjöberg för gott stöd och givande diskussioner samt till vår examinator Prof. Fredrik Gustafsson på institutionen för systemteknik vid Linköpings Universitet. Tack även till annan personal som hjälpt oss under arbetets gång. Dessutom riktas ett stort tack till Tony Ring och G2 Racing för visat intresse av vårt arbete och det mycket goda samarbetet vid insamling av mätdata. Ett stort tack ägnas teamets tekniska chef Stefan Lehmann. Besöket på Paul Ricard är ett minne för livet.

Innehåll

2.1.1 Mätning av accelerationer . . . 5

2.1.2 Mätning av vinkelhastigheter . . . 6

2.1.3 Felkällor . . . 6

2.2 Positionering med GPS . . . 6

2.2.1 Principen för positionering med GPS . . . 7

2.2.2 GPS-signaler . . . 7 2.2.3 Felkällor . . . 8 2.2.4 Mätning av hastighet . . . 8 2.2.5 Differentiell GPS (DGPS) . . . 9 2.3 Integration . . . 9 3 Navigationssystem 11 3.1 Koordinatsystem . . . 11

3.2 World Geodetic System 1984 (WGS84) . . . 13

3.3 Transformationer . . . 14

3.4 Attitydrepresentation . . . 16

3.4.1 Eulervinklar . . . 16

3.4.2 Direction Cosine Matrix, C_bn . . . 17

3.5 TNS-ekvationer . . . 18

3.5.1 Generell navigationsekvation . . . 18

3.5.2 Ekvationer i navigationsramen . . . 19

3.5.3 Feldynamik i navigationsramen . . . 20

3.5.4 Sammanfattning av ekvationer . . . 22

4 Integration av GPS och IMU 23 4.1 Sensormodeller . . . 23

4.1.1 Mätmodell för accelerometrar . . . 23

4.1.2 Mätmodell för gyron . . . 24 ix

x Innehåll

4.1.3 Mätmodell för GPS . . . 25

4.2 Kalmanfiltret . . . 25

4.2.1 Introduktion . . . 25

4.2.2 Filtermodell . . . 26

4.2.3 Extended Kalman Filter . . . 27

4.2.4 Trimning av filterparametrar . . . 29

4.2.5 Observerbarhet . . . 30

4.2.6 Tolkning av IMUns mätvärden . . . 30

4.2.7 Initiering . . . 31 4.3 Implementering . . . 32 4.3.1 Feltillståndsmodell . . . 32 4.3.2 TNS-modell . . . 33 4.3.3 Hävarm . . . 35 4.3.4 Icke-holonomiska villkor . . . 35 5 Hårdvara 37 5.1 IMU . . . 37 5.2 GPS . . . 37

6 Resultat och Analys 41 6.1 Datainsamling . . . 41 6.1.1 Monteringsfel . . . 42 6.2 Resultat . . . 43 6.2.1 Positionsstöttning . . . 46 6.2.2 Icke-Holonomiska Villkor . . . 48 6.2.3 Felanalys . . . 50 6.2.4 Observerbarhet . . . 56 7 Diskussion 57 7.1 Mervärden . . . 57 7.2 Problem . . . 57 7.3 Fortsatt arbete . . . 58 8 Slutsatser 63

Nomenklatur

[·×] Skevsymmetrisk form av kryssprodukt

diag() Diagonalmatris

Akronym Betydelse

DCM Direction Cosine Matrix ECEF Earth Centered Earth Fixed EKF Extended Kalman Filter GPS Global Positioning System IMU Inertial Measurement Unit TNS Tröghetsnavigeringssystem INS Inertial Navigation System KF Kalman Filter

MEMS Micro Electro Mechanical System NED North-East-Down-koordinatsystem WGS84 World Geodic System 1984

2 Nomenklatur

Symbol Betydelse

xk Tillståndsvektor vid tidpunkt k

xk+1 Tillståndvektor vid tidpunkt k + 1

u Insignal y Mätsignal p Position v Hastighet Ψ Eulervinklar φ Roll θ Pitch ψ Yaw δp Fel i position δv Fel i hastighet δΨ Fel i eulervinklar

δa Bias i accelerometrar

δω Bias i gyron

Ω Skevsymmetrisk form av ω

Rb_ω,a Transformationsmatris för vinkelhastigheter mellan koordinat-system a och b

x, y, z Koordinataxlar i ett kroppsfixt system

N, E, D Koordinataxlar i ett NED-system

aN, aE, aD Accelerationer i respektive koordinataxel i ett NED-system

ωx, ωy, ωz Vinkelhastigheter i respektive koordinataxel

u Insignal

A, B, C Matriser i tillståndsmodellen (kontinuerlig tid)

F, G, H Matriser i tillståndsmodellen (diskret tid)

FT N S, GT N S Matris med tröghetsdynamik

Q, R Kovariansmatriser och designvariabler i kalmanfiltret

P Av kalmanfiltret skattad kovariansmatris

In Enhetsmatris av storlek n

0m×n nollmatris med m rader och n kolumner

Cb

a Rotationsmatris mellan koordinatsystem a och b

RM, RT Avstånd till ekvatorialplanet respektive jordens rotationsaxel

RE Jordens ekvatorialradie

f Specifika krafter [m/s]

g Gravitationsvektor

ϕ Latitud

λ Longitud

Kapitel 1

Introduktion

1.1

Bakgrund

Positionering av olika slag har ett brett användningsområde idag. Det kan handla om allt från att hitta i skogen eller i trafiken till att få självgående fordon att navigera. När GPS (Global Positioning System) utvecklades av det amerikanska försvaret fick man tillgång till ett globalt system som med enkel utrustning kan användas för att mäta både hastighet och position. I många tillämpningar ger GPS mer än tillräcklig noggrannhet, men i vissa typer av system behövs bättre precision.

En annan typ av sensor som är användbar vid positionering är en IMU (Inerti-al Measurement Unit). En IMU mäter accelerationer och vinkelhastigheter varur dess position och attityd kan beräknas. Noggranna IMUer är i allmänhet dyra och strävan efter att bygga system som inte får kosta allt för mycket har lett till ut-veckling av billigare typer av sensorer. En MEMS-IMU (Micro Electro Mechanical System - Inertial Measurement Unit) är en typ av IMU som är betydligt billigare än de allra bästa systemen. Priset för detta är försämrad prestanda hos sensorerna.

1.2

Problemformulering

Inom banracing kan noggranna mätningar av fordonets position och rörelser ge värdefull information om dess beteende och för autonoma farkoster krävs någon form av positionsbestämning för att de ska kunna navigera. Olika typer av posi-tioneringssystem har olika svagheter. Genom att utnyttja flera sensorer med olika styrkor och svagheter kan bättre skattningar göras än vad som är möjligt med de respektive systemen var för sig.

1.3

Mål

Företaget AIMS vill ta fram ett system för att positionera ett fordon så bra som möjligt. Detta ska göras genom att integrera en eller flera IMUer med en

4 Introduktion

mottagare med hjälp av ett Extended Kalman Filter (EKF). I detta examensarbete tas ett filter för fusioneringen av sensordata fram. En studie ska därefter göras om vilka intressanta parametrar som kan beräknas ur de data som mäts. Fokus ligger inte på att ta fram ett filter som kan köras online, men det ska vara lätt att modifiera algoritmerna så att realtidsberäkning av intressanta parametrar kan göras.

Kapitel 2

Grundläggande positionering

I detta kapitel ges en översiktlig introduktion till de tekniker för positionering som kommer att användas i arbetet. Till en början beskrivs principerna för hur en MEMS-IMU fungerar. Därefter ges en introduktion till GPS-systemet. Till sist diskuteras hur kompensering för de brister som finns i respektive teknik kan ge-nomföras genom att integrera IMU och GPS i ett gemensamt system.

2.1

Positionering med IMU

Ett sätt att bestämma position för ett objekt är att kontinuerligt mäta dess ac-celeration. Positionen fås genom att integrera den uppmätta accelerationen två gånger med avseende på tiden. Ett enkelt exempel är ett tåg som kan röra sig i båda riktningarna längs ett rakt spår. Genom att mäta accelerationen och inte-grera två gånger får man positionen för tåget på spåret. För att positionera ett objekt i tre dimensioner behöver accelerationer mätas i tre riktningar. I de fall ett objekt även kan rotera måste kännedom finnas kring hur det kroppsfasta ko-ordinatsystemet är relaterat till inertialramen. I varje tidpunkt måste vinklarna mellan systemens koordinataxlar kännas till. Ett sätt att beräkna vinklarna är att integrera de mätta vinkelhastigheterna. Inledningsvis beskrivs hur accelerationer och vinkelhastigheter kan mätas.

2.1.1

Mätning av accelerationer

Newtons andra lag säger att en kropps acceleration är proportionell mot de sam-manlagda krafterna som verkar på den, med kroppens massa som proportionali-tetskonstant. Matematiskt kan detta skrivas som F = ma. För att mäta accele-rationer kan man tänka sig en anordning med en fjäder med fjäderkonstanten k och en massa m. När anordningen utsätts för en acceleration gör massans trög-hetsmoment att fjädern tänjs. Fjäderns kraft verkande på massan är proportionell mot fjäderns förlängning, δ som F = kδ. Genom att mäta förlängningen kan ac-celerationen beräknas som a = F/m = kδ/m. I grunden används denna princip i en accelerometer av MEMS-typ, men själva mätningen kan göras på ett flertal

6 Grundläggande positionering

olika sätt. Exempelvis kan istället för användning av en fjäder, ett magnetiskt fält låtas påverka massan för att hålla den på plats. Styrkan på magnetfältet är proportionell mot den kraft massan utsätts för vid acceleration. Om vetskap om magnetfältets styrka finns kan kraften beräknas, varur accelerationen sedan fås. Det vanliga är att använda tre stycken endimensionella accelerometrar monterade ortogonalt mot varandra. (Titterton och Weston, 2004)

2.1.2

Mätning av vinkelhastigheter

En sensor som är lämplig för att mäta rotationsrörelse är ett gyro. Gyrot mäter vinkelhastighet vilken genom en integration ger en vinkel. Principen för ett gyro kan se väldigt olika ut, men i grunden utnyttjas tröghetsmomentet hos en kropp som roterar med hög hastighet. När en sådan utsätts för en rotation kring en annan axel än kroppens rotationsaxel skapas en coriolisacceleration som kan mätas. För en IMU av MEMS-typ används ofta ett vibrationsgyro som huvudsakligen skiljer sig från grundprincipen genom att den så kallade kontrollmassan vibrerar istället för roterar. Ett gyro för varje axel som objektet kan rotera kring behövs och pre-cis som för accelerometrar monteras vanligen tre gyron ortogonalt mot varandra. (Titterton och Weston, 2004)

2.1.3

Felkällor

Om det i någon sensor existerar ett fel, integreras detta fel hela tiden och på sikt blir felet i position stort. Ett sådant fel kan till exempel vara en bias, det vill säga att en accelerometer mäter en acceleration med ett konstant fel eller att ett gyro mäter en vinkelhastighet även när IMUn ligger stilla. Detta leder till att positionen, som är en dubbelintegral av accelerationen, driver iväg. Ett annat fel som kan förekomma är skalfel. Sensorerna ger då ett mätvärde som är proportionellt mot den verkliga mätstorheten med en faktor skild från ett. Båda dessa sensorfel är vanliga problem, framför allt i billigare positioneringssystem, och reducering av dessa fel är av stor betydelse när sensorerna integreras i ett mätsystem. (Grewal m.fl., 2007)

2.2

Positionering med GPS

GPS-systemet är ett av flera system av typen Global Navigation Satellite System (GNSS). Systemet består av 24 eller fler (32 i februari 2008, varav 31 fungerande) satelliter som kretsar i sex olika banor runt jorden på en höjd av ungefär 20200 km över jordytan. Satelliter på denna höjd fullgör ett varv runt jorden på ungefär 12 timmar. Satelliterna är utspridda i sina banor på ett sådant sätt att en mottagare, var som helst på den del av jordytan där systemet är tänkt att användas, teoretiskt ska ha kontakt med minst fyra satelliter samtidigt. I verkligheten finns det mycket som kan vara i vägen för kommunikationen mellan satellit och mottagare. Berg och träd i naturen samt byggnader i städer kan vara tillräckligt för att någon satellit ska försvinna ur räckhåll för en mottagare. (Grewal m.fl., 2007)

2.2 Positionering med GPS 7

2.2.1

Principen för positionering med GPS

Ombord på varje GPS-satellit finns ett atomur med mycket hög noggrannhet. Principiellt beräknas mottagarens position genom att den tar emot ett medde-lande som innehåller den tid då meddemedde-landet skickades från satelliten. Genom att jämföra tidsdata med mottagarens klocka vid den tidpunkt som signalen kom fram kan avståndet till satelliten beräknas. Det beräknade avståndet till vardera satellit innehåller, på grund av bland annat osäkerheter i satelliternas klockor och stör-ningar i jonosfären, en viss osäkerhet. Figur 2.1 visar hur osäkerheten i avstånd till flera satelliter skapar ett område inom vilket mottagaren med säkerhet befinner sig.

Figur 2.1. Positionering i 2D. Mottagaren befinner sig någonstans i det grå området.

För att beräkna en position behövs generellt data från fyra satelliter. De tre koordinaterna som ska bestämmas ger tre okända variabler, men det faktum att mottagarens klocka inte är exakt introducerar en fjärde frihetsgrad, vilken kräver ytterligare en satellit för att kunna elimineras. (Misra och Enge, 2006)

2.2.2

GPS-signaler

Meddelandesignaler skickas kontinuerligt från alla satelliter, och för att kunna skil-ja på vilken satellit som skickat en specifik signal används Code Division Multi-plexing (CDM). CDM fungerar så att signalen förutom en tidpunkt även innehåller en kod som identifierar från vilken satellit signalen kommer. Varje satellit skickar signaler på två olika frekvenser. En anledning till att två frekvenser används är att signaler tidsfördröjs när de skickas från satelliten till mottagaren på jorden. Sig-naler med olika frekvens tidsfördröjs olika mycket och genom att mäta skillnaden

8 Grundläggande positionering

i tidsfördröjning kan kompensering av tidsfördröjningen genomföras och därmed fås en noggrannare positionsbestämning. Den ena bärvågen, L1-bandet, har en

frekvens på 1575,42 MHz och den andra, L2-bandet, använder sig av

frekven-sen 1227,6 MHz. På L1-bandet moduleras två olika koder; en kod som innehåller

en grov positionsangivelse (coarse acquisition code, C/A) och en kod med högre positionsupplösning (precision code, P). På L2-bandet skickas endast koden.

P-koden är krypterad och kan bara användas i USAs militära applikationer, medan C/A-koden är fritt tillgänglig och används av alla GPS-mottagare. (Grewal m.fl., 2007)

2.2.3

Felkällor

Flera felkällor finns som försämrar resultatet av en positionsmätning med GPS. Nedan beskrivs kort de största felkällorna som förekommer i dagens GPS-system. Utförligare beskrivning av felen kan till exempel hittas i Grewal m.fl. (2007) eller Schumacher (2006).

• I atmosfären gör förekomsten av bland annat vattenånga och laddade

par-tiklar att signalerna fördröjs på sin väg mot jordytan.

• Satelliterna avviker hela tiden lite från sina tänkta banor, vilket leder till

felaktigt avstånd mellan satellit och mottagare.

• Satelliternas klockor är mycket noggranna, men innehåller små fel. Då

av-ståndet till en satellit beräknas genom att multiplicera ljusets hastighet med tiden för signalen att ta sig till mottagaren, ger till exempel ett fel på en mikrosekund ett fel i avstånd på 300 meter.

• Innan en GPS-signal når mottagaren kan den studsa mot kringliggande

ob-jekt, till exempel byggnader. Detta gör att samma signal når mottagaren flera gånger men har färdats olika väg, och mottagaren skattar då två olika positioner. Fenomenet kallas flervägsutbredning och vissa dyrare mottagare har inbyggda filter som försöker kompensera för detta.

• Till sist finns också termiskt brus som antingen kan komma från omgivningen

eller skapas i mottagaren själv.

2.2.4

Mätning av hastighet

En GPS kan inte enbart mäta position. Många mottagare kan också mäta hastig-heten med vilken den färdas. Det enklaste sättet att mäta hastighastig-heten är att approximera derivatan av de mätta positionerna. Eftersom dessa innehåller ett okänt fel ger dock detta ett opålitligt resultat. Ett bättre sätt för en GPS att mäta hastighet bygger på dopplereffekten. Den kända frekvensen hos bärvågen jämförs med den frekvens som mottagaren mäter. Efter att ha kompenserat för satelliternas rörelse i sina banor runt jorden samt jordens rotation kan en hastighet beräknas. Det framräknade värdet på hastighet är relativt pålitligt vilket gör det lämpligt att använda till exempel som stöttning i ett GPS-TNS-system. (Grewal m.fl., 2007)

2.3 Integration 9

2.2.5

Differentiell GPS (DGPS)

Genom att placera en basstation med GPS-mottagare på marken och noggrant mäta in dess position kan mottagaren skatta parametrar för de flesta av de fel som beskrevs i avsnitt 2.2.3. De mätbara felen är starkt korrelerade i ett lokalt område. Om basstationen sänder ut justeringsparametrar till mottagare i närheten kan därmed mottagaren korrigera för dessa fel och en betydligt bättre positions-angivelse kan ges. Alla mottagare har inte funktionalitet för DGPS och mottagare som hanterar DGPS är oftast betydligt dyrare än enklare mottagare. (Grewal m.fl., 2007)

2.3

Integration

Förekomsten av fel i sensorer försämrar naturligtvis ett mätsystems prestanda. Om sensorer med olika typer av fel kombineras, kan styrkan i den ena sensorn eliminera inverkan av svagheter i den andra. En tröghetssensor mäter, som beskrivet ovan, accelerationer och vinkelhastigheter. Ett fel i ett sådant mätvärde som integreras till en position ger upphov till att positionen driver. En GPS mäter däremot en absolut position. Ett fel i denna mätning ger inte upphov till någon drift, istället fås en position med viss osäkerhet. Integrering av GPS och IMU motiveras väl av att de styrkor och svagheter som existerar hos de båda systemen kompletterar varandra på ett utmärkt sätt. (Grewal m.fl., 2007) Genom att modellera felpara-metrar hos sensorerna kan en optimal skattning av dessa göras med hjälp av ett kalmanfilter. Figur 2.2 visar en enkel systemskiss över hur ett system av GPS och IMU kan integreras. Mätningarna från GPS-mottagare och IMU tas in i systmet, där tröghetsberäkningar på IMU-data görs och ett kalmanfilter skattar intressanta tillstånd.

INS-beräkningar

och kalmanfilter Tillstånds-_skattningar

GPS IMU

Kapitel 3

Navigationssystem

I ett navigatonssystem med GPS och IMU fås mätstorheter i olika koordinatsystem som måste transformeras till en lämpling ram för bearbetning. I detta kapitel beskrivs först de vanligaste koordinatsystemen som används vid navigering. Sedan följer definitioner och härledning av de ekvationer som används för att få önskad data i navigationsramen. Dessa ekvationer ligger till grund för de diskretiserade ekvationerna som senare implementeras i kalmanfiltret. Under härledningen tas de fullständiga ekvationerna fram för att slutligen, om möjligt, förenklas genom att ta bort försumbara variabler.

3.1

Koordinatsystem

För olika typer av tillämpningar behöver olika hänsyn tas till påverkande källor, detta tillsammans med vilka typer av sensorer som används är en avgörande faktor för vilka koordinatsystem som skall komma att användas av positioneringssyste-met. Nedan listas några vanliga koordinatsystem som används vid positionering. Alla koordinatsystem är ortonormala och kartesiska om inget annat anges.

• Earth Center Inertial (i): En inertialram defineras som en ram där Newtons

rörelselagar kan tillämpas. För att dessa lagar ska gälla får inte systemet befinna sig i ett accelererande eller roterande tillstånd. För globala positio-neringssystem används ofta en inertialram med origo i jordens centrum och som är ickeroterande med hänsyn till fixa stjärnor. Se figur 3.1.

• Earth Centered Earth Fixed (e): Detta koordinatsystem har sitt origo i

jor-dens masscentrum. z-axeln löper längs jorjor-dens rotationsaxel och x- och y-axlarna ligger båda i ekvatorialplanet där x-axeln skär nollmeridianen. Det är ett högersystem där z-axeln sammanfaller med z-axeln i (i). Detta koor-dinatsystem roterar relativt ECI med en hastighet av ωie= 7, 292115· 10−5

rad/s. Se figur 3.1.

• Navigationsram (n): Origo för detta koordinatsystem är fixt i marken och

de-fineras ur en initialposition som till exempel fås från GPS. Axlarna är riktade 11

12 Navigationssystem

i nordlig och östlig riktning samt nedåt, vilket är ett så kallat NED-system (North-East-Down). För tillämpningar begränsade till ett litet geografiskt område kan detta ofta anses vara en inertialram. Se figur 3.1.

Figur 3.1. I navigationsramen används ett system där axlarna pekar mot norr, öst

respektive nedåt - ett NED-system (North-East-Down).

• Kroppsfixt (b): Ett koordinatsystem som är fixt i fordonet med origo i dess

masscentrum. Figur 3.2 illustrerar hur ett sådant koordinatsystem är defi-nierat. Koordinataxlarna sammanfaller med axlarna för fordonets roll, pitch respektive yaw. Det är ett högersystem där z-axeln pekar nedåt.

Figur 3.2. Ett kroppsfixt koordinatsystem. x-axeln är riktad rakt framåt medan y-axeln

är riktad åt höger och z-axeln nedåt.

Ännu ett koordinatsystem kan förekomma då mätinstrumenten inte är monterade i fordonets masscentrum. I denna rapport antas att detta koordinatsystems origo sammanfaller med det kroppsfixa systemets origo. Att mätinstrumentens axlar inte

3.2 World Geodetic System 1984 (WGS84) 13

sammanfaller med det kroppsfixa koordinatsystemets axlar tas hänsyn till genom att rotera allt mätdata från mätsystemet till det kroppsfixa koordinatsystemet. Detta beskrivs närmare i avsnitt 6.1.1.

Longitud och latitud är vinklar uttryckta i (e), vilket gör det enkelt att utifrån mätvärden från en GPS få positioner uttryckta i detta system. Mätdata från IMU i form av accelerationer och vinkelhastigheter fås i förhållande till (i). För appli-kationer som används över ett större geografiskt område eller över en längre tid måste även hänsyn tas till jordens rotation vilket skiljer (e) och (i) från varandra. I vissa tillämpningar kan dock bidraget från jordens rotation anses tillräckligt litet för att försummas. Tillsvidare beskrivs de fullständiga ekvationerna och obetydliga variabler försummas i slutet.

3.2

World Geodetic System 1984 (WGS84)

GPS använder sig av koordinatsystemet WGS84 som är en internationell stan-dard. WGS84 bygger på en modell av jorden där den genomsnittliga havsnivån approximeras med en ellipsoid. Koordinatsystemets axlar sammanfaller med koor-dinataxlarna i (e), vilket gör det enkelt att få positioner uttryckta i detta system. En sådan modell ger upphov till lokala variationer i gravitationen och en gravita-tionsvektor som inte är ortogonal mot jordytan. För globala positioneringssystem där stor noggrannhet efterfrågas krävs en god modell av jordens gravitationsfält. För det tillämpningsområde denna rapport behandlar där fordonet rör sig över en relativt liten geografisk yta kan gravitationen och jordradien antas konstant och beräknas utifrån positionen för navigationsramen. Värdet på jordens rotationshas-tighet då WGS84 används är 7, 292115· 10−5 rad/s, se Grewal m.fl. (2007).

• Longitud λ: Longituden är noll vid Greenwich-meridianen och når 360 grader

vid fullföljt varv österut kring jorden. Longituden talar därmed om hur långt österut eller västerut man befinner sig relativt Greenwich-meridianen. Se figur 3.1.

• Latitud ϕ: Latituden är noll vid ekvatorn och når 90 grader vid nordpolen och

-90 grader vid sydpolen och talar om hur långt från ekvatorn man befinner sig. Se figur 3.1.

• Höjd h: Höjd i meter över havsnivån.

I WGS84 används ett antal parametrar som beskriver jordens form och rörelse. Dessa parametrar definieras nedan.

WGS84-parametrar

Ekvatorialradie, RE = 6378137 m (3.1a)

Polradie, RP = 6356752 m (3.1b)

Jorden excentritet, e = 0.0818192 (3.1c)

14 Navigationssystem RM = RE(1− e2) (1− e2_sin2_(ϕ))3/2 (3.1e) RT = RE  (1− e2_sin2_(ϕ)) (3.1f)

där RM och RT representerar avståndet till ekvatorialplan respektive jordens

ro-tationsaxel från lokal position.

För att översätta en position angiven i WGS84-systenet till en lokal naviga-tionsram behövs en relation mellan dessa två koordinatsystem. Denna definieras i ekvationerna (3.2).

Transformation till lokala koordinater

ΔpNn = RMΔϕ (3.2a)

ΔpEn = RT cos(Δϕ)Δλ (3.2b)

3.3

Transformationer

För det tillämpningsområde som detta arbete berör, där rörelseområdet är begrän-sat till ett litet geografiskt område, är det intuitivt att använda (e) som inertialram för att enkelt transformera GPS-data till navigationsramen. Genom att presentera beräknad data i navigationsramen är det enkelt att tolka fordonets position och hastighet och positionen kan lätt jämföras med en karta över området. Position från GPS fås enkelt utifrån definitionen av WGS84 tillsammans med mätvärden från GPS. Den lokala postionen i navigationsramen uppdateras utifrån ändringar av longitud och latitud genom ekvation (3.2) där RM och RT beräknas en gång

initialt genom ekvationerna (3.1) och sedan antas konstanta över den yta man rör sig på.

Vid sidan av GPS-mätningar måste mätvärden från IMU transformeras till na-vigationsramen, vilket sker genom en rotation. Det finns flera metoder att repre-sentera attityd på. De tre vanligast förekommande är eulervinklar, DCM (Direction Cosine Matrix) samt kvaternioner. Eulervinklar lämpar sig bra för applikationen i detta arbete och är även lättast att tolka. Därför implementeras eulervinklar se-nare i modellen som beskrivs i avsnitt 4.2. Valet av eulervinklar medför visserligen en risk för singulariteter, men denna risk innebär ingen begränsning i praktiken i denna tillämpning, vilket motiveras senare i kapitlet. Fordonet och IMUn roterar relativt navigationsramen och mätvärden måste därmed kontinuerligt transforme-ras från (b) till (n). Detta görs genom att rotera mätdata med en rotationssmatris,

Cn

b, där nedsänkt index representerar vilket koordinatsystem rotation sker från och

upphöjt index representerar vilket koordinatsystem rotation sker till. Oberoende av metod att representera attityden, måste C_bnalltid beräknas för att göra transfor-mationen från koordinatsystemet för mätutrustningen till önskat koordinatsystem för beräkning.

3.3 Transformationer 15

Rotationen som utförs med C_bn är i grunden tre olika rotationer, en kring varje koordinataxel. Rotationen kring de kroppsfasta x-, y- och z-axlarna kallas roll, pitch respektive yaw, se figur 3.2. Varje rotation representeras av en matris. De tre matriserna Rx, Ry och Rz svarar mot rotationen kring x-, y- respektive z-axeln

och definieras enligt

Rx(φ) = ⎛ ⎝ 10 cos(φ)0 sin(φ)0 0 − sin(φ) cos(φ) ⎞ ⎠ (3.3) Ry(θ) = ⎛ ⎝ cos(θ)0 01 − sin(θ)0 sin(θ) 0 cos(θ) ⎞ ⎠ (3.4) Rz(ψ) = ⎛

⎝ _{− sin(ψ) cos(ψ) 0}cos(ψ) sin(ψ) 0

0 0 1

⎞

⎠ (3.5)

DCM från koordinatsystem (n) till (b) ges av:

C_nb = Rx(φ)Ry(θ)Rz(ψ) (3.6)

Transformationen från (b) till (n) fås genom inversen av (3.6) och eftersom C_bn är ortonormal fås inversen ur transponatet.

C_bn= (C_nb)T = Rz(ψ)TRy(θ)TRx(φ)T = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝

cos(θ) cos(ψ) − cos(φ) sin(ψ) + sin(φ) sin(θ) cos(ψ)

sin(φ) cos(ψ) + cos(φ) sin(θ) cos(ψ)

cos(φ) sin(ψ) cos(φ) cos(ψ) + sin(φ) sin(θ) sin(ψ)

− sin(φ) cos(ψ)

+ cos(φ) sin(θ) sin(ψ)

− sin(φ) sin(φ) cos(ψ) cos(φ) cos(θ)

⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ (3.7) Eftersom det Cn

b = 1 för alla vinklar bevaras volymen vilket innebär att ingen

16 Navigationssystem

3.4

Attitydrepresentation

I detta avsnitt beskrivs metoder för representation av attityd. Den mest intuitiva metoden är eulervinklar vilken används i detta arbete. Eulervinklar medför dock en risk för singulariteter vilket beskrivs senare. Ett annat alternativ är uppdate-ring av attityden med Direction Cosine Matrix (DCM) vilken även den beskrivs i detta kapitel. Den trejde metoden är kvaternioner vilken inte beskrivs här. För beskrivning av kvaternioner se istället Titterton och Weston (2004).

Implementeringsmässigt utförs kryssprodukter genom att utnyttja den skevsy-mmetriska formen enligt

a× ω = ⎛ ⎝ aazxωωyz− a− ayzωωzx ayωx− axωy ⎞ ⎠ = ⎛ ⎝ ω0z −ω0z −ωωyx −ωy ωx 0 ⎞ ⎠ a = Ωa (3.8) där a är en godtycklig tredimensionell vektor och Ω den skevsymmetriska formen av ω.

3.4.1

Eulervinklar

Med denna metod lagras attityden i en vektor [φ θ ψ]T och vinkelhastigheter som mäts av IMUn används för att uppdatera denna vektor. Detta görs genom att först transformera vinkelhastigheter till navigationsramen och sedan integrera dessa. Med de beräknade eulervinklarna kan sedan C_bntas fram. Genom att transformera vinkelhastigheter uttryckta i (b) till (n) fås derivatan av eulervinkarna [ ˙φ ˙θ ˙ψ]T

i navigationsramen. Eftersom rotation med eulervinklar sker i bestämd ordning måste detta tas hänsyn till när [ ˙φ ˙θ ˙ψ] beräknas. Följande ekvationssystem fås för

denna transformation. (Titterton och Weston, 2004) ⎛ ⎝ ωωxy ωz ⎞ ⎠ b = ⎛ ⎝ ˙ φ 0 0 ⎞ ⎠ n + Rx(φ) ⎛ ⎝ 0_θ˙ 0 ⎞ ⎠ n + Rx(φ)Ry(θ) ⎛ ⎝ 00 ˙ ψ ⎞ ⎠ n (3.9)

Använder vi oss av (3.3)-(3.5) kan ekvationssystemet skrivas som ⎛ ⎝ ωωxy ωz ⎞ ⎠ b = ⎛

⎝ 10 − sin(θ)cos(θ) cos(θ) sin(φ)0

− sin(φ) 0 cos(θ) cos(φ)

⎞ ⎠ ⎛ ⎝ ˙ φ ˙ θ ˙ ψ ⎞ ⎠ n (3.10)

Genom att multiplicera med inversen av ovanstående transformationsmatris på båda sidor av ekvationen kan attityden i navigationsramen lösas ut och följande system av differentialekvationer erhålls

⎛ ⎝ ˙ φ ˙ θ ˙ ψ ⎞ ⎠ n = ⎛

⎝ 10 sin(φ) tan(θ)cos(φ) cos(φ) tan(θ)− sin(φ) 0 sin(φ) sec(θ) cos(φ) sec(θ)

⎞ ⎠ ⎛ ⎝ ωωxy ωz ⎞ ⎠ b = R_ω,bn ⎛ ⎝ ωωxy ωz ⎞ ⎠ b (3.11)

3.4 Attitydrepresentation 17

Singulariteterna som eulervinklar medför ses i (3.11). Då cos(θ) = 0 är sec(θ) =

1

cos θ odefinierad. Detta skulle innebära att fordonet åker i en riktning rakt uppåt

eller rakt nedåt vilket oftast inte sker i ett markfordon. För system tillämpade på till exempel krigsflygplan är andra alternativ att föredra, till exempel kvaternioner eller uppdatering av attityden genom Cn

b.

3.4.2

Direction Cosine Matrix,

C

Ett alternativ till eulervinkelrepresentationen är att låta C_bn-matrisen representera attityden. Denna metod använder sig till skillnad från eulervinkelrepresentationen av tidsderivatan av Cn

b. Nackdelen med denna metod är att den kräver sex olika

tillstånd istället för eulervinkelrepresentationens tre tillstånd. Matrisen Cn b

inne-håller nio obekanta element men i och med dess ortonormala egenskap kan tre av dessa beräknas ur de andra sex. Denna metod har testats och resulterat i goda re-sultat. Om eulervinkarna önskas fås dessa ur C_bn genom nedanstående ekvationer. (Titterton och Weston, 2004)

φ = arctan c32 c33 (3.12) θ = arcsin(−c31) (3.13) ψ = arctan c21 c11 (3.14) där cij representerar elementen i Cbn och i och j representerar rad respektive

kolumn, 1≤ i, j ≤ 3. Då det kroppsfixa systemet vrider sig mäts en vinkelhastighet som kan utnyttjas för att uppdatera Cn

b. För små vinklar används första ordningens

taylorutveckling av de trigonometriska funktionerna, cos(α) ≈ 1 och sin(α) ≈ α för att approximera Cn b. Det ger C_bn ≈ ⎛ ⎝ ψ1 −ψ1 −φθ −θ φ 1 ⎞ ⎠ (3.15)

Vinkelhastigheterna i (b) kan representeras med en skevsymmetrisk matris Ωb bn.

Indexen betecknar rotation från (b) till (n) uttryckt i (b). Den vinkeländring som sker i (b) under ett sampel kan skrivas ω_bnb Δt vilka är små då sampelfrekvensen är hög. Det innebär att på skevsymmetrisk form fås Ωb_bnΔt. Rotationsmatrisen i tidpunkt t + Δt kan enligt Titterton och Weston (2004) formuleras som

C_bn(t + Δt) = C_bn(t)(I + ΔΨ) (3.16) där ΔΨ = ⎛ ⎝ Δψ0 −Δψ0 −ΔφΔθ −Δθ Δφ 0 ⎞ ⎠ = ⎛ ⎝ ωz0Δt −ω0zΔt −ωωyxΔtΔt −ωyΔt ωxΔt 0 ⎞ ⎠ = Ωb bnΔt (3.17)

18 Navigationssystem

där Δφ, Δθ och Δψ är de små vinkeländringarna som sker under en sampelperiod. Vidare kan derivatan av C_bn formuleras som

˙ C_bn(t) = lim Δt→0 C_bn(t + Δt)− C_bn(t) Δt = lim Δt→0 C_bn(t)(I + Ωb_bn(t)Δt)− C_bn(t) Δt = C_bn(t)Ωb_bn(t) (3.18)

Matrisen C_bn beräknas på diskret form ur följande ekvation

C_bn(k + 1) = C_bn(k) + ˙C_bn(k)ΔT = C_bn(k)(I + Ωb_bn(k)ΔT ) (3.19) Detta sätt att representera attityden på medför att C_bn av beräkningstekniska skäl kan förlora sin ortonormala egenskap och för att korrigera för detta kan normalisering enligt Titterton och Weston (2004) genomföras enligt

C_bn := C_bn−I− C n bC n b T 2 C n b (3.20)

3.5

TNS-ekvationer

I detta avsnitt härleds de ekvationer som beskriver dynamiken för tröghetsnavi-geringssystemet. Först tas den generella ekvationen fram för att sedan anpassas till navigationsramen. Även feldynamiken för de olika tillstånden tas fram då den ligger till grund för modellen i kalmanfiltret.

3.5.1

Generell navigationsekvation

I detta avsnitt härleds de generella differentialekvationerna som beskriver förhål-landet mellan insignalerna från IMUn och utsignalerna ur TNS-systemet (position, hastighet, attityd). Låt pi och pavara två tredimensionella vektorer som beskriver en position i ett inertialsystem (i) respektive ett godtyckligt koordinatsystem (a). Se figur 3.3. xi yi xa ya pi pa ri

3.5 TNS-ekvationer 19

Förhållandet mellan koordinaterna för denna position i de olika koordinatsy-stemen kan beskrivas med följande ekvation

pi = C_aipa+ ri (3.21) där Ci

a relaterar koordinatsystemens attityd och ri är en vektor mellan

koordinat-systemens origon. Deriveras (3.21) två gånger med avseende på tiden fås ¨

pi= C_aip¨a+ 2 ˙C_aip˙a+ ¨C_aipa+ ¨ri (3.22) Deriveras (3.18) fås

¨

C_ai = ˙C_aiΩa_ia+ C_aiΩ˙_iaa = C_ai(Ωa_iaΩa_ia + ˙Ωa_ia) (3.23) Vektorn ¨pi kan delas upp i gi+fi där g är gravitationsvektorn och f är de specifika krafterna som verkar på koordinatsystemet (a). Utnyttjas detta tillsammans med insättning av ekvationerna (3.18) och (3.23) i (3.22) fås

C_aip¨a+ 2C_aiΩa_iap˙a+ C_aiΩ˙_iaa pa+ C_aiΩa_iaΩ_iaa pa+ ¨ri = gi+ fi (3.24) Transformering till koordinatsystem (a) från koordinatsystem (i) genom multipli-cering med C_ia på båda sidor och omskrivning för att få ut ¨pa ger oss slutligen ekvationen

¨

pa= ga+ fa− 2Ωa_iap˙a− ˙Ω_iaapa− Ωa_iaΩa_iapa− ¨ra (3.25) Detta är den generella navigationsekvationen i ett godtycklig koordinatsystem (a). Denna ekvation kan användas för globala positioneringssystem och vid ett tillämp-ningsområde där det geografiska området begränsas till en liten yta kan några av termerna försummas. Används (i)-ramen som definierats i avsnitt 3.1 som iner-tialram och navigationsekvationerna implementeras i (e)-ramen sammanfaller de båda koordinatsystemens origo vilket innebär att ri = 0. I tillämpningen som denna rapport behandlar där en lokal navigationsram används som intertialram representerar ri_{avståndet mellan intertialramens och navigationsramens origo.}

3.5.2

Ekvationer i navigationsramen

Detta avsnitt beskriver de i föregående avsnitt framtagna ekvationerna anpassade för systemets tillämpning. Då mätningar från både accelerometrar och gyron fås relativt inertialramen (i) kommer dessa att påverkas av såväl jordens rotation som gravitation. Eftersom beräkningar sker i navigationsramen måste mätningarna kompenseras för dessa.

Modell av jordens rotation

Jordens rotationshastighet relativt (i) ges av

20 Navigationssystem

där ωe är jordens rotationshastighet, se ekvation (3.1d). Genom nedanstående

transformation fås vinkelhastigheten av jordens rotation uttryckt i (n) där ϕ är latituden. Eftersom navigationsramen är fix på jordytan gäller att ω_enn = 0.

ωn_in= ω_ien + ωn_en = C_enω_iee + 0 = (ωecos ϕ 0 − ωesin ϕ)T (3.27)

Beräkningen av ωn

ingörs initialt utifrån position mätt av GPS och antas konstant.

Vinkelhastigheten som gyrona mäter i (b) ges av

ω_ibb = ω_nbb + ω_inb = ω_nbb + C_nbω_inn (3.28) Den kompenserade vinkelhastigheten fås då som

ω_nbb = ω_ibb − C_nbω_inn (3.29)

Gravitationsmodell

Gravitationen varierar beroende av position på jorden. En enkel modell används här tagen ur Titterton och Weston (2004), och denna beräkning görs initialt i implementationen med hjälp av den med GPSen mätta startpositionen.

g(0) = 9, 780318(1 + 5, 3025· 10−6sin2(2ϕ)) (3.30)

Med modeller för jordens rotation och gravitation kan nu (3.25) uttryckas i navigationsramen med de specifika krafterna verkande på det kroppsfixa systemet,

fb_.

¨

pn= gn+ C_bnfb− 2Ωn_inp˙n− ˙Ωn_inpn− Ωn_inΩn_inpn+ ¨rn (3.31) Ekvation (3.31) kan förenklas genom att utelämna euleraccelerationen som re-presentaras av den fjärde termen i högerledet. Euleraccelerationen försummas då förändringen av jordens rotationshastighet antas vara obefintlig. Den femte ter-men är centripetalkraften orsakad av jordens rotation och den kan utelämnas på grund av den låga känsligheten hos mätutrustningen. Eftersom positionen av for-donet uttrycks relativt navigationsramen och inte relativt intertialramen kan ¨rn

försummas. Efter dessa antaganden fås ¨

pn = gn+ C_bnfb− 2Ωn_inp˙n (3.32) Ekvationerna som härletts i detta avsnitt kan sammanfattas i figur 3.4.

3.5.3

Feldynamik i navigationsramen

Genom kalibrering av IMUn kan bias och skalfel i accelerometrar kompenseras för, men mätninganra kommer ändå innehålla fel som varierar över tiden bland annat på grund av temperaturförändringar. Det ger upphov till fel även i skattningar-na av position och hastighet. Felet i position bestäms utifrån skillskattningar-naden mellan GPS-mätning och TNS-skattning, vilket är relaterat till hastigheten och attityden genom en uppsättning differentialekvationer. Dessa ekvationer ligger till grund för

3.5 TNS-ekvationer 21 Accelerometrar Gyron b n

C

C

³

³

C

Z

R

³

\

\

\

Figur 3.4. Blockschema över TNS-beräkningarna.

kalmanfiltret då loosely coupled implementering används, vilket vi kommer till i avsnitt 4.2. Ekvationerna beskrivs i detta avsnitt där termerna i (3.31) byts ut till motsvarande skattad ˆ(·) respektive mätt ˜(·) storhet.

Feldynamik för hastighet

Genom att substituera termerna i (3.31) till skattad respektive mätt storhet fås ˙ˆvn

= gn+ ˆC_bnf˜b− 2Ωn_invˆn (3.33) Då vinkelfelen är små kan, enligt Titterton och Weston (2004), förhållandet mellan skattad och sann Cn

b skrivas som

ˆ

C_bn= (I− [δΨ×])C_bn (3.34)

där [δΨ×] är en skevsymmetrisk form av attitydfelen δΨ.

[δΨ×] = ⎛ ⎝ δψ0 −δψ0 −δφδθ −δθ δφ 0 ⎞ ⎠ (3.35)

Genom att definera felet δ(·) som skillnaden mellan det riktiga värdet och det skattade värdet ˆ(·) för respektive term och sätta in detta i ekvation (3.33) fås

˙vn− δ ˙vn= gn+ (I− [δΨ×]C_bn(fb− δfb)− 2Ωn_in(vn− δvn) (3.36) Utvecklas denna ekvation fås

˙vn− δ ˙vn = gn+ C_bnfb− 2Ωn_invn    ˙ vn −Cn bδfb− [δΨ×]Cbnfb+ [δΨ×]δfb+ 2Ωninδvn (3.37) Vidare ger detta en ekvation för δ ˙vn där andra ordningens termer försummats

22 Navigationssystem

δ ˙vn = C_bnδfb+ [δΨ×]C_bnfb− 2Ωn_inδvn (3.38) Vidare är C_bnfb = fn och skriver vi fn på skevsymmetrisk form [fn×] fås

δ ˙vn _{= C}n bδfb+ [δΨ×](Cbnfb)− 2Ωninδvn = Cn bδfb− [fn×]δΨ − 2Ωninδvn (3.39) Feldynamik för attityd

Fel i attityd uppkommer vid fel i mätningnar av vinkelhastigheter. Transformering av dessa fel till navigationsramen sker på samma sätt som för vinkelhastigheter. Derivatan av felet δΨ ges av

δ ˙Ψ = ⎛ ⎝ δ ˙δ ˙φθ δ ˙ψ ⎞ ⎠ = ⎛

⎝ 10 sin(φ) tan(θ)cos(φ) cos(φ) tan(θ)− sin(φ) 0 sin(φ) sec(θ) cos(φ) sec(θ)

⎞ ⎠ ⎛ ⎝ δωδωxy δωz ⎞ ⎠ (3.40) och δΨ fås genom en integration.

3.5.4

Sammanfattning av ekvationer

Här sammanfattas de kontinuerliga ekvationerna som implementerats i naviga-tionssystemet och härletts tidigare i detta kapitel.

TNS-dynamik ˙ pn _{= v}n ˙vn _{= g}n_{+ C}n bfb− 2Ωninvn ˙ Ψn = Rn_ω,bωb (3.41) Feldynamik δ ˙pn = δvn δ ˙vn = C_bnδfb− [fn×]δΨ − 2Ωn_inδvn δ ˙Ψn _{= R}n ω,bδωb (3.42)

Kapitel 4

Integration av GPS och IMU

I detta kapitel beskrivs hur fusionen av GPS- och IMU-data genomförs. Kapitlet inleds med en beskrivning av sensormodeller och följs av teori för kalmanfiltret. Sedan följer härledning av den kalmanfiltermodell som implementerats.

4.1

Sensormodeller

I detta avsnitt härleds nödvändiga sensormodeller. Avsnittet är uppdelat i tre olika delar där varje sensortyp behandlas separat. Därefter följer en sammanfattning där mätekvationerna sätts ihop till en mätmodell. Precis som systemmodellen måste sensormodellerna diskretiseras, eftersom det inte är möjligt att erhålla mätdata kontinuerligt. Denna diskretisering görs i avsnitt 4.2, där sedan en färdig mätmo-dell ställs upp och implementeras i kalmanfiltret.

4.1.1

Mätmodell för accelerometrar

En accelerometer mäter den totala acceleration som den utsätts för. Denna består av gravitionen och rörelser hos den kropp som accelerometern är monterad på. Normalt är det bara den senare som är av intresse. Accelerometrarna kan dock inte skilja de två åt. Istället måste kompensering för detta göras i beräkningarna. En enkel modell för mätningen kan skrivas som

˜

ab = ab− gb (4.1)

där ab _{är den acceleration som ger upphov till rörelse hos fordonet och g}b _är

gravitationsvektorn. Index b anger att storheterna uttrycks i det kroppsfixa (body-) koordinatsystemet.

Ett problem med sensorer är att de alltid innehåller fel. Några kända typer av fel som kan modelleras beskrivs nedan.

• Bias innebär att mätvärdena innehåller en offset som varierar långsamt med

tiden.

24 Integration av GPS och IMU

• Skalfel gör att proportionalitetskonstanten mellan verklig storhet och dess

mätning är skild från ett.

• Monteringsfel betyder att sensorerna inte sitter monterade ortogonalt mot

varandra.

• Olinjäriteter i sensorerna innebär att kurvan som relaterar faktisk storhet

till dess mätning inte är linjär, vilket den i sensormodellen antas vara.

• Temperaturberoenden gör att sensorns mätvärden till viss del beror på

tem-peraturen på sensorn.

• Brus är en stokastisk process som adderas till mätningen.

Om alla dessa fel tas med i tillståndsmodellen leder det snabbt till att tillstånden inte kan skattas, ingen observerbarhet fås (se avsnitt 4.2.5). Vanligt är att bara modellera brus, bias och eventuellt skalfel. Om vi infogar mätbruset va∈ N(0, Ra)

och biasen δa i ekvation (4.1) får vi sensormodellen ˜

ab = ab− gb+ δa + va (4.2)

Ekvation (4.2) är en modell i det kroppsfixa koordinatsystemet. Önskas mätningen istället beskriven i ett annat koordinatsystem, till exempel en navigationsram, måste en rotation motsvarande skillnaden mellan de två systemen göras, med rotationsmatrisen C_bn.

De tre accelerometrarna är av samma modell och antas därför ha samma bruskaraktäristik. Vidare antas det inte finnas någon korrelation mellan accelero-metrarnas mätvärden. Den del av kovariansmatrisen som motsvarar acceleromet-rarna blir därmed diagonalmatrisen diag(σ2

acc σacc2 σacc2 ). Om accelerometrarna

inte sitter ortogonalt kan detta modelleras som en korellation mellan acceleromet-rarna, se Skog (2007), men till detta tas ingen hänsyn i det här arbetet.

4.1.2

Mätmodell för gyron

Ett gyro mäter vinkelhastigheten hos den kropp som det är monterat i, i förhål-lande till en inertialram. En enkel sensormodell blir

˜

ωb = ω_ibb (4.3)

De typer av fel som nämndes för accelerometrarna kan även förekomma i gyrona. Om vi tar med brus och bias i mätmodellen blir mätekvationen

˜

ωb = ωb_ib+ δω + vω (4.4)

där kovariansmatrisen för bruset av samma anledning som för accelerometrar-na antas vara en diagoaccelerometrar-nalmatris med samma varians i alla axlar, det vill säga diag(σ_gyro2 σ_gyro2 σ2_gyro).

4.2 Kalmanfiltret 25

4.1.3

Mätmodell för GPS

De fel som nämndes i avsnitt 2.2.3 modelleras tillsammans som normalfördelat vitt brus, vGP S, med väntevärde noll. En mätekvation som beskriver GPS-mätningarna

ges av

y = pe+ vGP S (4.5)

där alltså vGP S är en normalfördelad 3-dimensionell variabel med E[vGP S] = 03_×1.

4.2

Kalmanfiltret

I detta avsnitt ges en introduktion som motiverar användandet av kalmanfiltret. Först presenteras den grundläggande teorin och därefter följer förklaringar av de aspekter som behöver tas hänsyn till innan implementering kan ske. Ingen här-ledning av de ekvationer som filtret bygger på presenteras. För en mer komplett framställning, se till exempel Gustafsson m.fl. (2001) eller Gustafsson (2000).

Modellstrukturen för hopkopplingen av IMU- och GPS-mätningar i ett kalman-filter kan se ut på tre olika sätt; uncoupled, loosely coupled och tightly coupled, se figur 4.1. Uncoupled innebär att ingen återkoppling sker från kalmanfiltret till TNS- och GPS-beräkningar. I ett loosely coupled-filter används skillnaden mellan TNS- och GPS-beräkningar för att skatta fel i tillståndsskattningarna samt bias. I ett tightly coupled-filter kan dessutom fel i GPSen skattas. Vi återkommer till modellstrukturen i avsnitt 4.3.

INS

GPS

Kalmanfilter Position_Hastighet Attityd f,  Z ˆp (a) Uncoupled Kalmanfilter Position Hastighet Attityd f ,  Z ˆp ˆ _ˆ f, G GZ INS GPS (b) Loosely Coupled

Kalmanfilter Position_Hastighet Attityd f ,  Z , fD U ' ˆ _ˆ f, G GZ INS GPS ˆx (c) Tightly Coupled

Figur 4.1. Olika modellstrukturer.

4.2.1

Introduktion

Kalmanfiltret härleddes på 1960-talet av Rudolf E. Kalman och Richard S. Bucy och har sedan dess fått mycket stor betydelse inom signalbehandling av olika

26 Integration av GPS och IMU

slag. Filtret är härlett som det bästa linjära filtret för skattning av ett tillstånd. Med bästa menas här att skattningsfelets kovarians, P , minimeras. Filtret tillåter även flervariabla system med tillståndsvektorer av godtycklig storlek. För detta examensarbete är kalmanfiltret av yttersta intresse av den anledningen att flera brusiga mätsignaler kan viktas ihop på ett optimalt sätt, så kallad sensorfusion. Vi behöver inte begränsa oss till att bara använda mätsignaler från GPS och IMU, utan kan med fördel även ta in information från andra sensorer i bilen. Exempelvis kan mätningar av hastighet och rattutslag ge värdefull information som kan viktas samman med GPS- och IMU-data för att få en bättre tillståndsskattning. Det bör påpekas att även om kalmanfiltret är det bästa linjära filtret är det inget som säger att det inte finns ett olinjärt filter som skulle ge en mindre kovarians. Det går dock att visa (se till exempel Gustafsson m.fl. (2001)) att om process- och mätbrus har en gaussisk fördelning, är kalmanfiltret det absolut bästa filtret, det vill säga det finns inte ens något olinjärt filter som kan generera en skattning med lägre kovarians. (Gustafsson m.fl., 2001)

4.2.2

Filtermodell

En linjär, tidskontinuerlig systemmodell kan skrivas på tillståndsform som

˙x(t) = Ax(t) + Bu(t) + v1(t) (4.6a)

y(t) = Cx(t) + v2(t) (4.6b)

Ekvationerna (4.6) anger relationen mellan mätsignalerna y, styrsignalerna u och tillstånden x. Signalerna v1 och v2 representerar osäkerheten i systemmodellen

respektive mätningarna, vilken ofta modelleras som normalfördelat vitt brus med

E{v1(t)v1(t + τ )T} =  Q, om τ = 0 0 annars (4.7a) E{v2(t)v2(t + τ )T} =  R, om τ = 0 0 annars (4.7b)

Kovariansmatriserna Q och R är oftast okända och används då som trimningspara-metrar för att kalmanfiltret ska få ett bra uppförande. (Gustafsson m.fl., 2001) Om

Q och R väljs i kontinuerlig tid, kan det vara svårt att tolka kovarianserna när de

överförts till diskret tid. Istället väljs kovarianserna i tidsdiskret tid och vi släpper därför brusmodellen tillfälligt och återupptar den istället i avsnitt 4.2.4 efter att en tidsdiskret modell presenterats. Uppgiften är att, i varje tidpunkt, ta fram en så bra skattning av tillståndsvektorn x(t) som möjligt. Som redan nämnts ger kalmanfiltret den bästa linjära skattningen. Vi betecknar skattningen med ˆx(t|τ)

där τ är tidpunkten för den senaste mätning som tas med i beräkningarna. Filtret tillåter enligt Gustafsson m.fl. (2001) att skattningen görs på tre olika sätt:

• Prediktion: Skattningen av tillståndsvektorn i tidpunkten t bygger endast

4.2 Kalmanfiltret 27

• Filtrering: Skattningen bygger, förutom på samma mätvärden som i fallet

prediktion, även på mätningar i tidpunkten τ = t.

• Glättning: I skattningen tas även framtida mätningar med, dvs. τ > t. Detta

är alltså ett icke-kausalt filter.

Önskemålet är att i en slutlig produkt kunna se resultaten i realtid vilket innebär att endast mätdata fram till och med tidpunkten t finns tillgängligt vid beräkning av skattningen. Filtrering kommer därför användas, det vill säga tillståndsvektorn ˆ

x(t|t) skattas utifrån mätningarna y(t0), ..., y(t).

Kalmanfiltrets beräkningar sker i två steg. I det första steget sker en mätupp-datering där information från mätsignalerna vägs in. I det andra görs en tidsupp-datering av tillståndsvektorn och kovariansmatrisen med hjälp av modellen endast utifrån skattningen i förra tidpunkten med antagandet att brustermen v1(t) =

E[v1(t)] = 0. Detta är den bästa gissningen som kan göras eftersom det inte finns

någon information om hur v1 varierar i tiden.

Implementationen av filtret görs i diskret tid, varför systemmatriserna A, B och C i modellen (4.6) måste bytas mot dess tidsdiskreta motsvarigheter F , G respektive H. Givet ett initialt tillstånd, ˆx(0| − 1) = x0, samt en initial

kovarians-gissning, P (0| − 1) = P0 ger (4.8) och (4.9) ekvationerna för mätuppdatering och

tidsuppdatering. (Törnqvist, 2006) Mätuppdatering: K(t) = P (t|t − 1)H(t)T[H(t)P (t|t − 1)H(t)T + R(t)]−1 (4.8a) ˆ x(t|t) = ˆx(t|t − 1) + K(t)[y(t) − H(t)ˆx(t|t − 1)] (4.8b) P (t|t) = [I − K(t)H(t)]P (t|t − 1) (4.8c) Tidsuppdatering: ˆ x(t + 1|t) = F (t)ˆx(t|t) + G(t)u(t) (4.9a) P (t + 1|t) = F (t)P (t|t)F (t)T + G(t)Q(t)G(t)T (4.9b)

4.2.3

Extended Kalman Filter

Den tillståndsmodell som användes i beskrivningarna i föregående avsnitt är lin-jär. När modellen för systemet är olinjär, vilket är fallet i detta arbete, kan inte kalmanfiltret användas direkt. Det finns flera sätt att med kalmanfiltret behandla olinjära modeller och vi ska i detta avsnitt titta närmare på ett så kallat Extended Kalman Filter (EKF). En generell olinjär tillståndsmodell skrivs på formen

˙x(kT ) = f (x(kT ), u(kT )) (4.10a)

y(kT ) = h(x(kT ), u(kT )) (4.10b)

Skillnaden mot det linjära fallet är att vi här har olinjära funktioner f och h och vi kan därför inte skriva ekvationerna på matrisform med en konstant A-matris som i (4.6). Eftersom tillstånden på diskret form endast beskrivs i diskreta tidpunkter måste vi definiera ett antagande för hur tillstånden beter sig mellan

28 Integration av GPS och IMU

sampelpunkterna. Vid kort sampeltid är det rimligt att anta att insignalerna är styckvis konstanta. Uppdateringen av tillstånden blir då

xk+1 = xk+ k+1  k ˙xk = xk + k+1  k f (xk, uk)dt (4.11)

Eftersom vi antagit att insignalerna är konstanta mellan sampelpunkterna kan integralen i (4.11) beräknas som T f (xk, uk), vilket egentligen är Eulers

framåtap-proximation. Detta ger

xk+1 = xk+ T f (xk, uk) = fd(xk, uk) (4.12)

För att tillämpa ett kalmanfilter på den diskretiserade modellen fdmåste den först

linjäriseras, eftersom de olinjära funktionerna i fd inte kan skrivas på den

tidsdis-kreta motsvarigheten till formen (4.6) med konstant F-matris. EKF linjäriserar modellen i varje sampel och använder sedan de vanliga kalmanfilterekvationer-na (4.8) och (4.9) i varje diskret tidpunkt. (Ankelhed och Stenlind, 2005) En linjär modell fås genom en första ordningens taylorutveckling av funktionerna fd enligt

fd(x) = fd(ˆxk) + δfd(ˆxk) δx (xk − ˆxk) + 1 2(xk − ˆxk) Tδ2fd(ˆxξ) δx2 (xk − ˆxk)    restterm (4.13)

Resttermen kan i allmänhet försummas. Den blir dock större om modellen är väldigt olinjär eller om osäkerheten i skattningarna är stor. (Gustafsson, 2007)

Om modellen har n tillstånd och m styrsignaler resulterar taylorutvecklingen i den linjära modellen (4.6) där F , G och H ges av jacobianerna av fd och h enligt

F = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ δf_d,1(x,u) δx₁ δf_d,1(x,u) δx₂ . . . δf_d,1(x,u) δx_n δf_d,2(x,u) δx₁ . .. .. . δfd,n(x,u) dx1 . . . δfd,n(x,u) δxn ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ (4.14) G = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ δfd,1(x,u) δu₁ δfd,1(x,u) δu₂ . . . δfd,1(x,u) δu_m δf_d,2(x,u) δu₁ . .. .. . δfd,n(x,u) δu1 . . . δfd,n(x,u) δum ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ (4.15) H = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ δh1(x) δx1 δh1(x) δx2 . . . δh1(x) δxn δh2(x) δx1 . .. .. . δh_n(x) δx1 . . . δh_n(x) δxn ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ (4.16)

4.2 Kalmanfiltret 29 Anmärkning om EKF

Den ovan beskrivna metoden linjäriserar modellen i det skattade tillståndet onli-ne. För stora och kraftigt olinjära modeller blir detta relativt beräkningskrävande, och ett alternativ kan vara att använda ett så kallat Constant Gain Extended Kalman Filter (CGEKF). Ett CGEKF beräknar i förväg en linjärisering i ett an-tal tillstånd och använder under körning den linjärisering som ligger närmast den aktuella tillståndsskattningen. På detta sätt minskas beräkningsbördan kraftigt, men naturligtvis försämras samtidigt filtrets prestanda. (Jerhammar och Höcker-dal, 2006) Detta examensarbete fokuserar inte i första hand på att implementera en realtidslösning för positionsbestämning och vi nöjer oss därför med ett EKF. Men för vidare arbete kan det vara aktuellt att gå vidare med ett CGEKF för att lättare kunna köra filtret i realtid.

4.2.4

Trimning av filterparametrar

Som nämndes i avsnitt 4.2.2 används Q och R som designparametrar för att få till ett bra beteende hos skattningarna. Att hitta bra parametrar är ofta en iterativ process där flertalet simuleringar görs med olika inställningar bland parametrar. Det finns dock allmänna regler för hur parametrarna ska justeras för att få ett önskat beteende.

Matrisen Q modellerar processbrus, det vill säga hur pålitlig modellen är. Stora värden säger att det finns mycket processbrus, det är därför viktigt att ta stor hän-syn till mätningarna. Vid varje tidsuppdatering ökar då de skattade kovarianserna

P snabbt och när mätuppdatering sker blir hoppen i skattad position stora

ef-tersom vi litar relativt mycket på GPS-data. Omvänt betyder stora värden i R att vi har mycket mätbrus, vilket innebär att vi litar mindre på våra mätvärden. Det är endast relationen mellan Q och R som har betydelse, och denna relation styr en kompromiss mellan ett snabbt filter som är känsligt för brus, och ett långsamt filter som är okänsligt för störningar. (Gustafsson m.fl., 2001)

Positionen är en ren integration av hastigheten. Därför införs ingen ny osäker-het på positionsnivå och det finns alltså inget processbrus i positionen.

I denna tillämpning varierar hastigheter och vinkelhastigheter betydligt mer i vissa riktningar än i andra. Eftersom en bil i de allra flesta tidpunkter rör sig mer i det kroppsfixa koordinatsystemets x-led än y- eller z-led, är osäkerheten i denna riktning störst. Därför sätts kovarianserna för hastigheten i x-led större än i y-och z-led. För eulervinklarna finns den största osäkerheten i ψ eftersom bilen kan vrida sig fritt i denna led. För φ och θ är osäkerheten ganska låg eftersom vi vet att vi rör oss på marken.

Eftersom vi valt att representera hastigheterna i navigationsramen istället för i det kroppsfixa koordinatsystemet, måste kovariansmatrisen representeras i detta system. För att enkelt definiera kovariansmatrisen bestäms dock Q i det kroppsfixa koordinatsystemet och roteras sedan till navigationsramen med hjälp av rotations-matrisen C_bn.