攻击时间控制的动态逆三维制导律源代码

2019-04-13 21:47发布

站内文章 / 模拟电子
13779 0 1097
%参考文献：《攻击时间控制的动态逆三维制导律》，哈尔滨工程大学学报，2010 %编程难点：视线坐标系与参考坐标系间的转换关系 clear clc V=300;% 导弹速度，m/s x=-5000;%进攻弹坐标，m y=5000; z=6000; xt=0;%目标坐标，m yt=0; zt=0; Rm=sqrt((x-xt)^2+(y-yt)^2+(z-zt)^2); DRm=0; theta_m=20*pi/180; Dtheta_m=0; fea_m=10*pi/180; theta_s=atan((zt-z)/sqrt((xt-x)^2+(yt-y)^2)); fea_s=atan((yt-y)/(xt-x)); Dtheta_s=0; Dfea_s=0; g=9.8;%重力加速度 k1=10; c1=0.7; c2=0.9; k3=5; Td=45; n=1; t=0; dt=0.001; while DRm<=0 &&Rm>=0 et=Td-t-Rm/V; k2=(1-cos(theta_m)+c1*et*cos(theta_m)/(et+c2))/et; Aym=k1*abs(DRm)*Dtheta_s/cos(theta_m);%增广比例导引律 if Aym>8*g Aym=8*g; end if Aym<-8*g Aym=-8*g; end if fea_m>=0 && fea_m=-pi/2 && fea_m<0 Azm=-(V*(1-k2*et)*sin(theta_m)*Dtheta_m/cos(theta_m)+k2*V-k2*V*cos(theta_m)*cos(fea_m))/sqrt(1-((1-k2*et)/cos(theta_m))^2)+k3*V*cos(theta_m)*(fea_m+acos((1-k2*et)/cos(theta_m)))+V^2/Rm*sin(fea_m)*(1-sin(theta_m)*cos(theta_m)*cos(fea_m)*tan(theta_s)); end % Azm=-k1*abs(DRm)*Dfea_s/cos(fea_m);%方法2：偏航通道也采用增广比例导引律 if Azm>8*g Azm=8*g; end if Azm<-8*g Azm=-8*g; end DRm=-V*cos(theta_m)*cos(fea_m); Dtheta_m=Aym/V+V*cos(theta_m)*sin(fea_m)*sin(fea_m)*tan(theta_s)/Rm+V*sin(theta_m)*cos(fea_m)/Rm; Dfea_m=-Azm/(V*cos(theta_m))+V*sin(fea_m)/(Rm*cos(theta_m))-V*sin(theta_m)*cos(fea_m)*tan(theta_s)*sin(fea_m)/Rm; Dtheta_s=-V*sin(theta_m)/Rm; Dfea_s=-V*cos(theta_m)*sin(fea_m)/(Rm*cos(theta_s)); Rm=Rm+DRm*dt; theta_m=theta_m+Dtheta_m*dt; fea_m=fea_m+Dfea_m*dt; theta_s=theta_s+Dtheta_s*dt; fea_s=fea_s+Dfea_s*dt; %视线坐标系转换到参考坐标系- Tran=[cos(theta_s)*cos(fea_s) sin(theta_s) -cos(theta_s)*sin(fea_s); -sin(theta_s)*cos(fea_s) cos(theta_s) sin(theta_s)*sin(fea_s); sin(fea_s) 0 cos(fea_s)]; DP=Tran'*[V*cos(theta_m)*cos(fea_m);V*cos(theta_m)*sin(fea_m);V*sin(theta_m)]; Dx=DP(1); Dy=DP(2); Dz=DP(3); x=x+Dx*dt; y=y+Dy*dt; z=z+Dz*dt; P_s(:,n)=[x;y;z]; S_s(:,n)=[Rm;theta_m;fea_m;et]; A_s(:,n)=[Aym/g;Azm/g]; n=n+1; t=t+dt; end figure(1)%由于plot3函数默认采用美国坐标系，需要转化为我们习惯的俄式坐标系 plot3(P_s(2,:)/1000,P_s(1,:)/1000,-P_s(3,:)/1000); xlabel('Y/km') ylabel('X/km') zlabel('Z/km') figure(2) plot((1:n-1)*dt,S_s(1,:)) xlabel('time/s') ylabel('相对距离') figure(3) plot((1:n-1)*dt,S_s(2,:)*180/pi) xlabel('time/s') ylabel('俯仰通道前置角/deg') figure(4) plot((1:n-1)*dt,S_s(3,:)*180/pi) xlabel('time/s') ylabel('偏航通道前置角/deg') figure(5) plot((1:n-1)*dt,S_s(4,:)) xlabel('time/s') ylabel('et/s') figure(6) plot((1:n-1)*dt,A_s(1,:)) xlabel('time/s') ylabel('Amy/g') title('俯仰通道控制量') figure(7) plot((1:n-1)*dt,A_s(2,:)) xlabel('time/s') ylabel('Amz/g') title('偏航通道控制量')

热门文章