m基于拉道radau伪谱算法的非线性航迹规划matlab仿真-CFANZ编程社区

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.5a4(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.算法部分仿真结果图