L-SLAM: Μείωση διαστάσεων στην οικογένεια αλγορίθµων FastSLAM Πετρίδης Βασίλειος, Ζήκος Νικόλαος Αριστοτέλειο Πανεπιστήµιο Θεσσαλονίκης ΠΕΡΙΛΗΨΗ Στο παρών άρθρο προτείνεται µια νέα µέθοδος χαρτογράφησης και εντοπισµού (SLAM Simoultaneous Localization and Manpping) µε ονοµασία L-SLAM που αναπτύχθηκε στο εργαστήριο Αυτοµατισµού και Ροµποτικής του Τ.Η.Μ.Μ.Υ του Α.Π.Θ. Η προτεινόµενη µέθοδος µειώνει τον αριθµό των διαστάσεων του particle filter που χρησιµοποιείται στην οικογένεια αλγορίθµων FastSLAM. Με τον τρόπο αυτό επιτυγχάνει καλύτερα αποτελέσµατα µε µικρότερο αριθµό particles. Η µείωση του αριθµού των διαστάσεων αποτελεί και το σηµείο κλειδί του αλγορίθµου αυτού, αφού µπορεί να ανταπεξέλθει πιο γρήγορα σε προβλήµατα πολλών διαστάσεων όπως είναι το SLAM σε τρισδιάστατο περιβάλλον (3D-SLAM). Στην παρούσα εργασία παρουσιάζεται η µεθοδολογία εφαρµοσµένη σε ένα τετράτροχο όχηµα εφοδιασµένο µε αισθητήρα laser. Παρουσιάζονται αποτελέσµατα από προσοµοιωµένα περιβάλλοντα αλλά και από πραγµατικά δεδοµένα και γίνεται σύγκριση µεταξύ των µεθόδων FastSLAM και L-SLAM. Λέξεις κλειδιά: L-SLAM, FastSLAM, particle filters, µείωση διαστάσεων. 1 ΕΙΣΑΓΩΓΗ Η χαρτογράφηση και ο εντοπισµός είναι µια θεµελιώδη ικανότητα που πρέπει να έχουν όλες οι ροµποτικές πλατφόρµες που κινούνται σε ένα άγνωστο και αβέβαιο περιβάλλον. Συνήθως ο χάρτης αναπαρίσταται από ένα σύνολο σηµείων στο χώρο, µε κάθε ένα από αυτά να αντιστοιχεί και σε ένα εµπόδιο ή τµήµα εµποδίου. Το 22 προτάθηκε ο αλγόριθµος FastSLAM [2,3,4] που µε την χρήση των particle filters [5] και των Extended Kalman Filters (EKF) επιτύγχαναν πιο γρήγορα αποτελέσµατα σε σχέση µε τον EKF-SLAM [6]. Σε αυτή την εργασία παρουσιάζουµε µία νέα µέθοδο επίλυσης του προβλήµατος SLAM µε όνοµα L-SLAM (Low dimensional SLAM) [1]. Η µέθοδος αυτή αναπτύχθηκε στο εργαστήριο Αυτοµατισµού και Ροµποτικής του Τ.Η.Μ.Μ.Υ του Α.Π.Θ. Κεντρική ιδέα είναι να δειγµατοληπτούµε, για το particle filter, µόνο την γωνία προσανατολισµού του οχήµατος (σε αντίθεση µε το FastSLAM που γίνεται δειγµατοληψία στη θέση και τον προσανατολισµό), γιατί ουσιαστικά αυτή είναι που προσθέτει µη γραµµικότητες στο πρόβληµα. Ενώ η εκτίµηση της θέσης του οχήµατος µετατίθεται στο Kalman φίλτρο. Έτσι µειώνονται οι διαστάσεις του προβλήµατος από τρεις σε µία και άρα η πολυπλοκότητα του προβλήµατος από (Ο 3 ) σε (Ο). Σε αντίθεση µε τον αλγόριθµο FastSLAM που χρησιµοποιεί Extended Kalman Filter, ο L-SLAM αλγόριθµος χρησιµοποιεί µια γραµµικοποιηµένη µορφή των εξισώσεων κατάστασης και γι αυτό και γίνεται χρήση του απλού Kalman φίλτρου. Στην παρούσα εργασία παρουσιάζεται η µεθοδολογία εφαρµοσµένη σε ένα τετράτροχο όχηµα εφοδιασµένο µε αισθητήρα laser.
Παρουσιάζονται αποτελέσµατα από προσοµοιωµένα περιβάλλοντα αλλά και από πραγµατικά δεδοµένα από το dataset carpark του πανεπιστηµίου του Σίδνεϊ [7,9] και γίνεται σύγκριση µεταξύ των µεθόδων FastSLAM 1. και 2. και L-SLAM. 2 SLAM Συνήθως οι αυτόνοµες ροµποτικές πλατφόρµες κινούνται σε άγνωστα περιβάλλοντα µε εµπόδια. Σκοπός των αλγορίθµων SLAM είναι να κατασκευάσουν τον χάρτη του περιβάλλοντος ενώ ταυτόχρονα να ανακτούν κάθε στιγµή την θέση και τον προσανατολισµό του οχήµατος. Αυτό επιτυγχάνεται µε την χρήση αισθητήρων που αλληλεπιδρούν µε το περιβάλλον (όπως laser) και µε την χρήση των εντολών εισόδου (π.χ. γκάζι, τιµόνι) που εισέρχονται στο σύστηµα. Η θέση p και ο προσανατολισµός φ του οχήµατος αναπαρίστανται από µία χρονοσειρά. Ο χάρτης θεωρείται σαν ένα σύνολο Θ από σηµεία στον χώρο που κείτονται στις επιφάνειες των εµποδίων. Η πυκνότητα των σηµείων ανα µονάδα επιφανείας εξαρτάται από την παραµετροποίηση του data association [7]. Οι περισσότεροι αλγόριθµοι SLAM προσπαθούν να εκτιµήσουν την κατανοµή της παρακάτω πιθανότητας [8] : ( 2) όπου z είναι η χρονοσειρά µε τα δεδοµένα από τα αισθητήρια όργανα και u είναι η χρονοσειρά από τα διανύσµατα ελέγχου του οχήµατος, µέχρι την στιγµή t. Το κινηµατικό µοντέλο του οχήµατος της εικόνας 1 περιγράφεται από τις παρακάτω εξισώσεις, µε υ να εκφράζει την ταχύτητα του οχήµατος και s την γωνία των µπροστινών τροχών την χρονική στιγµή t. yg py OG yr L OR px 1 d1 1 Εικόνα 1 s xr ( 1) 2 xg ( 3) 3 L-SLAM Στην µέθοδο L-SLAM παραγοντοποιούµε την παραπάνω πιθανότητα ώστε η θέση του οχήµατος και η θέση των εµποδίων να υπολογίζεται ανεξάρτητα από τον προσανατολισµό εφόσον αυτός είναι γνωστός. ( 4) Το πρώτο βήµα της µεθόδου L-SLAM είναι να δηµιουργήσουµε τυχαίες τιµές για τον προσανατολισµό του οχήµατος βασιζόµενοι στην προηγούµενη χρονικά τιµή του προσανατολισµού, το κινηµατικό µοντέλο και τo διάνυσµα ελέγχου του οχήµατος. Κάθε µια από τις τυχαίες αυτές τιµές αντιστοιχεί και σε ένα particle.
όπου L το µεταξόνιο του οχήµατος και u, s οι τιµές της ταχύτητας και της γωνίας των εµπρόσθιων τροχών αντίστοιχα. Ο δείκτης i αντιστοιχεί σε κάθε particle. Σε αντίθεση µε τον αλγόριθµο FastSLAM προτείνεται εξίσωση τεσσάρων καταστάσεων που περιλαµβάνουν την θέση του οχήµατος και την θέση του κάθε εµποδίου. ( 5) ( 6) όπου µ είναι οι καταστάσεις του συστήµατος κάθε στιγµή t, για κάθε particle i και για κάθε εµπόδιο j. Οι εξισώσεις κατάστασης γραµµικοποιούνται στην παρακάτω µορφή ώστε να ενταχθούν στο φίλτρο Kalman. ( 7) Το διάνυσµα ν αποτελεί επίσης γραµµικοποιηµένη µορφή του θορύβου και εξαρτάται από τον θόρυβο της ταχύτητας ε 1 αλλά και από τον προσανατολισµό του οχήµατος όπως αυτός υπολογίστηκε παραπάνω για κάθε particle i ξεχωριστά. ( 8) Ως µοντέλο µέτρησης θεωρούµε την σχετική θέση του οχήµατος µε το κάθε εµπόδιο που παρατηρεί µέσω του laser. Έτσι το µοντέλο µέτρησης αποτελείται από την µετατόπιση κατά x και κατά y από το όχηµα προς το εµπόδιο. Θεωρώντας τις µετρήσεις d και θ ότι έχουν θόρυβο ε3 και ε4 αντίστοιχα καταλήγουµε στην παρακάτω εξίσωση. ( 9) ( 1) Επειδή οι τιµές του θορύβου είναι µικρές για τους αισθητήρες laser µπορούµε να απλοποιήσουµε τις παραπάνω εξισώσεις θεωρώντας ότι cos(ε)=1 και sin(ε)=ε. ( 11)
όπου ξ είναι η γραµµικοποιηµένη µορφή του διανύσµατος θορύβου στο µοντέλο µέτρησης. Έτσι καταλήγουµε στην γραµµικοποιηµένη µορφή του µοντέλου µέτρησης που περιγράφεται από την παρακάτω εξίσωση για κάθε particle i και για κάθε εµπόδιο j. ( 12) Εφόσον οι παραπάνω θόρυβοι ε 1,ε 3 και ε 4 είναι κανονικής κατανοµής και µηδενικής µέσης τιµής, µπορούµε µε τις παραπάνω εξισώσεις να συντάξουµε τις εξισώσεις ανανέωσης του φίλτρου Kalman. ( 13) ( 14) όπου µ είναι η µέση τιµή της εκτίµησης των καταστάσεων, Σ είναι ο πίνακας ετεροσυσχετίσεων των καταστάσεων, y το σφάλµα και Ζ ο πίνακας καινοτοµίας. Η ανανέωση του φίλτρου Kalman υπολογίζετε κάθε χρονική στιγµή t, για κάθε εµπόδιο j, σε κάθε particle i. Για κάθε ένα από αυτά υπολογίζεται το importance factor w χρησιµοποιώντας τον πίνακα Ζ και το σφάλµα y. Το γινόµενο των επιµέρους importance factor του κάθε particle πολλαπλασιαζόµενο µε το ολικό importance factor της προηγούµενης στιγµής αποτελεί το συνολικό importance factor που χαρακτηρίζει το particle i. ( 15) Ο παράγοντας αυτός αποτελεί και το µέτρο σύγκρισης για τα particles, µε το πιο ισχυρό να λαµβάνει και την υψηλότερη τιµή. Το particle µε την µεγαλύτερη τιµή importance factor καθορίζει ποια είναι η καλύτερη εκτίµηση του προσανατολισµού του οχήµατος αλλά επίσης καθορίζει και την καλύτερη θέση του οχήµατος και των εµποδίων µέσω των Kalman φίλτρων του ισχυρότερου particle. Στο τέλος της κάθε επανάληψης ένας αλγόριθµος επαναδειγµατοληψίας (resampling) [2,1,11] αναλαµβάνει να αντικαταστήσει τα πιο «ανίσχυρα» particles κλωνοποιώντας κάποια απο τα πιο «ισχυρά». Στην παρούσα εργασία εφαρµόστηκε η µέθοδος RSR (Residual Systematic Resample) [11]. Η µέθοδος αυτή δεν εφαρµόζεται σε κάθε επανάληψη, αλλά µόνο όταν το effective sample size [1] πέφτει κάτω από 75%. ( 16)
4 ΑΠΟΤΕΛΕΣΜΑΤΑ Meters 5 4 3 2 1 Robot Position Error Standard Deviation (m) 1.9.8.7.6.5.4.3.2.1 FastSLAM 1. FastSLAM 2. L SLAM 1 1 1 2 3 4 5 Meters 5 1 15 2 25 3 35 4 45 5 Particles Εικόνα 2α. Χάρτης εµποδίων από το περιβάλλον προσοµοίωσης. Εικόνα 2β. Συγκριτικά αποτελέσµατα των τριών µεθόδων µε διάφορες τιµές particles. Παρατίθενται η µέση τιµή και η τυπική απόκλιση του σφάλµατος θέσης του οχήµατος. Η µέθοδος αυτή αρχικά εφαρµόστηκε σε ένα προσοµοιωµένο περιβάλλον, όπως φαίνεται στην παρακάτω εικόνα. Η αντιστοίχηση µεταξύ των εµποδίων και των µετρήσεων του laser (data assocciation) θεωρείται γνωστή. Η τυπική απόκλιση των θορύβων στην µέτρηση της ταχύτητας, του τιµονιού, της απόστασης στο laser και της γωνίας στο laser θεωρήθηκε αντίστοιχα σ v =.4m, σ s =.5rad, σ d =.1m, σ θ =.1rad. Τρέξαµε την κάθε µέθοδο µε διαφορετικό αριθµό particles (5, 1, 2 και 4 particles) καθώς το όχηµα έκανε µία κυκλική κίνηση. Επαναλάβαµε την διαδικασία αυτή 2 φορές για κάθε µέθοδο για να συλλέξουµε στατιστικά του σφάλµατος της εκτίµησης. Η µέθοδος FastSLAM 1. δίνει τα χειρότερα αποτελέσµατα µε µικρό αριθµό particles διότι ο αριθµός αυτός δεν αρκεί για να συγκλίνει στην σωστή λύση. Αντιθέτως ο αλγόριθµος FastSLAM 2. δίνει τα καλύτερα αποτελέσµατα αλλά υστερεί σε υπολογιστική πολυπλοκότητα αφού είναι περίπου τρεις φορές πιο αργός από τους άλλους δύο αλγορίθµους [1] (όπως φαίνεται και στην εικόνα 4). 3 2 1 1 2 Robot Position Error Standard Deviation (m) 2 1.8 1.6 1.4 1.2 1.8.6.4.2 FastSLAM 1. FastSLAM 2. L SLAM 3 2 1 1 2 3 4 5 1 15 2 25 3 35 4 45 5 Particles Εικόνα 3α. Χάρτης εµποδίων και διαδροµή του οχήµατος στο dataset carpark. Εικόνα 3β. Συγκριτικά αποτελέσµατα των τριών µεθόδων µε διάφορες τιµές particles. Παρατίθενται η µέση τιµή και η τυπική απόκλιση του σφάλµατος θέσης του οχήµατος.
Οι τρεις αλγόριθµοι εφαρµόστηκαν και συγκριθήκαν και σε πραγµατικά δεδοµένα που προέρχονται από το dataset carpark του Πανεπιστηµίου του Σίδνεϊ[7,9] (εικ. 3α,β). Η αντιστοίχηση των δεδοµένων του laser µε τα εµπόδια έγινε και στις τρεις περιπτώσεις µε την µέθοδο Monte Carlo data assocciation[7]. Και αυτό το σετ πειραµάτων έγινε µε τον ίδιο τρόπο που αναφέρθηκε παραπάνω. Τα αποτελέσµατα δεν διαφοροποιούνται και πολύ από τα παραπάνω µε την µόνη διαφορά ότι ο αλγόριθµος FastSLAM 1. να δείχνει µεγαλύτερη ασυνέπεια[9] στα αποτελέσµατα όταν ο αριθµός των particles είναι µικρός. Η σύγκριση των εκτιµήσεων έγινε µε δεδοµένα από GPS και άλλα µέσα µέτρησης θέσης που χρησιµοποιήθηκαν από το Πανεπιστήµιο του Σίδνεϊ κατά την Εικόνα 4. Σύγκριση του χρόνου εκτέλεσης των τριών αλγορίθµων. Οι µέθοδοι δοκιµάστικαν για διεξαγωγή των πειραµάτων[7,9]. Όλοι οι 5, 1, 2 και 4 particles στο dataset carpark του παραπάνω αλγόριθµοι υλοποιήθηκαν στο Πανεπιστιµίου του Σίδνεϊ. Matlab και η εκτέλεσή τους έγινε στον ίδιο υπολογιστή. Στην εικόνα 4 παρατίθεται το διάγραµµα µε τους χρόνους εκτέλεσης και των τριών µεθόδων στο dataset carpark. 5 ΣΥΜΠΕΡΑΣΜΑΤΑ Τα αποτελέσµατα δείχνουν ότι η µέθοδος L-SLAM δίνει πολύ καλύτερα αποτελέσµατα σε σχέση µε την µέθοδο FastSLAM 1. ενώ δίνει περίπου τα ίδια αποτελέσµατα µε τον FastSLAM 2. αλλά µε πολύ µικρότερη υπολογιστική πολυπλοκότητα η οποία είναι περίπου τρεις φορές µικρότερη. Μελλοντικά θα γίνει επέκταση της µεθόδου L-SLAM για χρήση σε τρισδιάστατα περιβάλλοντα µε 3d laser scanners καθώς και σύγκριση µε άλλες µεθόδους. Επίσης σχεδιάζεται εφαρµογή της µεθόδου αυτής σε ροµποτική πλατφόρµα του Τ.Η.Μ.Μ.Υ. του Α.Π.Θ. 6 ΒΙΒΛΙΟΓΡΑΦΙΑ [1] V. Petridis and N. Zikos, L-SLAM: reduced dimensionality FastSLAM algorithms. WCCI 21, World Congress on Computational Intelligence, Jule 21. [2] M. Montemerlo and S. Thrun, Simultaneous localization and map- ping with unknown data association using fastslam, in Robotics and Automation, 23. Proceedings. ICRA 3. IEEE International Conference on Robotic and Automation, vol. 2, Sept. 23, pp. 1985 1991 vol.2. [3] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fastslam: A factored solution to the simultaneous localization and mapping problem, in AAAI/IAAI, 22, pp. 593 598. [4] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fastslam 2.: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges, in IJCAI, 23, pp. 1151 1156. [5] A. Doucet, N. de Freitas, K. P. Murphy, and S. J. Russell, Rao-blackwellised particle filtering for dynamic bayesian networks, in UAI, 2, pp. 176 183. [6] J. Sasiadek, A. Monjazeb, and D. Necsulescu, Navigation of an autonomous mobile robot using ekf-slam and fastslam, in Control and Automation, 28 16th Mediterranean Conference on, June 28, pp. 517 522. [7] J. Nieto, J. Guivant, E. Nebot, and S. Thrun, Real time data association for fastslam, in Robotics and Automation, 23. Proceedings. ICRA 3. IEEE International Conference on, vol. 1, Sept. 23, pp. 412 418 vol.1. [8] K. P. Murphy, Bayesian map learning in dynamic environments, in NIPS, 1999, pp. 115 121. [9] T. Bailey, J. Nieto, and E. Nebot, Consistency of the fastslam algorithm in Robotics and Automation, 26. ICRA 26. Proceedings 26 IEEE International Conference on, May 26, pp. 424 429. [1] N. Kwak, G.-w. Kim, and B.-h. Lee, A new compensation technique based on analysis of resampling process in fastslam, Robotica, vol. 26, no. 2, pp. 25 217, 28. [11] Bolić, M.a, Djurić, P.M.b, Hong, S.b "Resampling algorithms and architectures for distributed particle filters IEEE Transactions on Signal Processing Volume 53, Issue 7, July 25, Pages 2442-245