一.MPC模型预测控制理论推导
首先对汽车运动学模型进行建模：
设后轴速度为,前轴行速度为考虑后轴轴心的速度约束，得到和后轴速度的关系如下：
考虑到前后轴速度约束的运动学方程为：
设后轴行驶中心为，前轴行驶中心为，根据前后轴之间的几何约束关系可得到:
将上述三个公式联立可得到横摆角速度的表达式为：
由横摆角速度和车速可得到车辆的转向半径和车轮偏角:
即可得到矩阵形式的汽车运动学的表达方程：
由此方程可知，这个动力学系统可以看成是由两个输入,三个输出构成的控制系统，抽象成函数为：
设我们期望的参考轨迹点为,在参考点附近对该抽象映射函数进行泰勒展开以线性化，并且忽略高阶项：
考虑到最终我们控制的是误差的增量，设为误差：
整理计算后得到：
MPC的基本原理，就是在每个计算时域内，求解一个二次规划问题，并且以求解得到的二次规划问题的最优解作为下一个步长的输入，从而对未来多个时间步长内的状态量进行预测控制。在无人驾驶汽车中，目标函数要保证能够快速且平稳的跟踪期望轨迹，就要求我们对系统状态量做出优化。此处我们对状态量进行优化控制，以便快速平稳的跟踪轨迹。
代价函数（或优化函数）如下：
其中，Q，R为权重矩阵，反应了我们对状态量的侧重程度，等号左边的第一项是我们要控制的轨迹跟踪效果，反映了无人车对轨迹的跟随能力，第二项为对输出变化的约束。输出的u为前轮转角和速度。
由于我们最终控制的是误差的增量，对上述方程进行变形得到：
其中为松弛因子，为松弛因子系数
MPC具体迭代过程如下：
为方便后续的理论分析和计算，将状态变量和输出的增量矩阵写成一个矩阵：
由前面状态量的迭代关系可得到：
化简得到新的状态空间表达式：
其中为的单位阵，为单位阵，A为的矩阵,B为
的矩阵，为状态向量矩阵的维数，为输出向量的维数。
MPC滚动预测计算结果和流程如下,其中预测时域，为控制时域，在我们的计算过程中取60，取30，状态量计算过程：
输出的计算过程：
由上述方程可得到：
因为目标函数为
其中这一项是输出的增量，也就是,令为矩阵，得到：
忽略不影响最优结果的常数项，化简得到：
将优化函数配凑成标准二次规划函数得到：
将代价函数表示成标准的二次规划函数，在优化求解时的代价函数为：
在有了代价函数之后，我们还要知道约束条件，MPC的约束条件有两个，分别为上下限约束和增量约束，上下限约束如下：
逐项计算他们的增量关系得到：
令：
则最终得到的约束条件为：
增量约束的思想为，我们取每一次的增量都要小于我们人为设定的值：
即：
二.MPC代码实战（carsim+simulink）
第一步，选择车型，这一步无所谓，喜欢什么车选什么车，但是要注意轴距要和写的s-function一一对应：
车辆基础参数如下，除了轴距之外，也没什么要注意的：
下面是导入到simulink的接口配置：
输入的接口配置如下：
①IMP_SPEED：车辆行驶速度
②IMP_STEER_L1：左前轮转角
③IMP_STEER_R1：右前轮转角
④IMP_STEER_L2：左后轮转角，我们用不到，simulink中置0
⑤IMP_STEER_R2:右后轮转角，用不到，simulink模型中取0
输出的接口配置如下：
①X0:carsim输出的X轴坐标
②Y0：carsim输出的Y轴坐标
③Yaw：carsim输出的航偏角
④Vx：carsim输出的速度
⑤Steer_SW:carsim输出的车辆前轮轮转角
仿真频率的设置如下：
配置好工程之后，点击send to simulink将其发送到simulink，搭建具体的模型如下，xy graph为simulink的画图模块，我们为了方便对比，将车辆的实际行驶轨迹x0，y0和我们预设的理想轨迹x,y用to workspace模块导入到matlab的工作区间中然后画图：
在导入到simulink之后，我们可以看到to workspace前面有out.的前缀，看着让人十分的不爽，下面是清除方法：
点击设置打开：
按步骤操作，将此处的对勾去掉即可。
单位换算模块：
这两个地方为：
下面是完整的MPC控制器脚本代码，可以自行复制，也可以对着教材一行行敲：
%主函数部分
function[sys,x0,str,ts]=MpcControll(t,x,u,flag)
switch flag
case 0
[sys,x0,str,ts]=mdlInitializeSizes;
%初始化函数，对被控方程S-Function进行参数设置，类比构造类。
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)]); % Error handling
end
%初始化主函数
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 6;%离散状态变量的个数
sizes.NumOutputs = 5;%输出量的个数[速度(km/h)，前轮转角(deg)]
sizes.NumInputs = 3;%输入量的个数[X0,Y0,Yaw]
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[0;0;0;0;0;0]; %状态变量初始化
global U;
U=[0;0];
%初始化时间
str = []; % 置空即可
ts = [0.05 0]; % 仿真时间格式:[period, offset]
%更新离散状态函数
function sys = mdlUpdates(t,x,u)%算完了X(k)然后算X(k+1)
sys = x;
%计算输出子函数
function sys = mdlOutputs(t,x,u)
global a b u_piao;
global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]
global kesi;
tic
Nx=3;%状态量的个数
Nu =2;%控制量的个数
Np =60;%预测步长
Nc=30;%控制步长
Row=10;%松弛因子
fprintf('Update start, t=%6.3f\n',t)
yaw_angle =u(3)*3.1415926/180;%CarSim输出的Yaw angle为角度，角度转换为弧度
% %直线路径
% r(1)=5*t; %ref_x-axis
% r(2)=5;%ref_y-axis
% r(3)=0;%ref_heading_angle
% vd1=5;% ref_velocity
% vd2=0;% ref_steering
% %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为5m/s
X_ba=25*sin(0.2*t); %X坐标
Y_ba=35-25*cos(0.2*t); %Y坐标
fai_ba=0.2*t; %航向角
vd1=5; %速度
vd2=0.104; %转角
% %半径为35m的圆形轨迹, 圆心为(0, 35), 速度为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的圆形轨迹, 圆心为(0, 35), 速度为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的圆形轨迹, 圆心为(0, 35), 速度为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)-X_ba;
kesi(2)=u(2)-Y_ba;
kesi(3)=yaw_angle-fai_ba;
kesi(4)=U(1);
kesi(5)=U(2);
T=0.1;%采样时间为0.1s
T_all=40;%总的仿真时间。设置总的仿真时间防止仿真溢出。
L = 2.6; %轴距为2.6m
t_d1=fai_ba;
%对矩阵参数进行初始化
u_piao=zeros(Nu,1);%用来存放二次优化函数解出来的最优输出增量
Q=100 * eye(Nx*Np,Nx*Np);%状态变量权重矩阵，行列都为状态变量维数*预测步长=3*60=180
R=5*eye(Nu*Nc);%输出增量权重矩阵R，控制量最优权重矩阵
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);];%b为我们线性离散化后的第二个系数矩阵
%初始化状态矩阵X(k|t)
A_cell=cell(2,2);
B_cell=cell(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
B=cell2mat(B_cell); %这里的B就是我们在推导下一时刻的状态空间时候的B
C=[ 1 0 0 0 0;
0 1 0 0 0;
0 0 1 0 0];%C矩阵是我们输出方程的系数矩阵
PHI_cell=cell(Np,1);%这个PHI是我们通过总结规律得到的等式右边的第一个系数矩阵，60*1维度
THETA_cell=cell(Np,Nc);%这里的THETA为我们通过总结规律的到的等式右边的第二个系数矩阵，60*30维度
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);
%下面的函数参考教材即可
H_cell=cell(2,2); %这里的H为我们二次规划中的H矩阵，以下来构造二次规划中的H矩阵
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为二次规划的第二个向量
f_cell{1,1} = 2*(error'*Q*THETA);
f_cell{1,2} = 0;
f=cell2mat(f_cell);%将元胞数组转化为矩阵
%约束条件
A_t=zeros(Nc,Nc);%构造At矩阵，见教材第四章
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));%求直积
Ut=kron(ones(Nc,1), U);%此处的U表示上一时刻的输出矩阵。
umin=[-0.2; -0.332];%见教材第四章的约束
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);
%二次规划不等式约束 Ax <= b
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','interior-point-convex');
[X, fval,exitflag]=quadprog(H, f, A_cons, b_cons,[], [],lb,ub,[],options);
fprintf('quadprog EXITFLAG = %d\n',exitflag);
%% 计算输出
u_piao(1)=X(1);%X1为二次规划控制时域内最优的速度控制增量
u_piao(2)=X(2);%X2为二次规划最优前轮转角控制增量
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(1)=u_real(1);
sys(2)=u_real(2);
sys(3)=X_ba;
sys(4)=Y_ba;
sys(5)=fai_ba;
toc
这里使用to worspace模块把理想轨迹数据和预测轨迹数据都发到工作区然后用plot函数画图：
plot(X,Y,'y'); % 使用plot函数将X和Y数据可视化成线图
hold on;
plot(X0,Y0,'r');
hold off;运行结果如下：
参考：
无人驾驶车辆模型预测控制第四章_falcone论文_瑞瑞瑞12123的博客-CSDN博客
《无人驾驶车辆模型预测控制》（第二版）第四章详细学习——算法部分_无人驾驶车辆模型预测控制 pdf_总系学不废的博客-CSDN博客文章来源：https://www.toymoban.com/news/detail-780460.html
《无人驾驶车辆模型预测控制》（第二版）文章来源地址https://www.toymoban.com/news/detail-780460.html
到了这里，关于算法详解+代码（无人驾驶车辆模型预测控制第四章）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
