拉道radau伪谱算法

最新推荐文章于 2023-01-07 12:29:49 发布

fpga和matlab

最新推荐文章于 2023-01-07 12:29:49 发布

阅读量4.8k

点赞数 7

分类专栏： MATLAB 文章标签：拉道radau伪谱 radau伪谱 radau

本文链接：https://blog.csdn.net/ccsss22/article/details/117166749

版权

MATLAB 专栏收录该内容

1613 篇文章 1648 订阅

订阅专栏

出处-类目-编号

1.问题描述：

由于radau离散化之后的非线性方程组非常复杂，直接使用MATLAB自带的fmincon函数，无法计算出最后的结果。我们根据fmincon的原理以及方程组的特点，自己编写了优化算法。具体你可以看我们的代码，其基本原理也是基于搜索过程的。

2.部分程序：

clc;
clear;
close all;
warning off;
RandStream.setDefaultStream(RandStream('mt19937ar','seed',sum(1)));
%% 参数的定义
%各个参数的初始化
r0 = 6498;%飞行器的地心距
Theta0 = 0;%经度
Fai0 = 0;%纬度
V0 = 7.315;%飞行器的速度
Gamma0 = -1.5*pi/180;%飞行路径角
Si0 = 45*pi/180;%航向角
%如下的为两个控制变量
delta0 = 90*pi/180;%倾斜角
alpha0 = 20*pi/180;%攻角
%变量
g0 = 9.81;%重力加速度
%常数
P = 1.225; %海平面大气密度
Rs = 6376; %地球半径
Sref = 49.24; %气动参考面积
m = 1000; %质量
u = 9.81*Rs*Rs; %地球的引力常数为9.81*地球的半径的平方
%常系数
aa0 = -0.02963;
aa1 = 0.03077;
aa2 = 8.5e-5;
bb0 = 0.01565;
bb1 = 0.005185;
bb2 = 0.000562;
%目标函数的参数值
C = 30.5;
R = 0.15;
qmax = 150;
nmax = 20;
Qmax = 1600;

%% RPM操作
%RPM的基本思路就是将状态变量（本课题是6个状态变量）和控制变量（本课题是两个控制变量）在一系列的LGR点上离散化
%定义积分时间
t0 = 0;
tf = 10;
%分段为K个网络
K = 90;
%对每个网络段中进行插值的点数
Ns = 10;%插值点个数
for k = 1:90
ts(k) = t0 + k*(tf - t0)/90;
end

for k = 2:90
%做映射
syms t;
Tao(k) = ( 2*t - ( ts(k)+ts(k-1) ) )/( ts(k)-ts(k-1) );
end

%将原公式转换为RPM离散方程式
Dkl = zeros(90,Ns);
%离散状态变量定义为ak
a1 = zeros(90,1);
a2 = zeros(90,1);
a3 = zeros(90,1);
a4 = zeros(90,1);
a5 = zeros(90,1);
a6 = zeros(90,1);
%离散控制变量定义为bk
b1 = zeros(90,1);
b2 = zeros(90,1);

for k = 1:90%分成K段网络
g = u/a1(k)^2;
p = P*exp(-0.00015*(a1(k)-Rs));
%部分由状态变量决定的参数
D = 0.5*p*a4(k)^2*Sref*(bb0 + bb1*b1(k) + bb2*b1(k)^2);
L = 0.5*p*a4(k)^2*Sref*(aa0 + aa1*b1(k) + aa2*b1(k)^2);
G = m;
%状态方程（6个）
%公式1
for i = 1:Ns
Tmp1(i) = Dkl(k,i)*a1(k);
end
SS1(k) = sum(Tmp1) - (tf-t0)/2 * a4(k)*sin(a5(k));
%公式2
for i = 1:Ns
Tmp2(i) = Dkl(k,i)*a2(k);
end
SS2(k) = sum(Tmp2) - (tf-t0)/2 * a4(k)*cos(a5(k))*cos(a6(k)) / (a1(k)*cos(a3(k)));
%公式3
for i = 1:Ns
Tmp3(i) = Dkl(k,i)*a3(k);
end
SS3(k) = sum(Tmp3) - (tf-t0)/2 * a4(k)*cos(a5(k))*sin(a6(k)) / (a1(k));
%公式4
for i = 1:Ns
Tmp4(i) = Dkl(k,i)*a4(k);
end
SS4(k) = sum(Tmp4) + (tf-t0)/2 * (D/m + g*sin(a5(k)));
%公式5
for i = 1:Ns
Tmp5(i) = Dkl(k,i)*a5(k);
end
SS5(k) = a4(k)*(sum(Tmp5) - (tf-t0)/2 * (L/m * cos(b2(k)) -g*cos(a5(k)) + a4(k)^2/a1(k)*cos(a5(k))));
%公式6
for i = 1:Ns
Tmp6(i) = Dkl(k,i)*a6(k);
end
SS6(k) = a4(k)*(sum(Tmp6) - (tf-t0)/2 * (L/m * sin(b2(k))/cos(a5(k)) - a4(k)^2/a1(k)*cos(a5(k))*cos(a6(k))*tan(a3(k))));

%约束（3个）
YY1(k) = 0.5*p*a4(k)^2 - qmax;%动压
YY2(k) = sqrt(L^2 + D^2)/G - nmax;%过载
YY3(k) = 3.0078*sqrt(p)*a4(k)^3.08 - Qmax;%驻点热流
end

%根据映射结果计算L值
for i = 1:90
for kk=1:Ns
if kk~=i
LL(i)=(t-Tao(kk))/(Tao(i)-Tao(kk));
end
end
end
for i = 1:90
Ls(i)=int(LL(i),t,-1,1);
end
Ls = double(Ls);

%目标方程（1个）
for i = 1:90
Tmp7(i) = C/sqrt(R)*p^0.5*a4(k)^3.08/(Ls(i)^2);
end
J = -(tf-t0)/(90*(90+1))*sum(Tmp7);

%%
%六状态
r_line = zeros(1,90);
Theta_line = zeros(1,90);
Fai_line = zeros(1,90);
V_line = zeros(1,90);
Gamma_line = zeros(1,90);
Si_line = zeros(1,90);
%两个控制变量
delta_line = zeros(1,90);
alpha_line = zeros(1,90);
%动压，过载，驻点热流
Dynamic_pressure = zeros(1,90);
Overload = zeros(1,90);
Stagnation_point_heat = zeros(1,90);

for k = 1:90
k
if k == 1
Dkl(k,:) = func_D(t0,tf,ts,90,Ns,k);
%离散状态变量定义为ak
a1(k) = r0;
a2(k) = Theta0;
a3(k) = Fai0;
a4(k) = V0;
a5(k) = Gamma0;
a6(k) = Si0;
%离散控制变量定义为bk
b1(k) = delta0;
b2(k) = alpha0;
x = [a1(k) a2(k) a3(k) a4(k) a5(k) a6(k) b1(k) b2(k)];

%将每个网络的最优解方程到过程变量数据中
%六状态
r_line(k) = x(1);
Theta_line(k) = x(2);
Fai_line(k) = x(3);
V_line(k) = x(4);
Gamma_line(k) = x(5);
Si_line(k) = x(6);
%两个控制变量
delta_line(k) = x(7);
alpha_line(k) = x(8);
%动压，过载，驻点热流
Dynamic_pressure(k) = 0.5*p*x(4)^2;
D = 0.5*p*x(4)^2*Sref*(bb0 + bb1*x(8) + bb2*x(8)^2);
L = 0.5*p*x(4)^2*Sref*(aa0 + aa1*x(8) + aa2*x(8)^2);
Overload(k) = sqrt(L^2 + D^2)/G;
Stagnation_point_heat(k) = 3.0078*sqrt(p)*x(4)^3.08;

else
%注意，采用radau离散化之后的非线性方程组，没法直接使用fmincon进行求解，这里，我们自己编写了一个优化函数进行计算最优值
%对控制状态进行循环(fmincon的原理，也是基于如下过程进行的)
%优化精度Steps = 1;
Steps = 5;
%对于Radau分段后的每段网络中的配置点个数设置为Ns
N = Ns;
cnt = 0;
dtf = 0.4;
nn = 0;
mm = 0;
alphass = [ 10:Steps:20]/180*pi;
deltass = [-90:3*Steps:90]/180*pi;
for alphas = alphass
mm = 0;
nn = nn+1;
for deltas = deltass
mm=mm+1;
for NN = 1:N
k_= k-1;
Dkl(k_,:) = func_D(t0,tf,ts,90,Ns,k_);
g = u/a1(k_)^2;
p = P*exp(-0.00015*(a1(k_)-Rs));
%部分由状态变量决定的参数
D = 0.5*p*a4(k_)^2*Sref*(bb0 + bb1*alphas + bb2*alphas^2);
L = 0.5*p*a4(k_)^2*Sref*(aa0 + aa1*alphas + aa2*alphas^2);
G = m;

%状态方程（6个）
%公式1
a1(k_+1) = a1(k_)+dtf*( (tf-t0)/2 * a4(k_)*sin(a5(k_)));
%公式2
a2(k_+1) = a2(k_)+20*dtf*( (tf-t0)/2 * a4(k_)*cos(a5(k_))*cos(a6(k_)) / (a1(k_)*cos(a3(k_))));
%公式3
a3(k_+1) = a3(k_)+20*dtf*( (tf-t0)/2 * a4(k_)*cos(a5(k_))*sin(a6(k_)) / (a1(k_)));
%公式4
a4(k_+1) = a4(k_)+dtf*( (tf-t0)/2 * (D/m + g*sin(a5(k_))))/20;
%公式5
a5(k_+1) = dtf*( (tf-t0)/2 * (L/m * cos(deltas) -g*cos(a5(k_)) + a4(k_)^2/a1(k_)*cos(a5(k_))))/a4(k_);
%公式6
a6(k_+1) = dtf*( (tf-t0)/2 * (L/m * sin(deltas)/cos(a5(k_)) - a4(k_)^2/a1(k_)*cos(a5(k_))*cos(a6(k_))*tan(a3(k_))))/a4(k_);
end

D = 0.5*p*a4(k_+1)^2*Sref*(bb0 + bb1*alphas + bb2*alphas^2);
L = 0.5*p*a4(k_+1)^2*Sref*(aa0 + aa1*alphas + aa2*alphas^2);
if (0.5*p*a4(k_+1)^2 <= qmax) & (sqrt(L^2 + D^2)/G <= nmax) & (3.0078*sqrt(p)*a4(k_+1)^3.08 <= Qmax) &...
(0.5*p*a4(k_+1)^2 > 0) & (sqrt(L^2 + D^2)/G > 0) & (3.0078*sqrt(p)*a4(k_+1)^3.08 > 0)&...
a1(k_+1) >= Rs+30 & a4(k_+1) >= 1.508 & a1(k_+1) <= Rs+80;
for ii = 1:90
Tmp7(ii) = C/sqrt(R)*p^0.5*(a4(k_+1))^3.08/(Ls(ii)^2);
end
Js(nn,mm) = (tf-t0)/(90*(90+1))*sum(Tmp7);
else
Js(nn,mm) = inf;
end
end
end

%找到最小的J，从而计算出对应的状态变量和控制变量
[IX,IY]= find(Js==min(min(Js)));
IX2 = IX(end);
IY2 = IY(end);
for alphas = alphass(IX2)
for deltas = deltass(IY2)
for NN = 1:N
k_= k-1;
Dkl(k_,:) = func_D(t0,tf,ts,90,Ns,k_);
g = u/a1(k_)^2;
p = P*exp(-0.00015*(a1(k_)-Rs));
%部分由状态变量决定的参数
D = 0.5*p*a4(k_)^2*Sref*(bb0 + bb1*alphas + bb2*alphas^2);
L = 0.5*p*a4(k_)^2*Sref*(aa0 + aa1*alphas + aa2*alphas^2);
G = m;
%状态方程（6个）
%公式1
a1(k_+1) = a1(k_)+dtf*(+ (tf-t0)/2 * a4(k_)*sin(a5(k_)));
%公式2
a2(k_+1) = a2(k_)+20*dtf*( (tf-t0)/2 * a4(k_)*cos(a5(k_))*cos(a6(k_)) / (a1(k_)*cos(a3(k_))));
%公式3
a3(k_+1) = a3(k_)+20*dtf*( (tf-t0)/2 * a4(k_)*cos(a5(k_))*sin(a6(k_)) / (a1(k_)));
%公式4
a4(k_+1) = a4(k_)+dtf*( (tf-t0)/2 * (D/m + g*sin(a5(k_))))/20;
%公式5
a5(k_+1) = dtf*( (tf-t0)/2 * (L/m * cos(deltas) -g*cos(a5(k_)) + a4(k_)^2/a1(k_)*cos(a5(k_))))/a4(k_);
%公式6
a6(k_+1) = dtf*( (tf-t0)/2 * (L/m * sin(deltas)/cos(a5(k_)) - a4(k_)^2/a1(k_)*cos(a5(k_))*cos(a6(k_))*tan(a3(k_))))/a4(k_);
end
end
end

a1(k) = a1(k_+1);
a2(k) = a2(k_+1);
a3(k) = a3(k_+1);
a4(k) = a4(k_+1);
a5(k) = a5(k_+1);
a6(k) = a6(k_+1);
b1(k) = deltass(IY2);
b2(k) = alphass(IX2);

x = [a1(k) a2(k) a3(k) a4(k) a5(k) a6(k) b1(k) b2(k)];
%将每个网络的最优解方程到过程变量数据中
%六状态
r_line(k) = x(1);
Theta_line(k) = x(2);
Fai_line(k) = x(3);
V_line(k) = x(4);
Gamma_line(k) = x(5);
Si_line(k) = x(6);
%两个控制变量
delta_line(k) = x(7);
alpha_line(k) = x(8);
%动压，过载，驻点热流
Dynamic_pressure(k) = 0.5*p*x(4)^2;
D = 0.5*p*x(4)^2*Sref*(bb0 + bb1*x(8) + bb2*x(8)^2);
L = 0.5*p*x(4)^2*Sref*(aa0 + aa1*x(8) + aa2*x(8)^2);
Overload(k) = sqrt(L^2 + D^2)/G;
Stagnation_point_heat(k) = 3.0078*sqrt(p)*x(4)^3.08;
end
end

%%
%画图
%高度
figure;
subplot(4,2,1);
plot(1:90,r_line-Rs,'b-o');
xlabel('仿真时间');
ylabel('高度');
%速度
subplot(4,2,2);
plot(1:90,V_line,'b-o');
xlabel('仿真时间');
ylabel('速度');

%经纬度
subplot(4,2,[3,4]);
plot(1:90,Theta_line/pi*180,'r-o');
hold on;
plot(1:90,Fai_line/pi*180,'b-o');
xlabel('仿真时间');
ylabel('经纬度');
legend('经度','纬度');

%倾斜角
subplot(4,2,5);
plot(1:90,delta_line/pi*180,'r-o');
hold on;
xlabel('仿真时间');
ylabel('倾斜角');
%攻角
subplot(4,2,6);
plot(1:90,alpha_line/pi*180,'r-o');
hold on;
xlabel('仿真时间');
ylabel('攻角');

%动压
subplot(4,2,7);
plot(Dynamic_pressure(3:end),'r-o');
hold on;
xlabel('仿真时间');
ylabel('动压');
%驻点热流
subplot(4,2,8);
plot(Stagnation_point_heat(3:end),'r-o');
hold on;
xlabel('仿真时间');
ylabel('驻点热流');