Αυτόνομη παρακολούθηση και προσέγγιση δυναμικών αντικειμένων από ιπτάμενο ρομποτικό όχημα

ΑΡΙΣΤΟΤΕΛΕΙΟ ΠΑΝΕΠΙΣΤΗΜΙΟ ΘΕΣΣΑΛΟΝΙΚΗΣ ΠΟΛΥΤΕΧΝΙΚΗ ΣΧΟΛΗ ΤΜΗΜΑ ΗΛΕΚΤΡΟΛΟΓΩΝ ΜΗΧΑΝΙΚΩΝ & ΜΗΧΑΝΙΚΩΝ ΥΠΟΛΟΓΙΣΤΩΝ ΤΟΜΕΑΣ ΗΛΕΚΤΡΟΝΙΚΗΣ ΚΑΙ ΥΠΟΛΟΓΙΣΤΩΝ Διπλωματική Εργασία Αυτόνομη παρακολούθηση και προσέγγιση δυναμικών αντικειμένων από ιπτάμενο ρομποτικό όχημα Βογιατζής Νικόλαος ΑΕΜ: 7662 Επιβλέποντες: Επίκουρος Καθηγητής Συμεωνίδης Ανδρέας Αναπληρωτής Καθηγητής Πέτρου Λουκάς Δρ. Εμμανουήλ Τσαρδούλιας Θεσσαλονίκη, Ιούνιος 2017

Ευχαριστίες Θα ήθελα να ευχαριστήσω τους καθηγητές μου, Συμεωνίδη Ανδρέα και Λουκά Πέτρου, που μου δώσαν την ευκαιρία να ασχοληθώ με το θέμα της παρούσας διπλωματικής. Επίσης, θα ήθελα ιδιαιτέρως να ευχαριστήσω το Δρ. Εμμανουήλ Τσαρδούλια για την άψογη συνεργασία του και για την άμεση ανταπόκρισή του σε τυχόν ερωτήσεις μου. Τέλος, θα ήθελα να ευχαριστήσω τους γονείς μου, τα αδέρφια μου, τους φίλους και την κοπέλα μου που με στήριξαν κατά τη διάρκεια των σπουδών μου και ιδιαίτερα κατά τη διάρκεια της εκπόνησης της παρούσας διπλωματικής. 1

Περίληψη Τα drones έχουν ενσωματωθεί σε πολλούς τομείς, εργασιακούς και ερευνητικούς. Οι εργασίες τις οποίες αναλαμβάνουν, όμως, σε ορισμένες περιπτώσεις δεν απαιτούν ή είναι επικίνδυνο να περιλαμβάνουν ανθρώπινη παρέμβαση. Γι αυτό και η αυτονομία είναι κάτι που επιζητάται όταν πρόκειται να χρησιμοποιηθούν σε κάποια εργασία. Σε αυτό το πλαίσιο, η παρούσα διπλωματική προτείνει λύση στο πρόβλημα της αυτόνομης παρακολούθησης και προσέγγισης δυναμικού αντικειμένου με ένα drone. Για την υλοποίηση, αναπτύχθηκε κώδικας ο οποίος, κάνοντας χρήση των δεδομένων των αισθητήρων του drone και της εικόνας από τις δύο κάμερες που διαθέτει, υπολογίζει τη θέση, τη ταχύτητα και την κατεύθυνση του αντικειμένου. Στη συνέχεια, στέλνει κατάλληλες εντολές στο drone, ώστε να φτάσει στη θέση του αντικειμένου, με την ίδια ταχύτητα. Η προσέγγιση, αρχίζει με την οριζόντια κάμερα και συνεχίζει με την κάθετη, μέχρι να δοθεί εντολή προσγείωσης. Στα πειράματα που πραγματοποιήθηκαν, ως αντικείμενο στόχο τέθηκε ένα ρομπότ εδάφους το οποίο εκτέλεσε κινήσεις με ταχύτητα και στροφή διαφόρων εντάσεων. Από το ποσοστό επιτυχίας και τους χρόνους προσέγγισης, βγαίνει το συμπέρασμα ότι ο αλγόριθμος μπορεί να ανταπεξέλθει στη δυναμικότητα της κίνησης του αντικειμένου. Τα πειράματα εκτελέστηκαν μόνο σε περιβάλλον προσομοίωσης. 2

Abstract Drones have been implemented to various commercial and research sectors. In the tasks that they undertake, however, it is not needed or is deemed dangerous to include human personnel. That is why, autonomy is being sought for these kinds of tasks. In that context, this paper proposes a solution to the problem of autonomous tracking and approaching of a dynamic object with a commercial drone. For the implementation, code was developed that, using the data from the sensors and the camera on board the drone, calculated the position, the velocity and direction of the object. Then, it sent appropriate commands to the drone, so that it would reach the position of the object, with the same velocity. The approach starts with the horizontal camera activated, then it continues with the vertical one. In the experiments that were conducted, a ground robot was used as a target that moved at a trajectory with velocity and torque of various intensities. Looking at the success rate and the approach times, we can conclude that the algorithm can cope with the dynamic of the object s movement. The experiments were conducted in a simulated environment. 3

Περιεχόμενα ΕΥΧΑΡΙΣΤΙΕΣ... 1 ΠΕΡΙΛΗΨΗ... 2 ABSTRACT... 3 ΛΙΣΤΑ ΠΙΝΑΚΩΝ... 5 ΛΙΣΤΑ ΕΙΚΟΝΩΝ... 5 1. ΕΙΣΑΓΩΓΗ... 8 1.1. ΚΙΝΗΤΡΟ ΔΙΠΛΩΜΑΤΙΚΗΣ... 8 1.2. ΠΕΡΙΓΡΑΦΗ ΤΟΥ ΠΡΟΒΛΗΜΑΤΟΣ... 8 1.3. ΜΕΘΟΔΟΛΟΓΙΑ... 9 1.4. ΔΟΜΗ ΚΕΙΜΕΝΟΥ... 9 2. ΣΧΕΤΙΚΗ ΒΙΒΛΙΟΓΡΑΦΙΑ... 10 2.1. ΜΕΘΟΔΟΙ ΕΝΤΟΠΙΣΜΟΥ ΑΝΤΙΚΕΙΜΕΝΟΥ... 10 2.2. ΜΕΘΟΔΟΙ ΠΛΟΗΓΗΣΗΣ ΤΟΥ DRONE... 11 3. ΕΡΓΑΛΕΙΑ, ΑΛΓΟΡΙΘΜΟΙ ΚΑΙ ΥΛΙΚΟ... 13 3.1. ΥΛΙΚΟ (HARDWARE)... 13 3.1.1. AR.Drone 2.0... 13 3.2. ΑΛΓΟΡΙΘΜΟΙ ΚΑΙ ΠΡΟΓΡΑΜΜΑΤΙΣΤΙΚΑ ΕΡΓΑΛΕΙΑ (SOFTWARE)... 14 3.2.1. ROS... 14 3.2.1.1. Βασικοί ορισμοί/έννοιες του ROS... 15 3.2.2. Πακέτα του ROS... 15 3.2.3. Gazebo Simulator... 16 3.2.4. Αλγόριθμος TLD... 16 3.2.4.1. Tracker... 17 3.2.4.2. Detector... 17 3.2.4.3. Learner... 18 4. ΥΛΟΠΟΙΗΣΗ... 19 4.1. ΠΕΡΙΛΗΨΗ ΚΕΦΑΛΑΙΟΥ... 19 4.2. ΠΑΡΑΔΟΧΕΣ ΠΕΡΙΟΡΙΣΜΟΙ... 19 4.2.1. Μορφή εντολών... 19 4.2.2. Ιδιότητες κίνησης αντικειμένου-στόχου... 20 4.3. ΚΥΡΙΟΙ ΚΟΜΒΟΙ ΥΛΟΠΟΙΗΣΗΣ... 21 4

4.3.1. Κόμβος Οδομετρίας (custom_odom)... 22 4.3.2. Κόμβος ελεγκτή (PID)... 23 4.3.3. Κόμβος προσέγγισης (approach)... 27 4.3.3.1. Αρχικοποίηση τιμών και σταθερές... 27 4.3.3.2. Διαδικασία προσέγγισης... 28 4.3.3.3. Υπολογισμός απόστασης... 33 4.3.3.3.1. Υπολογισμός κατεύθυνσης και ταχύτητας αντικειμένου... 40 4.3.3.4. Μη ορατό αντικείμενο... 42 5. ΠΕΙΡΑΜΑΤΑ... 43 5.1. ΠΕΡΙΓΡΑΦΗ ΠΕΙΡΑΜΑΤΩΝ... 43 5.2. ΑΠΟΤΕΛΕΣΜΑΤΑ... 45 5.2.1. Στατικό αντικείμενο... 45 5.2.2. Αντικείμενο σε κίνηση (γραμμική)... 47 5.2.3. Αντικείμενο σε κυκλική κίνηση... 53 5.2.4. Αντικείμενο σε τυχαία τροχιά... 67 5.2.5. Γενικά αποτελέσματα... 73 6. ΣΥΜΠΕΡΑΣΜΑΤΑ ΚΑΙ ΜΕΛΛΟΝΤΙΚΗ ΕΡΓΑΣΙΑ... 74 6.1. ΣΥΜΠΕΡΑΣΜΑΤΑ... 74 6.2. ΠΡΟΒΛΗΜΑΤΑ... 74 6.3. ΜΕΛΛΟΝΤΙΚΕΣ ΥΛΟΠΟΙΗΣΕΙΣ/ΕΠΕΚΤΑΣΕΙΣ... 75 ΒΙΒΛΙΟΓΡΑΦΙΑ... 76 Λίστα πινάκων ΠΙΝΑΚΑΣ 1 ΤΙΜΕΣ ΠΑΡΑΜΕΤΡΩΝ PID ΕΛΕΓΧΟΥ... 25 ΠΙΝΑΚΑΣ 2 ΕΣΤΙΑΚΑ ΜΗΚΗ ΚΑΜΕΡΩΝ (PIXELS)... 28 ΠΙΝΑΚΑΣ 3 ΎΨΟΣ ΣΤΟΧΟΥ ΓΙΑ ΔΙΑΦΟΡΕΣ ΑΠΟΣΤΑΣΕΙΣ ΤΟΥ ΑΝΤΙΚΕΙΜΕΝΟΥ... 31 Λίστα εικόνων ΕΙΚΟΝΑ 1 AR.DRONE ΜΕ ΕΣΩΤΕΡΙΚΟ ΚΕΛΥΦΟΣ... 14 ΕΙΚΟΝΑ 2 AR.DRONE ΜΕ ΕΞΩΤΕΡΙΚΟ ΚΕΛΥΦΟΣ... 14 ΕΙΚΟΝΑ 3 ΤΟΠΙΚΟ ΚΑΙ ΚΑΘΟΛΙΚΟ ΣΥΣΤΗΜΑ ΣΥΝΤΕΤΑΓΜΕΝΩΝ... 20 5

ΕΙΚΟΝΑ 4 ΑΡΧΙΤΕΚΤΟΝΙΚΗ ΣΥΣΤΗΜΑΤΟΣ ΠΡΟΣΕΓΓΙΣΗΣ... 21 ΕΙΚΟΝΑ 5 ΠΡΟΒΟΛΗ ΤΑΧΥΤΗΤΩΝ ΣΤΟ ΚΑΘΟΛΙΚΟ ΣΥΣΤΗΜΑ ΚΑΙ ΜΕΤΑΤΟΠΙΣΗ ΘΕΣΗΣ DRONE... 23 ΕΙΚΟΝΑ 6 ΔΙΕΥΘΥΝΣΗ DRONE ΣΕ ΣΧΕΣΗ ΜΕ ΤΗ ΚΑΤΕΥΘΥΝΣΗ ΤΟΥ ΑΝΤΙΚΕΙΜΕΝΟΥ... 30 ΕΙΚΟΝΑ 7 ΔΙΑΓΡΑΜΜΑ ΡΟΗΣ ΚΟΜΒΟΥ ΠΡΟΣΕΓΓΙΣΗΣ... 32 ΕΙΚΟΝΑ 8 ΑΠΑΛΟΙΦΗ ΘΕΤΙΚΗΣ ΣΤΡΟΦΗΣ ΚΑΤΑ ΤΟΝ ΤΟΠΙΚΟ ΑΞΟΝΑ Y (ΟΡΙΖΟΝΤΙΑ ΚΑΜΕΡΑ)... 34 ΕΙΚΟΝΑ 9 ΑΠΑΛΟΙΦΗ ΑΡΝΗΤΙΚΗΣ ΣΤΡΟΦΗΣ ΚΑΤΑ ΤΩΝ ΤΟΠΙΚΟ ΑΞΟΝΑ Y (ΟΡΙΖΟΝΤΙΑ ΚΑΜΕΡΑ)... 35 ΕΙΚΟΝΑ 10 ΑΠΑΛΟΙΦΗ ΣΤΡΟΦΗΣ ΚΑΤΑ ΤΟΝ ΤΟΠΙΚΟ ΑΞΟΝΑ X (ΟΡΙΖΟΝΤΙΑ ΚΑΜΕΡΑ)... 36 ΕΙΚΟΝΑ 11 ΑΠΑΛΟΙΦΗ ΣΤΡΟΦΗΣ ΚΑΤΑ ΤΟΝ ΤΟΠΙΚΟ ΑΞΟΝΑ Y (ΚΑΤΑΚΟΡΥΦΗ ΚΑΜΕΡΑ)... 38 ΕΙΚΟΝΑ 12 ΥΠΟΛΟΓΙΣΜΟΣ ΑΠΟΣΤΑΣΗΣ ΑΝΤΙΚΕΙΜΕΝΟΥ (ΚΑΤΑΚΟΡΥΦΗ ΚΑΜΕΡΑ)... 39 ΕΙΚΟΝΑ 13 ΥΠΟΛΟΓΙΣΜΟΣ ΤΩΝ ΓΩΝΙΩΝ ΟΠΤΙΚΗΣ ΕΠΑΦΗΣ ΚΑΙ ΔΙΟΡΘΩΣΗΣ (ΕΠΙΠΕΔΟ ΕΙΚΟΝΑΣ)... 40 ΕΙΚΟΝΑ 14 ΕΝΤΟΠΙΣΜΟΣ ΑΝΤΙΚΕΙΜΕΝΟΥ ΣΤΟ ΠΕΡΙΒΑΛΛΟΝ GAZEBO (ΟΡΙΖΟΝΤΙΑ ΚΑΜΕΡΑ)... 45 ΕΙΚΟΝΑ 15 ΕΝΤΟΠΙΣΜΟΣ ΑΝΤΙΚΕΙΜΕΝΟΥ ΣΤΟ ΠΕΡΙΒΑΛΛΟΝ GAZEBO (ΚΑΘΕΤΗ ΚΑΜΕΡΑ)... 45 ΕΙΚΟΝΑ 16 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΣΤΑΤΙΚΟ ΑΝΤΙΚΕΙΜΕΝΟ)... 46 ΕΙΚΟΝΑ 17 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΣΤΑΤΙΚΟ ΑΝΤΙΚΕΙΜΕΝΟ)... 47 ΕΙΚΟΝΑ 18 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΣΤΑΤΙΚΟ ΑΝΤΙΚΕΙΜΕΝΟ)... 47 ΕΙΚΟΝΑ 19 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΑΜΜΙΚΗ, ΑΡΓΗ ΚΙΝΗΣΗ)... 48 ΕΙΚΟΝΑ 20 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΑΜΜΙΚΗ, ΑΡΓΗ ΚΙΝΗΣΗ)... 49 ΕΙΚΟΝΑ 21 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΑΜΜΙΚΗ, ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ)... 49 ΕΙΚΟΝΑ 22 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΑΜΜΙΚΗ, ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ)... 50 ΕΙΚΟΝΑ 23 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΑΜΜΙΚΗ, ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ)... 50 ΕΙΚΟΝΑ 24 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΑΜΜΙΚΗ, ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ)... 51 ΕΙΚΟΝΑ 25 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΓΡΑΜΜΙΚΗ, ΑΡΓΗ ΚΙΝΗΣΗ)... 51 ΕΙΚΟΝΑ 26 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΓΡΑΜΜΙΚΗ, ΚΑΝΟΝΙΚΗ ΤΑΧΥΤΗΤΑ)... 52 ΕΙΚΟΝΑ 27 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΓΡΑΜΜΙΚΗ, ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ)... 52 ΕΙΚΟΝΑ 28 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΑΡΓΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 54 ΕΙΚΟΝΑ 29 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΑΡΓΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 54 ΕΙΚΟΝΑ 30 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 55 ΕΙΚΟΝΑ 31 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ... 55 ΕΙΚΟΝΑ 32 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 56 ΕΙΚΟΝΑ 33 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 56 ΕΙΚΟΝΑ 34 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΑΡΓΗ ΚΙΝΗΣΗ, ΚΑΝΟΝΙΚΗ ΣΤΡΟΦΗ)... 57 ΕΙΚΟΝΑ 35 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΑΡΓΗ ΚΙΝΗΣΗ, ΚΑΝΟΝΙΚΗ ΣΤΡΟΦΗ)... 57 ΕΙΚΟΝΑ 36 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ ΚΑΙ ΣΤΡΟΦΗ)... 58 ΕΙΚΟΝΑ 37 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ ΚΑΙ ΣΤΡΟΦΗ)... 58 ΕΙΚΟΝΑ 38 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΚΑΝΟΝΙΚΗ ΣΤΡΟΦΗ)... 59 ΕΙΚΟΝΑ 39 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΚΑΝΟΝΙΚΗ ΣΤΡΟΦΗ)... 59 ΕΙΚΟΝΑ 40 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΑΡΓΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 60 ΕΙΚΟΝΑ 41 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΑΡΓΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 60 6

ΕΙΚΟΝΑ 42 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 61 ΕΙΚΟΝΑ 43 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 61 ΕΙΚΟΝΑ 44 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 62 ΕΙΚΟΝΑ 45 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 62 ΕΙΚΟΝΑ 46 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΑΡΓΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 63 ΕΙΚΟΝΑ 47 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 63 ΕΙΚΟΝΑ 48 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΑΝΟΙΧΤΗ ΣΤΡΟΦΗ)... 64 ΕΙΚΟΝΑ 49 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΑΡΓΗ ΚΙΝΗΣΗ, ΚΑΝΟΝΙΚΗ ΣΤΡΟΦΗ)... 64 ΕΙΚΟΝΑ 50 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ ΚΑΙ ΣΤΡΟΦΗ)... 65 ΕΙΚΟΝΑ 51 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΚΑΝΟΝΙΚΗ ΣΤΡΟΦΗ)... 65 ΕΙΚΟΝΑ 52 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΑΡΓΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 66 ΕΙΚΟΝΑ 53 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 66 ΕΙΚΟΝΑ 54 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ, ΚΛΕΙΣΤΗ ΣΤΡΟΦΗ)... 67 ΕΙΚΟΝΑ 55 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΤΥΧΑΙΑ, ΑΡΓΗ ΚΙΝΗΣΗ)... 68 ΕΙΚΟΝΑ 56 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΤΥΧΑΙΑ, ΑΡΓΗ ΚΙΝΗΣΗ)... 68 ΕΙΚΟΝΑ 57 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΤΥΧΑΙΑ, ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ)... 69 ΕΙΚΟΝΑ 58 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΤΥΧΑΙΑ, ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ)... 69 ΕΙΚΟΝΑ 59 ΔΙΑΓΡΑΜΜΑ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΤΥΧΑΙΑ, ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ)... 70 ΕΙΚΟΝΑ 60 ΔΙΑΓΡΑΜΜΑ ΠΡΟΒΟΛΗΣ ΕΥΚΛΕΙΔΕΙΑΣ ΑΠΟΣΤΑΣΗΣ (ΤΥΧΑΙΑ, ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ)... 70 ΕΙΚΟΝΑ 61 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΤΥΧΑΙΑ, ΑΡΓΗ ΚΙΝΗΣΗ)... 71 ΕΙΚΟΝΑ 62 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΤΥΧΑΙΑ, ΚΑΝΟΝΙΚΗ ΚΙΝΗΣΗ)... 72 ΕΙΚΟΝΑ 63 ΔΙΑΓΡΑΜΜΑ ΔΙΑΦΟΡΑΣ ΤΑΧΥΤΗΤΩΝ (ΤΥΧΑΙΑ, ΓΡΗΓΟΡΗ ΚΙΝΗΣΗ)... 72 7

1. Εισαγωγή 1.1. Κίνητρο Διπλωματικής Τα τελευταία χρόνια τα ιπτάμενα μη επανδρωμένα ρομποτικά οχήματα, (UAVs ή drones) έχουν αρχίσει να χρησιμοποιούνται κατά κόρον σε διάφορους επιστημονικούς αλλά και εταιρικούς τομείς. Η τεχνολογία δεν είναι καινούρια, αλλά το κόστος παραγωγής τους έχει μειωθεί τα τελευταία χρόνια, καθιστώντας τα έτσι διαθέσιμα ακόμα και σε ιδιώτες. Η κατασκευή τους επιτρέπει την πρόσβαση σε περιοχές που είναι δύσβατες για ανθρώπους ή περιοχές που έχουν πληγεί από κάποια καταστροφή, ενώ η εύκολη προσάρτηση αισθητήρων τα καθιστά ευέλικτα και ακριβή εργαλεία για κάθε εργασία που απαιτεί φωτογράφιση, διανομή ή απλά εξερεύνηση. Η ζήτηση, λοιπόν, αυτής της πλατφόρμας, αλλά και η ανάγκη ενασχόλησης με τον τομέα της ρομποτικής ήταν τα κύρια κίνητρα για την ανάληψη αυτής της διπλωματικής. Ο προγραμματισμός ενός ρομπότ είναι μία πρόκληση που στο τέλος θα προσέφερε πληθώρα γνώσεων και ικανοτήτων. Το τελικό αποτέλεσμα δε, θα ήταν επιθυμητό να γίνει εργαλείο για μελλοντικούς επιστήμονες και ερευνητές καθώς και για απλούς χρήστες ενός drone. 1.2. Περιγραφή του προβλήματος Το θέμα της διπλωματικής είναι η προσέγγιση με ένα drone ενός δυναμικού αντικειμένου και η προσγείωση πάνω σε αυτό, αυτόνομα και χωρίς τη χρήση GPS. To drone θα πρέπει να αναγνωρίζει ένα αντικείμενο με οπτικό τρόπο, να το ακολουθεί και τέλος να μειώνει το ύψος του έως ότου να μπορεί με ασφάλεια και ακρίβεια να προσγειωθεί πάνω του. Το αντικείμενο θα μπορεί να κάνει οποιαδήποτε κίνηση, αρκεί να μην προσπαθεί να κάνει ελιγμούς αποφυγής (π.χ. απότομη αλλαγή πορείας, ξαφνική αύξηση/μείωση ταχύτητας κ.λ.π. ). Η δυσκολία του όλου εγχειρήματος έγκειται στο σωστό υπολογισμό της θέσης και της ταχύτητας του αντικειμένου με δεδομένα τη θέση του αντικειμένου στην εικόνα και τη δικιά του θέση στους τρεις άξονες. Επίσης, το drone θα πρέπει να ανανεώνει τους υπολογισμούς του αρκετά γρήγορα ώστε να γνωρίζει κάθε στιγμή τη σωστή κατεύθυνση του αντικειμένου, ειδικά όταν αυτό έχει μία τροχιά με συνεχείς ή σποραδικές στροφές. 8

1.3. Μεθοδολογία Για να λυθεί αυτό το πρόβλημα, θα γίνει χρήση των δύο καμερών που διαθέτει το drone καθώς και ένας αλγόριθμος εντοπισμού αντικειμένου από το feed μιας κάμερας. Το drone θα ξεκινά και, αφού εντοπίσει το αντικείμενο, θα το πλησιάζει έως ότου να μην είναι πια ορατό με τη μπροστινή κάμερα. Εκείνη τη στιγμή, θα ενεργοποιεί την κάτω κάμερα και θα ανεβαίνει σε ύψος τέτοιο ώστε να μπορεί να έχει και πάλι το αντικείμενο στο πεδίο όρασης της κάμερας. Από κει και έπειτα, το drone θα ακολουθεί το αντικείμενο προσπαθώντας να κεντράρει την κάμερα από πάνω του ενώ ταυτόχρονα θα μειώνει το ύψος του. Όταν η υψομετρική διαφορά των δύο είναι κάτω από ένα όριο, τότε το drone θα παίρνει εντολή προσγείωσης. Η προσέγγιση θα γίνει με τη βοήθεια ενός ελεγκτή PID (proportional integral derivative) ώστε το drone να κινείται με σταθερότητα ενώ η παρακολούθηση του αντικειμένου θα γίνει μέσω του αλγορίθμου Predator, ο οποίος υλοποιεί ουσιαστικά την λογική του TLD (tracking learning detection ). Ο κώδικας που αναπτύχθηκε για τη λύση του προβλήματος εφαρμόστηκε σε περιβάλλον προσομοίωσης. 1.4. Δομή Κειμένου Το υπόλοιπο έγγραφο δομείται ως εξής: Στο 2 ο Κεφάλαιο, παρουσιάζεται η βιβλιογραφία με τις σχετικές προσεγγίσεις προς τη λύση του προβλήματος. Στο 3 ο Κεφάλαιο, αναφέρονται τα εργαλεία, οι έτοιμοι αλγόριθμοι και το υλικό που χρησιμοποιήθηκε για την υλοποίηση που προτείνεται. Στο 4 ο Κεφάλαιο αναλύεται η εν λόγω υλοποίηση και στο 5 ο Κεφάλαιο παραθέτονται τα πειράματα και τα αποτελέσματά τους. Τέλος, στο 6 ο Κεφάλαιο δίνονται τα συμπεράσματα, τα προβλήματα που προέκυψαν και οι πιθανές επεκτάσεις της παρούσας υλοποίησης. 9

2. Σχετική βιβλιογραφία Το πρόβλημα της παρακολούθησης αντικειμένου με drone έχει καλυφθεί από αρκετές δημοσιεύσεις, διαφοροποιούμενες στον αλγόριθμο εντοπισμού αντικειμένου και στη μέθοδο πλοήγησης του drone. Παρακάτω αναλύονται οι κυριότεροι μέθοδοι από τις δύο αυτές φάσεις, όπως παρουσιάζονται στη βιβλιογραφία. 2.1. Μέθοδοι εντοπισμού αντικειμένου Ο συνηθέστερος τρόπος εντοπισμού αντικειμένου (και ο πιο απλός) είναι με τη μέθοδο του color thresholding. Ένας αλγόριθμος δέχεται σαν είσοδο την εικόνα της κάμερας σε μορφή RGB και τη μετατρέπει σε μορφή HSV, η οποία αποτελείται από τρεις πίνακες, με κάθε κελί να αντιστοιχεί σε ένα pixel. Ο πίνακας Hue αναφέρεται στο χρώμα του pixel, ενώ οι υπόλοιποι δύο, Saturation και Value, στο πόσο αναμειγμένο είναι το χρώμα με μαύρο και άσπρο αντίστοιχα. Βάζοντας όριο σε αυτές τις τιμές (thresholding), μπορούμε να κρατήσουμε όσα pixels βρίσκονται στη χρωματική περιοχή που έχουμε επιλέξει για εντοπισμό. Έτσι, παράγεται μια εικόνα με λευκή την περιοχή όπου έχουμε εντοπίσει το αντικείμενο μας. Αυτή η τεχνική χρησιμοποιείται στα [1], [2], [3] και [4]. Παρακάτω, συναντάμε μία μέθοδο που πάλι χρησιμοποιεί το χρώμα, αλλά με μία άλλη προσέγγιση. Εδώ, συνδυάζεται ο αλγόριθμος Camshift με ένα Kalman φίλτρο (Ding et al. [5] ), όπου από την εικόνα υπολογίζεται η κατανομή πιθανότητας χρώματος μέσα σε ένα πλαίσιο που περικλείει το αντικείμενο και αυτή χρησιμοποιείται για την εύρεση του αντικειμένου σε επόμενο στιγμιότυπο της κάμερας. Το φίλτρο βοηθάει στην πρόβλεψη της θέσης και ταχύτητας του αντικειμένου, φτιάχνοντας ένα μοντέλο από τους προηγούμενους εντοπισμούς του. Οι Chakrabarty et al. [6] χρησιμοποιούν έναν αλγόριθμο ο οποίος βασίζεται σε σημεία κλειδιά (keypoints) μέσα σε ένα πλαίσιο του αρχικού στιγμιότυπου της εικόνας και τον εντοπισμό τους σε επόμενα στιγμιότυπα. Αυτός είναι ο CMT, με τον οποίο προβλέπεται η θέση των keypoints από την οπτική ροή (optical flow) και ταιριάζονται μεταξύ τους μέσω των περιγραφέων (descriptors) των σημείων, ενώ ταυτόχρονα κάθε σημείο ψηφίζει για τη θέση του κέντρου του πλαισίου ώστε να παραληφθούν οι ακραίες τιμές, και να σχηματιστεί το καινούριο πλαίσιο που περιέχει το αντικείμενο. Λιγότερο χρησιμοποιούνται αλγόριθμοι που βασίζονται σε ταξινομητές (classifiers), καθώς οι περισσότεροι απαιτούν offline εκπαίδευση (training) και μεγαλύτερη υπολογιστική ισχύ. Παρ όλα αυτά, πειράματα έχουν δείξει ότι υπερισχύουν σε απόδοση σε σχέση με 10

αλγόριθμους που βασίζονται στον εντοπισμό ακμών (edges) ή στο ταίριασμα χαρακτηριστικών (features) ή στον εντοπισμό μιας περιοχής μιας αρχικής εικόνας σε μια άλλη (template matching). Γι αυτό και προτιμήθηκαν στα [7], [8] και [9]. Ιδιαίτερα αξίζει να σταθούμε στη μέθοδο που χρησιμοποιήθηκε στα δύο πρώτα, τον αλγόριθμο TLD, αφού αυτός θα χρησιμοποιηθεί και σε αυτή εδώ την εργασία. Ο αλγόριθμος αυτός έχει σχεδιαστεί έτσι ώστε να εντοπίζει το αντικείμενο μέσα στο κάθε στιγμιότυπο της εικόνας (detection), να το ακολουθεί σε μία σειρά από στιγμιότυπα (tracking) και ταυτόχρονα να εκπαιδεύεται με κάθε στιγμιότυπο που είτε περιέχει είτε δεν περιέχει το αντικείμενο που ακολουθεί (learning). Οι επιδόσεις του αλγορίθμου σε συνεργασία με το drone και η ήδη υπάρχουσα υλοποίηση του σε έτοιμο πακέτο του ROS, είναι που κάναν εύκολη την επιλογή του για τη λύση του προβλήματος αυτής της εργασίας. Τέλος, στο [10] γίνεται χρήση ειδικών ετικετών (tags) τα οποία είναι πάνω στο αντικείμενο-στόχο, ή είναι τα ίδια ο στόχος και μέσω ενός αλγορίθμου εντοπίζονται μέσω της κάμερας του drone. 2.2. Μέθοδοι πλοήγησης του drone Περνώντας στη δεύτερη φάση της προσέγγισης/παρακολούθησης ενός στόχου, την πλοήγηση του drone, συναντάμε δύο κύριες μεθόδους: Τον PID έλεγχο μέσω του σφάλματος της θέσης και τον έλεγχο μέσω της σχετικής θέσης του drone με το αντικείμενο. Η πρώτη μέθοδος εφαρμόζεται στα [8] και [10], όπου γίνεται χρήση του πακέτου tum_ardrone για το ROS, το οποίο περιέχει τρεις κόμβους, έναν για VSLAM, ένα Extended Kalman Filter και έναν για τον ελεγκτή PID ( [11] ). Ο πρώτος κόμβος χαρτογραφεί το χώρο και τοποθετεί το drone στον κόσμο με καθολικές συντεταγμένες και ο δεύτερος προσπαθεί να μοντελοποιήσει την κίνησή του ώστε να μπορεί να προβλέψει τη θέση του και την κατεύθυνσή του όταν δεν υπάρχουν δεδομένα από τους αισθητήρες. Ο κόμβος του ελεγκτή, με δεδομένες όλες τις τιμές των 6 μεταβλητών που ορίζουν τη θέση του drone και τις συντεταγμένες του στόχου, υπολογίζει τρεις όρους για κάθε συντεταγμένη: Τον ποσοτικό (proportional) : Το σφάλμα θέσης. Η διαφορά της θέσης στόχου από αυτή που βρίσκεται το drone αυτή τη στιγμή. Τον ολοκληρωτικό (integral): Το άθροισμα όλων των σφαλμάτων θέσης από τη στιγμή που τέθηκε ο στόχος. Τον παραγοντικό (derivative): Ο ρυθμός μεταβολής του σφάλματος θέσης. Στη συγκεκριμένη υλοποίηση, αυτός αντιστοιχεί στην αντίθετη ταχύτητα του drone. 11

Αφού υπολογιστούν αυτοί οι όροι, πολλαπλασιάζονται με κατάλληλα κέρδη τα οποία έχουν υπολογιστεί πειραματικά, έτσι ώστε το drone να κινείται με σταθερότητα, και προστίθενται. Το άθροισμα που προκύπτει από κάθε συντεταγμένη είναι η νέα εντολή για κάθε μία από τις τέσσερις κινήσεις που μπορεί να κάνει το drone: Οι όροι από τις συντεταγμένες στο επίπεδο (x, y), ορίζουν τις εντολές για την κίνηση του drone μπροστά/πίσω και αριστερά/δεξιά (τιμές pitch και roll αντίστοιχα). Ο όρος από τη συντεταγμένη του ύψους (z) ορίζει την εντολή για αύξηση/μείωση του ύψους του drone. Ο όρος για τη διεύθυνση στην οποία θα κοιτάει το drone, ορίζει την εντολή που θα το περιστρέψει γύρω από τον κάθετο άξονά του (τιμή yaw). Η διαδικασία αυτή επαναλαμβάνεται έως ότου το drone να φτάσει σε μια ικανοποιητική ακτίνα από το στόχο, οπότε και στέλνεται εντολή αναμονής σ αυτή τη θέση μέχρι να έρθει επόμενη εντολή. Η δεύτερη μέθοδος χρησιμοποιεί κατά κύριο λόγο τη σχετική θέση του αντικειμένου από το drone για να στείλει κατάλληλες εντολές κίνησης. Η τεχνική αυτή ονομάζεται visual servoing. Στα [2], [7] και [9], αυτή υλοποιείται με έναν PID που αυτή τη φορά δέχεται σαν σφάλμα την απόσταση του αντικειμένου από το μέσο της εικόνας. Έτσι, οι εντολές που δίνονται στο drone έχουν ως στόχο το αντικείμενο να είναι κεντραρισμένο στην εικόνα και σε μια συγκεκριμένη απόσταση (συνήθως της αρχική) από αυτό. Με παρόμοιο τρόπο γίνεται η πλοήγηση και στο [6], όπου ο ελεγκτής δέχεται είσοδο το σφάλμα παρακολούθησης από τον CMT, που αναφέρθηκε παραπάνω. Στα [1] και [5] η διαδικασία ορίζεται σαφέστερα, αφού η εικόνα χωρίζεται σε 9 ζώνες, όπου αν το αντικείμενο βρεθεί σε κάθε μία από αυτές, δίνονται αντίστοιχες εντολές στο drone, όλες με στόχο το αντικείμενο να τοποθετηθεί στη μεσαία ζώνη. 12

3. Εργαλεία, αλγόριθμοι και υλικό Για να επιτευχθεί ο στόχος της διπλωματικής, έγινε χρήση μιας έτοιμης πλατφόρμας (drone) ώστε το επίκεντρο της εργασίας να είναι η ανάπτυξη κώδικα. Απαραίτητη κρίθηκε επίσης η ενσωμάτωση ήδη υπάρχοντος κώδικα για ορισμένες διαδικασίες είτε γιατί ήταν απαραίτητο είτε γιατί κάποιο μέρος του προβλήματος είχε ήδη λυθεί με πολύ καλά και αποδεκτά αποτελέσματα από τρίτους. 3.1. Υλικό (hardware) 3.1.1. AR.Drone 2.0 Το drone που θα χρησιμοποιηθεί για αυτή τη διπλωματική είναι το AR.Drone 2.0 της Parrot. Το AR.Drone 2.0 είναι ένα εμπορικό τετρακόπτερο (quadcopter), με σταθερούς έλικες, μικρό σχετικά σε μέγεθος, το οποίο το κάνει κατάλληλο για πτήσεις σε εσωτερικούς χώρους. Ενσωματωμένος υπάρχει ένας επεξεργαστής ARM Cortex A8 των 32 bit χρονισμένος στα 1GHz, ο οποίος τρέχει λειτουργικό Linux 2.6.32. Έχει μνήμη RAM DDR2 μεγέθους 1 Gbit στα 200 Hz. Διαθέτει αισθητήρες πίεσης και απόστασης (σόναρ), μαγνητόμετρο τριών αξόνων με ακρίβεια 6 μοιρών, επιταχυνσιόμετρο τριών αξόνων με ακρίβεια +-50mg και γυροσκόπιο τριών αξόνων με ακρίβεια 2000 μοιρών/δευτερόλεπτο. Το σύνολο των αισθητήρων ολοκληρώνουν οι δύο κάμερες, μία μπροστινή και μία στη βάση με κατεύθυνση προς το έδαφος. Η μπροστινή έχει δυνατότητα λήψης βίντεο σε ανάλυση 360p και 720p, με ταχύτητα 30fps. Η οριζόντια γωνία θέασής της είναι ευρεία στις 92 μοίρες. Η κατακόρυφη κάμερα έχει ταχύτητα λήψης 60fps σε ποιότητα QVGA, με οριζόντια γωνία θέασης 64 μοίρες, και χρησιμοποιείται κυρίως για την εκτίμηση της ταχύτητας του drone, μέσω visual odometry. Το drone επικοινωνεί με έναν υπολογιστή ή κινητό μέσω κεραίας Wi-Fi b,g,n. Για τη σύνδεση κάποιου αποθηκευτικού μέσου ή ενός GPS, διαθέτει θύρα USB 2.0. Η μπαταρία που διαθέτει είναι χωρητικότητας 1500 mah στα 11.1 V. Για πτήσεις σε εσωτερικό ή εξωτερικό χώρο, είναι διαθέσιμα δύο κελύφη. ένα με έμφαση στην προστασία των ελίκων (εσωτερικό) και ένα με έμφαση στην αεροδυναμική για σταθερότερες πτήσεις (εξωτερικό). Οι πτήσεις έχουν μέση διάρκεια 20 λεπτά. 13

Εικόνα 1 AR.Drone με εσωτερικό κέλυφος Εικόνα 2 AR.Drone με εξωτερικό κέλυφος 3.2. Αλγόριθμοι και προγραμματιστικά εργαλεία (software) 3.2.1. ROS Για να είναι εφικτή η επικοινωνία του drone με τον υπολογιστή με αυτόνομο τρόπο, πρέπει να υπάρχει μια πλατφόρμα η οποία θα διαχειρίζεται τη λήψη δεδομένων από το μεν και αποστολή εντολών από το δε. Η πλατφόρμα που επιλέχθηκε είναι το ROS 1 (Robotic Operating System). Το ROS προσφέρει έναν ευέλικτο τρόπο ανταλλαγής πληροφορίας μεταξύ δύο οντοτήτων (drone laptop) σε μορφή μηνυμάτων. Τη στιγμή που ορίζονται τα δεδομένα των μηνυμάτων, αυτά μεταδίδονται σε αντίστοιχα topics (θεματικές ενότητες). Για την επεξεργασία αυτών των μηνυμάτων, αναπτύσσεται κώδικας ο οποίος αποτελεί ένα κόμβο ROS και επιτελεί μια συγκεκριμένη λειτουργία. Κάθε κόμβος έχει τη δυνατότητα να έχει πρόσβαση σε κάποιο topic και να λαμβάνει τη πληροφορία που έχει σταλθεί σε μορφή μηνύματος. Μόλις γίνει η λήψη, ενεργοποιείται μια κατάλληλη συνάρτηση για να επεξεργαστεί τα δεδομένα. Ο ίδιος κόμβος μπορεί να γίνει και πομπός, στέλνοντας τα δεδομένα του σε όποιο topic επιθυμεί. Την αποστολή και λήψη μηνυμάτων, την προτεραιότητα και τη συχνότητα με την οποία θα ενεργοποιούνται οι κατάλληλες συναρτήσεις των κόμβων τις αναλαμβάνει και τις διαχειρίζεται με αυτόματο τρόπο το ROS. 1 http://www.ros.org/about-ros/ 14

3.2.1.1. Βασικοί ορισμοί/έννοιες του ROS Κόμβος εγγραφής σε topic (subscriber node): Κόμβος ο οποίος έχει εγγραφεί σε ένα topic και μπορεί να λαμβάνει κάθε μήνυμα που γίνεται διαθέσιμο σ αυτό από άλλους κόμβους Κόμβος αποστολής δεδομένων σε topic (publisher node): Κόμβος ο οποίος οργανώνει τα δεδομένα του σε κατάλληλες δομές (μηνύματα) και τα στέλνει στα αντίστοιχα topic. Ένας κόμβος μπορεί να ανήκει και στις δύο κατηγορίες και να εγγράφεται αλλά και να στέλνει σε παραπάνω από ένα topics. Υπηρεσία (service): Είναι μία δομή την οποία καλεί ένας κόμβος με κατάλληλα δεδομένα εισόδου για να επεξεργαστούν και να δοθεί πίσω στον κόμβο το αποτέλεσμα της επεξεργασίας. 3.2.2. Πακέτα του ROS Όταν δημιουργηθούν πολλοί κόμβοι οι οποίοι επιτελούν μία πιο σύνθετη δομή, τότε μπορούν όλοι μαζί να συνδυαστούν και είναι διαθέσιμοι ως ένα πακέτο του ROS. Για αυτή τη διπλωματική χρησιμοποιήθηκαν τα εξής πακέτα, προσφερόμενα από την επιστημονική κοινότητα: ardrone_autonomy 2 : Το πακέτο αυτό είναι ένα πρόγραμμα οδήγησης (driver), το οποίο μεταφράζει τις εντολές που στέλνονται από το laptop σε εντολές που καταλαβαίνει και εκτελεί το drone. Επιπρόσθετα, μέσω του προγράμματος αυτού γίνονται διαθέσιμα τα δεδομένα των αισθητήρων του drone στους διάφορους κόμβους που τα απαιτούν. Το πακέτο είναι βασισμένο στο AR.Drone SDK. 3 pandora_tld 4 : Σε αυτό το πακέτο είναι υλοποιημένη η λογική του αλγορίθμου που εντοπίζει και ακολουθεί το αντικείμενο μέσα στην εικόνα της κάμερας, του TLD. tum_ardrone 5 : Αυτό το πακέτο χρησιμοποιήθηκε μερικώς, κυρίως για την υλοποίηση του PID που αποτέλεσε βάση για τον ελεγκτή της διπλωματικής. 2 http://wiki.ros.org/ardrone_autonomy 3 http://developer.parrot.com/ 4 https://github.com/pandora-auth-ros-pkg/pandora_tld 5 http://wiki.ros.org/tum_ardrone 15

tum_simulator 6 : Το πακέτο αυτό περιέχει τον κώδικα μέσω του οποίου τρέχει σε προσομοίωση το AR.Drone 2.0. Οι αισθητήρες, οι κάμερες καθώς και το ίδιο το drone έχουν μοντελοποιηθεί ώστε να πλησιάζουν την πραγματικότητα. Αυτό δίνει τη δυνατότητα εκτέλεσης κώδικα που προορίζεται για το πραγματικό drone σε προσομοίωση του Gazebo Simulator, έτσι ώστε να γίνουν πιο γρήγορα η δοκιμή(testing) και η αποσφαλμάτωση(debugging) του. robot_localization 7 : Το πακέτο αυτό περιέχει τον κώδικα για ένα extended Kalman φίλτρο. Η λειτουργία που επιτελεί είναι ουσιαστικά να δέχεται τις μετρήσεις από τους διάφορους αισθητήρες του drone και να δίνει εκτιμήσεις για τη θέση και τη ταχύτητα του drone. Κατά τη διάρκεια εκτέλεσής του, δημιουργεί ένα μοντέλο για την κίνηση του drone, προσπαθώντας να την προβλέψει. Αυτό έχει αποτέλεσμα να μην κάνει απότομες αλλαγές σε μεταβλητές που παρακολουθεί και ορίζει μετά από μεμονωμένες, πιθανώς λαθεμένες μετρήσεις εφ όσον εκείνες δεν συμφωνούν με τις προβλεπόμενες του μοντέλου. Αν η απότομη αλλαγή αυτή επιμένει, τότε το φίλτρο θα φτάσει την τιμή της αντίστοιχης μεταβλητής στη νέα κατάσταση όσο πιο ομαλά γίνεται. 3.2.3. Gazebo Simulator Το Gazebo είναι η πλατφόρμα που χρησιμοποιήθηκε για την εκτέλεση πειραμάτων σε περιβάλλον προσομοίωσης. Επιλέχθηκε διότι υποστηρίζει κόμβους του ROS και έτσι τα πειράματα ήταν δυνατόν να γίνουν δίχως πολλές ρυθμίσεις και αλλαγές, είτε στον κώδικα είτε στο ίδιο το Gazebo. Το μοντέλο που είναι διαθέσιμο από το πακέτο tum_simulator τρέχει άψογα και συμπεριφέρεται όπως ένα πραγματικό drone, ενώ το Gazebo έχει διαθέσιμο για προσομοίωση ένα UGV, το Turtlebot 8, το οποίο είναι κατάλληλο για χρήση ως στόχος από το drone. 3.2.4. Αλγόριθμος TLD Για τη λύση του προβλήματος του εντοπισμού του αντικειμένου μέσω της κάμερας, χρησιμοποιήθηκε ο αλγόριθμος TLD ( [12] ). Ο αλγόριθμος αυτός αποτελείται από τρεις δομές: Τον tracker, τον learner και τον detector. O αλγόριθμος προσπαθεί να εντοπίσει ένα 6 http://wiki.ros.org/tum_simulator 7 http://wiki.ros.org/robot_localization 8 http://www.turtlebot.com/ 16

αντικείμενο είτε από ένα ήδη υπάρχον μοντέλο ενός αντικειμένου είτε από ένα πλαίσιο (bounding box) που θα πρέπει να σχεδιαστεί από τον χρήστη. 3.2.4.1. Tracker Ο tracker, για να μπορεί να παρακολουθήσει το αντικείμενο στη σειρά των στιγμιότυπων της κάμερας, χρειάζεται μόνο τη πληροφορία από το προηγούμενο στιγμιότυπο (frame). Για κάθε σημείο υπολογίζεται το optical flow με τη χρήση του Lucas- Kanade 9 tracker και έπειτα η μετακίνηση και η μεταβολή του bounding box χρησιμοποιώντας μια τεχνική που λέγεται forward-backward consistency. 3.2.4.2. Detector Ξεκινώντας, ο detector αναζητά το αντικείμενο σε κάθε στιγμιότυπο της εικόνας της κάμερας. Ο τρόπος που δουλεύει είναι ο εξής: Χωρίζει την εικόνα σε μικρότερες περιοχές (patches) και περνάει το καθένα ξεχωριστά από τρία φίλτρα για να αποφασίσει αν υπάρχει το αντικείμενο μέσα σε αυτές. Το πρώτο φίλτρο ελέγχει τη διασπορά της τιμής του γκρι χρώματος μέσα στο patch και το πόσο διαφέρει από την αρχική εικόνα. Αν η διαφορά είναι πάνω από 50%, τότε απορρίπτεται. Αν γίνει δεκτό, περνάει στο δεύτερο φίλτρο το οποίο αποτελείται από n classifiers (n = 10 στη δικιά μας περίπτωση). Κάθε classifier εκτελεί συγκρίσεις μεταξύ πλήθος ζευγαριών pixels, τα οποία είναι διαφορετικά για κάθε classifier. Αν οι εντάσεις των pixels σε ένα ζευγάρι είναι παρόμοιες, τότε ο classifier βάζει 1 στη αντίστοιχη θέση του ζευγαριού σε ένα πίνακα διάνυσμα με αριθμό θέσεων όσο και το πλήθος των ζευγαριών. Αλλιώς βάζει 0. Έτσι κάθε classifier παράγει ένα διάνυσμα από άσσους και μηδενικά. Το διάνυσμα αυτό, έπειτα, αντιστοιχίζεται σε έναν πίνακα ο οποίος δίνει τις πιθανότητες ένα patch που περιέχει το αντικείμενο, να έχει σαν διάνυσμα συγκρίσεων το συγκεκριμένο. Από τις πιθανότητες όλων των classifiers βγαίνει ένας μέσος όρος και αν αυτός είναι πάνω από 0.5, τότε το patch θεωρείται θετικό και περνάει στο τρίτο και τελευταίο φίλτρο, τον ταξινομητή πλησιέστερου γείτονα (Nearest Neighbor classifier). Στο τρίτο φίλτρο, το patch συγκρίνεται με βάση συγκεκριμένα κριτήρια για το πόσο παρόμοιο είναι με θετικά και αρνητικά patches που έχουν αποθηκευτεί στο παρελθόν από τη στιγμή που ξεκίνησε ο αλγόριθμος. Τέλος, υπολογίζεται το ποσοστό των παρόμοιων θετικών περιοχών στο σύνολο των περιοχών (αρνητικών και θετικών). Αν το ποσοστό είναι 9 http://cseweb.ucsd.edu/classes/sp02/cse252/lucaskanade81.pdf 17

πάνω από ένα όριο, τότε το patch θεωρείται ότι περιέχει το αντικείμενο και αποθηκεύεται μαζί με τα θετικά patches. 3.2.4.3. Learner O learner ουσιαστικά προσπαθεί να βελτιώνει την ακρίβεια με την οποία τα patches ταξινομούνται σε θετικά και αρνητικά. Αυτό το κάνει με δύο έννοιες, τον P-expert και τον N- expert. O P-expert υποθέτει ότι το αντικείμενο κινείται σε μία τροχιά. Έτσι μπορεί να πάρει ορισμένα παλιά θετικά bounding boxes και μαζί με τα patches που είναι παρόμοια με αυτά να σχηματίσουν έναν πυρήνα. Αν το bounding box που δίνεται από τον detector ή/και από τον tracker είναι μέσα σε αυτόν τον πυρήνα, τότε θεωρείται αξιόπιστο. Από την άλλη ο N- expert θεωρεί ότι το αντικείμενο βρίσκεται το πολύ μία φορά μέσα στην εικόνα. Έτσι, κάθε patch που ίσως να έχει θεωρηθεί θετικό αλλά είναι πολύ μακριά από το επιλεγμένο bounding box, θεωρούνται αρνητικά. 18

4. Υλοποίηση 4.1. Περίληψη Κεφαλαίου Σε αυτό το κεφάλαιο θα αναλυθεί ο τρόπος με τον οποίο έγινε η προσπάθεια επίλυσης του προβλήματος που πραγματεύεται η τρέχουσα διπλωματική. Αρχικά θα οριστούν οι παραδοχές και οι περιορισμοί του προβλήματος και έπειτα θα γίνει διεξοδική περιγραφή του αλγορίθμου προσέγγισης. 4.2. Παραδοχές περιορισμοί 4.2.1. Μορφή εντολών Για τον καλό ορισμό των εντολών που θα στέλνονται στο drone, ορίζονται δύο συστήματα συντεταγμένων. Το πρώτο είναι το καθολικό (world), με: Άξονα Χ την ευθεία που εκτείνεται κατά το μήκος του drone τη στιγμή λίγο πριν απογειωθεί, με τις θετικές τιμές να βρίσκονται μπροστά από την οριζόντια κάμερα. Άξονα Υ την ευθεία που εκτείνεται κατά το πλάτος του drone τη στιγμή λίγο πριν απογειωθεί, με τις θετικές τιμές να βρίσκονται στα αριστερά του. Άξονα Ζ τον κάθετο στο επίπεδο άξονα. Και το δεύτερο είναι το τοπικό σύστημα με κέντρο το drone, όπου έχει του ίδιους άξονες με το καθολικό, με τη διαφορά ότι με την αλλαγή κατεύθυνσης του drone, αλλάζουν με αντίστοιχο τρόπο και εκείνοι θέση σε σχέση με το καθολικό, το οποίο είναι σταθερό κατά τη διάρκεια της πτήσης. Στην Εικόνα 3 φαίνονται τα δύο συστήματα. Για αυτό το λόγο, πρώτα υπολογίζεται η θέση στην οποία πρέπει να φτάσει το drone στο καθολικό σύστημα και έπειτα η θέση αυτή μεταφράζεται στο πόσο αριστερά/δεξιά ή μπροστά/πίσω ή πάνω/κάτω από το drone βρίσκεται αυτή η θέση. Για τις έξι κινήσεις αυτές στο σύστημα συντεταγμένων του drone, ορίζονται και οι παρακάτω εντολές με τα κατάλληλα πρόσημα, σε μορφή μεταβλητών μηνύματος ROS: +linear.x : Κίνηση προς τα εμπρός -linear.x : Κίνηση προς τα πίσω +linear.y : Κίνηση προς τα αριστερά -linear.y : Κίνηση προς τα δεξιά +linear.z : Κίνηση προς τα πάνω -linear.z : Κίνηση προς τα κάτω 19

+angular.z : Στροφή γύρω από τον άξονα z προς τα αριστερά -angular.z : Στροφή γύρω από τον άξονα z προς τα δεξιά Οι κινήσεις αυτές μπορούν να εκτελεστούν ταυτόχρονα, δίνοντας την ικανότητα στο drone να κινείται με πιο σύνθετες μανούβρες. Εικόνα 3 Τοπικό και καθολικό σύστημα συντεταγμένων 4.2.2. Ιδιότητες κίνησης αντικειμένου-στόχου Το αντικείμενο που θα παρακολουθεί το drone θα πρέπει να κινείται στο έδαφος σε χώρο χωρίς εμπόδια. Το αντικείμενο θα μπορεί να εκτελεί περιστροφική κίνηση, αρκεί να γίνεται ομαλά και η συνολική του ταχύτητα να μην ξεπερνάει τα 0.2 m/s, καθώς υπάρχει μεγάλη πιθανότητα πάνω στην εναλλαγή της κάμερας, το αντικείμενο να έχει βγει εντελώς από το οπτικό πεδίο της κατακόρυφης κάμερας. Το αντικείμενο είναι επιθυμητό να έχει διακριτά χαρακτηριστικά (features) στην πρόσοψη και στην κάτοψή του και χρώμα το οποίο να ξεχωρίζει από το παρασκήνιο (background) του, ώστε να μειωθεί η πιθανότητα εντόπισης του αντικειμένου σε λάθος σημείο του χώρου. Το μέγεθος του αντικειμένου πρέπει να είναι τέτοιο ώστε να φαίνεται ολόκληρο στην εικόνα από την αρχική απόστασή του από το drone και ταυτόχρονα να καταλαμβάνει χώρο στην εικόνα μεγαλύτερο από ένα κάτω όριο (περίπου 30 x 30 px), ώστε να μπορεί ο TLD να το παρακολουθεί. Με το ίδιο κριτήριο επιλέγεται και η αρχική απόσταση από το drone. Επίσης, η επιφάνεια στο πάνω μέρος του αντικειμένου θα πρέπει να φαίνεται ολόκληρη στην εικόνα της κατακόρυφης κάμερας. 20

Ακόμα, ο TLD λειτουργεί με δύο μοντέλα, τα οποία πρέπει να έχουν δημιουργηθεί εκ των προτέρων για το εκάστοτε αντικείμενο-στόχο. Ένα για την οριζόντια κάμερα και ένα για την κατακόρυφη. Η δημιουργία του πρώτου γίνεται βάζοντας το αντικείμενο στην αρχική απόσταση από το drone και, έχοντας ενεργοποιημένο τον TLD, σχεδιάζοντας στην εικόνα ένα πλαίσιο που να το περικλείει. Έπειτα, με ανοιχτή την κατακόρυφη κάμερα, τοποθετείται το drone σε μια απόσταση πάνω από το αντικείμενο και σχεδιάζεται πάλι ένα πλαίσιο που να περικλείει την κάτοψη του αντικειμένου. Τα δύο μοντέλα αποθηκεύονται σε δύο αρχεία τα οποία χρησιμοποιούνται στις αντίστοιχες φάσεις της προσέγγισης. Το πρώτο φορτώνεται στην αρχή λειτουργίας του κόμβου και το δεύτερο φορτώνεται από τον χρήστη με το πάτημα ενός κουμπιού του πληκτρολογίου, όταν ενεργοποιείται η κατακόρυφη κάμερα. 4.3. Κύριοι κόμβοι υλοποίησης Παρακάτω αναλύονται οι κύριοι κόμβοι που λαμβάνουν μέρος στην διαδικασία προσέγγισης ενός αντικειμένου. Ο κόμβος της οδομετρίας λαμβάνει δεδομένα από τους αισθητήρες του drone και στέλνει την κατάστασή του στο φίλτρο Kalman. Αυτό με τη σειρά του στέλνει την φιλτραρισμένη κατάσταση στου κόμβους του PID και του Approach. Ο PID λαμβάνει από το Approach ένα στόχο και στέλνει εντολές πτήσης στο drone. Η αρχιτεκτονική του συστήματος φαίνεται στην Εικόνα 4. Εικόνα 4 Αρχιτεκτονική συστήματος προσέγγισης 21

4.3.1. Κόμβος Οδομετρίας (custom_odom) Ο κόμβος αυτός δημιουργήθηκε με σκοπό να υπολογίζει τη θέση και τη ταχύτητα του drone και να στέλνει αυτά τα δεδομένα στο Κalman φίλτρο. Επίσης στο ίδιο φίλτρο στέλνει και τις μετρήσεις από το μαγνητόμετρο, το γυροσκόπιο και το επιταχυνσιόμετρο, ώστε το φίλτρο να υπολογίσει όσο ακριβέστερα γίνεται την κατάσταση (θέση, διεύθυνση και στροφή) του drone. Ο τρόπος με τον οποίο επιτυγχάνεται αυτό, είναι ο εξής: Ο κόμβος δέχεται τα δεδομένα από όλους τους αισθητήρες ώστε να γνωρίζει την απόσταση του drone από το έδαφος και την ταχύτητά του στους 3 άξονες. Οι μετρήσεις των ταχυτήτων δίνονται με βάση το τοπικό σύστημα συντεταγμένων και για αυτό τα διανύσματά τους προβάλλονται στο καθολικό σύστημα (Εικόνα 5) Οι εξισώσεις της προβολής είναι: [1] v xworld = cos(yaw) v xdrone sin(yaw) v ydrone [2] v yworld = sin(yaw) v xdrone + cos(yaw) v ydrone όπου yaw είναι η γωνία κατά την οποία έχει περιστραφεί το τοπικό σύστημα σε σχέση με το καθολικό. Παίρνει θετικές τιμές όταν η στροφή είναι αντι-ωρολογιακή. Έπειτα οι ταχύτητες αυτές ολοκληρώνονται στο χρόνο, όπως φαίνεται στις παρακάτω εξισώσεις για να υπολογιστεί το πόσο προχώρησε το drone κατά τον x και τον y καθολικό άξονα: [3] dx = v xworld dt [4] dy = v yworld dt Τέλος, αυτές οι ποσότητες προστίθενται στην αντίστοιχη υπάρχουσα τιμή για τη θέση του drone στους 3 άξονες, διαμορφώνοντας έτσι τη τωρινή θέση του. Η διεύθυνση και η στροφή του υπολογίζονται κατευθείαν από τις μετρήσεις του drone για τις αντίστοιχες γωνίες στροφής χωρίς κάποια επεξεργασία. Εγγραφή γίνεται και στα topics των προαναφερθέντων αισθητήρων. Το επιταχυνσιόμετρο δίνει τις μεταβολές της ταχύτητας στους τρεις άξονες. Το γυροσκόπιο δίνει τις τιμές των γωνιών κατά τις οποίες έχει στραφεί το drone γύρω από τους άξονες x (τιμή roll) και y (τιμή pitch), με κέντρο των αξόνων το drone. Το μαγνητόμετρο δίνει την τιμή της γωνίας περιστροφής κατά των κάθετο άξονα z (τιμή yaw). Όλα τα δεδομένα θέσης και στροφής στέλνονται σε αντίστοιχα topics, στα οποία έχει κάνει εγγραφή ο κόμβος του Kalman φίλτρου. Το φίλτρο χρησιμοποιήθηκε διότι η τεχνική της 22

ολοκλήρωσης των ταχυτήτων για την εύρεση της μετακίνησης του drone δίνει σφάλματα κατά τη διάρκεια της πτήσης. Επίσης σφάλματα μπορεί να υπάρξουν στις μετρήσεις των αισθητήρων του drone. Στο φίλτρο, όλα αυτά θα συνυπολογιστούν ώστε να βγει όσο το δυνατόν ακριβέστερο το μοντέλο κίνησης του drone. Εικόνα 5 Προβολή ταχυτήτων στο καθολικό σύστημα και μετατόπιση θέσης drone 4.3.2. Κόμβος ελεγκτή (PID) Αυτό ο κόμβος εγγράφεται στο topic όπου στέλνονται οι θέσεις-στόχοι για το drone καθώς και στα topics που στέλνει το φίλτρο Kalman την κατάσταση του drone. Ο στόχος αποτελείται από ένα σημείο στον χώρο (x, y, z), μία γωνίας στροφής κατά τον κατακόρυφο άξονα (yaw) και ένα διάνυσμα ταχυτήτων στο οριζόντιο επίπεδο (v x, v y ). Όλες οι τιμές είναι με βάση το καθολικό σύστημα αναφοράς. Ο κόμβος αποθηκεύει αυτές τις τιμές και όσο δεν λαμβάνει κάποιο καινούριο στόχο, τις επεξεργάζεται και στέλνει κατάλληλες εντολές στο topic το οποίο παρακολουθεί το πακέτο ardrone_autonomy. Αυτό με τη σειρά του τις στέλνει με κατάλληλη μορφή στο drone. Η δομή της συνάρτησης που υπολογίζει τις κατάλληλες εντολές είναι εμπνευσμένη από το πακέτο tum_ardrone, στο οποίο υλοποιούνται 3 ελεγκτές: Ένας για τις τιμές x, y, ένας για τη τιμή z, και ένας για τη τιμή yaw. Η γενική εξίσωση του PID είναι: 23

[5] u(t) = K p e(t) + K d de(t) dt + K i t 0 e(τ) dτ,όπου u(t) είναι η συνάρτηση ελέγχου, e(t) είναι η συνάρτηση σφάλματος της ελέγξιμης μεταβλητής και K p, K d, K i είναι τα κέρδη του ελεγκτή. Για τους 3 ελεγκτές, ως συνάρτηση σφάλματος θεωρείται η διαφορά της τωρινής τιμής της ελέγξιμης τιμής από την επιθυμητή και αποτελεί το ποσοτικό σφάλμα. Η παράγωγος της συνάρτησης σφάλματος είναι το διαφορικό σφάλμα και στη συγκεκριμένη περίπτωση είναι ο ρυθμός μεταβολής της κάθε μεταβλητής με το αντίθετο πρόσημο. Ως ολοκληρωτικό σφάλμα θεωρείται το ολοκλήρωμα της συνάρτησης σφάλματος, δηλαδή το άθροισμα όλων των σφαλμάτων που έχουν υπολογιστεί από τη στιγμή που άρχισε ο έλεγχος πολλαπλασιαζόμενο με αυτό το χρονικό διάστημα. Τα τρία αυτά σφάλματα, ποσοτικό (P), ολοκληρωτικό (I) και παραγοντικό (D), πολλαπλασιάζονται το καθένα με κατάλληλα κέρδη και προστίθενται μεταξύ τους. Το αποτέλεσμα αυτής της πράξης αποτελεί την εντολή που θα σταλεί στο drone για κίνηση στον αντίστοιχο άξονα του drone. Οι εντολές αυτές, όμως, ορίζουν την κλίση του drone στον εκάστοτε άξονα ως ποσοστό μια μέγιστης γωνίας κλίσης (συνήθως 30 μοιρών) και η κλίση μπορεί να ελέγξει μόνο την επιτάχυνση. Έτσι, στη θέση-στόχο υπάρχει μηδενική επιτάχυνση, όμως δεν εξασφαλίζεται πάντα μηδενική ταχύτητα, με αποτέλεσμα να υπάρχουν ταλαντώσεις γύρω από το επιθυμητό στόχο μέχρι να ισορροπήσει στην κατάλληλη θέση το drone. Για αυτό το λόγο, στον PID προστέθηκε ένα νέο επίπεδο ελέγχου, το οποίο δέχεται σαν είσοδο το αποτέλεσμα της πρόσθεσης όπως υπολογίστηκε και το θεωρεί ως στόχο για τη ταχύτητα του drone. Η διαφορά της από την τωρινή ταχύτητα αποτελεί τη συνάρτηση σφάλματος και ως παραγοντικό σφάλμα ορίζεται η διαφορά της τωρινής από τη προηγούμενη ταχύτητα του drone. Τα σφάλματα αυτά πολλαπλασιάζονται με κατάλληλα κέρδη και προστίθενται μεταξύ τους ώστε να παραχθεί η τελική εντολή για την κίνηση του drone. Το επιπλέον αυτό επίπεδο είναι μόνο για την κίνηση ως προς τον άξονα x και y του drone. Η λογική του 2ου επιπέδου ελέγχου φαίνεται στις παρακάτω εξισώσεις: [6] v xtarget = kp speed pterm x + kd speed dterm x + ki speed iterm x [7] v ytarget = kp speed pterm y + kd speed dterm y + ki speed iterm y [8] a x p = kp accel (v xtarget dterm x ) [9] a y p = kp accel (u ytarget dterm y ) [10] a x d = kd accel (u xlast + dterm x ) [11] a y d = kd accel (u ylast + dterm y ) 24

[12] a xtarget = a x p + a x d [13] a ytarget = a y p + a y d όπου v xtarget και v ytarget είναι οι ταχύτητες στόχοι που έχουν υπολογιστεί από το πρώτο επίπεδο, dterm είναι η ταχύτητα στον αντίστοιχο άξονα με αντίθετο πρόσημο και u last είναι η προηγούμενη ταχύτητα στον ίδιο άξονα. Οι εξισώσεις [12] και [13] δείχνουν τις τελικές εντολές που στέλνονται στο drone για τις κινήσεις στους τοπικούς άξονες x και y. Αφού έχουν υπολογιστεί τα αθροίσματα και από τους τρεις ελεγκτές, οι τιμές αυτές συγκρίνονται με καθορισμένα κάτω και άνω όρια. Αν η εκάστοτε τιμή ξεπερνά κάποιο από τα δύο όρια, τότε ανατίθεται σε αυτή η τιμή του ορίου που ξεπέρασε. Τέλος, πριν από κάθε εκτέλεση της συνάρτησης υπολογισμού των εντολών ελέγχου, μία συνάρτηση ελέγχει αν το drone έχει φτάσει σε μια περιοχή κοντά στο σημείο-στόχο και έχει παραμείνει μέσα σε αυτήν και ένα καθορισμένο χρονικό διάστημα. Αν ο έλεγχος βγει θετικός, τότε ο κόμβος σταματάει να στέλνει περαιτέρω εντολές ελέγχου και στέλνει εντολή αιώρησης (hover) στο σημείο που έχει φτάσει. Αλλιώς συνεχίζει την διαδικασία ελέγχου. Οι μέγιστες και προεπιλεγμένες τιμές των κερδών για τα δύο επίπεδα ελέγχου, των οριακών εντολών που είναι δυνατόν να σταλούν στο drone και των αποστάσεων που ορίζουν την περιοχή γύρω από το σημείο όπου σταματάει ο έλεγχος, βρίσκονται στον παρακάτω πίνακα. Οι τιμές επιλέχθηκαν βάσει πειραμάτων. Πίνακας 1 Τιμές παραμέτρων PID ελέγχου Όνομα Περιγραφή Προεπιλεγμέ νη Τιμή Ki_yaw Ολοκληρωτικό κέρδος για το yaw Kd_yaw Παραγοντικό κέρδος για το yaw Kp_yaw Ποσοτικό κέρδος για το yaw max_yaw Μέγιστη τιμή για τον έλεγχο του yaw Ki_gaz Ολοκληρωτικό κέρδος για την ταχύτητα στον άξονα z Ελάχιστη Τιμή Μέγιστη Τιμή 0.0 0.0 1.0 0.0 0.0 1.0 0.04 0.0 1.0 1.0 0.0 1.0 0.001 0.0 1.0 25

Kd_gaz Kp_gaz rise_fac max_gaz_rize max_gaz_drop Ki_speed Kd_speed Kp_speed max_speed initialreacheddistance staywithindist staywithinyaw staytimecheck Παραγοντικό κέρδος για την ταχύτητα στον άξονα z Ποσοτικό κέρδος για την ταχύτητα στον άξονα z Πολλαπλασιαστής θετικών τιμών ελέγχου της ταχύτητας στον άξονα z Μέγιστη θετική τιμή ελέγχου της ταχύτητας στον άξονα z Μέγιστη αρνητική τιμή ελέγχου της ταχύτητας στον άξονα z Ολοκληρωτικό κέρδος για τον έλεγχο της ταχύτητας στους άξονες x, y Παραγοντικό κέρδος για τον έλεγχο της ταχύτητας στους άξονες x, y Ποσοτικό κέρδος για τον έλεγχο της ταχύτητας στους άξονες x,y Μέγιστη τιμή ελέγχου ταχύτητας στους άξονες x, y Τιμή αρχικής αποδεκτής απόστασης από τον στόχο (m) Τιμή δεύτερης αποδεκτής απόστασης από τον στόχο (m) Τιμή αποδεκτού σφάλματος για το yaw (μοίρες) Διάρκεια που πρέπει να μείνει το drone μέσα σε αποδεκτή περιοχή (ms) 0.05 0.0 1.0 0.5 0.0 1.0 2.5 0.0 5.0 0.7 0.0 1.0-0.5-1.0 0.0 0.0 0.0 1.0 0.3 0.0 1.0 0.8 0.0 1.0 11.0 0.0 11.0 0.1 0.0 1.0 0.15 0.0 1.0 5.0 0.0 10.0 20 0.0 1000 26

4.3.3. Κόμβος προσέγγισης (approach) Αυτός ο κόμβος είναι υπεύθυνος για την αποστολή σημείων στόχων στο χώρο στον PID μετά από επεξεργασία της τωρινής κατάστασης του drone και της θέσης του πλαισίου του αντικειμένου στην εικόνα από την εκάστοτε ενεργοποιημένη κάμερα. Ο κόμβος εγγράφεται στο topic όπου ο TLD μεταδίδει τη θέση του bounding box στην εικόνα σε μορφή μηνύματος που περιέχει ένα σημείο με τις συντεταγμένες της πάνω αριστερά γωνίας του πλαισίου σε pixels και ένα σημείο με το ύψος και το πλάτος του πλαισίου. Ακόμα, εγγραφή γίνεται και στο topic του kalman φίλτρου για τη λήψη της τωρινής κατάστασης του drone. Για όλους τους απαραίτητους υπολογισμούς, χρησιμοποιείται το μεσαίο σημείο της κάτω πλευράς του πλαισίου όταν είναι σε χρήση η οριζόντια κάμερα και το μεσαίο σημείο του πλαισίου όταν είναι ενεργοποιημένη η κατακόρυφη κάμερα. Οι άξονες της εικόνας έχουν ως αρχή την πάνω αριστερά γωνία της, με τα x να αυξάνονται όσο προχωράει κανείς προς τα δεξιά της εικόνας και τα y όσο προχωράει κανείς προς τα κάτω. Η ανάλυση της εικόνας είναι 360 * 640 και για αυτό το μέσο της εικόνας είναι το σημείο (180, 320 ). Παρακάτω, θα γίνει περιγραφή της γενικής διαδικασίας προσέγγισης. Έπειτα θα δοθεί έμφαση στους υπολογισμούς της απόστασης, των ταχυτήτων στους δύο καθολικούς άξονες x,y και της κατεύθυνσης του αντικειμένου αναλόγως την ενεργοποιημένη κάμερα, καθώς και στις ενέργειες που πραγματοποιούνται όταν το αντικείμενο δεν είναι ορατό. 4.3.3.1. Αρχικοποίηση τιμών και σταθερές Για τη σωστή λειτουργία του αλγορίθμου, απαραίτητη είναι η γνώση του εστιακού μήκους (focal length) της κάθε κάμερας σε pixels. Αυτό υπολογίζεται με την παρακάτω εξίσωση: [14] f = l px /tan(φ/2), όπου f, το εστιακό μήκος, l px το μήκος της εικόνας σε pixels και φ η γωνία του φακού σε μοίρες. Όμως λόγω της αναλογίας απεικόνισης (aspect ratio) δεν έχουμε το ίδιο εστιακό μήκος για τον οριζόντιο και τον κάθετο άξονα της εικόνας 10. Άρα υπολογίζουμε δύο μήκη, 10 http://wiki.ros.org/image_pipeline/camerainfo 27

ένα για την οριζόντια και ένα για την κάθετη γωνία του οπτικού πεδίου της εικόνας. Στη συγκεκριμένη περίπτωση, υπολογίστηκαν ως: Πίνακας 2 Εστιακά μήκη καμερών (pixels) Οριζόντιο εστιακό μήκος Κάθετο εστιακό μήκος Οριζόντια κάμερα Κάθετη Κάμερα Οριζόντια Κάμερα Κάθετη Κάμερα 303.66750 512.107049 352.17 510.209683 Επίσης, θεωρείται γνωστό το μήκος από το κέντρο του drone μέχρι τον φακό της οριζόντιας κάμερας καθώς και το ύψος του αντικειμένου που θα προσεγγίσει το drone. 4.3.3.2. Διαδικασία προσέγγισης Αρχικά, ο κόμβος δεν στέλνει καμία εντολή στο drone προτού να είναι βέβαιο ότι έχει απογειωθεί. Για αυτό και ελέγχει από τα δεδομένα του drone την μεταβλητή κατάστασης πτήσης. Όταν στη μεταβλητή δοθεί η κατάλληλη τιμή, τότε ξεκινάει ο κόμβος να στέλνει εντολές. Ο κόμβος στον οποίο υλοποιείται ο TLD, στέλνει ένα μήνυμα με μηδενικές τιμές αν το αντικείμενο δεν είναι ορατό. Έτσι, για όσο χρόνο αφού έχει απογειωθεί το drone, το αντικείμενο δεν είναι ορατό, το drone παίρνει εντολή να παραμένει στο ίδιο αρχικό σημείο στο χώρο και να περιστρέφεται με αντι-ωρολογιακή φορά, μέχρι να βρει το αντικείμενο. Αν δεν το βρει, ενώ έχει κάνει ένα πλήρη κύκλο, ο κόμβος δίνει εντολή να μειωθεί το ύψος του drone και να επαναληφθεί η διαδικασία. Για να μην στρέφεται με μεγάλη ταχύτητα το drone, η τιμή του yaw αυξάνεται πάντα κατά μία μικρή τιμή (εδώ, κατά 5 μοίρες). Η μικρή ταχύτητα περιστροφής βοηθάει στην απόδοση του TLD. Αν βρεθεί το αντικείμενο σε κάποιο στιγμιότυπο της ενεργοποιημένης κάμερας για πρώτη φορά από την στιγμή απογείωσης, τότε αρχίζει η διαδικασία προσέγγισης. Μετά τον πρώτο εντοπισμό του αντικειμένου, σε κάθε επανάληψη της συνάρτησης προσέγγισης, ελέγχεται αρχικά ποια κάμερα είναι ενεργοποιημένη. Αυτό γίνεται μέσω μιας λογικής μεταβλητής, η ο οποία είναι true όταν είναι ανοιχτή η κάθετη κάμερα. Η αρχικοποιημένη τιμή της είναι false, αφού ξεκινάει η διαδικασία με ανοιχτή την οριζόντια κάμερα. Μετά από αυτόν τον έλεγχο, προσδιορίζεται αν το αντικείμενο παραμένει ορατό από την οριζόντια κάμερα ή όχι. Αν είναι ορατό, τότε υπολογίζεται η απόσταση του αντικειμένου από το drone, η γωνία που πρέπει να στραφεί έτσι ώστε το αντικείμενο να εμφανίζεται στην μεσαία στήλη της εικόνας και η γωνία υπό την οποία το παρακολουθεί. Αν η τελευταία είναι 28

μικρότερη από ένα όριο, τότε στο drone θα σταλεί νέο ύψος στο οποίο πρέπει να ανέβει ώστε να αυξηθεί η γωνία πάνω από το όριο. Οι διορθώσεις αυτές γίνονται ώστε το αντικείμενο να μην κινδυνεύει να βγει εκτός οπτικού πεδίου αλλά και για να μην εμφανίζεται πολύ κοντά στη μέση της εικόνας, όπου η γωνία είναι αρκετά μικρή και ένα μικρό σφάλμα στον υπολογισμό της μπορεί να έχει σαν αποτέλεσμα σφάλμα έως και 1m από την πραγματική απόσταση του αντικειμένου. Το νέο ύψος συγκρίνεται επίσης με δύο όρια ένα άνω και ένα κάτω ώστε να μην δοθούν εντολές που θα οδηγήσουν το drone σε ασταθή συμπεριφορά. Η γωνία στρέψης του drone αλλά και το νέο ύψος που πρόκειται να δοθούν ως στόχος στο drone συγκρίνονται με καθορισμένες τιμές. Αν τις ξεπερνούν, τότε δίνεται προτεραιότητα στις αντίστοιχες κινήσεις, δηλαδή στροφή γύρω από τον άξονα Ζ ή/και κίνηση κατ αυτόν, ενώ σαν θέση-στόχο στο επίπεδο ΧΥ δίνεται η τωρινή θέση του drone. Με αυτόν τον τρόπο αποφεύγονται σύνθετες μανούβρες οι οποίες θα οδηγούσαν στο να χαθεί η οπτική επαφή με το αντικείμενο. Έπειτα, αν δεν απαιτείται μεγάλη μεταβολή στο ύψος ή στην γωνία στρέψης του drone, η απόσταση που υπολογίστηκε προβάλλεται στους καθολικούς άξονες x και y και οι προβολές προστίθενται στις τωρινές συντεταγμένες που βρίσκεται το drone. Οι νέες συντεταγμένες είναι η θέση του αντικειμένου εκείνη τη στιγμή. Στη συνέχεια υπολογίζεται η ταχύτητα και η κατεύθυνση του αντικειμένου στο καθολικό σύστημα (Βλέπε Παρ. 4.3.3.3.1) και οι ταχύτητες που προέκυψαν στέλνονται ως μέρος του μηνύματος στόχου στον PID. Αυτό γίνεται διότι το drone πρέπει να έχει την ίδια ταχύτητα με το αντικείμενο όταν θα φτάσει στη θέση-στόχο. Μετά από κάθε αποστολή στόχου, αυτός αποθηκεύεται στη μνήμη για επόμενη χρήση σε συγκρίσεις ή/και υπολογισμούς. Καθώς το drone πλησιάζει το αντικείμενο, σε κάποια απόσταση από αυτό και για ένα συγκεκριμένο ύψος, θα χαθεί η οπτική επαφή μαζί του. Η ανίσωση που εξετάζεται είναι η [15] z drone distleft+ disttraveled 0.5, όπου zdrone είναι το ύψος του drone, distleft είναι η απόσταση που drone από το αντικείμενο τη δεδομένη στιγμή και disttraveled είναι η απόσταση που πιθανώς να έχει διανύσει το αντικείμενο μέσα στο χρονικό διάστημα που δεν είναι ορατό, βάσει της υπολογισμένης ταχύτητάς του. Η απόσταση distleft υπολογίζεται βάσει της παρακάτω εξίσωσης: 29

[16] distleft = lastobjdistance (lostx currx) 2 + (losty curry 2 ),όπου lastobjdistance είναι η τελευταία απόσταση από το αντικείμενο που υπολογίστηκε, (lostx, losty) είναι το σημείο που ήταν το drone πριν χάσει επαφή με το αντικείμενο και (currx, curry) είναι το σημείο στο οποίο βρίσκεται τώρα το drone. Αν η ανίσωση είναι αληθής, τότε δίνεται εντολή να αλλάξει η κάμερα η οποία είναι ενεργοποιημένη. Έτσι, ανοίγει η κατακόρυφη κάμερα. Ταυτόχρονα, υπολογίζεται σε ποιο ύψος πρέπει να ανέβει το drone, έτσι ώστε να αντισταθμιστεί η διαφορά της απόστασης από το drone και το αντικείμενο να βρεθεί στο οπτικό πεδίο της κάμερας. Το ύψος υπολογίζεται ως [17] znew = (distleft+disttraveled) tan(32) Εκτός από το z, αλλάζει και η κατεύθυνση του drone και υπολογίζεται ώστε o τοπικός άξονας x να είναι κάθετος στην κατεύθυνση του αντικειμένου (Εικόνα 6). Έτσι, θα υπάρχει μεγαλύτερη πιθανότητα το αντικείμενο να βρεθεί στο οπτικό πεδίο του drone, αφού καλύπτεται μεγαλύτερο μέρος της περιοχής που πιθανώς κινείται το αντικείμενο. Εικόνα 6 Διεύθυνση drone σε σχέση με τη κατεύθυνση του αντικειμένου Τα νέα z και yaw στέλνονται, μαζί με τις ήδη υπολογισμένες συντεταγμένες και ταχύτητες του αντικειμένου και από τον χρήστη φορτώνεται το μοντέλο του αντικειμένου, 30

για την κατακόρυφη κάμερα. Μόλις το αντικείμενο βρεθεί στο οπτικό πεδίο της κάμερας, τότε αρχίζουν εκ νέου οι υπολογισμοί της απόστασης από το αντικείμενο, η ταχύτητα και η κατεύθυνσή του. Σε αυτή τη φάση της προσέγγισης, το drone πρέπει με ορισμένα κριτήρια να αρχίζει να μειώνει το ύψος του, μέχρι να βρεθεί σε κατάλληλο ύψος, ώστε να στείλει εντολή προσγείωσης. Το κριτήριο αυτό θα είναι το μέγεθος της απόστασης από το αντικείμενο και το ύψος στο οποίο βρίσκεται. Ο παρακάτω πίνακας δείχνει τις διάφορες περιπτώσεις για το ύψος που θα σταλεί στο drone. Πίνακας 3 Ύψος στόχου για διάφορες αποστάσεις του αντικειμένου ΎΨΟΣ > 1M ΎΨΟΣ <= 1M ΑΠΟΣΤΑΣΗ <= 0.05 z t 0.05 z t ΑΠΟΣΤΑΣΗ <= 0.1 z t 0.03 z t 0.01 ΑΠΟΣΤΑΣΗ > 0.1 z d z d Όπου z t είναι το ύψος που έχει σταλεί στην προηγούμενη εκτέλεση του κόμβου ενώ z d το τωρινό ύψος του drone. Είναι εμφανές ότι ακολουθείται μια αρκετά επιθετική προσέγγιση αφού όσο πιο κοντά βρίσκεται το drone στο αντικείμενο, τόσο πιο γρήγορα μειώνεται το ύψος του στόχου. Η διαφοροποίηση με βάση το ύψος του drone έγινε διότι ήταν απαραίτητο να μειωθεί η επιθετικότητα όταν η υψομετρική διαφορά ανάμεσα στο αντικείμενο και το drone είναι μικρή και υπάρχει κίνδυνος να χαθεί το πρώτο από το οπτικό πεδίο της κάμερας. Τέλος, όταν η υψομετρική διαφορά ανάμεσα στο αντικείμενο και το drone γίνει μικρότερη από 0.47m και η οριζόντια απόσταση από το αντικείμενο είναι κάτω από τα 0.055m, δίνεται εντολή στο drone να προσγειωθεί. Το όριο στην υψομετρική διαφορά δόθηκε βάσει πειραμάτων, καθώς για μικρότερο ύψος, το αντικείμενο κάλυπτε πάνω από το 90% της εικόνας, δυσκολεύοντας τον TLD να εντοπίσει το αντικείμενο, παρόλο που βρισκόταν στο οπτικό πεδίο της κάμερας. Η ίδια εντολή δίνεται εάν το αντικείμενο δεν είναι ορατό από τον TLD, όμως είναι δεδομένο ότι το drone βρίσκεται πάνω από το αντικείμενο και η υψομετρική τους διαφορά είναι αυτή τη φορά μικρότερη από 0.35m. Η εντολή προσγείωσης εκτελείται στέλνοντας ένα κενό μήνυμα στο αντίστοιχο topic. Στην Εικόνα 7 φαίνεται το διάγραμμα ροής του αλγορίθμου προσέγγισης. 31

NO Open Node takeoff YES set reset to true initialize variables Receive Bounding Box set reset to false NO bounding box empty YES reset NO send initial point YES front bottom YES NO which camera open NO YES bounding box empty bounding box empty set reset to true YES timepassed > 20s send last good position YES object lost from bottom camera is object NO close enough calculate time passed YES timepassed>7s NO YES switch to bottom camera calculate new height calculate new target obj speed and direction new height > threshold1 yawdifference > threshold2 YES calculate new target obj speed and direction NO YES switch to front camera calculate passed time timepassed > 20 NO droneheight> 0.8+objHeight NO send new target directed to the lost object increase height target.x = currx target.y = curry send Target NO (droneheight < objheight + 0.5 AND objdistance < 0.05) OR (Is drone above object AND droneheight < objheight + 0.35) land drone YES Εικόνα 7 Διάγραμμα ροής κόμβου προσέγγισης 32

4.3.3.3. Υπολογισμός απόστασης a. Ενεργοποιημένη οριζόντια κάμερα Για τον υπολογισμό της απόστασης, αρχικά πρέπει να είναι γνωστή η θέση του αντικειμένου στην εικόνα, δεδομένου ότι το drone είναι πάντα παράλληλο με το έδαφος. Αυτό σημαίνει ότι κάθε κλίση γύρω από τους άξονες x και y του drone, πρέπει με κάποιο τρόπο να αντισταθμιστεί. Πρώτα, διορθώνεται η κλίση κατά τον άξονα y του drone (pitch). Μια τέτοια στροφή, αν είναι θετική, έχει ως αποτέλεσμα το αντικείμενο να έχει μικρότερη τεταγμένη (y) στην εικόνα, έχει πλησιάσει δηλαδή το πάνω μέρος της. Επίσης, ο άξονας y δεν περνάει από το σημείο όπου βρίσκεται ο αισθητήρας της εικόνας, αλλά από το μέσο του drone, δηλαδή 0.21m πίσω από τον αισθητήρα. Λαμβάνοντας όλα αυτά υπόψιν και κάνοντας χρήση της γεωμετρίας (Εικόνα 8), προέκυψαν οι παρακάτω τύποι για τον υπολογισμό της θέσης του αντικειμένου στην εικόνας χωρίς την θετική στροφή: [18] dangle = pitch + atan ( distfromcentery focallengthvertical ) [19] ddist = ( sin(pitch) + 1 cos(pitch)) distcenter tan(dangle) [20] tempdist = currz tan(dangle) [21] dist = tempdist ddist [22] distwithpitch = focallengthvertical currz dist 33

Εικόνα 8 Απαλοιφή θετικής στροφής κατά τον τοπικό άξονα y (οριζόντια κάμερα) Αν η στροφή είναι αρνητική (Εικόνα 9), τότε οι τιμές των tempdist και ddist υπολογίζονται όπως παρακάτω: [23] dz = (sin(pitch) + cos(pitch) distcenter tan( π dangle)) 2 [24] tempdist = currz+dz tan(dangle) [25] ddist = distcenter όπου pitch είναι η στροφή κατά τον άξονα y του drone, distfromcentery είναι η κάθετη απόσταση του σημείου του πλαισίου από το μέσο της εικόνας, focallengthvertical το κάθετο εστιακό μήκος και currz είναι το τωρινό ύψος του drone. Θεωρείται ότι επηρεάζεται μόνο η κάθετη απόσταση από το μέσο της εικόνας κατά τη στροφή στον συγκεκριμένο άξονα. 34

Εικόνα 9 Απαλοιφή αρνητικής στροφής κατά των τοπικό άξονα y (οριζόντια κάμερα) Έπειτα διορθώνεται η στροφή κατά τον εγκάρσιο άξονα x του drone (roll). Εδώ, θεωρείται ότι η στροφή γίνεται γύρω από το μεσαίο σημείο της εικόνας και άρα δεν αλλάζει η ευκλείδεια απόστασή του σημείου του πλαισίου από αυτό (Εικόνα 10). Έτσι προέκυψαν οι παρακάτω εξισώσεις: [26] distfromcenter = (imagecenterx bbcenterx) 2 + distwithpitch 2 [27] anglewithroll = acos ( distwithpitch distfromcenter ) [28] distfromcenterx = cos(anglewithroll + roll) distfromcenter [29] distfromcentery = sin(anglewithroll + roll) distfromcenter Αν το σημείο βρίσκεται στα δεξιά του μέσου της εικόνας, τότε η κάθετη απόσταση από το κέντρο υπολογίζεται ως: [30] distfromcentery = sin(anglewithroll roll) distfromcenter όπου imagecenterx είναι η x συντεταγμένη του κεντρικού σημείου της εικόνας, bbcenterx είναι η τετμημένη του σημείου του πλαισίου και roll είναι η γωνία περιστροφής γύρω από τον άξονα x του drone. 35

Εικόνα 10 Απαλοιφή στροφής κατά τον τοπικό άξονα x (οριζόντια κάμερα) Αφού έχουν υπολογιστεί οι παραπάνω αποστάσεις, στη συνέχεια υπολογίζονται η γωνία διόρθωσης στροφής, ώστε το αντικείμενο να βρεθεί στη κεντρική στήλη της εικόνας, η γωνία υπό την οποία έχει οπτική επαφή το drone με το αντικείμενο και η απόσταση του από αυτό. Η γωνία διόρθωσης υπολογίζεται ως εκείνη που έχει εφαπτομένη το πηλίκο της οριζόντιας απόστασης του σημείου του πλαισίου από το κέντρο και του εστιακού μήκους. Υπόψιν ότι η οριζόντια απόσταση έχει οριστεί έτσι ώστε να είναι θετική όταν το σημείο είναι αριστερά του κεντρικού σημείου. Η γωνία οπτικής επαφής ορίζεται με παρόμοιο τρόπο, ως η γωνία με εφαπτομένη το πηλίκο της κάθετης απόσταση του σημείου του πλαισίου από το κέντρο και του εστιακού μήκους. Η απόσταση μεταξύ του drone και του αντικειμένου υπολογίζεται ως το πηλίκο του τωρινού ύψους του drone και της εφαπτομένης της γωνίας οπτικής επαφής. Οι αντίστοιχες εξισώσεις φαίνονται παρακάτω: [31] yawfixangle = atan ( distfromcenterx ) focallengthhorizontal [32] angle = atan ( distfromcentery focallengthvertical ) 36

[33] objectdistance = currz tan(angle) b. Ενεργοποιημένη κάθετη κάμερα Με την αλλαγή της εν ενεργεία κάμερας, αλλάζει και η λογική των παραπάνω υπολογισμών. Αυτή τη φορά, το drone βρίσκεται πάνω από το αντικείμενο. Άρα οι στροφές γύρω από τους άξονες τους drone, x και y, επηρεάζουν με διαφορετικό τρόπο την απόκλιση του σημείου του πλαισίου από τη κανονική του θέση, αν το drone ήταν παράλληλο με το έδαφος. Συγκεκριμένα, υποθέτοντας ότι η στροφή γύρω από τον y άξονα αλλάζει τη θέση μόνο του y του σημείου του πλαισίου ενώ η στροφή γύρω από τον x μόνο το x του σημείου, προέκυψαν οι παρακάτω τύποι: [34] distfromcentery = atan (tan ( imagecentery bbcentery ) pitch) focallengthvertical focallengthvertical [35] distfromcenterx = atan (tan ( imagecenterx bbcenterx ) roll) focallengthhorizontal focallengthhorizontal [36] distfromcenter = distfromcenterx 2 + distfromcentery 2 Στην Εικόνα 11 φαίνεται η γεωμετρία πίσω από τους παραπάνω τύπους. Δίνεται μόνο μία εικόνα, εκείνη για τη στροφή κατά τον άξονα y, ως παράδειγμα, καθώς είναι ακριβώς το ίδιο σχήμα και για τη στροφή κατά τον άξονα x. 37

Εικόνα 11 Απαλοιφή στροφής κατά τον τοπικό άξονα y (κατακόρυφη κάμερα) Περνώντας τώρα στον υπολογισμό της απόστασης, συναντάται ακόμα ένα πρόβλημα. Η απόσταση εξαρτάται από τη διαφορά του ύψους μεταξύ του drone και του αντικειμένου. Όμως, το πραγματικό ύψος του drone λαμβάνεται από την μέτρηση του sonar, το οποίο αν βρεθεί πάνω από το αντικείμενο, θα δώσει λανθασμένα μέτρηση μικρότερη κατά το ύψος του αντικειμένου. Παρ όλα αυτά, μια τέτοια μεταβολή θα ήταν εύκολο να μετρηθεί, αν η μέτρηση του ύψους δεν φιλτραριζόταν από το Kalman. Το φίλτρο θα λάβει αυτή την απότομη μεταβολή, αλλά επειδή έχει ήδη κατασκευάσει το μοντέλο κίνησης, δεν θα στείλει ως μέτρηση του ύψους αυτούσια την τιμή που έλαβε, αλλά θα προσπαθήσει να τη φτάσει ομαλά μέσα στις επόμενες δειγματοληψίες της κατάστασης του drone. Έτσι, στον κόμβο προσέγγισης ήταν αναγκαίο να μπει μια δομή που να ελέγχει το πότε βρίσκεται το drone πάνω από το αντικείμενο, συγκρίνοντας κάθε φορά τη μεταβολή του ύψους στο χρόνο με την ταχύτητα στον z άξονα και εντοπίζοντας τις μεταβολές που δεν συνάδουν με την αντίστοιχη ταχύτητα.. Το άθροισμα αυτών τον μεταβολών προστίθεται στο τωρινό ύψος του drone, για να προκύψει το πραγματικό ύψος. Η ίδια διαδικασία ακολουθείται για να διαπιστωθεί αν το drone δεν είναι πια πάνω από το αντικείμενο. Εφ όσον έχει καθοριστεί το πραγματικό ύψος του drone και οι αποστάσεις του σημείου του πλαισίου από το κέντρο της εικόνας, σειρά έχει ο υπολογισμός των γωνιών 38

διόρθωσης και οπτικής επαφής (Εικόνα 13) και της απόστασης του αντικειμένου (Εικόνα 12). Παρακάτω φαίνονται οι εξισώσεις των υπολογισμών, με τη σειρά που αναφέρθηκαν: [37] yawfixangle = asin( distancefromcenterx distancefromcenter ) [38] angle = atan ( distancefromcenter focallengthvertical ) [39] objectdistance = (realz objectheight) tan(angle), όπου realz, είναι το πραγματικό ύψος του drone και objectheight είναι το ύψος του αντικειμένου. Εικόνα 12 Υπολογισμός απόστασης αντικειμένου (κατακόρυφη κάμερα) 39

Εικόνα 13 Υπολογισμός των γωνιών οπτικής επαφής και διόρθωσης (επίπεδο εικόνας) 4.3.3.3.1. Υπολογισμός κατεύθυνσης και ταχύτητας αντικειμένου Για να γνωρίζει το drone τη ταχύτητα του αντικειμένου και τη διεύθυνση της κίνησής του, αξιοποιήθηκε η τεχνική της τοποθέτησης καμπύλης με τη μέθοδο των ελαχίστων τετραγώνων. Σύμφωνα με τη θεωρία, γνωρίζοντας ένα πλήθος από σημεία στο επίπεδο, αναζητείται μία ευθεία η οποία να απέχει το ελάχιστο από όλα τα σημεία. Έτσι εξετάζεται αν υπάρχει γραμμική σχέση μεταξύ δύο μεταβλητών, της ανεξάρτητης x και της εξαρτημένης y. Η ιδέα λοιπόν είναι να χρησιμοποιηθούν τα σημεία που δίνονται σαν στόχοι,ώστε να βρεθεί η ευθεία που περιγράφει τη σχέση μεταξύ των x και y, με την εφαπτομένη της κλίσης της να δίνει και την κατεύθυνση του αντικειμένου. Την ίδια στιγμή, θα χρησιμοποιηθούν ξεχωριστά οι τιμές των x και οι τιμές των y μαζί με αυτές του χρόνου ώστε από τις ευθείες που θα προκύψουν, παίρνοντας την κλίση τους, να υπολογιστούν οι ταχύτητες στους καθολικούς άξονες x και y. Όμως το αντικείμενο μπορεί να κάνει κίνηση που περιέχει στροφές. Άρα δεν μπορούν να χρησιμοποιηθούν όλα τα σημεία από την αρχή της προσέγγισης αφού σε μικρό χρονικό διάστημα, το αποτέλεσμα των υπολογισμών θα αποκλίνει πολύ από την πραγματικότητα, ειδικά αν η κίνηση είναι αποκλειστικά κυκλική. 40

Γι αυτό το λόγο, επιλέγονται κάθε φορά συγκεκριμένος αριθμός δειγμάτων έτσι ώστε να προλαμβάνεται η οποιαδήποτε αλλαγή στην κατεύθυνση του αντικειμένου ή της ταχύτητάς του. Οι γενικές εξισώσεις της γραμμικής παρεμβολής, δίνονται παρακάτω: [40] sumx = n 1 x [41] sumy = n 1 y [42] sumxsq = n 1 x 2 [43] sumxy = n 1 (x y) [44] D = n sumxsq sumx 2 [45] a = n sumxy sumx sumy, D όπου α η κλίση της ευθείας που περιγράφει της σχέση των x και y. Οι κλίσεις των ευθειών που περιγράφουν τις σχέσεις μεταξύ των συντεταγμένων των στόχων και του χρόνου, είναι ίσες με τις ταχύτητες το αντικειμένου στους αντίστοιχους άξονες. Έτσι, αποθηκεύονται και στέλνονται ως μέρος του μηνύματος στόχου στον PID. Για την κατεύθυνση του αντικειμένου, θα βρεθεί η γωνία που έχει εφαπτομένη ίση με a. Όμως με αυτόν τον τρόπο, η γωνία περιορίζεται στο διάστημα [-90,90]. Έτσι, εξετάζεται και το πρόσημο της ταχύτητας στον άξονα x. Αν η ταχύτητα είναι θετική, τότε οι γωνία μένει ως έχει. Αν, όμως, η ταχύτητα είναι αρνητική, τότε αν είναι και η γωνία αρνητική, προστίθενται 180 μοίρες. Στην αντίθετη περίπτωση, αφαιρούνται 180 μοίρες. Το τελευταίο αυτό δεδομένο αρχίζει να χρησιμοποιείται από τη στιγμή που θα γίνει η εναλλαγή στις κάμερες και θα ενεργοποιηθεί η κατακόρυφη. Σε αυτή τη φάση, ο στόχος είναι το drone να κινείται παράλληλα με το αντικείμενο, αλλά ο άξονας x του drone να έχει διεύθυνση κάθετη στην κίνηση του. Αυτό γίνεται ώστε στην εικόνα να φαίνεται όσο το δυνατόν περισσότερος χώρος από τον εικονικό διάδρομο στον οποίο κινείται το αντικείμενο, αφού καλύπτεται από το οριζόντιο μέρος της εικόνας. Με αυτόν τον τρόπο, σε περίπτωση αύξησης της ταχύτητάς του, το αντικείμενο μένει για μεγαλύτερο χρονικό διάστημα ορατό και το drone προλαβαίνει να αντιδράσει στην αλλαγή. Έτσι, δίνεται ως στόχος στροφής του drone η μία από τις δύο πιθανές γωνίες που είναι κάθετη στη διεύθυνση του αντικειμένου, με κριτήριο την ελάχιστη διαφορά της από την τωρινή γωνία. Με αυτό τον τρόπο, αποφεύγονται στροφές μεγαλύτερες των 90 μοιρών κάθε φορά, οι οποίες θα δυσκόλευαν την λειτουργία του TLD. Επίσης, ελέγχεται αν η διαφορά της γωνίας κατεύθυνσης από την προηγούμενη είναι μεγαλύτερη των 30 μοιρών, ένα όριο το οποίο τέθηκε εμπειρικά. Αν είναι, τότε η γωνία 41

παραμένει ως έχει και η λανθασμένη γωνία αποθηκεύεται για την επόμενη σύγκριση. Ο έλεγχος αυτός βοηθάει στον να προληφθούν ορισμένοι υπολογισμοί οι οποίοι περιέχουν αρκετά μεγάλα σφάλματα. 4.3.3.4. Μη ορατό αντικείμενο a. Ενεργοποιημένη οριζόντια κάμερα Σε περίπτωση που το αντικείμενο έχει χαθεί από το οπτικό πεδίο και δεν έχει φτάσει αρκετά κοντά στο αντικείμενο, ώστε να ενεργοποιηθεί η κάτω κάμερα, τότε πρέπει να γίνουν ορισμένοι έλεγχοι ώστε να εκτελεστεί η κατάλληλη ενέργεια. Αρχικά ελέγχεται αν είχε χαθεί και από την κατακόρυφη κάμερα. Αν ισχύει αυτό, τότε καλείται η συνάρτηση reset η οποία αρχικοποιεί όλες τις μεταβλητές της κόμβου και στέλνει ως στόχο στο drone να επιστρέψει στο σημείο (0, 0, 0.6). Αυτό σημαίνει ότι το αντικείμενο έχει εντελώς χαθεί, και η διαδικασία πρέπει να ξεκινήσει από την αρχή. Σε περίπτωση που το αντικείμενο χάθηκε από την οριζόντια κάμερα προτού προλάβει να ενεργοποιηθεί η κατακόρυφη κάμερα, τότε αποθηκεύεται εκείνη η χρονική στιγμή και μετράται το χρονικό διάστημα κατά το οποίο έχει χαθεί από την εικόνα. Αν αυτό ξεπεράσει τα 7 δευτερόλεπτα, τότε στο drone στέλνεται η θέση από την οποία ήταν τελευταία ορατό το αντικείμενο. Αν, παρ όλα αυτά, παραμένει ακόμα εκτός οπτικού πεδίου και έχουν περάσει 20 δευτερόλεπτα, τότε καλείται η συνάρτηση reset. b. Ενεργοποιημένη κάθετη κάμερα Αν το αντικείμενο έχει βγει εκτός οπτικού πεδίου, τότε αποθηκεύεται και πάλι η χρονική στιγμή την οποία χάθηκε και μετράται το χρονικό διάστημα κατά το οποίο παραμένει σε αυτή την κατάσταση. Αν το διάστημα περάσει τα 20 δευτερόλεπτα, τότε αλλάζει η κάμερα και εκτελείται η πρώτη ενέργεια που περιγράφηκε παραπάνω. Παράλληλα, όσο το ύψος του drone βρίσκεται κάτω από τα 0.8m πάνω από το ύψος του αντικειμένου, στέλνεται εντολή αύξησης του ύψους του κατά 0.02m. Ταυτόχρονα, οι συντεταγμένες x και y του τελευταίου στόχου μεταβάλλονται κατά μία ποσότητα και στέλνονται ως νέος στόχος στο drone. Η ποσότητα αυτή είναι ίση με 0.01m κατά τη διεύθυνση με γωνία το άθροισμα της τελευταίας γωνίας διόρθωσης (yawfixangle) που υπολογίστηκε και της γωνίας διεύθυνσης του drone. Ουσιαστικά, το drone ωθείται προς τη διεύθυνση που χάθηκε το αντικείμενο από την εικόνα. 42

5. Πειράματα Για την διεξαγωγή των πειραμάτων έγινε χρήση ενός laptop Asus με επεξεργαστή Intel Pentium 2020M, με δύο πυρήνες στα 2.4 GHz και μνήμη 4GB σε περιβάλλον Linux. Για τη προσομοίωση έγινε χρήση της πλατφόρμας Gazebo και ως αντικείμενο στόχος επιλέχθηκε το TurtleBot. Το ύψος του αντικειμένου είναι στα 0.4m και η επιφάνεια του στην κάτοψη είναι περίπου 0.03m 2. Το χρώμα του είναι κατά κύριο λόγο άσπρο και αποτελείται από μια βάση, με την οποία κινείται, και 3 επίπεδα πάνω στο οποία προσαρμόζονται οι διάφοροι αισθητήρες που έχει. Η μέγιστη ταχύτητά του είναι περίπου στα 0.2m/s. Τα πειράματα διεξάχθηκαν σε έναν εικονικό κόσμο (world) του Gazebo, χωρίς εμπόδια και γενικά άλλα αντικείμενα πέρα του drone και του Turtlebot. Τα μοντέλα της πρόσοψης και της κάτοψης του αντικειμένου δημιουργήθηκαν πριν την διεξαγωγή των πειραμάτων. Οι χρόνοι που φαίνονται στα διαγράμματα είναι σύμφωνα με το χρόνο προσομοίωσης, ο οποίος λόγω των δυνατοτήτων του laptop, ήταν στο 1 του πραγματικού. 3 5.1. Περιγραφή πειραμάτων Το Turtlebot, αρχικά, τοποθετείται σε μία σταθερή απόσταση από το drone, η οποία επιλέχθηκε να είναι στα 4.2m. Το ρομπότ είναι ορατό από το drone όταν τοποθετείται στον κόσμο και πρόκειται να μείνει σταθερό στη θέση του, ενώ βρίσκεται ελάχιστα εκτός από το οπτικό του πεδίο προς τα αριστερά όταν πρόκειται να εκτελέσει κάποια κίνηση. Το τελευταίο γίνεται ώστε το drone να αρχίσει την προσέγγιση αφού έχει δοθεί στο Turtlebot η κατάλληλη ταχύτητα και στροφή. Για τη κίνηση του drone και το περιορισμό στις εντολές που μπορούν να σταλούν, τέθηκαν ορισμένα όρια. Για το ύψος του drone, το κάτω όριο είναι τα 0.6 μέτρα, καθώς γίνεται ασταθές για τιμές ύψους κάτω από αυτή και το άνω όριο είναι τα 2.5 μέτρα, αφού είναι επιθυμητό να μπορεί να κινείται σε εσωτερικό χώρο. Για να δοθεί προτεραιότητα στη κίνηση ή/και στροφή κατά τον άξονα z, η γωνία στρέψης θα πρέπει να διαφέρει κατά 5 μοίρες τουλάχιστον από την τωρινή και το τωρινό ύψος του drone να απέχει από το ύψος στόχο τουλάχιστον 0.05m. 43

Ο υπολογισμός της ταχύτητας και της κατεύθυνσης του αντικειμένου έγινε επιλογή να εκτελείται κάθε 5 δείγματα όταν είναι ενεργοποιημένη η οριζόντια κάμερα και κάθε 10 για την κατακόρυφη κάμερα. Ο αλγόριθμος εξετάστηκε σε τέσσερα είδη κινήσεων του αντικειμένου: Σταθερό σε ένα σημείο Κινούμενο σε ευθεία Κινούμενο σε κυκλική τροχιά Κινούμενα σε τυχαία τροχιά με ευθείες και στροφές Για τα τρία τελευταία είδη, προσδιορίστηκε και η ταχύτητα με την οποία θα κινείται το αντικείμενο, η οποία χωρίστηκε σε τρεις κατηγορίες, αργή (0.09 m/s), κανονική (0.13 m/s) και γρήγορη (0.21 m/s). Με την ίδια λογική χωρίστηκε και η στροφή που δόθηκε στο Turtlebot, με τις κατηγορίες να είναι ανοιχτή, κανονική και κλειστή στροφή. Για κάθε συνδυασμό ταχύτητα και στροφής, έγιναν από τρία πειράματα. Κατά τη διάρκεια τους, μετρήθηκε η ευκλείδεια απόσταση του drone από το Turtlebot και η προβολή της στο XY επίπεδο, όπως τις υπολόγισε ο αλγόριθμος και όπως ήταν στην πραγματικότητα. Επίσης, μετρήθηκε ο χρόνος που χρειάστηκε το drone για να προσγειωθεί πάνω στο αντικείμενο για κάθε ένα πείραμα καθώς και η διαφορά της υπολογιζόμενης από την πραγματική συνολική ταχύτητα του αντικειμένου. Τέλος, για κάθε κίνηση μετρήθηκε το ποσοστό επιτυχιών στα πειράματα. 44

Εικόνα 14 Εντοπισμός αντικειμένου στο περιβάλλον Gazebo (οριζόντια κάμερα) Εικόνα 15 Εντοπισμός αντικειμένου στο περιβάλλον Gazebo (κάθετη κάμερα) 5.2. Αποτελέσματα 5.2.1. Στατικό αντικείμενο Το drone στη προσομοίωση εντοπίζει άμεσα το αντικείμενο, αν είναι μέσα στο οπτικό του πεδίο και αρχίζει να το προσεγγίζει. Στην Εικόνα 16 φαίνεται πως μειώνεται η ευκλείδεια απόσταση μεταξύ του drone και του αντικειμένου καθώς το πλησιάζει και στην Εικόνα 17 η 45