• 15 heures
  • Moyenne

Ce cours est visible gratuitement en ligne.

course.header.alt.is_video

course.header.alt.is_certifying

J'ai tout compris !

Mis à jour le 08/11/2019

Estimez la position du robot

Connectez-vous ou inscrivez-vous gratuitement pour bénéficier de toutes les fonctionnalités de ce cours !

Notre robot est maintenant capable de se déplacer. Mais comment estimer sa position à partir de la mesure du trajet qu'il a accompli depuis une position initiale ?

C'est ce que nous allons apprendre dans ce chapitre. Cette estimation est basée sur l’odométrie qui utilise le nombre de tours effectués par chaque roue. Ce nombre de tours est mesuré par les capteurs de roues ou estimé par la durée de fonctionnement à vitesse connue.

Nous verrons les équations de la mécanique utilisées, puis nous en déduirons comment obtenir la position du robot à chaque instant. Enfin, nous découvrirons le programme Arduino commenté qui code cette méthode.

À la fin de ce chapitre, connaître la position de votre robot n'aura plus aucun secret pour vous!

Principe de l’odométrie

La question de la trajectométrie en robotique est très importante. Elle correspond à déterminer à chaque instant position, vitesse et accélération de l’engin. Il ne faut toutefois pas oublier que le véhicule n’est pas un point matériel avec juste trois coordonnées d’espace qu’il suffit de dériver pour obtenir la vitesse et de dériver une deuxième fois pour obtenir l’accélération. Dans le cas général, l’état cinématique d’un objet non déformable (c’est le cas de notre petit robot) est entièrement déterminé par les trois coordonnées de son centre de gravité et de trois angles qu’on appelle respectivement lacet, roulis et tangage, selon la figure ci-dessous.

L'état cinématique d'un objet non déformable : roulis, tangage, lacet
l’état cinématique d’un objet non déformable est entièrement déterminé par les trois coordonnées de son centre de gravité et de trois angles qu’on appelle respectivement lacet, roulis, tangage
lacet, roulis, tangage

Si on appelle OX l’axe de l’engin, OY le second axe horizontal perpendiculaire à OX et OZ l’axe vertical, alors nous pouvons dire que :

  • le roulis correspond à l’angle de rotation selon l’axe OX. Cet angle est nul quand le véhicule est parfaitement horizontal ;

  • le tangage correspond à l’angle de rotation autour de OY ; là encore l’angle est nul quand le véhicule est horizontal ;

  • enfin, le lacet correspond à l’angle de rotation autour de l’axe OZ. Il faut choisir une direction privilégiée pour définir cet angle nul.

Des capteurs pour déterminer l'état cinématique de notre robot

Dans le cas le plus général, il existe de nombreux capteurs plus ou moins sophistiqués qui permettent de déterminer à chaque instant l’état cinématique. Tous ces capteurs présentent des limitations, si bien qu’on les utilise généralement de façon couplée. On peut citer les principaux capteurs :

  • le GPS permet de déterminer le centre de gravité de l’engin s’il voit les satellites ; ce qui suppose un fonctionnement outdoor. Les accéléromètres et gyroscopes sont souvent intégrés dans des centrales inertielles qui permettent de déterminer accélération linéaires et angulaires. Après deux intégrations, on accède aux positions ;

  • on utilise aussi souvent des magnétomètres qui permettent de mesurer les angles par référence au champ magnétique connu. Toutes ces technologies sont caractérisées de manière fonctionnelle par la précision des mesures et aussi par leur vitesse. Quand l’engin se déplace vite, pour obtenir les positions en temps réel, il faut que les capteurs soient rapides ;

  • pour l’indoor, quand les signaux GPS ne sont pas captés, la détermination de la position du centre de gravité peut être déterminée par triangulation sur des bornes Wi-Fi ou encore par vision numérique. Dans ce dernier cas, le robot se repère par exploitation d’images issues d’une, voire plusieurs caméras. Si l’environnement est connu, la précision du positionnement peut être très efficace.

Estimez la position d'un robot par odométrie

Nous allons montrer comment, en mesurant le nombre de tours que les roues gauche et droite ont effectués, on parvient à déterminer coordonnées x et y du centre de gravité ainsi que le lacet. C’est le principe de l’odométrie. Il repose sur la mesure individuelle des déplacements de chaque roue pour reconstruire le mouvement global du robot. En partant d'une position initiale connue et en intégrant les déplacements mesurés, on peut ainsi calculer à chaque instant la position courante du véhicule.

Puis dans la section suivante, nous étudierons le code Arduino qui en résulte.

Notre robot vire sur le différentiel entre roue gauche et roue droite. Nous commençons par écrire un modèle de déplacement, c’est-à-dire les équations qui mettent en relation les grandeurs suivantes :

Nous noterons :

  •  d_g  et d_d , les déplacements respectifs des roues gauche et droite ;

  •  vg  et vd , les vitesses respectives des roues gauche et droite ;

  • x, y et θ , respectivement coordonnées dans le plan et lacet du robot ;

  • d, le déplacement du robot ;

  • v, la vitesse du robot ;

  • e, l'écart entre les deux roues.

Nous faisons pour l’instant  l’hypothèse que la trajectoire du robot est un cercle de rayon R parcouru à la vitesse angulaire ω=dθ/dt. Si le cercle est parcouru dans le sens trigonométrique, alors R>0 et on peut écrire :

v=Rdθ/dt

On peut ainsi déterminer les vitesses des roues, puisque les rayons des trajectoires dépendent des positions des roues situées respectivement à R-e/2 et R+e/2.

vg=(Re/2)dθ/dt=(Re/2)v/R

vd=(R+e/2)dθ/dt=(R+e/2)v/R

Ces relations donnent les vitesses des roues gauche et droite en fonction de la vitesse du centre de gravité du robot et du rayon de sa trajectoire. En fait, nous voulons inverser ces équations pour obtenir vitesse du centre de gravité et rayon de sa trajectoire en fonction des deux vitesses de roue, puisque nous les avons par les capteurs de roues. Par inversion du système, on trouve :

v=(vg+vd)/2

R=e2vd+vgvdvg

On constate que par la seule mesure de vitesse des roues, on peut remonter à la vitesse du centre de gravité du robot et au rayon de courbure local de sa trajectoire. On peut commenter un peu ces résultats pour vérifier qu’ils sont raisonnables. Si les deux vitesses de roue sont égales, il est aisé d’en déduire que le robot roule droit, sans virer, et que la vitesse du robot est aussi celle commune de ses deux roues, ce qui est vérifié sur les équations avec notamment un rayon de courbure infini, qui correspond bien à une droite.

Il reste à convertir les relations précédentes en position par intégration de la vitesse. Nous obtenons ainsi les équations de rafraîchissement des coordonnées et du lacet, qui seront ensuite traduites en code dans la section suivante :

Implémentez l’odométrie par le code

Pour implémenter l'odométrie, on utilise la technique d'un sketch à 4 fichiers. Tous les codes de ces 4 fichiers figurent dans des zones de code ci-dessous. Ces codes sont abondamment commentés.

Le premier code correspond au programme principal dans lequel on trouve le void loop, c'est à dire la boucle principale.

/* Ce programme présente les bases du robot
déjà développées dans les chapitres précédents.
Dans le code ci-dessous, on a ajouté le calcul de la trajectoire moyennant deux méthodes, odométrie et distance parcourue par chaque roue par multiplication de la vitesse des roues par un temps de fonctionnement, cette deuxième méthode étant en l'occurence plus précise.
*/
/*********************************************************************/
// bibliothèque de fonctions pour la gestion des servomoteurs
#include <Servo.h>
// bibliotheque de la liaison série software
#include <SoftwareSerial.h>
#define RxD 4 // Pin 4 pour la reception dans l'arduino
#define TxD 5 //Pin 5 pour l'émission de l'arduino
SoftwareSerial BTSerie(RxD,TxD); // affectation des broches de communication de la liaison bluetooth
// mesures sur les broches 2 et 3 pour les capteurs d'odométrie
const int capteurG = 2; // capteur odométrique gauche
const int capteurD = 3; // capteur odométrique droit
int distanceD = 0; // distance parcourue par la roue droite
int distanceG = 0; // distance parcourue par la roue gauche
byte etatD; // état du capteur de roue droit
byte etatG; // état du capteur de roue gauche
// pour le calcul des distances nous devons connaitre le mouvement du robot
char action=0;
char commande = 0; // variable de mouvement
// position du robot par la méthode temporelle
float x = 20;
float y = 15;
float a = 0; // position X, Y, angle
float delta; // incrément de déplacement
float deltaAngle; // incrément de rotation
float e = 5.5; // entraxe des roues 2e=11 cm
// position du robot par la méthode odométrique
float x0 = 20;
float y0 = 15;
float a0 = 0; // position X, Y, angle
float delta0; // incrément de déplacement
float deltaAngle0; // incrément de rotation
// mesure du temps pour faire une mesure de distance à interval de temps régulier
unsigned long debut=0;
unsigned long fin=0;
int duree=0;
// pour les roues
Servo roueG, roueD; // définit le nom des deux servos
Servo servoT; // nom de la tourelle
int posG, posD; // position des servos
const int pinG = 6; // broche de la roue gauche
const int pinD = 7; // broche de la roue droite
const int vmax = 10; // vitesse maximale des servos
const int repos = 90; // position de repos des servos
// pour la tourelle
const int pinT = 8; // broche de la tourelle
const int droit = 180; // position de la tourelle
const int gauche = 0;
const int face = 90;
const int telemetre = 0; // broche analogique du capteur de distance
// pour le debug
const byte led = 13; // declaration de la led interne
volatile byte state = LOW; // etat de la led
/*********************************************************************/
// sous programme d'initialisation et il n'est exécuté qu'une fois
void setup() {
roueG.attach(pinG); // déclaration des broches des roues
roueD.attach(pinD);
servoT.attach(pinT); // déclaration de la broche de la tourelle
Serial.begin(9600); // initialise la liaison série à 9600 bauds;
while (!Serial); // attend l'ouverture du la fenêtre terminal
// initialisation de la la liaison Bluetooth
pinMode(RxD, INPUT); // place la broche RX en entrée
pinMode(TxD, OUTPUT); // place la broche TX en sortie
BTSerie.begin(9600); // déclare la vitesse de communication
pinMode(led, OUTPUT);
pinMode(capteurD, INPUT_PULLUP);
pinMode(capteurG, INPUT_PULLUP);
etatD = digitalRead(capteurD);
etatG = digitalRead(capteurG);
//initialisation des deplacements
delta = 0.52; // vitesse du robot
deltaAngle = delta / (2*e);
delta0 = 0.70; // incrément odométrie
deltaAngle0 = delta0 / (2*e);
arreter(); // place le robot à l'arret
}
/*********************************************************************/
// boucle infinie
void loop() {
commande=decodeCommande();
if (commande=='U') avancer();
if (commande=='X') arreter();
if (commande=='D') reculer();
if (commande=='R') tournerD();
if (commande=='L') tournerG();
if (commande=='M') resetDistance();
detecteur(); // odometrie par les roues
fin = millis(); // odometrie par le temps
duree = fin - debut;
if (duree >50){
debut = fin;
mesuredistance();
afficheDistance();
}
}
/*********************************************************************/
void resetDistance (){
x=20;
x=20;
y=15;
a=0;
distanceD=0;
distanceG=0;
x0=20;
y0=15;
a0=0;
}
// routine de debug
// la liaison série virtuelle ne permet pas la transmission de beaucoup de données
// sans perturber le reste du montage.
// nous avons changé le branchement entre le module bluetooth et l'arduino.
// nous utilisons la broche 1 (tx de l'arduino) au lieu de 5 (tx soft).
// ainsi le terminal et le module bluetooth recupèrent les doonées d'odométrie
void afficheDistance(){
if (action !='X') {
Serial.print('*');
Serial.print('H');
Serial.print('X');
Serial.print(x);
Serial.print('Y');
Serial.print(y);
/* Serial.print(','); // permet de tracer la trajectoire mesurée par les roues
Serial.print('X');
Serial.print(x0);
Serial.print('Y');
Serial.print(y0);
*/ Serial.print('*');
}
}
void mesureG(){
if (action=='D')
distanceG = distanceG - 1;
else
distanceG = distanceG + 1;
}

Le code qui suit correspond à la gestion de la liaison Bluetooth et a déjà été commenté dans la partie relative à la liaison Bluetooth :

/* sous-programme de gestion de la liaison Bluetooth
*
*/
// programme écho renvoie sur le liaison Bluetooth les signaux de la console et inversement
void echo(){
char recvChar;
//On lit caractère par caractère sur le BTSerie et on affiche sur le Terminal Série
if (BTSerie.available()) {
recvChar = BTSerie.read();
Serial.write(recvChar);
}
//On lit caractère par caractère sur le terminal Série et on affiche sur le BTSerie
if (Serial.available()) {
recvChar = Serial.read();
BTSerie.write(recvChar);
}
}
//Décode les touches envoyées par la tablette
char decodeCommande(){
char recvChar=0;
//On lit caractère par caractère sur le BTSerie et on affiche sur le Terminal Série
if (BTSerie.available()) {
recvChar = BTSerie.read();
}
return recvChar;
}
// envoie distance
void dessine(){
// if (action != "X") {
BTSerie.print('*');
BTSerie.print('H');
BTSerie.print('X');
BTSerie.print(x);
BTSerie.print('Y');
BTSerie.print(y);
BTSerie.print('*');
// }
}

À présent, nous définissons le code des mouvements qui sera le troisième fichier du sketch courant :

//////////////////////////////////
//déclaration des sous routine de mouvement
/////////////////////////////////
void mouvement(int posD, int posG){
roueG.write(posG); // indique au servo de rejoindre sa position
roueD.write(posD);
}
void arreter(){
mouvement(repos,repos);
action = 'X';
}
void accelerer(){
for (int i=0; i<vmax; i++){ // augmentation prograssive de la vitesse
posG = repos + i;
posD = repos - i;
mouvement(posD,posG);
delay(20); // vitesse de la rampe d'accélération
}
action = 'U';
}
void freiner(){
for (int i=vmax; i>0; i--){ // reduction prograssive de la vitesse
posG = repos + i;
posD = repos - i;
mouvement(posD,posG);
delay(20); // vitesse de la rampe de freinage
}
action = 'U';
}
void avancer(){
posG = repos + vmax;
posD = repos - vmax;
mouvement(posD,posG);
action = 'U';
}
void reculer(){
posG = repos - vmax;
posD = repos + vmax;
mouvement(posD,posG);
action = 'D';
}
void tournerD(){
posG = repos + vmax;
posD = repos;
mouvement(posD,posG);
action = 'R';
}
void tournerG(){
posG = repos;
posD = repos - vmax;
mouvement(posD,posG);
action ='L';
}

Enfin, le dernier bout de code, le quatrième onglet du sketch est donné ci-dessous :

/*
* nous utilisons la technique des arcs de cercle pour calculer la tractoire du robot.
* Il y a 4 mouvements : avancer, reculer, tourner à droite et tourner à gauche.
* Le robot sera modélisé par le centre de l’axe des roues : C
* Le point D représente la roue Droite et G la roue Gauche. la distance entre les roues est 2e.
* Ainsi nous repérons le robot dans la pièce par 3 variables : x, y les coordonnées de C
* et a l’angle en l’axe des X et la perpendiculaire à DG.
* Les points ont donc pour coordonné
* C (x,y)
* G (x - e * sin (a), y + e * cos (a))
* D (x + e * sin (a), y - e * cos (a))
* Pour une translation Avant d’une distance l à partir du point xd, yd, nous pouvons calculer les coordonnées d’arrivée par :
* xf = l * cos ad + xd
* yf = l * sin ad + yd
* Et l’angle ne change pas
* af = ad
* Pour un mouvement arrière, les équations sont les même mais l est négatif.
* Lorsque le robot tourne à droite. Le point D ne bouge pas et les points G et décrivent des arcs de cercle de centre D
* Avec l’odomètre nous mesurons la distance parcourue par la roue mobile. : l = 2 * e * ar.
* ar est l’angle de rotation du robot. Le robot tourne dans le sens horaire.
* le point C décrit un arc de cercle de rayon e. Le point D est le centre de la rotation donc il ne bouge pas.
* Calculons les nouvelles coordonnées de C :
* ar = l/(2 * e)
* af = ad - ar
* xf = x + e * sin (ad) - e * sin (ad - ar)
* yf = y - e * cos (ad) + e * cos (ad - ar)
* Pour une rotation à gauche. Cette fois c’est le point G qui ne bouge pas et le robot tourne dans le sens trigonométrique.
* ar = l / (2 * e)
* af = ad + ar
* xf = x - e * sin (ad) + e * sin (ad + ar)
* yf = y + e * cos (ad) - e * cos (ad + ar)
*/
void mesuredistance(){
if (action =='U'){
x=x+delta*cos(a);
y=y+delta*sin(a);
}
if (action =='D'){
x=x-delta*cos(a);
y=y-delta*sin(a);
}
if (action =='R'){
x = x + e * sin (a) - e * sin (a - deltaAngle);
y = y - e * cos (a) + e * cos (a - deltaAngle);
a = a - deltaAngle;
}
if (action =='L'){
x = x - e * sin (a) + e * sin (a + deltaAngle);
y = y + e * cos (a) - e * cos (a + deltaAngle);
a = a + deltaAngle;
}
}
/* mesure avec les capteurs sur les roues*/
void detecteur(){
byte tempo;
tempo = digitalRead(capteurD);
if (etatD != tempo) { // s'il y a un changement d'état du capteur
etatD = tempo; // nouvelle position
if (action =='U'){
x0=x0+delta0*cos(a0);
y0=y0+delta0*sin(a0);
}
if (action =='D'){
x0=x0-delta0*cos(a0);
y0=y0-delta0*sin(a0);
}
if (action =='L'){
x0 = x0 - e * sin (a0) + e * sin (a0 + deltaAngle0);
y0 = y0 + e * cos (a0) - e * cos (a0 + deltaAngle0);
a0 = a0 + deltaAngle0;
}
}
tempo = digitalRead(capteurG);
if (etatG != tempo) { // s'il y a un changement d'état du capteur
etatG = tempo; // nouvelle position
if (action =='R'){
x0 = x0 + e * sin (a0) - e * sin (a0 - deltaAngle0);
y0 = y0 - e * cos (a0) + e * cos (a0 - deltaAngle0);
a0 = a0 - deltaAngle0;
}
}
}

En résumé

Nous disposons à présent de la capacité d'estimer en temps réel la position du robot par odométrie. Dès lors, il devient possible en combinant avec la détection d'obstacles, de cartographier son environnement. Dans le chapitre suivant, nous allons voir comment tracer la trajectoire du robot sur le dispositif Android.

Exemple de certificat de réussite
Exemple de certificat de réussite