指数成本函数—模型预测控制—抗扰动船舶路径跟踪

​​​​​​​

1. 路径的设定、载荷的计算、船舶运动模型、参数、初始化、以及求解结果的可视化与

https://mp.csdn.net/mp_blog/creation/editor/134397658https://mp.csdn.net/mp_blog/creation/editor/134397658

中的代码和设定是相同的。

2. 不同部分是第4点，指数成本函数对应的模型预测控制代码如下。

for kk=1:LTJ
    Wu=Wu0(kk);
    Wv=Wv0(kk);
    Wr=Wr0(kk);
    d11=-(Xu+Xuu*abs(u)); 
    d22=-(Yv+Yvv*abs(v)); 
    d33=-(Nr+Nrr*abs(r));
    a1=(1-d11*T/m11);
    a2=2*m22*T*T/m11*v*r+T*T/m11*Wu;
    a3=T*T/m11;
    b1=(1-d22*T/m22);
    b2=-2*m11*T*T/m22*u*r+T*T/m22*Wv;
    b3=T*T/m22;
    c1=(1-d33*T/m33);
    c2=2*(m11-m22)*T*T/m33*u*v+T*T/m33*Wr;
    c3=T*T/m33;
    % puk=a1*pu+a2+a3*taou;
    % pvk=b1*pv+b2;
    % prk=c1*pr+c2+c3*taor;
    %fossen ship model
    
    % prediction function
    Ap1=[];
    Ap2=[];
    Bp1=[];
    Bp2=[];
    Cp1=[];
    Cp2=[];
    for i=1:p
        Ap1=[Ap1;a1^i];
        Ap2=[Ap2;(1-a1^i)/(1-a1)];
        Bp1=[Bp1;b1^i];
        Bp2=[Bp2;(1-b1^i)/(1-b1)];
        Cp1=[Cp1;c1^i];
        Cp2=[Cp2;(1-c1^i)/(1-c1)];
    end
    Ap3=Ap2;
    Bp3=Bp2;
    Cp3=Cp2;
    faip=Cp1*pr(:,kk)+Cp2*c2+Cp3*c3*taor;
    pup=Ap1*pu(:,kk)+Ap2*a2+Ap3*a3*taou;
    pvp=Bp1*pv(:,kk)+Bp2*b2;
    rrp=pup.*pup+pvp.*pvp;
    roup=sqrt(rrp);
    % prediction function
    
    % p by p point follow rou and fai 
    if kk<=(LTJ-p+1)
        j=kk;
    else
        j=(LTJ-p+1);
    end
    Tr=[];
    Tf=[];
    for i1=j:(j+p-1)
        Tr=[Tr;TJ(2,i1)];
        Tf=[Tf;TJ(1,i1)];
    end
    Trr=Tr.*Tr;
    % p by p point follow rou and fai 
    Trr1=TJ(2,kk)*TJ(2,kk);
    Q=1;
    R=0.0001;
    puc=pu(kk);
    pvc=pv(kk);

    aa=-a3^2;
    bb=-2*a3*(a2 + a1*puc);
    cc=Trr1 - (a2 + a1*puc)^2 - (b2 + b1*pvc)^2;
    fun=@(taou) Q*exp(aa*taou^2 + bb*taou +cc)+R*taou^2;
    nonlcon = @(taou) taounonl(taou,aa,bb,cc);
    options=[];
    Ar=[];
    br=[];
    Aeqr=[];
    beqr=[];
    ubr=[]; % 控制量U的上、下限
    lbr=[];
    [taou,fval,exitflag,output]=fmincon(fun,taou0,Ar,br,Aeqr,beqr,lbr,ubr,nonlcon,options)  
    
    if taou<2.0e+5&&kk<10
        taou=1.5e+5;
    else
        taou=taou;
    end

    taou0 = taou;
    taouk(:,kk) = taou;
    pu(:,kk+1)=a1*pu(:,kk)+a2+a3*taou;
    pv(:,kk+1)=b1*pv(:,kk)+b2;
    rou(:,kk+1)=sqrt(pu(:,kk+1)*pu(:,kk+1)+pv(:,kk+1)*pv(:,kk+1));
    
    % optimization of rou
    % syms  Cp1 Cp2 Cp3 pr c2 c3 taor Wr Q3 R3 Tf
    % faip=Cp1*pr+Cp2*c2+Cp3*c3*taor;
    % J2=R3*c3*c3*taor*taor;
    % J1=(faip-Tf)'*Q3*(faip-Tf)
    % J3=J1+J2;
    % J4=(Cp3'*Q3*Cp3+R3)*c3*c3*taor*taor+2*(Cp1*pr+Cp2*c2-Tf)'*Q3*Cp3*c3*taor+(Cp1*pr+Cp2*c2-Tf)'*Q3*(Cp1*pr+Cp2*c2-Tf);
    % collect(J3,taor)
    % collect(J4,taor)

    %%优化目标函数min{1/2*x′Hx+f′x+D}
    Q3=100*eye(size(Cp3,1)); 
    R3=100*eye(size(taor,1));  % 优化目标参数，加权矩阵      
    Ad=[];
    bd=[];
    Aed=[]; % 10000*ones(5,1);
    bed=[]; % -10000*ones(5,1);
    ubd=[]; % 控制量U的上、下限
    lbd=[];
    %Wr1=Wr(kk);
    Hd=2*(Cp3'*Q3*Cp3+R3)*c3*c3;
    fd=2*(Cp1*pr(:,kk)+Cp2*c2-Tf)'*Q3*Cp3*c3;
    % 优化目标函数min{1/2*x′Hx+f′x+D}的矩阵f
    [taor,fvald,exitflagd,outputd,lambdad]=quadprog(Hd,fd',Ad,bd,Aed,bed,lbd,ubd);
    % taor=1*rand(1);
    % taor=-fd/Hd;%-b/2a;
    taork(:,kk) = taor;
    pr(:,kk+1)=c1*pr(:,kk)+c2+c3*taor;
    % pr=mod((pr+T*r)*pi/180,2*pi);
    % optimization of fai
    
    % u v r update
    u=(pu(:,kk+1)-pu(:,kk))/T;
    v=(pv(:,kk+1)-pv(:,kk))/T;
    r=(pr(:,kk+1)-pr(:,kk))/T;
    % u v r update
end

并且有一个约束条件：

function [c,ceq] = taounonl(taou,aa,bb,cc)
    c = -(aa*taou^2+bb*taou+cc); %c<=0
    ceq = [];
end

相关代码网址：AugustZLH (AugustZLH) / Repositories · GitHubAugustZLH has 7 repositories available. Follow their code on GitHub.https://github.com/AugustZLH?tab=repositories

欢迎交流指正。