Navigering och styrning av ett autonomt markfordon

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 tekniska högskola

Linköpings universitet Linköpings universitet

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: Daniel Axehill

ISY, Linköpings universitet

Torbjörn Crona

Saab Bofors Dynamics

Pelle Carlbom

Saab Bofors Dynamics

Examinator: Anders Hansson

ISY, Linköpings universitet

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  ⊠

URL för elektronisk version

http://www.control.isy.liu.se http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-6006 ISBN — ISRN LiTH-ISY-EX--06/3796--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

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

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 pos-sible 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

for-don, 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än-das 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 till-stå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.

Tack

Det här examensarbetet hade aldrig blivit till utan hjälp och stöd från ett flertal person-er. 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

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.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 om-givningen. 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 ul-traljudssensorer 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.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 Con-trol hos Saab Bofors Dynamics i Linköping. Till att börja med studerades ett antal rap-porter, 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 mikro-processor. 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 ba-na. 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 ur-sprungliga 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äs-ning. I kapitel 3 presenteras problemet med navigering av fordonet. Kapitel 4 behandlar modellering och reglering av fordonets motorer. Kapitel 5 behandlar banföljningsprob-lemet och implementeringen av en regulator för detta. I kapitel 6 presenteras och analy-seras praktiska resultat. Kapitel 7 innehåller slutsatser. I kapitel 8 ges slutligen förslag på framtida arbete.

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 realis-erades. 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är-bara datorn. Projektet finns beskrivet i den tekniska dokumentationen [16]. En skiss över

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äs-ning. 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 I2C-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 pro-cessorns datablad [1]. I arbetet med det autonoma fordonet klockas processorn från en extern klockkälla på 3, 6864 MHz.

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 program-meringen. 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 rep-resentation. Detta sparar minnesplats och minskar kommunikationsflödet mellan mikro-processorn och den bärbara datorn. Kommunikationen mellan den bärbara datorn och mikroprocessorn sker seriellt med en hastighet av 115200 bitar per sekund. En komman-doö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 diam-eter på 20 cm, ett nav av plast, och en slityta av hårdgummi. Det bakre stödhjulet är ett mindre ledat hjul av polypropylen.

Belastningen från 10 kg utrustning och 5 kg chassi gör att motorerna måste klara en axiell belastning av minst 75 N 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 1 m/s (3, 6 km/h) och

en acceleration av 0, 5 m/s2_{. Detta innebär med en hjuldiameter på 20 cm att vridmomentet}

vid maximal acceleration blir M= m · a · r ≤ 7, 5 · 0, 5 · 0, 1 = 0, 375 Nm 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 an-vänds magnetiska pulsgivare, Hallgivare, som är monterade på motoraxlarna. Hallgivarna på motorerna ger tre pulser per motorvarv vilket motsvarar 75 pulser per hjulvarv efter-som 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å 20 cm ger pulsgivarna en puls per 8, 4 mm

förflyt-tning.

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 ljud-puls 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

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− 50 kHz 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 bil-dar 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äker-heten 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

ob-jektet.

Fördelarna med ultraljudssensorer är att de är små, robusta och drar lite ström. Sen-sortypen 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 po-sitionsbestä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 posi-tioneringsproblemet, 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 ut-bredningsloben 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 I2C. Med hjälp av I2C 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 sensor-modulen. 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 algo-ritmer 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.

intervallet 3 cm till 6 m. Den längsta tid som sensorn behöver för en mätning är unge-fär 65 ms. 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. Kalibrerin-gen av sensorerna är enligt tillverkaren utförda vid 24◦och man antar en ljudhastighet på 346 m/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 tem-peraturförändring från till exempel 18◦till 20◦ger ett mätfel på cirka 3, 5 mm på 1 meter.

Ä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 for-donets 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är-fö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ät-ningar 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· tsms där n är antalet

sensorer och ts 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 ex-periment 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. Pro-cesserna (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. Kom-munikationen 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änss-nittet 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.

Partikelfilter och användargränssnitt

Partikelfiltret hör samman med det grafiska användargränssnittet vilket reducerar mäng-den information som slussas mellan modulerna. Här uppdateras partikelfiltret och partik-larna 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ämp-ningar. 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 for-donets omgivning i kombination med avståndssensorer. Det finns även en teknik som kallas SLAM (Simultaneous Localisation And Mapping) eller CML (Concurrent Map-ping and Localisation) där kartan genereras samtidigt som roboten lokaliserar sig. Kom-binationer 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 partikelfil-ter. 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 rekom-menderas 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 till-stå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

xt+1 = f(xt, wt) (3.1)

yt = h (xt, et) (3.2)

där xt ∈ Rnär de okända tillstånd som ska skattas, wt är modellfel och et representerar

mätfel.

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

sanno-likhet.

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,Yt, ges fördelningen på samma sätt av

p(x|Yt) =

p(Yt|x) p (x)

p(Yt)

(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 markovan-tagandet. Detta innebär att varje mätning, yt är obetingat oberoende av de tidigare

mät-ningarnaYt−1givet tillståndet xt, det vill säga p(yt|xt, Yt−1) = p (yt|xt). Antagandet görs

även för tillstånden, p(xt+1|xt, Yt−1) = p (xt+1|xt), vilket innebär att tillståndet i varje

tid-punkt 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(xt+1|Yt) = Z Rn p(xt+1|xt) p (xt|Yt) dxt (3.5) p(xt|Yt) = p(yt|xt) p (xt|Yt−1) p(yt|Yt−1) (3.6) Nämnaren i (3.6) kan även skrivas som en integral på formen (3.5).

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)] =

Z

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

al-ternativa 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π(Xt|Yt).

Inte-gralen I kan då skrivas som

I(g) = E [g(Xt)] = Z g(Xt)p(Xt|Yt)dx = Z g(Xt) p(Xt|Yt) π(Xt|Yt)π(Xt|Yt )dx (3.8)

där man definierar viktfaktorn q(Xt) som

q(Xt) =

p(Xt|Yt)

π(Xt|Yt)

(3.9) Täljaren kan skrivas om med hjälp av Bayes sats enligt

p(Xt|Yt) =

p(Yt|Xt) p (Xt)

p(Yt)

(3.10) Problemet är här att det ofta inte går att hitta ett uttryck för normeringsfaktorn p(Yt)

vilket gör att vi känner viktfaktorn sånär som på en konstant. Därför normerar man vikt-faktorerna så att de summerar till den totala sannolikheten. Om vi nu kan dra N stycken oberoende och lika fördelade sampel från π(Xt|Yt), så kan integralen I approximeras

enligt teorin för perfekt sampling, [5]. ˆ I(g)N=∑ N i=1g(Xi)q(Xi) ∑N i=1q(Xi) = N

∑

i=1 g(Xi) ˜q(Xi) (3.11)

där ˜q(Xi) är den normerade viktfaktorn

˜ q(Xt) = q(Xi) ∑N i=1q(Xi) (3.12) Metoden ovan kräver dock fortfarande att vi för varje ny uppdatering utvärderar alla tid-punkter till och med tiden t. Man antar därför att fördelningen π(Xt|Yt) är rekursiv

enligtπ(Xt|Yt) =π(xt|Xt−1, Yt)π(Xt−1|Yt−1). Dessutom utnyttjar man

markovantagan-det,π(xt|Xt−1, Yt) =π(xt|xt−1). Detta gör att man kan härleda en rekursiv, eller

sekven-siell, 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 yt enligt

qt= p(Xt|Yt) π(Xt|Yt) ∝ p(yt|xt)p(xt|xt−1) π(xt|xt−1, yt) qt−1 (3.13)

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 sty-cken partiklarnx(i)o

N

i=1för att skatta tillståndet. Algoritmen återges i Algoritm 1. Här

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

(Sam-pling Importance Resam(Sam-pling) eller Bootstrap filter. Här ansätts specielltπ(Xt|Yt) =

p(Xt|Yt) vilket gör att varje partikelns viktfaktor uppdateras enligt

q_t(i)= p(yt|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 predik-teras med hjälp av (3.5). I detta steg adderas också normalfördelat brus wtvilket 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. Par-tiklar 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 partiklarnx(i)₀ oN

i=1enligt den initiala fördelningen p0

2. Mätuppdatering:

Beräkna viktfaktorerna för varje partikel enligt q_t(i)∝p(yt|x(i)t )q

(i)

t−1

Normalisera viktfaktorerna ˜q(i)_t = q(i)t

∑N

i=1q

(i)

t

3. Omsampling:

Generera N nya partiklarnx∗(i)o

N i=1enligt Pr(x ∗(i) t = x(i)) = q (i) t 4. Tidsuppdatering:

Prediktera nya partiklar enligt modellekvationen (3.1): x(i)_t+1= f (xt∗(i), wt)

3.4 Implementering 17

Partikelfiltret är en diskret metod där tillståndsrummet är uppdelat i diskreta steg. Meto-den 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 dimen-sion 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 plexiteten ökar drastiskt för problem av högre ordning. Metoder finns vilka minskar kom-plexiteten 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 partikelfil-teralgoritmen 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 puls-givarna 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 san-nolikhet. 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 sensormod-ellen 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 till-sammans 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ätuppda-tering. 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 par-tikelfiltret ä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 Tnatt

Figur 3.1. Blockschema över navigeringsimplementeringen. Här visas flödet av mätdata från sen-sorerna 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 om-givning 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 om-givning 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,

(eX, eY), i omgivningen enligt figur 3.3. Fordonets hastighet v införs som ytterligare ett

Figur 3.3. Definition av det fixa koordinatsystemet(eX, eY) för fordonet till vänster. (ex, ey)

an-vänds för att ge sensorerna en position relativt fordonet.ϕdefinieras till höger som vinkeln mellan sensorns utbredningsriktning och fordonets ex-axel.

tillstånd i syfte att underlätta designen av olika banregulatorer. Se vidare kapitel 5. For-donets tillstånd vid en given tidpunkt betecknas xt. Fordonets position är inte känd utan

endast skattad ˆxt= ˆX, ˆY, ˆΨ, ˆv
.

Koordinatsystemet(ex, ey,ϕ) används för att ge avståndssensorerna en position relativt

fordonets centrum. ϕ definieras som vinkeln mellan sensorns utbredningsriktning och fordonets ex-axel. Sensorernas placering och riktning används för att göra mätningar i

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, yt, med en modells mätning i kartan, ett förväntat värde, givet en

po-sition från xtför att erhålla p(yt|xt).

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 mini-mum 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 fig-ur 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 rikt-ning. 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 fig-uren. 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 efter-som 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 er-hålls som en produkt av sensorernas sannolikheter och föregående tidsstegs viktfunktion. Detta eftersom mätningarna ses som oberoende enligt markovantagandet

qt= M

∏

n=1 p(y(n)t |xt)qt−1 (3.15) Ultraljudssensorernas fördelningsfunktion

Fördelningsfunktionen p(yt|xt), 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 ar-bete används en gaussisk funktion som ger en sannolikhet för att sensorernas avstånd, ds,

skiljer sig åt i en omgivning från det förväntade avståndet dmap. Här kan osäkerheten

byg-gas 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 dmax. Exempel på en

möjlig fördelningsfunktion visas i figur 3.5.

0 50 100 150 200 250 0 0.02 0.04 0.06 0.08 0.1 0.12 d_s [cm] sannolikhet p(y t |xt

) ddmapmap ddmaxmax

Fördelningsfunktion för ultraljudssensorer

Figur 3.5. Sannolikheten för tillståndet xt givet en mätning yt. Sensorns uppmätta avstånd, ds,

För att komma tillrätta med det statiska antagandet av miljön ansätts även en liten san-nolikhet 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 to-tala 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

ks= π

d nmkp

(3.16) där d är hjulets diameter, nmär den utväxling som växellådan har och kpä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= ksNV/H,t (3.17)

där NV/H,tär det antal pulser som pulsgivaren på respektive hjul givit inom intervallet T .

Den linjära förflyttningen∆Ut samt kursförändringen∆Ψt för fordonets mittpunkt över

hjulaxeln blir då ∆Ut = ∆UV,t+∆UH,t 2 (3.18) ∆Ψt = ∆UV,t−∆UH,t b (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)

Xt = Xt−1+∆Utcos(Ψt) (3.21)

Yt = Yt−1+∆Utsin(Ψ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 an-talet 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 medel-hastigheten. Fordonets hastighet, v, beräknas som

vt=

vV,t+ vH,t

2 (3.23)

där vV,toch vH,t är respektive hjuls medelhastighet. Dessa beräknas med hjälp av∆SV/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 NV/H,tenligt

v_V/H,t=NV/H,t∆SV/H TV/H,t

(3.24) Uppdateringen av fordonets hastighet görs direkt med hjälp av den nuvarande medel-hastigheten v enligt vt= 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

vt = v + wv (3.25) Ψt = Ψt−1+∆Ψt+ wΨ (3.26) ∆U = ∆Ut+ vtwkv (3.27) Xt = Xt−1+∆U cos(Ψt+Ψt−1 2 ) + w∆U (3.28) Yt = Yt−1+∆U sin(Ψt+Ψt−1 2 ) + w∆U (3.29)

där v är den nuvarande medelhastigheten som kommer från sensorerna. Variabeln wkv

säga wv∈ N(0, dkv). Faktorn vtwkv blir därför den storhet som representerar osäkerheten

i förflyttningen på grund av hastigheten. Variablerna wv∈ N(0, dv), wΨ∈ N(0, dΨ) och

w∆U∈ N(0, d∆U). Standardavvikelserna bildas som en konstant multiplicerat med

sam-peltiden 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 20 cm blir hjulens omkrets 62, 83 cm. En fel i

diameter på 1 cm ger ett fel på 5% i total sträcka. Tabell 3.1 visar resultatet från en mät-ning 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 mod-ellen 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

khast= fT π

d nmkp

(3.30) där fT är den frekvens som räknaren uppdateras med, d är hjulets diameter, nm är den

utväxling som växellådan har och kpär antalet pulser per motoraxelvarv som

pulsgivar-na ger. Räkpulsgivar-naren i mikroprocessorn arbetar med en frekvens på 14, 4 kHz vilket ger en

tidsupplösning på ungefär 70µs. Detta ger, med en hjuldiameter på 20 cm 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 for-donets hjulbas b. Denna påverkar direkt tillståndetΨvia∆Ψoch därmed även position-sskattningen. 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.

Kapitel 4

Reglering av hjulhastigheten

För att fordonet ska kunna följa en förutbestämd bana är det lämpligt att hastigheterna hos fordonets hjul, och därmed fordonets fart och kurs, regleras. Detta görs i den mikropro-cessor som finns på fordonet. Regulatorn är av PID-typ. De båda drivande hjulen regleras individuellt. Figur 4.1 visar principen för regleringen där hjulens hastighet återkopplas till regulatorerna för att bilda reglerfelet.

Figur 4.1. Blockschema för hjulens hastighetsreglering. Styrsignalerna till motorerna utgörs av PWM-signaler. Referensvärdena från systemet samt utsignalerna från motorerna är hastigheter. Motorernas utsignaler återkopplas till regulatorn.

4.1

Signalrepresentation

En viktig faktor för reglering av ett system är en bra upplösning på data, det vill säga liten kvantisering på insignaler samt styrsignaler, eftersom regulatorer och signalnivåer är diskreta. In- och utsignaler representeras av heltal i mikroprocessorn vilket gör att antalet heltalssteg måste vara stort för att stora kvantiseringsfel i signalerna ska kunna undvikas.

4.1.1

Insignaler

Målet är att med befintliga pulsgivare erhålla en så bra representation av motorernas hastigheter som möjligt. I detta arbete har medelhastighet i ett variabelt tidsintervall an-vänts som hastighetsskattning. Tidsintervallet har satts lika med systemets sampeltid T . Två andra metoder har även undersökts: Räkning av antal pulser som pulsgivarna har gett i ett fixt tidsintervall samt uträkning av den momentana hastigheten genom mätning av tiden mellan två tick från pulsgivarna.

Räkning av antalet pulser i ett fixt intervall

Fördelen med att endast räkna antalet pulser i ett fast tidsintervall är att implementeringen är enkel. Intervallet kan sättas lika med samplingsperioden vilket gör att vi alltid har den senaste medelhastigheten vid sampeltillfället. Problemet med denna metod är att pulsgi-varna ger för få pulser. Vid en hastighet av 1 m/s ger gipulsgi-varna endast 10− 12 tick på 100 ms

och ger således endast ett eller inget tick per 10 ms. Detta ger en väldigt grov kvantisering av medelhastigheten vid korta samplingstider och därmed en dålig upplösning av hjulens hastighet. Detta gör att metoden för att räkna antalet pulser mellan två samplingstillfällen inte är tillräcklig för att klara av att reglera hastigheten utifrån. För detta skulle pulsgivare med mycket fler än 75 pulser per hjulvarv behövas.

Momentan hastighet

I likhet med metoden för att beräkna hjulens hastighet i avsnitt 3.4.4 var en andra ansats att beräkna en momentan hastighet för hjulen. Detta gjordes genom att med en räknare mäta tiderna mellan två pulser från pulsgivarna och räkna om detta till en hastighet. En felkälla är att hjulen inte är helt symmetriskt runda vilket leder till att de momentana hastigheterna kan skilja sig en del från medelhastigheten beroende på vilken punkt av hjulet som är i kontakt med underlaget. Hastigheterna visade sig dessutom vara känsliga för glapp i hjulaxlarna och att hjulen slirar mot underlaget, speciellt i accelerationsfasen. Hastighetsangivelserna blev inte sällan fysikaliskt orimliga. Detta problem kan mildras genom att sortera bort felaktiga och orimliga värden.

När hastigheten är låg, eller fordonet står stilla, kommer det inte in några pulser vilket gör att hastighetsangivelsen i detta moment kan skilja sig från den verkliga hastigheten. Problemet löstes med hjälp av en timeout som sätter hastigheten till noll om tiden mellan två pulser är för lång eller om det har gått en viss tid utan någon puls. Det minsta värdet av dessa båda tider motsvarar en låg hastighet som i princip kan ses som stillastående. Metoden ger en brusigare hastighetssignal och här kan inte en regelbunden samplingsperi-od utnyttjas på samma sätt som ovan. Åldern på signalen är okänd vid samplingstillfället. Signalen blir dock aldrig äldre än de timeouttider, timeout, som används för att behandla fallet då fordonet står still. För att minska inverkan från glapp och signalbrus på regula-torn filtreras hastigheten direkt i mikroprocessorn. Detta görs genom att ett medelvärde av den momentana hastigheten bildas i en buffer av längd n. Detta gör dock att hastighet-sangivelsen till regulatorn vid kraftiga accelerationer eller inbromsningar kommer att ha

4.1 Signalrepresentation 27

en maximal tidsfördröjning av n·timeout ms. Detta problem skulle kunna lösas med hjälp

av ett filter med adaptiv längd.

Medelhastighet i varje sampelintervall

Metoden som används för att reglera hastigheten i mikroprocessorn är densamma som i avsnitt 3.4.4 men nu med regulatorns sampelperiod, T , som periodtid. I avsnitt 3.4.4 erhölls en medelhastighet i ett visst tidsintervall. Fördelen med denna metod är liksom med metoden ovan där vi räknade antalet pulser att medelhastigheten kan erhållas i varje sampelintervall. Metoden använder räknare för att noggrant mäta tiden mellan första och sista pulsen i intervallet vilket eliminerar problemet med kvantisering. Medelhastigheter-na är dessutom direkt lågpassfiltrerade varför ingen medelvärdesbildning behövs för att minska brusigheten. En ytterligare principiell fördel är att endast beräkna hastighet på ett sätt i hela systemet. Om inga pulser erhålls under lång tid blir dessutom medelhastigheten automatiskt nära noll.

4.1.2

Utsignaler

Utsignalen från regulatorn, eller styrsignalen, till motorernas drivkretsar ges från for-donets mikroprocessor i form av en pulsbreddsmodulerad (PWM) signal. Drivkretsen använder spänning och ström direkt från batteriet för att leverera signalen vidare till mo-torerna. Även här är det önskvärt att kunna ställa ut relativt små skillnader i styrsignalen.

Pulsbreddsmodulering

Hastigheten hos en likströmsmotor är proportionell mot matningsspänningen vilket gör att en förändring i matningsspänningen för att kunna reglera hastigheten är önskvärd. Pulsbreddsmodulering utnyttjar att en likströmsmotor är av lågpasskaraktär, det vill säga att induktanser i motorn gör att den inte svarar direkt på förändringar i matningsspän-ningen. Detta fenomen utnyttjas i pulsbreddsmodulering genom att matningsspänningen till motorn periodiskt slås till och från med en relativt hög frekvens f . Denna princip används även i dimmers till lampor. En periodcykel där spänningen är tillkopplad 50% av tiden ger halva matningsspänningen. På detta sätt kan man åstadkomma en variabel spänning Vut, och därmed en variabel effekt till motorerna genom att den del av en period

där spänningen är tillslagen varieras. Detta trots att spänningskällan arbetar med en kon-stant spänning V0. Exempel på två olika PWM-signaler visas i figur 4.2. Vilken frekvens

pulserna ska genereras med är generellt svårt att säga. I till exempel [12] ges en koncis beskrivning av principerna för pulsbreddsmodulering och val av frekvens. Med en för låg frekvens svarar motorn som om strömmen verkligen slås på och av vilket gör att motorn får en ryckig och kraftlös gång. Ju högre frekvens desto jämnare strömnivåer in till mo-torn och alltså jämnare gång. Frekvenser mellan 20 Hz till 20 kHz kan användas och valet beror bland annat på motorns tröghetsmoment, yttre pålagt moment samt friktion. För att en PWM-signal med hög frekvens ska kunna genereras används en 16-bitars räknare i mikroprocessorn vilken kan generera olika sorters PWM-signaler. I detta ar-bete har en faskorrekt signal använts. Räknaren har dessutom möjlighet att generera upp till tre PWM-signaler samtidigt vilket gör att det bara behövs en räknare för att kontrollera

Figur 4.2. Pulsbreddsmodulering, PWM, med spänningen tillslagen 50 samt 25 procent av pe-riodtiden. Detta ger en fiktiv spänning, Vut, på 50 respektive 25 procent av matningsspänningen

V0.

fordonets båda motorer.

Enligt mikroprocessorns datablad [1] ges relationen mellan räknaromfång, det vill säga utsignalspann och PWM-signalens frekvens vid faskorrekt signalgenerering, av

fPW M=

fCPU

2· N · T OP (4.1)

där fPW Mär PWM-signalens frekvens, fCPUprocessorns klockfrekvens, N är en

skalfak-tor för klockfrekvensen (1, 8, 64, 256, eller 1024) och T OP är det räknarvärde som räknaren räknar upp till för att generera signaler med en given frekvens fPW M. T OP

är även det maximala värde som användaren kan ställa ut för att erhålla en 100% PWM-cykel, det vill säga konstant utsignal till motorerna.

Med fCPU givet som 3.6864 MHz måste skalfaktorn sättas till 1 för att komma upp i

frekvens och samtidigt få ett någorlunda stort utsignalspann i form av räknarsteg. En frekvens runt 16 kHz har använts här, vilket enligt ekvation (4.1) ger ett toppvärde för räknaren på 115. Det ger att 0-100 är ett lämpligt styrsignalområde. Rotationsriktningen för motorerna regleras separat.

4.2

Simuleringsmodell

För design och analys av regulatorn utformas en modell över systemet som ska regleras. Nedan ges en kontinuerlig modell över en likströmsmotor inspirerad av beskrivningar i [10] och [11]. Ett principschema för en elmotor visas i figur 4.3. Modellen beskriver förhållandet mellan spänningen in till motorn, u, och motoraxelns hastighetω. Kirchoffs andra lag ger för slingan i figur 4.3:

4.2 Simuleringsmodell 29

Figur 4.3. Principschema för en elmotor med en yttre last.

Där uemk, den inducerade elektromotoriska kraften (emk), är proportionell mot

motorax-elns hastighet

uemk= kemk·ω (4.3)

och uRges genom Ohms lag av den ström i som flyter genom kretsen

uR= R · i (4.4)

Spänningen uLär proportionell mot tidsderivatan av strömmen genom spolen L

uL= L ·

d

dti (4.5)

Det går även att ställa upp en jämvikt för de vridmoment som finns i kretsen

Ta− TJ− Tf− T = 0 (4.6)

där Taär det elektromagnetiska vridmomentet, TJ är momentet som kommer från

mo-toraxelns tröghetsmoment, Tf är momentet som kommer från friktion i motorn och T är

det vridmoment som kommer från en yttre last.

Vridmomentet Tauppkommer då det flyter en ström i genom lindningen i motorn och

kan skrivas som

Ta= ka· i (4.7)

där kaär en proportionalitetskonstant, torque constant på engelska. TJ är proportionellt

mot vinkelhastighetenω= ˙θ

TJ= J ·

d

dtω (4.8)

Det moment som uppkommer på grund av viskös friktion i motorn, Tf, är proportionellt

mot motoraxelns hastighet med en friktionskonstant f

Tf= f ·ω (4.9)

Genom att välja tillstånd enligt x=  x1 x2  =  i ω  och insignaler u=  u T  kan de båda formlerna (4.5) och (4.8) med hjälp av (4.2) - (4.4) respektive (4.6) och (4.7) skrivas som en tillståndsmodell för motorn på matrisform

˙x= R L − kemk L ka J − f J ! x+ 1 L 0 0 −1_J ! u (4.10)

Modellens konstanter hämtades från tillverkarens datablad. Det externa momentet T samt friktionen f anpassades så att modellen svarar mot de data som anges i databladet vid obelastad körning. Växellådan modellerades som en konstant skalfaktor 25 : 1. Hjulens tröghetsmoment approximerades utgående från stegsvarsförsök.

UL brukar ofta försummas då induktansen L ofta är liten. Den används dock här för att

strömmen i ska kunna levereras som en utsignal. Detta då drivkretsarna har en möjlighet att mäta den ström som levereras till motorerna. Diskretisering av modellen utfördes med hjälp av Eulers bakåtapproximation av derivata i Matlab

d dte(t) ≈

e(t) − e(t − TS)

TS

(4.11) där TSbetecknar systemets samplingsintervall.

4.3

Regulator

Eftersom regulatorn körs på mikroprocessorn är en enkel typ av regulator att föredra. PWM-signalen ut till motorns drivkrets är regulatorns styrsignal och hjulets hastighet är den storhet som regleras. Båda dessa signaler är representerade i heltal i mikroproces-sorn vilket gör det fördelaktigt att arbeta med en implementation av regulatorn i heltal som minskar processorns belastning. En flyttalsregulator har dock gett goda resultat då den kräver relativt få flyttalsoperationer. Motorerna kan regleras individuellt med olika parametrar för att kompensera för små olikheter i prestanda.

En regulator av PID-typ har en integrerande del som eliminerar stationära fel och en de-riverande del som kan dämpa svängningar i systemet. Samtidigt måste praktiska problem med instabilitet som kan uppkomma på grund av D-delen undvikas.

PID-regulator

En differentiell PID-regulator från [9] implementerades först i Matlab och sedan i C för mikroprocessorn. Parametrarna till regulatorn arbetades fram genom praktiska försök och

4.3 Regulator 31

simuleringar i Matlab med modellen från avsnitt 4.2. En kontinuerlig PID-regulator ges av u(t) = K  e(t) + 1 Ti t Z t0 e(τ)dτ+ Td d dte(t)   (4.12)

där K betecknar förstärkningen, Tiär regulatorns integraltid och Tdär regulatorns derivatatid.

e(t) är reglerfelet; det vill säga skillnaden mellan det önskade värdet och det faktiska

värdet. Vid diskretisering av regulatorn måste integralen i ekvation (4.12) approximeras. Detta kan göras enligt

t Z

t0

e(τ)dτ≈ Ts(e(t) + e(t − TS) + e(t − 2TS) + . . .) (4.13)

där TSbetecknar samplingsintervallet. Ekvation (4.13) tillsammans med Eulers

approxi-mation för derivata, (4.11), ger slutligen regulatorn på diskret form

un= K en+ TS Ti n

∑

k=0 ek+ Td en− en−1 Ts ! (4.14)

Index n betecknar värdet vid den n:te samplingstidpunkten.

PID-regulator på differentiell form

För att komma runt problem med så kallad integratoruppvridning som kan uppkomma i system med begränsad styrsignal, vilket alla fysikaliska system har, finns det flera metoder [9]. En av dessa är implementering på differentiell form där skillnaden i styrsignal mellan två tidpunkter används för att beräkna den nya styrsignalen.

Om regulatorn (4.14) först skrivs om på formen

un = Ken+ In+ K Td Ts (en− en−1) (4.15) In = In−1+ K TS Ti en (4.16)

ges sedan skillnaden mellan styrsignalen i två tidpunkter∆u som

∆u= un− un−1= K(en− en−1+ Ts Ti en+ Td Ts (en− 2en−1+ en−2)) (4.17)

tillsammans med en styrsignalbegränsning:

Algoritm 2 PID-regulator på differentiell form med styrsignalbegränsning, [9]

Läs in insignal och styrsignal Bilda styrsignaldifferensen:

∆v= K(en− en−1+T_TS_ien+T_Td_s(en− 2en−1+ en−2))

Bilda styrsignalen med hänsyn till signalbegränsningen:

if vn< uminthen

un= umin

else if vn> umaxthen

un= umax

else

un= un−1+∆v

end if

Ställ ut styrsignalen Lagra undan felen:

en−2= en−1