Teknisk Dokumentation Minröjningsbandvagn

Transkript

1 Teknisk Dokumentation Minröjningsbandvagn Version 0.1 Utgivare: Emmeline Kemperyd Datum: 16 december 2013 Status Granskad Emmeline Kemperyd 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 utkast AP EK

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 Xbox-tråden Klassdiagram Basstationen Användargränssnitt Statusrad Inställningsvy Kartvy Vägpunktsvy Nätverkanslutningsfönster Kontrollparameterfönster Bandvagnen Sensorer Sampling av sensorer IMU Ultraljudssensorer Specialfall vid uppstart av systemet Odometrar GPS Handkontroll Funktionalitet Implementation av handkontrollen Framdrivning Mindetektering Hinderhantering Detektering av hinder Navigering runt hinder Navigering Modeller Rörelsemodell

5 6.1.2 Mätmodell Brusmodeller Positionering Initialtillsånd Extended Kalman Filter (EKF) Partikelfilter Kartering Hinderkartan Sannolikhetskartan Ruttplanering Avsökningsalgoritm Hinderundvikning med hjälp A* Reglering Bandhastigheterna Vinkelfelet ɛ ψ,y är större än Avståndet till målet är större än en meter Avståndet till målet är mindre än en meter Kodstandard 41 9 Diskussion Utvecklingsmöjligheter Förbättrad hinderhantering Omplacering av frontsensorer Hantering av andra typer av hinder Förfining av rutkoordinatsystemet Förbättra eller byt ut ARM-enheten Uppgradering av bandvagnens hårdvara Refaktorering av kod relaterad till trådning Utveckla tidsuppdateringen i partikelfiltret Utökad räckvidd för mindetektering Ny algoritm för ruttplanering Ny GPS med möjlighet till inhämtning av rådata Ny GPS med mer avancerad funktionalitet Skak-funktion för handkontrollen A Anpassning mellan brus och normalfördelning samt Student s t-fördelning 46

6 Minröjningsbandvagn 1 1 Inledning Saab Dynamics önskar utveckla en minröjningsbandvagn med ändamålet att den ska kunna användas för detektion av 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 utvecklade bland annat specifikationer, kontrollprogram, nätverkskommunikation och navigationstekniker. Hösten 2009 fortsatte projektgruppen Carpe Locus arbetet, och utrustade bandvagnen med fjärrstyrning, motorreglering för framdrivning samt en GPS för att kunna följa en fördefinierad vägpunktsbana. Hösten 2010 arbetade projektgruppen 8Yare med att föra över tidigare funktionalitet till en industridator och installera en stereokamera av typen Bumblebee 2. Hösten 2011 tog projektgruppen imap vid och utökade bandvagnens funktionalitet genom att med hjälp av en kamera och sensorer bygga upp en 3D-karta av ett område. Det senaste projektet genomfördes hösten 2012, av projektgruppen MinenMarker. De arbetade framförallt med bandvagnens positionering och mindetektionen, samt försökte säkerställa att bandvagnen faktiskt sökte av hela området. De främsta målen med årets projekt har varit att förbättra positioneringen, ta fram effektiva brusmodeller för sensorerna, förbättra och utöka hinderhantering samt implementera en handkontroll som kan användas istället för basstationen. Detta dokument är en beskrivning av det system för minröjning som vidareutvecklats av projektgruppen Ostende Abscondita. Dokumentet inleds med en systemöversikt för att sedan beskriva de olika delsystemen och slutligen diskutera vidare utvecklingsmöjligheter. Dokumentet är utformat i enlighet med LIPS-mallen.[10] De förkortningar som används i dokumentet beskrivs i tabell 1. Förkortning Beskrivning PWN Pulse Width Modulation IMU Inertial Measurement Unit ARM Advanced RISC Machine-processor RISC Reduced Instruction Set Computer UTF-8 UCS Transformation Format 8-bit UCS Universal Character Set GNU GNU s Not Unix GPL General Public License GCC GNU Compiler Collection WGS84 World Geodetic System 1984 EKF Extended Kalman Filter PF Partikelfilter Tabell 1: Förkortningar som används i detta dokument

7 Minröjningsbandvagn 2 2 Systemöversikt Minröjningsbandvagnen består av två huvudsystem: basstationen och bandvagnen, som i sin tur är uppdelade i mindre delsystem. Tanken med indelningen är att varje delsystem i så hög grad som möjligt ska kunna konstrueras och testas individuellt. Varje delsystem består av en eller flera fysiska komponenter, av vilka vissa delas av flera delsystem. I figur 1 visas en överblick över minröjningsbandvagnen och dess delsystem, och i figur 2 visas en skiss över informationsflödet mellan de olika delsystemen. 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; dessa beskrivs nedan.

8 Minröjningsbandvagn Globalt koordinatsystem Koordinaterna x g, y g och z g definieras utifrån det globala koordinatsystemet, vilket är fixt relativt jorden. Den på bandvagnen monterade GPS:en ger en positionsskattning uttryckt i dessa koordinater. Koordinaterna följer standarden WGS-84 där y g motsvarar latituden, x g motsvarar longituden och z g motsvarar höjden ovanför jordytan för paret y g, x g Lokalt koordinatsystem Koordinaterna x, y och z definieras utifrån det lokala koordinatsystemet. Detta är fixt i operationsområdet och är en roterad och translaterad version av det globala koordinatystemet. Origo i det lokala koordinatsystemet är punkten längst ner till vänster i det valda operationsområdet. Koordinataxlarna består av x med positiv riktning åt höger, y med positiv riktning rakt fram och z med positiv riktning rakt ut ur kartan; se figur 4. För att uttrycka de globala koordinaterna i det lokala koordinatsystemet görs en omräkning enligt [ [ ] [ ] x cos α sin α xg x = g0. (1) y] sin α cos α y g y g0 Då z-koordinaten ej används görs ingen omräkning för denna. För omräkningen krävs koordinaterna för det lokala koordinatsystemets origo, uttryckta i latitud och longitud. Dessa anges i ekvation 1 som x g0 respektive y g0. Utöver detta behöver rotationsvinkeln α mellan det lokala och globala koordinatsystemet beräknas. Denna vinkel finns utmärkt 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 α med hjälp av sambandet tan(α) = y g 1 y g0 x g1 x g0. (2) y y g x (x g0,y g0 ) α x g Figur 3: Förhållande mellan det lokala och globala koordinatsystemet

9 Minröjningsbandvagn 4 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 4: De fyra koordinatsystemen i en och samma bild där det globala koordinatsystemet representeras av en kompass

10 Minröjningsbandvagn Rutkoordinatsystem Genom att partitionera det lokala koordinatsystemet i kvadratiska rutor fås det som benämns rutkoordinatsystemet. I detta koordinatsystem representeras en viss ruta i operationsområdet med hjälp av två koordinater, i och j. Rutan längs ner till vänster får koordinaterna (0, 0). Värdet på i ökar längs med det lokala koordinatsystemets positiva x-riktning och värdet på j ökar längs med den positiva y-riktningen; se figur Bandvagnskoordinatsystem Det fjärde koordinatsystem som används är fixerat till bandvagnen och benämns bandvagnskoordinatsystemet. I detta koordinatsystem definieras x b, y b och z b genom att x b betecknar rakt fram sett ur bandvagnens perspektiv, y b rakt åt vänster och z b rakt uppåt. Origo specificeras som mittpunkten på bandvagnen. Rotationen av bandvagnens koordinatsystem relativt det lokala systemet definieras av vinkeln ψ, se figur 5. y b y x b ψ x Figur 5: Bandvagnens koordinatsystem relativt det lokala koordinatsystemet

11 Minröjningsbandvagn 6 3 Programflöde Detta avsnitt ger en översiktlig beskrivning av den mjukvara som används på bandvagnen och i basstationen. 3.1 Filstruktur I figur 6 ges en översiktlig bild av filstrukturen. Figur 6: Filstruktur 3.2 Trådning Bandvagnens mjukvara använder sig av tre stycken trådar; realtime-, slow- samt xboxtråden. Det designmässiga beslutet att dela upp koden i realtime- respektive slowtråden har tagits av tidigare projektgrupper. Denna trådning fungerar men tillför ingen egentlig prestandavinst både på grund av att den nuvarande processorn endast har en kärna och att trådningen inte är helt korrekt implementerad. För närmare information om detta; se kommentarer i källkoden. Den nuvarande projektgruppen tog ett beslut om att låta denna struktur vara kvar, då tiden ej fanns för att implementera en ny struktur. I framtiden bör dock koden skrivas om för att underlätta läsning och felsökning. Xbox-tråden är däremot ny för i år. Denna krävs för att en läsning av kommandon från handkontrollen inte ska resultera i att resten av programmet hänger sig i väntan på indata. I realtime-tråden körs kod med realtidskrav; d.v.s. kod som behöver köras snabbt, ofta och i rätt tid. Kod med lägre tidskrav körs i slow-tråden. Xbox-tråden körs parallellt med båda dessa och ger ifrån sig en signal så fort något kommando givits via handkontrollen. Flödet i och mellan trådarna visas i figur 7.

12 Minröjningsbandvagn 7 Figur 7: Trådning Realtime-tråden Realtime-tråden har en uppdateringsfunktion som ges periodtiden 100 ms med hjälp av en timer. För att realtime-tråden alltid ska köras i enlighet med periodtiden har den högre prioritet än slow-tråden. Detta innebär att om realtimetråden vill köra så kommer slow-tråden få vänta tills dess att realtime-tråden kört klart. Det första som sker då uppdateringsfunktionen triggas av timern är att sensordata samlas in. Denna sensordata buffras, vilket innebär att gammal sensordata återanvänds i de fall då ingen ny data samlats in av sensorerna sedan den senaste uppdateringen. Efter datainhämtningen körs positioneringsfiltret, vilket ger en ny skattning av bandvagnens tillstånd. När detta är klart undersöks om en mina har detekterats, och därefter kontrolleras om ultraljudssensorerna har upptäckt ett hinder. Om bandvagnen är i autonomt läge skickas tillståndsdata till regulatorn, som i sin tur ställer ut nya styrsignaler baserat på den nya informationen gällande bandvagnens position. Manuella styrsignaler vidarebefodras så snart de detekterats till styrsystemet, vilken sköter kontakten med ARM-processorn på bandvagnen. Styrsystemet ansvarar för att korrekta hastigheter sätts för larvbanden så att styrkommandot utförs. När uppdateringsfunktionen 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. Förutom det som nämnts ovan sköter realtime-tråden även nätverkskommunikationen med basstationen.

13 Minröjningsbandvagn Slow-tråden Slow-tråden sköter alla beräkningar där realtidskrav inte föreligger. Detta innebär att den bland annat beräknar nya vägpunkter baserat på bandvagnens nuvarande position och vald styrstrategi, samt sköter uppdateringen av bandvagnens interna kartor Xbox-tråden Denna tråd har implementerats för att bandvagnen snabbt ska kunna reagera på nya styrkommandon från handkontrollen. Så snart något kommando givits med handkontrollen emitteras en signal från denna tråd; om inget nytt kommando givits gör tråden ingenting. 3.3 Klassdiagram I mjukvaran ingår tre olika projektfiler av typen.pro; bandvagn.pro, basstation.pro och common.pro. I figur 8, figur 9 respektive figur 10 visas klassdiagram för dessa. Figur 8: Klassdiagram för bandvagn.pro

14 Minröjningsbandvagn 9 Figur 9: Klassdiagram för basstation.pro Figur 10: Klassdiagram för common.pro

15 Minröjningsbandvagn 10 4 Basstationen Detta kapitel beskriver delsystemet basstation och dess funktion. Basstationens uppgift är att kommunicera med bandvagnen och att presentera information om bandvagnens status i ett användargränssnitt. Delsystemet består av en dator som kommunicerar trådlöst med bandvagnen över ett wifi-nätverk. Mycket av den funktionalitet som utvecklats under tidigare års projekt har återanvänts, men användargränssnittet har byggts ut för att kunna använda och åskådliggöra nya funktioner. 4.1 Användargränssnitt Figur 11 visar användargränssnittets nuvarande utseende. Användargränssnittet är uppdelat i fyra delar som alla har olika funktion. Överst finns en statusrad där status visas för bandvagn, nätverk och handkontroll. Under denna är gränssnittet indelat i tre vyer; en inställningsvy som används för att ge kommandon till bandvagnen och ändra parametrar för körningen, en kartvy och en vägpunktsvy. De olika delarna beskrivs mer utförligt i de kommande avsnitten. Från användargränssnittet kan även ett par fönster öppnas, i vilka olika inställningar kan göras. Dessa öppnas genom att i övre högra hörnet klicka på file och välja önskat fönster. Förutom de fönster som beskrivs nedan finns även ett fönster för att konfigurera handkontrollen; detta fönster visas och beskrivs i avsnitt Statusrad Statusraden är placerad överst i användargränssnittet. Längst till vänster visas vilket avsökningsläge som för närvarande används. Till höger om detta visas information om GPS:en varefter information visas om anslutningen mellan basstation och bandvagn. Längst till höger visas information om anslutningen mellan handkontroll och bandvagn Inställningsvy Till vänster i användargränssnittet finns inställningsvyn. Denna används för att konfigurera inställningar och skicka kommandon till bandvagnen. Överst finns ett antal knappar för val av körläge, liksom start- och resetknapp. Under dessa finns de kontroller som används för att ange origo i det lokala koordinatsystemet. Dessa koordinater anges enligt RT90-standarden, och det är även möjligt att låta GPS:en sätta koordinaterna till den punkt bandvagnen befinner sig i. Operatören fyller även i en vinkel som anger operationsområdets rotation i förhållande till globala koordinater; se vinkeln α i figur 3. Utifall att operatören hämtar koordinaterna från GPS:en 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 m. Nedanför detta visas information om nuvarande styrsignal till respektive larvband. Denna signal är proportionell mot hastigheten. 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/eller estimerad position ska visas.

16 Minröjningsbandvagn Kartvy I mitten av gränsnittet finns kartvyn, vilken ämnar ge operatören en översikt över hur minsökningen fortlöper. Kartan är en modell av operationsområdet och visar de rutor operationsområdet delats in 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 färgas svart i kartvyn. Då en mina detekterats markeras den ruta där minan upptäckts med en liten minliknande figur Vägpunktsvy Längst till höger i gränssnittet finns vägpunktsvyn. Här presenteras de vägpunkter som för närvarande används av bandvagnen, och vägpunkter kan här även anges manuellt. 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 även stöd för att spara vägpunkter till och läsa in vägpunkter från en textfil Nätverkanslutningsfönster Figur 12 visar det fönster som används för att ansluta basstationen till bandvagnen. Anslutningen initieras genom att IP-adressen skrivs in och connect väljs Kontrollparameterfönster Figur 13 visar det fönster som används för att ställa in regulatorparametrar via användargränssnittet. Parametrarna i detta fönster är till för hinderhanteringens PD-regulatorer. De två första avser ObstacleController, de två mittersta RotateController och de två understa AlignController. ObstacleController ser till att bandvagnen håller sig på rätt avstånd till hindrets vägg under hindernavigering, RotateController ser till att bandvagnen roterar 90 vid svängar och AlignController utför samma sak som RotateController men reglerar med hjälp av sidosensorerna så att bandvagnen står parallellt med hinderväggen. I nuläget används bara ObstacleController och RotateController. I kapitel 5.5 finns regulatorekvationerna samt närmare detaljer om hindernavigeringen.

17 Minröjningsbandvagn 12 Figur 11: Användargränssnittet

18 Minröjningsbandvagn 13 Figur 12: Nätverksanslutningsfönster Figur 13: Kontrollparameterfönster

19 Minröjningsbandvagn 14 5 Bandvagnen 5.1 Sensorer Bandvagnen är utrustad med ett antal sensorer som används för positionering och hinderdetektering. För positionering används tre olika sensorenheter; en IMU, en GPS samt odometrar som mäter hur långt bandvagnen rört sig. För hinderdetektering används ultraljudssensorer Sampling av sensorer Sensorerna samplas med en samplingsfrekvens på 10 Hz. Varje sensorklass kommunicerar med respektive sensor eller sensorer i en egen tråd. Svarstiden från sensorerna är lång och detta skulle annars leda till att samplingen tog mycket processortid. Samtliga 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. Den ber sedan sensorn om ny data. GetData() returnerar den data som senast samplats från sensorn För närvarande har inte läsningen av odometerdata en egen tråd. Detta medför att inläsningen av odometerdata tar längre tid än för data från de övriga sensorerna IMU 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 längs tre axlar. Den specificerade prestandan för denna IMU presenteras i tabell 2. 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-GX1 Den industridator som sitter på bandvagnen kommunicerar med IMU:n via en USBanslutning. Vid sampling ber datorn IMU:n om all tillgänglig data och får då tillbaka värden för 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.

20 Minröjningsbandvagn 15 Nedan följer en beskrivning av de olika data som fås från IMU:n. Accelerationsdata Accelerationsdata används i Kalman-filtret för att skatta positionen. Förutom rå accelerationsdata inhämtas även stabiliserad accelerationsdata, där bruset reducerats genom filtrering. Vinkelhastighetsdata Bandvagnens vinkelhastighet mäts med hjälp av ett gyro. Vinkelhastigheten används i Kalman-filtret för att skatta positionen. Magnetfältsdata Den magnetfältstyrka som uppmäts av magnetometern är en summa av jordens magnetfält, magnetfält från minor i marken samt olika typer av störningar som vid körning bland annat genereras av bandvagnens motorer. Minor ger upphov till tydliga spikar i magnetfältet, och magnetfältsdata används därför för att detektera minor. Dessvärre är störningarna från bandvagnens motorer av samma storleksordning som jordens magnetfält vilket gör det svårt att använda magnetfältsdata för navigering Ultraljudssensorer Bandvagnen är utrustad med fyra stycken ultraljudssensorer för hinderdetektion. Nedan följer en beskrivning av dessa samt implementationen av dem. Samtliga är av typen SRF10, och deras placering på bandvagnen visas i figur 14. Figur 14: Ultraljudsensornas positioner samt deras lober på bandvagnen

21 Minröjningsbandvagn 16 Sensorgränssnitt Ultraljudssensorerna använder kommunikationsbussen I2C, vilken 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 ställs 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, se [3]. 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 dess tekniska specifikation, se [6]. En avståndsmätning startas genom att ett kommando skickas till sensorns kommandoregister. Datorn väntar sedan i 100 ms innan mätningen är klar och resultatet kan läsas från sensorns resultatregister. Resultatet av mätningen returneras i centimeter. Prestanda Sensorerna av typen SRF10 har en bred lob där de kan upptäcka objekt. Enligt databladet är sensorernas lob 72 bred [6], 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 Specialfall vid uppstart av systemet Vid uppstart av systemet, det vill säga när Ubuntu startar på bandvagnen, måste ultraljudssensorerna vara urkopplade. Detta beror på att de annars blockerar initieringen av kommunikation mellan industridator och 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. Odometrarna består av 500 markeringar på hjulen på respektive larvband; utplacerade på ett avstånd av mm från varandra. När en markering passerar en sensor registreras detta som ett avbrott på en ARM-processor, som givet antal avbrott sedan senaste mätning beräknar hur långt larvbandet rört sig. ARM-processorn skattar även en momentanhastighet för larvbandet baserat på tiden mellan avbrott. Det program 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 standar-

22 Minröjningsbandvagn 17 diserat 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 information om positionen även information om tid, antal satelliter som använts för positionering samt positioneringsnoggrannhet. GPS-mottagaren ger en position i latitud och longitud enligt WGS84-standarden.[5] Denna information konverteras sedan till en position i koordinatsystemet RT90[4], då de övriga systemen i bandvagnen hanterar koordinater enligt denna standard. 5.2 Handkontroll Bandvagnen är utrustad med en trådlös handkontroll; en Microsoft Wireless Xbox 360 Controller vilken kör trådlös överföring på 2.4 GHz-bandet. Denna har kopplats samman med bandvagnens dator genom den medföljande mottagaren som är i form av en USBdongel Funktionalitet Handkontrollen fungerar primärt som ett trådlöst styrdon för bandvagnen. Efter att mjukvaran på bandvagnen är startad kan bandvagnens rörelse styras med handkontrollens vänstra joystick. Utöver manuell styrning kan viss annan funktionalitet styras med handkontrollens knappar. De funktioner som finns implementerade är: Starta manuellt läge Starta automatiskt läge Starta sökläge Skicka stopp-signal Skicka återställningssignal Sätt origo för det lokala koordinatsystemet till GPS:ens aktuella positionsskattning Funktionerna är förinställda och kopplade till handkontrollens knappar enligt tabell 3. I figur 15 visas konfigurationsfönstret för handkontrollen, med hjälp av vilket det är möjligt att ändra konfigurationen för kontrollens knappar. Ur denna figur kan även knapparnas id-nummer utläsas. Funktion Knapp-id Starta manuellt läge 0 Starta automatisk läge 6 Starta sökläge 7 Skicka stopsignal 1 Skicka återställningssignal 14 Sätta origo för GPS till aktuell position 3 Tabell 3: Standardkonfiguration för handkontrollens knappar Konfigurationsfönstret för handkontrollen nås på basstationen genom att välja menyalternativet File och sedan XBoxConfiguration. När man trycker på OK skickas den

23 Minröjningsbandvagn 18 Figur 15: Konfigurationsfönster för handkontrollen nya konfigurationen till bandvagnen och skrivs till en fil på dess hårddisk. Vid uppstart läses denna fil in och den senaste konfigurationen används. Om filen saknas vid uppstart används default-inställningarna enligt tabell 3 ovan Implementation av handkontrollen Kommunikationen mellan handkontrollen och bandvagnen sker med hjälp av en i Ubuntu medföljande drivrutin, i kombination med funktioner i c-biblioteken linux/input.h och linux/joystick.h. Implementationen av handkontrollen sker framförallt i två klasser; klassen xbox i koden för bandvagnen och klassen xboxbuttonmap i koden för common. Som bas för funktioner hos handkontrollen används samma funktioner som i resten av systemet. Principiellt fungerar kommunikationen med handkontrollen genom att en fil som skapas på bandvagnen då handkontrollens mottagare ansluts läses in. Denna fil är /dev/input/jsx, där X är en siffra som tilldelas handkontrollen. Läsning av filen med hjälp av de tidigare nämnda c-biblioteken möjliggör tolkning av datan i filen, vilken då kan översättas till olika kommandon. xbox Klassen xbox hanterar kommunikationen mellan handkontrollen och bandvagnen. Detta sköts i en tråd skiljd från resterande bandvagnskod, vilken är mycket lågt prioriterad för att inte hindra det övriga systemet. Tråden lyssnar kontinuerligt efter nya signaler från handkontrollen, genom att ständigt upprepa en loop. I denna loop görs först en läsning från joystickfilen, vilken blockerar tråden tills dess att ny data finns tillgänglig. När detta sker väcks tråden och kommandot tolkas. Informationen skickas sedan vidare till realtime-tråden som kopplar ihop signalerna från handkontrollen med övrig funktionalitet.

24 Minröjningsbandvagn 19 xboxbuttonmap Klassen xboxbuttonmap hanterar information gällande aktuell konfiguration, och sköter kommunikationen gällande ändringar i knapptilldelningen för handkontrollen. 5.3 Framdrivning Bandvagnens framdrivning kontrolleras av ARM-processorn, vilken kommunicerar med industridatorn via en USB-anslutning. Bandvagnen styrs genom att olika hastigheter anges för dess båda larvband. För att ställa in hastigheterna används ett enkelt textbaserat kommunikationsprotokoll. En textsträng skickas 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 0.8 Gauss (jämför med jordens magnetfältsstyrka på ca 0.5 Gauss) och mättas därmed snabbare när den närmar 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. Beroende på avståndet från bandvagnens skattade position till de minor som redan detekterats beslutas om den detekterade minan är en ny eller sedan tidigare detekterad mina. Då en mina dekteras sätts dess position till den nuvarande skattningen av bandvagnens position. 5.5 Hinderhantering Bandvagnens hinderhantering finns primärt i klasserna ObstacleDetector och Obstacle- Explorer. ObstacleDetector har som uppgift att detektera hinder och ObstacleExplorer har som uppgift att navigera runt och kartera de hinder som upptäcks med hjälp av ObstacleDetector Detektering av hinder När ett hinder upptäcks med frontsensorerna antas det befinna sig rakt framför bandvagnen. Avståndet till hindret är då antingen medelvärdet av båda frontsensorerna; om båda får kontakt med hindret, eller avståndet från en frontsensor; om bara en får kontakt med hindret. Figur 16 visar ett fall där ett hinder upptäckts av bandvagnen. Hindrets position beräknas enligt x h = x b + r cos Ψ y h = y b + r sin Ψ, (3a) (3b)

25 Minröjningsbandvagn 20 Figur 16: Ett hinder detekteras där x b och y b betecknar bandvagnens position, x h och y h hindrets skattade position, r betecknar medelvärdet av avståndet från frontsensorerna och Ψ är bandvagnens kursvinkel. Då ett hinder detekterats kontrollerar ObstacleDetector om hindret redan finns med i hinderkartan. Om hindret finns i hinderkartan är det redan avsökt och bandvagnen behöver inte gå in i avsökningsläget. Om hindret däremot inte finns i hinderkartan, samt om någon av frontsensorerna säger att hindret är närmare än 100 cm, går bandvagnen in i avsökningsläge Navigering runt hinder När bandvagnen går in i avsökningsläget kör den rakt framåt till dess att avståndet till hindret är ungefär 70 cm. Vid 70 cm stannar bandvagnen, sparar sin startposition och roterar med hjälp av RotateController upp sig så att den står parallellt med hindrets vägg. RotateController är en PD-regulator som beskrivs av ekvationerna v l,k = K Ψ ɛ k T Ψ (ɛ k ɛ k 1 ) v r,k = K Ψ ɛ k + T Ψ (ɛ k ɛ k 1 ), (4a) (4b) där ɛ = π 2 (Ψ Ψ saved) för högersväng och ɛ = π 2 (Ψ Ψ saved) för vänstersväng. När regulatorn är ±3 ifrån sitt mål anser regulatorn att den är färdig och bandvagnen börjar navigera längs med hindret med hjälp av ObstacleController.

26 Minröjningsbandvagn 21 ObstacleController är en PD-regulator som beskrivs av ekvationerna v l,k = v k + K ɛ k + T (ɛ k ɛ k 1 ) v r,k = v k K ɛ k T (ɛ k ɛ k 1 ), (5a) (5b) där ɛ är skillnaden i avståndsmätning mellan de två sidosensorerna, i figur 17 är ɛ således skillnaden mellan r 1 och r 2. Medan bandvagnen navigerar längs med hindret skickar den hindrets position till hinderkartan. Hindrets position beräknas med hjälp av ekvationerna x h = x b + r sin Ψ y h = y b r cos Ψ, (6a) (6b) där x b och y b betecknar bandvagnens position, x h och y h hindrets skattade position, r betecknar det uppmätta avståndet till hindret och Ψ är bandvagnens kursvinkel. Figur 17: Bandvagnen vid navigering längs med hinder med hjälp av de av sidosensorerna uppmätta avstånden r 1 och r 2 När bandvagnen har passerat hindret, det vill säga då den främre sidosensorn har tappat kontakt med hindret, fortsätter den ungefär 40 cm framåt, så att den har plats att rotera. När bandvagnen har kört fram stannar den, sparar sin position och roterar 90 åt höger med hjälp av RotateController. När regulatorn är ±3 ifrån sitt mål anser den sig färdig och bandvagnen kör framåt tills dess att någon av sidosensorerna får kontakt med väggen. Då minskar hastigheten hos larvbanden och bandvagnen kontrollerar kontinuerligt hur många sidosensorer som har kontakt med väggen. Om ingen har kontakt anser bandvagnen att den kört förbi ett nytt hörn och kör ca 40 cm rakt fram innan den åter roterar; om en sensor har kontakt fortsätter den sakta framåt och om båda sensorerna har kontakt går den in i regleringsloopen för att reglera mot väggen. Processen fortsätter sedan som förut tills bandvagnen är tillbaka vid sin startposition. Om bandvagnen vid navigering längs med hindret hittar något inom 65 cm framför siganser den att det finns en vägg framför bandvagnen och utför en vänstersväng. Det finns en klass vid namn AlignController färdigimplementerad i koden som kan användas istället för RotateController. AlignController roterar upp bandvagnen så att den står parallellt med hindret genom att reglera mot skillnaden i avståndsmätningen

27 Minröjningsbandvagn 22 mellan sidosensorerna, till skillnad mot RotateController som reglerar sig 90 från färdriktningen. AlignController reglerar alltså så att bandvagnen är parallell med hindret även om bandvagnen kommer in lite snett. AlignController är en PD-regulator som beskrivs av ekvationerna v l,k = K ɛ k + T (ɛ k ɛ k 1 ) v r,k = K ɛ k T (ɛ k ɛ k 1 ), (7a) (7b) där ɛ betecknar skillnaden i avståndsmätning mellan de två sidosensorerna. När bandvagnens avståndsskilland är ca 3 cm anser regulatorn att den står parallellt med väggen.

28 Minröjningsbandvagn 23 6 Navigering 6.1 Modeller För att bra och användbara skattningar av bandvagnens tillstånd ska kunna tas fram är det viktigt att bra modeller används. I år togs två mät- och rörelsemodeller fram, baserade på föregående års modeller. De två modellerna kallas för Basic och BasicBias, och jämfört med förra årets modell har de utökats med tillstånd för acceleration samt bias för vinkelhastighet och acceleration. Den modell som bedömts vara bäst och som sedermera använts i bandvagnen är BasicBias. Nedan följer först en presentation av ekvationerna för BasicBias, där både tillstånd för acceleration och bias för vinkelhastighet och acceleration inkluderats. Därefter redogörs för brusmodelleringen Rörelsemodell Bandvagnens rörelsemodell har formen x k+1 = f(x k ) + w k, (8) där x k betecknar tillståndsvektorn vid sampel k. Funktionen f betecknar en olinjär funktion som beskriver bandvagnens dynamik och w k betecknar processbruset. För BasicBias består tillståndsvektorn av x k y k v k x k = ψ k ω k, (9) a k ωk b a b k där x k och y k betecknar bandvagnens position i det lokala koordinatsystemet. v k och a k motsvarar hastighet respektive acceleration i bandvagnens färdriktning. ψ k motsvarar vinkeln mellan bandvagnens färdriktning och x-axeln i det lokala koordinatsystemet. ω k betecknar vinkelhastighet och ωk b och ab k motsvarar bias för vinkelhastigheten respektive accelerationen. Den nuvarande modellen för dynamiken är baserad på en sammanslagning av de diskretiserade modellerna (12.12) och CA (i tabell 13.1) i [7]. Detta innebär att det är en constant acceleration constant turn model. De delar som tagits från (12.12) har förenklats genom att vinkelhastigheten ω k har antagits vara liten. f definieras då som x k + T v k cos(ψ k ) + T 2 2 a k cos(ψ k ) T 2 2 ab k cos(ψ k) y k + T v k sin(ψ k ) + T 2 2 a k sin(ψ k ) T 2 2 ab k sin(ψ k) v k + T a k T a b k f(x k ) = ψ k + T ω k T ωk b ω k ωk b, (10) a k a b k ωk b a b k

29 Minröjningsbandvagn 24 där T betecknar sampeltiden; tiden mellan det senaste föregående och nuvarande sampel. Då ett första ordningens EKF används i bandvagnen behövs även Jacobianen för att lösa ekvation (10). Jacobianen definieras enligt T 1 0 T cos(ψ k ) F k, cos(ψ k) 0 T 2 2 cos(ψ k) T 0 1 T sin(ψ k ) F k, sin(ψ k) 0 T 2 2 sin(ψ k) T 0 T F k = T 0 T , (11) där F k,14 = T v k sin(ψ k ) T 2 2 a k sin(ψ k ) + T 2 2 ab k sin(ψ k) och F k,24 = T v k cos(ψ k ) + T 2 2 a k cos(ψ k ) T 2 2 ab k cos(ψ k)). I det kontinuerliga fallet antas processbruset w vara Gaussiskt fördelat med medelvärde 0 och kovarians Cov(w). w är det brus som adderats till tillstånden w k, a k, w b k och ab k. Detta brus påverkar även de övriga tillstånden då modellen diskretiserats enligt Q k = GCov(w)G T, (12) där T 3 6 cos(ψ k) T 3 6 sin(ψ k) T T G = (13) 0 T 0 0 T Mätmodell Bandvagnens mätmodell har formen y k = h(x k ) + e k, (14) där h betecknar en funktion som beskriver vad som mäts och e k betecknar mätbrus. För BasicBias består mätningen av y k = x GP S y GP S ω IMU v r+v l 2 v r v l d v a IMU, (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 motsvarar hastigheten på höger respektive vänster larvband. Konstanten d v betecknar en virtuell bredd,

30 Minröjningsbandvagn 25 definierad genom att modellera bandvagnen som en bredare robot med hjul istället för larvband. Denna modell används för att modellera slippet hos bandvagnen, som då ses som ett fordon med två hjul med bredd d v = 0.6. Detta är en lösning som har följt med sedan tidigare projekt [9]. a IMU betecknar accelerationen. I den nuvarande implementeringen definieras h(x k ) som x k y k h(x k ) = ω k + ωk b v k. (16) ω k a k + a b k Uttryck 16 medför att det finns två olika mätningar av vinkelhastigheten att tillgå, där den ena kommer från IMU:n och den andra mäter differensen mellan larvbandens hastigheter. Här syns även att det är vid mätningen av vinkelhastigheten och accelerationen, båda från IMU:n, som det tillkommer modellfel i from av bias. Jacobianen för (16) blir H k = (17) Brusmodeller Brus har studerats för relevanta sensordata. Nedan följer en beskrivning av respektive brusmodell samt en förklaring till varför detta brus var av intresse att studera. En översikt av de olika brus som analyserats, samt deras eventuella fördelning tillsammans med relevanta parametrar syns i tabell 4. De fördelningar som togs fram var alla av antingen typen normalfördelning eller Student s t-fördelning. Med medelvärdet µ och standardavvikelsen σ är täthetsfunktionen för normalfördelningen f(x) = 1 σ 2π (x µ) 2 e 2σ 2 (18) och för Student s t-fördelning f(t) = där Γ är Eulers gamma-funktion. 1 Γ( n 2 ) 1 (n 1)π Γ( n 1 2 ) (1 + t2 n 1 ) n 2, (19) Accelerometer Data från accelerometern används i EKF:et och då detta utgår från premissen att allt brus är normalfördelat [7] behövde detta bekräftas. Accelerometern mäter acceleration i x-, y- och z-led och det som var av största intresse var accelerationen i x-led. Även accelerationen i y-led skulle kunna vara intressant att

31 Minröjningsbandvagn 26 Brus Situation Fördelning Parametrar Accelerometer x-led Stillastående Normal µ = 0, σ = Accelerometer y-led Stillastående Normal µ = 0, σ = Accelerometer x-led Konstant hastighet Normal µ = 0, σ = Accelerometer y-led Konstant hastighet Normal µ = 0, σ = Vinkelhastighet z-led Stillastående Normal µ = 0, σ = Vinkelhastighet z-led Konstant hastighet Normal µ = 0, σ = GPS x-led Stillastående Vandrar GPS y-led Stillastående Vandrar GPS x-led Under körning Vandrar GPS y-led Under körning Vandrar Odometer distans vänster Stillastående Ingen Konstant 0 Odometer distans höger Stillastående Ingen Konstant 0 Magnetometer x-led Stillastående Student s t µ = 0.20, σ = Magnetometer y-led Stillastående Student s t µ = 0.22, σ = Magnetometer z-led Stillastående Student s t µ = 0.50, σ = Magnetometer totalt Stillastående Student s t µ = 0.59, σ = Magnetometer x-led Under körning Student s t µ = 0.20, σ = Magnetometer y-led Under körning Student s t µ = 0.19, σ = Magnetometer z-led Under körning Student s t µ = 0.48, σ = Magnetometer totalt Under körning Student s t µ = 0.55, σ = Tabell 4: Översikt över fördelningar för olika sensordata. använda men i årets projekt har den inte använts. Av denna anledning var det framförallt variabeln ax som studerades. Analys av bruset gjordes både då bandvagnen stod still och då bandvagnen startade från stillastående och sedan kördes rakt framåt med konstant hastighet. När bandvagnen stod still borde accelerationen i x-led självklart visa 0. Det verkliga resultatet syns längst upp i figur 18. Figur 18: ax (längst upp) och sax (längst ned) då bandvagnen står still

32 Minröjningsbandvagn 27 Som synes är datan väldigt brusig, vilket gör det svårt att använda accelerometern för positionsbestämning. Extra svårt blir det då positionsbestämning med hjälp av accelerometern innebär att accelerometerdata integreras två gånger, och vid mycket brusig signal kommer positionsbestämningen då att bli mindre och mindre korrekt desto längre tiden går. Av denna anledning undersöktes möjligheten att förfiltrera accelerometerdata innan det användes i EKF:et, och på så sätt få en mindre brusig signal med mindre varians. En sådan förfiltrerad signal finns dock redan som utsignal från IMU:n, och har benämningen sax. Denna signal visas tillsammans med den ofiltrerade signalen ax i figur 18 då bandvagnen står still och i figur 19 då bandvagnen kör rakt fram med konstant hastighet. Figur 19: ax (längst upp) och sax (längst ned) då bandvagnen kör rakt fram med konstant hastighet Användandet av sax som insignal istället för ax har testats tillsammans med EKF i Matlab, men impementering av denna lösning på bandvagnen har ej gjorts. Vid simulering verkar det dock lovande. En jämförelse av överensstämmelsen för bruset i x-led med normalfördelningen och Student s t-fördelning visas i figur 29. Där syns att bruset stämmer väl överens med normalfördelningen. Flera tester gjordes, och även om ett Anova-test förkastade att medelvärdet var detsamma för samtliga tester drogs slutsatsen att medelvärdet låg kring noll, då medelvärdet för olika tester blev varierande positivit och negativt med ständigt litet absolutbelopp. Standardavvikelsen valdes till den under testerna maximala uppmätta standardavvikelsen. I figur 30 visas överensstämmelsen mellan accelerationen i x-led då bandvagnen kör med konstant hastighet, både för normalfördelningen och Student s t-fördelning. Ingen av fördelningarna passar perfekt, men anpassningen anses vara tillräckligt god. t-test kunde ej förkasta att medelvärdet i x-led var noll varför noll valdes som medelvärde även här. Standardavvikelsen sattes som tidigare till den högst uppmätta under testerna. Medelvärdet för accelerationen i x-led valdes alltså till noll, både då bandvagnen står still och kör. Detta är en förenkling som kompenseras för genom införandet av ett tillstånd i EKF:et för accelerationens bias.

33 Minröjningsbandvagn 28 Vinkelhastighet Även vinkelhastigheten används i EKF:et, varför modellering av dess brus var av intresse. En anpassning mellan bruset för vinkelhastigheten i z-led då bandvagnen står still respektive kör för normalfördelningen samt t-fördelningen visas i figur 31 respektive 32. Medelvärdet var nära nog noll både då bandvagnen körde och då den stod still, varför detta valdes som medelvärde. Standardavvikelsen valdes som den största uppmätta. Även här kompenseras för det förenklade medelvärdet genom att ett tillstånd för vinkelhastighetens bias implementerats i EKF:et. GPS GPS används för positionsbestämning och mätningarna i x- och y-led är de som är av största intresse. GPS-koordinaterna används både i EKF:et och i partikelfiltret. Resultatet av en GPS-mätning då bandvagnen står still syns i figur 20. Mätningen är uppenbarligen inte särskilt exakt, utan från de ursprungligen identifierade GPS-koordinaterna flyttar sig bandvagnen enligt GPS-mätningen nästan fem meter. Det syns tydligt att felaktigheten i GPS-mätningen inte är slumpartad utan varje punkt ligger i närheten av föregående punkt. Figur 20: Resultat av GPS-mätning då bandvagnen står still Ett annat test gjordes då bandvagnen kördes i en fyrkant om ca 2x2 m. GPS-värdena torde då ge något som liknar en fyrkant. GPS-data för denna körning presenteras i figur 21. Här håller sig GPS-data närmare sanningen än när bandvagnen står still, som i figur 20, men slutpunkten hamnar ändå två meter från det som rimligtvis borde vara den riktiga slutpunkten; samma som startpunkten. Även här syns tydligt att någon typ av förhållande föreligger mellan efter varandra följande punkter. På grund av korrelationen mellan punkterna är ingen god anpassning möjlig mellan GPSbruset och en normalfördelning, eller andra kända fördelningar för den delen. För implementation av partikelfiltret behövdes dock en fördelning för bruset tas fram. Antagligen sker någon form av filtrering av bruset i GPS:en, och med den GPS-modell som används idag är det ej möjligt att få fram rådata. Ansatsen blev istället att försöka omvända den filtrering som görs i GPS:en.

34 Minröjningsbandvagn 29 Figur 21: Resultat av GPS-mätning då bandvagnen kör i en fyrkant Hypotesen var att det GPS-brus som noterades egentligen var linjärt filtrerat vitt brus, det vill säga resultatet av en ARMA-process. En ARMA-process beskrivs av y = C(z) D(z) e vilket ger en AR-process om c i = 0, i 0 och en MA-process om d i = 0, i 0.[12] Efter att både AR- och MA-processer studerats var för sig var det tydligt att AR-processen bättre beskrev den filtrering som gjordes. En jämförelse efter filtrering genom en ARrespektive MA-process för GPS-data i x-led, då bandvagnen står still, syns i figur 22. Valet av filtreringsprocess förr slutligen på en AR-modell av ordning 2 för både x- och y-värden. Den beskrivs i x-led av och i y-led av gpsx filter = z 2 z z gpsx real (20) gpsy filter = z 2 z z gpsy real. (21) Det är inversen av detta som kan användas för att omvända den filtering som görs i GPS:en, det vill säga gpsx real = z z z 2 gpsx filter (22)

35 Minröjningsbandvagn 30 Figur 22: GPS-data i x-led då bandvagnen står still, filtrerat genome en AR-process (t.v.) respektive en MA-process (t.h.), båda av ordning 2 i x-led och gpsy real = z z z 2 gpsy real (23) i y-led. Filtrering med dessa filter ger brus med täthetsfunktioner enligt figur 23. Figur 23: Täthetsfunktion för GPS-brus efter filtrering, i x- (t.v.) respektive y-led (t.h.) Odometer Odometern används för positionsbestämning, och ger väldigt exakta värden då bandvagnen står still. Både hastigheten och distansen är då enligt mätningar konstant exakt noll, som sig bör.