基于运动学模型的轨迹跟踪控制算法——模型预测控制（MPC）法（四）之代码实现
基于运动学模型的轨迹跟踪控制算法——模型预测控制（MPC）法（四）之代码实现（m脚本）
（KingkWinner——创作不易，转载请标明出处）
% carsim中的输出端为X坐标（m)、Y坐标(m)、横摆角(°）、质心处的纵向速度（km/h）、方向盘转角(°）
% Matlab 2020b,Carsim 2018.1
function [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag) %构建S-function函数，t为当前的仿真时间，x为状态变量，u是输入（simulink模块的输入），flag为仿真过程中的标志位
switch flag %sys输出根据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); %设置完后赋值给sys输出
x0 =[0;0;0]; %状态量初始化，横纵坐标以及横摆角都为0
global U; %设定U为全局变量
U=[0;0]; %初始的控制量为0 , 0。两行一列
% Initialize the discrete states.
str = []; % Set str to an empty matrix.str为保留参数，mathworks公司还没想好怎么用它，一般在初始化中将其滞空即可
ts = [0.1 0]; % ts为1*2维度的向量，采样周期+偏移量sample time: [period, offset]，仿真开始0s后每0.1秒运行一次
%End of mdlInitializeSizes
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u) %flag=2表示此时要计算下一个离散状态，即x（k+1)
sys = x;
%End of mdlUpdate.
%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u) %flag=3表示此时要计算输出，如果sys=[],则表示没有输出
global a b u_piao; %a,b和u_piao矩阵为全局矩阵
global U; %U全局控制量误差，2*1 维度矩阵
global kesi; %kesi为全局，新的状态向量，[k时刻的状态量误差；k-1时刻的控制量误差],5*1矩阵（误差即偏差）
tic
Nx=3;%状态量的个数
Nu =2;%控制量的个数
Np =60;%预测时域，t+60*0.1
Nc=30;%控制时域
Row=10;%松弛因子权重
fprintf('Update start,t=%6.3f\n',t)
t_d =u(3)*3.1415926/180; % t_d为横摆角，CarSim输出的为角度，角度转换为弧度。u(3)是车辆的实际航向角。
%直线路径
% r(1)=5*t;
% r(2)=5;
% r(3)=0;
% vd1=5;
% vd2=0;
% %半径为25m的圆形轨迹,速度为5m/s
r(1)=25*sin(0.2*t); %参考的X
r(2)=35-25*cos(0.2*t); %参考的Y
r(3)=0.2*t; % 参考的横摆角，0.2怎么算出来的？ 答案可以根据书中p106 公式推出,根据车辆运动学的公式带入速度，前轮转角和轴距即可算出
vd1=5; %速度5m/s，这是参考速度
vd2=0.104; %前轮偏角=tan(轴距/半径）=tan(2.6/25)=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;
% 上面代码已经定义过：U全局控制量误差。
kesi=zeros(Nx+Nu,1); %构造新的状态量矩阵 ，5*1维矩阵
kesi(1)=u(1)-r(1);%u(1)==X(1) 将状态量误差放到相应的位置上，第一行纵向位置X的误差。u（1）是指纵向位置x; r(1)是指参考的纵向位置
kesi(2)=u(2)-r(2);%u(2)==X(2) 第二横向位置Y的误差。u(2)是指横向位置；r(2)是指参考的横向位置。
kesi(3)=t_d-r(3); %u(3)==X(3) 第三行是横摆角的误差。t_d是指车辆的横摆角；r(3)是指参考的横摆角。（所谓参考的横摆角也就是期望横摆角或者理想的横摆角）
kesi(4)=U(1); % 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; %采样时间为0.1s，即100毫秒
T_all=40;%临时设定，总的仿真时间。设置总的仿真时间防止仿真溢出。
L = 2.6; %轴距为2.6m
%矩阵初始化 （）
% u_piao=zeros(Nx,Nu); %构造3*2维度的矩阵来存放控制量误差
u_piao=zeros(Nu,1); %构造3*2维度（小刚认为这应该是Nu=2*1维度）的矩阵来存放控制量误差的变化量
Q=100*eye(Nx*Np,Nx*Np); %权重矩阵Q为180*180的单位矩阵，Q是指输出状态矩阵
R=50*eye(Nu*Nc); %权重矩阵R为60*60的单位矩阵。R是指控制量误差的状态矩阵
% R=5*eye(Nu*Nc); %权重矩阵R为60*60的单位矩阵
t_d1 = r(3);%t_d1是指参考的车辆横摆角度
a=[1 0 -vd1*sin(t_d1)*T;
0 1 vd1*cos(t_d1)*T;
0 0 1;]; %a为我们线性离散化后的第一个系数矩阵
b=[cos(t_d1)*T 0;
sin(t_d1)*T 0;
tan(vd2)*T/L vd1*T/((cos(vd2)^2)*L);]; %b为我们线性离散化后的第二个系数矩阵
A_cell=cell(2,2); % 构建2*2的元胞数组
B_cell=cell(2,1); %构建2*1的元胞数组
A_cell{1,1}=a; %将a矩阵放到A_cell的第一行第一个位置
A_cell{1,2}=b; %将b矩阵放到A_cell的第一行第二个位置
A_cell{2,1}=zeros(Nu,Nx); %将2*3的零矩阵放到A_cell第二行的第一个位置
A_cell{2,2}=eye(Nu); %将2*2的单位阵放到A_cell第二行的第二个位置
B_cell{1,1}=b;%将b矩阵放到B_cell的第一行
B_cell{2,1}=eye(Nu);%将2*2的单位阵放到B_cell第二行
A=cell2mat(A_cell); %这里的A就是我们在推导下一时刻的状态空间时候的A，详见CSDN推导过程
B=cell2mat(B_cell); %这里的B就是我们在推导下一时刻的状态空间时候的B
C=[1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;];%这个C矩阵是我们输出方程yita的系数矩阵，因为我们可能不需要把每个状态量都输出所以就可以通过设置C矩阵来输出我们想要的状态量，在这里我们输出的是X的误差、Y的误差以及横摆角的误差
PHI_cell=cell(Np,1);%这个PHI是我们通过总结规律得到的等式右边的第一个系数矩阵，60*1维度
THETA_cell=cell(Np,Nc);%这里的THETA为我们通过总结规律的到的等式右边的第二个系数矩阵，60*30维度，具体请详见我们CSDN的推导过程
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; %C为3*5矩阵；A为5*5；B为5*2，所以C*A*B为3*2矩阵
else
THETA_cell{j,k}=zeros(Nx,Nu); %详见上节博客CSDN推导手稿过程
end
end
end
PHI=cell2mat(PHI_cell);%size(PHI)=[Nx*Np Nx+Nu]，180*5维度
THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*Nc+1] 180*
H_cell=cell(2,2); %这里的H为我们二次规划中的H矩阵，以下来构造二次规划中的H矩阵
H_cell{1,1}=THETA'*Q*THETA+R;
H_cell{1,2}=zeros(Nu*Nc,1); %60*1维度
H_cell{2,1}=zeros(1,Nu*Nc); %1*60维度
H_cell{2,2}=Row; %H矩阵的右下角的元素就只有一个就是我们的松弛因子
H=cell2mat(H_cell); %由于松弛因子的影响，最终的H矩阵为61*61
H=(H+H')/2;
error=PHI* %这里的error就是我们所设的E矩阵
f_cell=cell(1,2); %f为二次规划的第二个向量，下面我们来构造它
f_cell{1,1}=2*error'*Q*THETA;
f_cell{1,2}=0; %详见CSDN推导过程
% 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,求克罗内克积。仔细学习我的上一篇博客手稿对应部分，并结合北理工《无人驾驶车辆模型预测控制-第2版》P87部分即可理解
Ut=kron(ones(Nc,1),U);%此处的U表示的应该是上一时刻控制量误差（即偏差）组成的矩阵。
umin=[-0.2;-0.436;];%维数与控制变量的个数相同，课本P81页
umax=[0.2;0.436;];
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];%（求解方程）状态量上界，包含控制时域内控制增量和松弛因子上界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); % X1为二次规划求出来的是控制时域内最优的速度控制增量
u_piao(2)=X(2); % X2为二次规划求出来的最优前轮转角控制增量
U(1)=kesi(4)+u_piao(1); %kesi4为速度误差。kesi4为速度误差与速度误差增量u_piao(1)相加得到我们下一时刻的kesi矩阵的第四行
U(2)=kesi(5)+u_piao(2); %kesi5为k-1时刻前轮偏角误差，道理同上，与前轮偏角误差的增量相加得到我们下一时刻的kesi矩阵的第五行
u_real(1)=U(1)+vd1; %再与下面的参考速度相加才是真正的速度
u_real(2)=U(2)+vd2; %道理同上
sys= u_real; %将算出来的真正的控制量输出输出
toc
% End of mdlOutputs.
学习上面的代码后，大家估计在下面的最后部分（如下图）还有疑问，下面将解决这个疑问
！