-
Notifications
You must be signed in to change notification settings - Fork 1
/
manipulator2dof.m
300 lines (297 loc) · 8.27 KB
/
manipulator2dof.m
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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
close all
clear global
%% _DINAMICA DI UN MANIPOLATORE A DUE BRACCI_ %%
% calcolo simbolico
l1 = sym('l1'); %lunghezza braccio 1
l2 = sym('l2'); %lunghezza braccio 2
b1 = l1/2; %distribuzione di massa omogenea, posizione baricentro braccio 1
b2 = l2/2; %distribuzione di massa omogenea, posizione baricentro braccio 2
M1 = sym('M1'); %massa braccio 1
M2 = sym('M2'); %massa braccio 2
teta1 = sym('teta1'); %gdl 1, posizione braccio 1 rispetto alla terna
teta2 = sym('teta2'); %gdl 2, posizione braccio 2 rispetto al braccio 1
teta1_dot = sym('teta1_dot'); %velocità angolare braccio 1
teta2_dot = sym('teta2_dot'); %velocità angolare braccio 2
teta1_dot_dot = sym('teta1_dot_dot'); %accelerazione angolare braccio 1
teta2_dot_dot = sym('teta2_dot_dot'); %accelerazione angolare braccio 2
J1G = 1/12*M1*l1^2; %momento di inerzia baricentrale braccio 1
J2G = 1/12*M2*l2^2; %momento di inerzia baricentrale braccio 2
J1O = J1G + M1*b1^2; %momento di inerzia braccio 1 rispetto ad O
J2A = J2G + M2*b2^2; %momento di inerzia braccio 2 rispetto ad A
tau1 = sym('tau1'); %coppia giunto 1, forza generalizzata
tau2 = sym('tau2'); %coppia giunto 2, forza generalizzata
ag = sym('ag'); %accelerazione di gravità
% prima equazione del sistema
tau1_11 = teta1_dot_dot*(J1O + J2A + M2*l1^2 + 2*M2*l1*b2*cos(teta2))+ teta2_dot_dot*(J2A + M2*l1*b2*cos(teta2))+ teta1_dot*teta2_dot*(-2*M2*l1*b2*sin(teta2))+ teta2_dot^2*(-M2*l1*b2*sin(teta2))+ M1*ag*b1*cos(teta1)+ M2*ag*(l1*cos(teta1) + b2*cos(teta1+teta2));
% seconda equazione del sistema
tau2_21 = teta1_dot_dot*(J2A + M2*l1*b2*cos(teta2)) + teta2_dot_dot*J2A + teta1_dot^2*(M2*l1*b2*sin(teta2)) + M2*ag*b2*cos(teta1+teta2);
% coppie ai giunti
tau = [tau1_11;
tau2_21];
disp('EQUAZIONI DI LAGRANGE DEL SISTEMA DINAMICO');
fprintf('\n');
disp('Prima equazione:');
disp(tau1);
disp(tau1_11);
fprintf('\n');
disp('Seconda equazione:');
disp(tau2);
disp(tau2_21);
fprintf('\n');
disp('Forma vettoriale:');
disp(tau);
fprintf('\n');
% dati del manipolatore
l1 = 1; %lunghezza braccio 1 in m
l2 = 1; %lunghezza braccio 2 in m
M1 = 1; %massa braccio 1 in kg
M2 = 1; %massa braccio 2 in kg
ag = 9.8; %accelerazione di gravità in m/s^2
disp('Sostituendo i dati del manipolatore:');
disp(eval(tau));
% studio del sistema nello spazio di stato
disp('Scrivo il sistema nella forma: B(q)*q_dot_dot + C(q,q_dot)*q_dot + g(q) = tau');
fprintf('\n');
disp('Matrice B(q) dei termini inerziali:');
fprintf('\n');
J1O = sym('J1O');
J2A = sym('J2A');
l1 = sym('l1');
l2 = sym('l2');
b1 = sym('b1');
b2 = sym('b2');
M1 = sym('M1');
M2 = sym('M2');
ag = sym('ag');
b11 = J1O + J2A + M2*l1^2 + 2*M2*l1*b2*cos(teta2);
b12 = J2A + M2*l1*b2*cos(teta2);
b21 = J2A + M2*l1*b2*cos(teta2);
b22 = J2A;
B = [b11 b12;
b21 b22];
disp(B);
fprintf('\n');
disp('Vettore delle accelerazioni angolari q_dot_dot:');
fprintf('\n');
q11_dot_dot = teta1_dot_dot;
q21_dot_dot = teta2_dot_dot;
q_dot_dot = [q11_dot_dot;
q21_dot_dot];
disp(q_dot_dot);
fprintf('\n');
disp('Matrice C(q,q_dot) dei termini centrifughi e di Coriolis:');
fprintf('\n');
c11 = teta2_dot*(-2*M2*l1*b2*sin(teta2));
c12 = teta2_dot*(-M2*l1*b2*sin(teta2));
c21 = teta1_dot*(M2*l1*b2*sin(teta2));
c22 = 0;
C = [c11 c12;
c21 c22];
disp(C);
disp('Vettore delle velocità angolari q_dot:');
fprintf('\n');
q11_dot = teta1_dot;
q21_dot = teta2_dot;
q_dot = [q11_dot;
q21_dot];
disp(q_dot);
fprintf('\n');
disp('Vettore g(q) dei termini gravitazionali:');
fprintf('\n');
g11 = M1*ag*b1*cos(teta1) + M2*ag*(l1*cos(teta1) + b2*cos(teta1+teta2));
g21 = M2*ag*b2*cos(teta1+teta2);
g = [g11;
g21];
fprintf('\n');
disp(g);
fprintf('\n');
% SPAZIO DI STATO
tau1_ss = sym('tau1_ss');
tau2_ss = sym('tau2_ss');
u = [tau1_ss;
tau2_ss];
disp('SPAZIO DI STATO');
fprintf('\n');
disp('Vettore z2_dot:');
fprintf('\n');
z2_dot = -inv(B)*(C*q_dot+g) + inv(B)*u;
disp(z2_dot);
fprintf('\n');
teta1_dot_dot = z2_dot(1,1);
disp('Teta1_dot_dot:');
disp(teta1_dot_dot);
fprintf('\n');
teta2_dot_dot = z2_dot(2,1);
disp('Teta2_dot_dot:');
disp(teta2_dot_dot);
fprintf('\n');
disp('VARIABILI DI STATO');
% VARIABILI DI STATO (StateSpace)
% posizioni
x1 = teta1;
x2 = teta2;
%vettore posizioni
x = [x1; x2];
% velocità
x3 = teta1_dot;
x4 = teta2_dot;
x1_dot = x3;
x2_dot = x4;
%vettore velocità
x_dot = [x3; x4];
% accelerazioni
x3_dot = teta1_dot_dot;
x4_dot = teta2_dot_dot;
%vettore accelerazioni
x_dot_dot = [x3_dot; x4_dot];
% ingressi
u1 = tau1;
%coppia 1
u2 = tau2;
%coppia 2
%vettore degli ingressi
u = [u1; u2];
disp('Vettore degli ingressi:');
fprintf('\n');
disp(u);
% uscite
y1 = sym('y1');
y2 = sym('y2');
% SISTEMA SPAZIO DI STATO
% x_dot = A_ss*x + B_ss*u
% y = C_ss*x + D_ss*u
% funzioni del sistema
x1_dot = x3;
disp('Variabile di stato x1_dot:');
fprintf('\n');
disp(x1_dot);
fprintf('\n');
x2_dot = x4;
disp('Variabile di stato x2_dot:');
fprintf('\n');
disp(x2_dot);
fprintf('\n');
x3_dot = teta1_dot_dot;
disp('Variabile di stato x3_dot:');
fprintf('\n');
disp(x3_dot);
fprintf('\n');
x4_dot = teta2_dot_dot;
disp('Variabile di stato x4_dot:');
fprintf('\n');
disp(x4_dot);
fprintf('\n');
disp('MATRICI SPAZIO DI STATO');
% MATRICI SPAZIO DI STATO
% A_ss = matrice Jacobiana [dx_dot_k/dx_k]
d1d1 = diff(x1_dot,x1);
d1d2 = diff(x1_dot,x2);
d1d3 = diff(x1_dot,x3);
d1d4 = diff(x1_dot,x4);
d2d1 = diff(x2_dot,x1);
d2d2 = diff(x2_dot,x2);
d2d3 = diff(x2_dot,x3);
d2d4 = diff(x2_dot,x4);
d3d1 = diff(x3_dot,x1);
d3d2 = diff(x3_dot,x2);
d3d3 = diff(x3_dot,x3);
d3d4 = diff(x3_dot,x4);
d4d1 = diff(x4_dot,x1);
d4d2 = diff(x4_dot,x2);
d4d3 = diff(x4_dot,x3);
d4d4 = diff(x4_dot,x4);
A_ss = [d1d1 d1d2 d1d3 d1d4;
d2d1 d2d2 d2d3 d2d4;
d3d1 d3d2 d3d3 d3d4;
d4d1 d4d2 d4d3 d4d4];
disp('Matrice A (matrice dinamica del sistema ):');
fprintf('\n');
disp(A_ss);
% Matrice ingresso del sistema
fprintf('\n');
% B_ss = matrice Jacobiana [dx_dot_k/du_k]
d1u1 = diff(x1_dot,tau1_ss);
d1u2 = diff(x1_dot,tau2_ss);
d2u1 = diff(x2_dot,tau1_ss);
d2u2 = diff(x2_dot,tau2_ss);
d3u1 = diff(x3_dot,tau1_ss);
d3u2 = diff(x3_dot,tau2_ss);
d4u1 = diff(x4_dot,tau1_ss);
d4u2 = diff(x4_dot,tau2_ss);
B_ss = [d1u1 d1u2; d2u1 d2u2;
d3u1 d3u2; d4u1 d4u2];
disp('Matrice B (matrice ingresso del sistema ):');
fprintf('\n');
disp(B_ss);
fprintf('\n');
% Matrice di uscita del sistema
% C_ss = matrice identità
disp('Matrice C (matrice di uscita del sistema ):');
fprintf('\n');
C_ss = eye(4);
disp(C_ss);
fprintf('\n');
% Matrice ingresso-uscita
% D_ss = 0 per sistemi dinamici
disp('Matrice D (matrice ingresso-uscita):');
fprintf('\n');
D_ss = zeros(4,2);
disp(D_ss);
fprintf('\n');
% dati del manipolatore
l1 = 1; %lunghezza braccio 1 in m
l2 = 1; %lunghezza braccio 2 in m
b1 = l1/2; %distribuzione di massa omogenea, posizione baricentro braccio 1
b2 = l2/2; %distribuzione di massa omogenea, posizione baricentro braccio 2
M1 = 1; %massa braccio 1 in kg
M2 = 1; %massa braccio 2 in kg
ag = 9.8; %accelerazione di gravità in m/s^2
J1G = 1/12*M1*l1^2; %momento di inerzia baricentrale braccio 1
J2G = 1/12*M2*l2^2; %momento di inerzia baricentrale braccio 2
J1O = J1G + M1*b1^2; %momento di inerzia braccio 1 rispetto ad O
J2A = J2G + M2*b2^2; %momento di inerzia braccio 2 rispetto ad A
disp('Matrice A con i valori:');
fprintf('\n');
disp(eval(A_ss));
fprintf('\n');
disp('Matrice B con i valori:');
fprintf('\n');
disp(eval(B_ss));
fprintf('\n');
disp('Matrice C con i valori:');
fprintf('\n');
C_ss = eye(4);
disp(C_ss);
fprintf('\n');
disp('Matrice D con i valori:');
fprintf('\n');
D_ss = zeros(4,2);
disp(D_ss);
fprintf('\n');
disp('LINEARIZZAZIONE ATTORNO AL PUNTO DI EQUILIBRIO STATICO');
%LINEARIZZAZIONE ATTORNO AL PUNTO DI EQUILIBRIO STATICO
%teta1=0 teta2=0 teta1_dot=0 teta2_dot=0
teta1 = pi;
teta2 = 0;
teta1_dot = 0;
teta2_dot = 0;
tau1_ss = 0;
tau2_ss = 0;
disp('Matrice A valutata nel punto di equilibrio:');
fprintf('\n');
disp(eval(A_ss));
fprintf('\n');
disp('Matrice B valutata nel punto di equilibrio:');
fprintf('\n');
disp(eval(B_ss));
fprintf('\n');
disp('Matrice C valutata nel punto di equilibrio:');
fprintf('\n');
C_ss = eye(4);
disp(C_ss);
fprintf('\n');
disp('Matrice D valutata nel punto di equilibrio:');
fprintf('\n');
D_ss = zeros(4,2);
disp(D_ss);
fprintf('\n');