1.演算法概述

        偽譜法，又稱為正交配置法，主要利用Lagrange 插值多項式近似離散最優控制問題中的狀態變數和控制變數，將連續型最優控制問題轉化成離散形式的非線性規劃(NLP) 問題，然後利用相應的NLP 演算法求解。根據配置點的不同，偽譜法主要分為Legendre偽譜法、Gauss偽譜法 和Radau偽譜法 3 種。

       在本課題中，飛行器的運動方程取為：

2.部分程式

............................................................................
 
 
    %約束（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:K
   for kk=1:Ns
       if kk~=i
          LL(i)=(t-Tao(kk))/(Tao(i)-Tao(kk));
       end
   end
end
for i = 1:K
    Ls(i)=int(LL(i),t,-1,1);
end
Ls = double(Ls);
 
 
%目標方程（1個）
for i = 1:K
    Tmp7(i) = C/sqrt(R)*p^0.5*a4(k)^3.08/(Ls(i)^2);
end    
J = -(tf-t0)/(K*(K+1))*sum(Tmp7);
 
 
 
%%
%六狀態
r_line     = zeros(1,K);
Theta_line = zeros(1,K);
Fai_line   = zeros(1,K);
V_line     = zeros(1,K);
Gamma_line = zeros(1,K);
Si_line    = zeros(1,K);
k_         = 0;
CNT        = 0;
for k = 1:K
    CNT = CNT + 1;
    k
    if  k == 1
        Dkl(k,:) = func_D(t0,tf,ts,K,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);          
        
    else    
        %注意，採用radau離散化之後的非線性方程組，沒法直接使用fmincon進行求解，這裡，我們自己編寫了一個優化函式進行計算最優值
        %對控制狀態進行迴圈(fmincon的原理，也是基於如下過程進行的)
 
        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,K,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_)+dtf0*((tf-t0)/2 * a4(k_)*sin(a5(k_)) + a4(k_)*g*sin(1e3*a5(k_)));
                    %公式2
                    a2(k_+1) =  a2(k_)+dtf1*((tf-t0)/2 * a4(k_)*cos(a5(k_))*cos(a6(k_)) / (a1(k_)*cos(a3(k_))));
                    %公式3
                    a3(k_+1) =  a3(k_)+dtf2*((tf-t0)/2 * a4(k_)*cos(a5(k_))*sin(a6(k_)) / (a1(k_)));
                    %公式4
                    a4(k_+1) =  a4(k_)+dtf3*((tf-t0)/2 * (D/m + g*sin(a5(k_))));
                    %公式5
                    a5(k_+1) =  a5(k_)+dtf4*((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) =  a6(k_)+dtf5*((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)&...
                    a4(k_+1) >= 1.508 & a4(k_+1) <= V0 & a1(k_+1) <= Rs+80 & a1(k_+1) >= Rs+20;
 
..............................................
02-007m

3.演算法部分模擬結果圖