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’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+ +=