基于动力学模型的无人驾驶车辆MPC轨迹跟踪算法及carsim+matlab联合仿真学习笔记
目录
1 模型推导及算法分析
1.1 模型推导
1.1.1 车辆动力学模型
1.1.2 线性时变预测模型推导
1.2 模型预测控制器设计
1.2.1 目标函数设计
1.2.2 约束设计
2 代码解析
2.1 模板框架
2.1.1 S-Function
2.1.2 mdlInitializeSizes函数
2.1.3 mdlUpdates()函数
2.1.4 mdlOutputs()函数
2.2 MPC 算法主体
雅可比矩阵 a b 求解
E 矩阵
参考轨迹 Y ref
H 矩阵
f 矩阵
约束矩阵
quadprog 求解器
3. carsim、simulink联合仿真
3.1 Carsim 设置
3.1.1 车辆参数设置
3.1.2 仿真工况设置
3.1.3 输入输出设置
3.1.4 仿真结果：图形曲线
3.1.5 仿真结果：动画效果
3.2 simulink 仿真
3.2.1 carsim 路径添加
3.2.2 Simulink 搭建
3.2.3 仿真结果
4 不同工况下仿真结果
4.1 控制系统对路面附着条件的鲁棒性
4.2 控制算法对速度的鲁棒性
4.3 不同设计参数对控制器的影响
1 模型推导及算法分析
包括车辆非线性横摆动力学模型、线性时变预测模型推导，以及MPC控制器设计
1.1 模型推导
动力学模型+线性时变预测模型
1.1.1 车辆动力学模型
公式（2.38）是考虑轮胎侧向、纵向滑移的非线性动力学方程，成立的条件是假设前轮偏角 δf 较小，且轮胎模型工作在线性区域。
轨迹跟踪属于横向控制，控制量为小车前轮转角δf。这里默认纵向速度Vx恒定，轨迹跟踪的目的就是使车辆的横摆角 φ 和横向位置 Y 逼近参考轨迹的 φ_ref 和 Y_ref。
选取状态量ξ = [ y_dot, x_dot, φ, φ_dot, Y, X ] ，输出量 η = [ φ ; Y ]，于是得到非线性模型：
1.1.2 线性时变预测模型推导
1、离散化：采用前向欧拉法，将非线性公式（2.29）在 k 时刻离散化
2、 线性化
线性化方法泰勒展开首先需要选择一个起始参照点，参照点的选择有参考系统方法，及针对状态轨迹的线性化方法（参照第一版教材3.3.2节，第二版教材这部分内容缺失）。
a. 参考系统方法是指提前得到了每个时刻的参考控制量 Uref，和状态量 ξref 。即在期望轨迹上已经完全通过。这种设计目的是为了消除当前车辆状态和参考系统状态的偏差，本质上不是直接和期望路径进行偏差比对，而是和参考值进行比对。这种方法缺点在于需要提前获取到一个参考系统。本章节的期望轨迹只有 X_predict、Y_ref 及 φ_ref，不包含全部状态，因此无法提前得到一组参考系统，所以采用状态轨迹法。
b. 状态轨迹法：在 t 时刻工作点，对系统施加持续不变的控制量 ut ，得到一条状态轨迹 ξt，并在该 t 时刻做线性化。
难点：这里所谓对系统施加的持续不变的控制量 ut 就是后面用到的恒定量 u(k-1)，是在轨迹跟随控制前就已经施加到系统上的控制量，并且这个初始控制量在整个轨迹跟踪过程中都会恒定不变地施加在系统上，不会随着系统状态及控制量的变化而消失，每次求解得到的最优 Δu(k) 只是在上一时刻的 u(k-1) 的基础上补偿的一个增量，这个增量对原本施加在系统上的 ut 即 u(k-1) 没有任何影响，通俗的理解就是 u(k+Np)=u(k-1) + Δu(k) + Δu(k+2) + ... + Δu(k+Np-1) + Δu(k+Np), 即每次求解得到的Δu(k)对控制量的增益改变都会叠加到下一时刻。
难点2：这里的u(k-1)其实就是车辆在没有被施加控制增量时候已经被施加了的控制量，也就意味着是上一时刻的控制量在当前时刻的体现，因为还没有施加控制增量，所以自然可以表示上一时刻的控制量。
先在 t=0 时刻的工作点（ξ0,u0）进行泰勒展开，忽略高阶项：
（11-a）ξ(k + 1)表达式就是线性时变预测模型。 Ak,t 、Bk,t 及 dk,t 表示这是一个时变系统 LTV，需要在线求解系数。这种模型缺点在于表达式中不包含控制量的增量 Δu(k)，从而无法使其受到 ΔUmin 和 ΔUmax 的约束，如果在驾驶过程中方向盘的转动幅度过大，会给乘车人带来不好的体验，所以现在需要构造新的形式以便包含对Δu(k)的约束：
1.2 模型预测控制器设计
有了对 Δu(k) 的约束后，就可以开始设计MPC，首先进行目标函数的设计。
1.2.1 目标函数设计
鉴于车辆动力学模型的复杂度较高，加上更多的约束条件，因此控制器在执行过程中，很有可能出现规定时间内求不出最优解的情况，这时车辆方向盘处于失控的危险状态。为了规避这种情况，在目标函数中加入松弛因子ρ。
注意：这里 ρ 越大意味着求解的条件越宽松，但 ρ过大也意味着跟踪效果不理想。
公式（13-a）的 Y(t) 表达式包含了未来 Np 个预测时域内的 ξ(k+i)，将 Y(t) 代入目标函数。
公式（13-c）是标准的二次规划形式，其中待求解 X 已经由一维的 Δδf 扩增为包含 ε 的二维增广矩阵，接下来是约束条件的设计。
1.2.2 约束设计
控制量 u(k) 约束及输出量 φ(k) 和 Y(k) 的约束：
控制增量 Δu 的约束：
公式（14-a）是结合了控制量和输出量的综合形式，公式（14-b）是控制增量和松弛因子的约束。到目前为止已经完成轨迹跟踪的线性时变预测模型，接下来的工作是结合代码分析，并结合carsim联合仿真，查看轨迹跟踪效果。
2 代码解析
2.1 模板框架
2.1.1 S-Function
function [sys,x0,str,ts] = chapter5_2_2(t,x,u,flag) % u表示来自carsim的输入
switch flag,
case 0
[sys,x0,str,ts] = mdlInitializeSizes; % 初始化系统参数的一些基本的维度
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)]);
end2.1.2 mdlInitializeSizes函数
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 0; % 连续状态量个数为 0
sizes.NumDiscStates = 6; % 离散状态量个数为 6
sizes.NumOutputs = 1; % 输出个数为 1，delta_f 作为输出
sizes.NumInputs = 8; % 输入个数为 8，来自 carsim，还包括2个滑移率
sizes.DirFeedthrough = 1; % mdlOutputs函数模块中直接调用了u的数据，所以为1
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[0.001;0.0001;0.0001;0.00001;0.00001;0.00001]; % 6个状态量的初始值
global U; % 1维控制量，前轮转角输出 delta_f
U=[0]; % delta_f(0)=0，起步时前轮转角为 0°
str = [];
ts = [0.05 0]; % 仿真间隔是0.05秒，第二项为0表示从0秒开始立即仿真2.1.3 mdlUpdates()函数
function sys = mdlUpdates(t,x,u) % 这部分没有用到
sys = x;2.1.4 mdlOutputs()函数
u 表示来自carsim的 8 个状态输入，
function sys = mdlOutputs(t,x,u)
tic % 开始计时
fprintf('Update start, t=%6.3f\n',t) % 日志记录当前时刻控制动作的开始
Nx=6;
Nu=1;
Ny=2;
Np =20;
Nc=5;
T=0.05; % 采样时间2.2 MPC 算法主体
tic 含义：
下面这部分是车体的参数，目的是为了求解雅可比矩阵 a 和 b
%% a b 常量参数：车体
Sf=0.2; Sr=0.2;
lf=1.232;lr=1.468;
Ccf=66900;Ccr=62700;
Clf=66900;Clr=62700;
m=1723;g=9.8;I=4175;
%% a b 变量参数，carsim 实时采集
y_dot=u(1)/3.6; % Km/h => m/s
x_dot=u(2)/3.6+0.0001; % 防止分母为0
phi=u(3)*3.141592654/180; % degree => rad
phi_dot=u(4)*3.141592654/180;
Y=u(5);
X=u(6);
Y_dot=u(7); % 前轮纵向滑移率
X_dot=u(8); % 后轮纵向滑移率
global U; % 控制量
delta_f=U(1);
fprintf('Update start, u(1)=%4.2f\n',U(1)) % 日志记录每个控制周期的控制量 雅可比矩阵 a b 求解
接下来就是雅可比矩阵 a 和 b 的矩阵表达式，这里只写出了结果：
%% a b
global a b;
a=[ 1 - (259200*T)/(1723*x_dot), -T*(phi_dot + (2*((460218*phi_dot)/5 - 62700*y_dot))/(1723*x_dot^2) - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2)), 0, -T*(x_dot - 96228/(8615*x_dot)), 0, 0
T*(phi_dot - (133800*delta_f)/(1723*x_dot)), (133800*T*delta_f*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2) + 1, 0, T*(y_dot - (824208*delta_f)/(8615*x_dot)), 0, 0
0, 0, 1, T, 0, 0
(33063689036759*T)/(7172595384320*x_dot), T*(((2321344006605451863*phi_dot)/8589934592000 - (6325188028897689*y_dot)/34359738368)/(4175*x_dot^2) + (5663914248162509*((154*phi_dot)/125 + y_dot))/(143451907686400*x_dot^2)), 0, 1 - (813165919007900927*T)/(7172595384320000*x_dot), 0, 0
T*cos(phi), T*sin(phi), T*(x_dot*cos(phi) - y_dot*sin(phi)), 0, 1, 0
-T*sin(phi), T*cos(phi), -T*(y_dot*cos(phi) + x_dot*sin(phi)), 0, 0, 1];
b=[ 133800*T/1723
T*((267600*delta_f)/1723 - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot))
0
5663914248162509*T/143451907686400
0
0]; 定义雅可比矩阵 a 和 b 的目的，是为了求解 A 和 B 矩阵：
代码实现：
%% A B C
A_cell=cell(2,2);
A_cell{1,1}=a;
A_cell{1,2}=b;
A_cell{2,1}=zeros(Nu,Nx);
A_cell{2,2}=eye(Nu);
A=cell2mat(A_cell);
B_cell=cell(2,1);
B_cell{1,1}=b;
B_cell{2,1}=eye(Nu);
B=cell2mat(B_cell);
C=[0 0 1 0 0 0 0;
0 0 0 0 1 0 0;];这里 得到的A 和 B 矩阵的目的是为了得到 ψ θ Γ Φ 矩阵，参考公式（13-a）：
由于 θ 同时被矩阵E H f 调用，所以这里可以首先实现 θ：
%% THETA (被 E H f 调用)
THETA_cell=cell(Np,Nc);
for j=1:1:Np
for k=1:1:Nc
if k<=j
THETA_cell{j,k}=C*A^(j-k)*B;
else
THETA_cell{j,k}=zeros(Ny,Nu);
end
end
end
THETA=cell2mat(THETA_cell);我们的最终目标是定义 E H f 矩阵，以便构造二次规划标准型矩阵，下面我们按顺序依次实现 E H 和 f 矩阵，结合公式按顺序定义矩阵可以使代码段更加直观。
E 矩阵
第一项便是参考轨迹，这里可以用外部文件生成，也可以直接定义。
参考轨迹 Y ref
%% Y_ref
% phi_ref Y-ref
shape=2.4;%参数名称，用于参考轨迹生成
dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
Xs1=27.19;Xs2=56.46;%参数名称，以上参数是为了生成我们的双移线
X_predict= zeros(Np,1);%用于保存预测时域内的纵向位置信息，这是计算期望轨迹的基础
phi_ref = zeros(Np,1);%用于保存预测时域内的参考横摆角信息
Y_ref = zeros(Np,1);%用于保存预测时域内的参考横向位置信息
% Yita_ref
Yita_ref_cell=cell(Np,1); % ===========这里命名应该是Y_out_ref_cell，因为η的维度是2，不是 Np，但因为输出量Y和横向坐标命名冲突================
T_all=20; % 总的仿真时间，防止计算期望轨迹越界
for p=1:1:Np % 超出仿真时长，
if t+p*T>T_all
X_DOT=x_dot*cos(phi)-y_dot*sin(phi);
X_predict(Np,1)=X+X_DOT*Np*T;
z1=shape/dx1*(X_predict(Np,1)-Xs1)-shape/2;
z2=shape/dx2*(X_predict(Np,1)-Xs2)-shape/2;
Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));
phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2));
Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
else % 在仿真时长内（正常执行部分）
X_DOT=x_dot*cos(phi)-y_dot*sin(phi); % X的斜率，由非线性动力学公式（2.29给出）
X_predict(p,1)=X+X_DOT*p*T; % X是由carsim输出的车辆当前纵向坐标，X_predict(p,1)表示下一时刻的纵坐标
z1=shape/dx1*(X_predict(p,1)-Xs1)-shape/2;
z2=shape/dx2*(X_predict(p,1)-Xs2)-shape/2;
Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2)); % 下一时刻的横坐标参考值
phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2)); % 下一时刻的横摆角参考值
Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)]; % [phi_rel; Y_ref]
end
end
Yita_ref=cell2mat(Yita_ref_cell); % Y_ref 矩阵规范化接下来实现 ψ
%% ψ PSI
PSI_cell=cell(Np,1);
for j=1:1:Np
PSI_cell{j,1}=C*A^j;
end
PSI=cell2mat(PSI_cell);然后是 ξ，这里 ξ 是经过重构的增广矩阵：
%% ξ kesi
kesi=zeros(Nx+Nu,1); % ξnew(k)=[ξ(k); u(k-1)]
kesi(1)=y_dot;
kesi(2)=x_dot;
kesi(3)=phi;
kesi(4)=phi_dot;
kesi(5)=Y;
kesi(6)=X;
kesi(7)=U(1); % u(k-1)GAMMA
%% Γ GAMMA
GAMMA_cell=cell(Np,Np); % 维度 20*20
for p=1:1:Np;
for q=1:1:Np;
if q<=p; % 下三角矩阵
GAMMA_cell{p,q}=C*A^(p-q);
else
GAMMA_cell{p,q}=zeros(Ny,Nx+Nu);
end
end
end
GAMMA=cell2mat(GAMMA_cell);最后是 Φ 的实现比较复杂，首先来看表达式，
dk,t 表达式：
ξt_hat(k+1)：这里需要理解：
%% φ PHI
% ξt_hat_(k+1) 为求解 PHI 做准备
state_k1=zeros(Nx,1);% 下面开始公式（10-a1）即k+1时刻的值 = k时刻的初始值 + T * k时刻的斜率（公式2.38）
state_k1(1,1)=y_dot+T*(-x_dot*phi_dot+2*(Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)+Ccr*(lr*phi_dot-y_dot)/x_dot)/m);
state_k1(2,1)=x_dot+T*(y_dot*phi_dot+2*(Clf*Sf+Clr*Sr+Ccf*delta_f*(delta_f-(y_dot+phi_dot*lf)/x_dot))/m);
state_k1(3,1)=phi+T*phi_dot;
state_k1(4,1)=phi_dot+T*((2*lf*Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)-2*lr*Ccr*(lr*phi_dot-y_dot)/x_dot)/I);
state_k1(5,1)=Y+T*(x_dot*sin(phi)+y_dot*cos(phi));
state_k1(6,1)=X+T*(x_dot*cos(phi)-y_dot*sin(phi));因为前面已经定义了雅可比矩阵 a 和 b，现在可以直接写出 dk,t 矩阵形式：
% dk 为求解 PHI 做准备
d_k=zeros(Nx,1); % (6,1)
d_k=state_k1-a*kesi(1:6,1)-b*kesi(7,1); % 公式（11-b）d_k=ξt_hat_(k+1)-Ak,t * ξt_hat_(k) -Bk,t * u(k-1)这里还需要重构 d_piao_k：
% d_piao_k 为求解 PHI 做准备
d_piao_k=zeros(Nx+Nu,1) % (7,1)
d_piao_k(1:6,1)=d_k; % d_piao_k=[dk; 0]，见公式（12-a）
d_piao_k(7,1)=0;最终可以求 Φ 矩阵：
% PHI 最终求解
PHI_cell=cell(Np,1); % φ(20*7,1)
for p=1:1:Np;
PHI_cell{p,1}=d_piao_k;
end
PHI=cell2mat(PHI_cell);接下来可以实现 E 矩阵
%% E 最终求解
E=zeros(Ny*Np,1);% 源代码名为 error_1
E=Yita_ref-PSI*kesi-GAMMA*PHI; % E=Yref-phi*ξ-Γφ,这里Yita_ref就是YrefH 矩阵
% Q
Q_cell=cell(Np,Np);
for i=1:1:Np;
for j=1:1:Np;
if i==j
Q_cell{i,j}=[2000 0;0 10000;]; % 分别为横摆角权重、横向位置权重
else
Q_cell{i,j}=zeros(Ny,Ny); % Ny = 2
end
end
end
Q=cell2mat(Q_cell);
% R
R=5*10^5*eye(Nu*Nc);
% 松弛因子ρ
Row=1000;
% H 最终求解
H_cell=cell(2,2);
H_cell{1,1}=THETA'*Q*THETA+R; % H = θ'* Q * θ + 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);
H=(H+H')/2; % 保证 H 矩阵是严格的对称阵f 矩阵
f_cell=cell(1,2);
f_cell{1,1}=2 * E' * Q * THETA;
f_cell{1,2}=0;
f=-cell2mat(f_cell);到目前为止已经实现了全部要素矩阵 H f，接下俩可以开始定义约束矩阵。
约束矩阵
%% 控制量u(k)的约束，
A_t=zeros(Nc,Nc);
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)); % 这里A_I就是A_t，因为 Nu=1
Ut=kron(ones(Nc,1),U(1)); Ut = u(k -1)
umin=-0.1744; % 前轮偏角的上约束
umax=0.1744; % 前轮偏角的下约束
Umin=kron(ones(Nc,1),umin);
Umax=kron(ones(Nc,1),umax);输出量 Y(k) 的约束形式：
%% 输出量约束
ycmax=[0.21; 5]; % 横摆角和纵向位置的约束
ycmin=[-0.3;-3];
Ycmax=kron(ones(Np,1),ycmax);
Ycmin=kron(ones(Np,1),ycmin);u(k) 和 Y(k) 约束的增广混合形式：
% 结合控制量约束和输出量约束
A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1);THETA zeros(Ny*Np,1);-THETA zeros(Ny*Np,1)};
b_cons_cell={Umax-Ut;-Umin+Ut;Ycmax-PSI*kesi-GAMMA*PHI;-Ycmin+PSI*kesi+GAMMA*PHI};
A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值Δu(k) 的约束，也就是QP求解方程的标准状态量X的约束：
%% 控制增量约束
delta_umin=-0.0148*0.4; % 前轮偏角变化量的下约束
delta_umax= 0.0148*0.4; % 前轮偏角变化量的上约束
delta_Umin=kron(ones(Nc,1),delta_umin);
delta_Umax=kron(ones(Nc,1),delta_umax);
M=10; % ε的上界
lb=[delta_Umin;0]; % [Δu(k);ε]的增广下界约束 lb
ub=[delta_Umax;M]; % [Δu(k);ε]的增广上界约束 ubquadprog 求解器
调用 matlab 自带的二次规划求解器 quadprog 进行求解，这部分是求解器的核心
%% quadprog求解器
options = optimset('Algorithm','interior-point-convex');% 选用内点法求解
x_start=zeros(Nc+1,1); % [Δu(k);ε]的初始值为 0
[X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,x_start,options);
% [Δu(k);ε]的解存放在变量 X 中，包含未来Nc个时间点的Δu(k+i)结果处理，将计算结果 u(k) 输出到carsim中：
%% 计算输出
u_piao=X(1); % 从Nc个未来时刻的Δu(k+i)中提取第一个时刻的Δu(k)作为当前时刻的最优解
U(1)=kesi(7,1)+u_piao; % 这里kesi(7,1)是u(k-1)，加上Δu(k),则为当前时刻实际输出u(k)
sys= U; % 这里的 U 作为simulink的输出量，直接复制给carsim，进行前轮转角控制
toc % 一个完整的控制周期计时结束现在已经实现了matlab全部的代码，下一节将进行联合仿真。
3. carsim、simulink联合仿真
本节主要以实例讲解通过搭建 simulink/carsim联合仿真平台，对设计的MPC控制器进行验证。下图是基于动力学模型的MPC轨迹跟踪控制器进行carsim与simulink联合仿真的实例。carsim版本是8.02，matlab版本是2018a。
3.1 Carsim 设置
包括车辆参数设置、仿真工况设置、输入输出接口设置、仿真结果查看。
3.1.1 车辆参数设置
接下来就是车辆参数的具体设置，包括车辆尺寸信息和轮胎信息。
返回车辆参数设定界面，选择悬架和轮胎型号。
3.1.2 仿真工况设置
3.1.3 输入输出设置
由于本章的轨迹跟随为横向控制，只对前轮转角进行控制，所以carsim的输入端口选择车辆的四个轮胎的转角。其中2个前轮的转角由控制器提供，2个后轮的转角固定为0.0°，输出端口包括动力学系统的6个状态量和2个前轮的滑移率，共8个输出量。
这时就实现了下图所示的 carsim 和 simulink 之间的数据流传输：
3.1.4 仿真结果：图形曲线
新建数据集：
添加车辆质心的横向位置 Y 关于 X 的图像：
添加横摆角 φ 关于 纵向位置 X 的图像：
点击 Plot 开始绘图
carsim 图形曲线与 simulink 采集的数据对比，结果完全一致：
3.1.5 仿真结果：动画效果
回到主界面，在下拉列表中选择视角，或者新建自定义数据集：
进入视角设定界面，为了展示双移线的跟踪效果，这里选择拖影效果，即 Ghost Trail Images ，选择拖影数量、时间间隔，将观测视角设定成车身起始点后方的固定视角：
跟踪动画效果如下，可以观察到车辆实际的双移线运动轨迹：
3.2 simulink 仿真
3.2.1 carsim 路径添加
重启计算机后可在 MATLAB 的 Simulink Library Brower 中找到 Carsim 模块。
3.2.2 Simulink 搭建
首先确保matlab 安装了自动驾驶工具包
部分 matlab 版本不能直接显示，这里要点 Fix
然后选第二项
这时就可以添加 carsim s-function 模块
下面是搭建完成的 simulink 完整模块
在 s-function中编辑m文件
3.2.3 仿真结果
基于动力学模型的模型预测控制器主要用于较高速下的轨迹跟踪。在这种情况下对于控制器的评价不仅仅是跟踪精度，还需要重点考虑跟踪过程中的稳定性。在普通车轮形式稳定性的测试中，双移线工况是常用的测试路段。双移线参考轨迹如下图所示，由参考横向位置Yref和参考横摆角φre成，两者都为总关于纵向位置Y 的非线性函数：
在车速为30 km/h，道路摩擦系数为1的工况下，小车轨迹跟踪的仿真结果如下图，可以看出实际运行轨迹与双移线轨迹基本一致：
这里可以看到控制量被约束在限制范围内
4 不同工况下仿真结果
所设计的主动转向跟踪控制器以车辆前轮偏角作为控制器的输入，控制目标是通过不断减少与参考轨迹的偏差，跟踪参考轨迹。
4.1 控制系统对路面附着条件的鲁棒性
无人驾驶车辆在不同附着条件的道路上（如：干燥路面、湿滑路面）行驶时，车辆自身动力学参数，如轮胎侧偏刚度等将会发生变化，同时也会出现地面提供的侧向力不足的情况。这给控制器的性能带来一定的挑战。
若附着条件较差，地面不能提供足够的侧向力，车辆转向时横摆角会出现较大的偏差，轨迹跟踪控制器能够及时修正偏差，最终将偏差收敛为0。且跟踪过程中车辆的控制量及控制增量都处于约束范围内。软约束的加入能够保证系统在给定时间内得到可行解，所以控制系统能够在不同附着条件下较好地跟踪期望轨迹，且具备良好的稳定性。
Carsim 路面附着系数修改指导：
选择附着系数，通常干燥路面附着系数为0.7 ~ 1，湿滑路面附着系数为0.4左右
4.2 控制算法对速度的鲁棒性
很多控制器往往需要针对不同的行驶速度确定不同的控制参数，而模型预测控制器能够根据建立的车辆模型预测系统未来的输出，对车速的变化具有很强的鲁棒性。
主动转向系统仿真实验分别在10m/s 20m/s 和 30m/s的速度下行驶，道路附着条件良好，附着系数为0.8，控制器所采用的参数为T=0.05, Np=15，Nc=10。
从结果可以看出，速度越高，控制量的增量就越大，但不超过约束范围。各动力学约束保持在区间范围内，质心侧偏角则远低于极限范围，表明车辆行驶非常平稳，车速增加并不会导致车辆稳定性下降。
4.3 不同设计参数对控制器的影响
不同参数对于控制器性能的影响有所差异。对于模型预测控制器来说，最重要的设计参数就是预测时域和控制时域。采用较小的预测时域和控制时域会导致跟踪偏差较大，但对于提升控制系统的实时性效果明显。