Designspecifikation Minröjningsbandvagn

Transkript

1 Designspecifikation Minröjningsbandvagn Version 0.3 Utgivare: Jonatan Ekström Datum: 9 oktober 2013 Status Granskad Anton Pettersson Godkänd

2 Projektidentitet Gruppens e-post: Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: Johan Dahlin (JD), ISY, Linköpings universitet Torbjörn Crona, Saab Bofors Dynamics Daniel Axehill och David Törnqvist, ISY, Linköpings universitet Anton Pettersson (AP) Björn Johansson, Carl Nordheim, Stefan Thorstensson, Saab Bofors Dynamics, Niclas Evestedt (NE), ISY, Linköpings universitet Gruppmedlemmar Namn Ansvarsområde Telefonnummer E-post Jesper Carlsson (JC) Informationsansvarig jesca887 Jonatan Ekström (JE) Designansvarig jonek364 Emmeline Kemperyd (EK) Dokumentansvarig emmke921 Simon Lind (SL) Hinderansvarig simli718 Johan Norén (JN) Hårdvaruansvarig johno923 Anton Pettersson (AP) Projektledare antpe073 Markus Rombach (MR) Testansvarig marro498 Jon Tjerngren (JT) Navigationsansvarig jontj362

3 Dokumenthistorik Version Datum Ändringar Utförda av Granskad Första utkastet Jonatan Ekström Anton Pettersson Andra utkastet Jonatan Ekström Emmeline Kemperyd Tredje utkastet Jonatan Ekström Anton Pettersson

4 Innehåll 1 Inledning 1 2 Systemöversikt Koordinatsystem Globalt koordinatsystem Lokalt koordinatsystem Rutkoordinatsystem Bandvagnskoordinatsystem Programflöde Filstruktur Trådning Realtime-tråden Slow-tråden Flödesschema Klassdiagram Basstation Gränssnitt Statusrad Inställningsvy Kartvy Vägpunktsvy Vidareutveckling Bandvagn Sensorer Sampling av sensorer IMU Ultraljudssensorer Speciallfall vid uppstart av systemet Odometrar GPS dgps Handkontroll Framdrivning Mindetektering Hinderhantering Detektering av hinder Navigering runt hinder Navigering Modeller Rörelsemodell Mätmodell Brusmodeller

5 6.2 Positionering Extended Kalman Filter (EKF) Partikelfilter Implementation av partikelfiltret Kartering Hinderkartan Sannolikhetskartan Ruttplanering Avsökningsalgoritm Hinderundvikning med hjälp av A* Reglering Kodstandard 26

6 Minröjningsbandvagn 1 1 Inledning Saab Bofors Dynamics vill utveckla en minröjningsbandvagn som ska kunna detektera minor. Detta projekt är en fortsättning på tidigare projekt där det första påbörjades våren 2009 av projektgruppen O hara s. De arbetade med utveckling av exempelvis specifikationer, kontrollprogram, nätverkskommunikation och navigationstekniker. Under hösten 2009 fortsatte projektgruppen Carpe Locus arbetet och utrustade bandvagnen med fjärrstyrning samt motorreglering för framdrivning. Bandvagnen utrustades även med en GPS för att kunna följa en fördefinierad brytpunktsbana. Under hösten 2010 fortsatte projektgruppen 8Yare som förde över tidigare funktionalitet till en industridator och installerade en stereokamera av typen Bumblebee 2. Året därpå fortsatte projektgruppen imap att utöka bandvagnens funktionalitet genom att med hjälp av en kamera och sensorer bygga upp en 3D-karta av ett område. Under hösten 2012 fortsatte projektgruppen Minenmarker att utveckla bandvagnen. De valde att inte bygga vidare på 3D-kartans funktionalitet och fokuserade istället på att detektera minor samt att arbeta med bandvagnens positionering. Deras mål var utöver detta att med hög sannolikhet kunna säkerställa att ett område var avsökt och fritt från minor. Årets projekt tar i mångt och mycket vid där deras slutade. Målen för detta projekt är att dels förbättra noggrannheten i navigeringen, förfina sensorfusionen samt att koppla in en handkontroll som trådlöst kan kommunicera med bandvagnen. Designspecifikationen är en översikt över det system för autonom minröjning som ska utvecklas. Kraven som specifierar vad själva minröjningen ska klara av, vilken handkontrollens funktionalitet skall vara samt vad hela systemet ska klara av definieras i detalj i kravspecifikationen, se [16]. Designspecifikationen innehållar en kortare beskrivning av alla delsystem som ingår i systemet. Detta dokument följer LIPS-mallen, se [19] för mer information. I detta dokument används en del förkortningar vilka definieras i tabell 1. Förkortning PWN IMU ARM RISC UTF-8 UCS GNU GPL GCC Beskrivning Pulse Width Modulation Inertial Measurement Unit Advanced RISC Machine-processor Reduced Instruction Set Computer UCS Transformation Format 8-bit Universal Character Set GNU s Not Unix General Public License GNU Compiler Collection Tabell 1: Förkortningar 2 Systemöversikt Minröjningsbandvagnen består av två huvudsystem: basstationen och bandvagnen, som i sin tur är uppdelade i mindre delsystem. Delsystemens syften och övergripande uppgifter är specificerade i detta dokument. Tanken med indelningen är att varje delsystem ska kunna konstrueras och testas individuellt i så stor mån som möjligt. Varje delsystem berör en eller flera fysiska komponenter och dessa presenteras också för varje delsystem. Notera att vissa delsystem delar komponenter. I figur 1 och 2 visas en överblick över minröjningsbandvagnen tillsammans med en skiss över informationsflödet mellan de olika delsystemen.

7 Minröjningsbandvagn 2 Figur 1: Överblick av systemet Figur 2: Kommunikation mellan olika delar av hårdvaran 2.1 Koordinatsystem Bandvagnen använder sig av fyra olika koordinatsystem, vilka är specifierade nedan Globalt koordinatsystem I detta koordinatsystem definieras x g, y g och z g som koordinater. Det globala koordinatsystemet är fixt relativt Jorden. GPS:en som är monterad på bandvagnen ger en positionsskattning uttryckt i dessa koordinater. Breddgradden (latitud) motsvaras av värdet på y g och längdgraden (longitud) motsvaras av värdet på x g Lokalt koordinatsystem I detta koordinatsystem definieras x, y och z som koordinater. Det lokala koordinatsystemet är fixt i operationsområdet och är alltså en roterad och translaterad version av

8 Minröjningsbandvagn 3 det globala koordinatystemet. Origo i det lokala koordinatsystemet är den punkt som är längst ner till vänster i det valda operationsområdet. Koordinataxlarna representeras som x åt höger, y rakt fram och z ut ur kartan, se figur 5. För att uttrycka globala koordinater i det lokala koordinatsystemet görs en omräkning enligt [ [ ] [ ] x cos α sin α xg x = g0. (1) y] sin α cos α y g y g0 För att göra omräkningen behövs koordinaterna för det lokala koordinatsystemets origo uttryckt i latitud och longitud (x g0 och y g0 ). Utöver detta behöver rotationen av det lokala koordinatsystemet relativt det globala beräknas, denna vinkel benämns α och visas i figur 3. Vinkeln α kan beräknas då positionen för hörnen hos det specificerade operationsområdet är kända. Givet att de globala koordinaterna för det nedre högra hörnet är (x g1, y g1 ) bestäms α enligt tan(α) = y g1 y g0 x g1 x g0. (2) y y g x (x g0,y g0 ) α x g Figur 3: Hur det lokala koordinatsystemet förhåller sig till det globala koordinatsystemet Rutkoordinatsystem Genom att partitionera det lokala koordinatsystemet i kvadratiska rutor fås vad vi har valt att benämna rutkoordinatsystemet. I detta koordinatsystem representeras en viss ruta i operationsområdet med två koordinater, i och j. Rutan längs ner till vänster får koordinaterna (1, 1). Värdet på i ökar längs med det lokala koordinatsystemets x-axel och värdet på j ökar längs med y-axeln, se figur Bandvagnskoordinatsystem Vi använder oss även av ett koordinatsystem som är fixt i bandvagnen. I detta koordinatsystem definieras x b, y b och z b som koordinater där x b betecknar riktningen rakt framåt sett ur bandvagnens perspektiv, y b rakt åt vänster och z b rakt uppåt. Origo specificeras som mitten av bandvagnen. Rotationen av bandvagnens koordinatsystem relativt det lokala systemet definieras av vinkeln ψ, se figur 4.

9 Minröjningsbandvagn 4 y b y x b ψ x Figur 4: Hur bandvagnens koordinatsystem förhåller sig till det lokala koordinatsystemet. y j n j n-1 N W E S y b BALROG BALROG x b j 3 j 2 j 1 i 1 i 2 i 3 i m-1 i m x Figur 5: De fyra koordinatsystemen i en och samma bild där det globala koordinatsystemet representeras av en kompass 3 Programflöde Detta avsnitt ger en översiktlig beskrivning av den mjukvara som körs på bandvagnen och basstationen.

10 Minröjningsbandvagn Filstruktur I mjukvaran används en hel del filer och i figur 6 ges därför en översiktlig bild av filstrukturen Figur 6: Filstruktur 3.2 Trådning Den mjukvara som körs på bandvagnen använder sig av två stycken trådar. Realtimetråden och slowtråden. I realtimetråden körs kod som har realtidskrav, d.v.s. kod som behöver köras snabbt, ofta och i tid. I slowtråden körs den kod som inte har lika hårda tidskrav Realtime-tråden Realtime-tråden har en uppdateringsfunktion som sätts igång av en timer med periodtiden 100 ms. För att realtimetråden alltid ska köras periodiskt har den högre prioritet än slowtråden. Detta innebär att om realtimetråden vill köra så kommer slowtråden att kastas ut och vänta tills dess att realtimetråden kört klart. I uppdateringsfunktionen som triggas av timern hämtas först data från sensorerna. Sensordata buffras, vilket innebär att om inga nya data samlats in av sensorerna sedan den senaste uppdateringen kommer gammal sensordata att återanvändas. Efter datainhämtningen körs positioneringsfiltret vilket ger en ny skattning av bandvagnens tillstånd. Därefter undersöks om en mina har detekterats och sedan körs en funktion som kontrollerar om ett hinder har upptäckts av ultraljudssensorerna. Ifall bandvagnen är i autonomt läge kommer tillståndsdata att skickas till regulatorn som ställer ut nya styrsignaler baserat på den nya informationen om bandvagnens position. Manuella styrsignaler kommer så fort de detekteras att vidarebefodras till styrsystemet som sköter kontakten med ARM-

11 Minröjningsbandvagn 6 processorn på bandvagnen. Styrsystemet ansvarar för att korrekta larvbandshastigheter sätts så att styrkommandot utförs. Realtimetråden sköter även nätverkskommunikationen med basstationen. Under tiden som uppdateringsfunktionen körs räknar en timer hur lång tid som passerat. När funktionen kört klart beräknas hur lång tid som förflutit och ifall denna tid överstiger en viss säkerhetsgräns, skrivs ett felmeddelande ut. I detta projekt kommer ett partikelfilter implementeras som komplement till det nuvarande EK-filtret och detta kommer leda till att uppdateringsfunktionen i Realtime-tråden tar längre tid att slutföra. Ifall implementeringen av partikelfiltret kräver så pass mycket beräkningskraft att uppdateringsfunktionen inte kan köras periodiskt var 100:e ms kommer detta att märkas tack vare timern. Om detta inträffar får vi försöka lösa det problemet, och vi har ett par alternativ för hur det ska kunna göras.. Ett av dem är att försöka effektivisera partikelfilterimplementationen, ett annat är att öka periodtiden hos timern tills dess att timern inte längre varnar. Förhoppningsvis fås tillräckligt god prestanda trots en lägre uppdateringsfrekvens. Ett ytterligare alternativ är att ändra antalet partiklar som används av partikelfiltret, färre partiklar innebär mindre beräkningar men också en sämre noggrannhet. Tyvärr är den nuvarande industridatorn ganska föråldrad. Processorn är en enkärnig x86 Intel som har en klockfrekvens på 600 MHz. Detta kan innebära att vi måste dra ner på partikelantalet ganska ordentligt. Ett förslag till nästa års projekt är att köpa ett nytt moderkort och processor till industridatorn Slow-tråden Slow-tråden sköter alla beräkningar som inte har realtidskrav. Det är denna tråd som beräknar nya vägpunkter baserat på bandvagnens nuvarande position och vilken styrstrategi som används. Utöver detta sköter denna tråd även uppdateringen av bandvagnens interna kartor. 3.3 Flödesschema I figur 7 visas ovanstående förlopp i ett flödesschema. 3.4 Klassdiagram I mjukvaran ingår tre olika projektfiler av typen.pro. I figur 8, figur 9 respektive figur 10 visas klassdiagram för dessa, d.v.s. project: bandvagn, project: basstation och project: common. 4 Basstation Detta kapitel beskriver vad som ingår i delsystemet basstation. Delsystemet består av en dator som kan kommunicera trådlöst med bandvagnen över ett WiFi-nätverk. Mycket av den funktionalitet som utvecklats under tidigare års projekt kommer att återanvändas. Gränssnittet kommer dock byggas ut för att kunna visa och använda ny funktionalitet i basstationen. Basstationens uppgift är att kommunicera med bandvagnen och att presentera information om bandvagnens status i ett användargränssnitt.

12 Minröjningsbandvagn 7 Figur 7: Flödesschema kod Figur 8: Klassdiagram: bandvagn 4.1 Gränssnitt Figur 11 visar användargränssnittets nuvarande utseende. Gränssnittet är uppdelat i fyra delar med olika funktionalitet. Överst finns en rad som visar status för bandvagn och nätverk. Under denna finns tre olika vyer: en inställningsvy som används för kommandon och konfigurering, en kartvy, och en vägpunktsvy. Dessa delar beskrivs mer utförligt nedan.

13 Minröjningsbandvagn 8 Figur 9: Klassdiagram: basstation Figur 10: Klassdiagram: common Statusrad Statusraden är placerad överst i användargränssnittet. Längst till vänster i statusraden visas vilket avsökningsläge som för närvarande används av ruttplaneringen. Till höger om denna visas information om GPS:en. Längst till höger i statusraden visas information om anslutningen mellan basstation och bandvagn.

14 Minröjningsbandvagn 9 Figur 11: Användargränssnitt basstation Inställningsvy Till vänster i användargränssnittet ligger inställningsvyn. Denna vy används för att konfigurera inställningar och skicka kommandon till bandvagnen. Överst i vyn finns ett antal knappar för val av körläge, samt start- och resetknappar. Under dessa finns kontroller som operatören använder för att sätta origo i det lokala koordinatsystem som bandvagnen kör i. Dessa koordinater anges enligt RT90-standarden. Operatören fyller även i en vinkel som anger operationsområdets rotation i förhållande till globala koordinater. Se vinkeln α i figur 3. Ett alternativ till att ange koordinater och vinkel manuellt är att använda data från bandvagnens GPS. Om operatören utnyttjar detta alternativ sätts origo för det lokala koordinatsystemet till bandvagnens nuvarande position och rotationen av det lokala systemet sätts till 0. Under dessa kontroller finns en knapp för att slå på och av bandvagnens GPS samt fält för att ange bandvagnens operationsområde. Operationsområdet är alltid rektangulärt och dess utbredning i x- och y-led anges i meter. Standardvärdet för operationsområdet är meter. Längst ner i inställningsvyn finns kontroller som används för att filtrera vad som visas på kartan. Med hjälp av dessa kan operatören välja om vägpunkter, hinder och estimerad position ska visas.

15 Minröjningsbandvagn Kartvy I mitten av gränsnittet finns kartvyn som används för att ge operatören en översikt över hur minsökningen fortlöper. Kartan är en modell av operationsområdet som visar de rutor som operationsområdet delats upp i. Varje ruta har initialt en vit bakgrund som blir grönare och grönare ju större sannolikhet det är att rutan är avsökt. På kartan överlagras en bild av bandvagnen och dess skattade position tillsammans med osäkerheten i skattningen. Utöver detta ritas även bandvagnens positionshistorik och aktuella vägpunkter ut. Hinder som upptäcks i någon av rutorna under körning markeras genom att denna ruta rödfärgas i kartvyn. Då en mina detekteras markeras den ruta där minan upptäckts med föregående projektgrupps logotyp Vägpunktsvy Längst till höger i gränssnittet finns vägpunktsvyn. Vägpunktsvyn används av operatören för att presentera de vägpunkter som för närvarande används av bandvagnen samt för att manuellt kunna ange nya vägpunkter. Då bandvagnen utför en avsökning visas de vägpunkter som genereras av ruttplaneringen i listan. Operatören kan ange vägpunkter genom att specificera vägpunktens lokala koordinater eller genom att klicka i kartvyn. Det finns stöd för att spara och läsa in vägpunkter från en textfil Vidareutveckling Vissa förändringar/tillägg kommer att göras i användargränssnittet för att hantera ytterligare funktionalitet i systemet. Knappen som slår på och av GPS:en kommer bytas ut mot en knapp som är länkad till en konfigurationsdialog. I detta dialogfönster ska det vara möjligt att ställa in både vilka sensorer och vilket positioneringsfilter som används, samt dess parametrar. Statusraden kommer att utökas för att visa vilka sensorer som för närvarande är aktiva samt vilket filter som används. Dessutom ska status för anslutningen mellan handkontroll och bandvagn kunna visas. Det kommer även läggas till en knapp som öppnar en konfigurationsdialog för handkontrollen. I denna dialog ska det vara möjligt att ställa in mappningnen mellan knappar på handkontrollen och kommandon till bandvagnen. Sättet på vilket mindetektion presenteras i kartvyn kommer att ändras. Den nuvarande markeringen av en mina kommer att ersättas med en konceptuell bild av en landmina. 5 Bandvagn Detta avsnitt beskriver hur bandvagnsdelsystemet är uppbyggt. I detta system ingår den mjukvara som körs på stydatorn, sensorer och framdrivning. 5.1 Sensorer Bandvagnen är utrustad med ett antal sensorer som används för positionering och hinderdetektering. För positionering används tre stycken sensorenheter, en Inertial Measurement Unit (IMU), en GPS-enhet samt odometrar som mäter hur långt bandvagnen rört sig.

16 Minröjningsbandvagn Sampling av sensorer Sensorerna samplas med en samplingsfrekvens på 10 Hertz. Alla sensorklasser kommunicerar med respektive sensorer i en egen tråd, då svarstiden från sensorerna är stor och detta annars leder till att samplingen tar mycket processortid. Alla sensorklasser är uppbyggda enligt samma publika gränssnitt, med följande funktionalitet: Initialize(). Initierar sensorläsningen. Update(). Svarar med sant om det finns ett nytt mätvärde från sensorn, annars falskt. Ber sedan sensorn om ny data. GetData(). Returnerar den data som senast samplats från sensorn. För närvarande körs inte läsningen av odometerdata på en egen tråd. Detta medför att läsningen av odometerdata tar längre tid än för de övriga sensorerna. Om prestandaproblem uppstår kan detta behöva modifieras IMU Industridatorn som sitter på bandvagnen kommunicerar med IMU:n via en USB-anslutning. Vid sampling ber datorn IMU:n om all tillgänglig data och får då tillbaka acceleration, vinkelhastighet och magnetfältsstyrka i x-, y- och z-led. Utöver detta returneras skattningar av stabiliserade data för samma storheter liksom en skattning av absolutvinkel. Accelerationsdata Accelerationsdata från IMU:n är brusig och används för närvarande inte för skattning av varken hastighet eller position. Vinkelhastighetsdata Bandvagnens vinkelhastighet mäts med hjälp av ett gyro. Mätning av vinkelhastigheten i z-led då bandvagnen står stilla ger ett medelvärde som är skilt ifrån noll, vilket innebär att gyrot har ett bias. Programmet som körs på industridatorn på bandvagnen gör vid uppstart en skattning av detta bias genom att under 100 sampel medelvärdesbilda vinkelhastigheterna i alla led. Den vinkelhastighet som läses från sensorklasserna i programmet är alltså biaskompenserad. Denna lösning ger bra resultat om körning görs inom rimlig tid efter bias-skattningen och temperaturen runt gyrot inte förändras för mycket. Vid längre körningar bör bias-skattningen göras om med jämna mellanrum. Ett annan lika simpel lösning är att istället använda sig av medianen. Fördelen med detta alternativet är att det inte är lika känsligt för outliers. Magnetfältsdata Magnetfältstyrkan som mäts av IMU:ns magnetometer är en summa av jordens magnetfält, externa fält från minor i marken samt olika typer av störningar som bland annat genereras av bandvagnens motorer under körning. Minor i marken ger upphov till tydliga spikar i magnetfältet, och magnetfältsdata kan alltså användas för att detektera minor. Tyvärr är störningarna från bandvagnens motorer av samma storleksordning som jordens magnetfält och detta gör det svårt att använda magnetfältsdata för navigering. Den IMU som är monterad i bandvagnen är en MicroStrain 3DM-GX1 [2] som innehåller sensorer för mätning av acceleration, rotation och magnetfält i tre axlar. Denna IMU har specificerad prestanda enligt tabell 2. Möjlighet att ersätta den befintliga IMU:n finns om det skulle bedömas att dess prestanda ej är tillräckligt god för de behov som den är tänkt att fylla. För att avgöra om den fyller det behov på träffsäkerhet som finns utvärderas den empiriskt och dess datablad jämförs med möjliga ersättningsenheter. Om bedömningen blir att den uppmätta prestandan inte är tillräckligt god kommer den befintliga IMU:n ersättas av en XSens MTI100, för mer information se [14].

17 Minröjningsbandvagn 12 Parameter Värde Olinjäritet Accelerometer: 0.2% Bias-stabilitet Accelerometer: 0.010g Olinjäritet Gyro: 0.2% Bias-stabilitet Gyro 0.7 /s Olinjäritet Magnetometer 0.4% Bias-stabilitet Magnetometer Gauss Upplösning Orientering < 0.1 minimum Repeterbarhet 0.20 Träffsäkerhet ±0.5 vid statiska testförhållanden ±2.0 vid dynamiska (cykliska) testförhållanden Tabell 2: Prestanda för MicroStrain 3DM-GX Ultraljudssensorer Bandvagnen utrustas med åtta stycken ultraljudssensorer för hinderdetektion. Sex av sensorerna heter SRF10 och två heter SRF235. Sensorgränssnitt Ultraljudssensorerna använder kommunikationsbussen I2C. I2C-bussen gör det möjligt för flera olika enheter att kommunicera över samma fysiska ledare. Varje enskild enhet ges en unik adress som används för att specificera vilken enhet som skall skrivas eller läsas till. Industridatorn har inget gränssnitt för I2C-kommunikation utan använder sig av ett USB-I2C konverteringskort. Kommunikationsprotokoll Ultraljudssensorerna kan ställas in på olika adresser för att kunna kommunicera på samma I2C-buss. Konverteraren I2C-2-PC kontrolleras genom ett enkelt textbaserat protokoll som beskrivs i detalj i dess datablad [5]. Ultraljudssensorn har fyra olika interna register som kan läsas från och skrivas till via I2C-bussen. För en mer detaljerad beskrivning av sensorernas funktioner och gränssnitt hänvisas till den tekniska specifikationen för ultraljudssensorerna [13]. En avståndsmätning startas genom att skicka ett kommando till sensorns kommandoregister. Sedan väntar datorn i 100 millisekunder innan mätningen är klar och resultatet kan läsas tillbaka från sensorns resultatregister. Resultatet av mätningen returneras i centimeter. Prestanda Sensorerna SRF10 har en bred lob där de kan upptäcka objekt. Enligt databladet är sensorernas lob 72 bred [13] men i praktiken avgör många faktorer om ett föremål går att upptäcka eller ej. Som sensorerna är monterade på bandvagnen klarar de att upptäcka hinder med en sidlängd av storleksordningen m på ett avstånd av upp till cirka 3 m. Sensorerna är monterade enligt figur 12 vilket ger bra förmåga att upptäcka hinder i färdvägen samt snett framför bandvagnen. Eftersom de båda sensorernas lober överlappar till viss del, samplas de direkt efter varann för att inte ekon från den ena sensorn skall ge falska mätvärden i den andra. Bandvagnen kommer även att utrustas med minst två sensorer på vardera sida vilket ska underlätta vid kartering av hinder. Dessa är av typen SF235 och smal-lobiga med en lob om endast 15. De anses därför kunna utgöra ett gott komplement till de befintliga sensorerna. SRF235 har en räckvidd på mellan 10 och 120 cm.

18 Minröjningsbandvagn Speciallfall vid uppstart av systemet Vid uppstart av systemet, det vill säga när Ubuntu startar på bandvagnen, måste ultraljudssensorerna vara urkopplade. Detta då de annars blockerar initieringen av kommunikationen från industridator till ARM-enhet. När Ubuntu har startat och initierat sina USB-drivrutiner kan ultraljudssensorerna kopplas in igen Odometrar Odometrarna mäter den sträcka respektive larvband rört sig sedan senaste mätning. De räknar även ut en momentan hastighet för larvbanden. Odometrarna består av 500 markeringar, utplacerade med ett avstånd från varandra på mm, på hjulen i respektive larvband. När en markering passerar en sensor registreras detta som ett avbrott på en ARM-processor, som givet antal avbrott sedan senaste mätning kan räkna ut hur långt larvbandet rört sig. ARM-processorn skattar även en momentanhastighet för larvbandets hastighet baserat på tiden mellan avbrott. Programmet som körs på bandvagnenens industridator ber vid sampling ARM-enheten om avstånds- och hastighetsdata för båda larvbanden GPS Bandvagnen är utrustad med en GPS-mottagare av typen Ublox-5. Information från GPSmottagaren skickas över serieporten enligt standarden NMEA Detta är ett standardiserat textbaserat protokoll som används av olika typer av navigationsutrustning. För en specifikation av de NMEA-meddelanden som finns att tillgå, se [1]. I NMEA-meddelandet finns det förutom position även information om tid, antal satelliter som använts för positionering och information om positioneringsnoggrannhet. GPS-mottagaren ger en position i latitud och longitud enligt WGS84-standarden [11]. Denna information konverteras till en position i koordinatsystemet RT90 [10]. De övriga systemen i bandvagnen hanterar koordinater i RT dgps Ett dgps-system använder sig av två GPS:er, en som är placerad på en stationär basstation med känd position och en som är placerad på objektet man vill positionera. Basstationen beräknar skillnaden mellan den positionsskattning som dess GPS ger och sin kända position. Skillnaden sänds ut till den mobila mottagaren, som då kan korrigera sin egen positionsskattning utifrån detta. Med hjälp av detta system kan man få en mycket bättre positionsskattning än om man endast använder en vanlig GPS. Om vi inte lyckas komma upp i den noggrannheten i positionen som vi har specificerat i vår kravspecifikation, är ett alternativ att ersätta den befintliga GPS-lösningen med en dgps-lösning. För att använda oss av en dgps-lösning måste en GPS som använder detta system införskaffas. GPS:en som vi i så fall hade tänkt använda oss av är en EVK-6PPP, se [8]. 5.2 Handkontroll Bandvagnen har utrustats med en trådlös handkontroll ansluten till industridatorn på bandvagnen. När denna är ansluten startar bandvagnens mjukvara automatiskt utan koppling till basstationen och den går in i ett manuellt körläge. I detta läge körs mjukvaran på samma sätt som i det automatiska vad gäller kartering och sensorfunktioner, men styrsignalen från regulatorn till motorerna är förbikopplad. Möjligheten till förbikoppling

19 Minröjningsbandvagn 14 visas i figur 7 och symboliseras där av en brytare. Istället skickas styrsignaler från den klass som hanterar manuella styrsignaler skickade från handkontrollen. Denna klass lyssnar även till signaler från mindetekteringen och ger operatören taktil återkoppling vid mindetektion i form av vibrationer i handkontrollen. Som handkontroll används en trådlös kontroll till Xbox 360. Denna levereras med en trådlös mottagare som kopplas till datorn via en USB-port. Efter installation av drivrutinerna, som redan finns i Ubuntu, och efterföljande inkoppling av mottagaren kan funktionaliteten hos handkontrollen konfigureras genom kontrollens användargränssnitt. [4] För att implementera handkontrollen i C++ behövs biblioteket XInput. Genom att använda detta bibliotek kan kommandon även skickas till handkontrollen, vilket innebär att den kan fås att vibrera. För att kontrollera vilka tillstånd som vid ett ett visst tillfälle gäller för ett visst kommando på handkontrollen används hex-koder enligt tabell 3. [6] Kommando XINPUT GAMEPAD DPAD UP XINPUT GAMEPAD DPAD LEFT XINPUT GAMEPAD DPAD RIGHT XINPUT GAMEPAD START XINPUT GAMEPAD BACK XINPUT GAMEPAD LEFT THUMB XINPUT GAMEPAD RIGHT THUMB XINPUT GAMEPAD LEFT SHOULDER XINPUT GAMEPAD RIGHT SHOULDER XINPUT GAMEPAD A XINPUT GAMEPAD B XINPUT GAMEPAD X XINPUT GAMEPAD Y Hex-kod 0x x x x x x x x0100 0x0200 0x1000 0x2000 0x4000 0x8000 Tabell 3: Hex-kod och motsvarande kommando i C++ för Xbox360 kontroll 5.3 Framdrivning Bandvagnens framdrivning kontrolleras av en ARM-processor som kommunicerar med industridatorn via en USB-anslutning. Genom att ange olika hastigheter på bandvagnens båda larvband kan bandvagnen styras. För att ställa in larvbandshastigheterna används ett enkelt textbaserat kommunikationsprotokoll som beskrivs nedan. För att ställa in larvbandshastigheterna skickar man en textsträng på formen svxxyy där XX och YY betecknar två bytes som anger bandhastigheten i mm/s för vänster respektive höger larvband. Som svar från framdrivningsenheten fås textsträngen svok om allt gått bra. 5.4 Mindetektering Delsystemet mindetektering har som uppgift att under avsökning av området detektera närliggande minor. Detektering av minor görs genom att data från magnetometern i IMU:n analyseras. Denna magnetometer mättas vid magnetfältsstyrka 1.2 Gauss (jämför med jordens magnetfältsstyrka på ca 0.5 Gauss) och mättas därmed snabbare när den närmar

20 Minröjningsbandvagn 15 sig en magnet, vilket i detta projekt motsvarar minor. Mindetekteringen görs därmed genom att besluta om en mina är nära bandvagnen eller ej, vilket i sin tur avgörs genom att bestämma om sensorn har mättats eller inte. Om det skulle visa sig att störningar kan mätta sensorn kommer först en medianfiltrering av sensordata göras. Beroende på avståndet från bandvagnens skattade position till de minor som redan detekterats tas beslut om ifall att den detekterade minan är en ny eller sedan tidigare detekterad mina. Då en mina dekteras sätts dess position till bandvagnens nuvarande positionsskattning. 5.5 Hinderhantering Delsystemet hinderhantering har som uppgift att detektera och navigera runt hinder. För att detektera hinder använder delsystemet för närvarande ultraljudsensorer monterade på bandvagnen enligt figur 12, och vi kommer även att montera dit två ytterligare sensorer som beskrivet under kapitel Figur 12: Ultraljudsensornas positioner med deras lober på bandvagnen Detektering av hinder När bandvagnen med hjälp av ultraljudssensorerna upptäcker ett hinder kommer ett av tre fall att inträffa. Om båda frontsensorerna upptäcker ett hinder antas det befinna sig rakt framför bandvagnen, medan om endast en av sensorerna upptäcker ett hinder så antas det befinna sig längs den sensorns centrumlinje. Figur 13 visar ett fall där ett hinder upptäckts längs den högra sensorns centrumlinje. Hindrets position beräknas enligt x h = x b + r cos(ψ + β) y h = y b + r sin(ψ + β), (3a) (3b)

21 Minröjningsbandvagn 16 Figur 13: Hinderdetektering med ett hinder längs den högra sensorns centrumlinje där x b och y b betecknar bandvagnens position, Ψ bandvagnens kursvinkel, x h och y h hindrets skattade position, r betecknar det uppmätta avståndet till hindret och β betecknar den skattade vinkeln mellan bandvagnens färdriktning och hindret. β är positiv om hindret upptäcks med den vänstra sensorn, noll om hindret upptäcks med båda sensorerna och negativ om hindret upptäcks med den högra sensorn. Den ruta i hinderkartan där hindret enligt skattning bör vara placerat markeras sedan med en etta Navigering runt hinder Då bandvagnen har detekterat ett hinder med båda frontsensorerna fortsätter den framåt tills dess att avståndet till hindret är 30 cm. Utifall bandvagnen detekterar ett hinder med endast en av frontsensorerna sparar bandvagnen sin position, stannar och roterar så att båda frontsensorerna hittar hindret. Då båda frontsensorerna hittat hindret fortsätter bandvagnen framåt tills den är 30 cm ifrån hindret. Vid 30 cm stannar bandvagnen och roterar så att dess högra sidosensorer hittar hindret. När de smallobiga sidosensorerna har kontakt med hindret börjar bandvagnen navigera längs hindret. Bandvagnen positionerar sig parallellt med hindret och navigerar runt det, genom att använda en reglerstrategi som minimerar skillnaden i avståndsmätning från de två sidosensorerna. Avståndet till hindret från sidosensorerna för höger sida ges av x h = x b + r y h = y b, (4a) (4b) och för vänster sida av x h = x b r y h = y b, (5a) (5b)

22 Minröjningsbandvagn 17 där r betecknar det uppmätta avståndet till hindret. För att reglera längs ett hinder används en PD-regulator som beskrivs av ekvationerna v l,k = v k K ɛ,k T d, T s (ɛ,k ɛ,k 1 ) (6a) v r,k = v k + K ɛ,k + T d, T s (ɛ,k ɛ,k 1 ), (6b) där syftet är att minimera felet ɛ. ɛ betecknar skillnaden i avståndsmätning mellan två sidosensorer, i figur 14 är ɛ således skillnaden mellan r 1 och r 2. Figur 14: Bandvagnen vid navigering runt hinder med hjälp av sidosensorerna r 1 och r 2 När bandvagnen har passerat hindret, alltså då den främre sidosensorn har tappat kontakt med hindret, fortsätter den framåt en bit så att den har plats att rotera. När bandvagnen har kommit en bit ifrån hindret stannar den, roterar och fortsätter navigera runt hindret. När bandvagnen har kört ett varv runt hindret är det färdigmappat och bandvagnen kan fortsätta sin avsökning av området. Dock är ju hindret i vägen för den planerade rutten, varför bandvagenen navigerar ett halvt varv runt hindret så att den kan fortsätta på andra sidan. 6 Navigering Navigeringssystemets uppgift är att möjliggöra för bandvagnen att ta sig runt i hela operationsområdet så att den kan utföra sitt arbete på ett tillförlitligt sätt. För att detta ska vara möjligt krävs bland annat att bandvagnens tillståndsvektor (8) skattas, att kartor över området byggs upp enligt avsnitt 6.3, samt att en körbar rutt beräknas. Speciellt viktigt är det att få en tillförlitlig skattning av bandvagnens postion, d.v.s. en skattning så nära verkligheten som möjligt och som dessutom har en låg varians. En bra positionsskattning ger en tillförlitlig karta över detekterade hinder och minor på operationsområdet. Ruttplaneringssystemet måste även ha en bra positionsskattning för att fungera på önskvärt sätt.

23 Minröjningsbandvagn Modeller För att kunna få användbara skattningar måste bra modeller användas. Arbetet detta år kommer att bygga vidare på modeller från föregående projekt [17], en genomgång av dessa matematiska modeller föjer nedan Rörelsemodell Bandvagnen modelleras för närvarande med en tillståndsmodell uttryckt i det lokala koordinatsystemet. Den har formen x k+1 = f(x k ) + w k, (7) där x k betecknar en tillståndsvektor vid sampel k. Funktionen f betecknar en olinjär funktion som beskriver bandvagnens dynamik och w k betecknar processbruset. Tillståndsvektorn har följande form x k y k x k = v k ψ k ω k där x k och y k betecknar bandvagnens position i det lokala koordinatsystemet och v k betecknar hastighet i bandvagnens färdriktning. ψ k och ω k betecknar bandvagnens riktning samt vinkelhastighet i det lokala koordinatsystemet. I den nuvarande implementeringen är det ett extended Kalman filter (EKF), se avsnitt 6.2.1, som skattar alla tillstånden. I år ska det införas en möjlighet för att istället använda ett partikelfilter, se avsnitt 6.2.2, för att skatta positionen (x k och y k ) medan EKF:et skattar de övriga tillstånden (v k, ψ k och ω k ). Anledningen till detta är att GPS:ens mätbrus inte är Gaussiskt fördelat, och detta stämmer inte överens med de antaganden som görs för ett EKF. Partikelfiltret är en mer generell metod som kan användas för system vars mätbrus har en godtycklig men känd sannolikhetsfördelning. Den nuvarande modellen över dynamiken är baserad på den diskretiserade modellen (12.12) i [15]. Detta är en så kallad coordinated turn model. Denna modell har förenklats genom att anta att vinkelhastigheten ω k är liten. Detta ger rörelsemodellen x k + T v k cos(ψ k ) y k + T v k sin(ψ k ) x k+1 = v k ψ k + T ω k + w k, (9) ω k där T betecknar sampeltiden, eller snarare tiden mellan det senaste samplet och det nuvarande samplet. I det kontinuerliga fallet antas processbruset w vara Gaussiskt fördelat med medelvärde 0 och kovarians Cov(w). Detta brus påverkar även de övriga tillstånden då modellen diskretiserats enligt Q k = GCov(w)G T, (10) (8)

24 Minröjningsbandvagn 19 där T 2 2 cos(ψ k) 0 T 2 2 sin(ψ k) 0 G = T 0. (11) T T Något att notera är att denna rörelsemodell inte innehåller något tillstånd för accelerationen. Detta innebär att data som skulle kunna används till skattningen slängs bort. Detta på grund av att den IMU som för närvarande finns ger för dålig accelerationsdata. Om en ny IMU skulle införskaffas finns utvecklingsmöjligheten att införa acceleration som ett tillstånd i rörelsemodellen och på så sätt förbättra skattningen. Detta skulle ge tillståndsvektorn x k = Efter diskretisering ges rörelsemodellen av x k y k v k a k ψ k ω k. (12) x k + T v k cos(ψ k ) y k + T v k sin(ψ k ) x k+1 = v k + T a k a k + w k. (13) ψ k + T ω k ω k Mätmodell Enligt den tekniska dokumentationen från föregående projekt [17] används en mätmodell som består av fem mätningar y k = h(x k ) + e k, (14) där e k betecknar mätbrus. Det som mäts är y k = x GP S y GP S ω IMU v r+v l 2 v r v l d v, (15) där x GP S och y GP S betecknar den absoluta positionen mätt med GPS, ω IMU betecknar vinkelhastigheten i positiv z-led sett från bandvagnen och v r och v l betecknar hastigheten på höger respektive vänster larvband. Konstanten d v betecknar en virtuell bredd, definierad genom att modellera bandvagnen som en bredare robot med hjul istället för larvband. Bandvagnen ses alltså som ett fordon med två hjul med bredd d v = 0.6, en lösning som har följt med sedan tidigare projekt. Den tekniska dokumentationen från föregående projektgrupp [17] hänvisar vid presentationen av denna lösning i sin tur till den tekniska dokumentationen från år Eftersom 2012 års projekgrupp valt att behålla denna

25 Minröjningsbandvagn 20 lösning kommer vi initialt också att göra det. Vid behov kommer vi dock att utvärdera andra lösningar. I den nuvarande implementeringen skattas alla tillstånd av EFK:et (se avsnitt 6.2.1) med h(x k ) som h(x k ) = x k y k ω k v k ω k. (16) Uttrycket i 16 innebär att det finns två mätningar av vinkelhastigheten, där den ena använder sig av gyro och den andra differensen mellan larvbandens hastigheter. I den alternativa navigeringsalgoritm som skall införas kommer positionsmätningarna att användas i ett partikelfilter och de övriga mätvärdena användas i EKF:et Brusmodeller För ökad noggrannhet och för användning i partikelfiltret kommer brusmodeller tas fram för samtliga sensorer. Tillvägagångssättet kommer att vara följande: 1. Mätning av brus 2. Analys av brus 3. Modellering av brus Mätning av brus görs genom att samla in sensordata då bandvagnen står stilla utan att ges några kommandon. Analys av brus görs genom att studera spektrum för den insamlade datan i steg 1 samt autokorrelationsfunktionen för densamma. Ett studie av autokorrelationsfunktionen visar om det finns något samband mellan mätdata vid olika tidpunkter, och bör vara kring 0 om det är vitt brus som blivit uppmätt. Utöver att studera data i frekvensdomänen kan vi även använda oss av andra typer av plottar, exempelvis histogram eller QQ-plottar. Modellering av brus görs genom att jämföra egenskaper hos insamlade brusdata med egenskaper för olika kända fördelningar. Den fördelning vi i ett första skede kommer att jämföra med är normalfördelningen, vars spektrum visas i figur 12. Andra intressanta fördelningar som vi har tänkt studera är Students t-fördelning, χ 2 -fördelning och en mixtur av flera normalfördelningar. När vi hittat en lämplig kandidat kan vi gå vidare med att försöka skatta fördelningsfunktionens olika parametrar. 6.2 Positionering Positioneringssystemets uppgift är att skatta bandvagnens tillståndsvektor 8 utifrån sensorfusion av data som hämtas från sensordelsystemet. I den nuvarande implementeringen skattar EKF alla tillstånd. Ett alternativ kommer att implementeras där det kommer att vara möjligt att låta ett partikelfilter skatta bandvagnens position medan EKF:et skattar de övriga tillstånden. Detta ska förhoppningsvis förbättra positionsskattningen då mätbruset i EKF:et antas vara Gaussiskt, vilket dock antagligen inte stämmer för GPS:ens mätbrus. Partikelfiltret är en mer generell metod för tilståndsskattning i olinjära system, vilket gör det mer beräkningstungt men också medger en högre potentiell noggrannhet. För detaljerade beskrivningar av EKF och partikelfilter, se kapitel 8 respektive 9 i [15].

26 Minröjningsbandvagn 21 Figur 15: Vitt brus. Utmärkande är det konstanta spektrumet Extended Kalman Filter (EKF) Ett EKF av ordning 1 [15] finns för tillfället implementerat i bandvagnen. Filtret körs varje gång Realtime-trådens uppdateringsfunktion körs. De allmänna modeller som används är x k+1 = f(x k ) + w k, (17) y k = h(x k ) + e k, (18) där (17) betecknar rörelsemodellen och (18) betecknar mätmodellen. w k och e k betecknar process- respektive mätbrus och dessa antas i EKF-ramverket vara vita Gaussiska brus med medelvärde noll. EKF-algoritmen ser ut som följer ɛ k = y k h(ˆx k k 1 ), (19) S k = H k P k k 1 H T k + R k, (20) K k = P k k 1 Hk T S 1 k, (21) ˆx k k = ˆx k k + K k ɛ k, (22) P k k = P k k 1 K k H k P k k 1, (23)

27 Minröjningsbandvagn 22 ˆx k+1 k = f(ˆx k k ), (24) P k+1 k = F k P k k F T k + Q k, (25) där ekvationerna (19)-(23) är mätuppdateringen och ekvationerna (24)-(25) är tidsuppdateringen. ɛ k betecknar innovationen, S k betecknar innovationskovariansen, K k betecknar Kalmanförstärkningen, R k betecknar mätbrusets kovariansmatris, Q k betecknar processbrusets kovariansmatris, ˆx k k betecknar tillståndsskattningen och P k k betecknar tillståndskovariansen. Matriserna F k och H k betecknar jacobianerna till rörelsefunktionen f respektive mätfunktionen h i ekvationerna (17) och (18). Den nuvarande EKF-implementeringen kan modifieras under tiden som en körning pågår. Det kan vara så att mätningar från alla sensorer inte finns tillgängliga varje gång skattningarna beräknas. Modellen modifieras då genom att de mätningar som inte är aktiva tas bort ur mätfunktionen h. Det vill säga att de stryks ur h vilket medför att motsvarande rader stryks ur jacobianen H. I R k tas både rader och kolumner bort. Detta kommer att användas då partikelfiltret körs för att skatta positionen Partikelfilter En av uppgifterna för detta projekt är att implementera och utvärdera om ett partikelfilter kan ge en bättre skattning av GPS:ens positionsdata. För mer detaljer om partikelfiltret se [15]. Partikelfiltret använder sig av en olinjär, icke-gaussisk modell med den generella formen x k+1 = f(x k, u k, v k ), (26) y k = h(x k, u k, e k ), (27) där u k betecknar en styrsignal, v k processbrus, y k en mätning och e k mätbrus. I denna modell är sannolikhetstäthetsfunktionerna (PDF) för process- respektive mätbrus godtyckliga. Partikelfiltret beräknar en approximation till posteriorfördelningen p(x k y 1:k ) för tillståndet x k. Approximationen baseras på N stycken sampel {x i } N i=1, kallade partiklar (därav namnet) och deras associerade vikter {w i } N i=1. Den resulterande empiriska filterfördelningen blir p(x k y 1:k ) N wk i δ(x k x i k ), där δ betecknar Diracs deltafunktion. i=1 Partikelfiltret processar data rekursivt enligt nedanstående operationer. 1. Mätuppdateringen: Modifiera vikterna enligt mätekvationen för skillnaden mellan observerade och predikterade mätvärden. 2. Omsampling: Ta nya sampel av tillstånden från den nuvarande mängden partiklar. 3. Tidsuppdateringen: Simulera en trajektoria från en mättidpunkt till nästa genom att använda den dynamiska modellen. För att sålla bort osannolika partiklar används omsampling. Vi kommer att välja mellan två olika metoder för omsamplingen. Den ena är Sampling Importance Resampling (SIR) och den andra är Sampling Importance Sampling (SIS). SIR gör en omsampling då antalet effektiva partiklar har sjunkit under ett visst gränsvärde. En beskrivning av hur detta fungerar finns på sidan 225 i [15]. Vi kommer börja med att implementera SIS och i mån av tid även utvärdera hur bra SIR fungerar. För att implementera filtret tänkte vi använda en något modifierad algoritm 9.1 från [15].

28 Minröjningsbandvagn 23 Algoritm 9.1 för ett partikelfilter Börja med att välja en proposalfördelning q(x k+1 x 1:k, y k+1 ), strategi för omsampling samt antal partiklar N. Initialisering: Generera x i 1 p x0, i = 1,..., N och låt w i 1 0 = 1/N Iterera: För k = 1,2,... T 1. Mätuppdatering: För i = 1,2,..., N, där normaliseringsvikten ges av w i k k = 1 c k w i k k 1 p(y k x i k), (28) c k = N wk k 1 i p(y k x i k). (29) i=1 2. Estimering: Filterdensiteten approximeras med ˆp(x k y 1:k ) = N wk k i δ(x k x i k), (30) i=1 och tillståndstrajektorian approximeras med ˆx k N wk k i xi k. (31) i=1 3. Omsampling: Valfritt vid varje iteration, ta N sampel från ersättningsmängden {x i 1:k }N i=1 där sannolikheten att ta partikel i är wi k k och låt wi k k = 1/N. 4. Tidsuppdatering: Generera prediktioner enligt proposalfördelningen och kompensera vikterna x i k+1 q(x k+1 x i k, y k+1 ), (32) wk+1 k i = p(x i wi k+1 xi k ) k k q(x i k+1 xi, u k+1 ). (33) Implementation av partikelfiltret Eftersom det finns ett färdigimplementerat EK-filter för alla tillstånd kommer partikelfiltret att användas för de tillstånd vars mätbrus inte kan ses som Gaussiskt. Detta innebär att partikelfiltret används samtidigt som EK-filtret; vi låter partikelfiltret ta hand om de absoluta positionsmätningarna och lämnar resterande tillstånd till EK-filtret. För att utveckla partikelfiltret ska vi först samla in sensordata från GPS:en för att ha något att testa mot. Vi ska både samla in data från GPS:en när bandvagnen står stilla och när den kör runt. Därefter ska vi ta fram ett partikelfilter i Matlab, utgående från ovanstående algoritm. Detta för att på ett smidigt sätt kunna simulera och utvärdera filtret för olika parametervärden.

29 Minröjningsbandvagn 24 Slutligen ska vi översätta Matlab-koden till C++-kod samt försöka införa dynamisk skattning av brusparametrar. Detta genom att köra någon form av initialisering där bandvagnen samlar in data från GPS:en när den står stilla ett tag eller när den kör fram och tillbaka. Dessutom ska vi finjustera parametrarna när filtret körs på bandvagnen. Detta kan innebära att antal partiklar måste reduceras för att minska beräkningstyngden. För att implementera filtret i C++ har vi tänkt använda bibliotek för matris- och slumptalshantering. Tidigare års projekt har använt ett bibliotek som heter Eigen [7] som vi troligtvis kan använda för matrishanteringen. Ett alternativ till Eigen skulle vara att använda Armadillo [3]. En fördel med Armadillo är att dess syntax är lik Matlabs och att det bygger på LAPACK och BLAS som är väloptimerade paket för matrishantering. För slumptalshanteringen har vi tänkt att använda GNU Scientific Library (GSL) [9]. Ett sista alternativ vore att testa ett paket med färdiga partikelfilter [12]. 6.3 Kartering Bandvagnen har två interna kartor som representeras i form av matriser. Den ena kallas hinderkartan och innehåller information om vilka rutor som innehåller hinder. Den andra kallas sannolikhetskartan och innehåller information om hur stor sannolikheten är att bandvagnen besökt de olika rutorna Hinderkartan Hinderkartan är en matris som i nuläget endast innehåller binära heltal. Om ett hinder upptäckts i en ruta med koordinater (i, j) i rutkoordinatsystemet kommer motsvarande element i hinderkartan att sättas till Sannolikhetskartan Sannolikhetskartan är en av de kartor som byggs upp i bandvagnen, den är till för att hålla reda på hur sannolikt det är att varje ruta blir besökt då bandvagnen passerar. I ruttplaneringen används denna karta för att planera för eventuella återbesök av rutor som bandvagnen inte anser med tillräckligt hög sannolikhet vara avsökta. Sannolikheten att en ruta är avsökt uppdateras enligt p i:k (i, j) = 1 (1 p k (i, j))(1 p 1:k 1 (i, j)). (34) Vänsterledet i (34) är sannolikheten att en ruta (i, j) är avsökt fram till och med uppdatering k av sannolikhetskartan. I högerledet betecknar p k den momentana sannolikheten att bandvagnen befinner sig där nu och p 1:k 1 betecknar sannolikheten att rutan var avsökt fram till och med det förra samplet. För att skatta den momentana sannolikheten används nuvarande positionsskattning ˆx k och tillhörande kovarians P pos k det vill säga 2 2-matrisen som utgör det övre vänstra hörnet i kovariansmatrisen P k. Den skattade positionen tillsammans med kovariansmatrisen anses utgöra en tvådimensionell Gaussisk fördelning vilket leder till en tvådimensionell Gaussisk sannolikhetsfördelning enligt f X,Y (x, y) = 1 2π P pos k 1 2 e 1 2 x y ˆx k T P pos-1 k ŷ k x y ˆx k ŷ k Sannolikheten uppdateras endast för den ruta som bandvagnen skattar att den befinner sig i, det vill säga det i och j så att ˆx, ŷ (i, j) Den momentana sannolikheten att bandvagnen befinner sig i en ruta fås genom att integrera funktionen i ekvation (35) över denna (35)

30 Minröjningsbandvagn 25 Figur 16: Zick-zack-bana som körs av bandvagnen. ruta. Här görs ännu en approximation, genom att integralen approximeras som volymen av ett rätblock plus volymen av en pyramid. Basen av rätblocket är rutans area och höjden är medelvärdet av funktionsvärdena i rutans hörnpunkter. Ovanpå detta läggs en pyramid med höjd f ( X, Y )(ˆx k ŷ k ) minus höjden av rätblocket. Sannolikhetsuträkningarna är approximativa och är mest till för att få en relativ noggrannhet mellan rutor, själva straffet på vilka rutor som ska återbesökas sköts av ruttplaneringen. 6.4 Ruttplanering Delsystemet för ruttplanering ansvarar för att bestämma vägpunkter för bandvagnen att färdas mellan på sin väg till ett givet mål. Bandvagnens aktuella position och dess målposition knyts således samman av punkter på vägen mellan dem. Mellan två vägpunkter ska bandvagnen kunna färdas rakt Avsökningsalgoritm Avsökningsalgoritmen arbetar i dess nuvarande utförande i fyra steg. Dessa är: 1. Indelning av området i rutor med storlek 1 1 meter 2. Initial avsökning av området. Detta görs genom att välja vägpunkter i en zick-zackbana där alla rutor besöks. En sådan bana syns i figur Avsökning av delar av området som med låg sannolikhet har avsökts. Dessa besöks i ordning, där den ruta som med minst sannolikhet blivit avsökt besöks först. Resultatet av denna planering syns i figur 17, där andra rutan ovanifrån till höger inte med tillräckligt hög sannolikhet hade avsökts och därför besöks i denna omgång. 4. Återvänder till startpunkten då hela området med tillräckligt hög sannolikhet har avsökts. Vi kommer att försöka att göra en finare rutindelning än den nuvarande implementationen. Detta för att uppnå en högre upplösning i hinderuppritningen på basstationen.

31 Minröjningsbandvagn 26 Figur 17: Avsökning av ruta som inte med tillräckligt hög sannolikhet blivit avsökt Hinderundvikning med hjälp av A* I C++-koden finns en A*-algoritm implementerad. Genom att låta vägpunkterna skickas in i denna algoritm hanteras och undviks eventuella kända hinder. För närvarande skickar A*-algoritmen ut en vägpunkt i varje ruta mellan start- och målrutan. Regleringen är bristfällig när det kommer till att reglera bandvagnen mellan vägpunkter som ligger för nära varandra. A*-algoritmen är en vägsökningsalgoritm som används för att hitta en effektiv väg mellan två rutor. Se [18] för mer information. A* använder sig av en heuristik i sökningen efter den bästa vägen mellan den ruta bandvagnen befinner sig i och den den vill ta sig till. Funktionen som används utgörs av f(x) = g(x) + h(x). f(x) betecknar den funktion som anger sträckan mellan startrutan och den aktuella rutan, plus en heuristisk skattning av sträckan mellan start- och målrutan. Denna funktion används för att välja i vilken ordning rutorna besöks. g(x) betecknar sträckan från startrutan till aktuell ruta via den framräknade vägen. h(x) betecknar den heuristiskt underskattade sträckan mellan den aktuella rutan och målrutan. Denna sätts lika med det euklidiska avståndet mellan den aktuella rutan och målrutan. En sträcka kan anses svara mot en kostnad och A*-algoritmen minimerar denna kostnad. Figur 18 visar hur A*-algoritmen fungerar. 6.5 Reglering Regleringens uppgift är att ta bandvagnen till nästa vägpunkt. För detta ändamål används en PD-regulator. Syftet med regleringen är att minimera vinkelfelet ɛ ψ,k mellan önskad färdriktning och sann färdriktning. Denna regulator är likadan som i förra årets projekt. 7 Kodstandard All kod kommer att skrivas i C++ de enda undantagen är koden på ARM-processorn som har skrivits av tidigare projektgrupper i C och simuleringskod som kommer att skrivas i Matlab. Vi kommer använda samma kodkonvention som föregående projektgrupp