Post on 01-Sep-2020
transcript
MATLAB-SIMULINK
1
Simulink
Ing. Alessandro Pisano
pisano@diee.unica.it
Uso di “Matlab Functions” all’interno dimodelli Simulink
Simulazione di sistemi Robotici
2
Interpreted Matlab functions
Matlab functions
3.
4
X0=0.1;
Y0=0.1;
A=1;
B=1;
C=5;
D=1;
5
=
y
xq ( )
+−
−=
DxyCy
BxyAxqM
( )qMq =
Modello in forma matriciale
6
function [ out ] = M_LV( in )
global A B C D
X=in(1); Y=in(2);
out(1)=A*X-B*X*Y;
out(2)=-C*Y+D*X*Y;
end
Function file “M_LV.m” da creare nella Current Directory
clear all
global A B C D
q_zero=[0.1 0.1];
A=1;
B=1;
C=4;
D=1;
Script di parametrizzazione
7
“Matlab Function” block
“Matlab Function” block in Matlab R2013
8
Consente di eseguire un “function file” Matlab direttamenteall’interno di un modello Simulink
Default
Se si fa doppio click sul blocco lo si apre nell’editor
9
Modifichiamo il file
function [y1,y2] = fcn(u1, u2, u3)%#codegen
y1 = sin(u1)+u2;y2= u1*u2+u3
Tre ingressi e due uscite
L’aspetto del blocco cambia e compaiono i nuovi terminali diinput ed output
L’aspetto del blocco cambia e compaiono i nuovi terminali diinput ed output
10
function [y1,y2] = fcn(u1, u2, u3)%#codegen
y1 = sin(u1)+u2;y2= u1*u2+u3
All’interno di un Matlab function non sono visibili le variabili delworkspace, neanche se queste vengono definite come globali
11
Modello preda predatore con il blocco Matlab function
12
function qdot = fcn(q)
%#codegen
A=1;
B=1;
C=5;
D=1;
x=q(1);
y=q(2);
qdot=[A*x-B*x*y; -C*y+D*x*y];
13
14
SIMULAZIONE DI MANIPOLATORI ROBOTICI
Piano orizzontale
m1 , m2 masse dei link
l1 , l2 lunghezze dei link
J1 , J2 momenti di inerzia dei link
r1 , r2 coeff. di attrito viscoso
C1(t) , C2(t) coppie applicate ai giunti
15
Lo SCARA, acronimo di Selective ComplianceAssembly Robot Arm, è un tipo di robot industriale, che muove un "braccio" sul piano orizzontale e una presa che può salire e scendere in quello verticale.
16
Modello matematico
Termini inerziali
Attrito viscoso
Coppie applicate
Coriolis e centripete
2
121
2
11
*
14
1lmJlmm ++= 2
222
*
24
1lmJm +=
( )( )( )
( )
,,,,
2
1
inNLFtC
tCRM +
=
+
( )
+
+++=
*
2212
*
2
212
*
2212
*
2
*
1
cos2
1
cos2
1cos
,
mllmm
llmmllmmmM
17
Coppie applicate
Siano le coordinate costanti di un punto di lavoro desideratodd
( ) ( ) ( )( )tKtKtC d
pd −+−= 1
Si applichino le seguenti coppie ai giunti (controllore PD)
( ) ( ) ( )( )tKtKtC d
pd −+−= 2
pd KK , “guadagni” costanti
( )( )
( )
( )( )
−
+
−=
t
tK
t
tKtC
d
d
pd
=
2
1
0
0
r
rR
( )
−
+
=
sin2
1
sin2
1
,,,2
212
2
212
llm
llm
FinNL
18
( )( )( )
=
t
ttq
( ) ( ) ( ) ( )( ) ( ) ( ) tqRtCtqtqFqMtq inNL −+= − ,1
Vettore delle variabili di giunto
Modello vettoriale in forma esplicita
( ) ( ) ( ) ( )( ) ( ) ( )tqRtCtqtqFtqqM inNL −+= ,
( )( )( )
( )
,,,,
2
1
inNLFtC
tCRM +
=
+
( )( )( )
=
tC
tCtC
2
1
Vettore delle coppie applicate ai giunti
19
m1=5;
m2=5;
l1=2;
l2=2;
J1=1;
J2=2;
r1=5;
r2=5;
m1star=0.25*m1*l1^2+J1+m2*l1^2;
m2star=J2+0.25*m2*l2^2;
q0=[pi/2;0];
q0dot=[0;0];
R=[r1 0;0 r2];
Kp=100;
Kd=20;
Script di parametrizzazione
20
( ) ( ) ( ) ( )( ) ( ) ( ) tqRtCtqtqFqMtq inNL −+= − ,1
21
function [ out ] = generaA( in )
m1=5;
m2=5;
l1=2;
l2=2;
J1=1;
J2=2;
m1star=0.25*m1*l1^2+J1+m2*l2^2;
m2star=J2+0.25*m2*l2^2;
alfa=in(1);
beta=in(2);
out(1,1)= m1star+m2star+m2*l1*l2*cos(beta);
out(1,2)= m2star+0.5*m2*l1*l2*cos(beta) ;
out(2,1)=m2star+0.5*m2*l1*l2*cos(beta);
out(2,2)=m2star;
end
function [ out ] = generaFinNL( in )
m1=5;
m2=5;
l1=2;
l2=2;
J1=1;
J2=2;
m1star=0.25*m1*l1^2+J1+m2*l2^2;
m2star=J2+0.25*m2*l2^2;
alfa=in(1);
beta=in(2);
alfadot=in(3);
betadot=in(4);
out(1,1)=m2*l1*l2*(alfadot*betadot+0.5*betadot^2)*sin(beta);
out(2,1)=-m2*l1*l2*0.5*alfadot^2*sin(beta);
end
Function files
22
tempo=q.time;
q1=q.data(1,:);
q2=q.data(2,:);
plot(tempo,q1,tempo,q2),grid
legend('q_1','q_2')
0 2 4 6 8 10 12 14 16 18 200
0.5
1
1.5
2
2.5
q1
q2
23
Modello compatto con Embedded Matlab Function
( ) ( ) ( ) ( )( ) ( ) ( ) tqRtCtqtqFqMtq inNL −+= − ,1
24
function qddot = fcn(q,qdot,C)
%#eml
m1=5;
m2=5;
l1=2;
l2=2;
J1=1;
J2=2;
m1star=0.25*m1*l1^2+J1+m2*l2^2;
m2star=J2+0.25*m2*l2^2;
r1=5;
r2=5;
alfa=q(1);
beta=q(2);
alfadot=qdot(1);
betadot=qdot(2);
A=[m1star+m2star+m2*l1*l2*cos(beta)
m2star+0.5*m2*l1*l2*cos(beta);
m2star+0.5*m2*l1*l2*cos(beta) m2star];
FinNL=[m2*l1*l2*(alfadot*betadot+0.5*betadot^2)*sin(
beta);
-m2*l1*l2*0.5*alfadot^2*sin(beta)];
R=[r1 0;0 r2];
qddot = inv(A)*(FinNL+C-R*qdot);
Embedded Matlab Function
25
Manipolatore planare a 2 gdl
26
( ) ( ) ( ) ( ) ( ) =++ qgtqqBtqqM
( )( )
+++
+++++++=
2
2
222
2
222212
2
2
2222122221
2
2
2
121
2
11
21cos
coscos2,
IlmIlmqllm
IlmqllmIqllllmIlmqqM
ccc
ccccc
( )
−−−=
0,
1
212
21qh
qhqhqhqqB
2212 sin qllmh c=
( ) ( ) 112122111211 coscoscos, qlqqlgmqglmqqg cc +++=
( ) ( )2122212 cos, qqglmqqg c +=
( )( )( )
( )( )( )
( )( )
( )( )
=
+
+
t
t
qqg
qqg
tq
tqqqB
tq
tqqqM
2
1
212
211
2
1
21
2
1
21,
,,,
27
( )( )( )
( )( )( )
( )( )
( )( )
=
+
+
t
t
qqg
qqg
tq
tqqqB
tq
tqqqM
2
1
212
211
2
1
21
2
1
21,
,,,
( ) ( ) ( ) ( ) ( ) =++ qgtqqBtqqM
( )( )( )
=
tq
tqtq
2
1( )
( )( )
=
t
tt
2
1
( )
( )( )
=
qg
qgqg
2
1
( ) ( ) ( ) ( )qgtqqBqqF += ,( ) ( ) ( ) =+ qqFtqqM ,
28
( ) ( ) ( ) qqFqMtq ,1 −= −
( ) ( ) ( ) =+ qqFtqqM ,
Modello vettoriale in forma esplicita
Coppie applicate
Siano le coordinate costanti di un punto di lavoro desideratodq1
dq2
Si applichino le seguenti coppie ai giunti (controllore PD con gravity compensation)
pd KK , “guadagni” costanti
( ) ( ) ( ) ( )( )tqqKtqKqgt d
pd −+−=
=
d
d
d
q
2
1
29
30
31
32
function [ out ] = creaM( in )
m1=5;
m2=5;
l1=2;
lc1=1;
l2=2;
lc2=1;
I1=1;
I2=2;
q1=in(1);
q2=in(2);
out(1,1)= m1*lc1^2+I1+m2*(l1^2+lc2^2+2*l1*lc2*cos(q2))+I2;
out(1,2)= m2*l1*lc2*cos(q2)+m2*lc2^2+I2;
out(2,1)=out(1,2);
out(2,2)=m2*lc2^2+I2;
end
33
function [ out ] = creaF( in )
m1=5;
m2=5;
l1=2;
lc1=1;
l2=2;
lc2=1;
J1=1;
J2=2;
g=9.81;
q1=in(1);
q2=in(2);
q1dot=in(3);
q2dot=in(4);
qdot=[q1dot;q2dot];
h=m2*l1*lc2*sin(q2)
B=[-h*q2dot -h*(q1dot+q2dot); h*q1dot 0];
grav=[m1*lc1*g*cos(q1)+m2*g*(lc2*cos(q1+q2)+l1*cos(q1)); m2*lc2*g*cos(q1+q2)];
out=B*qdot+grav;
end
34
function [ out ] = creag( in )
m1=5;
m2=5;
l1=2;
lc1=1;
l2=2;
lc2=1;
g=9.81;
q1=in(1);
q2=in(2);
g=[m1*lc1*g*cos(q1)+m2*g*(lc2*cos(q1+q2)+l1*cos(q1)); m2*lc2*g*cos(q1+q2)];
out=g;
end
35
36
37
38
39
−
−=
ll
xxK
d
t
d
t
p
40
41