Where am I ? Contributions to the Localization Problem of Mobile Robots Von der Carl-Friedrich-Gauß-Fakult¨at der Technischen Universit¨at Carolo-Wilhelmina zu Braunschweig zur Erlangung des Grades eines Doktor-Ingenieur (Dr.-Ing.) genehmigte Dissertation von Dipl.-Inform. Ren´eIser geboren am 18. 07. 1981 in Braunschweig Eingereicht am: 29. 03. 2012 Mundliche¨ Prufung¨ am: 14. 09. 2012 1. Referent: Prof. Dr.-Ing. Friedrich M. Wahl 2. Referent: Prof. Dr. rer.-nat. Wolfram Burgard 2012 i Danksagung Die vorliegende Arbeit ist im Rahmen meiner T¨atigkeit als wissenschaftlicher Mitarbei- ter am Institut fur¨ Robotik und Prozessinformatik der Technischen Universit¨at Carolo- Wilhelmina zu Braunschweig entstanden. Bei dem Leiter des Instituts, Herrn Prof. Dr.-Ing. Friedrich M. Wahl bedanke ich mich ganz herzlich fur¨ die wertvolle und umfangreiche Unterstutzung,¨ sowie die M¨oglichkeit, sich als Wissenschaftler in einer freien und angenehmen Umgebung entwickeln zu k¨on- nen. Anders w¨are die Entstehung dieser Arbeit sicher nicht m¨oglich gewesen. Weiterhin m¨ochte ich mich bei Prof. Dr. rer.-nat. Wolfram Burgard fur¨ die Ubernahme¨ des Korefe- rats bedanken. Außerdem bedanke ich mich bei der Deutschen Forschungsgemeinschaft (DFG), welche diese Arbeit teilweise gef¨ordert hat (WA 848/14-3). Besonderer Dank geht auch an Arthur Martens, der mit viel Entwicklungsarbeit zur Umsetzung der beschriebenen Verfahren beigetragen hat. In diesem Zusammenhang bedanke ich mich bei Simon Winkelbach fur¨ die hilfreichen und konstruktiven Vorschl¨age w¨ahrend der Korrekturphase. Einzelne Teile dieser Arbeit sind in enger Zusammenarbeit mit Daniel Kubus entstanden. Vielen Dank dafur,¨ Daniel. An dieser Stelle bedanke ich mich auch bei Stefan H¨oßler und Harald Meyer-Kirk ohne deren wertvolle Arbeiten an den Robotern diese Dissertation nicht entstanden w¨are. Insgesamt m¨ochte ich bei allen Mitgliedern des Instituts ganz herzlich fur¨ die wunder- baren Jahre bedanken. Dies gilt insbesondere fur¨ die kreativen Konversationen in der Teekuche.¨ Ihr seid die besten Kollegen die man sich wunschen¨ kann. Mein Dank geht auch an Steffen und Chris fur¨ die moralische Unterstutzung¨ der letz- ten Jahre. Bei Hannes und Tati m¨ochte ich mich fur¨ die wunderbaren Urlaube und Tauchabenteuer bedanken. Schließlich m¨ochte ich mich bei meiner Babs bedanken. Danke fur¨ die sch¨one Zeit, die wir bisher miteinander hatten und danke, daß du fur¨ mich da bist. Braunschweig, im M¨arz 2012 Ren´eIser iii Kurzfassung Das Lokalisierungsproblem mobiler Roboter beschreibt die Aufgabe, deren Position und Orientierung, die sog. Pose, bezuglich¨ eines gegebenen Weltkoordinatensystems zu be- stimmen. Die F¨ahigkeit zur Selbstlokalisierung wird in vielen Anwendungsbereichen mo- biler Roboter ben¨otigt, wie etwa bei dem Materialtransport in der industriellen Ferti- gung, bei Eins¨atzen in Katastrophengebieten oder sogar bei der Exploration fremder Planeten. Die genannten Beispiele zeigen die fundamentale Bedeutung der Selbstlokali- sierung. Eine Unterteilung existierender Verfahren zur L¨osung des genannten Problems erfolgt je nachdem ob eine Lokalisierung auf lokaler oder auf globaler Ebene stattfindet. Globale Lokalisierungsalgorithmen bestimmen die Pose des Roboter bezuglich¨ eines Weltkoordinatensystems ohne jegliches Vorwissen, wohingegen bei lokalen Verfahren eine grobe Sch¨atzung der Pose vorliegt, z.B. durch gegebene Odometriedaten des Roboters. In Analogie zu einem Roboter, der von seiner aktuellen Pose verschwindet und an einem anderen Ort wieder erscheint, wird das globale Lokalisierungsproblem in der Literatur auch h¨aufig kidnapped robot Problem genannt. Im Rahmen dieser Dissertation wird ein neuer Ansatz zur L¨osung des globalen Lo- kalisierungsproblems vorgestellt. Die grundlegende Idee ist, eine lokale Karte und eine globale Karte in Ubereinstimmung¨ zu bringen. Die lokale Karte wird durch ein effizientes Scanmatching-Verfahren generiert. Der beschriebene Ansatz ist ¨außerst robust sowohl gegenuber¨ Mehrdeutigkeiten der Roboterpose als auch dem Auftreten dynamischer Hin- dernisse in nicht-statischen Umgebungen. Zur Berechnung der globalen Karte wird ein Verfahren zur L¨osung des Simultaneous Localization and Mapping (SLAM) Problems beschrieben, welches darauf basiert, die globale Karte in eine Menge lokaler Kartenfrag- mente zu unterteilen. Diese werden anschließend mittels eines Optimierungsalgorithmus konsistent aneinandergefugt,¨ welcher auf dem Prinzip der Swarm Intelligence beruht. Der Algorithmus zur globalen Lokalisierung besteht haupts¨achlich aus drei Komponen- ten: Einem Scanmatcher zur Generierung der lokalen Karte, einer Methode zum matchen von lokaler und globaler Karte und einer Instanz, welche entscheidet, wann der Robo- ter mit hinreichender Sicherheit korrekt lokalisiert ist. Das Matching von lokaler und globaler Karte wird dabei von einer parallelisierten Variante des Random Sample Mat- ching (pRANSAM) durchgefuhrt,¨ welche eine Menge von Posenhypothesen liefert. Diese Hypothesen werden in einem weiteren Schritt analysiert, um bei hinreichender Eindeu- tigkeit die korrekte Roboterpose zu bestimmen. Umfangreiche Experimente belegen die Zuverl¨assigkeit und Genauigkeit des in dieser Dissertation vorgestellten Verfahrens. v Abstract Self-localization addresses the problem of estimating the position and orientation (the pose) of mobile robots with respect to a certain coordinate system of their workspace. It is needed for various mobile robot applications like material handling in industry, disaster zone operations, vacuum cleaning, or even the exploration of foreign planets. Thus, self-localization is a very essential capability. This problem has received considerable attention over the last decades. It can be decomposed into localization on a global and local level. Global techniques are able to localize the robot without any prior knowledge about its pose with respect to an a priori known map. This problem is also referred to as the kidnapped robot problem, i.e. a robot disappears from its current pose and is carried to an arbitrary but unknown place. In contrast, local techniques (tracking) aim to correct so-called odometry errors occur- ring during robot motion. These errors are due to imprecise sensors estimating the active motion parameters like velocity, acceleration, etc.. Hence, tracking is mandatory for mobile robot navigation. In this thesis, the global localization problem for mobile robots is mainly addressed. The proposed method is based on matching an incremental local map to an a priori known global map. This approach is very time and memory efficient and robust to structural ambiguity as well as with respect to the occurrence of dynamic obstacles in non-static environments. Prior to global localization, a map representing the environment is re- quired. To this end, a solution to the SLAM-Problem is proposed which computes a set of local map fragments globally aligned by an optimization routine. Optimization is performed using swarm intelligence. The algorithm for global localization consists of several components like ego motion estimation or global point cloud matching. Nowa- days most computers feature multi-core processors and thus map matching is performed by applying a parallelized variant of the Random Sample Matching (pRANSAM) ap- proach originally devised for solving the 3D-puzzle problem. pRANSAM provides a set of hypotheses representing alleged robot poses. Techniques are discussed to postprocess the hypotheses, e.g. to decide when the robot pose is determined with a sufficient ac- curacy. Furthermore, runtime aspects are considered in order to facilitate localization in real-time. Finally, experimental results demonstrate the robustness of the method proposed in this thesis. vii Contents 1. Introduction 1 1.1. ExistingTechniques. 3 1.2. Contribution and Organization of this Work . ...... 6 References...................................... 7 2. AResampling-BasedScanMatchingApproach 11 2.1. Introduction.................................. 11 2.2. RelatedWorks................................. 13 2.3. Resampling-BasedScanMatching . ... 15 2.4. ExperimentalResults. 22 2.4.1. GroundTruthEvaluation . 24 2.4.2. MapBuilding ............................. 33 2.5. Conclusion................................... 38 References...................................... 39 3. AntSLAM - Applying Swarm Intelligence to Global Map Optimization 43 3.1. Introduction.................................. 43 3.2. RelatedWorks................................. 45 3.3. ReviewofAntColonyOptimization. ... 46 3.4. ApplicationofAntColonySystemtoSLAM . ... 49 3.4.1. ConstructionoftheGraph . 50 3.4.2. GlobalMapOptimization . 57 3.5. ExperimentalResults. 60 3.5.1. Discussion............................... 66 3.6. Conclusion................................... 68 References...................................... 68 4. Global Point Cloud Matching 73 4.1. Introduction.................................. 73 4.2. RelatedWorks................................. 75 4.3. Review of the Random Sample Matching (RANSAM) - Approach .... 76 4.4. pRANSAM - An Efficient Parallel Approach to Random Sample Matching 78 4.4.1. OptionsofParallelization . 79 4.4.2. Optimal Relation Table Assignment to the Processor Cores. 80 4.4.3. Real-TimeAspects . 85 4.5. ExperimentalResults. 86 4.5.1. HardwareandSoftwareSetup . 86 viii Contents 4.5.2. Scalability............................... 87 4.5.3. FuzzyContactCriterion . 90 4.5.4. ValidationofCharacteristics . ... 92 4.6. Conclusion..................................