function[sys,x0,str,ts]=MY_MPCController(t,x,u,flag)
switch flag,
case 0
[sys,x0,str,ts]=mdlInitializeSizes;
case 2
sys=mdlUpdates(t,x,u);
case 3
sys=mdlOutputs(t,x,u);
case {1,4,9}
sys=[];
otherwise
error(['unhandled flag=',num2str(flag)]);
end
%s函数主程序结束
function[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes;
sizes.NumContStates=0;
sizes.NumDiscStates=3;
sizes.NumOutputs=2;
sizes.NumInputs=3;
sizes.DirFeedthrough=1;
sizes.NumSampleTimes=1;
sys=simsizes(sizes);
x0=[0;0;0];
global U;
U=[0;0];
str=[];
ts=[0.05 0];
%初始化子函数结束
function sys=mdlUpdates(t,x,u)
sys=x;
%end of mdlUpdate.
function sys=mdlOutputs(t,x,u)
global a b u_piao;
global U;
global kesi;
Nx=3;%状态量的个数
Nu=2;%控制量的个数
Np=60;%预测步长
Nc=30;%控制步长
Row=10;
fprintf('Update start,t=%6.3f\n',t)
t_d=u(3)*3.14159265/180;%角度和弧度的转化
r(1)=25*sin(0.2*t);
r(2)=25+10-25*cos(0.2*t);
r(3)=0.2*t;
vd1=5;
vd2=0.104;
%半径为25m的圆形轨迹,速度为5m
%参数的设计
kesi=zeros(Nx+Nu,1);
kesi(1)=u(1)-r(1);
kesi(2)=u(2)-r(2);
kesi(3)=t_d-r(3);
kesi(4)=U(1);
kesi(5)=U(2);
fprintf('Update start,u(1)=%4.2f\n',U(1))
fprintf('Update start,u(2)=%4.2f\n',U(2))
T=0.05;
T_all=40;
L=2.6;
%Mobile Robor/Vehicle variable
u_piao=zeros(Nx,Nu);
Q=eye(Nx*Np,Nx*Np);
R=5*eye(Nu*Nc);
a=[1 0 -vd1*sin(t_d)*T;0 1 vd1*cos(t_d)*T;0 0 1;];
b=[cos(t_d)*T 0;sin(t_d)*T 0;tan(vd2)*T/L vdl*T/((cos(vd2)^2)*L);];
%对应4.6
A_cell=cell(2,2);
B_cell=cell(2,1);
A_cell{1,1}=a;
A_cell{1,2}=b;
A_cell{2,1}=zeros(Nu,Nx);
A_cell{2,2}=eye(Nu);
B_cell{1,1}=b;
B_cell{2,1}=eye(Nu);
A_cell2mat(A_cell);
B_cell2mat(B_cell);
C=[1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;];
%对应4.10
PHI_cell=cell(Np,1);
THETA_cell=cell(Np,Nc);
for j=1:1:Np
PHI_cell{j,1}=C*A^j;
for k=1:1:Nc
if k<=j
THETA_cell{j,k}=C*A^(j-k)*B;
else
THETA_cell{j,k}=zeros(Nx,Nu);
end
end
end
PHI=cell2mat(PHI_cell);
THETA=cell2mat(THETA_cell);
%对应4.12
H_cell=cell(2,2);
H_cell{1,1}=THETA'*Q*THETA+R;
H_cell{1,2}=zeros(Nu*Nc,1);
H_cell{2,1}=zeros(1,Nu*Nc);
H_cell{2,2}=0;
H=cell2mat(H_cell);
error=PHI*kesi;
f_cell=cell(1,2);
f_cell{1,1}=2*error'*Q*THETA;
f_cell{1,2}=Row;
f=cell2mat(f_cell);
%对应4.19
A_t=zeros(Nc,Nc);
for p=1:1:Nc
for q=1:1:Nc
if q<=p
A_t(p,q)=1;
else
A_t(p,q)=0;
end
end
end
A_I=kron(A_t,eye(Nu));
%对应4.17
Ut=kron(one(Nc,1),U);
umin=[-0.2;-0.54;];
umax=[0.2;0;0.332];
delta_umin=[-0.05;-0.0082;];
delta_umax=[0.05;0.0082];
Umin=kron(one(Nc,1),umin);
Umax=kron(one(Nc,1),umax);
A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1)};
b_cons_cell={Umax-Ut;-Umin+Ut};
A_cons=cell2mat(A_cons_cell);
%状态量不等式约束增益
b_cons=cell2mat(b_cons_cell);
M=10;
delta_Umin=kron(ones(Nc,1),delta_umin);
delta_Umax=kron(ones(Nc,1),delta_umax);
lb=[delta_Umin;0];
ub=[delta_Umax;M];
%开始求解
options=optimset('Algorithm','active-set');
%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;
toc
%End of mdlOutputs.
这是我的程序