3. роботтардың жеңілдетілген модельдері бойынша басқару синтезі
жүктеу/скачать 387,34 Kb.
|
| uz
y y y y N если abs P P x sign P если abs P , 2 2 3 3 200 x y P P , 2 3 1 2 / / 2 x ux x uy P F P N z , 1 1 2 / x uy x P N z P , 2 3 1 2 / / 2 y uy y ux P F P N z , 1 1 2 / x ux y P N z P , иначе 2 2 2 tan 2 ,y x a P P , 3 3 3 tan 2 ,y x a P P , 4 3 , мұндағы a tan 2-бұл доменде анықталған арктангенс функциясы. Позициялық - траекториялық тұйық жүйені модельдеу үшін дирижабльді басқару біз MATLAB пакетін қолданамыз. Модельдеу бағдарламасы екі негізгі файлдан тұрады-сахна- рия main_dir.M және ode функциялары fun_dir.m. қосымша қолданылады аэродинамикалық коэффициенттер жазылған бірнеше функциялар- таспалар және дирижабль мен қоршаған ортаның басқа параметрлері. 90 Main_dir сценарий файлы.m формасы бар clear all clc close all % Initial conditions of airship dynamical equations x_vec=[0;0;0;0;0;0]; % Initial conditions of the equations of airship kinemat-ics y_vec=[0;2000;0;0.5;0.01;0]; % Initial conditions of the equations of actuators delta_vec=[40;0;40;0;200;0;0;200;0;0]; % Initial conditions of estimator z_est=[0;0;0;0;0;0]; % current vector of vars we integrate all_vars_vec_0 = [y_vec;x_vec;delta_vec;z_est]; % integration procedure options options = odeset('RelTol',1e-3,'AbsTol',ones(28,1)*1e-3); % start and finish time t_start = 0; t_end = 20; % estimator parameters L_11=1; L=diag([L_11;L_11;L_11;L_11;L_11;L_11]); %volume of airship Vobol=4300; [t1,x1]=ode45('fun_dir',[t_start t_end], all_vars_vec_0,options,L,Vobol); figure(1); hold on; grid on; plot(t1,x1(:,2)); figure(2); hold on; grid on; plot(x1(:,1),x1(:,3)); figure(3); hold on; grid on; plot(t1,x1(:,7:12)); figure(4); hold on; grid on; plot(t1,x1(:,13:22)); Бағдарламада барлық айнымалылар тазаланады және барлық терезелер жабылады, бастапқы шарттар орнатылады, интеграция дәлдігі параметрлері орнатылады, бақылаушы параметрлері мен дирижабль көлемі орнатылады және ode45 операторының көмегімен сандық интеграция жасалады. Бағдарламаның соңында модельдеу нәтижелері экранға шығарылады. 91 Ode файлы-fun_dir функциясы.m формасы бар function y=fun_dir(t,x,flag,L,Vobol) % x coordinate x0=x(1); % y coordinate y0=x(2); % z coordinate z0=x(3); % yaw angle psi=x(4); % pitch upsilon=x(5); % angle of roll gamma=x(6); % X component of the linear velocity Vx=x(7); % Y component of the linear velocity Vy=x(8); % Z component of the linear velocity Vz=x(9); % X component of the angular velocity wx=x(10); % Y component of the angular velocity wy=x(11); % Z component of the angular velocity wz=x(12); % force of the first main drive P1=x(13); % angle of the first main drive alpha1=x(14); % force of the second main drive P2=x(15); % angle of the second main drive alpha2=x(16); % force of the first tail drive P3=x(17); % vertical angle of the first tail drive alpha3=x(18); % horizontal angle of the first tail drive beta3=x(19); % force of the second tail drive P4=x(20); % vertical angle of the secondtail drive alpha4=x(21); % horizontal angle of the secondtail drive 92 beta4=x(22); % estimator variables z_est(1,1)=x(23); z_est(2,1)=x(24); z_est(3,1)=x(25); z_est(4,1)=x(26); z_est(5,1)=x(27); z_est(6,1)=x(28); % additional vectors x_vec=[Vx;Vy;Vz;wx;wy;wz]; V_vec=[Vx; Vy; Vz]; omega_vec=[wx; wy; wz]; angles_vec=[psi;upsilon;gamma]; %-------------Kinematics equations--------------------- A = [cos(upsilon)*cos(psi), sin(upsilon), cos(upsilon)*sin(psi); -sin(gamma)*sin(psi) - cos(gamma)*cos(psi)*sin(upsilon), cos(gamma)*cos(upsilon), cos(psi)*sin(gamma) - cos(gamma)*sin(upsilon)*sin(psi); cos(psi)*sin(gamma)*sin(upsilon) - cos(gamma)*sin(psi), -cos(upsilon)*sin(gamma), cos(gamma)*cos(psi) + sin(gamma)*sin(upsilon)*sin(psi)]; Aw = [0 , cos(gamma)/cos(upsilon) , -sin(gamma)/cos(upsilon); 0 , sin(gamma) , cos(gamma) ; 1 , -cos(gamma)*tan(upsilon) , sin(gamma)*tan(upsilon) ]; y_kin=[A' zeros(3,3); zeros(3,3) Aw]*[V_vec;omega_vec]; %-------------Dynamics equations----------------------- % ballonets pressure dP_b=[500;500]; % added mass matrix lambda1=lambda_ii(y0,dP_b); lambda=zeros(6,6); lambda(1,1)=lambda1(1,1); lambda(2,2)=lambda1(1,2); lambda(3,3)=lambda1(1,3); lambda(4,4)=lambda1(1,4); lambda(5,5)=lambda1(1,5); 93 lambda(6,6)=lambda1(1,6); % inertia matrix and its elements J_m=J(y0,dP_b); Jx=J_m(1,1); Jxy=J_m(1,2); Jxz=J_m(1,3); Jy=J_m(2,2); Jyz=J_m(2,3); Jz=J_m(3,3); % vector of the center of mass coordinates and the coor-dinates r_m=r(y0); xT=r_m(1); yT=r_m(2); zT=r_m(3); % airship inertia matrix M = [m(y0,dP_b)+lambda(1,1) , 0 , 0 , 0 , m(y0,dP_b)*zT , -m(y0,dP_b)*yT; 0 , m(y0,dP_b)+lambda(2,2), 0 , -m(y0,dP_b)*zT , 0 , m(y0,dP_b)*xT ; 0 , 0 , m(y0,dP_b)+lambda(3,3), m(y0,dP_b)*yT , -m(y0,dP_b)*xT , 0 ; 0 , -m(y0,dP_b)*zT , m(y0,dP_b)*yT , Jx+lambda(4,4) , -Jxy , -Jxz ; m(y0,dP_b)*zT , 0 , -m(y0,dP_b)*xT , -Jxy , Jy+lambda(5,5) , -Jyz ; -m(y0,dP_b)*yT , m(y0,dP_b)*xT , 0 , -Jxz , -Jyz , Jz+lambda(6,6)]; % Determination of air speed Vwx1=3; Vwy1=0; Vwz1=0; Vw_vec = (A')^(-1)*[Vwx1; Vwy1; Vwz1]; Vwx=Vw_vec(1); Vwy=Vw_vec(2); Vwz=Vw_vec(3); V_air=((Vx-Vwx)^2+(Vy-Vwy)^2+(Vz-Vwz)^2)^(0.5); if(V_air>0) beta=asin((Vz-Vwz)/V_air); if(Vy-Vwy==0&&Vx-Vwx==0) alpha=0; 94 else alpha=-asin((Vy-Vwy)/V_air/cos(beta)); end else alpha=0; beta=0; end % calculation of aerodynamic coefficients c_vec=aerodyn_coeff(V_air,alpha,beta,wx,wy,wz); cx=c_vec(1); cy=c_vec(2); cz=c_vec(3); mx=c_vec(4); my=c_vec(5); mz=c_vec(6); % airship characteristic area s=Vobol^(2/3); L_dir=41; x12=0; x34=-21.63; y12=0; y34=0; z1=-10.7; z3=-4.1; % The universal gas constant R=8.31; % Acceleration of gravity g=9.807; P0=101000; mu_air0=0.029; T0=293; % Air density at a given height mu_air=mu_air0*P0*exp(-mu_air0*g*y0/(R*(T0-0.0065*y0)))/(R*(T0-0.0065*y0)); % The components of the dynamic forces and moments F_d1=-m(y0,dP_b)*(wy*Vz-Vy*wz+yT*wx*wy+zT*wx*wz-xT*(wy^2+wz^2))-Vx*(lambda(3,1)*wy-lambda(2,1)*wz)-Vy*(lambda(3,2)*wy-lambda(2,2)*wz)-Vz*(lambda(3,3)*wy-lambda(2,3)*wz)-... wx*(lambda(3,4)*wy-lambda(2,4)*wz)-wy*(lambda(3,5)*wy-lambda(2,5)*wz)-wz*(lambda(3,6)*wy-lambda(2,6)*wz)-0.5*cx*s*mu_air*V_air+40*(cos(alpha1)+cos(alpha2)); F_d2=-m(y0,dP_b)*(Vx*wz-Vz*wx+xT*wx*wy-yT*(wx^2+wz^2)+zT*wy*wz)+Vx*(lambda(1,1)*wz- 95
wx*(lambda(1,4)*wz-lambda(3,4)*wx)+wy*(lambda(1,5)*wz-lambda(3,5)*wx)+wz*(lambda(1,5)*wz-lamb-da(3,6)*wx)+0.5*cy*s*mu_air*V_air+40*(sin(alpha1)+sin(alpha2)); F_d3=-m(y0,dP_b)*(-Vx*wy+Vy*wx+xT*wx*wz+yT*wy*wz-zT*(wx^2+wy^2))+Vx*(lambda(2,1)*wx-lambda(1,1)*wy)+Vy*(lambda(2,2)*wx-lambda(1,2)*wy)+Vz*(lambda(2,3)*wx-lambda(1,3)*wy)-... wx*(lambda(2,4)*wx-lambda(1,4)*wy)+wy*(lambda(2,5)*wx-lambda(1,5)*wy)+wz*(lambda(2,6)*wx-lambda(1,6)*wy)+0.5*cz*s*mu_air*V_air; F_d4=wx*(Jxz*wy-Jxy*wz)+Jyz*(wy^2-wz^2)+(Jy-Jz)*wy*wz-m(y0,dP_b)*(-yT*Vx*wy+yT*Vy*wx-zT*Vx*wz+zT*Vz*wx)-Vx*(lambda(6,1)*wy-lambda(5,1)*wz)-Vy*(lambda(6,2)*wy-lambda(5,2)*wz)-Vz*(lambda(6,3)*wy-lambda(5,3)*wz)-... wx*(lambda(6,4)*wy-lambda(5,4)*wz)-wy*(lambda(6,5)*wy-lambda(5,5)*wz)-wz*(lambda(6,6)*wy-lamb-da(5,6)*wz)+0.5*mx*s*L_dir*mu_air*V_air+40*z1*(sin(alpha2)-sin(alpha1)); F_d5=wy*(Jxy*wz-Jyz*wx)+Jxz*(wz^2-wx^2)+(Jz-Jx)*wx*wz-m(y0,dP_b)*(xT*Vx*wy-xT*Vy*wx-zT*Vy*wz+zT*Vz*wy)-Vx*(lambda(4,1)*wz-lambda(6,1)*wx)-Vy*(lambda(4,2)*wz-lambda(6,2)*wx)-Vz*(lambda(4,3)*wz-lambda(6,3)*wx)-... wx*(lambda(4,4)*wz-lambda(6,4)*wx)-wy*(lambda(4,5)*wz-lambda(6,5)*wx)-wz*(lambda(4,6)*wz-lamb-da(6,6)*wx)+0.5*my*s*L_dir*mu_air*V_air+40*z1*(cos(alpha1)-cos(alpha2)); F_d6=wz*(Jyz*wx-Jxz*wy)+Jxy*(wx^2-wy^2)+(Jx-Jy)*wx*wy-m(y0,dP_b)*(xT*Vx*wz-xT*Vz*wx+yT*Vy*wz-yT*Vz*wy)-Vx*(lambda(5,1)*wx-lambda(4,1)*wy)-Vy*(lambda(5,2)*wx-lambda(4,2)*wy)-Vz*(lambda(5,3)*wx-lambda(4,3)*wy)-... wx*(lambda(5,4)*wx-lambda(4,4)*wy)-wy*(lambda(5,5)*wx-lambda(4,5)*wy)-wz*(lambda(5,6)*wx-lamb- 96 da(4,6)*wy)+0.5*mz*s*L_dir*mu_air*V_air+40*x12*(sin(alpha1)+sin(alpha2))-40*y12*(cos(alpha1)+cos(alpha2)); % vector of dynamic forces and moments F_d=[F_d1; F_d2; F_d3; F_d4; F_d5; F_d6]; % component of the gravitational force Gx=-sin(upsilon)*m(y0,dP_b)*g; Gy=-cos(upsilon)*cos(gamma)*m(y0,dP_b)*g; Gz=cos(upsilon)*sin(gamma)*m(y0,dP_b)*g; % components of moment of the gravitational force Ngx=(cos(upsilon)*cos(gamma)*zT+cos(upsilon)*sin(gamma)*yT)*m(y0,dP_b)*g; Ngy=(cos(upsilon)*sin(gamma)*xT-sin(upsilon)*zT)*m(y0,dP_b)*g; Ngz=(sin(upsilon)*yT-cos(upsilon)*cos(gamma)*xT)*m(y0,dP_b)*g; % Archimedes force components Ax=sin(upsilon)*mu_air*g*Vobol; Ay=cos(upsilon)*cos(gamma)*mu_air*g*Vobol; Az=-cos(upsilon)*sin(gamma)*mu_air*g*Vobol; % the vector of external forces F_v=[Gx+Ax; Gy+Ay; Gz+Az; Ngx; Ngy; Ngz]; disturbance=[0;0;0;0;0;0]; % calculation of control forces and moments Fconx=P1*cos(alpha1)+P2*cos(alpha2)+P3*cos(alpha3)*cos(beta3)+P4*cos(alpha4)*cos(beta4); Fco-ny=P1*sin(alpha1)+P2*sin(alpha2)+P3*sin(alpha3)+P4*sin(alpha4); Fconz=P3*cos(alpha3)*sin(beta3)+P4*cos(alpha4)*sin(beta4); Nconx=z1*(P2*sin(alpha2)-P1*sin(alpha1))+y34*(P3*cos(alpha3)*sin(beta3)+P4*cos(alpha4)*sin(beta4))+z3*(P4*sin(alpha4)-P3*sin(alpha3)); Ncony=z1*(P1*cos(alpha1)-P2*cos(alpha2))-x34*(P3*cos(alpha3)*sin(beta3)+P4*cos(alpha4)*sin(beta4))+z3*(P3*cos(beta3)*cos(alpha3)-P4*cos(beta4)*cos(alpha4)); Nconz=x12*(P1*sin(alpha1)+P2*sin(alpha2))-y12*(P1*cos(alpha1)+P2*cos(alpha2))+x34*(P3*sin(alpha3)+P4*sin(alpha4))-y34*(P3*cos(alpha3)*cos(beta3)+P4*cos(alpha4)*cos(beta4)); 97 % formation of the output vector F_con=[Fconx; Fcony; Fconz; Nconx; Ncony; Nconz]; y_dyn=M^(-1)*(F_d+F_v+F_con+disturbance); %-------------------Regulator------------------------- angles_vec_dt=Aw*omega_vec; dAw=[0, -sin(gamma)*angles_vec_dt(3)/cos(upsilon)+cos(gamma)/cos(upsilon)^2*sin(upsilon)*angles_vec_dt(2), -cos(gamma)*angles_vec_dt(3)/cos(upsilon)-sin(gamma)/cos(upsilon)^2*sin(upsilon)*angles_vec_dt(2);... 0, cos(gamma)*angles_vec_dt(3), -sin(gamma)*angles_vec_dt(3);... 0, sin(gamma)*angles_vec_dt(3)*tan(upsilon)-cos(gamma)*(1+tan(upsilon)^2)*angles_vec_dt(2), cos(gamma)*angles_vec_dt(3)*tan(upsilon)+sin(gamma)*(1+tan(upsilon)^2)*angles_vec_dt(2)]; A2 = [1 0 0; 0 1 0; 0 0 1]; a161=-2000; upsilon_0=-0.001*(y0+a161); if (upsilon_0>0.1) upsilon_0=0.1; elseif(upsilon_0<-0.1) upsilon_0=-0.1; end % positioning coefficients xc=-5000; zc=-5000; % references for speeds vk=10; psi_01=atan2((zc-z0),(xc-x0))-atan2(Vz,Vx); if (psi-psi_01<-pi) psi_01=psi_01-2*pi; end; if (psi-psi_01>pi) psi_01=psi_01+2*pi; end; 98 psi_0=psi+(psi_01-psi)/20; gamma_0=0; alpha0=0.1*3.14/180; if(Vx-Vwx>7) alpha0=(2.044e-005*(Vx-Vwx)^5-0.001068*(Vx-Vwx)^4+0.01869*(Vx-Vwx)^3-0.1204*(Vx-Vwx)^2+0.2082*(Vx-Vwx)+0.3561)*3.14/180; elseif(Vx-Vwx>20) alpha0=0.47*3.14/180; end Vy_0=-V_air*sin(alpha0)*cos(beta); A3 = [-psi_0; -upsilon_0; -gamma_0]; A4 = [1 0; 0 1]; A5 = [-vk; -Vy_0]; % path manifold psi_tr=A2*angles_vec+A3; for i=1:3 while( psi_tr(i) > pi ) psi_tr(i) = psi_tr(i) - 2*pi; end while( psi_tr(i) < -pi ) psi_tr(i) = psi_tr(i) + 2*pi; end end % path manifold time derivative psi_tr_dt=A2*Aw*omega_vec; % speed manifold X_vec1=[Vx;Vy]; psi_v=A4*X_vec1+A5; % control coefficients t11=0.05; t22=0.05; T1 = [2*t11+2*t22 0 0; 0 2*t11+2*t22 0; 0 0 2*t11+2*t22]; T2 = [4*t11*t22 0 0; 0 4*t11*t22 0; 0 0 4*t11*t22]; t311=0.05; t322=0.05; T3=[t311 0; 0 t322]; 99 % control Minv=M^(-1); Mdv=[Minv(1,:); Minv(2,:); Minv(4,:); Minv(5,:); Minv(6,:)]; Mcon=[Minv(1,1) Minv(1,2) Minv(1,4) Minv(1,5) Minv(1,6); Minv(2,1) Minv(2,2) Minv(2,4) Minv(2,5) Minv(2,6); Minv(4,1) Minv(4,2) Minv(4,4) Minv(4,5) Minv(4,6); Minv(5,1) Minv(5,2) Minv(5,4) Minv(5,5) Minv(5,6); Minv(6,1) Minv(6,2) Minv(6,4) Minv(6,5) Minv(6,6)]; psi_s=[A4^(-1)*T3*psi_v; (A2*Aw)^(-1)*(A2*dAw*omega_vec+T2*psi_tr+T1*psi_tr_dt)]; F_est = L*M*x_vec+z_est; F_con_reg=-Mcon^(-1)*(Mdv*(F_d+F_v+F_est)+psi_s); max_electro = 200; max_main = 3000; Fux=F_con_reg(1); Fuy=F_con_reg(2); Nux=F_con_reg(3); Nuy=F_con_reg(4); Nuz=F_con_reg(5); P31=max_electro; P41=P31; beta31=0; beta41=beta31; P3y=Nuz/2/x34; if(abs(P3y)>P31) P3y=P31*sign(P3y); end P3x=(P31^2-P3y^2)^(1/2); P2x=(Fux-2*P3x-Nuy/z1)/2; P1x=Nuy/z1+P2x; P2y=(Fuy-2*P3y+Nux/z1)/2; P1y=-Nux/z1+P2y; P11=sqrt(P1x^2+P1y^2); alpha11=atan2(P1y,P1x); P21=sqrt(P2x^2+P2y^2); alpha21=atan2(P2y,P2x); alpha31=atan2(P3y,P3x); alpha41=alpha31; % end forces distrubution if(P11>max_main) P11=max_main; end; 100 if(P21>max_main) P21=max_main; end; if(P31>max_electro) P31=max_electro; end; if(P41>max_electro) P41=max_electro; end; u=[P11;alpha11;P21;alpha21;P31;alpha31;beta31;P41;alpha41;beta41]; %-------------------Model of actuators----------------- u22=-5*x(2)+5*u(2); if(abs(u22)>3.1415926/9) u22=3.1415926/9*sign(u22); end; u44=-5*x(4)+5*u(4); if(abs(u44)>3.1415926/9) u44=3.1415926/9*sign(u44); end; u66=-5*x(6)+5*u(6); if(abs(u66)>3.1415926/9) u66=3.1415926/9*sign(u66); end; u77=-5*x(7)+5*u(7); if(abs(u77)>3.1415926/9) u77=3.1415926/9*sign(u77); end; u99=-5*x(9)+5*u(9); if(abs(u99)>3.1415926/9) u99=3.1415926/9*sign(u99); end; u1010=-5*x(10)+5*u(10); if(abs(u1010)>3.1415926/9) u1010=3.1415926/9*sign(u1010); end; y_actuator(1,1)=-5*P1+5*u(1); y_actuator(2,1)=u22; y_actuator(3,1)=-5*P2+5*u(3); y_actuator(4,1)=u44; y_actuator(5,1)=-5*P3+5*u(5); y_actuator(6,1)=u66; y_actuator(7,1)=u77; y_actuator(8,1)=-5*P4+5*u(8); 101
y_actuator(9,1)=u99;
Функция алдымен ыңғайлы болу үшін басқару жүйесінде қолданылатын айнымалы белгілерді енгізеді. Содан кейін бірқатар көмекші векторлар пайда болады. Әрі қарай Кинематика, Динамика, позициялық-траекториялық реттегіш, жетектер мен бақылаушының теңдеулері дәйекті түрде қалыптасады. Бұл жағдайда қосылған массаларды, массаларды, аэродинамикалық коэффициенттерді және инерция моменттерін есептеу үшін жеке функциялар қолданылады. жүктеу/скачать 387,34 Kb. Достарыңызбен бөлісу:
|