%% 火箭参数
%这里进行了一定的简化，忽略了地球的自转，如果考虑，需要设置好初始及步长；气动参数是胡乱输入的一组，具体的还是要用fluent算算，这个工作量较大；整个计算模型，隔了几段时间，看了两三次，需要反复，忘记再看，避免调入僵化思维中。其中火箭参数参考的是谋篇论文。内部还需细化，这里面可以走个简单的骨架。没有考虑P的三段，没有考虑太多，慢慢细化。
clc;clear all;clf;close all;
global pa op dad;
dad=zeros(1,14);%存储弹道数据
tcb=0.9;%燃烧比
isp=[2400 2400 2400 3000];%各子级比冲
mjjd=[350 400 300 160 350];%级间段123质量、尾舱质量
pa.m0=59.1e3;%火箭总质量 载荷590kg；4级1557kg，
d2r=57.142857142857146;
%一子级质量：
m1=350+41111+400;%尾段+1级发动机+级间段1
%二子级质量：
m2=11111+300;%2级发动机+级间段2
%三子级质量：
m3=4111+160;%3级发动机+级间段3
%四子级质量：
m4=pa.m0-m1-m2-m3;
pa.mhj=[41111 11111 4111 pa.m0-m1-m2-m3];%各级发动机质量，t
pa.pe=[1000000 400000 150000 10000];%各级发动机推力，N
pa.mp=pa.pe./isp;%各级发动机喷气速率，kg/s
pa.tk=0.9*pa.mhj./pa.mp;%各级火箭飞行时间
pa.tk=[pa.tk(1) pa.tk(2) pa.tk(3) 120 100];
%轨道高度
pa.sso=700000;
%转弯段参数
N0=pa.pe(1)/pa.m0/9.8;
%转弯段优化参数
op.t1=sqrt(40/(N0-1));
%%
%% 地球参数
pa.ae=6378.140e3;%赤道平均半径
pa.be=6356.755288e3;%地球极地平均直径
pa.J2=1.08263e-3;%带谐系数
pa.J=1.5*pa.J2;
pa.fM=398603e9;%m3/s2 引力系数
%发射点参数
pa.phi0=19.32/d2r;%发射点地心维度
pa.A0=-11.73/d2r; %射向
pa.ae_=(pa.ae-pa.be)/pa.ae;%中间参数
pa.miu0=pa.ae_*sin(2*pa.phi0);
pa.we=7.292115e-5;% rad/s 地球自转角速度
%发射点参数，纬度、地心半径，发射点坐标
pa.B0=pa.phi0+pa.miu0;%O点地理维度
pa.R0=pa.ae*pa.be/sqrt(pa.ae*pa.ae*sin(pa.phi0)^2+pa.be^2*cos(pa.phi0)^2);
pa.Rox=-pa.R0*sin(pa.miu0)*cos(pa.A0);
pa.Roy=pa.R0*cos(pa.miu0);
pa.Roz=pa.R0*sin(pa.miu0)*sin(pa.A0);
%地球自转在发射坐标系下分量
pa.wex=pa.we*cos(pa.B0)*cos(pa.A0);
pa.wey=pa.we*sin(pa.B0);
pa.wez=-pa.we*cos(pa.B0)*sin(pa.A0);
pa.we_xyz=[pa.we*cos(pa.B0)*cos(pa.A0),pa.we*sin(pa.B0),-pa.we*cos(pa.B0)*sin(pa.A0)];
%牵连加速度
a11=pa.we_xyz(1)^2-pa.we^2;a12=pa.we_xyz(1)*pa.we_xyz(2);a21=a12;
a22=pa.we_xyz(2)^2-pa.we^2;a23=pa.we_xyz(2)*pa.we_xyz(3);a32=a23;
a33=pa.we_xyz(3)^2-pa.we^2;a13=pa.we_xyz(3)*pa.we_xyz(1);a31=a13;
pa.a=[[a11 a12 a13];[a21 a22 a23];[a31 a32 a33]];
%哥氏加速度
b11=0;b22=0;b33=0;b12=-2*pa.we_xyz(3);b21=-b12;b31=-2*pa.we_xyz(2);
b23=-2*pa.we_xyz(1);b13=-b31;b32=-b23;
pa.b=[[b11 b12 b13];[b21 b22 b23];[b31 b32 b33]];
%坐标系变换矩阵，发射系->PM
pa.G2PM=[[cos(pa.B0)*cos(pa.A0),sin(pa.B0),-cos(pa.B0)*sin(pa.A0)];[-sin(pa.B0)*cos(pa.A0),cos(pa.B0),sin(pa.B0)*sin(pa.A0)];[sin(pa.A0),0,cos(pa.A0)]];
%% main 函数
%初始参数
format long;
op.t2=op.t1+15;op.am=pi/30;op.a=0.1;op.k1=-0.1/d2r;op.k2=-0.05/d2r;op.k3=-0.01/d2r;
options=odeset('MaxStep',1,'RelTol',1e-1,'AbsTol',1e-1);
tspan1=(0:0.5:pa.tk(1)); %t区间步长
% 初始值，vx vy vz x y z mass;
y1=[0 10 0 0 0 0 59.1e3];
[t1,d1]=ode45(@dt,tspan1,y1,options);%一子级求解
pa.phink0=dad(end,10);
y2=d1(end,:);%二子级分离前初始值
y2(7)=y2(7)-41111*0.1-350-400;%二子级分离后初始值
tspan2=(pa.tk(1):1:pa.tk(2)+pa.tk(1));%2级时间段
[t2,d2]=ode45(@dt,tspan2,y2,options);%二子级求解
y3=d2(end,:);%三子级分离前初始值
y3(7)=y3(7)-11111*0.1-300;%三子级分离后初始值
tspan3=((pa.tk(1)+pa.tk(2)):1:(pa.tk(2)+pa.tk(1)+pa.tk(3)));%3级时间段
[t3,d3]=ode45(@dt,tspan3,y3,options);%3级求解
y4=d3(end,:);%4子级分离前初始值
y4(7)=pa.m0-m1-m2-m3;%4子级分离后初始值
tspan4=((pa.tk(1)+pa.tk(2)+pa.tk(3)):1:(pa.tk(2)+pa.tk(1)+pa.tk(3))+pa.tk(4)+pa.tk(5)+150);%4时间段
[t4,d4]=ode45(@dt,tspan4,y4,options);%4级求解
%% 绘图
figure('numbertitle','off','name','t-v曲线')
plot(dad(:,1),dad(:,2),'b-','lineWidth',1.5);grid on;xlabel('t');ylabel('v');title('Velocity曲线');
figure('numbertitle','off','name','x-y曲线')
plot(dad(:,3),dad(:,4),'b-','lineWidth',1.5);grid on;xlabel('x/m');ylabel('y/m');title('x--y曲线');
figure('numbertitle','off','name','Vx-y-z曲线')
plot(dad(:,1),dad(:,6),'r-','lineWidth',1.5);grid on;xlabel('t/s');ylabel('v/m/s');hold on;
plot(dad(:,1),dad(:,7),'g-','lineWidth',1.5);grid on;hold on;
plot(dad(:,1),dad(:,8),'b-','lineWidth',1.5);grid on;hold on;legend('VX','VY','VZ')
% figure('numbertitle','off','name','x-y-z曲线')
% plot3(dad(:,5),dad(:,3),dad(:,4),'b-','lineWidth',1.5);grid on;xlabel('Z/m');ylabel('X/m');zlabel('Y/m'),title('x--y--z曲线');
figure('numbertitle','off','name','t-phi曲线')
plot(dad(:,1),dad(:,10)*d2r,'b-','lineWidth',1.5);grid on;xlabel('t/s');ylabel('phi/°');
figure('numbertitle','off','name','t-alp曲线')
plot(dad(:,1),dad(:,12)*d2r,'b-','lineWidth',1.5);grid on;xlabel('t/s');ylabel('alp/°');
figure('numbertitle','off','name','t-h曲线')
plot(dad(:,1),dad(:,11),'b-','lineWidth',1.5);grid on;xlabel('t/s');ylabel('h/m');
figure('numbertitle','off','name','t-mass曲线')
plot(dad(:,1),dad(:,13),'b-','lineWidth',1.5);grid on;xlabel('t/s');ylabel('mass/kg');
figure('numbertitle','off','name','t-cta曲线')
plot(dad(:,1),dad(:,14)*d2r,'b-','lineWidth',1.5);grid on;xlabel('t/s');ylabel('cta');
%%
%% 函数力计算
function Fqd=qdF(v,alp,bet,ma,h,the,sig)
global pa;
hjr=1.7;
Sm=pi*hjr*hjr;
ac=airCond(h);
q=v^2*ac(3)/2;
alphaqd=[-10,-6,-2,2,6,10];
maqd=[0.1,1.5,3,4.5,6,7.5];
cdqd=[[0.234800000000000,0.151800000000000,0.116700000000000,0.116700000000000,0.151800000000000,0.234800000000000];[0.440000000000000,0.319100000000000,0.268100000000000,0.268100000000000,0.319100000000000,0.440000000000000];[0.447700000000000,0.316500000000000,0.269000000000000,0.269000000000000,0.316500000000000,0.447700000000000];[0.466500000000000,0.338100000000000,0.292000000000000,0.292000000000000,0.338100000000000,0.466500000000000];[0.490600000000000,0.363600000000000,0.318400000000000,0.318400000000000,0.363600000000000,0.490600000000000];[1.46050000000000,1.36320000000000,1.33310000000000,1.33310000000000,1.36320000000000,1.46050000000000]];
clqd=[[-0.793500000000000,-0.420600000000000,-0.117900000000000,0.117900000000000,0.420600000000000,0.793500000000000];[-0.918800000000000,-0.445700000000000,-0.102100000000000,0.102100000000000,0.445700000000000,0.918800000000000];[-0.900300000000000,-0.409200000000000,-0.0899000000000000,0.0899000000000000,0.409200000000000,0.900300000000000];[-0.878000000000000,-0.395100000000000,-0.0852000000000000,0.0852000000000000,0.395100000000000,0.878000000000000];[-0.865400000000000,-0.386900000000000,-0.0823000000000000,0.0823000000000000,0.386900000000000,0.865400000000000];[-0.689900000000000,-0.278600000000000,-0.0457000000000000,0.0457000000000000,0.278600000000000,0.689900000000000]];
cmaqd=[[0.00120000000000000,0.00220000000000000,0.00360000000000000,0.00360000000000000,0.00220000000000000,0.00120000000000000];[-0.00190000000000000,0.00210000000000000,0.00530000000000000,0.00530000000000000,0.00210000000000000,-0.00190000000000000];[0.00230000000000000,0.00550000000000000,0.00830000000000000,0.00830000000000000,0.00550000000000000,0.00230000000000000];[0.00310000000000000,0.00640000000000000,0.00920000000000000,0.00920000000000000,0.00640000000000000,0.00310000000000000];[0.00340000000000000,0.00680000000000000,0.00970000000000000,0.00970000000000000,0.00680000000000000,0.00340000000000000];[0.00350000000000000,0.00700000000000000,0.0100000000000000,0.0100000000000000,0.00700000000000000,0.00350000000000000]];
%fcl=interp2d(maqd,alphaqd,clqd,'cubic');
%fcd=interp2d(maqd,alphaqd,cdqd,'cubic');
%fcma=interp2d(maqd,alphaqd,cmaqd,'cubic');
Cla=interp2(maqd,alphaqd,clqd,ma,alp,'spline');
Cd=interp2(maqd,alphaqd,cdqd,ma,alp,'spline');
Cma=interp2(maqd,alphaqd,cmaqd,ma,alp,'spline');
if h<80e3
D=-1*Cd*q*Sm;
L=Cla*alp*q*Sm;
Clb=0;
Z=-Clb*bet*q*Sm;
else
D=0;
L=0;
Z=0;
end
gam=0;
F1=D*cos(the)*cos(sig)-L*sin(the);
F2=D*sin(the)*cos(sig)+L*cos(the);
F3=-D*sin(sig);
Fqd=[F1 F2 F3];
end
function fp=pF(t,phi,psi)
%箭体坐标系下推力参数
global pa
if t<pa.tk(1)
fp=[cos(phi)*cos(psi)*pa.pe(1),sin(phi)*cos(psi)*pa.pe(1),-sin(psi)*pa.pe(1)];
elseif t<sum(pa.tk(1:2))
fp=[cos(phi)*cos(psi)*pa.pe(2),sin(phi)*cos(psi)*pa.pe(2),-sin(psi)*pa.pe(2)];
elseif t<sum(pa.tk(1:3))
fp=[cos(phi)*cos(psi)*pa.pe(3),sin(phi)*cos(psi)*pa.pe(3),-sin(psi)*pa.pe(3)];
elseif t<sum(pa.tk(1:4))
fp=[cos(phi)*cos(psi)*pa.pe(4),sin(phi)*cos(psi)*pa.pe(4),-sin(psi)*pa.pe(4)];
elseif t<sum(pa.tk(1:5))
fp=[0 0 0];
else
fp=[cos(phi)*cos(psi)*pa.pe(4),sin(phi)*cos(psi)*pa.pe(4),-sin(psi)*pa.pe(4)];
end
end
function dd=dxd(r_,phiq,x,y,z,vx,vy,vz)
global pa
Be=atan(pa.ae^2/pa.be^2*tan(phiq));
xyzp=pa.G2PM*[x+pa.Rox,y+pa.Roy,z+pa.Roz]';
xp=xyzp(1);yp=xyzp(2);zp=xyzp(3);
vp=pa.G2PM*[vx,vy,vz]';
vxp=vp(1);
vyp=vp(2);
vzp=vp(3);
if yp>0
dlam=atan(zp/yp);
else
dlam=pi+atan(zp/yp);
end
%Lam=pa.lam0+dlam;
P2NPE11=sin(Be);
P2NPE12=-sin(Be)*cos(dlam);
P2NPE13=-sin(Be)*sin(dlam);
P2NPE21=sin(Be);
P2NPE22=cos(Be)*cos(dlam);
P2NPE23=cos(Be)*sin(dlam);
P2NPE31=0;
P2NPE32=-sin(dlam);
P2NPE33=cos(dlam);
P2NPEM=[[P2NPE11,P2NPE12,P2NPE13];[P2NPE21,P2NPE22,P2NPE23];[P2NPE31,P2NPE32,P2NPE33]];
vnbe=P2NPEM*[vxp,vyp,vzp]';
vn=vnbe(1);
vb=vnbe(2);
ve=vnbe(3);
%弹下点方位角度、射程角，lam为经度,Beta：射程角
ae=atan(ve/vn);
Beta=(pa.R0/r_+(x*pa.Rox+y*pa.Roy+z*pa.Roz)/(r_*pa.R0));
Cta=atan(vb/sqrt(vn*vn+ve*ve));
dd=Cta;
end
function dydt=dt(t,y7)
% 发射坐标系火箭弹道主动段计算程序
% 近似处理：地球为均质椭圆球；忽略二阶小量；
% 状态变量5个： Y= 【vx，vy,vz,，x，y,z,mass】 dydt=Y'
%%
format long;
dydt=zeros(7,1);
bet=0;
global pa op dad;
vx=y7(1);vy=y7(2);vz=y7(3);x=y7(4);y=y7(5);z=y7(6);mass=y7(7);
r_=norm([y7(4)+pa.Rox,y7(5)+pa.Roy,y7(6)+pa.Roz]);v_=sqrt(vx*vx+vy*vy+vz*vz);
sinphi=([y7(4),y7(5),y7(6)]+[pa.Rox,pa.Roy,pa.Roz])*(pa.we_xyz') /(r_*pa.we);
phiq=asin(sinphi);
R=pa.ae*pa.be/norm([pa.ae*sin(phiq),pa.be*cos(phiq)]);
h=r_-R;
sig=-asin(y7(3)/v_);
psi=0;
bet=0;
% phi 程序角度
if t<op.t1
alp=0;
theat=atan(vy/vx);
phi=theat+alp;
elseif t<op.t2
theat=atan(vy/vx);
alp=-4*op.am*exp(-op.a*(t-op.t1))*(1-exp(-op.a*t+op.a*op.t1));
phi=theat+alp;
theat=atan(y7(2)/y7(1));
else
alp=0;theat=atan(vy/vx);
phi=theat+alp;
pa.phik2=phi;
end
if h<0
error('h不能小于0')
end
if t>sum(pa.tk(1:2))%三子级飞行程序角度
phi=pa.phik2+op.k1*(t-sum(pa.tk(1:2)));
alp=phi-theat;
pa.phi3k=phi;
elseif t>sum(pa.tk(1:3))%4子级飞行程序角度,第一主动段
phi=pa.phi3k+op.k2*(t-sum(pa.tk(1:3)));
alp=phi-theat;
pa.phi4k1=phi;
elseif t>sum(pa.tk(1:4))%4子级飞行程序角度,滑行段
phi=pa.phi4k1+op.k2*(t-(sum(pa.tk(1:4))));
alp=phi-theat;
pa.phihk=phi;
elseif t>sum(pa.tk(1:4))
phi=pa.phihk;
alp=phi-theat;
end
tpra=airCond(h);
ma=v_/tpra(4);
Q=qdF(v_,alp,bet,ma,h,theat,sig)/y7(7);
G=gF(r_,phiq,x,y,z,vx,vy,vz);
P=pF(t,phi,psi)/y7(7);
Ft=(P)+G;
dydt(1)=Ft(1);
dydt(2)=Ft(2);
dydt(3)=Ft(3);
dydt(4)=y7(1);
dydt(5)=y7(2);
dydt(6)=y7(3);
if t<pa.tk(1)
dydt(7)=-pa.mp(1);
elseif t<pa.tk(2)+pa.tk(1)
dydt(7)=-pa.mp(2);
elseif t<pa.tk(3)+pa.tk(2)+pa.tk(1)
dydt(7)=-pa.mp(3);
elseif t<sum(pa.tk(1:4))
dydt(7)=-pa.mp(4);
elseif t<sum(pa.tk(1:5))
dydt(7)=0;
else
dydt(7)=-pa.mp(4);
end
%% 近地点惩罚函数
v_=norm([vx,vy,vz]);
rp=r_;
ra=pa.sso;
vp=sqrt(2*pa.fM*ra/rp/(ra+rp));
cta=dxd(r_,phiq,x,y,z,vx,vy,vz);
dv=sqrt(v_*v_+vp*vp-2*v_*vp*cos(cta));
if vp-v_>-10
cta=dxd(r_,phiq,x,y,z,vx,vy,vz);
dv=sqrt(v_*v_+vp*vp-2*v_*vp*cos(cta));
pa.mtk1=y7(7)*exp(-dv/3000);
end
%% 弹道数据
dad1=[t,v_,x,y,z,vx,vy,vz,alp,phi,h,alp,y7(7),cta];
dad=[dad;dad1];
%
t
h
end
function F123= gF(r,phiq,x,y,z,vx,vy,vz)
global pa;%求解引力、离心力、哥氏力、牵连力
grp=-pa.fM/r^2*(1+pa.J*(pa.ae/r)^2*(1-5*sin(phiq)^2));
gwe=-2*pa.fM/r/r*pa.J*(pa.ae/r)^2*sin(phiq);
gr=grp/r*([x+pa.Rox,y+pa.Roy,z+pa.Roz]);
gw=gwe/pa.we*([pa.we_xyz]);
fzl=gr+gw;
r_xyz=[x+pa.Rox,y+pa.Roy,z+pa.Roz];
v_xyz=[vx,vy,vz];
flx=-r_xyz*pa.a;
fgs=-v_xyz*pa.b;
% F123=fzl+flx+fgs;
F123=fzl;%暂不考虑地球自转的影响
% F1=-grp*x/r+2*(pa.wez*vy-pa.wey*vz);
% F2=-grp*(y+pa.ae)/r+2*(pa.wex*vz-pa.wez*vx);
% F3=-grp*z/r+2*(pa.wey*vx-pa.wex*vy);
% F123=[F1 F2 F3];
end
2331
评论
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
查看更多评论
添加红包
