代码详解——改变控制器的参考速度

function  [sys,x0,str,ts] = mpc001(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,2,4,9}
        sys=[];
    otherwise
        DAStudio.error('Unhandled flag=', num2st(flag));
end
function[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes;
sizes.NumContStates=0;
sizes.NumDiscStates=3;
sizes.NumOutputs=4;
sizes.NumInputs=3;
sizes.DirFeedthrough=1;
sizes.NumSampleTimes=1;
sys=simsizes(sizes);
x0 =[0;0;0];
str = [];
ts  = [0.05 0];
global rr;
global T;
global vd1;
global w;
N=30000;
T=0.05;
vd1=2; %在此处修改参考速度
w=0;
rr=zeros(N+10,4);
%生成产考轨迹
for ir=1:1:N
    if ir<=5100
        rr(ir,1)=19.5+0.1*(ir-1)*T;
        rr(ir,2)=80;
        rr(ir,3)=0;
        rr(ir,4)=0;
    elseif ir<=9787
        rr(ir,1)=45+15*sin(0.0067*(ir-5100)*T);
        rr(ir,2)=65+15*cos(0.0067*(ir-5100)*T);
        rr(ir,3)=0-0.0067*(ir-5100)*T;
        rr(ir,4)=-0.06666667;
    elseif  ir<=15787
        rr(ir,1)=60;
        rr(ir,2)=65-0.1*(ir-9787)*T;
        rr(ir,3)=-1.57;
        rr(ir,4)=0;
    elseif ir<=20473
        rr(ir,1)=75-15*cos(0.0067*(ir-15787)*T);
        rr(ir,2)=35-15*sin(0.0067*(ir-15787)*T);
        rr(ir,3)=-1.57+0.0067*(ir-15787)*T;
        rr(ir,4)=0.06666667;
    elseif ir<=30473
        rr(ir,1)=75+0.1*(ir-20473)*T;
        rr(ir,2)=20;
        rr(ir,3)=0;
        rr(ir,4)=0;
    else
        rr(ir,1)=150;
        rr(ir,2)=20;
        rr(ir,3)=0;
        rr(ir,4)=0;
    end
end
function sys = mdlOutputs(t,x,u)
global rr;
global T;
global vd1;
global w;
i = round(t*20)+1;
%控制器参数
Nx =3;%状态量的个数
Nu =2;%控制量的个数
Np =15;%预测/控制步长
Nc=1;
l=2.7;
vkey=vd1*10;%由于参考路径分辨率的问题，设置vd1的最低分辨率为0.1m/s。此处为新增参数。
%系统约束条件
lv=0.05;
lw=0.0082;
dvx1=-lv;dvx2=lv;dw1=w-lw;dw2=w+lw;
if w>=0.44
    dw2=0.44;
elseif w<=-0.44
    dw1=-0.44;
end
lb(1)=dvx1;
lb(2)=dw1;
ub(1)=dvx2;
ub(2)=dw2;
%初始状态
vx=vd1;
w=0;
td=u(3);
%需要消除的误差
xe(1)=u(1)-rr((i-1)*vkey+1,1); %改变速度之后需要改变相应的跟踪目标点
xe(2)=u(2)-rr((i-1)*vkey+1,2);
xe(3)=u(3)-rr((i-1)*vkey+1,3);
%权重系数
q=[0.01 0 0;0 0.01 0;0 0 0.1];
Q_cell=cell(Np,Np);
for iq=1:1:Np
    for jq=1:1:Np
        if iq==jq
            Q_cell{iq,jq}=q;
        else
            Q_cell{iq,jq}=zeros(Nx,Nx);
        end
    end
end
Q=cell2mat(Q_cell);
R=0.000*eye(Nu*Nc,Nu*Nc);
%预测模型
a=[1    0  -vx*sin(td)*T;
    0    1   vx*cos(td)*T;
    0    0   1];
b=[cos(td)*T                       0;
    sin(td)*T                         0;
    tan(w)*T/l   vx*T/(l*cos(w)*cos(w));];
A_cell=cell(Np,1);
B_cell=cell(Np,1);
for jl=1:1:Np
    A_cell{jl,1}=a^jl;
    for kl=1:1:Nc
        if kl<=jl
            B_cell{jl,kl}=(a^(jl-kl))*b;
        else
            B_cell{jl,kl}=zeros(Nx,Nu);
        end
    end
end
A=cell2mat(A_cell);
B=cell2mat(B_cell);
H=2*(B'*Q*B+R);
f=2*B'*Q*A*xe';
%求解
A_cons=[];
b_cons=[];
X=zeros(10,1);
[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);
dvx=X(1,1);
dw=X(2,1);
vx=vx+dvx;
w=dw;
%控制器输出
mpcout(1)=vx;
mpcout(2)=w;
mpcout(3)=rr((i-1)*vkey+1,1);%输出也需要改变
mpcout(4)=rr((i-1)*vkey+1,2);
sys=mpcout;