简介：自动驾驶车辆路径跟踪控制是智能车辆研究中的核心问题，其本质是如何让车辆精准且稳定地跟随预设轨迹。模型预测控制（MPC）凭借其处理多约束与预测能力，成为该领域的主流算法之一。在实际工程落地中，通常需要借助高精度车辆动力学仿真软件Carsim与MATLAB/Simulink联合搭建验证环境，以低成本复现真实车辆的动态响应。本文从车辆运动学模型出发，阐述MPC控制器设计原理、线性时变预测模型的构建方法，以及如何利用Carsim与Simulink协同仿真实现路径跟踪控制。内容涵盖联合仿真环境配置、MPC参数整定、路径点插值与弗勒内坐标系误差求解等关键环节，并结合工程实践给出完整的源码框架与调试经验，为自动驾驶方向的研究生及工程师提供一套可快速上手的车辆路径跟踪控制实现方案。 做车辆路径跟踪控制这几年，绕不开的一套组合拳就是MATLAB加Carsim加Simulink再加MPC。尤其是做自动驾驶方向的学生或者刚入行的工程师，基本都会在某个阶段遇到"让车沿着给定路径点跑起来"这个需求。这个课题看起来简单，实际把环境搭好、控制器调通、视频导出来，中间坑不少。我这篇文章就把整条链路从头到尾拆开讲清楚，从Carsim联合仿真环境搭建，到MPC控制器设计，再到路径点生成和视频输出，最后附上我调试过程中踩过的坑和源码框架，希望能帮你一次性把流程跑通。
我用的环境是Windows 10加MATLAB R2020a加Carsim 2019，这套组合相对稳定，很多实验室也用这个版本。不同版本的MATLAB或Carsim在接口配置上略有差异，但核心逻辑不变。
2. Carsim与Simulink联合仿真：先把车辆这个"被控对象"搞定
2.1 为什么要用Carsim而不是直接在Simulink里搭整车模型
很多初学者会纠结一个问题：Simulink里面不是也有车辆动力学模块吗，为什么非要用Carsim？我的回答是：如果你的核心任务是验证MPC控制算法，那就别把精力耗在搭建精确的整车模型上。Carsim的优势在于它提供的是一个经过大量实车数据验证的整车模型，包括悬架、轮胎、转向和制动的非线性特性，这些细节用Simulink基础库自己搭，工作量非常大，而且精度还不一定能保证。Carsim以S-Function的形式嵌入Simulink，接口是现成的，你只需要把注意力集中在控制器上。
当然，如果你只是做纯粹的算法验证，不在意外部展示效果，也可以用简化的自行车模型或三自由度模型先跑通逻辑。但最终呈现课题结果或者写论文时，Carsim的完整车辆模型要比简化模型更有说服力，这也是为什么大多数高校和竞赛作品都采用这个方案。
2.2 版本匹配与联合仿真接口配置
先说说最容易被忽略的版本兼容问题。Carsim对MATLAB版本是有要求的，不是随便配都能顺利编译。我实测下来，Carsim 2019配MATLAB R2020a、R2021a都很稳；Carsim 2016则更适合配MATLAB R2017b以下。如果你用新版的MATLAB R2023b去配老版本Carsim，很可能在S-Function编译阶段就挂掉，报错信息还特别不友好。
具体配置步骤是这样：
在Carsim主界面新建一个仿真工况，在Run Control页面把仿真模式设置为Simulink外部模式。
打开Simulink Library Browser，找到Carsim提供的模块库，核心是Carsim S-Function模块。
把Carsim S-Function拖入模型，双击后在参数框中指定Carsim工况文件（.simfile文件）的路径。这一步非常关键，路径里不要有中文和空格，否则后续编译会遇到各种奇怪问题。
在Carsim主界面点击"Send to Simulink"按钮，它会自动启动MATLAB并定位到当前工作目录，同时把车辆模型参数写入MATLAB工作空间。
在Simulink里打开Carsim自带的示例模型，在其中加入你的MPC控制器即可。
提示：如果仿真时提示找不到S-Function或MEX文件，先检查Carsim安装目录下的solve和interface文件夹里是否有预编译文件，并确认它们已经加入MATLAB路径。这个问题在换电脑或换版本时特别常见。
2.3 信号输入输出端口的定义
Carsim与Simulink之间的交互，本质上是输入输出端口的信号传递。Carsim S-Function模块左侧是输入端口，方向是从Simulink到Carsim，也就是你给车辆的控制命令；右侧是输出端口，方向是Carsim反馈给Simulink的车辆状态。
我常用的输入信号有三个：
方向盘转角，通常用Steer_L1或Steer_R1表示，单位是deg或rad。
节气门开度，经过归一化后是0到1。
制动主缸压力，单位是MPa。
输出信号方面，根据MPC的控制需求选择：
纵向速度Vx、横向速度Vy。
横摆角速度Yaw Rate。
质心位置X和Y。
横摆角Yaw Angle。
质心侧偏角Beta，做高速工况时需要。
这里要特别提醒单位问题。Carsim内部默认很多量是国际单位制，但方向盘转角往往以deg为单位输出，而MPC计算时通常用弧度，所以需要在Simulink模型里加一个Gain模块做换算，或者在Carsim的Output Variables配置里直接把单位改成弧度。这个细节如果忽略，控制器输出的角度会差57.3倍，整个系统直接就飞了。
另外一个容易忽略的点：Carsim输出的X和Y坐标是车辆在仿真场景中的全局坐标，如果你做的路径本身就是在Carsim场景中定义的，那坐标可以直接用；但如果你用自己的参考路径点，就涉及坐标系的统一，这个在后面路径处理部分会细说。
3. MPC控制器设计：核心逻辑与参数整定的门道
3.1 预测模型的选取：线性时变还是非线性
MPC的核心在于预测模型，模型选得好不好直接决定控制精度。对于路径跟踪控制，业内最常见的是基于车辆运动学模型或动力学模型做预测。我的经验是：如果主要做低速场景，比如车速在10m/s以内，基于运动学自行车模型就够了。低速时轮胎侧偏特性不显著，模型简单反而稳，计算量也小。如果场景涉及高速或者紧急避障，就必须引入动力学模型，考虑轮胎力的非线性。
我在这个项目里用的是低速运动学自行车模型。状态量选择车辆位置坐标X、Y，横摆角psi，纵向速度vx，以及前轮转角delta。控制量选择前轮转角增量delta_delta和纵向加速度a。
为什么把前轮转角作为状态量而不是直接作为控制量？因为如果在MPC里直接优化前轮转角，优化出来的解会有明显的阶跃突变，方向盘转角执行起来既不舒服也不现实。把前轮转角纳入状态量，优化它的变化率，本质上是对控制动作做平滑，这样车辆跑起来姿态更自然，也更能反映真实驾驶特性。
至于为什么不直接用完整的非线性模型做NMPC，原因很现实：NMPC的求解器搭建成本高，计算量大，在Simulink实时仿真中很难在每个采样周期内完成求解。相对而言，线性时变MPC（LTV-MPC）在每个控制周期对当前工作点做线性化，既保留了非线性模型的适应能力，又保持了二次规划求解的高效性，是这个课题性价比最高的选择。
3.2 MPC控制器在Simulink中的实现方式
在MATLAB里实现MPC，有两条路线：用Model Predictive Control Toolbox自带的模块，或者手写MPC求解算法。两条路我都试过，简单说说体会。
用工具箱省事，直接拖一个MPC Controller模块，设置好被控对象的线性模型和约束就行。但它的局限性很突出：自定义灵活性差。比如你想在预测模型里加入时变的车辆速度，或者在约束里加入前轮转角的非线性限制，工具箱配置起来会非常绕。
我最终选择的是手写MPC，用MATLAB Function模块实现。这样做的好处是每一行代码都知道在干什么，后续改模型也好、加约束也好，都非常直观。具体流程是这样的：
在每个控制周期，根据当前车辆状态和参考路径点，计算跟踪误差方程。
在当前工作点对运动学模型进行线性化，得到状态空间矩阵A、B，然后离散化得到Ad、Bd。
构造预测时域内所有状态量和控制量的预测表达式，写成标准二次规划形式。
调用MATLAB的quadprog求解器，或者用MPC工具箱里的mpcqpsolver实时求解器。
取求解结果的第一组控制增量作用于车辆模型，下一周期重复以上过程。
我实际采用的是LTV-MPC方法，采样时间Ts设为0.05秒，预测时域Np取20步，控制时域Nc取5步。这个组合在跟踪精度和计算效率上的平衡表现不错，下面第5节会给到完整的代码框架。
3.3 权重矩阵、约束与调参的实战经验
MPC调参是最花时间的环节，也是最考验经验的部分。我把常用的初始参数直接列出来，你在这个基础上调，能少走很多弯路。
目标函数通常包含三部分：
跟踪误差惩罚项，包括横向位置误差和航向角误差。权重项Q，横向位置误差系数我习惯给20到50，航向角误差系数给10到20，速度误差给5到10。
控制增量惩罚项，主要限制前轮转角变化率。权重R给1到5。R太大会让转向变迟钝，R太小方向会明显抖动。
松弛变量惩罚项，用于处理优化问题无解的情况。权重rho设一个较大的值，比如1000，保证软约束在绝大多数情况下能被满足。
约束条件的设计：
前轮转角范围，正负0.5rad，大约对应正负28度。
前轮转角增量范围，正负0.05rad每步，防止转向过快。
纵向加速度范围，正负3m/s²，兼顾加速和制动需求。
横向位置误差软约束可以设置在正负2m，超出时通过松弛变量消化。
调参的顺序建议这样：先固定R，从小到大调Q，观察横向误差是否收敛；然后固定Q，从大到小调R，找到转向平顺和跟踪精度的平衡点。如果仿真途中出现quadprog求解失败，退出标志小于0，最直接的解决办法是放宽软约束边界，或者增大松弛变量权重，而不是盲目缩小硬约束范围。
4. 路径点处理与参考轨迹生成
4.1 给定位姿点转连续路径的插值方法
路径跟踪的前提是先有一条参考轨迹。但这个课题的输入往往只是一组稀疏的路径点，比如一系列(x, y)坐标，甚至只有GPS采集的经纬度。要在MPC控制里使用，必须先把离散点转成连续可求导的参考轨迹，还要计算每个点对应的参考航向角。
我首选的方法是三次样条插值。用MATLAB的spline函数，对x和y分别以路径点的累计弧长为自变量做插值，得到位置连续的轨迹曲线，再通过数值求导得到航向角参考值。具体步骤：
计算相邻路径点之间的距离，累加得到每个点的弧长参数s。
用s作为自变量，x和y分别做三次样条插值，得到x(s)和y(s)的插值函数。
参考航向角psi_ref通过对x、y关于s的导数求atan2得到。
如果需要曲率信息做前馈控制，也可以继续求二阶导计算曲率。
这里有个小坑：spline插值在曲率变化较大的区域可能出现轻微过冲，导致生成的参考路径出现不必要的波动。如果你的路径点是从实测数据里来的，噪声本身比较大，建议先做平滑处理，或者改用pchip插值，虽然光滑性稍弱，但不会产生大幅震荡。
4.2 跟踪误差的求取：弗勒内坐标系法
很多初学者在计算跟踪误差时，习惯直接把车辆当前位置和最近的参考点做差，这个做法在直线路径上问题不大，但在弯道里会严重失真。原因很简单：投影到路径上的参考点应该是最接近点，而不是简单地取距离最近的路径采样点。
正确做法是在弗勒内坐标系下定义误差。基本思路是：以参考路径本身为坐标系，把车辆当前位置投影到路径上，投影点处的切向和法向构成局部坐标系。跟踪误差分解为：
横向误差e_y：车辆质心到参考路径投影点的垂向距离，这是最核心的控制指标。
航向角误差e_psi：车辆实际横摆角与投影点处参考航向角的差。
每次MPC求解前，都要先找到车辆当前位置在参考路径上最近的点，也就是计算投影点的弧长坐标s。这个过程如果遍历所有路径点，计算量不小，我在代码里用一个简单策略：根据当前车速和预测时域，先估算未来几秒内车辆会走到的弧长范围，只在这个局部范围内搜索最近点，这样能大幅加快仿真速度。
还有一个细节：航向角误差是周期性的角度差，要处理成-pi到pi之间，否则在车辆转过180度时误差计算会突然跳变，导致控制器误判。
4.3 路径点坐标系的统一
坐标系不统一是一个很隐蔽但后果严重的问题。Carsim输出的车辆坐标是基于Carsim场景的全局坐标系，而你的参考路径点可能来自高精地图或者自己定义的车身坐标系。如果两者不统一，控制器会完全失效。
我的做法是仿真开始前，在MATLAB脚本里把参考路径平移到Carsim的全局坐标系下，让路径起点对齐到Carsim场景中的期望位置。如果需要旋转，还要计算路径主方向与Carsim X轴的夹角，把所有路径点做相应的旋转变换。这个操作代码量不大，但漏掉它，你会看到车辆在仿真里一路狂奔，完全不在路径上，而且怎么调参数都没用。
5. 完整源码框架：从初始化到仿真结束
5.1 主脚本的结构设计
这个项目建议用主脚本加Simulink模型配合的方式，把所有参数初始化放在主脚本里，Simulink模型里只放控制逻辑和被控对象。这样做的好处是调参不用动模型，只改脚本就行。
主脚本的关键步骤：
清理环境，设置工作目录。
定义参考路径点，调用轨迹生成函数得到连续的参考轨迹。
初始化MPC参数：预测时域、控制时域、权重矩阵、约束边界、采样时间。
加载Carsim工况参数到MATLAB工作空间。
设置Simulink仿真参数，包括开始时间、结束时间、求解器类型、步长。
调用sim命令运行仿真。
提取仿真结果，绘制跟踪曲线，生成视频。
5.2 关键函数：轨迹生成模块
轨迹生成函数的核心代码结构如下，你复制后根据实际情况修改变量名即可：
function [x_ref, y_ref, psi_ref] = generateReferencePath(waypoints, step) % 输入：waypoints为Nx2的路径点矩阵，step为采样弧长 % 输出：插值后的参考轨迹点及其航向角 x = waypoints(:,1); y = waypoints(:,2); % 计算累计弧长 dx = diff(x); dy = diff(y); ds = sqrt(dx.^2 + dy.^2); s = [0; cumsum(ds)]; % 三次样条插值 s_fine = 0:step:s(end); x_interp = spline(s, x, s_fine); y_interp = spline(s, y, s_fine); % 数值求导得到航向角 dx_interp = gradient(x_interp, step); dy_interp = gradient(y_interp, step); psi_ref_interp = atan2(dy_interp, dx_interp); % 输出 x_ref = x_interp'; y_ref = y_interp'; psi_ref = psi_ref_interp'; end5.3 关键函数：MPC控制器核心
这是整个项目的核心模块。我在MATLAB Function模块里实现这个逻辑，输入为当前车辆状态、参考状态、MPC参数结构体，输出为前轮转角增量和纵向加速度。简化框架如下：
function [delta_delta, a] = mpc_controller(x, y, psi, vx, refState, param) % 当前状态向量 xk = [x; y; psi; vx]; % 参考状态 xr = refState(1); yr = refState(2); psir = refState(3); % 计算误差 e_y = -(x - xr)*sin(psir) + (y - yr)*cos(psir); e_psi = psi - psir; e_v = vx - param.v_ref; state_err = [e_y; e_psi; e_v]; % 构造线性化离散状态空间矩阵 [Ad, Bd] = linearizeModel(vx, param.Ts); % 构建预测矩阵 [A_bar, B_bar] = buildPredictionMatrix(Ad, Bd, param.Np, param.Nc); % 构建QP矩阵 Q = kron(eye(param.Np), param.Q); R = kron(eye(param.Nc), param.R); H = B_bar' * Q * B_bar + R; f = B_bar' * Q * A_bar * state_err; % 约束 lb = [-0.5; -0.05]; ub = [0.5; 0.05]; % 求解 options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set'); [delta_opt, ~, exitflag] = quadprog(H, f, [], [], [], [], lb, ub, [], options); if exitflag > 0 delta_delta = delta_opt(1); a = delta_opt(2); else delta_delta = 0; a = 0; end end注意这里的代码是逻辑框架，实际中H矩阵和f向量的构造需要严格按照预测模型的展开式来做。建议你先从只包含线性误差项的简化版跑通，再逐步加入控制增量惩罚项和软约束。
5.4 参数初始化脚本
我把所有参数集中放在initParams.m脚本里，后续调参只改这一个文件：
param.Ts = 0.05; % 采样周期，单位s param.Np = 20; % 预测时域 param.Nc = 5; % 控制时域 param.v_ref = 10; % 参考纵向速度，单位m/s param.Q = diag([30, 15, 5]); % 横向误差、航向角误差、速度误差权重 param.R = diag([2, 1]); % 控制增量权重 param.rho = 1000; % 松弛变量权重 param.delta_max = 0.5; % 前轮转角上限，rad param.delta_min = -0.5; % 前轮转角下限 param.ddelta_max = 0.05; % 前轮转角增量上限 param.a_max = 3; % 纵向加速度上限6. 联合仿真运行与视频生成
6.1 Simulink模型搭建的完整流程
模型搭建是整个流程中最容易卡住的环节，我说一下我的搭建顺序，你按这个顺序操作基本不会乱：
从Carsim库中拖入Carsim S-Function模块，作为被控对象。
从User-Defined Functions库中拖入MATLAB Function模块，写入MPC控制器代码。
添加Clock模块记录仿真时间，添加信号拆分模块从Carsim输出中提取X、Y、横摆角、纵向速度等信号。
添加参考状态生成模块。这个模块通常是一个MATLAB Function，根据当前车辆位置在参考轨迹上进行最近点搜索和插值，输出当前时刻的参考位置和参考航向角。
在Carsim输入端口前，把MPC输出的前轮转角增量累积成实际前轮转角。我习惯用单位延迟模块配合加法器实现delta_k+1 = delta_k + delta_delta_k。
添加To Workspace模块，把仿真过程中的车辆位置、参考位置、控制量等数据保存到工作空间，方便后处理。
求解器的选择也很关键。我推荐使用定步长求解器，步长设为0.01秒，这样Carsim的动力学模型在每个控制周期内都能稳定积分。如果使用变步长求解器，Carsim S-Function在某些情况下会出现步长跳变导致的不稳定。
6.2 仿真常见的数据流卡壳问题
我遇到过几次Simulink模型一切正常但一运行就报错的情况，排查后基本都是三个原因：
一是Carsim工况文件没有正确关联。检查S-Function模块里的simfile路径是否有效，最好用绝对路径。
二是输入输出端口数量或顺序不匹配。Carsim S-Function模块的端口顺序是在Carsim GUI的I/O Channels里定义的，Simulink这边必须与之一一对应，多一个少一个都会报错。
三是MATLAB工作空间里没有Carsim的初始化变量。这个问题在直接用sim命令运行时特别常见。解决办法是先手动运行Carsim的初始化脚本，或者点击Carsim主界面的Send to Simulink按钮，让MATlab工作空间获得所有需要的变量，然后再运行模型。
6.3 把仿真结果录制成视频
视频是最终成果展示的重要部分，这里有两种录制路线供你选择。
第一种是用Carsim自带的动画显示功能。Carsim自带三维动画查看器，仿真结束后可以直接查看车辆运动的动画效果，也可以直接导出视频。这是最方便的方式，因为场景、道路、车辆模型都是Carsim自带的，导出质量也不错。
第二种是用MATLAB绘图加VideoWriter录制。这种方式的优点是可以自由控制视角，并在画面里叠加数据曲线，比如跟踪误差曲线、方向盘转角曲线等，适合做汇报展示或论文配图。
我推荐两种结合：先用Carsim动画导出场景视频，再用MATLAB画出跟踪曲线，最后用剪辑工具拼合。如果不想借助剪辑工具，MATLAB里也可以直接用VideoWriter把轨迹和误差图放在同一画布上录制，核心代码如下：
v = VideoWriter('path_tracking.avi'); open(v); figure('Position', [100, 100, 1200, 600]); for k = 1:length(t_out) % 左侧子图：车辆轨迹与参考轨迹 subplot(1,2,1); plot(x_ref, y_ref, 'b--', 'LineWidth', 1.5); hold on; plot(x_out(1:k), y_out(1:k), 'r-', 'LineWidth', 1.5); plot(x_out(k), y_out(k), 'ro', 'MarkerSize', 6, 'MarkerFaceColor', 'r'); xlabel('X (m)'); ylabel('Y (m)'); legend('参考路径', '实际轨迹', '当前位置'); axis equal; grid on; hold off; % 右侧子图：横向误差 subplot(1,2,2); plot(t_out(1:k), e_y_out(1:k), 'k-', 'LineWidth', 1.5); xlabel('时间 (s)'); ylabel('横向误差 (m)'); grid on; drawnow; frame = getframe(gcf); writeVideo(v, frame); end close(v);视频帧率建议设在10到15之间，也就是每秒写入10到15帧。如果你的仿真时长为20秒，总数据点是400个，按每秒10帧录制，视频实际播放时长和仿真时间同步，观感最自然。
7. 我在调试过程中遇到的高频问题与解决方案
7.1 Carsim车辆飞出路径或原地打转
这是最经典的问题，现象是车辆开出去没几步就偏离路径，或者干脆原地打转。我排查这类问题时有一个固定的顺序，基本能定位到九成以上的根因。
首先查参考航向角计算是否正确。特别是路径闭环或回环时，样条插值后的航向角可能在连接点处发生跳跃，导致MPC的航向误差项出现假的大偏差。
其次查误差坐标变换。弗勒内坐标系的横向误差正负号、航向角误差的周期性归一化，这两点几乎是必踩的坑。航向角误差必须在-pi到pi之间，否则误差出现跳变时，控制器会给出反向的纠正信号，系统表现为正反馈发散。
最后查MPC预测模型里的符号约定。比如你定义横向误差向左为正，但模型里前轮转角正方向是向左转，如果符号不一致，控制器会给出反向补偿，结果同样是发散。
7.2 quadprog求解失败或实时性不足
当预测时域很大或约束较多时，quadprog的求解时间可能超过采样周期，导致仿真速度急剧下降甚至卡死。我的优化思路有三个，依次尝试就能解决问题。
第一，减小预测时域Np。不要把Np设得过大，20步在0.05秒采样下已经对应1秒的预测时间，足以覆盖大多数工况的需求。
第二，改用mpcqpsolver。这是MPC工具箱里专为实时控制优化的QP求解器，采用KWIK算法，求解速度比通用quadprog快一个数量级。代价是需要手动管理约束矩阵和上下界，代码复杂度会上去一些。
第三，简化MPC内部的预测模型。用运动学模型做预测，把复杂动力学计算交给Carsim去处理，这是MPC分层控制的典型做法，牺牲少量预测精度换取实时性，实际效果完全够用。
7.3 仿真结果输出为NaN
输出NaN通常说明模型数值发散，或者某个模块输入出现了无穷大。最常见的原因是Carsim的输入端口收到了不合理控制量，比如前轮转角超过90度或加速度超过物理极限。
我的习惯是在MPC输出端再加一个饱和模块，即使MPC内部约束失效，输出端的硬限制也能兜底。另外，如果用了Lookup Table或者在MATLAB Function里做了插值操作，要确保输入变量在定义域内，越界会导致NaN。我在Reference Generator里做最近点搜索时，就因为路径点索引越界而得到过NaN，排查很久才发现是边界条件没有单独处理。
8. 从这套流程延伸出去的一些思考
整套流程跑通之后，你其实已经掌握了一个完整的车辆路径跟踪控制研究框架。可以做的扩展方向非常多：把低速运动学模型替换成高速动力学模型，控制量从转向和加速扩展到四轮独立驱动；把MPC与上层路径规划结合，从跟踪固定路径升级为动态避障；甚至把Carsim替换成硬件在环平台，把同样的控制器部署到嵌入式设备上。
如果你只是想快速完成一个课程项目，按照这篇文章的流程走下来，一到两周内拿到完整可运行的代码和视频是可行的。但如果你想深入研究，我建议花时间打磨两个地方。
第一个是MPC目标函数的设计。在现有基础上加入轮胎侧偏角软约束、侧向加速度约束，控制效果还能再上一个台阶。第二个是求解器的优化，尝试写基于OSQP或内点法的自定义求解器，这样才能真正理解MPC的底层实现，为后续做实时部署积累底气。
这个课题最大的价值我个人觉得不是复现出来有多炫，而是在整个系统联调过程中训练出来的系统思维：车辆模型、控制器设计、数值求解、可视化验证，任何一环掉链子整体就动不了。这种能力不管是后续做自动驾驶还是机器人控制，都是通用的硬技能。希望这篇文章能帮你把所有环节串起来，少走一点弯路。
本文还有配套的精品资源，点击获取
