Designspecifikation Autonom målföljning med quadcopter

Transkript

1 Version 1.0 Robo Ptarmigan 30 november 2015 Status Granskad NE,HC Godkänd

2 Projektidentitet Gruppmail: Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: Christian A. Naesseth, Linköpings universitet Telefon: , Mail: Maria Andersson, FOI Mail: Daniel Axehill, Linköpings universitet Telefon: , Mail: Karin Lockowandt Clas Veibäck, Linköpings universitet Telefon: , Mail: Gruppmedlemmar Befattning Namn Telefon Mail Karin Lockowandt Projektledare karlo343 Albin Flodell Testansvarig albfl803 Dokumentansvarig hamca089 Cornelis Christensson Mjukvaruansvarig corch348 Anders Brändström Integrationsansvarig andbr957 Niklas Ericson Designansvarig niker917 Gustav Norin Informationsansvarig gusno119

3 Dokumenthistorik Version Datum Ändringar Signatur Granskare Första versionen Robo Ptarmigan NE, HC Andra utkastet Robo Ptarmigan KL,CC,HC Första utkastet Robo Ptarmigan KL,CC

4 Innehåll 1 Inledning Parter Projektets bakgrund Syfte och mål Användning Hårdvara Definitioner Översikt av systemet Koordinatsystem Styrmoder Manuell styrning Målföljning Följning av mål Ingående delsystem Huvudbuss Övergripande design Modulbeskrivning Hantering av gemensamma resurser Informationsutbyte mellan moduler Kommunikationsmodul Övergripande design Modulbeskrivning Terminologi Gränssnitt Telemetri Videoström AT-kommandon Initiering av kommunikationslänk Räckvidd GUI Övergripande design Modulbeskrivning Program och språk Layout för GUI:t Kameramodul Övergripande design Modulbeskrivning Funktioner Mätmodell Inverterad mätmodell Omvandling mellan earth och body-koordinater

5 7 Associeringsmodul Övergripande design Modulbeskrivning Funktioner Gater Nearest Neighbor Positioneringsmodul Övergripande design Modulbeskrivning Funktioner Associera positionsmarkörer Sensorfusion med partikelfilter Målföljningsmodul Övergripande design Modulbeskrivning Funktioner Målet och dess rörelsemodell Tidsuppdatering Linjärisering av mätfunktion Associationsproblemet Skatta målets tillstånd Planeringsmodul Övergripande design Modulbeskrivning Manuell mod Avsökning av bana Målföljning Funktioner Bildbehandlingsmodul Övergripande design Modulbeskrivning Terminologi Detektion av objekt Måldetektion i en bild Förbehandling Formdetektion Färgdetektion Detektion av positionsmarkörer i en bild Rensa och sortera objekt Lagringsmodul Övergripande design Modulbeskrivning Skapa och spara filen

6 12.4 Skriva data till filen Initiera från fil

7 1 1 Inledning Detta dokument är en designspecifikation för CDIO-projektet Autonom målföljning med quadcopter. Projektet är en del i kursen - Reglerteknisk projektkurs som ges vid Linköpings universitet. en ska ingående och detaljerat förklara hur varje delsystem ska vara uppbyggt samt hur gränssnittet mellan de olika delsystemen ska se ut. Dessutom ges en översikt av systemet. 1.1 Parter I projektet finns följande parter Kund: Maria Andersson, FOI Beställare: Christian A. Naesseth, ISY Handledare: Clas Veibäck, ISY Examinator: Daniel Axehill, ISY Projektgrupp: Robo Ptarmigan 1.2 Projektets bakgrund Det finns idag ett ökande intresse för autonoma och obemannade farkoster (UAV:er). Användningsområdena för UAV:er är stort då de kan användas på ställen som är farliga eller på andra sätt olämpliga för människor. UAV:er kan användas vid naturkatastrofer och nödsituationer för att samla in information med hjälp utav deras sensorer. För att vara effektiva hjälpmedel och fungera så självständigt som möjligt måste UAV:erna ha korrekt information om sin egen och andra föremåls positioner. 1.3 Syfte och mål Syftet med projektet är att projektgruppen ska få tillämpa inhämtad kunskap i praktiken och arbeta på ett sätt som liknar arbetslivet. Målet med projektet är att vidareutveckla fjolårets quadcopter till att kunna köra inomhus utan GPS-signal men med en karta tillgänglig. Dessutom är tanken att systemet ska få utökad funktionalitet för multimålföljning samt kunna följa efter ett enskilt mål som rör sig. 1.4 Användning Tanken är att quadcoptern ska kunna användas för att avsöka områden som är farliga för människor att beträda men man ändå vill ha uppsyn över. Det kan till exempel vara en industrilokal där det brinner eller ett krigsscenario där man vill undersöka en fiendes rörelser inom ett område. Det långsiktiga målet för projektet är att skapa en bas för en FOI-demonstrator som kan visa nyttan och användningsområden för UAV:er, samt hur dessa kan användas för att spara liv och öka säkerheten.

8 2 1.5 Hårdvara I systemet ingår en quadcopter av typen Parrot AR-Drone 2.0 med tillhörande batterier, samt en dator. Quadcoptern innehåller förutom sitt interna drivpaket och regulatorer även ett antal sensorer, såsom en videokamera, en GPS, ett gyro, accelerometrar och en ultraljudssensor som används för att mäta höjden. GPS:en kommer dock inte att användas i detta projekt. 1.6 Definitioner Nedan följer ett antal definitioner på ordval som används i denna rapport. Standardmål: Objekt som skiljer sig markant från omgivningen och känns igen av bildbehandlingsmodulen. Positionsmarkör: Ett objekt som används för att öka precisionen på plattformens positionering. En positionsmarkör är en typ av standardmål, och har storleken 20x20 cm. Utseendet på en positionsmarkör beror på om den ligger i ett hörn, på en kant, eller i mitten av en bana. Bana: Ett rektangulärt område markerat med positionsmarkörer i ett rutnät med 2 m mellanrum. Plattform: Med detta menas quadcoptern. Program: Datorprogrammet som kommunicerar med plattformen, analyserar data samt skickar styrsignaler till plattformen.

9 3 2 Översikt av systemet Då produkten, som består av en quadcopter samt programvara, är färdigutvecklad ska denna kunna utföra målföljning för multipla mål, följning av enkelmål samt kunna styras manuellt. Produkten har delats upp i olika moduler eller delsystem. Tanken är att varje modul ska kunna utvecklas och testas enskilt. För att detta ska fungera krävs ett tydligt gränssnitt. I figur 1 visas hur modulerna som bygger upp produkten interagerar mellan varandra. Figur 1: Projektets modulstruktur. 2.1 Koordinatsystem Två stycken olika koordinatsystem kommer att användas i projektet. Koordinatsystemen är definierade enligt följande. Earth coordinates: Origo är definierat som sydvästra hörnet i banan. x e -axeln pekar alltid mot norr, y e -axeln pekar alltid mot öster och z e -axeln pekar alltid nedåt. Detta innebär att riktningen hos dessa axlar är oberoende av plattformens orientering. Body coordinates: Detta koordinatsystem är definierat utgående från plattformen. x b -axeln pekar rakt fram, y b -axeln åt höger och z b -axeln rakt ner utifrån plattformen sett. En illustration av koordinatsystemet visas i figur 2. Utifrån dessa koordinatsystem kan ett antal vinklar definieras för att beskriva rotationen mellan koordinatsystemen. Dessa är definierade enligt följande. Rollvinkel: Rotationen runt z e -axeln. Tiltvinkel: Rotationen runt skärningen mellan planen som spänns upp av x b, y b samt x e, y e axlarna. Girvinkel: Rotationen runt z b -axeln.

10 4 Figur 2: Plattformens koordinater i de olika systemen Translationen från tracking frame till body frame ges av plattformens position. Rotationsmatrisen från tracking frame till body frame, R TB ges i ekvation 1. cos θ cos ψ cos θ sin ψ sin θ R TB = sin φ sin θ cos ψ cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ (1) cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ sin φ cos ψ cos φ cos θ I ekvationen ovan är φ rollvinkeln, θ tiltvinkeln och ψ girvinkeln. Rotationsmatrisen för att gå åt motsatta hållet, det vill säga från body frame till tracking frame ges av inversen till R BT vilket är transponatet av matrisen då rotationsmatrisen är ortogonal. Detta kan sammanfattas i följande ekvation: R BT = R TB T. 2.2 Styrmoder Produkten ska kunna hantera tre olika styrmoder. Dessa är manuell styrning, målföljning samt följning av mål. Det är endast GUI:t samt planeringsmodulen som påverkas av aktuell styrmod. Resterande moduler ska arbeta oberoende av denna. Respektive styrmod beskrivs mer detaljerat i avsnitten nedan Manuell styrning I denna styrmod styrs plattformen manuellt av en operatör via GUI:t. Styrkommandona som genereras skickas sedan vidare till planeringsmodulen som tolkar dessa och omvandlar till kommandon som kan tolkas av plattformen. Dessa skickas sedan till plattformen via kommunikationsmodulen som utför dessa Målföljning I denna styrmod ska en fördefinierad bana avsökas av plattformen. Planeringsmodulen planerar en rutt som plattformen ska åka och ser sedan till att denna följs genom reglering. Plattformen ska detektera mål och visa upp dessa via GUI:t. Målen kan vara både stillastående och rörliga.

11 Följning av mål I denna styrmod ska plattformen följa efter ett mål. Det är operatören som via GUI:t väljer vilket mål som ska följas. Mål som detekteras under tiden som följning görs ska också visas upp via GUI:t. 2.3 Ingående delsystem Nedan beskrivs kort syftet med varje delsystem. Huvudbuss: Denna modul har till uppgift att hantera all intern kommunikation genom att vara ett gränssnitt mellan alla moduler. Kommunikationsmodul: Denna modul har till uppgift att hantera den trådlösa kommunikationen via Wi-Fi. GUI: Denna modul visar data från körning, videoström från plattformen samt detekterade positionsmarkörer och mål. Det är även här som operatören väljer vilken styrmod som planeringsmodulen ska använda. Positioneringsmodul: Denna modul har till uppgift att utifrån observerade positionsmarkörer och sensordata skatta plattformens position. Målföljningsmodul: Denna modul ska hålla reda på de detekterade målen samt skatta en trajektoria för respektive mål. Planeringsmodul: Denna modul ska innehålla tre olika styrmoder. Dessa är manuell styrning, avsökning av bana samt följning av mål. Det är denna modul som planerar hur plattformen ska åka och genererar styrsignaler för att uppfylla detta. Bildbehandlingsmodul: Denna modul har till uppgift att hitta standardmål och positionsmarkörer i bilderna samt positionera dessa i pixelkoordinater. Lagringsmodul: Denna modul har till uppgift att hantera lagring av data, som kan analyseras för att utveckla programmet ytterligare.

12 6 3 Huvudbuss Detta avsnitt beskriver implementationen av systemets huvudbuss. Modulens huvudansvar är att initiera alla övriga moduler, samt att distribuera data mellan dem. 3.1 Övergripande design Figur 3 nedan beskriver dataflöden som går genom huvudbussen. Modulen skickar data till samtliga moduler och mottager data från alla utom lagringsmodulen. För mer information om vad in- och utdata består av, se respektive moduls avsnitt nedan. Figur 3: Huvudbussens dataflöden. 3.2 Modulbeskrivning Denna modul utgör huvudtråden i programmet och ansvarar för den interna kommunikationen mellan de olika modulerna. Den sköter lagring av gemensamma variabler och gör på så sätt att systemet blir mer modulärt. Vid anrop startar huvudbussen resterande moduler i varsin tråd. Ett exempel på en gemensam variabel är plattformens position. 3.3 Hantering av gemensamma resurser Då Java innehåller funktionalitet för att skydda gemensamma resurser lämpar sig trådstrukturen väl. För att skydda de gemensamma resurserna kommer så kallade syncronized-block att användas. Ett exempel på hur det går till visas nedan. synchronized(sharedresource){ //Code that uses shared resources goes here } Denna funktion fungerar så att den låser alla andra trådar som försöker komma åt en gemensam resurs under tiden som koden inom blocket körs, för att förhindra komplikationer som kan uppstå om man till exempel försöker läsa och skriva till samma variabel samtidigt. Detta leder således till att endast en tråd med ett skyddat block kod kan köras åt gången. Därför är det viktigt att tänka på att endast använda skyddet för

13 7 de delar av koden som faktiskt läser eller skriver till den gemensamma resursen. Annars kommer programmet fort att bli för långsamt för att kunna fungera på ett bra sätt. 3.4 Informationsutbyte mellan moduler Tanken är att all data ska skickas via huvudbussen. För att möjliggöra detta kommer huvudbussen innehålla funktioner för att läsa variabler från andra klasser som används i mer än en modul. Ett exempel på hur en sådan funktion i huvudmodulen skulle kunna vara skriven visas nedan. public synchronized Position getposition() { return PositionModule.getPosition(); } Det gäller dock att vara noggrann med att positionsmodulens getposition() ser till att skicka med en kopia istället för en referens till objektet. Annars riskerar man att få en onödigt lång låsning av den gemensamma resursen.

14 8 4 Kommunikationsmodul Detta avsnitt beskriver implementationen av kommunikationsmodulen. Modulen är ett gränssnitt för kommunikation mellan plattform och dator. 4.1 Övergripande design Kommunikationen sker trådlöst via Wi-Fi, genom att plattformen agerar som server och tillhandahåller ett nätverk som programmet via kommunikationsmodulen kan ansluta till som klient. Den teoretiska överföringshastigheten är på upp till 300 Mbit/s, men för detta projekt räcker en överföringshastighet långt under detta. Det är antaget att den överföringshastighet som uppnås i praktiken är tillräcklig. I figur 4 visas vilka dataflöden som går till och från kommunikationsmodulen. Figur 4: Kommunikationsmodulens övergripande funktionalitet 4.2 Modulbeskrivning Kommunikationsmodulen sköter kommunikationen mellan plattformen och datorn. Den ansvarar för att video och sensorvärden strömmas från plattformen, och att styrkommandon överförs till plattformen. All kommunikation mellan plattform och datorprogram kommer att ske över det trådlösa nätverk (Wi-Fi) som plattformen tillhandahåller. Denna funktionalitet ska redan finnas från föregående års projekt. Det som behöver göras är att kontrollera att allting fungerar utan problem. 4.3 Terminologi UDP: User Datagram Protocol, ett paketorienterat protokoll för att skicka data över IP-nätverk. Protokollet genererar minimalt med overhead, men har ingen felhantering. TCP: Transmission Control Protocol, ett förbindelseorienterat protokoll för att skicka data över IP-nätverk. Protokollet garanterar felfri överföring, men med mer overhead än UDP. AT-kommando: En standard för att skicka instruktioner, ursprungligen utvecklad för kommunikation med modem.

15 9 4.4 Gränssnitt All kommunikation mellan plattformen och programmet sker via ett Wi-Fi-nätverk, som plattformen själv tillhandahåller. Nätverket är öppet och har ESSID ardrone , och plattformen har IP-adressen Serversidan är given av AR.Drone-programvaran, och det kommunikationsgränssnitt som är fördefinierat av denna visas i tabell 1. Data Port Protokoll Navdata 5554 UDP Videoström 5555 TCP AT-kommandon 5556 UDP Kritisk data 5559 TCP Tabell 1: Gränssnitt för kommunikation mellan plattform och program. Nedan förklaras de olika kommunikationstyper som finns mellan plattform och program Telemetri Telemetri (navdata) skickas från plattformen till programmet på port Den innehåller all sensordata och status för plattformen, och skickas med en uppdateringsfrekvens på 200 Hz. Informationen skickas från plattformen i binärt format (little endian), och består av ett antal sektioner uppdelade i datablock kallade för options. I figur 5 visas hur en sektion är uppbyggd. Figuren är hämtad från AR.Drone Developer Guide[2]. Figur 5: En sektion Kommunikationsmodulen ska utifrån mottagen telemetri läsa ut position i z e -led, hastighet i x b, y b, z b samt gir-, tilt- och rollvinkel (ψ, φ, θ) Videoström På port 5555 skickas videoströmmen med TCP från plattformen till programmet. Videoströmmen kommer att avkodas med hjälp av ett FFMPEG-bibliotek. Det går att ställa in hur stor datatakt videoströmmen ska skickas med, mellan 250 kbps och 4 Mbps AT-kommandon Kontroll och kommunikationskommandon till plattformen sker via AT-kommandon med UDP på port Dessa måste skickas tillräckligt frekvent för att plattformen fortfarande ska behålla kontakten. Tillverkaren av plattformen anger 30 gånger i sekunden som ett bra värde. Exempel på möjliga kommandon och deras parametrar (utom sekvensnummer), ges i tabell 2.

16 10 AT-kommando Parametrar Beskrivning AT*REF input Lyft/Landa/Nödstopp AT*PCMD flag, roll, pitch, gaz, yaw Kontrollera plattformen AT*PCMD MAG flag, roll, pitch, gaz, yaw, psi, psi accuracy Kontrollera plattformen (absolut kontroll) AT*FTRIM - Ställ referens för horisontalplan AT*CONFIG nyckel, värde Konfigurera AR.Drone AT*LED frekvens, tid Kontrollera LED-animation AT*ANIM animation Skapa en flyganimation AT*COMWDG - Nollställ kommunikationsvakthunden AT*CALIB enhetsnummer Kalibrera magnetometer (måste skickas under flygning) Tabell 2: Möjliga kontroll och kommunikationskommandon. Styrning av plattformen sker enklast via kommandot AT*PCMD. Detta kommando kräver följande argument: sekvensnummer, flagga, tiltvinkel, rollvinkel, vertikal hastighet, samt girvinkelhastighet. Ett alternativt sätt att styra plattformen är att använda AT*PCMD MAG, som utöver de argument som krävdes för AT*PCMD även kräver önskad girvinkel (utifrån magnetometern), samt noggrannheten för denna. 4.5 Initiering av kommunikationslänk Initiering av kommunikationslänken mellan programmet och plattformen sker via sekvensen i figur 6. Figur 6: Sekvens för initiering av kommunikation

17 Räckvidd Tillverkaren av plattformen uppger att räckvidden är runt 50 meter. Eftersom produkten är tänkt att köras inomhus i en sal, kommer räckvidden inte att bli någon begränsande faktor.

18 12 5 GUI Detta avsnitt beskriver implementationen av GUI. Modulen sköter gränssnittet mot operatören. 5.1 Övergripande design GUI:t ska visa information från plattformen och hantera användarens input. Den viktigaste funktionaliteten ska finnas direkt i fönstret och de mer avancerade finns i menyerna högst upp i fönstret. I figur 7 visas ut- och indata för GUI:t. Figur 7: GUI:s övergripande funktionalitet 5.2 Modulbeskrivning Denna modul sköter gränssnittet mot operatören. Mycket av funktionaliteten som ska implementeras finns redan sedan föregående års projekt. Dock kommer mindre förändringar att krävas för att kunna använda en fördefinierad karta istället för att ladda ner denna från Google Maps. Operatören ska i GUI:t kunna välja nödstopp som avbryter alla aktiviteter på plattformen. Det tillkommer även funktionalitet i form av att styrmod ska kunna väljas av operatören. Detekterade objekt ska ritas ut på kartan tillsammans med plattformens skattade position. Det ska även gå att välja maximal höjd för plattformen för att undvika att denna flyger in i tak då inomhusflygning ska klaras av. 5.3 Program och språk För att skapa ett grafiskt gränssnitt behövs ett grafikbibliotek. Det grafikbibliotek som kommer att användas är Java FX då detta används i föregående års projekt. Grafikbiblioteket kombinerat med tilläget Screen Builder gör det möjligt att bygga GUI:t med standardkomponenter som flikar, knappar och dialogrutor. 5.4 Layout för GUI:t En skiss över GUI:t kan ses i figur 8. Det ska finnas knappar för avfärd, landning och nödstopp i GUI:t. Övrig funktionalitet som berör plattformen som val av styrmod, val av max höjd och om målföljning ska kunna väljas i en meny. Plattformen och eventuella mål ska visas på en karta över banan. Under flygning ska video från plattformens kamera

19 13 visas i GUI:t. I en meny går det välja om positionsmarkörer och mål i video ska markeras. Övrig information som ska visas i GUI:t är batterinivån i plattformen och Wi-Fi status. Figur 8: Schematisk skiss över användargränssnittet

20 14 6 Kameramodul Detta avsnitt beskriver implementationen av kameramodulen. Modulen ansvarar för omvandlingen mellan olika koordinatsystem. Den kommer att omvandla mellan rumskoordinater och pixelkoordinater och dels mellan earth- och body-koordinater. 6.1 Övergripande design I figur 9 nedan visas vilka insignaler och utsignaler som modulen har. Omvandlingen mellan koordinaterna kräver en mätekvation. Denna ekvation beror på kamerans egenskaper samt plattformens position och orientering i rummet. Kameramodulen ska klara av att omvandla både pixelkoordinater till rumskoordinater samt rumskoordinater till pixelkoordinater. Figur 9: Kameramodulens övergripande funktionalitet I figur 10 visas ett flödesschema över modulens olika delmoment. Figur 10: Flödesschema för kameramodulen 6.2 Modulbeskrivning Nedan beskrivs mer detaljerat hur kameramodulen genomför omvandlingen mellan rumsoch pixelkoordinater.

21 Funktioner Tabell 3 nedan listas de olika funktionerna som ska implementeras i kameramodulen. Funktionsnamn initialize sethmatrix convertroomtopixel convertroomtopixelparticle convertpixeltoroom gethmatrix Beskrivning Initierar alla variabler och tillstånd. Väljer värde på H matrisen. Konverterar rumskoordinat till pixelkoordinat. Beräknar pixelkoordinater för positionsmarkörer för alla partiklar. Konverterar pixelkoordinat till rumskoordinat. Returnerar H-matrisen. Tabell 3: Funktioner för kameramodulen Mätmodell Detektioner från bildbehandlingsmodulen ges som pixelkoordinater i en bild. Det är viktigt att kunna mappa dessa pixelkoordinater mot positioner i x e y e -planet. Detta sker med mätfunktionen som beskrivs i ekvation 2. Det antas att allt som kameran ser har höjden noll vilket innebär att observerade objekt ligger på golvet. H = h 1,1 h 1,2 h 1,3 h 1,4 h 2,1 h 2,2 h 2,3 h 2,4 (2) h 3,1 h 3,2 h 3,3 h 3,4 Alla parametrar i ekvationen ovan är okända. Denna kan dock förenklas genom att dela upp den i två matriser, en matris som innehåller kameraegenskaperna så som fokallängd och principalpunkter och en annan matris som innehåller information om plattformens position och orientering. Omskrivningen kan ses i ekvation 3b. f x 0 p x r 11 r 12 r 13 t 1 H = 0 f y p y r 21 r 22 r 23 t r 31 r 32 r 33 t 3 ( RBT (φ, ψ, θ) T (p) ) r 11 r 12 r 13 t 1 = r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 (3a) (3b) I ekvationen ovan representerar f fokallängden och p principal-punkterna. R BT är rotationsmatrisen mellan de olika planen som beror på plattformens attityd och har dimensionen [3x3] och T translationsmatrisen mellan de olika planen vilken beror på plattformens position och vars dimension är [3x1]. För att beräkna kameraparametrarna används Camera Calibration Toolbox. Matrisen H beskriver hur punkter i rumsplanet mappas till pixelplanet där mätningarna ges. Detta kan skrivas som x p λ y p = H 1 x e y e z e 1 (4)

22 16 där x p och y p är pixelkoordinater och x e, y e, z e är rumskoordinater definierade enligt earth coordinates Inverterad mätmodell För att kunna genomföra konverteringen av pixelkoordinater till rumskoordinater måste H i ekvation 2 inverteras vilket inte är möjligt då den är icke-kvadratisk. Genom antagandet att allt kameran ser är på golvet blir z e känd och kan H skrivas om enligt ekvation 5. h 1,1 h 1,2 h 1,3 z e + h 1,4 H = h 2,1 h 2,2 h 2,3 z e + h 2,4 (5) h 3,1 h 3,2 h 3,3 z e + h 3,4 Då kan ekvation 4 skrivas om så att rumskoordinaterna ges av följande ekvation x e y e = λ H 1 x p y p (6) Omvandling mellan earth och body-koordinater Konvertering av koordinater från earth till body-koordianter och vice versa kommer hanteras av kameramodulen. Modulen kommer att använda ekvation 1 för att göra transformeringen.

23 17 7 Associeringsmodul Associeringsmodulen är till för att lösa ett delproblem som uppstår i både positioneringen och målföljningen: att associera ny mätdata med den information som modulerna redan har. Då problemet i princip ser likadant ut för de två olika modulerna, och det finns ett flertal mer eller mindre avancerade metoder för att associera mätdata placeras funktionaliteten i denna modul. Genom att införa denna modul blir koden ej duplicerad och systemet blir mer modulärt, vilket underlättar utbytandet av modulerna i framtiden. 7.1 Övergripande design Associering av nya mätningar sker på uppdrag av målföljnings- och positioneringsmodulen. Beroende på vilken modul som anropar associeringsmodulen ska olika mätningar associeras. Då modulen anropas fås kovarianser för varje känt mål eller positionsmarkör från den anropande modulen. Modulen beräknar först gater för varje objekt (mål/positionsmarkör) utifrån dess kovarians. Därefter associeras mätningar till dessa objekt med hjälp av associeringsalgoritmen Nearest Neighbor, se sidan 111 i [1]. Utdata från modulen är en associationslista där mätningar är associerade till objekt. Denna associationslista skickas sedan till modulen som från början anropade associcationsmodulen. I figur 11 nedan visas vad modulen ska ha för inargument samt vad den ska generera för utsignal. Figur 11: Positioneringsmodulens övergripande funktionalitet Figur 12 visar en enkel skiss över hur modulen är tänkt att fungera. 7.2 Modulbeskrivning Nedan beskrivs mer ingående hur associeringsproblemet löses Funktioner Tabell 4 nedan listar de olika funktionerna som ska implementeras i associeringsmodulen. Funktionsnamn initialize calculategate associatemeasurements Beskrivning Initierar alla variabler och tillstånd. Räknar ut gate utifrån kovariansmatris. Associerar mätningar till redan kända objekt. Tabell 4: Funktioner för assoceringsmodulen.

24 18 Figur 12: Flödesschema för positioneringsmodulen Gater För att lösa associeringsproblemet måste man först beräkna ett maximalt avstånd, som ett standardmål kan förflytta sig från en tidpunkt till en annan, utifrån dess kovarians. Detta avstånd är en elliptisk kurva som brukar kallas gate. Gaten kan beräknas enligt (y k ŷ k k 1 ) T S 1 k k 1 (y k ŷ k k 1 ) < γ G (7) där vänsterledet är χ 2 -fördelat med två frihetsgrader då vi har två dimensioner i mätdatan. Tröskeln för exempelvis ett 95-procentigt intervall kan då enkelt beräknas i MATLAB genom kommandot chi2inv(0.95,2) som visar sig ge γ G = 5, Nearest Neighbor Metoden som kommer användas för att mappa nya uppmätta pixelkoordinater till respektive objekt kallas Nearest Neighbor och är en av de enklare metoderna som kan användas för att associera mätdata. Metoden bygger på att man för varje gate vill hitta det mest sannolika mätvärdet. Mätdata väljs för varje gate enligt i = argmin(yk i ŷ k k 1 ) T S 1 k k 1 (yi k ŷ k k 1 ) (8) 1 i m G k där m G k är antalet mätningar inom gaten. När en mätning har associerats till ett predikterat objekt kan det inte längre associeras till något annat. När alla mätningar har associerats returneras en lista med vilka objekt som dessa hör ihop med. De mätningar som inte ligger inom någon gate måste behandlas separat av positioneringsoch målföljningsmodulen.

25 19 8 Positioneringsmodul Positioneringsmodulens uppgift är att skatta plattformens position i banan. Detta gör den genom att använda detekterade positionsmarkörer och associera dessa med en fördefinierad karta. När associationen är gjord skattas plattformens position med hjälp av ett partikelfilter. I ett senare skede av projektet kan det även vara aktuellt att skatta åkriktning. Som ett första steg kommer denna att ses som en insignal. Detta avsnitt baseras på teorin som behandlas i kapitel 9 i Statistical Sensor Fusion av Fredrik Gustafsson [1]. 8.1 Övergripande design Positioneringsmodulen tar in pixelkoordinater för de detekterade positionsmarkörerna samt det skattade tillståndet från plattformen. Det skattade tillståndet innehåller position i z e -led, hastighet i x b, y b, z b samt gir, tilt och rollvinkel (ψ, φ, θ). Utifrån dessa insignaler skattar modulen en position för plattformen och skickar positionen i x e och y e koordinater som utsignal. Dessa positioner levereras med respektive kovariansmatris. Figur 13: Positioneringsmodulens övergripande funktionalitet I Figur 14 visas ett flödesdiagram över modulens olika delmoment som den använder sig av för att skatta positionen. När modulen startar upp initieras den med en del förinställda värden så som kartlayout (positionsmarkörernas positioner), startpositionen för plattformen etc. Den skapar också partiklar utifrån vad som har definierats i GUI:t. Efter initiering väntar den på att få nya mätvärden från huvudbussen. Då nya mätvärden är tillgängliga skickas alla partiklar till kameramodulen för att skatta respektive positionsmarkörs position i pixelplanet. Tillbaka ifrån kameramodulen fås en uppsättning med partiklar för varje positionsmarkör. I positioneringsmodulen beräknas en kovarians för varje partikeluppsättning. Partiklarnas fördelning approximeras här som gaussisk. Dessa kovarianser skickas sedan till associationsmodulen som associerar mätningar med positionsmarkörer. Tillbaka fås en associationslista med information om vilket landmärke som varje mätning är associerad med. Nästa steg är att köra en mätuppdatering där alla partiklars vikter uppdateras och normeras. Därefter beräknas ett viktat medelvärde av positionen för de partiklar som ligger i rummet. Resultatet skickas vidare till huvudbussen som den skattade positionen i x e - och y e -led. I återsamplingssteget så omsamplas partiklarna med återläggning proportionerligt mot dess vikter. Efter återsamplingen sätts alla partiklars vikter lika. Det är viktigt att

26 20 inte återsampla för ofta eftersom information förloras i varje återsamplingssteg. I tidsuppdateringen uppdateras partiklarnas tillstånd. Nya tillstånd uppdateras med en sannolikhetsfunktion som baseras på det tidigare tillståndet, se sidan 228 i [1]. Nya vikter sätts till deras tidigare värde. Därefter är postioneringen klar och proceduren upprepas. Figur 14: Flödesschema för positioneringsmodulen 8.2 Modulbeskrivning Nedan beskrivs mer detaljerat hur positioneringsmodulen skattar positionen Funktioner Tabell 5 nedan listar de olika funktionerna som ska implementeras i positioneringsmodulen Associera positionsmarkörer Associering av mätningar sker genom att köra igenom alla kombinationer av positionsmarkörer och partiklar igenom mätekvationen. Detta görs i kameramodulen. Tillbaka får positionsmodulen alla partiklar för respektive positionsmarkör i pixelplanet. Nästa steg är att räkna ut kovariansen för en positionsmarkörs position i pixelplanet. Denna skickas sedan till associationsmodulen som associerar mätningar med positionsmarkörer. Tillbaka kommer en lista med associationer.

27 21 Funktionsnamn Beskrivning initialize Initierar alla variabler och tillstånd. Definierar kartan. createparticles Skapar partikelobjekten. propogateparticles Propagerar partiklarna genom mätekvationen så att de hamnar i pixelplanet genom att anropa kameramodulen. associate Anropar associeringsmodulen för att associera mätningar. measurementupdate Uppdaterar vikterna hos partiklarna givet hur sannolika de är. estimate Estimerar plattformens position och konfidensintervall. checkneedforresample Kollar om partiklarna behöver återsamplas. resample Återsamplar partiklarna. timeupdate Tidsuppdaterar partiklarna. calculatecovariance Räknar ut kovariansen för en mängd partiklar. Tabell 5: Funktioner för positioneringsmodulen Sensorfusion med partikelfilter För att skatta positionen hos plattformen givet associerade mätningar av landmärken används ett partikelfilter. Partikelfiltret har ett antal parametrar som bestäms av operatören, dessa ska vara input till modulen. Dessa är antalet partiklar N, startposition för plattformen, kovarians för startpositionen, kovarians för mätbrus i pixelplanet samt kovarians för processbrus. Mätbruset och processbruset anses vara gaussiskt med väntevärde noll. Partikelfiltret kan delas upp i mindre delar, dessa är: initiering, mätuppdatering, estimering, återsampling och tidsuppdatering. Partikelfiltret initieras genom att skapa det antalet partiklar som ska användas i filtret. Tillstånden hos partiklarna fördelas utifrån startpositionens kovarians. Tillståndet i en partikel beskriver positionen för denna samt en vikt som beskriver sannolikheten hos partikeln. Mätuppdateringen viktar partiklarna utgående från hur sannolik mätdatan är givet partikelns tillstånd. Detta görs genom att använda de i associationen framräknade partiklarna i pixelplanet. För varje partikel skapas en gaussisk fördelning. Utifrån denna räknas sannolikheten ut för att få den till positionsmarkören associerade mätningen. Alla mätningar ses som oberoende vilket innebär att sannolikheterna för varje partikel kan multipliceras ihop för att få den totala sannolikheten att få mätuppsättningen givet partikelns tillstånd. Ekvation 9 beskriver detta steg. Vikterna hos partiklarna normeras sedan så att summan av alla vikter blir ett. wk k i = 1 wk k 1 i c p(y k x i k) k N c k = wk k 1 i p(y k x i k) i=1 (9a) (9b) Efter mätuppdateringen estimeras en position för plattformen genom att beräkna ett viktat medelvärde över alla partiklar, samt ett konfidensintervall genom att beräkna kovariansen hos partiklarna.

28 22 Nästa steg är att återsampla partiklarna. Återsampling ska endast ske ifall det effektiva antalet partiklar understiger 2/3 av det totala antalet partiklar. Effektiva antalet partiklar räknas ut enligt ekvation 10. Totalt ska N partiklar plockas varvid en partikel kan plockas flera gånger. Det innebär att det efter återsamplingssteget kan finnas två partiklar med samma tillstånd. Sannolikheten att plocka en partikel är dess vikt. Då en partikel plockas sätts dess vikt till 1/N. ˆN eff = 1 i (wi k k )2 (10) Det sista steget är tidsuppdateringen. I detta steg flyttas tiden fram ett tidssteg och partiklarnas tillstånd uppdateras. Uppdateringen sker utifrån en så kallad proposal distribution vilket på svenska kallas förslagsdistribution. Den valda proposal distribution är prior proposal vilket innebär att det föreslagna tillståndet endast beror på tidigare tillstånd. Efter tidsuppdateringen kommer två partiklar, vars tillstånd var likadana innan tidsuppdateringen att ha olika tillstånd. Detta är viktigt för att filtret ska kunna konvergera mot det rätta värdet. Då tidsuppdateringen är klar väntar programmet tills dess att nya mätningar har fåtts.

29 23 9 Målföljningsmodul Målföljningsmodulen ansvarar för att analysera och behandla data gällande mål som skickas från bildbehandlingsmodulen. Modulen ska även spara data om målens rörelsebanor, hastighet och riktning, för att senare kunna skicka dessa data till GUI:t, samt på kommando kunna slås av och på. 9.1 Övergripande design I figur 15 visas informationsflödet in och ut ur modulen. Ingående data är de variabler som modulen behöver för att kunna uppfylla kravställd funktionalitet. Ut skickas estimerad data om de detekterade målen. Figur 15: Målföljningsmodulens övergripande funktionalitet I figur 16 visas de huvudsakliga delmomenten i modulen, samt hur de hänger ihop.

30 24 Figur 16: Målföljningsmodulens övergripande funktionalitet 9.2 Modulbeskrivning Målföljningsmodulen utgör en klass bestående av två listor; den ena hanterar bekräftade spår, och den andra så kallade initiatorer, det vill säga spår som ännu inte har bekräftas. Denna klass tillhandahåller funktionen som adderar och raderar element i listorna, samt funktionen för associering. Vardera element i listorna utgör ett objekt av en klass som specificerar egenskaperna för ett spår och tillhandahåller funktioner för linjärisering, kovariansberäkning, samt tids- och mätuppdatering. Inledningsvis vid varje samplingstid T s utför varje spår en tidsuppdatering enligt en Constant Velocity rörelsemodell (CV). Dessa projicerade koordinater översätts till pixelkoordinater genom att varje spår anropar kameramodulen (avsnitt 6) via huvudbussen som utför transformationen med dess mätfunktion. Tidsuppdateringens kovarians kommer beräknas med den linjäriserade mätfunktionen, vilket förklaras utförligare längre ned. Det tidsuppdaterade tillståndet och dess kovarians skickas senare till associationsmodulen (avsnitt 7) tillsammans med nya mätningar för att avgöra vilken mätning som hör till vilket spår. När ett track fått en ny mätning enligt ovan angivna logik sker en mätuppdatering enligt ett Extended Kalman Filter (EKF) som då estimera tillståndet för tracket i fråga, under antagandet att målets endast kan befinna sig på marken Funktioner De funktioner som ska implementeras i målföljningsmodulen finns beskrivna i tabell 6.

31 25 Funktionsnamn Beskrivning initialize Initierar målklassen, lista för bekräftade mål och initiatorer samt klassen mål. associate Skapar gates och associerar mätningar till mål eller initiatorer samt utföra Nearest Neighbor-algoritmen. timeupdate Skapar en tänkt postion för målet samt dess kovarians measurementupdate Uppdaterar tillståndsskattningen newinitiator Skapar en ny initiatorobjekt och initierar dess startpostion. deleteinitiator Tar bort ett mål från listan över initiatorer och dess spår. newtarget Lägger till ett spår i listan över bekräftade mål. deletetarget Tar bort ett mål från listan över bekräftade mål. Target Skapar ett nytt målobjekt. Tabell 6: Funktioner i målföljningsmodulen Målet och dess rörelsemodell Målets tillstånd utgörs av position i x e - och y e -led samt hastigheterna i båda dessa led. Hastigheterna kommer antas vara konstanta och därför kommer en tidsdiskret constant velocity model (CV) att användas. Eftersom rörelsen hos målet är linjärt beskrivs tillståndet linjärt med 11a medan 11b som transformerar målets rumskoordinater till pixelkoordinater är en olinjär funktion. x k+1 = F k x k + G v,k v k, Cov(v k ) = Q k (11a) y k = h(x k ) + e k, Cov(e k ) = R k (11b) Där tillståndsvektorn x ges av: x e x = y e x e (12) y e F k och G v,k ges, med T s som samplingsperiod, enligt CV-modellen av: T 1 0 T s 0 2 s F k = T s , G T v,k = 0 2 s 2 T s T s Modellbrusets kovariansmatris Q k och mätbrusets kovarians R k antas vara: ( ) ( ) Q k = σq 2 1 0, R 0 1 k = σr (13) (14) h(x k ) är mätekvationen som ges i (2).

32 Tidsuppdatering Utifrån modell ovan sker tidsuppdateringen enligt: x k+1 k = F k x k k P k+1 k = F k P k k F T k där x k, F k, P k, G v,k och Q k är angivet ovan. + G v,k Q k G T v,k (15) Linjärisering av mätfunktion Linjäriseringen kommer ske med en första ordningens taylorutveckling. Målmodulen anropar kameramodulen via huvudbussen för att komma åt (5). Eftersom det antas att målet är på backen har z e satts till ett visst värde som multiplicerats in i mätekvationen. Det betyder det endast är dimensionerna x och y kvar i tillståndsvektorn, vilket är samma dimensioner som tillståndet i pixelkoordinater. Från den sista raden i ekvation 5 fås då ett uttryck för λ och för att behålla ettan på nedersta raden i tillståndsvektorn i pixelkoordinater kommer uttrycken för x p och y p bli enligt ekvation 16. x p = ht 1 x h T 3 x y p = ht 2 x h T 3 x (16) Där h T i är respektive rad i ekvation 5 och x är enligt ekvation 17. x e x = y e (17) 1 Ekvation 16 (som hädanefter kallas för h) kommer då vara den mätfunktion som ska linjäriseras. Genom att derivera mätfunktionen numeriskt, fås Jacobianen, h (ˆx k ). Den linjäriserade mätekvationen blir då för respektive tillstånd ˆx k enligt en första ordningens Taylorutveckling, se ekvation 18. H k = h(ˆx k ) + h (ˆx k )(x k ˆx k ) (18) Associationsproblemet För att associationsmodulen ska kunna skapa gater behövs innovationskovariansen, S k, som ges av ekvation 19. S k = R k + h (ˆx k k 1 )P k k 1 (h (ˆx k k 1 )) T (19) Associationen sker först för de bekräftade målen och de överblivande mätningarna prövas senare på samma vis för initiatorerna. De mätningar som inte heller fångades upp av initiatorerna blir nya initiatorer som adderas till listan över initiatorer. För initiatorer ska bli bekräftade mål krävs att en N/M-logik uppfylls. Detta betyder att om en initiator har N mätningar i rad från den första detektionen samt efter det har N mätningar av de

33 27 M efterföljande iterationerna kommer initiatorn uppgraderas till ett bekräftat mål, dvs spåret i fråga raderas från initiatorlistan och adderas på listan över bekräftade mål. Om logiken inte uppfylls kommer initiatorn att raderas Skatta målets tillstånd För detektioner som utgör en ny mätning av ett existerande mål kommer en mätuppdateringen ske med ett EKF enligt: S k = R k + h (ˆx k k 1 )P k k 1 (h (ˆx k k 1 )) T K k = P k k 1 (h (ˆx k k 1 )) T S 1 k ɛ k = y k h(ˆx k k 1 ) (20) ˆx k k = ˆx k k 1 + K k ɛ k P k k = P k k 1 P k k 1 (h (ˆx k k 1 )) T S 1 k h (ˆx k k 1 )P k k 1 Den första raden som visar beräkningen av S k är redan gjord i linjäriseringen och kommer då inte göras igen utan den visas bara här för att läsaren ska kunna följa mätuppdateringen. Allt eftersom målens tillstånd skattats skickas dessa ut tillsammans med deras kovariansmatris.

34 28 10 Planeringsmodul Nedan hittas en utförlig beskrivning av systemets planeringsmodul. Modulen har till uppgift att planera och leverera styrkommandon åt plattformen Övergripande design Modulen är precis som övriga moduler tänkt att implementeras i form av en egen klass som körs på en separat tråd. Figur 17 nedan visar vilka dataflöden som går in och ut ur modulen. Figur 17: Planeringsmodulens övergripande funktionalitet Figur 18 visar modulens interna struktur. Efter initieringen fortsätter programmet på en av de tre looparna beroende på vilken styrmod som är aktiv.

35 29 Figur 18: Planeringsmodulens flödesschema 10.2 Modulbeskrivning Planeringsmodulen skickar styrkommandon till plattformen via huvudbussen och kommunikationsmodulen. Planeringsmodulen kan befinna sig i tre olika styrmoder: manuell styrning, avsökning av banan och följa efter ett av operatören givet mål. I den manuella styrningen tar planeringsmodulen in styrkommandon från GUI:t. Den manuella styrningen kommer att baserars på den funktionalitet som togs fram i fjolårets projektet. I avsökningen av banan kommer planeringsmodulen autonomt att planera en rutt genom att beräkna ett antal positioner, delmål, som förväntas täcka den rektangulära banan. Detta kan till exempel vara ett delmål i närheten av respektive hörn av banan, samt en punkt i mitten. Plattformen ska sedan åka mellan dessa punkter tills annat uppdrag ges. Funktionalitet för att följa efter ett givet mål kommer att baseras på att hålla målet i mitten av bilden. Dessutom ska det vara möjligt att växla mellan de olika styrmoderna. Om positionsskattningen antingen är för dålig eller hamnar utanför banan ska plattformen avbryta sitt uppdrag och landa omedelbart Manuell mod I manuellt läge kommer planeringsmodulen ta emot styrkommandon från GUI:t för att sedan skicka vidare dessa till plattformen via huvudbussen, förutsatt att plattformen fortfarande befinner sig i banan Avsökning av bana Vid avsökning av banan planeras en rutt utifrån en given karta. När positioneringsmodulen har nått en viss säkerhet på plattformens position kommer plattformen börja följa rutten från den punkt på planerad rutt närmast nuvarande position. Denna punkt kommer

36 30 specificeras som startpunkten på rutten. Plattformen fortsätter med avsökningen tills den får ett nytt kommando. Planeringsmodulen får ständigt ny information om plattformens position vilket den matchar med punkter på rutten. Med hjälp av en tvådimensionell P-regulator skickar modulen styrkommandon till plattformen som ska föra denna vidare till nästa punkt på rutten Målföljning Då användaren angett att målföljning ska utföras kommer planeringen försöka positionera plattformen rakt ovanför målet. Genom att jämföra plattformens och målets position kommer en tvådimensionell P-regulator implementeras för att styra in plattformen i X- och Y-led. Målets skattade hastighet kan även användas som I-del för att förbättra målföljningen Funktioner Tabell 7 nedan listar de olika funktionerna som ska implementeras i planeringsmodulen. Funktionsnamn initialize currentmode getcontrolcommand sendcontrolcommand getcurrentposition gettargets goalreached calculatecheckpoints setcurrentcheckpoint getcurrentcheckpoint calculateroute Beskrivning Initierar alla variabler. Returnerar vilken mod som är aktiv. Hämtar styrkommandon. Skickar styrkommandon. Hämtar plattformens nuvarande position. Hämtar målens nuvarande positioner. Returnerar om målet är nått eller inte. Beräknar ett antal delmål som förväntas täcka området. Väljer nästa delmål. Returnerar det nuvarande delmålet. Beräknar vilket håll plattformen bör åka för att nå sitt delmål. Tabell 7: Funktioner i planeringsmodulen

37 31 11 Bildbehandlingsmodul Bildbehandlingsmodulen behövs för att kunna plocka ut vital information från videoströmmen. Denna består av detektion av olika objekt på specifika pixelkoordinater som behövs för att plattformen ska kunna positionera sig och följa mål Övergripande design Figur 19 visar in- och utdata för modulen. Figur 19: Bildbehandlingmodulens övergripande funktionalitet 11.2 Modulbeskrivning Denna modul behandlar och analyserar data från plattformens videokamera. Uppgifterna inkluderar detektering av standardmål, positionsmarkörer samt att på kommando returnera pixelkoordinater för detekterade objekt i den senaste bilden. Främst kommer lösningar från förra året användas och anpassas till de nya problemen, men det kommer även tillkomma en del ny funktionalitet. Bildbehandlingen kommer även fortsättningsvis att ske med hjälp av OpenCV. Modulen ska kunna upptäcka mål med olika form och färg. Även multipla mål i en bild ska kunna upptäckas. En viktig aspekt som tillkommer är positionsmarkörer som bildbehandlingsmodulen måste kunna särskilja från andra objekt. En del brus kommer att filtreras bort redan i bildbehandlingsmodulen. Men även målföljningsmodulen samt positioneringsmodulen kommer ha till uppgift att särskilja falsklarm från faktiska objekt Terminologi De begrepp som används i detta kapitel beskrivs i listan nedan. Morfning används för att reducera brus i bilderna och till att förenkla former på detekterade objekt. Detta görs med hjälp av dilation och erosion. Erosion innebär att pixlar i utkanten av objekt tas bort vilket får objektet att minska i storlek.

38 32 Dilation innebär att pixlar läggs till i utkanten av objekt vilket får objektet att öka i storlek Tröskelvärde (engelska: thresholding) används för att segmentera en bild och innebär att alla pixlar med en intensitetsnivå över ett visst värde blir vita och alla under svarta Detektion av objekt För att plattformen ska kunna positionera sig och följa mål måste bildbehandlingsmodulen kunna detektera mål och positionsmarkörer. I detta kapitel beskrivs därför hur detta kommer att gå till. Figur 20 beskriver hur bildbehandlingsmodulen arbetar. De olika stegen i flödesschemat beskrivs närmre under kommande rubriker. Figur 20: Flödesschema för bildbehandlingsmodulen Måldetektion i en bild Eventuella mål i en bild kan detekteras med hjälp av olika metoder. Det alla metoder har gemensamt är att de tar vara på olika attribut som skiljer objektet från bakgrunden Förbehandling Genom att förbehandla bilderna innan objekt ska identifieras i dem minskar risken för falsklarm. Denna förbehandling utgörs av de morfologiska operationerna erosion och dilation. Först utförs erosion på bilderna vilket innebär att brus reduceras i bilden. Efter erosionen utförs dilation varvid eventuella objekt i bilden återfår sin storlek. Även konturerna av objekten blir tydligare efter dilationen.