MATLAB+Carsim+Simulink实现车辆路径跟踪MPC控制全流程

简介自动驾驶车辆路径跟踪控制是智能车辆研究中的核心问题其本质是如何让车辆精准且稳定地跟随预设轨迹。模型预测控制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实时仿真中很难在每个采样周期内完成求解。相对而言线性时变MPCLTV-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_k1 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的底层实现为后续做实时部署积累底气。这个课题最大的价值我个人觉得不是复现出来有多炫而是在整个系统联调过程中训练出来的系统思维车辆模型、控制器设计、数值求解、可视化验证任何一环掉链子整体就动不了。这种能力不管是后续做自动驾驶还是机器人控制都是通用的硬技能。希望这篇文章能帮你把所有环节串起来少走一点弯路。本文还有配套的精品资源点击获取