Institutionen för systemteknik

Institutionen för systemteknik Department of Electrical Engineering Examensarbete Navigering och styrning av ett autonomt markfordon Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Sixten Johansson LiTH-ISY-EX--06/3796--SE Linköping 2006 Department of Electrical Engineering Linköpings universitet SE-581 83 Linköping, Sweden Linköpings tekniska högskola Linköpings universitet 581 83 Linköping

Navigering och styrning av ett autonomt markfordon Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Sixten Johansson LiTH-ISY-EX--06/3796--SE Handledare: Examinator: Daniel Axehill ISY, Linköpings universitet Torbjörn Crona Saab Bofors Dynamics Pelle Carlbom Saab Bofors Dynamics Anders Hansson ISY, Linköpings universitet Linköping, 31 mars, 2006

Avdelning, Institution Division, Department Division of Automatic Control Department of Electrical Engineering Linköpings universitet S-581 83 Linköping, Sweden Datum Date 2006-03-31 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport ISBN ISRN LiTH-ISY-EX--06/3796--SE Serietitel och serienummer Title of series, numbering ISSN URL för elektronisk version http://www.control.isy.liu.se http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-6006 Titel Title Navigering och styrning av ett autonomt markfordon Navigation and control of an autonomous ground vehicle Författare Author Sixten Johansson Sammanfattning Abstract In this thesis a system for navigation and control of an autonomous ground vehicle has been implemented. The purpose of this thesis is to further develop the vehicle that is to be used in studies and evaluations of path planning algorithms as well as studies of other autonomy functions. With different sensor configurations and sensor models it is also possible to evaluate different strategies for navigation. The work has been performed using a given platform which measures the vehicle s movement using only simple ultrasonic sensors and pulse encoders. The vehicle is able to navigate autonomously and follow a simple path in a known environment. The state estimation is performed using a particle filter. The work is a continuation of a previous project, Collision Avoidance för autonomt fordon, at Linköpings University in the spring of 2005. Nyckelord Keywords particle filter, navigation, trajectory tracking, odometry, ultrasonic sensors

Abstract In this thesis a system for navigation and control of an autonomous ground vehicle has been implemented. The purpose of this thesis is to further develop the vehicle that is to be used in studies and evaluations of path planning algorithms as well as studies of other autonomy functions. With different sensor configurations and sensor models it is also possible to evaluate different strategies for navigation. The work has been performed using a given platform which measures the vehicle s movement using only simple ultrasonic sensors and pulse encoders. The vehicle is able to navigate autonomously and follow a simple path in a known environment. The state estimation is performed using a particle filter. The work is a continuation of a previous project, Collision Avoidance för autonomt fordon, at Linköpings University in the spring of 2005. Sammanfattning I detta examensarbete har ett system för navigering och styrning av ett autonomt fordon implementerats. Syftet med detta arbete är att vidareutveckla fordonet som ska användas vid utvärdering av banplaneringsalgoritmer och studier av andra autonomifunktioner. Med hjälp av olika sensormodeller och sensorkonfigurationer går det även att utvärdera olika strategier för navigering. Arbetet har utförts utgående från en given plattform där fordonet endast använder sig av enkla ultraljudssensorer samt pulsgivare på hjulen för att mäta förflyttningar. Fordonet kan även autonomt navigera samt följa en enklare given bana i en känd omgivning. Systemet använder ett partikelfilter för att skatta fordonets tillstånd med hjälp av modeller för fordon och sensorer. Arbetet är en fortsättning på projektet Collision Avoidance för autonomt fordon som genomfördes vid Linköpings universitet våren 2005. v

Tack Det här examensarbetet hade aldrig blivit till utan hjälp och stöd från ett flertal personer. Jag vill börja med att tacka alla på Saab Bofors Dynamics som har gjort min tid där trevlig och givande. Jag vill speciellt tacka mina handledare Torbjörn Crona och Pelle Carlbom för många goda uppslag, vägledning och givande diskussioner kring utförandet av arbetet. Jag är mycket tacksam för att jag fått möjlighet att utföra detta examensarbete. Ett stort tack också till Daniel Axehill, min handledare på universitetet, för kommentarer, hjälp och stöd i arbetet. Jag vill även tacka min examinator Anders Hansson. Sist men inte minst vill jag tacka mina vänner och min familj för att ni alltid stöttar mig. Linköping, mars 2006 Sixten Johansson vii

Innehåll 1 Inledning 1 1.1 Bakgrund.................................. 1 1.2 Mål och syfte................................ 1 1.3 Tidigare arbete............................... 2 1.4 Genomförande............................... 2 1.5 Avgränsningar................................ 2 1.6 Rapportens struktur............................. 2 2 Systemöversikt 5 2.1 Mikroprocessor............................... 6 2.2 Hjul och motorer.............................. 7 2.3 Pulsgivare.................................. 7 2.4 Ultraljudssensorer.............................. 7 2.5 Fordonets ultraljudssensorer........................ 9 2.6 Programvara................................. 10 3 Navigering 13 3.1 Navigeringsproblemet............................ 13 3.2 Bayesiansk statistik............................. 13 3.2.1 Bayes sats.............................. 14 3.2.2 Modellapproximationer....................... 14 3.2.3 Numerisk integration........................ 15 3.3 Partikelfilter................................. 16 3.4 Implementering............................... 17 3.4.1 Översikt............................... 17 3.4.2 Omvärldsrepresentation...................... 19 3.4.3 Sensormodell............................ 20 3.4.4 Rörelsemodell........................... 22 4 Reglering av hjulhastigheten 25 4.1 Signalrepresentation............................ 25 4.1.1 Insignaler.............................. 26 4.1.2 Utsignaler.............................. 27 4.2 Simuleringsmodell............................. 28 4.3 Regulator.................................. 30 ix

5 Banföljning 33 5.1 Tillståndsextrapolator............................ 34 5.2 Bangenerering................................ 34 5.3 Signalrepresentation............................ 34 5.4 Regulator.................................. 34 6 Utvärdering 37 6.1 Syfte..................................... 37 6.2 Metod.................................... 37 6.3 Experimentella resultat........................... 37 6.3.1 Hjulens hastighetsreglering..................... 37 6.3.2 Ultraljudssensorerna........................ 39 6.3.3 Navigering............................. 41 6.4 Diskussion.................................. 43 7 Slutsatser 47 8 Framtida arbete 49 Litteraturförteckning 51 A Förkortningar 53 B Kommandoformat för mikroprocessorn 54

Kapitel 1 Inledning 1.1 Bakgrund När ett fordon ska förflytta sig autonomt krävs att fordonet har ett system för att skatta sin position. Det enklaste fallet att betrakta är då omgivningen är helt känd. Problemet förekommer även i många andra tillämpningar och är ett grundproblem inom robotiken. I den här applikationen används ett partikelfilter som med hjälp av information från avståndssensorer samt pulsgivare på fordonets motoraxlar positionerar fordonet i omgivningen. När fordonets position väl är känd är uppgiften att styra fordonet så att det autonomt kan följa en förutbestämd bana med hjälp av sensorerna. 1.2 Mål och syfte Målet med examensarbetet är att utifrån en given grundkonstruktion implementera en praktiskt fungerande positionsskattning för ett autonomt fordon med hjälp av enkla ultraljudssensorer och odometri. Odometriska data är information om hur långt fordonet har förflyttat sig. Fordonet ska även autonomt kunna följa en enklare given bana med hjälp av positionsskattningen. Syftet med detta arbete är att vidareutveckla fordonet som ska användas vid utvärdering av banplaneringsalgoritmer och studier av andra autonomifunktioner. Om omgivningen är helt känd rör det sig om ett enklare banplaneringsproblem. Finns okända föremål i omgivningen, det vill säga om omgivningen bara delvis är känd, uppstår ett behov av mer avancerade algoritmer. Med hjälp av olika sensormodeller och sensorkonfigurationer kan strategier för navigering utvärderas. Arbetet är en fortsättning på projektet Collision Avoidance för autonomt fordon som genomfördes vid Linköpings universitet våren 2005. 1

2 Inledning 1.3 Tidigare arbete Projektet Collision Avoidance för autonomt fordon lade en grund för detta arbete genom att ett fordon konstruerades tillsammans med mjukvara och ett antal strategier för att kunna positionera och navigera med fordonet. Projektet genomfördes under våren 2005 i ett samarbete mellan Linköpings universitet och Saab Bofors Dynamics. Systemet kunde dock inte testas fullt ut på det verkliga fordonet utan strategierna testades i simuleringar. 1.4 Genomförande Detta examensarbete genomfördes under hösten 2005 på avdelningen Guidance and Control hos Saab Bofors Dynamics i Linköping. Till att börja med studerades ett antal rapporter, främst inom området positionering av robotar samt partikelfilter. Programkod och dokumentation från projektet Collision Avoidance detaljstuderades. För att kunna verifiera hur positioneringsalgoritmer och insamlad sensordata fungerar tillsammans utvecklades bland annat en provimplementering av ett partikelfilter i Matlab. Detta gjordes i likhet med den implementering av ett partikelfilter som sedan tidigare finns utvecklad i C++ inom projektet. Därefter implementerades och utvärderades ett system för att kunna positionera fordonet i dess omgivning. Arbetet fortsatte med framtagandet av en modell av fordonets motorer i syfte att simulera reglering av hastigheten. En PID-regulator implementerades sedan på fordonets mikroprocessor. För att sedan få en praktisk användning av resultatet av de två första delarna i arbetet studerades även problemet med att få fordonet att följa en enkel förutbestämd bana. Slutligen sattes ett fungerande system upp tillsammans med delar av den programvara som redan fanns tillgänglig genom projektet. 1.5 Avgränsningar De avgränsningar som är satta för fordonet och därmed denna rapport återfinns i de ursprungliga kraven i [8]. Här återges de viktigaste avgränsningarna: Fordonet ska verka inomhus i en plan miljö. Fordonets omgivning antas vara tidsberoende. Fordonets omvärld förutsätts vara tvådimensionell. Därutöver tillkommer de rent fysikaliska begränsningarna för fordonskonstruktionen [13] vilka beskrivs närmare i avsnitt 2.2. 1.6 Rapportens struktur Arbetet och tillika rapporten är indelad i tre huvudområden, navigering och positionering av fordonet, reglering av hastigheten samt banföljning.

1.6 Rapportens struktur 3 I kapitel 2 presenteras fordonet och dess huvudkomponenter som grund för vidare läsning. I kapitel 3 presenteras problemet med navigering av fordonet. Kapitel 4 behandlar modellering och reglering av fordonets motorer. Kapitel 5 behandlar banföljningsproblemet och implementeringen av en regulator för detta. I kapitel 6 presenteras och analyseras praktiska resultat. Kapitel 7 innehåller slutsatser. I kapitel 8 ges slutligen förslag på framtida arbete.

4 Inledning

Kapitel 2 Systemöversikt Det autonoma fordon som används i arbetet är framtaget i projektet Collision Avoidance för autonomt fordon i kursen Reglerteknisk projektkurs vid Linköpings universitet våren 2005. Fordonet har fått namnet Freke och visas i figur 2.1. Figur 2.1. Freke, det autonoma fordonet. I projektet arbetades en hårdvaruspecifikation fram [13] utifrån vilken ett fordon realiserades. Fordonet består av ett fordonschassi, en bärbar PC med GNU/Linux samt en mikroprocessor. Dessutom utvecklades programvara för mikroprocessorn och den bärbara datorn. Projektet finns beskrivet i den tekniska dokumentationen [16]. En skiss över 5

6 Systemöversikt fordonet och dess olika delar är återgivna i figur 2.2. Figur 2.2. Skiss över systemet och dess huvudkomponenter. Nedan följer en kortare beskrivning av systemets delar som en grund för fortsatt läsning. Nedan ges även en mer generell introduktion till ultraljudssensorer vilka utgör en väsentlig del av systemet. 2.1 Mikroprocessor Mikroprocessorns uppgift är att sköta kommunikationen mellan den bärbara datorn och de övriga komponenterna på det fysiska fordonet. För att erhålla stor flexibilitet i konstruktionen används en AVR-processor av modell ATmega128. Processorn har ett I 2 C-interface, en AD-omvandlare med 8 kanaler, flera räknare, två USART-interface, ett SPI-interface och totalt 53 I/O-pinnar. Den har 128 kb EPROM, 4 kb EEPROM samt 4 kb SRAM. För ytterligare information hänvisas till processorns datablad [1]. I arbetet med det autonoma fordonet klockas processorn från en extern klockkälla på 3,6864MHz. Processorn är en 8-bitars processor vilket gör att den lättast arbetar med 8- och 16-bitars heltal. Den klarar dock av 64-bitars heltal med det C-bibliotek som använts vid programmeringen. Det finns även möjlighet att använda flyttal med enkel precision (32 bitar). För att minska beräkningstiden används här i möjligaste mån heltal med kortast möjliga representation. Detta sparar minnesplats och minskar kommunikationsflödet mellan mikroprocessorn och den bärbara datorn. Kommunikationen mellan den bärbara datorn och mikroprocessorn sker seriellt med en hastighet av 115200 bitar per sekund. En kommandoöversikt för kommunikationen mellan mikroprocessorn och den bärbara datorn finns i appendix B.

2.2 Hjul och motorer 7 2.2 Hjul och motorer De likströmsmotorer som används är av typen E192 från Micromotors och drivs av en 12 V blyackumulator. För att klara av att driva och accelerera fordonet har motorerna en inbyggd växellåda med utväxling på 25 : 1. De båda främre drivande hjulen har en diameter på 20cm, ett nav av plast, och en slityta av hårdgummi. Det bakre stödhjulet är ett mindre ledat hjul av polypropylen. Belastningen från 10kg utrustning och 5kg chassi gör att motorerna måste klara en axiell belastning av minst 75N var, förutsatt att vikten delas lika mellan de båda främre hjulen. Drivhjulen är lagrade för att minska belastningen på motorernas axlar. Fordonet klarar med befintlig design av en maximal hastighet av ca 1m/s (3,6km/h) och en acceleration av 0,5m/s 2. Detta innebär med en hjuldiameter på 20cm att vridmomentet vid maximal acceleration blir M = m a r 7,5 0,5 0,1 = 0,375Nm per motor. Här har ingen hänsyn tagits till friktion eller hjulens masströghetsmoment. 2.3 Pulsgivare För att kunna beräkna hastigheten hos fordonet eller den sträcka fordonet har färdats används magnetiska pulsgivare, Hallgivare, som är monterade på motoraxlarna. Hallgivarna på motorerna ger tre pulser per motorvarv vilket motsvarar 75 pulser per hjulvarv eftersom motorerna är utväxlade 25 : 1. De lämnar även en andra puls, förskjuten 90, vilket gör att det går att detektera åt vilket håll hjulen snurrar. Pulsgivarna är anslutna till mikroprocessorns synkrona avbrottsingångar. Detta medför att pulserna måste vara minst en processorklockcykel långa för att de ska uppfattas av processorn. Med en hjuldiameter på 20cm ger pulsgivarna en puls per 8,4mm förflyttning. 2.4 Ultraljudssensorer För att ett autonomt fordon ska klara av att navigera måste det använda information om omgivningen. Denna information, insignaler till fordonet, erhålls med hjälp av sensorer. Ultraljudssensorer används ofta inom robotforskning för att mäta avstånd till närliggande objekt. Dessa mätningar används sedan för att detektera föremål eller positionera roboten i sin omgivning. Ultraljudssensorer arbetar enligt time of flight(tof)-principen vilken innebär att en ljudpuls skickas iväg från sändaren samtidigt som mottagaren registrerar den tid det tar innan en reflekterad våg, eller eko, kommer tillbaka. Sambandet ges av d = vt 2 (2.1)

8 Systemöversikt där d är avståndet till närmaste föremål, v är utbredningshastigheten för ljudvågen och t är den uppmätta tiden från sändning till mottagande av puls. Utbredningshastigheten för ljud beror på temperatur och tryck vilket i sin tur ger variationer i den uppmätta sträckan d. Frekvensen för sensorerna är vanligtvis 40 50kHz och detta ger direkt samband för upplösningen i mätningen samt pulsens förmåga att reflekteras eller absorberas i olika material. En enkel modell för ultraljudssensorernas mätningar är att den utgående ljudvågen bildar en vågfront inom ett tvådimensionellt koniskt område enligt figur 2.3. Figur 2.3. Modell över mätning av avstånd med ultraljudssensorer. Avståndet och vinkeln till ett föremål betecknas r respektive α. Utbredningsloben definieras av vinkeln β samt radien R. Ett föremål på ett avstånd r och en vinkel α från utbredningsnormalen måste ligga inom den koniska sektionen, utbredningsloben, som definieras av radien R och öppningsvinkeln β, för att detekteras. Det faktum att sensorerna och ljudet i sig har en spridning som generellt är ganska stor gör att det finns en osäkerhet i vinkelbestämningen av det närmaste föremålet. Osäkerheten kan minskas genom att använda information om hur omgivningen ser ut. Förutom osäkerheten i vinkeln α finns det ytterligare problem med ultraljudssensorer: Signalbrus. Reflektioner i blanka ytor. Överhörning mellan sensorer. Blinda punkter vid spetsiga föremål som till exempel kanter och hörn. Ljudpulser som reflekteras bort från sensorn på grund av låg infallsvinkel mot objektet. Fördelarna med ultraljudssensorer är att de är små, robusta och drar lite ström. Sensortypen används inom många olika tillämpningar. Då sensorerna ger ett avstånd till det närmaste objektet inom utbredningsloben används ofta flera sensorer som sitter runt om fordonet. Alternativt kan en sensor som går att rotera användas. Då fler sensorer används kan geometriska samband användas för att bättre positionsbestämma det detekterade föremålet. En bra modell för sensorerna tillsammans

2.5 Fordonets ultraljudssensorer 9 med eventuella geometriska samband gör att man lättare kan positionsbestämma objekt relativt roboten. Inom området för kartgenerering med ultraljudssensorer, vilket är nära knutet till positioneringsproblemet, har mycket forskning gjorts. Metoder för att på ett bra sätt göra fysikaliskt riktiga modeller finns i till exempel [15], [6]. Dessa modeller beskriver utbredningsloben på ett bra sätt samt försöker fånga upp problemen med reflektioner och överhörning. Försök har även utförts med att klassificera objekt i en omgivning utifrån sensormätningar [7]. Dessa metoder bygger på att sensorernas signaler finns tillgängliga för signalbehandling. Generellt används signalbehandling för att bestämma den exakta tiden t och för att försöka utvinna så mycket information som möjligt ur själva signalekot. Tiden t kan på ett enkelt sätt bestämmas genom att en tröskel sätts på signalnivån för den inkommande pulsen. När signalstyrkan överstiger denna nivå mäter man tiden. Ett problem kan vara att hitta ett bra tröskelvärde för att undvika att sensorerna reagerar på störningar och reflektioner från till exempel andra ultraljudssensorer. 2.5 Fordonets ultraljudssensorer Ultraljudssensorerna som används på det autonoma fordonet är en liten modul som heter SRF 10 från Devantech Ltd. Det är en modul med en ultraljudssändare och mottagare samt en mikroprocessor. Mikroprocessorn på modulen gör det möjligt att kommunicera med sensorn på ett enkelt sätt via ett standardiserat protokoll som heter I 2 C. Med hjälp av I 2 C kan flera enheter kopplas till en och samma buss vilket sparar ingångar på fordonets mikroprocessor. En mätning kan startas genom att skicka ett kommando och sedan hämta resultatet i antingen µs, cm eller tum. Det finns även två intressanta parametrar som kan skickas till sensormodulen. Den första kallas range time och är den tid som mottagardelen ska vänta innan den signalerar att inget föremål hittats. Denna parameter är användbar för att undvika störningar från reflektioner av föremål långt borta samt för att kunna mäta oftare i ett avståndsintervall nära sensorn. Den andra parametern av intresse är analouge gain som reglerar den uteffekt som sändaren ska använda vid mätning. En lägre uteffekt minskar risken för överhörning mellan sensorer. Utbredningslobens storlek minskar något med en lägre uteffekt men samtidigt minskas även räckvidden för sensorn något som inte är önskvärt. Nackdelen med dessa sensorer är att de inte ger tillgång till själva signalen, utan endast ett värde enligt ovan, vilket reducerar mängden information från sensorn. Detta gör att algoritmer för att ta till vara på information i själva signalen och dess ekon inte kan användas. Sensormodulen kommer inte tillrätta med de generella problemen hos ultraljudssensorer som fortfarande finns kvar. De tekniska data [4] som tillverkaren anger är att sensorerna kan detektera föremål inom

10 Systemöversikt intervallet 3cm till 6m. Den längsta tid som sensorn behöver för en mätning är ungefär 65ms. Utbredningsloben, β, är enligt modellen i figur 2.3 cirka 36 vilket gör det nödvändigt att minska utbredningsloben rent mekaniskt. En minskning av lobstorleken har här gjorts med de experiment som finns utförda i [3] som utgångspunkt. Kalibreringen av sensorerna är enligt tillverkaren utförda vid 24 och man antar en ljudhastighet på 346m/s. Detta gör att sensorernas funktion för att mäta direkt i cm här kan användas då förhållandena inomhus inte skiljer sig nämnvärt från de vid kalibreringstillfället. En temperaturförändring från till exempel 18 till 20 ger ett mätfel på cirka 3,5mm på 1meter. Även förändringar i mätresultat på grund av variationer i lufttryck kan försummas med ett liknande resonemang. De båda ovanstående parametrarna tillsammans med en cylinder som minskar utbredningsloben kan anpassas efter de förutsättningar som finns för fordonets omgivning. Försök med att minska sensorernas utbredningslob finns beskrivna i avsnitt 6.3.2. I avsnitt 6.3.2 redovisas även lämpliga parametervärden för sensorerna. Avståndsmätningar är i den här tillämpningen ögonblicksinformation och behöver därför inte samplas oftare än vad andra delar av systemet kräver. Ett problem när det gäller samplingsfrekvensen är att om närliggande sensorer avfyras samtidigt kan falska mätningar uppkomma på grund av överhörning mellan sensorerna. En sensors signal måste hinna dämpas ut innan nästa sensor kan avfyras för att denna inte skall reagera på det gamla signalekot och därmed ge ett felaktigt mätresultat. Sensorerna avfyras i en ring runt fordonet med ett fixt mellanrum mellan varje sensor vilket innebär att varje sensor avfyras med ett tidsintervall av n t s ms där n är antalet sensorer och t s intervalltiden. Denna intervalltid går att justera via ett kommando till mikroprocessorn. Den tid det tar för sensorerna att utföra en mätning kan minskas genom att använda en kortare range time och en lägre uteffekt (gain). Sensorernas placering på fordonet spelar naturligtvis en stor roll för överhörningsproblemen. En alternativ metod som går ut på att att avfyra alla sensorerna i princip samtidigt har även utvärderats i experiment som redovisas i avsnitt 6.3.2. 2.6 Programvara Programvaran för den bärbara datorn är utvecklad i C++ i en GNU/Linux-miljö för X- windows. De flesta funktionerna finns implementerade i den bärbara datorn eftersom de är relativt beräkningskrävande. Systemdesignen som utvecklades i projektet Collision Avoidance bygger på användningen av flera processer som körs samtidigt på den bärbara datorn. Dessa processer kommunicerar med varandra via TCP internt på datorn. Processerna (servrar eller moduler) är tänkta att kunna utlokaliseras till olika datorer i ett nätverk (se den tekniska dokumentationen [16] för mer information och bakgrund till designen). I arbetet har vissa moduler från projektet strukturerats om medan andra har använts mer eller mindre oförändrade. De moduler som har använts i detta arbete, samt i vissa fall modifierats, beskrivs kort nedan. Systemet och dess processer illustreras även i figur 2.4.

2.6 Programvara 11 Figur 2.4. Mjukvaran på PC:n är indelad i fristående moduler som kommunicerar via TCP. Kommunikationen mellan PC och mikroprocessor sker seriellt via RS-232. På mikroprocessorn körs ett enda program. Styr- & Mätmodul Denna modul ansvarar för kommunikationen med mikroprocessorn. I denna modul finns även den regulator som ser till att fordonet kan följa en given bana. Här samplas och lagras information från fordonet. Kartserver Denna modul har till uppgift att förse bangenereringen, partikelfiltret och användargränssnittet med information om fordonets omgivning, exempelvis avstånd till väggar. Här genereras information om omgivningen från en bild och det är möjligt för moduler att arbeta med omgivningen i olika upplösningar för att spara tid om kraven på upplösning är låga. Avståndsmätningar i kartan sker iterativt. Denna modul är direkt hämtad från projektet Collision Avoidance. Bangenerering Modulen använder sig av den så kallade A*-algoritmen med Manhattanmetoden för att hitta en väg mellan två punkter. Algoritmen söker den kortaste vägen mellan två punkter direkt i kartan. Se den tekniska dokumentationen [16] för mer information om algoritmen. Den arbetar med en lägre upplösning än partikelfiltret vilket reducerar beräkningstiden. Den genererade banan reduceras sedan ner till ett antal linjesegment som kan användas i banföljningen.

12 Systemöversikt Partikelfilter och användargränssnitt Partikelfiltret hör samman med det grafiska användargränssnittet vilket reducerar mängden information som slussas mellan modulerna. Här uppdateras partikelfiltret och partiklarna visas grafiskt på skärmen tillsammans med kartan och filtrets skattning av fordonets position. Användargränssnittet ger användaren möjlighet att placera ut fordonet i kartan samt markera ut ett mål för fordonet.

Kapitel 3 Navigering 3.1 Navigeringsproblemet För att vid en viss tidpunkt finna fordonets aktuella position måste information från olika sensorer användas. Detta är ett grundläggande problem som återkommer i många tillämpningar. Eftersom sensorerna och mätningarna aldrig är helt perfekta görs en skattning av fordonets verkliga position. Vid robotlokalisering används ofta en på förhand känd eller delvis känd karta över fordonets omgivning i kombination med avståndssensorer. Det finns även en teknik som kallas SLAM (Simultaneous Localisation And Mapping) eller CML (Concurrent Mapping and Localisation) där kartan genereras samtidigt som roboten lokaliserar sig. Kombinationer där en på förhand given karta förbättras eller ändras beroende på mätningar i omgivningen kan också användas. Nedan ges en kort genomgång av principen för bayesiansk teori och speciellt partikelfilter. I avsnitt 3.4 presenteras den implementerade navigeringslösningen. Presentationen av teorin i detta avsnitt är inspirerad av [14], [17] och [5], rapporter som även rekommenderas för djupare studier av partikelfilter och dess tillämpningar. 3.2 Bayesiansk statistik Det finns två vanliga sätt att se på det tillstånd som ska skattas. Enligt Fisher är tillståndet en okänd deterministisk storhet medan tillstånden i bayesiansk statistik ses som stokastiska variabler [17]. Antag att vi har en generell diskret olinjär modell enligt x t+1 = f (x t,w t ) (3.1) y t = h(x t,e t ) (3.2) där x t R n är de okända tillstånd som ska skattas, w t är modellfel och e t representerar mätfel. 13

14 Navigering Den bayesianska statistiken utgår från antagandet om att man har en på förhand känd fördelningsfunktion för tillstånden, en apriori-fördelning(a priori = det föregående), p(x). Den innehåller all information om tillstånden innan tiden t. Från p(x) tillsammans med mätningarna y(t) erhålls sedan en korrigerad aposteriori-fördelning(a posteriori = det efterföljande), p(x y). Sambandet mellan dessa ges av Bayes sats för betingad sannolikhet. 3.2.1 Bayes sats Bayes sats för x givet y ges av p(x y) = p(y x) p(x) p(y) (3.3) För en hel serie av mätningar, Y t, ges fördelningen på samma sätt av p(x Y t ) = p(y t x) p(x) p(y t ) (3.4) 3.2.2 Modellapproximationer Målet är att rekursivt kunna beräkna en optimal skattning för x. För att erhålla en sådan skattning används ofta modellapproximationer av olika slag tillsammans med numeriska approximationer. Här används en modellapproximation som vanligen kallas markovantagandet. Detta innebär att varje mätning, y t är obetingat oberoende av de tidigare mätningarna Y t 1 givet tillståndet x t, det vill säga p(y t x t,y t 1 ) = p(y t x t ). Antagandet görs även för tillstånden, p(x t+1 x t,y t 1 ) = p(x t+1 x t ), vilket innebär att tillståndet i varje tidpunkt innehåller tillräcklig information om systemet. En diskret rekursiv variant av (3.4) kan ges av en tidsuppdatering, ekvation (3.5), och en mätuppdatering, ekvation (3.6), med hjälp av Bayes sats samt markovantagandet ovan, se till exempel [17] för beräkningar. p(x t+1 Y t ) = R n p(x t+1 x t ) p(x t Y t ) dx t (3.5) p(x t Y t ) = p(y t x t ) p(x t Y t 1 ) p(y t Y t 1 ) Nämnaren i (3.6) kan även skrivas som en integral på formen (3.5). (3.6) Dessa ekvationer är grunden i den bayesianska teorin och kan användas för att ta fram ett estimat för de okända tillstånden x med någon estimeringsmetod. Integraler av den typ som återfinns ovan är svåra att lösa analytiskt. Ett specialfall är då tillstånden är linjära och alla störningar antas normalfördelade. Lösningen ges då av Kalmanfiltret. Andra modellapproximationer leder till andra typer av lösningar. Dessa behandlas dock inte här. Problemets lösning erhålls istället med hjälp av en numerisk approximation av integralerna i ekvation (3.5).

3.2 Bayesiansk statistik 15 3.2.3 Numerisk integration Integralerna i avsnitt 3.2.2 är på formen I = E[g(x)] = g(x)p(x)dx (3.7) En klass av metoder för att lösa numeriska integraler av denna typ är de så kallade Monte Carlo-metoderna [5]. Många av dessa bygger på att man kan dra sampel direkt från fördelningsfunktionen p(x). Detta är dock generellt inte möjligt varför man utvecklat alternativa metoder vilka blivit byggstenar i partikelfiltret. En sådan metod är Importance Sampling (IS) i vilken man drar sampel från en godtycklig fördelning π(x t Y t ). Integralen I kan då skrivas som I(g) = E[g(X t )] = g(x t )p(x t Y t )dx = där man definierar viktfaktorn q(x t ) som q(x t ) = p(x t Y t ) π(x t Y t ) Täljaren kan skrivas om med hjälp av Bayes sats enligt p(x t Y t ) = p(y t X t ) p(x t ) p(y t ) g(x t ) p(x t Y t ) π(x t Y t ) π(x t Y t )dx (3.8) (3.9) (3.10) Problemet är här att det ofta inte går att hitta ett uttryck för normeringsfaktorn p(y t ) vilket gör att vi känner viktfaktorn sånär som på en konstant. Därför normerar man viktfaktorerna så att de summerar till den totala sannolikheten. Om vi nu kan dra N stycken oberoende och lika fördelade sampel från π(x t Y t ), så kan integralen I approximeras enligt teorin för perfekt sampling, [5]. Î(g) N = N i=1 g(x i)q(x i ) N i=1 q(x i) där q(x i ) är den normerade viktfaktorn = N i=1 q(x t ) = q(x i) N i=1 q(x i) g(x i ) q(x i ) (3.11) (3.12) Metoden ovan kräver dock fortfarande att vi för varje ny uppdatering utvärderar alla tidpunkter till och med tiden t. Man antar därför att fördelningen π(x t Y t ) är rekursiv enligt π(x t Y t ) = π(x t X t 1,Y t )π(x t 1 Y t 1 ). Dessutom utnyttjar man markovantagandet, π(x t X t 1,Y t ) = π(x t x t 1 ). Detta gör att man kan härleda en rekursiv, eller sekvensiell, variant av importance sampling som då kallas Sequential Importance Sampling (SIS) eftersom man mellan två tidpunkter bara behöver spara det föregående tillståndet. Varje vikt uppdateras då för varje ny mätning y t enligt q t = p(x t Y t ) π(x t Y t ) p(y t x t )p(x t x t 1 ) q t 1 (3.13) π(x t x t 1,y t )

16 Navigering Ett problem återstår nu. Det kan nämligen visas att variansen vid importance sampling ökar med tiden vilket gör att lösningen kommer att divergera. 3.3 Partikelfilter Partikelfilter eller Sequential Monte Carlo Methods använder en diskret svärm av N stycken partiklar x (i) för att skatta tillståndet. Algoritmen återges i Algoritm 1. Här { } N i=1 använder man ett omsamplingssteg där de sampel som är de mest sannolika omsamplas. Därmed löses problemet med divergens. Den metod som används här kallas SIR (Sampling Importance Resampling) eller Bootstrap filter. Här ansätts speciellt π(x t Y t ) = p(x t Y t ) vilket gör att varje partikelns viktfaktor uppdateras enligt q (i) t = p(y t x (i) t )q (i) t 1 (3.14) I metoden ansätts en apriori-fördelning för partiklarna vid initieringen av filtret som sedan uppdateras med hjälp av mätningarna y(t) i en mätuppdatering. Varje partikel erhåller då en vikt q(t). Därefter omsamplas N stycken nya partiklar där sannolikheten för att partikel i omsamplas är q (i) t. De nya partiklarna samplas alltså från den approximativa posteriori-fördelningen. Detta gör att partiklar med låg sannolikhet inte omsamplas och de med hög sannolikhet omsamplas flera gånger. Slutligen följer en tidsuppdatering, där de nya partiklarna predikteras med hjälp av (3.5). I detta steg adderas också normalfördelat brus w t vilket sprider ut partiklarna i tillståndsrummet. Detta görs eftersom omsamplingssteget leder till att partiklarna kommer att skilja sig från varandra mindre och mindre för varje iteration. Partiklar med hög sannolikhet kommer på detta sätt få större och större vikt. Den minskade mångfalden av partiklar kommer av att sampel i omsamplingssteget dras från en diskret fördelningsfunktion istället för en kontinuerlig, [17]. Algoritm 1 SIR(Sampling Importance Resampling) 1. Initiering: { } Generera N partiklar x (i) N 0 enligt den initiala fördelningen p 0 i=1 2. Mätuppdatering: Beräkna viktfaktorerna för varje partikel enligt q (i) t p(y t x (i) t )q (i) t 1 Normalisera viktfaktorerna q (i) t = q(i) t N i=1 q(i) t 3. Omsampling: { } N Generera N nya partiklar x (i) enligt Pr(x (i) t = x (i) ) = q (i) t i=1 4. Tidsuppdatering: Prediktera nya partiklar enligt modellekvationen (3.1): x (i) t+1 = f(x (i) t,w t ) 5. Sätt t = t + 1 och gå till steg 2

3.4 Implementering 17 Partikelfiltret är en diskret metod där tillståndsrummet är uppdelat i diskreta steg. Metoden löser alltså det olinjära problemet approximativt i stället för att lösa ett approximerat problem optimalt. En nackdel med metoden är att om tillståndsrummet har hög dimension krävs många partiklar för att få en bra representation av tillståndsrummet vilket då blir beräkningskrävande. Metoden lämpar sig väl för problem med få tillstånd men komplexiteten ökar drastiskt för problem av högre ordning. Metoder finns vilka minskar komplexiteten vid högre ordningens problem. Dessa använder kalmanfilter för de delar av problemet som är linjära. Metoden kallas då för marginaliserat partikelfilter, [17]. 3.4 Implementering När partikelfiltret nu används för positionering kan en del av teorin i avsnitt 3.2 och 3.3 konkretiseras. I det här avsnittet presenteras den implementerade navigeringslösningen. 3.4.1 Översikt För att fordonets tillstånd ska få en så aktuell skattning som möjligt delas här partikelfilteralgoritmen upp i två delar: mätuppdatering och tidsuppdatering. Den ena delen av algoritmen används när ny odometridata har inkommit. Information hämtas här från pulsgivarna på motoraxlarna. Denna del kallas här för tidsuppdatering, T. Tidsuppdateringen sker med den så kallade rörelsemodellen i avsnitt 3.4.4. Partikelfiltrets osäkerhet ökar för varje tidsuppdatering. Den andra typen av uppdatering är ny information som säger något om partiklarnas sannolikhet. Här utgörs informationen av avståndsmätningar från ultraljudssensorer. Denna del kallas för mätuppdatering, M. Den nya informationen minskar i de flesta fall filtrets osäkerhet om fordonets tillstånd. Mätuppdatering görs med den så kallade sensormodellen vilken beskrivs närmare i avsnitt 3.4.3. Modulerna och dess kommunikation återges i figur 3.1. De två stegen kan appliceras var för sig. För att fånga upp fordonets dynamik samplas värden från mikroprocessorn till den bärbara datorn med en hastighet av 10 Hz. Datainsamlingen sker i en egen process på den bärbara datorn vilken här kallas Styr & Mätmodulen. Partikelfiltret arbetar tillsammans med användargränssnittet i en separat process för att minska informationsflödet mellan de olika processerna. I figur 3.2 visas förfarandet för partikelfiltret som helhet. Filtret initieras grafiskt genom att användaren placerar ut fordonet på kartan och ger det en riktning. Initialhastigheten antas vara noll. Partiklarna sprids sedan enligt en initial osäkerhet. Partikelfiltret hämtar vidare ultraljudssensorernas mätresultat från Styr & Mätmodulen och gör en mätuppdatering. Den tid det tar för filtret att göra detta är M enligt notationen i figur 3.2. Under tiden som detta sker samlas ett antal odometridata in av Styr & Mätmodulen. När partikelfiltret är klart med mätuppdateringen skickas den insamlade odometridatan till filtret som då gör lika många tidsuppdateringar. Dessa n stycken uppdateringar tar tiden T n att utföra. När uppdateringarna är gjorda kommer, eftersom mätuppdateringar går relativt

18 Navigering Figur 3.1. Blockschema över navigeringsimplementeringen. Här visas flödet av mätdata från sensorerna via mikroprocessorn och Styr & Mätmodulen till partikelfiltret. Partikelfiltret beräknar en tillståndsskattning för fordonet med hjälp av sensordata. fort, partikelfiltrets skattning vara som mest aktuell. Därför levererar partikelfiltret nu sin skattning tillbaka till Styr & Mätmodulen samt påbörjar en ny mätuppdatering. Den Figur 3.2. Tidsaxel för partikelfiltret. Filtret genomför först en mätuppdatering. Sedan gör filtret n stycken tidsuppdateringar för att slutligen leverera en skattning för fordonets tillstånd till systemet. Partikelfiltret startar sedan en ny beräkning med nya data. skattade tillståndsvektorn får vi genom att medelvärdesbilda värdena för de omsamplade partiklarna. Tillståndet Ψ, fordonets kurs (se figur 3.3), kan inte medelvärdesbildas direkt eftersom vinkeln ±π och vinkeln 0 inte tar ut varandra. För att kursskattningen ska bli korrekt måste vinklarnas x- och y-komposanter medelvärdesbildas var för sig.

3.4 Implementering 19 3.4.2 Omvärldsrepresentation Omgivningen kan representeras på flera olika sätt vilka är mer eller mindre komplexa och därför lämpar sig för olika tillämpningar. Inom detta arbete representeras fordonets omgivning med hjälp av en enkel pixelkarta med två färger. Den ena färgen betyder hinder och den andra inget hinder. Omvärlden i kartan är statisk, det vill säga den förändrar sig inte med tiden. Detta gör att det i omvärldsrepresentationen inte finns något stöd för dynamiska händelser. Den andra avgränsningen i avsnitt 1.2 som antar att fordonets omgivning kan förändras med tiden tas istället hänsyn till med hjälp av ultraljudssensorernas modell i avsnitt 3.4.3. Med den representation av omgivningen som är gjord är sökningar i kartan de mest beräkningskrävande anropen varför antalet sådana bör minimeras. I en tvådimensionell värld är det naturligt att använda positionen (X, Y) och kursen Ψ för att representera fordonets tillstånd. Positionen refererar till ett fixt koordinatsystem, (e X,e Y ), i omgivningen enligt figur 3.3. Fordonets hastighet v införs som ytterligare ett Figur 3.3. Definition av det fixa koordinatsystemet (e X,e Y ) för fordonet till vänster. (e x,e y ) används för att ge sensorerna en position relativt fordonet. ϕ definieras till höger som vinkeln mellan sensorns utbredningsriktning och fordonets e x -axel. tillstånd i syfte att underlätta designen av olika banregulatorer. Se vidare kapitel 5. Fordonets tillstånd vid en given tidpunkt betecknas x t. Fordonets position är inte känd utan endast skattad ˆx t = ( ˆX,Ŷ, ˆΨ, ˆv ). Koordinatsystemet (e x,e y,ϕ) används för att ge avståndssensorerna en position relativt fordonets centrum. ϕ definieras som vinkeln mellan sensorns utbredningsriktning och fordonets e x -axel. Sensorernas placering och riktning används för att göra mätningar i kartan.

20 Navigering 3.4.3 Sensormodell En av de viktigare aspekterna när det gäller att positionera fordonet är att ha bra men samtidigt beräkningseffektiva modeller för fordonets sensorer, speciellt eftersom vi ska använda partikelfiltret i realtid. En vanlig ansats för enkla avståndssensorer är att jämföra sensorns mätvärde, y t, med en modells mätning i kartan, ett förväntat värde, givet en position från x t för att erhålla p(y t x t ). Eftersom information från ultraljudssensorerna är begränsad kan det vara svårt att införa en avancerad modell för hur det förväntade värdet ska erhållas. Här har utbredningsloben modellerats med tre stycken strålar, en i centrum och två mätningar vid ±β enligt figur 3.4. Eftersom sensorerna svarar med avståndet till det närmaste objektet i loben får minimum av dessa tre strålar representera det förväntade värdet. Problemet har visat sig vara att sensorer med ett stort β kommer att svara med samma värde, det närmaste inom hela loben, medan mätningen i kartan skiljer sig åt när sensorns vinkel till föremålet varieras. Den nuvarande sensormodellen levererar alltså olika avstånd för olika infallsvinklar mot ett föremål medan de verkliga sensorerna inte gör det i samma utsträckning. De sensorer som används här har ett β som är stort, ungefär 36, se avsnitt 2.5. Detta gör att sensorns och därmed fordonets riktningsförändringar inte går att observera. Den högra bilden i figur 3.4 visar principen för modellen med en osäkerhet i mätningen markerad med en cirkel. En första ansats var att göra en sökning i kartan längs en enda stråle centrerad till mitten av utbredningsloben. Osäkerheten i vinkeln α modellerades alltså direkt i partikelfiltret som en osäkerhet i avstånd r till ett objekt som antas finnas i utbredningsnormalens riktning. För att erhålla en mer exakt relation mellan kartmätningen och sensorinformationen måste osäkerheten i mätningen göras mycket stor. Osäkerheten visas som cirklar i figuren. Denna modell använder inte informationen om hur sensorerna fungerar på samma sätt som den nu använda modellen. Osäkerheten i modellen blir därmed stor och ger inte någon bra observerbarhet för riktningsförändringar. Modellen illustreras i den vänstra bilden av figur 3.4. Figur 3.4. Två modeller för hur ultraljudssensorerna fungerar. Till vänster en enkel modell med en centrerad stråle och till höger en modell med tre strålar där den kortaste får representera vår förväntade mätning. Osäkerheten för mätningarna är markerade med cirklar.

3.4 Implementering 21 Med en mer avancerad representation av omgivningen skulle problemet med reflektioner och låg infallsvinkel kunna lösas genom att strålarnas reflektioner mot väggar och andra föremål följs. I realtid blir detta svårt med den nu valda omvärldsrepresentationen eftersom det krävs för många beräkningskrävande iterativa beräkningar. Efter att det förväntade mätvärdet erhållits från kartan och sensormodellen används en fördelningsfunktion för att beräkna sensorernas sannolikhet. Partikelns viktfunktion erhålls som en produkt av sensorernas sannolikheter och föregående tidsstegs viktfunktion. Detta eftersom mätningarna ses som oberoende enligt markovantagandet q t = M n=1 Ultraljudssensorernas fördelningsfunktion p(y (n) t x t )q t 1 (3.15) Fördelningsfunktionen p(y t x t ), där skillnaden mellan det i kartan uppmätta avståndet och sensorns faktiska mätning ger sannolikheten för partikeln, bygger ofta i grunden på en gaussisk funktion. Till denna finns dock en mängd ansatser och kombinationer. I detta arbete används en gaussisk funktion som ger en sannolikhet för att sensorernas avstånd, d s, skiljer sig åt i en omgivning från det förväntade avståndet d map. Här kan osäkerheten byggas in i modellen av ultraljudssensorernas utbredningslob. Eftersom en viss sannolikhet finns för att sensorerna inte detekterar något föremål inom sin räckvidd ansätts dessutom en sannolikhet för detta. Sensorernas maximala räckvidd betecknas d max. Exempel på en möjlig fördelningsfunktion visas i figur 3.5. 0.12 Fördelningsfunktion för ultraljudssensorer 0.1 sannolikhet p(y t x t ) 0.08 0.06 0.04 d map d max 0.02 0 0 50 100 150 200 250 d [cm] s Figur 3.5. Sannolikheten för tillståndet x t givet en mätning y t. Sensorns uppmätta avstånd, d s, jämförs med ett förväntat avstånd, d map, uppmätt i kartan.

22 Navigering För att komma tillrätta med det statiska antagandet av miljön ansätts även en liten sannolikhet att sensorerna mäter ett värde som är kortare än förväntat. Detta skulle kunna komma från en reflektion från en annan sensor eller från ett föremål som inte finns med i kartan från början. Modell för hastigheten Fordonets tillstånd modelleras direkt som den uppmätta medelhastigheten i varje intervall med ett adderat normalfördelat brus. 3.4.4 Rörelsemodell Eftersom fordonet har två individuellt drivna framhjul och ett ledat bakhjul används här en modell för fordonet enligt till exempel [2]. Information om hur fordonet rör sig erhålls från pulsgivarna på fordonets hjul. Det totala antalet pulser räknas här med tecken för respektive hjul. En noggrann räknare på mikroprocessorn används dessutom för att mäta tiden. Med hjälp av skillnaderna mellan dessa räknare i vissa tidsintervall, T, erhåller vi en rotation, en linjär translation och en medelhastighet. Sambandet mellan antalet pulser från pulsgivarna och hjulens förflyttning ges av k s = πd n m k p (3.16) där d är hjulets diameter, n m är den utväxling som växellådan har och k p är antalet pulser som pulsgivarna ger per motoraxelvarv. Skillnaden i förflyttning mellan två sampeltillfällen för vänster respektive höger hjul, U V/H,t, ges sedan av U V/H,t = k s N V/H,t (3.17) där N V/H,t är det antal pulser som pulsgivaren på respektive hjul givit inom intervallet T. Den linjära förflyttningen U t samt kursförändringen Ψ t för fordonets mittpunkt över hjulaxeln blir då U t = U V,t + U H,t 2 Ψ t = U V,t U H,t b (3.18) (3.19) där b är avståndet mellan de båda drivande hjulen. Fordonets nya position vid tidpunkten t beräknas som Ψ t = Ψ t 1 + Ψ t (3.20) X t = X t 1 + U t cos(ψ t ) (3.21) Y t = Y t 1 + U t sin(ψ t ) (3.22) För att inte riskera att gå miste om information beräknas skillnaden för räknartillstånden mellan två sampelperioder T i den bärbara datorn. Om ett sampeltillfälle faller bort i

3.4 Implementering 23 den seriella kommunikationen mellan mikroprocessorn och den bärbara datorn kommer det summerade antalet pulser ändå att stämma eftersom de missade pulserna räknas med vid nästa sampeltillfälle. Detta gör att fordonets totala förflyttning enligt pulsgivarna blir korrekt. Modellen tar inte hänsyn till de fel som uppstår i de fall hjulen slirar eller det faktum att fordonet förmodligen inte roterar runt mittpunkten över hjulaxeln. Då hjulen är av hårt gummi tas ingen hänsyn till deformation av hjulen i fordonsmodellen. Medelhastigheten för varje hjul i tidsintervallet T beräknas även den med hjälp av antalet pulser i intervallet. Här används den snabba räknaren för att noggrant mäta tiden mellan första och sista pulsen i intervallet. På så sätt erhålls en precis angivelse för medelhastigheten. Fordonets hastighet, v, beräknas som v t = v V,t + v H,t 2 (3.23) där v V,t och v H,t är respektive hjuls medelhastighet. Dessa beräknas med hjälp av S V/H som är höger respektive vänster hjuls sträcka per tick och tiden T V/H,t vilken är tiden mellan första och sista tick i N V/H,t enligt v V/H,t = N V/H,t S V/H T V/H,t (3.24) Uppdateringen av fordonets hastighet görs direkt med hjälp av den nuvarande medelhastigheten v enligt v t = v. Tidsuppdateringssteget i algoritmen har också funktionen att sprida ut partiklarna enligt avsnitt 3.3. Detta görs genom att addera normalfördelat brus till modellekvationerna. För att minska kursens drift i modellen ansätts här förändringen i kurs som medelvärdet av kursförändringen mellan två sampeltillfällen. Osäkerheten i modellen modelleras som en hastighetsberoende del och en del som alltid verkar på partiklarna. Vi skriver dessa storheter som funktioner av sampeltiden T för att enkelt kunna ändra den frekvens med vilken vi samplar fordonets rörelser. Hela uppdateringsförfarandet blir således v t = v+w v (3.25) Ψ t = Ψ t 1 + Ψ t + w Ψ (3.26) U = U t + v t w kv (3.27) X t Y t = X t 1 + Ucos( Ψ t + Ψ t 1 )+w U (3.28) 2 = Y t 1 + Usin( Ψ t + Ψ t 1 )+w U (3.29) 2 där v är den nuvarande medelhastigheten som kommer från sensorerna. Variabeln w kv är normalfördelad och stokastisk med standardavvikelse d kv och medelvärde 0, det vill

24 Navigering säga w v N(0,d kv ). Faktorn v t w kv blir därför den storhet som representerar osäkerheten i förflyttningen på grund av hastigheten. Variablerna w v N(0,d v ), w Ψ N(0,d Ψ ) och w U N(0,d U ). Standardavvikelserna bildas som en konstant multiplicerat med sampeltiden T. Störningar i modellen Fel i hjulens diameter, och därmed storheten sträcka per puls, spelar en viktig roll för modellen. Ett fel i denna storhet gör att fordonets positionsskattning får ett fel som växer med tiden. Om hjulens diameter är exakt 20cm blir hjulens omkrets 62,83cm. En fel i diameter på 1cm ger ett fel på 5% i total sträcka. Tabell 3.1 visar resultatet från en mätning av hjulen samt det fel detta innebär jämfört med den ideala omkreten. Hjul Ideal omkrets [cm] Omkrets [cm] Fel [%] Vänster 62,83 64,2 2,13 Höger 62,83 64,5 2,57 Tabell 3.1. Hjulens uppmätta omkrets som används i fordonets rörelsemodell jämfört med hjulens ideala omkrets. Tabellen visar att hjulen har olika omkrets. Exakta data för varje hjul bör användas i modellen för att undvika fel. En annan felkälla är om hjulen glider mot underlaget. För medelhastigheten spelar även upplösningen på räknaren en stor roll. Tiden kan bestämmas exakt eftersom pulsgivarna är kopplade till avbrottsingångar på mikroprocessorn. Sambandet mellan antalet räknarsteg mellan två pulser och hjulens hastighet ges av k hast = f T πd n m k p (3.30) där f T är den frekvens som räknaren uppdateras med, d är hjulets diameter, n m är den utväxling som växellådan har och k p är antalet pulser per motoraxelvarv som pulsgivarna ger. Räknaren i mikroprocessorn arbetar med en frekvens på 14,4kHz vilket ger en tidsupplösning på ungefär 70 µs. Detta ger, med en hjuldiameter på 20cm det vill säga 8, 38 mm i förflyttning per puls samt ekvation (3.19), en teoretisk upplösning på ungefär 8,3 µm/s. En annan parameter i modellen som spelar en viktig roll för tillståndsskattningen är fordonets hjulbas b. Denna påverkar direkt tillståndet Ψ via Ψ och därmed även positionsskattningen. Om hjulbasen skiljer sig från det sanna värdet kommer modellens skattning av positionen att växa med tiden, speciellt när fordonet inte kör längs en rät linje.