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

qq

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