Feature-Based Robot Navigation in Known and Unknown Environments

Home , Robot navigation

FEATURE-BASED ROBOT NAVIGATION IN KNOWN AND UNKNOWN ENVIRONMENTS

THÈSE N° 2765 (2003)

PRÉSENTÉE À LA FACULTÉ SCIENCES ET TECHNIQUES DE L’INGENIEUR SECTION DE MICROTECHNIQUE

ÉCOLE POLYTECHNIQUE FÉDÉRALE DE LAUSANNE

POUR L’OBTENTION DU GRADE DE DOCTEUR ÈS SCIENCES

PAR

Kai Oliver ARRAS

Dipl. El.-Ing ETH et de nationalité allemande

acceptée sur proposition du jury:

Prof. R. Siegwart, directeur de thèse Prof. W. Burgard, rapporteur Prof. H. Christensen, rapporteur Prof. J. Crowley, rapporteur Prof. D. Floreano, rapporteur Prof. J. Leonard, rapporteur

Lausanne, EPFL 2003

Acknowledgements

This thesis is a result of my efforts as a research assistant and doctoral student at the Au- tonomous Systems Lab of the Swiss Federal Institute of Technology Lausanne (EPFL). During that time, I have been supported by various people to whom I wish to express my gratitude. I am indebted to Prof. Roland Siegwart; he offered me the possibility to work in a liberal environment and gave me the freedom to conduct my research in an independent way. His receptiveness to new and different ideas and his willingness to leave them space and time were always important sources of inspiration and motivation. Thanks to him I also learned that “café au lait” has nothing to do with Spain. I would also like to thank Prof. Wolfram Burgard, Prof. Henrik I. Christensen, Prof. James L. Crowley, Prof. Dario Floreano and Prof. John J. Leonard for being my (very com- petent) co-examiners. Many people have contributed to this thesis: Thanks to Sjur Vestli for putting me on the track and being more than an important catalyst, Nicola Tomatis for his contribution in our collaboration on Pygmalion and giving eyesight to the robot, Gilles Caprari for our collaboration on his sugar-cube robot Alice and Jan “3d” Weingarten for the delightful finding that this thesis is about 2d navigation only. I wish to express my gratitude to several former students: Felix Wullschleger, now with the Institute of Robotics, ETH Zurich, for his collaboration on our navigation software, Jan Persson for his great obstacle avoidance algorithm, Cédric Glauser for recording loads of useful experimental SLAM data and Martin Schilt whose work had a significant impact to the global localization part of this thesis. Thanks also to Prof. José A. Castellanos from the University of Zaragoza for making my stay at the Robotics and Real-Time Group possible where I was a guest researcher during the period of November 2000 to February 2001. Finally, I am deeply grateful for the support of many kinds I got from my family, friends and flat-mates who were at my side during that time.

Abstract

This thesis is about feature-based mobile robot navigation in known and unknown environments. The work covers a wide range of problems, from system integration aspects up to the successful deployment of autonomous robots in application-like events. For the sensors of the robot error models with a sound physical basis are derived and used for the probabilistic extraction of lines, segments, retro-reflecting beacons and vertical edges. A line extraction algorithm with a sliding window technique and a model fidelity criterion for segmentation is presented. In order to avoid oversegmentation and for the sake of accuracy, significantly collinear lines get fused in a final step using a nearest-neighbor like clustering algorithm. Results from different laser scanners demonstrate a precise and consistent segment-based description of the scene. This description is augmented by the semantical information from the features. The result is a weighted string of symbols encoding the topology and main characteristics of scene in a concise way. As one of the basic skills of a mobile robot, the localization problem receives particular attention. Multisensor pose tracking with an extended Kalman filter (EKF) is taken as a base line in order to study precision, robustness and limitations of this approach. Aspects deriving from on-the-fly navigation are fully addressed: the need to compensate distortion of sensory raw data and to temporally maintain relations of observations (across sensors and time), estimation results and the current time. Despite very good result for precision, the experiments expose the limitations of EKF pose tracking which were found to be low feature discriminance and errors in data association. Both issues are addressed in the following chapter on global localization. The maxim of sound error models is carried on by the explicit treatment of measurement origin uncertainty as an additional source of error. An interpretation tree approach with discrete non-Baye- sian feature-to-feature association renders localization global with the ability to localize a robot without a priori knowledge. A new feature type – multi-segment lines (MS-lines) – is introduced. MS-lines exploit the high degree of collinearity of indoor environments and overcome problems with low feature discriminance. The static consideration of robot localization as a matching problem is pushed further by a pose tracking algorithm which performs track splitting under geometric constraints when ambiguous matching situations occur during motion. With the assumption that environments have no symmetries locally (whereas globally they might have), a framework for global EKF localization and pose tracking is proposed. The framework decides over track formation, track maintenance, track scoring and track rejection as a function of the robot

iii

being lost, localized, not localized globally and not localized locally. Experiments show that it enables a robot to recover from kidnapping, collisions and extensive odometry errors at relatively low computational costs. The outcome of the previous chapters is employed at the Expo.02 exhibition – the biggest installation of autonomous, freely navigating robots so far. Ten mobile robots were interacting with 686’405 visitors during a five month period. Starting from a three-layered architecture as navigation framework, all components which make a robot navigate are integrated and adapted. The results at the pioneering event Expo.02 demonstrate that the deployment of autonomous, freely navigating robots in mass exhibitions is feasible. Finally, in chapter 7 the important restriction of maps that are a priori known is released. With the stochastic map approach we address the representation problem of more complex features with a physical extension which can lead to inconsistent and too big maps. Relative features for simultaneous localization and map building (SLAM) are introduced allowing a redefinition of MS-lines. Further, as a consequence of feature representation problems encountered earlier, this chapter makes use of the SPmodel which is a novel representation scheme for uncertain geometric entities. Experiments with MS-lines show clearly that with the concept of relative features more consistent and smaller maps can be achieved. It is argued that map consistency of this type is particularly needed when the map is employed for constraint-based localization. The overall distance which the robots travelled in this work with feature-based EKF localization in unmodified environments exceeds 3,400 km. This number has been accumulated with different robots, sensors, features and environments. 3,400 km is once across Europe.

Kurzzusammenfassung

Diese Doktorarbeit beschäftigt sich mit merkmals-basierte Mobilroboternavigation in bekannter und unbekannter Umgebung. Angefangen bei Aspekten der Systemintegration überspannt diese Arbeit einen breites Feld der Roboternavigation bis hin zur erfolgreichen Anwendung autonomer Roboter in mehreren applikationsnahen Einsätzen. Für die Sensoren des Roboters werden physikalisch gut abgestützte Fehlermodelle herge- leitet welche anschliessend bei der probabilistischen Extraktion von Linien, Segmenten, Retro-reflektoren und vertikalen Linien zur Anwendung kommen. Es wird ein Extraktions- algorithmus für Linien präsentiert, der eine Gleitfenstertechnik und ein Modeltreuekriteri- um zur Segmentierung benutzt. Um eine Uebersegmentierung zu vermeiden und im Interesse der Genauigkeit, werden signifikant kollineare Linien in einem abschliessenden Schritt unter Gebrauch eines Clustering-Algorithmus nach dem Prinzip nächstliegender Nachbarn verschmolzen. Resultate mit verschiedenen Laserscannern bezeugen eine präzi- se und konsistente segmentbasierte Szenenbeschreibung. Diese wird darauffolgend mit der semantischen Information der Merkmale erweitert, was zu einer Symbolkette aus gewich- teten Symbolen führt, welche sowohl die Topologie als auch die Hauptcharakteristik der Szene kompakt und auf hohem Abstraktionsniveau widerspiegelt. Als eine grundlegende Fähigkeit eines mobilen Roboters kommt dem Lokalisierungspro- blem besondere Beachtung zugute. Positionsverfolgen mit mehreren Sensoren und einem erweiterten Kalman filter (EKF) wird als Ausgangspunkt, von dem aus Präzision, Robust- heit und Grenzen des Verfahrens untersucht werden, genommen. Aspekte welche aus der in Echtzeit fortlaufenden Lokalisierung während dem Bewegen (on-the-fly navigation) re- sultieren werden explizit angegangen: die Notwendigkeit Rohdatenverformungen zu kom- pensieren und zeitliche Beziehungen zwischen Beobachtungen (über die Sensoren und über die Zeit), Schätzresultate und Jetztzeit zu verwalten. Die Experimente legen trotz sehr guter Resultate für die Präzision die Begrenzungen von EKF-basiertem Positionsverfolgen offen. Es sind dies Probleme mit niedriger Merkmalsdiskriminanz und Fehler in der Daten- assoziierung. Beiden Punkten widmet sich das folgende Kapitel über globale Lokalisierung. Die Maxi- me von physikalisch gut basierten Fehlermodellen wird durch die Berücksichtigung von Mess-Ursprungsunsicherheit als eine zusätzliche Fehlerquelle weitergeführt. Einen Inter- pretationsbaum-Ansatz mit nicht-Bayes’scher Merkmal-zu-Merkmal-Datenassoziierung ermöglicht das Lokalisieren des Roboters ohne jegliches Vorwissen womit der merkmalsbasierter Lokalisierungsansatz zu einer globalen Methode wird. Eine neuer Merkmalstyp – Multi-Segment Linien (MS-Linien) – wird eingeführt. MS-Linien weisen eine klar verbes-

serte Merkmalsdiskriminanz auf und nutzen den hohen Grad der Kollinearität, welcher ty- pischerweise in von Menschen erstellten Umgebungen anzutreffen ist. Die statische Betrachtungsweise des Lokalisierens als Korrespondenzproblem wird durch einen Algorithmus zur Positionsverfolgung überwunden, welcher Hypothesenaufsplittung unter geometrischen Zwangsbedingungen macht wenn während des Fahrens mehrdeutige Assoziationssituationen entstehen. Mit der Annahme, dass Umgebungen lokal keine Sym- metrien aufweisen (wobei sie global vorkommen können), wird ein Rahmen für globale Lokalisierung und Positionsverfolgung vorgeschlagen. Das System entscheidet über Hypo- thesen-Generierung, Hypothesen-Beibehaltung, Hypothesen-Bewertung und Hypothesen- Falsifikationen je nach dem ob der Roboter verloren, lokalisiert, global nicht lokalisiert oder lokal nicht lokalisiert ist. Experimente zeigen dass der Roboter damit in der Lage ist, mit relativ geringem Rechenaufwand sich von kidnapping, Kollisionen und erheblichen Odometriefehlern retten zu können. Die vorhergehenden Teile der Arbeit kommen in der Expo.02 Ausstellung zum Einsatz – der bisher grössten Installation von autonomen, frei navigierenden Robotern weltweit, in der zehn mobile Roboter während fünfmonatiger Dauer mit 686’405 Besuchern interagiert haben. Ausgehend von einer dreischichtigen Architektur als Navigationsrahmen werden alle Komponenten, die einen Mobilroboter zum Navigieren bringen, integriert und ange- passt. Die Ergebnisse der Expo.02-Ausstellung zeigen erstmals, dass autonome, frei navi- gierende Roboter in Massenausstellungen technisch machbar sind. Schliesslich kann im Teil über Simultanes Lokalisieren und Kartographieren (SLAM) die wichtige Restriktion von a priori bekannter Umgebung fallengelassen werden. Dem Reprä- sentationsproblem komplexerer Merkmale mit physikalischer Ausdehnung welches zu in- konsistenten und zu grossen Karten führen kann, wird Beachtung geschenkt. Es werden relative Merkmale im SLAM-Kontext, welche eine Neudefinition von MS-Linien erlau- ben, eingeführt. Ferner, aufgrund vorherig angetroffener Merkmals-Repräsentationspro- blemen, macht dieses Kapitel Gebrauch vom SPmodel, einem neuen Formalismus zur Repräsentation unsicherer geometrischer Entitäten. Experimente mit MS-Linien zeigen klar, dass mit dem Konzept relativer Merkmale konsistentere und kleinere Karten resultie- ren. Es wird erkannt, dass Kartenkonsistenz in diesem Sinne besonders wichtig wird wenn die Karte für Lokalisierung unter Zwangsbedingungen eingesetzt werden soll. Die Gesamtdistanz, welche die Roboter mit merkmalsbasierter EKF Lokalisierung in un- modifizierter Umgebung in dieser Arbeit zurückgelegt haben, übersteigt 3’400 km. Diese Zahl wurde mit verschiedenen Robotern, Sensoren, Merkmalen und Umgebungen akku- muliert. 3’400 km sind einmal quer durch Europa.

Table of Contents

1 Introduction ...... 1

2 Sensor Modeling ...... 5 2.1 Introduction ...... 5 2.2 Laser Range Finder ...... 6 2.2.1 Acuity AccuRange 4000 LIR ...... 6 2.2.1.1 Distance Calibration...... 7 2.2.1.2 Angle Calibration ...... 7 2.2.1.3 Range and Angle Variance...... 8 2.2.1.4 On-the-fly Scan Compensation ...... 10 2.2.1.5 Timestamp Calibration...... 11 2.2.1.6 Experiments ...... 11 2.2.2 Sick LMS 200 ...... 13 2.3 Odometry...... 13 2.4 Camera ...... 16 2.5 Summary ...... 17

3 Feature Extraction and Scene Description ...... 19 3.1 Introduction ...... 19 3.2 Line Segments ...... 20 3.2.1 Estimating a line and its covariance matrix using polar coordinates . .21 3.2.2 Segmentation ...... 23 3.2.3 Feature Discriminance and Fusion ...... 24 3.2.4 Experiments ...... 27 3.2.5 Discussion ...... 31 3.3 Retro-Reflecting Beacons ...... 32 3.4 Vertical Edges ...... 33 3.5 Scene Description ...... 34

vii

4 Local EKF Localization ...... 41 4.1 Introduction ...... 41 4.2 Sensor Modeling and Feature Extraction ...... 43 4.3 Global Map ...... 44 4.4 Multi-Sensor On-The-Fly Localization...... 45 4.4.1 The Localization Cycle ...... 45 4.4.1.1 State Prediction...... 45 4.4.1.2 Observation ...... 46 4.4.1.3 Measurement Prediction...... 46 4.4.1.4 Matching...... 46 4.4.1.5 Estimation...... 47 4.4.2 Matching ...... 47 4.4.3 Time Stamps ...... 48 4.4.4 Sensor Data Registration ...... 49 4.5 Experiments...... 49 4.5.1 Stop-And-Go Under Controlled Conditions ...... 50 4.5.1.1 Results ...... 52 4.5.1.2 Discussion ...... 53 4.5.2 On-The-Fly Under Controlled Conditions ...... 54 4.5.2.1 Results ...... 55 4.5.2.2 Discussion ...... 55 4.5.3 On-The-Fly Under Uncontrolled Conditions ...... 58 4.5.3.1 Results ...... 59 4.5.3.2 Discussion ...... 60 4.6 Conclusions ...... 61

5 Global EKF Localization ...... 63 5.1 Introduction ...... 63 5.1.1 Problem Statement ...... 64 5.2 Multi-Segment Lines...... 66 5.3 Geometric Constraints...... 69 viii

5.3.1 Location Independent Constraints ...... 70 5.3.1.1 Unary Constraints...... 70 5.3.1.2 Binary Constraints ...... 72 5.3.2 Location Dependent Constraints ...... 77 5.3.2.1 Rigidity Constraint ...... 77 5.3.2.2 Visibility Constraints ...... 77 5.3.2.3 Extension Constraints...... 79 5.4 Hypothesis Generation ...... 81 5.4.1 The Search Algorithm ...... 83 5.4.2 Estimating the Robot Location ...... 85 5.5 Hypothesis Tracking ...... 86 5.5.1 Duplicate Hypotheses ...... 88 5.5.2 Hypothesis Scoring ...... 91 5.6 A Framework for Global EKF Localization ...... 91 5.7 Experiments...... 93 5.7.1 Simulation Experiments ...... 93 5.7.1.1 Results ...... 96 5.7.2 Experiments with the Robot ...... 97 5.7.2.1 Results ...... 97 5.8 Related Work...... 100 5.9 Conclusions ...... 102

6 From Localization to Navigation...... 105 6.1 Introduction ...... 105 6.2 Problem Statement ...... 106 6.3 The Navigation Framework ...... 107 6.3.1 Environment Model...... 107 6.3.2 Global Path Planning ...... 108 6.3.3 Command Queue ...... 109 6.3.4 Local Path Planning and Obstacle Avoidance ...... 109 6.3.5 Multi-Robot Planning ...... 111

6.3.5.1 The Robot-Sees-Robot Problem...... 111 6.3.6 Localization ...... 112 6.4 The Robot ...... 114 6.5 Operation Experience and Discussion...... 115 6.6 Conclusions ...... 118

7 SLAM with Relative Features ...... 121 7.1 Introduction ...... 121 7.2 Feature Representation, Revisited...... 123 7.2.1 The SPmodel ...... 124 7.3 Features as Relative Entities ...... 125 7.3.1 Multi-Segment Lines ...... 125 7.3.2 Multi-Segment Arcs ...... 127 7.4 Intersection of two Lines in the SPmodel ...... 128 7.5 The Map Building Cycle ...... 134 7.5.1 Robot Displacement ...... 134 7.5.2 Matching ...... 135 7.5.3 Integration of New Observations ...... 137 7.5.4 Estimation ...... 139 7.5.5 Centering ...... 139 7.5.6 Finalizing Relative Entities ...... 140 7.6 Experiments...... 141 7.6.1 Small Map ...... 142 7.6.2 Large Map ...... 145 7.7 Conclusions ...... 146

8 Conclusions ...... 149

A Robot Platforms ...... 153 A.1 Pygmalion ...... 153 A.2 Robuter ...... 155 A.3 MoPS – Mobiles Postsystem...... 155 x

A.4 Donald Duck ...... 156 A.5 Robox ...... 157 A.6 XO/2 ...... 158

B Feature Definitions ...... 161 B.1 Line Feature...... 161 B.1.1 Definition ...... 161 B.1.2 Frame Transformation ...... 162 B.1.3 Visibility Condition ...... 164 B.2 Point and Corner Features ...... 165 B.3 Angle Features...... 168

C Line Fitting to Points in Polar Coordinates ...... 169 C.1 First Moments ...... 169 C.2 Second Moments ...... 171 C.3 Practical Considerations ...... 174

D Quick Guide to the SPmodel ...... 179 D.1 Quick Guide to the SPmodel...... 179 D.1.1 Representing Different Geometric Features ...... 179 D.1.2 Transformations and Jacobians...... 181 D.1.3 Representing Uncertainty ...... 181 D.1.4 Operations with Uncertain Locations: Compounding ...... 183 D.1.5 Operations with Uncertain Locations: Inversion ...... 183 D.1.6 Operations with Uncertain Locations: Centering ...... 184 D.2 Quick Guide to the SPmap ...... 184 D.2.1 Formulation of the SPmap ...... 184 D.2.2 Uncertain Displacement of the Mobile Robot ...... 186 D.2.3 Matching ...... 188 D.2.4 Estimation of the SPmap...... 189 D.2.5 Adding Non-Matched Observations to the Map...... 190 D.2.6 Centering the State Vector ...... 191

xii

Chapter 1

Introduction

“We are the robots” Kraftwerk, The Man Machine, 1978

There are several reasons why robots shall navigate. On the one hand, mobile robots are research platforms with the purpose to study the embodiment of machine intelligence and its interaction with the environment. On the other hand, the next level of automation in in- dustrialized societies will require autonomous mobility. Mobility implies perception which in turn implies noisy and ambiguous sensorial information. Probabilistic sensor models are start points for the representation, propagation and reduction of uncertainty in all subsequent stages of perception, estimation and decision taking. Relying on the assumption of Gaussian errors, we will pursue a probabilistic approach for the treatment of uncertainty. Most real-world environments provide salient features which are useful for navigation and can be extracted from sensory data. It turns out that features with a simple geometry are a judicious choice as they are relatively easy to obtain and of frequent occurrence in man-made environments. By means of sound sensor models conveying the first two moments of the measurement statistics, probabilistic feature extraction becomes significantly more robust and precise than with first moments only. Mobility requires the knowledge of one’s own position in the environment. Localizing a robot precisely and robustly at low computational costs is a main motivation for the choices made in this work. Localization is further a task where time plays an important role, particularly when continuously updating the robot’s pose during motion in real-time. The researcher must face several issues, possibly close to system integration aspects. Mobility requires further a map of the environment in which one is supposed to navigate. Given sensory information in form of geometric features, the robot can start to build a map

1. INTRODUCTION of a previously unknown environment autonomously. Hand-made maps are often costly and of questionable quality. This motivates our work in the field of simultaneous localization and map building (SLAM). Mobility implies finally the absence of closed-form models of the operation area and thus the futility of research in simulation. Real robots are mandatory in this field of noise and unpredictable random events. Where trajectories of a few meters length were sufficient in the early days of mobile robotics, today progress requires more extensive empirical validation. Laboratory experiments under carefully controlled conditions are important but are not always able to determine more than optimistic performance bounds of a method. When robots shall be able to leave the lab and enter the real world, large-scale experiments in a multitude of settings are needed. This motivates our emphasis on application-near experiments, both, in quality and quantity. Partly already mentioned, two important assumptions are made during this work. First, the so called 2d-assumption says that the robot finds all relevant information for navigation in the cross-section of the environment with the plane of the robot’s range sensors. This limitation will be released in chapter 4, where multiple sensors extend the perception to 3d, although the representation of the robot and the environment stays in 2d. Second, we adopt the linear Gaussian assumption. This assumption is considered as a working hypothesis in the sense that the hypothesis is presumed to be true but when evidence for its falsification arises, we must be open minded enough for its rejection. Howev- er, as reality is rarely linear and Gaussian, the question in the context of this work is rather whether the violation of the linear Gaussian assumption has an impact on the performance of the robot. We will keep this working hypothesis in mind. The report is structured as follows: In chapter 2 we will derive, where possible, error models with a sound physical basis for the sensors in use: laser range finder, odometry and monocular CCD camera. A method for line extraction from 1D range data is introduced in chapter 3 and validated with three different laser scanners. The approach uses a sliding window technique and a model fidelity criterion for segmentation. The non-linear regression expressions for readings in polar coordinates minimizing geometrically meaningful errors are derived. From a segment-based description of the scene, a symbolic string description is determined encoding the scene topology and its main characteristics in a compact way. With the features from chapter 3, chapter 4 investigates Kalman filter-based multisensor on-the-fly localization using horizontal and vertical lines and a nearest neighbor standard filter (NNSF) with a strongest observation policy. System integration aspects and issues deriving from on-the-fly navigation are fully addressed. The question is posed: How far can

we go with feature-based EKF localization and where exactly are the limits? Chapter 5 addresses the limits found in chapter 4. They are data association errors due to low feature discriminance and heavy violations of system (noise) models. Origin uncertainty is explicitly addressed with an interpretation tree approach for location hypothesis generation using geometric constraints. A track splitting filter under geometric constraints is proposed forming thus a framework for global EKF localization with non-Bayesian data association. The problem of low feature discriminance is addressed by the introduction of multi-segment lines (MS-lines) which combine advantages from infinite lines and segments. Chapter 6 puts the elements together and embeds them into a three-layered navigation framework for multiple robots. The application of the framework is the Expo.02 exhibition where ten autonomous, freely navigating robot were interacting with 686’405 people during a five-month period. Operating experience from this event is discussed. The limitation of known environments is released in chapter 7. The difficulty to represent complex features with a physical extension is addressed by the introduction of relative entities which keep their information for estimation and data association separate. Their benefit is demonstrated with multi-segment lines which are redefined using the SPmodel. The SPmodel is a feature representation scheme which addresses problems previously found with the representation of uncertain geometric entities. Precision, map consistency and map size are experimentally compared to the segment-only case. The methods of this thesis are extensively validated by a number of application-near experiments. Conclusions from over 3,400 km autonomous navigation are drawn in chapter 8. Finally, appendix A describes the robot platforms which were used in the experiments of this work. The robot Pygmalion which has been built for this purpose is considered in more detail. Appendix B defines the features which are used in all chapters except chapter 7, appendix C derives the fit equations for the non-linear regression problem in chapter 3 and appendix D gives a quick introduction to the SPmodel which was used in chapter 7.

1. INTRODUCTION

Chapter 2

Sensor Modeling

2.1 Introduction

The fundamental implication of mobility is the need for perception. Mobile robots operate in working spaces which contain elements that change over time and whose dynamics is unknown. Opposed to robot manipulators, typically employed in static working cells, mobile robots require sensors in order to internally represent their environment and appropriately plan their actions. Sensor data are uncertain as any physical measurement process is noisy1. Type and extent of noise sources are specific for each sensing principle. For noise models with a strong physical basis, it is necessary to trace each source of uncertainty in the measurement process and, with knowledge of the exact measurement principle, propagate it through the sensor electronics up to the raw data the operator will see. Clearly, good noise models are of great importance for subsequent stages like feature extraction, data association and estimation. In practice, however, measurement principle and sensor circuitry is not always known unless the sensor is built by one’s own as in the work of (Adams 1999). Off-the-shelf de- vices often behave as black-boxes in this respect. The sensors which are considered in this chapter are laser range finder, CCD camera and odometry. Range finders and cameras are exteroceptive sensors as they receive external stimuli. Ultrasonic sensors, wide-spread during the early days of mobile robotics, have important drawbacks in comparison: low angular resolution, low measurement speed, correspondence ambiguity on the reflection source and a high dependency on the target surface properties. With today laser scanners, precise, fast and robust range data in two dimensions became affordable and the integration into a robot platform is close to plug-and-play. Be- sides exteroceptive sensors which allow perception of the robot’s surrounding, proprioceptive sensors measure robot-internal motion. They include wheel encoders for odometry and IMUs (inertial measurement units) for inertial navigation. They provide recursively an estimate of the vehicle pose based on a previous pose (dead reckoning). In this chapter it is attempted to derive rigorous noise models for all sensors involved.

1. Expressed to the point by the German phrase “wer misst misst Mist” (who measure measures muck)

2. SENSOR MODELING

Where possible, we will strive for sound error models in the above sense, trace noise sources and propagate their uncertainty up to the data level. This will be done also with a side- glance to practicability and, where necessary, empirical models will also be used.

2.2 Laser Range Finder

We consider two different sensors, the Acuity AccuRange4000LIR and the SICK LMS 200. While the latter employs a linear time-of-flight principle, the former is based on a more complex method. Both sensors have been integrated on the robot Pygmalion (appendix A) which serves as the experimental platform.

2.2.1 Acuity AccuRange 4000 LIR

The laser range finder which was used in the experiments is the AccuRange 4000 LIR from Acuity Research (Acuity Research 1995). The measurement principle, although resembling time-of-flight is frequency-to-range. The reflected signal is amplified, inverted, and used directly to modulate the laser diode. The light from the laser is collimated and emitted from the center of the front face of the sensor. This configuration forms an oscillator, with the laser switching itself on and off using its own signal. Time of flight plus the time needed to amplify the signal determines the frequency of oscillation which is proportional to the range to the target. The measurement is somewhat nonlinear and dependent on signal strength, temperature, and ambient light level (Clark 1994). The noise sources are: • Thermal noise from the signal detection photodiode • Long-term drift and fluctuations in sensor circuitry • Resolution limitation imposed by the frequency measurement clock • Amplitude of the returned signal, that is target reflectivity • Sensor temperature • Ambient light level • Noise and resolution limits of the ADC electronics for data acquisition Besides the range signal, the sensor provides also amplitude, temperature and ambient light level as analogue signals. The amount of noise of the first three sources depend much on the sampling frequency (Acuity Research 1995). In this work where the sensor operates at 1kHz, drift over time and fluctuations are predominant over thermal noise, resolution arte-

2.2 LASER RANGE FINDER

4 slope =3.2753583 intercept =-9.6859863 2

-2

-4

-6

-8

-10 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 Figure 2.1. Distance calibration of the AccuRange 4000 LIR. Distance (x-axis) versus AD value (volts). White target, constant ambient light level and temperature. facts and noise from the AD converter.

2.2.1.1 Distance Calibration The analogue range signal of the sensor is read in by the ADC periphery of the robot and sampled by a RT process at 1 kHz. A sensor internal look-up table compensates for amplitude, temperature, and ambient light level. It remains to validate the linearity of the sensor and to identify the relationship. Figure 2.1 shows the target distance versus the AD value together with the line which was fitted to the data. Under the conditions of the experiment we can observe a very linear transfer function.

2.2.1.2 Angle Calibration The mirror axis has to be synchronized as there is no absolute angle measurement device such as a zero-pulse or gray code encoder. We thus have to calibrate the angle each time at system start up. This is done by means of the four vertical profiles which are in the field of view of the sensor (figure 2.2). When the system is initialized, the current (arbitrary) mirror orientation is the zero angle. We need to determine a correction angle θc which maps all measured orientations into the robot reference frame. With the four given relative angles between the profiles, ∆θ12,,,∆θ23 ∆θ34 ∆θ14 , the calibration sequence is as follows: • The mirror makes a revolution at very low speed in favour of a high angular resolution. Readings in the two range intervals known from the (symmetric) robot geometry are filtered out. •A convolution along the θ -axis of the points and four small θ -windows centred around the relative angles is performed maximizing the total number of points within the windows. The points within each window are averaged and yield the angles ˆ {}θi i = 1..4 .

• With the given relative angles, we look for one of the absolute profile angles, say θ1 , by minimizing the squared errors between the averaged and known angles

2. SENSOR MODELING

Figure 2.2. The four vertical proﬁles which serve as beacons for angle calibration

ˆ 2 Σ()θi – θ1 – ∆θ1i . (2.1)

By deriving expression (2.1) with respect to θ1 , and with ∆θ11 = 0 , the correction angle θc is

θc = θ1 + ∆θ14 ⁄ 2 (2.2) with ˆ θ1 = ()Σθi – Σ∆θ1i ⁄ 4 (2.3) There is uncertainty in the corrected orientations, not from angular noise but from segmentation uncertainty as the number of points within the four windows vary. A constant variance σ 2 with σ = 0.8° from a number of calibration runs has been identified. This θs θs value does not represent an uncertainty to be associated to individual range readings since it affects all measurements equally. The points would thereby become fully correlated in angle which renders error propagation complex. Instead, the variance σ 2 represents an un- θs certain robot-to-sensor frame transform which is to be accounted for when using the sensor data.

2.2.1.3 Range and Angle Variance In spite of the internal calibration accounting for amplitude, temperature and ambient light level, amplitude remains the predominant factor as indicated range can vary as much as 3 inches (7.6 cm) between very weak and very strong signals (Acuity Research 1995). Fur- ther, the amplitude information allows to detect unreliable measurements, for instance, from beam-splitting and problematic surfaces. In (Adams 1999) a phase shift measurement principle has been examined yielding a non- 2 2 linear range variance to amplitude relationship of the form σρ = aV⁄ r + b , where σρ is range variance and Vr the measured amplitude. After identification, an inverse, slightly nonlinear function was found. For identification in our case, an experiment was performed with a white stationary target at about 1 meter distance. The returned signal strength s was varied systematically with a Kodak gray scale patch1 where 10,000 readings were taken at each of the twenty gray lev-

8 2.2 LASER RANGE FINDER els. Both, range and amplitude were read in over an ADC in the robot controller. We can observe an abrupt rise of noise below a certain amplitude smin (figure 2.3). This reduces our model for range variance σ 2 = () to a constant value σ 2 , independent on tar- ρ fs ρconst get distance and amplitude. Although this analysis lead to a simple result it permits rejection of uncertain readings if the condition ss< min holds. Finally, an error model for the angle of the measurements is sought. It is possible to read the angle of the mirror very accurately with a motor encoder. Some angular uncertainty is caused by the finite beam-width of any laser which produces an “optical footprint”. Given the encoder resolution of 4000 pulses per revolution in our setup and the relatively tightly collimated beam of the Acuity sensor (2.5 mm spot diameter and 0.5 mrad divergence) we neglect angular uncertainty. The same argumentation can be found in (Adams 1999). Drift and fluctuations, the most relevant factor for range variability at our sampling frequency, cause also cross-correlations of the data. Without correlations, each range reading of a wall, for instance, contains new, independent information on its position. With drift, the measurements statistically depend on each other over space and time. Neglecting these correlations will lead to optimistic, non-conservative variance estimates. Indeed, the constant value for range standard deviation identified in figure 2.3 is small (a 4σ interval below 2 mm) – correct for short periods of time but too small over the long term. To account correctly for correlations, one would have to identify their amount and to take them into account in all subsequent error propagation steps. However, with correlations, especially over time, expressions become quickly very complex, be this in model fitting, pose tracking or SLAM (the white noise assumption is violated). A pragmatic approach which was chosen here is to pretend uncorrelated data, perform error propagation and additively increase errors at the output with a value to be empirically determined. This results in error models of lower fidelity than theoretically possible but helps to avoids optimistic variance estimates and is a good trade-off between theory and the real-world.

3 x 10 3

2.5

1.5

0.5

0 9 8 7 6 5 4 3 2 1 0 Figure 2.3. Range standard deviation (y-axis, in meters) against measured amplitude (x-axis, in Volts). 10,000 readings, white, stationary target at 1 meter distance.

1. Color Separation Guide and Gray Scale Card, Q-13, CAT 152 7654, Kodak Scientiﬁc Imaging Systems

9 2. SENSOR MODELING

2 2

1.5 1.5

1 1

0.5 0.5

0 0

-0.5 -0.5

-1 -1

-1.5 -1.5 a) b) -2 -2 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 Figure 2.4. On-the-ﬂy compensation. a) Uncompensated and b) compensated scan of a robot turning on-the-spot. Accounting for the rotation of the vehicle yields the true angle which the beam traversed during a full scan. Here it was less than 2π which explains the gap in b).

In the case of the AccuRange 4000 LIR experiments have shown that the internal calibration is not able to fully compensate for temperature variations. We will therefore pursue this approach in later chapters.

2.2.1.4 On-the-fly Scan Compensation When the vehicle moves while scanning the movement imposes a distortion onto the raw data which is non-negligible for low mirror speeds. The scanning mirror of Pygmalion rotates with 2.778 Hz (17.453 rad/s) so as to have a one degree resolution at 1 kHz sampling frequency. Scan deformation in this setup is sensible as shown in figure 2.4. The problem is also know as data registration or alignment (Bar-Shalom & Li 1995). It is important to note that scans have to be compensated on the raw data level and not at later levels of perception (e.g. the feature level) since in this case, data analysis (the extraction method) would operate with an unmodeled, artificially modified raw data evidence. We compensate for the vehicle displacement during a scan by transforming each range ′ reading S P acquired in the sensor frame S′ into the non-stationary robot frame R′ and then into the world frame W . Followed by a re-transform into the stationary robot frame RS and finally into the desired reference frame of the scan . For a compensation on-the- fly, S must be the sensor frame at the start position of a new scan. By reading out odometry each time when a new range reading arrives, it gets immediately transformed by the expression

S R –1 W –1 W R′ S′ P = S T R T R′T S′T P (2.4) R R′ where ST = S′T . The 44× matrices T are homogeneous transforms casting the rota-

10 2.2 LASER RANGE FINDER

R tion and translation of the general transform into a single matrix. ST is the sensor-to-robot W frame transform and RT the world-to-robot transform given by the actual robot pose vector xS (Craig 1989). The compensated scan receives the timestamp of , that is, the time when the scan has started recording. The 1 kHz RT process which reads in the range value gets pose data from odometry which itself runs periodically at 5 kHz. Like this points are updated on-the-fly in groups of five.

2.2.1.5 Timestamp Calibration On-the-fly navigation requires precise timestamps. A fact that is rarely specified in most off-the-shelf sensors and related equipment such as scanning laser range finders or frame grabbers. With the AccuRange 4000 LIR a constant angular shift of 5° on all measurements was observed. Neither the AD conversion electronics (30µ s) nor the maximal scheduling jitter of the acquision task (around 400µ s) could have caused a temporal bias of this extent. As no document describes this phenomenon, it remains unknown. Identification lead to a correction factor of 5 ms.

2.2.1.6 Experiments Figure 2.5 shows a scan of the Acuity AccuRange 4000 LIR in a corridor. Associated uncertainties are depicted as gray lines, centered around their range value. We can observe several phenomena: • The relationship of amplitude and range variance seems to be somewhat more variable with natural surfaces than with the Kodak gray scale card. A nearly constant relationship with few very uncertain readings would be expected in view of the graph in figure 2.3. However, the variances seem to reflect the predominant factors correctly in general, that is, high uncertainty for low reflectance targets, low uncertainty otherwise. • Measurements lie in between two distinct objects which create a range discontinuity (A). This phenomenon is known as “beam splitting” (Adams 1999) or “mixed pixels” (Hebert & Krotkov 1991) where the spot is partly on both targets. These readings are typically accompanied by a low signal strength. • Readings in “infinity” (B). When the beam hits no target, the Acuity sensor returns an arbitrary range value typically with a low amplitude. • The four vertical profiles of Pygmalion are visible at (C). The high variance is due to a low reflectance of the material (black anodized aluminium) and beam splitting. • The linear objects on the bottom side of the corridor at (D) are metal cupboards var- nished in light green. They cause a relatively high range variance. The surface turned out to be highly problematic for the sensor. Specular reflection causes the decrease

11 2. SENSOR MODELING

3 A

1 y [m]

0 B C

1 B D

2 D

4 4 2 0 2 4 6 Figure 2.5. Scan of the AccuRange 4000x [m]LIR. Length and gray value of the associated lines show the range variance of the reading (4σ intervals magniﬁed by a factor of 5). See explanation in text.

of uncertainty at the position where the beam hits the surface perpendicularly. Application of the rejection threshold previously found discards only about 90% of the measurements at A, B and C. The other 10% have not been accompanied by a low amplitude and persist in the scan. As it can be seen also in the scans of figure 2.4, the sensor is in general subject to outliers. All described phenomena are reproducible and represent a “normal behavior of the sensor” (Clark 1998). On the one hand, on white targets or sufficiently rough surfaces, the sensor delivers excellent data. Resolution in range is also outstanding since analogue signals are used and the only limits are given by the acquisition ADC. On the other hand, high sensitivity to temperature, poor performance on everyday materials and colors, the high number of outliers and its behavior at range discontinuities and “infinity” readings leads to the conclusion, that with respect to ultrasonic sensors not much has been gained. The AccuRange 4000 LIR performs practically at a similar level than a good ultra sonic sensor array. The sensor was used in the localization experiments of chapter 4. It was then exchanged by a Sick LMS 200 (figure A.3).

12 2.3 ODOMETRY

2.2.2 Sick LMS 200

The Sick LMS series are based on a time-of-flight principle. They deliver calibrated range/ bearing data with different settings for maximal range and resolution. In all our experiments the sensor is configured for a 0.5° angular resolution and a 8 m maximal range which implies 1 mm radial resolution. Internally the mirror rotates with 50 Hz. The sensor makes a calibration of distance and angle obsolete. Further, with a 50 Hz scanning frequency, scan distortion from the moving vehicle becomes negligible. For angular uncertainties we apply the same argumentation as in the previous sections (no angular uncertainty), and for range standard deviation, the producer specifies a constant value of σρ = 1 cm. As the robot Pygmalion has two Sick sensors back-to-back covering 2π , there are two timestamps which are manually estimated and averaged.

2.3 Odometry

All robots which were used in the experiments of this work (appendix A) have a differential drive kinematics. The odometry model relies on a piecewise approximation using the displacements of the left and right wheel as the uncertain inputs

T u()k + 1 = []sr sl . (2.5) Let x = xyθ T be the robot position and orientation, P = Cov()x its associated 33× covariance matrix and b the wheel base of the robot (figure 2.6). Then the kinematic model is as follows

xˆ ()k + 1 k = f()xˆ ()kk, u()k + 1 (2.6)

∆θx ⋅ cos ()()k + ∆θ ⁄ 2 = xˆ ()kk + ∆θx ⋅ sin ()()k + ∆θ ⁄ 2 (2.7) ∆θ

()sr + sl ⁄ 2 ⋅ cos ()θ()k + ()sr – sl ⁄ 2b ˆ = x()kk + ()sr + sl ⁄ 2 ⋅ sin ()θ()k + ()sr – sl ⁄ 2b (2.8) ()sr – sl ⁄ b Non-systematic odometry errors occur in two spaces: • The wheel space which for a differential drive robot is two-dimensional and includes

13 2. SENSOR MODELING

∆θ ⁄ 2

+ 1 sl θ()k k sx≈ ∆ xk()+ 1 k , yk()+ 1 k

∆θ ⁄ 2

b θ()kk sr xkk(), ykk()

Figure 2.6. The kinematic model of a differential drive robot using a piecewise approximation sx≈ ∆ .

the left and right wheel (joint space in the nomenclature of manipulators). Effects of wheel slippage, uneven ground and limited encoder resolution appear in this space. • The Cartesian space spanned by the vector x encoding robot position and orientation. Effects of external forces (mainly collisions) occur in this space. Many models for non-systematic odometry models used so far (Chenavier & Crowley 1992), (Leonard & Durrant-Whyte 1992), (Lu & Milios 1995) ignore this separation. They assume the vehicle to move in steps of rotations and translation in the Cartesian space. As noise model, an uncorrelated uncertainty in rotation and translation is assumed. (Chong & Kleeman 1997b) point out that there is no physical basis for that assumption. Instead, they present a model for errors in the wheel space, starting from the uncertain input uk()+ 1 and the diagonal input covariance matrix

k s 0 Cov()u ==Uk()+ 1 R r (2.9) 0 kL sl which relies on the assumption of proportionally growing variances per sr, sl travelled. kR and kL are constants with unit meter. First-order error propagation linearizes f(). around the current values of the uncertain inputs, that is xˆ ()k + 1 k and u()k + 1 , yielding the Jacobians

∂ ∂ ∇f = f ; ∇f = f . (2.10) x ∂x u ∂u xˆ ()k + 1 k uk()+ 1 According to the definition if the Jacobian as the transposed outer product of the gradient operator and the transposed vector-valued function, we find

14 2.3 ODOMETRY

1 0 2 sin 2 fx –()sr + sl ⁄ ⋅ ()θ + ()sr – sl ⁄ b ==∂ ∂ ∂ (2.11) ∇fx f 0 1 ()s + s ⁄ 2 ⋅ cos()θ + ()s – s ⁄ 2b y ∂x ∂y ∂θ r l r l fθ 0 0 1 xˆ ()k + 1 k

fx ∇ ==∂ ∂ fu fy ∂sr ∂sl fθ

1 s – s s + s s – s 1 s – s s + s s – s --- cosθ + ------r l – ------r -l sinθ + ------r l --- cosθ + ------r l + ------r -l sinθ + ------r l 2 2b 4b 2b 2 2b 4b 2b 1 s – s s + s s – s 1 s – s s + s s – s --- sinθ + ------r l + ------r -l cosθ + ------r l --- sinθ + ------r l – ------r -l cosθ + ------r l (2.12) 2 2b 4b 2b 2 2b 4b 2b 1 1 --- –--- b b uk()+ 1 The error propagation law is then applied yielding the covariance estimate of the state prediction

T T Pk()+ 1 k = ∇fxPkk()∇fx + ∇fuUk()+ 1 ∇fu . (2.13) Non-systematic Cartesian errors can be added to equation (2.13) by a 33× covariance matrix Qk()+ 1 being a function of the relative displacement ∆θx, ∆ in the robot frame. Such a model has been used in (Chenavier & Crowley 1992), (Leonard & Durrant- Whyte 1992) and (Lu & Milios 1995). However, the authors in (Chong & Kleeman 1997b) emphasize the consistency of their method. With consistency they denote the fact that error propagation from time index kk to index + 2 is equal to the propagation result of k to k + 1 and then from k + 1 to k + 2 . They show that odometry models so far used in the literature yield different results for both scenarios which, of course, is incorrect.

The identification of kL, kR is not straight-forward. It will be difficult to obtain rigorous values for kL, kR (or the coefficients in Q in case of the Cartesian model). In order not to float completely in the air with the choice of kL and kR , an ad-hoc identification was performed. First, systematic errors have been identified and corrected with the method proposed in (Borenstein & Feng 1996). For differential drive robots the method allows to identify systematic errors in the wheel base and the radius difference of the left and right wheel. Then, the robot was joysticked along a cyclic trajectory bringing it back to the start pose. The coefficients were than adapted so as to have the start position within the 99% ellipse of the predicted end position (figure 2.7). The resulting values are –6 kL ==kR 5×10 m.

15 2. SENSOR MODELING

-2

-4

-6

-8

-10

-12 -4 -2 0 2 4 6 8 10 12 14 16

Figure 2.7. A cyclic test trajectory in order to roughly identify kL and kR where the robot comes back to its start position after 81 meters. The coefﬁcients have been chosen so that the start position is within the 99% ellipse of the predicted end position.

2.4 Camera1

In the feature-based approach we are interested in geometric information from the sensors. Accordingly, when using cameras we need to compensate the image distortion injected by the optical system. The robot Pygmalion carries an embarked vision system which consists in a Pulnix TM-9701 full-frame, EIA (640 x 480), grayscale camera and a Bt848 based frame grabber which delivers the images directly to the main CPU memory. The camera system is calibrated by combining the method from (Prescott & McLean 1997) with spatial knowledge from a test field. This provides a coherent set of extrinsic, intrinsic and distortion parameters (figure 2.8). In the multi-sensor experiments of chapter

Figure 2.8. CCD image compensated for radial distortion from an objective with 54° effective opening angle. The image shows the corridor in which several experiments in chapter 4 have been carried out.

1. This subsection describes work in collaboration with (Tomatis 2001)

16 2.5 SUMMARY

4 we will work with vertical lines extracted from grey level images and thus calibrate the camera only in horizontal direction. Uncertainties from the test field geometry and those caused by noise in the camera and acquisition electronics are propagated through the camera calibration procedure onto the level of camera parameters, yielding a 10× 10 parameter covariance matrix.

2.5 Summary

In this chapter, we have derived models of exteroceptive and proprioceptive sensors which will be used in this work. It was attempted – within the assumption of Gaussian errors – to put the models onto a sound physical basis and to identify noise sources at the places where they occur. For the Acuity laser range finder, radial variance depends predominantly on the amplitude of the returned signal. The relationship was identified and used to discard erroneous readings which are typically accompanied by low signal strength. Alignment of raw data into a common reference frame was found to be needed already for low vehicle speeds in view of the moderate scanning velocity of the mirror. With the setup in use, the angle has to be calibrated at each start up. Using least-squares estimation, the best correction angle which aligns the data into the sensor frame was found. However, angle calibration causes the robot-to-sensor frame transform to be uncertain. The relationship has been identified and must be taken into account when using the data. In contrast to the Acuity sensor, the SICK LMS 200 already delivers calibrated data in radius and angle. Scanning speed is high such that scan compensation can be omitted and the robot-to-sensor frame transform is deterministic. For odometry, non-systematic errors are modeled in the wheel space and propagated through the robot kinematics. This error model enjoys a good physical basis and solves a consistency problems with earlier approaches. Finally, for the CCD camera, uncertainties in the camera circuitry and the calibration test field are modeled and propagated to the level of calibration parameters. There, they will be taken into account in subsequent feature extraction steps covering also insufficiencies of the lens model and other systematic errors.

17 2. SENSOR MODELING

18 Chapter 3

Feature Extraction and Scene Description

3.1 Introduction

What is a feature? A feature is a physical object in the environment which is static, perceptible, locally unique and has an abstract description. Whether a feature is suitable or useless cannot be said in general but depends on the context, the perception system, the time constant of its dynamics and the description formalism at hand. Features carry a semantic and metric piece of information. Semantically they provide the feature type, metrically they transport an uncertain unary and binary geometric constraint. The unary constraint is the feature dimension, color, texture or any other instrinsic property which could have been measured. The binary constraint is its relative position to the robot. Features1 for navigation have been used early in mobile robotics (Chatila & Laumond 1985), (Crowley 1989), (Ayache & Faugeras 1990), (Leonard & Durrant-Whyte 1992), (Kleeman & Kuc 1995). The most widely employed models for features are line segments, corners, edges and cylinders. Besides the typical examples from indoor applications, a wide palette of features, environments and sensors have been used for navigation: rocks and other salient structures on rough terrain extracted from a 3D laser range finder and modeled by ellipsoids (Betgé-Brezetz, Chatila et al. 1995), pipes and valves found by image correlation in a automated inspection scenario (Davison & Kita 2000), plants extracted and classified from the acoustic density profile of ultrasonic sensors (Harper & McKerrow 2001) or artificial beacons in a multi-robot unmanned air vehicle application (Ridley, Nettleton et al. 2002). The strength of the feature-based approach lies in its abstraction from the data type, origin and amount. By this abstraction, it yields compact environment models and scales polynomially with the dimensionality of the environment. “Large-scale” of an environment is not defined in terms of square or cubic meters but as a function of the number of features, i.e. the feature density. Raw data have also been employed for robot navigation. Several successful localization techniques based on raw data have been developed: Markov localization (Fox, Burgard et

1. Features are sometimes called “landmarks”. We will use the terms interchangeably.

19 3. FEATURE EXTRACTION AND SCENE DESCRIPTION al. 1998b), scan matching (Lu & Milios 1994), and particle filter-based localization (Del- laert, Fox et al. 1999). The advantage of a straight-forward use of sensory data is its generality. The approaches work also if the environment does not contain salient features at all1. However, their advantage is also their main drawback: raw data come in large amounts which might pose efficiency problems, environment dynamics requires an explicit treatment (opposed to the feature extractor which typically filters out dynamic data as a side effect) and raw data do not carry semantical information.

3.2 Line Segments

In this chapter we will consider line segments as a basic primitive which later will serve to identify high-level features up to a pure semantical scene description. Feature extraction consists of two subproblems: (i) segmentation determines which data points contribute to the model to be found, and (ii) fitting gives an answer to how these points contribute. In (Ayache & Faugeras 1990) and (Taylor & Probert 1996) a Kalman filter was used where the model parameters constitute the state vectors in question. Assuming a Markov random process which holds all knowledge of the past in the present state, a Kalman filter permits a recursively implementation such that the best estimate is instantly available each time a new measurement arrives. This can be perceived as a real time problem on the level of single range readings. The notion “real time” is understood as the property of a system to process incoming information at the speed in which it arrives. However, for filter initialization which has to be done at start and each time a terminating segment edge was detected, a first estimate of the feature position has to be found without the Kalman filter. The Kalman filter has also been used for segmentation. In (Adams 1999) the sequence of data points are considered as a target tracking problem for the extraction of corner features. Abrupt changes of direction are considered as feature candidates. In (Castellanos & Tardos 1999) the same technique yields a partitioning of the points into homogeneous and spurious regions. The first are then used as input for a popular technique called the split-and-merge or iterative endpoint fit algorithm (Ballard & Brown 1982). It is a recursive algorithm which splits up into two segments each time when the maximal distance from the segment to a point is above a threshold. In (Crowley 1989) this technique has been used as well. Here we formulate the fit as a maximum likelihood estimation problem. The line is found by regression, nonlinear in the parameters, minimizing geometric meaningful errors. Seg- mentation is done by sliding a window over the scan and applying a model fidelity criterion. Further, similar to (Castellanos & Tardos 1996), we consider the extraction process as

1. One can argue that such environments do not exist with one exception: skiing through fog on deep powder snow

20 3.2 LINE SEGMENTS not yet finished with the segmented range image. Due to outliers, obstacles which partially occlude a landmark or discontinuities in the landmark structure or surface (e.g. a glass door in a wall) several segments are extracted belonging to the same object in the environment. In other words, the range image is oversegmented. Hence, it is required to associate these segments in order to involve as much information as available for a precise estimate of the ﬁnal model parameters before using them for further processing.

3.2.1 Estimating a line and its covariance matrix using polar coordinates

A standard regression problem is to find the best line to some data in Cartesian coordinates affected by errors in the y -coordinate. “Best” is with respect to unweighted squared algebraic errors. The solution, linear in the parameters, is well known and given by the pseudoinverse (Draper & Smith 1988). If geometry plays a role, care has to be taken which errors are minimized during regression. This becomes apparent when fitting circles or ellipses to Cartesian data. Fitting a circle to some points has an analytical solution but as it minimizes algebraic errors, the solution can be far from what was geometrically expected. For ellipses the problem is even more sensible (Gander, Golub et al. 1994). For lines the differences between algebraic and geometric errors are small but still matter. We therefore minimize perpendicular distances from the points to the line. This yields a nonlinear regression problem which is to be solved for polar coordinates, the coordinate system which is natural for two dimensional range data. If one follows the argumentation in chapter 2 and neglects angular uncertainties, a re-parametrization of the line model casts the fit problem back to the standard case in regression. This was done in (Taylor & Probert 1996). However, in order to stay general, range data have uncertainties in both coordinates, in range and bearing. With known uncertainties we can further determine a weight for each individual reading, and fit the line in the generalized least squares sense. The solution is closed form and as follows (appendix C):

2 1 2 ------∑ ∑ wi wj ρ ρj sin ()θi + θj + ------∑ ()wi – Σ wj wi ρi sin 2θi Σ wi i Σwi tan 2α = ------ij< ------(3.1) 2 1 2 ------∑ ∑ w w ρ ρ cos ()θ + θ + ------∑ ()w – Σ wj w ρ cos 2θ w i j i j i j w i i i i Σ i ij< Σ i

∑ w ρ cos ()θ – α r = ------i i i (3.2) ∑ wi with ()ρi,,θi wi being the measurement with index i , α and r the parameters of the line model

ρθαcos ()– – r = 0 , (3.3)

21 3. FEATURE EXTRACTION AND SCENE DESCRIPTION

and wi the weight if measurement i . For the implementation we suggest to use the identical Cartesian form of equations (3.1) and (3.2) which is of lower computational complexity:

–2∑ ()– ()– tan 2 = ------wi yw yi xw xi (3.4) α 2 2 ∑ wi[]()yw – yi – ()xw – xi

rx= w cos α + yw sin α (3.5) where = 1 ⁄ wi ρ cos θ and = 1 ⁄ wi ρ sin θ are the weighted xw Σ ∑ wi i i yw Σ ∑ wi i i means. Starting from Cartesian coordinates, a third approach by solving a constrained least squares problem with a QR-decomposition and a singular value decomposition is to be considered (Gander & Hrebícek 1993). Comparison of all three solutions showed that equation (3.4) and (3.5) is clearly the most efﬁcient implementation regarding the number of ﬂoating point operations. Nonetheless, if we want to determine the covariance matrix we need to know expressions (3.1) and (3.2). By writing the model parameters as random variables represented by their first-order Taylor series expansion about the mean point and assuming independence of P and Q , the first-order covariance estimate is

= ∇ ∇ T Cl fρθ Cρθ fρθ (3.6) T T = ∇fρCρ∇fρ + ∇fθCθ∇fθ where

∂α ∂α ∂P ∂Q ∇fρθ ==∇fρ ∇fθ (3.7) ∂r ∂r ∂P ∂Q is the p × 2n Jacobian matrix containing all partial derivatives of the model parameters with respect to PQ and about the mean point and Cρθ = diag()Cρ, Cθ the 2n × 2n 2 partitioned diagonal data covariance matrix with submatrices Cρ = diag()σρ and 2 i C = diag()σ where i = 1,,… n . θ θi According to the noise model for laser range finders whose scans are subsequently considered, the angular term in equation (3.6) has been omitted yielding a parameter covariance matrix Cl with elements (appendix B) 1 2 σ = ------∑ [ ()cos θ – sin θ – ρ cos 2θ αα 2 2 2 wi Nxw i yw i i i ()D + N 2 – Dx()]sin θ + y cos θ – ρ sin 2θ 2σ (3.8) w i w i i i ρi

22 3.2 LINE SEGMENTS

w 2 2 σ = ∑ ------i- cos ()θ – α + ∂α ()cos α – sin α σ (3.9) rr w i yw xw ρ Σ j ∂Pi i

∂α ∂ r 2 σαr = ∑ σρ (3.10) ∂Pi∂Pi i where ND and are numerator and denominator of the right hand side of equations (3.1) or (3.4) respectively. Note that all elements are of complexity On() and allow reuse of already computed expressions. The weights are chosen as the inverses of the variances giving most weight to data points with low variability which seems desirable.

2 w = 1 ⁄ σ (3.11) i ρi With a fit in the weighted least-squares sense and the sensor model from chapter 2 where amplitude determines range variance, a robust regression behavior will be the result. Read- ings with weak signal strength will be given less weight, readings with high amplitude will be considered more trustworthy for the fit. However, not all commercially available laser scanners provide this information and the relationship of range variance to distance must be experimentally identified. Weighting in regression is an own topic in general. A profound treatment can be found in (Carroll & Ruppert 1988).

3.2.2 Segmentation

So far it is clear how measurements contribute to the model to be found. Segmentation is the problem of determining which points contribute to the model, that is, which point is a model inlier and which point is a model outlier. Segmentation of the range image described hereafter can be seen from two different perspectives: a clustering viewpoint and a regression viewpoint. From a clustering viewpoint the method performs an evidence accumulation in the model space (Vestli 1995) similar to the Hough transform (Hough 1962). This leads to a clustering problem with n points (n denotes the number of data points) which can be solved efficiently also for large n only due to the particular character of the problem.

The model is fitted into a window of nf neighboring points and the covariance matrix is computed. The window is slided along the sequence of range readings obtaining the same number of points in the model space1. When adjacent groups of range readings lie on the

1. “points in model space” in this use of language are normal distributions, spatially diffused in the space of feature parameters.

23 3. FEATURE EXTRACTION AND SCENE DESCRIPTION same feature, their associated points constitute a cluster of nearby points in the model space corresponding to that landmark. Feature extraction is then the task of finding these clusters. In a general case a clustering problem of this size is impractical if no a priori knowledge is available. Here the systematics in acquisition makes that points on the same feature are con- secutive points. Due to this underlying regularity, a distance measure in the model space is defined which is applied to adjacent points.

T –1 di = ∑ ()xj – xw ()Cj + Cw ()xj – xw (3.12) j where ji= – 1,,… i + 1 and

–1 xw = Cw∑ Ci xi , (3.13)

–1 –1 Cw = ∑ Ci . (3.14) Equations (3.13) and (3.14) is the multivariate weighted mean also known as the information filter. Low distance indicates that the points involved have high model fidelity. If di is plotted against the measurement index, regions of low value can be observed at the corresponding regions of the sought clusters, that is, the sought features. A threshold dm is applied cutting off the regions of high model fidelity. A segment is now defined to be the set of measurement points those representations in the model space satisfy

di ≤ dm (3.15)

The minimal size of a segment is nf when equation (3.15) holds for a single point in the model space. This segmentation method allows also a pipeline implementation where each arriving measurement P triggers fitting of point P 1 2 and model fidelity check of point i in– ()f – ⁄ P 1 2 1 . The delay would therefore be ()n + 3 ⁄ 2 – 1 points where n is as- in– ()f – ⁄ – f f sumed to be an odd number. Each time when condition (3.15) changes from true to false a terminating segment edge is detected and a new fit has to be done with all points constituting the segment.

From a regression point of view, (3.15) is a model fidelity measure of the nf points in the window. A similar result can be obtained by a residual analysis, still general with respect to the feature type.

3.2.3 Feature Discriminance and Fusion

The range image is now partitioned into model inliers and model outliers, the former contributing to a segment, the latter discarded from further consideration. But as already stated

24 3.2 LINE SEGMENTS in section 1, the image is still oversegmented. Stopping here would ignore the information from collinear but distinct segments which lie on the same physical object (e.g. the same wall). On the other hand, segments from different features which differ only slightly in their model parameters should be declared as being distinct. This is the discriminance problem which accompanies the use of geometrical features. From a clustering viewpoint the problem size has now changed. Having gained a number of segments in the previous section (or any other scheme providing propagation of the first two moments), the number of points in the model space has been significantly reduced.

Starting from nn , where can be large, just the number of segments, ns , remain which, e.g., for straight lines in structured environments is typically around ten, rarely over twenty. Hence, from this viewpoint, segmentation can be perceived as a preprocessing step in order to reduce the input and lead to small clusters at the same locations where the original clusters have been observed before (ﬁgure 3.2). An agglomerative hierarchical clustering algorithm (Kaufman & Rousseeuw 1990) which permits an efficient implementation is utilized. A short outline is given:

With an ns × ns symmetric Mahalanobis distance matrix D , the procedure starts with each point as a separate cluster. • (1) Find the minimal distance of clusters and in and while ≤ dij Qi Qj Ddij dαε proceed to

• (2) Fuse Qi and Qj obtaining Qij . The number of clusters is decreased by one.

• (3) Update D by calculating the new distances from Qij to all other clusters. Only one column and row is changed. Go back to (1) In a typical clustering problem, the distance between two clusters is not straightforward to determine and there are many propositions for its solution. Often it remains a matter of experience to select the most appropriate method (Kaufman & Rousseeuw 1990). Here, however, distance and fusion of points in the model space is straight-forward since “points” describe Gaussian distributions. The distance between two points is calculated by the Ma- halanobis distance

2 T –1 dij = ()xi – xj ()Ci + Cj ()xi – xj (3.16) and fusion is performed by expressions (3.13) and (3.14). Thus, there is no need of dealing with clusters consisting in more than one point since two matching points are instantly fused. If error propagation were perfect, fusion with the information filter (3.13) and (3.14) would lead to the identical result than a fit with the combined set of measurement points. But as first-order error propagation is only approximative (see also the discussion in (Arras 1998)) there is a difference. Experiments showed that iterated fusion according to (3.13) and (3.14) lead to accumulated deviations from the (true) fit alternative, which for large

25 3. FEATURE EXTRACTION AND SCENE DESCRIPTION features where many small segments have been fused, became non-negligible. We therefore perform a fit with the combined set of points as fusion technique. 2 If x and x belong to the same feature, d has a χ2 distribution with two degrees of i j 2 ij freedom from which a threshold χα, 2 is chosen. If we leave the viewpoint of clustering, we recognize that the agglomerative hierarchical clustering algorithm is a popular data association method, namely the nearest neighbor standard filter. Under the Markov assumption an estimation result does not depend on the succession over time or space in which information is integrated. Ergo there is freedom when we fuse which segment. The integration policy applied here is threefold: overlap–adjacency–collinearity. Collinear segments (on the level α ) which are neighbors are merged first if they overlap. When no more overlapping segments are found, collinear segments (on the level α ) which are neighbors are merged and finally collinearity acts as the last sufficient condition. This implements a region growing-like technique which has found to work best. The algorithm has three parameters:

•,nf the window size: Here the minimal set of points is specified to which the model assumption applies. A linear object must carry at least nf points to be extracted

•,dm the threshold on model fidelity. Determines how loose or how strict the data must satisfy the spatial assumption of the model •,α the significance level for collinear segments which determines their discrimina- tion or fusion. 2 The complexity of the algorithm is On()⋅ nf + On()s . It is linear in the number of points nn multiplied by the window size f , and square in the number of segments before fusion. In typical applications nf ` nn and s ` n hold. Note that this algorithm does not depend on the feature type. In contrast to the split-and- merge technique, at no point, the algorithm has profited from a line-specific property. As long as a feature, be this a corner, a circle, an ellipse, or any other geometric model, can be found by regression similar to equations (3.1) and (3.2), this algorithm can be employed in a straightforward way. The model fidelity criterion (3.5) is feature-independent and the fusion of significantly nearby candidates as well. A further difference to the split-and-merge technique is that no decision is made on the basis of a single reading. This is expected to result in a segmentation which is somewhat sensible to outliers. In the algorithm of this chapter decisions are only made on the basis of the nf points in the sliding window.

26 3.2 LINE SEGMENTS

6 6

4 4

2 2 y [m] y [m] 0 0

2 2

4 4 a) b)

6 4 2 0 2 4 6 4 2 0 2 4 x [m] x [m] Figure 3.1. a) 360°-scan of the Acuity AccuRange 3000 LV. A scan with medium angular resolution and a relatively big radial noise. The standard deviations are indicated by 4σ -intervals. b) Range image after segmentation

3.2.4 Experiments

Scans of three different commercially available laser range finders with varying specifications are considered. The scan from the Acuity AccuRange 3000 LV1 has a medium angular resolution of 1˚ on 360˚ and is of relative low quality (figure 3.1). The Sick PLS 1002 has a radial quantization of 5 cm and an angular resolution of 0.5˚ on 180˚ (figure 3.4). The scan from the Leuze RS3 exhibits low noise and has a high angular resolution of 0.25˚ on 180˚ (figure 3.5). Each measurement is drawn with its standard deviation, indicated by 4σ - intervals. nf is chosen according to the ratio of angular resolution versus radial accuracy. The choice of nf = 7 for the PLS 100 and nf = 15 for the RS3 yields a similar product with their angular resolution (3.5˚ and 3.75˚ respectively) reflecting their similar accuracy. The AccuRange 3000 LV with more noise and a lower angular resolution leads to the choice of nf = 7 . The other parameters have been kept constant in all experiments. They are dm = 3 and α = 0.99 . A closer look on the extraction process is taken with the scan from the AccuRange 3000 LV (figure 3.1a). Figure 3.1b shows the result of segmentation. There are 31 extracted segments from this scan. Each segment is displayed in the parameter space with its 95% error ellipse (figure 3.2). They constitute groups of ellipses with small Mahalanobis distances.

Consider segment Sa in figures 3.3a,c. After fusion of collinear segments, it was found to lie on the same wall than segment Sb which itself has a great number of contributing points

1. This is the predecessor model of the Acuity AccuRange 4000 series. It has similar speciﬁcations and behavior. 2. This is an older model from SICK. Its main difference to the LMS 200 is the radial resolution (LMS: 1 cm).

27 3. FEATURE EXTRACTION AND SCENE DESCRIPTION

6 6 a) b)

5 5

4 4 4 10 9 5

3 3 r [m] r [m]

1 2 2 6

2 1 1 7

0 0 pi/2 0 pi/2 pi 3pi/2 pi/2 0 pi/2 pi 3pi/2 alpha [rad] alpha [rad] Figure 3.2. a) Segments seen in the model space. They are depicted by 95% error ellipses. Those which belong to the same environmental structure form clusters. They are to be merged, those originating from different landmarks are to be distinguished. b) Model space after the hierarchical clustering (nearest neighbor standard ﬁlter). Only the ﬁnal estimates remain.

6 0.4 b)

0.2

0 O c e H H H c e c O e H y [m] vh 0 -1.6 1.6

H H 1.7 HH Sb -1.8 1.8 Sa vl 1.9 -2 2 2 Sa 2.1 -2.2 2.2

2.3

-2.4 2.4 2.5 c) 4 -2.6 2.6 a) -1.4 1.4 1.2 -1 1 0.8 -0.6 0.6 0.4 0.2x [m]

6 4 2 0 2 4 x [m] Figure 3.3. a) Extracted segments. b) Symbolic description with weights, a seman-

tics-only representation of the scene enabling the calculation of vh and vl (see section 3.5). c) Detail from a) with two corners and their 99% error ellipse. and therefore low uncertainty. Since Sa and Sb share the same supporting line, the shape of the corner error ellipses in figure 3.3c reflect this by a low variability in the perpendicular direction to the common line. Although this algorithm is implemented on all five robots in appendix A, the first imple-

28 3.2 LINE SEGMENTS

3 y [m] 2

6 4 2 0 2 4 6 x [m] Figure 3.4. Scan from Sick PLS 100 which has a radial resolution of 5 cm. The resolution causes small regions of constant depth which can be identiﬁed where the beam hits a target perpendicularly (such on the upper horizontal wall at 90˚ or at about 45˚). Radial variance must take this into account.

0.50.5

5 0.25

0 0 symbolic description: HAHceccHcHcHO 4 HAHceccHcHcHO

3 y [m] 2 vh

1 vl H H HA H H 0

O 6 4 2 0 2 4 6 x [m] Figure 3.5. Extracted lines, corners (indicated by crosses and their 99% error ellipse) and symbolic description with their weights (see section 3.5). The scene is mainly characterized by three big lines constituting two concave corners and adjacent hidden corners. The weights reﬂect this property causing a similar interpretation of the scene also when the small segments are not reliably found each time.

29 3. FEATURE EXTRACTION AND SCENE DESCRIPTION

6 A

4 y [m] 3

6 4 2 0 2 4 6 8 Figure 3.6. Scan from the Leuze RS3 withx [m] an angular resolution of 0.25˚ and a radial resolution of 4 mm. Details are very well reproduced. However, without a physically well grounded sensor model using uncertainties only based on a distance relationship, other phenomena which inﬂuence range variance cannot be accounted for. The displaced points at position A originate from black velvet. Their true standard deviation is visibly greater than the “simulated”ones.

0.4 7

6 0 HceHHcHHHHHeccO 5

4 y [m] 3

2 vh 1 HHH HH H H vl H 0 O 6 4 2 0 2 4 6 8 x [m] Figure 3.7. Resulting segments, corners and weighted symbolic description. vh denotes the vector of highest confidence, vl the vector of least confidence. They con- cisely indicate where in the local environment a robot can expect to find secure terrain or where it can effectively gather new information on the environment.

30 3.2 LINE SEGMENTS mentation was on the Robosoft Robuter equipped with a Leuze RS3. The laser sensor was configured to have an angular resolution of 2˚ and a radial resolution of 4 mm. The Robuter carried a VME card with a Motorola 68020@20MHz CPU. The implementation was reduced such that only the first statistical moments are propagated. For all distances the Eu- clidian equivalents was taken, the thresholds were informally chosen by observation. Extraction accuracy and localization precision with a vehicle doing EKF localization was identified. The stationary robot was first put into a corner about two meters away from a metal cupboard and a large cardboard box, where 300 extractions were made. The hit rate was 100%, the standard deviation in r was found to be 5 mm, standard deviation in α was 0.2˚. Computation time was not greater than 240 ms. Then the algorithm was employed as the extraction method for an on-the-fly localization task with an extended Kalman filter (see chapter 4). The localization accuracy which was immediately obtained without further optimization of any vehicle or algorithm parameters was below one centimeter when the robot positioned itself at a desired location. As a further estimate for its precision, the noise of the position estimates has been identified when the robot was stationary. Standard deviation in xy and was found to be 6 mm, standard deviation in orientation θ was 0.05˚. The average localization cycle time during a journey through lab and corridors was 445 ms.

3.2.5 Discussion

It is interesting to compare the “non-probabilistic” version of this algorithm with the presented probabilistic variant. With the former method, a preprocessing step eliminating spurious points and thinning out raw data was indispensable. The robustness against outliers was still very low, clearly inferior to the probabilistic version. Further, the discriminance capacity of the Euclidian distance is considerably poorer than the Mahalanobis distance during fusion of collinear segments. Of course all this follows from probability theory and the plus of information the seconds moments represent. The overall performance is clearly superior – obvious for the researcher, instructive for the novice. In the scans presented here, we can state a very good description of all scenes by the extracted segments in general. Even with the noisy scan of the Acuity AccuRange 3000 LV, the extracted segments reproduce robustly the linear elements in the environments without being disturbed by the outliers. With the low radial resolution of the SICK PLS 100, the algorithm was not able to recognize that the two small segments at the lower right end of the longest wall belong also to that wall. Here, an adaptation of the significance level α which was kept constant in all experiments to the specificities of the sensor is justifiable. In the results for the Leuze RS3 we find four small segments forming a “Z” at the lower left hand where three would be correct – extraction was too loose. In view of the precision of the data, the adaptation of the algorithm to this sensor would tighten the model fidelity cri-

31 3. FEATURE EXTRACTION AND SCENE DESCRIPTION terion. We could afford a more conservative behavior permitting only readings with high model fidelity.

3.3 Retro-Reflecting Beacons

The feature-based paradigm permits a seamless combination of natural and artificial features. Artificial beacons are mostly considered as “cheating” in the research community because they fall into the class of environment modifications which is beyond the challenges research addresses today. Nevertheless, they have proven their appropriateness in countless industrial applications with a further growing market share of laser-based AGV navigation. Beacons render the perception problem trivial as the signal strength information from the laser sensor (if available) already solves the segmentation problem. Besides the Acuity Ac- cuRange series discussed in the previous chapter, the Sick LMS 200 provides this information too – although as a four-bit, nearly binary true/false or no beacon/beacon signal. Once this segmentation information is available, there are several measurement points on a retro- reflective beacon in general. We address hereafter their probabilistic extraction. We assume radial noise in a relationship to the distance of the target (the relationship can also be a constant) and, again, no angular uncertainty. The overall variability in radial direction is thus the weighted mean as found through the information filter equations (3.13) and (3.14), here in their scalar form

ρ ρ = σ 2 ∑ ------i- (3.17) b ρ σ 2 ρi –1 ()σ 2 –1 = ()∑ σ –2 (3.18) ρ ρi where ρ is a range reading with index i and σ 2 its associated variance. Note that equa- i ρi tion (3.18) is the parallel resistor formula. The angular beacon location uncertainty stems from segmentation noise of points at the extremities of the beacon. Even with a stationary sensor, they can be alternately classified as inliers or outliers in a sequence of scans. We therefore estimate the angular uncertainty 2 σθ according to the ad-hoc assumption that the centre of the beacon is within the angular interval spanned by the inlier readings and the two adjacent outlier points with a 95% prob- ablility. Let ∆θ be the angular resolution, nb the number of inlier points on the beacon, then the mean θb and the standard deviation σθ of the angle is given by 1 θb = ----- ∑ θb (3.19) nb i

32 3.4 VERTICAL EDGES

σ = --1-∆θ()+ 1 (3.20) θ 4 nb Further, assuming independence of radial and angular errors we find the beacon location in polar coordinates and its associated covariance matrix by

T xb = ρb θb (3.21)

–1 ()∑ σ –2 0 ρi C = –2 (3.22) b 0 --1-∆θ()+ 1 4 nb

With mixed (natural and artificial) features, a hybrid robot localization system was examined in (Arras & Vestli 1998) but will not be further considered here.

3.4 Vertical Edges1

Within the variety of visual features in indoor environments, vertical edges seduce by their frequency and the simplicity of extraction. They further allow for simplified camera calibration in only one direction (section 2.4 in chapter 2) which is interesting for embedded system implementations.

Figure 3.8. Extraction of vertical edges. a) uncalibrated half image, b) edge pixels after edge enhancement, non-maxima suppression and calibration, c) resulting vertical edges.

1. This subsection describes work from (Tomatis 2001)

33 3. FEATURE EXTRACTION AND SCENE DESCRIPTION

The extraction procedure which was used in this work has four steps: • Vertical edge enhancement: Specialized Sobel filter approximating the image gradient in the horizontal direction. • Non-maxima suppression with dynamic thresholding: The most relevant edge pixels (maximal gradient) are extracted and thinned by using a standard method. • Edge image calibration: The horizontal position of each edge pixel is corrected yielding a new position x with 2 4 6 8 xSx= []c + xc()k1r +++k2r k3r k4r (3.23) resulting from the camera model. Not the raw image is compensated but the extraction results. This of course is much more efficient. • Line fitting: Columns with a number of edge pixels above a threshold are labelled as vertical lines. Line fitting reduces to a one-dimensional problem. The resulting angle is ϕ = atan ()xC⁄ , where Cx is the focal length and the weighted mean of the position of the pixels in the extracted line. Uncertainty from the camera electronics is modelled on the level of the uncalibrated edge image. Together with the uncertainty of the calibration parameters (see section 2.4 in chapter 3) it is propagated through calibration and line fit, yielding the first two moments 2 ()ϕσ, ϕ of the vertical edges (figure 3.8).

3.5 Scene Description

Features carry semantical information. A feature-based representation obtained so far holds this information only implicitly. In this section the model of a scene will be semantically enhanced by making this kind of information explicit. We assume a segment-based description of the current scene without loss of generality for the multi-feature case. Different features can naturally be included since only angular landmark succession is important.

di Cii, + 1

Si si ri

αi

Figure 3.9. The deﬁnition of vectors si and di . Ei is the endpoint and Cii, + 1 the intersection point of the supporting lines from segment Si and Si + 1 .

34 3.5 SCENE DESCRIPTION

By classification of the intersection point type from adjacent segments, a weighted string of symbols is obtained which compactly reflects the topology of the scene and its main characteristics. The classes include typical high-level features in indoor environments such as convex and concave corners, three types of openings, and hidden corners. First, a classification framework which is free from heuristics and any particularity from the environment, the sensors or the robot is proposed. We consider adjacent pairs of line segments Si , Si + 1 , and classify their spatial configuration. The classification rule – the directionality of an endpoint – makes use of the scanning direction (counterclockwise or clockwise), known from the range sensor. We define λ as the directionality of an endpoint which is the scan direction of an end- Ei point Ei with respect to the intersection point Cii, + 1 :

T s d λ = ------i -i (3.24) Ei di

Vector si is the rotated normalized perpendicular to the supporting line of segment Si . Ro- tation angle is +π ⁄ 2 for a counterclockwise scanning laser sensor, –π ⁄ 2 for a clockwise scanning sensor. In other words, si is the direction in which the laser beam passed over endpoint Ei . di is the difference vector of Ei , the endpoint in question, and intersection point Cii, + 1 (ﬁgure 3.9):

di = Ei – Cii, + 1 . (3.25)

Given two lines in the Hessian model, ()αi + 1, ri + 1 and ()αi, ri , the intersection point Cii, + 1 is obtained by x sin α – sin α ci = ------1 - i + 1 i ri . (3.26) y sin ()α – α – cos α 1 cos α r 1 ci i + 1 i i + i i +

The directionality λ has two values, –1 and 1 , referring to the cases “scan direction of Ei is pointing towards Cii, + 1“ and “scan direction of Ei is pointing away from Cii, + 1“ (ﬁgure 3.11). It is then possible to formulate sufficient conditions for openings, hidden, concave, and convex corners:

hidden corners, H ⇔ λi = λi + 1 (3.27)

opening, O ⇔ ()λi = 1 ∧ ¬ H (3.28) concave corner, c ⇔ ()()0 <<∆πα ∨ ()∆πα < – ∧ ¬ O (3.29) convex corner, e ⇔ ¬ c (3.30) where ∆αα = i + 1 – αi . The interpretation of hidden corners is physically correct.

35 3. FEATURE EXTRACTION AND SCENE DESCRIPTION

Equal directionality of adjacent endpoints indicate that the laser beam did not pass over the intersection point of the two segments but jumped from one segment onto the other. Their spatial configuration hides those corners which connect the two endpoints. With openings, the beam travelled from the first segment into infinity and then to the second segment with their intersection point “in the back” (figure 3.10). Classiﬁcation according to (3.27) to (3.30) is elegant but suffers from a lack of realism. It assumes inﬁnite visibility of the sensor and a dimensionless point robot. Concave corners which lie beyond the robot’s sight, openings between two walls or within the same wall, originating from doors, are topological key points but not yet recognized. The scheme is therefore extended by two system parameters, RS , the perception radius of the sensor, and wr , a constant reflecting the size of the robot. Let G denote a gap which is an opening between two segments on the same supporting line and A an aperture which is an opening between two segments of different supporting lines. Let further r be the distance of Cii, + 1 intersection point C 1 from the origin of the sensor reference frame and d the ii, + EiEi + 1 distance of two adjacent endpoints. The extended classification scheme then becomes:

gap, Gl⇔ ()= l ∧ ()d > w (3.31) Si Si + 1 EiEi + 1 r

hidden corners, H ⇔ ()λi = λi + 1 ∧ ¬ G (3.32) opening, O ⇔ ()()λ = 1 ∨ ()r > R ∧ ¬ H (3.33) i Cii, + 1 S aperture, Ad⇔ ()> w ∧ ¬ O (3.34) EiEi + 1 r concave corner, c ⇔ ()()0 <<∆πα ∨ ()∆πα < – ∧ ¬ A (3.35) convex corner, e ⇔ ¬ c (3.36)

The condition l = l ensures that the segments under consideration have the same Si Si + 1 supporting line.

Finally by concatenation of the symbols from the set {}AceGHO,,, , , to a string we

a) “OHO” b) “HCO”c) “OEHCC” d) “OC”

Figure 3.10. Four situations together with their string description

36 3.5 SCENE DESCRIPTION

a) b) E2 S1 E1 S2 d2 S3 C12, d1 S4 A s1 s2

S5 c) d) E1

d1 E2 d3 d2 d5 E3 C23 , s3 C51, s1 s5 s2 E5

Figure 3.11. An example scene with two dead ends. The robot is in position A , scanning counterclockwise and extracts the displayed segments whose scan directions are indicated with arrows a). The directionality of an endpoint λ is 1 if s Ei i and di point into the same direction, and –1 otherwise. For segments S1 and S2 in b) we find the arrows pointing towards , thus λ ==λ –1 . For in C12, E1 E2 C23, c) we find λ = –1 and λ = 1 and for and in d), λ = 1 and E2 E3 S5 S1 E5 λ = –1 . According to condition (3.32), the spatial configuration of and is E1 S1 S2 classified as a “hidden corner” which is a correct interpretation as two concave corners are not visible from A . The resulting string for this scene is “HeccO”. obtain a compact string description of the scene. The string has variable length and is rotation variant: rotating the robot causes a cyclic shift. Although it is a scene model on a high abstraction level, the string is not free from noise. Small segments with a low hit rate in successive scans cause the length and structure of the string to change even when the vehicle is not moving. In Figure 3.5 the situation is mainly characterized by three walls. But the scene contains also small segments which only marginally reflect its characteristics. If the string should serve as a reliable tool, for example, for scene recognition, it has to encode the particularity of the local observation in a robust manner. Robustness is achieved by exploiting the uncertainty which is associated to the extracted features. By determining a weight w = fC(), C , where C and C are the i li li + 1 li li + 1 covariance matrices of the supporting lines li and li + 1 , a confidence measure for each symbol is gained. The general rule in order to determine wi is given by w = max()C , C (3.37) i li li + 1 where Cl returns the determinant of covariance matrix Cl , a measure of its probability mass. The weight is therefore given by the more uncertain line by which it is constituted.

37 3. FEATURE EXTRACTION AND SCENE DESCRIPTION

This rule applies for openings, apertures, hidden, concave and convex corners. For gaps which are not the result of an intersection as the two segments share the same supporting line, the weight is simply

w = C . (3.38) i li This is a conservative attitude for error propagation. Information from the two segments is not fused in favor of a reduction of uncertainty since a symbol is derived from two segments. Both give rise to the symbol and if only one of them disappears from the observation, the symbol disappears from the string. The confidence of a symbol is therefore given by the least individual confidence of its constituting segments. Finally, an interesting piece of information on the situation is the weighted vector sum and the inversely weighted vector sum. A normalized vector vi is determined for each symbol, indicating its direction in the scene. Then

1 vh = ------∑ wi ⋅ vi (3.39) Σwi 1 v = ------∑ v ⁄ (3.40) vl 1 vi wi Σ ⁄wi are referred to as the vector of highest confidence vh and the vector of least confidence vl .

These vector have applications in planning: vh heads to the regions of high feature visibility which enables the robot to approach secure terrain when it is desirable. In a map exploration scenario, the robot could choose the opening candidate which is closest to vl in order to efficiently explore new terrain if position uncertainties allow for it. See figures of section 3.2.4 for illustration. The string itself does not contain more information than we already have. But it reveals a different perspective on the same problem. In information theory, AI and genetics, a number of concepts have been developed for dealing with symbolic representations. The Lev- enshtein distance, for example, calculates a similarity measure of two strings of different length (Aoe 1994). Given an appropriate metric which keeps also track of the weighting, an explicit and concise similarity measure for matching and comparing local maps is available. Equivalent to the method of symbolic constraint-propagation which has been used for understanding line drawings of plane-faced objects (Waltz 1972), a real physical entity, the environment, is under consideration. This physical object manifests constraints and regu- larities which can be exploited when attempting to derive its understanding. Hence the strings, describing sections of the robot’s environment, adhere an underlying set of restric- tions which limits the amount of possible symbol sequences. E.g. “”OOO is impossible

38 3.5 SCENE DESCRIPTION since there is no configuration of three walls which result in that string. A number of con- flicting combination can be likewise determined in order to enable consistency testing of the actual environment model. As already stated, two types of spatial configurations are to be distinguished. Opening candidates stand for a lack of knowledge and corners are considered as perfect knowledge requiring no further investigation by the robot. According to this conception the alphabet of terminal symbols {}ce, and the auxiliary alphabet of nonterminal symbols {}AGHO,,, can be distinguished. This is a view of exploration as a syn- tactic approach which relies on the definition of a string grammar. Exploration of unknown line environments is then the task of applying a set of production rules from a recursively enumerable grammar in order to generate strings which consist only of terminal symbols {}ce, . When all nonterminal symbols have been substituted, i.e. all opening candidates have been explored and rewritten, the environment is perfectly known. See figure 3.11a where complete knowledge about this part of the environment is gained by maneuvering the robot towards the hidden corners which corresponds to the rule Hcc→ . This approach has lead to the idea of describing the environment by sequences of “finger- prints” (Lamon, Nourbakhsh et al. 2001).

39 3. FEATURE EXTRACTION AND SCENE DESCRIPTION

40 Chapter 4

Local EKF Localization

4.1 Introduction

Localization belongs to the basic skills of a mobile robot. Dead-reckoning techniques are impracticable since systematic and non-systematic measurement errors grow boundless over time. In order to stay localized, the robot has to externally reference itself using its exteroceptive sensors – a process commonly called localization. There are three important properties of a localization approach: robustness, accuracy, and practicability. In this chapter we will measure the feature-based approach to robot localization on these three scales. Opposed to former work in the field, the experiments try to explore the limits of the approach when applied under uncontrolled experimental conditions over long periods of time. This is done with a multi-sensor setup employing geometric features from two sensors with complementary properties. We consider localization by means of line segment extracted from range data of a 360° laser scanner and vertical lines extracted from images of an embarked monocular CCD camera. An extended Kalman filter (EKF) is used for fusion and position estimation. We further consider continuous localization during motion in real-time – henceforth referred to as on-the-fly localization. It confronts the researcher with issues which are present but hidden at low speeds or step-by-step motion. This includes timestamp quality for sensory inputs and the need for compensation of the raw data deformation cause by the vehicle movement. Timestamp resolution and variability impose bounds on localization precision and feature matching rates whose influence is to be studied when a localization method shall prove its relevance for real-world applications. We use the term local localization to indicate that in the Kalman filter matching step, the true pairing candidate for an observation can always be found in the validation gate around the measurement prediction. In other words, there is assumed to be no global data association problem. In chapter 5 we will extend this concept onto the global level. The Kalman filter for localization was introduced in the eighties (Crowley 1989), (Le- onard & Durrant-Whyte 1992) and subsequently used be many researchers (Castellanos, Tardos et al. 1996), (Arras & Vestli 1998), (Jensfelt & Christensen 1999), where line seg-

41 4. LOCAL EKF LOCALIZATION ments from range data is clearly the most widely employed feature. Vertical edges in combination with an EKF have been used in (Chenavier & Crowley 1992) and (Muñoz & Gonzales 1998). The combination of line segments and vertical edges was made in (Neira, Tardos et al. 1999) and (Pérez, Castellanos et al. 1999). Neira and colleagues (Neira, Tar- dos et al. 1999) use a laser sensor with an opening angle of 60° providing both, 3D-range and intensity images, and in the work of (Pérez, Castellanos et al. 1999), the absolute localization accuracy of laser, monocular and trinocular vision was examined and compared to ground truth. Similar precision has been found for the three cases. Other probabilistic localization techniques maintain a probability distribution over the space of possible robot positions in the map. They represent the Markov localization paradigm and have been used with purely topological maps and landmarks (Nourbakhsh, Pow- ers et al. 1995), topological-metric maps (Simmons & Koenig 1995), and grid-based maps with raw range data (Fox, Burgard et al. 1998b). Since a distribution over all possible robot poses is always maintained, these approaches are inherently global and can relocalize the robot after a lost-situation or at start-up. For the same reason, however, they need incom- parable more computational resources in terms of memory and calculation power if a localization accuracy is required similar to the one in this work (see e.g. (Gutmann, Burgard et al. 1998) for an experimental comparison). Raw range data are employed also in (Cox 1991) and matched against an a priori map composed of line segments by minimizing a least-square criterion. Similar methods are used for scan matching where the best alignment of the local and the global map is sought, both maps being a set of sensed points (Lu & Milios 1994), (Gutmann & Schlegel 1996). The robot pose is also represented as a single Gaussian which makes scan matching a local method, and a simplified Kalman filter is used to calculate the posterior state. Compared to feature-based approaches, scan matching techniques usually operate with memory-intensive maps since the environment model consists in raw range data recorded from a set of reference positions. We perform three sets of multi-sensor localization experiments.1 • Stop-and-go navigation under controlled conditions where the contribution of the visual information is qualitatively and quantitatively analyzed • On-the-fly navigation under controlled conditions (same purpose) • On-the-fly navigation under uncontrolled conditions during a four-day telerobotic event over more than 5 km travel distance In a first two sets of experiments, we examine under controlled conditions the improvement with respect to the precision when the vision information is added to the laser range

1. The results in this chapter are the outcome of a collaboration with Nicola Tomatis who was mainly in charge of the vision part. Refer to his thesis (Tomatis 2001) for more details.

42 4.2 SENSOR MODELING AND FEATURE EXTRACTION

22222

1.51.51.51.51.5 11111 11111

22222 0.50.50.50.50.5

00000 33333

0.5 0.50.50.50.5 44444 1 1 1111111

1.5 1.51.51.51.5 55555 66666

2 2 2222222

2.5 2.52.52.52.5

3 3 3333333

3.5 3.53.53.53.5 2 2 2222222 1 1 1111111 00000 11111 22222 33333 44444 55555 Figure 4.1. A scan of the Acuity AccuRange 4000 LIR and the extraction result. Eight segments on six lines have been found. Two closely situated objects produce evidence for two small outlier segments. Thus, the local map contains six ()α, r - pairs which are passed to the EKF matching step. finder. Alike (Neira, Tardos et al. 1999) the uncertainty bounds are considered which, under the assumption of realistic uncertainty models, permit inference about the sought first moments of the robot position to a certain degree. For this, throughout of this work, it was attempted to employ physically well grounded uncertainty models for odometry, the laser range finder and the vision system. The third experiment is the Computer 2000 event, an annual computer tradeshow in Lau- sanne, where during four days visitors could teleoperate the robot by means of high-level navigation commands from a web interface. With a system up-time of 28 hours, an overall travel distance of 5 km and more than 140,000 localization cycles, long-term reliability under uncontrolled application-like conditions was the main concern. What we further want to find out with these experiments is the question to which extent feature-based localization formulated as a Kalman filter pose tracking problem is an appropriate choice under application-like conditions and whether it is ready for integration into a navigation framework as a black-box. And if no, where exactly are the limits.

4.2 Sensor Modeling and Feature Extraction

In the experiments we will use infinite lines as features, opposed to range-only information in (Leonard & Durrant-Whyte 1992), and line segments of finite length in (Crowley 1989), (Castellanos, Tardos et al. 1996), (Neira, Tardos et al. 1999) and (Pérez, Castellanos et al. 1999). The line model is

43 4. LOCAL EKF LOCALIZATION

Figure 4.2. A 12× 6 m section of the environment model. Crosses indicate vertical edges. The complete map consists of 191 inﬁnite lines and 172 vertical edges and describes twelve ofﬁces, two corridors, the seminar and the mail room.

ρθαcos ()– – r = 0 (4.1) where ()ρθ, is the raw measurement and ()α, r the model parameters. The algorithm for line extraction has been described in chapter 2. The method delivers first and second moments at each time index k of the localization cycle (figure 4.1):

2 α σα σαr zl()k = ; Rl()k = 2 (4.2) r σαr σr

The measurements are accompanied by a timestamp Tl . Vertical edges are modeled by vertical infinite lines. The extraction method described in chapter 2 delivers both moments

2 zv()k = ϕ ; Rv()k = σϕ (4.3) plus the timestamp Tv . We assume independent errors among the features of the same type and among the features of both sensors.

4.3 Global Map

The a priori map of the environment, G , contains infinite lines and point features, the latter being the global representation of vertical edges (see appendix B).

= {}n ; = (), (4.4) Ggi i = 1 gi mi Mi

44 4.4 MULTI-SENSOR ON-THE-FLY LOCALIZATION

The map was hand-measured and consists in 191 infinite lines and 172 vertical edges for the 50× 30 m portion of the institute building shown in figure 4.8. The features receive a constant, uncorrelated uncertainty in form of the diagonal covariance matrix M which reflects the error when manually tape-measuring an indoor environment. It also contains a graph which encodes the global environment topology. During the experiments the robot drives autonomously using this graph for path planning and its nodes as intermediate goals for position control or obstacle avoidance. The memory requirement of this map is about 2 30 bytes ⁄ m which makes it an environment model of extreme compactness. A particularity of the line model (4.1) is that its uncertainty is not frame transform invariant as the Jacobians for error propagation contain the robot position (see appendix B). This yields incorrect local uncertainties during the measurement prediction step. The problem is avoided by neglecting angular uncertainties of the lines in the map. This assumption is well satisfied for “perpendicular” office environments as shown in figure 4.8. Walls and furniture put in front of walls are parallel and perpendicular to each other, complying with the four axis directions in the global reference frame1.

4.4 Multi-Sensor On-The-Fly Localization

Under the assumption of independent errors, the estimation framework of a Kalman filter can be extended with information from additional sensors in a straight-forward way. Let

xˆ ()kk = xyθ T ; Pkk()= Cov()x (4.5) be the estimates of the first and second moments of the robot pose at time index k given all information up to k .

4.4.1 The Localization Cycle

A brief summary of the Kalman filter localization cycle is given and aspects particular for this work are considered in more detail below. The localization cycle consists in five steps (Leonard & Durrant-Whyte 1992):

4.4.1.1 State Prediction The state xˆ ()k + 1 k and its covariance P()k + 1 k is determined from odometry, based on the previous state moments xˆ ()kk ,P()kk and the uncertain input u()k + 1 .

xˆ ()k + 1 k = f()xˆ ()kk, u()k + 1 (4.6)

1. Exceptions come from postmodern deconstructivist architecture

45 4. LOCAL EKF LOCALIZATION

T T P()k + 1 k = ∇fxP()kk∇fx + ∇fuU()k + 1 ∇fu (4.7) This has been described in chapter 2.

4.4.1.2 Observation

The extracted feature parameters zl for lines and zv for vertical edges constitute the vector of observations z()k + 1 and its associated observation covariance matrix R()k + 1 . Since measurement errors of sensors and features are independent, R()k + 1 is blockwise diagonal. That means that all subsequent equations operate with 22× -matrices for lines and scalars for vertical edges.

4.4.1.3 Measurement Prediction The modeled features in the global map, {}n , get transformed into the frame of the gi i = 1 observations which in this work is the corresponding sensor reference frame (lines in the frame of the laser scanner, vertical edges in the frame of the camera). The first moments are computed by

ˆ ˆ z i()k + 1 = h()x()k + 1 k , mi (4.8) where h(). is the nonlinear measurement model (the global-to-local transform). For the second moments, error propagation is done by a first-order approximation: let SM()k + 1 be the covariance matrix of a map feature in the corresponding sensor frame {}S . Then, with an uncertain global feature gi = ()mi, Mi , and an uncertain robot-to-sensor trans- R T R form xS = xs ys θs with the associated 33× covariance matrix PS , we find

S T T R T Mi()k + 1 = ∇hx P()k + 1 k ∇hx ++∇hg Mi ∇hg ∇hs PS ∇hs (4.9) with the Jacobians

∂ ∂ ∂ ∇h = ------h- ∇h = ------h ∇h = ------h - (4.10) x g s R ∂x ˆ ∂m ∂ x x()k + 1 k mi S R xs For the measurement model of lines and vertical edges see appendix B.

4.4.1.4 Matching Once the measurement prediction step has transformed the model features from the map into the observation reference frame and once the feature extractors have furnished a non- zero number of features, we face a crucial part in the localization cycle: data association. It is the problem of finding out which observed feature belongs to which predicted feature. This step is explained below in more detail.

46 4.4 MULTI-SENSOR ON-THE-FLY LOCALIZATION

4.4.1.5 Estimation Successfully matched observations, stacked in z()k + 1 , and predictions, stacked in zˆ ()k + 1 , yield the vector of innovations

υ()k + 1 = z()k + 1 – zˆ ()k + 1 (4.11) and the innovation covariance

S()k + 1 = SM()k + 1 + R()k + 1 . (4.12) Finally, with the Kalman gain matrix

T –1 W()k + 1 = P()k + 1 k ∇hx S ()k + 1 (4.13) the posterior estimates of the robot pose and associated covariance are computed:

xˆ ()k + 1 k + 1 = xˆ ()k + 1 k + W()k + 1 υ()k + 1 (4.14)

T P()k + 1 k + 1 = P()k + 1 k – W()k + 1 S()k + 1 W ()k + 1 (4.15)

4.4.2 Matching

With the blockwise diagonal observation covariance matrix R()k + 1 we have the freedom to integrate information into the estimation process in a succession of our choice. Hereafter an integration strategy is described which is advantageous for filter convergence with the features under consideration. The laser observations are integrated first since they typically exhibit far better mutual discriminance making their matching less error-prone. With vertical edges from the camera, ambiguous matching situations occur often. They are integrated subsequently. Consis- tent with this idea, pairings are also iteratively integrated according to their quality (the iterated Kalman filter). One iteration is as follows: (i) matching of the current best pairing (ii) estimation (iii) re-prediction of features not associated so far This approach has also been used in (Neira, Tardos et al. 1999) where the same observations concerning feature discriminance are reported. The quality of a pairing pij formed by ˆ prediction z i and observation zj is different for each sensors (time index k omitted): • For lines we employ a strongest observation policy. The quality criterion of a pairing is smallest observational uncertainty – not smallest Mahalanobis distance like in the

47 4. LOCAL EKF LOCALIZATION

nearest neighbor standard filter (Neira, Tardos et al. 1999), (Pérez, Castellanos et al. 1999). This renders the matching robust against small and possibly spurious segments which yield small Mahalanobis distances (see also figure 4.1). The “current ˆ best pairing” pij = ()z li, , zlj, which satisﬁes the validation test – ˆ –1 – ˆ T 2 (4.16) ()zlj, z li, Sij ()zlj, z li, ≤ χα, n

is therefore that of observation zlj, with an observation covariance matrix Rlj, minimizing its determinant, a measure of its probability mass. Sij is the innovation covariance matrix of pairing and 2 a value taken from a 2 -distribution with pij χα, n χ n = 2 degrees of freedom. α is the signiﬁcance level on which the hypothesis of pairing correctness is rejected. ˆ • The criterion for vertical edges is uniqueness. Predictions z vi, with a single observation zvj, in their validation gate are picked first and integrated according to their smallest Mahalanobis distance provided that they satisfy condition (4.16) for n = 1 , subscript lv becomes . When there is no unique pairing anymore, also candidates with multiple observations in the validation region are accepted and chosen according to the smallest Mahalanobis distance.

4.4.3 Time Stamps

The main difference from the viewpoint of multisensor localization between step-by-step and on-the-fly navigation is that temporal relations of observations, predictions and estimations of all sensors involved have to be maintained and related to the present. The problem is described as follows:

When sensor AT performs its data acquisition, the data receive a time stamp A . Feature extraction delivers the observations ready to be paired. The matching step requires temporal consistency of observations and predictions. Measurement prediction must therefore also carry timestamp TA , which implies knowledge of the robot position at TA . When at the current time tT estimation is completed, the corrected robot position is still valid for A which now is in the past and lies back the distance which the robot travelled between TA and t . Based on the odometry model, a means is then needed to relate this old position estimate to the current position of time t . In order to know the robot position at any time, odometry data is timestamped and recorded in a cyclic buffer with resolution ∆t . With the given timestamps Tl (or Tv ) of the exteroceptive sensors, the buffer returns the temporally closest state prediction. The maximal timestamp error Terr between an observation and a measurement prediction is therefore ∆t ⁄ 2 . Finally, when the position estimate arrives from the Kalman filter, valid at Tl , the

48 4.5 EXPERIMENTS final position is found by forward simulation of the odometry model (4.6) and (4.7) from

Tl to t given the data in the buffer which also includes the inputs sl, sr . For a multisensor system, care has to be taken that prediction and estimation results of one sensor are not overwritten by those of another sensor. This would be the case if each sensor had its own EKF running independently from the others with individual cycle times yielding temporally nested position updates. Nested updates are to be avoided since a slow outer update cycle (e.g. vision) overwrites the estimation results of a faster running inner cycles (e.g. laser). A sequential scheme of EKFs is therefore chosen. In this scheme the integration sequence must account for the succession of the respective observation. The resulting cycle time is then given by the slowest cycle.

Note that Terr can be a source of data association mistakes since temporal inconsistency will metrically shift all observations in the reference frame in which matching is performed. Even if correct pairings are found, unmodeled bias will be injected into the estimation process. In the current implementation, odometry is a periodic real-time task at 5 ms (i.e. Terr = 2.5 ms). Forward simulation of the odometry model using the buffer data is an example of an attempt to access a shared resource (shared with the odometry task) under real-time constraints. The XO/2 operation system which runs on our robots offer ways to deal with this.

4.4.4 Sensor Data Registration

Registration of sensor data is another implication of on-the-fly localization. In any (finite- speed) serial data acquisition process such as with scanning sensors – and opposed to CCD cameras – each data is acquired at a different time and vehicle pose. Processing groups of such data (e.g. scans) requires them to be put into the same reference frame – a process commonly referred to as registration (Bar-Shalom & Li 1995). With the scanning speed of the Acuity laser range finder, the deformation of scans is non- negligible already at moderate robot velocities. In chapter 2 the problem has been addressed. The solution provides full scans whose points have been transformed into the reference frame at the robot pose of the first reading in the scan and its timestamp Tl .

4.5 Experiments

Three sets of experiments have been conducted. Note that for the last two sets the experimental platform Pygmalion underwent a change in hardware (embedded computer and la-

49 4. LOCAL EKF LOCALIZATION

1.5

25130 1 135

652

0.5 140 145 99.1 P()k + 1 k 80 0 99 y [m]

98.9 0.5 103 190 98.8

1 40 185 180 175 170 98.7

1.5 98.6 98.5 + 1 + 1 2 a) b) P()k k

1 0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 78.3 78.2 78.1 78 77.9 77.8 77.7 77.6 77.5 x [m] Figure 4.3. a) Predictions and observations in the robot frame before estimation at step 14 of the test trajectory. All line segments and three vertical edges have been matched (to predictions 145, 175, 180). b) Update resulting from the matches in a). The Kalman ﬁlter corrects the robot about 5 cm backwards and turns it slightly in the clockwise sense, establishing good correspondence of observations and predictions of the three matched vertical edges in a). The matches allow for a full update; the posterior state covariance matrix P()k + 1 k + 1 stays bounded. ser sensor).

4.5.1 Stop-And-Go Under Controlled Conditions

Long corridors notoriously recur in mobile robotics. They are, however, an excellent benchmark for a localization system, since they occur often in application scenarios and contain the difficulty to stay localized in the direction of travel. In order to demonstrate the contribution of the visual information to the localization process, we chose the corridor environment shown in 4.4 and 4.5 as benchmark. All runs have been performed under controlled conditions: In the evening with limited environment dynamics and a well-defined illumination. Almost all corridors, offices and rooms in the test environment are subject to direct daylight which otherwise would influence the performance of the vision system. The robot moved autonomously in a stop-and-go mode. In order to assure trajectories as reproducible as possible, obstacle avoidance has been turned off and replaced by a position controller for non-holonomic vehicles (Astolfi 1995). In the first set, two experiments have been conducted on a distance of more than 1 km. In both experiments the vehicle followed the trajectory depicted in figures 4.4 and 4.5 with a length of about 78 m. From the figures it can be seen that the corridor would actually be

50 4.5 EXPERIMENTS

70 75 80 85 90 95 100 10 Figure 4.4. Run with laser range finderx [m] only. All lines which would allow for an update in the corridor directions have been removed from the map. At each of the 47 trajectory steps, the robot is depicted with an ellipse reflecting the 99.9% probability region of the posterior state covariance matrix P()k ++1 k 1 for xy and . Grid in x -axis is 5 meters. The result is typical. False matches due to great position errors and extensive uncertainty at the right-hand corridor end yield filter inconsistency and divergence. The robot is lost.

70 75 80 85 90 95 100 105 Figure 4.5. Run with laser and vision. Verticalx [m] edges have been added to the map in figure 4.4. The pose uncertainty stays bounded during navigation in the corridor. Only moderate corrections at the left and right-hand corridor end are necessary; the robot accomplishes its task and returns safely to the start point. Filled objects in light gray (doors and cupboards on the left side) are barely visible for the laser range finder. They have a shiny, green surface and turned out to be almost perfect specular reflectors for the Acuity sensor (see also figure 2.5). well-conditioned in the sense that many structures exist which allow for updates in the direction of travel. In order to inhibit matches in perpendicular direction, all these structures have been removed from the map in this set of experiments. In figures 4.4 and 4.5 modeled structures are in black, unmodeled ones in gray. It is attempted to qualitatively study the need of additional environment information, if, due to a difficult working area, information of one type is not sufficient for accomplishing the navigation task.

51 4. LOCAL EKF LOCALIZATION

4.5.1.1 Results Figure 4.4 shows one of the runs which were performed with the laser range finder only. As to be expected during navigation in the corridor, uncertainty grew boundless in the direction of travel. Inaccurate and uncertain position estimates lead reproducibly to false matches and incorrect pose updates in front of the right-hand lab. In the second run of the first experiment (figure 4.5, one of three runs shown), the vision system was able to ameliorate precision and bound uncertainty to an uncritical extent. This is illustrated in figure 4.3, where for one of the corridor situations (step 14), predictions and extraction results are displayed in the robot frame before estimation. The three matched vertical edges allow for a full update. The trajectory could always be completed to the start point. Localization accuracy at the endpoint has been hand-measured and was typically in a sub-centimeter range, showing a slight temperature dependency of the laser sensor (see chapter 2)1. In the second experiment, the contribution of monocular vision to the reduction of estimation uncertainty has been examined. The same test path was travelled five times with the laser range finder only and five times with both sensors. The resulting averaged 2σ -error bounds are shown in figure 4.6. The error bounds say that, based on the uncertainty model, the robot is with a 95% probability within twice this value. In figure 4.7 the numbers of predictions, observations and matchings of the 47 step path are presented, averaged over the five runs. Table 4.1 quantifies the overall averages of error bounds

2σfrontal,,2σlateral 2σangular , number of matched lines nl , number of matches vertical edges nv , and execution times texe . The resulting error bounds in figure 4.6 and their means in table 4.1 show that, compared to the laser-only case, the multisensor system particularly contributes to the reduction of uncertainty in the vehicle orientation (–58 %). This although the number of matched vertical edges is moderate (below two per step on average). However, this reduction in orientation uncertainty is also comprehensible as the robot-to-sensor frame transform for the laser sensor is uncertain in angle and deterministic for the CCD camera. We further “recognize” the corridor in these experiments by the fact that uncertainty in frontal direction is much higher than in lateral direction. Remember when comparing the numbers for frontal uncertainty that in the laser-only runs the trajectory could not have been finished. The robot went lost before. The average execution times express overall durations with a CPU under full load of sensor acquisition, feature extraction, position control, communication and task scheduling. Processing of the visual information was also done on the main CPU, no dedicated hardware was available.

1. In fact, when the robot positioned itself at the start point, there was a winter position and a summer position which differed reproducibly around two or three centimeters. Internal temperature calibration of the Acuity sensor was not able to compensate this.

52 4.5 EXPERIMENTS

frontal [cm] 5 a) 0 5 10 15 20 25 30 35 40 45

1 lateral [cm] b) 0 5 10 15 20 25 30 35 40 45

2.5 2 1.5 1 angular [deg] 0.5 c) 0 5 10 15 20 25 30 35 40 45 Figure 4.6. Averaged 2σ –error bounds of frontal a), lateral b) and angular c) a posterior uncertainty during the 47 steps of the test trajectory. Five runs have been made for each mode. Solid lines: laser range ﬁnder only, dashed lines: laser and vision.

4.5.1.2 Discussion The results from the qualitative experiment are no surprise as they follow straightforwardly from the 18th century (Bayes 1763). The experiment is an illustration of the benefit of multi-sensor fusion when two sensors inject complementary environment information into the estimation process. Matching vertical edges has turned out to be error-prone due to their lack of depth information and their frequent appearance in groups (figure 3.8). With large validation regions, false matches have been produced sometimes despite the iterative integration strategy described in section 4.4.2. However, their effect remain weak as these groups are typically very compact. Figure 4.3 also illustrates this with predictions 140 and 145 which are close to each other. Although only one of them denotes the same physical object which has been actually observed, pairing with both prediction will not sensibly affect the posterior pose estimate. Here we see already the limits of single feature data association without exploiting binary constraints which particularly in the case of vertical edges would improve matching robustness. This has been done for vertical edges in (Delahoche, Mouaddib et al. 1996) and (Muñoz & Gonzales 1998). The fact that the robot went lost in the laser-only experiments is also due to a very low update rate per distance travelled (see at the right-hand end of the corridor). This is typical for stop-and-go navigation when a reasonable average vehicle speed is yet to be achieved. Such update rates make successful data association difficult for any feature.

53 4. LOCAL EKF LOCALIZATION

Figure 4.7. Averaged number of predictions (dash-dot), observations (dashed) and matches (solid with circles) for line segments a) and vertical edges b). See also table 4.1.

laser laser and vision

2σfrontal 5.19 cm 4.26 cm

2σlateral 1.34 cm 1.25 cm

2σangular 1.63° 0.69°

nl ⁄ nv 3.53 / – 3.51 / 1.91

texe 377 ms 1596 ms

Table 4.1. Overall mean values and error bounds for the stop-and-go experiment

under controlled conditions. The number of matched lines nl , matched vertical edges nv , and the average execution time per iteration texe are given.

However, it can be stated a reasonable localization accuracy in frontal and high accuracy in lateral direction. Clearly, the uncertain robot-to-sensor transform of the laser sensor bounds accuracy in the vehicle orientation.

4.5.2 On-The-Fly Under Controlled Conditions

In the second set of experiments, the robot was driving under the same controlled conditions but now in continuous motion. This changes two things: on the one hand timestamp uncertainty and resolution artefacts are added, on the other hand the update rate per time or distance travelled will be much higher. Figure 4.8 shows the floor plan of the 50× 30 m test environment which has been enlarged by several offices and an additional hallway. In the meantime, Pygmalion, the experimental platform, received a faster embedded computer

54 4.5 EXPERIMENTS

(from a PowerPC 604 at 100 MHz to 300 MHz, more memory) and the laser scanner was exchanged with a Sick LMS 200 (see also chapter 2 and appendix A). The results for localization accuracy in frontal and lateral direction are not expected to differ as the precision of the Acuity and the Sick sensor is similar. In orientation we can expect an improvement as the robot-to-sensor frame transform is now deterministic. Execution times, of course, will decrease. Again, in the laser-only mode and in the multisensor mode the trajectory has been driven five times. In all experiments the map was complete, that is, the removed features in the first set of experiments were included in the map. Average vehicle speed was 0.3 m/s, maximal speed 0.6 m/s. Care has been taken that both experiments (laser-only, laser and vision) have the same localization cycle time by limiting the implementation to 2 Hz resulting in about 950 localization cycles on each of the 140 m test trajectories.

4.5.2.1 Results The resulting 2σ -uncertainty bounds of the a posteriori position estimates are illustrated in figure 4.9. For both cases they generally reflect a very high localization accuracy in all three state variables. Subcentimeter precision is approached. Table 4.2 shows the overall means of 2σ -error bounds, number of matched lines nl , number of matches vertical edges nv , and execution times texe . The lateral error values correspond well with the previous set of experiments. And opposed to the first test trajectory, almost fully in the horizontal hallway, the robot was now, with the extended environment and a complete map, able to obtain equal position uncertainty in frontal and lateral directions. Again, the vision information contributes to a reduction of uncertainty, equally in xy and (-20 %), but particularly in the orientation (-40 %). As expected, execution times texe are smaller, and the decrease in angular uncertainty is less strong due to the known robot-to-sensor displacement of the Sick scanner.

4.5.2.2 Discussion What do the error bound say beyond the relative improvements? Clearly, even carefully derived uncertainties do not necessarily permit inference about the sought first moments, since the estimation error could be arbitrarily big without being noticed. However, we argue that by the simple fact that the robot always succeeded in returning to its start point, there is compelling evidence for the correctness of these bounds. Indeed, they are conservative since the true bounds can be even better. If they were wrong the robot would have gone lost due to first moments drifting away from the truth causing incorrect validation gates and a lack of proper matches. Ground truth information like in (Pérez, Castellanos et al. 1999) is of course preferable but impractical and expensive to obtain for this kind and extent of experimentation. Positioning accuracy of the vehicle at the endpoint has been measured by hand and confirm the obtained values in tables 4.1 and 4.2. These results are

55 4. LOCAL EKF LOCALIZATION

115

seminar room

110

coffee room

105

elevator

start 100 office 1

95 office 2 Figure 4.8. Floor plan of the environment where the experiments have been carried out. One of the test trajectories is shown with

90 points at places where the robot localized itself. Crosses indicate office 3 the modelled vertical edges. The robot starts in the laboratory, goes to the elevator, then passes through the corridor to offices 1, 85 2 and 3, continues to the seminar room and returns to the laboratory via the coffee room.75 The trajectory80 length85 is 14090 m and has95 been driven100 10 105times with110 about 950115 localization cycles per run. The average speed was 0.3 m/s, maximal speed 0.6 m/s, resulting in about 7’45” for the whole path. In order to compare the multisensor setup and the laser-only setup with respect to localization accuracy, five runs have been made with laser-only, five with laser and vision. The resulting uncertainty of the posterior position estimates are illustrated in figure 4.9, overall averages are given in table 4.1. also very similar to those obtained in the line extraction experiments from chapter 2 and to the accuracy reported in (Pérez, Castellanos et al. 1999). In (Gutmann, Burgard et al. 1998) experiments resulted in a maximal precision of about 5 cm for Markov localization, whereas scan matching produced, in the best case, estimates also in the millimeter range.

Interestingly, the average number of matched edges (nv = 2.00 ) is very similar to the stop-and-go experiment (nv = 1.91 ). It seems that the effects of timestamp uncertainty and improved update rate per travel distance cancel out. However, In spite of the better update rate, the problem of falsely paired vertical edges persists. Door frames with opened doors, for example, commonly have multiple vertical borders which, dependent on the illumination conditions, produce evidence for several closely located vertical edges. In the matching stage, they might be confronted with large validation gates, position bias from odometry and timestamp uncertainty making the true edge difficult to find. This lack of dis-

56 4.5 EXPERIMENTS

2σ [m] 0.02 x

0.015

0.01

0.005 a)

0 20 40 60 80 100 120 140 160 180 2σ [m] 0.02 y

0.015

0.01

0.005 b)

0 20 40 60 80 100 120 140 160 180

2σθ [rad] 0.02

0.015

0.01

0.005 c) 0 20 40 60 80 100 120 140 160 180

Figure 4.9. Averaged 2σ -error bounds of global a) xy, b) and c) θ a posterior uncertainty during the test trajectory (showing only each 5th step). In each mode, five runs have been made. Solid lines: laser range finder only, dashed lines: laser and vision. In some cases the uncertainty in the multisensor mode is greater than for the single-sensor setup. This is possible since the values are averaged over five runs containing noise on the level of matched features.

laser laser and vision

2σfrontal 1.31 cm 1.07 cm

2σlateral 1.35 cm 1.05 cm

2σangular 0.92° 0.56°

nl ⁄ nv 2.73 / – 2.66 / 2.00

texe 64 ms 411 ms

Table 4.2. Overall mean values and error bounds for the on-the-ﬂy experiment under controlled conditions. The number of matched line segments nl and matched vertical edges nv per cycle are also given. Note that texe denotes the average execution time not the localization cycle time which was limited to 500 ms for all experiments.

57 4. LOCAL EKF LOCALIZATION criminance was the cause of occasionally produced incorrect pairings also in this set of experiments. Especially the frame grabber in use does not provide exact timestamp information from the instant when the image was taken. A constant shift between image acquisition and arrival was assumed and manually identified. However, this shift turned not out to be constant and depends on system-specific factors such as traffic on the PCI-bus and alike. Odometry quantization (5 ms in our case), further bounding timestamp accuracy, became noticeable during fast turns (the camera of Pygmalion is not mounted on a turret which keeps a constant orientation).

4.5.3 On-The-Fly Under Uncontrolled Conditions

In the last experiment, the robot took part in a telepresence setup. The “Computer” tradeshow is an annual fair for computer hard- and software at the Palais de Beaulieu exposition centre in Lausanne, Switzerland. Our laboratory was present during the four days of May 2nd to May 5th 2000, giving visitors the opportunity to control Pygmalion by means of the web interface shown in figure 4.11. The robot itself was at EPFL, in the environment illustrated in figures 4.8 and 4.11. The Computer 2000 event was the final testbed for our localization system where we were mainly interested in long-term reliability under application-like conditions. The setup was active during normal office hours with an average of about 7 hours system up-time per day.

Figure 4.10. During the four days of the Computer 2000 event, visitors could teleoperate Pygmalion in the corridors and offices of our institute building at EPFL. The environment which was attainable to the robot is 50× 30 meter in size and contains twelve offices, two corridors, the seminar and the mail room. The robot was navigating seven hours each day during normal office hours with typical environment dynamics.

58 4.5 EXPERIMENTS

Figure 4.11. The Pygmalion web interface, a plugin-free Netscape application. It provides context-sensitive menus on the map and all subwindows with intuitive click- and-move-there commands for robot teleoperation. Four different real-time streams constitute the visual feedback on the current robot state: an external web-cam (top- right), an embarked camera (top middle), raw data from the laser range finder together with predicted, observed and matched features (top-left) and the robot animated in its model map (left middle). By clicking onto the map an office is defined as destination, clicking onto the camera image turns the robot and clicking on the laser scanner image defines a goal in the ()xy, -plane.

The environment exhibited typical dynamics from people, doors, chairs and other robots, as well as daylight illumination. Several doors open into the corridor (see figures 4.10 and 2.8). Travel speed has been limited to 0.4 m/s since the robot shares its environment with persons, some of them not implied into robotics. Obstacle avoidance (Arras, Persson et al. 2002) based on the laser range data was active during the event. The user can give global navigation commands (e.g. an office) or local navigation commands (a ()xy, -point or an orientation) with the web-interface. Each navigation command defines a global or local mission to the robot.

4.5.3.1 Results The event statistics of Computer 2000 is shown in table 4.3. We define a lost situation as

59 4. LOCAL EKF LOCALIZATION

Hours of operation 28 Environment size 50 x 30 m Environment type ofﬁce, dynamic Overall travel distance 5,013 m Average speed 0.2 m/sec Maximal speed 0.4 m/sec Number of missions 724 Number of localization cycles 145,433 Number of lost-situation 0 Number of collisions ~ 60

Table 4.3. Overall statistics of the Computer 2000 event. follows: a lost situation occurred if the goal of a mission could not have been attained due to a localization error and manual intervention to relocalize the robot was required. We do not count missions where the robot went lost due to a collision (e.g. with glass door or object lower than the beam height of the scanner) and missions where the robot was already lost (after such a collision). No lost situation occurred during the 754 missions on 5 km travel distance. When the vehicle were surrounded by people, was in less structured terrain, or when odometry delivered poor state predictions (e.g. from driving over uneven floors), it happened several times that there were no matches during a certain period. We counted 14 of 724 missions where the robot had no matches during 10 seconds, 21 missions where it had no matches during 5 seconds. In all such missions the robot succeeded in relocalizing itself. None of them required manual intervention during or after the mission. They were about 60 collisions. We counted 10 collisions with an unknown cause (robot was unsupervised over extended periods of time) and about 50 collisions with objects in the environment not visible at all to the laser sensor: chairs, coffee table, clothes trees, glass door, bags and luggage on the floor, open door in the blind zone between the two Sick laser scanners but also students and staff member who play with the robot. More details on the obstacle avoidance method in (Arras, Persson et al. 2002).

4.5.3.2 Discussion These are outstanding results which were further underlined by the feedback we got from the big number of visitors during Computer 2000. In particular, they enjoyed the easy-to- use interface which allowed anybody to control a mobile robot and discover our institute building over the internet.

60 4.6 CONCLUSIONS

Another result of the Computer 2000 event is that under these experimental conditions vertical lines performed unsatisfactorily. Vertical edges appeal by their frequency in man- made environments, their simple availability and the little requirements of computational power compared to many other visual features. Despite these advantages, the shortcomings observed in this experiment were: • Environment dynamics. When navigating with a robot in a populated office environment, its free space and hence its vision sensor are often blocked by obstacles (e.g. co-workers, doors; see figure 4.10). This results in two independent problems. First, a blocked sensor cannot contribute to the localization update. Second, when avoid- ing these obstacles the robot turns typically with high angular velocities which in combination with the timestamp uncertainties and the low feature discriminance was likely to produce false matches. • Illumination conditions. It is obvious that the illumination in an office environment cannot be controlled to suit best the need of a vision sensor. In particular both corridors have big windows at their end. The camera is heading to these windows from a relatively dark corridor when navigating in this direction. Low feature extraction rates were the result.

4.6 Conclusions

The experiments presented in this chapter on an overall length of 6.4 km and 150,000 localization cycles lead us to the following conclusions: In general it was shown that feature-based localization allows for very precise localization, high degrees of practicability and robustness against environment dynamics in office environments. The zero-lost situation result is also a confirmation that the initial statistical assumptions – independency, linearity, white-noise Gaussian errors – are sufficiently ful- filled in the presented scenario. Or to speak in other words: the method was confirmed to be sufficiently robust against their violation. However, as initially mentioned, the limits of the approach were also of interest and the experiments exposed them very clearly: each of the 60 collisions caused the robot to go lost, although the vehicle was typically close to its pose prediction after collision. The robot had be manually pushed back to a predefined place in order to localize it again and to continue operation. Here, the limits of the data association strategy – though slightly more advance as the NNSF – become visible and the need for more sophisticated techniques reveal directions of future work. For the multisensor aspects of this work, it can be concluded that the visual information is of great utility in situations where the laser sensor receives insufficient information for

61 4. LOCAL EKF LOCALIZATION localization. Also the improvement in accuracy was found to be important (40% in orientation). But with an application-close thinking, we must state that unless one cannot re- nounce the quality of visual information as such, the use of vertical edges becomes questionable when compared to the effort it takes to make it available on a robot. It is further emphasized that this type of experimentation is necessary. They reflect the “real case” and are indispensable when a localization approach shall prove its real-world performance. It is rated as not surprising that during this work an approach which in the beginning seems promising (vertical edges), and which has been successfully applied by several researchers before (Neira, Tardos et al. 1999), (Pérez, Castellanos et al. 1999), turns out to be partially incompatible with more demanding experimental conditions. Clearly, this kind of experimentation always remains in a certain dependency on the specific robot setting. In our case, a different vision/frame grabber system or a turret keeping a constant camera orientation could have performed better under the same conditions. De- spite the results being specific for this experimental setup, the work has hopefully shed light onto the questions which arise for multisensor systems and on-the-fly localization: choice of the feature, choice of the sensor/sensor equipment, the importance of high quality timestamp information, and finally, the choice of the matching strategy which comes back to the overall conclusions made above.

62 Chapter 5

Global EKF Localization

5.1 Introduction

To summarize the results from the previous chapter, Kalman filter-based pose tracking with geometric features was demonstrated to be a successful localization technique with several desirable properties: It operates with minimalistic environment representations, it is robust with respect to environment dynamics and combines unbounded localization accuracy with light-weight implementations. But it was also shown that position tracking, as a local localization technique, has the risk of loosing the track and going lost. Data association errors were found to be at the source of the problem which motivates the extension to global localization. Analogous to the term local, global refers to the search scope for data association. It covers now the whole global map and is not limited anymore to the validation gate of the measurement predictions. Other global localization methods are the POMDP and Markov approach (Simmons & Koenig 1995), (Nourbakhsh, Powers et al. 1995), (Fox, Burgard et al. 1998b) which maintain a probability distribution over a topology of nodes, previously overlaid onto the environment. Within this graph the robot can never go lost as long as a location probability is maintained for each node. In this manner, arbitrary densities can be represented in order to cope with the problem of location ambiguity. Recently, new approaches which overcome limitations of earlier methods have been proposed (Dellaert, Fox et al. 1999), (Jensfelt, Wijk et al. 2000). They employ the principle of particle filters where the density function of the robot location is approximated by a set of randomly drawn samples. However, all these techniques maintain constantly a big number of hypotheses which in the case of particle filters has to be carefully weighted, updated and re-distributed. The ability of these techniques to represent and properly react to location ambiguity is still due to the quantity of samples and a distribution strategy which must be appropriately chosen. Unlike these methods which can be denoted location-driven, our approach to global localization will be feature-driven. It reacts directly to the environment in the sense that features tell us when and where to place a location hypothesis – not an a priori topological graph or a dynamically maintained sample set. This allows to maintain exactly as many hypotheses as necessary and as few as possible. The technique which provides this property

63 5. GLOBAL EKF LOCALIZATION is a constraint-based search in an interpretation tree (Grimson & Lozano-Pérez 1987), (Drumheller 1987), (Castellanos, Tardos et al. 1996), (Lim & Leonard 2000). This tree is spanned by all possible local-to-global associations, given a local map of observed features and a global map of model features. We further address robust tracking by a constraint- based tracking splitting filter which employs the same technique for hypothesis tracking. Hypothesis generation and tracking constitute together a complete framework for global EKF localization which is tested during the experiments. Earlier work which deals with multiple hypotheses in a robot navigation context is (Cox & Leonard 1994) where map building with a known robot position under Bayesian data association is made. Using segments and corners from ultrasonic sensors, their hypotheses model a typological feature ambiguity since the features are difficult to distinguish from the sonar returns. With an additional typological feature ambiguity the number of hypothesis is likely to explode. We believe that with today sensors (laser and vision) feature extraction can be made very reliable and that the likelihood of mixing up features of different types is negligible. Rather spatial feature ambiguity is the issue to address. Other approaches like (Tomatis, Nourbakhsh et al. 2001) propose hybrid models to combine advantages from the EKF and POMDP-worlds. However, they require a clearly structured room–hallway topology. Otherwise the approach reduces to an EKF technique with the known limitations.

5.1.1 Problem Statement

After the experiments in chapter 4 and our earlier experience in EKF-based position tracking (Arras & Siegwart 1997), (Arras & Vestli 1998), we conclude that the cause of lost- situations is almost always an incorrect data association. We summarize the predominant reasons for false associations as follows: • Heavy violations of system and system noise models. Collisions and severe odometry drift in directions which were not correctable by the observations (figure 5.1) • Feature discriminance. Low feature discriminance is spatial sensing ambiguity on the level of extracted features and expresses itself as proximity in the feature’s parameter space (figure 5.2) As it has been seen during the Computer 2000 experiment in chapter 4, single hypothesis tracking can often relocalize a robot which went lost due to non-discriminant features, since they typically yield close-to-the-truth pose estimates. But in general, both problem sources, especially in simultaneous occurrence, can lead to false matchings and irrecoverable lost situations. Robust localization must therefore cope with the above phenomena and the data association ambiguity they cause.

64 5.1 INTRODUCTION

l2 g2 g3 g1 l1

local map global map

Figure 5.1. A situation where the robot goes lost and where this is very difﬁcult to detect: when the vehicle arrives at the end of a corridor with a critical amount of accumulated odometry drift (predicted pose in gray, true pose in black), the local point feature {}l2 is wrongly matched even if the uncertainty models are correct. Instead of the pairing {}l2, g2 , the wrong pairing {}l2, g3 is produced.

a) gi b) gi gj gj

c) d) gi gi gj gj

Figure 5.2. Examples of feature types which are typically subject to low feature discriminance: a) angle features modeling a door frame, c) point features modeling columns, b) and d) line features modeling walls. Less critical are features of higher parameter dimensionality as segments or circles or features of natural discriminance as doors.

We follow the line of research by (Grimson & Lozano-Pérez 1987), (Drumheller 1987), (Castellanos, Tardos et al. 1996), (Lim & Leonard 2000) to approach this problem and formulate the robot localization task as a search in the interpretation tree for feasible local-to- global pairings. Opposed to (Castellanos, Tardos et al. 1996) we add a visibility constraint to the category of location dependent constraints where it helps to reduce the space to be explored and accelerate the search. The visibility constraint in (Castellanos, Tardos et al. 1996) is applied after hypothesis generation and tests on mutual occlusion of features in order to detect infeasible hypotheses. Furthermore, multi-segment lines are introduced. This feature type combines advantages of infinite lines and segments. Their geometric constraints are derived.

65 5. GLOBAL EKF LOCALIZATION

Figure 5.3. Partial observation of a wall segment. The endpoints contain no information. The measured length however is a lower bound on the true segment length.

RS denotes a maximal perception radius of the sensor.

5.2 Multi-Segment Lines

In chapter 4 we have worked with infinite horizontal and vertical lines as features. Infinite lines have poor mutual discriminance as the segment information, initially available after line extraction, is ignored. The reason for this is that segment endpoints can contain no information as illustrated in figure 5.3. In principle, walls and furniture described by segments are finite objects and would provide full robot pose updates. We are however rarely able to observe the segment in its full length. When the segment are delimited by detectable features (convex or concave corner, door opening) we can determine the true endpoints with a scheme for high-level feature extraction. Without it, it is not possible to know whether the segment endpoints are trustworthy. Then, the observation contains only information from the supporting line, that is, in orientation and perpendicular direction. For data association, it would be wasteful to ignore the segments completely as the observed length is a lower bound on the true segment length. We will therefore develop an uncertainty model for segments including their length. Endpoint uncertainty stems from segmentation noise, which is the uncertainty of points being classified as model inliers or outliers. This uncertainty is visible at the segment’s extremities and typically spans several raw data points. In (Castellanos & Tardos 1999) the angular uncertainty of the two raw endpoints readings is taken to determine endpoint uncertainty. But this is an optimistic error model since, confirmed by observation, segmentation noise spans always several points at both extremities. We therefore approach the representation and error propagation of segments as follows: A segments is defined by the Cartesian coordinates of its endpoints

= (),,, . (5.1) sxe1 ye1 xe2 ye2 The degree of freedom in the succession of endpoints is exploited to add visibility information. A segment with endpoints = (), and = (), is interpret- sPe1 xe1 ye1 Pe2 xe2 ye2

66 5.2 MULTI-SEGMENT LINES ed as a vector from to which is visible from its right-hand side. Pe1 Pe2

With θe as the angle of the endpoint Pe = ()xe, ye and ∆θ as the angular resolution of the range sensor, we define an angular interval n ⋅ ∆θ , centered around θe , which contains Pe with probability α (figure 5.4). The choice of n depends on the sensor noise, segmentation method and angular resolution. With Gaussian distributions, a significance level α = 0.95 and n = 4 , for example, we say with other words that the angular variation σ = ∆θ . (5.2) θe

The intersection point of the ray at θe with the supporting line is given by

r cos θe r sin θe xe = ------; ye = ------(5.3) cos ()αθ– e cos ()αθ– e and the partial derivatives of (5.3) with respect to the uncertain variables are

cos θ sin ()αθ– cos θ ∂------fx- = ------r e e ; ∂------fx- = ------e - (5.4) 2 ∂α cos ()αθ– e ∂r cos ()αθ– e sin θ sin ()αθ– ∂------fx- = ------–r sin α - ; ------∂fy = ------r e - (5.5) 2 2 ∂θe cos ()αθ– e ∂α cos ()αθ– e sin θ ------∂fy = ------e - ; ------∂fy = ------r cos α - (5.6) 2 ∂r cos ()αθ– e ∂θe cos ()αθ– e For the 22× -covariance matrix of the endpoint location, we obtain with the derivatives (5.4)–(5.6) according to the error propagation law

2 2 σ 2 = [ 2 cos2 θ tan2 ()αθ– σ 2 ++cos2θ σ 2 ------r sin α -σ 2 xe r e e α e r 2 θe cos ()αθ– e

2 2 + 2r cos θe tan ()αθ– e σαr ] ⁄ cos ()αθ– e (5.7)

θe

n ⋅ ∆θ

()α, r

Figure 5.4. Endpoint uncertainty is derived from intersection of an angular interval with the segment’s supporting line ()α, r .

67 5. GLOBAL EKF LOCALIZATION

2 2 σ 2 = [ 2 sin2 θ tan2 ()αθ– σ 2 ++sin2 θ σ 2 ------r cos α -σ 2 ye r e e α e r 2 θe cos ()αθ– e

2 2 + 2r sin θe tan ()αθ– e σαr ] ⁄ cos ()αθ– e (5.8)

sin 2θ 2 σ = ------e [ 2 tan 2()αθ– σ 2 + σ 2 – ------r sin 2α -σ 2 xye 2 r e α r 2 θe 2 cos ()αθ– e sin 2θe cos ()αθ– e

+ 2r tan ()αθ– e σαr ] (5.9) 2 For the segment length l and its variance σl we introduce the reference frame {}R′ such that its xE -axis is aligned to the supporting line and the frame {} attached to the endpoint. By means of the transforms between {}R′ and {}E expressed in the robot reference frame {}R

T xRR′ = 00απ– ⁄ 2 (5.10)

T xRE = xe ye θe (5.11) we express Pe in {}R′ (Neira, Montano et al. 1993): = û ∆ xR′E xRR′ xRE (5.12) = xR′R ∆ xRE Applying the transform and error propagation result for corner features (see appendix B), we obtain for the first two moments

xe sin α – ye cos α xR′E = xe cos α + ye sin α (5.13) θe – α + π ⁄ 2

T T T ()= ∆ () ∆ + ∆ ()∆ CxR′E J1 Jû CxRR′ Jû J1 J2 CxRE J2 (5.14) T = J2∆ Cx()RE J2∆ Since {}R′ is aligned to the supporting line, l is merely the difference of the x -components 2 of xR′E 1 and xR′E 2 . Opposed to the derivation of σl via the Euclidian distance, this scheme has the advantage that only linear operations are involved and uncertainties can be propagated without approximation errors. Let ()xe 1, ye 1 = ()x1, y1 , ()xe 2, ye 2 = ()x2, y2 , and then the segment length and its variance are as follows: l = ()sin α – cos α – ()sin α – cos α x1 y1 x2 y2 (5.15) = ()x1 – x2 sin α – ()y1 – y2 cos α

2 = 2 + 2 sin2 + 2 + 2 cos2 – + sin 2 (5.16) σl ()σx 1 σx 2 α ()σy 1 σy 2 α ()σxy1 σxy2 α

68 5.3 GEOMETRIC CONSTRAINTS

Nonetheless, infinite lines have an advantage which makes them a very appropriate feature for indoor environments; man-made buildings exhibit a high degree of collinearity. Hallways, for instance, share two walls. Modeling these walls by two infinite lines, is semantically more correct than the set of isolated segments between each door, bookshelf and closet in this corridor. The capability to correctly represent collinearity and to correctly reflect the probabilistic information – namely in perpendicular direction and in orientation – is the main advantage of infinite lines. Here we want to combine this with the plus of information from segments by introducing multi-segment line features: With a set of segments {}ns and a supporting line , a multi-segment line (MS- ns si i = 1 l line) is defines as

n = (), {}s . (5.17) mlsi i = 1 For estimation, the probabilistic information of the feature is that one of the line l , for data association, the segments come into play. Multi-segment lines have specific, powerful geometric constraints which will be discussed below.

5.3 Geometric Constraints

We denote the local map the set of observed features, = {}p and the global L Lli i = 1 map the set of model feature in the map, = {}m . Given two pairings GGgj j = 1 pij = {}li, gj , pkl = {}lk, gl of local and global features, a geometric constraint is a condition on li and gj or a condition on li and lk and on gj and gl . Geometric constraints direct the search in the space of possible local-to-global data associations and reduce enormously the space to be explored. Before we proceed to the search for valid data associations, we consider geometric constraints in more detail. Geometric constraints can be classified into two categories (Neira, Montano et al. 1993): Location independent constraints can be validated without having an estimation of the robot location. They include unary and binary constraints. Unary constraints apply on intrinsic properties of a feature. Examples are feature type, color, texture or dimension such as length or width. Unary compatibility is directly found by comparison (function satisfy_unary_constraints in algorithm 1). They are powerful since whole subspaces can be excluded from the search beforehand. Binary constraints always apply to the features of two pairings. Binary constraints are used to validate whether two local features are consistent with two global features (function satisfy_binary_constraints in algorithm 1). Examples include relative measures such as distance or angle.

69 5. GLOBAL EKF LOCALIZATION

The second category are location dependent constraints which come into play as soon as a robot position is available. The rigidity constraint performs a single-feature global-to-local frame transform also known from the matching step in a EKF localization cycle. Given a robot location Lh with moments x and P , an observation li , and a pairing candidate gj from the map, rigidity is satisfied if the observed feature metrically coincides with the model feature after transformation into the same reference frame.

Visibility constraints indicate whether a model feature gj is visible from a robot location Lh . Non-visibility can be due to feature properties as relative view direction or from sensing limitation as maximal range or resolution. Extension constraints test whether an observed feature is fully contained in the candidate model feature (they completely overlap). This is relevant for finite-dimension features like line segments or circular arcs whose observations can be smaller than the model features in some sense. Since we deal with uncertain geometric information all comparisons make use of the Ma- halanobis distance and a significance level α . Hereafter we discuss in more detail the geometric constraints between the following features: lines, segments, multi-segment lines, angles and points. Opposed to (Castellanos & Tardos 1999) where they use the SPmodel as formalism for feature representation (see chapter 7), features will be represented by the individual models from appendix B. For the sake of comprehensibility, we will assume uncorrelated features: uncorrelated local and global features, uncorrelated global features and no feature-to-robot correlations. The extension follows from the error propagation law and yields slightly more complicated expressions (Breipohl 1970), (Arras 1998). In a localization scenario this assumption is satisfied very well. For the reminder of the section it is further assumed that the difference of two angles is always wrapped to the minimal difference and that all angles are in the interval [0 …2π [.

5.3.1 Location Independent Constraints

5.3.1.1 Unary Constraints With features obtained from range data, the instrinsic property which can be determined is their physical dimension. Features of finite dimension such as linear or circular segments can take advantage from unary constraints (table 5.1).

70 5.3 GEOMETRIC CONSTRAINTS

Line –

Segment ll ≤ lg

MS-Line lmax, l ≤ lg Angle – Point –

Table 5.1. Unary constraints

Unary Constraint of Segments The condition for line segments applies on their length. A pairing is valid if the length of the local segment ll is equal or smaller than those of the global segment lg (figure 5.5).

ll ≤ lg (5.18) With uncertain geometric information the test is done in a two-step way. The Mahalanobis 2 distance D tests on equality on the given significance level α while the second, non-probabilistic condition tests on inequality. We assume no correlation between the length uncer- 2 2 tainties σ , σ . Thus the joint uncertainty is the sum of the uncertainties ll lg 2 2 2 σ = σ + σ , (5.19) l ll lg and the Mahalanobis distance is

2 T 2 –1 Dl = ()ll – lg ⋅⋅()σl ()ll – lg 2 2 (5.20) = ()ll – lg ⁄ σl The first sufficient condition that the constraint is satisfied if

2 2 Dl < χα, 1 (5.21) 2 2 where χα, 1 is a number of a χ –distribution with one degree of freedom and α the prob-

local map global map

g gk li gi j

Figure 5.5. The pairing {}li, gj does not satisfy the unary constraint for segments whereas {}li, gk does.

71 5. GLOBAL EKF LOCALIZATION ability to reject a good match. The second sufficient condition is (5.18).

Unary Constraint of Multi-Segment Lines The unary constraint of multi-segments lines is the logical conjunction of the unary constraints from its segments.

∩ ll ≤ lg (5.22) i,j i j The constraint becomes powerful by the “and”-operation but can play out its strength only in combination with the extension constraint. Each segment of the local MS-line has to satisfy for itself the unary constraint. But once a global segment has been paired, it cannot be removed from this matching process since several small segments along a local MS-line can lie on the same model segment (see figure 5.6 where e.g. a person in front of a wall produced the two small segments of this wall). Thereby, the unary constraint reduces to the condition on the length of the longest local segment (table 5.1). Despite this reduction the

local map global map

li si + 2 si + 1 si sj + 2 gj sj + 1 sj

Figure 5.6. A local map with three segments on a multi-segment line and a global map of a typical hallway situation. Thanks to the conjunctive unary constraints from

the segments, there is only one possible pairing of li namely with gj . The segment pairings are: {si,}sj + 1 , {,si + 1 sj + 2 } and {,si + 2 sj + 2 } . constraint is still stronger than the unary constraint of single segments and the one of infinite lines which have no unary constraint at all.

5.3.1.2 Binary Constraints With five feature types there are fifteen different binary constraints. However, many of them turn out to occur twice or do not exist. Table 5.2 illustrates the binary constraints among the features. Lines, segments and MS-lines have the same binary constraints as they share the same probabilistic information which is the ()α, r -parameters of the supporting line.

72 5.3 GEOMETRIC CONSTRAINTS

START

2 2 Y D l < χα,1

Y ll ≤ lg

FALSE TRUE TRUE

Figure 5.7. Condition tree for the unary constraint of segments Angle features do not have binary constraints since their global and local representations differ. Locally, they are angles in the sensor frame suggesting that the angular difference could be used for a binary constraint. But globally, angle features are points in the map. The angle under which points are seen depends on the robot location which by definition of location independent constraints is unknown. This argument holds for any combination with angle features.

Binary Constraint of Line, Segment, and MS-Line Lines have two different binary constraints. There is the angle between two lines and – when they are parallel to each other – the radius (Castellanos & Tardos 1999). The condition tree has therefore a three-step structure illustrated in figure 5.8. Let = ()α , , = ()α , , = ()α , and = ()α , be l i l i rl i l j l j rl j g i g i rg i g j g j rg j the local and global features of the two pairings pii = {}l i , g i and pjj = {}l j , g j . Each feature has an associated covariance matrix containing the variances and covariance of the line parameters. Identical relative angle between the candidates is first tested.

Line Segment MS-Line Angle Point

LineDα, D r Dα, D r Dα, D r – Dd

Segment Dα, D r Dα, D r – Dd

MS-Line Dα, D r – Dd Angle ––

Point Dd Table 5.2. Binary constraints between the features

73 5. GLOBAL EKF LOCALIZATION

START

2 Y Dα < χα,1

N Y < χ2 Dαg α,1

N 2 Y Dr < χα,1

FALSE TRUE FALSE TRUE

Figure 5.8. Condition tree for binary constraint involving lines, segments and MS- lines. If the lines are parallel, equal radii is demanded as a further constraint.

= α – α ; = α – α (5.23) dαl l i l j dαg g i g j Since we assume no correlations, the resulting variance sums up over all involved uncertainties.

σ 2 = σ 2 +++σ 2 σ 2 σ 2 . (5.24) d α α il, α jl, α ig, α jg, The first condition is then

2 2 Dα < χα, 1 (5.25) with

2 2 –1 D = ()dα – dα T()σ ()dα – dα α l g d α l g (5.26) = ()– 2 ⁄ σ 2 dαl dαg d α The second condition tests on collinearity

()2 < χ 2 ∨ ()2 – π < χ 2 (5.27) Dαg α, 1 Dαg α, 1 whereas the third and last one demands equal radii

2 D r < χα, 1 (5.28) with

74 5.3 GEOMETRIC CONSTRAINTS

2 D = ()d – d T()σ –1()d – d r rl rg d r rl rg (5.29) = ()– 2 ⁄ σ 2 drl drg d r where , and σ 2 are calculated analogously to equations (5.23) and (5.24). drl drg d r

Binary Constraint Line—Point, Segment—Point, MS-Line—Point The binary constraint between a line and a point is a test on their smallest distance to each other (figure 5.9). Given an ()α, r -line, a point ()xy, , and their associated 22× -covari-

()α, r d ()xy,

Figure 5.9. The line–point binary constraint tests on minimal distance from the point onto the line ance matrices, the shortest distance is directly

dx= cos α + y sin α – r . (5.30) Since we assume only correlations between the respective feature parameters but not be- 2 tween the features, the distance variance σd is computed as follows

2 2 2 2 σ 2 = ∂f σ 2 ++∂f σ 2 2∂f ∂f σ +∂f σ 2 ++∂f σ 2 2∂f ∂f σ d ∂α α ∂r r ∂α∂r αr ∂x x ∂y y ∂x∂y xy (5.31) with

∂ ∂ f = y cos α – x sin α ; f = –1 (5.32) ∂α ∂r

∂f = cos α ; ∂f = sin α (5.33) ∂x ∂y and equation (5.30) as the relationship f (). . Substituting the partial derivatives into (5.31) yields

2 2 2 2 2 σd = σx cos α ++σy sin α σxy sin 2α (5.34)

2 2 2 ++σα ()y cos α – x sin α σr – 2σαr()y cos α – x sin α (5.35)

75 5. GLOBAL EKF LOCALIZATION

The constraint is satisfied if

2 2 Dd < χα, 1 (5.36) holds with

2 T 2 –1 Dd = ()dl – dg ()σd ()dl – dg 2 2 (5.37) = ()dl – dg ⁄ σd and

σ 2 = σ 2 + σ 2 (5.38) d d l d g where we have added the subscripts lg and to denote the local and global line-point pair.

Binary Constraint Point—Point Because point features do not carry orientation information, their mutual binary constraint is their Cartesian distance dP . Given two points i = ()xi , yi , Pj = ()xj , yj and their associated 22× -covariance matrices, the relationship f (). is

2 2 dy= ()j – yi + ()xj – xi (5.39) with derivatives

– – ------∂f = –------xj xi ; ------∂f- = –------yj yi (5.40) ∂xi d ∂yi d – – ------∂f = ------xj xi ; ------∂f- = ------yj yi (5.41) ∂xj d ∂yj d Again, we assume only correlations between the parameters of the same feature but not between the features, so

2 2 σ 2 = ------∂f σ 2 ++------∂f- σ 2 2------∂f ------∂f- σ d xi yi xyi ∂xi ∂yi ∂xi ∂yi

2 2 +++------∂f 2 ------∂f- 2 2------∂f ------∂f- (5.42) σx σy σxy ∂xj j ∂yj j ∂xj ∂yj j

= ------1 [()– 2()σ 2 + σ 2 + ()– 2()σ 2 + σ 2 2 2 xj xi xi xj yj yi yi yj ()yj – yi + ()xj – xi + 2()– ()– ()]σ + σ (5.43) yj yi xj xi xyi xyj The constraint is satisfied if

76 5.3 GEOMETRIC CONSTRAINTS

2 2 Dd < χα, 1 (5.44) holds with

2 T 2 –1 Dd = ()dl – dg ()σd ()dl – dg 2 2 (5.45) = ()dl – dg ⁄ σd and

σ 2 = σ 2 + σ 2 . (5.46) d d l d g

5.3.2 Location Dependent Constraints

5.3.2.1 Rigidity Constraint Rigidity tests on coincidence of global and local feature after their transformation into a common reference frame. This is what is done in the measurement prediction and matching step in a Kalman filter localization cycle. Refer to section 4.4 of chapter 4 for details. Under the Gaussian assumption, the resulting Mahalanobis distance has distribution χ2 with r degrees of freedom where r is the number of feature parameters of the entities involved. Individual compatibility is satisfied if the distance is smaller than a value chosen from this χ2 distribution on a significance level α . With correlated local or global features, rigidity test on joint compatibility. Correlated observations are stacked into vectors and processed in a batch estimation mode. The number of degrees of freedom of the χ2 distribution from which α is chosen will now be q where qpr is the product of the number of observed features times their number of parameters . If, what is the case in this section, no correlations are assumed, joint compatibility equals individual compatibility.

5.3.2.2 Visibility Constraints When a robot location estimate is known, we can employ the visibility constraint to verify if a global feature is visible from that location. The feature-specific visibility constraints are given in table 5.3.

Visibility Constraint of Line Features

Given a line whose parameters ()α, r carry the visibility information according to appendix B, lines can be seen from their visible side and from a distance not greater than a sensor- specific perception radius RS . With the known robot position we transform the line parameters into the robot frame and apply the condition

77 5. GLOBAL EKF LOCALIZATION

R R ()r > 0 ∧ ()r ≤ RS . (5.47) With uncertain geometric information we first employ the Mahalanobis distance which tests on equality of the two conditions in (5.47) before applying the non-probabilistic test (5.47).

When using the SPmodel (Castellanos & Tardos 1999), the test ()Rr > 0 is not needed as the feature reference frame which is attached to all entities already defines an orientation, and by that, the line’s one-sided visibility.

Visibility Constraint of Segments In addition to lines, segments have a condition on the minimal angle under which they are seen. If this angle, ϕ , is equal or greater than a threshold ΦS the segment is perceptible and the visibility constraint is satisfied:

R R ()r > 0 ∧∧()r ≤ RS ()ϕΦ≥ S (5.48) The angle ϕ is obtained by

y – yr y – yr ϕ = atan ------e1 - ; ϕ = atan ------e2 - (5.49) 1 – 2 – xe1 xr xe2 xr

ϕϕ= 1 – ϕ2 (5.50) with and as the segment’s endpoints. Remember that angle differ- ()xe1, ye1 ()xe2, ye2 ences must always be unwrapped. This visibility constraint is motivated by two effects. The first one is specular reflection which occurs at steep view angles of a surface (typically with ultrasonic sensors but also with laser range finders – see chapter 2). The second motivation comes from a natural con-

Line R R ()r > 0 ∧ ()r ≤ RS Segment R R ()r > 0 ∧∧()r ≤ RS ()ϕΦ≥ S

MS-Line R R ∪ {}()r > 0 ∧∧()r ≤ RS ()ϕi ≥ ΦS i

Angle dR≤ S

Point dR≤ S Table 5.3. Visibility constraints in dependence of feature-speciﬁc and sensor- speciﬁc properties

78 5.3 GEOMETRIC CONSTRAINTS dition of the extraction algorithm: A segment is only found if a minimal number of adjacent range readings lie on a line-like environmental object. What looked like an ad-hoc threshold ΦS is thereby on a sound basis, either from physical constraints from the sensor and/ or from the minimal-point condition in the line extraction method of chapter 3.

Visibility Constraint of Multi-Segment Lines The visibility constraint of multi-segment line features is the logical disjunction of the visibility constraints of its segments (table 5.3). It is sufficient that one of the nS segments is visible in order the MS-line feature to be visible.

Visibility Constraint of Point and Angle Features The visibility of point- and angle-features (whose global representation is identical) depends on the Cartesian distance from which they are seen. Given the sensor-specific perception radius RS , their global position in the map gi = ()xi , yi and the robot pose ()xr ,,yr θr , the visibility constraint is satisfied if

dR≤ S (5.51) holds, where

2 2 dx= ()r – xi + ()yr – yi . (5.52) Note that condition (5.51) assumes a 360° sensor and a 360° visibility of the feature. This is true, for example, for a free-standing column and a range finder which covers 2π . If the sensor had a limited angular working range (forward looking CCD camera), condition

(5.51) would also depend on the vehicle orientation θr and sensor-specific angular limits. If the feature had a limited angular visibility (a vertical edge of a door frame, a retro-reflecting beacon on a wall), condition (5.51) would also depend on the orientation of the feature and its specific angular visibility limits (by that, the feature would actually become a corner feature as it carries an orientation). With uncertain geometric information we perform a two-step decision as in figure 5.7 of section 5.3.1.1. First, equality is tested with the Mahalanobis distance

2 = – T 2 –1 – DR ()dRS ()σd ()dRS (5.53) 2 2 = ()dR– S ⁄ σd 2 where σd is computed similarly to the binary constraint point–point in equation (5.43) of section 5.3.1.2. If equality does not hold, the non-probabilistic condition (5.51) is applied.

5.3.2.3 Extension Constraints For finite-dimension features like segments in a line or arc segment on a circle, observa-

79 5. GLOBAL EKF LOCALIZATION

gj Figure 5.10. Full overlap (top), partial overlap (middle) and no overlap (bottom) tions can be smaller than the candidate model features in some sense. The extension constraint is satisfied if the global feature fully overlaps with the local feature seen from the robot pose.

Extension Constraint of Segments

Given two segments, si and sj , whose supporting lines have found to satisfy the rigidity constraint – they are collinear on the level α – and the unary constraint – the local segment is shorter than the global segment. Then, let = (), and = (), be Pel xli yli Peg xgj ygj those endpoints of segments si and sj which maximize the Euclidian distance = – 2 + – 2 (5.54) dmax ()xli xgj ()yli ygj among the four endpoint-to-endpoint distances. Let further lmax be the length of the longer of the two segments which is the global segment by definition of the unary constraint. From figure 5.10 it can then be seen that the condition for a full overlap is

dmax ≤ lmax . (5.55) For a probabilistic decision we require the propagated uncertainties from the endpoints to 2 the variables of (5.55). The variance of lmax is the length variance σl of the longer segment according to equation (5.16) in section 5.2. We apply first-order error propagation to determine σ 2 , the variance of , with the covariance matrices of and d dmax Pel Peg

Line –

Segment dmax ≤ lmax

MS-Line ∩ ()dmax, i ≤ lmax, i i Angle – Point – Table 5.4. Extension Constraints

80 5.4 HYPOTHESIS GENERATION

2 2 σx σxy σx σxy CP()= l l ; CP()= g g (5.56) el σ σ 2 eg σ σ 2 xyl yl xyg yg and equation (5.54) as f (). :

2 2 2 ∂f 2 ∂f 2 ∂f ∂f σ = σx ++σy 2 σ d ∂x l ∂y l ∂x ∂y xyl

2 2 ∂f 2 ∂f 2 ∂f ∂f +++σx σy 2 σ (5.57) ∂x g ∂y g ∂x ∂y xyg Evaluation of (5.57) is analogous to the result in equation (5.43). The extension constraint is satisfied if

2 2 De < χα, 1 (5.58) with

2 T 2 –1 De = ()dmax – lmax ()σe ()dmax – lmax 2 2 (5.59) = ()dmax – lmax ⁄ σe where

2 2 2 2 σe = σd ++σl σdl . (5.60) 2 2 The covariance σdl is non-zero since both, σd and σl , share one endpoint as a common source of uncertainty.

Extension Constraint of Multi-Segment Line Similar to the unary constraint of multi-segment line features, the extension constraint is the logical conjunction of the extension constraints of its segments (table 5.4). While the unary constraint reduced to a condition of just the longest segment along the local MS-line, the feature can now play out the advantage of the conjunctive condition: the extension constraint of MS-lines is validated if each segment along the local MS-line satisfies its extension constraint with a segment along the global MS-line. Global segments can be associated several times (table 5.4).

5.4 Hypothesis Generation

Data association ambiguity leads to location ambiguity. In the approach pursued here, location ambiguity will be represented by a set of multiple Gaussian location hypotheses

81 5. GLOBAL EKF LOCALIZATION

g6 l1 g1 g3 g7 l3 l2 g8 g5 g4 Figure 5.11. Local and global map. The local map contains two segment observations and one point observation, the global map a total of eight model features

l1 g1 g2 g3 g4 g5 g6 g7 g8 ...

l2 g1 g2 g3 g4 g5 g6 g7 g8 g1 g2 g3 g4 g5 g6 g7 g8 g1 g2 g3 g4 g5 g6 g7 g8 ......

l3 Figure 5.12. Interpretation tree of the two maps in ﬁgure 5.11 with three levels and eight branches. Not shown is the star-branch which is added as an extra global feature to each subtree.

n Hh= {}i = 1 . A location hypothesis h consists of two elements. The first one is the robot location estimate Lh , a random variable with first and second moment: ˆ EL[]h = x ; Cov()Lh = P (5.61) The second element is the supporting set, = {}p , which contains pairings that Sh pi i = 1 satisfy the geometric constraints from the features on a given significance level α :

hL= ()h, Sh (5.62)

A pairing pij = {}li, gj is an interpretation of the measurement li saying that li and gj denote the same physical object in the environment. li is a feature from the local map = {}p , (the observation), a feature from the global map = {}m . Lli i = 1 gj Ggj j = 1

82 5.4 HYPOTHESIS GENERATION

Data association is performed on a feature-to-feature level – or measurement-to-measurement level (Bar-Shalom & Li 1995) – and not on a hypothesis-to-hypothesis level. Note that H is not a mixture of Gaussians since this is a non-Bayesian approach to data association. Hypotheses are not in competition to each other and are considered individually. Mobile robot localization formulated as a matching problem is the problem of finding a set of correct associations of observations to model features in the space of all possible ones. “Correct” denotes statistical compatibility on the level α given all involved uncertainties. The search space has the structure of a tree (Grimson & Lozano-Pérez 1987) with pm levels and + 1 branches where pm is the number of observed features, and the number of modeled feature in the map (figures 5.11 and 5.12). The extra branch (called star- branch) allows associations in the presence of outlier observations (false positives) and thus accounts for environment dynamics and map errors. The star branch can be seen like a map feature and adds a branch to each subtree. Although the problem is of exponential complexity, geometric constraints reduce enormously the space to be explored. They allow to discard whole subtrees each time when an incompatible pairing is found at the root of such a subtree.

5.4.1 The Search Algorithm

Tree traversal is realized as a depth-first, backtracking search which applies geometric constraints at each tree node to validate whether geometric relations among observations and their associated model features are (still) satisfied. This is realized in a identifying while locating scheme in which pairing formation and location estimation is performed simultaneously. The strategy reflects the fact that location dependent constraints are more powerful in falsifying infeasible hypotheses than location independent constraints. This leads to algorithm 1 (Castellanos & Tardos 1999). Algorithm 1 tries first to find a minimal supporting set with location independent constraints such that a location estimate can be determined (part B ). When an observation is selected from the local map (function select_observation), optional rules can be applied to choose an observation which generates as few pairings as possible. An example rule for lines is to return lines from the local map which are perpendicular to those already chosen. Thus a robot location can be rapidly estimated. These rules are optional as they only affect speed but not the completeness of the algorithm.

As soon as a robot location estimate is available (function location_available returns true), the algorithm applies location dependent constraints (satisfy_location_dependent_cnstr). The succession in which they are applied is visibility–rigidity–extension. Visibility is the least expensive one since it involves only global features and extension requires compati-

83 5. GLOBAL EKF LOCALIZATION

function generate_hypotheses(hLG,, )

if L = {} then H ← Hh∪ {} else l ← select_observation(L ) for gG∈ do p ← {}lg, if satisfy_unary_constraints(p ) then if location_available(h ) then  accept ← satisfy_location_dependent_cnstr(, )  Lh p  if accept then A  h' ← h

 Sh' ← Sh ∪ {}p  estimate_robot_location( ) Lh' ← Sh'  end else  accept ← true  for ∈ while accept pp Sh  accept ← satisfy_binary_constraints(, )  pp p  end  if accept then  h' ← h  Sh' ← Sh ∪ {}p B  ← estimate_robot_location( )  Lh' Sh'  if location_available(h' ) then for while accept  pp ∈ Sh  satisfy_location_dependent_cnstr( ) accept ← Lh', p  end   end  end end if accept then H ← H ∪ generate_hypotheses(h',,L \ {}l G ) end end end H ← H ∪ generate_hypotheses(hL,, \ {}l G ) end

return H end

Algorithm 1. Given a hypothesis h (with empty Sh and no location in the beginning), the local map LG and the global map , the algorithm returns the set of generated location hypotheses H . bility in a rigidity-sense: lines must be collinear in order the overlapping test to work correctly. If a new acceptable pairing is found, it is added to the supporting set

84 5.4 HYPOTHESIS GENERATION

Sh = {}p1,,,p2 … pp , the location estimate is refined using the extended Sh (function estimate_robot_location) and the function recurs in a depth-first manner (part A ). Thus, at each tree level, all consistent pairings between the observation l and all model features gG∈ are generated. Note that the significance level α is the only parameter the user has to specify. It decides on acceptance or rejection of the geometric constraints.

Each time when the algorithm reaches the bottom of the tree, all observed features li have been assigned, either to a model feature gj or to the star branch. Then, we have a valid robot location hypothesis which can be added to HH . In the beginning, with = {} and a given, non-empty LGh and , is not needed. It appears however as an argument of generate_hypotheses since the algorithm is recursive. The proper h to start with has location_available Sh = {} and Lh such that returns false.

5.4.2 Estimating the Robot Location

Given a supporting set S = {}{}l1, g ,,,{}l2, g … {}l , g , the robot position h j1 j2 p jp Lh can be estimated using the extended Kalman filter. The Kalman filter is however a recursive formulation, well suited for tracking applications where there is always an a priori state estimate. For the case of hypothesis generation where no a priori position is available, an adequate reformulation of the EKF is the extended information filter (EIF). The EIF is a batch estimator and resembles directly the weighted mean (refer to (Bar-Shalom & Li 1993) for derivation and further details). Adopting the same notation than in chapter 4, υ denotes the stacked innovation vector of all pairings {}l , g and Rh its associated covariance matrix. Let further ∇ be the i ji q × 3 Jacobian matrix of the linearized feature measurement model (the frame transform) with respect to the robot position. q is the number of observations which is the number of observed features pr times their number of parameters . Then the EIF is as follows:

T –1 P –1()k ++1 k 1 = P –1()k + 1 k + ∇h R()k + 1 ∇h (5.63)

xˆ ()k ++1 k 1 = P()k ++1 k 1 ⋅ [P –1()k + 1 k ⋅ xˆ ()kk+ 1 T –1 + ∇h R()k + 1 ∇h ⋅ ξ()k + 1 ] (5.64) where ξ()k + 1 is a 3 × q -matrix such that

∇h ⋅ ξ()k + 1 = υ()k + 1 . (5.65) Assigning zero weight to the odometry-based state prediction can be elegantly done by setting its inverse – the information matrix – to zero

–1 P ()k + 1 k = 033× . (5.66)

85 5. GLOBAL EKF LOCALIZATION

By substituting equation (5.66) into equations (5.63) and (5.64) and using (5.65), we obtain a conventional equation system where we can easily see that dependent on q , being greater or smaller than three, the system is over- or underdetermined.

∇h ⋅ xˆ ()k ++1 k 1 = υ()k + 1 (5.67) The solution of (5.67) is obtained via the pseudoinverse

T –1 T ∇h' = ()∇h ∇h ∇h . (5.68) where we can distinguish between ∇hT ∇h being singular or non-singular. In the latter case, the equation system (5.67) has a unique solution in the least square sense (location_available returns true). In the former case, only a non-unique pose estimate with infinite number of solutions is returned (location_available returns false). This can occur if a location estimation from a supporting set was attempted which contains, for example, only one line, parallel lines or a single point feature.

5.5 Hypothesis Tracking

With a localized robot doing pose tracking, data association ambiguity can arise as discussed in the section 5.1 and chapter 4. In such a situation there are several statistically feasible pairing candidates for an observation. The closest one (in a Mahalanobis sense) is not necessarily the correct one. Choosing it when it is the wrong candidate will lead to filter inconsistency and likely to filter divergence. This is the most widely applied strategy, called nearest neighbor standard filter (NNSF). Estimator inconsistency is illustrated in figure 5.1: an incorrect pairing is formed, causing the robot to be wrongly aligned to model feature g3 instead of g2 , and though, its uncertainty to collapse. As (Cox & Leonard 1994) point out, robot navigation deals with two types of uncertainty: uncertainty in the values of measurements and uncertainty in the origin of the measurements. The interpretation of l2 in figure 5.1 is ambiguous since l2 could be g2 or g3 . This is the kind of association ambiguity which can be generally encountered in target tracking (Bar-Shalom & Li 1995) and has been described in the robot localization context (Leonard & Durrant-Whyte 1992). Here, we will pursue another strategy. As soon as there is association ambiguity, that is, there is no guarantee anymore for the correct association to be found, we will regenerate hypotheses locally during tracking (figures 5.13, 5.14). This is what track_hypothesis does, given a predicted robot location, a local and a global map. It splits up into multiple offspring hypotheses if statistical compatibility with several supporting sets can be established at that location. This strategy is also known as the track splitting filter (Bar-Shalom & Li

86 5.5 HYPOTHESIS TRACKING

a) b) c)

Figure 5.13. The idea behind multi-hypothesis pose tracking of algorithm 2: A well localized robot in a) moves and observes a single feature in b) where it is impossible to say which is the correct pairing in view of the uncertainties. Instead, the hypothesis splits up representing thereby all possible pairings at that location. The two hypotheses are tracked using location dependent constraints until a single one remains in c).

1995). Here we perform track splitting under geometric constraints which bounds the number of possible tracks as explained below. Algorithm 2 has the identical structure than algorithm 1 but employs location dependent constraints only and does not recur with a refined position estimation. In this manner the algorithm finds all supporting sets in the vi- cinity of the initially predicted location Lh and returns them in form of a hypothesis set Ht . Again, the second recursion call implements the extra branch in the interpretation tree that allows correct associations in the presence of outlier observations and map errors. track_hypothesis Since Lh is not reestimated, will generate hypotheses in a region whose size depends on the uncertainty of Lh . If Lh is certain, track splitting is unlikely to occur. If it is uncertain, feasible supporting sets will be found in a larger area around Lh (see also figure 5.15).

track_hypothesis After has been applied for each hi ∈ H , we can distinguish the three cases verification, falsification and split-up:

•,|Ht| = 1 hypothesis verification. The hypothesis hi is confirmed. This is the estimation step in an EKF localization cycle. Given the supporting set S , the robot hi location is estimated and hi is admitted to the new H .

•,|Ht| = 0 hypothesis falsification. The hypothesis can not be held any more by location dependent constraints on the significance level α . It gets rejected.

•,|Ht| > 1 hypothesis division. The track of hypothesis hi splits up into several offspring hypotheses {}hi,1 ,,,hi, 2 … hio, which all can be held by location dependent constraints at the predicted robot location. The robot locations are esti-

87 5. GLOBAL EKF LOCALIZATION

function track_hypothesis(hLG,, )

if L = {} then

Ht ← Ht ∪ {}h else l ← select_observation(L ) for gG∈ do p ← {}lg, if satisfy_unary_constraints(p ) then if satisfy_location_dependent_cnstr( ) then Lh, p ← ∪ {} Shp Sh p track_hypothesis( ) Ht ← Ht ∪ hp,,L \ {}l G end end end track_hypothesis( ) Ht ← Ht ∪ hL,, \ {}l G end

return Ht end

Algorithm 2. Given the local map LG , the global map and the hypothesis h to be tracked at location Lh , the algorithm returns the set of tracked hypotheses Ht .

mated with the EIF using their respective supporting set.

Note that track_hypothesis is structurally identical to generate_hypotheses. With a flag which switches between the modes generation/tracking, algorithm 2 can be integrated into algorithm 1 thus forming a single algorithm for local and global localization. With multiple discrete hypotheses, there is no strict distinction of being localized and being lost. There are three cases which can be characterized by n , the number of hypotheses in Hn: being lost is formulated as = 0 (without any valid hypotheses), not localized means that there is unresolved location ambiguity, n > 1 , and being localized is simply expressed as having a single location hypothesis, n = 1 .

5.5.1 Duplicate Hypotheses

When an uncertain hypothesis splits up, it can happen that duplicate hypotheses are produced. This is shown in figure 5.15, where two hypotheses h1, h2 split up and produce each four hypotheses. If these duplicates are not eliminated, H will contain redundant information, and thus undermining our intent to reach and maintain n = 1 .

Two hypotheses hi, hj are identical if they contain the same piece of information which

88 5.5 HYPOTHESIS TRACKING

gj gi B B gi gj

A A Figure 5.14. Two examples for track splitting with point and line features. Given a hypothesis at AAB , a displacement to resulting in an uncertain pose prediction and a local map with a single feature Ll= {}1 , two offspring hypotheses are generated. One with the supporting set Sh = {}{}l1, gi and the other one with Sh = {}{}l1, gj . Both can be held on the signiﬁcance level α .

local map Ll= {}1, l2 g2

g1 l 1 B g3

h1 g4 h2

l2 A Figure 5.15. Hypothesis duplication. Given a local map with a ()xy, –point feature l1 , and a angle-only feature l2 , hypotheses h1, h2 split up each into four offsprings after a (very) uncertain movement AB to . This results in eight hypotheses at B , four of them being redundant. in our case is identical location. Identical location is due to identical supporting sets

h ≡ h ⇔ ()S = S . (5.69) i j hi hj This condition is further to be generalized with the distinction of a unique and a non-unique robot location estimate. In the latter case the current observation does not contain enough information to uniquely estimate a robot position (the pseudoinverse ∇h' of equation (5.68) does not exist). Then, the EIF is underdetermined and will return an infinite number of solutions. These solutions denote a degree of freedom in the robot position. Along this degree of freedom, condition (5.24) is unable to distinguish duplicate hypotheses because several distinct hypotheses can be aligned to the same model feature (figure 5.16). We

89 5. GLOBAL EKF LOCALIZATION

A h4 h12, ≡ h21, h3 l 1 h ≡ h 32, 41, B B g2 h31, ≡ h42, h11, ≡ h22, g1

h2 A h1 local map Ll= {}1 global map

Figure 5.16. A situation which illustrates what it means to be “aligned” to the same model feature. The four hypotheses {}4 split up each into two offspring hi i = 1 hypotheses after an uncertain movement AB to where the j -th offspring of hypothesis hi is denoted hij, . Hypotheses h11, , h22, and h31, , h42, are two pairs of duplicates with equal supporting set, namely Sh = {}{}l1, g1 , and would be wrongly declared as duplicates by condition (5.69). They are however (pairwise) distinct and require the detection condition (5.71). therefore add a distance condition along this degree of freedom. Let x , x and P , P hi hj hi hj be the first and second moments of L and L respectively, then “closeness” is defined hi hj by means of the Mahalanobis distance d . Thus hihj d = ()x – x ()P + P –1()x – x T , (5.70) hihj hi hj hi hj hi hj

 ()S = S $ ∇h' hi hj hi ≡ hj ⇔  (5.71)  ()S = S ∧ ()d < χ 2 ± ∇h' hi hj hihj α 2 2 with χα a value chosen from a χ -distribution with three degrees of freedom. Why can this kind of hypothesis duplication happen? It happens since we generate robot locations that satisfy, from these locations, geometric constraints from the features of their supporting sets. These locations are therefore not the result of random draws but follow causally, that is, geometrically, from the sensor observations. When hypotheses are locally generated as illustrated in figure 5.15, they will “snap” into the places from where a location can satisfy its constraints. Now we can also understand how track splitting under geometric constraints will work: in the region defined by the uncertainty of the predicted robot location, algorithm 2 will put hypotheses only into these “snap”-locations whose number is limited by the geometric constraints from the local and global map.

90 5.6 A FRAMEWORK FOR GLOBAL EKF LOCALIZATION

5.5.2 Hypothesis Scoring

Unlike Bayesian approaches to multi-hypothesis localization, hypotheses generated with our method do not have an individual probability. They are equally plausible robot locations since they satisfy their uncertain geometric relationships on the same given significance level α . Hypotheses differ, however, in their support by paired features and their geometric quality. The former is measured by the number of valid (non-star-branch) associations p' in Sh and the latter by the joint Mahalanobis distance. The joint Mahalanobis is like the Mahal- anobis distance except that it applies not only to a single pairing but sums up over the whole supporting set including correlations (the joint compatibility rigidity constraint in section 5.3.2.1). It accumulates the weighted squared error distances (residuals) and thus is a goodness-of-fit measure.

The best hypothesis is the one maximizing p' and, in case of a tie, minimizing the joint Mahalanobis distance. In other words, we choose the hypothesis which has most paired features and, from its location, satisfies best the rigidity constraint.

5.6 A Framework for Global EKF Localization

In this section hypothesis generation and tracking are brought together to form a concise framework for feature-based global localization (flow diagram in figure 5.17). An example scenario will help to explain the framework: at start, the robot has no information on its position, the hypothesis set H is empty. The flag loc is set to zero and new hypotheses are generated. At the next iteration the hypothesis set H contains more than one hypothesis and we will enter the second branch. After prediction of all hypotheses in H according to their individual uncertainty and to the uncertain inputs from odometry, each hypothesis is tracked with algorithm 2. Due to the movement of the robot, several hypotheses can be discarded since they produce geometric contradiction at the level α . However, the robot is not yet localized – loc is false – and we will continue to enter the second branch until a single hypothesis remains. Once a single hypothesis is left, the robot is localized, the third branch is entered and the flag loc is set to true. After prediction and tracking of its (only) hypothesis, the hypothesis is scored according to section 5.5.2 which of course is trivial with a single candidate. Now the robot is in what can be said to be the steady-state of localization. As long as nothing happens it will remain in cycle nr. 3. Kidnapping is the act of clandestinely bring the robot away from its true position and drop

91 5. GLOBAL EKF LOCALIZATION

START

1 ← 0 H. = 0 loc n generation N

2 H. H. N 1 = 1 n > prediction tracking loc N

← 1 H. H. H. = 1 loc n prediction tracking scoring

Figure 5.17. The ﬂow diagram for global EKF localization it somewhere in the environment. The robot is lost and unlike at the start pose it has no knowledge of being lost. If a well localized robot is kidnapped it will sooner or later discard all its hypotheses as the observation at the new position does not fit the prediction from the old position. Zero hypotheses is the result, we will enter the n = 0 -branch, set loc to false and regenerate hypotheses. Then, the scenario is rehearsed as above. Collisions, even if they happen only “locally”, are a form of kidnapping. Suppose the robot is well localized with a single hypothesis (cycle nr. 3) and is subject to an ambiguous data association situation from high location uncertainty and/or low feature discriminance. Consequently, algorithm 2 will split up and produce offspring hypothesis. In this situation, n > 1 , we distinguish between being not localized locally or globally by means of the flag loc . If a robot is not localized globally, it makes no sense to determine a best hypothesis for path planning because the environment can contain true symmetries1. The scoring outcome would be random. If a robot is not localized locally, that is, it has several location hypotheses locally after track splitting, we can rely on the fact that locally an environment has typically no symmetries. Therefore the robot can continue its mission with the hypothesis which was found “best” by the scoring strategy. Continuing the scenario from above, the robot will enter the second branch after track splitting and predict and track the current set of locations. According to the flow diagram, the vehicle will now continue to determine the best hypothesis in order to carry on its mission. This lasts until the robot is localized again.

1. Environment symmetry is commonly called the property of an environment to have several equally looking places for the perception system of the robot. An example is a multi-ﬂoor ofﬁce building.

92 5.7 EXPERIMENTS

We propose a conservative attitude in the problem of explosion-like growth of the number of tracks: The motion model of the robot – non-systematic odometry errors or an error model accounting for collisions – defines a region around the current state prediction. We want this region to be the area in which the track splitting filter senses location ambiguity. If we allowed offspring hypotheses to split up again, we would enlarge this area. Therefore, only the current best hypothesis is allowed to do track splitting whereas offspring hypotheses can only be confirmed or falsified. As the number of geometric “snap” locations in this region is limited, the offspring hypotheses of the current generation are likely to be duplicates of the offspring hypotheses of former generations. With condition (5.71) they can be detected and rejected.

5.7 Experiments

The framework has been implemented twice. With a lines-only implementation in simulation (Matlab) the first set of experiments has been carried out. The embedded system implementation on the robot Pygmalion and Robox (appendix A) provides multi-segment lines, points and angle features.

5.7.1 Simulation Experiments

A simulation environment has been developed which allows to create line-based maps and to manually guide the robot through a virtual environment. Local maps are generated by ray tracing where a 360° range finder with 1° resolution is simulated. The maximal range has been limited to two meters. In the simulation experiment, We assume to sources of odometry errors (see below) whereas observations and model features receive a typical, constant and uncorrelated uncertainty. In the beginning, the user drops the robot at a position from which – since H is empty – the hypothesis generation phase is started. Tracking is done by manually placing the robot relative to its last true position. These user positions are the predicted odometry positions for which the error models compute the corresponding uncertainties (robots drawn in gray with 95%-ellipses in figure 5.18). The real robot (black in figure 5.18) is subject to errors according to the models and reaches the specified locations only approximately. Finally, kidnapping noise can be introduced as illustrated in the experiment. The simulation run of figure 5.18 shall test simultaneous hypothesis generation and tracking under conditions of significant odometry errors and low feature discriminance. We inject • Wheel space noise accounting for uneven floors, wheel slippage or resolution arti-

93 5. GLOBAL EKF LOCALIZATION

6 17 15

4 14

12 11

9 -2 8

23 7 -4 22 6 2 5 21 1 3 4 -6

20 18 19 -8

-6 -4 -2 0 2 4 Figure 5.18. The simulated test path. Besides extensive odometry uncertainties and errors, the robot collides with a person at step 11 and gets kidnapped at step 18.

94 5.7 EXPERIMENTS

Resulting Hypotheses Set (step 1) a) 8 b) 8

7 6

6 4

5 2

4 0 y [m]

3 -2

2 -4

1 -6

0 -8

-6 -4 -2 0 2 4 -6 -5 -4 -3 -2 -1 Resulting Hypothesesx [m] Set (step 20) Resulting Hypotheses Set (step 21)

8 c) 8 d)

6 6

4 4

2 2

0 0 y [m] y [m]

-2 -2

-4 -4

-6 -6

-8 -8

-6 -4 -2 0 2 4 -6 -4 -2 0 2 4 x [m] x [m]

Figure 5.19. Hypotheses set H at a) step 1, b) steps 13-17, c) step 20 and d) steps 21 (four hypotheses), 22 and 23. Ellipses in ﬁg. 5.18 and ﬁg. 5.19 denote 95% probability levels.

95 5. GLOBAL EKF LOCALIZATION

0 2 4 6 8 10 12 14 16 18 20 22 Figure 5.20. Number of hypotheses. Diamonds (steps 4-10,12,13,22,23): the robot is localized, circles (steps 11,19): the robot is lost, points: the robot is not localized.

facts. Error growth factors have been magnified by a factor of two with respect to the identified values in chapter 2. • Cartesian space noise accounting for collisions. A simple model with error growth proportional to the relative angular and translational displacement has been taken. Growth factors have been magnified by a factor of ten of what would be physically suggested. • Kidnapping noise accounting for the case of a robot clandestinely brought away from its true position. This type of noise is unmodeled.

5.7.1.1 Results In step 1, the robot has no a priori knowledge on its position and observes two perpendicular lines. This yields 72 hypotheses (figure 5.19 a). Steps 3 and 4 are sufficient to localize the robot which stays localized until step 8. This although the robot moves blindly on a long distance between steps 6 and 7, causing the uncertainty to grow extensively and thus the error of the true robot as well. In step 11, the robot tries to move forward but collides with a person. It ends up far from the predicted odometry position. No valid pairings can be produced with the current local map at that prediction yielding zero hypotheses – the robot is lost. Hypothesis generation is therefore activated at step 12 with four observed lines. These four lines turn out to be globally unique in combination and therefore yield a single (the true) hypothesis. During steps 13 to 17 (figure 5.19 b) this hypothesis splits up several times since uncertainties do not allow to uniquely determine the true supporting set. Although the lines which give rise to the track splitting are 40 cm apart, the uncertainties from odometry force track_hypothesis to generate two or more hypotheses aligned to these lines. In step 18 we kidnap the robot and bring it far down to the bottom of the corridor. The observation at step 18 is still compatible with its expectation from the predicted position (gray). There is no evidence yet to the robot of what happened. Only at position 19 no location dependent constraints can be satisfied anymore – the robot is lost again. The local map from position 20 consists of three lines and yields twelve hypotheses (figure 5.19 c) which can be falsified during the last steps up to the true one (figure 5.19 d): the robot is localized again. During this 23 step path, the following data has been recorded: The average relative dis-

96 5.7 EXPERIMENTS placement between the observations of each step is 1.49 m and -18.0° in θ . The average prediction error – difference of predicted (gray) and true (black) location – is 0.26 m and 10.2°. A total of 31 hypotheses performed track splitting into a total of 70 offspring hypotheses. Further, the number of floating point operations has been determined as 58 kflops in average and 355 kflops maximal. The algorithm succeeded always in generating, tracking and confirming the true robot hypothesis. This is remarkable in view of the extent of odometry errors and the average distance between two observations. The robot stays localized in the presence of errors and sensing ambiguities where, drawn from experience, a single hypothesis tracking would fail. This is a dramatic increase in robustness which is made possible with relative small computational costs. Furthermore, after steps 1, 12 and 20 where the hypotheses are generated without an a priori position estimate, we can state a very fast convergence toward the true hypothesis (figure 5.20). This although infinite lines provide only minimalistic environment information and have no unary and no extension constraint.

5.7.2 Experiments with the Robot

The flow-diagram in figure 5.17 and all involved parts have been implemented on the robot Pygmalion and Robox under the RT operating system XO/2 (appendix A). For all on-the- fly aspects, please refer to chapter 4. For the sake of efficiency, two specific aspects of multi-hypothesis on-the-fly localization are provided: we assume equal uncertainty of hypothesis prediction. This accelerates code execution as no hypothesis must be predicted individually by forward simulation of the odometry model but a single prediction result is valid for all hypotheses. Further, multiple hypotheses require, by the nature of the problem, dynamic data structures. Care has been taken that during localization no new heap memory is allocated (which is slow) but both algorithms dispose of a sufficiently large number of preallocated hypotheses (this although XO/2 has garbage collection). The experiments on the real platform have, unlike the simulation experiments, normal uncertainty parameters and are shown in figures 5.21 and 5.22. Figure 5.21 illustrates the global and figure 5.22 shows an experiment on multi-hypothesis tracking where by random movement the true hypothesis is found. The line extraction method used in the experiments is the one from chapter 3. Experiments with Robox are shown in the next chapter.

5.7.2.1 Results The local map in the experiment of figure 5.21 contains six segments with segment number 202 stemming from a door standing half open. With pp= ′ + p∗ as the number of local observations, let p′ be the number of successfully paired observations, and p∗ the number of associations to the star branch. With at least two associated local features, we obtain 63

97 5. GLOBAL EKF LOCALIZATION

2.5 local map 2 6 segments 1.5 205

0.5 204 206 0 203

-0.5

-1 201 202 -1.5

-2

-3 -2 -1 0 1 2 3

6 α = 0.95 6 α = 0.95 5 p' ≥ 2 5 p' ≥ 3

4 63hyp. 4 4 hyp.

3 3

2 2

1 1

0 0

-1 -1

-2 -2

-3 -3

-10 -8 -6 -4 -2 0 2 4 -10 -8 -6 -4 -2 0 2 4

6 α = 0.95 6 α = 0.95 5 p' ≥ 4 5 p' ≥ 5

4 2 hyp. 4 1 hyp.

3 3

2 2

1 1

0 0

-1 -1

-2 -2

-3 -3

-10 -8 -6 -4 -2 0 2 4 -10 -8 -6 -4 -2 0 2 4

Figure 5.21. Hypotheses for different values of p′ given the shown local map (top right). Execution times is 633 ms. The experiment was carried out on Pygmalion (top left) with a PowerPC604e at 300MHz CPU. hypotheses as shown in figure 5.21. There are four hypotheses with p′ ≥ 3 , two hypotheses for p′ ≥ 4 and a single one with p′ = 5 which is also the true one. Since there is an unmodeled feature in the local map (the door), no hypothesis with p′ = 6 , or p∗ = 0 respectively, is generated. Therefore, hypothesis scoring according to section 5.5.2 yields the correct result immediately. The execution time for hypothesis generation was 633 ms. Starting from five location hypotheses in figure 5.22, the robot is able to reject the four

98 5.7 EXPERIMENTS

3 2 4 5 2 1

1 3

-1

-2

-3

-10 -8 -6 -4 -2 0 2 4 Figure 5.22. Active global localization with random movement. Track #3 turns out to be the true one after the last track (#2) was rejected at a distance of 1.89 meters at step 47.

Track # Length Step of elimin. 1 0.62 m 13 2 1.89 m 47 3 2.52 m (end) 82 (end) 4 0.77 m 20 5 0.77 m 20

Table 5.5. Statistics of the tracking experiment in ﬁgure 5.22. incorrect hypotheses after a 1.89 m path (table 5.5). The fact that the robot of track #1 and #4 is partially standing within an object is not a rejection criterion since algorithm 2 only falsifies hypotheses by geometric contradiction and because this kind of information is not contained in the map – feature maps are not free-space maps. The irregular distribution of path points in figure 5.22 visualizes the instants when the OS scheduler brought the localization task (a non-RT thread) into foreground. The average tracking time is about 10 ms per hypothesis. Note that free space information from an additional map (a grid map, for instance) could be seamlessly integrated as an additional criterion for track rejection.

99 5. GLOBAL EKF LOCALIZATION

5.8 Related Work

A more profound comparison to related work will help to reveal the advantages of the interpretation tree approach to localization. In order to highlight the differences, the discussion will focus on the shortcomings of the techniques cited in this section. It has to be said that the related methods are state-of-the-art in the field and proved all to be successful in practice. Firstly, in this work features are matched to features – not locations to locations (Jensfelt & Christensen 1999), (Austin & Jensfelt 2000). Secondly, opposed to other approaches using Gaussian hypotheses with features (Jensfelt & Christensen 1999), (Austin & Jensfelt 2000), (Roumeliotis & Bekey 2000), we do not associate features serially, one at a time, but consider the whole local map L with its binary constraints at once. This has several advantages: It substantially reduces the number of possible data associations since features must ad- ditionally satisfy mutual constraints from their geometric configuration (L ) in order to be admitted as valid data association members. An example illustrates the difference: Given a robot which observes a door and two walls, a global map containing, say, 50 lines and 30 doors and assuming that this particular geometric combination of a door and two walls is globally unique in the map. When considering each feature independently, the observed door will already produce 30 location hypotheses and each wall in L will give rise to 50 further hypotheses, making 130 possible locations. Our technique will directly generate the single globally unique pose being the only location from which the door and the two walls are seen as in the local map. A further advantage is with regard to non-Gaussian location densities, for instance, if the local map consist only in a single line, segment or ()xy, -point. The resulting distributions have the form of a diffuse line, a diffuse segment and a diffuse ring respectively. This is a common limitation of approaches with Gaussian hypotheses since such distributions are poorly described by the first two moments and require the use of higher-order statistics. With a single feature-to-feature scheme, this limitation is very sensible since each feature must allow for a full pose update. A full update is given if q ≥ 3 or, to be general, if ∇hT ∇h is non-singular (see section 5.4.2). Parallel lines, for example, have a q greater than three but yield an infinite number of solutions in the EIF by a singular expression ∇hT ∇h . The authors in (Austin & Jensfelt 2000) and (Roumeliotis & Bekey 2000) for example, do not consider the above feature types at all but only features which allow a full update. In (Jensfelt & Christensen 1999) the distinction between creative (∇hT ∇h is reg- ular) and supportive features (∇hT ∇h is singular) is made. For the generation of location hypothesis they can only use creative features. To overcome this drawback, they form high- level features with several supportive features which permit a full update. This, however,

100 5.8 RELATED WORK requires an individual treatment – in the EIF, for instance – of each high-level feature, that is, for each specific combination in type and number of supportive features. Our method is less sensitive to that limitation because it requires only that the whole local map Lh provides a non-singular ∇ T ∇h . This does not solve the problem (e.g. when L contains a single line observation) but it significantly reduces its importance. A comparison to Markov or particle filter-based localization is insightful as well: Gener- ation of hypothetical robot locations and incorporation of sensory information is decoupled in Markov or particle filter-based techniques. The filter framework is a sampling machinery in the problem space of the posterior probability density function p()x Z given a predictive density. Z encodes the knowledge of the initial state and all measurements up to the current time. The user hopes that the machinery generates (enough) particles around significant places of p()x Z . If the particle distribution strategy fails to put samples at locations of high likelihood, the algorithm has a hard time to recover from this, that is, to re- discover the forgotten locations containing possibly the true location. After a collision of the robot, wheel slippage or kidnapping, the distribution strategy based on the robot motion model is likely to miss the region around the true robot location. It has been reported that Monte Carlo localization (Dellaert, Fox et al. 1999) suffers from this shortcoming if the motion model is of poor predictive quality (Lenser & Veloso 2000). The capacity to correctly represent location ambiguity and provide globalness to localization cannot be guaranteed but is a matter of hope. Opposed to the initial attempt to be computationally less demanding than Markov techniques with grids, the user enters with particle filters a trade off between globalness and efficiency. The problem that particles are diffused “blindly”, not accounting for the information from the sensors was recognized and addressed in (Lenser & Veloso 2000), (Jensfelt, Austin et al. 2000), (Jensfelt, Wijk et al. 2000) and (Thrun, Fox et al. 2000). The authors in (Lenser & Veloso 2000) introduce sensor based resampling. Dependent on the average probability of the particle population, more or fewer samples are directly drawn from p()x z replac- ing samples from the posterior p()x Z . z denotes the sensory observation which has – interestingly – the form of features. With the same underlying idea, Jensfelt and colleagues (Jensfelt, Austin et al. 2000), (Jensfelt, Wijk et al. 2000) perform planned sampling with line, door and point features giving them each an individual p()x z . How many samples are taken from p()x z is specific for each feature type. Sampling from p()x z can be seen as the inversion of the problem and it seems that features are a key element in there. The concept that location hypotheses or particles are generated temporarily and causally after the sensor observations has been called feature-driven in the introduction of this chapter. The term can also be widened to sensor-driven since features are only a means to determine p()x z and not a instrinsically required element. As they do not use features but raw data, Thrun and friends (Thrun, Fox et al. 2000) build

101 5. GLOBAL EKF LOCALIZATION up a kd -tree representing the joint distribution p() z, x in order to sample from p()x z . The tree must be learned in a preprocessing phase using real robot data as a “sample” of z , and pz()x with randomly generated poses x to generate a weighted sample that represents pz(), x . The authors mention that it permits efficient sampling of the desired condi- tional density but details are omitted and it remains unclear what effort it takes to learn this tree and what resource requirements it has in a typical environment. The approach of this chapter – and the literature it relies on – enjoys a deterministic inverse formulation of the problem. Generation of possible robot locations is not a matter of random draws but the result of a closed-form process. Location generation and incorporation of sensory information is not only tightly coupled but coincide. The user has a guarantee on localization as long as sensor models are not heavily violated. Further, localization accuracy is not part of a trade-off. Global localization with the method of this chapter has the same sub-centimeter precision than its local variant from the previous chapter. In fact, there is no bound in precision from the algorithm but only from sensors and practical aspects as the precise knowledge of the sensor-to-robot displacement, for example. Accuracy, too, is not a result of random draws but follows from the formulation of the problem. We speak therefore of unbounded localization precision. In the other hand, particle filters have no problems to represent the non-Gaussian densities from the abovementioned single-feature observations which is a consequence of the non-parametric representation of probability distributions.

5.9 Conclusions

The problem of globally localizing a robot and robustly tracking its position has been addressed in this chapter. Following an interpretation tree approach initiated by several researchers before (Grimson & Lozano-Pérez 1987), (Drumheller 1987), (Castellanos, Tardos et al. 1996), (Lim & Leonard 2000), we have introduced the visibility constraint as a location dependent constraint which helps to limit the space to be explored during tree traversal. The problem of low feature discriminance of infinite lines and the fact that indoor environments exhibit typically a high degree of collinearity was the motivation to introduce multi-segment lines as features. Probabilistically they carry the information from the line, while the segments are taken for data association. To the initial algorithm for track formation, we have added a tracking algorithm which performs verification, falsification and track splitting under geometric constraints. Togeth- er with a track scoring strategy based on a non-Bayesian data association approach, the two algorithms form a framework for global feature-based EKF localization. The experiments suggest that geometric constraints for data association is a powerful de-

102 5.9 CONCLUSIONS cision mechanism which provides fast convergence to the true hypothesis at reasonable computational costs. Opposed to localization approached with non-parametric representation of probability densities, this approach provides a deterministic closed-form solution to the problem of location generation and permits a management of hypotheses which can be called optimal in the sense that there are always as many hypotheses as necessary and as few as possible. We have demonstrated that the approach remains practical in a real world embedded system implementation. Note that no answer is given whether hypothesis assessment by a full Bayesian approach to data association is better or worse than a non-Bayesian goodness-of-fit approach pursued here. The track splitting filter under geometric constraints as presented in section 5.5 works independently on this. A systematic study of this question shall be beyond the scope of this work and is an issue worth to be addressed in future work. Finally, path planning under global location ambiguity, commonly called active localization, is another direction from here. Literature on this topic is somewhat limited: (Guibas, Motwani et al. 1997) and (Dudek, Romanik et al. 1998) lay the theoretic fundament with localization guarantee but unrealistic assumptions on sensors and environment, whereas (Fox, Burgard et al. 1998b) and (Seiz, Jensfelt et al. 2000) present two greedy solutions without localization guarantee but well working experiments.

103 5. GLOBAL EKF LOCALIZATION

104 Chapter 6

From Localization to Navigation

6.1 Introduction

Navigation responds to three questions: “where am I?”, “where am I going?” and “how do I get there?”. A navigation framework has the task to offer the simplest possible interface to these questions, hiding their complexity to the user or the application layer. So far we have given an answer to the “where am I?” question which is the localization problem. In this chapter the remaining elements and the architecture which provides their integration are discussed. At the inside of the framework, navigation deals with various constraints on different time scales and levels of abstraction. A common approach to structure the problem is a three- layered architecture which consists in a planning layer, an execution layer and a reactive behavior layer (Bonasso, Firby et al. 1997), (Simmons, Goodwin et al. 1997) (figure 6.1). • The planning layer decides how to achieve high-level goals using a model of the environment. Planning takes places under constraints of task-specific cost functions and limited resources. • The execution layer subdivides a plan into executable subplans, activates and deacti- vates behaviors and supervises their completion. • The reactive behavior layer interfaces the robot’s sensors and actuators. In mobile applications, it acts as a position controller under constraints of a dynamic environment, the robot shape, vehicle kinematics and dynamics. Controls, abstracted sensor data and status information flow vertically between layers. Typically, controls such as plans and subplans are passed to lower levels and information such as status codes and termination flags are passed to higher levels. In case of failures in a layer (e.g. path blocked), requests for revised controls are sent to higher layers (e.g. re- planned path). Time scale and abstraction increase from bottom to top, model fidelity and real-time concerns increase from top to bottom. We adopt this three-layered architecture here as it accommodates deliberative and reactive behaviors, allows for constraint distribution over the layers and embodies an intuitive

105 6. FROM LOCALIZATION TO NAVIGATION

user

environ. model planning

execution

reactive behavior environment

control

Figure 6.1. The layered navigation framework way of increasing abstraction from bottom to top. It is further suitable for applications with manipulators and multiple robots (Bonasso, Firby et al. 1997), (Simmons, Goodwin et al. 1997), (Simmons, Smith et al. 2002).

6.2 Problem Statement

The Swiss National Exhibition takes place once in 40 years. The latest edition, Expo.02, went from May 15 to October 20, 2002. It was a major national happening with 37 exhibitions and a rich event program. The exhibition Robotics was intended to show the increasing closeness between man and robot technology. The central visitor experience of Robotics was the interaction with ten autonomous, freely navigating mobile robots on a surface of about 315 m2 . Their task includes tour giving, entertaining and picture taking of visitors. They share the same space and the same goals for the tours and operate in an unmodified environment. The exhibition is scheduled for five hundred persons per hour. For this task, the main specifications can be summarized as follows: • Navigation in unmodified, highly populated environments with visitors and other freely navigating robots • Bidirectional multi-modal interaction using easy-to-use, intuitive yet robot-typical interaction modalities. Speech output in four languages: French, German, Italian and English • Safety for visitors and robots at all time. • Reliable operation up to twelve hours per day, seven days per week, non-stop during 159 days • Minimal manual intervention and supervision

106 6.3 THE NAVIGATION FRAMEWORK

• Adaptive multi-robot coordination scenarios in function of the number of visitors and their interests • Development of ten robots within tight budgets and schedules In this chapter we present how the architecture was adapted to suit the needs from this application and the choices we made for its components. Further, operating experience with this framework is discussed.

6.3 The Navigation Framework

Today we (still) face limited computational resources in embedded systems for real-time. While radio-linked off-board hardware might be an alternative for a single robot, it would result in prohibitive bandwidth costs for multiple robots, if, for instance, raw sensor data for localization were transmitted. As we want the robots to be truly autonomous, we look out for computational efficiency and compact representations.

6.3.1 Environment Model

Our approach to environment modeling is feature-based using the geometric primitives multi-segment lines and points. The environment topology is encoded in a weighted directed graph with nodes and edges between the nodes. Neither for global path planning nor for localization we use a free space model like occupancy grids. The advantage of this choice is compactness: in indoor environments, a map of this type (features plus graph) requires typically around 30 bytes per m2 . Further, scaling to 3d is polynomial, whereas freespace models such as grid maps scale exponentially. The graph has two types of nodes: station nodes and via nodes. Station nodes correspond to application-specific ()xy,,θ -locations in space with a meaning. Examples from Ex- po.02 include: showcase with industrial robot, tour welcome point or location to trigger picture caption. Via nodes have two tasks. First, they correspond to topology-relevant locations like doors or corridor-crossings. Thereby the graph models the environment topology. Second, in environments with large open spaces, they further provide topological redundancy by locations with favorable traversability. Favorable, for instance, with respect to visitor flow criteria or other specific requirements from the application.

The map further contains so called ghost points. Ghost points are ()xy, -positions in the world reference frame which act as invisible barriers. If the environment contains forbid- den areas undetectable for the robot’s sensors (e.g. staircases, glass doors, exits, etc.) ghost points are used to prevent the robot to go there by injecting them into the sensor data as

107 6. FROM LOCALIZATION TO NAVIGATION

-5

-10

-15 0 5 10 15 20 Figure 6.2. The Expo.02 map virtual readings. See also section 6.3.4 and (Fox, Burgard et al. 1998a).

The Expo.02 environment covers a surface of 315 m2 and has 15 places of interest for the robots. The map contains 44 segments on 44 lines, 15 station nodes, 37 via nodes and 20 ghost points (figure 6.2). Its exact memory requirement is 8 kbytes or 26 bytes per m2 . The ghost points (not shown in figure 6.2) are at the entrance (bottom of figure 6.2) and the exit (top) of the circulation area.

6.3.2 Global Path Planning

With a topological graph, global path planning becomes a graph search problem for which a multitude of algorithms exist. From simple depth-first search with fixed costs to dynamic programming techniques and probabilistically learned cost functions for edge traversability (Hu & Brady 1997). Global path planning in our case uses a priority-first search (Sedgewick 1988) and costs assigned to edges and nodes (Tschichold-Gürman, Vestli et al. 1999). In a single-robot context costs are fixed, for multi-robot planning the costs of nodes are variable and depend on the distance to other robots. In the multi-robot case, besides paths, goals are shared as well, and must be negotiated among the robots. See section 6.3.5. At Expo.02 we give visitors the opportunity to choose their next station of a tour by them- selves. This closes the first loop to the environment which is asynchronous and has a cycle

108 6.3 THE NAVIGATION FRAMEWORK time in the order of 0.01 Hz (figure 6.5). Path planning in the graph of figure 6.2 is a matter of a few milliseconds in our implementation.

6.3.3 Command Queue

Paths from the global planner consist in a list of nodes. In the execution layer, a command queue passes the list to the behavior layer in a node-by-node manner. For via nodes it ac- tives a ()xy, -position-only variant of the obstacle avoidance, in case of the last list node, the full pose ()xy,,θ must be met. Near the last node in the list (typically a station node), localization gets deactivated (10 cm in our implementation). This is because localization causes the robot pose estimates to be noisy such that for any position controller the goal cannot be reached. The reached condition for via nodes is also treated on this level. It is satisfied when the robot enters a large disk around the node (2 m in our implementation).

6.3.4 Local Path Planning and Obstacle Avoidance

For mobile robots, the primary role of the reactive layer is that of a position controller. A number of constraints must be accounted for on this level: vehicle shape, vehicle kinematics, vehicle dynamics and, of course, environment dynamics which in the case of a mass exhibition is extensive. Since purely reactive obstacle avoidance methods usually suffer from local minima problems, we divide the task into a reactive and a path planning sublayer (Brock & Khatib 1999), (Schlegel 1998), (Arras, Persson et al. 2002). For the reactive sublayer we rely on the idea of the dynamic window approach (DWA) (Fox, Burgard et al. 1997). The method uses a simple model of the vehicle dynamics (maximal acceleration / deceleration) and can – with the appropriate extension – take into account an arbitrary robot shape. In comparison to the original version of the DWA our approach differs in the following points1: • Working with differential drive robots, the objective function trading off speed,

heading and clearance are calculated in the actuator phase space ()vl , vr instead of the Cartesian ()vw, -space. This models the acceleration limits of the vehicle physically more properly. • As in (Schlegel 1998) and (Arras, Persson et al. 2002), we account for polygonal robot shapes. The robot shape is not hard-coded in our implementation and can be specified at boot time. • Instead of using the distance to collision as a clearance measure, we use time to col-

1. Obstacle avoidance as presented here is not work of the author. Please refer to (Philippsen & Siegwart 2003).

109 6. FROM LOCALIZATION TO NAVIGATION

a) b)

robots

Figure 6.3. Two situations from Expo.02 which show how the elastic band ﬁnds a smooth path around people. In a) there are two robots virtually blown up with ghost points.

lision. This solves a singularity when turning on the spot (any collision would seem instantaneous because the distance travelled seems zero). It also means the robot will choose more clearance when travelling at higher speeds. • Ghost points from the global map are taken into account. After a global-to-local transform they are injected as virtual sensor readings. The dynamic window is part of the time- and safety-critical software of our robot. We therefore install this process as a deadline-driven real time task with a 10 Hz frequency. Special attention was paid to optimize its execution time to be short and predictable. For reasons similar to those mentioned in (Schlegel 1998), we use a look-up table for the clearance measure. The second sublayer is a path planner which operates locally as it relies on sensory data without memory. A modified elastic band (Quinlan & Khatib 1993) is employed which generates smooth trajectories around obstacles (figure 6.3) and uses an NF1 navigation function (Lamon, Nourbakhsh et al. 2001) for initialization. Although the NF1 always yields topologically correct solutions (within its scope and if a solution exists), it generates unsmooth trajectories with the tendency to graze obstacles. With the elastic band, The initial NF1 plan continuously evolves towards a smoother curve. Updates of the band are implemented in a non-time critical thread which runs at several Hz. As soon as the elastic band “snaps”, replanning is initiated. At Expo.02, this takes place typically in the order of 0.1 Hz. For path planning and evolution of the elastic band, the robot is assumed to be circular and heuristics are used to ignore some sensor readings. This results in simplified and speed- up implementations. The simplifications are acceptable because the DWA ensures the dynamic, kinematic, and geometrical constraints. At the lowest level finally, the speed controller, also installed as a real-time task, runs at a 1 kHz frequency (figure 6.5).

110 6.3 THE NAVIGATION FRAMEWORK

6.3.5 Multi-Robot Planning

For multi-robot planning we distinguish goal coordination and path coordination. For the paths, environment dynamics from visitors is important. Visitors who play with a robot easily deviate the vehicle from its path and provoke path collisions where at planning time no collision had occurred. We therefore face the problem of replanning paths for all robots on-the-fly. This has to happen in real-time since we do not want the robots to stop and wait each time we detect a path deviation somewhere. For this we employ a topological potential field approach where graph nodes receive costs proportional to the distance to a robot. Each robot assigns weights to those nodes which are in its current plan. The resulting graph superimposes the weights of all robots. Using this graph, the global planner delivers cost-optimal paths that contain nodes which are currently unused by other robots. Theoretically, there is no guarantee on collision freeness with potential fields. One can construct (pathological) situations where it is cheaper for two robots to use the same node at the same time. More sophisticated methods (Lamon, Nourbakhsh et al. 2001) can provide a guarantee but are computationally unfeasible in our application. The advantage of this technique is its efficiency. It enables multiple robots to adapt their plans quickly and requires a minimum of shared information: a list of elements ()iw, i , with i being the node identifier and wi its weight – a matter of a few bytes. Coordination of the goals is necessary since a limited number of shared goal locations is to be allocated to multiple robots. Path coordination cannot avoid that several robots choose simultaneously the same station node since robots with the same goal would, due to the lack of redundancy, insist to go there regardless the costs. To give visitors the choice of their next tour station the robot makes a proposition which is based on the currently unoccupied stations, the list of stations included in the tour and the stations already visited. The selected station is then reserved for this robot. More details on goal negotiation and implications for visitor flow can be found in (Jensen, Froidevaux et al. 2002).

6.3.5.1 The Robot-Sees-Robot Problem Multi-robot coordination so far presented, depends on the knowledge of the robot positions. Even with a reliable and accurate localization, this creates critical interdependencies. Ro- bots shall therefore be able to see each other on a raw data level. This, however, is not straight-forward with platforms of the same mechanical design which all measure at the same height since, at this height, the true vehicle size will be underestimated from the sensor readings.

111 6. FROM LOCALIZATION TO NAVIGATION

Our approach is to mount two retro-reflecting beacons onto the vertical profiles in the blind zone between the two laser range finders. The Sick LMS 200 provides an intensity signal which allows to easily extract the reflector information (see chapter 2). We then use ghost points to artificially create a virtual robot contour at the extracted reflector positions (see also figure 6.3). Thus, obstacle avoidance provides an anytime fall-back solution.

6.3.6 Localization

Localization is a key element in a navigation framework and localizing a robot robustly in a mass exhibition can certainly be called a challenge. In former exhibition projects, with the museum robots Rhino (Burgard, Cremers et al. 1999) and Minerva (Thrun, Beetz et al. 2000), localization was based on off-board hardware due to the computational demand of the employed Markov localization approach (Fox, Burgard et al. 1999). The robot Kapros (Maeyama, Yuta et al. 2000) served as an avatar for remote visitors of an art museum during two weeks. Unlike these short-term projects, (Nourbakhsh, Bodenage et al. 1999) describe the permanent installation of the autonomous tour-guide robot Sage in a museum of natural history which gave rise to three successor installations. The environment was modified with color patches in order to facilitate localization. A multi-robot installation which is operational since March 2000 is presented in (Schraft, Graf et al. 2001). The three museum robots do EKF pose tracking with features. Their environment is well structured and exhibit a medium extent of environment dynamics. Finally, the 72-robot installation at the World Fair Expo 2000 in Hannover, Germany, was the first application of mobile “artefacts” in a mass exhibition. The vehicles were very low-tech. Localized and controlled from a (very expensive) external infrastructure, the served as freely moving swarm entities forming a hugh interactive art installation during the six months of Expo 2000 (there is no publication to the knowledge of the authors). At Expo.02, localization had to be done in an unmodified mass exhibition environment on fully autonomous robots. For this, an earlier version of the framework for global localization described in chapter 5 has been implemented. In this version, the framework is tuned to stay always in cycle nr. 3 of the flow diagram in figure 5.17 which is basically a single-hypothesis tracking mode. When the robot is lost, hypothesis generation is manually activated. The predominant requirements on localization reliability and the plus of experience we had in single-hypothesis tracking lead to this conservative attitude. For line extraction we use the method of chapter 3. Extraction times are around 20 ms. Localization was implemented as a non-RT thread. Its cycle time was slowed down to 2 Hz in favor of other concurrent non-RT processes (e.g. motion planning, communication). With a single hypothesis to be tracked, cycle times of 10 Hz and more are achievable on the Robox hardware. Figure 6.4 shows two examples of hypothesis generation in the Expo.02

112 6.3 THE NAVIGATION FRAMEWORK

10 10

8 8 202 203 6 6 204

4 4

2 2 202 205 0 0 201

-2 -2

-4 203 -4 201

206 -6 -6

-8 -8 a) b) -10 -10 -10 -8 -6 -4 -2 0 2 4 6 8 10 -10 -8 -6 -4 -2 0 2 4 6 8 10

t = 105 ms t = 446 ms

10 10

5 5

0 0

-5 -5

-10 -10

a) b) -15 -15 0 5 10 15 20 0 5 10 15 20

a) b) Figure 6.4. Global EKF localization at Expo.02. Given the local maps in a) and b), hypotheses are generated at locations where the local map geometrically “ﬁts” into the global map. In a) there are 8 hypotheses; the location is ambiguous. In b) there is a single hypothesis; the robot is instantaneously localized. “t” denotes the execution time. The experiments were carried out at the positions shown below.

113 6. FROM LOCALIZATION TO NAVIGATION

user other robots graph search environ. 1 ~0.01 Hz model command queue

2 ~0.1 Hz elastic environment band

3 modified 10 Hz DWA

4 1 kHz speed control

Figure 6.5. Layering and control loops with their precise (3, 4) and typical (1, 2) cycle time. environment. The marked hypothesis in figure 6.4.a) is the only one with three paired observations (p′ = 3 , p∗ = 0 ), while the seven other hypotheses have only two pairings and a star-branch match (p′ = 2 , p∗ = 1 ). Finally, figure 6.5 shows the resulting decomposition of the architecture, the four control loops and their cycle times. The DWA sublayer requires the position of the ghost points which explains its connection to the environment model. Localization can be seen as a behavior as it is switched by the execution layer and connected to the robot’s sensors. It is however invisible since there is no direct connection to other components or behaviors. It acts in the background continuously correcting the odometry. Thereby, all other framework components can consider odometry as being “perfect”. For multi-robot coordination, robots are connected on the level of the planning layer.

6.4 The Robot

In view of the requirements formulated in section 6.2 and based on former experience in robot design and system integration, it was concluded that building a robot from scratch is the best to do. The main arguments were • off-the-shelf hardware is not flexible. Mechanical, electrical and software-related extensions are hard to interface with commercial systems • few commercially available mobile platforms run per default a hard real-time operating system which we consider as crucial • high fluctuation of robot companies

114 6.5 OPERATION EXPERIENCE AND DISCUSSION

Figure 6.6. The ten robots at the charging stations over night. At the right-hand side of ﬁgure 6.2 there are the ten nodes for the charging stations.

The outcome, Robox, is described in appendix A and shown in figure 6.7. For navigation, a PowerPC G3 at 380 MHz serves as main CPU running the RT operating system XO/2. The robot is made for fully autonomous operation with one exception at Expo.02 where it relied on off-board hardware: a central server for multi-robot planning. The approach described in section 6.3.5 does not require this at all but with the implementation using HTTP, opening a connection took more time than data transmission itself. The central unit allowed us to optimize latencies and transmission times – see also (Jensen, Froidevaux et al. 2002).

6.5 Operation Experience and Discussion

During Expo.02 (May 15th–October 20th, 2002), 686’405 persons visited the “Robotics” exhibition (4317 per day, around 400 per hour). Assuming an average visit duration of 15 minutes (typical for mass exhibitions), around 100 persons share the 315 m2 circulation area with ten robots. In other words, the robots encounter environment dynamics similar to a railway station at rush-hour, mostly benign but also hostile (people who run into, play with, try to outwit or kick the robots). The overall travel distance was 3,316 km during an overall operation time of 13,313 hours. Safety. It was never observed that a robot was the cause of a dangerous situation e.g. with small children, elderly or handicapped people (fig. 6.7). The lack of additional sensors close above floor (IR or ultrasonic) is easily bearable with the combination of tactile plates and soft bumpers. Blocked-situations due to bumper contact were also handled by the interactive part (robot expressing friendly menaces). Further, there was almost no vandalism. Visitors kicked the bumpers regularly and touched the (unprotected) two pan-tilt eyes of the face but never caused a real damage. We

115 6. FROM LOCALIZATION TO NAVIGATION believe that this has two reasons: first, people in mass exhibitions are always supervised (independent whether there are robots). With the pavilion staff members in the circulation area of the robots, visitors never felt unobserved. Second, during the design of Robox we complied with the rule in industrial design which states that objects tell us by their look how they want to be treated. The bottom part of the Robox makes a stable impression. It says “you can kick me”, and people kicked it. The face has a fragile appearance which says “take care”, and people were cautious when they touched it. Reliability. The division of obstacle avoidance into a purely reactive part with high model fidelity and a planning part is a powerful conjunction. Very often, groups of visitors form a U-shaped obstacle or leave only small “holes” for passage. The robots have no difficulty to escape from such situations, and due to the fact that we account for Robox’ true octagonal shape, narrow passages are efficiently used (fig. 6.3). Further, the elastic band generates smooth and good-looking trajectories. No problems with mutually interfering laser scanners from different robots have been observed. We were surprised by the reliability of localization in view of the environment dynamics and the fact that we used laser data only. A fall-back solution with lamp features extracted from a camera looking to the ceiling has been prepared but had not to be employed. How- ever, lost situations occurred for two main reasons: staff members that push/rotate the robot in order to untangle congestions of robots or visitors and robots (see below), and visitors imitating this behavior. Global localization as illustrated in fig. 6.4 was then useful since it allowed in most cases to instantaneously relocalize the robot within all people, enabling it to resume operation. The geometry of the Expo.02 environment was helpful here since it contained little symmetry. We believe that the use of geometric features for navigation is a very appropriate choice for localization, particularly for highly dynamic environments. During feature extraction, sensor readings are filtered out that do not satisfy the spatial model assumption from the feature (lines in our case). Thereby, the extraction process says which reading is to be taken for localization and which one is to be ignored. This filter relies typically on sound regression techniques and works independently on whether the robot is localized or not. People standing in front of the robot do not produce evidence for the line extraction since legs are not linear or too short (see also chapter 3). Spurious segments, for example by line-like objects carried by people, do occur and are treated by the star-branch in the localization algorithm. The environment contained walls which differ by less than five degrees in angle (bottom wall in fig. 6.2). Line extraction had to be tuned for a slight tendency to oversegmentation in order to correctly detect the true wall segments. Otherwise the observed line segments would have been too long and the unary constraint testing on the segment length would have prevented their match.

116 6.5 OPERATION EXPERIENCE AND DISCUSSION

a) b)

c) d)

e) f)

Figure 6.7. Scenes from Expo.02: a) a small group interacting with Robox, b) a compact group of visitors (~30) and robots (four), making it difﬁcult to navigate, c) a children on the ﬂoor, possibly invisible to the laserscanners but apparently with good faith in the robot, d) a group of people listening to Robox and completely blocking its path, e) a blind visitor interacting with Robox, f) the queue in front of the Robotics pavilion.

However, path-coordination (section 6.3.5) was not always available and the robots relied on the fall-back solution of section 6.3.5.1 during this period1. But the concept for the robot-sees-robot problem turned out to be an oversimplification. The employed model (blow- ing up a virtual contour at the detected reflector position) caused the robots occasionally to block each other. This happened when the reflectors of two nearby robots were occluded (e.g. by visitors) and became suddenly visible (when they moved away) causing the virtual contours to overlap and to be within the hull of the other robot. The motion planner then stopped both vehicles and pavilion staff members had to intervene. By using a switch dis- connecting the motors from the amplifiers, the 115 kg robot can be easily pulled away (such an intervention took a few seconds). However, wrong manipulation of the switch made the robot slip and caused unmodeled odometry errors of such an extent that the robot often

1. There was a software issue. The author of the path-coordination software left the team short after opening of the exhibition for a backpack trekking in South America.

117 6. FROM LOCALIZATION TO NAVIGATION went lost during such interventions (robot kidnapping). Multi-hypothesis tracking presented in chapter 5 aims at that problem. But recovering from robot kidnapping is hard. In the experiments which have been carried out at Expo.02, no solution was found to work reliable enough. The main problem is to reliably decide when in the set of locally generated hypotheses the robot has to jump to which hypothesis. The question persists and motivates future work in this direction. Precision. Localization accuracy varies and depends on how much persons occlude the sensors and how much segments have been paired. Goal nodes were typically reached with a relative error less than or equal to one centimeter. This was measured by the traces on the floor at goal locations the robots left after several months of operation.

6.6 Conclusions

On 3,316 km travel distance, the framework and its components as presented in this paper met the requirements of safe and reliable operation in a highly dynamic environment very well. To the knowledge of the authors, this project was the world-wide biggest installation of interactive autonomous robots so far. Being the first one, we believe that it will not remain the last robot exhibition of its kind. Robots in exhibitions are a young and relatively new field of application which is expected to have a limited yet significant impact in the future (Arras & Burgard 2002). With this pioneering event, we have shown that the deployment of autonomous, freely navigating robots in a mass exhibition is feasible. We have also demonstrated that the feature-based approach to localization is an adequate choice for highly dynamic environments. This has been showed in former work with two museum robot installations (Burgard, Cremers et al. 1999), (Thrun, Beetz et al. 2000) for the Markov localization approach. From its application at Expo.02, we conclude that the feature-based approach allows to be robust and precise, retaining its efficiency also in the “globalized” form of chapter 5. Finally, a mass exhibition with robots is not the experimental platform as we initially hoped. The pavilion was open between ten and a half and twelve hours per day, non-stop during 159 days. In the evening batteries were empty and had to be charged for the next morning. All experiments had to be done under daytime conditions, that is, within uncon- trollable visitor dynamics. Contracts did not allow to reduce or even close the exhibition to suit the needs of a research laboratory. Partly as a consequence, the robots at Expo.02 still required manual intervention. The main reason for this are the described problems on the level of the robot collective. On a single-robot basis the robots were significantly more reliable. But the sheer mass of people on a relatively small surface expose and amplify the slightest insufficiency in robot hardware and software.

118 6.6 CONCLUSIONS

The problems encountered reveal relevant directions for future research: More multi-hypothesis tracking experiments under controlled conditions to address the robot kidnapping problem and refined models of visitors and robots for multi-robot coordination in populated environments.

119 6. FROM LOCALIZATION TO NAVIGATION

120 Chapter 7

SLAM with Relative Features

7.1 Introduction

Up to now we have assumed an a priori known map of the environment. Hand-made maps or maps taken from existing plans or CAD models have important drawbacks: (i) percep- tual incompatibility: acquisition and operation is done with different sensory systems, (ii) changing environments: maps are to be keep up-to-date which can be laborious, (iii) effort and time: measuring an environment manually can be a time-consuming and costly procedure and (iv) imprecision of buildings with respect to existing plans. It is therefore desirable that the robot learns its environment autonomously. For the purpose of map building, three elementary questions need an answer: • How is the world and the robot represented ? • Which formalism for integrating new information is employed ? • Where to acquire new information ? Several world representations are in use which are best separated by their degree of abstraction: From raw data maps with no abstraction, to metric maps with features and their spatial dependencies, over topological maps holding relationships between locally distinct places up to semantic high-level descriptions of the world. For the purpose of compact environment models and precise navigation, metric, feature- based maps are a wise choice and will be employed here. This leads to the problem of the representation and management of uncertain spatial relationships, constituting a globally referenced stochastic map according to (Smith, Self et al. 1990) and (Moutarlier & Chatila 1989). In the stochastic map, all features are referenced in a common world frame. The robot and the map entities are modeled as uncertain spatial relationships by estimating their first two moments in that frame. They are stacked into a growing system state vector x and a growing system covariance matrix C . The matrix C has a block-wise organization where the robot and each individual map feature has its associated covariance matrix in the diagonal. An important point is that C is not block-wise diagonal but that the off-diagonal matrices encode the cross-covariances between the features and between the robot and the features. It is important to maintain these cross-correlations as demonstrated in (Hébert,

121 7. SLAM WITH RELATIVE FEATURES

Betgé-Brezetz et al. 1995) and (Castellanos, Tardos et al. 1997). When inter-feature and robot-to-feature correlations are neglected, phenomena early leading to inconsistencies in the world model can be observed. However, simultaneous localization and map building (SLAM) is the problem of estimating absolute relationships by the uncertain observation of relative relationships. And also with a full-covariance variant of the stochastic map, the question arises whether by repeated reobservation of features, the map state vector converges to the truth. This has been at- tacked by (Dissanayake, Newman et al. 2001) where a convergence proof for linear observation models could have been conducted. However, due to the orientation term of the vehicle pose, there is no feature with a linear observation model in practice and the question remains open whether the linearization of non-linear measurement models prevents convergence. Non-linearity is an issue also raised by (Hébert, Betgé-Brezetz et al. 1995) which point out that even when maintaining cross-correlations is not the solution to all problems. The approach initially proposed in (Smith, Self et al. 1990) has been shown to be sensitive to bias due to incorrect error models and inadequate error propagation (Moutarlier & Chatila 1989). Several propositions have been made to alleviate these difficulties, including suboptimal schemes (Leonard & Durrant-Whyte 1992), (Moutarlier & Chatila 1989), or improved techniques for error propagation in nonlinear systems (Julier, Uhlmann et al. 1995). But the principle problem of the stochastic map framework in practice is that it assumes no systematic errors injected, for instance, from odometry. After an exploration path of arbitrary length, the validation gates for matching are still assumed to be statistically correct. But a badly calibrated odometry is sufficient to make the filter diverge and so to render the map useless. Robustness with respect to bias and non-Gaussian errors is weak and represent today a bottleneck for feature-based SLAM. The problem, however, did not remain unno- ticed and was identified as a consequence of insufficiencies in data association. As pointed out by (Cox & Leonard 1994) and recognized in chapter 5, a model for origin uncertainty of measurements is mandatory. Topological consistency (the closing-the-loop problem) cannot be guaranteed in general as long as data association in SLAM is ignored. Recent work (Neira & Tardos 2000), (Leonard, Newman et al. 2001), (Nebot, Masson et al. 2003) propose different approaches to a solution: A hybrid scheme is pursued in (Nebot, Masson et al. 2003) where a particle filter and stochastic map is combined. The particle filter solves the “global” issues by proposing candidate locations for a topologically correct integration of local maps, particularly when closing a loop. Leonard and colleagues (Leonard, New- man et al. 2001) augment the map state vector with past states in order to perform a delayed decision making on several data association histories. (Neira & Tardos 2000) apply the interpretation tree in a stochastic map framework. As it allows for global localization alike chapter 5 it is able to provide candidate locations for the same purpose than in (Nebot, Mas- son et al. 2003). By maintaining a co-visibility information among the features, the authors

122 7.2 FEATURE REPRESENTATION, REVISITED show in (Neira, Tardos et al. 2003) that the tree search of initially exponential complexity can be reduced to linear complexity – a relevant result for big and feature-rich maps.

The growing state vector x and covariance C in big maps and the computational implications have also been subject of several works. (Guivant & Nebot 2001) update the map only partially, in a local region around the robot, and can later propagate the information of the updated region to the rest of the map. (Leonard & Feder 2001) perform decoupled mapping where the map is subdivided into several smaller maps. Care has to be taken during map transitions that estimations remain rigorous. The abovementioned problems and remedies are subject of on-going research and can be assigned to the second of the three elementary questions initially raised. Approaching the problem from the viewpoint of the third issue, that is, by means of adequate exploration strategies, is also an alternative which has been investigated. Frequency and succession of observations and re-observations of features play a role for system convergence especially in the presence of bias. Related work (Dudek, Freedman et al. 1996b), (Lee & Recce 1997), (Yamauchi, Schultz et al. 1998), (Wullschleger, Arras et al. 1999), (Feder, Leonard et al. 1999) investigates different exploration strategies and implementations. The work of (Lee & Recce 1997) is highlighted since the authors define map quality measures in order to compare the benefit of different exploration strategies. Map quality measures are expected to be of interest for the management of multiple map hypotheses. Multiple map hypotheses from multiple data associations are one way to address the guarantee of topological consistency. Map quality measures could play the role of a hypothesis scoring strategy when multiple maps are compared. Many researchers employ simple geometric primitives without dimension as features for SLAM (e.g. ()xy, -point). But more complex, possible composite features with a physical extension would be much more appropriate for indoor navigation. According to the work with multi-segment lines in the previous chapters we will address this problem and present an approach which allows to describe complex features by a location and set of relative entities. We will also show that with the concept of referencing features relative to features in the map, the resulting maps can be made smaller with respect to outcomes reported in the literature.

7.2 Feature Representation, Revisited

In chapter 4 we have used vertical and horizontal infinite lines as features. They allow to model collinearity in indoor environments and yield very compact representations. How- ever, infinite lines suffer from weak mutual discriminance. In chapter 5, this model was extended by the introduction of multi-segment lines. Multi-segment lines still carry the probabilistic line-information and overcome discriminance problems as they take advan-

123 7. SLAM WITH RELATIVE FEATURES tage from segment information for data association. In both chapters the assumption was made that lines in the map have no angular uncertainty due to incorrect error propagation in frame transforms of the Hessian line model. It was argued that in “perpendicular” office environments this assumption holds very well. However, the supposition of disappearing angular uncertainty can definitively not be held anymore for map building. We are therefore forced to look out for a different way to represent lines, still one of the most important features for indoor.

7.2.1 The SPmodel

The symmetries and perturbation model (SPmodel) (Tardos 1992) offers a unified way to represent uncertain geometric entities. For the motivation of the SPmodel we follow the argumentation of (Castellanos & Tardos 1999): • When using different geometric models for different features, frame transformations, parameter estimation and feature-to-feature comparisons are different in each case and can become quite complex. • Many feature representations have singularity problems. Examples include the slope-intercept model for lines which cannot properly represent vertical lines. By switching models as a function of the parameter values, the problem can be worked around but is not solved. Covariances tend to be infinite and numerical precision decreases near singularities. • Some models are overparametrized. In overparametrized representations, certain parametes can have an unclear meaning and remain undetermined, giving infinite covariance values. Or the set of parameters must satisfy one or several (possibly nonlinear) constraints making least squares fitting difficult and yielding also singular covariance matrices. • Error propagation results depend on the location of the absolute reference frame. This is the abovementioned problem with the Hessian line model where a small uncertainty in the line orientation gives rise to a very big variance in radius if the line is transformed to a location far from the frame origin. Such representations make it impossible to compare uncertainty values. If the feature in use exhibits none of the above problems, which is the case, for instance, with ()xy, -points and ()xy,,θ -corners, and if only a single feature of such a type is employed, the disadvantages of the SPmodel dominate: a bulky notation and less efficient expressions (the “generalization overhead”). If multiple features are employed – like in this work with lines, vertical edges and beacons – or one of the feature shows the mentioned representation problems, the SPmodel is an appropriate formalism by its generality and

124 7.3 FEATURES AS RELATIVE ENTITIES

Figure 7.1. Three examples of composite features with physical extension represented by endpoints relative to the location reference frame: convex corner with two segments, multiple segments on a line, arc segment on a circle. concept of local errors. We will therefore adopt the SPmodel for this chapter. A brief introduction to the SPmodel can be found in appendix D, a complete discussion in (Castellanos & Tardos 1999).

7.3 Features as Relative Entities

We define relative entities or relative features as locations which are expressed in the reference frame of features in the map state vector. The map feature whose attached frame serves as the base frame for the relative entity is called anchor feature. The benefit of representing locations relatively for SLAM is twofold: to separate information for estimation and data association and not to burden the map state vector with the latter. Second, to have a general and consistent means to represent more complex features. Features with a physical extension (e.g. wall segments, fluorescent lamps, circular structures, corners with adjacent segments) carry information for estimation by their uncertain location and information for data association by their dimension (figure 7.1). The latter adds an unary and an extension constraint. By using entities which are expressed relative to the location, we strive for a generalization of the representation of such features without affecting the size of the map state vector and with a gain of consistency in the description of these structures. This gain is particularly sensible if the extension of the feature is more complex with, for example, several segments on a supporting line. The concept of relative features allows us to reformulate multi-segment lines from chapter 5 as SPmodel entities.

7.3.1 Multi-Segment Lines

Segments in the SPmodel are described by a tuple

125 7. SLAM WITH RELATIVE FEATURES

{}E11, {}E21, {}M {}E12, xME11, xME21, xME12, {}E22,

xME22,

xWM y

{}W x

Figure 7.2. Multi-segment line feature

S = ()LWS, lS (7.1) ˆ 2 with L = ()xˆ ,,pˆ C ,B and l = ()lS, σ . The self-binding matrix of a WS WS S S S S lS segment is those of an infinite line:

= 010 (7.2) BS 001

The reference frame S is attached to the center of the segment S . Infinite lines are completely described by the uncertain location LWS without a length lS . Opposed to this, and analogous to the definition in chapter 5, a multi-segment line M is defined as a tuple

M = (), {}(), nS (7.3) LWM LME1 LME2 i i = 1 with

LWM = ()xˆ WM,,pˆ M CM ,BM (7.4)

= 010 (7.5) BM 001 and a relative segment endpoint E which is defined by an uncertain location

LME = ()xˆ ME,,pˆ E CE ,BE (7.6)

BE = 100. (7.7) The reference frame of relative segment endpoints is attached to the endpoint with the x - axis aligned to the supporting line. A multi-segment line can carry an arbitrary number nS

126 7.3 FEATURES AS RELATIVE ENTITIES

{}E11, {}E21,

{}E12 {}A , {}E22, xWA x {}W AEij,

Figure 7.3. Multi-segment arc feature of segments, each one described by the two locations , relative to the at- x ME1, i x ME2,i tached reference frame Mn (figure 7.2). A single segment is expressed by S = 1 . The binding matrix BE expresses the fact that the endpoints carry location information only along the supporting line. As argued in chapter 5, this information is not absolute but relative which is sufficient to calculate the corresponding segment length lS regardless the location of the M along the symmetry of the supporting line. According to the error model of segment endpoints in chapter 5, the endpoint is obtained by intersection of a ray with an uncertain angle with the supporting line. This is the generalized case of the intersection of two lines in the SPmodel which is derived further below.

7.3.2 Multi-Segment Arcs

For illustration and, with the same concept, we define the multi-segment arc A as a triple

A = (),,{}(), nA (7.8) LWA rA LAE1 LAE2 i i = 1 with

LWA = ()xˆ WA,,pˆ A CA ,BA (7.9)

= 100 (7.10) BA 010

2 r = ()rˆ , σ (7.11) A A rA

127 7. SLAM WITH RELATIVE FEATURES to describe a circle by a set of circular arcs. The relative arc endpoint E is defined as an uncertain location

LAE = ()xˆ AE,,pˆ E CE ,BE (7.12)

BE = 001. (7.13)

A multi-segment arc can carry an arbitrary number nA of arc segments, each one described by the two locations relative to the reference frame attached to the circle x AE1 , x AE2 A center (figure 7.3). A single segment is expressed by nA = 1 . The reference frame of relative arc endpoints is attached to the circle center, with the x -axis aligned to the endpoint angle.

7.4 Intersection of two Lines in the SPmodel

ˆ ˆ Given two infinite lines LWA = ()xˆ WA ,,p A CA ,BA and LWB = (xˆ WB ,,p B CB , BB ) with attached frames AB and . We look for the uncertain location of the intersection ˆ corner LWC = ()xˆ WC,,p C CC ,BC , and, as the corner uncertainty depends on the uncertainty of the lines, the cross-correlations CCA and CCB between the corner and the lines. For the sake of generality, correlations between all involved entities are assumed. Two solutions are presented: first, expressed in the frame of one of the lines, say, A (figure 7.4), and second, with the corner expressed in the base frame W . Relative to A we find

xAC = ûxWA ∆ xWB ∆ xBC′ ∆ xC′C (7.14) with the involved uncertain relationships

T T xWA = xa ya ϕa ; xWB = xb yb ϕb (7.15)

{}A {}C {}C′

xAC

xWA xWC {}B xWB {}W

Figure 7.4. Intersection of two lines

128 7.4 INTERSECTION OF TWO LINES IN THE SPMODEL

00T T xBC′ = xe2 ; xC′C = 00ψ (7.16)

00T xAC = xe1 (7.17)

Equation (7.14) is an equation system with three equations and three unknowns, xe1 , xe2 and ψ which yields the sought relative displacement xe2 as

sin ϕa()xb – xa – cos ϕa()yb – ya xe2 = ------(7.18) sin ()ϕb – ϕa and the alignment angle ψ as

ψϕ= a – ϕb (7.19) For error propagation, equation (7.14) is evaluated

xAC = ûxWA ∆ xWB ∆ xBC′ ∆ xC′C ˆ ∆ ∆ ˆ ∆ ∆ ˆ ∆ ∆ ˆ ∆ = û()x WA dA ()x WB dB ()x BC′ dC′ ()x C′C dC

= xˆAW ûJWA dA ∆ ()xˆ WB ∆ xˆ BC′ ∆ xˆ C′C ∆ ()JCC′JC′B dB ∆ JCC′dC′∆dC

= ()xˆAW ∆ xˆ WC ∆ ()ûJCWJWAdA ∆ JCC′JC′B dB ∆ JCC′dC′∆dC

= xˆAC ûJCWJWA dA∆JCC′JC′B dB∆JCC′dC′∆dC (7.20) Assuming small errors, the differential location vector becomes

A dC ≈ –JCAdA ++JCBdB JCC′dC′ +dC (7.21) or the corresponding perturbation vector

A pC ≈ –BCJCApA ++BCJCB pB BCJCC′pC′ +pC . (7.22) A For the covariance of pC and the sought cross-correlations CCA and CCB we take advantage of the result from the error propagation law that augmenting the functional relationship by unity yields the cross-correlations between the inputs and outputs. Instead of looking for

A T Cov()pC ==CC J ⋅⋅C J (7.23) with

T T T T J = –BCJCABA BCJCBBB BCJCC′BC′ BCBC (7.24)

129 7. SLAM WITH RELATIVE FEATURES

CA CAB CAC′ CAC C T C C C = AB B BC′ BC , (7.25) C T T CAC′ CBC′ CC′ CCC′ T T T CAC CBC CCC′ CC the augmented relationship yields directly

C′ = J′ ⋅⋅C ()J′ T (7.26) with

CA CAB CAC T C′ = CAB CB CBC (7.27) T T CAC CBC CC and

IrA 000 = 0 00. (7.28) J′ IrB – T T T BCJCABA BCJCBBB BCJCC′BC′ IrC where rA = rank()BA , rB = rank()BB and rC = rank()BC and zero-matrices of corresponding dimensions.

The covariance matrix C is obtained by the same procedure with the difference that the 00T uncertainties of the two alignment transformations xBC′ = xe2 and T xC′C = 00ψ depend on the uncertainty from the lines and are not given in their respective local frames. We therefore consider first their correct derivation. In the SPmodel errors are represented locally, expressed in the frame attached to each geometric entity. The propagation of local uncertainties across frame transforms works as long as one stays on the level of locations. Sometimes, uncertainty must be transformed into a global reference frame, if, for instance, an uncertain geometric entity is plotted for visualization or if geometric relations of two entities are determined. Let dF be the differential location vector in frame F and xEF the location vector of FE in frame . Then, the error in FE expressed in is obtain by:

E T Cov()dF = J2∆{}xˆ EF, 0 ⋅⋅Cov ()dF J2∆{}xˆ EF, 0 (7.29) which comes from an alternative expression of the composition xEF ∆ dF as

xEF ∆ dF = xEF + J2∆{}xˆ EF, 0 dF (7.30) where

130 7.4 INTERSECTION OF TWO LINES IN THE SPMODEL

cos ϕ – sin ϕ 0 J2∆{}x, 0 = sin ϕ cosϕ 0 (7.31) 001 A recast of errors from global to local is done by inversion of equation (7.29). When intersecting two lines, we “descend” from the level of locations to the level of coordinates. On this level we need the derivatives of xe2 and ψ with respect to the uncertain variables for error propagation. Note that we derive against the uncertain values given in the base frame. In order for them to be correct, a local-to-global recast of errors is needed.

∂xe – sin ϕ ∂xe sin ϕ ------2 = ------a - ; ------2 = ------a - (7.32) ∂xa sin ()ϕb – ϕa ∂xb sin ()ϕb – ϕa

∂xe cos ϕ ∂xe – cos ϕ ------2 = ------a - ; ------2 = ------a - (7.33) ∂ya sin ()ϕb – ϕa ∂yb sin ()ϕb – ϕa

∂xe ()– sin ϕ – ()– cos ϕ ------2 = ------xb xa b yb ya -b (7.34) ∂ϕ 2 a sin ()ϕb – ϕa

∂xe ()()– cos ϕ – ()– sin ϕ cos ()ϕ – ϕ ------2 = ------yb ya a xb xa a b a- (7.35) ∂ϕ 2 b sin ()ϕb – ϕa

------∂ψ ==------∂ψ- 0 ; ------∂ψ- ==------∂ψ 0 (7.36) ∂xa ∂ya ∂xb ∂yb

------∂ψ- = 1 ; ------∂ψ = –1 . (7.37) ∂ϕa ∂ϕb Starting from the input covariance matrix of the two lines

C C = A AB (7.38) Cin T CAB CB the fully correlated covariance matrix C is determined by

T C = JinCinJin . (7.39)

The Jacobian Jin reflects again a unity relationship for entities AB and . For C′ and C it contains the derivatives (7.32)–(7.37) of expressions (7.18) and (7.19).

I rA + rB Jin = (7.40) F ′ In order to recast errors from their respective local frames to the global coordinates, the Ja- cobian F ′ is according to expression (7.29) the product of the Jacobian F postmultiplied

131 7. SLAM WITH RELATIVE FEATURES by the matrix Q :

F ′ = F ⋅ Q (7.41)

x ′ ∂ ∂ F = BC ------x ′ ∂d ∂d C C A B ˆ ˆ x WA , x WB ∂x ∂x ∂x ∂x ∂x ∂x ------e2 ------e2 ------e2 ------e2 ------e2 ------e2 ∂xa ∂ya ∂ϕa ∂xb ∂yb ∂ϕb 000000 = ∂∂∂∂∂∂ (7.42) 000000 ------∂ψ ------∂ψ------∂ψ------∂ψ------∂ψ ------∂ψ ∂xa ∂ya ∂ϕa ∂xb ∂yb ∂ϕb

{}ˆ , 0 T 0 = J2∆ x WA BA (7.43) Q 0 T J2∆ {}xˆ WB, 0 BB An application related to multi-segment line features are segment endpoints which arise from intersection of a ray, uncertain in angle, with a line where the ray is given by T xWB = 00ϕb . Figures 7.5 shows a local map with six multi-segment lines where one line carries two segments. The segment endpoints have been gained by intersection where for visualization the relative endpoint features received full rank. Figure 7.6 shows the same extraction result but with an artificially magnified radial uncertainty of the lines and no angular uncertainty of the ray. The resulting uncertainties are as expected and illus-

-1

-2

-3 -1 0 1 2 3 4 5 Figure 7.5. Local map with six multi-segment lines. Endpoints are shown as full- rank locations with their 95% ellipse

132 7.4 INTERSECTION OF TWO LINES IN THE SPMODEL

-1

-2

-3 -1 0 1 2 3 4 5 Figure 7.6. Resulting 95% uncertainty regions with a magniﬁed radial uncertainty and zero angular uncertainty of the laser ray. trates the correctness of the obtained results. Opposed to the scheme for relating corners and segments in (Castellanos & Tardos 1999) which makes use of a Kalman filter, the presented approach can account for correlated lines.

For the intersection corner expressed in the base frame W , the same procedure applies:

The relative displacement xe2 is

sin ϕb()xb – xa – cos ϕb()yb – ya xe2 = ------(7.44) sin ()ϕb – ϕa and the alignment angle ψ as

ψϕ= ()a + ϕb ⁄ 2 + kπ ; kZ∈ (7.45) where kx is such that the -axis is aligned to the bisector of the two lines. Relative to W we find

xWC = xWB ∆ xBC′ ∆ xC′C (7.46) with the Jacobian J′

IrA 000 = 0 00. (7.47) J′ IrB 0 T T BCJCBBB BCJCC′BC′ IrC

Analogous to the previous case, the Jacobian F contains the derivatives of alignment trans-

133 7. SLAM WITH RELATIVE FEATURES formations which are now equations (7.44) and (7.45). Multi-segment lines with segments which are close enough to each other will be intersected in order to obtain corners in the experiments of section 7.6.

7.5 The Map Building Cycle

In general, relative features have cross-correlations to their anchor features. When the latter is altered during a map building cycle, these cross-correlations must be updated accordingly. By their separate treatment, they do not affect the size of map state vector or covariance and thus the complexity for their update is linear in the number of relative features. In this section we will study how cross-correlations to relatives features are to be updated during a cycle with the example of multi-segment lines. The reader is assumed to be famil- iar with the use of the SPmodel in a SLAM context (otherwise refer to appendix D). Fur- ther, the odometry model from chapter 2 is integrated into the first step, that is robot displacement.

7.5.1 Robot Displacement

Models for non-systematic odometry errors are often considered in a stepchild manner. Many researchers assume robot displacements as steps of rotations and translation with an uncorrelated uncertainty in both movements. This assumption has a weak physical basis and in (Chong & Kleeman 1997b) it is shown that it suffers from a consistency problem (see also chapter 2). Further, a robot that explores autonomously or by joystick is unlikely to perform pure straight-line motions but will, in general, follow arbitrary trajectories between the locations of observation. Here we will develop the expressions relevant for SLAM when using the wheel space model proposed in (Chong & Kleeman 1997b). We assume that the robot followed an arbitrary path between two observation points and accu- mulate errors along this path. The Jacobian which is needed for map update is calculated recursively. With JRkRk – 1 C as the robot covariance estimate at time index kk given all observations up to we Rkk have from chapter 2 (and in the notation of this chapter):

= T + T. (7.48) CRkk– 1 Fx CRk – 1 k – 1 Fx Fu Uk Fu This propagates errors from one odometry step to the next step and is performed each time when a new encoder measurement arrives. With = (subscript omitted) C0 CRk – 1 k – 1 R

134 7.5 THE MAP BUILDING CYCLE

= T + T C1 Fx 1 C0Fx 1 Fu 1 U0Fu 1 = T + T C2 Fx 2 C1Fx 2 Fu 2 U1Fu 2 = ()T + T T + T (7.49) Fx 2 Fx 1 C0Fx 1 Fu 1 U0Fu 1 Fx 2 Fu 2 U1Fu 2 = T T ++T T T Fx 2 Fx 1 C0Fx 1 Fx 2 Fx 2 Fu 1 U0Fu 1 Fx 2 Fu 2 U1Fu 2

C3 = …

n n C = ()∏ F T TC ()∏ F T n i = 1 xi 0 i = 1 xi

n n n + ∑ ()∏ F T TF U F T ()∏ F T (7.50) i = 1 ji= + 1 xj u i i u i ji= + 1 xj

The sought Jacobian after n odometry update cycles is therefore

n J = ()∏ F T T (7.51) RkRk – 1 i = 1 xi and the covariance estimate of the relative location xRk – 1Rk

n n T T T n T CR R = ∑ ()∏ F F U F ()∏ F (7.52) k – 1 k i = 1 ji= + 1 xj u i i u i ji= + 1 xj As no feature is updated during this step, cross-correlations to relative entities are not affected.

7.5.2 Matching

In the matching step, complex features represented by a set of relative entities can play out the plus of information they carry. For multi-segment lines, data association is supported by the segments besides the probabilistic information from the line. First we consider how the segment information from two paired multi-segment lines is fused (figure 7.7). We express the relative entity from the local feature with attached frame E in the frame of the global matching partner and calculate the cross-correlation between the global feature and the new entity:

135 7. SLAM WITH RELATIVE FEATURES

model line observed line

{} L xRL x {}G {}E LE {}R

xGE

xWR xWG {}W Figure 7.7. Integration of the segment information from multi-segment lines

xGE = ûxWG ∆ xWR ∆ xRL ∆ xLE

= û()xˆ WG ∆ d G ∆ ()xˆ WR ∆ dR ∆ ()xˆ RL ∆ dL ∆ ()xˆ LE ∆ dE

= xˆ GW ûJWGd G ∆ ()xˆ WR ∆ xˆ RL ∆ xˆ LE ∆ ()JELJLRdR ∆ JELdL ∆ dE ˆ ˆ = ()xGW ∆ x WE ∆ ()ûJEWJWGd G ∆ JELJLRdR ∆ JELdL ∆ dE

= xˆ GE ûJEWJWGd G ∆ JELJLRdR ∆ JELdL ∆ dE (7.53) thus the perturbation becomes

G T Cov()pE = J ⋅⋅C J (7.54) with

– T T T J = BEJEG BG BEJER BR BEJELBL Ire (7.55)

CR CRG 00 C T C 00 C = RG G (7.56) 00CL CLE T 00CLE CE

In order to determine the new cross-correlation CGE between the global feature and its new relative entity, we augment the functional relationship by unity for the global feature and obtain

136 7.5 THE MAP BUILDING CYCLE

CG CGE T T = J ′ ⋅⋅C ()J ′ (7.57) CGE CE with

Ir 000 ′ = g (7.58) J – T T T BEJEG BG BEJER BR BEJELBL Ire Evaluation yields

T T T T T CGE = CRG BRJER BE – CGBGJEG BE (7.59) Then, we perform a perpendicular projection from the observed line onto the model line by premultiplication with the row selection matrix SE

xˆ GE ′ = SE xˆ GE (7.60) with

100 SE = 000 (7.61) 000 Provided that two multi-segment lines satisfy the rigidity constraint, that is, are compatible on a significance level α , they are declared as pairing candidates. Clearly, as infinite lines have low mutual discriminance and hold no information on the location where they have been observed, there might be several candidates from the whole map in the validation gate. Here, we apply a maximal overlap policy where segment endpoints of the local pairing candidate are transformed and projected according to the above procedure and the overlap length of local and global segments is accumulated. The multi-segment feature in the validation gate that maximizes the overlap length is chosen. In case of no overlap, we propose a heuristic. Instead of rejecting the match, we accept the pairing if the local and global segments after projection are not farer away than a threshold distance. This distance can be the typical door width in the environment1 or has a similar meaning.

7.5.3 Integration of New Observations

When a new observation is integrated into the map, no cross-correlations between the map

1. 1 m for Swiss federal buildings

137 7. SLAM WITH RELATIVE FEATURES entities and the relative feature arise as it will be shown: Integration of new observation F into the map, which for simplicity contains the robot R and a single feature M , means formally: ˆ x WR dR ˆ W W ()x ' = xˆ WM ; ()p ' = pM (7.62) xˆ WR ∆ xˆ RF BFJFR dR + pF where the covariance estimate of the new map entry is

W T Cov()pF = J ⋅⋅C J (7.63) with

0 T CR J = BFJFRBR Irf ; C = (7.64) 0 CF We augment the functional relationship by unity for the robot, the map feature and the relative entity in order to determine their new interdependencies:

CR′ CRM ′ CRF′ CRE′ CR CRM 00 T T CRM ′ CM′ CMF′ CME′ CRM CM 00 T T T = J ⋅⋅J (7.65) CRF ′ CMF′ CF′ CFE′ 00CF CFE T T 00 CRE ′ CME′ CFE′ CE′ CFE CE with

000 Irr 0 I 00 = rm (7.66) J T 0 0 BFJFRBR Irf 000 Ire

The block-wise structure of the input covariance matrix C at the right hand side of equation (7.65) reflects the correlations among the map elements and among the new elements but not between them. Evaluation of (7.65) yields

T T CR′ CRM ′ CRF′ CRE′ CR CRM CRJFR BF 0 T T T T T CRM ′ CM′ CMF′ CME′ CRM CM CRM JFR BF 0 T T = CRF ′ CMF′ CF′ CFE′ BFJFRCR BFJFRCRM CF CFE T T T T CRE ′ CME′ CFE ′ CE′ 00CFE CE (7.67) The zero block matrices on the right hand side demonstrate the above assertion. Due to this property, the treatment of relative features can be made separately. Of course, the new cross-correlations between F and M and F and the robot equal those in equation (D.80).

138 7.5 THE MAP BUILDING CYCLE

7.5.4 Estimation

During estimation, the cross-correlation CME to relative entities is modified. To derive its update expression we augment the simple map containing the robot and a single feature M by the relative feature with attached frame EM expressed in : ˆ x WR dR ˆ W ˆ W x = x WM ; p = pM (7.68) ˆ xME pE The covariance update equation

W = ()– W (7.69) Ckk I Kk Hk Ckk– 1 evaluates to

CR CRM 0 CR CRM 0 T = T – W (7.70) CRM CM CME CRM CM CME Kk Hk Ckk– 1 0 T 0 T CME CE kk CME CE kk– 1 where the last matrix term holds the sought ()23, -element. Substituting the Kalman gain matrix and writing Ch for the innovation covariance yields W = W T ()–1 W (7.71) Kk Hk Ckk– 1 Ckk– 1 Hk Ch Hk Ckk– 1 where evaluation of the terms lead to the update rule for the cross-correlation

T T –1 CME′ = []I – ()CRM HR + CM HM Ch HM ⋅ CME (7.72)

According to equation (D.63) are HR and HM the elements of the Jacobian Hk at the corresponding positions. Observations are integrated iteratively, repredicting the updated map each time. Like in chapter 4, we apply a strongest observation policy where the most certain local observations are integrated first in order to foster good filter convergence during estimation.

7.5.5 Centering

The estimation step causes the perturbation vectors to become uncentered. Centering the map avoids bias and poor error propagation in non-linear relationships. From appendix D or (Castellanos & Tardos 1999) we see that the map covariance matrix is determined by

139 7. SLAM WITH RELATIVE FEATURES

()C W ' = QW C W ()QW T (7.73) where QW is

–1{}, 0 T 0 W = BR J2∆ dR BR (7.74) Q –1 T T 0 BM J2∆{}BM pM , 0 BM for a simple map which contains the robot and a single feature M . If we augment the map by a relative entity with attached frame E ˆ x WR dR ˆ W ˆ W x = x WM ; p = pM (7.75) ˆ xME pE

CR CRM 0 W T C = CRM CM CME (7.76) T 0 CME CE

W –1 T T and extend Q by the diagonal term BE J2∆{}BE pE , 0 BE – which equals the identity matrix since pE = 0 –, expression (7.73) yields after substitution and evaluation the update rule for cross-correlations to relative entities

CME′ = QM ⋅ CME (7.77) –1 T T with QM = BM J2∆{}BM pM , 0 BM

7.5.6 Finalizing Relative Entities

Transformed and projected relative entities from updated or newly integrated multi-segment line features might represent redundant information after a map building cycle. Over- lapping segments, for example, are described by four endpoint after successful matching, the two from the local segment and the two from the global segment. To finalize the cycle, we perform three operations: • Endpoints of segments on the same multi-segment line which are significantly close to each other – on the level α – are merged • Redundant endpoints from overlapping or merged segments are removed • Intersecting segments those extremities overlap (typically at reobserved convex corners) are intersected according to section 7.4. The corresponding endpoints receive the corner uncertainty. The cross-correlation between the corner and the lines become the new cross-correlations between the anchor feature and the relative feature. The semantical information of the corner is currently not used.

140 7.6 EXPERIMENTS

7.6 Experiments

For the experiments, the robot Donald was used as platform (appendix A). Since we do not consider map building as a real-time problem, data were recorded. The robot was joysticked and proprioceptive (encoders) and exteroceptive (laser range data) sensory data were written into a file including timestamps of each measurement. As long as exploration is not done in an autonomous mode where decisions are taken according to the acquired information, there is no difference whether processing is done on-board/on-line or off- board/off-line. A full covariance stochastic map using the SPmodel (SPmap) was implemented in Matlab.

B 1588 1512

A 2682 2380 411 571 4100 722 A–B: 4889 mm 1230 B–C: 4100 mm

788 1800 D C–D: 2430 mm C 612 1607 2430 Figure 7.8. Ground truth for the small map experiment. The three objects in the middle have equal dimension. All numbers in mm, hand measurement precision.

1.5

0.5

-0.5

-1

-1.5

-2

-2.5

-3

-3.5 -2 -1 0 1 2 3 4 Figure 7.9. Accumulated raw data. The black robots mark the positions of the observations. The experiment contains 52 steps.

141 7. SLAM WITH RELATIVE FEATURES

2 6 1.5

1 1

0.5

0 0

-0.5

-1 -1

-1.5

-2 -2

-2.5 -3 a)-3 b) -3.5 -2 -1 0 1 2 3 -2 -1 0 1 2 3 4 Figure 7.10. Local map at step 6 a) and global map b) after integration. The ﬁgures illustrate how the map can become inconsistent with segments only. The horizontal wall at the top is described by two overlapping segments. The same will happen to the inclined wall at the bottom when the segments grow after reobservation.

-0.2

-0.4 y [m]

-0.6

-0.8

-1

0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 x [m] Figure 7.11. Detail from ﬁgure 7.12 a). Corners, obtained from intersection

Two experiments have been carried out. A small indoor map with hand-measured ground truth (figure 7.8) and a larger indoor map over a trajectory length of 170 meters. In the experiments we will study the benefit of multi-segment lines and compare the results to those obtained with a segment-only representation. We will compare precision of the map, consistency in the representation of environment and map size.

7.6.1 Small Map

The first experiment is a 52-step map with 17 lines whose ground truth is shown in figure 7.8. The robot is driven in eight-shaped trajectories around the two free standing objects in the middle (figure 7.9). Exploration start point is always at x ==0, y 0 with orienta-

142 7.6 EXPERIMENTS

52 1.5

0.5

-0.5

-1

-1.5

-2

-2.5

-3 a) -3.5 -2 -1 0 1 2 3 4 2

52 1.5

0.5

-0.5

-1

-1.5

-2

-2.5

-3 b) -3.5 -2 -1 0 1 2 3 4 Figure 7.12. Resulting maps. a) With multi-segment lines, b) with segments only

143 7. SLAM WITH RELATIVE FEATURES

multi-segments lines segments

n / nS 21 / 21 30 / –

D2 29.895 28.631 2 0.656 m / 0.0272 m 0.641 m / 0.0269 m Σεi / Σεi kbytes 108.7 187.7 Table 7.1. Result of the small map experiment. tion θ = 0 . The minimal gap heuristic for matching MS-lines (section 7.5.2) was set to 1 meter. The resulting map with multi-segment lines is depicted in figure 7.12 a, the map with segments only in figure 7.12 b. Table 7.1 quantifies their difference.

Size. The number of map features n is 21 for the map with multi-segment lines and 30 for the one with segments. The former has an additional 21 segments, described each by two relative endpoints. The equal number of MS-lines and segments say that, in the final map, there is no multi-segment line with more than one segment. The MS-line map has not only a smaller map state vector but is also slightly more memory efficient (in Matlab). However, note that for the segment-only case, segments can be represented by a location and a length instead of two endpoints. This representation is more memory efficient.

Precision. The joint Mahalanobis distance D2 between the two final maps and the ground truth map has been determined where a nearest neighbor matching strategy was applied. The ground truth map is assumed to have uncorrelated map entries, correlations for the learned map are taken into account. The resulting values are within a 95% bound 2 ()χ34, 0.95 = 48.603 with almost equal precision results (the absolute values depend also on the empirical error model of hand measurements). The segment-based map is slightly more accurate which is a consequence of multiple entries for the same environmental structures allowing the NNSF to choose the most precise segment in the validation gate. The ac- 2 cumulated squared error Σεi and the accumulated absolute errors Σεi are also given. They basically reflect the hand measurement imprecision. Consistency. Here we see a far better description of the environment by the multi-segment lines. Where, for example, five segments describe the long vertical wall in the segment- based map, there is a single segment in the MS-line map. The segment-only map reproduc- es the results in (Castellanos & Tardos 1999) where models with a similar degree of inconsistency have been obtained. Figure 7.10 demonstrates how inconsistency is introduced into the map during exploration. The horizontal wall at the top is described by several segments which have been integrated already in the ﬁrst step (ﬁgure 7.5 shows step 1). When segments grow after reobservations, they will start to overlap. This is also what happened to the inclined wall at

144 7.6 EXPERIMENTS

-5

-10

-15

-20

-25

-30

-35

-30 -20 -10 0 10 20 30 Figure 7.13. Accumulated raw data and the 170 meter-exploration path the bottom. Two partial segments, one from step 1, the second from step 6 were enlarged until the overlap in the ﬁnal map (ﬁgure 7.12 b). Finally, figure 7.11 shows the corners gained by intersection of overlapping segments during the finalization step for relative entities. They allow a description of high model fidelity of the objects in question. However, as mentioned, the semantical information of corners is not used. There is also a spurious map entry: the small inclined segment in both maps (middle, left) has been extracted from raw data on the legs of the operator.

7.6.2 Large Map

The second map covers approximatively 60× 35 m (figure 7.13). The 170 m exploration path consist of 180 steps. The minimal gap heuristic was again set to 1 meter. The resulting two maps are shown in figures 7.14, 7.15 and compared in table 7.2. Again, The degree of consistency is visibly better for the MS-line map which is also man-

145 7. SLAM WITH RELATIVE FEATURES

-5

30 -15 -10 -5 0 5 10 15 20 25 30 Figure 7.14. Resulting map of the large-scale experiment for multi-segment lines (scale is ﬁve meter per grid line). ifested by the number of features and the memory requirements. The multi-segment lines map is almost three times smaller, the map state vector almost two times smaller.

7.7 Conclusions

Maintaining relative features to entities in the map is a way to represent more appropriately complex features with a physical extension. Their treatment can be separated from the map state vector. Cross-correlations between the relative entities and the anchor features must be maintained and updated, their memory requirement and computational complexity being linear in the number of relative features per anchor feature.

146 7.7 CONCLUSIONS

-5

30 -15 -10 -5 0 5 10 15 20 25 30 Figure 7.15. Resulting segments-only map of the large-scale experiment (scale is ﬁve meters per grid line).

The experiments in this chapter exemplify this concept by means of multi-segment lines. From the experiments we conclude that multi-segment line features enable robots to learn significantly more consistent and more compact maps of typical indoor environments. While there is not necessarily a gain in map precision, a robot which performs localization with a consistent map will be more accurate if it receives more information from longer segments. But the most important drawback of inconsistency from overlapping small segments on the same wall is apparent for global localization according to chapter 5: the unary and extension constraint with such segments become useless and can even mislead the search in the interpretation tree. An observed segment of, say, two meters length will not be matched to a wall modeled by two segments of, say, each one meter, although it was the observed wall.

147 7. SLAM WITH RELATIVE FEATURES

multi-segments lines segments

n / nS 77 / 111 139 / – kbytes 974.0 2’738.7 Table 7.2. Result of the large map experiment.

Robustness during exploration is also improved with multi-segment lines. Each time when more than one segment on the same physical object is observed (see figure 7.5, long horizontal wall or figure 7.10, same wall), and given that this wall is already mapped and correctly matched to one of the observed segments, the other segment will be integrated as a new feature. But in dynamic environments of with sensors which are subject to outliers it is likely to make multiple observations of this kind during the map learning process. With a single segment representation, an open door, an outlier or a person standing in front of a wall might give rise to such a new map entry.

148 Chapter 8

Conclusions

This thesis is about feature-based mobile robot navigation in known and unknown environments. The work covers a wide range of problems, from system integration aspects up to the successful deployment of autonomous robots in application-like events. Error models with a sound physical basis for the sensors of a mobile robot were derived in chapter 2. The benefit of correct noise models cannot be underestimated for later stages like feature extraction, estimation and decision taking. The probabilistic extraction of lines, segments, retro-reflecting beacons and vertical edges has been described in chapter 3. The line extraction algorithm employs a sliding window technique to identify regions of high model fidelity with a fidelity criterion which is model-independent. Significantly collinear lines were fused in a final step using a NNSF-like clustering algorithm. This avoids oversegmentation of the range image, yields higher precision (more points contribute to the fused lines) and implements a segment-while-fit-like behavior. The complexity of the algorithm is linear in the number of scan points which leads to short extraction times of around 20 ms on our platforms. Extraction results from four different laser scanners illustrate a precise and consistent segment-based scene description. From the segment-based description the step was made towards a semantics-only representation of the local map. A string of weighted symbols compactly reflects the main characteristics of scene and encodes its topology. Chapter 4 investigated the performance of on-the-fly pose tracking with a multisensor system. On-the-fly navigation reveals issues which are present but hidden at low speed or with step-by-step motion. These issues are the compensation of raw data distortions due to the motion of the vehicle during the measurement process and the maintenance of temporal relations of observations (across sensors and time), estimation results and the current time. Solutions are presented and discussed. The contribution of vertical edges from gray level images to the laser-based localization system was found to be more qualitative than quantitative. On the one hand, visual information was shown to be indispensable in situations where the laser sensor receives insufficient information for localization. On the other hand, under uncontrolled conditions during the four-day teleoperation event “Computer 2000”, vertical edges turned out to be of limited utility due to their tendency to ambiguous and incorrect data association. Local-

149 8. CONCLUSIONS ization accuracy (conservative 2σ -bounds) was found to be around one centimeter in x and y , and around 0.5° in θ . These numbers were confirmed or even improved in all our experiments. Despite good results for robustness, the experiments exposed also the limitations of EKF pose tracking, which were found to be low feature discriminance and the ig- norance of the data association problem. Both issues make that the robot can go lost and cannot recover from being lost. They are addressed in the following chapter on global localization. Origin uncertainty of measurements, whose neglect was previously identified to be the bottleneck for local localization, is addressed with an interpretation tree approach to discrete non-Bayesian feature-to-feature association. This makes the feature-based localization approach a global technique with the ability to localize a robot without a priori knowledge. An additional visibility constraint was introduced as a location dependent constraint. Its benefit is to further restrict search and to discard wrong hypotheses early during tree traversal. In the experiments presented in chapters 5 and 6, we have demonstrated that this approach is feasible in an embedded system implementation and in a highly dynamic environment. Overcoming the “static” consideration of robot localization as a matching problem, we have presented an algorithm for pose tracking which incorporates the interpretation tree concept and performs track splitting under geometric constraints when ambiguous matching situations occur during motion. The implication of the geometric constraints for track splitting is that they produce hypotheses only at a limited number of predefined “snap” locations – predefined by the constraints from the involved features. With the assumption that environments have no symmetries locally (whereas globally they might have), a framework for global EKF localization and pose tracking has been proposed. The framework decides over track formation, maintenance, scoring and rejection according to the robot being lost, localized, not localized globally and not localized locally. Simulation experiments demonstrate a highly robust behavior; it enables a robot to recover from kidnapping, collisions and extensive odometry errors. During the experiments with the real platform and an implementation with infinite lines (which have few geometric constraints), a fast convergence towards the true location hypothesis was observed in general. This illustrates that geometry is a powerful decision mechanism and help to avoid memory-intensive free-space information in the maps. The outcome of the previous chapters was employed in the Expo.02 exhibition – the biggest installation of autonomous, freely navigating robots so far. Ten mobile robots were interacting with 686’405 visitors during a five month period. Starting from a three-layered architecture as navigation framework, all components which make a robot navigate have been integrated into the framework and adaptations which deviate from the initial design have been outlined. Despite extensive environment dynamics constantly produced by around 100 visitors on a surface of 315 m2 , the laser-based localization system performed

150 surprisingly well. Here an empirical evidence for the robustness of feature-based localization could have been furnished. The significant contribution of the pioneering event Ex- po.02 is to have shown that autonomous, freely navigating robots are able to work reliably in unmodified mass exhibition environments. This is important in general and for the young field of robots in exhibitions in particular. A flow of 400 persons per hour was achieved while assuring high visitor satisfaction: in a large-scale survey (2031 visitors interviewed) 38% said the exhibition was “very good” and 47% said it was “good”. In chapter 7 the important restriction of a priori known maps of the environment could have been released. The problem of more complex features with physical extension was approached with the introduction of relative entities for SLAM. Their second advantage is that information for estimation and data association can be kept separate. The update rules for the anchor-to-relative feature cross-correlations have been developed for all steps of a map building cycle. The experimental results with multi-segment lines as an exemplifica- tion of the concept of relative features demonstrate clearly more consistent and smaller maps compared to the segment-only case. Up to 45% smaller map state vectors have been achieved. It is argued that consistency of this type is particularly needed when the map is employed for constraint-based localization. This work allows also conclusions on the problem of feature representation. The course from compact and minimalistic infinite lines in chapter 4 over the introduction of multi- segment lines in chapter 5 to the SPmodel in the last chapter shows that the choice of a feature representation can have a far-reaching impact. The SPmodel, despite its more complicated notation at first glance, is an appropriate choice due to its generality and concept of local errors. Finally, a major conclusion of this work is that from the introduction of the Kalman filter for robot localization in the eighties, the approach was shown to be suitable in all significant respects for a real-world application in industry and service. Precision, robustness, and practicability have been examined under controlled and uncontrolled experimental conditions. We believe that academic case study experiments under carefully controlled conditions can only give optimistic performance bounds of a method. If robots shall be able to leave the lab and enter the real world, it must be verified whether these bound can be held or whether they represent instrinsic limits. Not always, as shown in chapter 4, an idea turns out to “survive” this step. The overall travel distance of the robots described in this work – especially of Pygmalion, the robot which was built for this thesis, MoPS, and the Robox family –, exceeds 3,400 km of feature-based navigation in unmodified environments. This number has been accumulated with different robots, sensors, features and environments. 3,400 km is a distance once across Europe and has hopefully helped to shed light on questions and answers in the do- main of feature-based localization and mapping.

151 8. CONCLUSIONS

Three directions for future work from here are highlighted: Expo.02 revealed the necessity of more experiments with MHT for robust pose tracking. The local kidnapping situations after manual interventions caused the robot to go lost. The question whether Bayesian or a non-Bayesian data association is the more adequate approach for hypothesis assessment with the track splitting filter of chapter 5 is worthwhile to be studied. Active localization. As mentioned, related literature offers either approaches with a localization guarantee but unrealistic assumptions or greedy methods which work in practice but provide no guarantees. An algorithm for active localization which bridges the gap would be an important extension of the global localization framework in chapter 5. Data association for SLAM. The work discusses a technique for data association for localization (chapter 5) but makes no use of it in SLAM where robust data association would be more than welcome. An outline of their combination could look as follows: location hypotheses generated in the partially learned map are points of topological consistent integration of the local map. The supporting set of each hypothesis provides directly a data association solution. The current best location could be chosen by means of a criterion based on the distance prediction-to-hypothesis location (i.e. odometry). However, each hypothetical location is also birth point of a map hypothesis whose maintenance allows recon- sideration of past and possibly wrong decisions. Maintaining multiple map hypotheses in one or the other form is mandatory if topological map consistency is to be guaranteed in general. Map quality measures could play an important role for assessing and rejecting wrong hypotheses. They could sense metric or topological inconsistency of map candidates, supervise the filter behavior or employ a priori knowledge from the environment to be learned.

152 Appendix A

Robot Platforms

This appendix contains the robots which have served as research platforms during this work. The operation system XO/2 which runs on all robots is briefly outlined in the last section.

A.1 Pygmalion

b) a)

Figure A.1. Pygmalion, the robot which has been built during this work. The chassis a), the ﬁrst design with the Acuity AccuRange laser scanner b) and the ﬁnal design with two Sick LMS 200 and a 4-dof face (not built by the author), c).

The experimental platform that was used most in this work is the robot Pygmalion which has been built as the lab’s first robot (figure A.1). Its design principles are oriented towards an application as service or personal robot. Long-term autonomy, safety, extensibility, and friendly appearance were the main objectives for design. With its dimensions of about 45 x 45 x 70 cm and its weight of 55 kg it is of moderate size and danger opposed to many robots in its performance class. The controller (delivered by Mechatronic Research Sys- tems) is a VME-based six axis controller for generic mechatronics applications. In the first design, the embedded computer was PowerPC 604 at 100 MHz with standard peripheral IP-modules for analogue, digital I/Os and encoder inputs. The card was later replaced by a PowerPC 604 at 300 MHz. The controller has several RS-232 channels, wire-

153 A. ROBOT PLATFORMS less ethernet and galvanically isolated sensor/actor and CPU/periphery lines. Batteries provide autonomous operation up to seven hours and speech output is in English and French. The drive modules consist in two EC-motors with 1:100 harmonic drives which yield an encoder resolution of 50 000 pulses per revolution. The wheels have an antistatic coating and include a fast lock/unlock mechanism which enables a rapid mechanical de- coupling from the harmonic drive/motor side. This allows the robot to be pushed anytime without effort. Industry standards like VME, IP, PMC, CAN-busses and alike assure a high degree of hardware reliability and extensibility. The latter permits the addition or exchange of further processors, periphery and sensors. Besides the wheel encoders, Pygmalion has bumpers at two different heights divided in eight tactile zones. The main sensor in the first design was the Acuity AccuRange 4000 LIR described in chapter 2. It was replaced by two Sick LMS 200 (figure A.3). For the multisensor experiments in chapter 2, the robot was equipped with a Pulnix TM-9701 full frame, gray-scale CCD camera, EIA (640 x 480) and a Bt848 based frame grabber. In the long and eventful life of Pygmalion (“Piggy”), we highlight three important moments in figures A.2, A.3 and A.4.

a) b)

Figure A.2. a) The authors after Pygmalion’s ﬁrst multisensor kilometers and the completion of (Arras & Tomatis 1999). b) The voodoo dolls of our robot part suppli- ers: Richi (on the left), Sjur (on the right with chocolate bar)

a) b) c)

Figure A.3. The AccuRange 4000 LIR becomes history. a) The last image of its old domicile before it gets ripped out, b), and joyfully presented, c). In the last picture the two Sick LMS 2000 already took the place of their predecessor.

154 A.2 ROBUTER

a) b) Figure A.4. Pygmalion in the press: a) cover page of the cultural supplement in Le Temps, Mars 31st, 2001, b) live interview at Swiss TV SF 1 on May 3rd, 2001.

A.2 Robuter

Figure A.5. Robosoft Robuter

The very first on-the-fly localization system in the frame of this thesis has been implemented on Robuter at the Institute of Robotics, ETH (Arras, Vestli et al. 1996). The robot dis- posed of a Leuze RS3 laser scanner (not shown in the picture) whose data were also used in chapter 3. Later, two Sick LMS 200 replaced the Leuze scanner. The robot controller was redesigned and consist in a VME-rack sporting a PowerPC based embedded computer at 100 MHz.

A.3 MoPS – Mobiles Postsystem

The mobile robot MoPS has been developed and built at the Institute of Robotics, ETH Zürich (Tschichold-Gürman, Vestli et al. 1999), (Vestli 1995). The robot is employed to distribute mail to different floors in the institute building at ETH. The platform is non-holonomic with two diametrically opposed drive wheels and a free- wheeling castor. It contains a VME-based multi-processing controller for servo motors (DC, EC or AC). The embedded computer is a 200 MHz PowerPC 604 processing board (previously a dual processor 68040 system was used). The drive wheels constitutes two servo axes, and the remaining 4 axes are consequently available for a manipulating mecha-

155 A. ROBOT PLATFORMS

a) b) Figure A.6. a) The mail distributing robot MoPS, b) the robot at the charging station on the ﬁrst ﬂoor nism. Such a mechanism has been integrated on top of the platform. The sensor system includes two Sick LMS 200 laser range finders (front, rear), twelve ultrasonic range finders, and a bumper system with six segments. An infrared communication system is used to communicate with the building infra-structure (elevator, pigeon hole blinds, etc.). Experiments with a hybrid localization, using natural and artificial features (lines and retro-reflecting beacons) have been carried out on this platform (Arras & Vestli 1998). Since 1998 the robot’s localization system is the one from chapter 4 (laser only) with the line extraction method of chapter 3. Its reliability is demonstrated almost on a daily basis and several 100 km travel distance (Wullschleger & Brega 2001).

A.4 Donald Duck

Figure A.7. The Ducks. Daffy (left) and Donald (right).

The two SmartRob robots have been developed partly at the Institute of Robotics, ETH Zürich and partly at the Autonomous Systems Lab, EPFL. Their embedded controller is a custom made VME-rack. The PowerPC-based embedded computer and two SICK LMS 200 make the robots fully compatible with Pygmalion. Donald has been used for the SLAM experiments in chapter 7.

156 A.5 ROBOX

A.5 Robox

Figure A.8. The Robox family.

Robox is the robot built for the Swiss National Exhibition Expo.02. The lower part of the robot (octagonal base) contains a CompactPCI rack, two SICK LMS 200 laser range finders, a redundant security circuit, the batteries, tactile plates with foam bumpers, a gray scale camera interfaced by a Bt848 framegrabber (mounted upwards looking behind the robot face), and the analogue and digital I/O periphery hardware. The two main CPUs are a Pow- erPC 750 (G3) at 380 MHz and a Pentium III at 700 MHz. The robot has a symmetric differential drive kinematics, actuated by two EC motors via 1:50 harmonic drives and a castor wheel on each side – one on a spring suspension. This gives Robox an good maneuverabil- ity and stability in spite of its height of 1.65 m. The batteries provide autonomy for about twelve hours. The upper part of the robot incorporates and interfaces the interaction modalities. The face has five degrees of freedom: two eyes with independently actuated pan-tilt units and two mechanically coupled eyebrows. The right eye is equipped with a Firewire color camera, the left eye integrates an LED matrix for display of static and animated icons. Below the face there are two loudspeakers for speech output and sound playback. The central input device for a bidirectional communication are four individually colored, capacitive buttons that allow language selection, response to questions and other types of interaction. Two of the ten Roboxes are also equipped with a directional microphone matrix for experiments on speech recognition. Each robot is in connection to its own off-board PC via a 2.4 GHz radio Ethernet link. The PC serves as a supervision and logging tool, displaying and storing important state information in real time. All ten robots share the bandwidth of this network. The radio link is also used by the domotic system of Robox. It allows to communicate with networked de- vices in the environment. In the Expo.02 pavilion, the robots can switch on and off light sources, take pictures, trigger a flash or remote control other robots via infrared. Robox has been designed by a number of people. More details of Robox are found in (Ar- ras, Tomatis et al. 2003) and (Tomatis, Terrien et al. 2003), the interaction system is de-

157 A. ROBOT PLATFORMS scribed in (Jensen, Froidevaux et al. 2002).

A.6 XO/2

The deadline-driven real-time operating system XO/2 runs on all robots of this appendix. As it is relatively unknown and performance of the robots partly depends on the function- ality of this operating system, a brief outline is sketched. The question whether real-time and autonomy is needed in mobile robotics has been discussed in (Brega, Tomatis et al. 2000) with the conclusion that deadline-driven scheduling and a tighter integration between low-level real-time and high-level modules of a mobile robot is not mandatory but a welcome feature. It liberates the roboticist from problems of programming real-time systems. In other words, he/she can stay a “happy user” without taking care of related computer science issues. All technical details are found in the thesis of Brega (Brega 2002). Here a user point-of- view is given where a few properties from practice are highlighted which help to compare it with other RTOS in robotics. • The user programs in the language Oberon-2. Oberon is the successor of Pascal and Modula, featuring strong-typing, compatibility by name and not by structure, object- orientation, and modularization. The programmer is forced to produce code which is typically cleaner than with most common languages. • The application program is one coherent piece of code from the lowest-level axis controller up to the highest-level reasoning module. Changing the latter is a few mouse clicks away from modifying the PID values of the former, for example. Most robots separate RT and non-RT issues into two hardware systems: a microcontroller for axis control and a PC for “all the rest”. • Deadline-driven scheduling means for the user (loosely speaking): A periodic RT task is installed (accounting for the current real-time allocation of the CPU), profiled and started. The scheduler guarantees the completion under the specified timing constraints (within the tolerance of the scheduling jitter). Never, the user is concerned with issues of concurrent processes, missed deadlines or interrupt management. Task scheduling is done with a time-slice of 100 µs . • XO/2 is a target-host system. Development is done on a host computer. After cross- compilation, the code is sent to the target and dynamically linked where the XO/2 system allows safe dynamic linking (opposed to C-based operating systems). Edit- run-compile cycles are typically fast, and the user has not care about issues deriving from incompatible module interfaces.

158 A.6 XO/2

• The robots have no harddisk. At start-up the embedded computer (a VME or Com- pactPCI card) gets the boot-image containing the XO/2 system from a FTP or TFTP server (10-30 sec, depending on the network) or from a local Flash ROM (negligible). The robot program is then downloaded unless it was in the bootfile (again a few seconds). • XO/2 is a rare case of an RTOS with garbage collection. The user is not bothered by memory leaks or dangling pointers at runtime. • XO/2 implements many common IETF standards for communication and visualization. The threaded TCP/IP stack implements a Telnet, FTP, and HTTP server, and a TFTP, SNTP, SMTP, and POP3 client. Object linearization through XML allows browsers, java-applets or custom solutions to access remote objects on the HTTP transport. Facilities like the in-system creation of GIF and JPG images are used for streaming visual information to the client. • The XO/2 system runs on PowerPC-based hardware as it uses PowerPC specific properties for task profiling. • XO/2 is freeware. The advantages of XO/2 are apparent in practice, the most important drawback, however, is that Oberon is not a wide-spread programming language.

159 A. ROBOT PLATFORMS

160 Appendix B

Feature Definitions

This appendix contains definitions and often used transforms of line, corner, point and angle features.

B.1 Line Feature

B.1.1 Definition

The model for infinite lines is

x cos α + y sin α – r = 0 (B.1) where α is the angle of the perpendicular and r its length (figure B.1). A point is on the line if its coordinates ()xy, satisfy equation (B.1). If the point is given in polar coordinates ()ρθ, , (B.1) becomes

ρθαcos ()– – r = 0 . (B.2) In this model which is called Hessian line model, r is always positive. We can exploit this fact by using the sign of r to add visibility information. Given a line

gl = ()α, r (B.3) we redefine the parameters ()αv, rv which carry the visibility information as follows:

 ()α, r l visible from W ()α , r =  (B.4) v v ()απ+ , –r l invisible from W with 0 ≤ αv < 2π . Wr denotes the origin of the world frame. In the case where = 0 , the line according to equation (B.1) is not defined.

161 B. FEATURE DEFINITIONS

: gl2 radius: rv = –r angle: αv = απ+

g : l2 r radius: rv = r y angle: α = α α v r α

{}W x Figure B.1. Line feature deﬁnition; two examples.

B.1.2 Frame Transformation

W W Given a line ()α, r in the world coordinate system {}W , a robot at pose ()xr,,yr θ r with coordinate system {}R and a sensor with attached coordinate system {}S . Then we look for the line parameters ()Sα, Sr expressed in {}S . Due to the frequent use of this transform and its importance for the incorporation of the visibility information, we will briefly illustrate the derivation. For the sake of brevity we let coincide robot and sensor frame, i.e. {}S = {}R . The extension is straightforward.

y x gl R Rα R2 r y

R r R R1 α Wr x d1 x' y d2 Wα A1

y' WW= ' x A2

Figure B.2. Deriving the expression for the world-to-robot transform

162 B.1 LINE FEATURE

We first calculate the distances dRW in the triangle (),,' A and introduce the coordinate system {}W' by turning {}W around Wα (figure B.2). Then we express the robot W W frame origin RW in {}' which is done with the rotation matrix W'R . The columns of W'R are the unit vectors of {}W' expressed in {}W (Craig 1989).

W W W cos α – sin α W'R = . (B.5) sin Wα cos Wα

W T With PR = []xr yr we find

W' W' W W T W PR ==WR ⋅ PR W'R ⋅ PR (B.6) W' and the distance dx being the -coordinate of PR

W W dx= r cos α + yr sin α . (B.7) R The sought radius r is

R = W – r r d (B.8) W W W = r – xr cos α – yr sin α whereas for the angle Rα we recognize two cases

 Wα – θ , and on the same side Rα =  r W R (B.9)  W α – θ r + π, W and R on the opposite side From figure B.2 we see that for any robot position which is on the same side with respect R to the line than the origin Wr , is positive (like R1 ). This remains valid even if the robot is farer away from the line than Wd with being negative. For any robot position on the R opposite side, r is negative (like R2 ). Of course, this result confirms the convenient property of the Hessian line model, that the shortest distance from the line to a point

Px= ()P, yP is always given by

W W W rP = xP cos α – yP sin α – r . (B.10) Now let

2 2 σx σxy σxθ σα σαr 2 Cx()WL = 2 Cx()WR = σxy σy σyθ (B.11) σαr σr 2 σxθ σyθ σθ be the symmetric covariance matrices of the line parameters and the robot pose (we omit the superscript W in the reminder of this section). With the partial derivatives with respect to the uncertain parameters in the map

163 B. FEATURE DEFINITIONS

∂ ∂ ------fα- = 1 ------fα- = 0 (B.12) ∂α ∂r ∂ ∂ ------fr = x sin α – y cos α ------fr = 1 (B.13) ∂α r r ∂r and with respect to the uncertain robot pose

------∂fα = 0 ∂------fα- = 0 ∂------fα- = –1 (B.14) ∂xr ∂yr ∂θr

------∂fr = – cos α ------∂fr- = – sin α ------∂fr = 0 (B.15) ∂xr ∂yr ∂θr we obtain the expression for the line covariance in the robot frame Cx()RL using first-order error propagation

T T Cx()RL = Fg Cx()WL Fg + Fx Cx()WR Fx (B.16) where we assume no robot-to-feature cross-correlations. The partial derivatives of (B.14) and (B.15) have been cast into the matrices Fg and Fx respectively. Note that none of them depends on the visibility of the feature. The concrete expressions are

R 2 2 2 σα = []σα g + []σθ x (B.17)

2 R 2 ------∂fr 2 2------∂fr 2 σr = σα ++σαr σr ∂α ∂α g sin cos 2 sin cos sin 2 cos + []α()σxy α + σy α + α()σxy α + σx α x (B.18)

∂ Rσ = σ 2 ------fr + σ + []σ cos α + σ sin α (B.19) αr α αr xθ yθ x ∂α g where the contribution of the first term in (B.16) is indicated by []. g and those of the second by []. x . An important property of lines represented in the Hessian form (B.1) is that the uncertainty is not invariant against frame transforms. This can be seen in expression

(B.18) which depends on ∂αfr ⁄ ∂ being a (squared) function of the robot position. This R 2 means that the uncertainty in the parameter r , σr , is big if the robot is far away and is small if it is close to the origin of {}W .

B.1.3 Visibility Condition

W W Given a robot pose ()xr,,yr θ r and a line ()α , r we want to know whether the line is visible for the robot or not. For this, we introduce the visibility information according to

164 B.2 POINT AND CORNER FEATURES

W W R R W (B.4) with ()αv , rv . Let r be the r of equation (B.8) computed with r instead of W W rv , that is, with a r being positive. Then we conclude from equation (B.8) and figure W R B.2 that a line is invisible if rv and r have different sign

line invisible ⇔ ()()R > 0 ∧ ()W < 0 ∨ ()()R < 0 ∧ ()W > 0 r rv r rv (B.20) R W ⇔ ()r ⋅ rv < 0 Using equation (B.8) this leads to the condition of visibility

line visible ⇔ ()⋅ W < W ⋅ W drv rv r (B.21) W ⇔ ()dv < rv W W with dv = xr cos αv + yr sin αv . The last simplification is possible due to the definition of αv in equation (B.4) which causes dv to change its sign accordingly. Given the definition (B.4) and the transform (B.8), (B.9) we can further simplify both, the R transform and the visibility test. A line invisible from Wr causes to change its sign and R π to be added to α . A robot at the opposite side of the line w.r.t. W , has according to R R (B.8) a negative r and α turned by π . In this case and in general, it is easy to see that R visibility and robot pose compensate each other yielding an always positive r and an always correct Rα for lines that are visible from the current robot location.

R W W W r = rv – xr cos αv – yr sin αv (B.22)

R W α = αv – θ r (B.23)

R line visible ⇔ ()r > 0 (B.24) R R For invisible lines, r will be negative and α is turned about π .

B.2 Point and Corner Features

A corner features gc is the most general spatial relationship in two dimensions. A point feature gp is a corner feature without orientation. They are defined as

gc = ()xy,,ϕ ; gp = ()xy, (B.25) Frame transformation of the corner feature contains the results for point features as well as angle features which will be defined in the next subsection. Due to the importance of this relationship and the complete yet very compact discussion in (Smith, Self et al. 1990), we briefly derive the solution.

165 B. FEATURE DEFINITIONS

{}C x y xWC

xRC x xWR x {}W xRW y {}R Figure B.3. Spatial relationships when transforming a corner feature into the robot reference frame

Attaching a reference frame {}C to gc , we look for gc expressed in {}R , that is xRC given the uncertain relationships xWC = ()xi ,,yi ϕi and xWR = ()xr ,,yr θr

xWC = xWR ∆ xRC (B.26) The solution is derived using the compound and inverse operator (Smith, Self et al. 1990). Remember that these transformations involve rotations, so they are not merely vector addition.

= û ∆ xRC xWR xWC (B.27) = xRW ∆ xWC

Substituting the formulae of the respective operator yields the sought xRC ,

()xi – xr cos θr + ()yi – yr sin θr xRC = –()xi – xr sin θr + ()yi – yr cos θr . (B.28) ϕi – θr The Jacobian of the inverse operator is

– cos θr – sin θr xr sin θr – yr cos θr Jû = sin θr – cos θr xr cos θr + yr sin θr . (B.29) 00– 1 The Jacobian of the compound operator can be divided into its left 33× - and its right 33× -half

J∆ = J1∆ J2∆ (B.30) with submatrices

166 B.2 POINT AND CORNER FEATURES

10–yi cos θr + xi sin θr = (B.31) J1∆ 01 xi cos θr + yi sin θr 00 1

– cos θr – sin θr ()yi – yr cos θr + ()– xi + xr sin θr = (B.32) J2∆ sin θr – cos θr ()– xi + xr cos θr – ()yi – yr sin θr 00– 1

Given the covariance matrix of the feature, Cx()WC , the robot Cx()WR and their cross- covariance terms Cx()WC, xWR , Cx()WR, xWC , the first-order covariance estimate of the composite relationship (B.27) is

() (), ()= CxRW CxRW xWC T CxRC J∆ (), ()J∆ CxWC xRW CxWC (B.33) ()T (), = Jû CxWR Jû Jû CxWR xWC T J∆ T J∆ Cx()WC , xWR Jû Cx()WC If we assume that the cross-covariance terms are zero, we get

T T T Cx()RC = J1∆ Jû Cx()WR Jû J1∆ + J2∆ Cx()WC J2∆ (B.34) If equation (B.34) shall be implemented with a side-glance to machine efficiency, or if no matrix library is available, we might develop the six assignments given in table B.1 explicitly (Cx()RC is symmetric).

(((sxxi + sxxr + ((yi - yr))*(((-2)*sxtr + sttr*yi - sttr*yr))))*cos(thetar)^2 + ((syyi + syyr + ((xi - xr))*((2*sytr + sttr*xi - sttr*xr))))*sin(thetar)^2 + sxx ((sxyi + sxyr + sxtr*xi - sxtr*xr - sytr*yi - sttr*xi*yi + sttr*xr*yi + sytr*yr + sttr*xi*yr - sttr*xr*yr))*sin(2*thetar)) -sin(thetar)*(sxxi*cos(thetar) + sxyi*sin(thetar)) + cos(thetar)*(sxyi*cos(thetar) + syyi*sin(thetar)) - sin(thetar)*((sxxr + sxtr*(- yi + yr))*cos(thetar) + (sxyr + sxtr*(xi - xr))*sin(thetar)) + cos(thetar)*((sxyr sxy + sytr*(-yi + yr))*cos(thetar) + (syyr + sytr*(xi - xr))*sin(thetar)) + ((sxtr + sttr*(-yi + yr))*cos(thetar) + (sytr + sttr*(xi - xr))*sin(thetar))*((xi - xr)*cos(thetar) + (yi - yr)*sin(thetar)) (sxti + sxtr - sttr*yi + sttr*yr)*cos(thetar) + (syti + sytr + sttr*xi - sxt sttr*xr)*sin(thetar) (((syyi + syyr + ((xi - xr))*((2*sytr + sttr*xi - sttr*xr))))*cos(thetar)^2 + ((sxxi + sxxr + ((yi - yr))*(((-2)*sxtr + sttr*yi - sttr*yr))))*sin(thetar)^2 - syy ((sxyi + sxyr + sxtr*xi - sxtr*xr - sytr*yi - sttr*xi*yi + sttr*xr*yi + sytr*yr + sttr*xi*yr - sttr*xr*yr))*sin(2*thetar)) (syti + sytr + sttr*xi - sttr*xr)*cos(thetar) - (sxti + sxtr - sttr*yi + syt sttr*yr)*sin(thetar) stt stti + sttr

Table B.1. The elements of Cx()RC as direct expressions

For point features, the same formulae apply. The first moments are given by equation 2 2 (B.28), the second moments by the expressions for σx , σy and σxy in table B.1. All expressions do not depend on the moments of the orientation ϕ .

167 B. FEATURE DEFINITIONS

B.3 Angle Features

Angle features ga differ in their local and global representation. As map features in the global map, they are stored as ()xy, -points with an associated 22× -covariance matrix, 2 whereas locally, their observation yields an angle ϕ with angular variance σϕ . Using the results of the previous section, we assume to have a position ()xy, with asso- 2 2 ciated uncertainties σx , σy and σxy already expressed in the robot frame. The sought angle in the robot frame is then

ϕ = atan ()yx⁄ . (B.35) The partial derivatives of (B.35) with respect to the uncertain variables are

------∂f = ------–y ------∂f- = ------x - . (B.36) ∂x x2 + y2 ∂y x2 + y2 This yields the sought variance

2 2 2 ∂f 2 ∂f 2 ∂f ∂f σϕ = ------σx ++------σ 2------σxy ∂x ∂y y ∂x ∂y (B.37) 2 2 2 2 2 2 2 = []x σx + y σy – 2xyσxy ⁄ ()x + y In practice, (B.35) will use the four-quadrant arcus tangens atan2(). which adds or sub- 2 tracts π in function of the quadrant of the point ()xy, . Note that σϕ is independent on this.

168 Appendix C

Line Fitting to Points in Polar Coordinates

This appendix derives the expressions when fitting a line to points in polar coordinates in a weighted least squares sense minimizing perpendicular errors from the points to the line. As it will be shown, the problem, despite being nonlinear in the regression parameters, has a closed form solution.

C.1 First Moments

Consider the nonlinear equation system

∂S = 0 ; ∂S = 0 (C.1) ∂r ∂α where S is the weighted sum of squared errors

n cos cos sin sin 2 S =.∑ wi()ρi θi α + ρi θi α – r (C.2) i = 1 We start solving the system (C.1) by working out parameter r .

∂S = 0 =∑2w ()ρ cosθ cosα + ρ sinθ sinα – r ()–1 (C.3) ∂r i i i i i = –2 ρ ()cosθ cosα + sinθ sinα + 2 (C.4) ∑wi i i i ∑wir = –2 ρ cos()θ – α + 2 (C.5) ∑wi i i rw∑ i 2 = 2 ρ cos()θ – α (C.6) rw∑ i ∑wi i i

ρ cos()θ – α ∑wi i i r = ------(C.7) ∑wi

Parameter α is slightly more complicated. We introduce the following notation

cos sin θi = ci ; θi = si . (C.8)

169 C. LINE FITTING TO POINTS IN POLAR COORDINATES

∂S ∂ = ∑2w ()ρ c cosα + ρ s sinα – r ()ρ c cosα + ρ s sinα – r (C.9) ∂α i i i i i ∂α i i i i

1 2 cos sin cos sin cos ∂ = ∑wi ρici α + ρisi α – ------∑wjρj ()θj – α –ρici α + ρisi α – r Σwj ∂α 1 2 cos sin cos = ∑wi ρici α + ρisi α – ------∑wjρj ()θj – α Σwj 1 cos sin sin ⋅ ρisi α – ρici α – ------∑wjρj ()θj – α (C.10) Σwj

2 2 2 1 2 2 = 2 ρ cos2 α – ρ cosαsinα – ------ρ cosα ρ sin()θ – α + ρ cosαsinα ∑wi i cisi i ci ici ∑wj j j i si Σwj 2 1 1 – ρ sin2 α – ------ρ sinα ρ sin()θ – α – ------ρ cos()θ – α ρ cosα i sici isi ∑wj j j ∑wj j j si Σwj Σwj i 1 1 + ------∑w ρ cos()θ – α ρ s cosα + ------∑w ρ cos()θ – α ∑w ρ sin()θ – α w j j j i i 2 j j j j j j Σ j ()Σwj 2 2 2 2 1 2 cos2 sin2 cos sin cos sin = ∑wi ρi cisi()α – α – ρi α α()ci – si – ------∑wjρiρjci α ()θj – α Σwj 1 1 – ------ρ ρ sinαsin()θ – α – ------ρ ρ cosαcos()θ – α ∑wj i jsi j ∑wj i jsi j Σwj Σwj 1 1 + ------∑w ρ ρ c sinαcos()θ – α + ------∑∑w w ρ ρ cos()θ – α sin()θ – α w j i j i j 2 j k j k j k Σ j ()Σwj j k 2 2 = 22cos α ρ – 2cosαsinα ρ ∑wi i cisi ∑wi i c2i 2 + ------ρ ρ ()cosα + sinα ()cosα – sinα ∑∑wiwj i j ci si sj cj Σwi 2 + ------ρ ρ [– cosα()cosα – sinα – sinα()cosα – sinα ∑∑wiwj i j ci sj cj si sj cj Σwi cos cos sin sin cos sin ++si α()cj α + sj α ci α()cj α + sj α ] (C.11)

2 2 =cos2α ρ – sin2α ρ ∑wi i s2i ∑wi i c2i 2 + ------ρ ρ [ cos2 α – sinαcosα + sinαcosα – sin2 α ∑∑wiwj i j cisj cicj sisj sicj Σwi cos2 sin cos sin cos sin2 cos2 – cisj α + cicj α α – sisj α α + sicj α – sicj α sin cos sin cos sin2 – sisj α α ++cicj α α cisj α ] (C.12)

2 2 = cos2α ρ – sin2α ρ ∑wi i s2i ∑wi i c2i 2 + ------ρ ρ ()– cos2 α – sinαcosα ++sinαcosα sin2 α (C.13) ∑∑wiwj i j sicj sisj cicj cisj Σwi

2 2 = cos2α ρ – sin2α ρ ∑wi i s2i ∑wi i c2i 2 + ------ρ ρ []sinαcosα()– + sin2 α – cos2 α (C.14) ∑∑wiwj i j cicj sisj cisj sicj Σwi

170 C.2 SECOND MOMENTS

2 2 2 = cos2α ρ – sin2α ρ + ------sinαcosα ρ ρ ∑wi i s2i ∑wi i c2i ∑∑wiwj i jcij+ Σwi 2 2 + ------sin2 α ρ ρ – ------cos2 α ρ ρ (C.15) ∑∑wiwj i jcisj ∑∑wiwj i jsicj Σwi Σwi

2 2 1 = cos2α ρ – sin2α ρ + ------sin2α ρ ρ ∑wi i s2i ∑wi i c2i ∑∑wiwj i jcij+ Σwi 2 – ------ρ ρ ()cos2 α – sin2 α (C.16) ∑∑wiwj i jcisj Σwi

2 2 = cos2αρ – ------ρ ρ ∑wi i s2i ∑∑wiwj i jcisj Σwi 2 1 – sin2αρ – ------ρ ρ (C.17) ∑wi i c2i ∑∑wiwj i jcij+ Σwi From (C.17) we can obtain the result for α or tan 2α respectively.

2 2 ------ρ ρ – ρ ∑∑wiwj i jcisj ∑wi i s2i sin2 wi ------α- = ------Σ - (C.18) cos2α 1 2 ------ρ ρ – ρ ∑∑wiwj i jcij+ ∑wi i c2i Σwi

2 2 ------ρ ρ cosθ sinθ – ρ sin2θ ∑∑wiwj i j i j ∑wi i i Σwi tan2α = ------(C.19) 1 2 ------ρ ρ cos()θ + θ – ρ cos2θ ∑∑wiwj i j i j ∑wi i i Σwi Equation (C.19) contains double sums which needs not be fully evaluated. Due to the symmetry of trigonometric functions the corresponding off-diagonal elements can be added and thus simplifies calculation. For the final result (C.20), the four-quadrant arc tangent is used (Craig 1989).

2 1 2 ------ρ ρ sin()θ + θ + ------()– wj ρ sin2θ ∑ ∑ wiwj i j i j ∑ wi Σ wi i i 1 Σwi Σwi α = ---atan2------ij< ------(C.20) 2 2 1 2 ------∑ ∑ w w ρ ρ cos()θ + θ + ------∑()w – Σwj w ρ cos2θ w i j i j i j w i i i i Σ i ij< Σ i

C.2 Second Moments

The model parameter α and r are just half the battle. Besides these estimates of the mean position of the line we would also like to have a measure for its uncertainty. According to the error propagation law (Breipohl 1970), (Arras 1998), an approximation of the output uncertainty, represented by the 22× covariance matrix CAR , is subsequently determined.

171 C. LINE FITTING TO POINTS IN POLAR COORDINATES

At the input side, mutually independent uncertainties in radial direction only are assumed. T T T The 2n × 1 input random vector X = []P Q consists of the random vector P T = []P1,,,P2 … Pn denoting the variables of the measured radii, and the random vector T QQ = []1,,,Q2 … Qn holding the corresponding angular variates. The 2n × 2n input covariance matrix CX is therefore of the form

2 CP 0 diag()σρ 0 C ==i . (C.21) X 0 CQ 0 0

We represent both output random variables A , R by their first-order Taylor series ex- T T T pansion about the mean µ = []µρ µθ . The vector µ has dimension 2n × 1 and is T composed of the two n ×µ1 mean vectors = []µ ,,,µ …µ and ρ ρ1 ρ2 ρn µ = []µ ,,,µ …µ T . θ θ1 θ2 θn ∂α ∂α A ≈ α()µ∗ ++∑ []P – µρ ∗ ∑ []Q – µθ ∗ (C.22) i i i i ∂Pi ∂Qi X= µ∗ X= µ∗

∂r ∂r Rr≈ ()µ∗ ++∑ []P – µρ ∗ ∑ []Q – µθ ∗ (C.23) i i i i ∂Pi ∂Qi X= µ∗ X= µ∗ The relationships r(). and α(). are the results of equations (C.7) and (C.20). We do not know µ in advance and its best available guess is the actual value of the measurements vector µ∗ .

In (Arras 1998) it has been shown that under the assumption of independence of P and Q , the following holds

2 2 2 ∂α 2 ∂α 2 σ ≈ ∑ σρ + ∑ σθ (C.24) A i i ∂Pi ∂Qi

2 2 2 ∂r 2 ∂r 2 σ ≈ ∑ σρ + ∑ σθ (C.25) R i i ∂Pi ∂Qi

We further want to know the covariance σAR under the abovementioned assumption of negligible angular uncertainties:

COV[] A, R =EA[]⋅ R – EA[]ER[] (C.26)

∂α ∂r = E α + ∑ []P – µρ ⋅αr + ∑ []P – µρ – r (C.27) i i i i ∂Pi ∂Pi

∂α ∂r ∂α ∂r = E αrr++∑ []P – µρ α∑ []P – µρ +∑ []P – µρ ∑ []P – µρ i i i i i i i i ∂Pi ∂Pi ∂Pi ∂Pi – αr (C.28)

172 C.2 SECOND MOMENTS

∂α ∂α ∂r ∂r = E[]αr ++rE ∑ P – ∑ µρ αE ∑ P – ∑ µρ i i i i ∂Pi ∂Pi ∂Pi ∂Pi ∂α ∂r + E ∑∑ []P – µρ []P – µρ – αr (C.29) i i j j ∂Pi∂Pi

∂ ∂α ∂r ∂ = αrr++∑ ()α EP[]– r∑ E[]µρ α∑ EP[]– α∑ ()r E[]µρ i i i i ∂Pi ∂Pi ∂Pi ∂Pi ∂α ∂r ∂α ∂r 2 + E ∑ ∑ []P – µρ []P – µρ + ∑ []P – µρ – αr (C.30) i i j j i i ij≠ ∂Pi∂Pi ∂Pi∂Pi

∂α ∂r ∂α ∂r 2 = E ∑ ∑ ()P P – P µ – P µ + µ µ + E ∑ []P – µρ (C.31) i j i Pj j Pi Pi Pj i i ij≠ ∂Pi∂Pi ∂Pi∂Pi

∂α ∂r ∂α ∂r 2 = ∑ ∑ EP[]P – P µ – P µ + µ µ + ∑ EP[]()– µρ (C.32) i j i Pj j Pi Pi Pj i i ij≠ ∂Pi∂Pi ∂Pi∂Pi

∂α ∂r ∂α ∂r 2 = ∑ ∑ EP[]P – P µ – P µ + µ µ + ∑ σρ (C.33) i j i Pj j Pi Pi Pj i ij≠ ∂Pi∂Pi ∂Pi∂Pi

Since Pi and Pj are independent, the expected value of the bracketed expression disappears. Hence

∂α ∂r 2 σ = ∑ σρ . (C.34) AR i ∂Pi∂Pi If, however, the input uncertainty model provides non-negligible angular variances, it is easy to show that under the independency assumption of PQ and the expression keeping track of Q can be simply added to yield

∂α ∂r 2 ∂α ∂r 2 σ = ∑ σρ + ∑ σθ . (C.35) AR i i ∂Pi∂Pi ∂Qi∂Qi It follows from the error propagation law that the results (C.24), (C.25) and (C.35) can also be obtained in the more compact but less intuitive form

T CAR = FPQCXFPQ . (C.36) With

∂α ∂α ∂α ∂α ∂α ∂α ∂α ∂α … … ∂ ∂ ∂ ∂ ∂ ∂ ==∂P ∂Q =P1 P2 Pn Q1 Q2 Qn (C.37) FPQ FP FQ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ r r r r … r r r … r ∂P ∂Q ∂P1 ∂P2 ∂Pn ∂Q1 ∂Q2 ∂Qn being the composed p × 2n Jacobian matrix containing all partial derivatives of the mod-

173 C. LINE FITTING TO POINTS IN POLAR COORDINATES el parameters with respect to the input random variables about the guess µ∗ . Under the conditions which lead to equation (C.35), the right hand side can also be decomposed to

T T CAR = FPCPFP + FQCQFQ . (C.38)

C.3 Practical Considerations

We determine implementable expressions for the elements of covariance matrix C . Un- 2 AR2 der the assumption of negligible angular uncertainties, concrete expressions of σA , σR and σAR will be derived. We keep in mind that the problem might be a real time problem, requiring an efficient implementation. Expression are therefore sought which minimize the number of floating point operations, e.g. by reuse of already used subexpressions. Although calculating with weights does not add much difficulty, we will omit them in this chapter1. For the sake of brevity we introduce the following notation

1 α = ---atan2----D (C.39) 2 N with

2 2 N = --- ∑∑P P cosQ sinQ – ∑P sin2Q , (C.40) n i j i j i i

1 2 D = --- ∑∑P P cos()Q + Q – ∑P cos2Q . (C.41) n i j i j i i We will use the result that the parameters α (C.20) and r (C.7) can also be written in Car- tesian form, where xi = ρi cos θi and yi = ρi sin θi :

1 –2∑()yy– ()xx– α = --- atan2 ------i i , (C.42) 2 2 2  ∑()yy– i – ()xx– i

rx= cosα + ysinα . (C.43) With the means

1 1 x = ⁄ n ⋅ ∑xi y = ⁄ n ⋅ ∑yi . (C.44) From equations (C.24), (C.25) and (C.34) we see that the covariance matrix is defined when both partial derivatives of the parameters with respect to Pi are given. Let us start with the derivative of α .

1. See chapter 3 for the results with weights. Performance comparison results for three different ways to determine α and r are also given.

174 C.3 PRACTICAL CONSIDERATIONS

∂D ∂N ∂D ∂N N – D N – D ∂α 1 1 ∂Pi ∂Pi 1 ∂Pi ∂Pi ==--- ⋅ ------⋅ ------⋅ ------(C.45) 2 2 2 2 2 2 2 ∂Pi 1 + N ⁄ D D D + N

The partial derivatives of the numerator and the denominator with respect to Pi can be obtained as follows:

∂ ∂ 2 ∂ 2 N = --- ∑∑ cos sin – {}∑ sin2 (C.46) ∂ PjPk Qj Qk Pj Qj Pi ∂Pin ∂Pi

2 2 2 sin2 ∂ = – Pi Qi + --- {P1c1s1 ++++P1c1P2s2 P1c1P3s3 P1c1P4s4 … n ∂Pi 2 +++++P2c2P1s1 P2c2s2 P2c2P3s3 P2c2P4s4 … 2 +++++P3c3P1s1 P3c3P2s2 P3c3s3 P3c3P4s4 … } (C.47)

2 ∂ ∂ = --- {}cos sin + {}cos sin – 2 sin2 (C.48) ∑ Pj QjPi Qi ∑ Pi QiPj Qj Pi Qi n ∂Pi ∂Pi 2 = --- ()sinQ ∑P cosQ + cosQ ∑P sinQ – 2P sin2Q (C.49) n i j j i j j i i 2 = --- ()sinQ nx+ cos Q ny – 2P sin2Q (C.50) n i i i i

2 sin cos sin2 =()xQi + yQi – Pi Qi (C.51) The mean values x and y are those of equation (C.44).

∂ ∂ 1 ∂ 2 D = --- ∑∑ cos()+ – {}∑ cos2 (C.52) ∂ PjPk Qi Qj Pj Qj Pi ∂Pin ∂Pi

1 2 2 cos2 ∂ = – Pi Qi + --- {P1c11+ ++++P1P2c12+ P1P3c13+ P1P4c14+ … n ∂Pi 2 +++++P2P1c21+ P2c22+ P2P3c23+ P2P4c24+ … 2 +++++P3P1c31+ P3P2c32+ P3c33+ P3P4c34+ … } (C.53)

1 ∂ cos ∂ cos 2 cos2 = --- {}∑PjPi ()Qi + Qj + {}∑PiPj ()Qj + Qi – Pi Qi (C.54) n ∂Pi ∂Pi 2 ∂ cos 2 cos2 = --- {}∑PjPi ()Qi + Qj – Pi Qi (C.55) n ∂Pi 2 = --- ∑P cos()Q + Q – 2P cos2Q (C.56) n j i j i i

175 C. LINE FITTING TO POINTS IN POLAR COORDINATES

2 2 = --- ∑P cosQ cosQ – --- ∑P sinQ sinQ – 2P cos2Q (C.57) n j i j n j i j i i 2 2 = --- cosQ ∑P cosQ – --- sinQ ∑P sinQ – 2P cos2Q (C.58) n i j j n i j j i i 2 2 = --- cosQ nx – --- sinQ ny – 2P cos2Q (C.59) n i n i i i

2 cos sin cos2 = ()xQi – yQi – Pi Qi (C.60) Substituting into equation (C.45) gives

∂D ∂N N – D ∂α 1 ∂P ∂P = --- ⋅ ------i i (C.61) ∂ 2 2 2 Pi D + N

1 2()xQcos – yQsin – P cos2Q N – 2()xQsin + yQcos – P sin2Q D = --- ⋅ ------i i i i i ------i i i (C.62) 2 2 2 D + N

Nx()cos Q– yQsin – P cos2Q – Dx()sin Q+ yQcos – P sin2Q ------i i i i i ------i i i- = 2 2 (C.63) D + N It remains the derivative of r :

∂ ∂ 1 r = --- ∑ cos()– α (C.64) ∂ Pj Qj Pi ∂Pin

1 ∂ cos = --- ∑ {}Pj ()Qj – α (C.65) n ∂Pi 1 1 cos ∂ ∂ cos = --- ∑ ()Qj – α {}Pj + --- ∑Pj {}()Qj – α (C.66) n ∂Pi n ∂Pi

1 1 cos sin ∂α = --- ()Qi – α + --- ∑Pj ()Qj – α (C.67) n n ∂Pi

1 1 cos ∂α sin = --- ()Qi – α + --- ∑Pj ()Qj – α (C.68) n ∂Pin

1 ∂α ∂ cos r = --- ()Qi – α + ⋅ (C.69) n ∂Pi ∂α 1 cos ∂α cos sin = --- ()Qi – α + ()y α – x α (C.70) n ∂Pi

Note that we made use of the already known expression ∂α ⁄ ∂Pi .

176 C.3 PRACTICAL CONSIDERATIONS

Let us summarize the results. The sought covariance matrix is

2 σ σ C = A AR (C.71) AR 2 σAR σR with elements

2 1 2 2 σ = ------∑[]Nx()cosθ – ysinθ – ρ cos2θ – Dx()sinθ + ycosθ – ρ sin2θ σρ A 2 2 2 i i i i i i i i i ()D + N (C.72)

2 2 1 ∂α 2 σ = ∑ --- cos()θ – α + ()ycosα – xsinα σρ (C.73) R i i n ∂Pi

∂α ∂r 2 σ = ∑ σρ (C.74) AR i ∂Pi∂Pi

All elements of CAR are of complexity On() and allow extensive reuse of already computed expressions. This makes them suitable for fast calculation under real time, limited computing power and the absence of libraries for matrix computation.

177 C. LINE FITTING TO POINTS IN POLAR COORDINATES

178 Appendix D

Quick Guide to the SPmodel

This appendix is a brief introduction to and reference of the Symmetries and Perturbations Model (Tardos 1992) and summarizes chapter 2 and chapter 6 of (Castellanos & Tardos 1999). Despite its usefulness, the SPmodel has not yet found broad proliferation in the robotics community. Since chapter 7 makes use of it, an overview of basic concepts is given.

D.1 Quick Guide to the SPmodel

The non-triviality of how uncertain geometric entities can be represented, transformed and compared leads us to the study of the Symmetries and Perturbations Model (SPmodel). The SPmodel is a probabilistic model that allows to unify the representation of geometric feature or sensorial observation and its uncertainty. The user is raised from the level of feature parameters and single coordinates onto the level of uncertain transforms.

D.1.1 Representing Different Geometric Features

The SPmodel associates a reference E to every geometric element E , whose axes are aligned with the geometric element. The reference attached to a point, for example, must coincide with the point, the reference attached to a infinite straight line must have its x - axis along the line.

The location of a geometric element E is defined with respect to a reference, say W , by a transformation t which is represented by a location vector xWE . In 3D xWE is formed by three Cartesian coordinates and three roll-pitch-yaw angles:

xWE = ()xyz,,,ψθφ ,, (D.1) in the 2D case, the location reduces to two Cartesian coordinates and an angle:

xWE = ()xy,,φ (D.2) To represent different types of geometric elements, we use the concept of symmetry. The symmetries of a geometric element are the set of transformations that preserve the element.

179 D. QUICK GUIDE TO THE SPMODEL

We represent the set of continuous symmetries of a geometric element using a row selection matrix BE , denominated binding matrix. The binding matrix represents the degrees of freedom associated to a location. Thus, BE decodes the type of geometric element (see table D.1 for different 2D elements). Further, the binding matrix of a geometric entity allows us to express in a generic way, independent on the type, one of the fundamental geometric concepts, coincidence. Two geometric entities with attached frames AB and coincide up to their symmetries if

BAB xAB = 0 (D.3) holds. BAB is called the binding matrix of the pairing. It contains the conjunctive (in a Boolean sense) symmetries of the entities involved.

Example: Coincidence between two point features Given two 2D point features with attached references AB and , whose relative location is:

T xAB = []xAB,,yAB φAB (D.4)

== =100 (D.5) BA BB BAB 010 According to equation D.3 and table D.1 we have:

= T BAB xAB []xAB, yAB (D.6) = 0 The result expresses the (evident) fact that two points coincide if the relative position of their associated frames AB and is zero, regardless of what their relative orientation is.

Example: Coincidence between a line and a point Consider a line EP and a point , whose relative location is given by:

T xEP = []xEP ,,yEP φEP (D.7)

= 010 ; = 100 (D.8) BE 001 BP 010 According to table D.1 and the conjunction of the symmetries of points and lines, the binding matrix of the pairing is:

BEP = 010 (D.9) Finally, according to equation D.3 we have:

= BEP xEP yEP (D.10) = 0

180 D.1 QUICK GUIDE TO THE SPMODEL

That is, the point belongs to the line if its position in the direction of the y -axis of the line reference E (the perpendicular distance from the point to the line) equals zero.

D.1.2 Transformations and Jacobians

The composition of two transformations, given by their location vectors, is expressed by:

xWF = xWE ∆ xEF (D.11)

The composite vector xWF expresses now the geometric element F in the reference frame W . In 2D we have:

xEF cos φWE – yEF sin φWE + xWE xWF ==xWE ∆ xEF xEF sin φWE ++yEF cos φWE yWE (D.12) φWE + φEF

Similarly, given the location of a geometric element E in W , the inversion of a transformation is denoted by:

xEW = ûxWE (D.13) which expresses the location of the entity W in E . In 2D we have:

– xWE cos φWE – yWE sin φWE xEW ==ûxWE xWE sin φWE – yWE cos φWE (D.14) –φWE Note that if we had no orientations, the compounding and its inversion operations would reduce to vector addition and subtraction.

D.1.3 Representing Uncertainty

In classical probabilistic models sensor imprecision is represented by a white noise Gaus- sian random variable, characterized by its mean and its covariance matrix. Our representation uses this same idea, but employs a local representation of uncertainty. In the SPmodel, the estimated location of a geometric element F with respect to a reference, say W , is denoted by xˆ WF (hat!) representing the best (currently available) approximation of the real location. The estimation error is represented locally by a differential location vector dF relative to the reference attached to F . Therefore, the true location of the element is given by the composition

181 D. QUICK GUIDE TO THE SPMODEL

xWF = xˆ WF ∆ dF (D.15) with dF expressed as:

T dF = []dx,,d y dφ (D.16)

To account for the symmetries of the geometric element, we assign in dF a null value to those components which correspond to the degrees of freedom of the element. They do not effectively represent an error. The degrees of freedom of a geometric element F are removed from further calculation by defining a random vector called perturbation vector pF formed by premultiplication of the binding matrix BF :

T pF = BF dF ; dF = BF pF (D.17) See table D.1 for the perturbation vector for different types of geometric elements.

Feature Binding Matrix Perturbation Vector

Point = 100 = T B 010 p dx dy

Inﬁnite Line = 010 = T B 001 p dy dφ

Corner B = I3 p = d

Vision Edge B = 001 p = dφ

Robot B = I3 p = d Table D.1. SPmodel representation of some geometric entities Summarizing, the SPmodel represents the information about the location of a geometric element F by a quadruple:

L WF = ()xˆ WF,,pˆ F CF ,BF (D.18) called the uncertain location of the geometric element F , where:

T xWF = xˆ WF ∆ BF pF (D.19)

pˆ F = E[]pF (D.20)

CF = E[]()pF – pˆ F ()pF – pˆ F (D.21)

The transformation xˆ WF is an estimation taken as base for perturbations, pˆ F is the estimated value of the perturbation vector, and CF its covariance. When pˆ F = 0 we say that

182 D.1 QUICK GUIDE TO THE SPMODEL the estimation is centered. The main advantage of the SPmodel is its generality: it is valid for any geometric feature or sensorial observation. Note that the representation of uncertainty using a perturbation vector does not depend on the base reference used, has a clear interpretation, is not overparametrized, and has no problems related to singularities in the parameters.

D.1.4 Operations with Uncertain Locations: Compounding

Consider a geometric element F , whose uncertain location with respect to reference E is expressed by LEF . And suppose that E is also affected by a location error with respect to W , expressed by L WE . Then the uncertain location of FW with respect to is obtained as follows:

∆ xWF = xWE xEF = ˆ ∆ T ∆ ˆ ∆ T x WE BE pE x EF BF pF (D.22) T T = ()xˆ WE ∆ xˆ EF ∆ ()JFEBE pE ∆ BF pF W = xˆ WF ∆ dF Assuming small errors yields:

W T pF ≈ BF JFEBE pE + pF . (D.23) Thus, the uncertain location of FW with respect to is given by ˆ W W ˆ W W L WF = ()xWF ,,pˆ F CF ,BF L WF = ()xWF ,,pˆ F CF ,BF , with:

xˆ WF = xˆ WE ∆ xˆ EF (D.24)

W T pF = BF JFEBE pE + pF (D.25)

W T T T CF = BF JFEBE CE BEJFE BF + CF (D.26) where we assume that the estimations of EF and are not correlated, i.e. CEF = 0 .

D.1.5 Operations with Uncertain Locations: Inversion

Consider further the uncertain location LEF = ()xˆ EF ,,pˆ F CF ,BF of a geometric element F expressed in the reference E . Its inverse LFE = ()xˆ FE ,,pˆ E CE ,BE is given by:

183 D. QUICK GUIDE TO THE SPMODEL

xFE = ûxEF T = û()xˆ EF ∆ BF pF T = ûBF pF ûxˆ EF (D.27) T = ûBF pF ∆ xˆ FE T = xˆ FE ûJEFBF pF Assuming small errors yields:

xˆ FE = ûxˆ EF (D.28)

T pE — –BF JEF BF pF (D.29)

BE = BF (D.30)

T T T CE = BF JEF BF CF BF JEF BF (D.31)

D.1.6 Operations with Uncertain Locations: Centering

Given an uncertain location L WF = ()xˆ WF,,pˆ F CF ,BF , where pˆ F ≠ 0 , it can be transformed to L WF' = ()xˆ WF' ,,pˆ F ' CF ' ,BF with pˆ F ' = 0 in the following way:

T xˆ WF' = xˆ WF ∆ BF pˆ F (D.32)

–1 T T –1 T T T CF ' = ()BF J2∆{}BF pˆ F , 0 BF CF ()BF J2∆{}BF pˆ F , 0 BF (D.33)

When pˆ F = 0 , the uncertain location is named centered. This operation considerably simplifies the expressions and calculations involved in fusing different observations.

D.2 Quick Guide to the SPmap

Based on the previous section, this section derives the stochastic map using the SPmodel, which the authors call SPmap.

D.2.1 Formulation of the SPmap

The estimated location vector of the mobile robot and the map features is

184 D.2 QUICK GUIDE TO THE SPMAP

xˆ WR xˆ xˆ W = WF1 (D.34) ∂ xˆ x WFN where xˆ is the estimated location of the mobile robot and xˆ is the estimated location x WR x WFi F of the feature i , i ∈ {}1…N both with respect to the base reference W . Imprecision in the SPmap is represented by a set of white-noise Gaussian random variables – the perturbation vector of the robot and the map features – which are stacked together in the system state vector pW :

dR p pW = F1 (D.35) ∂ p FN where d is the perturbation vector of the robot, and p , i ∈ {}1…N , is the perturbation R Fi vector of each map feature.

The system covariance matrix C W represents the uncertainty in the estimated locations of the robot and the map features and their correlations:

C C C µ C R RF1 RF2 RFN C T C C µ C RF1 F1 F1F2 F1FN W = C T C T C µ C (D.36) C RF2 F1F2 F2 F2FN ∂∂∂∏∂ C T C T C T µ C RFN F1FN F2FN FN which is formed by the covariance matrix of the robot CR , the covariance matrix of each map feature C , i ∈ {}1…N , the cross-covariance matrices between the robot and each Fi of the map features C , i ∈ {}1…N , and finally the cross-covariance matrices be- RFi tween the map features C , ij, ∈ {}1…N . FiFj To summarize, the SPmap can be interpreted as a quadruple:

SPmap = ()xˆ W,,pW C W ,B W (D.37) with:

x W = xˆ W ∆ ()B W TpW (D.38)

pˆ W = E[]pW (D.39)

C W = E[]()pW – pˆ W ()pW – pˆ W T (D.40)

185 D. QUICK GUIDE TO THE SPMODEL where B W is the binding matrix of the SPmap, which takes into account symmetries in the location of features. Is is a diagonal matrix formed by the self-binding matrices of the robot B and each of the map features B , i ∈ {}1…N : R Fi B W = diag()B ,,,,B B … B (D.41) R F1 F2 FN The dimensions of the different elements of the SPmap are as follows:

xˆ W : 3()N + 1 (D.42)

pW : rank()BW (D.43)

W B : 3 + ∑ rank()BF (D.44) i i C W : rank()BW × rank()BW (D.45) where N is the number of map features included in the SPmap. During map building, the perturbation vector of the SPmap pW and its covariance matrix C W are obtained by application of suboptimal estimation based on the EKF.

D.2.2 Uncertain Displacement of the Mobile Robot

We assume that the only dynamic entity in the problem is the mobile robot. An estimation of the robot displacement between two intermediate points along a trajectory is obtained by odometry. Our model of non-systematic odometry errors assumes that this displacement is corrupted by white Gaussian noise. The relative displacement between those two intermediate points of the robot trajectory is represented by an uncertain location. Thus, let Rk – 1 and Rk be the reference frames attached to the trajectory points at time index k – 1 and k . We may describe their relative location by

= ˆ ˆ (D.46) LRk – 1Rk ()x Rk – 1Rk,,d Rk – 1Rk CRk – 1Rk ,I3 with the relative location vector expressed by:

= ˆ ∆ (D.47) xRk – 1Rk x Rk – 1Rk d Rk – 1Rk where ˆ represents the estimated displacement obtained by odometry and x Rk – 1Rk N 0 the imprecision in its estimation. d Rk – 1Rk ∼ (), CRk – 1Rk The predicted location of the robot at time can be calculated by the compo- xWRkk– 1 k sition of the robot location at time – 1 and the relative displacement xWRk – 1 k – 1 k

186 D.2 QUICK GUIDE TO THE SPMAP

: xRk – 1Rk = ∆ xWRkk– 1 xWRk – 1 k – 1 xRk – 1Rk = ˆ ∆ ∆ (D.48) x WRkk– 1 JRkRk – 1d Rk – 1 k – 1 d Rk – 1Rk = ˆ ∆ x WRkk– 1 d Rkk– 1 with the Jacobian of the transformation . Assuming small errors, the JRkRk – 1 xRkRk – 1 perturbation vector associated to the predicted robot location at time k can be obtained as: ≈ + (D.49) d Rkk– 1 JRkRk – 1d Rk – 1 k – 1 d Rk – 1Rk Thus, after the robot displacement, the predicted location vector of the SPmap is

ˆ ∆ ˆ x WRk – 1 k – 1 x Rk – 1Rk xˆ ˆ W = WF1, k – 1 k – 1 . (D.50) x kk– 1 ∂ xˆ x WFNk, – 1 k – 1 To simplify notation we rewrite the perturbation vector and its covariance matrix as:

W dR W CR CRM p = ; C = T (D.51) pM CRM CM thus, from equation D.49 we can write the plant model of the system as:

W = W + (D.52) pk Fk pk – 1 Gk dRk – 1Rk where:

03 JRkRk – 1 × NM I31× Fk = ; Gk = (D.53) 0 × 3 I 0N × 1 NM NM × NM M with NM as the dimension of the vector pM . The predicted perturbation vector of the SPmap ˆ W and its covariance matrix pkk– 1 W are therefore calculated by: Ckk– 1 pˆ W = F pˆ W kk– 1 k k – 1 k – 1 (D.54) W = W T + T Ckk– 1 Fk Ck – 1 k – 1 Fk Gk CRk – 1Rk Gk expressions which are expanded to:

ˆ ˆ d R 1 JR R – 1 d Rk – 1 k – 1 pˆ W ==kk– k k (D.55) kk– 1 pˆ ˆ Mkk– 1 p Mk – 1 k – 1

187 D. QUICK GUIDE TO THE SPMODEL and:

W T + JRkR – 1 CR – 1 – 1 JR R CRk – 1Rk JR R CRM – 1 – 1 C W = k k k k k – 1 k k – 1 k k kk– 1 C T J T C RMk – 1 k – 1 RkRk – 1 Mk – 1 k – 1 (D.56) The complexity of this phase scales linearly with O()N with the number of features included in the map.

D.2.3 Matching

Matching a local observation expressed in the robot reference frame R to a map feature represented in the world frame W can be considered as imposing a geometric constraint onto the locations of the map entities. In the estimation step, the EKF reestimates the map entities in order to satisfy the set of constraints from all matched observations in a least- square sense. In general, we formulate the constraints by stating coincidence of a set of M measurements

yˆ m = y m + u m ; u m ∼ N()0, S m (D.57) with a state vector xk by means of an implicit measurement function:

fm()xk, y m = 0 (D.58) which in general will be a non-linear function mainly due to orientation terms of the locations. Here, the perturbation vector of E , pE , constitutes the measurement used to improve the estimation of pW through the relation established by the pairing:

y m = pE ; S m = CE (D.59) The uncertain location of observation E with respect to the robot reference frame R can ˆ ˆ be written as LRE = ()xRE ,,p E CE ,BE , similarly, the uncertain location of the global map feature F expressed in the world frame W is given by ˆ L WF = ()xWF ,,pˆ F CF ,BF . Features E and F will be matched if the attached frames EF and can be considered equivalent up to the symmetries of the pairing.

W fm()p , pE = 0 (D.60)

With BFE as the binding matrix of the pairing which depends on the geometric features we have:

188 D.2 QUICK GUIDE TO THE SPMAP

W fm()p , pE = 0 = B x FE FE (D.61) û ∆ ∆ = BFE ()x WF x WR x RE T ˆ T = BFE ()ûBF pF ∆ x FE ∆ JERdR ∆ BE pE Linearization of the measurement equation is performed by the usual first-order Taylor expansion:

W ˆ h m ==fm()pˆ , pˆE BFE x FE (D.62)

∂f ==------m- R 0 … 0 F 0 … 0 (D.63) H m W H m H m ∂p ()ˆ W, ˆ p pE

∂f R ==------m- {}ˆ , 0 (D.64) H m W BFE J2∆ x FE JER ∂p ()ˆ W, ˆ p pE

∂f F ==------m- – {}0, ˆ T (D.65) H m W BFE J1∆ x FE BF ∂p ()ˆ W, ˆ p pE

∂f G ==------m B J {}xˆ , 0 B T (D.66) m ∂ FE 2∆ FE E pE ()ˆ W, ˆ p pE where the Jacobians are evaluated at the best available centered estimation W ()pˆ = 0, pˆE = 0 . The term h m is the innovation, sometimes also called υ . Finally, compatibility between E and F is statistically decided by application of a hypothesis test based on the squared Mahalanobis distance D2 . For a given significance level α , the pairing is compatible if

2 2 D ≤ χr, α ; r = rank()h m . (D.67)

D.2.4 Estimation of the SPmap

The perturbation vector pW and its covariance matrix C W are estimated from the constraints introduced by the matching between local observations and global map features. The general linearization equation can be written as:

= W + ; N 0 T (D.68) zkm, Hkm, pk v v ∼ (), Gkm, Skm, Gkm, thus, the EKF estimation equations, for the perturbation vector becomes

189 D. QUICK GUIDE TO THE SPMODEL

ˆ W = ˆ W + ()– ˆ W p km, p km, – 1 Kkm, zkm, Hkm, p km, – 1 (D.69) = ˆ – p km, – 1 Kkm, h m Similarly, for the covariance matrix of the map we have:

W W Ckm, = ()I – Kkm, Hkm, Ckm, – 1 (D.70) where the filter gain matrix Kkm, is calculated by:

T T T –1 Kkm, = Ckm, – 1Hkm, ()Hkm, Ckm, – 1Hkm, + Gkm, Skm, Gkm, (D.71) with the initial conditions

pˆ W = pˆ W k, 0 kk– 1 (D.72) W = W Ck, 0 Ckk– 1 Note that the iterative formulation of the EKF is taken. The iterative EKF integrates uncorrelated matched observations step-by-step, over the index m , while the classical EKF integrates all matched observations at once using stacked matrices. The advantage of the iterative EKF is that the state estimate becomes successively better, thereby reducing linearization errors. When reobserving a feature, uncertainty of all features, even those not visible from the robot location, will decrease. This is due to the cross-correlation terms in C W . The complexity of this phase scales polynomially according to O()N2M with N the number of features and M the number of matched local observations.

D.2.5 Adding Non-Matched Observations to the Map

Local observations which cannot be paired with any of the map features is new knowledge about the environment. These local observation are directly added to the map. Again, we use the simplified notation of the SPmap:

xˆ d xˆ W = WR ; pW = R (D.73) xˆ WM pM

W CR CRM C = T (D.74) CRM CM Let E be the non-matched observation with an attached reference frame E . Its uncertain location with respect to the reference frame of the robot R can be expressed by ˆ LRE = ()xRE,,pˆ E CE ,BE . Thus, with the estimation of the robot’s location with respect to the world frame W represented by xWR , the location of E with respect to W be-

190 D.2 QUICK GUIDE TO THE SPMAP comes:

= ∆ xWE xWR x RE T (D.75) = xˆ WE ∆ JERdR ∆ BE pE W = xˆ WE ∆ dE where, considering small errors, we have

W T dE ≈ JERdR + BE pE (D.76) and the perturbation vector of E expressed in W becomes:

W W pE = BE dE ≈ BEJERdR + pE (D.77) Therefore, adding the non-matched observation to the location vector xˆ W of the SPmap, produces the augmented location vector given by:

xˆ WR ˆ W ()x ' = xˆ WM (D.78) xˆ WR ∆ xˆ RE The perturbation vector of the SPmap pW is also updated. It is augmented with the perturbation vector of the non-matched observation, expressed in the world frame W :

dR W ()p ' = pM (D.79) BEJER dR + pE with covariance given by:

T T CR CRM CRJER BE W T T T T ()C ' = CRM CM CRM JER BE (D.80) T T BEJER CR BEJER CRM BEJER CRJER BE + CE The complexity of this phase scales linearly with the number of features O()N . Highly noisy local observations directly added to the map may produce unreliable parings in future time instants, therefore, it is recommended to remove them from early stages of the processing.

D.2.6 Centering the State Vector

Precision of the results obtained by the application of suboptimal (i.e. linearized) estimation techniques such as the EKF is highly influenced by the linearization performed on the non-linear measurement equations. In order to keep consistency and to avoid biases in the estimation, linearization is accomplished after each observation is integrated into the state

191 D. QUICK GUIDE TO THE SPMODEL vector, thus obtaining a centered state vector pW . To simplify notation, let us assume that the SPmap contains the location estimates of the robot and a single map feature F :

xˆ d xˆ W = WR ; pW = R (D.81) xˆ WF pF

W CR CRF C = T (D.82) CRF CF thus, the centered estimated location vector is computed as:

ˆ' ˆ ∆ ˆ ()ˆ W ' ==x WR x WR d R (D.83) x ˆ ˆ T x'WF x WF ∆ BF pˆ F its perturbation vector becomes the null vector:

()pˆ W ' = 0 (D.84) and its covariance matrix is computed by:

()C W ' = QW C W ()QW T (D.85) where the matrix QW is:

–1{}, 0 T 0 W = BR J2∆ dR BR (D.86) Q –1 T T 0 BF J2∆{}BF pF , 0 BF Not only the diagonal elements of ()C W ' are updated with respect to the values of C W but also its off-diagonal elements, thus restating dependencies between the map features. The complexity of this stage scales quadratically with the number of features O()N 2 .

192 Bibliography

(Acuity Research 1995) Acuity Research, "Using the Accurange 4000," White Paper, http://www.acuityresearch. com, June 4th, 1995. (Adams 1999) M.D. Adams, Sensor Modelling, Design and Data Processing for Autonomous Navigation, World Scientiﬁc Publishing, 1999. (Aoe 1994) J.-I. Aoe (ed.), Computer Algorithms: String Pattern Matching Strategies, Los Alamitos, USA, IEEE Computer Society Press, 1994. (Arras 1998) K.O. Arras, "An Introduction to Error Propagation: Derivation, Meaning, and Examples of Cy = Fx Cx Fx'," Technical Report EPFL-ASL-TR-98-01 R3, Autonomous Systems Lab, Swiss Federal Institute of Technology Lausanne, September 1998. (Arras & Burgard 2002) K.O. Arras, W. Burgard (eds.), Robots in Exhibitions, Workshop Proceedings, IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, 2002. (Arras, Persson et al. 2002) K.O. Arras, J. Persson, N. Tomatis, R. Siegwart, "Real-Time Obstacle Avoidance for Polyg- onal Robots With a Reduced Dynamic Window," IEEE International Conference on Robotics and Automation, Washington DC, USA, 2002. (Arras, Tomatis et al. 2003) K.O. Arras, N. Tomatis, R. Siegwart, "Robox, a Remarkable Robot for the Real World," in Experimental Robotics VIII, vol. 5, B. Siciliano and P. Dario (eds.), Springer, 2003. (Arras & Siegwart 1997) K.O. Arras, R.Y. Siegwart, "Feature Extraction and Scene Interpretation for Map-Based Navigation and Map Building," Proceedings of SPIE, Mobile Robotics XII, Pittsburgh, USA, 1997. (Arras & Tomatis 1999) K.O. Arras, N. Tomatis, "Improving Robustness and Precision in Mobile Robot Localization by Using Laser Range Finding and Monocular Vision," 3rd European Workshop on Advanced Mobile Robots (Eurobot’99), Zurich, Switzerland, 1999. (Arras, Tomatis et al. 2001) K.O. Arras, N. Tomatis, B. Jensen, R. Siegwart, "Multisensor On-the-Fly Localization: Precision and Reliability for Applications," Robotics and Autonomous Systems, vol. 34, Nr. 2-3, pp. 131-143, 2001. (Arras & Vestli 1998) K.O. Arras, S.J. Vestli, "Hybrid, High-Precision Localisation for the Mail Distributing Mobile Robot System MOPS," IEEE International Conference on Robotics and Automation, Leuven, Belgium, 1998.

193 (Arras, Vestli et al. 1996) K.O. Arras, S.J. Vestli, N. Tschichold-Guerman, "Echtzeitfaehige Merkmalsextraktion und Situationsinterpretation aus Laserscannerdaten," Autonome Mobile Systeme, Munich, Germany, 1996. (Astolfi 1995) A. Astolfi, "Exponential Stabilization of a Mobile Robot," Proceeding of the Third European Control Conference, Italy, 1995. (Austin & Jensfelt 2000) D. Austin, P. Jensfelt, "Using Multiple Gaussian Hypotheses to Represent Probability Distri- butions for Mobile Robot Localization," IEEE International Conference on Robotics and Automation, San Francisco, USA, 2000. (Ayache & Faugeras 1990) N. Ayache, O.D. Faugeras, "Maintaining Representation of the Environment of a Mobile Robot," in Autonomous Robot Vehicles, I. J. Cox and G. T. Wilfong, Eds.: Springer, 1990, pp. 205. (Ballard & Brown 1982) D.H. Ballard, C.M. Brown, Computer Vision, New Jersey, Prentice Hall, 1982. (Bar-Shalom & Fortmann 1988) Y. Bar-Shalom, T.E. Fortmann, Tracking and Data Association, Academic Press Inc., 1988. (Bar-Shalom & Li 1993) Y. Bar-Shalom, X.-R. Li, Estimation and Tracking: Principles, Techniques and Software, Artech House, 1993. (Bar-Shalom & Li 1995) Y. Bar-Shalom, X.-R. Li, Multitarget-Multisensor Tracking: Principles and Techniques, 1995. (Bayes 1763) T. Bayes, "Essay Towards Solving a Problem in the Doctrine of Chances," Philosophical Transactions of the Royal Society of London, 1763. (Betgé-Brezetz, Chatila et al. 1995) S. Betgé-Brezetz, R. Chatila, M. Devy, "Object-Based Modeling and Localization in Natural Environments," IEEE International Conference on Robotics and Automation, Nagoya, Japan, 1995. (Bonasso, Firby et al. 1997) R.P. Bonasso, R.J. Firby, E. Gat, D. Kortenkamp, D. Miller, M. Slack, "Experiences with an Architecture for Intelligent, Reactive Agents," Journal of Experimental and Theoretical Arti- ficial Intelligence, vol. 9, Nr. 2, 1997. (Borenstein & Feng 1996) J. Borenstein, L. Feng, "Measurement and Correction of Systematic Odometry Errors in Mobile Robots," IEEE Transactions on Robotics and Automation, vol. 12, Nr. 5, pp. 869-80, 1996. (Brega 2002) R. Brega, A Combination of System Software Techniques Aimed at Raising the Run-Time Safety of Complex Mechatronic Applications, Doctoral Thesis Nr. 14513, ETH Zürich, Zürich, 2002.

194 (Brega, Tomatis et al. 2000) R. Brega, N. Tomatis, K.O. Arras, "The Need for Autonomy and Real-Time in Mobile Robotics: A Case Study of XO/2 and Pygmalion," IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2000), Takamatsu, Japan, 2000. (Breipohl 1970) A.M. Breipohl, Probabilistic Systems Analysis: An Introduction to Probabilistic Models, Decisions, and Applications of Random Processes, John Wiley & Sons, Inc., 1970. (Brock & Khatib 1999) O. Brock, O. Khatib, "High-Speed Navigation Using the Global Dynamic Window Approach," IEEE International Conference on Robotics and Automation, Detroit, USA, 1999. (Burgard, Cremers et al. 1999) W. Burgard, A.B. Cremers, D. Fox, D. Hähnel, G. Lakemeyer, D. Schulz, W. Steiner, S. Thrun, "Experiences with an Interactive Museum Tour-Guide Robot," Artiﬁcial Intelligence, vol. 114, Nr. 1-2, pp. 3-55, 1999. (Carroll & Ruppert 1988) R.J. Carroll, D. Ruppert, Transformation and Weighting in Regression, Chapman and Hall, 1988. (Castellanos & Tardos 1996) J.A. Castellanos, J.D. Tardos, "Laser-Based Segmentation and Localization for a Mobile Robot," Sixth International Symposium on Robotics and Manufacturing (ISRAM '96), Montpellier, France, 1996. (Castellanos & Tardos 1999) J.A. Castellanos, J.D. Tardos, Mobile Robot Localization and Map Building: A Multisensor Fusion Approach, Kluwer, 1999. (Castellanos, Tardos et al. 1996) J.A. Castellanos, J.D. Tardos, J. Neira, "Constraint-Based Mobile Robot Localization," Inter- national Workshop on Advanced Robotics and Intelligent Machines, Salford, UK, 1996. (Castellanos, Tardos et al. 1997) J.A. Castellanos, J.D. Tardos, G. Schmidt, "Building a Global Map of the Environment of a Mobile Robot: The Importance of Correlations," IEEE International Conference on Robotics and Automation, Albuquerque, USA, 1997. (Chatila & Laumond 1985) R. Chatila, J.P. Laumond, "Position Referencing and Consistent World Modeling for Mobile Robots," IEEE International Conference on Robotics and Automation, St. Louis, USA, 1985. (Chenavier & Crowley 1992) F. Chenavier, J.L. Crowley, "Position Estimation for a Mobile Robot Using Vision and Odometry," IEEE International Conference on Robotics and Automation, Nice, France, 1992. (Chong & Kleeman 1997a) K.S. Chong, L. Kleeman, "Sonar Based Map Building for a Mobile Robot," IEEE Interna- tional Conference on Robotics and Automation, Albuquerque, USA, 1997.

195 (Chong & Kleeman 1997b) K.S. Chong, L. Kleeman, "Accurate Odometry and Error Modelling for a Mobile Robot," IEEE International Conference on Robotics and Automation, Albuquerque, USA, 1997. (Clark 1994) R.R. Clark, "Scanning Rangeﬁnder with Range to Frequency Conversion," US Patent Nr. 5309212, 1994. (Clark 1998) B. Clark, Personal Communication, October 30th, 1998. (Cox 1991) I.J. Cox, "Blanche–An experiment in guidance and navigation of an autonomous robot vehicle," IEEE Transactions on Robotics and Automation, vol. 7, pp. 193-204, 1991. (Cox & Leonard 1994) I.J. Cox, J.J. Leonard, "Modeling a Dynamic Environment Using a Bayesian Multiple Hypothesis Approach," Artiﬁcial Intelligence, vol. 66, Nr. 2, pp. 311-44, 1994. (Crowley 1989) J.L. Crowley, "World Modeling and Position Estimation for a Mobile Robot Using Ultra- sonic Ranging," IEEE International Conference on Robotics and Automation, Scottsdale, AZ, 1989. (Craig 1989) J.J. Craig, Introduction to Robotics: Mechanics and Control, 2nd ed., Addison-Wesley, 1989. (Davison & Kita 2000) A.J. Davison, N. Kita, "Active Visual Localisation for Coorperating Inspection Robots," IEEE/RSJ International Conference on Intelligent Robots and Systems, Takamatsu, Japan, 2000. (Delahoche, Mouaddib et al. 1996) L. Delahoche, E. Mouaddib, C. Pégard, P. Vasseur, "A Mobile Robot Localization Based on a Multisensor Cooperation Approach," 22nd IEEE International Conference on Industrial Electronics, Control, and Instrumentation (IECON'96), Taiwan, 1996. (Dellaert, Fox et al. 1999) F. Dellaert, D. Fox, W. Burgard, S. Thrun, "Monte Carlo Localization for Mobile Robots," IEEE/RSJ International Conference on Intelligent Robots and Systems, 1999. (Dissanayake, Newman et al. 2001) M.W.M.G. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, M. Csorba, "A Solution to the Simultaneous Localization and Map Building (Slam) Problem," IEEE Transactions on Robotics and Automation, vol. 17, Nr. 3, pp. 229-241, 2001. (Draper & Smith 1988) N.R. Draper, H. Smith, Applied Regression Analysis, 3rd ed., John Wiley & Sons, 1988. (Drumheller 1987) M. Drumheller, "Mobile Robot Localization Using Sonar," IEEE Transactions on PAMI, vol. 9, Nr. 2, pp. 325-32, 1987. (Dudek, Freedman et al. 1996a) G. Dudek, P. Freedman, S. Hadjres, "Mapping in Unknown Graph-Like Worlds," Journal of Robotic Systems, vol. 13, Nr. 8, pp. 539-559, 1996.

196 (Dudek, Freedman et al. 1996b) G. Dudek, P. Freedman, I.M. Rekleitis, "Just-in-time sensing: efficiently combining sonar and laser range data for exploring unknown worlds," IEEE International Conference on Robotics and Automation, Minneapolis, USA, 1996. (Dudek, Romanik et al. 1998) G. Dudek, K. Romanik, S. Whitesides, "Localizing a Robot with Minimum Travel," SIAM Journal on Computing, vol. 27, Nr. 2, pp. 583-604, 1998. (Feder, Leonard et al. 1999) H.J.S. Feder, J.J. Leonard, C.M. Smith, "Adaptive Mobile Robot Navigation and Mapping," International Journal of Robotics Research, vol. 18, Nr. 7, pp. 650-668, 1999. (Fox, Burgard et al. 1997) D. Fox, W. Burgard, S. Thrun, "The Dynamic Window Approach to Collision Avoidance," in IEEE Robotics and Automation Magazine, vol. 4, 1997, pp. 23-33. (Fox, Burgard et al. 1998a) D. Fox, W. Burgard, S. Thrun, "A Hybrid Collision Avoidance Method For Mobile Robots," IEEE International Conference on Robotics and Automation, Leuven, Belgium, 1998. (Fox, Burgard et al. 1998b) D. Fox, W. Burgard, S. Thrun, "Active Markov Localization for Mobile Robots," Robotics and Autonomous Systems, vol. 25, Nr. 3-4, pp. 195-207, 1998. (Fox, Burgard et al. 1999) D. Fox, W. Burgard, S. Thrun, "Markov Localization for Mobile Robots in Dynamic Envi- ronments," Journal of Artificial Intelligence Research, vol. 11, pp. 391-427, 1999. (Gander, Golub et al. 1994) W. Gander, G.H. Golub, R. Strebel, "Least-squares fitting of circles and ellipses," BIT, vol. 34, Nr. 4, pp. 558-578, 1994. (Gander & Hrebícek 1993) W. Gander, J. Hrebícek, Solving Problems in Scientific Computing Using Maple and MATLAB, Springer-Verlag, 1993. (God 0000) God, "How I did the World," First Intergalactical Symposium on World Design, Atlantis, 0000. (Grewal & Andrews 1993) M.S. Grewal, A.P. Andrews, Kalman Filtering: Theory and Practice, Prentice Hall, 1993. (Grimson & Lozano-Pérez 1987) W.E.L. Grimson, T. Lozano-Pérez, "Localizing Overlapping Parts by Searching the Interpre- tation Tree," IEEE Transactions on PAMI, vol. 9, Nr. 4, pp. 469-82, 1987. (Guibas, Motwani et al. 1997) L.J. Guibas, R. Motwani, P. Raghavan, "The Robot Localization Problem," SIAM Journal on Computing, vol. 26, Nr. 4, pp. 1120-38, 1997. (Guivant & Nebot 2001) J. Guivant, E. Nebot, "Optimization of the Simultaneous Localization and Map Building Algorithm for Real Time Implementation," IEEE Transactions on Robotics and Automation, vol. 17, Nr. 3, pp. 242-257, 2001.

197 (Gutmann, Burgard et al. 1998) J.-S. Gutmann, W. Burgard, D. Fox, K. Konolige, "An Experimental Comparison of Local- ization Methods," IEEE/RSJ International Conference on Intelligent Robots and Systems, Victoria, Canada, 1998. (Gutmann & Schlegel 1996) J.-S. Gutmann, C. Schlegel, "Amos: Comparison of scan matching approaches for self-localization in indoor environments," 1st European Workshop on Advanced Mobile Robots (Eurobot’96), Kaiserslautern, Germany, 1996. (Gutmann, Weigel et al. 2001) J.-S. Gutmann, T. Weigel, B. Nebel, "A Fast, Accurate, and Robust Method for Self-Local- ization in Polygonal Environments Using Laser-Range-Finders," Advanced Robotics Journal, vol. 14, Nr. 8, pp. 651-668, 2001. (Harper & McKerrow 2001) N. Harper, P. McKerrow, "Recognising Plants with Ultrasonic Sensing for Mobile Robot Navigation," Robotics-and-Autonomous-Systems, vol. 34, Nr. 2-3, pp. 71-82, 2001. (Hébert, Betgé-Brezetz et al. 1995) P. Hébert, S. Betgé-Brezetz, R. Chatila, "Probabilistic Map Learning: Necessity and Difﬁcul- ties," International Workshop on Reasoning with Uncertainty in Robotics, Amsterdam, Neth- erlands, 1995. (Hebert & Krotkov 1991) M. Hebert, E. Krotkov, "3-D Measurements from Imaging Laser Radars: How Good Are They?," IEEE/RSJ International Conference on Intelligent Robots and Systems, 1991. (Hough 1962) P.V.C. Hough, "Method and Means for recognising complex patterns,". US Patent 3069654, 1962. (Hu & Brady 1997) H. Hu, M. Brady, "Dynamic Global Planning with Uncertainty for Mobile Robots in Manu- facturing," IEEE Transactions on Robotics and Automation, vol. 13, Nr. 5, pp. 760-767, 1997. (Hummerl 1995) R. Hummerl, "Uncertainty Reasoning in Object Recognition by Image Processing," in Reasoning with Uncertainty in Robotics, vol. 1093, Lecture Notes in Artiﬁcial Intelligence, L. Dorst, M. v. Lambalgen, and F. Voorbraak, Eds.: Springer, 1995. (Jensen, Froidevaux et al. 2002) B. Jensen, G. Froidevaux, X. Greppin, A. Lorotte, L. Mayor, M. Meisser, G. Ramel, R. Sieg- wart, "The Interactive Autonomous Mobile System Robox," IEEE/RSJ International Confer- ence on Intelligent Robots and Systems, Lausanne, Switzerland, 2002. (Jensfelt & Christensen 1999) P. Jensfelt, H. Christensen, "Laser Based Pose Tracking," IEEE International Conference on Robotics and Automation, Detroit, USA, 1999. (Jensfelt, Austin et al. 2000) P. Jensfelt, D. Austin, O. Wijk, M. Andersson, "Feature Based Condensation for Mobile Robot Localization," IEEE International Conference on Robotics and Automation, San Fran- cisco, USA, 2000.

198 (Jensfelt, Wijk et al. 2000) P. Jensfelt, O. Wijk, D. Austin, M. Andersson, "Experiments on Augmenting Condensation for Mobile Robot Localization," IEEE International Conference on Robotics and Automa- tion, San Francisco, USA, 2000. (Julier, Uhlmann et al. 1995) S.J. Julier, J.K. Uhlmann, H.F. Durrant-Whyte, "A New Approach for Filtering Nonlinear Systems," Proceedings of the American Control Conference, Seattle, USA, 1995. (Kaufman & Rousseeuw 1990) L. Kaufman, P.J. Rousseeuw, Finding Groups in Data: An Introduction to Cluster Analysis, Wiley, 1990. (Kleeman & Kuc 1995) L. Kleeman, R. Kuc, "Mobile Robot Sonar for Target Localization and Classiﬁcation," Inter- national Journal of Robotics Research, vol. 14, Nr. 4, pp. 295-318, 1995. (Kuipers & Byun 1988) B.J. Kuipers, Y.T. Byun, "A Robust, Qualitative Approach to a Spatial Learning Mobile Robot," SPIE, Sensor Fusion: Spatial Reasoning and Scene Interpretation, 1988. (Lamon, Nourbakhsh et al. 2001) P. Lamon, I. Nourbakhsh, B. Jensen, R. Siegwart, "Deriving and Matching Image Fingerprint Sequences for Mobile Robot Localization", IEEE International Conference on Robotics and Automation, Seoul, Korea, 2001. (Latombe 1991) J.-C. Latombe, Robot Motion Planning, Kluwer Academics Publishers, 1991. (Lee & Recce 1997) D. Lee, M.A.A. Recce, "Quantitative Evaluation of the Exploration Strategies of a Mobile Robot," International Journal of Robotics Research, vol. 16, Nr. 4, pp. 413-47, 1997. (Lenser & Veloso 2000) S. Lenser, M. Veloso, "Sensor Resetting Localization for Poorly Modelled Mobile Robots," IEEE International Conference on Robotics and Automation, Detroit, USA, 2000. (Leonard & Durrant-Whyte 1992) J.J. Leonard, H.F. Durrant-Whyte, Directed Sonar Sensing for Mobile Robot Navigation, Kluwer Academic Publishers, 1992. (Leonard & Feder 2001) J.J. Leonard, H.J.S. Feder, "Decoupled Stochastic Mapping for Mobile Robot & Auv Navi- gation," IEEE Journal of Oceanic Engineering, vol. 26, Nr. 4, pp. 561-71, 2001. (Leonard, Newman et al. 2001) J.J. Leonard, P. Newman, R.J. Rikoski, J. Neira, J.D. Tardos, "Towards Robust Data Associ- ation and Feature Modeling for Concurrent Mapping and Localization," International Symposium on Robotics Research, 2001. (Lim & Leonard 2000) J.H. Lim, J.J. Leonard, "Mobile Robot Relocation from Echolocation Constraints," IEEE Transactions on PAMI, vol. 22, Nr. 9, pp. 1035-41, 2000. (Lu & Milios 1994) F. Lu, E. Milios, "Robot pose estimation in unknown environments by matching 2D range scans," IEEE Computer Vision and Pattern Recognition Conference (CVPR), 1994.

199 (Lu & Milios 1995) F. Lu, E.E. Milios, "Optimal Global Pose Estimation for Consistent Sensor Data Registra- tion," IEEE International Conference on Robotics and Automation, Nagoya, Japan, 1995. (Maeyama, Yuta et al. 2000) S. Maeyama, S. Yuta, A. Harada, "Experiments on a Remote Appreciation Robot in an Art Museum," IEEE/RSJ International Conference on Intelligent Robots and Systems, Taka- matsu, Japan, 2000. (Moutarlier & Chatila 1989) P. Moutarlier, R. Chatila, "Stochastic Multisensory Data Fusion For Mobile Robot Location And Environment Modelling," Proceedings of the 5th International Symposium of Robotics Research, 1989. (Muñoz & Gonzales 1998) A.J. Muñoz, J. Gonzales, "Two-Dimensional Landmark-based Position Estimation from a Single Image," IEEE International Conference on Robotics and Automation, Leuven, Belgium, 1998. (Nebot, Masson et al. 2003) E. Nebot, F. Masson, J. Guivant, H. Durrant-Whyte, "Robust Simultaneous Localization and Mapping for Very Large Outdoor Environments," in Experimental Robotics VIII, vol. 5, B. Siciliano and P. Dario (eds.), Springer, 2003. (Neira, Montano et al. 1993) J. Neira, L. Montano, J.D. Tardos, "Constraint-Based Object Recognition in Multisensor Systems," IEEE International Conference on Robotics and Automation, Atlanta, USA, 1993. (Neira, Tardos et al. 1999) J. Neira, J.D. Tardos, J. Horn, G. Schmidt, "Fusing Range and Intensity Images for Mobile Robot Localization," IEEE Transactions on Robotics and Automation, vol. 15, Nr. 1, pp. 76- 84, 1999. (Neira & Tardos 2000) J. Neira, J.D. Tardos, "Data Association in Stochastic Mapping: The Fallacy of the Nearest Neighbor," Workshop on mobile robot navigation and mapping (part of ICRA 2000), San Francisco, USA, 2000. (Neira, Tardos et al. 2003) J. Neira, J.D. Tardos, J.A. Castellanos, "Linear Time Vehicle Relocation in SLAM," IEEE International Conference on Robotics and Automation, Taipei, Taiwan, 2003. (Nourbakhsh, Bodenage et al. 1999) I. Nourbakhsh, J. Bodenage, S. Grange, R. Lutz, R. Meyer, A. Soto, "An Affective Mobile Robot Educator with a Full-time Job," Artificial Intelligence, vol. 114, Nr. 1-2, pp. 95-124, 1999. (Nourbakhsh, Powers et al. 1995) I. Nourbakhsh, R. Powers, S. Birchfield, "DERVISH, an office-navigating robot," in AI Magazine, vol. 16, 1995, pp. 53-60. (Pérez, Castellanos et al. 1999) J.A. Pérez, J.A. Castellanos, M.M. Montiel, J. Neira, J.D. Tardós, "Continuous Mobile Robot Localization: Vision vs. Laser," IEEE International Conference on Robotics and Automation, Detroit, USA, 1999.

200 (Philippsen & Siegwart 2003) R. Philippsen, R. Siegwart, "Smooth and Efficient Obstacle Avoidance for a Tour Guide Robot," IEEE International Conference on Robotics and Automation, Taipei, Taiwan, 2003. (Prescott & McLean 1997) B. Prescott, G.F. McLean, "Line-Based Correction of Radial Lens Distortion," Graphical Models and Image Processing, vol. 59, Nr. 1, pp. 39-47, 1997. (Quinlan & Khatib 1993) S. Quinlan, O. Khatib, "Elastic bands: connecting path planning and control," IEEE Interna- tional Conference on Robotics and Automation, 1993. (Ridley, Nettleton et al. 2002) M. Ridley, E. Nettleton, S. Sukkarieh, H. Durrant-Whyte, "Tracking in Decentralised Air- Ground Sensing Networks," Fifth International Conference on Information Fusion, Annap- olis, USA, 2002. (Roumeliotis & Bekey 2000) S.I. Roumeliotis, G.A. Bekey, "Bayesian Estimation and Kalman Filtering: a Unified Frame- work for Mobile Robot Localization," IEEE International Conference on Robotics and Auto- mation, San Francisco, USA, 2000. (Schilt 2000) M. Schilt, Matching Techniques for Global Localization with Infinite Lines, Semester Project Thesis, Autonomous Systems Lab, EPFL, Lausanne, Switzerland, 2000. (Schlegel 1998) C. Schlegel, "Fast Local Obstacle Avoidance Under Kinematic and Dynamic Constraints," IEEE/RSJ International Conference on Intelligent Robots and Systems, Victoria BC, Canada, 1998. (Schraft, Graf et al. 2001) R.D. Schraft, B. Graf, A. Traub, D. John, "A Mobile Robot Platform for Assistance and Entertainment," Industrial Robot, vol. 28, Nr. 1, pp. 29-34, 2001. (Sedgewick 1988) R. Sedgewick, Algorithms, 2nd ed., Addison-Wesley, 1988. (Seiz, Jensfelt et al. 2000) M. Seiz, P. Jensfelt, H. Christensen, "Active Exploration for Feature-Based Global Localiza- tion," IEEE/RSJ International Conference on Intelligent Robots and Systems, Piscataway, USA, 2000. (Simmons, Goodwin et al. 1997) R. Simmons, R. Goodwin, K. Haigh, S. Koenig, J. O’Sullivan, "A Layered Architecture for Office Delivery Robots," First International Conference on Autonomous Agents, 1997. (Simmons & Koenig 1995) R. Simmons, S. Koenig, "Probabilistic Navigation in Partially Observable Environments," International Joint Conference on Artificial Intelligence, 1995. (Simmons, Smith et al. 2002) R. Simmons, T. Smith, M.B. Dias, D. Goldberg, D. Hershberger, A. Stentz, A. Zlot, "A Layered Architecture for Coordination of Mobile Robots," in Multi-Robot Systems: From Swarms to Intelligent Automata: Kluwer, 2002.

201 (Smith, Self et al. 1990) R. Smith, M. Self, P. Cheeseman, "Estimating Uncertain Spatial Relationships in Robotics," in Autonomous Robot Vehicles, I. J. Cox and G. T. Wilfong, Eds.: Springer-Verlag, 1990, pp. 167-193. (Tardos 1992) J.D. Tardos, "Representing Partial and Uncertain Sensorial Information Using the Theory of Symmetries," IEEE International Conference on Robotics and Automation, Nice, France, 1992. (Taylor & Probert 1996) R.M. Taylor, P.J. Probert, "Range Finding and Feature Extraction by Segmentation of Images for Mobile Robot Navigation," IEEE International Conference on Robotics and Automation, Minneapolis, USA, 1996. (Thrun, Beetz et al. 2000) S. Thrun, M. Beetz, M. Bennewitz, W. Burgard, A.B. Cremers, F. Dellaert, D. Fox, D. Hähnel, C. Rosenberg, N. Roy, J. Schulte, D. Schulz, "Probabilistic Algorithms and the Inter- active Museum Tour-Guide Robot Minerva," International Journal of Robotics Research, vol. 19, Nr. 11, pp. 972-99, 2000. (Thrun, Fox et al. 2000) S. Thrun, D. Fox, W. Burgard, "Monte Carlo Localization with Mixture Proposal Distribu- tion," AAAI National Conference on Artiﬁcial Intelligence, Austin, USA, 2000. (Tomatis 2001) N. Tomatis, Hybrid, Metric-Topological, Mobile Robot Navigation, Doctoral Thesis Nr. 2444, Microengineering Departement, EPFL, Lausanne, Switzerland, 2001. (Tomatis, Nourbakhsh et al. 2001) N. Tomatis, I. Nourbakhsh, K.O. Arras, R. Siegwart, "A Hybrid Approach for Robust and Precise Mobile Robot Navigation with Compact Environment Modeling," IEEE Interna- tional Conference on Robotics and Automation, Seoul, Korea, 2001. (Tomatis, Terrien et al. 2003) N. Tomatis, G. Terrien, R. Piguet, D. Burnier, S. Bouabdallah, K.O. Arras, R. Siegwart, "Designing a Secure and Robust Mobile Interacting Robot for the Long-Term," IEEE Inter- national Conference on Robotics and Automation, Taipei, Taiwan, 2003. (Tschichold-Gürman, Vestli et al. 1999) N. Tschichold-Gürman, S.J. Vestli, G. Schweitzer, "Operating Experience with the Service Robot MOPS," 3rd European Workshop on Advanced Mobile Robots, Zürich, Switzerland, 1999. (Vestli 1995) S.J. Vestli, Fast, accurate and robust estimation of mobile robot position and orientation, Doctoral Thesis Nr. 11360, ETH Zürich, Zürich, Switzerland, 1995. (Waltz 1972) D. Waltz, "Understanding Line Drawings of Scenes with Shadows," in Psychology of Computer Vision, P. H. Winston, Ed. Cambridge, USA: MIT Press, 1972. (Wullschleger, Arras et al. 1999) F. Wullschleger, K.O. Arras, S.J. Vestli, "A Flexible Exploration Framework for Map Building," 3rd European Workshop on Advanced Mobile Robots (Eurobot’99), Zurich, Swit- zerland, 1999.

202 (Wullschleger & Brega 2001) F. Wullschleger, R. Brega, "The Mops Has Grown-up from a Research Platform to a High- Availability Service Robot," 4th European Workshop on Advanced Mobile Robots (Eurobot’01), Lund, Sweden, 2001. (Yamauchi, Schultz et al. 1998) B. Yamauchi, A. Schultz, W. Adams, "Mobile Robot Exploration and Map-Building with Continuous Localization," IEEE International Conference on Robotics and Automation, Leuven, Belgium, 1998.

203 204 Curriculum vitae

I was born in Basel, Switzerland, on October 14th, 1970. I grew up in Allschwil and grad- uated from the Naturwissenschaftlich-Mathematisches Gymnasium in Basel in 1990 with the obtainment of the Matur type C. I received my Master’s in electrical engineering from the Swiss Federal Institute of Technology Zurich (ETHZ) with the diploma thesis “Map Building” in 1995 and worked as a research assistant in Nanorobotics at the Institute of Ro- botics in Zurich. In 1996 I joined Prof. Siegwart to help setting up the Autonomous Systems Lab at the Swiss Federal Institute of Technology Lausanne (EPFL) as the lab’s very first assistant. From November 2000 to February 2001 I was a visiting researcher at the Robotics and Real-Time Group, University of Zaragoza. My fields of interest are sciences of artefacts, that is, engineering and art in general, and mobile robots and related fields in particular. I initiated several interdisciplinary collabora- tions with artists, anthropologists, and other labs at EPFL. I was one of the organizers of the IROS workshop on Robots in Exhibitions, co-organized several conferences and co- founded two spin-off companies.

205 206