clc;
clear;
close all;
global setup
% 基础参数设置(与原始代码相同)
setup.n = [30]; % LG点个数
setup.ns = [5]; % 状态变量个数
setup.nu = [1]; % 控制变量个数
Re = 6378145; % 地球半径
rho0 = 1.225; % 海平面大气密度
H = 7200; % 大气范围
g0 = 9.80665; % 重力加速度
s = 15; % 参考面积
mach = 340;
setup.Re = Re;
setup.rho0 = rho0;
setup.s = s;
setup.H = H;
setup.g0 = g0;
setup.mach = mach;
mI = 656080; % 运载器起飞质量
lI = 0.9; % 初始推重比
F0 = mI * lI * g0;
F0_ = 1.1 * F0;
setup.F0_ = F0_;
% 过程约束
qmax = 50e3;
nmax = 5;
setup.qmax = qmax;
setup.nmax = nmax;
m0 = 136080; % 初始质量
mf = 60400; % 末端质量
t0 = 0; % 初始时间
t1 = 200;
r0 = 2000 + Re; % 初始地心距
v0 = 200; % 初始速度
gamma0 = 30/57.3; % 初始弹道倾角
rf = 65000 + Re; % 终端高度
vf = 5 * mach; % 终端速度
gammaf = 20/57.3; % 终端弹道倾角
alpha0 = 20/57.3; % 起始攻角
alphaf = 0/57.3; % 终端攻角
% 无量纲化参数
tc = (Re/g0)^0.5; % 时间尺度
rc = Re; % 长度尺度
vc = (Re*g0)^0.5; % 速度尺度
Pc = m0 * g0; % 力尺度
mc = m0; % 质量尺度
setup.tc = tc;
setup.rc = rc;
setup.vc = vc;
setup.Pc = Pc;
setup.mc = mc;
% 保存原始设置用于蒙特卡洛
orig_setup = setup;
orig_m0 = m0;
orig_r0 = r0;
orig_v0 = v0;
% 蒙特卡洛参数
num_runs = 100; % 运行次数
compute_times = zeros(num_runs, 1); % 存储计算时间
final_velocities = zeros(num_runs, 1); % 存储终端速度
% 优化选项设置
options = optimset('TolX',1e-20, 'TolFun',1e-20, 'TolCon',1e-20, ...
'MaxFunEvals',10000000, 'Algorithm','sqp', ...
'MaxIter',100, 'Display','off', 'largescale','on');
% 主循环 - 蒙特卡洛模拟
for run = 1:num_runs
fprintf('蒙特卡洛运行 %d/%d...\n', run, num_runs);
% 重置全局设置
setup = orig_setup;
% 添加随机扰动 (±5%)
m0_perturbed = orig_m0 * (0.95 + 0.1*rand());
r0_perturbed = orig_r0 * (0.95 + 0.1*rand());
v0_perturbed = orig_v0 * (0.95 + 0.1*rand());
% 重新计算无量纲初始状态
r0dot = r0_perturbed / rc;
v0dot = v0_perturbed / vc;
gamma0dot = gamma0;
m0dot = m0_perturbed / mc;
alpha0dot = alpha0;
X0 = [r0dot; v0dot; gamma0dot; m0dot; alpha0dot];
setup.X0 = X0;
setup.r0dot = r0dot;
setup.v0dot = v0dot;
setup.m0dot = m0dot;
setup.gamma0dot = gamma0dot;
setup.alpha0dot = alpha0dot;
% 重新初始化阶段参数
phase = struct();
iphase = 1;
phase.t0{iphase} = t0/tc;
phase.tf{iphase} = t1/tc;
phase.m0{iphase} = m0_perturbed/mc;
phase.mf{iphase} = mf/mc;
% 生成Legendre-Gauss点
[phase.tau{iphase}, phase.w{iphase}, phase.D{iphase}] = LG_Points(setup.n(iphase));
% 初始猜测
phase.X{iphase} = [r0dot v0dot gamma0dot phase.m0{iphase} alpha0dot;
r0dot v0dot gamma0dot phase.mf{iphase} alpha0dot];
phase.Xguess{iphase} = interp1([-1;1], phase.X{iphase}, [-1; phase.tau{iphase}; 1], 'spline');
phase.Uguess{iphase} = (1/57.3) * ones(setup.n(iphase), 1);
phase.x0guess{iphase} = [reshape(phase.Xguess{iphase}, [], 1); reshape(phase.Uguess{iphase}, [], 1)];
% 边界约束
rmindot = Re / rc;
rmaxdot = 2 * Re / rc;
vmindot = -10000 / vc;
vmaxdot = -vmindot;
gammamindot = 0;
gammamaxdot = 80/57.3;
alphamindot = -10/57.3;
alphamaxdot = 30/57.3;
dalphamin = -5/57.3;
dalphamax = 5/57.3;
phase.XU{iphase} = [rmaxdot*ones(setup.n(iphase)+2,1) vmaxdot*ones(setup.n(iphase)+2,1) ...
gammamaxdot*ones(setup.n(iphase)+2,1) phase.m0{iphase}*ones(setup.n(iphase)+2,1) ...
alphamaxdot*ones(setup.n(iphase)+2,1)];
phase.UU{iphase} = dalphamax * ones(setup.n(iphase), 1);
phase.xU{iphase} = [reshape(phase.XU{iphase}, [], 1); reshape(phase.UU{iphase}, [], 1)];
phase.XL{iphase} = [rmindot*ones(setup.n(iphase)+2,1) vmindot*ones(setup.n(iphase)+2,1) ...
gammamindot*ones(setup.n(iphase)+2,1) phase.mf{iphase}*ones(setup.n(iphase)+2,1) ...
alphamindot*ones(setup.n(iphase)+2,1)];
phase.UL{iphase} = dalphamin * ones(setup.n(iphase), 1);
phase.xL{iphase} = [reshape(phase.XL{iphase}, [], 1); reshape(phase.UL{iphase}, [], 1)];
setup.phase = phase;
x0 = [phase.x0guess{1}; phase.tf{1}];
xU = [phase.xU{1}; phase.tf{1}];
xL = [phase.xL{1}; phase.t0{1}];
% 运行优化并计时
tic;
[x, f] = fmincon(@Objective, x0, [], [], [], [], xL, xU, @NonlconFun, options);
compute_times(run) = toc;
% 提取终端速度(无量纲转有量纲)
final_velocities(run) = x(2) * vc;
fprintf('运行 %d 完成: 时间 = %.2f 秒, 终端速度 = %.2f m/s\n', ...
run, compute_times(run), final_velocities(run));
end
% 生成散点图
figure('Color', 'white', 'Position', [100, 100, 800, 600]);
scatter(compute_times, final_velocities, 50, 'filled', 'MarkerFaceColor', [0.2, 0.4, 0.8], ...
'MarkerEdgeColor', 'k', 'LineWidth', 1.2);
% 添加趋势线
hold on;
p = polyfit(compute_times, final_velocities, 1);
yfit = polyval(p, compute_times);
plot(compute_times, yfit, 'r-', 'LineWidth', 2);
% 图表标注
title('轨迹优化速度性能分析', 'FontSize', 16, 'FontWeight', 'bold');
xlabel('计算时间 (秒)', 'FontSize', 14);
ylabel('终端速度 (m/s)', 'FontSize', 14);
grid on;
box on;
% 添加统计信息
mean_time = mean(compute_times);
mean_vel = mean(final_velocities);
text(0.7*max(compute_times), min(final_velocities)+100, ...
sprintf('平均时间: %.2f s\n平均速度: %.2f m/s', mean_time, mean_vel), ...
'FontSize', 12, 'BackgroundColor', 'white');
% 保存结果
save('monte_carlo_results.mat', 'compute_times', 'final_velocities');
disp('蒙特卡洛模拟完成,结果已保存。');
% 辅助函数:生成Legendre-Gauss点
function [tau, w, D] = LG_Points(n)
[tau, w] = legendre_points(n);
D = collocation_matrix(tau);
end
function [x, w] = legendre_points(n)
% 返回n阶Legendre多项式的根(LG点)和权重
beta = 0.5./sqrt(1-(2*(1:n-1)).^(-2)); % 三项递推系数
T = diag(beta,1) + diag(beta,-1); % Jacobi矩阵
[V, D] = eig(T); % 特征值分解
x = diag(D); % LG点
w = 2*V(1,:).^2; % 权重
% 排序
[x, idx] = sort(x);
w = w(idx)';
end
function D = collocation_matrix(tau)
% 计算微分矩阵
n = length(tau);
D = zeros(n);
for j = 1:n
for k = 1:n
if j ~= k
D(j,k) = lagrange_derivative(tau, j, k);
end
end
end
D(j,j) = -sum(D(j,:));
end
function d = lagrange_derivative(tau, j, k)
% 计算Lagrange基函数在节点处的导数
n = length(tau);
prod_val = 1;
for m = 1:n
if m ~= j && m ~= k
prod_val = prod_val * (tau(k) - tau(m))/(tau(j) - tau(m));
end
end
d = prod_val / (tau(j) - tau(k));
end
% 目标函数 (简化示例)
function J=Objective(x)
global setup
J=x(end)*setup.tc;
end
% 非线性约束函数 (简化示例)
function [c, ceq] = NonlconFun(x)
% 实际应用中应根据具体问题实现
c = []; % 无不等式约束
ceq = []; % 无等式约束
end在以上代码中加入以下非线性约束函数:%% 约束满足
function [c,ceq]=NonlconFun(x)
global setup
rho0=setup.rho0;
Re=setup.Re;
H=setup.H;
s=setup.s;
g0=setup.g0;
qmax=setup.qmax;
nmax=setup.nmax;
%无量纲化参数
rc=setup.rc;
vc=setup.vc;
tc=setup.tc;
mc=setup.mc;
n=setup.n; %LG点个数
ns=setup.ns; %状态变量个数
nu=setup.nu; %控制变量个数
np=length(n); %阶段数
nx=[0];
nx(1)= n(1)*(ns(1)+nu(1))+2*ns(1);
setup.nx=nx;
xi{1}=x(1:nx(1));
Xi{np}=[];
Ui{np}=[];
Fi{np}=[];
DXi{np}=[];
t0=setup.phase.t0;
tf=setup.phase.tf;
D=setup.phase.D;
w=setup.phase.w;
oe=setup.oe;
tf{np}=x(end);
if tf{np}<1/tc
aa=0;
end
for i1=1:np
X=ones(n(i1)+2,ns(i1));
for i2=1:ns(i1)
X(:,i2)=xi{i1}((i2-1)*(n(i1)+2)+1:i2*(n(i1)+2),1);
end
Xi{i1}=X;
U=ones(n(i1),nu(i1));
for i3=1:nu(i1)
U(:,i3)=xi{i1}(ns(i1)*(n(i1)+2)+(i3-1)*n(i1)+1:ns(i1)*(n(i1)+2)+i3*n(i1),1);
end
Ui{i1}=U;
F=MyDeo(X,U,i1);
Fi{i1}=F;
%动力学约束
ceqd=D{i1}*X(1:end-1,:)-(tf{i1}-t0{i1})/2*F;
ceqdr=reshape(ceqd,[],1);
ceqdri{i1}=ceqdr;
%终端约束
ceqe=X(1,:)+(tf{i1}-t0{i1})/2*w{i1}'*F-X(end,:);
ceqei{i1}=ceqe';
%路径约束
v=X(2:end-1,2); %动压
r=X(2:end-1,1);
rho=rho0*exp(-(r-1)*Re./H);
q=0.5.*rho.*(v.*vc).^2;
cq=q-qmax*ones(n(i1),1);
ciq=cq;
ceqi{i1}=[ceqdr;ceqe'];
ci{i1}=[-1];
end
ceql=[0];
%边界条件-初值
ceqs=Xi{1}(1,:)-setup.X0';
ceqsr=ceqs';
%边界条件-终值
rf=Xi{1}(end,1)';
Vf=Xi{1}(end,2)';
ceqo=[Vf]-oe(2);
ceq=[ceqi{1};ceqsr;ceql;ceqo'];
c=[ci{1}];
end
生成完整的代码
最新发布