-34- this file and other auxiliary files require to use the following -34- LaTeX packages: frenchle! -34- check or remove these files.Typesetting is aborted!
On présente ici la couche mathématique qui permet au robot de connaître en permanence sa position.
Reprenons d'abord l'architecture matérielle et logicielle qui
implémente l'asservissement et le positionnement.
|
|
L'asservissement est réalisé par les 2 LM629 qui décodent et
comptent les impulsions en provenance des roues codeuses. A partir
d'une roues codeuses délivrant 500 impulsions par tour sur 2 voies
en quadrature, le LM compte 2000 impulsions. Le DSP vient
périodiquement demander aux LM leurs positions qui correspondent
au nombre d'impulsions comptées sur chaque roue. A l'origine,
cette opération de polling était réalisée toutes les 10
millisecondes et elle est maintenant faite toutes les
millisecondes 1.
Voyons maintenant le repère absolu qui a été utilisé.
|
|
Le choix de ce repère présentait dans notre cas plusieurs avantages :
Les impulsions qui sont comptées par le LM ne correspond à aucune
réalité physique. Or, pour faciliter la mise en oeuvre et la
compréhension du système, il est pratique d'utiliser en haut
niveau des unités courantes (m, cm, mmm...). De plus, on a
constaté une dissymétrie en les deux roues codeuses et on a du
introduire un facteur correctif. là aussi, il est plus convivial
de pas avoir à se soucier de ce facteur en haut niveau.
D'autre part, le cablage et le montage des moteurs et des roues
codeuses peuvent faire que les signes soient plus ou moins
n'importe quoi. Or, on aimerait bien lorsqu'on dit aux deux roues
d'avancer de 10 cm, qu'elles avancent (Je dis bien avancent et non
reculent) de 10 cm. Ce qui est loin d'être gagné au départ.
C'est pourquoi, on a implémenté dans le DSP une sorte de couche bas niveau qui permet de convertir les données brutes en informations classiques et vice-versa. Les fonctions ou macros ainsi écrites permettent d'accéder à l'asservissement sans trop se soucier des conversions d'unités une fois que tout les paramètres de la couche basse sont bien réglés.
La seule unité de distance que le LM connaisse est ce que nous
allons appelé le count. Ainsi avec une roue codeuse de 500 points,
le LM s'incrémente de 2000 counts en un tour de roue. En haut
niveau, on souhaite n'utiliser que des unités classiques ; le
millimètre a été retenu bien qu'au final le mètre eu été
préférable.
Notons N, le nombre de points que possèdent une roue codeuse. Le
LM est alors capable de décoder 4 ×N counts sur cette roue
codeuse. Donc un déplacement de 4N counts correspond à un
déplacement effective de la roue d'une distance équivalente à sa
circonférence qui vaut pD où D représente le diamètre de la
roue codeuse.
La constante qui est utilisée dans le DSP pour les conversions de
distances est COUNT_PER_MM qui représente le nombre de
counts produits par le LM lorsqu'une roue codeuse avance d'un
millimètre. Son unité est donc le count/mm. Pour déterminer cette
constante, il suffit de faire avancer le robot de 2 m par exemple,
de regarder le nombre de counts produits et d'en déduire la valeur
du COUNT_PER_MM. Cette valeur est commune aux deux roues
codeuses et correspond à la moyenne des valeurs de cette
constantes pour chaque roue.
Comme on l'a déjà dit et répété, il y a une dissymétrie entre les
circonférences des deux roues codeuses et c'est pour cette raison
que l'on a introduit le facteur de correction SYM_LM. Une
valeur positive de cette constante permet de corriger une dérive
du robot verts la droite. Pour être ajustée, cette valeur
nécessite beaucoup de tests. L'une des techniques les plus fiables
consiste à faire effectuer au robot une trajectoire rectangulaire
qui suit des lignes. Si au bout d'un certain nombre de tour le
robot coupe les lignes de la gauche vers la droite, alors il faut
augmenter la valeur du SYM_LM.
Ces deux conversions ont été regroupées en deux constantes :
COUNT_PER_MM_D = COUNT_PER_MM*(1-SYM_LM) COUNT_PER_MM_G = COUNT_PER_MM*(1+SYM_LM)Ainsi, que l'on veut envoyer une distance au LM il suffit de la multiplier par l'une de ces valeurs. Et inversement, quand on récupère une donnée du LM, il faut la diviser par l'une de ces constantes pour retrouver une valeur en mm.
ATTENTION : Toutes les valeurs qui sont transmises au LM sont des entiers alors que les calculs sont fait en virgule flottante sur le DSP et ceci pour des raisons de précision. Il na faut donc pas oublier de les convertir avant de les envoyés.
La seul unité de vitesse que le LM connaisse est le count/sample.
Curieuse unité me direz-vous alors essayons d'éclaircir la chose.
Le count, on a déjà vu avec les distances maintenant qu'est que le
sample. le sample est l'unité de temps du LM, elle correspond à la
période de la boucle d'asservissement qui est de 2048/fCLK où
fCLK est la fréquence de l'oscillateur qui pilote le LM.
Donc, pour un oscillateur à 8 MHz, un sample correspond à
256 ms. Dans le code DSP, cette constante a été appelée
SAMPLING_PERIOD et son unité est la seconde/sample.
Mais la ruse du LM ne s'arrête pas là. En fait, lorsque l'on
charge une vitesse dans le LM, on l'envoie sur 32 bits. Or, seuls
les 16 bits de poids forts correspondent à des counts/sample alors
que les 16 bits de poids faible correspondent à la partie
décimale.
Si on dispose d'une vitesse v_DSP en cm/s que l'on souhaite l'envoyer au LM droit sur 32 bits dans v_LM, il faut appliquer la formule suivante pour la convertir :
v_LM = v_DSP * 10 * COUNT_PER_MM_D * SAMPLING_PERIOD * 65536
Si on récupère une vitesse du LM sur 32 bits et que l'on veut la convertir en cm/s, on appliquera :
v_DSP = v_LM / (10 * COUNT_PER_MM_D * SAMPLING_PERIOD * 65536)
ATTENTION : Les vitesses envoyées au LM sont sur 32 bits et certaines de celles que l'on peut récupérer sont sur 16 bits seulement. Dans ce cas, il ne faut pas oublié de décaler de 16 bits vers la droite la valeur récupérée. Lorsque l'on, ne récupère que 16 bits, il s'agit des 16 bits de poids forts dont l'unité est le count/sample.
Comme pour la vitesse, le LM ne connaît que des accélérations en
counts/sample/sample. Et là aussi les 16 bits de poids forts
correspondent à la entière et les 16 bits de poids faible à la
valeur décimale.
Si on dispose d'une accélération a_DSP en cm/s2 que l'on souhaite l'envoyer au LM droit sur 32 bits dans a_LM, il faut appliquer la formule suivante pour la convertir :
a_LM = a_DSP * 10 * COUNT_PER_MM_D * SAMPLING_PERIOD * SAMPLING_PERIOD * 65536
Si on récupère une vitesse du LM sur 32 bits et que l'on veut la convertir en cm/s, on appliquera :
v_DSP = v_LM / (10 * COUNT_PER_MM_D * SAMPLING_PERIOD * SAMPLING_PERIOD * 65536)
ATTENTION : Là aussi le LM renvoie dans certains cas les accélérations sur 16 bits.
Ces routines de conversion bas niveau permettent de s'abstraire légèrement du matériel dans le code de traitement. Cependant, nous avons commencé un peu tard à vouloir faire une coche de conversion bas niveau si bien qu'elle n'est pas vraiment opérationnelle et que l'on retrouve souvent les formules de conversion au milieu du code. Ces conversions gagneraient sans doute à être intégrées dans des routines bas niveau d'accès au LM.
Le positionnement du robot est obtenu par odométrie, c'est à dire
que la position est obtenue par intégration des déplacements.
L'intérêt de l'odométrie est qu'elle est assez simple à mettre en
oeuvre et qu'elle est fiable. Par contre, quand on intègre les
déplacements, on intègre aussi l'erreur ce qui fait que l'erreur
de position croît avec le temps.
Voyons maintenant comment on réalise concrètement ce
positionnement. Comme je l'ai dit, le DSP vient lire régulièrement
(toutes les 1/10 ms) la position de chaque roue codeuse. Entre
deux lectures, on peut savoir de combien s'est déplacée chaque
roue codeuse et il faut à partir de cela en déduire la position du
robot.
|
|
Il y a en fait deux manières d'approximer la trajectoire élémentaire parcourue par le robot pendant ce temps Te :
Sur le robot, il n'y a que l'approximation par des segments qui a
été implémentée et elle donnait de bons résultats. Par contre,
l'approximation par des arcs de cercle a été réalisée en
simulations sous MATLAB et elle n'a pas fait apparaître
de différences notoires lorsque la fréquence d'échantillonnage
1/Te reste suffisamment élevée. Avec les valeurs utilisées sur
le robot, on ne voit absolument aucune différence.
On présentera quand même ici les deux méthodes pour voir en quoi elles différent et pourquoi elles convergent lorsque Te devient suffisamment petit. Par contre, il faut garder à l'esprit qu'aucune de ces méthodes n'est absolument exacte.
Appelons Dd et Dg les distances (en mm) parcourues
respectivement par les roues droites et gauches entre deux
lectures des LM soit un intervalle de temps Te. Connaissant la
pose du robot à l'instant n-1, on cherche la pose à l'instant
n. On emploie volontairement ici le terme "pose" en accord avec
de nombreuses publications. En effet, la pose représente à la fois
la position du robot (x,y) et son orientation q. En
général, on utilise dans la littérature le terme "position" pour
le doublet (x,y), le terme "pose" pour la triplet (x,y,q)
et le terme "posture" (x,y,q,k) où k représente
le courbure. Il faut remarquer que le terme position est souvent
utilisé abusivement 3 pour désigner un état du robot qui peut intégrer bien
d'autres paramètres que sa position (x,y).
|
|
On a donc :
Dmoyn = [(Ddn + Dgn)/2]
Ddifn = Ddn - Dgn
Dxn = Dmoyn cosqn
Dyn = Dmoyn sinqn
Dqn = [(2 Ddifn)/L]
En effet, lorsque le robot fait un tour sur lui-même (2p radians), la différence de parcours entre les deux roues est de 2L. Ce qui donne une différence de L/2 pour une rotation de 1 radian. Il s'agit là d'ailleurs de la définition du radian. Cette constante L/2 a été appelée MM_PER_RADIAN dans le code DSP.
Ainsi,
xn = xn-1 + Dxn
yn = yn-1 + Dyn
qn = qn-1 + Dqn
Or, l'orientation du robot doit toujours rester dans l'intervalle
[-p,+p]. Ce choix est totalement arbitraire mais il nous est
apparu comme étant le plus pertinent et le plus facile à utiliser;
on aurait pu choisir [0,2p]. On a donc du écrire une fonction
réalisant un modulo dont le résultat est toujours dans
[-p,+p], cette fonction nommée fmod_pmpi peut être
trouvée dans math.c. C'est
pourquoi on trouvera dans le code :
pos.angle = fmod_pmpi(pos.angle + (diff_d - diff_g)/MM_PER_RADIAN);
Le calcul et le stockage de la position sont désormais effectués sur des float. En effet, lors des premiers tests réalisés sur le portable, nous utilisions des float pour le calcul et des nombres entiers pour le stockage de la position en dixièmes de millimètres. Ce type de stockage engendrait une perte de précision non négligeable. C'est pourquoi nous réalisons maintenant tous les calculs en float ce qui apporte une souplesse d'utilisation importante et surtout ne prend pas plus de temps de calcul sur le DSP.
En reprenant les mêmes notations que précédemment, on obtient la figure suivante :
|
|
On a toujours :
Dmoyn = [(Ddn + Dgn)/2]
Ddifn = Ddn - Dgn
Dqn = [(2 Ddifn)/L]
Si on tourne vers la droite (Ddn > Dgn) :
Ddn = Dqn ×(R+L/2)
Dgn = Dqn ×(R-L/2)
par contre, si on tourne vers la gauche (Ddn < Dgn) :
Ddn = Dqn ×(R-L/2)
Dgn = Dqn ×(R+L/2)
On en tire :
R = L[(Dmoy)/(Ddif)]
Dxn = R [cos(qn-1+p/2) + cos(qn-p/2)]
Dyn = R [sin(qn-1+p/2) + sin(qn-p/2)]
ou encore,
Dxn = R [sin(qn) - sin(qn-1)]
Dyn = R [cos(qn-1) + cos(qn)]
Il faut bien voir qu'il est nécessaire de prévoir un cas spécial
lorsque les deux roues parcourent exactement la même distance.
Dans ce cas, l'arc de cercle se transforme en segment ce qui
correspond à un cercle de rayon infini. Bien sûr, on ne peut pas
faire de calcul avec une valeur infinie, c'est pourquoi, il faut
prévoir un cas spécial.
Ce calcul doit absolument être effectué en virgule flottante (float) car il nécessite une dynamique très importante : rayon très grand et déplacement des roues très petits.
Or,
qn = qn-1 + Dqn
Ce qui donne après développement :
Dxn = R [sin(qn-1)cos(Dqn) + cos(qn-1)sin(Dqn) + sin(qn-1)
Dyn = R [sin(qn-1)sin(Dqn) - cos(qn-1)cos(Dqn) + cos(qn-1)
On retombe alors sur l'approximation par des segments de droites
si on suppose que Dqn est très petit. En effet,
dans ce cas :
cos(Dqn) » 1
sin(Dqn) » 0
ce qui donne :
Dxn = R [sin(qn-1) + sin(qn-1)
Dyn = R [-cos(qn-1) + cos(qn-1)
L'approximation Dqn très petit est d'autant plus justifiée que la fréquence d'échantillonnage 1/Te est très grande et que l'angle varie très peu entre deux estimations de la position 4. Ceci explique donc que les deux méthodes d'approximations convergent lorsque l'on diminue la période d'échantillonnage.
On va maintenant s'intéresser à la génération de trajectoires,
c'est à dire comment faire pour aller d'un point A à un point B.
On ne s'intéressera ici qu'à la génération de trajectoires
rectilignes qui a été utilisée pour la Coupe de France de
Robotique. Depuis cette date, la génération de trajectoires
courbes a été étudiée et une implémentation est disponible.
Cependant, comme cette deuxième technique est beaucoup plus
complexe que la première, elle fera l'objet d'un autre document.
On souhaite donc faire passer le robot d'une pose de départ
(xn-1,yn-1,qn-1) à une position d'arrivée
(xn,yn). Pour l'arrivée, on ne spécifie pas d'orientation
puisqu'elle ne présentait pas d'intérêt dans notre cas. En effet,
il n'y a pas vraiment de cas où la spécification de l'angle à
l'arrivée apporte un plus. Si on souhaite arriver à un point avec
un angle précis 5
la carte principale doit utiliser un point intermédiaire qui
permettra lors du déplacement vers le point désiré d'avoir la
bonne orientation.
|
|
Au départ comme à l'arrivée, la vitesse du robot est nulle. Cette contrainte est due aux LM629 qui utilisé en mode "Position" ne permettent de relier des configurations où la vitesse nulle. Pour passer d'une position à l'autre, on réalise un trajectoire composée de 2 mouvements :
Pour réaliser cette trajectoire, il suffit donc de calculer l'angle q duquel il faut tourner sur place et la distance d qu'il faut parcourir en ligne droite.
Pour la distance, la formule est assez simple :
|
Pour l'angle, ça n'est pas beaucoup plus compliqué :
|
Le fmod_pmpi externe permet de minimiser la rotation en ramenant toujours l'angle dans l'intervalle [-p;+p]. On évite ainsi au robot de tourner de 3p/2 au lieu de -p/2.
atan2 est une arc-tangente un peu particulière qui est implémentée dans la majorité des langages. Contrairement à l'arc-tangente classique qui est modulo p, cette version améliorée est modulo 2p et retourne en général 6 un angle compris entre -p et +p. En lui indiquant une ordonnée et une abscisse, cette fonction très pratique retrouve l'angle correspondant sur le cercle trigonométrique.
Voila, une fois que l'on a l'angle q et la distance d, il
ne reste plus qu'à envoyer des ordres aux LM. Il y aura en fait
deux trajectoires pour chaque LM.
La première trajectoire correspond à la rotation sur place. Il
suffit d'imposer au LM droit de se déplacer de q×MM_PER_RADIAN/2 et au LM gauche de se déplacer de
-q×MM_PER_RADIAN/2. A partir de cela, les
LM vont générer en interne des profils de vitesse qui leur
permette d'atteindre ces positions en respectant les contraintes
de vitesse
et d'accélération.
La seconde trajectoire correspond à la ligne droite et il suffit là d'envoyer la même consigne de position aux 2 LM en leur demandant de se déplacer de la distance d après avoir pris soin de la convertir en mm.
1Il n'y a pas de différences majeures entre ces deux périodes au niveau des résultats.
2Il faut éviter de choisir des repères ou des systèmes d'unités qui obligent à se torturer le cerveau pour leur donner une signification pratique
3Ce qui sera certainement le cas ici aussi
4De la posturepose devrais-je dire !
5Pour l'approche d'un panier par exemple
6Ca
dépend beaucoup des environnements utilisés. Le mieux c'est de
testé pour être sûr et éventuellement de rajouter un
fmod_pmpi si besoin est.
