北方工业大学建模与仿真2011.ppt

自适应动态逆控制 自适应项 Lyapunov稳定性的渐进稳定性 仿真程序设计 function [dxv]=quad(xv,uv,kv) Mass=2.3 ; %kg LeverX=0.60 ; %meter LeverY=0.35 ; %meter Ixx=8.04*10^-3 ; %kgm2 Iyy=8.46*10^-3 ; %kgm2 Izz=14.68*10^-3 ; %kgm2 kl=6000 ; zl=Mass*9.8/kl ; Ir=5.861*10^-5 ; %propeller gyroscoptic inertia ub=xv(1) ; %m/s vb=xv(2) ; %m/s wb=xv(3) ; %m/s pb=xv(4) ; %rad/s qb=xv(5) ; %rad/s rb=xv(6) ; %rad/s phi=xv(7) ; %rad the=xv(8) ; %rad psi=xv(9) ; %rad xe=xv(10) ; %m ye=xv(11) ; %m ze=xv(12) ; %m %define the state varibale of four motor w1=xv(13) ; %rad/s w2=xv(14) ; %rad/s w3=xv(15) ; %rad/s w4=xv(16) ; %rad/s u1=uv(1) ; %u1 1th motor u2=uv(2) ; %u2 2th motor u3=uv(3) ; %u3 3th motor u4=uv(4) ; %u4 4th motor k1=kv(1) ; k2=kv(2) ; k3=kv(3) ; %define the propel force kf=kv(4) ; kt=kv(5) ; Mp=kf*LeverX*(-w1*w1 + w2*w2 + w3*w3 - w4*w4) ; Mq=kf*LeverY*(-w1*w1 + w2*w2 - w3*w3 + w4*w4) ; Mr=kt*(LeverX^2+LeverY^2)^0.5*( w1*w1 + w2*w2 - w3*w3 - w4*w4) ; Fz=kf* ( w1*w1 + w2*w2 + w3*w3 + w4*w4) ; %define the ground support or elastic force of body landing if (zl-ze)0 Fl=kl*(zl-ze) ; else Fl=0 ; end %define body motion kub=0.2;kvb=0.2;kwb=0.25; dub= -( -rb*vb + qb*wb) + 9.8*sin(the) + kub*ub/Mass; dvb= -( rb*ub - pb*wb) - 9.8*cos(the)*sin(phi) + kvb*vb/Mass; dwb= -(-qb*ub +pb*vb ) - 9.8*cos(the)*cos(phi) + Fz/Mass + Fl/Mass + kwb*wb/Mass; %dpb= - ( -rb*Iyy*qb +qb*Izz*rb)/Ixx + Mp/Ixx %dqb= - ( rb*Ixx*pb -pb*Izz*rb)/Iyy + Mq/Iyy %drb= - (-qb*Ixx*pb +pb*Iyy*qb )/Izz + Mr/Izz kpb=0.00001; kqb=0.00