平面三自由度弹道仿真matlab代码

clear; close all; r2d = 180/pi;%常数 draw = 1;%绘图控制符 method = "eul";%欧拉法积分 %% 全局变量 global Curvature Selfrotation rotate jd0 wd0 h0 A0 dt; Curvature = 0;%不考虑曲率 Selfrotation = 0;%不考虑自转 rotate = "321";%坐标转序321 jd0 = 0;% 发射点经纬度射向 wd0 = 0; h0 = 0; A0 = 0; dt = 0.5;%步长 [x,fval] = fminsearch(@Traj_optimize,[-5,-4]);%弹道优化 让落点能打在目标上 不优化的可以用下面一句直接赋值 %x = [-3.48726480046295 -2.77917373062853]; alpha_max = x(1); beta_max = x(2); %% 初始化结构体 state.t = 0; state.m = 1000;%初始质量 state.const.alpha_max = alpha_max;%弹道模型常数/设计诸元 state.const.beta_max = beta_max; state.Sxyz = [0;0;0];%发射系位置、速度 state.Vxyz = [0;0;0]; [state.Sxyz_t,state.Vxyz_t] = target(state); state.R_t = norm(state.Sxyz-state.Sxyz_t);%弹目距离 state.Vxd = (state.Vxyz-state.Vxyz_t)'*(state.Sxyz-state.Sxyz_t)/norm(state.Sxyz-state.Sxyz_t);%相对目标速度 state.dmdt = mass(state); state.ppg = [pi/2;0;0];%初始姿态角 phi psi gamma [state.theta,state.sigma] = gts(state.Vxyz,rotate);%弹道倾角 弹道偏角 state.qlj = gabm(state.ppg(1),state.ppg(2),state.ppg(3),state.theta,state.sigma,rotate);%攻角 侧滑 倾侧角 state.h = state.Sxyz(2);%高度与 [state.T, state.p,state. rho, state.a] = Environment(state.h);%气温 气压 密度 音速 state.V = norm(state.Vxyz); state.Ma = state.V/state.a; state.Q = 0.5*state.rho*state.V^2;%动压 state.range = 0;%射程 state.F_ground = Force_ground(state);%地面坐标系外力 state.F_sped = Force_sped(state);%速度坐标系外力 state.F_body = Force_body(state);%弹体坐标系外力 state.gravity = [0;-9.8;0];%重力 state.Axyz = dynamic3(state);%发射系加速度 %% 循环更新结构体 %垂直发射段 i = 1; record(i) = state; while state.t<5 record(i) = state;%垂直发射段时间长度5s 采用姿态角phi psi gamma方式确定姿态 stage = 1; state = renew_eul_1(state,dt); i = i+1; end i = i-1; state = record(i); %攻角转弯 + 惯性飞行段 while state.h>0&& state.t<500 record(i) = state; stage = 1; state = renew_eul_2(state,dt);%直到导弹落地 采用气流角alpha beta mu方式确定姿态 i = i+1; end i = i-1; state = record(i); while state.h>0&& state.t<500 record(i) = state; stage = 1; state = renew_eul_2(state,dt/10);%前面最后一步缩小步长 提高计算精度 i = i+1; end i = i-1; state = record(i); while state.h>0 && state.t<500 record(i) = state; stage = 1; state = renew_eul_2(state,dt/100);%前面最后一步缩小步长 提高计算精度 i = i+1; end i = i-1; state = record(i); %% 数据结果转换 将各个recrod结构体转换为一个 for i=1:1:length(record) output.t(i) = record(i).t; output.m(i) = record(i).m; output.dmdt(i) = record(i).dmdt; output.Sxyz(:,i) = record(i).Sxyz; output.Vxyz(:,i) = record(i).Vxyz; output.Axyz(:,i) = record(i).Axyz; % output.Bxyz(:,i) = record(i).Bxyz; % output.Wxyz(:,i) = record(i).Wxyz; output.ppg(:,i) = record(i).ppg; output.theta(i) = record(i).theta; output.sigma(i) = record(i).sigma; % output.theta_loc(i) =record(i).theta_loc; % output.ppgT(:,i) = record(i).ppgT; output.qlj(:,i) = record(i).qlj; % output.Rxyz(:,i) = record(i).Rxyz; % output.Rxyz_cent(:,i) = record(i).Rxyz_cent; % output.jwh(:,i) = record(i).jwh; output.h(i) = record(i).h; output.T(i)=record(i).T; output.p(i)=record(i).p; output. rho(i)=record(i).rho; output.a(i) =record(i).a; output.V(i)=record(i).V; output.range(i)=record(i).range; output.Ma(i)=record(i).Ma; % output.Vaxyz(:,i) =record(i).Vaxyz; output.R_t(i) =record(i).R_t; output.F_ground(:,i) =record(i).F_ground; output.F_sped(:,i) =record(i).F_sped; output.F_body(:,i) =record(i).F_body; output.gravity(:,i) =record(i).gravity; output.Sxyz_t(:,i) =record(i).Sxyz_t; output.Q(i) =record(i).Q; end %% 绘图 if draw ==1 figure(1) plot(output.t,output.R_t,"lineWidth",2); title("弹目距时间曲线") grid on xlabel("t(s)") ylabel("Rt(m)"); figure(2) plot(output.t,output.m,"lineWidth",2); title("质量时间曲线") grid on xlabel("t(s)") ylabel("m(kg)"); figure(3) plot(output.t,output.Sxyz,"lineWidth",2); hold on plot(output.t,output.Sxyz_t); title("发射系位置时间曲线") grid on xlabel("t(s)") ylabel("Sxyz(m)"); legend("x","y",'z',"x_t","y_t",'z_t') figure(4) plot(output.t,output.Vxyz,"lineWidth",2); title("发射系速度时间曲线") grid on xlabel("t(s)") ylabel("Vxyz(m/s)"); legend("Vx","Vy",'Vz') figure(5) plot(output.t,output.ppg*r2d,"lineWidth",2); title("姿态角时间曲线（"+rotate+"转序）") grid on xlabel("t(s)") ylabel("phi/psi/gamma(deg)"); legend("phi","psi",'gamma') figure(6) plot(output.t,output.qlj*r2d,"lineWidth",2); title(" 气流角时间曲线") grid on xlabel("t(s)") ylabel("alpha/beta/mu(deg)"); legend("alpha","beta",'mu') figure(7) plot(output.t,output.theta*r2d,output.t,output.sigma*r2d,"lineWidth",2); title(" 航迹角时间曲线") grid on xlabel("t(s)") ylabel("theta/sigma(deg)"); legend("theta",'sigma') figure(8) plot3(output.Sxyz(1,:),output.Sxyz(3,:),output.Sxyz(2,:),"lineWidth",2) hold on %plot3(output.Sxyz_t(1,:),output.Sxyz_t(3,:),output.Sxyz_t(2,:),"lineWidth",2) grid on xlabel("x(m)"); ylabel("y(m)"); zlabel("h(m)"); text(50000,40000,0,"*"); text(50000,40000,0,"目标点"); figure(9) plot(output.t,output.V,"lineWidth",2); title("速度时间曲线") grid on xlabel("t(s)") ylabel("V(m/s)"); legend("theta",'sigma') end