3. роботтардың жеңілдетілген модельдері бойынша басқару синтезі



бет6/10
Дата18.10.2023
өлшемі387,34 Kb.
#118937
1   2   3   4   5   6   7   8   9   10
Байланысты:
дарын жумыс

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
lambda(3,1)*wx)+Vy*(lambda(1,2)*wz-lambda(3,2)*wx)+Vz*(lambda(1,3)*wz-lambda(3,3)*wx)+...


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;
y_actuator(10,1)=u1010;
%-----------------Estimator-------------------------
y_est=-L*z_est-L*L*M*x_vec-L*(F_d+F_v+F_con);
y=[y_kin;y_dyn;y_actuator;y_est];

Функция алдымен ыңғайлы болу үшін басқару жүйесінде қолданылатын айнымалы белгілерді енгізеді. Содан кейін бірқатар көмекші векторлар пайда болады. Әрі қарай Кинематика, Динамика, позициялық-траекториялық реттегіш, жетектер мен бақылаушының теңдеулері дәйекті түрде қалыптасады. Бұл жағдайда қосылған массаларды, массаларды, аэродинамикалық коэффициенттерді және инерция моменттерін есептеу үшін жеке функциялар қолданылады.






Достарыңызбен бөлісу:
1   2   3   4   5   6   7   8   9   10




©emirsaba.org 2024
әкімшілігінің қараңыз

    Басты бет