Teknisk rapport Följning av djur Kolmården djurpark

Transkript

1 Version 1.0 Projektgrupp: Tar-Get Status Granskad GM, JH Godkänd Beställare

2 PROJEKTIDENTITET 2017/HT, Linköpings Universitet, ISY Gruppdeltagare Namn Ansvar Telefon E-post Daniel Arnström Mjukvaruansvarig Adam Bergenhem Integrationsansvarig Joakim Ekström Hårdvaruansvarig Tim Fornell Designansvarig Dokumentansvarig Henrik Lillberg Projektledare Gustav Magnusson Leveransansvarig Peter Mrad Testansvarig Johan Svensson Informationsansvarig E-postlista för hela gruppen: Hemsida: Beställare: Christian A. Naesseth, Linköpings Universitet Telefon: , Mail: Kund: Gustaf Hendeby, Linköpings Universitet Telefon: , Mail: Kursansvarig: Daniel Axehill, Linköping Universitet Telefon: , Mail: Handledare: Fredrik Ljungberg, Linköpings Universitet Mail:

3 Innehåll Dokumenthistorik 1 1 Inledning Parter Projektets bakgrund Syfte och mål Kortsiktiga mål Långsiktiga mål Användning Definitioner Översikt Hårdvara Mjukvara Koordinatsystem Kommunikation Implementering Topics Services GUI Kommunikation mellan ROS och Qt Gränssnitt Vidareutveckling Bildbehandling Övergripande design Terminologi Förbehandling Detektera mål Markering av mål Omvandling av koordinater Konvertering av bildkoordinater Konvertering till lokalt koordinatsystem Gränssnitt Resultat Vidareutveckling Positionering DJI Matrice AR Drone Gränssnitt Vidareutveckling

4 7 Målföljning Övergripande design Rörelsemodell Tidsuppdatering Associering Avståndsberäkning Nearest Neighbor Mätuppdatering Hantering av inkommande detektioner Gränssnitt Resultat Vidareutveckling Uppdragsplanering Övergripande design Uppdrag Nodplacering Kostnadsmatris TSP Uppdrag Uppdrag Gränssnitt Resultat Uppdrag Uppdrag Uppdrag Vidareutveckling Simulering Plattform Säkerhet 46

5 1 Dokumenthistorik Version Datum Utförda förändringar Utförda av Granskad Första utkast. Projektgruppen HL, TF Kompletterade efter kommentarer Projektgruppen HL, JE från handleda- ren Kompletterade efter kommentarer AB, DA, TF JH, GM från handleda- ren.

6 2 1 Inledning Det här dokumentet är den tekniska dokumentationen för projektet Följning av djur i Kolmården djurpark. Dokumentet är ett examinationsmoment i kursen Reglerteknisk projektkurs som ges av ISY vid Linköpings universitet under höstterminen Den tekniska dokumentationen beskriver systemet och de övergripande lösningarna som har implementerats. 1.1 Parter Projektet innehåller följande parter Kund: Gustaf Hendeby, ISY Beställare: Christian Andersson Naesseth, ISY Handledare: Fredrik Ljungberg, ISY Examinator: Daniel Axehill, ISY Projektgruppens medlemmar 1.2 Projektets bakgrund Bakgrund till projektet var att hjälpa parkvakter i Ngulia, Kenya, att skydda noshörningar i dess noshörningsreservat. Med hjälp av drönare ska parkvakterna kunna få bättre förmåga att upptäcka inkräktare och ge en djupare förståelse för hur många djur reservatet innehåller samt djurens rörlsemönster. För att på ett mer lättillgängligt sätt kunna testa tekniken finns ett samarbete med Kolmården. 1.3 Syfte och mål Syftet med projektet var att projektmedlemmarna skulle tillämpa tidigare inlärd kunskap inom området och använda sig av den praktiskt. Dessutom har arbetet utförts enligt projektmodellen LIPS för att få ett tillvägagångsätt som är snarlikt arbetslivet. Projektets mål delades in i kortsiktiga samt långsiktiga Kortsiktiga mål Målet med projektet var att bygga ett system som kan detektera och följa djur. Systemet består av en drönare utrustad med kamera, GPS-mottagare och tillhörande mjukvara Långsiktiga mål Ett långsiktigt mål är att utveckla en plattform som kan övervaka ett större område autonomt från luften. I första hand kan systemet tänkas användas för demonstration av tekniken på Kolmården, och på längre sikt användas för att hjälpa parkvakter i Kenya.

7 3 1.4 Användning Slutprodukten kan söka av ett område och identifiera mål i kombination med att följa flera mål samtidigt. Produkten kan användas som en grund för vidareutveckling och möjligen till slut användas i Kenya för att autonomt övervaka noshörningsreservat. 1.5 Definitioner En beskrivning av de begrepp som används i dokumentationen visas nedan. ROS: Robot Operating System. Plattform: Quadcopter med tillhörande hårdvara. Objekt: Allt som detekteras i bildbehandlingen såsom mål, brus och störningar. Mål: Objekt som försöker lokaliseras av systemet. Produkt: Quadquoptern med tillhörande mjukvara som körs på en dator. Gazebo: Simuleringsverktyg.

8 4 2 Översikt Produkten utgörs av en drönare som, både manuellt och autonomt, kan söka av ett valt område och identifiera mål. Utöver detta finns även målföljning tillgängligt. Både stillastående enkla mål och rörliga mål kan detekteras. Systemet i helhet ses i figur 1, där blocken motsvarar moduler eller komponenter. Pilarna visar hur kommunikation mellan dessa sker. Modulerna inkluderar GUI, uppdragsplanering, bildbehandling, målföljning samt positionering. Modulerna kommunicerar med varandra via ramverket ROS. I figur 2 visas ett översiktligt blockschema för systemet. Plattform Kamera GPS Drönare Trådlös kommunikation Dator ROS Master GUI Uppdragsplanering Bildbehandling Målföljning Positionering Kartor (Internet) Figur 1: Skiss över systemets uppbyggnad och ingående moduler.

9 5 Figur 2: Översiktligt blockschema för systemet 2.1 Hårdvara Hårdvaran som användes i projektet är en AR-drone 2.0 med en visuell kamera samt, projektmedlemmarnas egna datorer som användes för mjukvaran. 2.2 Mjukvara ROS används som ramverk för att köra mjukvaran från systemets delsystem. Varje delsystem bildar en ROS-nod som sedan kan köras oberoende av systemets andra noder. Systemets noder är: roscore: Har som uppgift att bland annat övervaka övriga noder. tracking: Har som uppgift att skatta positioner för detekterade objekt. image_processing: Har som uppgift att detektera objekt i den videoström som fås från kameran på drönaren. planning: Har som uppgift att planera hur plattformen ska röra sig för att utföra ett givet uppdrag optimalt. positioning: Ska se till att drönaren följer trajektorian som tas fram av uppdragsplaneringen. gui: Gör det möjligt för användaren att kommunicera med samt se information om plattformen. dji_sdk: Skickar bland annat videoströmmen från kameran, data från GPS:en och statusinformation om drönaren till datorn samt ta emot styrkommandon från datorn. Koden till alla delsystem har skrivits i C++. Utöver ramverket ROS består mjukvaran av ett grafiskt gränssnitt (GUI) som gjorts i Qt och en simuleringsmiljö som upprättades i Gazebo. OpenCV har använts för att bearbeta bilddata och matrisbiblioteket Eigen har nyttjats i uppdragsplaneringen samt målföljningen.

10 6 2.3 Koordinatsystem Tre olika koordinatsystem används av modulerna. Det första använder de globalt fastslagna geografiska koordinaterna latitud (λ) och longitud (ϕ). De är sfäriska koordinater uttrycka utifrån ett kartesiskt koordinatsystem (X, Y, Z) med origo i jordens centrum. Radien motsvarar jordens radie. Det andra är ett kartesiskt koordinatsystem (x L, y L, z L ) med origo där plattformen startar sin flygning. Detta koordinatsystem är ett ENU-koordinatsystem vilket innebär att x L, y L och z L är riktade åt öst, norr och upp och kommer att refereras till som det lokala koordinatsystemet. Hur koordinatsystemet relateras till jordens koordinatsystem kan ses i figur 3. λ φ Figur 3: Förhållandet mellan ett fixt koordinatsystem utifrån jordens mittpunkt och ett lokalt ENU-system. Slutligen kommer ett kartesiskt koordinatsystem (x d, y d, z d ) med origo i drönaren. Plattformens rotation relativt (x L, y L, z L ) definieras av eulervinklarna (φ, θ, ψ) och motsvarande rotationsmatris R = R x φr y θ Rz ψ där ψ är girvinkeln, θ är tiltvinkel och φ är rollvinkel. Dessa vinklar beskriver hur mycket drönarens koordinatsystem (x d, y d, z d ) har roterat relativt det lokala koordinatsystemet (x L, y L, z L ). Figur 4 illustrerar hur drönarens koordinatsystem samt dessa vinklar är definierade.

11 7 Figur 4: Drönarens lokala koordinatsystem.

12 3 Kommunikation 8 Kommunikationen sker med hjälp av ramverket ROS. ROS-noderna kan kommunicera med varandra genom att publicera och prenumerera på meddelanden från rostopics. Flera ROS-noder kan publicera och prenumerera på samma topic. Kommunikation via topics är till för informationsflöden som är kontinuerliga. Om man istället vill göra enstaka efterfrågningar mellan moduler kan istället rosservices användas. ROS-noder kommunicerar via services genom att en nod begär en service och en annan nod tillhandahåller denna service. 3.1 Implementering Då strukturen som använts till respektive modul är klassbaserad har ROS-noderna utformats på samma sätt. Detta innebär att varje modul i princip består av två klasser, en klass som innehåller all funktionalitet för modulen (kan ses som en huvudklass ) och en klass som innehåller all funktionalitet som behövs för att kommunicera med andra ROS-noder. Klassen som har ansvar för kommunikationen innehåller en pekare till den andra klassen och kallas för en Topic Handler. Till exempel så innehåller modulen för bildbehandling objekt av klasserna ImageProcessing och ImageProcessingTopicHandler där den sistnämnda innehåller en pekare till ett objekt av klassen ImageProcessing. Orsaken till att detta gjordes var för att när det publiceras data på en topic som nodens Topic Handler lyssnar på behöver den sparas undan i huvudklassen. Pekaren till objektet av huvudklassen gör det då möjligt att förmedla vidare den nya datan. 3.2 Topics Eftersom projekten i början utgick från att en DJI matrice 100 skulle finnas tillgänglig baserades de topics som användes på detta. Då det inte blev som planerat och en AR drone 2.0 fick användas istället innebar det att vissa topics som användes fick modifieras. Tabell 1 visar de topics som är gemensamma för båda drönarna, vilka noder som prenumererar/publicerar på respektive topic samt vilken meddelandetyp som publiceras på respektive topic. Tabell 3 och 4 visar de topics som är unika för DJI matrice 100 respektive AR drone 2.0. För en utförlig beskrivning av standardmeddelanden från std_msgs, geometry_msgs, och sensor_msgs, se [1][2]. Notera även att bara de mest relevanta topics från dji_sdk listas. För att se samtliga topics som dji_sdk prenumererar/publicerar till, se [3].

13 9 Tabell 1: Gemensamma ROS-topics för systemet. Topic Publishers Subscribers Meddelandetyp /tracking/goal_estimations tracking planning positioning GoalEstimateArray gui /image_processing/map_coordinates image_processing tracking geometry_msgs/point /image_processing/image_detection image_processing gui geometry_msgs/point /planning/trajectory planning positioning gui PointArray /planning/search_area gui planning PointArray /planning/mission_state gui planning positioning std_msgs/uint8 /positioning/manual_command gui positioning std_msgs/uint8 /planning/search_position gui planning geometry_msgs_point /planning/alpha gui planning std_msgs/float32 /planning/trajectory_distance planning gui std_msgs/float64 /sim/true_states simulering gui gazebo_msgs/modelstates Nedan beskrivs vad systemets gemensamma topics (enligt tabell 1) innehåller för information. /goal_estimations Positionsestimeringar för mål med en kovarians för varje estimering. Positionerna ges i ett lokalt kartesiskt koordinatsystem. /image_processing/map_coordinates Kartkoordinater för ett detekterat mål. Positionen ges i ett lokalt kartesiskt koordinatsystem. /image_processing/image_coordinates Bildkoordintar för ett detekterat mål. /planning/_trajectory Innehåller en array med punkter, som motsvarar positioner i ett lokalt kartesiskt koordinatsystem, som plattformen önskas följa. /planning/search_area En array med punkter som definierar ett sökområde som plattformen ska söka av. /planning/mission_state Ett heltal som representerar vilket uppdrag som plattformen ska utföra. Tabell 2 visar vilket värde på heltalet som motsvarar vilken uppdragstyp.

14 10 Tabell 2: Värde på mission_state som motsvarar uppdragstyp Värde Uppdragstyp -1 Avbryt/Nödstopp 0 Manuell styrning 1 Uppdrag 1 2 Uppdrag 2 3 Uppdrag 3 4 Landa 5 Lyft /positioning/manual_command Ett heltal som representerar vad plattformen ska göra vid manuell styrning. /planning/search_position Koordinater för startpunkten vid uppdrag 3. /planning/alpha Värdet på α som används i uppdrag 2, se under planerinsmodulen för definition. /planning/trajectory_distance Totala längden på den planerade sträckan drönaren ska åka. /sim/true_states Vid simulering skickas de exakta positionerna för objekten till gui:t. Tabell 3: ROS-topics för systemet baserat på DJI Matrice 100. Topic Publishers Subscribers Meddelandetyp /dji_sdk/local_position dji_sdk image_processing planning positioning geometry_msgs/pointstamped gui /dji_sdk/imu dji_sdk image_processing sensor_msgs/imu /dji_sdk/image_raw dji_sdk image_processing gui sensor_msgs/image /dji_sdk/gps_health dji_sdk image_processing planning positioning std_msgs/uint8 gui /dji_sdk/battery_state dji_sdk gui sensor_msgs/batterystate /dji_sdk/flight_control_setpoint_generic positioning dji_sdk sensor_msgs/joy /dji_sdk/velocity dji_sdk gui geometry_msgs/vector3stamped /command/twist_limit gui simulering geometry_msgs/twist Nedan beskriv vad de topics som används för en DJI Matrice 100 (tabell 3) innehåller för data. /dji_sdk/local_position Plattformens nuvarande position som anges i ett lokalt kartesiskt koordinatsystem.

15 11 /dji_sdk/imu Mätvärden från plattormens IMU. Innehåller plattformens orientation, vinkelhastighet och hastighet. /dji_sdk/image_raw Bildström från plattformens kamera. /dji_sdk/gps_health Anger kvalitén på signalstyrkan för plattformens GPS. Kvalitén representeras av ett heltal mellan 0 och 5; där 5 innebär god signalstyrka. /dji_sdk/battery_state Anger status för batteriet. Innehåller bland annat ett flyttal mellan 0 och 1 som representerar batteriprocenten. /command/twist_limit Vid simulering av DJI publiceras hastighetsbegränsingen på denna topic. Tabell 4: ROS-topics för systemet baserat på AR Drone 2.0. Topic Publishers Subscribers Meddelandetyp /ardrone/front/image_raw ardrone image_processing gui sensor_msgs/image /ardrone_autonomy/imu ardrone image_processing sensor_msgs/imu /drone_position simulering image_processing planning geometry_msgs/pointstamped /qualisys/target/odom qualisys image_processing positioning nav_msgs/odometry gui /qualisys/target/pose qualisys planning geometry_msgs/poststamped /planning/node_density gui planning std_msgs/float32 /planning/spiral_distance gui planning std_msgs/float32 /positioning/position_command - positioning geometry_msgs/point /positioning/velocity_limit gui positioning std_msgs/float32 /positioning/mission_height gui positioning std_msgs/float32 /ground_truth/state simulering positioning gui nav_msgs/odometry /cmd_vel positioning ardrone geometry_msgs/twist /ardrone/land positioning ardrone std_msgs/empty /ardrone/takeoff positioning ardrone std_msgs/empty /ardrone/reset positioning ardrone std_msgs/empty /ardrone/navdata ardrone gui ardrone_autonomy/navdata Nedan beskriv vad de topics som används för en AR Drone 2.0 (tabell 4) innehåller för data. /ardrone/front/image_raw Videoströmmen i realtid från drönaren. /ardrone_autonomy/imu IMU-data från drönaren, används endast i simulering.

16 12 /drone_position Drönarens position i simuleringen. /qualisys/target/odom Drönarens position, orientation och hastighet när positioneringssystemet i Visionen används. /qualisys/target/pose Drönarens posering när positionerinssystemet i Visionen används. /planning/node_density Användaren kan via gui:t välja hur tätt noderna för planeringen av trajektoria ska placeras. /planning/spiral_distance Användaren kan via gui:t välja hur lång spiralen vid uppdrag 3 ska vara. /positioning/position_command Säger åt drönaren att åka till en position, kan användas med positioneringssystemet i Visionen. /positioning/velocity_limit Användaren kan via gui:t sätta en övre gräns på drönarens hastighet. /positioning/mission_height Användaren kan via gui:t sätta vilken höjd drönaren ska utföra ett updrag på. /ground_truth/state Drönarens position publiceras här när simuleringen körs. /cmd_vel Skickar hastighet i (x, y, z)-led samt rotationshastigheter i roll, pitch och yaw. /ardrone/land Publiceras ett Empty meddelande på denna topic landar drönaren. /ardrone/takeoff Publiceras ett Empty meddelande på denna topic lyfter drönaren. /ardrone/reset Publiceras ett Empty meddelande på denna topic utför drönaren ett nödstopp. /ardrone/navdata Här publiceras data från drönarens IMU, accelerometer osv. 3.3 Services Tabell 6 visar systemets services, vilka noder som tillhandahåller/begär dessa och av vilken typ som servicen är. Notera att bara de mest relevanta services från dji_sdk listas. För att se samtliga services som dji_sdk tillhandahåller se [3].

17 13 Tabell 5: Gemensamma ROS-services för systemet. Service Provider Requester /tracking/clear_targets tracking gui /tracking/remove_target tracking planning /positioning/platform_origin positioning gui /positioning/start_log positioning gui /positioning/stop_log positioning gui /tracking/clear_targets Användaren kan via gui:t välja att rensa listan med detekterade objekt, tracking tar då bort de detekterade objekt som finns. /tracking/remove_target Denna service implementerades med avsikt att be tracking ta bort ett objekt med ett visst id, men används ej i dagsläget. /positioning/platform_origin Anropas när drönaren startas för att få världskoordinater till var det lokala koordinatsystemets origo är placerat. /positioning/start_log Anropas när användaren vill starta sparandet av positionsdata. /positioning/stop_log Anropas när användaren vill stoppa sparandet av positionsdata. Tabell 6: ROS-services för systemet när DJI Matrice 100 används. Service (/dji_sdk/...) Provider Requester mission_waypoint_upload dji_sdk positioning mission_waypoint_action dji_sdk positioning sdk_control_authority dji_sdk positioning /dji_sdk/mission_waypoint_upload Anger ett waypoint-uppdrag som plattformen ska utföra. Uppdraget består av waypoints som plattformen ska besöka samt andra parametrar som exempelvis flyghastighet. En waypoint anges i latitud, longitud och altitud. /dji_sdk/mission_waypoint_action Servicen startar, stoppar, pausar eller återupptar ett waypoint-uppdrag. //dji_sdk/sdk_control_authority Används för att begära att få kontroll över plattformens styrning. Returnerar sant eller falskt beroende på om begäran accepteras eller nekas.

18 14 4 GUI Denna modul sköter all interaktion mellan användaren och systemet, hur GUI:t ser ut kan ses i figur 5. GUI:t skapades med hjälp av utvecklingsmiljön Qt [4] och kartorna som behövs hämtas med Marble [5]. Figur 5: Figur över uppbyggnaden av GUI:t. GUI:t innehåller följande: Karta där positionen för plattformen och de mål som hittats visas. Videoström från drönarens kamera där målen markeras genom information från bildbehandlingsmodulen. Lista över hittade mål, samt information om dem (senast estimerade koordinater, tid för senaste detektion som osäkerheten för positionsskattningen). Information om plattformens batteristatus samt dess hastighet. Styrningsdel där användaren kan välja vilket uppdrag som ska utföras eller om drönaren ska styras manuellt. Terminal som kan användas vid utveckling samt för att skriva ut information. Knapp för att börja logga data. Knapp för att rensa upptäckta mål. Knapp som tar en skärmdump av videoströmmen. Knapp för att landa eller starta/avsluta uppdraget Knapp för nödstopp.

19 15 För att se tydligare instruktioner om vad respektive del i GUI:t gör se användarmanualen[6]. I tabell 7 ses vad som skickas in till GUI:t och vad som GUI:t skickar ut till övriga noder. Tabell 7: GUI In Ut Videoström Statusinformation för drönare Estimering av position för mål Osäkerhet för estimeringar Bildkoordinater för detekterade mål Styrmod Styrkommandon Avsökningsområde 4.1 Kommunikation mellan ROS och Qt Eftersom att systemets delmoduler är ROS-noder och att GUI:t består av QWidgets, har en klass, QRosNode, implementerats för att skapa en brygga mellan ROS och Qt. Figur 6 visar översiktligt hur kommunikation mellan systemets delmoduler och GUI:t implementerats. QRosNode lyssnar på rostopics som delmodulerna publicerar på och vidarebefordrar meddelanden med Qt:s Signal/Slot-metodik [7] till QWidgets som åskådliggör informationen. Kommunikationen sker även i andra riktningen när användaren matar in information i GUI:t genom exempelvis knapptryckningar. QWidgets registrerar den inmatade informationen och skickar vidare informationen till QRosNode som sedan vidarebefordrar informationen till systemets delmoduler genom att publicera till topics som delmodulerna prenumererar på. Figur 6: Figur över kommunikationen mellan Qt och ROS.

20 Gränssnitt GUI:t körs med ROS-noden gui. Tabell 8 visar vilka topics som noden publicerar/prenumererar på. För att se vad en topic innefattar se avsnitt 3. Tabell 8: Topics som noden gui prenumererar på och publicerar till. Subscribe Subscribe (DJI) Subscribe (AR 2.0) Publish Publish (DJI) Publish (AR 2.0) Topic /tracking/goal_estimations /image_processing/image_detection /planning/trajectory /planning/trajectory_distance /sim/true_states /dji_sdk/local_position /dji_sdk/image_raw /dji_sdk/gps_health /dji_sdk/battery_state /dji_sdk/velocity /ardrone/front/image_raw /qualisys/target/odom /ground_truth/state /ardrone/navdata /planning/search_area /planning/mission_state /planning/search_position /planning/alpha /positioning/manual_command /command/twist_limit /planning/node_density /planning/spiral_distance /planning/velocity_limit /planning/mission_height De services som gui begär ses i tabell 9 Tabell 9: Services som noden gui:t begär. Service /positioning/platform_origin /positioning/start_log /positioning/stop_log /tracking/clear_targets Provider positioning positioning positioning tracking

21 Vidareutveckling I GUI:t visas en cirkel runt positionsskattningarna som symboliserar osäkerheten. I dagsläget motsvarar den cirkeln gaten som räknas ut i målföljningsmodulen. Fås en mätning inom cirkeln kan den associeras till målet som har centrum i cirkeln. Storleken på denna cirkel är framtagen via empiriska tester och inte utritad exakt. En förbättring skulle vara att rita ut den mer exakt. För att göra detta måste ellipsens ekvation räknas ut. Sedan måste även konverteringen från en cirkel i kartesiskt koordinatsystem till en cirkel i longitud/latitud utföras och därefter projiceras ner på pixlar i kartan. Det finns många parametrar som påverkar funktionaliteten i stor grad. För fortsatt utveckling, till exempel vid byte av drönare eller kamera, skulle det underlätta om fler parametrar kan styras från GUI:t. Speciellt bildbehandlingen, målföljningen och uppdragsplaneringen skulle med fördel kunna justeras under körning. Till exempel finns stöd för röda, gröna och blå mål implementerat men val av färg går inte att ändra i GUI:t just nu.

22 5 Bildbehandling 18 Bildbehandlingensmodulen är den modul som tar in live-videoströmmen från plattformen och behandlar datan. Bildbehandlingen använder videoströmmen för att identifiera objekt och ta fram deras bildkoordinater. Efter detta konverteras bildkoordinaterna tillsammans med plattformens position för att ta fram objektens kartkoordinater i ett lokalt koordinatsystem. Bildbehandlingen har implementerats med hjälp av programvarubiblioteket OpenCV [8]. 5.1 Övergripande design In- och utsignaler för modulen visas i tabell 10 och ett blockschema visas i figur 8. I figur 7 visas ett flödesschema över hur bildbehandlingsmodulen fungerar. Figur 7: Flödesschema för bildbehandlingssmodulen.

23 19 Tabell 10: Bildbehandling In Ut Videoström Position för drönare IMU-data Kartkoordinater för detekterade mål Bildkoordinater för detekterade mål Figur 8: Blockschema över bildbehandlingssmodulen. 5.2 Terminologi Här beskrivs de uttryck och termer som används inom bildbehandlingsmodulen. RGB - Förkortning på röd, grön, blå vilka utgör grundfärgerna i additiv färgblandning. HSV - En färgrymd som består av beståndsdelarna hue, saturation, value. 5.3 Förbehandling För att enklare kunna identifiera objekt i videoströmmen förbehandlas den med morphologiska operationer. Det finns flera olika metoder för detta men de två som används är erode och dilate. Metoderna används för att de är bra på att minska bruset samt isolera individuella element i bilder. Båda operationerna utförs med ett element bestående av en cirkel. Resultatet kan ses i figur 9 och 10, där figur 9 är innan förbehandling och figur 10 är efter. Det går tydligt att se att utan förbehandling finns det väldigt många detektioner, de små objekten önskas filtreras bort då de med största sannolikhet inte är ett mål. När förbehandlingen sedan används ses att efter de morphologiska operationerna utförts har alla små objekt försvunnit.

24 20 Figur 9: Detekterade objekt utan förbehandling. Figur 10: Detekterade objekt med förbehandling.

25 Detektera mål För att detektera målen används färgbaserad detektion. Nedan beskrivs teorin bakom att detektera objekt avseende på färg i en videoström. Videoströmmen tar in bilder i RGB-format som sedan omvandlas till HSV-format, vilket utgör optimala färgrymden för färgbaserad bildsegmentering. Genom att ställa in trösklar för de olika representationerna kan en viss färg detekteras. Det krävs att de färger som ska upptäckas definieras samt att tröskelvärdena för respektive färg identifieras. Till exempel om röda bollar ska detekteras i en miljö av andra färger kan trösklarna i HSV-formatet ställas in för att extrahera alla färger förutom just rött. Utifrån detta är det enkelt att identifiera målen. Ett problem som uppstår när enbart färgdetektion används är att alla objekt med den sökta färgen detekteras. För att minimera antalet oönskade detektioner används de morphologiska operationerna som tar bort brus och därmed väldigt små objekt. Utöver dessa estimeras även arean hos de detekterade objekten och objekt med avvikande area ignoreras. 5.5 Markering av mål När ett mål har hittats kommer bildbehandlingsmodulen ta fram koordinater och storlek för detta objekt. Denna information används sedan av GUI:t för att avgöra hur den ska rita ut markeringen. Först räknas målets mittpunkt ut och därefter tas sidolängderna fram för den rektangel som ska ritas ut runt objektet, oavsett objektets form. Då förbehandlingen tar bort brus filtreras små objekt ut, men objekt med lite större yta gör tyvärr inte det. Därför används även rektangeln för att avgöra om det identifierade objektet är stort eller litet nog för att kunna vara ett mål. Detta görs genom att estimera arean hos rektangeln, genom att helt enkelt beräkna de lokala koordinaterna (se nedan hur) för det övre vänstra hörnet och det nedre högra hörnet och sedan beräkna arean. 5.6 Omvandling av koordinater Bildbehandlingsmodulen är även ansvarig för att konvertera ett detekterat objekts koordinater i bilden till koordinater i ett lokalt system med origo positionerat vid drönarens startpunkt. Detta görs i två steg, först konverteras bildens tvådimensionella koordinater till tredimensionella koordinater i ett koordinatssystem med centrum i drönaren. Sedan konverteras punkten i drönarens koordinatsystem till det lokala systemet med hjälp av en rotations- och translationsmatris Konvertering av bildkoordinater Konverteringen mellan objektets bildkoordinater och koordinater i drönarens koordinatsystem illustreras i figur 11.

26 22 Figur 11: Samband mellan drönarens och bildens koordinatsystem, till vänster är systemet beskrivet i 3D och till höger är figuren ritad i 2D sedd ur drönarens (z d, x d ) respektive (z d, y d )-axlar. Om den tredimensionella bilden ritas upp som (x,z) respektive (y,z) system fås två trianglar och ur dessa kan följande samband härledas: f z o,d = y o,b y o,d, f z o,d = x o,b x o,d (1) där (x o,b,y o,b ) är objektets bildkoordinater för ett koordinatsystem med origo mitt i bilden, f är kamerans fokallängd och (x o,d,y o,d,z o,d ) är objektets koordinater i drönarens koordinatsystem. Till följd av hur kameran är placerad på drönaren i simuleringen (se figur 11) och då bildens koordinatsystem har origo i ett hörn ges (x o,b, y o,b ) av: (x o,b, y o,b ) = ( (y o,c ), (x o,c 640 )) (2) 2 där 480 och 640 är bildens höjd respektive bredd angivna i pixlar. Dessutom kommer z o,d, som anger objektets position i höjdled (notera att detta värde i drönarens koordinatsystem kommer att vara negativt), beräknas enligt: z o,d = D d f (3)

27 23 där D är objektets riktiga bredd och d är objektets bredd angiven i antal pixlar. Om ett uttryck för objektets position i drönarens koordinatsystem, p o,d löses ur ekvation 1 fås: p o,d = (x o,d, y o,d, z o,d ) = ( zo,d x o,b f där z o,d och (x o,b, y o,b ) ges av ekvation 3 respektive 2., z ) o,dy o,b, z o,d f (4) Konvertering till lokalt koordinatsystem För att få ut de lokala koordinaterna för objektet, (x o,l, y o,l, z o,l ) behöver drönarens rotationer roll, tilt och gir samt drönarens position kompenseras för. När detta är gjort kommer drönarens koordinatsystem, (ˆx d, ŷ d, ẑ d ), vara positionerat med samma orientering som det lokala koordinatssystemet, (ˆx L, ŷ L, ẑ L ). En illustation av hur objektets position i det lokala koordinatsystemet beräknas med hjälp av drönarens position (x d,l, y d,l, z d,l ) och objektets position i drönarens koordinatsystem, (x o,d, y o,d, z o,d ) kan ses i figur 12. Konverteringen som detta resulterar i kan ses i ekvation 5 där rotations- och translationsmatrisen som åstadkommer detta ses i ekvation 6. Figur 12: Relationen mellan objektets position i drönarens och objektets position i det lokala koordinatsystemet. x x o,d o,l y y o,l = R o,d z z o,d, (5) o,l 1 a 11 a 12 a 13 x d,l R = a 21 a 22 a 23 y d,l, (6) a 31 a 32 a 33 z d,l

28 24 där a 11 = cos(ψ + α) cos(θ), a 12 = sin(ψ + α) cos(θ), a 13 = sin(θ), a 21 = cos(ψ + α) sin(θ) sin(φ) sin(ψ + α) cos(φ), a 22 = sin(ψ + α) sin(θ) sin(φ) + cos(ψ + α) cos(φ), (7) a 23 = cos(θ) sin(φ), a 31 = cos(ψ + α) sin(θ) cos(φ) + sin(ψ + α) sin(φ), a 32 = sin(ψ + α) sin(θ) cos(φ) cos(ψ + α) sin(φ), a 33 = cos(θ) cos(φ), där φ är rollvinkeln, θ är tiltvinkeln, ψ är girvinkeln och α är en offset från när drönaren startas ifall den inte är roterad i liknande ψ-led som det lokala koordinatsystemet. Rotationerna utförs i ordningen R x φr y θ Rz ψ där värdena a i,j är tagna från kursboken i sensorfusion [9]. Värdena (x o,d, y o,d, z o,d ) är objektets koordinater i drönarens koordinatsystem medan (x d,l, y d,l, z d,l ) är drönarens position i det lokala koordinatsystemet och används för translationen. 5.7 Gränssnitt Bildbehandlingen utförs av ROS-noden image_processing. Tabell 11 visar vilka topics som noden publicera/prenumererar på. För att se vad en topic innefattar se avsnitt 3. Tabell 11: Topics som noden image_processing prenumererar på och publicerar till. Subscribe (DJI) Subscribe (AR 2.0) Publish Topic /dji_sdk/image_raw /dji_sdk/local_position /dji_sdk/imu /dji_sdk/gps_health /ardrone/front/image_raw /ardrone_autonomy/imu /drone_position /qualisys/target/odom /image_processing/map_coordinates /image_processing/image_detection 5.8 Resultat Bildbehandlingsmodulen uppfyller alla krav som ställts i kravspecifikationen, den ger koordinater för objekt som är inom rimliga gränser av de riktiga värdena. Men det finns utrymme för förbättringar (se vidareutveckling). Då enbart en kamera används är det svårt

29 25 att estimera avståndet till objekt och den avståndsestimering som används nu fungerar bra så länge drönaren inte lutar kraftigt i någon vinkel (pitch eller roll). Detta har lett till att detektioner ibland blir mindre noggranna när drönaren är i rörelse. 5.9 Vidareutveckling Nedan listas några möjliga förslag på vidareutveckling som kan göra bildbehandlingen mer noggrann. Stereokamera: För tillfället används endast en kamera (mono) för att identifiera objekt, detta innebär att konverteringen från bildkoordinater till 3D-koordinater är väldigt osäker. Detta gäller speciellt avståndet från drönarens kamera till objektet. För att lösa detta bör två kameror användas (stereo), detta kommer ge en större noggrannhet i positionen. Den andra kameran bör vara en IR-kamera för att lättare kunna identifiera getter från omgivningen, men även en vanlig VGA-kamera bör ge bättre skattningar. Formdetektion: I nuläget används bara färgdetektion för att identifiera objekt. På grund av detta kommer alla objekt av denna färg vara potentiella objekt. Med hjälp av formdetektion kan vissa objekt uteslutas, t.ex. så är det inte troligt att en get är triangelformad. Kalibrering: För att enkelt säga åt systemet vilka färger det ska leta efter kan någon form av kalibrering utvecklas. Kalibreringen skulle kunna utföras genom att hålla den färg som ska sökas efter framför kameran när systemet är i kalibreringsläge.

30 6 Positionering 26 Positioneringsmodulen ser till att drönaren följer den väg som uppdragsplaneraren tagit fram. Modulen ansvarar även för att positionera plattformen utifrån manuella styrkommandon. Utöver positionering av plattformen lagrar även modulen plattformens position och skattningar av målens position. I tabell 12 ses in- och utsignaler till modulen. Figur 13 visar ett blockschema över modulen. Tabell 12: Positionering In Ut Trajektoria/önskad position Drönarens GPS-koordinater Skattningar av målens position Styrkommandon Figur 13: Blockschema över positioneringsmodulen. Eftersom systemet först anpassades efter DJI Matrice 100 och leverans av denna drönare uteblev har två olika principer för positionering av plattformen implementerats. En princip för DJI Matrice 100 och en annan för AR Drone DJI Matrice 100 DJI Matrice 100 har inbyggt stöd för waypoint-uppdrag, vilket innebär att den kan positionera sig efter en lista av GPS-koordinater. Således behöver inte positioneringsmodulen reglera plattformens position utan behöver bara paketera om trajektorian som skickas från uppdragsplaneraren till ett format som DJI Matrice 100 kan förstå. 6.2 AR Drone 2.0 För att kunna förflytta AR Drone 2.0 till en specificerad position har en PID-regulator implementerats. Styrkommandon som kan skickas till en AR Drone 2.0 är hastigheter

31 27 upp/ner, framåt/bakåt och höger/vänster samt vinkelhastigheter i tre riktningar. Först beräknas skillnaden mellan referenspositionen p ref och plattformens nuvarande position p i det globala koordinatsystemet. Denna skillnad tolkas sedan som en hastighetsreferens v ref och om positionsskillnaden är större än den satta hastighetsbegränsningen mättas v ref. Skillnaden mellan hastighetsreferensen och plattformens nuvarande hastighet v skickas sedan till en PID-regulator som skapar en styrsignal u. Slutligen används plattformens nuvarande orientering q för att transformera u till en styrsignal u i plattformens koordinatsystem. En översikt av positionsreglering visas i Figur 14. q p ref Σ v ref Begränsning Σ - - PID u Transformation u Plattform p v Figur 14: Översikt av positioneringsregleringen. Följning av en trajektoria har även behövts implementerats i positioneringsmodulen. Ett flödesschema för hur detta har implementerats visas i Figur 15 Figur 15: Flödesschema för följning av en trajektoria.

32 Gränssnitt Positioneringen utförs av ROS-noden positioning. Tabell 13 visar vilka topics som noden publicera/prenumererar på. För att se vad en topic innefattar se avsnitt 3. Tabell 13: Topics som noden positioning prenumererar på och publicerar till. Subscribe Subscribe (DJI) Subscribe (AR 2.0) Publish (DJI) Publish (AR 2.0) Topic /tracking/goal_estimations /planning/trajectory /planning/mission_state /positioning/manual_command /dji_sdk/local_position /dji_sdk/gps_health /qualisys/target/odom /positioning/position_command /positioning/velocity_limit /positioning/mission_height /ground_truth/state /dji_sdk/flight_control_setpoint_generic /cmd_vel /ardrone/land /ardrone/takeoff /ardrone/reset Styrkommandon till DJI Matrice 100 ges i form av waypoints och dessa skickas via services. Vilka services som noden begär visas i tabell 14. Tabell 14: Services som noden positioning begär. Service /dji_sdk/mission_waypoint_upload /dji_sdk/mission_waypoint_action /dji_sdk/sdk_control_authority Provider dji_sdk dji_sdk dji_sdk

33 Vidareutveckling Koden skrevs från början för en DJI Matrice 100. När projektet nådde slutfasen insågs att den drönaren inte skulle hinna levereras inom projektets tidsramar. Istället modifierades koden för att kunna köras med en AR Drone 2.0. Regleringen som då krävdes implementerades på en väldigt kort tid och parametrarna för regleringen skulle kunna justeras för att erhålla en bättre positionering av drönaren. Följningen av trajektorian blev på grund av den hastigt implementerade positionsregleringen, ganska ryckig. Regleringen skulle kunna utvecklas ytterligare vilket skulle ge en mjukare följning av trajektorian.

34 7 Målföljning 30 Modulens uppgift är att skatta positioner för mål och uppskatta hur osäkra dessa är. Skattningarna baseras på kartkoordinater för detekterade mål som erhålls från bildbehandlingen. I det fallet kommer kartkoordinaterna inte att vara associerade med ett specifikt mål vilket innebär att en sådan association måste göras innan skattningarna kan uppdateras. Associationen görs genom att hitta det mål vars senaste skattning har minst avstånd till den aktuella detektionen. För att kunna följa målen har ett linjärt Kalmanfilter med en konstant positionsmodell (CP-model) implementerats. Filtret utför bara en mätuppdatering om målet har detekterats. 7.1 Övergripande design I figur 16 visas ett flödesschema för målföljningsmodulen.

35 31 Figur 16: Flödesschema för målföljningssmodulen. I tabell 15 visas in- och utsignaler för modulen. Tabell 15: Målföljning In Ut Kartkoordinater för detekterade mål Estimering av position för mål Osäkerhet för estimering I figur 17 visas ett blockschema över målföljningsmodulen. Figur 17: Blockschema över målföljningsmodulen.

36 Rörelsemodell Eftersom målens rörelse är slumpmässig används en konstant positionsmodell (CP-model). Position i x-led respektive y-led utgör målets tillstånd. Tillståndsvektorn blir då enligt följande, x = ( ) xl där x L och y L är givna i det globala kartesiska koordinatsystemet. Mätekvationen är linjär eftersom mätningarna som fås in beskrivs av samma globala koordinatsystem som tilståndsvektorn. Således beskrivs tillståndsmodellen av y L (8) där F, G v och H ges av F = x k+1 = F x k + G v v k cov(v k ) = Q (9) y k = Hx k + e k cov(e k ) = R (10) [ ] [ ] , G 0 1 v = T s, H = 0 1 [ ] I ekvationen ovan är T s samplingstiden. Kovariansen för modellbruset (Q) och kovariansen för mätbruset (R) kommer justeras till lämpliga värden under projektets gång. De antas vara på formen Q = σ 2 Q [ ] 1 0, R = σr [ ] Tidsuppdatering Tidsuppdateringen ges av ekvation 11 [9]. ˆx k+1 k = F ˆx k k P k+1 k = F P k k F T + G v QG T v (11) 7.4 Associering Ett problem som uppstår i målföljningsmodulen är att associera inkommande mätdata med information som redan finns. Modulen ska kunna avgöra om ett detekterat objekt är ett helt nytt eller om det är ett tidigare detekterat objekt där en mätuppdatering behöver göras. För att kunna göra detta måste avståndet ett objekt kan röra sig under en viss tidpunkt beräknas.

37 Avståndsberäkning För att kunna göra en avståndsberäkning på hur långt ett mål har rört sig under en viss tid används Gating. En Gate formas runt målets predikterade position och tar formen av en elliptisk kurva. Gate:n räknas ut enligt följande, där S k ges av ekvation 14 nedan. (y k ŷ k k 1 ) T S 1 k (y k ŷ k k 1 ) < G (12) Vänsterledet i ekvation 12 är χ 2 fördelad med två frihetsgrader då mätdatan har två dimensioner. G väljs efter vilken sannolikhet det tillåts att ett mål som är associerat med en gate, befinner sig utanför gate:n. Till exempel om det önskas en säkerhet på 95% att ett mål befinner sig inom gaten väljs G till χ 2 (0.95) = Nearest Neighbor För att kunna avgöra vilket objekt som tillhör vilka koordinater används en variant av algoritmen Nearest neighbor. Algoritmen bygger på att för varje gate antar man att det mål som ligger närmast den predikterade positionen är det mest sannolika. Mätdatan mappas enligt följande ekvation. z = arg min 1 i m k (y i k ŷ i k k 1) T S 1 k (yi k ŷ i k k 1) (13) Där m k är antalet mål inom gaten. Om flera objekt finns detekterade observerar algoritmen vilket mål inom gaten som ligger närmast den predikterade positionen och associerar dessa. Ett mål kan inte associeras mer än en gång. Om ett mål detekteras och plattformen inte befinner sig inom någon gate kommer den registreras som ett nytt mål. Nearest neighbor är en av de enklare algoritmer för detta ändamål. Målen i projektet är utspridda över ett stort område och klusterbildningar sker relativt sällan, detta gör att en enklare algoritm som Nearest neighbor ger tillräckliga resultat. 7.5 Mätuppdatering Mätuppdateringen för Kalmanfiltret beskrivs av ekvation 14 nedan [9]. S k = R + HP k k 1 H T K k = P k k 1 H T S 1 k ˆx k k = ˆx k k 1 + K k ɛ k (14) P k k = (I K k H)P k k Hantering av inkommande detektioner Allt som detekteras i bildbehandlingsmodulen, mål, brus och störningar, måste urskiljas från varandra på något sätt. Detta hanteras genom att jobba med två olika listor, kallade

38 34 objectlist och targetlist. För att klassas som ett mål och därmed ligga i targetlistan måste ett objekt ha detekterats i bildbehandlingen i varje bildframe under 1 sekund. Därefter uppgraderas ett objekt till ett mål. Denna lösning kräver att associeringen sker i en specifik ordning listat nedan med en kort förklaring. Se även figur Associera mätningarna med aktiva mål. Med aktiva mål menas mål som fick en mätuppdatering i föregående sampelperiod. 2. Sätt de mål som inte fick någon mätuppdatering till inaktiva. 3. Ta bort de mätningar som blivit associerade med aktiva mål. 4. Associera resterande mätningar med befintliga objekt. Härifrån finns två möjliga vägar att gå. Associera mätning med befintligt objekt som är inaktivt och öka N, som talar om i hur många bilder efter varandra som en mätuppdatering är gjord på ett objekt. Sätt det associerade objektet till att vara aktivt. Med aktivt objekt menas att det har blivit mätuppdaterat i den nuvarande bilden. Om en mätning inte kan associeras med ett befintligt objekt skapas ett nytt objekt som sätts till aktivt. Efter detta steg har alla mätningar blivit antingen associerade med mål eller objekt eller så har nya objekt skapats. 5. Se om några objekt har varit detekterat i alla bilder under 1 sekund och isåfall associera dem med befintliga inaktiva mål. Då kan följande två beslut tas. Associera objekt med ett inaktivt mål genom en mätuppdatering med objektets kovariansmatris som R-matris i kalmanfiltret. R-matrisen används eftersom objektet har detekterats i alla bilder i en sekund och dess position är tämligen säker. Skapa ett nytt mål om ingen associering är möjlig. 6. Ta bort alla objekt som fortfarande är inaktiva (dessa antas alltså vara brus eller ha hamnat utanför kameran utan att blivit uppgraderade till ett mål). 7.7 Gränssnitt Målföljningen utförs av ROS-noden tracking. Tabell 16 visar vilka topics som noden publicera/prenumererar på. För att se vad en topic innefattar se avsnitt 3. Tabell 16: Topics som noden tracking prenumererar på och publicerar till. Subscribe Publish Topic /image_processing/map_coordinates /tracking/goal_estimations

39 Resultat Målföljningesmodulen uppfyller de krav som är satta i kravspecifikationen. Det som kan gå fel är att ett tidigare känt mål associeras till fel mätning, detta är dock ingen bugg utan mer en begränsning när man använder sig av associeringsalgoritmer likt nearest neigbour som bygger associeringen på geometriska avstånd. 7.9 Vidareutveckling Det finns gott om potential att vidareutveckla målföljningsmodulen. Det är främst två av delarna som det går att göra stora skillnader i, dels associeringen dels valet av rörelsemodell i filtret. För valet av rörelsemodell skulle en möjlighet vara att implementera att det i GUI:t går att välja bland ett antal olika fördefinierade rörelsemodeller då en konstant positionsmodell är ganska begränsande om inte målet strövar omkring sporadiskt. Skulle exempelvis djuret springa, skulle nuvarande filter tappa kontroll över målet ganska snabbt, men hade man haft möjlighet att i GUI:t manuellt bestämma exempelvis en konstant hastighetsmodell skulle filtret hinna med målet mycket bättre. På detta sätt skulle alla mål ha möjlighet att ha en egen rörelsemodell beroende på vad målet håller på med. Det andra som skulle kunna förbättras är att utveckla en mer sofistikerad associeringsalgoritm. Projektgruppen hade från början planer på att om tid fanns implementera en algoritm kallad Hungarian algorithm som i princip använder sig av samma summa som finns i ekvation 13. Istället för att välja det mål som ligger närmast mätningen som görs i dagsläget skulle algoritmen se till att summan av avstånden för alla associeringar i samma bildframe minimeras. Detta skulle troligvis göra störst skillnad om målen skulle ligga nära varandra.

40 8 Uppdragsplanering 36 Modulens uppgift är att utifrån ett givet uppdrag se till att en trajektoria beräknas för att uppfylla uppdraget. Dessa uppdrag innefattar: 1. Plattformen söker av ett definierat område och identifierar möjliga mål samt sparar deras positioner. 2. Plattformen utgår utifrån ett fördefinierat antal mål, målens osäkerhet minskas genom att kontinuerligt röra sig mellan dem. 3. Plattformen utgår från specificerad position och identifierar det närmaste målet, sedan följs målet. 8.1 Övergripande design I tabell 17 visas in- och utsignaler för modulen och i figur 18 visas ett flödesschema för modulen. Tabell 17: Uppdragsplanering In Ut Estimering av position för mål Osäkerhet för estimeringar ID för estimeringar Position för drönare Uppdragstyp Startpunkt uppdrag 3 Avsökningsområde Trajektoria/önskad position

41 37 Figur 18: Flödesschema för uppdragsplanering.

42 Uppdrag 1 Det första uppdraget listat ovan innebär att plattformen ska kunna söka av ett definierat område, identifiera eventuella mål, samt spara deras positioner. Delmomenten i uppdrag 1 består av att placera ut noder, ställa upp en kostnadsmatris och lösa ett handelsresandeproblem. Alla dessa delar beskrivs nedan Nodplacering Det specificerade sökområdet delas in i ett antal noder, vilket görs i blocket Nodplacering i figur 18. Området som ska sökas av specificeras av dess hörnpunkter som bildar ett trapetsformat område. Nodplaceringen tar in dessa koordinater och skapar ett rektangulärt område som täcker in koordinaterna (gränserna sätts utefter koordinaternas extremvärden). I detta område placeras sedan ett förutbestämt antal noder per areaenhet ut. Kamerans täckningsyta bestämmer hur nära noderna ska placeras varandra. Slutligen tas alla noder som ligger utanför det ursprungliga området bort. Nodplaceringen är skriven i Matlab som omvandlats till C++ kod. Kamerans täckningsyta behöver vara känd för att veta att hela området täcks och för att undvika onödigt många utplacerade noder. Eftersom kameran inte ger en kvadratisk bild väljs den kortaste sidan och noderna placeras ut med det avståndet mellan sig. Den kortaste sidan a beräknas enligt a = 2 tan ( θ v 2 )z 0 (15) där θ v är kamerans vidvinkel och z 0 är plattformens höjd. Slutligen halveras resultatet vilket gör att två noder alltid är i kamerans täckningsyta Kostnadsmatris Bågkostnaderna sätts i blocket Kostnadsmatris till avstånden mellan noderna. Kostnadsmatrisen beräknas enligt x 1... x n X =....., (16) x 1... x n K = y 1... y n Y =....., (17) y 1... y n, (X X T ) 2 + (Y Y T ) 2 (18) där X och Y innehåller nodernas x- respektive y-koordinater, n är antalet noder och K är den slutgiltiga kostnadsmatrisen. Alla operationer sker elementvis och kostnadsmatrisen K innehåller värden som representerar kostnaden att ta sig mellan två noder.

43 TSP För att minimera flygtiden och effektivisera arbetet ställs uppdragsplanering inför ett handelsresandeproblem. Detta löses i blocket TSP, där TSP står för Traveling Salesman Problem. Den algoritm som implementerades var Lin-Kernighans heuristik (LKH). LKH är i dagsläget en av de effektivare algoritmer för att lösa handelsresandeproblem. LKH-algoritmen utgår från en godtycklig väg mellan noderna och letar sedan den efter en mängd av kortare/billigare bågar som kan bytas ut mot en mängd av de nuvarande bågarna. Processen itereras tills inga kortare/billigare bågar hittas. [10] LKH utgår från en väg mellan noderna och desto bättre denna väg är desto bättre blir resultatet. Därför användes algoritmen Nearest Neighbour för att skapa en bra första väg. Nearest Neighbour innebär att den närmaste noden hela tiden besöks tills alla noder är uppsökta. I den implementerade algoritmen föredras det att gå i raka linjer. Därför är det dyrare att ändra färdriktning än att fortsätta i samma riktning. Algoritmen prövas för ett antal olika startnoder, desto mindre område desto större andel av områdets noder prövas som startnod. Sedan används den väg med lägst kostnad i LKH-algoritmen. Innan trajektorian är helt färdig förenklas den lite genom att ta bort noder som ligger på en rak linje mellan två andra noder. Detta görs genom att titta på tre noder i taget och om de ligger längs samma linje tas punkten i mitten bort från trajektorian. En estimering av hur lång tid avsökningen kommer ta görs genom att anta att plattformen har en konstant hastighet. Eftersom trajektorians längd är känd fås en estimering av tidsåtgången för att utföra uppdrag Uppdrag 2 Vid kontinuerlig estimering av målpositioner beräknas en önskad position ˆp för plattformen genom en planeringsalgoritm på formen ( ˆp = arg min α ) qi 2 (ˆx i p) + (1 α) p p 0 p {x 1,..,x n} i, i / i s, α [0, 1] (19) där ˆx i är senaste positionsskattningen för mål i, q i är ett osäkerhetsmått på denna skattning, p 0 är plattformens nuvarande position och i s är det senaste besökta målet. I implementeringen tas inte i s bort från ekvationen utan värdet för i s sätts till ett högt värde och således väljs aldrig den positionen. Kostnadsfunktionen valdes till en linjär viktning mellan osäkerheten på målens positionsskattning och avstånden till positionskattningarna, med viktningsfaktorn α. När målet hittats eller plattformen är framme på önskad position kommer ett nytt ˆp räknas ut. Målet hittas genom att verifiera att dess kovarians har minskat.

44 Uppdrag 3 I uppdrag tre ska plattformen identifiera och följa det närmsta målet utifrån en specificerad position. Detta problem löses med Arkimedes spiral. Arkimedes spiral utgår ifrån en fix punkt (i det här fallet plattformens specificerade position). Plattformen rör sig sedan längs spiralen tills ett mål upptäcks. Då avbryts rörelsen längs spiralen och målet följs istället. Ekvationen för Arkimedes spiral är: x = (a x + b θ) cos(θ) y = (a y + b θ) sin(θ) Där a = (a x, a y ) är centrumkoordinaten, b är avstånd mellan spiralerna och θ är vinkeln, θ [0, 2πn] där n är antalet varv. Variablerna b och n är designvariabler där b har satts utifrån kamerans täckningsyta och n har valts till ett tillräckligt stort värde att om målet inte hittats efter n varv är det med största sannolikhet borta. 8.5 Gränssnitt Uppdragsplaneringen utförs av ROS-noden planning. Tabell 18 visar vilka topics som noden publicerar/prenumererar på. För att se vad en topic innefattar se avsnitt 3. Tabell 18: Topics som noden planning prenumererar på och publicerar till Subscribe Subscribe (DJI) Subscribe (AR 2.0) Publish Topic /tracking/goal_estimations /planning/search_area /planning/search_position /planning/mission_state /planning/alpha /dji_sdk/local_position /dji_sdk/gps_health /drone_position /qualisys/target/pose /planning/node_density /planning/spiral_distance /planning/trajectory /planning/trajectory_distance De services planning begär ses i tabell 19. Tabell 19: Services som noden planning begär. Service /tracking/remove_target Provider tracking

45 Resultat I den här delen beskrivs planeringens resultat. Samtliga krav på uppdragsplaneringen uppfylls i simulering och alla uppdrag har testats med drönaren med önskat resultat Uppdrag 1 I uppdrag 1 täcks hela sökområdet fullständigt i simulering. Plattformen håller sig på eller innanför de angivna områdesgränserna, se figur 19 för simulering av uppdrag 1. Ibland korsas flygvägar i trajektorian vilket betyder att den resulterade trajektorian inte alltid är optimal och flygvägen kan bli lite längre än nödvändigt. När trajektorian är beräknad görs en estimering av tidsåtgången för uppdraget. Figur 19: Exempel på trajektorian i uppdrag 1, den blå streckade linjen visar den planerade trajektorian Uppdrag 2 Kraven på uppdrag 2 uppfylls i simulering. Nu prioriteras osäkerhet framför flygväg (α = 0.9). Ett problem med metoden som används uppstår om ett antal mål är placerade i en cirkel runt två andra mål som finns i mitten. Då åker plattformen mellan målen i mitten tills något av målen i cirkeln tas bort eller syns i kamerans bild. Detta beror på att den valda planeringsalgoritmen inte klarar av detta scenario.

46 42 Figur 20: Exempel på uppdrag 2. I figur 20 visas uppdrag 2 under körning i simulering. De gröna prickarna visar målens sanna position, de rödstreckade cirklarna avspeglar inom vilket område en mätning skulle kunna associeras. Siffran innanför cirklarna visar målens ID. I figuren kan man se att plattformen rör sig mot målet med ID 3, detta pågrund av att dess osäkerhet är störst. Även plattformens tidigare väg illusteras genom den streckade rosa linjen Uppdrag 3 Samtliga krav på uppdrag 3 är uppfyllda i simuleringen. Plattformen följer trajektorian baserad på Arkimedes spiral tills ett mål är detekterat och då följs målet istället. När plattformen följer trajektorian täcks området fullständigt. I figur 21 visas spiralen i simulering. Figur 21: Exempel på Arkimedes spiral.

47 Vidareutveckling Det finns stora möjligheter för vidareutveckling av den här modulen. I simulering är alla krav uppfyllda men flera krav med prioritet 2 är inte uppfyllda under körning med den fysiska plattformen. Nu har trajektorian skarpa kurvor vilket tvingar plattformen att göra kraftiga svängar, en interpolering av trajektorian skulle skona drönarens motorer och minimera positioneringsfelet. Även vid avsökning av större områden kan flygvägar ibland korsas. Det kan lösas genom att straffa annorlunda i kostnadsmatrisen. Det går också givetvis att prova en annan optimeringsalgoritm för att lösa handelsresandeproblemet. En annan möjlighet är att förbättra planeringsalgoritmen i uppdrag 2. Nu beräknas en ny position varje gång plattformen når önskad position. Det vore bättre om algoritmen planerar flera steg i taget. Då kan plattformen optimera flygsträckan bättre och hitta ett mål som ligger nära vägen till ett annat mål. Om ett mål inte hittas vid en önskad position räknar algoritmen ut en ny position. En utvecklingsmöjlighet är att implementera Arkimedes spiral om ett mål inte hittas. Något som skulle vara intressant är en kombination av de tre uppdragen. Allting startas med uppdrag 1, plattformen börjar söka av området och hittar objekt under utförandet. Om något måls osäkerhet blir alltför stor avbryts uppdrag 1 och uppdrag 2 startas, när osäkerheten sedan minskats återupptas uppdrag 1. Detta pågår tills hela området är genomsökt. Medan uppdrag 2 körs och ett mål inte hittas körs Arkimedes spiral för att söka av det närliggande området, storleken på spiralen skulle anpassas efter storleken på osäkerheten.

48 9 Simulering 44 En simuleringsmiljö i Gazebo[11] har upprättas som använts för att kunna simulera systemet och för att verifiera att varje modul uppfyller sina krav. Simuleringsmiljön består av en plattform som kan flyga till en given kartkoordinat och är utrustad med en nedåtriktad kamera, en IMU samt en GPS-mottagare. I simuleringen finns det även rörliga mål på marken som används för att utvärdera modulernas funktion och integration. En figur från simuleringsmiljön med en plattform och fyra mål visas i figur 22. Reglering av motorer och dylikt simuleras inte eftersom plattformen antas ha funktionalitet att flyga till en given kartkoordinat (finns i DJI:s Onbard SDK [12]). Figur 22: Exempelbild från simuleringsmiljön i Gazebo. De röda sfärerna representerar mål. I simuleringen används en generell plattform, alltså inte specifikt DJI Matrice 100, då det i startskedet av projektet var oklart när och om denna skulle levereras. För att simulera en generell plattform används ros-paketet hector_quadrotor [13]. Den simulerade plattformen emulerar kommunikationen från en DJI Matrice 100 som är kopplad till en dator med ros-paketet dji_sdk [3]. Med detta menas att den simulerade plattformen publicerar och prenumererar på samma topics som noden dji_sdk kommer att publicera och prenumerera på. Eftersom en AR drone 2.0 har använts i projektets slutskede har även en simuleringsmiljö för att simulera en AR Drone 2.0 upprättats. Detta görs med hjälp av ros-paketet tum_simulator [14].