Professional Documents
Culture Documents
Diplmant : Florent Gnter Section : microtechnique Professeur : Aude Billard Assistants : Sylvain Calinon et Andr Guignard
Florent Gnter
Projet de diplme
20.02.2004
Florent Gnter
Projet de diplme
20.02.2004
1 Introduction :
Le projet Robota consiste en une poupe robotise qui a t dveloppe dans le but de faire un jouet ducatif pour les enfants normaux ou handicaps. Ce projet se base sur la communication et lapprentissage par la vision et par les gestes. Une srie de poupes a dj t dveloppe avec des mouvements limits (pour en savoir plus, asl.epfl.ch sous la rubrique recherche). Le projet a t lanc daugmenter les possibilits de Robota en concevant un vritable petit robot humanode. La mcanique dun nouveau bras a t conue en projet de semestre lanne passe dans ce but. Il a cinq degrs de libert dans ltat actuel des choses, mais la main sera modifie pour en rajouter deux au niveau du poignet (projet de diplme de Lorenzo Piccardi). Le bras du premier projet a t construit pendant lt et il lui faut maintenant un moyen de contrle pour les quatre premiers degrs de libert. Le but de ce projet est donc de concevoir les circuits et le software ncessaires au contrle du bras et de le tester pour en amliorer les ventuels dfauts. Dans la suite de ce rapport, les degrs de libert (abrgs dgl) seront numrots selon la figure 1.
Florent Gnter
Projet de diplme
20.02.2004
Dans un systme mchatronique, linterfaage des multiples capteurs et actuateurs est un point critique. Cest dans le but de simplifier ces interfaces que le LSA (laboratoire de systmes autonomes) a dvelopp une srie de petits modules de fonctions diverses qui peuvent tre relis par I2C. Parmis ces diffrents modules, il y en existe un qui a t conu pour contrler un moteur DC (en position, en vitesse et en couple). Cest ce module qui a t choisi pour contrler le prototype du bras de Robota.
Ce module est principalement constitu dun microprocesseur PIC, qui gre le contrle du moteur, et de plusieurs autres sous-systmes annexes pour les mesures ou lalimentation du moteur. Le plus important de ces sous-systmes est le pont H qui sert alimenter le moteur. Il y a galement un petit systme de mesure du courant form dune rsistance faisant office de capteur et dun amplificateur. Un dcodeur pour lencodeur optique du moteur a galement t prvu pour donner la vitesse et la direction de rotation du moteur. Le dernier sous-systme du circuit est un chip qui
Florent Gnter
Projet de diplme
20.02.2004
donne le 5V ncessaire au fonctionnement de lamplificateur au niveau de la mesure du courant. Certain des sous-systmes de ce module moteur doivent tre adapts selon le type de moteurs. Le pont H, par exemple, peut varier suivant la puissance du moteur que lon veut contrler. La rsistance pour la mesure du courant doit aussi tre adapte en fonction du moteur. Des modifications ont donc t apportes au schma dorigine.
2.1 Le pont H
Le pont H est un lment qui sert alimenter un moteur de faon ce quon puisse changer le sens de rotation de ce dernier.
Figure 3 : Schma dun pont H. Avec ce schma, on voit bien de quelle faon cela se fait. Si on veut faire tourner le moteur dans un sens, on ouvre les transistors Q1 et Q4 et on ferme les deux autres, le courant circule ainsi de gauche droite dans le moteur du schma de la figure 3. A loppos, si lon ferme Q1 et Q4 et que lon ouvre Q2 et Q3, le moteur tournera dans lautre sens. Pour synchroniser les transistors, il faut un circuit logique dans le chip. En dehors de la puissance quest capable de fournir le pont H, cest aussi les possibilits annexes offertes par le chip qui vont jouer un rle dans le choix de ce dernier (par exemple, la limitation du courant).
Les moteurs qui sont utiliss dans le prototype du bras de Robota pour les quatre dgl de lpaule jusquau coude, sont tous pareils sauf un. Trois dentre eux ont un courant maximum (moteur bloqu) de 330mA donn par le fabricant et le dernier a un courant maximum de 1,2 A. Malheureusement, le march des ponts H intgr est relativement rduit. Les ponts H intressants sont les suivants :
Florent Gnter
Projet de diplme
20.02.2004
Numro
Fabriquant
Dimensions Tension (mm) dalimentation (V) 5-50 7.5x10x2.5 20-50 9.5-50 3.8-13.2 12-60 7.5x15x2.5 7.5x15x2.5 4x5x1.5 13x31x5
Images
MPC17510 MC33887
1 5
2-15 5-40
6.5x8x1.5 10x16x3.5
La caractristique prioritaire, une fois que la liste des ponts H a t tablie, est le volume minimum. Le plus petit des ponts H de la liste est le Vishay SI9986. Cest en fait une bonne chose, car il a dj t implment dans le programme de base du module moteur et a dj t test. Malheureusement, il supporte 1A au maximum et est assez sensible aux courants trop grands (expriences faites par Alessandro Crespi). De plus, il ne possde pas de systme de limitation de courant qui pourrait lempcher de griller en cas de dpassement des limites.
Florent Gnter
Projet de diplme
20.02.2004
Comme les circuits doivent tre intgrs directement dans le bras pour ne pas avoir trop de files dans les articulations, sachant que les deux moteurs qui sont dans le haut du bras sont des moteurs qui tirent tout deux 330mA (figure 4) quand ils sont bloqus, la dcision a t prise dutiliser ce pont H pour les petits moteurs.
Pour le gros moteur qui tire 1.2A (figure 5), les plus petits ponts H supportant cet amprage sont les Allegro. Le A3958 peut dj tre limin de par sa trop grande tension dalimentation minimum. Cette tension leve sert en fait manipuler les transistors MOS qui constituent le pont H (les bloquer ou les saturer). Le A3953 est plus petit et a une tension dalimentation minimum de 5V. Malheureusement, son pont H est constitu de transistor bipolaire (ce qui explique la basse tension dalimentation). Lorsquils sont en mode satur, il y a une chute de tension denviron 1V pour 1.2A dans ces transistors (c.f. data sheet). Cela fait 1.2W de dissip pour un transistor, mais cela double, car le courant passe dans deux transistors. On a donc 2,4W de dissip uniquement dans le pont H. Cest une puissance perdue trop importante, en comparaison, le moteur utilis pour cette articulation dveloppe 4,5W.
Cest pour cela que le A3959 a t choisi, les transistors MOS dissipent nettement moins dnergie et la tension dalimentation de 9.5V est relativement raisonnable, sans parler du fait que ce pont H a aussi t implment dans le programme du module moteur. Le A3959 existe aussi dans une version plus petite (TSSOP : Thin Shrink Small Outline Package). Les dimensions du chip version TSSOP sont de 4.5x9.5x1.2mm, cela donne 38% de la surface du chip SOP (Small Outline Package) dorigine, ce qui est considrablement plus petit. Malheureusement, il a t impossible dobtenir cette version, et comme il y a dj une immense demande pour la version SOP, il a juste t possible den obtenir un chantillon de sept pices chez Telion Zrich.
Florent Gnter
Projet de diplme
20.02.2004
Le problme de la tension dalimentation du pont H a 9,5V a t rsolu en introduisant une tension dalimentation des moteurs spare. Cette tension surcharge les moteurs qui ont une tension dalimentation recommande de 6V, mais comme il sont contrlable en vitesse et en position grce une tension dalimentation PWM (Pulse Width Modulation), la tension apparente sur les moteurs est plus petite que 9,5V. Par exemple, avec une vitesse de 15 (il ny a pas dunit car la vitesse est donne par un byte sur le PIC) qui quivaut environ 2.5 Rad/s, la tension apparente sur les deux premiers moteurs, quand le bras est tendu, peut atteindre 8.3V et dans les cas plus favorable (bras repli), elle atteint les 7V. Les moteurs sont donc lgrement en surcharge, ce qui fait quil y a quand mme un risque de surchauffe si ils sont utiliss trop intensment.
2.2 Le microprocesseur
Pour le microprocesseur, il na jamais t question de changer de marque car toute la programmation a t faite sur le PIC16F876 qui est le microprocesseur qui quipe les modules I 2C du LSA. Par contre, pour autant quil soit compatible, on peut changer le modle du microprocesseur. Dorigine, le programme a t conu pour tre support par le PIC16F876 ou le PIC16F877. Voici donc les diffrentes versions possibles des PIC dans cette gamme.
images
Tableau 2 : Microprocesseurs PIC. Aprs discussion avec Francesco Mondada qui avait le mme problme de miniaturisation sur le projet Swarm-Bot, le PIC16F876A en version MLF a t choisi en raison de sa taille trs rduite. En effet, si lon compare le volume du botier du PIC dorigine avec le botier du PIC16F876A, on remarque que le volume de ce dernier est 11.5 fois plus petit que lautre et que la surface est 3.6 fois plus petite. 8
Florent Gnter
Projet de diplme
20.02.2004
Pour pouvoir rendre compatible le programme sur ce PIC, il a suffit dinclure la librairie spcifique ce microprocesseur au dbut du code, la place de lancienne. En fait, les deux librairies ne sont pas trs diffrentes, il ny a que un ou deux petits dtails qui changent.
Florent Gnter
Projet de diplme
20.02.2004
dcouplage a engendr des problmes de bruit sur les alimentations et une instabilit du circuit dans le sens o les registres du PIC changeaient de valeurs continuellement ce qui rendait inutilisable le circuit. En fait, les interfrences venaient des fils dalimentations qui taient simplement torsads autour de lalimentation et qui ne faisaient pas parfaitement contact. Il a suffit damliorer le contact (en serrant les fils sur les bornes) et de raccourcir les fils pour que le systme fonctionne. Pour ces raisons dinstabilit la moindre interfrence, il a t dcid de garder les dcouplages dorigine pour une plus grande robustesse du circuit. Un nouveau module de test a t conu pendant les essais avec le module de la Salamandre. Le nouveau module est quip dun pont H A3959 et du nouveau PIC 16F876A-ML. Le seul problme sur ce module est la connexion pour le moteur qui ncessite une adaptation. Autrement, tout a march ds les premiers essais qui ont t effectus sur le gros moteur de larticulation de lpaule, et aucune trace dinstabilit na t remarque.
10
Florent Gnter
Projet de diplme
20.02.2004
Les lments qui prennent le plus de place sont les connecteurs, un six ples pour la communication I2C, un six ples pour le moteur et un quatre ples pour le potentiomtre. La hauteur (8mm) de ces connecteurs peut tre gnante suivant lemplacement du circuit. Un connecteur 6 ples est encore ncessaire pour la programmation du PIC, mais il nest pas soud et est retir une fois le programme charg. Un autre lment est aussi particulirement volumineux, cest la capacit de dcouplage (Figures 6.a 6.c) de la tension dalimentation du moteur qui non seulement occupe une surface relativement grande, mais a aussi une hauteur importante (7 X 4 X 4mm).
Dans le dernier module (figure 6.c) les lments cits ci-dessus sont tous placs au centre du PCB car ce module se glisse dans la partie suprieure du bras par dessus les moteurs et cest dans le centre que la hauteur disponible est la plus grande (Figure 7).
Figure 7 : Module moteur pour les dgl trois et quatre son emplacement.
Pour les deux autres modules, lemplacement ne joue pas un grand rle, il a juste fallu faire attention en plaant les connecteurs de penser aux cbles plats qui devront en sortir sans tre gns. Par exemple, sur la figure 7, le connecteur qui est droite doit avoir un cble qui part loppos du connecteur gauche sous peine de ne pas pouvoir tre connect.
11
Florent Gnter
Projet de diplme
20.02.2004
En ce qui concerne le programme du PIC, il est trs facile utiliser. En effet, au dbut du programme, il y a une srie de variables initialiser suivant le matriel que lon a sur le module moteur ou suivant la configuration adopte. Ces variables sont au nombre de cinq :
COMMUNICATION_PROTOCOL
ENCODER_TYPE
DRIVER_TYPE
CURRENT_MEASUREMENT
SPECIAL_HARDWARE
Protocole de communication avec le PIC : 0 = I2C 1 = SPI (non implmente) 2 = USART (non implmente) 3 = PSP (non implmente) Type dencodeur utilis : 0 = LSI LS7084 1 = Potentiomtre pour mesure en position. 2 = Tachymtre pour mesure en vitesse (non implmente) Type de driver moteur utilis : 0 = Allegro A3959 1 = Allegro A3953 2 = Vishay SI9986 3 = Motorola MPC17511 1 (non implmente) Type de mesure du courant : 0 = Amplificateur INA 146 pour mesure rel du courant. 1 = calcul du courant bas sur la vitesse (non implmente) Configuration spciale des entres-sorties du PIC : 0 = Configuration standard 1 = Configuration Swarm-Bot
La dernire variable concerne un changement spcifique pour le Swarm-Bot, dans le cas du bras de Robota, cest la configuration standard qui est valable. Une fois que ces variables sont initialises, le programme inclus les fichiers supplmentaires correspondants. Ceci permet de gagner de la place mmoire en excluant du programme tout le code qui nest pas ncessaire au bon fonctionnement du module. La dernire chose ventuellement changer est ladresse du module pour la communication part I2C. Ensuite le programme peut tre compil et charg sur le PIC.
12
Florent Gnter
Projet de diplme
20.02.2004
Pour la lecture du potentiomtre par le PIC, il a dj t implment un code qui permet dutiliser le potentiomtre comme encodeur. Ce fichier contient galement des fonctions pour dterminer la vitesse et le sens de rotation de larticulation. Hors, pour ce projet, seul la lecture de la position est intressante, et il se trouve quune fonction existe dans le code principal qui lit la valeur dun potentiomtre pour la mettre dans le registre SetPoint qui indique la position o le moteur doit aller. Dans la mesure o elle nest daucune utilit dans le bras, cette fonction a t modifie de faon Figure 8 : Potentiomtre sur larticulation du coude. crire la valeur du potentiomtre dans un autre registre.
En fait, la seule modification effectue dans la fonction elle-mme, est la condition en dbut de fonction qui teste si le bit ANALOG_SETPOINT_FLAG est 1 ou 0. Dorigine, la fonction tait effectue seulement si il tait 1 (les ordres donns au moteur venaient du potentiomtre et non de lI2C), maintenant elle sera effectue en continu lors de lutilisation du module moteur. Lenregistrement des donnes dans un autre registre, se fait au niveau de la routine dinterruption. En effet, la fonction de lecture du potentiomtre ne fait quautoriser une interruption sur le convertisseur analogique digital et cest lors du traitement de cette interruption que la valeur est enregistre dans un registre spcialement cr cet effet (AnaPositionNew, registre 0x5F). Ce registre contient donc la position du potentiomtre qui est ractualise chaque cycle du programme du PIC.
Ensuite, pour initialiser les encodeurs avec la position des potentiomtres, il faut transformer la position 8 bits en position 32 bits. La position de lencodeur est donne par un compteur 32 bits qui est incrment chaque pas de lencodeur. La position qui est utilise pour le contrle est un nombre 8 bits qui est tir du compteur 32 bits en fonction de la prcision dsire. Par exemple, dans le cas dun moteur simple avec un encodeur, comme la position utilise est un nombre 8 bits sign, on aura une plage de 13
Florent Gnter
Projet de diplme
20.02.2004
mesure de -128 127. A supposer que cette plage couvre un tour complet de laxe du moteur, on aura 256 incrments par tour. Si maintenant on fixe un rducteur la sortie, la plage de mesure de lencodeur couvrira un angle moins grand aprs ltage de rduction. On aura donc une rsolution plus grande, mais une plage de mesure rduite.
Pour remdier cela, on a une variable (DividerPos) qui sert dplacer les 8 bits utiliss sur les 32. Si DividerPos est gal un, on dcalera lextraction des 8 bits de un bit vers la gauche, ce qui a pour effet de diviser la rsolution par 2 donc daugmenter la plage de mesure la sortie de la mme faon. Il faudrait donc, pour initialiser la position de lencodeur, transformer la position analogique en un nombre 32 bits dont les 8 bits sont dcals de la valeur de DividerPos.
Actuellement, la position des encodeurs nest pas initialise dans le programme du PIC, mais il y a la possibilit de le faire dans la classe Robras (classe conue en C++ pour le contrle du bras, c.f. chapitre 5). Le problme est que suivant les rducteurs des moteurs (tant donn que les potentiomtres se trouvent sur laxe de sortie), le rapport entre la position donne par les potentiomtres et la position donne par les encodeurs nest pas forcment divisible par deux ce qui fait quavec DividerPos on arrive approcher un rapport de un, mais on ne latteindra pas toujours. Pour les dgl 2 et 4, le rapport est de 1, mais pour le dgl 3, le rapport est de 1.16. Dans la classe Robras, on utilise un rapport dfinit par un nombre flottant afin de multiplier la position des potentiomtres avant de la renvoyer sur la position donne par les encodeurs.
14
Florent Gnter
Projet de diplme
20.02.2004
Figure 10.a : Le PIC est en cours de programmation. Le jumper est retir, le 13V ne peut donc pas alimenter tout le circuit par lintermdiaire de VDD.
Pour communiquer avec les modules pour les premiers tests, le programme PicWatchII sest avr extrmement utile. Le translateur RS232/I2C est celui dvelopp par lASL 1, il a t configur 38400 bauds pour le RS232 et 100kb/s pour I2C. Des essais ont t faits 115kb/s, mais des erreurs survenaient lors de la lecture et de lcriture des donnes. Il faut donc ouvrire une connexion 38400 bauds sur PicWatchII, puis tout simplement rentrer ladresse des modules et des registres lire ou modifier.
Les rglages pour le contrle en position, et en vitesse ont t grandement facilits par les frottements importants dans les articulations ainsi que par la relativement petite inertie du bras. Pour plus de prcision sur lutilisation du module moteur et de ses registres, il faut sen rfrer au travail de diplme de Daniel Baer [2].
15
Florent Gnter
Projet de diplme
20.02.2004
4 Modlisation :
Pour pouvoir contrler le bras par software, il faut le modliser mathmatiquement, afin de trouver la relation qui lie lespace des coordonnes cartsiennes de la main (endeffector) et lespace des coordonnes articulaires du robot (dans le cas prsent, les angles des diffrents degrs de libert).
La position de rfrence choisie est celle du bras tendu lhorizontale. Pour faciliter les calculs, il faut commencer par le dernier degr de libert et revenir vers le premier. En positionnant lorigine des coordonnes cartsiennes lpaule et laxe x dans la direction du bras, cela donne : 1 : Une rotation de 4 autour de laxe z dcale de L1 le long de laxe x. 2 : Une rotation de 3 autour de laxe x. 3 : Une rotation de 2 autour de laxe y. 4 : Une rotation de 1 autour de laxe x.
Florent Gnter
Projet de diplme
20.02.2004
C 4 S 4 S4 C4 K4 = 0 0 0 0 C2 0 K2 = S 2 0
0 L1 * V 4 0 L1 * S 4 1 0 0 1
0 1 0 0 C 3 S 3 K3 = 0 S 3 C 3 0 0 0 0 1 0 0 C1 S1 K1= 0 S1 C1 0 0 0
0 0 0 1 0 0 0 1
0 S 2 0 1 0 0 0 C 2 0 0 0 1
O C1 = Cos(1), S1 = Sin(1), V4 = 1-Cos(4), etc... Pour la multiplication des matrices, afin de respecter lordre des transformations de coordonnes, il faut commencer par la gauche. Cela donne : K1*K2*K3*K4=Kmgd
Kmgd= C2C4+ S2S3S4 S2C3 L1(C2V4S2S3S4) C2S4+ S2S3C4 S1(C2S3S4S2C4) +C1C3S4 S1(C2S3C4+ S2S4) +C1C3C4 C1S3S1C2C3 L1(S1(C2S3S4+ S2V4) C1C3S4) C1(C2S3S4S2C4) + S1C3S4 C1(C2S3C4+S2S4) + S1C3C4 S1S3+C1C2C3 L1(C1(C2S3S4+ S2V4) + S1C3S4) 0 0 0 1
O Kmgd est la matrice de transformation des coordonnes entre la position de rfrence et la position donne par les angles des dgl, cest la matrice du modle gomtrique direct (les signes de multiplications ont t enlevs pour des raisons de place). X L1 + L 2 Y = Kmgd()* 0 Z 0 1 1
En effectuant la partie droite de lquation, nous obtenons les coordonnes x, y et z en fonction des angles . Pour simplifier les quations, tant donn que L1=108.5 mm et L2=108 mm, les deux distances ont t fixes 108.5mm ce qui reprsente la position dun point dcal de 0.5mm par rapport au centre de la main.
17
Florent Gnter
Projet de diplme
20.02.2004
Figure 12 : Schma du lieu des positions possibles pour le coude Pour extraire une solution, on peut commencer par calculer le jacobien qui donne la relation entre dX et d. Le jacobien est dfinit comme suit :
X 1 Y Jacobien= 1 Z 1 X 2 Y 2 Z 2 X 3 Y 3 Z 3 X 4 Y 4 Z 4
O X, Y et Z sont les quations trouves partir du modle gomtrique directe. En passant par le jacobien, on obtient une solution qui minimise le mouvement pour atteindre la position donne. Le problme est que le jacobien dpend encore des angles . En considrant dX comme un dplacement infinitsimale, on peut se
18
Florent Gnter
Projet de diplme
20.02.2004
permettre de linariser le jacobien afin de linverser et dobtenir les dplacements dangles d correspondants. Comme le jacobien nest pas une matrice carre (4 dgl pour 3 coordonne cartsiennes), il nest normalement pas inversible, mais il existe plusieurs techniques pour le rendre inversible. Pour ce projet, cest la technique de la pseudo-inverse avec optimisation qui a t choisie. La pseudo-inverse du jacobien est dfinie de la manire suivante : Pseudo-inverse : J+ = JT(JJT)-1 Cette pseudo-inverse offre lavantage dexhiber le noyau du jacobien. En effet, un changement des paramtres selon ce sous-ensemble ninfluence pas la position finale de la main et permet ainsi loptimisation de la position du coude selon une certaine fonction. Dans ce projet, la fonction minimiser et la fonction 0- qui pour but une position du bras le plus proche possible de la position de repos (0) qui se traduit dans la plus part des cas par un coude dirig vers le bas. Le terme doptimisation est le suivant : Terme doptimisation : (I-J+J)(0-) Qui correspond la projection de la fonction optimiser sur le noyau du jacobien pondre par un coefficient . Lquation finale pour un petit dplacement X donne donc : = J+x + (I-J+J)(0-) Afin deffectuer de plus grands dplacements, il faut procder par itration. Il faut commencer par dterminer le vecteur entre la position de dpart et la position darrive. Ce vecteur est ensuite divis en petit pas X. Pour chaque nouveau X, on linarise le jacobien en y introduisant les angles correspondant la position actuelle, puis on applique lquation de la pseudo-inverse avec optimisation afin de retrouver les correspondants qui sont ajouts aux angles . Pour un X suffisamment petit, on arrive au point dsir avec une erreur trs faible, mais on perd beaucoup de temps. Pour aller plus vite, on augmente les X et pour viter une erreur trop grande, on refait le calcul du vecteur entre la position darrive et la position actuelle chaque incrmentation, ainsi on obtient une stabilisation de la position du bras autour de la position donne avec une erreur rduite. La condition de sortie de lalgorithme est la longueur du vecteur entre la position actuelle et larrive qui doit tre plus petite quune certaine valeur.
19
Florent Gnter
Projet de diplme
20.02.2004
La solution de la pseudo-inverse qui est implmente dans la classe Robras (c.f. chapitre 5) pose de gros problme la proximit de singularit. Si le trajet lors de litration passe par une singularit, le dterminant de la matrice carre JJT tend vers 0 et la pseudo inverse tend vers linfini. De ce phnomne rsulte un dplacement trs grand pour un petit X et une trajectoire chaotique pour arriver au point recherch. Il se peut dans ce cas que lon narrive pas converger vers la position donne.
Figure 13 : Cheminement des itrations pour un parcours donn. Le parcours part du point de repos pour aller la position (150,0,0), puis aller en (0,150,0) et enfin finir en (0,0,150). La dernire partie est un peu chaotique cause du passage prs dune singularit, mais lalgorithme a tout de mme converg vers la position demande.
Linfluence du terme doptimisation est rgle par le coefficient . Comme le terme doptimisation dpend aussi de la pseudo-inverse, il explose galement lapproche dune singularit. Ce qui fait que si le coefficient est trop grand, la trajectoire devient encore plus chaotique et le phnomne prcdent empire, lalgorithme ne trouve pas de solution. Pour viter cela, une deuxime condition pour sortir de la boucle ditration est pose. La diffrence entre lancienne distance au but et la nouvelle distance au but doit tre positive, cest dire quon sort de la boucle au moment o on commence sloigner du point quon veut atteindre.
Pour pouvoir visualiser la trajectoire lors des itrations, un programme de simulation utilisant la classe Robras a t conu (la figure 13 est une image tire de ce programme). Il utilise OpenGL et est trs utile pour dimensionner les paramtres tel que le coefficient , la longueur dun pas effectu lors dune itration ou les conditions de sortie de la boucle ditration de lalgorithme de cinmatique inverse. Dans ce petit programme, le bras est reprsent par des systmes daxes orthonorms, il y a un systme daxe par dgl et un supplmentaire pour reprsenter la main. La trajectoire emprunte par le bras est elle aussi dessine en trois dimensions. Lorsque le programme tourne, il y a la possibilit dintervenir avec certaine touche du clavier. Aprs avoir entr les coordonnes du point atteindre, la simulation est lance, on peut faire une pause en pressant la lettre s (stop) et reprendre en pressant la lettre c (continuer), la lettre q sert quitter le programme et la lettre n sert rentrer les coordonnes dun nouveau point.
20
Florent Gnter
Projet de diplme
20.02.2004
5 La classe Robras :
La programmation a t faite en C++, ce qui prsente le principale avantage davoir runis toutes les fonctions et les variables utiles au contrle du bras dans une seule classe, ainsi dans le programme principal, on a un objet de type Robras qui contient toutes les variables et les mthodes associes au contrle du bras. La majeur partie des calculs est du calcul matriciel, cest pourquoi il a t dclar deux nouveaux types pour dfinir des matrices et des vecteurs (Matrix et Vector). Ces deux nouveaux types sont constitus dune structure incluant un tableau pour les lments de la matrices ou pour ceux du vecteur. Elle contient aussi deux entiers qui reprsentent le nombre de lignes et le nombre de colonnes (uniquement un entier pour le nombre de lignes chez les vecteurs).
Une seule matrice est dfinie dans la classe Robras, cest le jacobien qui est utilis pour calculer la cinmatique inverse. Parmis les vecteurs, trois sont utiliss pour les positions en coordonnes cartsiennes de la main : -
Vector Cart : Vecteur qui contient la position actuelle. Vector NewCart : Vecteur qui contient la position atteindre. Vector Dcart : Vecteur qui est la diffrence entre les deux prcdents vecteurs.
Vector Teta0 : Vecteur qui contient les angles de la position de repos. Vector Teta : Vecteur utilis pour calculer la position atteindre par itration et pour passer les angles au robot. Vector TetaEff : Vecteur qui contient les angles mesurs directement sur les encodeurs ou sur les potentiomtres. Vector DTeta : Vecteur qui reprsente le dplacement infinitsimal fait lors dune itration lors du calcul de la cinmatique inverse.
Toutes ces variables sont publiques except les deux vecteurs DCart et DTeta qui ne sont daucune utilit pour lutilisateur de la classe Robras.
21
Florent Gnter
Projet de diplme
20.02.2004
Dans les mthodes de la classe, les deux premires sont deux fonctions dinitialisation, une pour les membres de la classe (void InitRobras(void)), et une pour initialiser les encodeurs (void InitEncoder(void)) des moteurs au cas ou les modules auraient subi un reset ou lors du dmarrage. Cette dernire fonction utilise la position des potentiomtres pour initialiser lencodeur, ainsi, lors du dmarrage du robot, il ny a pas besoin de le faire retourner en position de rfrence. Les cinq mthodes qui viennent ensuite servent configurer les modules. Elles accdent aux quatre modules moteurs en mme temps. En principe, il ny a pas besoins de faire des configurations diffrentes sur les moteurs, toutefois, il est possible daccder directement au registre via le translateur RS 232/I2C en utilisant la librairie I2C inclue dans Robras.h (le rsum des registres du rapport de projet de diplme de Daniel Br [2] est trs utile pour cela). Ces fonctions de configuration sont les suivantes : -
void Actif (void) : Cette fonction met tous les modules en mode de contrle moteur normal. void Passif (void) : Cette fonction configure tous les modules en mode passif, les moteurs sont libres, mais les valeurs des capteurs sont constamment ractualises, ce qui permet de lire la position en faisant bouger le bras de lextrieur. void Reset (void) : Cette fonction effectue un reset sur tous les modules, attention, la position des encodeurs est remise 0, il faut les rinitialiser aprs un reset. void SetSpeAcc (byte speed, byte acc) : Cette fonction configure la vitesse et lacclration maximum utilises pour le contrle avec profile de vitesse trapzodale. Il faut donner un nombre entre 0 et 255. void SetControlMode (int mode) : Cette fonction configure le type de contrle utilis pour le moteur. On a 7 types de contrles (modes) diffrents : o o o o o o o
0 : contrle en boucle ouverte. 1 : contrle en position. 2 : contrle en position avec un profil de vitesse trapzodal. 3 : contrle en vitesse. 4 : contrle en vitesse avec profil trapzodal. 5 : contrle en couple. 6 : contrle en couple pour un frottement nul.
Les dernires mthodes publiques de la classe Robras sont des fonctions qui servent au contrle proprement dit. : -
void GetPosition (int AD) : Cette fonction va lire la position sur les modules. Les positions qui sont lues sont, la position donne par les encodeurs si largument pass la fonction
22
Florent Gnter
Projet de diplme
20.02.2004
(AD) est 0 et si cest 1 la position qui est donne par les potentiomtres. Une transformation est applique pour avoir les angles en radians. -
void Go_Angle (void) : Cette fonction transfre les angles du vecteur Teta sur les modules dans le registre SetPoint. Si le bras est actif, il va aller dans la position indique. Les angles sont transforms en variable 8 bits avant dtre envoy sur les modules. void WaitImmobility (void) : Cette fonction a t conue pour tre utilise avec la fonction Go_Angle, elle sert attendre limmobilisation du bras avant de continuer le programme. int CalcNewPosition (void) : Cette fonction calcule les angles Teta ncessaires pour atteindre la position donne par le vecteur NewCart. Ceci se fait par la mthode ditration dcrite au chapitre 4.2. Lentier qui est retourn vaut 0 si il y a un problme (matrice singulire) et 0 sinon. void ForwardKinematics (void) : Cette fonction calcule la position cartsienne partir du vecteur Teta.
Les mthodes prives sont en majorit des fonctions pour le calcul matriciel. Pour ces mthodes, la taille maximum des matrices impliques est dfinie par le nombre de degrs de libert. Elles ont t conues pour des matrices de taille quelconque, toutefois, le calcul du dterminant se faisant laide dune fonction rcursive, si les matrices dpassent la taille de 10X10, linversion de la matrice se fait trs lentement (environ 11 seconde pour une matrice 10X10). En plus des mthodes de calcul matriciel, on a deux mthodes qui sont utilises lors du calcul par itration de la nouvelle position. InvKinematics sert calculer la cinmatique inverse pour une itration de CalcNewPosition, elle retourne 0 si il y a un problme de matrice singulire et 1 sinon. CalcJacobien linarise le jacobien pour une mme itration. Les deux dernires mthodes sont des mthodes de conversions entre les entiers et les bytes signs. En effet, lors de lchange de donnes, les bytes signs du PIC sont lus comme des bytes non signs (0 255) pour retrouver les donnes dorigine (-128 127), il faut les convertires.
23
Florent Gnter
Projet de diplme
20.02.2004
6 Test du bras :
Suite quelques problmes (longs dlais lors de commande, encodeur grill, retard, etc) le bras naura pas pu tre test aussi longtemps que prvu. Mais une srie de tests a quand mme t effectue pour valuer si la mcanique et le systme de contrle sont efficaces.
Figure 14 : schma du systme de rglage de lengrenage. Le systme de serrage consiste en une douille qui peut coulisser sur laxe moteur et sur laquelle est mont lengrenage. Le jeu entre les deux roues se rgle en approchant la roue mobile de la fixe puis en serrant deux vis places 120 autour de la douille. Ces deux vis serrent la douille sur laxe et le tout devient solidaire. Le problme cest que la douille est trs paisse et on a de la peine bloquer le systme. De plus, si lon serre trop les engrenages ensembles, une force radiale vient sexercer sur laxe du moteur et augmente considrablement le frottement ce qui peut rendre la transmission irrversible. Le plus gros problme engendr par ce jeu, cest que les encodeurs qui sont utiliss pour le contrle moteur nindique plus forcement le bonne angle, cela dpend de la position du bras. Langle de jeu dans lpaule au niveau du 2me dgl est de 7.9 ce qui fait en bout de bras un dplacement libre de 30 mm. Dans le coude, le jeu est de 11.6 ce qui engendre un dplacement de 22 mm en bout de bras. On peut donc imaginer que dans certaines positions, cela engendre un jeu pouvant aller jusqu plus de 50 mm ce qui est en proportion, pratiquement un quart de la longueur du bras.
24
Florent Gnter
Projet de diplme
20.02.2004
Lautre problme gnr par ce jeu est la diffrence entre la position des potentiomtres et la position des encodeurs. Lors de la mise en route du robot, linitialisation des encodeurs ce fait laide des potentiomtres pour ne pas devoir chaque fois aller dans une position de rfrence. On peut voir sur les graphiques suivants la diffrence entre les potentiomtres et les encodeurs dun mme dgl.
2 1.5 1
Angle (radian)
25
Florent Gnter
Projet de diplme
20.02.2004
Lamplitude du premier dgl est de 180. Pour cet axe, il serait possible daugmenter un peu langle du bras vers larrire sur la premire photo de la figure 15. En effet, lhumain peut tourner le bras vers larrire presque 45. Il suffirait pour cela de dplacer la bute qui est fixe sous le bras. Il ny aurait aucun autre inconvnient mcanique.
Pour le deuxime dgl, lamplitude est dun peu plus de 180. Il faut not que quand le bras est dirig vers lavant (horizontal), lamplitude minimum est plus grande quen position de repos (vertical). En effet, le moteur du premier dgl fait office de bute sur la premire photo de la figure 16.
26
Florent Gnter
Projet de diplme
20.02.2004
Lamplitude du troisime dgl est denviron 110. Ici, comme pour le premier axe, cest une goupille qui empche de trop amples mouvements. Cette goupille empche galement aux cbles passant dans le haut du bras dtre cisaills.
Figure 18 : amplitude possible du dgl 4. Lamplitude du quatrime dgl est de 90. Ici, pour un robot humanode, il faudrait quil puisse mont un peu plus haut. Malheureusement, dans ce cas, cest la conception du bras qui pose problme, puisque lavant bras bute sur le circuit du potentiomtre du coude. Il ne monte donc plus aussi haut que ce qui tait initialement prvu.
Pris individuellement, tous les dgl sont donc dans les normes humaine, mais en les combinant, on obtient des mouvements et des positions qui pourraient ventuellement tre attribus des contorsionnistes, mais qui ne sont pas possible chez les personnes normalement constitues (exemple la figure 19).
Florent Gnter
Projet de diplme
20.02.2004
Ceci vient du fait quessayer dimiter un bras humain qui utilise des muscles qui ont une force de traction linaire avec des moteurs rotatifs nest pas la meilleure solution.
Comme toute la partie itration pour la recherche dune solution ce fait sur lordinateur et comme cest les angles finaux qui sont envoys sur les modules, le bras va respecter les angles quon lui a transmis jusqu ce quil arrive en butt. Ensuite, la position effective est recalcule et corrige si le bras na pas pu atteindre la position donne. Dans certains cas, la position pourrait physiquement tre atteinte par le bras, mais cela demande un dplacement plus labor. La mthode de la pseudo-inverse minimise la rotation de chaque dgl cela empche le programme dexplorer tout lespace des solutions.
Figure 20 : Position du bras quand la position atteindre est le point (150,0,0) Pour atteindre le point (150,0,0) par exemple, le dgl qui bloque est le 3me, car il ne peut pas tourner plus de 23 dans le sens demand cause de la goupille (figure 20), alors quil devrait tourner de 90. Il est vrai quil existe une autre solution o le coude est aussi dirig vers le bas pour avoir une position qui demande un minimum dnergie (figure 21). Mais cette position demande une rotation de 180 du 1er dgl et pratiquement la mme chose pour le 2me dgl, cest donc une solution relativement loigne dans lespace des angles du robot.
28
Florent Gnter
Projet de diplme
20.02.2004
Cest en fait une bonne chose que de choisir un minimum de dplacement et doptimiser la position du bras par rapport la position de repos, car, ainsi, les positionnements restent relativement semblable ceux dun humain. Mais le dsavantage est quune partie du volume de travail du bras nest pas utilise.
Figure 21 : Autre position possible pour atteindre le point (150,0,0), mais qui nest pas naturelle (vue depuis larrire du bras).
29
Florent Gnter
Projet de diplme
20.02.2004
7 Conclusions :
Lide de prendre le module moteur de lASL pour contrler le bras de Robota a beaucoup davantages. En premier lieu, il existe et a dj t test, cela reprsente un gain de temps considrable. Ensuite, cest un module trs complet et qui peut se placer directement proximit du moteur car sa taille a pu tre diminue. Un des avantages driv de cela, cest quil faut faire passer un nombre rduit de fils dans les articulations du bras. Le problme de ce module moteur, cest quil a t conu de faon tre utilis dans des situations trs varies et de ce fait, les fonctionnalits qui nous proccupent nont pas t forcement optimises pour le cas de Robota. Un des problmes les plus srieux engendrer par cela, cest le manque de rapidit de linterface qui fait que le mouvement ne peut pratiquement pas tre contrl en boucle ferm sur lordinateur. Il faut envoyer les destinations finales sur les modules pour que ces derniers fassent le contrle en interne. Cela engendre une impossibilit de contrler la trajectoire du bras dune faon fluide. Pour un robot humanode comme Robota, il pourrait tre intressant de modifier un peu le concept du module pour optimiser la vitesse dinteraction entre les PIC et lordinateur.
30
Florent Gnter
Projet de diplme
20.02.2004
8 Remerciements :
Je tiens remercier le Prof. Aude Billard ainsi que Sylvain Calinon et Andr Guignard pour lencadrement prcieux quil mon offert lors de mon projet. Je tiens galement remercier George Vaucher, pour sa disponibilit lors de la fabrication de la partie lectronique, Alessandro Crespi, Francesco Mondada et Michael Bonani pour leurs conseils et leurs aides en ce qui concerne les modules moteurs. Merci galement Telion Zurich pour les chantillons de drivers moteurs Allegro A3959 quils nous ont gnreusement offert.
9 Bibliographie :
[1] Lorenzo Piccardi et Matto Calzascia : Construction dun bras mcanique pour le robot humanode Robota, rapport de projet de semestre, Laboratoire de Systmes Autonomes 3, EPFL, juin 2003. [2] Daniel Br :
The I2C DC motor driver and motion controller module, rapport de projet de diplme, Laboratoire de Systmes Autonomes 2, EPFL, fvrier 2003.
[3] Profs. H.Bleuler, R.Clavel, R.Siegwart : Robotique et microrobotique, polycopi, Institut de Systmes Robotiques, EPFL, octobre 2001. [4] Dr. Stefan Schaal:
Inverse Kinematics, course CS545, Computational Learning and Motor Control Lab, University of Southern California, 2001.
[5] Dr. Stefan Schaal et Gaurav Tevatia: Inverse Kinematics for Humanoid Robots, International Conference on Robotics and Automation, 2000. [6] Prof. Liebling :
31
Florent Gnter
Projet de diplme
20.02.2004
Sites Internet
Revendeur de matriel lectronique : http://www.spoerle.com http://www.distrelec.com http://www.reselec.ch
http://www.telion.ch/ http://www.farnell.com/
http://www.microchip.com
http://www.allegromicro.com/ http://www.vishay.com/ http://www.maxim-ic.com/ http://www.national.com/ http://www.motorola.com/
32
Florent Gnter
Projet de diplme
20.02.2004
10 Annexes :
I. schmas lectroniques des modules moteurs II. Code source de la classe Robras
33