Odométrie en C (robot e_puck)
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:
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.
