Tutorial and exercises on mobile robots localization Emanuele FrontoniFabio CaponettiAdriano ManciniDIIGA - Università Politecnica delle Marche

Outline

Software architectures for mobile robots ActivMedia Robots and Aria libraryAria Matlab Gateway (Mex functions)Exercise : obstacle avoidanceTutorial : sonar based MCL simulationsFrom the simulator to the real robot Tests with a real robot

Software architectures for mobile robot

GOALSWe need to easy reuse solutions and algorithms implemented for different tasks in roboticsWe want to use Matlab for high level programming (reasons ?)We need to use C/C++ SDK for robot control and time consuming / multithread processWe want to easily switch between a robot simulator and a real robotWe 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

Mex functions / Gateway

C C++ SDK / Low level control algorithms

C C++ algorithms / Time consuming process

MEX-FilesYou 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 high-productivity 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 environmentIn every MEX function we need the following special function to interface Matlab and C/C++ code

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

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

{ … }

The AriaMatlab environmentThere 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 layerbetween the ARIA library and Matlab

ARIA libraryARIA 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 functionsSome 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 1r = robot('127.0.0.1');

for i=1:10% Move 0.5 meters forwardmove(r, 500);% Wait until the robot is donewhile ~ismovedone(r)end% Rotate 180 degrees clock-wisesetdeltaheading(r, 180);% Wait until the have rotated 180 degreeswhile ~isheadingdone(r)end

enddisconnect(r);

The SRIsim simulator

ActiveMedia provides 2 different simulators: SRIsim and MobileSim

OdometrySonars errorsLoad worldsLoad robot settings

Exercise Setup

Download the ZIP file fromhttp://psfmr.univpm.it/loc.zipUnzip the file in a folderExample

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 localizationState space:

at time k we have a set of particles nParticles

where is the weight o the particle j

[ ], ,k k k kx x y θ=

{ }, : 1... j j jk k kS x w j nParticles⎡ ⎤= =⎣ ⎦

jkw

Inizialitation

Update

Resampling

Pose evalutation

While exploring

Weights generation( , ) , 1...j j

k k kw f x z j nParticles= =%

, 1...j

j k kk

k

RealSonarMeasure - SimulatedSonarMeasureInnov j nParticlesRealSonarMeasure

= =

1

1 ( ) , 1...nSensors

jk

idifference Innov i j nParticles

nSensors =

= ⋅ =∑

, 1...jk-differencej

kw =e +0.001 j nParticles=%

1

, 1...i

i kk nParticles

jk

j

ww i nParticlesw

=

= =

∑%

%

⎧⎪⎨⎪⎩

Motion model

Every movement is divided in a rotation and a shiftIn 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 reflectionsEco distortion (different materials)Edge obstacle…

Modeled obstacles are WallsCorners

How we find obstacles

For j=1 to Num_particles

For i=1 to Num_SonarDist_corner= Minimum distance from cornersDist_wall= Minimum distance from walls

Dist=min(Dist_corner,Dist_wall)End

End

Sensor model

MCL simulations

From a multimodal distribution of possible robot poses we want to have a unimodal distribution -LOCALIZATIONSome 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 Pioneer3Map of the environment

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

x 104

0

0.5

1

1.5

2

2.5x 104

X (mm)

Y (m

m)

Mappa DIIGA

Ingresso Lab.IMAD

Ingresso Lab. Robotica

Colonna

Quadro elettrico

Ingresso

Colonna

Colonna

0 1000 2000 3000 4000 5000 6000 7000 8000 9000 100000

0.5

1

1.5

2

2.5x 104

X (mm)

Y (m

m)

Localizzazione entro DIIGA

Traiettoria Robot

Posizione stimata

Results on Robot Pioneer3

Results on Robot Pioneer3

0 10 20 30 40 50 60 700

1000

2000

3000

4000

5000

6000

7000

8000

9000

10000

Passi dell'algoritmo di localizzazione

Erro

re in

val

ore

asso

luto

(mm

)

Errore di localizzazione

Media dell'errore dopo l'aggancio della posizioneErrore di localizzazioneMedia meno deviazione standardMedia più deviazione standard

Localization error (mm)

0 10 20 30 40 50 60 70150

200

250

300

Passi dell'algoritmo di localizzazione

Num

ero

di p

artic

elle

impi

egat

eNumero di particelle impiegate nel processo di localizzazione

Numero di particelle

Results on Robot Pioneer3Total number of used particles

Using a vision systemvidobj = 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 FrontoniFabio CaponettiAdriano ManciniDIIGA - Università Politecnica delle Marche

Outline

Study the implementationAnalyze results on the real robotTry to modify something in the implementation of the MCL using the simulator to evaluate itThe “best” improvement will be selected and tested on the real robot

Motion model

La rotazione del robot a partire da un orientamento per un angolo può essere descritta dalla seguente relazione:

1 ( , ):

: '

k k rot rot

rot

rot

N MM valor medioerroredi orientamento

varianza dell erroredi orientamento

θ δθ θ σ δθ

σ

+ = + + ⋅

kθ δθ

Resampling techniquesPasso 1: Ricampionamento in funzione dei pesi

Passo 2: Ridimensionamento della popolazione

Passo 3: Controllo della posizione delle particelle

( )1k kx SelectWithReplacement x+ =

2 2

2 2

( )

, , :

:

( ) , :

T

xP x y

x y

Ip

varianza della distribuzionedi particelle

IpxMap yMap

dovexMape yMap rappresentanoledimensioni del rettangolo più piccolochecontienela mappanParticles int nParticles e smorz

θ

λ

σ σ σ σ

σ σ

λ− ⋅

⎡ ⎤= ⎣ ⎦

+=

+

= ⋅

1 1( , )k k

amentox ResizeParticlePopulation x nParticles+ +=

( )1 1,k kx CheckParticles x Map+ +=

L’algoritmo Monte Carlo

L’ambiente di lavoro

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

::

::

::

1:

trs

drf

trs

drf

trs

distanza da percorrerenSteps numero di passiM valor medioerroredi traslazioneM valor medioerroredi drift

varianza dell'erroredi traslazionevarianza dell'erroredi drift

nStepsfor k nSteps

E

ρ

σσ

ρδρ =

== ( , )

( , )

( ) ( )( ) ( )

trs trs

drf drf drf

drf

trs

drf

drf

N ME N M

E

x x E cosy y E sen

E

end

δρ σ δρδρ σ δρ

θ θ

δρ θδρ θ

θ θ

⋅ ⋅= ⋅ ⋅

= +

= + + ⋅

= + + ⋅

= +

Presentazione e codifica dei modelli – Modello del movimento

Durante la traslazione l’orientamento 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 l’azione combinata degli errori di deriva e di traslazione.

Presentazione e codifica dei modelli – Modello sensoriale

Aspetto cruciale del metodo Monte Carlo per la localizzazione di robot mobili è la definizione del modello sensoriale.Nell’algoritmo MCL è richiesta la conoscenza di per poter procedere alla localizzazionePer effettuare la localizzazione si hanno a disposizione 8 sensori ad ultrasuoni

( | )ik kp z x

Resampling techniquesPasso 1: Ricampionamento in funzione dei pesi

Passo 2: Ridimensionamento della popolazione

Passo 3: Controllo della posizione delle particelle

( )1k kx SelectWithReplacement x+ =

2 2

2 2

( )

, , :

:

( ) , :

T

xP x y

x y

Ip

varianza della distribuzionedi particelle

IpxMap yMap

dovexMape yMap rappresentanoledimensioni del rettangolo più piccolochecontienela mappanParticles int nParticles e smorz

θ

λ

σ σ σ σ

σ σ

λ− ⋅

⎡ ⎤= ⎣ ⎦

+=

+

= ⋅

1 1( , )k k

amentox ResizeParticlePopulation x nParticles+ +=

( )1 1,k kx CheckParticles x Map+ +=