Mobile Robot Localization and Map Building A Multisensor Fusion Approach Mobile Robot Localization and Map Building A Multisensor Fusion Approach Jose A. Castellanos Juan D. Tard6s Department of Computer Science and Systems Engineering University of Zaragoza ~. " Springer Science+ Business Media, LLC Library of Congress Cataloging-in-Publication Data Castellanos, Jose A., 1969- Mobile robot loealization and map building : a multisensor fusion approaeh / Jose A. Castellanos, Juan D. Tard6s. p. em. IncIudes bibliographical referenees and index. ISBN 978-1-4613-6982-0 ISBN 978-1-4615-4405-0 (eBook) DOI 10.1007/978-1-4615-4405-0 1. Mobile robots. 2. Multisensor data fusion. 1. Tard6s, Juan D., 1961-II. Title. TJ211.415. C37 2000 629.8'92--dc21 00-020317 Copyright © 1999 by Springer Science+Business Media New York Originally published by Kluwer Academic Publishers in 1999 Softcover reprint of the hardcover 1s t edition 1999 AH rights reserved. No part of this publication may be reproduced, stored in a retrieval system or transmitted in any form or by any means, mechanical, photo copying, recording, or otherwise, without the prior written pennission of the publisher, Springer Science+Business Media, LLC. Printed on acid-free paper. To Angel, Maria Jose and Yolanda. JAG To Maria, Lucia and Pedro. JDT Contents Preface xi Acknowledgements xiii 1 Introduction 1 1.1 The First-Location Problem. 4 1.2 Simultaneous Localization and Map Building 6 2 Uncertain Geometric Information 9 2.1 Introduction ............ . 9 2.2 Models for Geometric Uncertainty 11 2.3 Symmetries and Perturbations Model 13 2.4 Pairing Geometric Features . . . . 20 2.5 Suboptimal Estimation Techniques 26 2.6 Summary ............. . 30 3 Segment-based Representation of Indoor Environments 31 3.1 Introduction................ 31 3.2 Representation of Laser Range Readings 33 3.3 2D Laser Segments . . . . . . . . 38 3.4 Segmentation of Laser Readings. 41 3.5 Estimating Edges from Points . . 48 3.6 Fusion of 2D Laser Edges . . . . 51 3.7 Estimating the Length of a Segment 53 3.8 Summary . . . . . . . . . . . . . . . 54 4 Detecting High-Level Features by Multisensor Fusion 57 4.1 Introduction................. 57 4.2 High Level Features ............ 59 4.3 Processing Monocular Vision Information 69 Vlll CONTENTS 4.4 Calibration of the Multisensor System 73 4.5 Fusing Laser and Vision 75 4.6 Experimental Results. 77 4.7 Surnmary . . . . . . . . 81 5 The First-Location Problem 83 5.1 Introduction ........ . 83 5.2 Formulation of the First-Location Problem 88 5.3 Geometric Constraints . . . . . . . . . . . 90 5.4 Matching Algorithms. . . . . . . . . . . . 101 5.5 Estimation of the Mobile Robot Location 104 5.6 Visibility Constraints. . . . . . . . . . . 107 5.7 Experimenting with Laser Observations 108 5.8 Experimenting with Multisensor Fusion 120 5.9 Summary ................ . 125 6 Simultaneous Localization and Map Building 127 6.1 Introduction ............... . 127 6.2 The Symmetries and Perturbations Map 131 6.3 Neglecting Correlations . 147 6.4 Experimental Results. . . 151 6.5 Navigation in the SPmap 165 6.6 Surnmary 168 7 Conclusions 171 A Transformations and Jacobian Matrices in 2D 175 A.1 Homogeneous Matrices and Location Vectors 175 A.2 Jacobians of the Composition ......... . 177 A.3 Jacobian of the Inversion 178 A.4 Differential 'Transformations and Jacobian of a 'Trans- formation ............... . 179 B Operations with Uncertain Locations 181 B.1 Inversion of LAB .. 181 B.2 Composition of LWF and LFE 181 B.3 Centering Lw E 182 C Geometric Relations 183 IX D Experimental Equipment 187 D.l Mobile Robot OTILIO . · 187 D.2 Mobile Robot VEGA .. · 189 D.3 Mobile Robot MACROBE . · 190 Bibliography 193 Index 203 Preface The present work is a revision of the doctoral dissertation of J. A. Cas tellanos presented in the Department of Computer Science and Systems Engineering of the University of Zaragoza (Spain) in May 1998. The work is structured in six main chapters which progressively dis cuss the contributions of our research introduced in chapter 1. Thus, chapter 2 briefly describes the Symmetries and Perturbations Model (SPmodel), the geometric entities used throughout the work and some results about suboptimal estimation techniques, describing both a re cursive formulation and a batch formulation. Chapter 3 deals with the construction of a segment-based representation of the local environment of the mobile robot by using a laser rangefinder sensor. In chapter 4 we semantically upgrade the segment-based representation by using higher level features obtained by multisensor fusion. A monocular vision sys tem provides redundant information about the environment which is combined with the information gathered by the laser rangefinder to in crease the robustness and reliability of features from early stages of the processing. Chapter 5 presents the first-location problem as a matching between observed features, both monosensorial features and multi sen sorial features, and an apriori hand-made model map of the navigation area. Finally, chapter 6 describes our contributions to the problem of simultaneous mobile robot localization and map building, by introducing the concept of Symmetries and Perturbations Map (SPmap). Experi mental results are described throughout the work to verify the applic ability of the theoretical results to the real navigation of a mobile robot in a human-made indoor environment. Jose A. Castellanos and Juan D. Tard6s Zaragoza, December 15th, 1999. Acknowledgements "If I have seen farther than other men, it is because I stood on the shoulders of giants. " Sir Isaac Newton. The research work presented in this document has been developed during the last few years in the Robotics and Real Time Group of the Department of Computer Science and Systems Engineering of the Uni versity of Zaragoza. The authors would like to thank to all the group members for their collaboration and fruitful discussions. We are in debt with Jose Neira, who actively collaborated during the development of the work and who participated in many of the pub lications derived from the present research activities. Also, we thank to Jose M. Martinez-Montiel for his ever present support, his advice and his always welcomed suggestions. Part of the research work was performed in other European research laboratories during the research stays of Jose A. Castellanos. On the one hand, in the Laboratoire d'Informatique, de Robotique et de Micro electronique de Montpellier (France), where we would like to thank to P. Dauchez as head of the Robotics Department and also to M. J. Aldon and O. Strauss for their help during the research stays. On the other hand, we sincerely thank Prof. Gunther Schmidt, head of the Lehrstuhl fur Steuerungs-und Regelungstechnik, of the Technische Universitat of Munchen (Germany) for supporting the research stay in Munich and for his contributions to achieve good research goals. Different organizations funded the research activities described in this document. Authors would like to thank to the CICYT projects TAP94-0390 and TAP97-0992-C02-01 of the Spanish Government, to the Programa Europa of the CAI-DGA and to the UE project ERNET, contract CHRX-CT93-0381. Chapter 1 Introduction Caminante, son tus huellas / el camino, y nada mas; caminante, no hay camino, / se hace cam'ino al andar, Al andar se hace camino, / y al volver la vista atras se ve la senda que nunca / se ha de volver a pisar. Caminante, no hay camino, / sino estelas en la mar. "Campos de Castilla ", Antonio Machado During the last decade, many researchers have dedicated their efforts to construct revolutionary machines and to provide them with some kind of artificial intelligence to perform some of the most disgusting, risky or monotonous tasks historically assigned to human beings. Among those machines, mobile robots are undoubtedly in the cutting-edge of the cur rent research directions. An initial classification of mobile robots can be immediately considered: on the one hand, mobile robots oriented to human-made indoor environments; on the other hand, mobile robots ori ented to unstructured outdoor environments, which could include flying oriented robots, space-oriented robots and underwater robots. In our opinion, the most extended motion mechanism for surface mobile robots is the wheeled-based mechanism, adapted both to the flat surfaces, found in human-made environments, and to rough terrain, found in outdoor environments. However, some researchers have repor ted successful developments with legged-based mobile robots capable of climbing up stairs, although they require further investigation. The research work presented in this book focuses on wheeled-based mobile robots that navigate in human-made indoor environments. Some assumptions are considered throughout the described work: J. A. Castellanos et al., Mobile Robot Localization and Map Building © Kluwer Academic Publishers 1999