Simulink+carsim轨迹跟踪联合仿真
CarSim与Simulink联合仿真广泛应用于自动驾驶领域，本次以MPC算法进行轨迹跟踪控制的仿真验证，展示了算法的有效性。模块化设计的Simulink模型包括驾驶员控制模块（纵向双PID和横向MPC）、接口模块以及偏差计算模块，实现对参考轨迹的精确跟踪。通过MPC算法优化前轮转角，结合CarSim的车辆状态信息，实现了对目标轨迹的误差控制。仿真结果表明，轨迹跟踪误差在合理范围内，验证了算法的准确性。
0.Intro.
CarSim作为一种用于自动驾驶控制算法开发的仿真工具，在自动驾驶领域得到了广泛应用。特别是在与Simulink进行联合仿真时，能够有效地验证算法的准确性。此外，网上丰富的教程资源，如Carsim与Simulink联合仿真的教程，也非常值得学习。本次主要实现的是自动驾驶轨迹跟踪算法（MPC）的仿真验证工作，希望能与大家共同分享一些心得。如果有任何问题或建议，欢迎大家在评论区留言。【软件配置：MATLAB2018a+CarSim2019
1.Simulink_Model.
其中，Simulink模块采用模块化设计，支持多种控制算法（算法链接：
Model
驾驶员控制模块主要由纵向控制（PID）和横向控制（MPC）组成；接口模块主要负责处理参考轨迹输入并输出仿真结果；偏差计算模块用于计算车身状态偏差，作为驾驶员控制模块的输入信号。
1.1.Driver_Model
驾驶员模型主要采用横向和纵向控制方式，其中纵向控制采用双PID控制策略。该系统对纵向位移和速度偏差进行控制，以确保车辆动态性能的优化。
双PID
主要采用MPC控制策略实现横向控制，通过优化算法计算出最佳前轮转向角，以实现对目标轨迹的精确跟踪。基于CarSim模拟器获取的车辆状态数据，结合实时反馈信息，确保系统运行的稳定性和可靠性。
Part-codes
function [sys,x0,str,ts] = MPCDL(t,x,u,flag)
switch flag
case 0
[sys,x0,str,ts]=mdlInitializeSizes;
case 2
sys=mdlUpdate(t,x,u);
case 3
sys=mdlOutputs(t,x,u);
case {1,4,9}
sys=[];
otherwise
error.(['Unhandled flag=',num2str(flag)]);
end
%==============================================================
% 初始化
%==============================================================
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 4;
sizes.NumOutputs = 1;
sizes.NumInputs = 5;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = [0,0,0,0]'; %状态量初始化
str = [];
ts = [0.01 0];
global U
U=[0];
%==============================================================
% 更新
%==============================================================
function sys=mdlUpdate(t,x,u)
sys = x;
%==============================================================
% 输出
%==============================================================
function sys=mdlOutputs(t,x,u)
global A B u_piao U kesi
%%
%车辆物理参数
m=1274+71*2;%整车质量
a=1.015;%质心到前轴距离
b=1.895;%质心到后轴距离
Iz=1536.7;%转动惯量
k1=-112600;%前轮侧偏刚度
k2=-89500;%后轮侧偏刚度
%%
%控制参数
Nx=4;
Nu=1;
Np=35;
Nc=1;
Row=10;
T=0.01;
vx=u(5);
%%
kesi=zeros(Nx+Nu,1);
kesi(1)=u(1);
kesi(2)=u(2);
kesi(3)=u(3);
kesi(4)=u(4);
kesi(5)=U;
q=[1,0,0,0;
0,1,0,0;
0,0,1,0;
0,0,0,1];
Q=kron(eye(Np),q);
R=eye(Nc*Nu);
%%1.2.Deviation_Model
主要基于基准的车身状态信息与CarSim输出信息之间的偏差计算，同时包括单位转换的处理（特别注意CarSim输出与MPC算法之间的单位转换 ）
1.3.In_Out_Model
主要用于给定规划轨迹的输入以及仿真结果的输出便于可视化处理。
输入输出Model
2.Carsim_conf.
对于Carsim基础操作，网上已经提供了丰富的教程，无需过多赘述。重点讲述容易出现错误的地方。
1）输入规划的轨迹、速度信息
2）输入输出接口配置
输入
输入配置用于MPC算法给定参考的前轮转角，以及踏板开度。
输出
输出配置主要包含车辆的位移量、速度值以及横摆角速度值等关键指标，这些参数被用于作为MPC算法的状态输入量，以实现对车辆动态行为的精确控制。
3）与simulink模块关联
3.Simulation.
本次仿真以高速公路换道轨迹进行跟踪控制，仿真结果如下：
可视化代码（部分）
close all;
figure;
plot(out.x.data,out.y.data,'r-','linewidth',1);
hold on;
plot(out.x1.data,out.y1.data,'b--','linewidth',1);
xlim([40,250]);
h1=legend("Actual","Test",'Location','NorthWest');
set(h1,'Orientation','horizon')
title("X-Y-Position(m)")
xlabel("x(m)");
ylabel("y(m)");
figure;
plot(out.ryaw,'b--','linewidth',1);
hold on;
plot(out.yaw,'r-','linewidth',1);
h1=legend("Test","Actual",'Location','NorthWest');
set(h1,'Orientation','horizon')
title("Yaw")
xlabel("Time(sec)");
ylabel("Yaw(rad)");
figure;
plot([0,10],[0,0],'r--');
hold on;
plot(out.ydelt,'b','linewidth',1);
title("lateral deviation")
xlabel("Time(sec)");
ylabel("Bias(m)");
figure;
plot(out.steer,'b','linewidth',1);
title("Steer Angle")
xlabel("Time(sec)");
ylabel("Angle(deg)");
通过实验结果可以看出，轨迹的跟踪误差维持在合理范围内，达到了对参考轨迹的有效跟踪。受时间限制，模型仍有改进空间，建议有意愿的读者可以自行优化，期待大家共同探讨。