【参数受限MPC/Matlab】变量含约束优化求解

最新推荐文章于 2024-07-28 09:51:03 发布

Amanda1m

最新推荐文章于 2024-07-28 09:51:03 发布

阅读量2.1k

点赞数 4

分类专栏：控制理论学习

本文链接：https://blog.csdn.net/m0_37764065/article/details/108665174

版权

控制理论学习专栏收录该内容

34 篇文章 80 订阅

订阅专栏

ConstrainedMPC.m 滚动求解，生成最优控制量。

理论部分见https://blog.csdn.net/m0_37764065/article/details/107207726

function [k,r,y1,u1,x1,x2,x3,yout] = ConstrainedMPC(Ad,Bd,Cd,Nc,Np,rw,r,u_min,u_max,deltau_min,deltau_max)
%UNTITLED 此处显示有关此函数的摘要
%   此处显示详细说明
[Phi_Phi,Phi_F,PhiR,A_e,B_e,C_e]=mpcgain(Ad,Bd,Cd,Nc,Np);
% pole=[0.8 0.8 0.8 0.8];
% Kob=acker(A_e',C_e', pole)';
pole=[0.5 0.6 0.7 0.8];
Kob=zeros(size(A_e,1),1);
Kob=acker(A_e',C_e', pole)';
%Kob=[38.5114   32.6463   -9.9179    1.2720]';
[n,n_in]=size(B_e);
nd=size(Ad,1);
xm=zeros(nd,1);
Xf=zeros(n,1);
u=0;
y=0;
H=Phi_Phi+rw*eye(Nc);
Xr=zeros(n,1);
Xr(n,1)=r(1);
L0=zeros(Nc,1);
L0(1,1)=1;
A_cons=[L0';-L0';L0';-L0'];
b_d=[deltau_max;-deltau_min];
%A_cons_d=zeros(2*Nc,Nc);
%A_cons_a=zeros(2*Nc,Nc);
%for i=1:Nc
%    A_cons_d(i*2-1,i)=1;
%    A_consy_d(i*2,i)=-1;
%end
%for i=1:Nc
%    for j=1:i
%        A_cons_a(i*2-1,j)=1;
%        A_cons_a(i*2,j)=-1;
%    end
%end
%A_cons=[A_cons_d;A_cons_a];
%b_d=zeros(2*Nc,n_in);
%for i=1:Nc
%    b_d(i*2-1,:)=deltau_max;
%    b_d(i*2,:)=-deltau_min;
%end
%b_a=zeros(2*Nc,n_in);
N_sim=size(r,2);
for kk=1:N_sim
    %for i=1:Nc
        %b_a(i*2-1,:)=u_max-u;
        %b_d(i*2,:)=-u_min+u;
    %end
    b_a=[u_max-u;-u_min+u];
    b=[b_d;b_a];
    Xr(n,1)=r(kk);
    f=Phi_F*(Xf-Xr);%Phi_F*r(kk)=PhiR
    DeltaU=QPhild(H,f,A_cons,b);
    deltau=DeltaU(1,1);
    Xf=A_e*Xf+B_e*deltau+ Kob*(y-C_e*Xf);
    u=u+deltau;
    x1(kk)=Xf(1,:);
    x2(kk)=Xf(2,:);
    x3(kk)=Xf(3,:);
    yout(kk)=Xf(4,:);
    u1(kk)=u;
    y1(kk)=y;
    xm_old=xm;
    xm=Ad*xm+Bd*u;
    y=Cd*xm;
    %Xf=[xm-xm_old;y];
end
k=0:(N_sim-1);
end