The University of BirminghamSchool of Computer Science

MSc in Advanced Computer Science

Mini-Project II

Monte Carlo Localisation for mobile robots

Marek Kopicki

Supervisor: Dr Jeremy Wyatt

April 26, 2004

Abstract

The localisation is not a trivial problem of mobile robotics. The robot must localise itself,having merely a map of the environment and sensor readings. This is a base knowledge that robotmust possess in order to be able to complete higher level tasks like path planning. The MonteCarlo Localisation (MCL) is a probabilistic algorithm successfully applied in many mobile robotsystems.

The core of the system that implements the MCL algorithm is the “T1000” Java-written robotapplication platform which cooperates with the “Saphira” robot control system developed at SRIInternational’s Artificial Intelligence Center. T1000 is a client application communicating viaTCP/IP network with a Windows/Linux server for Saphira, what enables running and testingseveral robot applications at the same time.

Thanks to affine transformation of robot’s sensor measurements, the algorithm does not haveto be synchronised with incoming sensor data, and allows processing several measurements at atime. Moreover, they can be processed at any convenient time, what simplifies using any pre-selection algorithm. In addition, the improved particles’ initialisation can also reduce the numberof particles without decreasing the effectiveness of the basic MCL algorithm.

Even though the system was not tested yet on real robots, the experiments with the “Pioneer”robot simulator are promising, and it seems that the system is comparable to the original MCLsoftware attached to Saphira.

Keywords

Localization, Monte Carlo Localisation, particle filters, position estimation, robot navigation, mo-tion model, sensor model.

ii

Acknowledgements

I would like to take this opportunity to thank my project supervisor Dr Jeremy Wyatt whopainstakingly guided me during the mini-project. Without his insightful suggestions as well asenlightening discussions with him, I would not have been able to complete this mini-project.

iii

Contents

Abstract ii

Keywords ii

Acknowledgements iii

Table of contents v

List of Figures vi

List of Tables vii

1 Preface 1

2 Monte Carlo Localisation 22.1 Localisation problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22.2 Bayesian filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22.3 Belief implementations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.3.1 Kalman Filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.3.2 Multi-hypothesis Tracking (MHT) . . . . . . . . . . . . . . . . . . . . . . . 42.3.3 Grid-based approaches (Markov Localisation) . . . . . . . . . . . . . . . . . 52.3.4 Topological approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.3.5 Particle filters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.4 How does it work? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.4.1 Sampling importance resampling (SIR) . . . . . . . . . . . . . . . . . . . . 7

3 Probabilistic Models for Bayesian Filters 93.1 Motion model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.2 Sensor model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.2.1 Modelling of sensor noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.2.2 Integration of sensor measurements . . . . . . . . . . . . . . . . . . . . . . . 14

3.3 World model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

4 Conclusions 19

A Robot motion 23A.1 Position estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23A.2 Position integration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23A.3 Extended robot motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

iv

B A few notions from statistics 26B.1 Measurements and errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26B.2 Random variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26B.3 Movement Errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

C The software platform 30C-1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

C-1.1 The server architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30C-1.2 The client architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

C-2 Installation and usage instructions . . . . . . . . . . . . . . . . . . . . . . . . . . . 32C-2.1 Files description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32C-2.2 General requirements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32C-2.3 The server . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32C-2.4 The client . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

D Mini-project declaration 35

E Statement of information search strategy 371 Parameters of literature search . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

1.1 Forms of literature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 371.2 Geographical and language coverage . . . . . . . . . . . . . . . . . . . . . . 371.3 Age-Range of literature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

2 Appropriate search tools . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 372.1 Engineering Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 372.2 Science Citation Index (SCI) . . . . . . . . . . . . . . . . . . . . . . . . . . 382.3 Dissertations Abstracts International (DAI) and Index to Theses . . . . . . 38

3 Search statements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384 Brief evaluation of the search . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

Bibliography 39

v

List of Figures

2.1 Monte Carlo localisation algorithm successfully localises the robot . . . . . . . . . 8

3.1 The probabilistic evolution of the robot pose . . . . . . . . . . . . . . . . . . . . . 113.2 Sensor readings of moving robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.3 Sensor model geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.4 Sensor noise models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.5 Affine transformation of sensor measurements. . . . . . . . . . . . . . . . . . . . . 143.6 The world map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.7 The improved initialisation of particles . . . . . . . . . . . . . . . . . . . . . . . . . 18

4.1 The Monte Carlo localisation on a large map . . . . . . . . . . . . . . . . . . . . . 22

A.1 Robot displacement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24A.2 Realistic robot motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

vi

List of Tables

2.1 Continuous implementations of belief . . . . . . . . . . . . . . . . . . . . . . . . . . 42.2 Discrete implementations of belief . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

C.1 The T1000 applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34C.2 The properties of the T1000 applications . . . . . . . . . . . . . . . . . . . . . . . . 34

vii

Chapter 1

Preface

Before futurology eventually lost its former status in our century, most of their representatives wereforeseeing a bright future for robots in our live. The role of futurology was definitely succeeded byscience which unveiled, unfortunately, that many a time, the discrepancies between fantasy andtechnical feasibility, and even theory, are very difficult to overcome. The science area is currentlyso wide that making any substantial progress in any discipline has become a real challenge. Eventhough building robots with human-like capabilities is absolutely unthinkable at the moment, onecan notice some successful application areas of robots with famous Mars-rover at the forefront - asemi-autonomous mobile robot. But is the era of intelligent home servant robots really approach-ing?

The main subject of interest of this report is a robot localisation problem, which is the problemof estimation of robot’s location, having merely a map of the environment and sensor readings. Therobot’s location is a very base knowledge that robot must possess in order to be able to completehigher level tasks like path planning. The report analyses the Monte Carlo Localisation (MCL) -a probabilistic algorithm successfully applied in many mobile robot systems [13][5][3]. Because ofits probabilistic nature, the algorithm performs well even in dynamically changing environment,where moving obstacles permanently disturb sensor readings.

The core of the system that implements the MCL algorithm is the “T1000” Java-written robotapplication platform (see appendix C) cooperating with the “Saphira” and ”Aria” - a robot controlsystem developed at SRI International’s Artificial Intelligence Center and ActivMedia Robotics [1].T1000 is thought to be a universal and easy in use platform for development miscellaneous algo-rithms for “intelligent robots”. T1000 is a client application that communicates via TCP/IPnetwork with a multi-client server, what enables running and testing several robot applications atthe same time. The server is implemented as a Windows/Linux dynamic library downloadable bythe Saphira’s Colbert environment.

Unfortunately, because I was not able to test yet the software on real robots, it is difficult to sayabout any results of experiments. The only exception is the “Pioneer” robot simulator [1] whereresults are promising and it seems that the system is comparable to the original MCL softwareattached to Saphira. The whole project, however, is only a part of the summer one where I intendto extend the system in probably mapping algorithms and do much more experimental work.

1

Chapter 2

Monte Carlo Localisation

2.1 Localisation problem

Localisation problem is a general class involving several localisation problem instances depending onthe techniques used [13][5]. The simplest and the best known one is the position tracking problem.Here the estimate of initial robot pose is known, and the problem amounts to compensation ofaccumulating errors of robot’s odometry readings. Because position tracking plays an importantrole in the MLC algorithm, I discuss this topic separately in the appendix A. More difficult is theglobal localisation problem or the wake-up robot’s problem, where a robot’s initial pose is unknown.The robot must be able to deal with multiple hypotheses about its initial pose [13]. The methodsthat solve this kind of problems are called global techniques [5]. Finally, the most difficult is thekidnapped robot problem, where already localised robot is suddenly transferred into any other placewithout being told. The kidnapped robot initially believes to be somewhere else than in fact it is.The algorithms solving kidnapped robot problem should also be capable to solve global localisationproblem, which in turn can be considered as a sub-class of the first one.

Most of existing algorithms deal only with the position tracking problem. The most efficientones are Kalman Filters, which however cannot cope with the global localisation problem. MarkovLocalisation is an extension of Kalman Filters, and solves this problem by a piecewise constantapproximation of possible robot’s poses. Unfortunately, this method is computationally expensive,even though there were proposed many improvements.

Monte Carlo Localisation appears to be the best solution of the global localization problemso far. Moreover, the MCL algorithm can solve the kidnapped robot problem either, while beingmore efficient and accurate than Markov Localization.

2.2 Bayesian filtering

The majority of approaches to localisation problem base on so called Bayes Filters. Bayes Filterrecursively estimates the posterior distribution representing the pose of the robot, basing entirelyon collected sensor data. Typically, the pose of the robot is a two-dimensional Cartesian positionplus the robot’s heading direction. Bayes filters assume that the Markov assumption holds, i.e.the current state contains all the information which is necessary to predict a future state.

The posterior or conditional distribution, also called belief Bel(xt), represents the robot’s posext

1 at time t:1or more general - the state of a dynamical system

2

Bel(xt) = p(xt|dt, dt−1, ..., d0) (2.1)

where dt, dt−1, ..., d0 denote the collected sensor data from time 0 up to time t. In particular,picking up any pose xt (at time t), the belief answers the question “what is the probability thatthe robot is at the pose xt if the collected data is dt, dt−1, ..., d0?”. Assuming that the dataconsists of perceptual data (sonar or laser readings) denoted by o (observations), and odometrydata denoted by a (actions), from the equation 2.1 we have [13]:

Bel(xt) = p(xt|ot, at−1, ot−1, ..., a0, o0) (2.2)

The equation 2.2 can by further transformed. According to Bayes rule we obtain:

Bel(xt) =p(ot|xt, at−1, ..., o0)p(xt|at−1, ..., o0)

p(ot|at−1, ..., o0)(2.3)

Note that, the denominator remains a constant value when testing the belief Bel(xt) for variousposes xt, therefore we can rewrite the equation 2.3 as:

Bel(xt) = ηp(ot|xt, at−1, ..., o0)p(xt|at−1, ..., o0) (2.4)

where

η = p(ot|at−1, ..., o0)−1 (2.5)

Since the Markov assumption implies that the future state is independent of the past state if thecurrent state is known, from the equation 2.4 we obtain:

Bel(xt) = ηp(ot|xt)p(xt|at−1, ..., o0) (2.6)

where

p(ot|xt, at−1, ..., o0) = p(ot|xt) (2.7)

The total probability theorem (see [6]) implies

Bel(xt) = ηp(ot|xt)∫

p(xt|xt−1, at−1, ..., o0)p(xt−1|at−1, ..., o0)dxt−1 (2.8)

where we integrated over all states at time t− 1. Exploiting the Markov assumption again, we cansimplify the first term in the integral 2.8:

p(xt|xt−1, at−1, ot−1, at−2, ..., o0) = p(xt|xt−1, at−1) (2.9)

From the belief definition 2.2, we find that the second term in the integral 2.8 equals:

Bel(xt−1) = p(xt−1|ot−1, at−2, ot−2, ..., a0, o0) = p(xt−1|at−1, ot−1, at−2, ot−2, ..., a0, o0) (2.10)

as the action at−1 taken at time t−1 does not influence the current state xt−1 but the future statext. Finally, we obtain a recursive update equation:

Bel(xt) = ηp(ot|xt)∫

p(xt|xt−1, at−1)Bel(xt−1)dxt−1 (2.11)

3

Approach Features Pros ConsKalman Filter unimodal accuracy sensor variety

Gaussian distribution efficiencyfirst and second moment of PDF(non)linear dynamics and observationspolynomial complex. in state dimensionposition tracking problem

Multi-hypothesis multimodal accuracy sensor varietyTracking Gaussian distribution robustness implementation

(non)linear dynamics and observationspolynomial complex. in state dimensionglobal localisation problem

Table 2.1: An overview of continuous implementations of belief Bel(x) in Bayesian Filters.

The probability p(xt|xt−1, at−1) will be further referred to as motion model, and the probabilityp(ot|xt) will be called sensor model. Since both models are usually assumed to be stationary, wecan denote:

p(x′|x, a) − motion modelp(o|x) − sensor model (2.12)

Motion model, sensor model and belief representation are the subjects of the next chapters.

2.3 Belief implementations

Bayesian Filters are very general abstract concept for recursive state estimation. The belief rep-resentation 2.1 is the key part of the update equation 2.11 and varies depending on the approachused. We can divide all representations into two groups: continuous representations (table 2.1)and discrete representations (table 2.2).

2.3.1 Kalman Filters

Kalman Filters are the most widely used variants of Bayesian Filters, especially for solving the posetracking problem. The belief distribution is approximated only by the first and the second moment(see definitions B.2 and B.4), what makes the Gaussian distribution an optimal choice (since allmoments higher than two are equal 0 - see [2]). Though Kalman Filters are very efficient (linearalgebra operations), they can represent only unimodal beliefs with limited initial uncertainty.However, in practise they works fine even in highly nonlinear systems, despite underlying linearassumptions about system’s dynamics 2.

2.3.2 Multi-hypothesis Tracking (MHT)

In Multi-hypothesis tracking the belief is represented by a linear combination of unimodal Gaussiandistributions:

2This assumption was relaxed in the extended version of Kalman Filters, by using first-order Taylor seriesexpansions to linearise the system.

4

Approach Features Pros ConsGrid-based discrete robustness efficiency

piecewise constant approximation sensor varietynon-linear dynamics and observationsexponential complex. in state dimensionglobal localisation problem

Topological discrete robustness accuracyabstract state-spaceabstract dynamics and observationsone-dimensional graphglobal localisation problem

Particle filter discrete robustnessSample-based approximation accuracynon-linear dynamics and observations sensor varietyexponential complex. in state dimensionglobal localisation problem

Table 2.2: An overview of discrete implementations of belief Bel(x) in Bayesian Filters.

Bel(xt) =∑

i

w(i)t

1√(2π)n|Λ(i)|

exp(−1

2

((xt − µ(i))T (Λ(i))−1(xt − µ(i))

))(2.13)

MHT tracks each single Gaussian hypothesis about possible robot poses (represented by weightsw(i)), overcoming in this way the main limitation of Kalman Filter. Unfortunately, MHT needssophisticated techniques and heuristics to solve sensor data association problem and to decide whento add or remove hypotheses.

2.3.3 Grid-based approaches (Markov Localisation)

In Grid-based approaches the state space is divided into regular network of adjacent cells, and thebelief is represented by a value assigned to each cell. This trick allows grid approaches modellingarbitrary distributions, and as a result, solving the global localisation problem. Grid approachesare also relatively accurate and very robust against sensor noise. The obvious disadvantage of thegrid approaches is the space and computational complexity which grows exponentially with thenumber of space dimensions, what may prevent from whichever applications of the algorithm inhigher dimensional spaces.

2.3.4 Topological approaches

The problem with the exponentially growing complexity in the grid-based approaches can beavoided by using non-metric representation of space - graphs. Each node of the graph representsa certain location in the environment, while the edges are physical connections between them3.Whereas the topological approaches can be quite efficient, they are not very precision about spacelocations, as the number of state spaces is limited.

3The topological approach is undoubtedly the most “natural” one among all the others.

5

2.3.5 Particle filters

The belief in Particle filters is represented by weighted set of n particles:

Bel(x) ≈ {x(i), w(i)}i=1,...,n (2.14)

where x(i) are poses, and w(i) are weights4 also called importance factors. In order to imitate a“regular” PDF function a particle set must satisfy the same conditions as such a regular PDF does(see condition B.1)5. Of course, it is not possible to sample at an arbitrary pose x from so defineddistribution - instead we have to use the procedure called sampling importance resampling (SIRalgorithm) described in the next chapter 2.4. The particle approximation is the core part of theMCL algorithm, which itself came by a few improvements [13], and it was proved to be the bestalgorithm known (in overall):

� the modelled system does not have to be linear

� particles can approximate virtually all possible distributions

� can tackle both the global localisation and the pose tracking problem

� copes well with noisy sensor data

� particles can “concentrate” only in interesting space regions

� adjustable CPU workload and accuracy by changing the number of particles

� easy applicable anytime algorithm

2.4 How does it work?

Implementing the MCL algorithm is not very difficult once we know how to realize a motionmodel (see 3.1) and a sensor model (see 3.2). However, due to the discrete representation of beliefBel(x), the MCL recursive equation 2.11 can be easy to understand without the detailed knowledgeof motion and sensor models.

As we learnt from the previous section 2.3.5, the belief Bel(x) is a PDF function which returnsa value of the probability that the robot pose is x. In particle filters we cannot evaluate Bel(x) forany arbitrary x6, instead we have a set of poses x(i) and weights w(i) which are simply the valuesof Bel(x) in x = x(i).

The set of particles {x(i)0 , w

(i)0 } for t = 0 represents the knowledge about the system at the

beginning of iteration of the recursive equation 2.11. If we do not have any information about apossible pose of the robot, all n particles are uniformly distributed on the map (see figure 2.1), andall weights have the same value 1/n. In the MCL algorithm instead of integration over continuousspace (equation 2.11), we repeat the subsequent steps exactly n times:

1. Sample a particle x(i)t−1 from the belief distribution Bel(xt−1) using sampling importance

resampling algorithm 2.4.1. In other words, the higher the weight w(i)t−1 of a particle i, the

larger probability of picking up this particle when sampling.4which correspond to PDF’s values.5To be strict, the normalisation condition is not required, but it can be used to avoid over/underflow.6In fact, it is required in the Mixture-MCL [13] algorithm, and can be attained by using kd-trees [12].

6

2. Apply the motion model p(x(i)t |x(i)

t−1, at−1) to the sampled particle x(i)t−1. Intuitively, if the

robot has moved by a distance d in a direction θ7, the particle x(i)t−1 will be translated by the

same distance and in the same direction8 to the new pose x(i)t , but with some uncertainty

(see equation 3.9).

3. Apply the sensor model p(ot|x(i)t ) to the new particle pose x

(i)t . In other words, calculate the

probability of observing ot (what robot observes at time t), being at the pose x(i)t

9. Assigna new weight value to the i− th particle:

w(i)t = p(ot|x(i)

t ) (2.15)

After repeating the above steps n times, we obtain a new set of n particles {x(i)t , w

(i)t } comprising

the new belief Bel(xt). Some of them will be quickly eliminated (small weights), whereas someof the particles will concentrate in regions where the probability of observing ot is higher (largeweights).

The effects of updating the belief Bel(x) according to the above procedure, are shown in thefigure 2.1. The MCL algorithm quickly localises the robot on the map.

2.4.1 Sampling importance resampling (SIR)

In the first step 1 of the recursive procedure, we draw a single particle xi from the belief distributionBel(x) with probability proportional to the particle’s weight wi. In the simplest approach weconstruct a cumulative density function CDF (a table of length n) from the particle set {x(i), w(i)}as below:

CDF =

x(1), w(1)

x(2), w(1) + w(2)

. . .x(n), w(1) + w(2) + . . . + w(n)

(2.16)

To draw a particle from so defined CDF function, we do:

� generate a random number y ∈ [0, w(1) + w(2) + ... + w(n))10

� use binary search to find an index i of a particle from the CDF table, such that w(i−1) ≤y < w(i) and w(0) = 0

7what is encoded in action at−18but related to the current location and heading of the particle9Note that, because each pose x

(i)t consists of two elements - location and heading direction, the world may look

completely different in different directions even at the same location.10One can normalise the particle set before constructing the CDF table - then y ∈ [0, 1).

7

Robot location ������:

Robot location ������*

Robot location ������*

Robot location XXXXXXXz

Figure 2.1: The effects of updating the belief Bel(x) according to the recursive MCL equation 2.11.In the topmost figure all the particles are uniformly distributed on the map, since initially a robotpose is unknown. After a few updates the particles are concentrating in regions indicated by lastobservations as the most probable locations of the robot. Finally, the number of “suspected”regions lowers to one, what points out that the robot is successfully localised.

8

Chapter 3

Probabilistic Models for BayesianFilters

3.1 Motion model

The motion model p(x′|x, a) answers the question: “what is the probability of finding the robot atpose x after performing the action a′, if the robot’s initial pose was x′?”. The model is characterisedby deterministic kinematic equations describing robot’s movement, as well as it must containinformation about movement uncertainty.

As shown in the appendix A, the uncertainty of robot’s movement can be described by threetypes of possible errors which accumulate under motion. We can distinguish two mechanismsproducing errors [7]:

� the evolution of already accumulated errors during motion

� “the addition of new integration errors” [7]

Let p denote the pose of the robot:

p =

xyΘ

(3.1)

then the uncertainty of the robot pose is expressed by three dimensional covariance matrix (seealso definition B.12):

Λ(p) =

σ2xx σ2

xy σ2xΘ

σ2yx σ2

yy σ2yΘ

σ2Θx σ2

Θy σ2ΘΘ

(3.2)

If all components of the robot pose vector 3.1 were independent, all off-diagonal elements wouldequal zero. Unluckily this is not the case - for example if the robot performs any move forward,uncertainty of the initial heading of the robot will result in uncertainty of its location. Thus, thecovariance matrix should evolve as the robot’s pose does.

In the chapter A.3 we derived the iterative formula of robot motion:

p′ = f(p) (3.3)

9

where

fx = x + ∆s cos(Θ + α)fy = y + ∆s sin(Θ + α)fΘ = Θ + ∆Θ (3.4)

and each step consists of two parts:

1. translation ∆s in the direction Θn

2. rotation by the angle ∆Θ

The covariance matrix evolves according to the formula (for details see [7]):

Λ(p′) = F(p̂)Λ(p)F(p̂)T + R(−Θ)ER(Θ) (3.5)

where F(p̂) is the Jacobian

F(p̂) =

∂fx

∂x∂fx

∂y∂fx

∂Θ∂fy

∂x∂fy

∂y∂fy

∂Θ∂fΘ∂x

∂fΘ∂y

∂fΘ∂Θ

p̂

(3.6)

describing the evolution of an initial error, R(Θ) is the ordinary rotation operator

R(Θ) =

cos(Θ) sin(Θ) 0− sin(Θ) cos(Θ) 0

0 0 1

(3.7)

and E is the error matrix

E =

kT ∆s 0 00 0 00 0 kΘ∆Θ + kD∆s

(3.8)

The error matrix introduces to the resultant covariance matrix Λ(p′) a new integration error inevery iteration step of the equation 3.5 (see chapter B.3). In the matrix 3.8 we also assumed thatthe initial robot’s heading is along X axis, so there is no pose uncertainty along Y axis.

The equation 3.5 is an elegant way of expressing the evolution of the covariance matrix. Buton the other hand, in the motion model we are only interested in the probabilistic evolution ofparticle poses1, not in the explicit form of the covariance matrix. If we assume that the initialuncertainty is zero, and the translation and rotation are independent variables, we can use directlythe formula 3.1 simply adding zero mean Gaussian noise with the variance B.16. The robot motionformula takes the form:

xn+1 = xn + (∆s + Ntrans) cos(Θn + α + Nrotation + Ndrift)yn+1 = yn + (∆s + Ntrans) sin(Θn + α + Nrotation + Ndrift)Θn+1 = Θn + ∆Θ + Nrotation + Ndrift (3.9)

1This is true only for particle filters.

10

Figure 3.1: The probabilistic evolution of the robot pose represented by a “particle cloud”. Thecloud spreads and takes a characteristic banana shape after a few turns. The solid line representsthe robot’s trajectory measured by odometry sensors, the clouds of particles approximate thedensity p(x′|x, a).

where the noise is modelled by Gaussian distributions: Ntrans

Nrotation

Ndrift

=

N (0, kT ∆s)N (0, kΘ∆Θ)N (0, kD∆s)

(3.10)

Figure 3.1 depicts the effect of adding the noise factors 3.10 to the deterministic motion equa-tion 3.1. The solid line represents the robot’s trajectory measured by odometry sensors2, the“clouds” of particles approximate the density p(x′|x, a) - the more crowded particles are, the morelikely the robot’s pose is. Starting from the leftmost figure, we can observe the evolution of particlecloud. The cloud spreads and takes a characteristic banana shape after a few turns.

3.2 Sensor model

The sensor model p(o|x) answers the question: “what is the probability of observing o if the robotpose is x?”. To simplify the problem, we turn our attention only to simple range finders, inparticular to sonars. For range finders, to calculate the value of p(o|x) we have to perform thefollowing steps (compare to [13]):

1. compute the average (most probable) locations of observed targets (see figure 3.2)

2. model sensor noise

3. for each single measurement compute the probability of the measurement knowing both therobot pose and the target location

4. integrate calculated probabilities into a single result

Since the first step is usually the task of low-level sensor drivers, we can leave out many tediouscalculation related to a sensor design, and focus solely on probabilistic and geometrical aspects ofthe problem.

2We used here the Pioneer simulator.

11

Figure 3.2: The long continuous line (black) represents robot moving along the walls of a smallroom. Sensor readings are denoted by “radial beams” (green lines), in most cases ending at theroom walls (orange spots).

Figure 3.3: Sensor model geometry: l is the distance to the nearest target, the flat surface of thetarget is at an angle φ to the perpendicular [7].

3.2.1 Modelling of sensor noise

While Bayesian Filter equations are quite general, an implementation of the sensor model to a largeextent depends not only on sensor design (approach and implementation) but also on environmentcharacteristics. The most popular way of modelling of measurement uncertainties requires onlythe knowledge of the actual (l) and measured (d) distance to the target (figure 3.3)3. The sensornoise can be approximated by a Gaussian distribution centred at l (see [5]). For sonars we need,however, a third parameter - the incidence angle φ of the sonar signal (figure 3.3). In the approachpresented in [7], the sensor noise is modelled by two distributions:

1. the on-axis distribution where φ = 0˚ (single reflections from target)

2. the off-axis distribution where φ is too large to detect a direct reflection of the sonar signal(multi-reflections)

An example of the on-axis distribution is shown in figure 3.4. The on-axis distribution modelsthe following phenomena:

3All objects are usually assumed to be flat surfaces that can be modelled by lines in two dimensional world.

12

Figure 3.4: Sensor noise models. Starting from the left: on-axis distribution for φ = 0˚, interpo-lation of on-axis and off-axis distributions for φ = 10˚, off-axis distribution for φ ≥ 15˚.

� a true sensor return described by a Gaussian distribution, with mean d = l and varianceσ2

sensor

� a random sensor return up to the distance to a target l4, caused by unmodelled dynamicobjects, and described by a Poisson distribution (see [6]) with false positive rate λF

� an sensor max-range return with finite probability Prange (object detection failure), modelledas narrow uniform density at d ∼ r, where r is a sensor range5.

Furthermore, the Gaussian distribution is attenuated by the linear factor β(l), as the chance ofdetecting of any object decreases with increasing distance. The complete mathematical formula ofthe on-axis distribution can be expressed as [7]:

d < l : λF exp(−λF d) + β(l)N(l,σ2sensor)(d) exp(−λF d)

l ≤ d < r : β(l)N(l,σ2sensor)(d) exp(−λF l)

d ∼ r : Prange (3.11)

The off-axis distribution (figure 3.4) models the multi-reflection of the sonar echo before return-ing, i.e.:

� a random sensor return up to the distance to a target l (similarly to on-axis distribution),described by a Poisson distribution with false positive rate λF

� an multi-reflection return, also described by a Poisson distribution but with higher rateparameter λR (we expect to detect anything eventually)

� an sensor max-range return, the same as for the on-axis distribution

Finally, we can denote the formula of the off-axis distribution [7]:

d < l : λF exp(−λF d)l ≤ d < r : λR exp(−λRd) exp(−λF l)

d ∼ r : Prange (3.12)

If the incidence angle φ is between 0 and a certain threshold value, the PDF of the modelled sensornoise can be obtained as an interpolation6 of the on-axis and the off-axis distributions, so that

4also called false positive measurement5Compare to the approach proposed in [7].6Before interpolating, both densities should be normalised.

13

Figure 3.5: Transformation of sensor measurements. The affine transform is entirely defined bytwo pairs of locations: {xt, xt−1} and {x′t, x′t−1}. The transformation does not change the anglesα1 and α2. Because the scale of the transform |x′t − x′t−1|/|xt − xt−1| is usually different than1, in order to preserve the distances to targets d1 and d2, their locations must be transformedseparately. We also do not assume that the robot moves along the straight line between the posesxt−1 and xt.

the resultant PDF is a function of three variables: the measured distance to a target d, the realdistance to a target l, and the incidence angle φ (see figure 3.4)7.

3.2.2 Integration of sensor measurements

In order to calculate the final value of the sensor model p(ot|xt), for an arbitrary observation ot,and if the previous robot pose was xt−1, we perform the following steps (see figure 3.5):

1. Collect all sensor measurements mi8 recorded between time t−1 and t, where each measure-

ment mi contains the target location and the robot location.

2. For each measurement mi : calculate the distance di between the robot and a target, calculatethe angle αi between the reading direction and the robot heading direction (see figure 3.5).

3. Transform robot locations at which the measurements mi were recorded onto new coordinates(see figure 3.5) - the affine transform is entirely defined by two pairs of locations: {xt, xt−1}and {x′t, x′t−1}9.

4. In new coordinates : restore the locations of targets from di and αi; this step is necessary so7Note that the PDF must be also determined for l > r to give correct densities for d < r in case a target is just

behind the sensor range l > r.8contributing to observation ot.9In the MCL algorithm the first pair {xt, xt−1} describes the robot move from time t − 1 up to t, whereas the

second one {x′t, x′t−1} describes the move of a particle within the same time period.

14

as to keep old measured di10.

5. For each measurement m′i (in new coordinates) : extract from the world model (see chap-

ter 3.3) the actual distance li to the target, and the incidence angle φi.

6. For each measurement m′i : calculate the probability pi of observing m′

i, using the sensornoise model 3.2.1, and having all three parameters di, li and φi.

7. Finally, assuming conditional independence of the individual measurements m′i, calculate the

probability p(ot|xt):

p(ot|xt) =∏

i

pi (3.13)

where pi is the probability of observing m′i (after transforming to new coordinates).

Because of using affine transforms the above procedure differs a bit in some points from this whatis known in literature. The most important features of the procedure are:

� Enables the MCL algorithm processing several measurements at a time, each of them can berecorded at any time between t− 1 and t.

� The MCL algorithm does not have to be synchronised with sensor measurements. Theevaluations of possible robot’s poses (particles) can be performed at any time.

� The CPU workload can be smoothly adjusted by changing the length of the period [t− 1, t],and the number of used measurements11.

� The additional needed CPU time can be neglected, as the real bottleneck of the MCL algo-rithm is the procedure of extraction of the parameters li and φi (see step 5).

3.3 World model

The conception of the world model arose out of the idea of separation the MCL algorithm (includingthe motion and sensor models) from the implementation of a robot’s world. Though an absoluteseparation could not come into play12, the only moment the world model interacts with the MCLalgorithm was reduced to the extraction of the distance li to a target and the incidence angle φi,given the measurement mi (see step 5).

The main features of the current implementation of the world model are:

� the world is two dimensional

� the world is bounded by a rectangle

� all objects have floating-point vector representation

� all objects are lines

� the world is stored in files in two formats compatible with those from Saphira13

15

Figure 3.6: The world map is divided into a set of adjacent square cells. Each cell contains pointersto all objects which can be visible to the robot from within the cell (defined by the robot’s sonarrange). The map consists of about 1200 cells.

16

The world map is divided into a set of adjacent square cells (see 3.6), where each cell i containstwo things:

1. pointers to all “local” objects which can be visible to the robot from within the cell (definedby robot’s sonar range).

2. the minimal c(i)min and maximal c

(i)max distance to all local objects

The additional resource overhead needed to maintain all data structures is not large and is ap-proximately proportional to the number of cells. The overall growth in the efficiency of the MCLalgorithm is especially visible for large world maps. For example, for the world map 3.6 the MCLalgorithm works about 20 times faster, and on Pentium IV 2.6 MHz it is possible to update up to100000 particles every second.

The procedure of extraction of the parameters li and φi, given the measurement mi, involvesbasic geometrical transformations and consists of:

1. identifying the cell on the basis of the coordinates of the measurement mi

2. calculating the reading “direction” αi having the target and the robot locations

3. finding the nearest target in direction αi from the group of visible objects from the cell

4. calculating li and φi

Each cell contains also information about the minimal and maximal distance to the local objects,what can be used for “smart” initialisation of particles (see figure 3.7). The whole procedure isonly slightly slower than the “regular” one (also O(n)), and needs at least one measurement:

1. Calculate the minimal min and the maximal max distance from the robot to recorded tar-get(s), having the group of measurements mi.

2. Collect all the cells {i} that c(i)min < max and c

(i)min > min14.

3. Generate n random particles by repeating the following steps: pick up at random one of thecollected cells, uniformly generate a single particle within the chosen cell.

This procedure has a higher potential: for example one can generate particles which are alreadyorientated against the objects. One must add, that the current version of the procedure is notresistant against false measurements, as well as does not take into account its uncertainties.

10We do not assume the Doppler effect! The affine transform changes the scale of objects, while it does not changetheir proportions.

11In addition to the numbers of particles.12The model of the sensor noise makes many assumptions about the shape of world’s objects, and of course, the

number of world dimensions is two (it can be changed relatively easy).13Unfortunately Saphira uses more formats, but the two supported ones are the most common.14One must be careful about using the minimal distance, since as some of the close object might not be recorded

by the robot.

17

Figure 3.7: The improved initialisation of particles. Basing on sensor measurements, all the parti-cles are generated only within the restricted regions.

18

Chapter 4

Conclusions

Although the system was not tested yet on real robots, I experimented with the “Pioneer” robotsimulator. No matter how perfect the simulator is, it will remain only the simulator. Therefore,the main purpose of the experiments were not obtaining any concrete numerical results, but ratherdeveloping, testing and optimising of the software. The experiments verified most strong and weakpoints of the basic MCL algorithm. I mention here only a few major problems.

The tradeoff between the number of particles and efficiency. Since the belief distributionBel(x) can contain only a finite number of particles, it is possible that there will be noparticles initiated close to the actual pose of the robot1. To make the matter worse, evensupposing there are any particles there, because the algorithm is probabilistic, the particlesmay not “survive”. This scenario is especially probable if sensors are not very accurate, and inconsequence their weights will not be significantly higher then the average. A trivial remedyto this problem is to increase a number of particles, what cannot be satisfying, though.

Movement approximation. The solution to the aforementioned problem might be to add somemore uncertainty to the motion model. The particles would then “penetrate” larger spaceand finally localise the robot. This approach works well in combination with noisy sensors- even if initially a group of particles is not very precisely localised, it can gradually adjustor synchronise with the true robot pose when moving. Unluckily, such a group of alreadylocalised particles tends to spread very quickly, what is particularly visible in the absence of(non-collinear) sensor measurements.

The sensor noise model. Paradoxically, accurate sensors (plus “narrow” sensor distribution)make the MCL algorithm converge too quickly - this gives false results especially if the sensoraccuracy does not correspond to its noise model. In practise, the modelled sensor noise shouldalways overestimate the actual one, what gives particles more chance to synchronise with theoptimal pose (at least optimal in the context of sensor readings).

Summarising, it is not enough to have perfectly modelled motion and sensor noise, the MCLalgorithm will not be optimal at all. On the other hand, an excessive increase of the number ofparticles makes the system inefficient. Are they any solutions to these problems?

The two improvements proposed in the report could be the base for any more sophisticated algo-rithms. Moreover, they can be easily combined with, for example, the mixture MCL algorithm [13]described briefly at the end.

1in three dimensional space

19

The affine transformation of sensor measurements 3.5 would allow using a procedure,which selects out the measurements that “sketch” any characteristic shapes or objects in theenvironment. Obviously, one cannot have too much influence on sensor measurements, but such asmart selection, even together with controlling the robot itself2, may help the problem of prematureconvergence of the algorithm to false solutions.

With using the improved particle initialisation 3.7, the conventional MCL algorithm needsapproximately two times (the map 2.1) or even four times (the map 4.1) less particles to have thesame effectiveness as with the standard uniform initialisation. These results vary depending onthe map, sensor readings, number of used cells etc. The main strength of the algorithm 3.7 is itssimplicity, speed3(all time-consuming calculations are done off-line), and not very large memoryrequirements (number of cells × two floating point numbers).

The mixture MCL algorithm [13] is an interesting algorithm, which combines the regularMCL and so called dual MCL algorithm according to the scheme:

Mixture MCL = (1− Φ) · Regular MCL + Φ ·Dual MCL 0 ≤ Φ ≤ 1

In the regular MCL 2.4 samples are drawn form p(x′|x, a) · Bel(x), and then the weights arecalculated from the sensor model p(o|x). The dual MCL “inverses” this process - the proposaldistribution is p(x|o) and the weights are now directly proportional to belief Bel(x). The dual MCLrequires sampling from p(x|o) what can be realised by using kernel-density trees [12] - a piece-wisecontinuous probability distribution built out of particle set. The mixture MCL algorithm has beenproven to be superior to the regular MCL [13], in particular the localisation error can be an orderof magnitude smaller than in the conventional MCL.

2To explore the regions that seem to be characteristic. Going further, basing on the map, one can even createan optimal exploration strategy.

3otherwise it would be better to simply increase the number of particles

20

Robot locationCCCCCCCCCO

Robot locationAA

AA

AA

AAAK

21

Robot locationAA

AA

AA

AAAK

Figure 4.1: The Monte Carlo localisation on a large map. All particles are initialised only in thevicinity of objects (the topmost figure). After about 15 seconds the MCL algorithm successfullylocalises the robot’s pose (the bottom figure).

22

Appendix A

Robot motion

Modern mobile robots can walk, jump, swim, fly but even the simplest way of moving - rollingin two dimensional spaces can be surprisingly complex and the exact robot motion can only beapproximated. But even though in this report we discuss only motion of rolling robots, the MCLalgorithm remains the same, with the exception of so called motion model (chapter 3.1).

A.1 Position estimation

Position estimation is an inherent part of all navigation systems. One can distinguish two generalgroups of methods of position estimation: internal and external[7]. Internal methods (odometry)base on open-loop sensors, e.g. they do not rely on any feedback from environment. Externalmethods uses sensor measurements directly from environment to record and correct the actualrobot pose. The most popular internal methods use wheel encoders registering the number ofcounts of revolving shaft when robot is moving. External methods can employ Global PositioningSystem (GPS), external beacons like sonar or laser but also not very accurate compass. WhereasSaphira can support external methods, in the motion model (see chapter 3.1) it is usually sufficientto use only odometry readings.

A.2 Position integration

Odometric sensors gives information about a distance of robot moves, its speed and relative changesof robot’s heading direction. It is necessary to integrate this information over time to give anestimate of the robot pose.

Assuming that we know what is the correspondence between the number of counts and thetravelled distance, and that the robot has two wheels (left and right), from the figure A.1 is clearlyvisible that in time period ∆t robot changes its pose according to the following rule:

dΘ =2π(∆r −∆l)

2πd=

(∆r −∆l)d

∆s =(∆r + ∆l)

2

v =∆s

∆t(A.1)

23

Figure A.1: Robot displacement within the time period ∆t.

where the angle ∆Φ is given in radians, ∆s is the overall movement of the robot and v is its speed.If at the time tn the global pose1 of the robot Pn is given by (xn, yn,Θn), then the pose Pn+1 atthe time tn+1 can be calculated from the simple rule:

xn+1 = xn + ∆s cos(Θn)yn+1 = yn + ∆s sin(Θn)Θn+1 = Θn + ∆Θ (A.2)

Both the equation A.1 and the equation A.2 are no more than a crude simplification of a realphysical motion, however they can be shown to converge to the ideal solution when ∆t → 0. Whilethe equation A.1 is usually precise enough, as the sensor update rate can be as small as 10 ms, theequation A.2 can be a source of large integration errors. This is especially the case for externalcontrol systems2, where the time period ∆t is typically larger than in the equation A.2 and dependson used communication channel3.

A.3 Extended robot motion

The equation A.2 is valid only for small values ∆s and ∆Θ, ideally when the robot moves exactlyin direction given at the beginning of each update step.

1or more precisely, the pose relative to the robot’s starting point2The Saphira control system and the T1000 platform are good examples.3In fact, on Saphira system all sensor readings and a pose of the robot are updated every 100 ms (at least

accessible to Saphira user), so the current pose is updating internally. However, to simulate particle motion, therelative movement ∆s and the angle ∆Θ can be extracted directly from the current pose of the robot. Note that inthis case there will not be any integration errors, but only the “instantaneous” robot’s heading error which can beentirely removed (see the next chapter A.3).

24

Figure A.2: Realistic robot motion. The initial robot heading direction ∆Θ is usually differentfrom the resultant motion direction.

Figure A.2 depicts more realistic case of robot motion[7] - the initial robot’s heading directionis generally different from the resultant direction of motion. If we denote this difference by angleα, we can write new equations for extended robot motion:

xn+1 = xn + ∆s cos(Θn + α)yn+1 = yn + ∆s sin(Θn + α)Θn+1 = Θn + ∆Θ (A.3)

The only problem here is to find the angle α. Assuming that the angular velocity of the robot isconstant during update time period ∆t, we can write that:

α ≈ (Θn+1 −Θn)2

(A.4)

On the other hand, there are often known both the initial pose Pn and resultant pose Pn+1,and we only need to know a value of the angle α4. In this case the exact value of α can be obtaineddirectly from the equation A.4:

α = arctan(

∆y

∆x

)−Θn (A.5)

where ∆x = xn+1 − xn and ∆y = yn+1 − yn are relative robot’s displacement.

4to simulate exact particle motion, for instance

25

Appendix B

A few notions from statistics

B.1 Measurements and errors

Measurement is not only the source of information but also various errors. The process of measure-ment consists in comparison of measured value with a suitable measurement standard. Basically,the result of measurement gives two pieces of information: the unit (meter, second, kilogram, etc.)and the numerical value. Because so obtained value is never absolutely error-free, the result mustcontain additional information describing its accuracy. One can distinguish three types of errors:

1. Blunders or mistakes. This type of error can be caused by for example wrongly chosenalgorithm.

2. Systematic errors are a consequence of imperfect sensors or inaccurate measurement method.The calibration procedure usually eliminates most of these type of errors, but they still remainsubject to sensor drift1 and imprecision linearisation methods.

3. Random errors cannot be eliminated during calibration and they are effect of underlyingphysical processes like noise in electronic circuit, or unknown (not possible to model) inter-actions with the environment.

The errors which elimination is not possible are frequently called measurement uncertainties.

B.2 Random variables

It is advantageous to consider random error as random variable, what allows us to use powerfulstatistical methods. Continuous random variables can be described by probability density function(PDF) p(X) which must satisfy the following conditions:

p(x) ≥ 0, ∀x∫ +∞−∞ p(x)dx = 1 (B.1)

The n− th moment of random variable X is defined as:

Mn = E[Xn] =∫ +∞

−∞xnp(x)dx (B.2)

1which itself depends of environmental conditions

26

The 1− st moment of random variable X is called the expected or mean value

µ = E[X] =∫ +∞

−∞xp(x)dx (B.3)

The n− th moment about the mean is defined as:

Cn = E[(X − µ)n] =∫ +∞

−∞(x− µ)np(x)dx (B.4)

The second moment about the mean is called the variance of variable X:

σ2 = E[(X − µ)(X − µ)] = E[X2]− E[X]2 (B.5)

The standard deviation is the square root of the variance and is a measure of uncertainty orspread of random variable X around its mean value. Random variable X is said to be Gaussian ifits PDF function is defined as:

p(x) =1√2πσ

exp(− (x− µ)2

2σ2

)(B.6)

where µ and σ2 are the mean and the variance of X. If any measurement quantity is described byGaussian variable X, then 68% of all measured values of X will be within a standard deviation ofa mean.

If we denote by X1, X2, ..., Xn a set of n continuous random variables (see [6]):

X = [X1, X2, ..., Xn]T (B.7)

then we can define the joint probability density function p(x1, ..., xn), that similarly to the condi-tion B.1, must satisfy the following relations:

p(x1, ..., xn) ≥ 0, ∀x1, ..., xn∫ +∞−∞ ...

∫ +∞−∞ p(x1, ..., xn)dx1...dxn = 1 (B.8)

Random variables X1, X2, ..., Xn are said to be independent if the joint probability density functionp(X1, X2, ..., Xn) can be factorised:

p(X) = p(X1, X2, ..., Xn) = p(X1)p(X2)...p(Xn) (B.9)

Let µ denote the mean vector:

µi = E[Xi]µ = [µ1, µ2, ..., µn]T (B.10)

and define the covariance σ2ij of variables Xi and Xj as

σ2ij = E[(Xi − µi)(Xj − µj)] (B.11)

Note that σ2ij = σ2

ji. We can use these covariance values to form the covariance matrix :

Λ =

σ211 ... σ2

1n...

. . ....

σ2n1 ... σ2

nn

(B.12)

27

If random variables X1, X2, ..., Xn (equation B.9) are independent, then all off-diagonal elementsσ2

ij , i 6= j of the covariance matrix Λ are equal zero. The covariance matrix Λ of a vector randomvariable X has similar meaning to the variance (equation B.5) of a scalar random variable X, butadditionally the off-diagonal elements σ2

ij , i 6= j stand for correlations between variable componentsXi and Xj .

The multivariate Gaussian distribution is defined as:

p(x) =1√

(2π)n|Λ|exp

(−1

2((x− µ)T Λ−1(x− µ)

))(B.13)

It can be shown that if the components of the random variable X are independent, the multivariateGaussian distribution function (equation B.13) factorises according to the equation B.92.

It is easy to show that the variance of the sum of linear combinations of random variablesX1, X2, ..., Xn satisfies the equality:

E[(n∑

i=1

aiXi)(n∑

j=1

ajXj)] =n∑

i=1

a2i E[Xi] + 2

n∑i=1

n∑j=i+1

aiajσ2ij (B.14)

If the random variables X1, X2, ..., Xn are independent, the equation B.14 can be rewritten as:

E[(n∑

i=1

aiXi)(n∑

j=1

ajXj)] =n∑

i=1

a2i E[Xi] (B.15)

The above relation is the foundation of modelling of probabilistic particle motion, where eachvariable Xi represents the robot’s “quant” movement (see chapter 3.1).

B.3 Movement Errors

No matter how precise sensors we used to measure a travelled distance, the accumulated errorof the estimated position would reach any value. Fortunately, because the variance of the sumof independent random variables is simply the sum of its variances (equation B.15), an averageposition error is proportional to the square root of travelled distance. This is the case, however,only for ideally calibrated sensors - all systematic errors have fixed sign3, so they are proportionalto the total distance not to its square root.

There can be many sources of errors, and probably the most important are wheel slippage andnon-planar surfaces. In general, one can distinguish three types of possible errors connected withrobot movement[7]:

1. Rotation error. When robot turns an angle ∆Θ, the noise from odometry readings can bemodelled by the term kΘ∆Θ.

2. Translation error. When robot moves a distance ∆s, the noise can be modelled by the termkT ∆s.

3. Drift error is an effect of changing robot’s orientation during translation. It can be modelledby the term kD∆s.

2The assumption of independence of random variables is used, for example, in the motion model and the sensormodel (see chapters 3.1 and 3.2)

3otherwise they would not be systematic but random

28

All these values should be obtained experimentally by measuring standard deviations of translationand rotation separately. In the end, the variance of translation and rotation can be expressed by4:

σ2D = kT ∆s

σ2Θ = kΘ∆Θ + kD∆s (B.16)

4under the assumption of independence of all three types of errors

29

Appendix C

The software platform

C-1 Overview

One of the aims of this project was to be a creation a universal and easy in use frameworkfor development miscellaneous algorithms for “intelligent” robots. The Monte Carlo localisationis currently the most advanced supported algorithm. The system is based on a client-serverarchitecture, which separates the Java-written client from the OS-dependent server.

The server works under control of the Saphira program [9] [15] - a robot control system writtenby SRI International’s Artificial Intelligence Center. Saphira is in turn built on Aria [14] whichis a sophisticated low-level robot software developed at Activ-Media Robotics [1]. Aria directlycommunicates with a real robot or the “Pioneer” robot simulator [9].

C-1.1 The server architecture

The server is implemented as a dynamic library downloadable by Saphira’s “Colbert” environ-ment [8]. Colbert is a special sequencer language, situated in the second layer of the generalthree-layer robot control architecture (planning, execution, motion control; for details see [8]).Because the server simply collects and sends via TCP/IP protocol the data from sensors, it doesnot have to be touched unless the client requires any new special data format.

The main features of the server are:

� supports Windows and Linux OS1

� handles multi-client connections

� uses multi-threading and asynchronous sockets (see for example [10])

� supports both data sending and data receiving2 (bidirectional communication can be usedfor robot control applications).

C-1.2 The client architecture

The real core of the system is the “T1000” Java-written robot application platform. Java languagegreatly simplifies and accelerates developing new complex applications, to say nothing about soft-ware portability [4]. In general, the client can be written in any language and on any OS platform,

1I tested the server only on Microsoft Windows XP and Linux RedHat 9.0.2Data receiving feature needs some simple minor changes in the implementation of the server.

30

so one does need to know only the very basis of network programming and a simple format oftransferred data.

T1000 is only an application framework, and is naturally in its early stage of maturity. In thenearest future I expect to extend the system in the robotic mapping algorithms [11] and perhapsin the mixture MCL algorithm [13].

T1000 consists of four main functional blocks implemented as Java classes:

1. The MotionModel class corresponds to the motion model 3.1, and “knows” everything aboutrobot motion, in particular how to model the probabilistic motion of particles.

2. The SensorModel class corresponds to the sensor model 3.2, and “knows” everything aboutcurrent sensor readings, modelling of sensor noise, and the affine transformation of sensorreadings 3.5.

3. The WorldModel class corresponds to the world model 3.3, and “knows” everything aboutthe robot’s world, in particular how to retrieve from the map the incident angle and thedistance to obstacles 5.

4. The MonteCarlo is a class that “links” together the abovementioned classes, and supportsthe particle initialisation algorithm 2.4.1 and the SIR algorithm 2.4.1.

The most important helper classes are:

� The Client class implements the TCP/IP client.

� The Data class is a simple database containing the current robot pose and the history ofsensor readings. It is a kind of a “glue” linking the MotionModel, SensorModel and Clientclasses.

� The Tester is an abstract class which is a base class for all Java window applications.

� The T1000 is a class responsible for loading and storing the application properties C.2,displaying main application window, and handling keyboard and mouse events.

The T1000 platform is a set of Java applications which work in separate windows. All these windowapplications are classes inherited from Tester.

� The MotionTester class demonstrates various aspects of the probabilistic robot motion 3.1.

� The SensorTester class demonstrates the affine transform of sensor measurements 3.5.

� The DistributionTester class demonstrates the model of sensor noise 3.2.1.

� The WorldTester class demonstrates the features of the world model 3.3 - a map and cells(grid of the map).

� The MonteCarloTester class is the main application of the system and demonstrates theMCL algorithm.

The DistributionTester and the WorldTester classes are not TCP/IP clients, so they can berun independently on the server.

31

C-2 Installation and usage instructions

C-2.1 Files description

All program files are accessible in the author’s home directory ~msc35msk/work/Mini-project2/software.

Server.zip The server for Saphira (Windows and Linux). The file contains:

� the Colbert colbert/myStartup.act control file

� the Windows lib/server.dll and Linux lib/server.so dynamic libraries

� the full source code in server directory

T1000.zip The T1000 robot application platform. The file contains:

� directory structure T1000/* containing the T1000 source code (*.java and *.class)

� configuration files T1000/*.xml

� a few testing world maps T1000/worlds/*

� the Java JAR file T1000/T1000.jar duplicating most of the above files

Readme.txt A short overview and installation instructions.

C-2.2 General requirements

To run the program one must have:

� Windows 2000/XP or Linux RedHat 9.0

� Saphira software version 8.3-2 [1]

� Java runtime environment 1.4 or newer (e.g. http://java.sun.com/j2se/1.5.0/download.jsp)

For further development is necessary to install:

� Microsoft Visual C++ 6.0 (Saphira 8.x does not work with .NET platform) or gcc-2.95.3 forLinux (Saphira 8.x does not work with any newer versions of GNU compiler)

� Java software development kit 1.4 or newer (e.g. http://java.sun.com/j2se/1.5.0/download.jsp)

C-2.3 The server

Installation instructions

� Go to Saphira main directory.

� Save your colbert/myStartup.act file (will be overwritten).

� Unzip Server.zip into Saphira main directory.

� Restart Saphira.

Usage

By default server starts automatically after each (re)start of Saphira. To stop manually the servertype “Stop()” command, to start again - “Start()” command.

32

Settings

The default server port is 7070 and can be changed (as well as a few other things) by edit-ing parameters of the Init() function in the colbert/myStartup.act file: Init(portNumber,maxNumberOfConnections, maxConnectionTime). The maximum number of connections andmaximum connection time currently works only in Windows environment - there are no limitsfor the Linux server version.

Problems

At the moment, unfortunately, it is necessary to shut down the server manually by issuing “Stop()”command3, as I have not worked out yet how to intercept the signal that the main Saphira windowis being closed.

C-2.4 The client

Installation instructions

� Unzip T1000.zip in your favourite place ;-)

� Enjoy!

Usage

Before running any client program it is necessary:

� to connect a robot or start the Pioneer simulator

� for the Pioneer simulator to load a world map from the Pioneer menu: Files → Load World

� to start Saphira (server should start as well displaying a message: ”Server is listening on port7070”)

� to connect Saphira to the robot or Pioneer from the main menu: Connect → Local

� to open the Saphira activity window: View → Activity Window

� to double click on ”Follow” or ”Wander” activity

As mentioned before (C-1.2), the T1000 platform is designed as a set of separate Java applications(table C.1). The main Java window application that uses Monte Carlo localisation algorithm canbe run by writing in the OS command line:

java -jar T1000.jar

or directly loading the class file from the T1000/system subdirectory:

java system.MonteCarlo$Tester

Do not forget that in Unix ’$’ is a special character, so:

java system.MonteCarlo\$Tester

The rest of applications can be run in a similar way to the main one, as shown in the table C.1.Moreover, all Java programs should be executed from the client main directory T1000/.

Each application reacts to a few hot keys: to restart or resample - press ’r’, to pause the program- ’p’, to show particle motion - ’m’, to show world cells - ’c’ key.

3otherwise TCP/IP server keep working as if nothing has happened, and as a result Saphira crashes immediately

33

Application Command Configuration fileMonte Carlo Localisationalgorithm

java system.MonteCarlo$Tester MonteCarloTester.xml

Motion model visualisa-tion

java system.Motion$Tester MotionTester.xml

Sensor model visualisation(sensor readings)

java system.Sensor$Tester SensorTester.xml

Sensor model visualisation(noise model)

java system.Sensor$DistributionTester DistributionTester.xml

World model visualisation java system.MonteCarlo$Tester WorldTester.xml

Table C.1: The T1000 applications: execution commands and configuration files.

Name Descriptionclient.address server addressclient.port server port numberworld.file current world mapparticle.number number of particlestester.refresh minimum refresh (recalculation) time rate between iterations of

the recursive equation 2.11 in millisecondssensor.readings number of sensor readings used by sensor model for evaluation of

particle weights (see 3.2)motion.translation.deviation motion translation deviation per meter (see 3.1)motion.drift.deviation motion drift per meter (see 3.1)motion.rotation.deviation motion rotation deviation per radian (see 3.1)sensor.range sensor range in meterssensor.deviation sensor deviation in meters

Table C.2: The most important properties of the T1000 applications.

Settings

Because of the early stage of the project, GUI does not support yet application settings. Thuseach application has only its own XML configuration file4 in the main client directory. The defaultconfiguration file names are shown on the table C.1.

The default configuration files can be also used as templates for creation custom ones. To usea custom file e.g. xxx.xml write simply:

java -jar T1000.jar xxx.xml

or

java system.MonteCarlo$Tester xxx.xml

Because most of underlying classes are the same for all the applications (see C-1.2), the applicationproperties stored in the configuration files have the same name and meaning. The most importantproperties are collected in the table C.2.

4in fact, the XML properties are available only in Java 1.5

34

Appendix D

Mini-project declaration

The University of BirminghamSchool of Computer Science

MSc in Advanced Computer Science

Second semester mini-project

This form is to be used to declare your choice of mini-project in the second semester of the course.Please complete this form, obtain the signature of your supervisor and post it in the appropriateassessed work pigeon hole.

Deadline: 16.00 hrs, 22nd January 2004

Name: Marek Kopicki

Student number: 0564641

Mini-project title: Probabilistic approaches for mobile robots localisation and map-building.

Mini-project supervisor: Dr Jeremy Wyatt

The following questions should be answered in conjunction with a reading of the course book.

Aim of mini-project Robotic implementation of Monte Carlo Localisation techniques,extension comparison to other methods, possible integration withmap-building.

35

Objectives to be achieved

1. Revise and deepen knowledge from stochastic processes andAI areas like time dependent probabilistic reasoning.

2. Study and implement various Monte Carlo techniques forrobots localisation.

3. Study and possibly use probabilistic techniques for map-building.

Project management skills

Briefly explain how youwill devise a managementplan to allow your su-pervisor to evaluate yourprogress

1. Familiarise with particle filtering technique and a robot plat-form.

2. Propose and write a general framework of an algorithm em-ploying mentioned MC techniques.

3. Test the algorithm in various configurations.

4. Implement map-building algorithms, if there will be time.

5. During the semester discuss key issues and emerging prob-lems as they come.

Systematic literature skills

Briefly explain how youwill find previous relevantwork

My main source of knowledge will be journals, various technicalreports, books and also Internet.I will use keywords like mobile robots, localization, Bayes filters,particle filters, robotic mapping, Kalman filters etc. and sugges-tion from supervisor.

Communication skills

What communicationskills will you practiseduring this mini-project?

I will practise all types of communication skills, especially writingone.

Signed (student) Marek Kopicki

Date:

Signed (supervisor) Jeremy Wyatt

Date:

36

Appendix E

Statement of information searchstrategy

1 Parameters of literature search

My supervisor Dr Jeremy Wyatt provided me with the most important papers related to the MCLalgorithm. Later, I extended the search area to various problems connected with robot motionand sensor models. During writing the program, I focused mainly on programming issues: theAria/Saphira system architecture, Colbert language, concurrency and inter-process communicationunder Windows and Linux, BSD asynchronous sockets, etc.

1.1 Forms of literature

The categories explored maximally were (in order of usage, not relevance): a range of manuals,journal articles, web pages, books, technical reports, conference papers, theses books.

1.2 Geographical and language coverage

A vast majority of work comes from the North America and Europe and is written in English. Ialso found very helpful a few books translated into Polish. Beside my native language I am ableto read papers written in French and Russian.

1.3 Age-Range of literature

The search for localisation algorithms was carried out mostly from 1998 onwards, whereas forposition estimation techniques from 1990.

2 Appropriate search tools

2.1 Engineering Index

The Compendex http://www.engineeringvillage.org is a huge interdisciplinary engineering databasewith 7.5 million records referencing 5,000 engineering journals and conference materials. Abstractsare available for most publications.

37

2.2 Science Citation Index (SCI)

I used this for finding journal articles and technical reports. Cited reference search was alsoperformed using SCI. In SCI keywords can be used to find some papers to start with, furthercited reference search can be used. The CiteSeer http://citeseer.nj.nec.com/cs is the largest andfree public citation index of scientific literature. Additionally, the ISI Web of Knowledge Servicehttp://wos.mimas.ac.uk can be used as a support.

2.3 Dissertations Abstracts International (DAI) and Index to Theses

DAI http://wwwlib.umi.com/dissertations/gateway can be used to retrieve North American theses.The Index to Theses http://www.theses.com can be used to retrieve theses accepted for higherdegrees by universities in Great Britain and Ireland.

3 Search statements

The search statements were based on following groups of keywords: localization, Monte CarloLocalisation, particle filters, position estimation, robot navigation, motion model, sensor model.Additionally, the above keywords were combined together with AND and OR boolean operators,and also with usage of wildcard characters (depending on a search engine).

4 Brief evaluation of the search

Working on the program, using various keywords for searching in Science Citation Index andGoogle, I retrieved the following items (including subsequent cited reference search).

� tens of web pages

� 10 journal articles

� 9 conference papers

� 2 technical reports

� 2 books

During writing the mini-project report I finally decided to make use of the following relevantsources.

� about 10 web pages

� 4 journal articles

� 3 conference papers

� 1 book

38

Bibliography

[1] Active-Media Robotics. http://robots.activmedia.com.

[2] P. Billingsley. Probability and Measure. Wiley, 1986.

[3] F. Dellaert, D. Fox, W. Burgard, and S. Thrun. Monte carlo localization for mobile robots.

[4] B. Eckel. Thinking in Java. Prentice-Hall, 3rd edition, 2002.

[5] D. Fox, W. Burgard, and S. Thrun. Markov localization for mobile robots in dynamic envi-ronments. Journal of Artificial Intelligence Research, 11:391–427, 1999.

[6] H. Hsu. Random Variables and Random Processes. Schaum, 1996.

[7] K. Konolige. Robot motion: Probabilistic model; sampling and gaussian implementations;markov localization. Technical report, SRI International, 2001.

[8] K. G. Konolige. COLBERT: A Language for Reactive Control in Saphira. SRI International,2001.

[9] K. G. Konolige. Saphira Software Manual. SRI International, 2001.

[10] M. Mitchell, J. Oldham, and A. Samuel. Advanced Linux Programming. New Riders, 1stedition, 2001.

[11] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. Fastslam: A factored solution to thesimultaneous localization and mapping problem, 2002.

[12] A. W. Moore. An intoductory tutorial on kd-trees. Technical Report No. 209, University ofCambridge, Computer Laboratory, 1991.

[13] S. Thrun, D. Fox, W. Burgard, and F. Dellaert. Robust monte carlo localization for mobilerobots. Artificial Intelligence, 128(1-2):99–141, 2001.

[14] D. van Heesch. Aria Reference Manual 1.3.2. Activ-Media Robotics, 2001.

[15] D. van Heesch. Saphira Reference Manual 8.3.2. Activ-Media Robotics, 2001.

39