MPC控制算法代码matlab实现，matlab func实现，自编写matlab function实现MPC算法

MPC工具箱提供的MPC模块不能实现权重参数的实时修改，有必要自己编写一个实现模型预测控制算法的matlab function。

• 主义事项

求解QP问题的时候使用哪一个函数更好？？？？

mpcqpsolver   (To be removed) Solve a quadratic programming problem using the KWIK algorithm

quadprog

mpcInteriorPointSolver

以下物理量必须设置相同，不然容易报错或求解不出理论控制量：

1. matlab func模块的调用频率

2. MPC算法的采样频率。

3. 被控对象的状态空间方程的离散化频率。

以一个双积分系统为例子，MPC的实现代码如下

function U = myMPC(Ref,x,v,q1,ut)
% clear
% s=tf('s')
% G=1/s^2
% G=ss(G)
% A=G.A
% B=G.B
% C=eye(2)
% D=zeros(2,1)
% sys=ss(A,B,C,D)
% plant=c2d(sys,0.01)
coder.extrinsic('quadprog');
U = 0;
deltaU =0;

Ak = ...
  [1     0;
  0.01     1];
 
Bk = ...
[0.01;
5e-05];

Ck=eye(2);
Dk=zeros(2,1);

dk=zeros(6,1);
ek=zeros(4,1);
% 权重矩阵
Q=diag([1.5+q1,100]);  % 1.52 100  2.8 
R=diag([0]);
S=diag([0]);
pho=0.05;
e=0.1;

Ts=0.01;

xk=[v;x;ut];
yref=Ref;

% 维数说明
m=1; % 控制
n=2; % 状态变量
n0=m+n; % 新的状态维度
Hc=2;
Hp=10;
p=size(Ck,1) ; %输出量维数
% 已知量
u_1 = ut;
U_1=kron(ones(Hc,1),u_1);
% syms deltau deltau1 deltau2 deltau3 deltau4
% deltaU=[deltau;deltau1;deltau2;deltau3;deltau4]
% U=M*deltaU+U_1

%% 令 x=[x(k);u(k-1)]  控制量变为 delta u  则新系统的状态空间为
Ak=[ Ak,Bk;zeros(m,n),eye(m) ];
Bk=[Bk;eye(m)];
Ck=[Ck,Dk];
Dk=Dk;
dk=[dk;zeros(m,1)];
ek=ek;
%% 预测输出矩阵 k+1 ... K+Hp    共Hp个
% 控制输出矩阵  k...K+Hc-1   共Hc个

% compute PSIk
[m1,n1]=size(Ck*Ak);
PSIk=zeros(m1*Hp , n1);
for i=1:Hp
    PSIk( (i-1)*m1+1 :(i-1)*m1+m1 , 1:n1)=Ck*Ak^i;
end
% compute THETAk
[m2,n2]=size(Ck*Bk);
THETAk=zeros(m2*Hp , n2*Hc);
for i=1:Hp
    for j=1:Hc
        if i>=j
            THETAk( (i-1)*m2+1:(i-1)*m2+m2, (j-1)*n2+1:(j-1)*n2+n2 ) = Ck*Ak^(i-j)*Bk;
        elseif j-i==1
            THETAk( (i-1)*m2+1:(i-1)*m2+m2, (j-1)*n2+1:(j-1)*n2+n2)=Dk;
        else
            THETAk( (i-1)*m2+1:(i-1)*m2+m2, (j-1)*n2+1:(j-1)*n2+n2)=zeros(m2,n2);
        end
    end
end
% compute TAUk
[m3,n3]=size(Ck);
TAUk=zeros(m3*Hp , n3*Hp);
for i=1:Hp
    for j=1:Hp
        if i>=j
            TAUk( (i-1)*m3+1:(i-1)*m3+m3, (j-1)*n3+1:(j-1)*n3+n3 ) = Ck*Ak^(i-j);
        else
            TAUk( (i-1)*m3+1:(i-1)*m3+m3, (j-1)*n3+1:(j-1)*n3+n3)=zeros(m3,n3);
        end
    end
end
% compute PHIk
PHIk=zeros(n0*Hp,1);  
% compute LAMBDA
LAMBDAk=zeros(p*Hp,1);   %% 线性时变时候不为0！！！！！！！！！！！！！

% compute Qe
[m4,n4]=size(Q);
Qe=zeros(m4*Hp,n4*Hp);
for i=1:Hp
    for j=1:Hp
        if i==j
            Qe( (i-1)*m4+1:(i-1)*m4+m4, (j-1)*n4+1:(j-1)*n4+n4 ) = Q;
        else
            Qe( (i-1)*m4+1:(i-1)*m4+m4, (j-1)*n4+1:(j-1)*n4+n4 )=zeros(m4,n4);
        end
    end
end
% compute Re
[m5,n5]=size(R);
Re=zeros(m5*Hc,n5*Hc);
for i=1:Hc
    for j=1:Hc
        if i==j
            Re( (i-1)*m5+1:(i-1)*m5+m5, (j-1)*n5+1:(j-1)*n5+n5 ) = R;
        else
            Re( (i-1)*m5+1:(i-1)*m5+m5, (j-1)*n5+1:(j-1)*n5+n5 )=zeros(m5,n5);
        end
    end
end
% compute Se
[m6,n6]=size(S);
Se=zeros(m6*Hc,n6*Hc);
for i=1:Hc
    for j=1:Hc
        if i==j
            Se( (i-1)*m6+1:(i-1)*m6+m6, (j-1)*n6+1:(j-1)*n6+n6 ) = S;
        else
            Se( (i-1)*m6+1:(i-1)*m6+m6, (j-1)*n6+1:(j-1)*n6+n6 )=zeros(m6,n6);
        end
    end
end

K=tril(ones(Hc));
Im=eye(m,m);
M=kron(K,Im);

[m7,n7]=size(yref);
Yrefk=zeros(m7*Hp,1);
for i=1:Hp
    Yrefk( (i-1)*m7+1:(i-1)*m7+m7 , 1)=yref;
end
epsilon=PSIk*xk+TAUk*PHIk+LAMBDAk-Yrefk;
%% 最终变量 Hk Gk Pk
Hk=...
    [ 2*(THETAk'*Qe*THETAk+Re+M'*Se*M) ,  zeros(m*Hc,1) ;
    zeros(1,m*Hc)                    ,  pho             ];

Gk=...
    [  2*epsilon'*Qe*THETAk+2*U_1'*Se*M  ,  0  ];


Pk=...
    [  epsilon'*Qe*epsilon + U_1'*Se*U_1 + pho*e^2  ];


%% QP问题

完整代码请看文尾...

...

MPC算法的参考文献为：

基于模型预测控制的无人驾驶车辆轨迹跟踪控制算法研究_孙银健

！！！！！

声明：若果您觉得您被侵犯了权利，请联系本博客作者，同样的，若引用了本文也请注明出处。

！！！！！

想要完成代码的同学请联系我