Dronn
Dronn
Dronn
MECANICA
Taller N. 3
Carlos Caicedo
César López
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:
𝑈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
𝑈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
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);
end
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