Dronn

Descargar como docx, pdf o txt
Descargar como docx, pdf o txt
Está en la página 1de 7

DEPARTAMENTO DE CIENCIAS DE LA ENERGIA Y

MECANICA

CARRERA DE INGENIERÍA MECATRÓNICA

ASIGNATURA: Automatización Mecatrónica

Taller N. 3

Profesor: Ing. Alejandro Gómez

Carlos Caicedo
César López

15 de Agosto del 2017 – Sangolquí


Tema:
Control de posición del DRON por Modos Deslizantes

Objetivos:
 Realizar el control del movimiento del Dron por el método de Modos Deslizantes
 Utilizar el software Matlab con su herramienta Simulink para visualizar de mejor
manera el desempeño del mismo
 Realizar una simulación del movimiento del Dron en Simulink

Teoría
Desarrollo
Para el sistema propuesto del control de movimiento del Dron necesitamos definir algunas
ecuaciones matemáticas, las cuales detallaremos a continuación:

 Ecuaciones del Modelamiento del Dron

cos ∅ 𝑠𝑒𝑛 𝜃 cos 𝜑 + 𝑠𝑒𝑛 ∅ 𝑠𝑒𝑛 𝜑


𝑥̈ = ( ) 𝑈1
𝑚
cos ∅ 𝑠𝑒𝑛 𝜃 𝑠𝑒𝑛 𝜑 − 𝑠𝑒𝑛 ∅ cos 𝜑
𝑦̈ = ( ) 𝑈1
𝑚
cos ∅ cos 𝜃
𝑧̈ = −𝑔 + ( ) 𝑈1
𝑚
𝐼𝑦 − 𝐼𝑧 𝑈2
∅̈ = 𝜃̇ 𝜑̇ +
𝐼𝑥 𝐼𝑥
𝐼𝑧 − 𝐼𝑥 𝑈3
𝜃̈ = ∅̇𝜑̇ +
𝐼𝑦 𝐼𝑦
𝐼𝑥 − 𝐼𝑦 𝑈4
𝜑̈ = 𝜃̇ ∅̇ +
𝐼𝑧 𝐼𝑧

 Relación Inicial entre Empuje y Velocidad Angular de los motores

𝑈1 = 𝑏(𝜔1 2 + 𝜔2 2 + 𝜔3 2 + 𝜔4 2 )

𝑈2 = 𝑙𝑏(𝜔4 2 − 𝜔2 2 )

𝑈3 = 𝑙𝑏(𝜔3 2 − 𝜔1 2 )

𝑈4 = 𝑑(−𝜔1 2 + 𝜔2 2 − 𝜔3 2 + 𝜔4 2
 Función de Transferencia de los Motores

7880
𝐺(𝑠)𝑚𝑜𝑡𝑜𝑟 =
𝑠 + 357,73

 Relación entre Fuerza y Velocidad Angular de los Motores

𝑈1 𝑈2 𝑈3 𝑈4
𝑤12 = + − −
4∗𝑏 4∗𝑙∗𝑏 2∗𝑙∗𝑏 4∗𝑑
𝑈1 −3 𝑈2 𝑈4
𝑤22 = − +
4∗𝑏 4∗𝑙∗𝑏 4∗𝑑
𝑈 1 𝑈2 𝑈3 𝑈4
𝑤32 = + + −
4∗𝑏 4∗𝑙∗𝑏 2∗𝑙∗𝑏 4∗𝑑
𝑈1 𝑈2 𝑈4
𝑤42 = + +
4∗𝑏 4∗𝑙∗𝑏 4∗𝑑

Vamos a definir las ecuaciones para el control por Modos Deslizantes de nuestras 4 variables
z, phi, theta, psi

 Para z
𝑠𝑧 = (𝑧𝑑𝑜𝑡 − 𝑧𝑟 𝑑𝑜𝑡) + (𝜆𝑧 ∗ (𝑧 − 𝑧𝑟 ))
𝑚
𝑢𝑒𝑞𝑧 = (−𝜆𝑧 ∗ (𝑧𝑑𝑜𝑡−𝑧𝑟 𝑑𝑜𝑡) + 𝑧𝑟 𝑑𝑜𝑡𝑑𝑜𝑡 − 𝑔) + ( )
𝑐𝑜𝑠𝜙 𝑐𝑜𝑠𝜃

𝑢𝑐𝑟𝑧 = 𝜂𝑧 𝑠𝑖𝑔𝑛 (𝑠𝑧 )

𝒛 = 𝑢𝑒𝑞𝑧 + 𝑢𝑐𝑟𝑧

 Para phi

𝑠𝜙 = (𝜙𝑑𝑜𝑡 − 𝜙𝑟 𝑑𝑜𝑡) + (𝜆𝜙 ∗ (𝜙 − 𝜙𝑟 ))

𝑚
𝑢𝑒𝑞𝜙 = (−𝜆𝜙 ∗ (𝜙𝑑𝑜𝑡−𝜙𝑟 𝑑𝑜𝑡) + 𝜙𝑟 𝑑𝑜𝑡𝑑𝑜𝑡 − 𝑔) + ( )
𝑐𝑜𝑠𝜙 𝑐𝑜𝑠𝜃

𝑢𝑐𝑟𝜙 = 𝜂𝜙 𝑠𝑖𝑔𝑛 (𝑠𝜙 )

𝜙 = 𝑢𝑒𝑞𝜙 + 𝑢𝑐𝑟𝜙
 Para theta

𝑠𝜃 = (𝜃𝑑𝑜𝑡 − 𝜃𝑟 𝑑𝑜𝑡) + (𝜆𝜃 ∗ (𝜃 − 𝜃𝑟 ))


𝑚
𝑢𝑒𝑞𝜃 = (−𝜆𝜃 ∗ (𝜃𝑑𝑜𝑡−𝜃𝑟 𝑑𝑜𝑡) + 𝜃𝑟 𝑑𝑜𝑡𝑑𝑜𝑡 − 𝑔) + ( )
𝑐𝑜𝑠𝜙 𝑐𝑜𝑠𝜃

𝑢𝑐𝑟𝜃 = 𝜂𝜃 𝑠𝑖𝑔𝑛 (𝑠𝜃 )

𝜃 = 𝑢𝑒𝑞𝜃 + 𝑢𝑐𝑟𝜃

 Para Psi

𝑠𝜓 = (𝜓𝑑𝑜𝑡 − 𝜓𝑟 𝑑𝑜𝑡) + (𝜆𝜓 ∗ (𝜓 − 𝜓𝑟 ))

𝑚
𝑢𝑒𝑞𝜓 = (−𝜆𝜓 ∗ (𝜓𝑑𝑜𝑡−𝜓𝑟 𝑑𝑜𝑡) + 𝜓𝑟 𝑑𝑜𝑡𝑑𝑜𝑡 − 𝑔) + ( )
𝑐𝑜𝑠𝜙 𝑐𝑜𝑠𝜃

𝑢𝑐𝑟𝜓 = 𝜂𝜓 𝑠𝑖𝑔𝑛 (𝑠𝜓 )

𝜓 = 𝑢𝑒𝑞𝜓 + 𝑢𝑐𝑟𝜓

Para realizar las ecuaciones en Matlab, definiremos algunos valores determinados que
tenemos para el sistema, como se observa en la tabla a continuación

Constante Valor
b
d
l
Ix
Iy
Iz
m
Lz 60
Ez 20
Lphi 5
Ephi 100
Ltheta 5
Etheta 100
Lpsi 5
Epsi 100
A continuación, vamos a observar la programación de Matlab para cada etapa de las descritas
en el sistema

Ecuaciones del Modelamiento del Dron

function Y = dron_Modelo(u1,u2,u3,u4,phi,theta,psi,phidot,thetadot,psidot)
m=0.52;Ix=0.006228;Iy=0.006225;Iz=0.01121;g=-9.8;
xdd=((cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi))/m)*u1;
ydd=((cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi))/m)*u1;
zdd=g+((cos(phi)*cos(theta))/m)*u1;
phidd=thetadot*psidot*(Iy-Iz)/Ix+(u2/Ix);
thetadd=phidot*psidot*(Iz-Ix)/Iy+(u3/Iy);
psidd=thetadot*phidot*(Ix-Iy)/Iz+(u4/Iz);

Y=[xdd ydd zdd phidd thetadd psidd];


end

Relación Inicial entre Empuje y Velocidad Angular de los motores


function Y = conver(w1,w2,w3,w4)
l=0.235; d=0.00000075; b=0.0000313;
u1=b*(w1^2+w2^2+w3^2+w4^2);
u2=l*b*(-w2^2+w4^2);
u3=l*b*(-w1^2+w3^2);
u4=d*(-w1^2+w2^2-w3^2+w4^2);
Y=[u1 u2 u3 u4];

end

Relación entre Fuerza y Velocidad Angular de los Motores


function Y = conver2(u1,u2,u3,u4)
b=0.0000313;d=0.00000075;l=0.235;
w1=sqrt(abs(u1/(4*b)-u3/(2*l*b)-u4/(4*d)));
w2=sqrt(abs(u1/(4*b)-u2/(2*l*b)+u4/(4*d)));
w3=sqrt(abs(u1/(4*b)+u3/(2*l*b)-u4/(4*d)));
w4=sqrt(abs(u1/(4*b)+u2/(2*l*b)+u4/(4*d)));

Y=[w1 w2 w3 w4];
end
Control por Modos Deslizantes
function Y =
Modos(z,zd,zr,zdr,zddr,phi,phid,phir,phidr,phiddr,theta,thetad,thetar,theta
dr,thetaddr,psi,psid,psir,psidr,psiddr)
g=-
9.8;Ix=0.006228;Iy=0.006225;Iz=0.01121;m=0.52;Lz=60;Lf=5;Lt=5;Lp=5;etaz=20;
etaf=100;etat=100;etap=100;
ueq1=(m/(cos(theta)*cos(phi)))*(-Lz*(zdr-zd)+zddr+g);
sz=zdr-zd+Lz*(zr-z);
ucr1=-(sz*etaz)/(0.5+abs(sz));
uz=ueq1+ucr1;
ueq2=(-Lf*(phidr-phid)+phiddr-thetad*psid*((Iy-Iz)/Ix))*Ix;
sf=phidr-phid+Lf*(phir-phi);
ucr2=-(sf*etaf)/(0.3+abs(sf));
uf=ueq2+ucr2;
ueq3=(thetaddr-phid*psid*((Iz-Ix)/Iy)-Lt*(thetadr-thetad))*Iy;
st=thetadr-thetad+Lt*(thetar-theta);
ucr3=-(st*etat)/(0.3+abs(st));
ut=ueq3+ucr3;
ueq4=(psiddr-thetad*phid*((Ix-Iy)/Iz)-Lp*(psidr-psid))*Iz;
sp=psidr-psid+Lp*(psir-psi);
ucr4=-(sp*etap)/(0.3+abs(sp));
up=ueq4+ucr4;

Y=[uz uf ut up];
end

Modelado en Simulink
En el gráfico observamos las etapas realizadas para el control por Modos Deslizantes del
Dron, vamos a describir etapa por etapa

También podría gustarte