Micha l Lisowski. Differential Evolution approach to the Localization Problem for Mobile Robots

Transcription

1 Micha l Lisowski Differential Evolution approach to the Localization Problem for Mobile Robots Master s Thesis, September 2009

2

3 Micha l Lisowski Differential Evolution approach to the Localization Problem for Mobile Robots Master s Thesis, September 2009

4

5 Differential Evolution approach to the Localization Problem for Mobile Robots, This report was prepared by Micha l Lisowski Supervisors Zhun Fan Ole Ravn Department of Management Engineering Technical University of Denmark (DTU) Produktionstorvet Building 424 DK-2800 Kgs. Lyngby Denmark Tel: (+45) Fax: (+45) info@man.dtu.dk Release date: 28 September 2009 Category: 1 (public) Edition: Comments: First This report is part of the requirements to achieve the Master of Science in Engineering (M.Sc.Eng.) at the Technical University of Denmark. This report represents 40 ECTS points. Rights: c Micha l Lisowski, 2009

6

7 Preface This report serves as a part of the requirements for obtaining a degree of Master of Science in Engineering at the Technical University of Denmark (DTU). It represents 40 ECTS points work which was carried out at the Department of Management Engineering, section for Engineering Design and Product Development, from February until September I would like to thank Zhun Fan and Ole Ravn not only for their supervision, suggestions and support during this project, but also for their excellent courses which I had the pleasure to participate in during my master studies. I also give my appreciation to Jose-Luis Blanco-Claraco from MAPIR lab of the University of Malaga for developing the Mobile Robot Programming Toolkit (MRPT) and for providing the maps. Last but not least, I would like to express my gratitude to my fiancée Paulina for her support and motivation during this project. Thank you for your patience and understanding. Micha l Lisowski (s061085)

8

9 Abstract This project focuses on the mobile robot localization problem, which was divided into three subproblems: pose tracking, global localization and robot kidnap. An existing and well established algorithm - Monte Carlo Localization (MCL) - was selected as the benchmark and the base for new method. The main purpose of this study was to investigate how Differential Evolution (DE) can be applied to extend MCL in order to better solve localization problem by increasing localization reliability and speed. In addition, a novel mechanism for effective robot kidnap detection was proposed and tested. Experiments were performed using computer simulations based on the odometry data and laser range finder measurements collected in advance by a robot in real-life. Four different indoor environments were used to ensure reliable results. Based on the experiments outcome, it was found that integrating DE enables MCL to provide more accurate robot pose estimations in shorter time while using less particles.

10

11 Contents List of Figures viii List of Tables xi List of Algorithms xiv 1 Introduction Background Project definition Mobile robot localization and mapping Genetic Algorithms Related research Monte Carlo Localization Theory Overview Motion models Measurement models Sensor resetting Experiments Definitions

12 2.2.2 Setup details Maps Results Differential Evolution Theory Introduction Overview of DE Mutation Crossover Selection Differential Evolution Monte Carlo Localization (DEMCL) Experiments Population convergence MCL and DEMCL performance comparison Detailed DEMCL performance analysis Difficulties with COR and UWB maps Modified DEMCL Motivation Method description Experiments Conclusions Further work References 62 Appendix 67 A Monte Carlo Localization 67

13 vii B Differential Evolution Monte Carlo Localization 71 C Modified DEMCL 83

14 viii

15 List of Figures 1.1 Examples of mobile robots Probability robot pose representations in 1D Basic GAs operators: reproduction, crossover and mutation Robot state transformation and input data diagram Successful global localization of the robot using MCL Sampling from odometry motion model Sampling from motion model Beam model Maps used in the experiment MCL successful trials rates MCL average cumulative localization rates DE mutation example in 2D space DE crossover example in 2D space Exponential crossover example Population convergence Ratio of successfully localized trials as a function of step number (time) Population parameters in kidnap experiment

16 x LIST OF FIGURES A.1 MCL pose tracking accuracy A.2 MCL effective sample size A.3 MCL computation time B.1 DEMCLMCL global localization success rates for ISA map.. 75 B.2 Average step at which localization occured for ISA map B.3 DEMCL global localization success rates for SEM map B.4 Average step at which localization occured for SEM map B.5 DEMCL global localization success rates for small number of generations B.6 DEMCL global localization success rates and average step at which localization occured for ISA map B.7 DEMCL global localization success rates and average step at which localization occured for SEM map B.8 DEMCL algorithm computation time C.1 Modified DEMCL global localization success rates for ISA map. 85 C.2 Average step at which localization occured for ISA map C.3 Modified DEMCL global localization success rates for SEM map C.4 Average step at which localization occured for SEM map C.5 Modified DEMCL global localization success rates for UWB map C.6 Average step at which localization occured for UWB map C.7 Modified DEMCL global localization success rates for COR map C.8 Average step at which localization occured for COR map C.9 Modified DEMCL successful recovery ratio after robot kidnap for ISA map C.10 Modified DEMCL length of recovery after robot kidnap for ISA map C.11 Modified DEMCL successful recovery ratio after robot kidnap for SEM map

17 LIST OF FIGURES xi C.12 Modified DEMCL length of recovery after robot kidnap for SEM map C.13 Modified DEMCL successful recovery ratio after robot kidnap for UWB map C.14 Modified DEMCL length of recovery after robot kidnap for UWB map C.15 Modified DEMCL successful recovery ratio after robot kidnap for COR map C.16 Modified DEMCL length of recovery after robot kidnap for COR map

18 xii LIST OF FIGURES

19 List of Tables 2.1 Map summary Common DE mutation strategies Crossover probability p m and expected number of crossed parameters E(L) Comparison of core concepts of MCL and DE MCL and DEMCL effectiveness comparison MCL and DEMCL computation time MCL and DEMCL average pose tracking accuracy comparison Global localization results summary for DEMCL variants Sufficient number and density of particles providing highest success rates in global localization Robot kidnap overall results for DEMCL and MCL A.1 MCL experiment results for ISA map A.2 MCL experiment results for SEM map A.3 MCL experiment results for COR map A.4 MCL experiment results for UWB map B.1 DEMCL convergence speed in steps for 500 particles B.2 DEMCL convergence speed in steps for 5000 particles

20 xiv LIST OF TABLES B.3 DEMCL results with different number of generations for ISA map B.4 DEMCL results with different number of generations for SEM map B.5 DEMCL results with small number of generations for ISA map. 78 B.6 DEMCL results with small number of generations for SEM map B.7 DEMCL results with different number of particles for ISA map. 80 B.8 DEMCL results with different number of particles for SEM map C.1 Modified DEMCL global localization results for ISA map C.2 Modified DEMCL global localization results for SEM map.. 84 C.3 Modified DEMCL global localization results for UWB map.. 84 C.4 Modified DEMCL global localization results for COR map.. 84 C.5 Modified DEMCL robot kidnap results for ISA map C.6 Modified DEMCL robot kidnap results for SEM map C.7 Modified DEMCL robot kidnap results for UWB map C.8 Modified DEMCL robot kidnap results for COR map

21 List of Algorithms 1 MCL(X t 1, u t, z t, m) sample motion model(x t 1, u t ) DEMCL(X t 1, u t, z t, m, F, CR, g max ) population reset(x t, w avg, A, α 1, α 2 )

22 xvi LIST OF ALGORITHMS

23 Chapter 1 Introduction 1.1 Background Any machine able to move in the environment and perform given tasks could be considered as a mobile robot. An autonomous mobile robot should additionally present signs of artificial intelligence and high level of self-control. Roomba R series of vacuum cleaners (Figure 1.1a) serve as a good example of an autonomous mobile robot. Why mobile robots are important? Because in the near future they could take over many tasks and provide valuable services. Potential applications are diverse, from cleaning and transportation through enhanced communication (e.g. telepresence) and entertainment to security and military use (Figure 1.1c). Unfortunately, nowadays it is still more likely to find mobile robot in a research laboratory (Figure 1.1b) or in a factory, rather than at home or at the office. Even though a significant progress in mobile robotics field has been made in the last two decades, the levels of autonomy presented by most of mobile robots are still not enough for the mass market. One of the requirements for successful commercialization is that interaction between humans and robots is more of cooperation than supervision. To achieve this state, when mobile robots are commonly present in our everyday lives, a number of remaining problems have to be fully solved. One of the first is localization and in this project we are going to focus on this problem.

24 2 Introduction (a) (b) (c) Figure 1.1: Examples of mobile robots (a) Roomba R 1 (b) PowerBot TM2 (c) Inspector Project definition Problem definition This project is dedicated to mobile robot localization problem which can be shortly stated as: Assume a mobile robot is starting from unknown pose and continues to move in a known indoor environment. Estimate and track its pose based on the map, motion and sensor readings. It can be divided into three distinct subproblems [Thrun et al. (2001)]: Global localization is chronologically the first problem, because robot starts from an unknown pose. Robot pose should be approximated quickly and as close as possible to ground truth (robot true pose) [Burgard et al. (1998); Jensfelt and Kristensen (2001)]. Pose tracking problem appears when robot pose estimate is obtained. The aim is to update it accordingly to robot movements, so that at any time it corresponds to ground truth [Borenstein et al. (1996); Schiele et al. (1994); Weiß et al. (1994)]. 1 irobot R ( 2 MobileRobots Inc ( 3 The Industrial Research Institute for Automation and Measurements - PIAP (

25 1.2 Project definition 3 Kidnap problem requires robot to be able to recover from a sudden and significant change of its pose. The main challenge lies in the fact that robot is not informed about kidnap, so it is still confident about the recent pose estimate, which becomes incorrect. Therefore solving this problem requires first robust kidnap detection and then recovering pose estimate from scratch, just like in global localization. In real life, robot kidnap problem may occur as a result of collision with an obstacle or unexpected interaction with people [Engelson and McDermott (1992); Fox and Burgard (1999)]. Approach and motivation There is a number of ways to approach the stated problem, which potentially may yield satisfactory solutions. This project is dedicated to particle filters and evolutionary algorithms, particularly Monte Carlo Localization (MCL) and Differential Evolution (DE). Although both methods are mature and have long history, they were applied together to mobile robot localization problem only once by Moreno et al. (2006). As for the time of writing this report, there was no other evidence in the literature of research in mobile robotics field regarding specifically MCL and DE. Another reason for selecting these two methods are potential benefits and improvements of extending MCL with DE, reported by Moreno et al. (2006). However, in this paper there is no comparison between plain MCL and MCL with DE, and also the kidnap problem is not considered. Additionally, the fundamental concept of particle used in MCL corresponds well in mathematical terms to the concept of individual found in DE. This observation leads to full, yet simple, integration of both algorithms [Moreno et al. (2006)]. Project aim and scope The main purpose of this project is to develop a solution to mobile robot localization problem which could address all three subproblems: global localization, pose tracking and kidnap problem. Investigation is limited to the indoor environment and typical medium-size research mobile robot equipped with laser range finder sensor (Figure 1.1b). However, in general proposed solution could be also applied to other types of mobile robots after some modifications. Second goal is to perform detailed evaluation of both algorithms: MCL and MCL with DE, in order to reveal the benefits of including DE. Experiments are performed as computer simulations using the odometry data and laser

26 4 Introduction range finder measurements collected in advance by a robot in real-life. Since evaluation is not performed directly on a robot, real-time solution is not necessary. As a consequence, computational expense is not the main concern during implementation and evaluation of the algorithms. In our approach, available computational power is only limited by computer used to run the simulations. It is very important to notice that environment map is considered as known. Otherwise we would be facing other, even more challenging, problem of simultaneous localization and mapping (SLAM) [Grisetti et al. (2007); Haehnel et al. (2003); Montemerlo et al. (2002)]. Although interesting, it is beyond the scope of this project. However, solution developed in this project could be used as a localization method in SLAM algorithms. In this project, 2D environment is being considered, so it is sufficient to represent robot pose as position in XY-coordinates and orientation (heading direction). Report structure Remaining part of this Chapter contains introduction to mobile robots localization and mapping and Genetic Algorithms. It ends with a brief literature survey presenting recent solutions to mobile robot localization problem using evolutionary approach and particle filters. Chapter 2 is focused on the Monte Carlo Localization and starts with theory (Section 2.1) followed by experimental evaluation (Section 2.1). Chapter 3 is devoted to the Differential Evolution and begins with theory (Section 3.1). Next, an algorithm combining MCL with DE is presented and discussed (Section 3.2). Chapter 3 is concluded with experiments (Section 3.3). Chapter 4 contains description and evaluation of improved algorithm based on MCL and DE. Chapter 5 presents overall conclusions and indications for further research. Please observe that references starting with a capital letter point to figures and tables placed in the corresponding chapter of Appendix. 1.3 Mobile robot localization and mapping Although mobile robotics is a relatively young field, due to space limitation it is impossible to include here a comprehensive introduction to localization

27 1.3 Mobile robot localization and mapping 5 and mapping approaches and algorithms. If needed, please refer to Siegwart and Nourbakhsh (2004). Instead, this section briefly introduces only the fundamental concepts underlying the methods used in this project. Probabilistic robotics Probabilistic approach to localization and mapping problems in mobile robotics is the most recent and also very promising one [Thrun et al. (2005)]. The core idea of probabilistic robotics is to explicitly model and represent uncertainty about robot and environment using probability theory. This uncertainty is mostly due to sensing limitations (noise) and imperfect robot actions. In case of localization, this approach means that probabilistic algorithms distribute certain probability values (likelihoods) over the whole space of possible poses, forming a probability distribution. As a result, instead of a single robot pose estimate, each possible pose has a likelihood corresponding to the probability of being the true robot pose. Important characteristic of probabilistic algorithms is good performance with only approximated mathematical models of robot and environment. Therefore, even noisy sensors can be applied with satisfactory results. Another feature is flexibility, because no assumptions regarding the type of noise in sensing or locomotion are needed. Map representation In general, internal map can be either topological or metric. Although, in recent years, there has been an attempt to combine both representations [Thrun (1998); Zimmer (2000)]. Metric maps can be further divided into feature and occupancy grid maps. Topological maps consist of interconnected nodes resembling graphs. Topological representation is highly abstract, for each application meaning of nodes and connections needs to be defined. However, it offers great flexibility and compactness, because even large maps can be stored and processed using limited computer resources. On the other hand, topological maps do not represent the environment in details, so they are not suitable for precise robot navigation when accurate manoeuvres are necessary. Metric maps can be easily understood by humans, so they are very popular. Feature maps are used to represent e.g. walls, corners, doors etc. Each feature has a defined symbol placed in the map at given coordinates. When using occupancy grid maps, there is no need to extract features from raw

28 6 Introduction sensor data. Simply, the whole map is divided into cells, which hold individual values. Most common practice is to use probability values, where free space is represented by 0 and obstacle by 1. All values in between indicate not fully explored map. Metric maps provide detailed representation, however they do not scale well with the environment size and require much more computer memory compared to topological maps. In this project, occupancy grid maps with m cells are used, because the aim is to develop a precise solution for mobile robot localization. Additionally, only indoor environment is considered, so the size of occupancy grid maps is acceptable. For examples, please refer to Figure 2.6 on page 28. Robot pose representation Probably the most traditional robot pose representation is a unimodal Gaussian distribution. It is computationally very convenient and widely used in statistics. Unfortunately, this distribution is suitable only for pose tracking purposes, because multiple distinct hypotheses cannot be represented by a single mean value. In order to address both pose tracking and global localization problems, three types of pose representation are available: mixture of Gaussian distributions, grid-based and particles (Figure 1.2). Mixture of Gaussian distributions allows to overcome limitations of a single Gaussian distribution, because multiple pose hypotheses may exists in parallel. This benefit comes with a cost of increased complexity. Grid-based representation uses discrete variables thus requires continuous pose space to be divided. Grid size is adjustable, but the smaller the grids the more precise pose representation. On the other hand, computation time is much longer when the total number of grids is increased. The advantage of grid-based representation is simplicity of algorithms. Every time robot pose needs to be updated, the algorithm simply iterates in a loop through each grid and updates its value. Particle representation is very flexible, because there are no assumptions regarding pose probability distribution and no need to use discrete parameters. Particles allow tracking multiple hypothesis with small computational effort and their number can be easily scaled to match given environment size. The basic principle is that each particle is a separate hypothesis about robot pose, so the more particles are concentrated in a certain area the more likely the true robot pose is located within this area.

29 1.4 Genetic Algorithms 7 Figure 1.2: Probability robot pose representations in 1D: (a) mixture of Gaussian distributions (b) grid-based (c) particles. In case of this project, particles are used as robot pose representation, because they are required in MCL. In addition, this representation is also very suitable for DE, because particles can be considered as individuals in DE. This observation allows simple integration of both algorithms, as discussed in Section Genetic Algorithms Genetic Algorithms (GAs) were designed to solve search and optimization problems by imitating evolutionary processes observed in nature. GAs were developed in mid 70 s by Holland (1975) and his team at University of Michigan. Significant contribution in 80 s is due to Goldberg (1989). Up to now, GAs has attracted much attention from researchers and this field is still actively investigated.

30 8 Introduction Figure 1.3: Basic GAs operators: reproduction, crossover and mutation. One of the reasons for wide interest in GAs and numerous applications in real-life problems are limited requirements. To operate properly, GAs need only proper genetic representation of the problem space and solutions (often called individuals), and definition of fitness function used for evaluating individual solutions. There is no need to ensure continuity nor presence of derivatives, as it is the case for classic direct and indirect search methods. Another advantage of using GAs is the fact that they are suitable for solving global optimization problems. Traditional calculus-based methods mostly solve only local optimization problems, due to very strong dependence on the point where search is initialized. There are four main differences between classical optimization methods and GAs: GAs work with a coding of the solutions (e.g. binary), not the solution itself. GAs initialize search from a number of points, not a single point. GAs use information from fitness function (objective function). No derivatives or any other a priori knowledge is necessary. GAs use probabilistic transition rules, not deterministic ones. Every GA starts by creating an initial population of individuals (1 st generation), which represent potential solutions to the optimization problem. It is a common practice to initialize the first generation randomly, all over the problem domain (search space). Then, each individual is is assigned with a certain value returned by the fitness function. This function evaluates how good is the given individual, in terms of solving the problem. Once the first population is initialized and evaluated, most GAs perform the following operations: reproduction, crossover and mutation (Figure 1.3).

31 1.5 Related research 9 Reproduction is a process of selecting individuals which produce offspring to next generation. One of the approaches is to select the parent according to its relative fitness function value. In this way, the better the individual the more chance it has to breed. Additionally, poor individuals might not get even a single chance to breed and will die out from the population. This approach mimics the survival of the fittest rule observed in the nature. During crossover two randomly selected individuals exchange information. In case of simple crossover using binary string representation, this process takes place by selecting a random position in the string and swaping remaining part of the string. The main purpose is to increase variety in the population and create new individuals. Such definition of crossover operator allows to preserve sections of parent individuals, which might contain valuable information, and combine them to obtain potentially even better individuals. Mutation is the last operator applied to the current population. This operator acts randomly by changing single bits of information in the individual. In this way, new individuals are created, which might not be achievable only by crossover. Another reason for using mutation is the fact that even small changes to individuals might result in much higher fitness function values. Because mutation probability is often very low, this operator is only complementary to crossover and not used in some GAs. At the end, new individuals are evaluated using fitness function and next generation is created. Some GAs replace the least fit individuals from the current generation with the best new individuals to form the next generation. Another strategy is to compare new individual with its parent, in terms of fitness function, and maintain only the better one. Using this elitist approach, population average fitness is never decreasing [Rudolph (1996)]. In most cases, solution found in the first generation is not satisfactory, so it is necessary to evolve a number of generations, until the near-optimal solution is found or some limits are reached (e.g. time or maximum number of generations). 1.5 Related research This section presents selected papers which are closely related to this project and has been published recently. From this point of view, one of the most relevant papers is Moreno et al. (2006). This article presents an evolutive localization algorithm for mobile robots combining DE and MCL. Algorithm implementation is based on occupancy grid maps and laser range finder sensor, similarly to this project.

32 10 Introduction Authors focus their attention on the definition of fitness function. They notice that typical choice of fitness function, the sum of squared error function, is not optimal for global localization purposes due to limited information from sensors and geometrical similarities in the environment. Proposed solution is to update particles at each step using only the single best estimate displacement and also include in the fitness function as a penalty the distance between estimated robot pose and candidate solutions. Authors demonstrated that such definition of fitness function reduced the ambiguities and improved algorithms performance in indoor environment. Obtained results indicate that MCL combined with DE can be successful in solving the mobile robot localization problem. Interesting conclusion from the experiments is that only 500 particles were required, which gives 0.57 particles per square meter of the considered environment. However, the work presented by Moreno et al. (2006) has some limitations, because the kidnap problem is not taken into account and the classic MCL algorithm is not evaluated, so there are no reference results for comparison. Monte Carlo Localization A comprehensive paper on MCL is Thrun et al. (2001). It presents a family of MCL algorithms including Mixture-MCL. This algorithm combines classic MCL, where poses are predicted based on robots motion, with dual MCL, where information from sensors is used to predict robot poses. Both approaches have their distinct strengths and weaknesses, but combined they complement each other and provide very good results. However, Mixture- MCL implementation is much more difficult than classic MCL, because it requires poses to be sampled according to sensor measurements, which is a non-trivial task for laser range finders. Experimental results presented in Thrun et al. (2001) are extensive and come from both systematic simulations and real-word tests. For the later, a robot equipped with sonar sensors and upward-pointed camera was used. It served for two weeks as an autonomous tour-guide in the National Museum of American History. Obtained results indicate that Mixture-MCL outperformed classic MCL by a large margin, not only in global localization but also in robot kidnap problem. In addition, the proposed algorithm was much more computationally efficient. One of the interesting observations made in Thrun et al. (2001) on classic MCL limitations is that the algorithm performs poorly if not enough particles are placed close to the true robot pose. This requires proposal distribution, from which particles are drawn, to correspond well to the true distribution of robot poses. Recently, Blanco et al. (2008) proposed an opti-

33 1.5 Related research 11 mal filtering algorithm, which dynamically generates the minimum number of particles that best represent the true distribution. Another limitation of classic MCL is that the algorithms performance decreases for both relatively high and low sensor noise levels. According to Thrun et al. (2001) the optimal sensor noise level is from 10% to 20%. When the sensor is not accurate it is natural to expect higher errors. However, it is counter-intuitive that errors are higher also for very accurate sensors e.g. laser range finders. This problem comes from the fact that accurate sensors provide more information to discriminate between particles and as a result fewer particles survive. This reduces variety in the particle set and can easily lead to loosing robot pose. The simplest solution is to artificially inflate sensor noise. Another solution proposed by Blanco et al. (2007) provides a new measurement model which can take full advantage of sensors accuracy. It is based on the idea of representing the map as a collection of individual points. Always the complete set of points is compared with each measurement, therefore this approach is accurate and less prone to discarding accidentally valuable particles. In the literature, the problem of reduced variety between particles is often refered to as particle impoverishment and was studied by Kwok et al. (2005) from the genetic algorithms perspective. This paper indicates the source of the impoverishment problem, which is the resampling process performed on a finite particle set. Genetic algorithms crossover operators were used to modify particles at each algorithm iteration, in order to increase variety in the particle set. Simulation results indicated high effectiveness of this approach in solving the impoverishment problem. Evolutionary approach An interesting attempt to combine MCL with evolutionary mechanisms is presented by Ronghua and Bingrong (2004). The main focus in this paper is the problem of premature convergence when using MCL in highly symmetric environments. Proposed algorithm clusters particles into species, which evolve on two levels: inter- and intra-species. Inter-species coevolution is derived from competition of species observed in the nature and follows the Lotka-Volterra competition model, which controls the number of particles in competing species. Intra-species evolution is based on simple crossover and mutation operators, different from the ones used in DE. Experimental results confirm that the main problem of premature convergence is solved by the proposed algorithm. In addition, number of particles is adaptively adjusted and localization is precise even with few particles. Another solution to mobile robot localization problem based on MCL and

34 12 Introduction evolutionary approach is due to Gasparri et al. (2006). The key concept of the algorithm presented in this paper is a dynamic clusterization method, which allows to apply the evolutionary filter locally in each particle subset. This approach results in parallel exploration of the environment by a number of subsets. In each of them, the search for best individual is performed using custom crossover and mutation operators. Since evolutionary filter operates only locally inside subsets, this algorithm is computationally very efficient. By using multiple independent subsets the risk of errors in localization is reduced. Presented real-world experimental results confirm algorithms ability to solve the localization problem. Although not based on MCL, an interesting genetic algorithm for mobile robot localization is presented by Moreno et al. (2002). It also applies evolutionary filter only locally, but instead of MCL uses Extended Kalman Filter (EKF) to provide an estimate of robots pose. Next, a neighbourhood is created around given estimate with radius related to the confidence about the estimate. Evolutive filter is then applied in a loop to a set of solutions in the neighbourhood, in order to find the best one. At the end, the best estimate is used as a feedback for EKF to predict the new state. Algorithm proposed in Moreno et al. (2002) was tested indoor using a mobile robot equipped with 24 ultrasonic sensors. The map of environment represented rooms and corridors modelled as line segments. Compared to classic EKF, proposed algorithm demonstrated remarkable improvement in the results and was less computationally expensive than MCL. Summary Conclusion from this brief literature survey is that although there is a number of solutions to mobile robot localization problem using evolutionary approach, each is very distinct and different. The only reference in the literature to mobile robot localization algorithm based on MCL and DE is Moreno et al. (2006). Research carried out in this thesis follows the work presented in Moreno et al. (2006) with three major differences: 1. We recognise the importance of the kidnap problem as a challenging benchmark for localization algorithms. Therefore we would like to develop a solution which could also address this problem. 2. We are going to perform systematic experiments on all considered algorithms in order to obtain reliable results which could reveal the differences between the algorithms and serve as a reference for comparison.

35 1.5 Related research For the experiments, instead of artificial maps as in Moreno et al. (2006), we are going to use maps created from the odometry and measurements collected in advance by a robot in real-life. Such maps closely represent the environment and preserve the natural noise levels. By using maps based on real-life data we expect our results to be valid also in real-life.

36 14 Introduction

37 Chapter 2 Monte Carlo Localization 2.1 Theory Overview Monte Carlo Localization (MCL) represents a sampling-based class of filters in which theoretical probability distribution on the state space is approximated by a finite set of samples X t = {x p t ; p = 1,...,P }, called particles, for each time-step t. 1 Every particle is a single point in the state space and represents a hypothesis (belief) about the true state. In robot localization, we are interested in estimating the current state of the robot x t. Typically, two-dimensional case is considered where x t = [x, y, θ] T, i.e. x t represents robot position in XY-coordinates and orientation. State estimation is based on the input data Y t. Here, Y t represents all the data gathered from time-step 0 until t. In mobile robotics, typically two types of data are used: sensor readings Z t = {z 0,...,z t } and motion readings (odometry) U t = {u 0,...,u t }. Often in practice both types of data arrive alternately i.e. Y t = {z t, u t 1, z t 1, u t 2,...,z 1, u 0, z 0 }. Since odometry data u t represents robots movement (motion command) from state x t to x t+1, at time-step t the most recent available odometry data is u t 1, as indicated in Figure 2.1. In order to correctly estimate current robot state, particles should be sam- 1 For the sake of notation simplicity, particle index p will be skipped when not necessarily required. In such cases, x t refers to any particle from the X t set.

38 16 Monte Carlo Localization Figure 2.1: Robot state transformation and input data diagram. pled according to conditional probability p(x t Y t, m), where m represents environment model (map). This posterior distribution can be estimated recursively at each time-step using Bayes filters under Markov assumption, which states that the future data is independent of past data if the current state is known [Dellaert et al. (1999); Thrun et al. (2001)]. The Bayesian recursive calculation of posterior distribution p(x t Y t, m) can be accomplished in two phases: prediction and update. In the prediction phase a motion model, defined as conditional probability p(x t x t 1, u t 1 ), is used to form the new particle set X t based on X t 1. As a result, X t corresponds to a new (updated) belief about robot state, but only with odometry taken into account. Motion models are discussed in Section Next is an update phase where each particle x p t from the X t set is assigned with a weight w p t according to the measurement model. In general, measurement model is as a conditional probability p(z t x t, m) which defines the likelihood of obtaining a z t measurement assuming robot pose in the environment m is x t. Measurement models are discussed in Section At the end, it is necessary to resample X t according to particles weights (Algorithm 1, lines 7-9). Particles with higher weights are more likely to be copied to X t, even more than once. Only after resampling process, particle set X t approximates posterior distribution p(x t Y t, m). Each iteration of MCL algorithm involves both phases and is performed every time a new motion and sensor data are available, providing recursive state estimation of the robot pose. Figure 2.2 presents an example of successful global localization of the robot using MCL. Please note that step numbers used in the figure refer to algorithm iterations.

39 2.1 Theory 17 Algorithm 1 MCL(X t 1, u t, z t, m) 1: X t = X t = φ 2: for p = 1 to P do 3: x p t = sample motion model(u t, x p t 1 ) 4: w p t = measurement model(z t, x p t, m) 5: X t = X t + x p t, wp t 6: end for 7: for p = 1 to P do 8: draw p [1,...,P] with probability w p t 9: add x p t to X t 10: end for 11: return X t (a) Step 0 (b) Step 2 (c) Step 4 (d) Step 6 Figure 2.2: Successful global localization of the robot using MCL (a) initial random distribution of particles (b) significant uncertainty about robots pose (c) particles start to gather in the most likely areas (d) successful localization. True robot pose is indicated by the arrow. Particles are marked in blue and sensor readings in red.

40 18 Monte Carlo Localization Motion models The motion model, p(x t x t 1, u t 1 ), is a probabilistic representation of robot kinematics [Thrun et al. (2005)]. For robots operating in a plane x t = [x, y, θ] T is sufficient pose representation. Second input parameter u represents either odometry reading or control command. Control command, used by velocity motion model, is in( a form ) of translational velocity v and rotational velocity ω, thus u t =. The vt benefit of such representation is that it allows to calculate x t+1 in advance at time t. However, physical robot movements are erroneous, so there might be a significant difference between actual robot state and the one predicted with velocity motion model. In general, odometry motion model is more accurate, because robot pose is calculated after the movement, based on the data from wheel encoders. In this project, we use odometry motion model, which is described with details in the remaining part of this section. Odometry system provides information about relative robot displacement ) ( xt 1 from state x t 1 = [ x,ȳ, θ] T to x t = [ x, ȳ, θ ] T, thus u t =. Next, x t u t is decomposed into a sequence of three steps: rotation δ rot1, followed by a straight line motion δ trans (translation) and finally second rotation δ rot2 (Figure 2.3a). Each of these movements is assumed to be corrupted by independent noise. ω t (a) (b) (c) Figure 2.3: Sampling from odometry motion model (a) most typical case, when translational and rotational errors are similar and moderate (b) large translational error and small rotational error (c) small translational error and large rotational error. Each point in the figure represents a single particle.

41 2.1 Theory 19 Algorithm 2 sample motion model(x t 1, u t ) 1: δ rot1 = atan2(ȳ ȳ, x x) θ 2: δ trans = ( x x ) 2 + (ȳ ȳ) 2 3: δ rot2 = θ θ δ rot1 4: ˆδrot1 = δ rot1 sample(α 1 δ rot1 + α 2 δ trans ) 5: ˆδtrans = δ trans sample(α 3 δ trans + α 4 ( δ rot1 + δ rot2 )) 6: ˆδrot2 = δ rot2 sample(α 1 δ rot2 + α 2 δ trans ) 7: x = x + ˆδ trans cos(θ + ˆδ rot1 ) 8: y = y + ˆδ trans sin(θ + ˆδ rot1 ) 9: θ = θ + ˆδ rot1 + ˆδ rot2 10: return x t = (x, y, θ ) T Decomposed u t is used to draw particles from p(x t x t 1, u t 1 ) according to Algorithm 2. Motion errors e.g. due to drift or slippage are represented by α 1 to α 4 and are robot specific. Influence of this parameters on particles drawn from motion model is presented in Figure 2.3. In Algorithm 2, sample represents sampling from arbitrary noise function e.g. zero-centered Gaussian distribution with given standard deviation. Since sample parameters depend on the actual movement (δ rot1, δ trans, δ rot3 ), the further the robot travels the more uncertain is its pose estimation from motion model. An example is presented in Figure 2.4 where robot is assumed to travel in NE direction. At a distance of 1, 2 and 5m a number of particles is sampled from the motion model and each time they are spread more widely, indicating higher uncertainty about robot pose. It is important to notice that Algorithm 2 implements sampling approach rather than a closed-form calculation of p(x t x t 1, u t 1 ). Sampling approach requires two input parameters (x t 1, u t ) and returns a random particle x t drawn from p(x t x t 1, u t 1 ) distribution, which is required by MCL algorithm. We are not interested in calculating the exact value of probability distribution p(x t x t 1, u t 1 ) for a given x t, x t 1 and u t, which is the purpose of closed-form calculation approach Measurement models Measurement models describe the process of taking measurements of the environment by the robot [Thrun et al. (2005)]. They explain how physical properties of the environment are translated and represented as measurements. Exact definition of the measurement model depends on the type of sensors used by the robot e.g. camera, laser range finder, sonar etc. Each

42 20 Monte Carlo Localization Figure 2.4: Sampling from motion model with small translational error and rather large rotational error. Particles after 1, 2 and 5m of robots motion in NE direction. of them has different properties and measurement methods. However, every measurement model has to explicitly model noise in the sensors. Common approach in the literature is to define measurement model as a conditional probability distribution p(z t x t, m), where x t is robot pose at time-step t, z t is the measurement and m is the map of the environment. It is important to notice that, contrary to motion models, we are now interested in closed-form calculation of p(z t x t, m). In other words, we want to calculate the probability of obtaining measurement z t assuming x t to be robot pose. In the literature, a number of algorithms exist, which present approach similar to sampling from motion model [Rusinkiewicz and Levoy (2001); Zhang (1992)]. Such algorithms are designed to find a transformation that matches best two clouds of points (e.g. representing measurement and map), but not to evaluate how given input point clouds match. Therefore, they are not applicable as measurement models in MCL. Since the data used is this project was collected by a robot equipped with a laser range finder, we will focus on this type of sensors. In principles, distance to nearby obstacles is measured using a laser beam. Complete measurement z t = {z k t ; k = 1,...,K} consists of K individual readings from the sensor, covering a given range (e.g. 180 ) at time-step t. In the remaining part of this section three types of measurement models are presented: beam model, likelihood fields model and Range Scan Likelihood Consensus.

43 2.1 Theory 21 Beam model Beam model is the most physically plausible and thus intuitive measurement model. In principles, it simulates each individual reading zt k in a measurement using ray casting method. Expected distance to obstacle zt k is calculated based on robot pose, sensor orientation, beam relative angle and map. Next, it is compared with the actual sensor reading. Based on the assumption that each laser beam reading is independent, following factorization is possible K p(z t x t, m) = p(zt k x t, m) k=1 and allows to calculate observation likelihood for the complete measurement z t. Individual reading probability p(z k t x t, m) in the beam model (Figure 2.5) is a combination of four densities, each representing a different type of noise in laser range finder measurements: 1. Correct reading with local noise When laser range finder reading is correct, i.e. zt k corresponds to the estimated distance to the nearest obstacle zt k, measurement noise is modelled as a narrow Gaussian distribution with mean zt k. Such noise is resulting from limited sensor resolution and physical disturbances to the measurement signal. 2. Unexpected objects Rising from the fact that map m is static, but the environment is dynamic, laser range finder can sometimes produce readings which are smaller than expected. In the indoor environment this is the case e.g. with people or doors. Such dynamic objects are treated as a noise and modelled by exponential distribution. 3. Failures Sometimes obstacles are not detected and sensor returns maximum range reading z max. Such errors typically occur when measuring black, light-absorbing objects. This type of errors is common and thus it is important to include it in the measurement model as a single pointmass distribution centered at z max. 4. Random measurements This category includes all unexplainable readings which are not covered by previous three noise models and simply represents such random noise as a uniform distribution spread over entire sensing range.

44 22 Monte Carlo Localization Figure 2.5: Individual reading probability p(zt k x t,m) in the beam model. Numbers indicate the type of noise represented by the corresponding part of the curve. zt k is the expected reading. Beam model has three main disadvantages. First of all, the ray casting operation has to be performed for each individual beam and robot pose resulting in long computation time. It might be reduced either by using only part of the information (e.g. every second reading) or computing in advance and pre-cashing ray casting algorithm. However, in both cases results precision might be decreased. Secondly, if the assumption about individual reading independence is not fulfilled (e.g. when adjacent beams are obscured by the same obstacle), then beam model may cause robot to be overconfident and yield poor results. Last drawback is high number of intrinsic parameters required to model p(z k t x t, m) accurately. They can be obtained only by empirical studies of particular sensor. Likelihood fields Introduced by Thrun et al. (2005), this model offers interesting alternative to the beam model, but it lacks a plausible physical explanation. It is rather an ad hock algorithm not derived from sensor physics. First step in likelihood fields model is to translate measurement from local sensor coordinates to global map coordinates. Next, Euclidean distance to the closes obstacle is calculated for each individual reading. Finally, measurement probability is calculated assuming zero-centered Gaussian distribution in XY-space. Similarly to the beam model, failure and unexpected random measurement probabilities are also included in the estimated measurement likelihood. For details please refer to Thrun et al. (2005). This main advantage of this approach is smooth likelihood function, because

45 2.1 Theory 23 small changes to robot pose x t are not affecting Euclidean distance to the closest object too much. In case of beam model, even a small change in robot pose can result in significant changes of likelihood, because distance to object is calculated along a specific beam, instead of XY-space search. Likelihood fields model has three main shortcomings. First of all, it does not take into account dynamic objects in the map causing short readings. Secondly, each reading is directly transformed to map coordinates without ensuring clear path as if the sensor could see through walls. Last but not least, likelihood fields model requires complete map of the environment, so it cannot handle unexplored areas well. Range Scan Likelihood Consensus (RSLC) Recently proposed by Blanco et al. (2007), Range Scan Likelihood Consensus (RSLC) is claimed to be a robust measurement model, which exploits high precision properties of laser range finders. Authors define RSLC as a consensus theoretic method for combining the likelihood values of individual readings in the measurement. The key point of RSLC is the likelihood approximation method for a sensor reading z k t. After translating z k t to map coordinates, likelihood is calculated as a probability that the measured obstacle corresponds to any occupied point in the map m. Not only to the closes point (likelihood fields) and not only to the closest point along given beam (beam model). Put formally, p(z k t x t, m) = M p(c kj x t, m j ) j=1 where c kj represents the correspondence between the map point m j and the reading z k t. In this approach, the map consists of a set of M points, each represented by a Gaussian distribution. Once all likelihood values for each reading are calculated, they are fused together using Linear Opinion Pool method with equal weights for each reading, providing the complete measurement likelihood p(z t x t, m) = K p(z k t x t, m) }{{} k=1 Individual likelihood values This measurement model has a significant advantage over the other two, because it exploits the fact that laser range finder measurements are very precise. However, it requires a specific map representation and can not operate on occupancy grid maps.

46 24 Monte Carlo Localization Sensor resetting Method description Sensor resetting technique is an extension to the classic MCL algorithm [Lenser and Veloso (2000); Thrun et al. (2005)]. In principles, this method is based on monitoring long-term (w slow ) and short-term (w fast ) average particles weights. During resampling step, new random particles are added to the set with probability max {0.0, 1 w fast /w slow } where w fast and w slow are estimated using the exponential filters w slow = w slow + α slow (w avg w slow ) w fast = w fast + α fast (w avg w fast ) and where w avg is the average particle weight at given time-step t P w avg = 1 P p=1 w p t In order for w fast and w slow to have desired averaging performance, α slow and α fast parameters have to fulfil condition 0 < α slow α fast. If w fast > w slow then 1 w fast /w slow < 0, so no new random particles are added. The logic behind is that there is no need to add random particles if the recent particle filter performance is better or comparable with the one observed during a longer time period. More interesting case is when recent particle filter performance (represented by w fast ) is suddenly decreased, so that w fast < w slow and then the probability of adding a random particle is greater than zero. The main purpose of sensor resetting is to improve MCL performance in pose tracking and prevent localization errors by introducing new random particles to the set. Practical issues Sensor resetting is a relatively simple extension to MCL, however the critical part is adding new random particles. If they are not placed close to the true robot pose, they might have low weights and no beneficial influence on pose estimation. To ensure high weights of new particles, they should not be selected randomly, but sampled from the measurement model p(z t x t, m).

47 2.2 Experiments 25 Using this approach, new particles appear close to the most likely robot pose calculated based on the recent measurement. Unfortunately, when using a laser range finder sensor, sampling from the measurement model is much more difficult than sampling from the motion model. There is no formula to calculate a pose which gives best correlation between measurement and map. The state space (x, y, θ) is very often too large for a convenient search of such pose. The solution for sampling from the measurement model is complex and requires approximations as presented by Thrun et al. (2001). It is worth noting that sampling from the measurement model is not always so difficult. Contrary to the laser range-based localization, precise robot pose estimation can be obtained directly when using a number of beacons to localize. Most likely pose is easily estimated by intersecting circles originating from a particular beacon position with a radius corresponding to the measured distance from that beacon. 2.2 Experiments It is particularly a problem in the mobile robotics field to compare different algorithms performance because of many parameters and sensitivity to input data. The most important variables are: environment (map), robot path as well as sensor and odometry data. Unfortunately, there are no common methods nor standards in the mobile robotics field regarding experiments setup and performance evaluation. Additional difficulties come from the fact that Monte Carlo Localization is based on stochastic processes. As a result, localization outcome is not deterministic, so MCL performance should be measured in statistical terms by investigating a number of repeated trials. This approach ensures that results are not significantly influenced by the stochastic nature of MCL Definitions In this section we will present and discuss the most important concepts used in experiments setup. Pose estimate Robot pose is estimated from the particle set as a weighted average. Each particle contributes to the estimate according to its normalized weight. Pose error Pose error is defined as a distance from the estimated robot

48 26 Monte Carlo Localization pose to the true pose (ground truth) provided as an input for each experiment. Orientation error is not taken into account. The most important reason for excluding orientation error is that it is already implicitly incorporated into position error. For differential drive robots it is impossible to maintain a small position error and significant orientation error for extended periods of time. In other words, error in orientation quickly leads to increased position error. In case of omnidirectional robots, where orientation is independent of heading direction, such situation would be possible and explicit orientation error measurement would be necessary. Another reason for excluding orientation error is to avoid combining different units (meters and degrees) into one metric. Successful localization At any particular step of the experiment, we assume robot to be localized successfully if the pose error is less than arbitrary maximum pose error. Results of the initial experiments show that pose error can have in practice wide range of values: from meters when localization fails, down to centimetres when it is successful. Thus we set the maximum pose error to 0.25m as it seems to be a proper value to distinguish between success and failure in localization. As a result, localization is successful if the estimated robot pose is within a 0.5m circle centered at the true pose. This value is suitable also because it corresponds to an average footprint of a typical research mobile robot 2. Successful trial Every experiment consists of a number of individual and independent trials with fixed length (number of steps). At each step localization is marked as successful or not, however it might change multiple times during the trial, so there is no obvious indication if the trial was successful or not. Therefore we decided to use the following relative condition: a trial is considered as successful only if the robot is successfully localized at least for 75% of the trial duration. Such formulation has two benefits. First of all, it prevents from false trial acceptance when localization is successful only for a short period of time and lost afterwards. Secondly, if the true pose is lost and quickly retrieved, the trial can still be successful as long as the condition is satisfied. Unfortunately, the disadvantage of this approach is that it requires an arbitrary limit value which, contrary to maximum pose error limit, has direct influence on the experiment results and is also dependent on the trial length. An argument for 75% value is that it ensures successful 2 Research robot bases

49 2.2 Experiments 27 localization most of the time, while leaving a reasonable amount of time for the robot to localize when the trial length is set to 50 steps Setup details All localization experiments are performed as computer simulations based on the data gather by a real robot. In this way, experiment variables (environment, input data etc.) can be easily controlled to establish same conditions for all trials. To ensure results are not biased or case-specific four different maps are used (Section 2.2.3). Further, each trial has the same length of 50 steps. Each step represents a new measurement and a new movement read from the input files. Initial position in the map has influence on the global localization results when some parts of the map are much more distinguishable than the others. In order to eliminate influence of this factor, starting point for each trial is selected randomly. Taking into account both statistical and practical reasons, experiments are conducted until 100 successful trials are observed. Number of particles is the main parameter in the experiments. Particle density is used as a parameter only when comparing success rates due to different map sizes. Measurement model used in the experiments is likelihood fields, because it is more robust than beam model and operates on occupancy grid maps, contrary to RSLC model Maps There are two main approaches to creating maps for mobile robots localization purposes. First is top-down, where robot maps are modelled based on the available information e.g. building blueprints or local maps. Second is bottom-up, where robot maps are build from the data obtained by the robot itself during environment exploration phase. In our experiments, we use the later approach. 3 Maps are represented as occupancy grids with 0.045m resolution (cell size). Maps are presented in Figure 2.6 and their main characteristics are summarised in Table Log files and maps are provided by Jose-Luis Blanco-Claraco from MAPIR lab of the University of Malaga as a part of the MRPT project ( index.php/main_page).

50 28 Monte Carlo Localization (a) Map #1 (ISA) (b) Map #2 (COR) (c) Map #3 (SEM) (d) Mao #4 (UWB) Figure 2.6: Maps used in the experiment.