Etude de la motorisation du robot LEGO 1
Objectif - analyser le fonctionnement des moteurs NXT en régime statique et dynamique - aborder la régulation de position et/ou de vitesse - intégrer les moteurs dans l'étude du mouvement du robot - étudier le déplacement du robot en situation réelle - piloter les mouvements en vue de la réalisation d'un objectif - asservir les mouvements en vue du déplacement autonome du robot suivi de ligne 2
Etude de la motorisation NXT - éléments du servo-moteur - caractéristiques du moteur - capteur de position - asservissement de base - étude du régulateur PID - numérisation de la régulation - prise en compte des non-linéarités 3
Servo-moteur NXT Actionneur électrique : moteur à courant continu à rotor sans fer et aimants permanents (flux constant) Capteur : codeur optique de position (rotation) sur l'arbre du moteur Problèmes inhérents à la construction: - Jeu codeur/ moteur, jeu moteur/arbre de sortie - Pas de mesure électrique (tension, courant) - Pas de mesure de vitesse instantanée Effecteur: réducteur mécanique à pignons avec sortie rotation Réducteur étage par étage 10:30:40 = 1:4 9:27 = 1:3 10:20 = 1:2 10:13:20 = 1:2 Réduction totale 1:48 4
Servo-moteur NXT : Codeur optique Projet Robotique LEGO NXT remise à zéro Y = A XOR B codeur en quadrature A B logique Y DIR Compteur décompteur position numérique de l'arbre de sortie éléments réalisés par software sur processeur ARM 360 encoder pulses per revolution of a LEGO motor (1 resolution)with quadrature signals, (max resolution is 720 pulses/rev but not used) 5
Servo-moteur NXT : Câblage général Projet Robotique LEGO NXT Hacheur de commande intégré à la brique NXT 6
Servo-moteur NXT : Hiérarchie du contrôle Programme AVR32 (32 bits) AVR ATmega (8 bits) Codeur optique Batterie 7.2V commande pwm Hacheur bidirectionnel sortie puissance pwm liaison mécanique Moteur CC Hacheurs AVR32 ATmega8 7
Servo-moteur NXT: équations Projet Robotique LEGO NXT Equation électrique: avec Equation mécanique: avec e f = K e C m = K m i a L di a dt R i a e f = u a J d dt f = C m C r En régime statique, les dérivées sont nulles. R I a K e = U a K m I a f = C r La relation statique est donc : [ R K e ][ ] I a K m f = [ U a C r ] Les valeurs des paramètres peuvent être déduites de quelques expériences i a e a e f C m C r L R K m K e J f courant rotor A tension rotor V force contre électromotrice V angle arbre sortie rad vitesse arbre sortie rad/s couple moteur N.m couple résistant N.m inductance rotor H résistance rotor constante de couple méca N.m/A constante de fem V/rad/s moment d'inertie kg.m 2 coefficient de frotement visqueux Nm/rad/s 8
Servo-moteur NXT : expérimentations Projet Robotique LEGO NXT caractéristique à vide (philohome.com) travail : calculer Ke en V/rd/s à partir de la courbe caractéristique en court circuit travail : évaluer Ra et Km 9
Servo-moteur NXT : expérimentations Projet Robotique LEGO NXT caractéristique en charge à couple constant (philohome.com) travail : évaluer f à partir du tableau ci-dessus 10
Servo-moteur NXT : expérimentations (philohome.com) C u = K m I a f droite de pente K m, ordonnée à l'origine non nulle C m = K m R U K e K m a R droite de pente K e K m R travail : évaluer Km, Ke et f à partir des relevés graphiques fournis ci-dessus 11
Servo-moteur NXT : valeurs déduites ( signalées dans diverses publications, à comparer aux valeurs trouvées): R a = de 6.85 à 7.6 K e = 0.46V / rad /s K m = 0.32 N.m/ A f = de 1.12 à 2.2 10 3 Nm/ rad / s autres valeurs signalées: L = 4.88 mh J m = 1 10 5 kg.m 2 (mesure directe) (valeur à confirmer par les mesures!) valeur du couple de friction (collage) : T c = K m v c R =3.56 10 4 Nm avec v c tension de décollage du moteur 12
Servo-moteur NXT : simulation de la partie mécanique travail : réaliser cette simulation multiphysique; elle sera faite avec l'extension COSELICA sur Scicoslab; l'installation de l'extension sera guidée par l'enseignant entrée modulation MLI/PWM tension moteur sortie mécanique arbre moteur Hacheur 1 quadrant 13
Servo-moteur NXT : simulation du modulateur PWM Horloge PWM 7.5kHz sortie PWM entrée commande [0..100] modulateur 1 quadrant 14
Servo-moteur NXT : simulation du moteur Signal = échelon d'amplitude 50% 2.0 1.5 Graphic 1 y 1.0 0.5 0.0-0.5-1.0 0.0000 0.0 005 0.0010 0.0015 0.0020 0.0025 0.0030 0.0035 0.0040 0.0045 0.0050 15 Graphic t 2 10 y 5 0-5 0.0000 0.0 005 0.0010 0.0015 0.0020 0.0025 0.0030 0.0035 0.0040 0.0045 0.0050 50 Graphic t 3 y 40 30 20 10 0 0.0000 0.0 005 0.0010 0.0015 0.0020 0.0025 0.0030 0.0035 0.0040 0.0045 0.0050 t travail : vérifier la valeur du régime permanent; le gain statique correspond-il à la valeur prévue? 15
Servo-moteur NXT : hacheur 4 quadrants signal MLI entrée modulateur Modulateur MLI et Schéma hacheur 4Q moteur travail pour moi: modifier le schéma pour que la tension de batterie soit une variable d'entrée du hacheur 16
Servo-moteur NXT : hacheur 4 quadrants 60 Graphic 1 y y 50 40 30 20 10 0 0.0000 0.0005 0.0010 0.0015 0.0020 0.0025 0.0030 0.0035 0.0040 0.0045 0.0050 14 12 10 8 6 4 2 0 Graphic t 2 0.0000 0.0005 0.0010 0.0015 0.0020 0.0025 0.0030 0.0035 0.0040 0.0045 0.0050 20 Graphic t 3 15 y 10 5 0 0.0000 0.0005 0.0010 0.0015 0.0020 0.0025 0.0030 0.0035 0.0040 0.0045 0.0050 t Réponse échelon 17
Servo-moteur NXT : équations dynamiques Pour un couple utile Cr = cste, on a : u = L di dt Ri K e K m i = J d dt f Passage en Laplace: U = L p I R I K e K m I = J p f p U p = 1 K e / K e K m Rf RJ fl K e K m Rf p LJ K e K m Rf p 2 k 0 par identification avec le second ordre, on obtient : 1 2 p p2 2 n n k 0 = K e K e K m Rf n = K e K m Rf L J = 1 2 1 K e K m Rf 1/ 2 RJ fl LJ 1/ 2 travail : calculer la fréquence propre et l'amortissement, vérifier la cohérence avec la simulation 18
Servo-moteur NXT : équations dynamiques Cas où le système est amorti 1 On pose : e = L R m = RJ K e K m Rf d'où p U p K 0 1 e p 1 m p avec K 0 = K e K e K m + Rf Travail : calculer les constantes de temps avec les paramètres proposés Réponse : Avec les valeurs proposées, on obtient : e =0.72 ms m =0.41 ms ces valeurs étant proches, le modèle simplifié n'est pas valide; il faut conserver le modèle complet. 19
Servo-moteur NXT : test direct (en l'air sans roues) Travail à faire: Faire un programme nxc de relevé à l'échelon de la position des moteurs Méthode : - les mesures [position] converties en chaîne de caractère seront stockées dans un fichier "test.txt" qui contiendra 100 mesures (temps, moteur G, moteur D). Pour une relecture sus Excel/Calc ou Matlab/Scilab, les champs texte seront séparés par des blancs et chaque ligne terminée par "return". - créer le fichier par CreateFile (s'il existe un ancien fichier du même nom, le supprimer préalablement) - l'heure d'une mesure est donnée par CurrentTick - la position d'un moteur est donnée par MotorTachoCount, la vitesse par différence des positions entre deux mesures - initialiser toutes les variables utilisées - ne pas oublier d'arrêter les moteurs en quittant! - contrôler le temps de boucle par Wait(ms) [valeur très petite, voire nulle] Exloitation: Tracer la réponse en position sur Calc ou Scilab et valider [ou non!] les paramètres proposés du moteur. En déduire les caractéristiques du moteur 20
Servo-moteur NXT : test direct (en l'air sans roues) Résultat: tracé sur Scilab pour PWM=50% 80 70 60 50 40 30 20 10 0 0 50 100 150 200 250 Commandes sous Scilab [M,text]=fscanfMat('test.txt'); plot2d(m(:,1),m(:,2:3)) 21
Servo-moteur NXT : test direct (en l'air sans roues) Résultat par exploitation graphique directe: La réponse en position est une rampe (intégrale de la réponse à l'échelon), plutôt caractéristique d'un premier ordre que d'un deuxième ordre On constate que l'asymptote pour t-->infini est une droite de pente 52 pulses en 150ms soit 6.01 rad/s La commande est de 50% avec une tension de batterie (relevée avec Bricx) de 7.52V. En supposant les pertes du hacheur négligeables (commutateurs idéaux à Ron très faible), la commande est donc de 3.76V On en déduit le gain en vitesse k 0 = 6.01 =1.60 rad /s /V 3.76 d'où K e 0.62 rad / s /V Cette valeur est comparable aux valeurs décrites par les autres auteurs. En supposant que la commande a été appliquée sans retard par le processeur, l'asymptote montre un traînage de 50 ms ; c'est la constante de temps mécanique du système = 50 10 3 s il est vraisemblable que la valeur de Jmoteur proposée précédemment est fausse! 22
Servo-moteur NXT : test direct (en l'air sans roues) Résultat par modélisation de la réponse : La réponse en position est une rampe (intégrale de la réponse à l'échelon), plutôt caractéristique d'un premier ordre que d'un deuxième ordre. En considérant que la fonction de transfert est du 1 er ordre (avec intégration pour la position), la réponse temporelle 1[ est de la forme : y(t)=l A K avec τ= p p(1+τ p)] RJ 2 et K =1/ K e K e d'où y(t )= A K (t τ) + A K τ e t /τ Résolution sous Scilab par les moindres-carrés non-linéaires : function z = fun (p, t, y ) z = y - p (1 ) ( t p(2)) - p (1) * p(2) exp( t / p(2) ) ; endfunction p0=[ 0, 0 ] ; Uo=0.50*7.2; [ f f, p]=leastsq ( l i s t ( fun, t, y/uo), p0 ) ; Travail à faire : - appliquer la méthode proposée aux mesures faites précédemment. référence State Feedback Controller Design Roberto Bucher April 18, 2010 23
Servo-moteur NXT : avec roues (en l'air) L'inertie totale est la somme de l'inertie des systèmes en rotation (rotor moteur, pignons, roue) ajout de l'inertie de la roue masse = 0.017 kg; Rayon = 0.028 m ; A.N. En supposant la masse concentrée sur la périphérie de la roue, l'inertie est : soit J= 1.38 e-5 kgm^2 (valeur surévaluée) J = mr 2 Travail : - Refaire la mesure de la réponse à l'échelon. - A-t-elle évolué par rapport à la réponse en l'air sans roues? - Conclure sur la valeur de J estimée. 24
Servo-moteur NXT : avec roues (en l'air) Résultat: Les nouvelles mesures donnent sensiblement le même résultat que précédemment. L'inertie des roues est donc négligeable devant l'inertie propre du moteur. Ceci vient confirmer que la valeur d'inertie proposée en début de l'étude est incorrecte! 25
Servo-moteur NXT : robot en translation L'inertie totale est la somme de l'inertie des systèmes en rotation (rotor, pignons, roue) et de l'inertie du robot (masse en translation) M r v masse totale en translation pour le robot différentiel : 0.590 kg L'énergie cinétique de M en translation est égale à l'énergie cinétique d'une inertie J_equ en rotation: 1 avec d'où : 2 J equ 2 = 1 2 M v2 v = r J equ = Mr 2 Travail : Refaire la réponse à l'échelon en posant le robot sur une surface et évaluer la nouvelle constante de temps. Peut-on donner une estimation de l'inertie propre du moteur par comparaison avec l'expérience précédente à vide? 26
Servo-moteur NXT : robot en translation Résultat: 150 100 50 0 0 50 100 150 200 250 300 350 400 450 500 La constante de temps passe à 60ms environ. Etant donné que m = RJ K e K m Rf, on peut en déduire que la constante de temps a évolué de 20% soit J moteur = 5 J translation 2 J translation = M 1/2corps r roue = 0.295 0.028 2 =0.23 kg.m 2 J moteur = 1.15 10 3 kg.m 2 27
Servo-moteur NXT : Robot sur roues Projet Robotique LEGO NXT Travail : - Compléter le schéma de simulation du robot avec l'inertie des roues, la conversion du mouvement en translation et la 1/2 masse du corps du robot - Observer la simulation de la motorisation avec ces nouveaux éléments et ajuster les paramètres (en particulier Jmoteur) pour reproduire le comportement expérimental du moteur. - Quelle est la valeur de Jmoteur permettant de correspondre aux valeurs expérimentales de comportement? 28
Servo-moteur NXT : Robot sur roues Résultat: Projet Robotique LEGO NXT 29
Servo-moteur NXT : Robot sur roues Résultat: 14 Graphic 1 12 10 y 8 6 4 2 0 0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 0.40 0.45 0.50 1.0 t Graphic 2 0.8 0.6 y 0.4 0.2 0.0 0.00 0.05 0.10 0.15 0.20 0.25 0.30 0.35 0.40 0.45 0.50 t Inertie du moteur : J=1 e-3 kg.m^2 soit une valeur sensiblement identique à l'estimation précédente 30
Servo-moteur NXT : Robot sur roues Programme type pour les mesures demandées: #define FILE_SIZE 200*3 task record() { byte filehandle; short byteswritten,byteswrittentotal,err; long phi_d,phi_g; long t_init,t_current; string val; err=deletefile("myfile.txt"); CreateFile("myFile.txt", FILE_SIZE, filehandle); byteswrittentotal=0; byteswritten=0; //Ecriture entête strcpy(val,"temps MotG MotD"); WriteLnString(fileHandle,val,bytesWritten); byteswrittentotal+=byteswritten; 31
Servo-moteur NXT : Robot sur roues Projet Robotique LEGO NXT Programme type pour les mesures demandées (suite): //start move with tachos reset; OnFwdEx(OUT_AC,20,RESET_NONE); t_init=currenttick(); while (byteswrittentotal + byteswritten<file_size) { //------------------- t_current= CurrentTick()-t_init; phi_d=motorrotatecount(out_a); phi_g=motorrotatecount(out_c); //------------------- val=numtostr(t_current); val=strcat(val," "); val=strcat(val,numtostr(phi_g)); val=strcat(val," "); val=strcat(val,numtostr(phi_d)); //------------------- WriteLnString(fileHandle,val,bytesWritten); byteswrittentotal += byteswritten; Wait(3); } Off(OUT_AC); CloseFile(fileHandle); } task main() { start record; } 32
Servo-moteur NXT : Asservissement de position Objectif : réaliser un asservissement de position du robot par commande discrète implantée en langage NXC. La commande retenue sera du type Proportionnel puis Proportionnel/Dérivée. On suppose que le système en boucle ouverte a la forme suivante : commande OnFwd 0..100 Hacheur Moteur Codeur K H tension 1/ K e p(1+τ p) Position angulaire K C Position numérique Travail à faire: - Déterminer les coefficients du robot à partir des expériences précédentes et des informations déjà fournies. - Construire ce schéma sur Scicos - Appliquer un échelon d'amplitude 50% et vérifier que la simulation est conforme aux résultats pratiques déjà obtenus. 33
Servo-moteur NXT : Asservissement de position Bouclage basique avec régulateur proportionnel: Consigne de Position numérique Regulateur Sat Hacheur Moteur Codeur K H 1/ K e p(1+τ p) K C Position numérique Travail à faire: - Tenir compte dans le schéma de la contrainte de saturation de la commande bornée entre -100 et +100 (fonction SAT) - Introduire un régulateur type "Proportionnel" de coefficient Kp (fonction GAIN) - Boucler le système simulé avec un retour en position. - Appliquer un échelon de position et vérifier que la simulation est conforme aux résultats pratiques. - Rechercher expérimentalement la valeur de Kp permettant d'obtenir un amortissement de 0.7. 34
Servo-moteur NXT : Asservissement de position Bouclage numérique : le programme d'asservissement doit effectuer périodiquement les tâches suivantes : - lire l'heure courante - lire la position du moteur (codeur ) - calculer l'erreur de position (différence entre la consigne et la mesure) - calculer la commande (loi proportionnelle puis saturation) - l'appliquer au moteur - attendre jusqu'à la fin de période d'échantillonnage - recommencer au début La boucle de régulation est infinie. Pour des considérations pratiques, il est souhaitable de prévoir un arrêt (lecture bouton, nombre maximum de boucle...) Travail à faire: - Les conditions expérimentales seront : Kp à la valeur définie précédemment, échelon de position d'amplitude 100, période d'échantillonnage 5 ms, durée totale 1s. - Ecrire le programme en NXC, en incluant l'enregistrement du temps, de la position réalisée et de la commande. - Discuter du résultat (précision, rapidité). Pensez-vous que la période d'échantillonnage a une influence critique pour ce type de commande?. 35
Servo-moteur NXT : Asservissement de position exemple de résultat pour kp=5 140 120 Position angulaire en rad 100 80 60 40 20 0 0 200 400 600 800 1000 1200 1400 Tem ps 36
Servo-moteur NXT : Compensation PID de l'asservissement de position Le système possédant un pôle nul en boucle ouverte (intégration), il ne présente pas d'erreur de position en boucle fermée L'introduction d'un terme d'intégration dans le correcteur permet d'éliminer l'erreur de position en cas de frottements (collage) ou de couple d'opposition (robot sur une pente). Pour améliorer les performances dynamiques, il convient d'introduire une compensation dérivée. Le compensateur PID a pour équation continue : C ( p) = K p (1 + 1 T i p + T d p) ou encore : u (t) = K p( ε(t ) + 1 T i ε(t)dt + T d d ε(t ) dt ) Compensateur PD ou PID avec dérivateur à bande passante limitée : ce correcteur permet de limiter l'influence du bruit, très sensible pour le terme dérivé C ( p) = K p (1 + 1 T i p + T d p 1+ p T d / N ) Généralement, on adopte 5 < N <20 pour un effet de filtrage. 37
Servo-moteur NXT : Compensation PID de l'asservissement de position Algorithme numérique Terme Intégral En considérant ε constant entre deux instants d'échantillonnage, l'intégrale est approximée par la somme des valeurs de ε aux instants de mesure (intégration d'euler dite des rectangles) u I [n] = u I [n 1] + T e T i ε[n] C I ( p) = U I ( p) ε( p) = 1 T i p u I (t) = 1 t ε(t ) T i 0 Lors des transitoires importants (erreur importante), l'intégrale peut atteindre des valeurs élevées et provoquer la saturation de la commande. Il est judicieux de limiter l'intégrale à des valeurs et définies par l'utilisateur. u I min u I max 38
Servo-moteur NXT : Compensation PID de l'asservissement de position Algorithme numérique Terme Dérivée C D ( p) = U D( p) ε( p) = T d p 1+ p T d / N l'expression temporelle est : u D (t) = T d ε(t ) T d N u D (t) En approximant la dérivée par la différence arrière, on obtient : u D [n] = T d ε[n] ε[n 1] T e T d N u D [n] u D [n 1] On en déduit la récurrence de calcul du terme dérivé : T e u D [n] = NT d T d + NT e (ε[n] ε[n 1]) + T d T d +N T e u D [n 1] Commande du hacheur : c'est la somme des 3 termes P, I et D u [n] = K p (ε[n] + u I [n] + u D [n]) 39
Servo-moteur NXT : Compensation de l'asservissement de position Travail à faire: - Compléter la simulation pour ajouter la compensation dérivée seule (modèle continu) à bande passante limitée avec N=10 - Rechercher la valeur de Td donnant un compromis rapidité/dépassement acceptable. - Implanter cette commande dans le robot et observer le comportement. 40