RobotDif M1ASE

Master ASE Projet S1 - Pierre Bonnet , John Klein - Sept/Dc 2011
1
Projet Robotique LEGO NXT
Robot diffrentiel suiveur de ligne
2
Etude du robot diffrentiel
Objectif
- comparaison de quelques robots
- analyse du mouvement du robot diffrentiel
- modle de commande du robot diffrentiel
- tude de la trajectoire
- tude du capteur et de sa communication avec l'unit centrale
- analyse du signal capteur et localisation de la ligne
- asservissement de direction du robot
3
Robot roues
Il existe de nombreux types de roues en robotique
roue simple adhrence

roues dplacement latral
roue galets roue sudoise roue omnidirectionnelle
roue centre fixe roue centre orientable roue dcentre orientable
4
Robot roues
Robot Tricycle :
deux roues fixes indpendantes
de mme axe de rotation et une
roue centre orientable sur
l'axe longitudinal du robot
quivalent une voiture usuelle
Robot Omnidirectionnel :
quip de trois roues
sudoises, peut se dplacer
dans toutes les directions
du plan et s'orienter
indpendamment.
Totalement libre / 3 degrs
de libert (2 translations,
une rotation).
Y
X
O
y
x
O'
0
Y
X
O
y
x
O'
0
5
Robot roues
Robot diffrentiel :
deux roues fixes indpendantes de mme axe de rotation et une roue libre autodirectionnelle
l'orientation se fait par la diffrence de marche des deux roues de propulsion
le robot peut changer d'orientation si la roue libre n'induit pas de frottement
Y
X
O
y
x
O'
0
axe x'
axe y'
La posture du robot est dfinie par
( =( x , y , 0)
6
Robot roues
Roue sans glissement :
Y
X
O
y
x
O'
axe x'
axe y'
pneu
0
axe x'
pneu
axe
P
Q
axe z
u
Q'
r
En l'absence de glissement longitudinal (patinage) et transversal (drapage), le dplacement se
fait selon l'axe O'x' dans la direction avec la vitesse linaire
d'o

v = r u 0
x = r ucos 0
y = r usin 0
7
Robot diffrentiel roues
Dplacement en translation du robot :
Si les roues droite et gauche tournent la mme vitesse sans glissement, le robot se dplace
selon l'axe O'x' dans la direction avec la vitesse linaire
d'o
Si les roues ne tournent pas la mme vitesse, le robot dcrit une trajectoire circulaire.
v = r u 0
x = r ucos 0
y = r usin 0
u
Y
X
O
y
x
O'
axe x'
axe y'
pneu
0
8
Y
X
O
Centre instantan
de rotation
O'
0
0+d 0
O''
d 0
L
p
La relation liant la rotation et le parcours est :
parcours roue arc de centre CIR
par drivation, on obtient :
r u
d
=(p+L)

0 r u
g
=(pL)
0
r D
d
=(p+L)0
Rotation du robot :
Au cours de sa rotation la vitesse
la roue droite parcourt un arc de
cercle de centre CIR et de
rayon
La vitesse angulaire de rotation
autour du CIR est
p+L
0 =
d 0
dt
u
d
9
quations du dplacement :
r u
d
=(p+L)

0
r u
g
=(pL)
= r

d
g
2L
= L

g
+
d
d
la vitesse linaire du centre du robot dans la direction est :
0 v = r
(
g
+
d
2
)
les composantes de la vitesse linaire dans le repre OXY sont :
x = r
(
u
d
+u
g
2
)
cos 0

y = r
(
u
d
+u
g
2
)
sin 0
par intgration, on retrouve la relation dj cite : 0 =
D
d
D
g
2L
par intgration, la distance parcourue est : D =
D
g
+ D
d
2
10
Commande du dplacement : translation et rotation du robot
Pour de nombreuses applications, une trajectoire est un dplacement dans lequel sont
fixs plusieurs paramtres:
- la vitesse de translation (ou vitesse linaire instantane)
- une information sur la direction de la trajectoire
soit le rayon de courbure
soit la vitesse angulaire
les vitesses et s'obtiennent par rsolution d'un systme de deux quations
deux inconnues .
v
translation
= r
(
d
+
g
2
)
= L

d
+
g
= r

d
g
2L
d

g
11
Commande du dplacement : mise en oeuvre par programmation
OnFwdReg(OUT_A,Power_Value, OUT_REGMODE_SPEED);
Cette commande est applique au travers du hacheur, du moteur et de la roue. La chane
de commande est la suivante :
Le gain en vitesse de la chane tant :
La commande appliquer est: ou bien
roue
=
V
bat
K
e
PWM
100
Tension
quivalente
Hacheur
4
quadrants
Batterie
7,45V
Commande PWM
[-100 , +100]
Moteur Rducteur Roue Sol
1/Ke
r =27mm
v
1
1+ p
PWM =100
K
e
V
bat

roue
PWM =100
K
e
V
bat
v
Troue
r
12
Commande du dplacement ( mise en oeuvre par programmation)
Application des commandes aux deux moteurs:
OnFwdReg(OUT_A,PWM_d, OUT_REGMODE_SPEED);
OnFwdReg(OUT_C,PWM_g, OUT_REGMODE_SPEED);
M_d H_d Rducteur
roue_d
/sol
M_g H_g Rducteur
roue_g
/sol
Calcul
Commandes
PWM_d
PWM_g
d U_d
U_g

g
v
translation
ou

v
d
v
g
13
Commande du dplacement ( mise en oeuvre par programmation)
Il est commode d'utiliser les variables intermdiaires et
pour les combiner par:
PWM
g
=PWM
commun
+ PWM
differentiel
PWM
commun
PWM
diffrentiel
PWM
d
=PWM
commun
PWM
differentiel
v
Translation
= r
(
g
+
d
2
)
= L

g
+
d
d
v
T
Exemple: faire un dplacement la vitesse avec un rayon de courbure
D'o:

g
+
d
2
=
v
T
r
PWM
com
=100
K
e
V
bat
v
T
r
d
2
=
L

g
+
d
2
PWM
diff
=100
K
e
V
bat
L
v
T
r
14
Travail 1:
- Faire un dplacement en ligne droite la vitesse constante v= 10cm/s
pour une dure de 5 s .
Mthode:
- Dterminer la vitesse linaire puis la commande PWM partir des donnes numriques du
problme, puis faire un programme de dplacement avec la mme commande applique aux deux
moteurs.
Travail 2:
- Enregistrer les positions avec une priode Te = 0.1s et vrifier que la distance parcourue
correspond la demande.

Travail 3:
- Enchaner la translation avec un arc de cercle de rayon de courbure rho=30cm , d'angle 90
et de vitesse moyenne 10cm/s
Mthode:
- Dterminer les vitesses angulaires droite et gauche puis les commandes PWM . Appliquer la
commande pendant une dure calcule partir des donnes du problme
15
Capteur LineLeader
Le capteur LineLeader est un capteur optique de suivi de ligne
Il est compos de :
- 9 sources ponctuelles (leds rouge)
- 8 capteurs photosensibles
Les proprits annonces par son fabricant sont :
- Easy to mount on your NXT robot facing ground
- Reporting dark and light pattern from array of 8 sensors
- Detect and follow a line
- Write line follower programs with your own decision making
- Develop your own PID control algorithms
- Upgradable firmware using simple software.
Documentation : http://www.mindsensors.com user guide, sample programs, API doc
16
Capteur LineLeader
Montage du capteur sur le robot
Le capteur est articul librement de faon tre appliqu plat sur le sol.
La liaison sert de ressort pour plaquer le capteur (le courber pour obtenir l'effet recherch)
Le capteur peut tre branch sur l'un des ports d'entre (S1, S2, S3 ou S4, utiliser la valeur
correspondante dans les programmes )
17
Capteur LineLeader : Bus I2C
Le capteur LineLeader suit le protocole standard de communication I2C.
C'est protocole de communication entre un maitre et plusieurs esclaves par liaison srie
asynchrone deux fils SDA (Serial Data) SCL (Serial Clock); il permet d'changer des
donnes (octets) dans les deux sens (lecture et criture des esclaves) 400 khz max
Les niveaux lectriques sont imposs un collecteur ouvert et une rsistance de rappel au +3.3V
18
Architecture d'une trame (8bits) :
Trame d'adresse (premire trame d'un change initi par le matre) :
La capacit d'adressage est de 127 priphriques en mode de base
si dernire
trame
si premire
trame
19
Utilisation d'un registre d'adressage (pointage interne) :
Pour dialoguer avec les structures internes d'un priphrique (registres internes), on utilise
gnralement un (ou plusieurs) registres d'adressage interne.
Dans ce type de dialogue I2C, le deuxime change aprs l'adresse d'esclave est toujours une (ou
plusieurs) critures du registre d'adressage interne.
La suite du dialogue est :
- soit une criture du registre de donnes interne point par le registre adresse
- soit un nouveau message de lecture du registre de donnes interne. La norme I2C prvoit un
enchanement direct de la lecture aprs l'criture sans relancer l'adresse d'esclave (transaction plus
rapide).
L'criture ou la lecture provoquent l'incrmentation automatique du registre de pointage. Il est ainsi
possible de lire ou d'crire n registres de donnes conscutifs sans relancer le dialogue.
maitre
registre
adresse
data1
data2
data3
BUS
I2C
20
Capteur LineLeader : registres
Le capteur LineLeader ne comporte qu'un seul registre d'adressage (8bits).
Quelques registres de donnes (points par le registre d'adressage):
21
Capteur LineLeader: commandes
Le registre de donnes 0x41 "command" permet de dfinir les modes de fonctionnement.
Quelques commandes :
22
Capteur LineLeader : criture directe
L'accs direct par les commandes I2C - Exemple : commande de calibration noir
byte port = S1;
byte addr = 0x02; // default slave n
byte register = 0x41;
byte cmd StrToNum("B"); // code command
byte cmdBuf[];
byte nByteReady = 0;

ArrayBuild(cmdBuf, addr, register, cmd);
I2CWrite(port, 0, cmdBuf);
while (I2CStatus(port, nByteReady) == STAT_COMM_PENDING);
status = I2CCheckStatus(port);
while (status > NO_ERR)
status = I2CCheckStatus(port);
23
Capteur LineLeader : lecture directe
L'accs direct par les commandes I2C - Exemple : lecture 8 datas calibres
byte port = S1;
byte addr = 0x02; // default slave n
byte register = 0x49;
byte message[];
byte nByteReady = 0;
int count = 8;
unsigned byte returnValue[8];

ArrayBuild(message, addr, register);
// verify no activity on port
while (I2CStatus(port, nByteReady) == STAT_COMM_PENDING);
err = I2CBytes(port, message, count, returnValue);
// If no err, can use returnValue
24
Capteur LineLeader : principales fonctions de la bibliothque ll-lib.nxc
Activation du capteur :
SetSensorLowspeed(SensorPort);
LL_WakeUp(SensorPort, ADDR);
Lecture directe des 8 capteurs (2 octets par capteurs):
LL_ReadRaw_Uncalibrated (SensorPort, ADDR, data_tab);
Dfinition du niveau de rfrence blanc:
LL_SendCommand(SensorPort, ADDR, "W");
Dfinition du niveau de rfrence noir:
LL_SendCommand(SensorPort, ADDR, "B");
Lecture des 8 capteurs calibrs (1 octet par capteur):
LL_ReadRaw_Calibrated (SensorPort, ADDR, data_tab);
25
Capteur LineLeader : calibration du capteur
La calibration est faite en mesurant le niveau du noir et le niveau du blanc
peru par chaque capteur. Les mesures brutes sont corriges selon la loi affine :
Les mesures corriges sont directement calcules par le capteur, partir des valeurs mmorises
des niveaux de rfrence noir et blanc (apprentissage pralable).
L'chelle des valeurs calibres est [0..100] .

La mmoire est du type permanent (valeurs conserves mme aprs coupure du courant).
Le capteur est fourni avec le programme d'apprentissage LL-calib.nxc des valeurs de rfrences.
b
i
w
i
u
i
c
i
=100
u
i
b
i
w
i
b
i
26
Capteur LineLeader : calibration du capteur
Travail :

- Tlcharger depuis le site Mindsensors la bibliothque LL-lib.nxc, le programme
de calibration LL-Calib.nxc et l'exemple d'utilisation LL-raw-sample.nxc
- Utiliser le programme d'apprentissage LL-Calib.nxc pour apprendre les niveaux
de noir et de blanc sur la table de parcours.
- Avec l'exemple LL-raw-sample.nxc , observer la valeur des mesures sans calibration et
avec calibration pour le blanc de rfrence, le noir de rfrence et sur la ligne .
- Si ncessaire , recalibrer le capteur .
27
Capteur LineLeader : utilisation du capteur
La mission du robot tant de suivre la ligne noire, il convient de dduire la position
partir des mesures du capteur LL .
mesures
calibres
ligne
noire
LineLeader
ci
position
100
100-ci
position
100
mesures
calibres
inverses
Pour faciliter les traitements,
il est possible d'inverser les
mesures (dtection d'un
lment blanc sur un fond noir)
28
Pour dtecter la ligne noire, il convient de mettre en place un
traitement des niveaux de gris mesurs par les capteurs ci.
Deux approches :
- dfinir un seuil de binarisation pour dfinir le ou les capteurs situs sur la ligne (registre 0x44),
puis calculer une position moyenne (registre 0x43) . Cette fonction est intgre dans le capteur et
s'excute automatiquement voir dtails page 8 notice capteur.

- utiliser directement les mesures pour calculer le barycentre de l'observation. Cette solution a
l'avantage d'tre beaucoup plus robuste (calcul par sommation)
29
position i
amplitude
mesure
barycentre
x
B
c
i
x
i
Calcul du barycentre de la ligne (hypothse: ligne blanche sur fond noir):
Chaque capteur a pour masse sa mesure calibre et occupe une position (origine = centre du
capteur par ex.). La position du barycentre par rapport l'origine choisie est donne par :
Pour faire le calcul en format entier, prendre comme position les valeurs suivantes
(approximativement la position de chaque capteur en mm avec origine au centre) :
c
i
x
i
x
i
= 35,25,15,5,+5,+15,+25,+35|
x
B
=
c
i
x
i
c
i
30
Travail :
1) Etablir une fonction NXC de calcul du barycentre comprenant
- lecture des valeurs calibres
- Inversion les mesures pour que le noir reprsente le signal utile
- Calcul de la position du barycentre (int) avec les poids proposs . Eventuellement , mettre
l'chelle le rsultat (largeur relle du capteur 47.5mmet non pas 70mm ).
- Afficher les principaux rsultats .
2)Utiliser cette fonction dans un programme principal main comprenant:
- l'initialisation du capteur
- l'appel de la fonction en boucle infinie (ventuellement , prvoir une sortie par appui sur
une touche )
Tester le fonctionnement en plaant manuellement le capteur sur la ligne .
Donner la valeur mesure lorsque la ligne est centre, sur la bord droit, la bord gauche.
Que reprsente le signede la mesure?

31
Solution:
int LLBarycenter()
{ string message;
int i;
unsigned byte rawCalibrated[8];
int position[]={-35,-25,-15,-5,5,15,25,35}; // position en mm
int sum_mesures,cg_position;
sum_mesures = 0; cg_position = 0 ;
LL_ReadRaw_Calibrated(SensorPort, ADDR, rawCalibrated);

for ( i=0; i < 8; i++) {
sum_mesures = sum_mesures + (100 - rawCalibrated[i]);
cg_position = cg_position + position[i]* (100 - rawCalibrated[i]);
}

cg_position = cg_position/sum_mesures;
message = "Position Ligne";
TextOut(0, LCD_LINE2, message, true);
NumOut(0, LCD_LINE3, cg_position, false);
return cg_position;
}
task main()
{ SetSensorLowspeed(SensorPort);
LL_WakeUp(SensorPort, ADDR);
Wait(100);
ClearScreen();
while (!ButtonPressed(BTNCENTER, true)) {
LLBarycenter();
}
}
Pas pour les tudiants!!!!
32
Capteur LineLeader : suivi de ligne par asservissement en boucle ferme
O
O'
G
Pour piloter le robot suiveur de ligne, la proposition est de faire une boucle
d'asservissement de la direction du robot afin de maintenir le robot dans la direction de la ligne.
La direction relative du robot par rapport la ligne est value par le LineLeader:
tan =
O' G
O' O
Les angles tant petits, on peut assimiler la tangente l'angle:
O' G
O' O
Pour l'application pratique, on remarque que O'O est un facteur de gain dans le calcul de
l'angle. Pour simplifier l'asservissement, l'ensemble des facteurs de gain de la boucle sera
rassembl en un gain unique global.
33
Boucle d'asservissementde la direction :
O
O'
G
Calcul
Commandes
PWM_d
PWM_g
V_T
correcteur
u
consigne
=0

La consigne angulaire est nulle: le robot doit s'aligner sur la ligne .
La vitesse de translation sera fixe par des considrations externes:
- valeur constante
- valeur fixe par l'oprateur (par ex: transmission par la liaison Bluetooth)
- asservissement de la vitesse celle du robot qui prcde sur la piste (par ex: mesure de
la distance par le capteur ultrason )
Pour une premire approche, le correcteur est un simple gain . Une tude plus complte
permettrait de mettre en place un correcteur (par ex: PID) en vue d'une meilleure rponse de
l'asservissement (meilleure dynamique donc plus rapide).
34
Travail :
1) Crer une fonction Move(VT,Theta) comprenant les lments suivants:
- calcul de l'erreur epsilon
- calcul de la commande u
- calcul de PWMcommun et PWMdifferentiel
- commande des moteurs Droite et Gauche
Note: pour simplifier, on peut ne pas tenir compte des diffrents coefficients physiques du robot.
En particulier, la vitesse VT sera exprim en %PWM (valeur entre 0 et 100); l'angle Theta sera
exprim en mesure OG tel que calcul dans la fonction du Barycentre. Le gain du correcteur
proportionnel sera fix 1 . Ces conditions permettent d'arriver un asservissement stable ds
le premier essai .
2) Modifier la tche Main pour y introduire la fonction Move dans la boucle infinie.
Tester le bon fonctionnement de l'asservissement pour la vitesse VT= 25% (inverser le signe de
Theta si la correction se fait dans le mauvais sens) .
L'asservissement est-il encore suffisamment stable pour une vitesse plus leve?
Pour VT= 25% , introduire un retard de 10ms dans la boucle . Comment volue l'asservissement?

RobotDif M1ASE

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

RobotDif M1ASE

Uploaded by

Copyright:

Available Formats

Master ASE Projet S1 - Pierre Bonnet , John Klein - Sept/Dc 2011

You might also like