目录
4.Simulink框图搭建：
5.代码：
代码：大家也可以通过更换设定的轨迹曲线去跟踪别的曲线轨迹。
function [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
switch flag
case 0
[sys,x0,str,ts] = mdlInitializeSizes; % Initialization
case 2
sys = mdlUpdates(t,x,u); % Update discrete states
case 3
sys = mdlOutputs(t,x,u); % Calculate outputs
case {1,4,9} % Unused flags
sys = [];
otherwise
error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.
%==============================================================
% Initialization
%==============================================================
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 3;
sizes.NumOutputs = 2;
sizes.NumInputs = 3;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[0;0;0];
global U;
U=[0;0];
% Initialize the discrete states.
str = []; % Set str to an empty matrix.
ts = [0.1 0]; % sample time: [period, offset]
%End of mdlInitializeSizes
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)
sys = x;
%End of mdlUpdate.
%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)
global a b u_piao;
global U;
global kesi;
tic
Nx=3;%状态量的个数
Nu =2;%控制量的个数
Np =60;%预测步长
Nc=30;%控制步长
Row=10;%松弛因子
fprintf('Update start,t=%6.3f\n',t)
t_d =u(3)*3.1415926/180;%CarSim输出的为角度，角度转换为弧度
%直线路径
% r(1)=5*t;
% r(2)=5;
% r(3)=0;
% vd1=5;
% vd2=0;
%半径为25m的圆形轨迹,速度为5m/s
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的圆形轨迹,速度为3m/s
% r(1)=25*sin(0.12*t);
% r(2)=25+10-25*cos(0.12*t);
% r(3)=0.12*t;
% vd1=3;
% vd2=0.104;
%半径为25m的圆形轨迹,速度为10m/s
% r(1)=25*sin(0.4*t);
% r(2)=25+10-25*cos(0.4*t);
% r(3)=0.4*t;
% vd1=10;
% vd2=0.104;
% %半径为25m的圆形轨迹,速度为4m/s
% r(1)=25*sin(0.16*t);
% r(2)=25+10-25*cos(0.16*t);
% r(3)=0.16*t;
% vd1=4;
% vd2=0.104;
kesi=zeros(Nx+Nu,1);
kesi(1)=u(1)-r(1);%u(1)==X(1)
kesi(2)=u(2)-r(2);%u(2)==X(2)
kesi(3)=t_d-r(3); %u(3)==X(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.1;
T_all=40;%临时设定，总的仿真时间，主要功能是防止计算期望轨迹越界
% Mobile Robot Parameters
L = 2.6;
% Mobile Robot variable
%矩阵初始化
u_piao=zeros(Nx,Nu);
Q=100*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 vd1*T/((cos(vd2)^2)*L);];
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;];
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);%size(PHI)=[Nx*Np Nx+Nu]
THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*(Nc+1)]
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}=Row;
H=cell2mat(H_cell);
error=PHI*kesi;
f_cell=cell(1,2);
f_cell{1,1}=2*error'*Q*THETA;
f_cell{1,2}=0;
% f=(cell2mat(f_cell))';
f=cell2mat(f_cell);
%% 以下为约束生成区域
%不等式约束
A_t=zeros(Nc,Nc);%见falcone论文 P181
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));%对应于falcone论文约束处理的矩阵A,求克罗内克积
Ut=kron(ones(Nc,1),U);%此处感觉论文里的克罗内科积有问题,暂时交换顺序
umin=[-0.2;-0.54;];%维数与控制变量的个数相同
umax=[0.2;0.332];
delta_umin=[-0.05;-0.0082;]; %源代码有错，速度下界没加负号
delta_umax=[0.05;0.0082];
Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(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'); %新版quadprog不能用有效集法，这里选用内点法
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.注：克罗内克积
定义：克罗内克积是两个任意大小的矩阵间的运算。
如果A是一个 m x n 的矩阵，而B是一个 p x q 的矩阵，克罗内克积则是一个 mp x nq 的矩阵。
比如在A=[1,2;3,1];B=[0,3;2,1]，kron(A,B)就表示A中的每一个元素都与B相乘，得到一个4*4的一个大矩阵。
