robot différentiel suiveur de lignebonnet/projet_lego/robotdif_m1ase.pdf · architecture...

Master ASE – Projet S1 - Pierre Bonnet , John Klein - Sept/Déc 2011 1

Projet Robotique LEGO NXT

Robot différentiel suiveur de ligne



Etude du robot différentiel

Objectif

- comparaison de quelques robots

- analyse du mouvement du robot différentiel

- modèle de commande du robot différentiel

- é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



Robot à roues

Il existe de nombreux types de roues en robotique roue simple à adhérence

roues à déplacement latéral

roue à galets roue suédoise roue omnidirectionnelle

roue centrée fixe roue centrée orientable roue décentrée orientable



Robot à roues

Robot Tricycle : deux roues fixes indépendantes de même axe de rotation et une roue centrée orientable sur l'axe longitudinal du robotéquivalent à une voiture usuelle

Robot Omnidirectionnel :équipé de trois roues suédoises, peut se déplacer dans toutes les directions du plan et s'orienter indépendamment. Totalement libre / 3 degrés de liberté (2 translations, une rotation).

Y

XO

y

x

O'

Y

XO

y

x

O'



Robot à roues

Robot différentiel : deux roues fixes indépendantes de même axe de rotation et une roue libre autodirectionnelle

→ l'orientation se fait par la différence de marche des deux roues de propulsion → le robot peut changer d'orientation si la roue libre n'induit pas de frottement

Y

XO

y

x

O'

axe x'

axe y'

La posture du robot est définie par = x , y ,



Robot à roues Roue sans glissement :

Y

XO

y

x

O'

axe x'

axe y'

pneu

axe x'

pneuaxe

P

Q

axe z

Q'r

En l'absence de glissement longitudinal (patinage) et transversal (dérapage), le déplacement se fait selon l'axe O'x' dans la direction avec la vitesse linéaire

d'où

v = r

x = rcosy = rsin



Robot différentiel à roues

Déplacement en translation du robot :

Si les roues droite et gauche tournent à la même vitesse sans glissement, le robot se déplace selon l'axe O'x' dans la direction avec la vitesse linéaire

d'où

Si les roues ne tournent pas à la même vitesse, le robot décrit une trajectoire circulaire.

v = r

x = r cosy = r sin

Y

XO

y

x

O'

axe x'

axe y'

pneu



Robot différentiel à rouesY

XO

Centre instantané de rotation

O'

d O''

d

L

La relation liant la rotation et le parcours est :

parcours roue arc de centre CIR

par dérivation, on obtient : r d = L r g = −L

r d = L

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

L

=d dt

d



Robot différentiel à roues

équations du déplacement :

r d = L

rg = −L

θ = rωd−ωg

2Lρ = L

ωg+ωdωg−ωd

la vitesse linéaire du centre du robot dans la direction est : v = r (ωg+ωd

2 )

les composantes de la vitesse linéaire dans le repère OXY sont :

x = r dg

2 cos y = r dg

2 sin

par intégration, on retrouve la relation déjà citée : =Dd − Dg

2L

par intégration, la distance parcourue est : D =Dg Dd

2



Robot différentiel à roues Commande du déplacement : translation et rotation du robot

Pour de nombreuses applications, une trajectoire est un déplacement dans lequel sont fixés plusieurs paramètres :

- la vitesse de translation (ou vitesse linéaire instantanée)

- une information sur la direction de la trajectoire

soit le rayon de courbure

soit la vitesse angulaire

les vitesses et s'obtiennent par résolution d'un système de deux équations à deux inconnues .

v translation = r ( ωd+ωg

2 )

ρ = Lωd+ωgωg−ωd

θ = rωd−ωg

2L

ωd ωg



Robot différentiel à roues Commande du déplacement : mise en oeuvre par programmation

OnFwdReg(OUT_A,Power_Value, OUT_REGMODE_SPEED);

Cette commande est appliquée au travers du hacheur, du moteur et de la roue. La chaîne de commande est la suivante :

Le gain en vitesse de la chaîne étant :

La commande à appliquer est : ou bien

ωroue =V bat

K e

PWM100

Tensionéquivalente

Hacheur4

quadrants

Batterie7,45V

Commande PWM[-100 , +100] Moteur Réducteur Roue Sol

1/Ke r =27mm

v1

1+τ p

PWM = 100K e

V batωroue

ω

PWM = 100K e

V bat

vTroue

r



Robot différentiel à roues Commande du déplacement ( 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_dH_d Réducteur roue_d/sol

M_gH_g Réducteur roue_g/sol

Calcul Commandes

PWM_d

PWM_g

ωdU_d

U_g ωg

v translation

ρou θ

v d

v g



Robot différentiel à roues Commande du déplacement ( mise en oeuvre par programmation)

Il est commode d'utiliser les variables intermédiaires et pour les combiner par :

PWM g=PWM commun + PWM differentiel

PWM commun PWM différentiel

PWM d=PWM commun − PWM differentiel

vTranslation = r (ωg+ωd

2 ) ρ = Lωg+ωdωg−ωd

vT ρExemple : faire un déplacement à la vitesse avec un rayon de courbure

D'où : ωg+ωd

2=

vT

rPWM com = 100

K e

V bat

vT

r

ωg−ωd

2=

Lρ

ωg+ωd

2PWM diff = 100

K e

V bat

Lρ

vT

r



Robot différentiel à rouesTravail 1: - Faire un déplacement en ligne droite à la vitesse constante v= 10cm/s pour une durée de 5 s .

Méthode:- Déterminer la vitesse linéaire puis la commande PWM à partir des données numériques du

problème, puis faire un programme de déplacement avec la même commande appliquée aux deux moteurs.

Travail 2: - Enregistrer les positions avec une période Te = 0.1s et vérifier que la distance parcourue

correspond à la demande. Travail 3:

- Enchaîner la translation avec un arc de cercle de rayon de courbure rho=30cm , d'angle 90° et de vitesse moyenne 10cm/s

Méthode:- Déterminer les vitesses angulaires droite et gauche puis les commandes PWM . Appliquer la

commande pendant une durée calculée à partir des données du problème



Capteur LineLeaderLe capteur LineLeader est un capteur optique de suivi de ligne

Il est composé de :- 9 sources ponctuelles (leds rouge)- 8 capteurs photosensibles

Les propriétés annoncées 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

http://www.mindsensors.com/



Capteur LineLeaderMontage du capteur sur le robot

Le capteur est articulé librement de façon à ê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'entrée (S1, S2, S3 ou S4, utiliser la valeur correspondante dans les programmes )



Capteur LineLeader : Bus I2CLe capteur LineLeader suit le protocole standard de communication I2C.

C'est protocole de communication entre un maitre et plusieurs esclaves par liaison série asynchrone à deux fils SDA (Serial Data) SCL (Serial Clock); il permet d'échanger des données (octets) dans les deux sens (lecture et écriture des esclaves) à 400 khz max

Les niveaux électriques sont imposés un collecteur ouvert et une résistance de rappel au +3.3V



Capteur LineLeader : Bus I2C

Architecture d'une trame (8bits) :

Trame d'adresse (première trame d'un échange initié par le maître) :

La capacité d'adressage est de 127 périphériques en mode de base

si dernière trame

si première trame



Capteur LineLeader : Bus I2C

Utilisation d'un registre d'adressage (pointage interne) :

Pour dialoguer avec les structures internes d'un périphérique (registres internes), on utilise généralement un (ou plusieurs) registres d'adressage interne.

Dans ce type de dialogue I2C, le deuxième échange après 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 données interne pointé par le registre adresse - soit un nouveau message de lecture du registre de données interne. La norme I2C prévoit un enchaînement direct de la lecture après l'écriture sans relancer l'adresse d'esclave (transaction plus rapide).

L'écriture ou la lecture provoquent l'incrémentation automatique du registre de pointage. Il est ainsi possible de lire ou d'écrire n registres de données consécutifs sans relancer le dialogue.

maitre registre adresse

data1

data2

data3BUS I2C



Capteur LineLeader : registres

Le capteur LineLeader ne comporte qu'un seul registre d'adressage (8bits). Quelques registres de données (pointés par le registre d'adressage):



Capteur LineLeader: commandes

Le registre de données 0x41 "command" permet de définir les modes de fonctionnement. Quelques commandes :



Capteur LineLeader : écriture directe

L'accès 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);



Capteur LineLeader : lecture directe

L'accès direct par les commandes I2C - Exemple : lecture 8 datas calibrées

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



Capteur LineLeader : principales fonctions de la bibliothèque 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);

Définition du niveau de référence blanc:

LL_SendCommand(SensorPort, ADDR, "W");

Définition du niveau de référence noir:

LL_SendCommand(SensorPort, ADDR, "B");

Lecture des 8 capteurs calibrés (1 octet par capteur):

LL_ReadRaw_Calibrated (SensorPort, ADDR, data_tab);



Capteur LineLeader : calibration du capteur

La calibration est faite en mesurant le niveau du noir et le niveau du blanc perçu par chaque capteur. Les mesures brutes sont corrigées selon la loi affine :

Les mesures corrigées sont directement calculées par le capteur, à partir des valeurs mémorisées des niveaux de référence noir et blanc (apprentissage préalable).

L'échelle des valeurs calibrées est [0..100] . La mémoire est du type permanent (valeurs conservées même après coupure du courant).

Le capteur est fourni avec le programme d'apprentissage LL-calib.nxc des valeurs de références.

bi wiui

c i = 100ui−bi

wi−bi



Capteur LineLeader : calibration du capteur

Travail :

- Télécharger depuis le site Mindsensors la bibliothèque 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 référence, le noir de référence et sur la ligne .

- Si nécessaire , recalibrer le capteur .



Capteur LineLeader : utilisation du capteurLa mission du robot étant de suivre la ligne noire, il convient de déduire la position à partir des mesures du capteur LL .

mesurescalibréesligne

noire

LineLeader

ci

position

100

100-ci

position

100

mesurescalibréesinversées

Pour faciliter les traitements, il est possible d'inverser les

mesures (détection d'un élément blanc sur un fond noir)



Capteur LineLeader : utilisation du capteurPour détecter la ligne noire, il convient de mettre en place un traitement des niveaux de gris mesurés par les capteurs ci.

Deux approches :

- définir un seuil de binarisation pour définir le ou les capteurs situés sur la ligne (registre 0x44), puis calculer une position moyenne (registre 0x43) . Cette fonction est intégrée dans le capteur et s'exécute automatiquement voir détails 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)



Capteur LineLeader : utilisation du capteur

position i

amplitude mesure barycentre

xB

ci

xi

Calcul du barycentre de la ligne (hypothèse: ligne blanche sur fond noir):

Chaque capteur a pour masse sa mesure calibrée et occupe une position (origine = centre du capteur par ex.). La position du barycentre par rapport à l'origine choisie est donnée 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 ]

xB = ∑ ci x i

∑ c i



Capteur LineLeader : utilisation du capteur

Travail :

1) Etablir une fonction NXC de calcul du barycentre comprenant

- lecture des valeurs calibrées - Inversion les mesures pour que le noir représente le signal utile- Calcul de la position du barycentre (int) avec les poids proposés . Eventuellement , mettre

à l'échelle le résultat (largeur réelle du capteur 47.5mmet non pas 70mm ).- Afficher les principaux résultats .

2)Utiliser cette fonction dans un programme principal main comprenant :- l'initialisation du capteur - l'appel de la fonction en boucle infinie (éventuellement , prévoir une sortie par appui sur une touche )

Tester le fonctionnement en plaçant manuellement le capteur sur la ligne . Donner la valeur mesurée lorsque la ligne est centrée, sur la bord droit, la bord gauche. Que représente le signe de la mesure?



Capteur LineLeader : utilisation du capteurSolution : int LLBarycenter()

{ string message;int i;unsigned byte rawCalibrated[8];int position[]={-35,-25,-15,-5,5,15,25,35}; // position en mmint 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 !!!!



Capteur LineLeader : suivi de ligne par asservissement en boucle fermée

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 évaluée par le LineLeader :θ

tanθ =O ' GO ' O

Les angles étant petits, on peut assimiler la tangente à l'angle : θ ≈O ' GO ' 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.



Capteur LineLeader : suivi de ligne par asservissement en boucle fermée

Boucle d'asservissement de la direction :

O O'

G

θCalcul

Commandes

PWM_d

PWM_g

V_T

correcteuruθconsigne=0 ε

La consigne angulaire est nulle : le robot doit s'aligner sur la ligne .La vitesse de translation sera fixée par des considérations externes : - valeur constante - valeur fixée par l'opérateur (par ex : transmission par la liaison Bluetooth) - asservissement de la vitesse à celle du robot qui précède sur la piste (par ex : mesure de la distance par le capteur à ultrason )Pour une première approche, le correcteur est un simple gain . Une étude plus complète permettrait de mettre en place un correcteur (par ex : PID) en vue d'une meilleure réponse de l'asservissement (meilleure dynamique donc plus rapide).



Capteur LineLeader : suivi de ligne par asservissement en boucle ferméeTravail : 1) Créer une fonction Move(VT,Theta) comprenant les éléments 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 différents 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 dès le premier essai .

2) Modifier la tâche 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 élevée ?

Pour VT= 25% , introduire un retard de 10ms dans la boucle . Comment évolue l'asservissement ?

robot différentiel suiveur de lignebonnet/projet_lego/robotdif_m1ase.pdf · architecture...

Documents