Discussion :

[benji77fr]

Bonjour à tous

Actuellement , je suis en stage à mon IUT et je suis en train de plancher sur l'odométrie en C .

Je travail sous le programme Webots qui permet de simuler un environnement 3D avec des objets , des robots comme mon e_puck , et de coder en C notamment le programme qui contrôle le robot ( nommé respectivement controller sous webots).

J'ai réalisé une fonction pour l'odométrie dont voici le code.

Petits détails sur des variables #define :

le 0,1288 est obtenu par le calcul suivant : (2*pi*41)/1000
- 41 étant le diamètre de la roue en mm
- 1000 est le nombre de step par tour de la roue codeuse (20 points avec un rapport de 50)

AXLE_LENGTH est la distance entre les deux points de contact au sol des roues.

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 
 
double Xk = 0, Yk = 0, X=0,Y=0, dt=0; // Je déclare mes variables et les initialise
 
static void compute_odometry() 
{
  double l = wb_differential_wheels_get_left_encoder();// récupération du codeur gauche
  double r = wb_differential_wheels_get_right_encoder();// récupération du codeur droit
 
  wb_differential_wheels_set_encoders(-1*l,-1*r);// calibration des codeurs pour remise à zéro
 
  double dl = l * 0.1288; // Distance parcouru par la roue gauche / période échantillonnage
  double dr = r * 0.1288; // Distance parcouru par la roue droite / période échantillonnage
  double da = (dr - dl) // AXLE_LENGTH; // Angle de rotation élémentaire du robot 
  double dd = (dr + dl) / 2; // Distance moyenne élémentaire parcouru par le robot
 
  /*
  Pour ces deux calcules , je ne suis pas sûr de leur utilités.
  J'ai trouvé sur Wiki ceci pour calculer la position actuelle du robot
  et ensuite la mettre à jour avec les deux calcules qui suivent 
  */
 
  Xk = X+dd*cos(dt);
  Yk = Y+dd*sin(dt);
 
  // calcul de mise à jour de la position par récurrence 
  // Formule générale et simplifié d'après méthode de l'ingénieur
  X = Xk + dd * cos(dt+(da/2));
  Y = Yk + dd * sin(dt+(da/2));
 
  dt = dt + da;// calcule du cap à l'instant k + 1
 
  // j'affiche dans la console la position et le cap 
  printf("Position en X : %g \n",Xk);
  printf("Position en Y : %g \n",Yk);
  printf("Angle teta : %g \n",da);
}

Nous récupérons les informations en mm et l'angle en radian .

Après un test linéaire , par rapport à la longueur d'une feuille , on récupère un valeur de 900 qui ne correspond à rien .

Nous pensons que le problème réside dans le 0,1288, maintenant ai-je fais une erreur dans mon code, j'aimerai bien pouvoir appliqué l'odométrie à ce petit robot.

En espérant avoir été le plus claire possible dans ce post , je vous remercie pour toute aide apporté , il est vrai que ça s'apparente plus à un problème mathématique mais je me suis dis que certains on déjà du rencontrer ce problème en codant.

PS : je me suis servi de la méthode de Marie josé ALDON de LIRMM et de notre ami google bien entendu.

[Invité(e)]

Bonjour,

Est il possible de sortir l, r, Xk et Yk pour voir pas à pas comment évolue le mobile ?

[benji77fr]

pour l et r on peut visionner soit avec la remote du programme qui est une petite fenêtre indiquant l'état de tous les capteurs du robot dont la valeur des roues codeuse , soit en printf dans la console du programme.

Si j'ai bien compris ta question , tu me demande si je peux regarde l'évolution de Xk et Yk hors de ma fonction ?

Si c'est le cas on peut , on utilise juste le calcule de base dans le main, sans trigonométrie pour voir son avancement linéaire.

PS : la remote permet de switch entre le robot virtuel et le réel grâce à la sélection du canal bluetooth du robot réel. les résultats des capteurs sont parfaitement en adéquation avec la simulation .

[Invité(e)]

Si j'ai bien compris ta question , tu me demande si je peux regarde l'évolution de Xk et Yk hors de ma fonction ?

En fait, je voulais voir la tête de X et Y en fonction du déplacement du robot.

Cela dit, on peut s'en passer :
On suppose qu'on passe de la position 0 à la position 1 sur les deux roues et qu'on déroule le calcule, on trouve Xk = 2*0,1288 et Yk = 0.
En fait, j'ai l'impression que le déplacement est deux fois pris en compte, une fois dans le calcul de X et dans celui de Xk.

Mais, je pense qu'il y a aussi un problème dans le calcul de da.
En effet, dl et dr sont des distances, da un angle, il manque quelque chose pour passer de l'un à l'autre.

Combien faut-il de tours de roue (gauche en avant, droite en arrière) pour que le robot fasse un tour complet ?

[benji77fr]

Je ne sais pas du tout , je vais faire un test et je te dis ça une fois que j'ai fini.

EDIT : je viens de faire le test il faut 1415 tick de la roue codeuse pour faire un tour complet avec une vitesse de 250 .

[benji77fr]

petit up.

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
  double l = wb_differential_wheels_get_left_encoder();
  double r = wb_differential_wheels_get_right_encoder();
  wb_differential_wheels_set_encoders(0*l,0*r);
  double dl = l / ENCODER_RESOLUTION * WHEEL_RADIUS; // distance covered by left wheel in meter
  double dr = r / ENCODER_RESOLUTION * WHEEL_RADIUS; // distance covered by right wheel in meter
  //double dl = (l * 0.1288)/WHEEL_RADIUS; // distance covered by left wheel in meter
  //double dr = (r * 0.1288)/WHEEL_RADIUS; // distance covered by right wheel in meter
  double da = (dr - dl) / AXLE_LENGTH; // delta orientation  
  double dd = (dr + dl) / 2; // delta distance
  dt = dt + da;
 
  //Xk = X+dd*cos(da);
  //Yk = Y+dd*sin(da);
 
  Xk = Xk + dd * cos(dt+(da/2));
  Yk = Yk + dd * sin(dt+(da/2));

J'ai fait cette petite modification et j'ai lancer une simulation , le robot fonctionne en Braitenberg donc évite tout ce qu'il voit .

Ça fonctionne pas trop mal mais après plusieurs changement de cap les valeurs en X et Y deviennent complètement fausse .