Tutorial and exercises on mobile robots localization

Emanuele Frontoni Fabio Caponetti Adriano Mancini DIIGA - Universit Politecnica delle Marche

Outline
Software architectures for mobile robots ActivMedia Robots and Aria library Aria Matlab Gateway (Mex functions) Exercise : obstacle avoidance Tutorial : sonar based MCL simulations From the simulator to the real robot Tests with a real robot

Software architectures for mobile robot

GOALS We need to easy reuse solutions and algorithms implemented for different tasks in robotics We want to use Matlab for high level programming (reasons ?) We need to use C/C++ SDK for robot control and time consuming / multithread process We want to easily switch between a robot simulator and a real robot We want to use the same platform also for multirobot

Software architectures for mobile robot

SOLUTION
Matlab / High level algorithms

Mex functions / Gateway

C C++ SDK / Low level control algorithms

Software architectures for mobile robot

OPTIMIZATION
Matlab / High level algorithms
C C++ algorithms / Time consuming process

Mex functions / Gateway

C C++ SDK / Low level control algorithms

MEX-Files

You can call your own C/C++ or Fortran subroutines from MATLAB as if they were built-in functions. MATLAB callable C and Fortran programs are referred to as MEX-files. MEX-files are dynamically linked subroutines that the MATLAB interpreter can automatically load and execute. MEX-files have several applications:

Large pre-existing C/C++ and Fortran programs can be called from MATLAB without having to be rewritten as M-files. Bottleneck computations (usually for-loops) that do not run fast enough in MATLAB can be recoded in C/C++ or Fortran for efficiency.

MEX-files are not appropriate for all applications. MATLAB is a highproductivity system whose specialty is eliminating time-consuming, low-level programming in compiled languages like Fortran or C/C++. In general, most programming should be done in MATLAB. Don't use the MEX facility unless your application requires it.

Work environment

In every MEX function we need the following special function to interface Matlab and C/C++ code

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { }

Using these parameters - nlhs,*plhs,nrhs,*nrhs - Matlab and C/C++ interact and exchange data To exchange data coherently we can use different special functions for type casting provided by MATLAB (es. mxGetChar,mxGetString,)

The AriaMatlab environment

There is unfortunately no standardized way to interface with robots. Each manufacturer provide software developers kits (SDKs) for their own products (I.E. Khepera robots are controlled from Matlab and AmigoBot robots from C++).

Implementation of an adapter layer between the ARIA library and Matlab

ARIA library

ARIA is an open source object oriented interface to ActivMedia mobile robots. The interface is implemented as a cross platform C++ library. Besides a lot of robot related functionality the library also contains wrappers around platform specific functions such as threading and networking.

SUPPORTED ROBOTS

AmigoBotTM - Classroom and team robot. Pioneer 3-AT - High performance all-terrain robot. Pioneer 3-DX - Research and educational robot. PatrolBotTM - A surveillance robot. PowerBotTM - High-agility, high-payload robot. PeopleBotTM - Human interface robot.

Available functions
Some useful functions: r = robot (address) move (r, distance) setheading (r, heading) ismovedone (r) isheadingdone (r) data = readsonar (r) [1..16] array (Pioneer has 8 sonars !)

Example 1
r = robot('127.0.0.1'); for i=1:10 % Move 0.5 meters forward move(r, 500); % Wait until the robot is done while ~ismovedone(r) end % Rotate 180 degrees clock-wise setdeltaheading(r, 180); % Wait until the have rotated 180 degrees while ~isheadingdone(r) end end disconnect(r);

The SRIsim simulator

ActiveMedia provides 2 different simulators: SRIsim and MobileSim

Odometry Sonars errors Load worlds Load robot settings

Exercise Setup
Download the ZIP file from http://psfmr.univpm.it/loc.zip Unzip the file in a folder Example

Exercise 1 : obstacle avoidance

GOAL : using AriaMatlab environment Write a simple obstacle avoidance algorithm and test it in the simulator The best O.A. algorithm will be selected and tested on the real robot

Exercise 2: problem formulation

Test a sonar based Monte Carlo Localization process for a Pioneer Mobile Robot Suggest us tuned parameters for the test

Particle filter localization

Inizialitation

State space:
xk = [ xk , yk , k ]

While exploring Update

at time k we have a set of particles nParticles

{S

j k

j j = xk , wk : j = 1...nParticles

where w k is the weight o the particle j

Resampling Pose evalutation

Weights generation
% kj = f ( xkj , zk ) , j = 1...nParticles w
RealSonarMeasurek - SimulatedSonarMeasurekj , j = 1...nParticles Innov = RealSonarMeasurek
j k

difference = 1 Innov(i) , j = 1...nParticles nSensors j - difference w %k =e +0.001 , j = 1...nParticles

nSensors i =1 j k

j k

i wk =

i %k w nParticles

j =1

, i = 1...nParticles % kj w

Motion model

Every movement is divided in a rotation and a shift In this way we can consider 2 different odometry models for simulations

Sensor model

The only sensor used in this exercise is the sonar. Sonars have several drawbacks:

Multiple reflections Eco distortion (different materials) Edge obstacle

Sensor model

Modeled obstacles are Walls Corners

How we find obstacles For j=1 to Num_particles For i=1 to Num_Sonar Dist_corner= Minimum distance from corners Dist_wall= Minimum distance from walls Dist=min(Dist_corner,Dist_wall) End End

MCL simulations

From a multimodal distribution of possible robot poses we want to have a unimodal distribution LOCALIZATION Some possible questions :

How many particles we need ? Who to generate new particles ? Who to dial with a kidnapped robot after a correct localization ?

Results on Robot Pioneer3

2.5

Map of the environment

x 10
4

Mappa DIIGA

2 Colonna

1.5 Y (mm) 1 Colonna Quadro elettrico 0.5 Ingresso Colonna 0 0 0.2 0.4 0.6 Ingresso Lab.IMAD Ingresso Lab. Robotica

0.8

1 1.2 X (mm)

1.4

1.6

1.8

2 x 10
4

Results on Robot Pioneer3

2.5 x 10
4

Localizzazione entro DIIGA

1.5 Y (mm)

Traiettoria Robot

0.5 Posizione stimata

0 0

1000

2000

3000

4000

5000 6000 X (mm)

7000

8000

9000 10000

Results on Robot Pioneer3

Localization error (mm)
Errore di localizzazione 10000 Media dell'errore dopo l'aggancio della posizione Errore di localizzazione Media meno deviazione standard Media pi deviazione standard

9000

8000

Errore in valore assoluto (mm)

7000

6000

5000

4000

3000

2000

1000

10

20

30 40 Passi dell'algoritmo di localizzazione

50

60

70

Results on Robot Pioneer3

Total number of used particles
Numero di particelle impiegate nel processo di localizzazione 300 Numero di particelle

Numero di particelle impiegate

250

200

150

10

20 30 40 50 Passi dell'algoritmo di localizzazione

60

70

Using a vision system

vidobj = videoinput('winvideo', 1); % Open the preview window. preview(vidobj) snapshot = getsnapshot(vidobj); % Display the frame in a figure window. image(snapshot); % Extract featurs from images ... ...

Tutorial and exercises on mobile robots localization

Emanuele Frontoni Fabio Caponetti Adriano Mancini DIIGA - Universit Politecnica delle Marche

Outline

Study the implementation Analyze results on the real robot Try to modify something in the implementation of the MCL using the simulator to evaluate it The best improvement will be selected and tested on the real robot

Motion model

La rotazione del robot a partire da un orientamento k per un angolo pu essere descritta dalla seguente relazione:
k +1 = + k + N ( M rot , rot )
M rot : valor medio errore di orientamento

rot : varianza dell ' errore di orientamento

Resampling techniques

Passo 1: Ricampionamento in funzione dei pesi x k + 1 = SelectW ithReplacem ent ( x k ) Passo 2: Ridimensionamento della popolazione
xP = x , y , : varianza della distribuzione di particelle
Ip =
2 x2 + y T

xMap 2 + yMap 2

dove : xMap e yMap rappresentano le dimensioni del rettangolo pi piccolo che contiene la mappa nParticles = int (nParticles e( Ip ) ) , : smorzamento xk +1 = ResizeParticlePopulation( xk +1 , nParticles )

Passo 3: Controllo della posizione delle particelle

xk +1 = CheckParticles ( xk +1 , Map )

Lalgoritmo Monte Carlo

Lambiente di lavoro

ActivMedia Robotics Interface for Application ovvero ARIA, rappresenta un software object oriented per la gestione di robot come il Pioneer3 o lAmigoBot. Tale software scritto in C++ permette di interagire in modo completo, funzionale e snello con un robot.

Presentazione e codifica dei modelli Modello del movimento

Durante la traslazione lorientamento del robot pu subire leggere variazioni facendo s che per lunghe distanze la posizione raggiunta si distingua sensibilmente nei confronti di quella attesa. Discretizzando la traslazione in sottopassi possibile modellare facilmente lazione combinata degli errori di deriva e di traslazione.

: distanza da percorrere nSteps : numero di passi M trs : valor medio errore di traslazione M drf : valor medio errore di drift

trs : varianza dell'errore di traslazione drf : varianza dell'errore di drift =

nSteps for k = 1: nSteps Etrs = N ( M trs , trs ) Edrf = N ( M drf , drf )

= + Edrf
x = x + ( + Etrs ) cos ( ) y = y + ( + Edrf ) sen( )

= + Edrf
end

Presentazione e codifica dei modelli Modello sensoriale

Aspetto cruciale del metodo Monte Carlo per la localizzazione di robot mobili la definizione del modello i sensoriale. p ( zk | xk ) Nellalgoritmo MCL richiesta la conoscenza di per poter procedere alla localizzazione Per effettuare la localizzazione si hanno a disposizione 8 sensori ad ultrasuoni

Resampling techniques

Passo 1: Ricampionamento in funzione dei pesi x k + 1 = SelectW ithReplacem ent ( x k ) Passo 2: Ridimensionamento della popolazione
xP = x , y , : varianza della distribuzione di particelle
Ip =
2 x2 + y T

xMap 2 + yMap 2

dove : xMap e yMap rappresentano le dimensioni del rettangolo pi piccolo che contiene la mappa nParticles = int (nParticles e( Ip ) ) , : smorzamento xk +1 = ResizeParticlePopulation( xk +1 , nParticles )

Passo 3: Controllo della posizione delle particelle

xk +1 = CheckParticles ( xk +1 , Map )