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.
Nous récupérons les informations en mm et l'angle en radian .
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); }
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.
Partager