Teknisk dokumentation. Autonom målföljning med quadcopter

Transkript

1 Teknisk Dokumentation Version 1.0 Robo Ptarmigan 14 december 2015 Status Granskad KL,AF,CC,AB 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 KL,AF,CC,AB Andra utkastet Robo Ptarmigan GN Första utkastet Robo Ptarmigan KL,CC,AF,AB

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 Resultat Kommunikationsmodul Övergripande design Modulbeskrivning Terminologi Gränssnitt Telemetri Videoström AT-kommandon Initiering av kommunikationslänk Räckvidd Resultat GUI Moduldesign Program och språk Layout för GUI:t Huvudfliken Funktioner Bildbehandlingsfliken Funktioner MapView Funktioner Resultat

5 6 Kameramodul Övergripande design Modulbeskrivning Funktioner Mätmodell Inverterad mätmodell Omvandling mellan rumskoordinater och kroppskoordinater Resultat Associeringsmodul Övergripande design Modulbeskrivning Gater Nearest Neighbor Funktioner Resultat Positioneringsmodul Övergripande design Modulbeskrivning Funktioner Associera positionsmarkörer Sensorfusion med partikelfilter Resultat 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 Resultat Planeringsmodul Övergripande design Modulbeskrivning Manuell mod Avsökning av bana Målföljning Regulator Funktioner Resultat Bildbehandlingsmodul Övergripande design

6 11.2 Modulbeskrivning Terminologi Detektion av objekt Måldetektion i en bild Förbehandling Detektion av positionsmarkörer genom formdetektion Detektion av mål genom färgdetektion Resultat Lagringsmodul Övergripande design Modulbeskrivning Skapa och spara filen Skriva data till filen Initiera från fil Resultat Resultat 41

7 1 1 Inledning Detta dokument utgör den tekniska dokumentationen för CDIO-projektet Autonom målföljning med quadcopter. Projektet är en del i kursen - Reglerteknisk projektkurs som ges vid Linköpings universitet. Den tekniska dokumentationen syftar till att i detalj förklara hur produkten är designad samt dess tekniska detaljer. 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. 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

8 2 ä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 användes dock inte 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 15x15 cm. Utseendet på en positionsmarkör beror på om den ligger på en kant, eller i mitten av en bana. Bana: Ett rektangulärt område markerat med positionsmarkörer i ett rutnät med 1 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 Produkten som består av quadcoptern samt den utvecklade programvaran klarar av att styras manuellt, autonomt avsöka banan, följa efter enskilda mål samt utföra målföljning på multipla mål. Produkten har delats upp i olika moduler eller delsystem. Tanken är att varje modul ska kunna utvecklas och testas enskilt samt fungera separat. Det gör det enkelt att vidareutveckla produkten genom att ersätta eller lägga till nya moduler. 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 Fyra olika koordinatsystem används i projektet. Koordinatsystemen är definierade enligt följande. GUI-koordinater: Detta koordinatsystem används i GUI:t och har origo definierat som hörnet högst upp till vänster med x-axeln pekandes åt höger samt y-axeln pekandes nedåt. Enheten för denna är pixlar. Kroppskoordinater: Detta är plattformens lokala koordinatsystem där origo är i kameran, x-axeln pekar framåt, y-axeln pekar åt höger samt z-axeln pekar nedåt. Enheten för denna är meter. Rumskoordinater: Detta koordinatsystem är definierat utifrån banan. Origo är definierat som hörnet längst ner till vänster, x-axeln är definierat åt höger i banan, y-axeln är definierat som framåt/bortåt i banan och z-axeln är definierad som uppåt. Enheten för denna är meter. Bildkoordinater: Detta koordinatsystem är definierat utifrån bilderna som tas av plattformen. Origo är definierat som hörnet längst ned till vänster, x-axeln pekar åt höger och y-axeln pekar uppåt. z-axeln är inte relevant för detta koordinatsystem istället är denna definierat som noll hela tiden. Enheten för denna är pixlar.

10 4 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: Figur 2: Plattformens koordinater i de olika systemen Rollvinkel: Rotationen runt y 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 e -axeln. Translationen från rumskoordinater till kroppskoordinater ges av plattformens position. Rotationsmatrisen från rumskoordinater till kroppskoordinater som ges av cos θ cos ψ cos θ sin ψ sin θ R RK = 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 kroppskoordinater till rumskoordinater ges av inversen av (1) som, eftersom matrisen är ortogonal, blir R KR = R RK T. (2) 2.2 Styrmoder Programmet innehåller tre stycken 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.

11 Målföljning I denna styrmod avsöks en fördefinierad bana av plattformen. Planeringsmodulen planerar en rutt som plattformen ska åka och ser sedan till att denna följs genom reglering. Datorprogrammet detekterar mål och visar upp dessa via GUI:t. Målen kan vara både stillastående och rörliga Följning av mål I denna styrmod följer plattformen 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 visas också 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 WiFi. 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. Kameramodul: Denna modul sköter konverteringen mellan bildkoordinater och rumskoordinater. Detta är en så kallad bimodul som ej kommunicerar via huvudbussen utan tas istället in som ett objekt i de moduler där denna används. Associeringsmodul: Detta är en bimodul som sköter associering av mätningar till redan kända positionsmarkörer eller mål. Positioneringsmodul: Denna modul har till uppgift att utifrån observerade positionsmarkörer och sensordata skatta plattformens position. Målföljningsmodul: Denna modul håller reda på de detekterade målen samt skattar 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 bildkoordinater. 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 utom kameramodulen och mottager data från alla utom lagringsmodulen och kameramodulen. 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 synchronized-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

13 7 kan köras åt gången. Därför är det viktigt att tänka på att endast använda skyddet för 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 innehåller huvudbussen en gemensam variabel för varje variabel som används i mer än en modul, samt en funktion för att läsa och skriva varje sådan variabel. Ett exempel på hur en sådan funktion i huvudmodulen är skriven visas nedan. public synchronized Position getposition() { return this.platformposition; } 3.5 Resultat Huvudbussen uppfyller dess krav som finns ställda på den i kravspecfikationen [3] för projektet. Modulen är baserad på fjolårets projekt och har endast anpassats till årets. Tanken var från början att modulen istället för att spara gemensamma variabler endast skulle kalla på funktioner i andra moduler som skickar kopior av variablerna, men modulstrukturen från föregående år tillät inte detta på något enkelt sätt och därför valdes denna lösning istället.

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 sker över det trådlösa nätverk (Wi-Fi) som plattformen tillhandahåller. Denna funktionalitet fanns redan från föregående års projekt. Det som gjordes var att kontrollera att allting fungerade 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 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. 4.7 Resultat Kommunikationsmodulen uppfyller kraven som finns ställda på den i kravspecfikationen [3] för projektet. Modulen är utvecklad av 2014 års projekt.

18 12 5 GUI Detta avsnitt beskriver implementationen av GUI:t. Modulen sköter gränssnittet mot operatören. Gränssnittet består huvudsakligen av ett fönster där information om plattformens tillstånd visas. Användaren kan genom detta fönster interagera med plattformen och till exempel ansluta till plattformen, aktivera andra moduler samt välja vilken styrmod som ska vara aktiv. Genom tangentbordskommandon styrs plattformens rörelse i det manuella läget. 5.1 Moduldesign Projektgruppen valde att bygga vidare på föregående års GUI, som är baserat på ett fliksystem. Mycket av funktionaliteten som skulle implementeras fanns redan. För att spara tid återanvändes stora delar, såsom uppbyggnaden av klasstrukturer, samt grunden till fliken med bildbehandling. Det finns två flikar: en huvudflik som innehåller allt som användaren behöver för att interagera med plattformen, samt en flik med lite mer avancerade funktioner för bildbehandling. Den senare har främst använts i utvecklingssyfte av bildbehandlingsmodulen, men det kan tänkas att fliken kan behövas för att exempelvis justera trösklar för detektion av mål i olika ljusförhållanden. I figur 7 visas hur klasstrukturen för modulen är uppbyggd. JavaFX är det grafikbibliotek som används. Från klassen GUI Worker initieras Base controller, som innehåller ett objekt för respektive flik. Flikarna kallar i sin tur på MapView-objektet som ritar ut saker i GUI:t. Figur 7: GUI-modulens klasstruktur. 5.2 Program och språk För att skapa ett grafiskt gränssnitt behövs ett grafikbibliotek. Det grafikbibliotek som använts är Java FX då detta används i föregående års projekt. Grafikbiblioteket

19 13 kombinerat med tilläget Screen Builder gör det möjligt att bygga GUI:t med standardkomponenter som flikar, knappar och dialogrutor. 5.3 Layout för GUI:t GUI:t är som tidigare nämnt uppdelat i två flikar med lite olika funktionalitet. Nedan beskrivs mer ingående vad respektive flik innehåller och har för syfte Huvudfliken I denna flik visas allt som en användare kan tänkas behöva för att använda systemet. Figur 8 visar hur fliken ser ut. Det finns bland annat en karta i övre vänstra hörnet där alla positionsmarkörer och plattformen är utritade. Plattformens synfält visas också upp med rektangeln runt plattformen. Vid körning ritas associationer och mål upp. Detta gäller även styrsignaler vid autonom styrning. Det finns även knappar med diverse funktionalitet som beskrivs närmare i användarmanualen [8]. Under knapparna visas videoströmmen med detektioner från bildbahandlingen. Under denna visas batteristatus och Wi-Fi-status. Till sist visas sensorvärden samt reglerparametrar under kartan. Figur 8: Huvudflikens utseende. Funktionsknappar, karta över avsökningsområdet, kameravy samt plattformsdata ingår Funktioner Tabell 3 listar de olika funktioner som har implementerats i klassen för huvudfliken.

20 14 Funktionsnamn Beskrivning loadmap Hämtar information om kartan från huvudbussen. mapcontainerclick Hanterar musklick på kartan. Markerar targets. init Ser till att GUI:t endast laddas en gång. loadmaingui Initierar huvudflikens knappar, mapview-objekt och videoström. updatecolortemplates Uppdaterar vilka mallar som används för detektion av mål. drawmap Hämtar all nödvändig data och skickar vidare till mapview-objektet som sedan ritar ut det. updatemovie Uppdaterar videoströmmen. updatewifistatus Returnerar en sträng med nuvarande statusen på Wi-Fi-uppkopplingen. updatebatterystatus Returnerar en sträng med nuvarande batteristatusen. getparent Returnerar en pekare till BaseController-objektet. setparent Sätter pekaren till BaseController-objektet. update Uppdaterar modvalsknappen. Tabell 3: Funktioner i huvudfliken Bildbehandlingsfliken Bildbehandlingsfliken innehåller främst funktionalitet för att finjustera parametrar i bildbehandlingsmodulen i realtid. Från denna flik finns möjlighet att starta och stoppa bildbehandlingen, samt att välja om kontur- respektive färgdetektion ska vara aktiva. Det finns två videoströmmar: en där den råa bildströmmen visas, och en som visar den trösklade bilden. Figur 9: Bildbehandlingsflikens utseende. Knappar för start och stopp av bildbehandlingsmodulen, samt konfigurationer för denna ingår Funktioner Tabell 4 listar de olika funktioner som implementerats i klassen för bildbehandlingsfliken.

21 15 Funktionsnamn initializemodebuttons loadipgui updatecolortemplates updateimage getparent setparent Beskrivning Initierar knappar för bildbehandlingsalternativ Initierar knappar för start, stopp samt alternativ för bildbehandling. Uppdaterar vilka mallar som används för detektion av mål. Uppdaterar videoströmmen. Returnerar en pekare till BaseController-objektet. Sätter pekaren till BaseController-objektet. Tabell 4: Funktioner i bildbehandlingsfliken. 5.4 MapView Denna klass ansvarar för att rita ut alla element i GUI:t, såsom kartan med positionsmarkörer, mål och dylikt. Klassen är en underklass till flikarna, och tar i sin konstruktor in pekare till respektive panel där allt ska ritas ut Funktioner Tabell 5 listar de olika funktioner som implementerats i MapView-klassen. Funktionsnamn Beskrivning clear Rensar fliken från alla utritningar. drawmap Ritar ut allt som ska ritas i GUI:t. drawmark Ritar ut att ett mål har markerats. drawtriangle Ritar ut en kantmarkör på kartan. drawsquare Ritar ut en inre positionsmarkör på kartan. drawquad Ritar ut quadkoptermarkören på kartan. drawquaddata Visar plattformsdata i GUI:t. truncate Trunkerar antal värdesiffror. drawtarget Ritar ut mål på kartan. drawvelocity Ritar ut måls hastighet. drawconf Ritar ut konfidensintervall. drawparticle Ritar ut partiklar på kartan. drawcameraview Ritar ut plattformens synfält i kartan, givet dess nuvarande position och orientering i rummet. drawasslandmark Ritar ut var positioneringsmodulen tror att landmärken befinner sig. drawgoal Ritar ut den nuvarande målpositionen för avsökning. drawtrajectory Ritar ut plattformens trajektoria på kartan. earthtogui Konverterar rumskoordinater till GUI-koordinater. earthtoguivelocity Konverterar hastigheter från rumskoordinater till GUI-koordinater. drawcontrollersignals Ritar ut reglerstorheter på kartan. Tabell 5: Funktioner i MapView-klassen

22 Resultat GUI:t uppfyller alla krav som finns ställda på det i kravspecfikationen [3] för projektet. Modulen är baserad på fjolårets projekt även om stora delar av koden har modifierats eller gjorts om från början.

23 17 6 Kameramodul Kameramodulen ansvarar för omvandlingen mellan bildkoordinater och rumskoordinater. Dess funktionalitet används av GUI, målföljningsmodulen och positioneringsmoduelen. Kameramodulen är inte implementerad som körbar vilket innebär att den inte körs i en egen tråd. 6.1 Övergripande design I figur 10 visas vilka insignaler och utsignaler som modulen har. Omvandlingen mellan koordinaterna kräver en mätekvation. Denna ekvation beror på kamerans egenskaper samt plattformens orientering i rummet. Kameramodulen klarar av att omvandla rumskoordinater till bildkoordinater samt bildkoordinater till rumskoordinater. Figur 10: Kameramodulens övergripande funktionalitet I figur 11 visas ett flödesschema över modulens olika delmoment. Figur 11: Flödesschema för kameramodulen 6.2 Modulbeskrivning Nedan beskrivs mer i detalj hur kameramodulen genomför omvandlingen mellan rumsoch bildkoordinater.

24 Funktioner I tabell 6 listas de olika funktioner som finns implementerade i kameramodulen. Funktionsnamn Beskrivning sethmatrix Väljer värde på H matrisen utifrån plattformens attityd. convertearthtopixel Target objektets bildkoordinater blir dess konverterade rumskoordinater. propogateparticles Beräknar bildkoordinater för varje positionsmarkör för varje partikels position av plattformen. convertpixeltoearth Omvandlar bildkoordinater till rumskoordinater. jacobian Returnerar Jacobianen av mätfunktionen. Tabell 6: Funktioner för kameramodulen Mätmodell Detektioner från bildbehandlingsmodulen ges som bildkoordinater i en bild. Det är därför viktigt att kunna beräkna motsvarande position i x e y e -planet. Detta sker med mätfunktionen, enligt [9], h 1,1 h 1,2 h 1,3 H = h 2,1 h 2,2 h 2,3 (3) h 3,1 h 3,2 h 3,3 där alla parametrar är okända. Denna ekvation innehåller kameraparametrarna samt en rotationsmatris. Det antas att allt som kameran ser har höjden noll vilket innebär att observerade objekt ligger på golvet. 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 orientering. Uppdelningen kan skrivas som där f x 0 p x r 11 r 12 r 13 H = 0 f y p y r 21 r 22 r 23 (4) r 31 r 32 r 33 r 11 = cos (φ + π) cos (ψ + π) r 12 = cos (φ + π) sin (ψ + π) r 13 = sin (φ + π) r 21 = sin ( θ) sin (φ + π) cos (ψ + π) + cos ( θ) sin (ψ + π) r 22 = sin ( θ) sin (φ + π) cos (ψ + π) + cos ( θ) cos (ψ + π) r 23 = sin ( θ) cos (φ + π) r 31 = cos ( θ) sin (φ + π) cos (ψ + π) + sin ( θ) sin(ψ + π) r 32 = cos ( θ) sin (φ + π) cos (ψ + π) + sin ( θ) cos (ψ + π) r 33 = cos (φ + π) cos ( θ) (5) och där φ är roll-vinkeln, θ är tipp-vinkeln och ψ är gir-vinkeln [9]. Vinkeln π och minus tipp-vinkel är för att få de olika koordinatsystemen att initialt ska stämma överens.

25 19 Givetvis går det lätt att förenkla ekvationerna ovan. Detta gjordes och verifierades i MATLAB men av någon anledning fungerade den förenklade matrisen inte i programmet. I ekvationen ovan representerar f x och f y fokallängden och p x och p y principal-punkterna. r ij är elementen i rotationsmatrisen mellan de olika planen som beror på plattformens attityd. För att beräkna kameraparametrarna användes Camera Calibration Toolbox [6]. Matrisen H beskriver hur punkter i rumskoordianter kan uttryckas i bildkoordinater, i vilken mätningarna från bildbehandlingsmodulen ges. Detta kan skrivas som λ x p y p = H x e x c y e y c (6) 1 0 z c där x p och y p är objektets bildkoordinater och x e, y e är objektets rumskoordinater och x c, y c, z c är kamerans rumskoordinater. I och med att kamerapositionen subtraheras i (6) sker också en translation Inverterad mätmodell För att genomföra konverteringen av bildkoordinater till rumskoordinater måste H i (3) inverteras. Antagandet att alla objekt befinner sig på marken gör att ekvationen innehåller okända positionen i rumskoordinater (x e,y e ) och en okänd normeringskonstant λ som därmed kan beräknas. Då kan (6) skrivas om så att objektet ges av x e y e = λh 1 x p y p + x c y c. (7) 0 1 z c Omvandling mellan rumskoordinater och kroppskoordinater Konvertering av koordinater från rumskoordinater till kroppskoordinater och vice versa kommer hanteras av kameramodulen. Modulen kommer att använda (1) för att göra transformeringen. 6.3 Resultat Det finns inga direkta krav på modulen i kravspecifikationen [3], istället måste den fungera för att andra krav ska vara uppfyllda. Kameramodulen uppfyller de uppgifter den har ålagts. Prestandan i kanterna av bilden skulle kunna förbättras. Idag har detta lösts genom att bildbehandlingen klipper bort kanterna i bilden. En rimlig vidareutveckling vore att undersöka om det går att ta fram en bättre kameramodell för att öka prestandan ytterligare.

26 20 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. Detta har lösts genom att skapa en ny klass som både positionerings- och målföljningsmodulen använder sig av. Associeringsmodulen är inte implementerad som körbar vilket innebär att den inte körs i en egen tråd. 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å associering anropas skickas både mätningar och redan kända objekt med till funktionen. De redan kända objekten måste innehålla kovarians och väntevärde för objekten. För att associera objekt används algoritmen Nearest Neighbor, se sidan 111 i [1] i kombination med gating. Utdata från modulen är en associationslista där mätningar är associerade till objekt. I figur 12 nedan visas vad modulen har för inargument samt vad den ska generera för utsignal. Figur 12: Associeringsmodulens övergripande funktionalitet 7.2 Modulbeskrivning Nedan beskrivs mer ingående hur associeringsproblemet är löst Gater För att lösa associeringsproblemet beräknas först 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 beräknas i enlighet med föreläsninganteckningarna från en doktorandkurs i målföljning vid Linköpings universitet [5] som (y k ŷ k k 1 ) T S 1 k k 1 (y k ŷ k k 1 ) < γ G (8) där vänsterledet är χ 2 -fördelat med två frihetsgrader då vi har två dimensioner i mätdatan. χ 2 :s sannolikhetsfördelning går inte att integrera analytiskt. För att få fram ett tröskelvärde brukar man därför läsa ur tabeller. Tröskeln för ett 95-procentigt intervall kan hämtas i MATLAB genom kommandot chi2inv(0.95,2) som visar sig ge γ G = 5, 99.

27 Nearest Neighbor Metoden som används för att mappa nya uppmätta bildkoordinater 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 mätvärde vill hitta det mest sannolika objektet. Det kollas även så att mätvärdet ligger inom gaten för objektet. Associationen beräknas för varje mätvärde, enligt [5], i = argmin(yk i ŷ k k 1 ) T S 1 k k 1 (yi k ŷ k k 1 ) (9) 1 i m G k där m G k är antalet objekt som har en gate som mätningen ligger inom. När en mätning har associerats till ett 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 Funktioner Tabell 7 nedan listar de olika funktionerna som ska implementeras i associeringsmodulen. Funktionsnamn associate calculatemahalanobisdistance convertlandmarkstoassociation- Position converttargettoassociation- Position Beskrivning Funktionen som anropas för att starta en associering. Räknar ut Mahalanobisavståndet för en partikel. Konverterar LandmarkParticles-objekt till AssociationPosition-objekt. Detta behövs för att kunna köra associate ifrån positioneringsmodulen. Konverterar Target-objekt till AssociationPosition-objekt. Detta behövs för att kunna köra associate ifrån målföljningsmodulen. Tabell 7: Funktioner för assoceringsmodulen. 7.3 Resultat Associeringsmodulen klarar att associera mätningar till landmärken och mål. Det finns inga direkta krav på modulen i [3] istället måste den fungera för att uppfylla andra krav. Idag använder sig modulen av algoritmen Nearest Neighbor för att associera mätningar till landmärken och mål. En annan lösning hade varit att istället använde Global Nearest Neighbor, detta hade gett en något bättre associering.

28 22 8 Positioneringsmodul Positioneringsmodulens har till uppgift att skatta plattformens position i banan i x- och y-led. Detta gör den genom att använda detekterade positionsmarkörer och associera dessa med en karta som har fördefinerats i GUI:t. När associationen är gjord skattas plattformens position med hjälp av ett partikelfilter. Hastigheten i x-,y- och z-led hos plattformen ses som insignaler till rörelsemodellen i partikelfiltret. Detta avsnitt baseras på teorin som behandlas i kapitel 9 i [1]. 8.1 Övergripande design Det uppmätta 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 rumskoordinater som utsignal. Positionen levereras med ett konfidensintervall. 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 ett antal 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 modulen 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 bildplanet. Tillbaka från kameramodulen fås en uppsättning med partiklar för varje positionsmarkör. I positioneringsmodulen beräknas en kovarians och medelvärde 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 som sker är en mätuppdatering där vikterna för alla partiklar uppdateras och normeras. Därefter beräknas ett viktat medelvärde samt viktad kovarians 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 inte återsampla för ofta eftersom information förloras i varje återsamplingssteg. I tidsuppdateringen uppdateras partiklarnas tillstånd. Nya tillstånd uppdateras med en

29 23 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 8 nedan listar de olika funktionerna som ska implementeras i positioneringsmodulen Associera positionsmarkörer För att associera mätningar från bildbehandlingen till kända positionsmarkörer använder sig positioneringsmodulen först av kameramodulen och sedan av associeringsmodulen. Positioneringsmodulen tar in två olika typer av mätningar; kantmarkörer samt mittmarkörer. Associeringsmodulen skiljer på dessa så att till exempel en mätning av en kantmarkör inte ska associeras till en mittmarkör. Första steget i associeringen som sker är att propagera alla partiklar genom mätekvationen för varje positionsmarkör. Detta görs genom att anropa kameramodulen. Resultatet är att det för varje positionsmarkör finns en uppsättning partiklar som skattar positionsmarkörens position i bildplanet. Nästa steg är att kovarians samt

30 24 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 bildplanet 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 8: Funktioner för positioneringsmodulen. medelvärde beräknas för varje partikeluppsättning i bildplanet. Först därefter anropas associeringsmodulen som gör själva associeringen av mätningarna. Den algoritm som används är Nearest Neighbor. Resultatet är en lista med mätningar associerade till positionsmarkörer 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 inparametrar som läses in från en textfil. Dessa är antalet partiklar N, startposition för plattformen, kovarians för startpositionen, kovarians för mätbrus i bildplanet samt kovarians för processbrus. Dessa parametrar kan ej ändras under tiden som programmet körs, istället måste programmet startas om efter att dessa har ändrats. 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. Initiering Partikelfiltret initieras genom att skapa det antalet partiklar som ska användas i filtret. Tillstånden hos partiklarna fördelas utifrån startpositionen samt kovariansen för denna. Tillståndet i en partikel beskriver positionen för denna samt en vikt som beskriver sannolikheten hos partikeln. Summan av vikten hos alla partiklar summeras alltid till ett vilket benämns som att vikterna är normaliserade. Mätuppdatering 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 bildplanet. För varje partikel skapas en gaussisk fördelning. Utifrån denna berä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

31 25 partikelns tillstånd. Varje partikel i rumsplanet genererar en partikel per positionsmarkör i bildplanet. Vikterna hos partiklarna normeras sedan så att summan av alla vikter blir ett. Ekvationerna blir då w i k k = 1 c k w i k k 1 p(y xi k) c k = p(y x i k) = N wk k 1 i p(y xi k) i=1 M p(y m x i k) m=1 M m=1 { exp 1 2 (y m f m (x i } k ))2 σ 2 (10a) (10b) (10c) där m är ett index som går över alla associerade mätningar, i är index över partiklarna och k är tidsindexet. f m ( ) är funktionen som tar en partikel i rumsplanet genom mätekvationen till bildplanet för landmärke m. Mätkovariansen är σ 2 I varför det går att förenkla till att endast dividera med σ 2. För att få en numeriskt stabil mätuppdatering har logaritmen av vikterna beräknats istället. Ekvationerna blir då { } { 1 1 log wk k i = log wk k 1 i c p(y xi k) = log k { p(y m x i k) exp 1 2 c k = (y m f(x i } k ))2 σ 2 N wk k 1 i p(y xi k) i=1 } wk k {log i = exp wk k i max (log wk k i ) i där vikterna sedan normeras så att summan av dessa blir ett. } M wk k 1 i log { p(y m x i c k) } k m=1 (11a) (11b) (11c) (11d) Estimering Efter mätuppdateringen estimeras en position för plattformen genom att beräkna ett viktat medelvärde över alla partiklar i rumsplanet, samt ett konfidensintervall genom att beräkna kovariansen hos partiklarna. Medelvärdet beräknas enligt x = Den viktade kovariansen beräknas enligt Q = 1 1 N i=1 (wi ) 2 n w i x i. (12) i=1 N w i ( x i x ) ( x i x ) T. (13) i=1 Där x är det viktade medelvärdet för partiklarna och x i är partikel i. Q betecknar här kovariansmatrisen för partiklarna. Återsampling Nästa steg är återsampling av partiklarna. Återsampling sker endast ifall det effektiva

32 26 antalet partiklar understiger två tredjedelar av det totala antalet partiklar. Effektiva antalet partiklar räknas ut enligt ˆN eff = 1 i (wi k k )2. (14) 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. Tidsuppdatering 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 har 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. Rörelsemodellen som används beskrivs enligt [ ] [ ] [ ] [ ] xk+1 xk Ts 0 vx = + + e(t), (15) y k+1 y k 0 T s v y där v x och v y är hastigheten, T s är samplingstiden och e k är vitt Gaussiskt brus med kovariansen Q. 8.3 Resultat Positioneringsmodulen uppfyller alla krav som är satta i kravspecifikationen för projektet [3]. Problem kan uppstå då plattformen inte flyger tillräckligt högt upp så att för få positionsmarkörer kan ses i bild. Ett vettigt utvecklingssteg vore att även skatta girvinkel för plattformen. Just nu litar positioneringsmodulen fullt ut på mätningarna vilka inte är helt exakta, ibland driftar denna även iväg. Det vore även snyggt om startpositionen inte behövde specificeras utan att plattformen kan starta på en godtycklig plats i kartan och att partikelfiltret sedan konvergerar mot den sanna positionen.

33 27 9 Målföljningsmodul Målföljningsmodulen ansvarar för att analysera och behandla data gällande detektioner som skickas från bildbehandlingsmodulen. Modulen hittar mål och skattar dess positioner samt hastigheter i x- och y-led i rumskoordinater. Skattad data skickas vidare till GUI:t. Modulen kan på kommando slås av och på. Med en detektion menas att bildbehandlingsmodulen har markerat en bildkoordinat som antingen är en felmätning eller en mätning som kommer från ett fysiskt mål. För nya detektioner som inte kan associeras till konfirmerade mål skapas så kallade initiatorer. Om en initiator fått ett visst antal mätningar som associerats till denna uppgraderas denna till ett mål. Spår är det samlingsnamn som hädanefter används för både initiatorer och mål. 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 skattad data om målen. Figur 15: Målföljningsmodulens övergripande funktionalitet I figur 16 visas de huvudsakliga delmomenten i modulen, samt hur de hänger ihop.

34 28 Figur 16: Målföljningsmodulens övergripande funktionalitet 9.2 Modulbeskrivning Målföljningsmodulen utgörs av en klass som kör själva modulen (TargetTrackingModule) samt en klass som specificerar karaktäristik för spår (Target). 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 bildkoordinater genom att varje spår anropar kameramodulen (avsnitt 6) via huvudbussen som utför transformationen med dess mätfunktion. Tidsuppdateringens innovationskovarians beräknas med den linjäriserade mätfunktionen. De predikterade mätningarna och dess kovarians skickas sedan till associationsmodulen, som beskrivs i kapitel 7 tillsammans med nya mätningar för att avgöra vilken mätning som hör till vilket spår. När ett spår fått en ny mätning enligt ovan angivna logik sker en mätuppdatering enligt ett Extended Kalman Filter (EKF) som då estimerar tillståndet för spåret i fråga, under antagandet att målet endast kan befinna sig på marken. Mål och initiatorer är båda objekt av klassen Target men hålls isär genom att de läggs in i två separata listor Funktioner De funktioner som implementerats i målföljningsmodulen finns beskrivna i tabell 9.

35 29 Funktionsnamn Beskrivning timeupdatelists Loopar igenom listorna av initiatorer och mål och utför en tidsuppdatering enligt rörelsemodellen för varje spår. measurementupdatet Utför en mätuppdatering enligt ett EKF på vardera mål. measurementupdatei Utför en mätuppdatering enligt ett EKF på vardera initiator. nomatches Returnerar mätningar som ej associerats med existerande spår. nmlogic Kollar N/M-logiken på alla initiatorer. newinitiator Skapar initiatorer för mätningar som ej associerats till existerande spår. removehighcovariance Tar bort spår som får för hög kovarians. Tabell 9: 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 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 x k+1 = F k x k + G v,k v k, Cov(v k ) = Q k. (16) Transformationen av målets rumskoordinater till bildkoordinater beskrivs av y k = h(x k ) + e k, Cov(e k ) = R k (17) som är en linjär funktion. Där tillståndsvektorn x ges av x e x = y e x e (18) F k och G v,k ges, med T s som samplingsperiod, enligt CV-modellen av y e T 1 0 T s 0 2 s F k = T s , G T v,k = 0 2 s 2 T s 0. (19) T s Modellbrusets kovariansmatris Q k och mätbrusets kovarians R k har trimmats in och bestämts enligt (20). ( ) ( ) Q k = σq 2 1 0, R 0 1 k = σr (20) 0 1 h(x k ) är mätekvationen som ges i (4) och (5).

36 Tidsuppdatering Utifrån modellen ovan sker tidsuppdateringen enligt (21). ˆx k+1 k = F k x k k P k+1 k = F k P k k F T k + G v,k Q k G T v,k (21) där ˆx k+1 k är det predikterade tillståndet, ˆx k k är det estimerade tillståndet, P k+1 k är det predikterade tillståndets kovarians, P k k är det estimerade tillståndets kovarians, F k, G v,k och Q k är angivet ovan. Tidsupdateringen avslutas med att spår vars kovarians är större än 1.5x1.5m tas bort Linjärisering av mätfunktion Linjäriseringen sker med en första ordningens Taylorutveckling. Från den sista raden i (7) fås då ett uttryck för λ och för att behålla ettan på nedersta raden i tillståndsvektorn i bildkoordinater kommer uttrycken för x p och y p bli x p = ht 1 x h T 3 x y p = ht 2 x h T 3 x (22), där h T i är respektive rad i (7) och x ges av där x c,y c,z c är kamerans position i rumskoordinater. x = x e x c y e e c (23) 0 z c (22) (som hädanefter kallas för h) är då den mätfunktion som ska linjäriseras. Genom att derivera mätfunktionen analytiskt, fås Jacobianen, h (ˆx k ) Associationsproblemet För att associationsmodulen ska kunna skapa gater behövs innovationskovariansen, S k, som ges av (24). S k = R k + h (ˆx k k 1 )P k k 1 (h (ˆx k k 1 )) T (24) Associationen sker först för de bekräftade målen och mätningarna som inte associerats 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 M efterföljande iterationerna, om så är fallet uppgraderas initiatorn till ett bekräftat mål, det vill säga 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.

37 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 ) (25) ˆ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 till GUI:t. 9.3 Resultat Målföljningsmodulen uppfyller alla de kraven som är ställda på den i kravspecifikationen [3] för projektet. Målföljningen går att vidareutvecklas genom att lägga till flera rörelsemodeller för spår och användaren eller modulen väljer vilken modell som är lämpligast, det vill säga implementera en Interactive Motion model (IMM).

38 32 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 implementerad 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.

39 33 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 baseras på den funktionalitet som togs fram i fjolårets projekt. I avsökningen av banan planerar planeringsmodulen autonomt en rutt genom att beräkna ett antal positioner, delmål, som förväntas täcka den rektangulära banan. I figur 19 tydliggörs hur avsökningen fungerar. Plattformen ska sedan åka mellan dessa punkter tills annat uppdrag ges. När plattformen ska följa efter ett mål tar planeringsmodulen det målets position och sätter det som delmål som uppdateras kontinuerligt. Om positionsskattningen antingen är för dålig eller hamnar utanför banan avbryter plattformen sitt uppdrag och landar omedelbart.

40 34 Figur 19: Avsökning av en rektangulär bana. De blå punkterna visualiserar delmålen och de svarta linjerna plattformens tilltänkta flygväg Manuell mod I manuellt läge tar planeringsmodulen emot styrkommandon från GUI:t för att sedan skicka vidare dessa till plattformen via huvudbussen, förutsatt att plattformen fortfarande befinner sig på banan. Om plattformen befinner sig farligt nära höjdgränsen tillåts bara en ytterst liten styrsignal uppåt Avsökning av bana Vid avsökning av banan planeras en rutt utifrån en given karta. Autonom avsökning av banan kan väljas när plattformen positionerats och fått korrekt gir-vinkel. Dessa inställningar måste i dagsläget göras manuellt. Plattformen börjar då följa rutten och börjar på den första i listan. Denna punkt kommer 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 PD-regulator skickar modulen styrkommandon till plattformen som för denna vidare till nästa punkt på rutten Målföljning Då användaren angett att målföljning ska utföras försöker planeringsmodulen positionera plattformen rakt ovanför målet. Detta fungerar på samma sätt som när den flyger efter delmål, förutom att delmålen uppdateras efter målets position kontinuerligt Regulator Regleringen är konstruerad som en tvådimensionell tidsdiskret PD-regulator med två olika arbetsområden. Den proportionella delen ges av en konstant, K, gånger felet, och den