gusucode.com > 一个简单的协同自适应巡航控制的MPC控制器设计源码程序 > cmpcQP.m

    %求解有约束预测控制问题，函数返回值为系统输出及控制输入
%此函数针对无限预测时域与有限控制时域的情况
%function [Y,U]=cmpcQP()
clear;
%初始化
load dstatespace.mat;%下载辨识出的状态空间模型
%输入输出个数
usize=size(Bd,2);
ysize=size(C,1);
xsize=size(Ad,1);
YW=[10 0 0 0;0 6 0 0;0 0 3 0;0 0 0 0];%输出误差加权阵
% YW=[10 0;0 1];%输出误差加权阵
UW=0;%输入加权阵
m=5;%控制时域
p=10;%预测时域
Ts=0.1;%采样周期
Tend=30;%仿真总时间
Nrow=Tend/Ts+m;
LR1=5*ones(Nrow,1);%L1输出期望值
LR2=zeros(Nrow,1);%L2输出期望值
LR3=zeros(Nrow,1);%L2输出期望值
LR4=zeros(Nrow,1);%L2输出期望值
r=[LR1 LR2 LR3 LR4];
% r=[LR1 LR2];

L=zeros(Nrow,3);
for k=1:1:150
    L(k,1)=0;
    L(k,2)=3;
    L(k,3)=3*k*Ts+2;
end
for k=151:1:160
    L(k,1)=-1;
    L(k,2)=L(k-1,2)+L(k,1)*Ts;
    L(k,3)=L(k-1,3)+L(k-1,2)*Ts+0.5*L(k,1)*Ts*Ts;
end
for k=161:1:Nrow
    L(k,1)=0;
    L(k,2)=L(k-1,2);
    L(k,3)=L(k-1,3)+L(k-1,2)*Ts;
end


% v=zeros(1,Nrow);
% for i=20:1:24
%     v(1,i)=0.5;
% end
% for i=40:1:44
%     v(1,i)=-1.5;
% end

% % %产生均值为a，标准差为b的白噪声
% % n=randn(1,Nrow); 
% % n=n/std(n); 
% % n=n-mean(n);
% % a=0;         %均值为0
% % b=sqrt(0.01); %方差为0.01
% % n=a+b*n;
% % %dn=[n;n];
% % dn=n;
% % K=[0.1;0.1;0.1;0.1];

%输出约束
% % YMAX=ones(2*m,1);
% % for i=1:2*m
% %     if rem(i,2)==0
% %         YMAX(i,1)=3;
% %     else
% %         YMAX(i,1)=3;
% %     end
% % end
% % YMIN=ones(2*m,1);
% % for i=1:2*m
% %     if rem(i,2)==0
% %         YMIN(i,1)=-3;
% %     else
% %         YMIN(i,1)=0;
% %     end
% % end
YMAX=ones(4*m,1);
for i=1:4*m
    if rem(i,4)==0
        YMAX(i,1)=40;
    elseif rem(i,3)==0
         YMAX(i,1)=Inf;
    elseif rem(i,2)==0&&rem(i,4)~=0
        YMAX(i,1)=30;
    else
        YMAX(i,1)=10;
    end
end
YMIN=ones(4*m,1);
for i=1:4*m
    if rem(i,4)==0
        YMIN(i,1)=0;
    elseif rem(i,3)==0
         YMIN(i,1)=-Inf;
    elseif rem(i,2)==0&&rem(i,4)~=0
        YMIN(i,1)=-30;
    else
        YMIN(i,1)=0;
    end
end

%输入约束
UMAX=2*ones(m,1);
UMIN=-4.5*ones(m,1);
%输入变化率约束
DUMAX=3*ones(m,1);
DUMIN=-3*ones(m,1);
II=eye(m*usize);
IDI=II-[zeros(usize,m*usize);eye((m-1)*usize) zeros((m-1)*usize,usize)];
%调用转化为QP标准型中二次型矩阵的函数
[H,f,YC]=genQP(Ad,Bd,C,YW,UW,m,p);
H=(H'+H)/2;%将H转化为对称阵
%产生不等式约束阵A
A=[YC;-YC;II;-II;IDI;-IDI];
X=zeros(xsize,Nrow);
X(1,1)=2;
X(2,1)=2;
X(3,1)=0;
X(4,1)=1;
U=zeros(m*usize,Nrow);

XR=zeros(3,Nrow);

k=0;%初始步数
t=Ts*k;
x=X(:,k+1);%初始状态 

xr=XR(:,k+1);
XR(1,1)=0;
XR(2,1)=1;
XR(3,1)=0;
% for k=1:1:Nrow
%     vl=1;
%     pl=2*k*Ts+2;
% end


for i=0:Ts:Tend
    [F,CL]= get_F(Ad,C,f,r(k+1:k+m,:),x,m);
    b=[YMAX-CL;CL-YMIN;UMAX;-UMIN;DUMAX;-DUMIN];
    U(:,k+2)=quadprog(H,F,A,b);%求解QP问题得到最优解U(k)
%     X(:,k+2)=Ad*x+Bd*U(1:usize,k+2);
    XR(:,k+2)=ARd*xr+BRd*U(1:usize,k+2);
%     X(:,k+2)=Ad*x+Bd*U(1:usize,k+2)+K*dn(:,k+1);
%     X(:,k+2)=Ad*x+Bd*U(1:usize,k+2)+Bd1*v(1,k+2);
    k=k+1;
%     x=X(:,k+1);
    xr=XR(:,k+1);
%     vl=3;
%     pl=3*k*Ts+2;
    vl=L(k,2);
    pl=L(k,3);
    X(1,k+1)=(pl-xr(1,1)-xr(2,1));
    X(2,k+1)=(vl-xr(2,1));
    X(3,k+1)=xr(3,1);
    X(4,k+1)=xr(2,1);
    x=X(:,k+1);
    t=[t;Ts*k];
end



Y=C*X;
save('cmpc_data.mat');
%绘制输入输出曲线
figure(1);
subplot(2,1,1)
plot(0:Ts:Tend,Y(1,1:Tend/Ts+1));
hold on;
plot(0:Ts:Tend,Y(2,1:Tend/Ts+1),'r');
xlabel('Time(sec)');
ylabel('error_real');
hold on;
plot(0:Ts:Tend,Y(3,1:Tend/Ts+1),'y');
hold on;
plot(0:Ts:Tend,Y(4,1:Tend/Ts+1),'g');
grid on;
title('Outputs');
legend('ep_real','e_v','ai+1','vi+1'); 

subplot(2,1,2)
plot(0:Ts:Tend,U(1,1:Tend/Ts+1));
grid on;
xlabel('Time(sec)');
ylabel('a_real()');
title('Manipulated Variables'); 
legend('u');

figure(2);
plot(0:Ts:Tend,L(1:Tend/Ts+1,3),'r');
hold on
plot(0:Ts:Tend,XR(1,1:Tend/Ts+1),'b');
grid on 
title('positions');
xlabel('Time(sec)');
ylabel('positions');
legend('P_l','P_f');

figure(3);
plot(0:Ts:Tend,L(1:Tend/Ts+1,2),'r');
hold on
plot(0:Ts:Tend,XR(2,1:Tend/Ts+1),'b');
grid on 
title('velocity');
xlabel('Time(sec)');
ylabel('velocity');
legend('V_l','V_f');