Implémentation de l'odométrie dans le FPGA

FIXME Recherche des images en cours…

On appelle odométrie le système qui permet au robot de se situer sur le terrain. Le plan de ce document est le suivant :

1. Considérations mathématiques

2. Implémentation dans le FPGA

Considérations mathématiques

Le robot dispose des informations données par les roues codeuses, à partir des signaux en quadrature de ces dernières le robot doit pouvoir calculer sa position. Nous allons traiter le changement de coordonnées induit par un seul pas d'une seule roue codeuse, puisque cette opération sera ensuite la même à chaque pas.

robot.png

Le robot est repéré sur le terrain par le système de coordonnées (X, Y, θ). Après un pas élémentaire les nouvelles coordonnées seront (X’, Y’, θ') avec :

X’ = X + dX

Y = Y + dY

θ' = θ + dθ

Avec le paramétrage de la figure ci-contre on pose P le pas élémentaire d’une roue codeuse et α l’angle élémentaire d’un pas de roue codeuse, on a α = P/D.

Lors d’un pas élémentaire on considère que le robot s’est déplacé en rotation autour de la roue fixe. Ainsi le fameux théorème de Talès nous indique que le centre du robot s’est déplacé de P/2 dans la direction de θ + α/2 (le choix d’ajouter α/2 permet de diminuer la dérive de moitié sans trop complexifier le calcul).

Si on distingue les pas de la roue codeuse droite (Pr, αr) et ceux de la roue codeuse gauche (Pl, αl) le résumé des transformations est le suivant :

Pas roue droite sens avant Pas roue droite sens arrière Pas roue gauche sens avant Pas roue gauche sens arrière
dX Pr/2*cos(θ + αr/2) -Pr/2*cos(θ - αr/2) Pl/2*cos(θ + αl/2) -Pl/2*cos(θ - αl/2)
dY Pr/2*sin(θ + αr/2) -Pr/2*sin(θ - αr/2) -Pl/2*sin(θ + αl/2) l/2*sin(θ - αl/2)td
dq αr r l αl

repere.png

La dérive théorique de cette méthode de calcul est de P/(D-P) soit dans notre cas avec un codeur en quadrature 4096imp/tr , r = 1.5cm, P = 2*Pi*r/4096 et D =32.4cm on a une dérive de 5.7*10-5% soit 0.6mm tous les 10m…

Implémentations dans le FPGA avec Quartus II

Pour faire les calculs d’odométrie dans le FPGA il n'est pas intéressant de faire des calculs en virgule flottante car le concept de virgule flottante n’est pas adapté aux FPGA (pour un certain nombre de considérations qu’on ne détaillera pas ici).

Ainsi les calculs se feront en virgule fixe, il faut alors se choisir une représentation de la virgule fixe. On va se prendre des calculs dans le FPGA sur 32bits (bon compromis précision/taille/compatibilité), maintenant où vas t'on placer cette fichue virgule ?

Alors voyons ce que l’on va être amener à calculer:

  • les angles feront entre –π et π soit entre -3.5 et 3.5
  • le terrain lui fait pour sa plus grande dimension 3,1 m

On va donc se prendre 1 bit de signe et 2 bits avant la virgule, on pourra donc avoir des données entre -4 et 4 (avec les chiffres derrière la virgule en plus), on a donc 29 bits restants pour décrire ce qu’il y a derrière la virgule ce qui donne une précision de environ 10-8 (log10(229) = 8.7) ce qui est très largement suffisant pour ce que l’on veut faire.

Bon ceci étant fait on est pas encore tiré d’affaire car pour calculer des sinus et cosinus dans le FPGA il ne suffit pas d’appeler la librairie Math.h. Un méthode parmi d'autre consiste à utiliser l’algorithme CORDIC, ce dernier est une manière très astucieuse et très performante de calculer des cosinus et sinus en virgule fixe en quelques itérations. Pour ne pas réinventer la roue, j’ai pris le code tout fait sur internet: http://www.ht-lab.com/freecores/cordic/cordic.html.

En testant ce code pour voir comment il fonctionnait j’ai découvert qu'il n’était capable de faire des calcul de cos et sin que sur l’intervalle [ -π/2 ;π/2] il m’a donc fallu coder des blocs PreCalc et PostCalc pour effectuer les réduction d’argument et les adaptation de signe qui vont avec (en accord avec les fameuses formules de trigo de lycée). Autour de ce bloc SinCos perso il m’a fallu quelques autres blocs de maths comme le bloc Mult, AddSub et Sign (faisant respectivement des multiplications en virgule fixe, des additions ou soustractions et des changements de signe). Voilà pour la partie math du code.

Ensuite il m’a fallu un système pour synchroniser tous ces calculs, pour mémoriser les coordonnées et les mettre à la disposition de la M32 (processeur responsable de toutes les tâches de plus haut niveau). Pour synchroniser le tout rien de tel que de faire un bloc dédié au lancement des calculs et un système de bit Start/Done sur chaque opérations pour savoir si le calcul peut être effectué (donnée disponibles sur les port d'entré) et quand le calcul est fait (résultat disponible sur le port de sortie).

Pour la mémorisation et la communication avec la M32 il m’a semblé plus pratique de faire tout dans un même bloc puisque ces 2 fonctionnalités vont jouer avec les mêmes variables. Du point de vue de la M32 le FPGA se comporte comme un espace mémoire externe, il peut lire et écrire à des adresses correspondant à des blocs du FPGA, les données auxquelles la M32 accède librement sont actualisées par les calculs odométriques à tout moment. Enfin un bloc Constante permet à la M32 de modifier les constantes du calcul, à savoir αr/2, αl/2, Pr/2 et Pl/2.

Cliquer sur l'image pour l'agrandir

odometrie.jpg

Article écrit par SAVOYAT Marc-Antoine

16 juin 2010