Teknisk Rapport Autonom spaning med quadcopter

Transkript

1 Teknisk Rapport Autonom spaning med quadcopter Version 0.4 Projektgrupp: Datum: Status Granskad Godkänd Teknisk Rapport

2 Projektidentitet E-post: Hemsida: Beställare: Kund: Kursansvarig: Christian A. Naesseth, ISY, Linköping University Tel.: , E-post: Maria Andersson, FOI E-post: Daniel Axehill, Linköpings universitet Tel.: , E-post: Clas Veibäck, Linköpings universitet Tel.: , E-post: Gruppmedlemmar Namn Ansvar Telefon E-post Magnus Blomberg Designansvarig magbl113 Tobias Hammarling Testansvarig tobha614 Teodor Johnsson teojo382 Emil Klinga Projektledare emikl364 Oliver Larsson olila044 Anton Niglis antni601 Martin Pettersson Dokumentansvarig marpe238 Per Öberg perob757

3 Dokumenthistorik Version Datum Utförda ändringar Utförda av Granskad Ändringar efter kommentar från Alla Alla handledare Ändringar efter kommentar från Alla Alla beställare Ändringar efter kommentar från Alla Alla handledare Utkast Alla Alla Teknisk Rapport

4 Innehåll 1 Inledning Parter Projektets bakgrund Syfte och mål Kortsiktigt mål Långsiktigt mål Användning Definitioner Översikt av systemet Styrmoder Koordinatsystem Huvudbuss Variabler Gränssnitt Kommunikation GUI Bildbehandling Uppdragsplanering Uppdragsföljning Manuell kontroll Resultat och diskussion Utvecklingsmöjligheter GUI/HMI Program och språk Övergripande design Uppdragsplanering Datorseende Uppdragsutförande Kartor Utökning av Gmapsfx och Google Maps API Utvecklingsmöjligheter Bildbehandling Inledning Språk och bibliotek Terminologi Övergripande design Programstruktur ImageProcessingMainProgram MainBusIPInterface IPGUI Detekteringsklasser FFMPEGDecoder

5 5.5.6 Tracking Måldetekteringsalgoritmer Målbildsmatchning Färgmatchning Bakgrundssubraktion Stödfunktioner Kalibrering Bildkvalitetsbestämning Målföljning Identifikation Matchning Filtrering Geoprojicering Resultat Utvecklingsmöjligheter Optimering Filtrera projicerade koordinater Bakgrundssubtraktion Gimball Uppdragsplanering Inledning Kommunikation Övergripande design Terminologi Modulbeskrivning Algoritmbeskrivning Förbehandling Polygrid Kostnadsmatris TSP-algoritm Runt koordinat Interpolering Sista kontroll Tidsberäkning/hastighetsreferensvekotor Övriga beräkningar Resultat Diskussion/Vidareutveckling Uppdragsföljning Övergripande design Språk och program Positionsestimering Förenklingar och felkällor Resultat och diskussion: Kalmanfilter Referensväljare Teknisk Rapport

6 7.4.1 Resultat och diskussion: Referensväljare Regulatorstrukur Styrmod: konstant yaw-vinkel Styrmod: konstant hastighet Resultat och diskussion: Regulator Kommunikation Inledning Övergripande Design Språk och program Terminologi Server-sidan Klient-sidan Tillstånds-data Referens/styr-signaler Räckvidd Programstruktur Resultat: Kommunikation Problem Vidareutveckling Plattform Förändringar Säkerhetåtgärder Nödstopp Batterinivå Startsekvens Resultat Vidare utveckling Hårdvara Radiolänk Integration Enklare kommunikationsprotokoll med plattformen Lägga delar av programmet på plattformen Teknisk Rapport

7 Autonom spaning med quadcopter 1 1 Inledning Detta är en teknisk rapport i projektet Autonom spaning med quadcopter. Projektet görs i samband med kursen TSRT10 - Reglerteknisk projektkurs på ISY, Instutitionen för systemteknik, på Linköpings universitet. Dokumentet presenterar de tillvägagångssätten som är använda för att lösa de uppgifter som följer av kraven i kravspecifikationen [4]. 1.1 Parter De delaktiga parterna i projektet är som följande: Kund: Maria Andersson, FOI Beställare: Christian A. Naesseth, ISY Handledare: Clas Veibäck, ISY Examinator: Daniel Axehill, ISY Projektgrupp: 1.2 Projektets bakgrund Intresset för autonoma och obemannade farkoster (UAV:er) har på senare tid vuxit enormt. Möjligheterna som lockar är att kunna utföra speciella typer av uppdrag som är för farliga, eller av annan anledning, olämpliga för människor. Den kanske vanligaste typen av uppdrag är spaning. UAV:n är då utrustad med olika sensorer för att samla in data för den specifika tillämpningen. Detta skulle till exempel kunna vara övervakning av kritisk infrastruktur eller överblicksbilder vid nödsituationer. Farkosten skulle i dessa fall vara ett välkommet inslag för att minska mänskliga fel och målet är att den fungerar så väl att den frigör operatören till att göra annat. 1.3 Syfte och mål Syftet med projektet är att tillämpa inhämtade kunskaper för att skapa en produkt på ett tillvägagångssätt liknande hur arbetet sker i arbetslivet. Projektet ska användas för att ge projektets medlemmar en chans att lära sig mer om hur projektarbete går till samtidigt som kunden får en produkt som har reella användningsområden. Projektets mål kan delas upp i kortsiktiga och långsiktiga mål Kortsiktigt mål Det kortsiktiga målet med detta projekt är att utveckla en demonstrator för enklare högnivåautonomi. Plattformen ska vara baserad på en quadcopter med tillgång till GPSoch strömmad videodata i realtid. Projektet består i att utveckla och implementera funktionalitet, baserat på en quadcopterplattform, för: Att planera ett autonomt uppdrag där plattformen söker av ett område runt en given koordinat efter mål. Teknisk Rapport

8 Autonom spaning med quadcopter 2 Att planera ett autonomt uppdrag där plattformen övervakar och detekterar mål längs en given sträcka eller i ett givet område. Att utföra planerade uppdrag. Att detektera mål i sensordata som levereras från plattformen då den utför ett uppdrag. Att i mån av behov utföra enkel målfölning av detekterat mål Långsiktigt mål Det långsiktiga målet med projektet är att skapa en bas för en FOI-demonstrator som kan användas för att visa på möjligheterna och nyttan med olika typer av autonom UAVfunktionalitet. Plattformen och den struktur som byggs upp kan även användas för att utveckla och testa nya typer av autonoma funktioner, algoritmer för måldetektion och algoritmer för målföljning eller flera samverkande plattformar. 1.4 Användning Slutprodukten kommer att användas för att demonstrera användningsområden inom autonom spaning med en UAV i form av en quadcopter. Tanken är att FOI efter projektets avslutande ska kunna använda produkten som demonstrator samt som en bas för vidareutveckling. Med anledning av detta har mjukvaran skrivits så att detta blir så enkelt som möjligt. 1.5 Definitioner I dokumentationen av projektet kommer flertalet områdesspecifika begrepp att användas. Dessa är punktade nedan med tillhörande beskrivning. Plattform: Trajektoria: Standardmål: Täckningsgrad: GUI/HMI: GPS: Med plattform avses quadcoptern sammanlänkad med GPS och videokamera som en enhet. Den kurva som plattformen har som mål att följa vid autonom styrning. Enkla geometrier som bildbehandlingsmodulen klarar av att känna igen. Andelen av en yta som blivit avsökt. Grafiskt användargränssnitt. (eng. Graphical User Interface, Human Machine Interface) Globalt positionssystem Systemet är begränsat till att behandla tre stycken olika typer av uppdrag, dessa är av väldigt generell karaktär och lämpar sig därför för en mängd olika användningsområden. De olika uppdragstyperna finns listade nedan med tillhörande beskrivning. Inom område: Denna typ av uppdrag innefattar ett område som ska avsökas, men kan även innehålla förbjudna områden som inte är tillåtna för plattformen att passera. Områdena definieras av geografiska lägeskoordinater (WGS84). Teknisk Rapport

9 Autonom spaning med quadcopter 3 koordi- Runt nat: Uppdraget specificeras med en läges-koordinat och en radie som definierar det område som ska avsökas. Längs linje: En avsökning längsmed en av operatören definierad trajektoria. Teknisk Rapport

10 Autonom spaning med quadcopter 4 2 Översikt av systemet Den färdiga produkten ska möjliggöra för en quadcopter att utföra enklare spaningsuppdrag autonomt. I denna kedja ingår uppdragsplanering, uppdragsföljning, reglering samt bildbehandling för måldetektion. Produkten kan i grova drag delas upp i komponenter enligt figur 1. Varje block i schemat har en specifik uppgift där tanken är att varje modul ska kunna utvecklas och fungera separat. Pilarna i figuren representerar beroenden och kommunikationsvägar. I tabell 1 visas en kort förklaring till modulernas ansvarsområden. Figur 1: Övergripande flödesschema för systemets generella upplägg Modul Huvudbuss Kommunikation Bildbehandling Bildavkodare Uppdragsplanering Uppdragsföljning GUI Kartor Lagring Ansvarsområde Kommunikationslager mellan alla systemets moduler. Sköter kommunikation mellan plattformen och huvudbussen. Använder insamlad videoström för att lokalisera mål. Ser till att bildbehandlingen har rätt datatyp. Tillhandahåller referenssignaler till uppdragsföljningsmodulen. Beräknar styrsignaler till plattformen från givna referenssignaler och skattade fysikaliska tillstånd. Interaktiv planering och utförande av spaningsuppdrag. Tillgodoser GUI med översiktskartor för planering. Möjliggör persistens av data, t.ex. planerade uppdrag, kartor och konfiguration. Tabell 1: Systemets olika moduler och ansvarsområden. Teknisk Rapport

11 Autonom spaning med quadcopter Styrmoder Systemet har två olika styrmoder för plattformen. Den första är full autonom flygning. Autonom flygning kräver tillgång till GPS-signal för att initieras och är således menad för utomhusflygning endast. Vid autonom flygning används uppdragsföljningsmodulen för styrning av plattformen och uppdragsplanering för att generera referenssignal. Den andra moden är manuell styrning, menad för flygning inomhus och eventuella tester utomhus. Manuell styrning kan initieras när som helst under både flygning inomhus och utomhus och görs genom GUI. När manuell styrning initieras förbikopplas uppdragsplaneringoch uppdragsföljningsmodulen, styrsignaler erhålls då istället av piltangenter på ett tangentbord. Plattformen har därför inte någon autonom förmåga vid inomhusflygning. 2.2 Koordinatsystem Tre olika koordinatsystem används för att enkelt kunna beskriva olika tillstånd och uppkomna situationer. De tre koordinatsystemen är WGS-84, N-frame och B-frame vilka går att betrakta i figur 2 och är definierade enligt följande. WGS-84 är koordinatsystemet som används för att beskriva position med GPSkoordinater och är definierad efter jordens geometri. WGS-84 är beskrivet med longitud X gps, latitud Y gps och altitud H gps. Navigation frame (N-frame), är ett lokalt koordinatsystem som är fixeras i plattformens startpunkt. Y-axeln pekar alltid mot norr, X-axeln mot öster och Z-axeln nedåt.det är utifrån denna som yawvinkeln är definierad. Navigation frame är utskrivet som X, Y, Z. Body frame (B-frame), är ett lokalt koordinatsystem på plattformen. Detta koordinatsystem följer plattformens rörelser och vinklar och är definierad efter plattformens geometri. Koordinatsystemet har x-,y och z-axlarna pekandes rakt fram, åt höger och rakt ner (dessa kallas även roll-, tipp- och giraxel), genomgående i rapporten kommer dock de engelska beteckningarna användas (roll, pitch och yaw). Bodyframe är utskrivet som x, y och z. Figur 2: Övergripande figur som visar hur de tre olika koordinatsystemen hänger ihop. GPS koordinater är definerade på sfären (föreställande jordklotet), bilden till höger visar en 2-dimensionell bild över hur E-frame ligger i N-frame och vinkeln mellan dem är yawvinkeln. Teknisk Rapport

12 Autonom spaning med quadcopter 6 3 Huvudbuss Huvudbussen är det medium vilken den interna kommunikationen mellan modulerna är uppbyggd på. Om all form av kommunikation sker via bussen bidrar detta till en större grad av modularitet då modulerna ej direkt är beroende av varandra. Bussen hanterar förvaring av gemensamma variabler samt gränssnitt för de andra modulerna för åtkomst av dessa variabler. 3.1 Variabler Då systemet är ett realtidssystem och samtliga moduler körs på separata trådar krävs att variablerna är trådskyddade. Java har ett inbyggt system för att hantera synkronisering och skydd av variabler. För att skydda variabler används Javas synchronized-metod. Då en tråd går in i ett block som ligger inom synchronized låses variabeln som är argument i anropet. Detta gör att inga andra objekt kan komma åt variabeln samtidigt vilket är viktigt då läsning och skrivning till variabler annars skulle kunna samtidigt och leda till en konflikt. Ett synchronized-anrop med huvudbussen kan se ut enligt kodutsnitt 1. Kodutsnitt 1: Exempel på ett trådsäkert anrop. synchronized ( huvudbuss )\{ // Read or write safely from /to variable huvudbuss. } På detta vis undviks konflikter och gemensamma variabler är synkroniserade. Samtliga funktioner för att hämta eller skriva till och från huvudbussen är implementerade som synchronized och implementerar då det som beskrivs i exemplet ovan. Huvudbussen används för att lagra programmets aktuella tillståndsvariabler så som om en modul ska köra eller inte. Modulerna kan sedan vänta på dessa variabler tills de sätts. För att undvika att trådarna som processerna ligger på kör medan de väntar används Java-metoderna wait och notify/notifyall. När en modul väntar på att bli startad väntar den på huvudbussen och om en annan modul vaknar upp kan modulen kontrollera tillståndsvariablen igen. Detta effektiviserar programmet då trådar som ej ska exekveras inte tar någon schemalagd processortid. 3.2 Gränssnitt Nedan specificeras gränssnitt mot huvudbussen för samtliga moduler. Metoder listas och beskrivs i tabeller för respektive modul. Metoderna som är listade har implementerats i huvudbussen så att kommunikation kan ske. I tabellerna listas endast metodnamn och en beskrivning av metoden. Inga datatyper specificeras här. För mer information om detta se källkoden för projektet. Teknisk Rapport

13 Autonom spaning med quadcopter Kommunikation Nr. Metod Beskrivning 0 setcontrolsignal Sätter array med styrsignaler. 1 getcontrolsignal Returnerar en array med kontrollsignaler. 2 EmergencyStop Sätter nödstopps-flaggan i huvudbussen. 3 setshouldstart Sätter flaggan som indikerar om initiering av plattformen ska ske. 4 shouldstart Hämtar flaggan som indikerar om initiering av plattformen ska ske. 5 setisstarted Sätter flaggan som indikerar om exekvering av plattformen ska ske eller ej. 6 isstarted Hämtar flaggan som indikerar om plattformen kör eller ej. 7 setgpsfixok Sätter flaggan för OK/NOK GPS-fix. 8 setwififixok Sätter flaggan för OK/NOK Wifi-länk 9 setruncontroler Sätter om regulatorn ska vara igång eller ej. 10 setisarmed Sätter flaggan som indikerar om kommunikation med plattformen är upprättad. 11 getisarmed Hämtar flaggan som indikerar om kommunikation med plattformen är upprättad GUI Nr. Metod Beskrivning 0 togglecontroler Sätter flaggan som indikerar om regulatorn körs eller ej. 1 setshouldstart Sätter flagga om initiering ska ske eller ej. 2 setisstarted Sätter flaggan som indikerar om exekvering ska ske eller ej. 3 isstarted Hämtar flaggan som indikerar om exekvering ska ske eller ej. 4 getcurrentspeed Hämtar plattformens nuvarande hastighet. 5 getcurrentquadposition Hämtar plattformens nuvarande GPSposition. 6 wififixok Kontrollerar flaggan för ok wifi-länk. 7 gpsfixok Kontrollerar flaggan för ok GPS-fix. 8 setassignmentplaneron Sätter flaggan som indikerar om uppdragsplanerarens arbete med att optimera aktuell trajektoria ska exekvera eller ej. 9 isassignmentplaneron Hämtar flaggan som indikerar om uppdragsplanerarens arbete med att optimera aktuell trajektoria ska exekvera eller ej. 10 setmissionobject Sätter nuvarande uppdrag. 11 getmissionobject Hämtar nuvarande uppdrag 12 getimage Hämtar nuvarande bild. 13 setemergencystop Sätter flaggan som indikerar nödstopp. 14 getbattery Hämtar nuvarande batterinivå. 15 gettargets Hämtar en lista med målpositioner och identiteter. 16 getisarmed Kollar flaggan som indikerar om plattformen är redo för start. Teknisk Rapport

14 Autonom spaning med quadcopter Bildbehandling Förkortingen IP i metodnamnen nedan står för image processing. Nr. Metod Beskrivning 0 initipvariables Initierar bildbehandlingsvariabler. 1 getipactivemodes Hämtar vilka moder som är aktiva. 2 settipactivemodes Sätter vilka moder som är aktiva. 3 activateipmode(i) Aktivera mod i. 4 deactivateipmode(i) Avaktivera mod i. 5 getisiprunning Hämtar flaggan som indikerar om bildbehandlingen är på eller ej. 6 setisiprunning Sätter flaggan som indikerar om bildbehandlingen är på eller ej. 7 getipimagemode Hämtar flaggan som indikerar vilken bild som ska visas. 8 setipimagemode Sätter flaggan som indikerar vilken bild som ska visas. 9 getipcolortemplates Hämtar de färgmallar som finns. 10 setipcolortemplates Sätter de färgmallar som finns. 11 addipcolortemplate Lägger till en färgmall. 12 getipformtemplates Hämtar de formmallar som finns. 13 addipformtemplate Lägger till formmall. 14 getcalibformtemplate Hämtar den formmall som ska kalibreras. 15 setcalibformtemplate Sätter den formmall som ska kalibreras. 16 setiptargetlist Sätter måldata för de mål som följs. 17 setipimagetoshow Sätter bilden som ska visas i GUI:t. 18 getipimagetoshow Hämtar bilden som ska visas i GUI:t 19 setipcalibtemplate Sätter färgmall som ska kalibreras. 20 getipcalibtemplate Hämtar färgmall som ska kalibreras. 21 getquaddata Hämtar senast uppdaterad sensordata Uppdragsplanering Nr. Metod Beskrivning 0 setmissionobject Sätter nuvarande uppdragsobjekt. 1 getmissionobject Hämtar nuvarande uppdragsobjekt. 2 setassignmentplaneron Sätter flaggan som indikerar om uppdragsplanerarens arbete med att optimera aktuell trajektoria ska exekvera eller ej. 3 isassignmentplaneron Hämtar flaggan som indikerar om uppdragsplanerarens arbete med att optimera aktuell trajektoria ska exekvera eller ej. 4 setmatlabproxyconnection Sätter objekten som upprättar matlabproxyuppkopplingen. 5 getmatlabproxyconnection Hämtar objekten som upprättar matlabproxyuppkopplingen. 6 addvisitedpoint Lägger till besökt punkt. 7 getvisitedpoints Hämtar besökta punkter. Teknisk Rapport

15 Autonom spaning med quadcopter Uppdragsföljning Nr. Metod Beskrivning 0 getquaddata Hämtar senast uppdaterad sensordata. 1 getmissionobject Hämtar nuvarande uppdragsobjekt. 2 setcontrolsignalobject Sätter nuvarande styrsignaler. 3 isstarted Hämtar flaggan som indikerar om exekvering ska ske eller ej. 4 setruncontroller Sätter flaggan som indikerar om regulatorn är på eller ej. 5 getruncontroller Hämtar flaggan som indikerar om regulatorn är på eller ej. 6 getisarmed Kontrollerar flaggan som indikerar om plattformen är redo för start Manuell kontroll Nr. Metod Beskrivning 0 setspeed Sätter plattformens hastighet. 1 setemergencystop Sätter nödlandningsflaggan. 2 getmanualcontrol Hämtar flaggan som indikerar om manuell styrning är på eller av. 3 setmanualcontrol Sätter flaggan som indikerar om manuell styrning är på eller av. 4 getcontrolsignal Hämtar nuvarande kontrollsignaler. 5 setcontrolsignal Sätter nuvarande kontrollsignaler. 6 getruncontroller Hämtar flaggan som indikerar om regulatorn är på eller ej. 7 setruncontroller Sätter flaggan som indikerar om regulatorn är på eller ej. 8 isstarted Hämtar flaggan som indikerar om exekvering ska ske eller ej. 9 setisstarted Sätter flaggan som indikerar om exekvering ska ske eller ej. 10 setshouldstart Sätter flaggan som indikerar om initiering ska startas eller ej. 3.3 Resultat och diskussion Gränssnitten gör arbetet med kommunikation mellan modulerna lättare och en bättre och mer pålitlig kommunikation erhålls. På ett enkelt och överskådligt sätt ges även möjlighet till vidareutveckling helt oberoende av respektive modul när interface och kommunikationsvariabler är specificerade. Gränssnitten för modulerna upprättades i ett relativt sent skede av projektet. Det är något som borde gjorts tidigare för att få en bra struktur på huvudbussen och kommunikationen mellan den och moduler redan från början. På så sätt skulle en del dubbeljobb även kunnat undvikits i och med korrigering av kommunikationsvariabler och interface. Teknisk Rapport

16 Autonom spaning med quadcopter Utvecklingsmöjligheter Vid studie av ovanstående tabeller syns det att det finns en del möjliga förbättringar att göra. Ytterligare några objekt skulle förmodligen kunna slås ihop till större datatyper för att underlätta kommunikationen och på så sätt minska gränssnitten. Gränssnitten skulle även kunna struktureras upp något. Vissa metoder gör ungefär samma saker men heter olika saker och några metoder kan slås ihop. Det är möjligt att använda tillståndsvariabler för att införa bättre synkning av information. Kommunikationsmodulen skulle t.ex. kunna meddela regulatorn precis när ny sensorinformation fås och på så sätt skulle regulatorn kunna arbeta så snabbt som möjligt. Teknisk Rapport

17 Autonom spaning med quadcopter 11 4 GUI/HMI Modulen GUI/HMI är den modul som ansvarar för att visa slutanvändaren ett gränssnitt för att planera och följa uppdrag. I planeringsdelen kan användaren med hjälp av översiktskartor planera navigation och samtidigt ställa in övrig konfiguration. I utförandedelen kan plattformen startas och följas under ett pågående uppdrag. 4.1 Program och språk Användargränssnittet är liksom stora delar av det övriga programmet skrivet i Java 8 och det tillhörande biblioteket JavaFX. JavaFX tillsammans med Screenbuilder möjliggör att enkelt skapa användargränssnitt och händelsedriven användarinteraktion [12]. 4.2 Övergripande design Då gränssnittet i huvudsak har två uppgifter var är det indelat i två separata flikar, planing och execute. Till detta finns även en extra flik för konfiguration av bildbehandlingen. Anledningen till detta är att modulen erbjuder många konfigurationsmöjligheter som ska kunna anpassas till varje specifik situation och inte vara direkt kopplad till det planerade uppdraget. En schematisk bild över den huvudsakliga uppdelningen kan ses i figur 3. Figur 3: Användargränssnittets huvudsakliga upplägg. Programstrukturen är uppbyggd enligt figur 4. I det översta lagret finns Javafx inbyggda händelsesystem där klick, zoom, scroll åtgärder översätts till funktionsanrop i flikens kontroller. Dessa ansvarar sedan för att delegera ut uppgifter till övriga komponenter så som kartor eller huvudbuss. Teknisk Rapport

18 Autonom spaning med quadcopter 12 Figur 4: Användargränssnittets programstruktur. 4.3 Uppdragsplanering Uppdragsplanereraren ger användaren möjlighet att i förväg specificera områden som plattformen ska söka av. I detta steg finns inställningsmöjligheterna enligt tabell 2. Flikens layout visas i figur 5. Konfiguration Namn Uppdragstyp Höjd Navigationskoordinater Förbjudna områden Startposition Interaktion Generera trajektoria Spara uppdrag Ge uppdraget ett namn som referens. Välj typ av uppdrag: Runt koordinat/längs sträcka/inom område. På vilken höjd ska uppdraget ske. GPS-koordinater för navigation. GPS-koordinater för förbjudna områden. Ungefär var kommer plattformen starta (används av optimeringsalgoritmen). Använder specificerade koordinater för att generera optimal väg. Sparar uppdrag. Tabell 2: Möjlig konfiguration och interaktion för uppdragsplaneringen. Teknisk Rapport

19 Autonom spaning med quadcopter 13 Figur 5: Planeringsflikens layout. 4.4 Datorseende I GUI:t för datorseende finns funktionalitet för att konfigurera inställningar för måldetektion under planering och utförande av uppdrag. Klassen TabDatorseendeController innehåller källkoden som hanterar logik kring utritning och användarinteraktion med datorseendefliken. Klassen ansvarar även för kommunikation med huvudbussen. Figur 6 visar flikens utseende följt av en lista med konfigurationsmöjligheter. Figur 6: Planeringsflikens layout. Teknisk Rapport

20 Autonom spaning med quadcopter Knapparna för toggling av detektionsmetod och val av bild är implementerade med hjälp av JavaFX-klassen ToggleButton. Vid knapptryck läses det valda alternativet in och skickas vidare till huvudbussen. 2. Knappar för valda färg- och formmallar är implementerade med hjälp av JavaFXklassen ComboBox. Här väljer man vilka mallar som ska aktiveras. 3. Vid knapptryck på Color Template Settings öppnas ett nytt fönster för inställningar av färgmallens tröskelvärden. Fönstret är implementerad i klassen HSV-Sliders. 4. Vid knapptryck på Template matching Settings öppnas ett nytt fönster för inställningar av målbild-mallens intresseområde. Fönstret är implementerad i klassen TemplateMatchSliders. 5. I fliken visas även plattformens videoström. Denna ström kan användas för att direkt se resultatet av den utförda konfiguration. Bilden visas med hjälp av JavaFX-klassen ImageView. 4.5 Uppdragsutförande När ett uppdrag är planerat och datorseendet är inställt kan uppdraget utföras under fliken Execute. Möjlig interaktion och konfiguration kan ses i tabell 3. Layouten är utformad så att användaren lätt ska kunna se all relevant information om ett pågående uppdrag, se figur 7. Figur 7: Utförandeflikens layout. Teknisk Rapport

21 Autonom spaning med quadcopter 15 Konfiguration Uppdrag Interaktion Initiera (Arm) Execute Abort Emergency Auto/Manuel Välj det uppdrag som ska utföras Initierar alla underliggande moduler. Kontrollerar att allt fungerar Plattformen startar sitt uppdrag. Plattformen avbryter sitt uppdrag och återvänder hem. Plattformen avbryter och stanna på sin nuvarande plats. Växlar mellan automatisk / manuell styrning. Tabell 3: Interaktion med uppdragsutföraren. 4.6 Kartor I både fliken Plan och Execute används översiktskartor. Tjänsten som används för att tillhandahålla kartorna är Google Maps API [13]. API:t är skrivet i Javascript vilket kräver någon form av kompabilitetslager för att kunna användas i en Java applikation. För detta användes Gmaps FX, ett open-source bibliotek skrivet av Rterp [14]. Biblioteket möjliggör interaktion med Google Maps API direkt via Javakod istället för Javascript Utökning av Gmapsfx och Google Maps API Även om Googles API redan innehöll mycket av den funktionalitet som behövdes var den inte fullt tillräcklig. Den funktionalitet som behövde kompletteras var möjligheten att rita ut olika typer av symboler samt att funktionerna abstraherades iväg en nivå för att lättare utföra vanliga ritåtgärder. En klasskarta över det som utökades kan ses i figur 8. Pilar definierar beroende. Alla map shapes (polygoner, banor och cirklar) som användes samlades bakom samma gränssnitt för att den övriga koden skulle kunna interagera med dessa på samma sätt. Detta gränssnitt kan ses i kodutsnitt 2. Figur 8: Klasskarta över utökningen av Gmapsfx. Grå färg utgör bas-bibliotek som används. Pilar visar beroende. Teknisk Rapport

22 Autonom spaning med quadcopter 16 public interface MapShapeInterface { Kodutsnitt 2: Gränsnitt för kartsymboler /** * Uses GPSMarkes in list and draws the shape on the map */ public void draw (); /** * Add coordinate to Path */ public void addcoordinate ( LatLong clickedcoordinate ); /** * Remove all traces of the shape from the map */ public void remove (); /** * Returns an array of all markers associated with this shape */ public ArrayList <AbstractGPSMarker > getmarkers (); } /** * Check if the shape " valid ". * For a path valid is considered more than 2 points. * For a polygon this is more than 3 points. * For a Circle this is considered 1 point and 1 point only */ public boolean isvalid (); 4.7 Utvecklingsmöjligheter Utvecklingsmöjligheterna för användargränssnittet är många men det är framför allt två huvudsakliga åtgärder som borde prioriteras. Den använda kartmodulen och användarvänligheten. Kartorna hämtas idag kontinuerligt från internet vilket kräver en uppkoppling. Detta bidrar till att användandet blir något svårare än vad som skulle vara önskvärt. Programmet måste idag vara anslutet till plattformen via det trådlösa nätverket samtidigt som internet krävs. Detta kan lösas genom att vara uppkopplad mot plattformen via nätverket och samtidigt använda en mobiltelefon via USB för internetuppkoppling. Om det är möjligt att få kartorna att fungera utan internet vore detta ett ordentligt lyft för programmet. Det har också noterats att nya användare av programmet inte direkt förstår upplägget och det krävs en kortare tids användande innan uppdragsplaneringen går smidigt. Detta tyder på att programmet kan utvecklas till någon mer intuitivt och lättanvändligt. Teknisk Rapport

23 Autonom spaning med quadcopter 17 5 Bildbehandling I denna del beskrivs bildbehandlingsmodulen i sin helhet och dess ingående komponenter. Bildbehandlingsmodulen har som ansvar att detektera och följa objekt. 5.1 Inledning Bildbehandlingsmodulen används för att strömma bilddata från plattformen samt behandla denna bilddata för att detektera olika standardmål. Först beskrivs vilka verktyg som används i modulen. Därefter beskrivs grundläggande begrepp och den övergripande designen av modulen. Sedan beskrivs metoderna och deras ingående deluppgifter i detalj. 5.2 Språk och bibliotek Bildbehandlingsmodulen är skriven i programspråket Java och använder sig av en implementation av datorseende- och bildbehandlingsbibioteket OpenCV Ett externt bibliotek Xuggler[10] används för implementering av ffmpeg-avkodning. För att modulen ska fungera krävs Java 7 men bildbehandlingens GUI kräver Java Terminologi Nedan beskrivs några grundläggande koncept som kommer att användas i bildbehandlingsalgoritmerna. Intressepunkter och Deskriptorer: Intressepunkt är en position i bildrymden. Till varje intressepunkt hör en så kallad deskriptor (feature på engelska). En deskriptor håller information om intressepunktens omgivning. Exempel på sådan information är ett histogram över färgvärdena på pixlar inom en radie kring intressepunkten. Deskriptorn kan också innehålla en sannolikhet för att intressepunkten ligger på en kant i bilden. Överensstämmelse: Ett mått på hur väl intressepunkters deskriptorer överensstämmer med de sökta intressepunkterna hos objektets deskriptorer. SIFT: SIFT är en metod för att extrahera skal- och rotationsinvarianta intressepunkter. Morfning: Morfning innebär att pixelvärden i bilden ändras beroende på närliggande pixlar. Objekt kan expanderas och på så vis kan glapp fås bort. Objekt kan även kontraheras. Dessa metoder kallas erosion och dilation och är de enklaste morfologiska operationerna, men det finns fler. De olika operationerna används för att tydliggöra hur objektet ser ut. Färgskala: I bildbehandling används flertalet färgskalor i olika operationer. HSV: Färgskala som används vid färgmatchning. Står för Hue (nyans), Saturation (mättnad), och Value (värde). Detta är en cylindrisk färgskala. Detta betyder att hue antar värden i intervallet [0:179] där nästa värde efter 179 är 0 och på så sätt är cirkulär. Saturation och value antar värden i intervallet [0:255]. BGR: Gråskala: Färgskala vilken bilderna är i efter avkodning. Färgskala då endast en färgkanal används. Avgränsande lådor: Avgränsande lådor används för att ge detekterade objekt en position. Teknisk Rapport

24 Autonom spaning med quadcopter Övergripande design I denna del beskrivs den övergripande designen av programmets huvudloop. Figur 9: En schematisk bild över bildbehandlingens huvudloop. Avkodare: Bildavkodare som avkodar videoströmmen till ett format som går att behandla i OpenCV. Hämta nästa bild: Nästa bild hämtas från en kö av bildobjekt till vilken avkodaren levererar bilder. Kön används om behandlingen av bilden i något skede skulle gå fortare än vad avkodaren kan leverera bild. Bestämma bildkvalitet? Flagga som hämtas från huvudbussen. Sätts genom programmets GUI. Teknisk Rapport

25 Autonom spaning med quadcopter 19 Bildkvaliteten bestäms enligt algoritm som beskrivs i sek- Bildkvalitetsbestämning: tion Flagga som hämtas från huvudbussen. Sätts ge- Detekteringsmetoder som är på?: nom programmets GUI. Detekteringsmetoder Metoder som används för att detektera olika typer av objekt. Mer om dessa i sektion 5.7 respektive Målföljning på?: Flagga som hämtas från huvudbussen. Sätts i programmets GUI. Målföljning: Identifiering och följning av detekterade objekt. Beskrivs i sektion Vilken bild som ska visas: Hämtas från huvudbussen. Sätts i programmets GUI Leverera bild och måldata: Levereras till huvudbussen där de senare kan hämtas av programmets GUI för visning. 5.5 Programstruktur Nedan beskrivs övergripande design av bildbehandlingens programstuktur. För mer ingående delinformation ombedes läsaren läsa projektets Javadoc ImageProcessingMainProgram Bildbehandlingens huvudprogram, som ärver funktionalitet av ProgramClass, initierar avkodaren, de detektionsalgoritmer som används och sköter kommunikation med huvudbussen. Kommunikationen med huvudbussen sköts genom gränssnittet MainBusIPInterface vilket innehåller de funktioner som krävs för att kommunikation med övriga moduler ska kunna ske. Huvudprogrammet sköter: Initiering av dekoder Hantering av bilddata Initiering av färgmatchning Initiering av målbildsmatchning Kommunikation med bussen MainBusIPInterface Bildbehandlingens kommunikations-gränssnitt med huvudbussen. De funktioner som specificeras i gränssnittet måste finnas i huvudbussen. I gränssnittet specificeras funktioner för: Bildbehandling på/av Överföring av bild Överföring av måldata Överföring av sökta objekt Överföring av måldata Teknisk Rapport

26 Autonom spaning med quadcopter IPGUI Användargränssnitt för att sköta bildbehandlingens uppgifter och variabler. Är ett tillägg till det vanliga användargränsnittet. Mer om detta i sektion Detekteringsklasser De detekteringsklasser som används är ColorDetection och TemplateMatching. ColorDetection Klassen ColorDetection sköter färgdetektering. Den går igenom lista med ColorTemplates (färgmallar) som skapats på förhand eller under körning i GUI:t. För varje färgmall körs algoritmen som beskrivs i avsnitt 5.7. Färgmallarna uppdateras sedan genom de objekt som detekterats färgskala. TemplateMatching Klassen TemplateMatching sköter formdetektering. Användaren kan vid programmets uppstart välja ut och kalibrera en bild på ett eftersökt objekt. Vid kalibreringen sparas bilden med tillhörande data (intressepunkter och deskriptioner) ned i ett FormTemplate-objekt. Algoritmen för formmatchning jämför inkommande bild från quadkopterns videoström med ett FormTemplate-objekt och försöker hitta överrensstämmelse. Om det finns en överrensstämmelse mot FormTemplate-objektet så har vi fått en måldetektion FFMPEGDecoder I klassen FFMPEGDecoder finns funktionalitet för att koppla upp sig mot Quadkopterns video-port. Dekodern tar emot och kodar av bildströmmen. Det senaste avkodade bilderna sparas ned i en köbuffer som bildbehandlings modulen sedan kan läsa ifrån. FFMPEG-decoder klassen bygger på Xuggler-biblioteket Tracking Tracking-klassen implementerar målföljning enligt avsnitt Måldetekteringsalgoritmer Nedan beskrivs de detekteringsalgoritmer som används i projektet. Samtliga detekteringsalgoritmer har bilddata att hämta för att synliggöra processen Målbildsmatchning I denna del beskrivs målbildsmatchningen i detalj. Målbildsmatchning används för att i bildströmmen hitta delar av bilden som stämmer överens med en tidigare tagen målbild. På detta sätt kan specifika former hittas i bildströmmen. Metoden beskrivs i 10. In: Bild från decoder Ut: Detekterade objekt Bild att hämta: Bild med nyckelpunkter Teknisk Rapport

27 Autonom spaning med quadcopter 21 Figur 10: En schematisk bild över hur målbildsmatchningen sker. Intressepunkter: Beräknar intressepunkter för den inkommande bilden. Här används OpenCV s funktioner för att ta fram och beräkna dessa. Överensstämmelse mot Målbild: Intressepunkter matchas mot mål-/mallbildens intressepunkter. Om mer än fyra bra matchningar erhålls forsätter algoritmen. Om färre eller lika med fyra matchningar erhålls avbryts algoritmen och returnerar en tom lista med detekterade mål. Homografi: Med hjälp av matchningar mellan inkommande bild och mall-bilden beräknas sedan homografin. Homografin beskrivs i detta fall av en transformationsmatris som transformerar pixelkoordinater från målbilden till inkommande bild. Vid kalibreringen av mallbilden definieras ett intresseområde. Detta intresseområde kan med hjälp av homografin transformeras till den inkommande bilden. Till målföljning: Nu bestäms fyra hörnpunkter som beskriver det område i den inkommande bilden vilket målet befinner sig inom. Dessa fyra hörnpunkter, tillsammans med antalet träffar, skickas vidare till trackingen. 5.7 Färgmatchning Färgmatchning används för att detektera bilddata med en viss färg. Detta kan vara lämpligt om ett objekt med någon väldigt specifik färg ska detekteras mot en bakgrund med annan färg, t.ex röd boll mot grönt gräs i ett enkelt fall. Algoritmen för färgmatchning synliggörs i figur 11. In: Bild från decoder Ut: Detekterade objekt Bild att hämta: Utskurna färger Teknisk Rapport

28 Autonom spaning med quadcopter 22 Figur 11: En schematisk bild över hur färgmatchningen kommer att ske. RGB till HSV: I algoritmens första steg förs färgskalan över till en mer lättbehandlad skala HSV, som är en cylindrisk representation av färgskalan. Tröskling av kanaler: I nästa steg sker tröskling av HSV-kanalerna i bilden med avseende på färgmallar som används. Mallarna har ett minimumvärde och ett maximumvärde för varje skala som kanalen måste vara inom. Detta gör att enbart HSVvärden inom de tröskelvärden som finns i färgmallen finns kvar i bilden. Morfning: Pixelvärden måste sedan morfas på rätt sätt för att få bort eventuella luckor och göra objektet mer igenkännligt. Detta gör att även om det finns luckor i bilddata som kan vara detekterbara objekt kan dessa sammanflätas till ett helt objekt och blir på så sätt lättare att klippa ut i nästa steg. Extrahera konturer: I detta steg extraheras konturer runt de erhållna färgområden. Dessa konturer kan användas för att beräkna områdens area och position. Teknisk Rapport

29 Autonom spaning med quadcopter 23 Avgränsande lådor: Konturerna kan nu användas för att få ut storlek på områdena. De områden som har area inom ett visst område anses vara ett detekterat objekt. Dessa objekt tilldelas en avgränsande låda vars storlek beror på deras kontur. Den avgränsade lådan har en bildkoordinat. Detta är det detekterade objektets bildkoordinater. Uppdatering av färgmallar: De konturer som motsvarar objekt kan användas till att extrahera vilka HSV-värden objektet som detekterats har. Detta görs genom att använda konturerna som mask i HSV-bilden och ta medelvärdet av de utskurna pixelvärdena. Objektets HSV-värden är av intresse då dessa används för att uppdatera färgmallarnas tröskelvärden. Målföljning: Informationen från de avgränsade lådorna översätts till måldata. Måldatan innehåller position av det detekterade objektet samt objektets HSV-värden. Dessa värden kan sedan användas för att matcha objektet mot tidigare detekterade objekt i målföljningen. Måldatan skickas sedan vidare till målföljningen vilken beskrivs i sektion Bakgrundssubraktion Arbetet på algoritmen för bakgrundssubtraktion påbörjades. Men algoritmen hann aldrig bli körduglig. Algoritmen implementerades enligt designspecifikationen.[6] Det som inte fungerade var transformationen mellan föregående bild och nuvarande bild. Detta kan bero på: Fel vid detektion av intressepunkter i de båda bilderna. Dåliga matchningar av intressepunkter mellan de olika bilderna. För fortsatt utveckling av algoritmen bör man undersöka: Andra metoder för detektion av intressepunkter. Andra metoder för att matcha intressepunkter mellan två olika bilder Metod för att sortara bort dåliga matchningar. 5.9 Stödfunktioner Nedan beskrivs de större stödfunktioner som används i bildbehandlingsmodulen Kalibrering För kalibrering användes OpenCV-funktionerna Calib3d.findChessboardCorners och Calib3d.calibrateCamera. Här används bilder på schack-mönster tagna med plattformens kamera. I första steget av kalibreringen körs findchessboardcorners för att ta fram hörnpukterna i schackmönstret för varje bild. Då man tagit fram hörnpunkter för ett par olika schack-bilder skickas sedan punkterna vidare in till funktionen calibratecamera. calibratecamera returnerar kameramatrisen för plattformens kamera. Teknisk Rapport

30 Autonom spaning med quadcopter Bildkvalitetsbestämning Bildkvaliteten som bestäms är ett mått på hur mycket rörelseoskärpa det är i bilden. Flödesschemat i figur 12 beskriver den principiella processföljden. Måttet bestäms utifrån kantskärpa. En skarp kant har hög intensitet på sin gradient. En hög intensitet motsvarar alltså en skarp kant och omvänt låg intensitet motsvarar låg kantskärpa. Kvaliteten bestäms i både horisontell och vertikal led. Figur 12: Flödesschema för bestämning av bildkvalitet Till gråskala: Bildkanalerna transformeras till gråskala. Sobelfilter: Ett sobelfilter appliceras i horisontell respektive vertikal led för att ta fram gradienter till kanter i bilden. Hitta kantbredd: Lokala maximum i horisontellt och vertikalt led tas fram genom att stega igenom bilden i bitar. Detta görs kolonnvis respektive radvis. Skarpa kanter ger höga lokala maxima och oskarpa kanter ger breda låga lokala minima. Ett tröskelvärde används för att bestämma om något är en kant eller inte för att inte få med tomma ytor. Medelvärde: Medelvärde på lokala maxima i horisontellt led respektive vertikalt led tas var för sig. På så vis fås ett mätvärde på estimerad skarphet i de olika leden. Teknisk Rapport

31 Autonom spaning med quadcopter 25 Beslut: Den estimerade skarpheten från föregående steg normeras med en maximal kantskärpa och subtraheras från 1. På så sätt fås ett mått på oskärpan i de olika leden i intervallet [0:1] Målföljning De olika detekteringsmodulerna skickar måldata till målföljningsmodulen. Målföljningsmodulen innehåller ett tidsdiskret linjärt Kalmanfilter[3]. I Kalmanfiltret skattas de detekterade målens position och hastighet. Målobjekten innehåller information om tillståndet och kovariansen som används i filtret. De innehåller även identifikationsinformation för att möjliggöra målföljning och förknippa varje målobjekt med ett specifikt ID. Filtret arbetar på bildkoordinater men är förberett för att i stället använda koordinater projicerade på geokoordinater. Målföljningens övergripande funktionalitet beskrivs i figur 13. Figur 13: Målföljningens övergripande funktionalitet Teknisk Rapport

32 Autonom spaning med quadcopter Identifikation När en detekteringsmodul upptäcker ett målobjekt skapas målobjektet tillsammans med ett identifikationsobjekt. Identifikationsobjektet innehåller information om färginnehåll från färgmatchningsmodulen och träffresultat från målbildsmatchningsmodulen. Färginformationen är medelvärdesbildade HSV-värdena över det detekterade området. Träffresultatet beskriver det procentuella antalet träffar av antal möjliga för den målbild som detekterats. Varje gång en matchning görs så uppdateras den sparade identifikationsinformationen för att närma sig den nya informationen. Uppdateringen sker med genom att filtrera mätvärdena med ett lågpassfilter där stegsvaret har en tidskonstant enligt variabeln timeconstant i funktionen update till klassen Identifier Matchning För att förknippa de, för varje ny bild, upptäckta målen med följda mål så används en matchningsfunktion. Matchningen gör att varje upptäckt mål förknippas med ett följt mål alternativt ses som ett nytt mål. Matchningen sker genom att jämföra positionsinformation och identifikationsinformation. Nivån på överensstämmelsen mellan nya mål och följda mål sparas i en tabell. Tabellvärdena utgörs av en linjärkombination mellan överensstämmelsen från positionsinformation och identifikationsinformation. Överensstämmelsen baserad på positionsdata är en kombination av avstånd mellan de jämförda målen och deras positionsosäkerheter. Det intuitiva är att ett stort avstånd försämrar matchningen. I avståndsjämförelsen tas även osäkerheten för positionerna med. Överensstämmelsen från positionsdata är en produkt mellan en funktion på kvoten mellan avstånd och osäkerhet och en funktion av osäkerheten. Funktionernas utseende återfinns i figur 14 där enheterna för avstånd är beskrivna i meter. Funktionerna är tänkta att användas för koordinater projicerade på jordytan och vid implementationen har de skalats om för att fungera i bildkoordinater. (a) Påverkan från kvoten avstånd mot osäkerhet (b) Osäkerhetspåverkan Figur 14: Målmatchning med avseende på avstånd Identifikationsinformationen för två mål jämförs med avseende på data från de använda detekteringsmodulerna. För färgmatchning jämförs differansen mellan de två målens olika medelvärdesbildade innehåll i de olika färgkanalerna. Överensstämmelsen Y, baserat på färginnehåll, mellan de två målen A och B beskrivs av ekvation 1 där C x,y är Teknisk Rapport

33 Autonom spaning med quadcopter 27 medelvärdet för bild x i kanal y och p x är säkerheten i färginnehållet för bild x där C x,y [0, 1]. Y = p A p B (1 C A,c C B,c ) 4 (1) c H,S,V Den motsvarande överensstämmelsen från målbildsmatchning baserat på antalet upptäckta och matchade deskriptorer jämfört med totala antalet deskriptorer för en målbild. Om två mål har upptäckt P A respektive P B procentuella antal träffar i samma målbild så är överensstämmelsen U: U = 1 P A P B (2) Filtrering De följda målens positionsdata skattas av ett linjärt tidsdiskret Kalmanfilter. Varje följt mål sparar data för sitt skattade tillstånd ˆx k och tillståndets kovarians P k. Tillståndet vid tidpunkten k är definierat som position (p) och hastighet (v) i x- respektive y-led på formen: p x ˆx k = p y v x v y Uppdateringen i filtret sker i två steg; en tidsuppdatering och en mätuppdatering. Vid tidsuppdateringen beräknas tiden sedan senaste uppdateringen och matrisen F k som beskriver dynamiken i systemet beräknas om. Även systembruset Q k beräknas om med avseende på tiden. Tidsuppdateringssteget sker enligt följande: ˆx k+1 k = F k ˆx k k (4) P k+1 k = F k P k k F T k + Q k, (5) där F k och Q k beror på T s som beskriver tiden sedan senaste uppdateringen och störningsnivån σ 2 : 1 0 T s 0 F k = T s (6) T 4 s 4 0 T 3 s 2 0 T Q k = 0 4 s T s 2 T 3 s 2 0 Ts 2 0 σ2 (7) T 0 3 s 2 0 Ts 2 Mätuppdateringen med ny mätning z sker genom följande tillvägagångssätt. Innovationen y beräknas som y k = z k H ˆx k k 1, (8) där H beskriver vad i tillståndet som mäts, i detta fall position i x- och y-led. Innovationskovariansen S beräknas som S k = HP k k 1 H T + R k, (9) (3) Teknisk Rapport

34 Autonom spaning med quadcopter 28 där R är mätbruset. Kalmanförstärkningen är implementerad med pseudoinvers och beskrivs av K k = P k k 1 H T S + k (10) Tillståndsuppdateringen och kovariansuppdateringen blir då ˆx k k = ˆx k k 1 + Ky (11) P k k = P k k 1 KHP k k 1 (12) För att säkerställa att numeriska fel inte gör kovariansmatrisen osymetrisk räknas P k om så att symmetri säkerställs. P k = 1 2 (P k + P T k ) (13) Geoprojicering En enkel funktion för att projicera bildkoordinater till geografiska koordinater är implementerad. Funktionen tar hänsyn till plattformens position, höjd och vinklar och antar att målet är upptäckt på höjden 0 m (definierad som då plattformens höjd är 0). Då kamerabilden är tagen med ett vidvinkelobjektiv blir dock projektionen osäker. Projektionen ger dock en övergripande bild av var målet befinner sig i geografiska koordinater och gör det möjligt att visa målet i det grafiska användargränssnittet Resultat Bildbehandlingsmodulen klarar samtliga grundläggande krav i kravspecifikationen [4]. Då samtliga ingående algoritmer i bildbehandlingsmodulen körs blir behandlingen av bilder något långsam. Detta då några av algoritmerna är krävande beräkningsmässigt och stora krav ställs på att behandlingen ska ske snabbt. En typsik målföljning kan se ut som i figur 15 där ett objekt har detekterats och följs med den implementerade målföljningsalgoritmen. Den cirkel som visas runt objektet är storleken på skattningens kovarians Utvecklingsmöjligheter Nedan listas ett antal förslag till vidare utveckling av bildbehandlingen vilka skulle medföra ytterligare funktionalitet till systemet Optimering Somliga metoder inom bildbehandlingen skulle kunna optimeras med avseende på flyttalsberäkningar som görs. Andra metoder gör beräkningar över alla pixlar. Dessa operationer är dyra och om de kan undvikas skulle det effektivisera beräkningarna Filtrera projicerade koordinater För att få bättre prestanda från målföljning kan detekterade objekts position direkt projiceras ner i GPS-koordinater. Dessa koordinater kan därefter följas på ett bättre sätt. Det skulle även bli lättare att rita ut korrekt konfidensinterval för skattningen av objektets position om detta införs. Teknisk Rapport

35 Autonom spaning med quadcopter 29 Figur 15: Bild som visar hur ett ma l fo ljs och ritas ut i GUI:t Bakgrundssubtraktion Bakgrundssubtraktion fo r en kamera i ro relse utan na gon stabilisation a r sva rimplementerat. Dock skulle en sa dan algoritm ge bra mo jligheter att detektera objekt i ro relse Gimball En ha rdvara med monterad gimball hade varit ba ttre fo r att genomfo ra de bildbehandlingsalgoritmer som na mnts tidigare i denna del. Detta da en sa dan skulle stabilisera bilden sa formmatchningen skulle vara mindre ka nslig. Dessutom skulle det ga att rikta kameran dit man vill. Teknisk Rapport

36 Autonom spaning med quadcopter 30 6 Uppdragsplanering 6.1 Inledning För att kunna navigera sig över det av GUI:t specificerade sökområdet planeras en nära optimal trajektoria. Beroende på om uppdraget är av typen inom område, runt koordinat eller längs linje kommer olika algoritmer för det gemensamma problemet att hitta en så bra trajektoria som möjligt att användas. I samtliga fall är det den estimerade tiden för uppdraget som är den slutgiltiga kostnadsfunktionen för optimeringsproblemet. Problemen klassificeras dock i subproblem med varierade lösningsmetoder för att hantera de olika deluppgifterna för varje uppdragstyp. 6.2 Kommunikation I första hand tar uppdragsplaneringsmodulen enbart emot information ifrån GUI:t via huvudbussen. Denna information är kopplad till uppdraget som modulen ska generera en trajektoria till. Med dess modulära struktur kan den dock hantera information av annan karaktär ifall exempelvis en regenerativ algoritm skulle implementeras i någon av de övriga modulerna för att hantera missade ytor i avsökningen som då skulle kunna efteravsökas vartefter ett uppdrag fortskrider. Vid färdigplanerad trajektoria sparas denna tillsammans med en estimerad tidsåtgång, en hastighetsreferensvekor, trajektorialängd och täckningsarea i huvudbussens minne för att göras åtkomlig för övriga moduler. 6.3 Övergripande design I ett första steg läser modulen in den objektorienterade datan som genererats av systemet, denna utvärderas för att avgöra vilken typ av lösningsmetod som modulen ska använda sig av för att beräkna den eftersökta trajektorian. I stora drag är det främst den aktuella uppdragstypen som avgör detta. Med lösningsmetod definierad och informationen sorterad kan modulen vidare påbörja sin optimering. Ett försök att illustrera modulens tillvägagångsätt för denna process är genomfört i figur 16 där det interna informationsflödet finns beskrivet. Beroenden framgår av pilar. Teknisk Rapport

37 Autonom spaning med quadcopter 31 Figur 16: Flödesschema för uppdragsplaneringsmodulen. De signaler som modulen hanterar beskrivs av signalflödena definierade i tabell 4. Förutom dessa kräver modulen även att en del konstanter definieras som beskrivs vidare i samband med algoritmutvecklingen, dessa finns att studera i tabell Terminologi Vid utvecklandet av planeringsalgoritmen har en hel del teori från optimeringsläran implementerats. För att enklast beskriva dessa delar kommer därför de termer som är specifika för detta fackområde att användas. Utförligare beskrivningar av dessa kan återfinnas nedan. Teknisk Rapport

38 Autonom spaning med quadcopter 32 Tabell 4: Signalflöde för uppdragsplaneringmodul Insignal Utsignal Uppdragstyp Geografiska koordinater för områden, linje eller sökpunkt beroende på uppdrag. Flyghöjd, h Radie, r (runt koordinat) Trajektoria Trajektorialängd [m] Estimerad uppdragstid [min] Hastighets-referensvektor tillhörande trajektorian Tabell 5: Konstanter använda av diverse funktioner i uppdragsplaneringsmodulen. Konstant Funktion Beskrivning Φ - bildvinkel D - sensorstorlek f - brännvidd v 1,v 2 och v 3 Förbehandling Tidsestimering, Övriga beräkningar Kameraegenskaper Dessa hastigheter är de som lägger grund för hastighetsreferensvektorn och därmed tidsberäkningen ε RDP Avståndskonstant använd av RDPalgoritmen P Interpolering Antal punkter att interpolera fram mellan varje nodpar Jordradie Övriga beräkningar Används vid koordinattransformation TSP Ordet är en förkortning av Traveling salesman problem, även kallat handelseresandeproblem på svenska. Detta är ett välkänt problem inom optimeringsläran som enklast beskrivs via problematiken med att hitta den kortaste vägen genom ett givet antal städer med olika positioner. Ett problem vars komplexitet följer fakultetens funktion och därmed blir väldigt svårlöst för ett stort antal noder. LKH Lin-Kernighan heuristic, detta är i dagsläget den algoritm som löst det största handelsresandeproblemet någonsin av alla befintliga algoritmer, närmare bestämt en instans på noder [2]. RDP Ramer Douglas Peucker algorithm, en algoritm som kortfattat beskrivet minskar ner antalet onödiga punkter. Vilket i denna applikation innebär att punkter längs en raksträcka hos trajektorian elimineras. Enhetsavstånd Den minimala euklidiska sträckan mellan två nodpar (alltså två närliggande noder) i kostnadsmatrisen. 6.5 Modulbeskrivning Det finns många sätt att angripa areatäckningsproblem, den gemensamma nämnaren är att resultatet nästan alltid blir en kompromiss mellan beräkningskomplexitet och optimalitet. Via grundliga förstudier hittades LKH som ska ha en unik lösnings-heuristik som gör den väldigt snabb. Enligt specifikationerna bör den därför inte ha några problem med att lösa de aktuella problemen vars nodantal aldrig kommer överstiga cirka 2000 noder eftersom den möjliga avsökningsarealen begränsas av WIFI-anslutningens räckvidd (förutsatt att kameran har ett omfång på minst 4m 2 ). Teknisk Rapport

39 Autonom spaning med quadcopter 33 I fallet av uppdragstypen inom område formuleras problemet som ett handelsresandeproblem där noder fördelas ut över det området som ska täckas. Hur väl trajektorian utformas begränsas därför till kostnadsmatrisen som indirekt blir problemformuleringen, eftersom LKH utlovar en optimal lösning utifrån hur problemet formulerats (då detta problem aldrig kommer att uppnå en storlek där lösningen måste kompromissas). Fördelarna med detta blir även att de flesta kriterierna som förbjudna områden, områdesgränser och en strävan om trajektoriamönster kan implementeras som straff direkt i konstnadsmatrisen. Implementationen delas upp på olika funktioner som ansvarar för att lösa separata delar av huvuduppgiften att ta fram en trajektoria. Runt koordinat Vid uppdragstypen runt koordinat används enbart ett begränsat antal av de funktionerna som finns tillgängliga i modulen. Beräkningen blir förhållandevis enkel och kräver enbart en subfunktion som ritar ut den spiralformade trajektorian som därefter skickas direkt till interpoleringen utan vidare utvärdering, se flödesschema i figur 16 och de använda funktionaliteterna presenterade i tabell 6. Tabell 6: Funktionsanropsföljd för uppdragstypen runt koordinat. Ordningsföljd Funktionalitet 1 Förbehandling 2 Archimedes sprial 3 Interpolering 4 Sista kontroll 5 Tidsberäkning 6 Övriga beräkningar Linjesökning Vid linjesökning används enbart ett begränsat antal av de funktioner som finns tillgängliga i modulen. Informationen skickas direkt till interpoleringen, se flödesschema i figur 16 och de använda funktionaliteterna presenterade i tabell 7. Tabell 7: Funktionsanropsföljd för uppdragstypen linjesökning. Ordningsföljd Funktionalitet 1 Förbehandling 2 Interpolering 3 Sista kontroll 4 Tidsberäkning 5 Övriga beräkningar Areatäckning Vid areatäckning används samtliga funktioner i en ordning enligt flödesschemat i figur 16 och de använda funktionaliteterna presenterade i tabell 8. Beräkningen innehåller även en modell för utvärdering som fattar beslut om den erhållna trajektorians kvalitet. 6.6 Algoritmbeskrivning Under detta avsnitt kommer de använda algoritmerna att beskrivas och utvärderas beträffande deras funktionalitet för problemlösningen. Teknisk Rapport

40 Autonom spaning med quadcopter 34 Tabell 8: Funktionsanropsföljd för uppdragstypen inom område. Ordningsföljd Funktionalitet 1 Förbehandling 2 Polygrid 3 Kostnadsmatris 4 TSP-Algoritm 5 Interpolering 6 Sista kontroll 7 Tidsberäkning 8 Utvärdering 9 Övriga beräkningar Förbehandling Eftersom modulen har som uppgift att lösa olika typer av problem (givna av uppdragstypen) måste inobjektet tolkas för att rätt funktioner ska anropas. Detta hanteras av förbehandlingen som även genomför en del enklare beräkningar beträffande kameran och dess täckningsyta. Vid areatäckning delas även informationen upp i submodulspecifika strukturer, där koordinater för förbjudna områden och områdeskoordinaterna läggs var för sig. Beräkning av kamerans täckningsyta För att kunna planera trajektorians utformning måste algoritmen ha kännedom av den täckningsyta som kameran har gentemot underlaget. Denna fås med hjälp av höjden h och kamerans egenskaper i form av bildvinkel som beräknas med ekvation 14. Det tillsammans med ekvation 15 ger den slutgiltiga täckningsytan under förutsättning att bilden är fyrkantig och att regulatorn kan hålla plattformen på en konstant höjd över underlaget. Eftersom plattformens aktuella kamera inte ger upphov till en fyrkantig bild har den minsta bildvinkeln använts, därav fås ett minimum för den täckta ytan. Φ är bildvinkeln, D är sensorstorleken, f är brännvidden, A är kamerans täckningsyta och L är längden hos en av sidorna hos den kvadraten som utgör kameras tillrättalagda täckningsyta. ( ) D/2 Φ = 2 arctan f (14) A = h tan( Φ 2 ) (15) L = A (16) Planerandet av trajektorian för moderna inom område och runt koordinat förlitar sig till stor del på kamerans täckningsyta. Då denna kan komma att påverkas väldigt mycket under flygning till följd av vinkelutslag, varierande höjd och yaw-vinkel så multipliceras täckningsytan med en säkerhetsfaktor. Genom detta kan den resulterande trajektorian erhålla ett överlapp som reducerar risken för dålig täckningsgrad vid färdigställt uppdrag. Teknisk Rapport

41 Autonom spaning med quadcopter Polygrid Denna funktion har som uppgift att gridda upp noder inom de områden som ska avsökas. Det har lösts genom att analysera de koordinater som definierar det/de områden som ska sökas av. Utifrån dessa bestäms en övre respektive undre områdesgräns för latituden och longituden. Dessa bildar ett rektangulärt område där noder placeras ut efter ett förutbestämt antal per areaenhet som beräknas med hjälp av kamerans täckningsyta. I ett nästa steg skapas en eller flera polygoner utifrån de koordinaterna som definierar hörnen på områdesgränserna beroende på hur många områden som ska sökas av. Därefter tas alla noder som ligger utanför dessa samt de som ligger inom ett förbjudet område bort. Resultatet är en lång array innehållandes noder Kostnadsmatris I konstnadsmatrisen hanteras majoriteten av problemspecifieringen då trajektorian formas utifrån de kostnaderna som är definierade här. Därav kommer både förbjudna områden och uppföljningen av att områdesgränser respekteras att hanteras i definitionen av denna. Därtill kan även ett önskat flygmönster implementeras vilket gör att antalet svängar kan fås med i problemspecifieringen. Flygmönstret kan sedan itereras för att minimera den slutgiltiga kostnadsfunktionen som är en estimering av tidsåtgång. Anledningen till att antalet svängar inte kan straffas dynamiskt i LKH är att detta skulle kräva att algoritmen i sådana fall hade behövt skapa en ny kostnadsmatris för varje nodutvärdering vilket skulle göra den mycket tidsineffektiv. Vid byggandet av en kostnadsmatris skapas i ett första steg en grundmatris C utifrån de euklidiska avstånden mellan noderna. Dessa beräknas som en energi för att eliminera problemet med negativa avstånd till följd av numeriska koordinater (ekvation 18). Operationerna är elementvisa och den skapade matrisen innehåller element vars värde motsvarar kostnaden mellan två noder. x 1... x n X =..... x 1... x n Y = y 1... y n..... y 1... y n x = 1,..., n y = 1,..., n n = Antal noder (17) C = (X X T ) 2 + (Y Y T ) 2 (18) I ett andra steg studeras trajektoriamönstret med tre olika alternativ. Antingen med en strävan efter att flyga lattitudinellt, longitudinellt eller korsvis. Detta implementeras genom att minska straffet för de nodpar som ligger på samma lattitud, longitud eller korsvis. För att snabba upp algoritmen undersöks enbart noder i anslutning till varandra samt att enbart den övre triangeln behandlas, eftersom denna kan speglas då kostnadsmatrisen är symmetrisk. Slutligen kollas områdesgränser. Här studeras en eventuell skärning mellan nodpar och förbjudna områden alternativt yttre gränser. En sådan skärning straffas additativt så att vägen mellan två områden ska få likvärdigt straff. I annat fall skulle algoritmen alltid sträva efter att sluta så nära ett annat område som möjligt även om detta i meningen av antalet svängar inte skulle vara optimalt. För att undersöka dessa eventuella skärningar utvärderas varje nodpar vars euklidiska avstånd är under 10 gånger ett enhetsavstånd, denna logik är en tidseffektivisering då större Teknisk Rapport

42 Autonom spaning med quadcopter 36 avstånd aldrig kommer användas i den slutgiltiga lösningen till följd av deras höga energi i ursprungsdefinieringen av matrisen. Varje nodpar bildar en rät linje, denna undersöks iterativt gentemot alla gränser där en skärning straffas. Detta illustreras i figur 17 där en otillåten skärning plottats med röda linjer. Figur 17: Området i grått är sökområdet, nodpar inom detta är sammanbundna med linjer. Det vita området i mitten är ett förbjudet område och alla nodpar som korsar detta har målats med röda linjer för att illustera att dessa ska straffas. Tillåtna nodkombinationer är plottade med gröna linjer. Observera att plotten enbart syftar till att visa på straff för korsning av gränser och noderna är inte utplacerade för att ge full täckningsgrad TSP-algoritm Problemet har som tidigare nämnts formulerats som ett TSP-problem, detta skulle kunna formuleras matematiskt som ett diskret linjärt problem enligt nedan. Det som kvarstår är att applicera en lämplig lösningsalgoritm. { 1, vägen går från nod i till nod j x i,j = (19) 0, annars Låt u i vara en artificiell variabel och c ij vara kostnaden för att ta sig från en nod i till en annan nod j där i, j = 1,..., n (n är antalet noder). Teknisk Rapport

43 Autonom spaning med quadcopter 37 min n n i=0 j i,j=0 c ij x ij (20a) 0 x ij 1 i, j = 1,..., n (20b) u i Z, i = 1..., n (20c) n x ij = 1 i = 1,..., n (20d) i=0,i j n j=0,j i x ij = 1 j = 1,..., n (20e) u i u j + nx ij n 1 1 i j n (20f) Den första likheten i ekvation 20 ser till att varje nod blir mål för enbart en annan nod och den andra likheten ser på motsvarande sätt till att denna enbart lämnas en gång. Därmed ges en försäkran om att varje nod blir besökt exakt en gång. Det sista villkoret ser till att varje nod blir besökt i en enda handelseresandetur så att inte flera subturer binder samman noderna. Med ett klokt val av u i är det möjligt att uppfylla samtliga villkor presenterade i ekvation 20. Med de utökade villkoren 21 till 23 går det slutligen att visa att den funna turen uppfyller samtliga krav. För att visa att den funna lösningen är fullständig och därmed innehåller samtliga noder samt att det inte existerar några subturer räcker det med att visa att den första noden finns med, eftersom en summering av alla x i,j = 1 annars skulle ge resultatet i ekvation 21 vilket motstrider bivillkor 20d och 20e. nk (n 1)k, (21) Slutligen måste existensen av den artificiell variabeln u i visas för att bivillkor 20f ska vara uppfyllt. Detta kan göras genom att förutsätta att turen börjar och slutar i samma nod 0 och sätta u i = t för de noder som är besökta i tidssteg t (i, t = 1, 2,..., n). Då gäller villkor 22 eftersom u i inte kan vara större än n samtidigt som u j inte kan vara mindre än 1 då villkoren alltid skulle vara uppfyllda om x i,j = 0. För x ij = 1 så gäller 23 som uppfyller villkoret. u i u j n 1 (22) u i u j + nx ij = (t) (t + 1) + n = n 1, (23) För att modulen ska hållas snabb har en heuristisk lösare använts, detta innebär att optimeringsproblemet löses diskret istället för kontinuerligt. Konsekvensen blir som tidigare nämnt en kompromiss mot optimalitet, gällande implementationen av LKH är detta dock något som ger sig tillkänna först hos problem av betydligt större omfattning och komplexitet än de som kommer att bli aktuella i detta sammanhang. Heuristiken är implementerad i den dynamiska optimeringen och har alltså inte med definitionen i ekvation 20 att göra. Förkastade algoritmer Under utvecklingen av uppdragsplaneringen har flera metoder studerats men förkastats Teknisk Rapport

44 Autonom spaning med quadcopter 38 på grund av otillräckliga resultat. I ett första försök användes linjär programmering med interpolation för att lösa problemet. Denna metod visade sig däremot vara otillräcklig beträffande tidsåtgången då den använder sig av en iterativ lösningsgång som går igenom samtliga lösningar. Detta blir ohållbart vid större problem då antalet lösningar ökar med fakultetens funktion. För att komma runt problemet testades en genetisk lösningsalgoritm ovanpå den linjära programmeringen. Detta fick ner beräkningstiden avsevärt men problemet kvarstod samtidigt som kompromissen mot optimalitet blev av för stor grad, vilket gav dåliga lösningar. En annan metod som också baserar sig på en genetisk lösningsalgorim är att dela upp sökområdet i subområden och ha en enklare variant för att rita ut trajektorian inom dessa, exempelvis med ett antal olika mönster. Slutligen testar den genetiska algoritmen olika konfigurationer tills den hittar en nära optimal lösning. Denna metod förkastades på grund av problematiken med att dela upp ett okänt område innehållandes förbjudna områden i subområden. Detta blir mycket svårt eftersom områdena kan anta väldigt många olika former. Logiken för denna beslutshirearki kan därför komma att bli ganska omfattande och skulle troligtvis kräva mycket testning för att nå tillfredställande resultat Runt koordinat Vid beräkning av trajektoria kring en koordinat har Archimedes spiral använts. Denna utgår ifrån centrumkoordinaten c och arbetar sig utåt med en konstant spannbredd S vilket ger trajektorian utmärkta egenskaper i form av försiktiga kurvor med full täckningsgrad. Spannbredden sätts till halva längden L hos en sida i den kvadranten som utgör kamerans täckningsyta, denna längd är beräknad i ekvation 16 och genom att stoppa in den i ekvation 25 fås den färdiga trajektorian T som en funktion av vinkeln θ runt centrumkoordinaten c. En illustration av en sådan trajektoria kan studeras i figur 18. Antalet rotationer N r kring centrumpunkten c ges av ekvation 24. N r = r S, S = L 2, N r = Antal rotationer (24) T (θ) = c + S 2π [θcos(θ) θsin(θ)], θ = [2π,..., 2πN r] (25) Teknisk Rapport

45 Autonom spaning med quadcopter 39 Figur 18: En spiral-trajektoria generad av uppdragsplaneringsmodulen i moden runt koordinat. Det gröna transparenta området är det som ska avsökas. Bilden är tagen direkt från GUI:t Interpolering Interpolationen sker med kubiska splines som binds samman med not-a-knot -ändvillkor. Detta innebär att tredjederivatan ska vara samma vid varje nod [1], dvs. förändringshastigheten för kurvan blir densamma och därmed får trajektorian både mjuka kurvor och ett högre antal punkter vilket är nödvändigt för att matcha samplingsfrekvensen hos systemet. Med en iterativ implementation kan ett tredjegradspolynom motsvarande 26 ställas upp för varje nodpar utifrån vilket punkter kan placeras med valfri täthet. S = { bx + m, a 0 x 3 + a 1 x 2 + a 2 x + a 3, koord. i linje kurvor (26) Här är S ett polynom som tas fram för varje nodpar, därav erhålls n 1 polynom som beskriver den slutgiltiga trajektorian. Interpolationen är nödvändig för alla typer av uppdrag och lägger bana för den senare beskriva estimeringen av tidsåtgång. Eftersom noderna är utplacerade så att hela området täcks under förutsättning att varje nod passeras minst en gång kommer interpolationen inte att påverka täckningsgraden. Detta eftersom funktionen S(t) är uppdelad på intervall mellan varje nodpar i trajektorian. Intervallens olika funktioner är därav baserade på not-a-knot -ändvillkoren och nodernas position, varpå deras existens i den slutgiltiga trajektorian garanteras Sista kontroll I en sista kontroll utvärderas den erhållna trajektorian gentemot förbjudna områden. Algoritmen för detta är uppbyggd så att den går igenom hela trajektorian från början Teknisk Rapport

46 Autonom spaning med quadcopter 40 till slut och letar efter skärningar med ett förbjudet område. Hittas en sådan utvärderas den snabbaste vägen mellan skärningspunkterna runt det förbjudna området genom att sortera polygonen som utgör detta i både medsols och motsols ordning. Därefter väljs den kortaste vägen Tidsberäkning/hastighetsreferensvekotor Den estimerade tiden tjänar syftet att ge användaren en uppfattning om hurvida det är möjligt att genomföra ett uppdrag med den aktuella batterikapaciteten. Denna implementation grundar sig på antagandet om att batterikapaciteten avtar linjärt eftersom plattformen håller en konstant höjd och yttre påfrestningar samt de vinkelutslag som krävs för att genera förflyttningar är av försumbar betydelse. Tiden för uppdragen är en skattning som baserar sig på trajektorians distans och den hastigheten som plattformen förväntas kunna hålla beroende på dess utformning. Därav måste hastighetsreferensvektorn bestämmas först, denna baserar sig på tre olika referenshastigheter beroende på trajektorians utformning, den första är för skarpa kurvor, den andra för svaga kurvor och den tredje för raksträckor, se ekvation 27, där parametern T är tröskelvärden. En illustration av en bestämd hastighetsvektor kan studeras i figur 19 där den plottade trajektorian har olika färger beroende på hastighetsreferensvektorns värde. I ekvation 27 visas en teoretisk beskrivning av implementationen. Här är L längden mellan två punkter i trajektorian som beror av det lokala polynomets gradient S. Denna utvärdering sker efter att RDP-algoritmen har körts på trajektorian och därav bestäms hastigheterna utifrån de punktavstånden som har modifierats med RDP. Figur 19: I figuren visas ett planerat uppdrag i moden inom område. De gråa områdena är de som ska avsökas och de röda är förbjudna områden. Hastighetsreferensvektorn är färgkodad med gul motsvarandes v 3, brun v 2 och blå v 1. Teknisk Rapport

47 Autonom spaning med quadcopter 41 Med denna implementation kan tiden integreras fram enligt ekvation 28. Den beräknade tiden syftar enbart till att ge en uppfattning om hur vida det är möjligt att genomföra det planerade uppdraget, beslutet lämnas således åt operatören. v 1, om L( S(k)) T 1 v(k) = v 2, om T 1 L( S(k)) T 2 k (punkter i trajektoria) (27) v 3, om L( S(k)) T 2 t = n k=1 s k v k, k (punkter i trajektoria) (28) För att avgöra vilken av de tre hastigheterna som ska sättas utvärderas trajektorians utformning. Detta görs med hjälp av RDP som kort sagt minskar ner antalet punkter där de inte tillför något. Den erhållna trajektorian kommer med andra ord att få en lägre upplösning av punkter längsmed raka partier. Därefter kan den erhållna trajektorians punktavstånd användas för att sätta hastighet. Tröskelvärdena T är således punktavstånd. Algoritmens resultat illustreras i figur 20 där man i den högra plotten kan se resultatet av RDP-algoritmen på referenstrajektorian som är ritad till vänster. Här framgår det tydligt att det väsentliga utseendet hos trajektorian fångas samtidigt som en mängd onödiga punkter har eliminerats. RDP-algoritmen går rekursivt igenom varje punkt i trajektorian. För varje punkt ritar den upp en linje gentemot de framförliggande och går sedan igenom de ortogonala euklidiska avstånden till orginaltrajektorian. Om avståndet visar sig vara lägre än konstanten ε elimineras punkterna däremellan. Pseudokoden finns att studera kodutdrag 3. (a) Referenstrajektoria (b) RDP trajektorian Figur 20: I den högra plotten ses resultatet av RDP-algoritmen på referenstrajektorian till vänster. Kodutsnitt 3: RDP beskriven i pseudokod. function DouglasPeucker ( PointList [], epsilon ) // Find the point with the maximum distance dmax = 0 index = 0 end = length ( PointList ) for i = 2 to ( end - 1) { d = shortestdistancetosegment ( PointList [i], Line ( PointList [1], PointList [ end ])) if ( d > dmax ) { Teknisk Rapport

48 Autonom spaning med quadcopter 42 end index = i dmax = d } } // If max distance is greater than epsilon, recursively simplify if ( dmax > epsilon ) { // Recursive call recresults1 [] = DouglasPeucker ( PointList [1... index ], epsilon ) recresults2 [] = DouglasPeucker ( PointList [ index... end ], epsilon ) // Build the result list ResultList [] = { recresults1 [1... end -1] recresults2 [1... end ]} } else { ResultList [] = { PointList [1], PointList [ end ]} } // Return the result return ResultList [] Övriga beräkningar Utöver den beräknade trajektorian har modulen även som uppgift att ta fram en del data som är hårt knuten till denna. Dessa är estimerad tidsåtgång (som beräknas i en separat funktion), en hastighetsreferensvekor (beräknas i samband med tidsestimeringen), trajektorialängd och den täckningsarean som fås med den beräknade trajektorian. Trajektorialängd Trajektorians längd approximeras som en summering av varje trajektoriasegments längd. Detta sker innan RDP-algoritmen körts varpå längdsegmenten blir väldigt små tack vare den omfattande interpolationen. Detta medför att approximationen blir väldigt god. Avvikelser hos jordradien till följd av jorden inkonsekventa elliptiska form blir av försumbar betydelse. Täckningsarea Täckningsarean beräknas på olika sätt beroende på vilken uppdragstyp som gäller. För moden inom område och runt koordinat beräknas arean utifrån det området som är täckt med klassisk trigometri. För moden längs linje så multipliceras kameras bildbredd L med trajektorians längd. 6.7 Resultat Den erhållna trajektorian uppfyller i samtliga fall de kraven som är formulerade för uppdragsplaneringsmodulen. Trajektorian går aldrig in i ett förbjudet område och den ger full täckningsgrad under de förutsättningar som har gjorts gällande plattformens vinkelutslag vid passering av varje nod. Det vill säga att den befinner sig i ett plant läge så att kameran ger den täckningsytan som har antagits vid beräkningarna. Även om optimalitet är något som är väldigt svårt att mäta vid problem av större omfattning kan det i alla fall fastslås att algoritmen genomför en utvärdering som resulterar i en närmare optimal trajektoria än att bara söka av området fram och tillbaks, vilket skulle vara det intuitiva alternativet. 6.8 Diskussion/Vidareutveckling Det största haken med modulen i dagsläget är att den använder sig av metoder för att interpolera fram väldigt täta punkter som var tänkta att användas av en mer avance- Teknisk Rapport

49 Autonom spaning med quadcopter 43 rad regulator som då kunde utnyttja dessa tillsammans med den generade hastighetsreferensvektorn. Därav görs en hel del beräkningar som i slutskedet inte används och som i viss mån har kommenterats bort. Denna implementation skulle bli högaktuell vid en vidareutveckling av projektet då dessa förmodligen kan komma till väldigt stor användning. Eftersom plattformen inte heller hann testas tillräckligt mycket finns det fortfarande en del hårdkodade parametrar som kräver en del finjustering. Exempel på dessa är hastighetesreferenserna v 1, v 2 och v 3. Samtliga parametrar finns beskrivna mer utförligt i Matlab skriptet. En annan implementation som skulle kunna vidareutvecklas är den sista kontrollen av förbjudna områden. Här kontrolleras som tidigare beskrivet en skärning utifrån vilken algoritmen bestämmer den kortaste vägen runt det aktuella förbjudna området. Problematiken med detta är att algoritmen låser sig till att åka runt området längs gränsen. I framtiden skulle denna algoritm kunna vidareutvecklas så att trajektorian tilläts ta kortare vägar och inte behövde hålla sig strikt till gränslinjerna. Teknisk Rapport

50 Autonom spaning med quadcopter 44 7 Uppdragsföljning Uppdragsföljningsmodulens uppgift är att generera styrkommandon till plattformen utifrån erhållen sensordata och referenstrajektoria. Dessa kommandon består av fyra stycken variabler för att reglera lateral hastighet, hastighet framåt, yaw-vinkel och höjd, utöver dessa variabler finns även ett kommando för start och landning av plattformen. Som insignal till systemet erhålls sensordata i form av hastighet framåt, hastighet i lateral riktning, höjd, yaw-vinkel och GPS-position. De referensdata som krävs som indata är GPS-koordinater, höjd, yaw-vinkel och hastighet framåt. Alla dessa kommunikationsvariabler finns presenterade i tabell 9. Insignal Utsignal Sensordata Position, [X gps, Y gps ] Hastighet, [ẋ, ẏ] Höjd, [h] Yaw-vinkel, [ψ] Referensdata Referensposition, [X gps,ref, Y gps,ref ] Referenshöjd, [h ref ] Referensyaw, [ψ ref ] Referenshastighet framåt, [ẏ ref ] Styrsignaler Pitch (styr ẏ), [θ ref ] Roll (styr ẋ), [φ ref ] Yawrate, [ ψ ref ] Vertikal hastighet, [ḣ] Starta/landa Tabell 9: Signalflöde uppdragsföljningsmodulen 7.1 Övergripande design Uppdragsföljningsmodulen är uppdelad i tre undermoduler: positionsestimering, förfiltrering av referensdata och regulator. Alla dessa undermoduler kan kommunicera med varandra och är samplade i samma frekvens. Positionsestimeringen utförs med hjälp av ett Kalmanfilter som skattar plattformens position och hastighet utifrån sensordatat. Förfiltreringen av referensdatat undersöker om plattformen har nått sin referenskoordinat och ställer i så fall ut en ny uppdaterad sådan. Slutligen använder regulatorn den samlade informationen för att beräkna plattformens styrsignaler och skickar vidare dessa till huvudbussen. Ett övergripande flödesschema över hur kommunikationen sköts och vilka signalflöden som finns mellan undermodulerna går att betrakta i figur 21. En kort punktlista över modulens arbetsgång är presenterad nedan, arbetsgången är uppdelad i initialisering och samplingsgång: Teknisk Rapport

51 Autonom spaning med quadcopter 45 Initialisering 1. Läs in plattformens GPS position och fixera N-frame. 2. Läs in uppdragets referensdata och initiera filtrering av referensdata. 3. Se till att plattformen har startat. 4. Initiera filtrering av sensordata. 5. Initiera regulator. Samplingsgång 1. Uppdatera till senaste sensordata. 2. Filtrera sensordata. 3. Filtrera referensdata. 4. Beräkna styrsignaler. 5. Skicka styrsignaler till huvudbuss. Figur 21: Överskådlig bild över signalflöden i uppdragsföljningsmodulen och hur de tre ingående delmodulerna samverkar. 7.2 Språk och program Modulen är skriven i programspråket Java 8. För hantering av matriser har bibloteket EJML (efficient Java matrix library) använts, se [11]. 7.3 Positionsestimering Positionsestimeringen utförs med hjälp utav ett diskret Kalmanfilter och bygger på teori hämtad från kurslitteratur i sensorfusion [3]. Kalmanfiltret använder den samlade informationen från en rörelsemodell, bias estimering i sensorer, hastighets-mätdata och GPSmätdata. Eftersom hastigheten har en samplingsfrekvens som är cirka 200 ggr. snabbare än GPS:ens används den för att förbättra positioneringen mellan GPS-mätpunkterna. Rörelsemodellen som används är av typen konstant hastighetsmodell och beskriver plattformens rörelser på ett bra sätt om samplingsfrekvensen är tillräckligt hög. Rörelsemodellen är endast definierad i N-frame varpå data givet i Body-frame och WGS-84 måste transformeras innan det kan används i filtret. Modellen som kalmanfiltret bygger på i X-led finns beskrivet i ekvation 29, då modellen har samma utseende i Y-led är detta inte presenterat. Modellen har tillstånden X, Ẋ och b som motsvarar position och hastighet i X-led i N-frame, samt bias i hastighetsmätningen. y vel,x beräknas från hastighetsmätningen definierad i B-frame, ẋ, transformerad enligt ekvation 30. y GP S,X är gps-positionsmätningen, X gps transformerad från WGS-84 till N-frame enligt ekvation 31. Teknisk Rapport

52 Autonom spaning med quadcopter 46 X t+t s Ẋ t+t s = 1 T s 0 X t Ẋ t y vel,x b t+t s b t 0 t = [ ] GP S,X y R = E[e 2 t ] Q = E[v 2 t ] X t Ẋ t b t + e t t + T s v t Funktion som transformerar hastigheter i B-frame till N-frame. [Ẋ ] [ ] [ẋ ] cos(ψ) sin(ψ) = Ẏ sin(ψ) cos(ψ) ẏ T s (29a) (29b) (29c) (29d) (30) Funktion som transformerar WGS-84 till N-frame. lat init : Latitud där N-frame fixeras. lon init : Longitud där N-frame fixeras. r = 6371 : Jordradie π Lat = (Y gps lat init ) (31a) 180 π Lon = (X gps lon init ) (31b) 180 Y = r Lat 1000 (31c) ( ) (Ygps + lat init ) π X = r Lon cos 1000 (31d) Likt kalmanfiltret i bildbehandlingen är filtret implementerat med pseudoinvers och ekvation 32 ser till att kovariansmatrisen hålls symmetriskt vid numeriska beräkningar. P k = 1 2 (P k + P T k ) (32) Filtret är konstruerat på ett sådant sätt att hastighetsmätningar är satta som insignaler till systemet och dessa är modellerade med en bias. GPS-mätpunkter är satta som mätsignal och uppdateras således med en mätuppdatering. En tidsuppdatering av tillstånden sker enligt en samplingsfrekvens på 20Hz och mätuppdatering sker när ny GPS data har erhållits, vilket i praktiken är runt 2-4Hz. Både mät och tidsuppdatering sker på samma sätt som beskrivet för kalmanfiltret i bildbehandlingen under kapitel Förenklingar och felkällor Ett kalmanfilter kräver att allt brus ska vara okorrelerat och vitt för att fungera optimalt. I denna implementation har vissa förenklingar gjorts och att allt brus skulle vara okorrelerat har till viss del bortsetts ifrån. Bland annat sker med största sannolikhet en intern skattning av hastighet på plattformen vilket resulterar i att bruset på denna mätsignal inte är helt okorrelerat. I implementeringen är det något som inte tagits i beaktning vilket bidrar till att det i praktiken inte kan garanteras att det blir ett optimalt filter. Inverkan Teknisk Rapport

53 Autonom spaning med quadcopter 47 har dock efter utförliga tester ansetts vara små och därför ger förenklingen ingen direkt komplikation för skattningen som helhet. Ytterligare felkällor ligger i modellen som har använts då det är en förenkling av verkligheten. Modellen kan dock anses vara en godtagbar förenkling om samplingsfrekvensen är tillräckligt hög. En avvägning har gjorts och samplingsfrekvensen har därför satts till ca 20Hz för att till viss del matcha samplingsfrekvensen på mätsignaler för hastigheter men även så att systemet inte ska vara alltför beräkningstungt Resultat och diskussion: Kalmanfilter Kalmanfiltret bidrog till en klar förbättring av positionsestimeringen och ett tydligt exempel på detta visas i figur 22. Samplingsintervallet hos pålitlig positionsskattning ökade från ca 2-4 Hz till 20 Hz vilket ger klart bättre underlag för reglering av plattformen. Vid flygning i konstant hastighet i en riktning blir positionsskattningen väldigt bra och några direkta avvikelser och outliers syns inte till. Vid flygning i svängar syns dock att hastighetsdatat inte helt räcker till för att skatta position en längre tid än några tiondels sekunder. Därefter behövs en mätuppdatering från GPS:en för att korrigera banan. I framtiden finns potential för vidareutveckling av positionsskattningen. Om en modell över plattformen tas fram och rådata från accelerometrar, gyron, barometer och sonar används skulle ett EKF (extended Kalmanfilter) kunna konstrueras. På så sätt skulle positionsskattningen, vinklar, hastigheter och höjd bli noggrannare och snabbare. Troligtvis skulle även positionsskattning utan GPS ge bättre data än nuvarande konstruktion, exempelvis om GPS:en skulle tappa fix under någon sekund skulle uppdraget kunna fortsätta att köras. Eftersom regulatorn är högst beroende av dessa positionsskattningar och dess kvalité, skulle ett EKF också ge en snabbare och stabilare referensföljning. I dagsläget finns det också stora problem med den interna höjdskattningen ty den har en hög osäkerhet. Det är således något som behöver ses över om ett EKF inte skulle konstrueras. Teknisk Rapport

54 Autonom spaning med quadcopter Interpolerad väg Tidsuppdatering Mätuppdatering [m] [m] [m] [m] Figur 22: Överskådlig bild över hur kalmanfiltret presterar i en positionsestimering av en flygning. Den övre figuren visar positionsskattning då plattformen flyger i en konstant hastighet och den undre under en sväng med konstant hastighet framåt och yaw-vinkeln ändras. Röda markörer är tidsuppdatering och blå mätuppdatering. 7.4 Referensväljare Referensväljarens uppgift är att filtrera all referensdata som finns och endast skicka det för tillfället intressanta datat till regulatorn. Mer specifikt undersöker den om plattformens position är nära nog för att uppdatera till nya referenspunkter och beräknar då aktuell referens för yaw-vinkel. Eftersom referensen i yaw-vinkel hanteras olika beroende på vilken regulatormod som körs beräknas den på två olika sätt. För moden konstant yaw-vinkel beräknas vinkeln endast en gång då referensdatat uppdateras och för moden konstant hastighet beräknas den för varje sampel. Referensväljarens arbetssätt finns beskrivet i figur Resultat och diskussion: Referensväljare Referensväljaren har tre parametrar som kan användas för att trimma uppdragsföljningen. Dessa är den tillåtna positionsavvikelsen i höjd, X- och Y-led. På grund utav tveksam mätdata gällande höjdsensorer tas för närvarande ingen hänsyn till avvikelser i höjdled innan referensen kan uppdateras. Positionsavvikelser i X- och Y-led kan däremot trimmas för att få en jämnare och snabbare referensföljning (högre tillåtna positionsavvikelser) gentemot att erhålla en sämre täckningsgrad. Därav bör valet av parametrarna vara en väl undersökt avvägning mellan dessa egenskaper. I ett fortsättningsprojekt är dessa parametrars värde något som skulle kunna undersökas närmare. Något som även skulle kunna undersökas är möjligheten till att ta hänsyn till nästkommande referenspunkter och vad som bör hända när plattformen missar en referenspunkt. Är det värt att åka tillbaka för Teknisk Rapport

55 Autonom spaning med quadcopter 49 att se till att alla referenspunkter blir besökta gentemot att uppdraget tar längre tid och får ett sämre flyt? Figur 23: En schematisk figur över hur referensväljaren arbetar under en sampling. 7.5 Regulatorstrukur Regulatormodulen är uppbyggd av två olika styrmoder. Den första moden använder sig utav konstant hastighet framåt och reglerar position med hjälp utav yaw-vinkeln. Plattformen regleras även till att ha hastigheten noll i lateral riktning. Den är därför benämnd som Styrmod: konstant hastighet. Styrmoden är menad till att användas för att få ett bra flyt i flygningen i och med att den har en konstant hastighet framåt. Den andra styrmoden reglerar istället position med hälp utav hastighet i både lateral- och riktning framåt. Yaw-vinkelns referens uppdateras endast då en ny referenskoordinat erhålls. Det betyder att plattformen riktar in sig en gång och därefter reglerar enbart med hastigheter i olika led tills den når referenspositionen. Denna styrmod är därför benämnd som Styrmod: konstant yaw-vinkel. Styrmoden enligt denna metod erhåller inte lika bra flyt i och med att hastigheten på plattformen hela tiden ändras beroende på hur nära referenspositionen den befinner sig. En annan nackdel är även att för referenskoordinater som ligger väldigt nära varandra blir regleringen långsam om inte en väldigt hög förstärkning väljs på regulatorerna. Fördelen är att den är mycket mindre känslig för yttre störningar såsom vind och fel i positionsskattning. För att yaw-vinkelreglering ska fungera på ett tillfredställande sätt för båda styrmoderna, dvs. vinkeln bör regleras åt det håll som går snabbast för att nå referensvinkeln, finns en funktion för detta implementerad. Den fungerar på sådant sätt att funktionen beräknar vilket håll vinkeln bör regleras åt för att ta den kortaste vägen och bestämmer om förstärkningen i p-regulatorn ska vara positiv eller negativ. På så sätt görs regulatorn en kort period instabil för att styra vinkeln över 180 från norr. I kodutdragen nedan finns funktionen beskriven i pseudokod. Funktionen tar in en referens i yaw-riktning, nuvarande yaw-riktning och regulatorförstärkningen K Y aw. K Y aw returneras sedan igen, antingen med samma eller motsatt tecken, beroende på vilket håll regulatorn bör styra yaw-vinkeln. Teknisk Rapport

56 Autonom spaning med quadcopter 50 Kodutsnitt 4: Hur riktning för yaw-vinkelreglering väljs, beskriven i pseudokod. function SelectYawDirection ( Yawreference, Yaw, KYaw ) direction = ( KYaw >= 0) if ( Yawref - Yaw > (2* PI - Yawref - Yaw )){ if ( direction ){ KYaw = -KYaw ; } } else { KYaw = KYaw ; } return KYaw ; } end Styrmod: konstant yaw-vinkel Styrmoden konstant yaw-vinkel består utav fem stycken p-regulatorer som till viss del ligger parallellt och till viss del i serie. En överskådlig bild på hur nätverket av regulatorer ser ut kan betraktas i figur 24. På grund utav att positionen styrs med hjälp utav hastigheter erhålls en integrerande del automatiskt och därför fås heller inget stationärt fel även då endast p-regulatorer används. På samma sätt erhålls heller inget stationärt fel i yaw-vinkelregleringen ty styrsignalerna är derivatan av yaw-vinkeln. Därav ger regulatorn även möjlighet till att hovra över en viss koordinat om så önskas. Figur 24: Regulatorskretsen uppdelad i alla delblock för att åskådliggöra regulatorstrukturen. Teknisk Rapport

57 Autonom spaning med quadcopter Styrmod: konstant hastighet Regulatorn för styrmoden konstant hastighet är konstruerad framförallt för att få ett bra flyt i flygningen utan stora förändringar i hastigheter och vinklar. Regulatorn består utav en snarlik uppsättning regulatorer som vid reglering med konstant yaw-vinkel. Den största skillnaden är att hastighetsregulatorerna ej har ett positionsfel som insignal utan istället en referenshastighet. En överskådlig bild på regulatorstrukturen kan betraktas i figur 25. Hastigheten framåt väljs normalt i intervallet 0-2 m/s och 0 i lateral ledd. Positionen regleras istället genom att yaw-vinkeln alltid har som referens att vara riktad mot nästa referenskoordinat. Således försöker regulatorn till viss del få plattformen att likna rörelser från ett flygplan. Regulatorn består av två stycken p-regulatorer för att styra höjd och yaw-vinkel. Hastighetsregulatorerna har däremot utökats till PI-regulatorer för att erhålla ett stationärt fel som går mot noll. Figur 25: Regulatorskretsen uppdelad i alla delblock för att åskådliggöra regulatorstrukturen. 7.6 Resultat och diskussion: Regulator De två olika regulatorerna har lite olika angreppssätt gällande hur plattformens position styrs. På grund utav detta erhålls även olika beteenden vid flygning. Styrmoden konstant yaw-reglering presterade med en stabil och säker flygning men något långsam. Regulatorn gjorde även att plattformen fick ett något ryckigt beetende då hastigheterna ej hålls konstant under flygning. Regulatorn har även vissa problem vid stark vind då den inte innehåller någon integrerande del. Stark vind kan således få plattformen att stanna helt i luften eller gå baklänges. En lösning på detta skulle kunna vara att utvidga till PIregulatorer, hänsyn måste då tas till den integratoruppvridning som kommer att uppstå på grund av mättningar i styrsingalerna. Styrmoden konstant hastighetsreglering har istället vissa problem med instabilitet, under normal drift erhålls dock en mycket jämnare flygning. Vid för stora yttre störningar såsom vind eller mätfel kan plattformen missa en referenskoordinat och hamnar då i en instabil spinn runt koordinaten. Det resulterar i att uppdraget måste avbrytas och/eller manuell styrning måste initieras. Instabiliteten beror med största sannolikhet på de Teknisk Rapport