positioning and control of mobile robots_miamo_ekf

8/3/2019 Positioning and Control of Mobile Robots_miamo_ekf

1/43

Laboratoire dAutomatique

Jean Martial Miamo Nguekam Dr. Denis GilletEtudiant en 4ime anne Electricit Levente Bodizs

EPFL - LA

Projet de Semestre / Et

Anne acadmique 2004 / 2005

POSITIONING AND CONTROL OF

MOBILE ROBOTS


2/43

2

TABLE DES MATIERES

1 INTRODUCTION.............................................................................................4

2 MODLISATION DU ROBOT KHEPERA...................................................5

3 LE SYSTME DE MESURE ............................................................................7

4 LE TRAITEMENT DIMAGE ..........................................................................8

4 1 Matrice de pixels couleur et matrice de pixels noirs et blancs............................. 8

4 2 Dtermination du centre de gravit des deux robots ................................................ 8

4 3 Sparation de limage ........................................................... ........................................................ 9

4 4 Calcul des positions de chaque robot dans le repre associ chaqueimage......................................................... ........................................................... ............................................. 10

4 4 1 Cas de la coupe verticale .......................................................... ................................... 104 4 2 Cas de la coupe horizontale .................................................... ................................... 11

4 5 Test du programme de traitement dimage .................................................... ............... 12

5 LE FILTRE DE KALMAN ETENDU ............................................................13

5 1 La prdiction........................................................... ........................................................... ............... 13

5 2 La correction des mesures .......................................................... ............................................. 15

5 3 Simulation du filtre de Kalman sous Matlab et Labview ........................................ 16

5 4 Implantation du filtre de Kalman en temps rel................................................ ......... 17

6 LA COMMANDE PRDICTIVE ...................................................................19

6 1 Schma de commande .......................................................... ...................................................... 20

6 2 Gnration de la trajectoire de rfrence........................................................ ............... 21

6 3 Les paramtres de la commande prdictive implante .......................................... 22

7 TESTS EXPRIMENTAUX...........................................................................23

7 1 Test de convergence du filtre de Kalman Etendu ............................................... ........ 237 1 1 Exprience 1 ........................................................ ........................................................... ..... 237 1 2 Exprience 2 ........................................................ ........................................................... ..... 24

7 2 Effet du filtre de Kalman suite aux perturbations...................................................... 257 2 1 Exprience 1 : 4 perturbations ........................................................ ......................... 257 2 2 Exprience 2 : 3 perturbations ........................................................ ......................... 26

7 3 Trajectoire du matre et de lesclave ........................................................ ......................... 287 3 1 Matre immobile........................................................... ...................................................... 28


3/43

3

7 3 1 1 Exprience 1 ........................................................................... 287 3 1 2 Exprience 2 ........................................................................... 29

7 3 2 Matre mobile ...................................................... ........................................................... ..... 307 3 2 1 Exprience 1 ........................................................................... 307 3 2 2 Exprience 2 ........................................................................... 32

7 3 2 3 Exprience 3 ........................................................................... 33

8 CONCLUSION ................................................................................................34

ANNEXE A ..............................................................................................................35

ANNEXE B ..............................................................................................................37


4/43

4

1 Introduction

Le robot Khepera est un vhicule autonome muni de deux roues qui a t

dvelopp par le Laboratoire des systmes autonomes de lEPFL. Il a fait lobjet

de plusieurs projets mens au sein du Laboratoire dautomatique de lEPFL.

Contrairement aux tudes prcdentes, notre tude porte sur deux robots.

Lobjectif tait dimplmenter en temps rel sur un Robot esclave ou

prdateur une technique de contrle commande qui lui permet de rattraper un

deuxime robot matre qui se dplace selon une trajectoire alatoire sur une

plate-forme.

Le systme de mesure que nous utilisons est celui propos par Herrera [1]. Il

sagit principalement dune camera digitale. Limage quelle fournit est traite

par un programme de traitement dimage dvelopp sous LabView. Le rsultat

dudit traitement est lobtention en temps rel de la position du robot mobile sur

la plate-forme. Les positions mesures sont entaches derreurs. Cest la raison

pour laquelle nous avons choisi de les filtrer avant de les utiliser dans

lalgorithme de commande : pour cela, le filtre de Kalman est utilis. Les raisonsde ce choix seront exposes dans les paragraphes suivants.

Pour commander le robot esclave , la commande Prdictive a t utilise.

Dans la suite, nous consacrerons un bref paragraphe la modlisation du robot

Khepera. Le systme de mesure et le programme de traitement dimage pour

deux robots mobiles feront lobjet dun autre paragraphe dtaill. Ensuite, il sera

question du Filtre de Kalman Etendu puis limplmentation de la commande

prdictive.


5/43

5

2 Modlisation du robot Khepera

La modlisation dun systme dynamique est une tape trs importante pour la

conception dune commande telle que la commande prdictive. La qualit dun

rgulateur dpend fortement de celle du modle du systme dynamique quon

veut commander. Herrera [1] et Niederberger [2] ont propos deux modles pour

le robot Khepera. Seul celui bas sur une trajectoire rectiligne et qui considre le

robot comme un point a t retenu. Il est causal et non linaire :

( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )( )kVkVL

hkk

kkVkVh

kyky

kkVkVh

kxkx

lr

lr

lr

+=+

++=+

++=+

1

sin

2

1

cos2

1

Ce set dquations montre quil sagit dun systme multivariable qui possde

deux entres ( )kVr et ( )kVl qui sont respectivement les signaux qui permettent

dalimenter les roues droite et gauche du robot. Il ya trois sorties qui sont ( )kx ,

( )ky , ( )k qui sont respectivement labscisse, lordonne et la position angulaire

du robot dans le repre dfinit comme sur la figure ci-dessous :


6/43

6

h est la priode dchantillonnage et L est la distance entre les deux roues du

robots. Le modle dtat du robot est donn ci-dessous :

Le programme et les rsultats de simulation dans Matlab et Labview du modle

se trouvent en annexe A. Le programme Labview qui permet de simuler le

modle doit tre utilis avec la version 6.1 de Matlab. Avant de lexcuter, il

faut au pralable ouvrir Matlab 6.1


7/43

7

3 Le systme de mesure

1 : camera digitale

2 : plate-forme

3 : robot Khepera

Sur la plate-forme de couleur noire, on peut voir deux robots : lesclave et le

matre. Vu de dessus, le matre est de couleur blanche alors que lesclave lest

aussi mais avec deux cercles remplis en noire qui permettent au programme de

traitement dimage de calculer sa position angulaire. La camera digitale fournit

une image en couleur qui sera ensuite traite par le programme de traitement

dimage afin de dterminer les trois tats de chaque robot.

1

23


8/43

8

4 Le Traitement dimage

Nous nous sommes largement inspirs du programme de traitement dimage

dvelopp par Herrera [1] qui permet de calculer les trois tats x, y, dun seul

robot. Le programme que nous avons dvelopp tient compte de la prsence de

deux robots sur la plate forme.

La procdure de traitement est divise en plusieurs tapes.

4 1 Matrice de pixels couleur et matrice de pixels noirs et blancs

Tout dabord, limage en couleur acquise est transforme en une matrice de

pixels dont chacun est cod sur 24 bits. Cette matrice est ensuite transforme en

une autre matrice toujours code sur 24 bits mais dont les couleurs sont noires

ou blanches. Cette matrice donne une image qui a lapparence suivante :

4 2 Dtermination du centre de gravit des deux robots

Dans loptique de diviser limage ci-dessus en deux images distinctes, labscisse

et lordonne du centre de gravit des deux robots sont calcules.

Robot esclave Robot matre


9/43

9

4 3 Sparation de limage

Selon le rsultat dun critre qui porte sur la position dun robot par rapport

lautre, une coupe verticale ou horizontale de limage est effectue.

Si les deux robots ont par exemple la position suivante, alors une coupe verticale

est faite :

Une coupe horizontale est faite dans le cas suivant :


10/43

10

4 4 Calcul des positions de chaque robot dans le repre associ chaque

image

Aprs avoir effectu les coupes, chaque robot se trouve dans un systme daxe

dans lequel on dterminera sa position. Pour le robot esclave , les trois tats

seront calculs alors seuls labscisse et lordonne du robot matre seront

estimes.

4 4 1 Cas de la coupe verticale

Calcul de 111 ,, yx


11/43

11

Calcul de 22 , yx

4 4 2 Cas de la coupe horizontale

Calcul de 111 ,, yx


12/43

12

Calcul de 22 , yx

4 5 Test du programme de traitement dimage

Lacquisition dimage et son traitement prennent un temps relativement grand.

En ce qui concerne les rsultats fournis par le programme, ils sont noys

dans du bruit. Cest la raison pour laquelle nous devons les filtrer. Le filtre de

Kalman set bien adapt pour ce type dexprience.


13/43

13

5 Le filtre de Kalman Etendu

Le filtre de Kalman a t dvelopp dans les annes 1960 par R.E Kalman. Il est

constitu dun set dquations discrtes qui permettent destimer les tats dun

systme par rapport aux donnes mesures. Etant donn que le modle du

systme est non linaire, nous avons choisi le filtre de Kalman Etendu.

Lexcution du filtre de Kalman Etendu se fait en deux tapes et dune faon

cyclique.

5 1 La prdiction

Connaissant le modle dtat du systme, on estime ltat a priori du systme.


14/43

14

Il faut initialiser lexcution avec le vecteur dtat nul c'est--dire

=

=

=

=

0

0

0

0

y

x

x .

Le vecteur

=

1

1

1 k

l

k

rk

vvu est fourni en temps rel par le robot esclave qui se

dplace.

Pour calculer KP , il faut initialiser 1kP par

=

0,0,1

00,1,

01,0,

0P la matrice identit de

dimension 3.

=

lr

lr

lr

v*L

h-v*

L

h+

v*sin*2

h+v*sin*

2

h+y

v*cos*2

h+v*cos

2

h+x

f

=

y

x

X

=

l

r

v

vu

( )XfjacobienAk ,

0,0,1

(k)v*(k))cos(*h*1/2+(k)v*(k))cos(*h*1/20,1,

(k)v*(k))sin(*h*1/2-(k)v*(k))sin(*h*1/2-1,0,

lr

lr

=

=

=

0,0,1

00,1,

01,0,

kW


15/43

15

=2

3

2

2

2

1

0,0,

0,0,

0,0,

kQ Matrice constante obtenue par tuning qui modlise le bruit du

systme.

5 2 La correction des mesures

On calcule le gain de Kalman kK par lexpression suivante :

=

0,0,1

00,1,

01,0,

kH

=

0,0,1

00,1,

01,0,

kV

=

2

2

y

2

x

0,0,

0,0,

0,0,

k

R Matrice constante qui modlise le bruit sur les mesures.

Mise jour de ltat du systme sur la base des mesures

=

mesure

mesure

mesure

k y

x

z

:

Mise jour de lerreur de covariance :

=

0,0,1

00,1,

01,0,

I


16/43

16

5 3 Simulation du filtre de Kalman sous Matlab et Labview

Nous avons simul le modle avec un bruit blanc ce qui caractrise les mesures.

Ensuite lalgorithme du filtre de Kalman Etendu a t utilis pour filtrer ces

mesures.

Le programme et les rsultats de la simulation se trouvent en annexe B. Avant

dexcuter le programme sous Labview, il faut ouvrir Matlab 6.1.


17/43

17

Face avant

5 4 Implantation du filtre de Kalman en temps rel

Aprs avoir obtenu des bons rsultats lors de la simulation, nous avons implant

filtre de Kalman Etendu afin quil puisse march en temps rel. Nous avons

remarqu deux problmes :

Lalgorithme utilise comme tat initial le vecteur

==

=

=

00

0

0y

x

x . Avant la

mise en marche du robot, il occupe une position qui diffrente de celle du

vecteur initial. Il faut donc attendre un certain temps pour que le rsultat

converge vers la vraie position du robot sur la plate-forme avant de mettre

le robot en marche. Sinon les rsultats seront fausss.

La priode dchantillonnage utilise pour la simulation nest identique

celle de Labview. Nous lavons dabord estime. Elle est environ 140ms.


18/43

18

Pour vrifier la qualit du filtre implant, nous avons ralis deux expriences :

le robot esclave dcrivant une trajectoire rectiligne

le robot esclave dcrivant une trajectoire circulaire

Les rsultats de ces deux expriences se trouvent en annexe B.


19/43

19

6 La commande prdictive

Pour que le robot esclave puisse rattraper le robot matre , nous avons

opt dutiliser la commande prdictive. Ce choix a t motiv par les tudes

prcdemment faites par Niederberger [2] et Herrera [1].

En fait, la commande prdictive consiste trouver sur un horizon donn, la

commande appliquer sur un systme et qui minimiserait une fonction de cot.

Cette dernire dpend de la trajectoire de rfrence ( )kr , de ltat du systme

( )kx et de la grandeur de rglage ( )ku .

La fonction de cot que nous avons choisie est de la forme suivante :

Connaissant le modle du systme, le prdicteur est :


20/43

20

La commande qui minimise la fonction de cot est la suivante :

Avec :

6 1 Schma de commande

Le PC1 permet de commander le robot matre afin quil puisse se dplacer

sur la plate-forme en dcrivant une trajectoire alatoire. La camera est reli au

PC2.Sur ce dernier, est install le programme de traitement dimage qui fournit

en temps rel les positions des deux robots mobiles. Le filtre de Kalman Etendu

y est aussi utilis pour filtrer les mesures de position du robot esclave . Enfin

la commande prdictive y est aussi implante pour le robot esclave .

Camera

Commande du robot Matre

Commande du robot Esclave

-Traitement dimage-Implantation du filtre deKalman

-Commande prdictive

Position des deux robots

Choix de la trajectoire du Matre


21/43

21

6 2 Gnration de la trajectoire de rfrence

Pour utiliser la commande prdictive, il faut connatre la trajectoire de rfrence.

Etant donn que les deux robots sont mobiles, cette trajectoire doit tre gnre

aprs chaque priode dchantillonnage. A cause de cette trajectoire variable

dune priode dchantillonnage lautre, on pourrait dire quil ne sagit plus de

la commande prdictive classique.

La gnration de cette trajectoire se fait en plusieurs tapes :

Grce la camera digitale et au programme de traitement dimage, les

positions des deux robots sont utilises pour estimer en temps rel la

distance qui les spare.

Ensuite on choisit la vitesse laquelle le robot esclave doit se dplacer

pour rattraper le robot matre .

Connaissant la distance entre les deux robots et la vitesse de dplacement,

on peut estimer le temps de parcours ncessaire au robot esclave pour

rattraper le robot matre .

Ce temps de parcours est ensuite converti en nombre dchantillons.

Par interpolation linaire, on gnre une trajectoire partant de la position

de lesclave celle du matre.

Esclave


22/43

22

6 3 Les paramtres de la commande prdictive implante

Nous avons choisi un horizon de control 5=cN et un horizon de

prdiction 6=pN . Les matrices Q et sont des matrices identit.


23/43

23

7 Tests exprimentaux

Lesclave doit rattraper le matre quelques soit la trajectoire que ce dernier

dcrit. Lesclave ne doit pas percuter le matre. Le robot esclave sarrte

lorsque la distance qui spare les centres de gravit des deux robots est gale 5

fois L (distance qui spare les deux roues dun robot).

7 1 Test de convergence du filtre de Kalman Etendu

Lalgorithme du filtre de Kalman Etendu que nous avons implant a t initialis

avec le vecteur( )0,0,0 === yx . Exprimentalement, le robot esclave peut

tre initialement positionn en nimporte quel point sur la plate-forme. Cest la

raison pour laquelle avant de dmarrer lexprience, on laisse dabord quelques

secondes afin que le filtre de Kalman converge vers la vraie position initiale du

robot.

7 1 1 Exprience 1

Trajectoire dun robot


24/43

24

Angle

7 1 2 Exprience 2



25/43

25

Angle

7 2 Effet du filtre de Kalman suite aux perturbations

7 2 1 Exprience 1 : 4 perturbations



26/43

26

Angle

7 2 2 Exprience 2 : 3 perturbations



27/43

27

Angle

Lexprience 2 a t ralise avec les valeurs des matrices de covariance

diffrentes de celles de lexprience 1: Pour la seconde exprience, les lments

diagonaux de la matrice Q sont 10 fois infrieures celle de la premire

exprience. Le filtre de Kalman de la premire exprience est moins robuste aux

perturbations. Alors quil attnue quasiment bien les perturbations lors de la

seconde exprience.


28/43

28

7 3 Trajectoire du matre et de lesclave

7 3 1 Matre immobile

7 3 1 1 Exprience 1

Trajectoire


29/43

29

Angle

7 3 1 2 Exprience 2

Trajectoire


30/43

30

Angle

7 3 2 Matre mobile

7 3 2 1 Exprience 1

Trajectoire


31/43

31

Angle

Lorsquon observe la variation de langle tout au long de cette exprience, on

se rend compte que lorsque lesclave est proche du matre, il varie brusquement.

Ceci est d au programme de traitement dimage.


32/43

32

7 3 2 2 Exprience 2

Trajectoire

Angle


33/43

33

7 3 2 3 Exprience 3

Trajectoire

Angle


34/43

34

Pour les deux expriences prcdentes, lesclave percute le matre lorsquil le

rattrape. La reprsentation dans Matlab des trajectoires et de la position finale

des deux robots montre le contraire. Nous pensons que ceci est la camera qui

filme les deux robots sous un angle.

8 Conclusion

Malgr plusieurs difficults rencontres lors de la phase dimplantation du filtre

de Kalman et de la commande prdictive non classique, on peut dire que le

cahier de charge a t quasiment respect. Il reste encore des quelques

amliorations apportes, notamment sur l'amlioration du temps de traitement

des diffrents algorithmes qui est encore relativement lev et le programme de

traitement dimage. Nous avons aussi bnfici dune solide base thorique

ralise par Niederberger et Herrera.


35/43

35

ANNEXE A

A1 : Programme Matlab de calcul du Jacobien

A2 : Programme Matlab pour le modle

A3 : Programme de simulation du modle


36/43

36

A4 : rsultat de la simulation

A5 : rsultat simulation dans Labview


37/43

37

ANNEXE B

B1 : Programme qui simule le modle bruit

B2 : Simulation du filtre de Kalman Etendu


38/43

38

B3 : Rsultat de la simulation du modle bruit


39/43

39

B4 : Rsultat de la simulation du filtre de Kalman Etendu

B5 : Rsultat sous Labview : modle bruit


40/43

40

B6 : Rsultat sous Labview : sortie filtre

B7 : Simulation en temps rel : trajectoire rectiligne


41/43

41

B8 : Simulation en temps rel : angle pour trajectoire rectiligne

B9 : Simulation en temps rel : trajectoire circulaire


42/43

42

B10 : Simulation en temps rel : angle pour trajectoire circulaire


43/43

BIBLIOGRAPHIE:

[1] Trajectory tracking trough predictive control: application to the Khepera

robot, Juan Herrera, Diploma thesis, 1999

[2] Predictive control Design for a Khepera robot: Principles, Simulations and

Real-time Implementation, Andr S.P Niederberger, Diploma thesis, February

2002

[3] Position estimation and control of an autonomous vehicle, Joana Maria

Comas Marti, Diploma thesis, June 2005

[4] An introduction to the Kalman filter, Greg Welch and Gary Bishop,

University of North California at Chapel Hill, April 2004

positioning and control of mobile robots_miamo_ekf

Documents