Υλοποίηση localization στα Nao robots Προσέγγιση φίλτρου Kalman Ιωακείμ Πέρρος Αυτόνομοι Πράκτορες Εργασία εξαμήνου Χειμερινό 2011-2012 4/4/2012
4 βασικά προβλήματα ρομποτικής πλοήγησης Mapping Localization Markov Grid Gaussian Monte-Carlo Path Planning Motion Control
Classic Kalman Filter 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
Classic Kalman Filter 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 Μη γραμμικότητα Στην περίπτωση του robot, υπάρχουν τριβές
Βήμα Πρόβλεψης Μοντέλο κίνησης οδομετρίας Το οδόμετρο του robot παρέχει x, ȳ, θ ανά dt
Βήμα Πρόβλεψης Μοντέλο κίνησης οδομετρίας Το οδόμετρο του robot παρέχει x, ȳ, θ ανά dt Μετρήσεις σε εσωτερικό σύστημα συντεταγμένων του robot Εξαιτίας drift & ολίσθησης, δεν ξέρουμε σχέση με καθολικές συντεταγμένες του κόσμου Αν την ξέραμε, θα λυνόταν το πρόβλημα του localization
Βήμα Πρόβλεψης Μοντέλο κίνησης οδομετρίας Το οδόμετρο του robot παρέχει x, ȳ, θ ανά dt Μετρήσεις σε εσωτερικό σύστημα συντεταγμένων του robot Εξαιτίας drift & ολίσθησης, δεν ξέρουμε σχέση με καθολικές συντεταγμένες του κόσμου Αν την ξέραμε, θα λυνόταν το πρόβλημα του localization Άρα τί εκμεταλλευόμαστε από την οδομετρία;
Βήμα Πρόβλεψης Μοντέλο κίνησης οδομετρίας Το οδόμετρο του robot παρέχει x, ȳ, θ ανά dt Μετρήσεις σε εσωτερικό σύστημα συντεταγμένων του robot Εξαιτίας drift & ολίσθησης, δεν ξέρουμε σχέση με καθολικές συντεταγμένες του κόσμου Αν την ξέραμε, θα λυνόταν το πρόβλημα του localization Άρα τί εκμεταλλευόμαστε από την οδομετρία; Σχετική διαφορά διαδοχικών στάσεων οδόμετρου: καλή εκτίμηση για την διαφορά τους στον πραγματικό κόσμο
Βήμα Πρόβλεψης Μετασχηματισμός σχετικής διαφοράς σε ακολουθία 3 βημάτων: Περιστροφή, κίνηση σε ευθεία γραμμή & μία ακόμα περιστροφή Odometry Motion Model x x + (δ trans cos(θ + δ rot1 )) y y + (δ trans sin(θ + δ rot1 )) θ θ + (δ rot1 + δ rot2 )
Βήμα Διόρθωσης Στοιχεία που λαμβάνουμε από το κομμάτι του vision / παρατήρηση: Απόσταση & διόπτευση (bearing) από το landmark 2 στοιχεία / παρατήρηση 3 στοιχεία προς εκτίμηση (x, y, θ)
Βήμα Διόρθωσης Στοιχεία που λαμβάνουμε από το κομμάτι του vision / παρατήρηση: Απόσταση & διόπτευση (bearing) από το landmark 2 στοιχεία / παρατήρηση 3 στοιχεία προς εκτίμηση (x, y, θ) Λύση: Sampling
Βήμα Διόρθωσης 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, θ
Βήμα Διόρθωσης Στιγμιότυπο αναπαράστασης της δειγματοληψίας (1000 samples)
Βήμα Διόρθωσης Όλα τα σημεία στον κύκλο είναι πιθανές θέσεις του robot: ποιό θα εμπιστευθούμε;
Βήμα Διόρθωσης Όλα τα σημεία στον κύκλο είναι πιθανές θέσεις του robot: ποιό θα εμπιστευθούμε; Απόρριψη δειγμάτων εκτός γηπέδου Επιλογή εγγύτερου διανύσματος (x, y, θ) ως προς την εκτίμηση του βήματος πρόβλεψης
Βήμα Διόρθωσης Υπολογισμός ευκλείδιας απόστασης μεταξύ δειγμάτων και προβλεπόμενης πεποίθησης μέσω οδομετρίας tmp_cost for i = 1 no_of_samples do ( ) T xi y i θ i SLMKC( µ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 (-) Πίνακες θορύβου συνδιακύμανσης (πρόβλεψης/διόρθωσης) Ευριστική προσέγγιση του όρου
Βήμα Διόρθωσης Βρήκαμε ποιό σημείο του κύκλου θα χρησιμοποιήσουμε Πόσο θα το εμπιστευθούμε σε σχέση με το διάνυσμα της πρόβλεψης; Εύρεση ισορροπίας στη ζυγαριά Καθορισμός Kalman Gain (-) Πίνακες θορύβου συνδιακύμανσης (πρόβλεψης/διόρθωσης) Ευριστική προσέγγιση του όρου Σύγκριση προηγούμενης με τρέχουσα mincostbelief Αν βρίσκονται σε ένα παράθυρο (πχ 200x200): διπλασιασμός Διαφορετικά: υποδιπλασιασμός Μέγιστη τιμή: 01
Συνολική εικόνα Τελική εκτίμηση πεποίθησης (Πρόβλεψη & Διόρθωση) x (1 K) x + K mincostbel x y (1 K) y + K mincostbel y θ (1 K) θ + K mincostbel θ Καλύτερα αποτελέσματα με: 1 Χρήση κυρτού συνδυασμού 2 Εκχώρηση της παραπάνω μέσης τιμής (x, y, θ) ως πεποίθηση και όχι την δειγματοληψία με βάση αυτή
Τεχνικές Δυσκολίες Εξοικείωση με ανάπτυξη κώδικα για τα Nao Κυρίως, κατανόηση απαραίτητων τμημάτων κώδικα (πχ επικοινωνία robot, υπολογισμός φίλτρου μπάλας) Άσχετα με τον κώδικα bugs Χρονοβόρο calibration των παραμέτρων (πχ max τιμή Kalman Gain) Υλοποίηση μόνο στο εργαστήριο
Μελλοντικές Επεκτάσεις Ενσωμάτωση με Monte-Carlo localizationparticle filters του κώδικα της ομάδας Βάση για υλοποίηση πιο εξελιγμένων εκδοχών Kalman filter (Extended/Unscented)
Αποτελέσματα Ικανοποιητικά τόσο στο tracking όσο και στο global localization / kidnapping πρόβλημα Εξάρτηση από πολλούς παράγοντες (πχ φωτισμός γηπέδου)
Demo Video demonstration
Ευχαριστώ πολύ για την προσοχή σας Ερωτήσεις / Σχόλια;