The MPC controller image range is incorrect

Question

0 个投票

I built an MPC controller, the height of the trace path of the output is correct, but the range is not quite right, can you help me see if the MPC controller is wrong？Stop time is 250 seconds.The following is the code.

function [sys,x0,str,ts] = MPC_car1(t,x,u,flag)
switch flag
  case 0
    [sys,x0,str,ts]=mdlInitializeSizes;
  case 2
    sys=mdlUpdate(t,x,u);
  case 3
    sys=mdlOutputs(t,x,u);
  case {1,4,9}
    sys=[];
  otherwise
    error.(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 3;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 6;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);
x0  = [0.0001,0.0001,0.0001]';
str = [];
ts  = [0.01 0];
global U
U=[0;0];
function sys=mdlUpdate(t,x,u)
sys = x;
function sys=mdlOutputs(t,x,u)
global A1 B1 u_piao U kesi
Nx=3;
Nu=2;
Np=100;
Nc=80;
Row=10;
T=0.01;
xo=u(1);
yo=u(2);
yaw=u(3);
x_ref=u(4);
y_ref=u(5);
yaw_ref=u(6);
vd1=15;
vd2=0;
L=2.6;
kesi=zeros(Nx+Nu,1);
kesi(1)=xo-x_ref;
kesi(2)=yo-y_ref;
kesi(3)=yaw-yaw_ref;
kesi(4)=U(1);
kesi(5)=U(2);
Q=eye(Nx*Np);
R=eye(Nc*Nu);
A1=[1 0 -vd1*T*sin(yaw);
    0 1 vd1*T*cos(yaw);
    0 0 1];
B1=[cos(yaw)*T 0;
    sin(yaw)*T 0;
    tan(vd2)*T/L vd1*T/L/(cos(vd2))^2];
A_cell=cell(2,2);
A_cell{1,1}=A1;
A_cell{1,2}=B1;
A_cell{2,1}=zeros(Nu,Nx);
A_cell{2,2}=eye(Nu);
A=cell2mat(A_cell);
B_cell=cell(2,1);
B_cell{1,1}=B1;
B_cell{2,1}=eye(Nu);
B=cell2mat(B_cell);
C_cell=cell(1,2);
C_cell{1,1}=eye(Nx);
C_cell{1,2}=zeros(Nx,Nu);
C=cell2mat(C_cell);
PHI_cell=cell(Np,1);
for i=1:1:Np
    PHI_cell{i,1}=C*A^i;
end
PHI=cell2mat(PHI_cell);
THETA_cell=cell(Np,Nc);
for i=1:1:Np
    for j=1:1:Nc
        if  i>=j
            THETA_cell{i,j}=C*A^(i-j)*B;
        else
            THETA_cell{i,j}=zeros(3,2);
        end
    end
end
THETA=cell2mat(THETA_cell);
H_cell=cell(2,2);
H_cell{1,1}=THETA'*Q*THETA+R;
H_cell{1,2}=zeros(Nc*Nu,1);
H_cell{2,1}=zeros(1,Nc*Nu);
H_cell{2,2}=Row;
H=cell2mat(H_cell);
error=PHI*kesi;
f_cell=cell(2,1);
f_cell{1,1}=2*error'*Q*THETA;
f_cell{1,2}=0;
f=cell2mat(f_cell);
A_t=zeros(Nc,Nc);
for i=1:1:Nc
    for j=1:1:Nc
        if i>=j
            A_t(i,j)=1;
        else
            A_t(i,j)=0;
        end
    end
end
A_I=kron(A_t,eye(Nu));
Ut=kron(ones(Nc,1),U);
umax=[0.2;0.44];
umin=[-0.2;-0.44];
umax_dt=[0.05;0.005];
umin_dt=[-0.05;-0.005];
Umax=kron(ones(Nc,1),umax);
Umin=kron(ones(Nc,1),umin);
Umax_dt=kron(ones(Nc,1),umax_dt);
Umin_dt=kron(ones(Nc,1),umin_dt);
A_cons_cell=cell(2,2);
A_cons_cell{1,1}=A_I;
A_cons_cell{1,2}=zeros(Nu*Nc,1);
A_cons_cell{2,1}=-A_I;
A_cons_cell{2,2}=zeros(Nu*Nc,1);
A_cons=cell2mat(A_cons_cell);
B_cons_cell=cell(2,1);
B_cons_cell{1,1}=Umax-Ut;
B_cons_cell{2,1}=-Umin+Ut;
B_cons=cell2mat(B_cons_cell);
lb=[Umin_dt;0];
ub=[Umax_dt;Row];
options=optimset('Algorithm','interior-point-convex');
[X,fval,exitflag] =quadprog(H,f,A_cons,B_cons,[],[],lb,ub,[],options);
u_piao(1)=X(1);
u_piao(2)=X(2);
U(1)=kesi(4)+u_piao(1);
U(2)=kesi(5)+u_piao(2);
u_real(1)=U(1)+vd1;
u_real(2)=U(2)+vd2;
sys = u_real;