# -*- coding: utf-8 -*-
"""
Created on Fri Oct 13 09:29:39 2023
@author: Administrator
"""
# -*- coding: utf-8 -*-
"""
Created on Fri Oct 13 09:29:39 2023
@author: Administrator
"""
# -*- coding: utf-8 -*-
"""
Created on Wed Oct 11 09:19:17 2023
Pb0----发动机额定比冲，N/(kg/s)
Sa----发动机尾部喷管面积；
p p0----空中、地面大气压力，pa
Sap0----推力高度特性
t=72.3 一级分离；
t=142.5 整流罩分离
t=145.52 二级分离
t=236.63 三级分离
t=764.93 入轨关机
lam0=0 发射点经度
@author: Administrator
"""
# -*- coding: utf-8 -*-
"""
Created on Wed Oct 11 09:19:37 2023
ae=6378.2e3;#赤道平均半径
phi lam 地心维度，经度
J带谐系数
r
A0,发射方位角度；
B0,发射点地理纬度
miu0，发射点地理维度与地心维度之差
fM，引力系数；
we，地球自转角速度；
ae，be，子午面椭圆长短半轴；
@author: Administrator
"""
import warnings
warnings.filterwarnings("ignore")
import param as pa
from scipy.interpolate import interp2d
import numpy as np;
# 重力求解函数 地面发射坐标系
def pF(t,dphi,dpsi):
# 箭体坐标系下推力参数
if t<pa.t1k:
fp1=pa.phjn[0]*np.cos(dphi)*np.cos(dpsi);
fp2=pa.Cdphin[0]*dphi;
fp3=pa.Cdpsin[0]*dpsi;
elif t<pa.t2k:
fp1=pa.phjn[1]*np.cos(dphi)*np.cos(dpsi);
fp2=pa.Cdphin[1]*dphi;
fp3=pa.Cdpsin[1]*dpsi;
elif t<pa.t3k:
fp1=pa.phjn[2]*np.cos(dphi)*np.cos(dpsi);
fp2=pa.Cdphin[2]*dphi;
fp3=pa.Cdpsin[2]*dpsi;
elif t<pa.t41k:
fp1=pa.phjn[3]*np.cos(dphi)*np.cos(dpsi);
fp2=pa.Cdphin[3]*dphi;
fp3=pa.Cdpsin[3]*dpsi;
elif t<pa.t4hk:
fp1=0;
fp2=0;
fp3=0;
else:
fp1=pa.phjn[3]*np.cos(dphi)*np.cos(dpsi);
fp2=pa.Cdphin[3]*dphi;
fp3=pa.Cdpsin[3]*dpsi;
# print(t,fp1)
Fp=np.array([fp1,fp2,fp3])
return Fp
def airCond(h):
# %UNTITLED5 此处显示有关此函数的摘要
# % 输入 h，m；输出t p rho a
h=h/1000;
R00=6371110/1000;
rhosl=1.225;
psl=101325;
z=1/(1/h-1/R00);
if z<=11.0191:
w=1-h/44.3308;
T=288.15*w;
p=psl*w**5.2559;
rho=rhosl*w**4.2559;
elif z<=20.0631:
w=np.exp((14.9647-h)/6.3416);
T=216.65;
p=psl*w*0.11953;
rho=rhosl*w*0.15898;
elif z<=32.1619:
w=1+(h-24.9021)/221.552;
T=221.552*w;
p=psl*w**(-34.1629)*0.025158;
rho=rhosl*w**(-35.1629)*0.032722;
elif z<=47.3501:
w=1+(h-39.7499)/89.4107;
T=250.350*w;
p=psl*w**(-12.2011)*0.0028338;
rho=rhosl*w**(-13.2011)*0.0032618;
elif z<=51.4125:
w=np.exp((48.6252-h)/7.9223);
T=270.650;
p=psl*w*0.00089155;
rho=rhosl*w*0.0009492;
elif z<=71.8020:
w=1-(h-59.439)/88.2218;
T=247.021*w;
p=psl*w**12.2011*0.00021671;
rho=rhosl*w**11.2011*0.00025280;
elif z<=86:
w=1-(h-78.0303)/100.2950;
T=200.59*w;
p=psl*w**17.0816*0.000012274;
rho=rhosl*w**16.0816*0.000017632;
elif z<=91:
w=np.exp((87.2848-h)/5.47);
T=186.87;
p=psl*(2.273+1.042e-3*h)*1e-6*w;
rho=rhosl*w*3.6411e-6;
else:
T=0;
p=psl*0;
rho=rhosl*0;
a=20.0468*T**0.5;
y=np.array([T,p,rho,a]);
return y
def qdF(v,alp,bet,ma,h):
# 气动力求解函数 箭体坐标系下
d2r=pa.d2r
Sm=pa.sm
alp=alp*d2r;
bet=bet*d2r;
ac=airCond(h)
q=v**2*ac[2]
alphaqd=np.array([-10,-6,-2,2,6,10]);
maqd=np.array([0.1,1.5,3,4.5,6,7.5]);
cdqd=np.array([[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=np.array([[-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=fcl(ma,alp);
Clb=fcl(ma,bet);
Cd=fcd(ma,alp);
# Cma=fcma(ma,alp);
if h<70e3:
D,L,Z=[-1*Cd*q*Sm,Cla*alp*q*Sm,-Clb*bet*q*Sm]
else:
D,L,Z=[0,0,0]
# D,L,Z=[0,0,0]
BV11=np.cos(alp)*np.cos(bet);
BV12=np.sin(alp);
BV13=-np.cos(alp)*np.sin(bet);
BV21=-np.sin(alp)*np.cos(bet);
BV22=np.cos(alp);
BV23=np.sin(alp)*np.sin(bet);
BV31=np.sin(bet);
BV32=0;
BV33=np.cos(bet);
BV=np.array([[BV11,BV12,BV13],[BV21,BV22,BV23],[BV31,BV32,BV33]])
Fa=BV.dot(np.array([D,L,Z])).reshape(1,3)
return Fa[0]
def gF(phi,x,y,z,vx,vy,vz):
we=pa.we;#rad/s
ae=pa.ae;#赤道平均半径
fM=398603e9;#m3/
A0=pa.A0; #射向，
B0=pa.B0;
r=np.sqrt((x+pa.Rox)**2+(y+pa.Roy)**2+(z+pa.Roz)**2);
grp=-fM/r**2*(1+pa.J*(ae/r)**2*(1-5*np.sin(phi)**2));
gwe=-2*fM/r/r*pa.J*(ae/r)**2*np.sin(phi);
[wex,wey,wez]=[we*np.cos(B0)*np.cos(A0),we*np.sin(B0),-we*np.cos(B0)*np.sin(A0)];
gr=grp/r*np.array([x+pa.Rox,y+pa.Roy,z+pa.Roz]);
gw=gwe/we*np.array([wex,wey,wez]);
Fg=1*np.array(np.sum([gr,gw],axis=0));
r_xyz=np.array([x+pa.Rox,y+pa.Roy,z+pa.Roz]);
v_xyz=np.array([vx,vy,vz])
we_xyz=np.array([wex,wey,wez]);
fae=-np.cross(we_xyz,np.cross(we_xyz, r_xyz));
fbe=-2*np.cross(we_xyz,v_xyz)
Fg=fae+Fg+fbe;
return Fg
=================================
import numpy as np
# 火箭参数
mhj=1000*np.array([33,8.3+4.2+1.5,4.2+1.2,0.7]);
sm=np.pi*0.7**2;
thjn=np.array([73,73,70,600]);
mp=[250,105,50,0.83]#发动机燃烧率
# 推力参数
phjn=9.8*1000*np.array([60,30,15,2.5/9.8]);
Cdphin=[0,0,0,0];#控制力分配系数YC1
Cdpsin=[0,0,0,0];#控制力分配系数ZC1
# 地球参数
d2r=57.142857142857146;
we=7.292115e-5;#rad/s7.292115e-5
ae=6378.140e3;#赤道平均半径
J2=1.08263e-3;
J=1.5*J2;
fM=3986005e8;#m3/s2
be=6356.755288e3
# 发射点参数
phi0=19.32/d2r#发射点地心维度
ae_=(ae-be)/ae
miu0=ae_*np.sin(2*phi0);
print(miu0)
A0=195/d2r #射向， --------优化变量---------
t41=258#四级第一次点火时长 --------优化变量---------
t42=400
t4h=20#四级滑行段时长， --------优化变量---------
p42=-30/d2r#四级定轴程序角 --------优化变量---------
am=9.6/d2r;#程序转弯最大攻角 --------优化变量---------
k3=-0.3/d2r;#--------优化变量---------
B0=phi0+miu0;#O点地理维度，地面发射坐标系转动角速度矢量
we_xyz=np.array([we*np.cos(B0)*np.cos(A0),we*np.sin(B0),-we*np.cos(B0)*np.sin(A0)]);
R0=ae*be/np.sqrt(ae*ae*np.sin(phi0)**2+be**2*np.cos(phi0)**2);
Rox,Roy,Roz=[-R0*np.sin(miu0)*np.cos(A0),R0*np.cos(miu0),R0*np.sin(miu0)*np.sin(A0)];
# 程序 参数
t1=6;#负攻角程序转弯点
t2=20;#负攻角程序转弯结束
a=2;#程序转弯调节因子
p3k=0;
p2k=0/d2r
# 子级工作时长max
# 一子级终止时间
t1k=thjn[0];
# 二子级终止时间
t2k=thjn[1]+thjn[0];
# 三子级终止时间
t3k=thjn[2]+thjn[1]+thjn[0];
# 四子级一次关机时间
t41k=thjn[2]+thjn[1]+thjn[0]+t41;
#四子级滑行段终止时间点
t4hk=thjn[2]+thjn[1]+thjn[0]+t41+t4h;
t42k=thjn[2]+thjn[1]+thjn[0]+t41+t4h+t42;
# 输出参数
dat=np.array([np.zeros(20)])
G2P11=np.cos(B0)*np.cos(A0);
G2P12=np.sin(B0)
G2P13=-np.cos(B0)*np.sin(A0)
G2P21=-np.sin(B0)*np.cos(A0)
G2P22=np.cos(B0)
G2P23=np.sin(B0)*np.sin(A0)
G2P31=np.sin(A0);
G2P32=0;
G2P33=np.cos(A0);
G2PM=np.array([[G2P11,G2P12,G2P13],[G2P21,G2P22,G2P23],[G2P31,G2P32,G2P33]])
lam0=0
gdgs=np.zeros(6)
# 飞行程序角; 由于固体运输火箭采用固体发动机 ,
# 其弹道设计、能量管理方式不同于液体运载火箭 .
# 采用“攻角转弯“ + “重力转弯“的组合方式进行能量管理。
# 以四级固体运输火箭为例 ,
# 一级亚音途飞行段采用“攻角转弯；
# 一级后半段及二级全段飞行采用零攻角“重力转弯“;
# 三级一般采用对飞行程序角进行线性变化，
# 此处需要设计三级程序角线性速率k3 ;
# 四级通常采用液体发动机 , 可多次关机 ,
# 如果两次关机, 则需要优化设计四级第一次点火定轴飞行时间t41、
# 无动力飞行时间t4h ，第二次点火定飞行程序角p42.
===============================
import matplotlib.pyplot as plt
from scipy.integrate import odeint
from sko.GA import GA;
import param as pa
import numpy as np
import math
import Force as fo
class ddG(object):
# 定义程序角
def alpPhi(t,the):
if t<pa.t1:
# 垂直段
alp=0
phi=90/pa.d2r;
# m=pa.mhj[0]-t*dm
elif t<pa.t2:
# 亚音速段
alp=4*pa.am*np.exp(pa.a*(pa.t1-t))*(np.exp(pa.a*(pa.t1-t))-1);
f=np.pi*(t-pa.t1)/((pa.tm-pa.t1)/(pa.t2-pa.tm)*(pa.t2-t)+(t-pa.t1))
alp=-pa.am*np.sin(f)**2
phi=alp+the;
# m=pa.mhj[0]-t*dm
elif t<pa.t1k:
#一级重力转弯段，火箭依旧在大气层内，为了减小气动载荷和干扰
# 0攻角飞行，
alp=0;
phi=the+alp;
# m=pa.mhj[0]-t*dm
elif t<pa.t2k:
# 二级飞行段，抛罩前
alp=0;
phi=the+alp;
pa.p2k=phi;
# m=pa.mhj[1]-(t-pa.t1k)*dm
elif t<pa.t3k:
# 三级飞行，罩子已抛，瞄准飞行,phi=phi2k-k*dt,k3
# print(pa.p2k)
phic3=pa.p2k+(t-pa.t2k)*pa.k3
phi=phic3;
alp=phi-the;
pa.p3k=phi;
# m=pa.mhj[2]-(t-pa.t2k)*dm
elif t<pa.t41k:
# 四级第一次飞行,定轴飞行
# phic41=pa.p3k
# phi=phic41;
# alp=0;
# the=phi
# alp=0
# phi=alp+the;
phic41=pa.p3k+(t-pa.t3k)*pa.k4
phi=phic41;
alp=phi-the;
pa.p41k=phi;
elif t<pa.t4hk:
#四级滑行段，火箭调姿态，朝向pa.p42
phich=pa.p3k+(t-pa.t41k)*(pa.p42-pa.p3k)/(pa.t4hk-pa.t41k)
alp=0;
phi=phich
the=phi
# m=pa.mhj[3]-pa.t4h*dm
else:
phic42=pa.p42;
phi=phic42;
alp=0;
the=phi
# m=pa.mhj[3]-pa.t4h*dm-dm*(t-pa.t4hk)
return [alp,phi,the]
# 定义m'
def mP(t):
if t<pa.t1k:
dm=pa.mp[0];
elif t<pa.t2k:
dm=pa.mp[1]
elif t<pa.t3k:
dm=pa.mp[2];
elif t<pa.t41k:
dm=pa.mp[3]
elif t<pa.t4hk:
dm=0
else:
dm=pa.mp[3]
return dm
# 定义地面坐标系到箭体系
def G2B(gam,psi,phi):
G2B11=np.cos(phi)*np.cos(psi);
G2B12=np.sin(phi)*np.cos(psi);
G2B13=-np.sin(psi);
G2B21=np.cos(phi)*np.sin(psi)*np.sin(gam)-np.sin(phi)*np.cos(gam)
G2B22=np.sin(phi)*np.sin(psi)*np.sin(gam)+np.cos(phi)*np.cos(gam);
G2B23=np.cos(psi)*np.sin(gam)
G2B31=np.cos(phi)*np.sin(psi)*np.cos(gam)+np.sin(phi)*np.sin(gam);
G2B32=np.sin(phi)*np.sin(psi)*np.cos(gam)-np.cos(phi)*np.sin(gam);
G2B33=np.cos(psi)*np.cos(gam);
G2BM=np.array([[G2B11,G2B12,G2B13],[G2B21,G2B22,G2B23],[G2B31,G2B32,G2B33]])
return G2BM
# 入轨六根数计算
def gdGS(par):
# 1
[tk,rk,xk,yk,zk,vxk,vyk,vzk,Cta,phiek,lamk,Ak,Bk]=par
rkA=rk;
phikA=phiek;
lamkA=lamk;
Ctak=Cta;
vk=np.sqrt(vxk*vxk+vyk*vyk+vzk*vzk)
tanakA=np.tan(Ak)+pa.we*rk*np.cos(phiek)/vk/np.cos(Ctak)/np.cos(Ak);
AkA=np.arctan(tanakA);
tanCtakA=np.tan(Ctak)*np.cos(AkA)/np.cos(Ak);
CtakA=np.arctan(tanCtakA);
vkA=vk*np.sin(Ctak)/np.sin(CtakA);
a=-pa.fM*rkA/(rkA*vkA*vkA-2*pa.fM);
miukA=vkA*vkA*rkA/pa.fM;
b=np.sqrt(miukA/(2-miukA))*rkA*np.cos(CtakA);
P=b*b/a;
e=np.sqrt(1-b*b/a/a);
EkA=np.arccos(a-rkA)/e
n=np.sqrt(pa.fM/a/a/a)
tp=tk-(EkA/n-e*np.sin(EkA)/n);
i=np.arccos(np.sin(AkA)*np.cos(phikA));
fk=np.arccos((P-rkA)/e/rkA);
# 不太确定，tG,omegag 发射日时角
tG=tk-8;
omegag=40;
acj=omegag+pa.we*tG*3600+lamkA;
cosOMG=(np.cos(AkA)*np.cos(acj)+np.sin(AkA)*np.sin(acj)*np.sin(phikA))/np.sin(i)
sinOMG=(np.cos(AkA)*np.sin(acj)-np.sin(AkA)*np.sin(phikA)*np.cos(acj))/np.sin(i)
tanOMG=sinOMG/cosOMG;
OMG=np.arctan(tanOMG);
cosw=(np.sin(phikA)*np.sin(fk)+np.cos(phikA)*np.cos(fk))/np.sin(i)*np.cos(AkA)
sinw=(np.sin(phikA)*np.cos(fk)-np.cos(phikA)*np.sin(fk))/np.sin(i)*np.cos(AkA)
w=np.arctan(sinw/cosw)
return [a,e,i,OMG,w,tp]
# 弹道积分程序
def dydt(x7,t,dm):
# 状态变量
# X'= [0,V]+[Wx,0]'+[g,0];
# X'=A+B+C；
# global dat,jd,gdgs
# 状态变量X7
vx,vy,vz,x,y,z,m=x7;
vx,vy,vz=np.array([vx,vy,vz])+np.array([vx,vy,vz]).dot(pa.we_xyz)
A6=np.array([0,0,0,vx,vy,vz,dm]);
##################################################################
rx=x+pa.Rox
ry=y+pa.Roy
rz=z+pa.Roz
r=np.sqrt(rx*rx+ry*ry+rz*rz)
v=np.sqrt(vx*vx+vy*vy+vz*vz)
r_xyz=np.array([rx,ry,rz])
##################################################################
# 求解地理参数，lam，phiew：弹下点地心经纬度；R：弹下点地心矢径；r：地心-xyz矢径；
# h：当地高度；
sinphie=r_xyz.dot(pa.we_xyz)/(r*pa.we)
phie=np.arcsin(sinphie)
R=pa.ae*pa.be/np.sqrt((pa.ae*sinphie)**2+(pa.be**2*(1-sinphie**2)));
h=r-R;
#求解弹道参数，the：弹道倾角；bet、sig：侧滑角、航迹偏角；alp,phi:攻角、俯仰角度，psi偏航角
the=math.atan2(vy, vx)#弹道倾角
sig=-math.sin(vz/v)#航迹偏角
alp,phi,the1=ddG.alpPhi(t,the)#攻角，俯仰角
gam=0;
bet=0
psi=0
# sinbeta=np.cos(the-phi)*np.cos(sig)*np.sin(psi)*np.cos(gam)+\
# np.sin(phi-the)*np.cos(sig)*np.sin(gam)-\
# np.sin(sig)*np.sin(psi)*np.cos(gam)
# bet=np.arcsin(sinbeta)
# 控制方程求解，根据瞬态平衡，暂不考虑，缺少数据？？？？？？？？？？？
dphi=0;
dpsi=0;
#求解矩阵A，B，C；包括控制力，推力，气动力，重力，变质量
# 重力函数，求解地面发射坐标系下重力分量，包含的地球自转的牵连效应,引力、离心力、哥氏力
gx,gy,gz=fo.gF(phie,x,y,z,vx,vy,vz);
#求解大气参数， T,p,rho,a，ma：温度，压力，密度，声速,马赫数,rvv:动压
T,p,rho,a=fo.airCond(h)
ma=v/a;
rvv=v*v*rho;
# 气动力拟合插值函数，求解箭体坐标系下分力，插值函数待定，现在置0
aFB=fo.qdF(v, alp, bet, ma, h);
# 推力求解，包含了推力项目，也包含了控制力项目，这里暂且不考虑控制项目，
# dpsi，dphi为偏航、俯仰方向发动机等效摆角度，置0
pFB=fo.pF(t,dphi,dpsi)
# 坐标变换矩阵，BODY->Earth Launch，箭体坐标系到地面坐标系
B2G11=np.cos(phi)*np.cos(psi);
B2G12=-np.sin(phi)
B2G13=np.cos(phi)*np.sin(psi)
B2G21=np.sin(phi)*np.cos(psi);
B2G22=np.cos(phi)
B2G23=np.sin(phi)*np.sin(psi)
B2G31=-np.sin(psi);
B2G32=0;
B2G33=np.cos(psi);
B2G=np.array([[B2G11,B2G12,B2G13],[B2G21,B2G22,B2G23],[B2G31,B2G32,B2G33]]);
B2Gm=1/m*B2G;
# 组合B矩阵
wxp,wyp,wzp=B2Gm.dot(pFB)+B2Gm.dot(aFB);
# print("推力")
# print(t,wyp)
# print("重+离+科")
# print(t,gy)
B6=np.array([wxp,wyp,wzp,0,0,0,0]);
C6=np.array([gx,gy,gz,0,0,0,0])
# 微分方程组
dydt6=B6+C6+A6
################################################################
# 求过载系数 gam:滚转角，G2BM：地面发射系->箭体坐标系
# nx1，ny1,nz1:箭体过载系数
gam=0;
G2BM=ddG.G2B(gam,psi,phi);
[nx1,ny1,nz1]=G2BM.dot([wxp,wyp,wzp])/9.8
##################################################################
##################################################################
#弹下点计算，Be：弹下点地理维度，G2PM：发射坐标系->地心坐标系（计算经纬度）
# dlam:弹下点经度-发射点经度；lame：弹下点经度；NPE：北天东坐标系，天轴指向r，E轴垂直弹下点子午面
Be=np.arctan(pa.ae**2/pa.be**2*np.tan(phie))
[xp,yp,zp]=pa.G2PM.dot([x+pa.Rox,y+pa.Roy,z+pa.Roz]);
[vxp,vyp,vzp]=pa.G2PM.dot([vx,vy,vz]);
if yp>0:
dlam=np.arctan(zp/yp)
else:
dlam=np.pi+np.arctan(zp/yp)
lame=pa.lam0+dlam;
P2NPE11=np.sin(Be);
P2NPE12=-np.sin(Be)*np.cos(dlam)
P2NPE13=-np.sin(Be)*np.sin(dlam)
P2NPE21=np.sin(Be)
P2NPE22=np.cos(Be)*np.cos(dlam)
P2NPE23=np.cos(Be)*np.sin(dlam)
P2NPE31=0
P2NPE32=-np.sin(dlam)
P2NPE33=np.cos(dlam)
P2NPEM=np.array([[P2NPE11,P2NPE12,P2NPE13],[P2NPE21,P2NPE22,P2NPE23],[P2NPE31,P2NPE32,P2NPE33]])
[vn,vb,ve]=P2NPEM.dot([vxp,vyp,vzp]);
# 弹下点方位角度、射程角，lam为经度,Beta：射程角
ae=np.arctan(ve/vn)
Beta=np.arccos(pa.R0/r+(x*pa.Rox+y*pa.Roy+z*pa.Roz)/(r*pa.R0))
Cta=np.arctan(vb/np.sqrt(vn*vn+ve*ve))
rp=r;
ra=700000;
vp=np.sqrt(2*pa.fM*ra/rp/(ra+rp))
vt_vp=v-vp;
if vt_vp>-10:
dv1=np.sqrt(v**2+vp**2-2*v*vp*np.cos(Cta))
m1=m*np.exp(-dv1/2400);
print(t,m1,vp)
va=np.sqrt(2*pa.fM*rp/ra/(ra+rp))
dat1=[t,vx,vy,vz,x,y,z,m,\ h,v,Cta*pa.d2r,nx1,ny1,nz1,rvv,phi*pa.d2r,the*pa.d2r,psi*pa.d2r,\
gam*pa.d2r,alp*pa.d2r]
pa.dat=np.append(pa.dat,np.array([dat1]),axis=0)
if t>pa.t4h:
pa.gdgs=ddG.gdGS([t,r,x,y,z,vx,vy,vz,Cta,phie,lame,ae,Be])
print(v,h/1000)
return dydt6;
# vx,vy,vz,x,y,z,m=x7;
def plot(self):
dat=pa.dat
dat=np.delete(dat,[0,1],axis=0)
plt.subplots(3,3,constrained_layout=True)
ax1=plt.subplot(3,3,1)
ax2=plt.subplot(332)
ax3=plt.subplot(333)
ax4=plt.subplot(334)
ax5=plt.subplot(335)
ax6=plt.subplot(336)
ax7=plt.subplot(337)
ax8=plt.subplot(338)
ax9=plt.subplot(339)
# plot中参数的含义分别是横轴值，纵轴值，线的形状，颜色，透明度,线的宽度和标签
ax1.plot(dat[:,0],dat[:,8], 'r--', alpha=1, label='h')
# ax11.plot(dat[:,0],dat[:,9]/1000, 'g-', alpha=1, label='v')
ax2.plot(dat[:,0],dat[:,9], 'g-', alpha=1, label='v')
ax3.plot(dat[:,4],dat[:,5], 'b-', alpha=1, label='x-y')
ax4.plot(dat[:,0],dat[:,-5], 'r-', alpha=1, label='phi')
ax4.plot(dat[:,0],dat[:,-4], 'b-', alpha=1, label='the')
ax4.plot(dat[:,0],dat[:,-10], 'g-', alpha=1, label='Cta')
ax5.plot(dat[:,0],dat[:,2], 'b-', alpha=1, label='vy')
ax5.plot(dat[:,0],dat[:,1], 'r-', alpha=1, label='vx')
ax5.plot(dat[:,0],dat[:,3], 'g-', alpha=1, label='vz')
ax7.plot(dat[:,0],dat[:,4], 'r-', alpha=1, label='x')
ax7.plot(dat[:,0],dat[:,5], 'g-', alpha=1, label='y')
ax7.plot(dat[:,0],dat[:,6], 'b-', alpha=1, label='z')
ax6.plot(dat[-100:-1,0],dat[-100:-1,7], 'b-', alpha=1, label='mass')
ax8.plot(dat[:,0],dat[:,11], 'b-', alpha=1, label='nx')
ax9.plot(dat[1:800,0],dat[1:800,-1], 'b-', alpha=1, label='alp')
# plt.plot(t, pp[:,5], 'go-', color='#0000FF', alpha=0.8, linewidth=0.1, label='Vz')
# 显示标签，如果不加这句，即使在plot中加了label='一些数字'的参数，最终还是不会显示标签
ax1.legend(loc="upper left")
ax2.legend(loc="upper right")
# ax3.legend(loc="upper right")
ax4.legend(loc="upper right")
ax5.legend(loc="upper right")
# ax55.legend(loc="upper right")
ax6.legend(loc="upper right")
ax7.legend(loc="upper right")
ax8.legend(loc="upper right")
ax9.legend(loc="upper right")
# ax11.legend(loc="upper right")
# ax1.set_xlabel('time(s)')
# ax1.set_ylabel('R/m')
# ax2.set_xlabel('time(s)')
# ax2.set_ylabel('h/m')
plt.show()
# if __name__=="__main__":
def goal(self,am,k3,t41,t4h,p42,A0):
XX=[am,k3,t41,t4h,p42,A0]
[pa.am,pa.k3,pa.t41,pa.t4h,pa.p42,pa.A0]=XX
#############################################################
# 一子级终止时间
pa.t1k=pa.thjn[0];
# 二子级终止时间
pa.t2k=pa.thjn[1]+pa.thjn[0];
# 三子级终止时间
pa.t3k=pa.thjn[2]+pa.thjn[1]+pa.thjn[0];
# 四子级一次关机时间
pa.t41k=pa.thjn[2]+pa.thjn[1]+pa.thjn[0]+pa.t41;
#四子级滑行段终止时间点
pa.t4hk=pa.thjn[2]+pa.thjn[1]+pa.thjn[0]+pa.t41+pa.t4h;
pa.t42k=pa.thjn[2]+pa.thjn[1]+pa.thjn[0]+pa.t41+pa.t4h+pa.t42;
#############################################################
t1=np.arange(0,pa.t1k, 0.001)
t2=np.arange(pa.t1k,pa.t2k,0.001)
t3=np.arange(pa.t2k,pa.t3k,0.001)
t41=np.arange(pa.t3k,pa.t41k,0.001)
t4h=np.arange(pa.t41k,pa.t4hk,0.001)
t42=np.arange(pa.t4hk,pa.t42k,0.001)
##########################################################
##########################################################
x0=[0,0.001,0,0,0,0,pa.mhj[0]];
x1j=odeint(ddG.dydt,x0, t1, args=(-pa.mp[0],)) ;
##########################################################
x2j0=x1j[-1,:];
x2j0[-1]=pa.mhj[1]
x2j=odeint(ddG.dydt,x2j0, t2, args=(-pa.mp[1],)) ;
##########################################################
x3j0=x2j[-1,:];
x3j0[-1]=pa.mhj[2]
x3j=odeint(ddG.dydt,x3j0, t3, args=(-pa.mp[2],));
##########################################################
##########################################################
x41j0=x3j[-1,:];
x41j0[-1]=pa.mhj[3]
x41j=odeint(ddG.dydt,x41j0, t41, args=(-pa.mp[3],));
# ##########################################################
x4hj0=x41j[-1,:];
x4hj=odeint(ddG.dydt,x4hj0, t4h, args=(0,));
# ##########################################################
# x42j0=x4hj[-1,:];
# x4hj=odeint(ddG.dydt,x42j0, t42, args=(-pa.mp[3],));
##########################################################
h=pa.dat[-1,8];
v=pa.dat[-1,9];
if h<200000 or v<7900:
obj=1e9;
else:
obj=7700/v+700000/h
print(v,h)
self.plot()
return obj
def __init__(self,d):
self.cal=d
=================================================
# -*- coding: utf-8 -*-
"""
Created on Thu Oct 19 15:21:29 2023
@author: Administrator
"""
import matplotlib.pyplot as plt
from scipy.integrate import odeint
from sko.GA import GA;
import param as pa
import numpy as np
import math
import Force as fo
from dydt import ddG
import time
if __name__ == '__main__':
dd1=ddG(1)
print("程序正在优化")
pa.t1=5
pa.t2=25
pa.t42=300
pa.k4=-0.1/pa.d2r
pa.tm=pa.t1+(pa.t2-pa.t1)/6
# pa.a=0.6391/(tm-pa.t1)
XXlb=[3/pa.d2r,-0.3/pa.d2r,100,400,-90/pa.d2r,-11.72/pa.d2r]
XXub=[12/pa.d2r,0.45/pa.d2r,200,400,20/pa.d2r,198/pa.d2r]
goa=dd1.goal(7.8/pa.d2r,0.1/pa.d2r,350,1,-30/pa.d2r,195.72/pa.d2r)
# k3的作用是调节转移轨道当地倾角，降低调姿态
# ga = GA(func=dd1.goal, n_dim=6, size_pop=40, max_iter=30, \
# lb=XXlb, ub=XXub,precision=0.1e-4,prob_mut=0.1);
# best_x, best_y = ga.run()
# print('best_x:', best_x, '\n', 'best_y:', best_y)
在这里插入代码片
7597
09-12
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
