Α Π Ε (Χ 2011/2012) Υλοποίηση εντοπισμού στα Nao robots μέσω προσέγγισης του φίλτρου Kalman Ιωακείμ Πέρρος, ΑΜ: 2007030085 2 Απριλίου 2012
Περιεχόμενα 1 Εισαγωγή / Πρόβλημα 1 2 Προσέγγιση / Λύση 2 2.1 Βήμα Πρόβλεψης................................. 2 2.2 Βήμα Διόρθωσης................................. 3 3 Τεχνικές δυσκολίες 5 4 Aποτελέσματα / Μελλοντικές επεκτάσεις 5 1 Εισαγωγή / Πρόβλημα Η εργασία είχε ως σκοπό την υλοποίηση μιας προσέγγισης του φίλτρου Kalman στα Nao robots στα πλαίσια του κώδικα της ομάδας Κουρήτες (& Noxious), ο οποίος χρησιμοποιείται σε αγώνες ρομποτικού ποδοσφαίρου. Συνεπώς, κάνοντας χρήση αυτής της υποδομής (άρα και των αισθήσεων/δεδομένων που λαμβάνουμε από τα υπόλοιπα τμήματα του κώδικα),επιχειρήθηκε η δημιουργία ενός φίλτρου Kalman με σκοπό τον Gaussian εντοπισμό του robot στον χώρο του γηπέδου, πέρα από την ήδη υπάρχουσα υλοποίηση Monte Carlo (με particle filters) στον κώδικα της ομάδας. Ακολουθεί παράθεση του κλασσικού φίλτρου Kalman ώστε να αναδειχθούν οι λόγοι για τους οποίους χρειάστηκε να καταφύγουμε στην προσέγγισή του: KalmanFilter Inputs: µ t 1, Σ t 1, u t, z t Outputs: µ t, Σ t µ t A t µ t 1 + B t u t Σ t A t Σ t 1 A T t + R t K t Σ t C T t (C t Σ t C T t + Q t ) 1 µ t µ t + K t (z t C t µ t ) Σ t (I K t C t ) Σ t return µ t, Σ t Όπως φαίνεται και παραπάνω, το φίλτρο υποθέτει ότι έχουμε πίνακες A, B για το βήμα πρόβλεψης και C για το βήμα διόρθωσης ώστε να επιτύχουμε την ανανέωση των μεταβλητών μέσω γραμμικών εξισώσεων των πινάκων αυτών με τις μεταβλητές κατάστασης και μέτρησης (παρατήρησης). Η υπόθεση όμως ότι τα μοντέλα που ακολουθεί το robot (π.χ. το μοντέλο που περιγράφει την κίνησή του) είναι γραμμικά, είναι εσφαλμένη, καθώς υπάρχουν τριβές κατά την κίνησή του που δεν μπορούν να περιγραφούν με αυτές τις απλές γραμμικές εξισώσεις. Συνεπώς, οφείλουμε να καταφύγουμε σε μία προσέγγιση του φιλτραρίσματος που εφαρμόζει το Kalman φίλτρο, ώστε να παρακάμψουμε το πρόβλημα της μη γραμμικότητας. 1
2 Προσέγγιση / Λύση Κρίνεται σκόπιμο η περιγραφή της λύσης να παρουσιαστεί σε 2 στάδια, όπως ακριβώς λειτουργεί δηλαδή και γενικά η μέθοδος του φιλτραρίσματος. 2.1 Βήμα Πρόβλεψης Το βήμα της πρόβλεψης στο φιλτράρισμα που εφαρμόστηκε παρακάμφθηκε ουσιαστικά μέσω της χρήσης του μοντέλου κίνησης της οδομετρίας. Το ζήτημα που επιλύουμε με την χρήση μοντέλου κίνησης και όχι απλά των στοιχείων ( x, ȳ, θ) που λαμβάνουμε από το οδόμετρο, είναι το ότι τα συγκεκριμένα στοιχεία αποτελούν τιμές σε ένα εσωτερικό σύστημα συντεταγμένων του robot, του οποίου δεν γνωρίζουμε την μετατροπή στο δικό μας σύστημα συντεταγμένων λόγω πλαγιοδρόμησης και ολίσθησης. Αν ξέραμε την μετατροπή αυτή άλλωστε, θα λυνόταν το πρόβλημα του localization! Συνεπώς το χρήσιμο σε εμάς είναι ότι η σχετική διαφορά των στάσεων που δίνει το οδόμετρο, αποτελεί έναν καλό εκτιμητή για την διαφορά των πραγματικών στάσεων του robot και τελικά εκμεταλλευόμαστε την οδομετρία, κρατώντας κάθε φορά σε προσωρινές μεταβλητές τη διαφορά των στάσεων προηγούμενης και τρέχουσας χρονικής στιγμής. Ο τρόπος με τον οποίο η προσωρινή μετατόπιση, κατεύθυνση και περιστροφή εξάγονται από τα προαναφερθείσες προσωρινές μεταβλητές φαίνεται στο παρακάτω πλαίσιο (με πράσινο φόντο), ενώ το σχήμα [4] που δείχνει γεωμετρικά αυτές τις τιμές, βοηθάει στην εξαγωγή των παρακάτω εξισώσεων, μέσω των οποίων τελικά κάνουμε μία πρόβλεψη για την θέση του robot στον 3-d χώρο. Οι παρακάτω εξισώσεις περιγράφουν την εν λόγω κίνηση, άρα και την ανανέωση του 3-διάστατου διανύσματος που επιθυμούμε να προβλέψουμε. Έχοντας εξάγει συνεπώς τις μεταβολές σε μετατόπιση, κατεύθυνση και περιστροφή, έχουμε: Odometry Motion Model x x + (δ trans cos(θ + δ rot1 )) y y + (δ trans sin(θ + δ rot1 )) θ θ + (δ rot1 + δ rot2 ) 2
2.2 Βήμα Διόρθωσης Έπειτα, αν καταγραφεί σε αυτό το διάστημα κάποια παρατήρηση (1 ή 2 δοκάρια ενός τέρματος), τότε εφαρμόζουμε τον αλγόριθμο sample landmark model known correspondence που βρίσκεται στο [1], που μας βοηθά να δειγματοληπτήσουμε την 3η διάσταση που επιθυμούμε να εκτιμήσουμε, καθώς μόνο 2 στοιχεία είναι διαθέσιμα για κάθε παρατήρηση, η απόσταση και ο προσανατολισμός σε σχέση με το παρατηρηθέν ορόσημο. Εφαρμόζοντας, συνεπώς, αυτό το sampling, σχηματίζεται ένας κύκλος από πιθανές τοποθεσίες του robot με ακτίνα ίση με την εκτιμώμενη απόσταση από το ορόσημο. Σε όλη την διαδικασία του debugging του κώδικα, έγινε χρήση του KLocView debugger που υπήρχε υλοποιημένος ως μέρος του κώδικα της ομάδας των Κουρητών. Καθώς ο συγκεκριμένος debugger αφορούσε στην αποσφαλμάτωση της υλοποίησης Monte Carlo localization με particle filters, κάναμε χρήση των particles αυτών, με σκοπό να αναπαραστήσουμε στοιχεία που αφορούσαν στην δική μας υλοποίηση (όπως δείγματα μιας πολυμεταβλητής gaussian κατανομής ή τα δείγματα που βρίσκονται γύρω από τις παρατηρήσεις των οποίων τη χρήση θα εξηγήσουμε παρακάτω). Μέσω λοιπόν του debugger αυτού, αναπαρίσταται κάθε φορά ο κύκλος με δείγματα γύρω από τις παρατηρήσεις που προαναφέρθηκε. Ο τρόπος με τον οποίο ο αλγόριθμος που παράγει αυτά τα δείγματα λειτουργεί, είναι ο εξής: Sample landmark model known correspondence Inputs: m, Outputs: x, y, θ γ rand(0, 2π) r r + sample(σ r ) ϕ ϕ + sample(σ ϕ ) x m x + r cos(γ) y m y + r sin(γ) θ γ π ϕ return x, y, θ Η παραπάνω διαδικασία καλείται για όσα δείγματα θέλουμε να παράγουμε και παρακάτω παρατίθεται ένα στιγμιότυπο χρήσης του debugger για 1000 δείγματα (με κόκκινο χρώμα στο στιγμιότυπο), όταν το robot βρίσκεται στην αρχική του θέση (όταν βλέπει το κίτρινο τέρμα και βρίσκεται στο κέντρο του γηπέδου θεωρούμε ότι και οι 3 τιμές του διανύσματος x, y, θ είναι στο 0 και αυτή είναι και η αρχικοποίηση που εφαρμόζεται γενικά σε κάθε run του κώδικα). 3
Από τον παραπάνω κύκλο, απορρίπτουμε κάθε φορά τα δείγματα (συνδυασμούς x,y και θ) που είναι εκτός γηπέδου (παρόλο που για καλύτερη εποπτεία εμφανίζονται στο παραπάνω screenshot) και έπειτα επιλέγουμε τον συνδυασμό που είναι εγγύτερος (σε σχέση με την ευκλείδια απόσταση) στην προβλεπόμενη στάση μας από το πρώτο βήμα. Αυτό γίνεται τον εξής τρόπο: Υπολογισμός ευκλείδιας απόστασης μεταξύ δειγμάτων και προβλεπόμενης πεποίθησης μέσω οδομετρίας tmp_cost for i = 1 no_of_samples do ( ) T xi y i θ i sample_landmark_model_known_correspondence( µx, µ y, µ θ, ˆd, ˆb) angle_diff angle_difference(θ i, µ θ ) tmp_cost (x i µ x ) 2 + (y i µ y ) 2 + angle_diff 2 if tmp_cost < min_diff then measured_x x i measured_y y i measured_θ θ i Έχοντας καταλήξει ως προς ποιό διάνυσμα θα θεωρήσουμε ως διάνυσμα παρατήρησης, μένει να δούμε πόσο πολύ θα το εμπιστευθούμε, δηλαδή μένει να ορίσουμε το Kalman Gain. Η ύπαρξη πινάκων συνδιακύμανσης στην εξίσωση εξαγωγής του όμως, των οποίων τις τιμές έπρεπε ανάλογα να αυξομειώνουμε, οδήγησε σε μία ευριστική προσέγγιση της εξαγωγής του όρου αυτού. Καταγράφοντας τον εγγύτερο συνδυασμό στην προηγούμενη εκτίμηση της στάσης του robot (τον συνδυασμό δηλαδή με το ελάχιστο κόστος), συγκρίνουμε κάθε φορά τον προηγούμενο με τον τρέχοντα και εάν βρίσκονται σε ένα παράθυρο στον δισδιάστατο χώρο x,y (π.χ. 200x200) τότε διπλασιάζουμε το Kalman Gain στη λογική της ενίσχυσης της εμπιστοσύνης στις παρατηρήσεις όταν παίρνουμε συνεχόμενες που οδηγούν σε παρόμοιο ``καλύτερο'' ως προς την εκτίμηση συνδυασμό. Κατ' αναλογία, όταν οι 2 συνεχόμενοι συνδυασμοί δεν βρίσκονται σε μία τέτοια απόσταση, υποδιπλασιάζουμε το Kalman Gain. Μέγιστη τιμή του έχει τεθεί το 0.1 μετά από πολλαπλές δοκιμές, καθώς κάτι τέτοιο μας εξασφαλίζει ότι θα αλλάξει γρήγορα την πεποίθηση του αν λάβει μία σειρά όμοιων παρατηρήσεων, αλλά 4
και ότι δεν θα επηρεάσει πολύ την πεποίθησή μας όταν κάτι τέτοιο δεν είναι επιθυμητό (σε περιπτώσεις δηλαδή που πρέπει να εμπιστευθούμε λιγότερο την παρατήρηση και περισσότερο την προηγούμενή του πεποίθηση από το πρώτο βήμα φιλτραρίσματος). Η κατασκευή αυτή, τελικώς, του Kalman Gain, μας δίνει τη δυνατότητα να το εντάξουμε σε ένα κυρτό συνδυασμό στον οποίο λειτουργεί ως διελκυνστίδα μεταξύ των 2 στάσεων του robot - του εγγύτερου στην προηγούμενη εκτίμηση δείγματος μέσω των παρατηρήσεων και της προηγούμενης εκτίμησης καθεαυτής. Μέσω λοιπόν αυτού του κυρτού συνδυασμού, εξάγουμε και την τελική μας μέση τιμή για την στάση του robot, η οποία και δεν εντάσσεται σε δειγματοληψία μέσω κανονικής κατανομής, καθώς παρατηρήθηκε ότι εκχωρώντας την απευθείας ως πεποίθηση για τη στάση του robot, οδηγούμαστε σε καλύτερα αποτελέσματα. Τελική εκτίμηση πεποίθησης (Πρόβλεψη & Διόρθωση) x (1 K) x + K mincostbel x y (1 K) y + K mincostbel y θ (1 K) θ + K mincostbel θ 3 Τεχνικές δυσκολίες Οι βασικότερες τεχνικές δυσκολίες που παρουσιάστηκαν στην συγκεκριμένη εργασία ήταν η εξοικείωση με τον τρόπο που πρέπει να κατασκευάσει κάποιος μία activity (ο κώδικας που τρέχουν τα robots είναι διαιρεμένος σε activities και επιλέγεται κάθε φορά μέσω ενός xml αρχείου ποιές από αυτές θα συμμετέχουν στην εκτέλεση) και η κατανόηση του τρόπου με τον οποίο τα Nao τρέχουν το κάθε τμήμα κώδικα, καθώς και τί χρειάζεται να υπάρχει στον κώδικα και τί όχι (από πλευράς π.χ. κώδικα επικοινωνίας που είναι ανεξάρτητος από το κομμάτι του localization) για να γίνει αυτό με επιτυχία. 4 Aποτελέσματα / Μελλοντικές επεκτάσεις Τα αποτελέσματα, όπως φαίνονται και στην παρουσίαση, ήταν ικανοποιητικά τόσο στο tracking problem - δηλαδή στην εύρεση της 3-d στάσης του robot στην περίπτωση που γνωρίζει την αρχική του θέση και κινείται στο χώρο - όσο και στο kidnapping problem - δηλαδή στον προσδιορισμό της θέσης του robot σε περίπτωση που κάποιος το ``απαγάγει'' από την θέση του. Μελλοντικές επεκτάσεις που προτείνονται για την συγκεκριμένη υλοποίηση, αποτελούν τόσο η ενσωμάτωση του φιλτραρίσματος αυτού, με τα particle filters που ήδη υπάρχουν στον κώδικα της ομάδας των Κουρητών και η εναλλαγή στην εμπιστοσύνη μεταξύ των 2 υλοποιήσεων κατά περίπτωση καθώς και η ύπαρξη αυτού του κώδικα ως βάση για πιο εξελιγμένες μεθόδους Kalman filtering (extended/unscented) που λύνουν το πρόβλημα της μη γραμμικότητας με μαθηματικό τρόπο (π.χ. Ιακωβιανές, μερική παραγώγηση κλπ.). Αναφορές [1] Sebastian Thrun, Wolfram Burgard and Dieter Fox Probabilistic Robotics September 2005 5
[2] Kalman Filter Tutorial http://www.youtube.com/watch?v=fkct_lv9syk [3] Eleftherios A. Chatzilaris Visual-Feature-based Self-Localization for Robotic Soccer Chania, October 2009 [4] Michail Lagoudakis Autonomous Agents (Postgraduate): Course Slides 6