Discussion :

[candrau]

Bonjour,
dans le cadre de mon projet de robotique, je dois trouver les équations dynamiques et la fonction de transfert d'un robot à 2 bras, sachant que je vais exercer un régulateur PID dessus, et que je travaille avec les angles des bras du robot (voir image).

Je pense avoir trouver les équations dynamiques en utilisant Lagranges, mais je n'arrive pas à trouver la fonction de transfert afin de la mettre par la suite sur simulink.
Voici mon code actuel concernant ma fonction "dynamics"

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

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
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    dynamics2: describes the dynamics of the robot, by using the Lagranges
%equations
%ref:
%http://www.wseas.us/e-library/transactions/control/2011/54-210.pdf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
function [A,B,D,M]=dynamics(teta_v,teta_a,...
    l1,l2,teta)
%teta = [teta1 teta2]  %position vector
%teta_v =>veocity matrix
%teta_a =>acceleration matrix
%equations of dynamics : 
%[M11 M12;M21 M22]*teta_a' = [V1 V2]' + [W1 W2]' = [tau1 tau2]
%with M(teta) is the inertia matrix
%with V(teta,teta_v) the centripetal matrix
%with W(teta) the gravity vector
%tau the generalized force vector
 
 
g=6.643848*10^(-11); %magnitude of gravitational acceleration
m1=1500;
m2=1000; %mass of the links, unit: gram
ml=5000; %mass of the payload
r1=l1/2; %distance between joint 1 and the center of mass of link 1
r2=l2/2; %distance between joint 2 and the center of mass of link 2  
J1=(1/12)*m1*l1^2; %moment of inertia of link 1
J2=(1/12)*m2*l2^2; %moment of inertia of link 2
teta1=(teta(1)*pi)/180; %conversion degree/radian
teta2=(teta(2)*pi)/180;
c1=cos(teta(1));
c2=cos(teta(2));
c12=cos(teta(1)+teta(2));
s1=sin(teta(1));
s2=sin(teta(2));
s12=sin(teta(1)+teta(2));
A=[0 1;0 0];
C=[1 0];
I=eye(2); %Identity matrice 2*2
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Inertia matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
M11 = J1 + J2 + m1*r1^2 + m2*l1^2 + m2*r2^2 + 2*m2*l1*r2*c2 + ml*l1^2+...
    ml*l2^2 + 2*ml*l1*l2*c2;
 
M12 = J2 + m2*r2^2 + 2*m2*l1*r2*c2 + ml*l2^2 + ml*l1*l2*c2;
 
M21 = J2 + m2*r2^2 + m2*l1*r2*c2 + ml*l2^2 + ml*l1*l2*c2;
 
M22 = J2 + m2*r2^2 + ml*l2^2;
 
M = [M11 M12;M21 M22];
 
det = M11*M22 + M21*M12; %determinant of M
if det==0       %calculation of M^-1
    error
else
    Minv = (1/det) * [M22 -M12;-M21 M12];
end
Minv=Minv';
zero=[0 0;0 0];
B=[zero Minv]';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Coriolis/centripetal matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
V1 = (m2*l1*r2 + ml*l1*l2)*(2*teta_v(1) - teta_v(2)) * teta_v(2) * s2;
 
V2 = (m2*l1*r2 + ml*l1*l2) * teta_v(2) * s2;
 
V = [V1 V2]';
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Gravity matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
W1 = (m1*g*r1 + m2*g*l1 + ml*g*l1)*s1 + (m2*g*r2 + ml*g*l2)*s12;
 
W2 = (m2*g*r2 + ml*g*l2)*s12;
 
W = [W1 W2]';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Equations of the dynamics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
D = [V1 + W1;V2 + W2];

Merci d'avance pour votre aide

[Invité]

Bonsoir,

Cela n'a rien à voir avec la question mais je repère deux erreurs:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
det = M11*M22 - M21*M12; %determinant of M
if det==0       %calculation of M^-1
    error
else
    Minv = (1/det) * [M22 -M12;-M21 M11];
end

[candrau]

Merci pour m'avoir signalé ces 2 erreurs

[candrau]

Je précise mon problème en mettant en pièce jointe l'équation dynamique de départ. Merci d'avance à celui qui pourrait m'aider, je ne sais même plus depuis combien de temps je suis bloqué sur ce problème

[tachmou]

Bonjour,

Je voudrais bien essayer de t'aider, mais j'ai du mal à déterminer tes entrées et tes sorties dans ton équation... Pourrais-tu les préciser stp ?

Personnellement, je travaille sur des systèmes hélico... Si ça peut t'aider, voici les équations dynamiques du départ (mis sous forme de représentation d'espace d'état - de state space model) :

.
X = A X + B U
Y = C X + D U

Avec :

• X étant mon vecteur d'état (mes positions)
• Y mon vecteur de sortie
• U mon vecteur commande qui est ici mon entrée
• A B C D sont des matrices (respectivement matrice d'état, matrice de commande, matrice d'observation et matrice de couplage) qui dépendent de la physique du système... que je ne détaillerai pas ici

On cherche donc la "matrice de fonction de transfert" G telle que : Y = G U Pour simplifier le problème, je pose C = I (matrice identité) et D = 0 (matrice nulle) on obtient :

.
X = A X + B U
Y = X

D'où, plus simplement :

.
Y = A Y + B U

En passant en Laplace :

p Y = A Y + B U

Qui peut aussi s'écrire avec la matrice identité I :

p I Y = A Y + B U

(p I - A) Y = B U

G = Y / U = B (p I - A)^-1

Je ne sais pas si c'est faisable, mais tu peux essayer d'adapter la méthode à ton système. C'est pour cela que je t'ai demandé tes entrées et tes sorties dans tes équations.

Cordialement,

ps : help tf.
Exemple :

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
s = tf('s');
G = (s^2 + 4*s + 1) / (s - 1);
bode(G);

[candrau]

Bonjour,
Tout d'abord merci pour ton aide
J'ai essayé d'identifier les différents paramètres du "state-space model" avec mon équation dynamique. Je pense que mon point de départ serait ceci :

.
X = AX + BU + BD
Y = CX

avec

• X = mon vecteur position
• U = mon vecteur "tau" représentant le vecteur force de mon système (voir la pièce jointe de mon message précédent), donc mon entrée?
• D = la matrice suivante :

  Code :

  Sélectionner tout - Visualiser dans une fenêtre à part

  [V1+W1 ; V2+W2];

  (ce D n'a pas de relation avec le D des équations de ton précédant message).

De plus, je pense avoir trouvé l'expression de A et B en fonction des termes de mon équation dynamique.

Un point sur lequel je bloque un peu, c'est justement sur les entrées et sorties du système. En effet, mon superviseur m'a dit que l'entrée était le vecteur "tau", et ma sortie la nouvelle position du robot, que je reboucle à l'entrée de mon régulateur PID avec la position que je veux atteindre, afin d'avoir l'erreur. Dans ce cas, l'entrée de ma fonction de transfert ne serait pas plutôt un vecteur position, et non le vecteur "tau"? J'ai une réunion demain avec mon superviseur, et je lui reposerai la question afin d'éclaircir les choses.

[tachmou]

Bonjour,

Il faut que tu éclaircisses plusieurs points. Cela fait un bout de temps que je ne fais pas de robotique sérielle, donc j'ai un peu de mal, mais en gros :
- Tu commandes quoi ? Un angle ? Un couple ? En tout cas tu envoies une valeur de commande à ton moteur qui possède un capteur sur son axe qui te renvoies un angle... quelle que soit cette commande, ça doit - je pense (et je peux me tromper, attention) - être l'entrée de ton système (sans l'asservissement)
- Tu veux obtenir quoi ? Une position en bout de bras (x,y,orientation) ? Un effort en bout de bras suivant telle ou telle direction ? [Exercer un couple en un point de fixation ? EDIT : impossible avec un bras à 2DDL]... quel que soit ton objectif, ça doit - de même - être cette fois-ci la sortie de ton système (toujours sans asservissement)

Une fois que ces deux points sont clairs, fais les apparaître clairement dans tes équations. Une fois que ça c'est fait, alors tu peux essayer d'appliquer la méthode que je t'ai montré (attention, elle ne fonctionne que si tes équations sont linéaires, avec des coefficients homogènes (ne me demande pas la définition de l'homogénéité ça me dépasse un peu pour l'instant) et constants...).

EDIT2: tu dois certainement pouvoir "décomposer" tau en - par exemple - un truc du genre :

tau = M*teta

Avec teta ton vecteur d'angles et M une matrice 2x2 que tu obtiens avec la physique ou autre... Ainsi, tu te ramènes au problème précédent...

Tiens nous au courant.

Cordialement,

[candrau]

Dans ce cas, gros problèmes, lol, car mes équations sont non-linéaires, et les coefficients non-constants, je dois les recalculer à chaque fois avec la nouvelle position du bras... Sinon, pour préciser, je calcule l'erreur sur la position du bras, et ce non dans l'espace cartésien, mais dans l'espace des angles du robot (teta1 et teta2, cf. image), puis je réinjecte l'erreur dans mon régulateur PID, dont la sortie est connectée à ma fonction de transfert, qui me sort une nouvelle position, et ainsi de suite

[tachmou]

Bonsoir,

Ok, je vois. Dans ce cas, je te conseille de discrétiser ton problème. Je ne suis pas un pro là-dessus, mais en gros, tu transforme un d(X)/dt en (X(n+1)-X(n))/(t(n+1)-t(n)) ainsi, tu as X(n+1) en fonction :
- de X(n) que tu as
- de ton pas de temps de simulation que tu choisis (t(n+1)-t(n)) : donc que tu as
- des autres paramètres... tu dis que tu les recalcules : c'est que tu dois savoir comment les recalculer à ce que je comprends... donc tu les as

Tu bidouilles la tambiouse et tu obtiens ton X(n+1) en fonction de plein de paramètres (que tu as) : donc c'est bon. Il ne te reste qu'à initialiser ton système : X(0) et tu lances ta boucle de simu.

Je ne sais pas si ça peut t'aider, tiens nous au courant.

Cordialement,

[candrau]

J'ai opté pour une autre solution : au lieu de passer en Laplace où j'ai affaire à des équations non-linéaires qui me bloquent, je reste en temporel. En effet, en temporel, je connais déjà toutes les équations, à savoir :

.
X = A*X + B*U + B*D   et  Y = C*X

.
X = [vitesse;acceleration]   et  X = [position;vitesse]

A, B , D, et U sont connus, concernant X, je prends les valeurs de l'état précédent (à l'initialisation, je connais la position, et la vitesse=0). Grâce à ça, je calcule .X avec la première équation.
J'intègre .X, j'obtiens donc le nouveau vecteur X, que je multiplie à C (que je connais aussi), et j'obtiens finalement mon vecteur de sortie Y.

Reste plus qu'à voir si ça marche....

[tachmou]

Bonjour,

Euh... c'est exactement ce que je te conseille de faire, mais j'ai essayé de concrétiser un peu parce que tu fais comment pour intégrer concrètement sur matlab, surtout pour faire de la simulation temporelle ? ...

Pose toi cette question, et d'après moi tu te demanderas comment on fait telle ou telle chose et tu en arriveras à discrétiser je pense. Sinon, j'aimerai savoir comment tu t'y prends stp, étant donné que je pense que je vais être confronté au même type de problème à moment donné.

Cordialement,

[candrau]

Pour intégrer, j'utilise le bloc "integrator" de simulink, qui apparemment marche aussi pour les simulations temporelles.
J'ai mis du temps à répondre, car évidemment, j'ai eu des erreurs dans mon code, et ça m'a prit pas mal de temps pour tout débugger.
Mais au final, ma méthode semble fonctionner, et je suis maintenant capable de contrôler mon robot via un PID.
Merci encore pour l'aide apportée