ΕΚΠ 413 / ΕΚΠ 606 Αυτόνοµοι (Ροµ οτικοί) Πράκτορες Ροµ οτικός Εντο ισµός Robo Localizaion Τµήµα Ηλεκτρονικών Μηχανικών και Μηχανικών Υ ολογιστών Πολυτεχνείο Κρήτης
Ε ανάληψη Χρονικά ιθανοτικά µοντέλα κρυφά µοντέλα Markov φίλτρα Kalman δυναµικά δίκτυα Bayes φίλτρα σωµατιδίων Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 2
Robo Navigaion Problems Wha should I remember? Mapping Where am I? Localizaion Where should I go? Pah Planning How can I go? Moion Conrol Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 3
Σήµερα Εντο ισµός προβλήµατα εντοπισµού χαρακτηριστικά Αλγόριθµοι Markov localizaion Grid localizaion Gaussian localizaion Mone-Carlo localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 4
Εντο ισµός Localizaion
Ροµ οτικός Εντο ισµός Using sensory informaion o locae he robo in is environmen is he mos fundamenal problem o providing a mobile robo wih auonomous capabiliies. [Cox 91] εδοµένα χάρτης του περιβάλλοντος ακολουθία παρατηρήσεων Ζητούµενο εκτίµηση της θέσης του ροµπότ (εκτίµηση της θέσης κάποιου αντικειµένου) Προβλήµατα εντο ισµού παρακολούθηση θέσης (posiion racking) καθολικός εντοπισµός (global localizaion) πρόβληµα της απαγωγής (robo kidnapping) Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 6
Θέση / Κατάσταση συντεταγµένες κέντρου x y και προσανατολισµός θ κατάσταση = <x y θ> πιο σύνθετη κατάσταση για υποβρύχια και εναέρια ροµπότ Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 7
Το ικός / Καθολικός Εντο ισµός Το ικός εντο ισµός γνωστή αρχική θέση φραγµένη αβεβαιότητα αντιµετώπιση του θορύβου µονοτροπική εκτίµηση Καθολικός εντο ισµός άγνωστη αρχική θέση αβεβαιότητα χωρίς φράγµα αντιµετώπιση της άγνοιας πολυτροπική εκτίµηση Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 8
Στατικά / υναµικά Περιβάλλοντα Στατικά εριβάλλοντα καµία µεταβολή στο περιβάλλον µόνο η θέση του ροµπότ ως κατάσταση βολικές µαθηµατικές ιδιότητες υναµικά εριβάλλοντα µεταβολές στο περιβάλλον µακροπρόθεσµη επιρροή στην αντίληψη του ροµπότ π.χ. ανοικτές/κλειστές πόρτες άνθρωποι άλλα ροµπότ... δυναµικά στοιχεία µέρος της κατάστασης Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 9
Παθητικός / Ενεργός Εντο ισµός Παθητικός εντο ισµός έλεγχος του ροµπότ ανεξάρτητος από εντοπισµό παθητική λήψη αντιλήψεων Ενεργός εντο ισµός έλεγχος του ροµπότ προς διευκόλυνση του εντοπισµού ενεργός αναζήτηση κρίσιµων αντιλήψεων αποσαφήνιση διφορούµενων εκτιµήσεων π.χ. coasal navigaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 10
Μονο-/Πολυ-Ροµ οτικός Εντο ισµός Μονοροµ οτικός εντο ισµός ένα µόνο ροµπότ στο περιβάλλον όλα τα δεδοµένα σε ένα ροµπότ δεν τίθεται θέµα επικοινωνίας Πολυροµ οτικός εντο ισµός οµάδα από ροµπότ σε κοινό περιβάλλον κρίσιµη η ικανότητα αναγνώρισης άλλων ροµπότ κρίσιµες οι πληροφορίες σχετικής θέσης κρίσιµη η δυνατότητα επικοινωνίας ανταλλαγή πληροφοριών εντοπισµού Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 11
Εντο ισµός µε Πρόβλεψη; Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 12
Εντο ισµός µε Οδόµετρο; Πραγµατική Τροχιά Εκτιµώµενη Τροχιά Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 13
Χαρτογράφηση Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 14
Χαρτογράφηση χωρίς Εντο ισµό; Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 15
Percepual Aliasing Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 16
Γενικό Πλαίσιο εδοµένα ακολουθία παρατηρήσεων z και ενεργειών u µοντέλο παρατήρησης P(z x) µοντέλο µετάβασης/κίνησης P(x u x ) αρχική πεποίθηση P(x) Ζητούµενο εκτίµηση της κατάστασης X εκ των υστέρων πεποίθηση d = { u z1 u Bel ( x ) = P( x u z1 u Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 17 z 1 } z 1 )
υναµικό ίκτυο Bayes p z x z u ) = ( 0: 1: 1: p( z x p( x x1 : 1 z1: u1: ) = p( x x 1 u ) Παραδοχές στατικό περιβάλλον ανεξάρτητος θόρυβος µαρκωβιανό σύστηµα πρώτης τάξης ακριβή µοντέλα Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 18 )
υναµικό ίκτυο Bayes p( z x z u m) = p( z x m) 0: 1: 1: p( x x z u m) = p( x x u ) 1: 1 1: 1: 1 Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 19
Αλγόριθµοι Εντο ισµού Markov localizaion γενική αναπαράσταση της πεποίθησης ενηµέρωση πεποίθησης µε Bayes filers Grid localizaion διακριτή αναπαράσταση της πεποίθησης ενηµέρωση πεποίθησης µε discree Bayes filers (ΗΜΜs) Gaussian localizaion αναπαράσταση της πεποίθησης µε γραµµικές κατανοµές Gauss ενηµέρωση πεποίθησης µε Kalman filers Mone-Carlo localizaion αναπαράσταση της πεποίθησης µε συλλογή δειγµάτων ενηµέρωση πεποίθησης µε paricle filers Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 20
Markov Localizaion Bayes Filer
Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 22 1 1 1 ) ( ) ( ) ( = dx x Bel x u x P x z P η Φιλτράρισµα Bayes ) ( ) ( 1 1 1 1 u z u x P u z u x z P =η Bayes ) ( ) ( 1 1 z u z u x P x Bel = Markov ) ( ) ( 1 1 u z u x P x z P =η Markov 1 1 1 1 1 ) ( ) ( ) ( = dx u z u x P x u x P x z P η 1 1 1 1 1 1 1 ) ( ) ( ) ( = dx u z u x P x u z u x P x z P η Toal prob. Markov 1 1 1 1 1 1 ) ( ) ( ) ( = dx z z u x P x u x P x z ηp
Αλγόριθµος Bayes Filer Bel ( x ) = η P ( z x ) P ( x u x 1 ) Bel ( x 1) dx 1 1. Algorihm Bayes_filer( Bel(x) d ) 2. η=0 3. If d is a percepual daa iem z hen 4. For all x do 5. Bel '( x) = P( z x) Bel( x) 6. η=η+bel' ( x) 7. For all x do 8. 1 Bel'( x) =η Bel'( x) 9. Else if d is an acion daa iem u hen 10. For all x do 11. Bel' ( x) = P( x u x') Bel( x') dx' 12. Reurn Bel (x) Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 23
Παράδειγµα Markov Localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 24
Παράδειγµα Markov Localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 25
Grid Localizaion Discree Bayes Filers
Grid Localizaion Ανα αράσταση διακριτοποίηση του χώρου καταστάσεων οµοιόµορφο τετραγωνικό πλέγµα (µετρική αναπαράσταση) ανοµοιόµορφο τοπολογικό πλέγµα (τοπολογική αναπαράσταση) κρίσιµη παράµετρος η ανάλυση του πλέγµατος άµεσα σχετιζόµενη µε τα µοντέλα κίνησης και παρατήρησης τυπική ανάλυση για µετρική αναπαράσταση: 15cm για x y και 5 ο για θ διακριτή κατανοµή πιθανότητας για την πεποίθηση Ενηµέρωση αναλυτική ενηµέρωση για κάθε στοιχείο του πλέγµατος διακριτό φίλτρο Bayes Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 27
ιακριτή Ανα αράσταση Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 28
Ανάλυση ιακριτο οίησης Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 29
Αλγόριθµος Discree Bayes Filer Bel ( x ) = η P ( z x ) P ( x u x ) Bel ( x ) 1 1 x 1. Algorihm Discree_Bayes_filer( Bel(x) d ) 1 2. η=0 3. If d is a percepual daa iem z hen 4. For all x do 5. 6. 7. For all x do 8. 9. Else if d is an acion daa iem u hen 10. For all x do 11. 12. Reurn Bel (x) Bel '( x) = P( z x) Bel( x) η=η+bel' ( x) Bel'( x) =η Bel'( x) = x' 1 Bel'( x) P( x u x') Bel( x') Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 30
Παράδειγµα Grid Localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 31
Παράδειγµα Grid Localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 32
Gaussian Localizaion Kalman Filers
Gaussian Localizaion Ανα αράσταση πεποίθηση ως γραµµική κατανοµή Gauss µονοτροπική εκτίµηση ιδανική για διακριτές αντιλήψεις (landmarks) Ενηµέρωση Kalman filer µοντέλα µετάβασης και παρατήρησης γραµµικές κατανοµές Gauss η πεποίθηση είναι γραµµική κατανοµή Gauss exended Kalman filer (EKF) µη γραµµικά µοντέλα µετάβασης και παρατήρησης τοπική γραµµικοποίηση µε ανάπτυγµα Taylor η πεποίθηση προσεγγίζεται ως γραµµική κατανοµή Gauss Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 34
Παράδειγµα Gaussian Localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 35
Παράδειγµα Gaussian Localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 36
Muli-Hypohesis Tracking Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 37
Muli-Hypohesis Tracking Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 38
Mone Carlo Localizaion Paricle Filers
Mone Carlo Localizaion Ανα αράσταση µη παραµετρική αναπαράσταση προσέγγιση κατανοµής µε πληθυσµό δειγµάτων θεωρητικά η πεποίθηση µπορεί να είναι µια αυθαίρετη κατανοµή Ενηµέρωση Sampling Imporance Resampling paricle filer (SIR) διάδοση σωµατιδίων (µοντέλο κίνησης) ανάθεση βάρους (µοντέλο παρατήρησης) αναδειγµατοληψία AUXiliary paricle filer (AUX) ανάθεση βάρους (µοντέλο παρατήρησης) αναδειγµατοληψία διάδοση σωµατιδίων (µοντέλο κίνησης) Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 40
Ανα αράσταση Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 41
Παράδειγµα MCL Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 42
Παράδειγµα MCL Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 43
Παράδειγµα MCL Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 44
Παράδειγµα MCL Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 45
Αλγόριθµος Paricle Filer (SIR) 1. Algorihm paricle_filer( S -1 u -1 z ) 2. S 3. For i =1 n Generae new samples 4. Sample index j(i) from he discree disribuion given by w -1 i j( i) 5. Sample x from p x x u ) using and i i 6. w = p( z x ) Compue imporance weigh i 7. η=η+w Updae normalizaion facor i i 8. S = S { < x w > } Inser 9. For = η= 0 i =1 n ( 1 1 x u 1 1 i i 10. w = /η Normalize weighs w Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 46
Πειραµατικά Α οτελέσµατα Experimenal Resuls
Laser-Based MCL Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 48
Κουρήτες: RoboCup Localizaion Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 49
Μελέτη Σύγγραµµα Κεφάλαιο 25.3 Βιβλία S. Thrun W. Burgard and D. Fox Probabilisic Roboics MIT Press 2005 Ch. 4 7 8. Άρθρα M. Arulampalam S. Maskell N. Gordon and T. Clapp Tuorial on Paricle Filers for Online Nonlinear/Non-Gaussian Bayesian Tracking IEEE Transacions on Signal Processing 50(2):174-188 2002. I. Rekleiis Paricle Filer Tuorial for Mobile Robos Technical Repor TR-CIM-04-02 McGill Universiy Canada 2002. Μ. Γ. Λαγουδάκης Τµήµα ΗΜΜΥ Πολυτεχνείο Κρήτης Σελίδα 50