Scribd
Upload
24 views

Modelado FT

This document defines variables and equations to model the kinetics and dynamics of a two-link robot arm. It derives the Lagrangian equations of motion and linearizes the system around an operating point to obtain state-space matrices representing the robot arm dynamics. The linearized model is then converted to a transfer function for control system analysis and design.

Uploaded by

nikolay molano
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
24 views

Modelado FT

This document defines variables and equations to model the kinetics and dynamics of a two-link robot arm. It derives the Lagrangian equations of motion and linearizes the system around an operating point to obtain state-space matrices representing the robot arm dynamics. The linearized model is then converted to a transfer function for control system analysis and design.

Uploaded by

nikolay molano
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 9

syms s u1 u2 c1 c2 c3 c4 ac1 ac2 t q1(t) q2(t) x1(q1) y1(q1) x2(q1,q2) y2(q1,q2) qt1 qt2 qpt1 q

%Defino Variables
qp1=diff(q1,t)

qp1(t) =

qp2=diff(q2,t)

qp2(t) =

%Calculo Posicion
x1=lc1*sin(q1)

x1(t) =

y1=-lc1*cos(q1)

y1(t) =

x2=l1*sin(q1)+lc2*sin(q1+q2)

x2(t) =

y2=-l1*cos(q1)-lc2*cos(q1+q2)

y2(t) =

%Calculo Vel x,y


x1d=diff(x1,t)

x1d(t) =

y1d=diff(y1,t)

y1d(t) =

x2d=diff(x2,t)

x2d(t) =

y2d=diff(y2,t)

y2d(t) =

1
%Calculo Velocidades v1 y v2
v1=simplify(sqrt(x1d^2+y1d^2));
v2=sqrt((x2d^2)+(y2d^2));
temp1=v1^2;
temp2=simplify(v2^2);

K1=(1/2)*m1*temp1+(1/2)*I1*(qp1^2);
K2=simplify((1/2)*m2*temp2+(1/2)*I2*(qp1+qp2)^2);
simplify(K2);
K=K1+K2 %Energia Cinetica Total

K(t) =

U1=-m1*lc1*g*cos(q1);
U2=-m2*l1*g*cos(q1)-m2*lc2*g*cos(q1+q2);
U=U1+U2 %Energia Potencial Total

U(t) =

L=K-U %Lagrange

L(t) =

%Cambio de variable
L1=subs(L,q1,qt1);
L2=subs(L,q2,qt2);
L3=subs(L,qp1,qpt1);
L4=subs(L,qp2,qpt2);

2
q1L=diff(L1,qt1);
q1L=simplify(subs(q1L,qt1,q1))%Derivada de L/q1

q1L(t) =

q2L=diff(L2,qt2);
q2L=simplify(subs(q2L,qt2,q2))%Derivada de L/q2

q2L(t) =

qd1L=diff(L3,qpt1);
qd1L=simplify(subs(qd1L,qpt1,qp1))%Derivada de L/qp1

qd1L(t) =

qd2L=diff(L4,qpt2);
qd2L=simplify(subs(qd2L,qpt2,qp2))%Derivada de L/qp2

qd2L(t) =

qd1tL=diff(qd1L,t)%Derivada respecto a t de dL/qp1

qd1tL(t) =

qd2tL=diff(qd2L,t)%Derivada respecto a t de dL/qp2

3
qd2tL(t) =

%Hallo ecuacion por Lagrange


ec1=simplify(qd1tL-q1L)-u1

ec1(t) =

ec2=simplify(qd2tL-q2L)-u2

ec2(t) =

ec1=subs(ec1,diff(qp1,t),ac1);
ec2=subs(ec2,diff(qp2,t),ac2);
%Despejo Aceleracion
f2temp=solve(ec1 == 0,ac1)

f2temp =

4
f4temp=solve(ec2 == 0,ac2)

f4temp =

%Realizo Cambio de variable a


%q1=C1 qp1=C2
%q2=C3 qp2=C4
f2temp=subs(f2temp,q1,c1);
f2temp=subs(f2temp,qp1,c2);
f2temp=subs(f2temp,q2,c3);
f2temp=subs(f2temp,qp2,c4);

f4temp=subs(f4temp,q1,c1);
f4temp=subs(f4temp,qp1,c2);
f4temp=subs(f4temp,q2,c3);
f4temp=subs(f4temp,qp2,c4);

%Funciones para control


f1=c2

f1 =

f2=f2temp

f2 =

f3=c4

f3 =

5
f4=f4temp

f4 =

%A simbolica
as=jacobian([f1;f2;f3;f4],[c1,c2,c3,c4])

as =

%B simboLica
bs=jacobian([f1;f2;f3;f4],[u1,u2])

bs =

C=[1 0 0 0 ; 0 0 1 0]

C = 2×4
1 0 0 0
0 0 1 0

%Punto de operacion
m1=0.4878

m1 = 0.4878

6
m2=0.3442

m2 = 0.3442

I1=0.0036

I1 = 0.0036

I2=0.0021

I2 = 0.0021

l1=0.1933

l1 = 0.1933

l2=0.1458

l2 = 0.1458

lc1=0.0741

lc1 = 0.0741

lc2=0.04849

lc2 = 0.0485

g=9.8

g = 9.8000

c1=pi/4

c1 = 0.7854

c2=0

c2 = 0

c3=pi/4

c3 = 0.7854

c4=0

c4 = 0

%Hallo u1 y u2 Referencia
u1=solve(f2==0,u1)

Warning: Solutions are valid under the following conditions: I1 + I2 + l1^2*m2 + lc1^2*m1 + lc2^2*m2
+ 2*l1*lc2*m2*cos(c3) ~= 0. To include parameters and conditions in the solution, specify the
'ReturnConditions' value as 'true'.
u1 =

u2=solve(f4==0,u2)

7
Warning: Solutions are valid under the following conditions: I2 + lc2^2*m2 ~= 0. To include parameters and
conditions in the solution, specify the 'ReturnConditions' value as 'true'.
u2 =

u1=eval(u1)

u1 = 0.8751

u2=eval(u2)

u2 = 0.1636

%Hallo matrices evaluadas


A=eval(as)

A = 4×4
0 1.0000 0 0
-26.7381 0 -0.0000 0
0 0 0 1.0000
-0.0000 0 -0.0000 0

B=eval(bs)

B = 4×2
0 0
37.5780 0
0 0
0 343.7240

D=[0 0 ; 0 0 ]

D = 2×2
0 0
0 0

states = {'beta' 'yaw' 'roll' 'phi'};


inputs = {'rudder' 'aileron'};
outputs = {'yaw rate' 'bank angle'};

sys_mimo = ss(A,B,C,D,'statename',states,...
'inputname',inputs,...
'outputname',outputs);
tf(sys_mimo)

ans =

From input "rudder" to output...


37.58 s^2 + 1.294e-13
yaw rate: ---------------------------
s^4 + 26.74 s^2 + 9.205e-14

-1.294e-13
bank angle: ---------------------------
s^4 + 26.74 s^2 + 9.205e-14

From input "aileron" to output...


-3.752e-13
yaw rate: ---------------------------
s^4 + 26.74 s^2 + 9.205e-14

8
343.7 s^2 + 9191
bank angle: ---------------------------
s^4 + 26.74 s^2 + 9.205e-14

Continuous-time transfer function.

You might also like