Development and implementation of new techniques for the localization of autonomous robotic systems, for real - time applications

Vision based navigation systems of autonomous vehicles in unknown terrain, constitute a very active research field that finds applications in other scientific regions as well. In this PhD thesis a number of electronic applications are designed and implemented with the purpose to support the process of simultaneous localization and map building in autonomous vehicles. At the same time computational techniques based on Baysian inference are studied in the framework of a fast and state of the art algorithm for simultaneous localization and mapping, the so called FastSLAM algorithm. These techniques are implemented using an observation system based on stereo vision. Hardware and software implementations are developed with the purpose to localize and build the map in real time. A final result of this thesis is the development of an original electronic circuit with very large scale of integration, for the visual observation and real-time extraction of features, which are appropriate for 3D robotic mapping. The device was developed as a system-on-aprogrammable- chip (SoC), using a Field Programmable Gate Array (FPGA). In this work, feature extraction is based on dense depth maps produced in real-time using a stereo head. For this reason, significant effort was given in order to design and implement hardware systems that are able to accelerate the computational demanding problem of correspondence in stereo pairs. A hardware system-on-a-chip was built which is able to fully parallelize the algorithm Sum of Absolute Differences (SAD), which is a local correlation technique. This hardware system is based on the parallelism of comparisons between a window in the right image and the multitude of candidate windows in the left image. Window-buffering is implemented using on-chip memory. Processing rates of hundreds of frames per second are achieved, and a maximum of 64 levels of disparity is supported. Working further on the problem of accelerating dense depth extraction, we propose a new maximum likelihood technique which can parallelize the steps of dynamic programming. This algorithm can globally optimize correspondences along epipolar scanlines. Based on this technique a new stereo system is proposed and implemented on a programmable chip. This system achieves frame rates similar to those achieved by the local correlation system, however it produces disparity maps of better quality, due to global optimization. Moreover, the system was expanded to receive support from neighboring scanlines, a technique that reduces the error rate. Both systems produce disparities in parallel with the streaming input pixels, so that they output one pixel of disparity for each pair of input pixels of the left and right image frame. Data streaming is controlled by an embedded Nios II processor. The system is supported by external RAM memory and peripheral controllers. A Cyclone II 2C35 medium FPGA device by Altera Corporation was used for the implementation and assessment of the systems. In the final observation system for robot localization and mapping the stereo accelerator based on dynamic programming was used. This stereo detector was supplemented with hardware blocks for feature extraction from the images of the stereo pair. Point features are extracted as corners under the strict condition of consistency between the right and left image. The consistency check is based on the disparity value extracted from the dense depth map. According to the observation system above, the mathematical model of observation is developed, in the framework of the fast localization and mapping algorithm (FastSLAM). This algorithm functions as a filter for uncertainty with respect to observation and vehicle motion. The algorithm is implemented using a general purpose computer, which is a host to the systemon- the-programmable-chip. The host computer and the hardware accelerator are linked through usb2.0 high speed controller. The computational techniques implemented in the framework of FastSLAM algorithm are a Rao-Blackwellized particle filter, which is used to estimate the vehicle track and a multitude of Extended Kalman Filters, used for the position update of the measured landmarks in the map. The total software and hardware system is assessed using a stereo-head developed in the laboratory. The stereo-head is attached to a small radio-controlled platform, appropriate to monitor features in an indoor environment. Part of the system odometry is based on the microcontroller which controls the servomotors, while the orientation of the vehicle is extracted with visual accuracy, making use of successive image frames. The stereo-assisted experiments of simultaneous localization and mapping prove that despite the great computational load, providing support to feature extraction by a hardware accelerator allows the track and map estimation in real-time. The proposed features are lowdimensional but especially robust for 3D mapping, due to the support by the dense depth map. The accurate extraction of vehicle orientation through the proposed visual techniques also contributes to the desired performanceΤα συστήματα πλοήγησης αυτόνομων ρομποτικών οχημάτων σε άγνωστο περιβάλλον και ειδικά τα συστήματα που στηρίζονται σε τεχνικές μηχανικής όρασης, αποτελούν πολύ ενεργό ερευνητικό πεδίο, με προεκτάσεις και σε άλλες εφαρμοσμένες περιοχές της επιστήμης. Στη διατριβή αυτή σχεδιάζονται και αξιολογούνται ηλεκτρονικές εφαρμογές για την υποστήριξη της διαδικασίας του ταυτόχρονου εντοπισμού και χαρτογράφησης σε αυτόνομα ρομποτικά οχήματα. Παράλληλα, εξετάζονται οι υπολογιστικές τεχνικές επαγωγής κατά Bayes, που αρθρώνουν τον ταχύ αλγόριθμο ταυτόχρονου εντοπισμού και χαρτογράφησης (FastSLAM). Οι τεχνικές αυτές υλοποιούνται χρησιμοποιώντας ένα σύστημα παρατήρησης με βάση τη στερεοσκοπική όραση μηχανής. Το υλικό και το λογισμικό αναπτύσσονται με σκοπό τον εντοπισμό και τη χαρτογράφηση του ρομπότ σε πραγματικό χρόνο. Ένα τελικό αποτέλεσμα της διατριβής αυτής είναι η ανάπτυξη ενός πρωτότυπου ηλεκτρονικού κυκλώματος πολύ υψηλής πυκνότητας ολοκλήρωσης, για την οπτική παρατήρηση και εξαγωγή χαρακτηριστικών σημείων, κατάλληλων για τρισδιάστατη ρομποτική χαρτογράφηση. Το κύκλωμα υλοποιήθηκε με τη μορφή συστήματος σε προγραμματιζόμενη ψηφίδα (system-on-a-programmable-chip), χρησιμοποιώντας διάταξη πυλών προγραμ- ματιζόμενων στο πεδίο (Field Programmable Gate Array). Για την εξαγωγή των χαρακτηριστικών σημείων χρησιμοποιήθηκαν τεχνικές παραγωγής πυκνών χαρτών βάθους σε πραγματικό χρόνο. Έτσι, σημαντικό μέρος αυτής της διατριβής εστιάζεται στο πρόβλημα της σχεδίασης συστημάτων υλικού (hardware) για την υλοποίηση απαιτητικών αλγορίθμων αντιστοίχησης των εικονοστοιχείων στερεοσκοπικού ζεύγους. Αναπτύχθηκε ένα σύστημα υλικού σε προγραμματιζόμενη ψηφίδα, που παραλληλίζει πλήρως τον αλγόριθμο τοπικής συσχέτισης «Άθροισμα Απόλυτων Διαφορών» (Sum of Absolute Differences). Το σύστημα αυτό στηρίζεται στον παραλληλισμό των συγκρίσεων ανάμεσα σε ένα παράθυρο της δεξιάς εικόνας και στο σύνολο των υποψήφιων αντίστοιχων παραθύρων στην αριστερή εικόνα. Για την αποθήκευση των παραθύρων χρησιμοποιείται ενσωματωμένη μνήμη επί της ψηφίδας. Επιτυγχάνονται ρυθμοί εκατοντάδων πλαισίων το δευτερόλεπτο και μέγιστο εύρος παραλλάξεων 64 επιπέδων. Επίσης, προτείνεται ένας νέος αλγόριθμος μεγιστοποίησης της πιθανότητας αντιστοίχησης (maximum likelihood), που παραλληλίζει πλήρως τα βήματα του δυναμικού προγραμματισμού. Ο αλγόριθμος αυτός, επιτελεί καθολική βελτιστοποίηση των αντιστοιχήσεων κατά μήκος των επιπολικών γραμμών. Με βάση τον αλγόριθμο αυτό σχεδιάστηκε και υλοποιήθηκε ένα νέο στερεοσκοπικό σύστημα σε προγραμματιζόμενη ψηφίδα. Το σύστημα αυτό επιτυγχάνει επιδόσεις αντίστοιχες με αυτές του συστήματος τοπικής συσχέτισης, αλλά παράγει καλύτερους ποιοτικά χάρτες βάθους. Επιπλέον, το σύστημα επεκτάθηκε ώστε να λαμβάνει υποστήριξη από γειτονικές γραμμές σάρωσης, κάτι που μειώνει αισθητά το ρυθμό εμφάνισης σφαλμάτων αντιστοίχησης. Τα δύο παραπάνω συστήματα παράγουν παραλλάξεις παράλληλα με τη ροή των εισερχόμενων εικονοστοιχείων, ώστε εξάγουν ένα εικονοστοιχείο παράλλαξης για κάθε εισερχόμενο ζεύγος εικονοστοιχείων της δεξιάς και της αριστερής εικόνας. Η ροή των δεδομένων στην ψηφίδα υποστηρίζεται από ενσωματωμένο επεξεργαστή Nios II, στον οποίο ανατίθεται ο έλεγχος της διάταξης. Το σύστημα συμπληρώνεται από εξωτερική μνήμη και περιφερειακούς ελεγκτές. Για την υλοποίηση και την αξιολόγηση των συστημάτων χρησιμοποιήθηκε η διάταξη Cyclone II 2C35 της εταιρίας Altera. Στο τελικό σύστημα παρατήρησης για τον εντοπισμό και τη χαρτογράφηση, χρησιμοποιήθηκε το στερεοσκοπικό σύστημα δυναμικού προγραμματισμού. Ο στερεοσκοπικός ανιχνευτής συμπληρώθηκε με μια διάταξη εξαγωγής χαρακτηριστικών σημείων από τις εικόνες του στερεοσκοπικού ζεύγους. Τα σημεία εξάγονται ως γωνίες, με την απαίτηση να υπακούουν στο αυστηρό κριτήριο της συνέπειας ανάμεσα στη δεξιά και στην αριστερή εικόνα (left-right consistency). Ο έλεγχος της συνέπειας στηρίζεται στην τιμή της παράλλαξης, που εξάγεται από τον πυκνό χάρτη βάθους. Με βάση την παραπάνω διάταξη παρατήρησης, αναπτύσσεται το μαθηματικό μοντέλο παρατήρησης (observation model), το οποίο ενσωματώνεται στον αλγόριθμο ταχέως εντοπισμού και χαρτογράφησης. Ο αλγόριθμος λειτουργεί ως φίλτρο αβεβαιότητας για την τροχιά και τον χάρτη, και υλοποιείται σε υπολογιστή γενικού σκοπού, ο οποίος παίζει το ρόλο του ξενιστή για το σύστημα στην προγραμματιζόμενη ψηφίδα. Η σύνδεση ανάμεσα στον ξενιστή υπολογιστή και στον συνεπεξεργαστή υλικού γίνεται μέσω ελεγκτή διασύνδεσης usb2.0, υψηλής ταχύτητας (high speed). Οι βασικές τεχνικές που υλοποιούνται στο πλαίσιο του αλγορίθμου FastSLAM είναι ένα φίλτρο σωματιδίων κατά Rao-Blackwell, για την εκτίμηση της τροχιάς, και μια πληθώρα επεκτάσεων του φίλτρου Kalman (Extended Kalman Filters) για την ανανέωση της θέσης των χαρακτηριστικών σημείων στο χάρτη. ..