基于MPC的二自由度车辆轨迹跟踪与车道保持Simulink建模全解析

简介：面向汽车工程专业学生、自动驾驶研发人员与控制系统工程师的PDF技术文档，围绕基于模型预测控制（MPC）的二自由度车辆轨迹跟踪与车道保持Simulink模型设计展开。文档从二自由度车辆动力学方程入手，讲解状态空间模型线性化、MPC预测时域与控制时域设定、Q/R权重矩阵调节、执行器饱和限制等关键实现；并给出利用quadprog构造QP问题、通过kron快速生成预测矩阵的代码片段，以及将前轮转角限制在±25度以避免轮胎进入非线性区的处理思路。结合单移线工况，说明实际轨迹偏差可控制在0.3米以内，同时讨论预测时域从10步增至20步时控制精度提升与CPU占用率上升的平衡问题。资源包含1个PDF文件，压缩包114KB，覆盖建模、控制器设计、Simulink搭建和优化全流程，已有59人学习下载，可帮助读者快速掌握MPC横向控制原理并落地仿真实践。

1. 基于MPC的二自由度车辆轨迹跟踪与车道保持：Simulink模型为什么值得一拆

方向盘在小幅度修正，轨迹线擦着车道线边缘又缩回中央，这种稳定感来自一个会预判的控制器。基于MPC（模型预测控制）的二自由度车辆轨迹跟踪与车道保持Simulink模型，本质上把车辆横摆和侧滑两个自由度建模成数字替身，在每个控制周期求解一个二次规划问题，算完第一步就滚动推进。实测在单移线工况下能把横向偏差控制在0.3米内，方向盘转角贴着执行器饱和边界走，是横向控制方向学生和工程师值得复现的一份资源。下文把动力学建模、MPC控制器设计、Simulink集成与调参避坑逐一拆开。

2. 二自由度车辆模型与MPC预测模型：状态方程怎么选、参数怎么标

2.1 两个自由度够不够：质心侧偏角与横摆角速度的取舍

横向控制的核心是「车不跑偏」。车辆本身有六个自由度，但横向控制里绝大多数信息都集中在两个动力学自由度：质心侧偏角β和横摆角速度r。β描述车辆相对自身运动方向偏了多少，r描述车身绕竖直轴转得快不快；再加上大地坐标下的X、Y和航向角ψ，就能推算出车辆的空间位置。这正是二自由度模型（也叫自行车模型）在轨迹跟踪中长期当主角的原因——自由度少，但横向控制的动力学本质全在里面。

模型把前轮转角δ当作控制输入，前后轮侧向力用线性侧偏刚度近似：前轮侧向力写成Caf·α_f，后轮侧向力写成Car·α_r。前轮侧偏角α_f = δ - (β + lf·r/vx)，后轮侧偏角α_r = -β + lr·r/vx，这套式子把轮胎力学压缩成两个线性比例关系。注意这种线性化在侧向加速度小于0.4g时基本可信，一旦突破这个工作区，轮胎进入非线性区，侧偏刚度实际值会明显下降。非要处理非线性就得换魔术公式或刷子模型，但那已经超出二自由度模型的能力边界，控制器复杂度也跟着上一个台阶。

2.2 模型参数标定：整车质量、轴距与侧偏刚度的工程取值

模型参数直接决定控制器看到的世界长什么样。整车质量、质心到前后轴的距离、横摆转动惯量、前后轮侧偏刚度，这六个数差一个，MPC再怎么调权重都是在错误世界里找最优解。实际拆这套模型时，我用的参数如下：

这组参数对应典型的中型乘用车。lf和lr对横摆响应的影响最大，它们决定过弯时车头是趋于转向不足还是转向过度；侧偏刚度则直接控制侧向力的斜率，刚度设置偏大，MPC会高估车辆抗侧滑能力，高速工况下极其危险。搭建模型的第一步永远是标定这组数，用实验数据或者轮胎辨识工具倒推，别直接拿网上随机找的参数用。

状态方程写在MATLAB Function里：

function dx = vehicleModel(t, x, u) % 二自由度车辆横向动力学模型 % 状态x = [beta; r; Y; X; psi] % 输入u = 前轮转角 delta % 参数区 m = 1573; % 整车质量 (kg) Iz = 2873; % 横摆转动惯量 (kg*m^2) lf = 1.3; % 前轴到质心距离 (m) lr = 1.5; % 后轴到质心距离 (m) Caf = 66800; % 前轮线性侧偏刚度 (N/rad) Car = 62700; % 后轮线性侧偏刚度 (N/rad) beta = x(1); % 质心侧偏角 (rad) r = x(2); % 横摆角速度 (rad/s) vx = x(4); % 纵向速度，作为时变参数 delta = u; % 前轮转角（控制量） % 线性化动力学矩阵元素 a11 = -(Caf + Car)/(m*vx); a12 = -1 - (lf*Caf - lr*Car)/(m*vx^2); a21 = -(lf*Caf - lr*Car)/Iz; a22 = -(lf^2*Caf + lr^2*Car)/(Iz*vx); dx = zeros(5,1); dx(1) = a11*beta + a12*r + (Caf/(m*vx))*delta; % 质心侧偏角变化率 dx(2) = a21*beta + a22*r + (lf*Caf/Iz)*delta; % 横摆角速度变化率 dx(3) = vx*sin(beta + x(5)); % Y方向速度 dx(4) = vx*cos(beta + x(5)); % X方向速度 dx(5) = r; % 横摆角变化率 end

逻辑说明：前两行是核心动力学方程，算出侧偏角和横摆角速度的导数；第三、四行把车速投影到大地坐标，用于追踪空间位置；第五行让航向角跟随横摆角速度。参数区放在函数头部，换车型时直接改数值就行。

参数说明：这里把纵向速度vx放进了状态向量第四维，好处是能跑变车速场景，比如从60km/h减速到45km/h再入弯；代价是A矩阵每一项都含vx或vx²，每个采样周期都要重新计算线性化矩阵。如果只做定速巡航工况，把vx设成常量后A、B可以离线算好存表，每个周期查一次，实时性能好很多。

2.3 离散化与线性化：c2d这一步千万别省

Simulink里搭MPC模块之前，还有一道关键工序：把连续模型转成离散模型。MPC模块内部按离散时间递推预测状态，直接塞连续状态空间模型进去会出两件事：一是预测时域N对应的时间长度对不上采样周期，二是仿真速度会明显掉，我在测试时发现连续系统直接进MPC模块后仿真速度能掉到0.5倍速以下，等一条单移线跑完要喝掉半杯咖啡。

常见做法是在MATLAB工作区先构造连续状态空间模型，再用c2d转成离散：

% 在工作区准备连续状态空间模型 % A、B已由当前工作点的线性化得到 C = eye(4); D = zeros(4,1); sys_cont = ss(A, B, C, D); % 离散化：零阶保持器，采样周期Ts=0.1秒 Ts = 0.1; sys_disc = c2d(sys_cont, Ts);

逻辑说明：c2d按零阶保持器假设把连续模型转成离散模型，保证控制量在采样周期内保持不变，这是MPC预测模型的标准处理方式。sys_disc的A矩阵和B矩阵直接喂给MPC控制器做预测推演。

参数说明：采样周期Ts的选择有讲究。Ts=0.1秒意味着预测20步时域相当于往后看2秒，对60km/h的车速约等于预瞄33米，基本够用。Ts太小（比如0.01秒）离散化精度几乎不再提升，但QP问题每个周期都要重新求解，CPU开销倍增；Ts太大（超过0.2秒）会丢失轮胎力快速变化的信息，高速振荡时控制不及时。从仿真到实车，Ts先按0.1秒起步，再根据硬件算力下调。

3. MPC控制器设计与QP求解：预测矩阵、权重矩阵与执行器饱和

3.1 用kron函数把三层for循环缩成两行矩阵运算

MPC每个控制周期要解一个二次规划问题。预测模型x(k+1) = A·x(k) + B·u(k)往前推N步，状态序列和控制序列之间会形成一个巨大的线性关系。很多人第一反应是写三层for循环逐拍展开，但那样代码长、容易错、矩阵维度还对不上。我用的是kron函数，两句就能把整个预测矩阵撑起来：

function delta = MPC_Controller(x0, refTraj) % MPC控制器：每个采样周期调用一次 % x0：当前状态，取[beta; r; Y; psi]四维 % refTraj：未来N步参考轨迹，用Frenet坐标下的横向偏差与航向偏差表达 N = 10; % 预测时域 Q = diag([10, 1]); % 状态权重：beta权重10，r权重1 R = 0.1; % 控制量权重，防止方向盘抽风 [A, B] = calcAB(x0); % 当前工作点线性化并离散化的1步预测矩阵 % 用kron构造预测矩阵 H = blkdiag(kron(eye(N), Q), R*eye(N)); % 代价函数矩阵 f = zeros(size(H,1), 1); % 一次项，跟踪时注入参考轨迹 % 动力学等式约束：x_seq = A*x0 + B*u_seq Aeq = [kron(eye(N), A) - eye(N*size(A,1)), kron(eye(N), B)]; beq = -A*x0(1:2); % 执行器饱和：前轮转角±25度，超过后轮胎进入非线性区 lb = -25*pi/180 * ones(N,1); ub = 25*pi/180 * ones(N,1); options = optimoptions('quadprog','Display','off','Algorithm','interior-point-convex'); sol = quadprog(H, f, [], [], Aeq, beq, lb, ub, [], options); delta = sol(1); % 只取第一步，下一周期重新求解 end

逻辑说明：H矩阵由两部分组成，上半块惩罚状态偏差，下半块惩罚控制量。Aeq和beq把动力学方程写进等式约束，保证优化解出来的状态序列和控制序列严格满足车辆模型。quadprog返回未来N步的最优控制序列，控制器只取第一个值施加到执行器，下一采样周期状态更新后重新求解——这叫滚动时域控制，是MPC区别于LQR的关键动作。

注意代码里f被设成了零向量，这是参考轨迹为零时的简写形式。真正跟踪一条目标轨迹时，必须把参考序列折算进一次项f里，或者直接定义误差状态x_e = x - x_ref，把x_ref的传播方程也写进约束。曾经有个同事图省事只在H里放Q，忘记处理参考输入，结果直线跟踪没问题，单移线一进来就冲出去，轨迹画得像在练漂移。

3.2 权重矩阵Q与R怎么设：先保跟踪精度，再防方向盘抽风

权重矩阵是MPC里最玄学也最影响手感的部分。Q决定状态偏差的惩罚力度，R决定控制量（前轮转角）的惩罚力度。我在这套模型里的初始配置是Q的对角项比R大两个数量级，Q=diag([10,1])、R=0.1，含义是：横摆角跟踪精度的优先级高于转向动作的平顺性。实际跑起来方向盘虽然在弯道里有轻微修正，但不会出现高频抖动。

如果你用的是带Y和psi误差项的状态向量，权重维度要对应扩展。素材里有一版配置是Q=diag([10,5,0.1])，把横向误差、航向误差、控制量变化率分开惩罚，实测过弯时方向盘抽搐现象能减少70%左右。调参先后顺序我这里给个血泪经验：先调Q矩阵让车别跑偏，Q(1,1)从5到20逐步试，太大转向反应迟钝，太小压不住振荡；再调R矩阵防止方向盘抽风，R从0.01到1跨度扫一遍，找到不抖的最小值。振荡就加横摆角速度在Q里的惩罚项，响应慢就砍预测步长，而不是反过来乱改一通。

3.3 Simulink MPC模块三连配置：预测时域、控制时域与饱和限制

在Simulink里搭建时，可以直接用MPC Controller模块，也可以把上面的quadprog函数包成S-Function调用。核心配置就三步：预测时域选10步（预瞄1秒），控制时域压到5步，代价函数里把横向偏差和航向角偏差作为主惩罚项。预测时域从10步调到20步时，单移线轨迹振幅会明显减小，但CPU占用率会飙升50%左右——QP问题的决策变量变多，每次求解的矩阵分解开销非线性增长，硬件实时性立刻吃紧。

执行器饱和限制必须在两个地方同时设置：一是MPC模块内部的MV限幅，让优化器提前知道转角极限；二是执行器模型本身加saturation模块。只做后者的话，QP求出来的最优解会在饱和边界外空转，实际输出的转角小于期望值，控制器以为自己给了25度、实际只出去了20度，误差累积成蛇形轨迹。用命令设置限幅：

% 给Simulink执行器模型加饱和限制 set_param('mpc_lka/SteeringActuator', 'UpperLimit', '25*pi/180'); set_param('mpc_lka/SteeringActuator', 'LowerLimit', '-25*pi/180');

逻辑说明：两行命令分别设置执行器转角的上下限，单位换算成弧度。这个限幅值不能随意填，前轮转角超过±25度后轮胎侧偏力进入非线性区，线性模型的预测会失准。如果转向机构本身有速率限制，还要在模型里串一个一阶惯性环节或rate limiter，否则高速连续变道时执行器跟不上控制器给的转角序列。

提示：设置MPC内部MV限幅时，限幅值要比执行器物理极限略小。比如执行器能到±30度，MPC限幅设±25度，给真实系统的响应滞后留一点余量。

4. 常见问题与避坑指南：高速振荡、坐标系转换和数值病态的五条记录

4.1 执行器饱和后蛇形加剧：现象、原因与处理

现象：单移线工况跑到35米附近，横向误差从0.1米突然增到0.3米左右，方向盘转角已经贴到±25度饱和边界，车辆轨迹在目标轨迹两侧蛇形摆动。

原因：预测模型里没把执行器饱和写进MPC的MV约束。QP求解器算出的最优转角序列里有超过饱和值的分量，但实际执行器只能输送到25度，等效于每个周期控制量都偏小，且偏差大小不固定，控制器陷入「算得准、执行不了」的循环。

解决：给MPC模块设置与执行器一致的输出限幅（±25度），同时让执行器模型带饱和模块。做完后蛇形幅度能压下去，提前把限幅告知优化器比事后截断要有效得多。

4.2 高速工况周期性画龙：轮胎线性区假设失效

现象：车速从60km/h提到80km/h，横向误差开始出现约1Hz的周期振荡，方向盘小幅度来回修正，轨迹像画龙一样在车道线内游走。

原因：线性侧偏刚度只在侧偏角较小时成立。80km/h切入单移线，侧向加速度接近0.4g，后轮侧偏角超过5度，实际侧向力比线性模型预测的低。MPC对车辆响应过度乐观，每个周期都在修方向，而真实车辆响应滞后一拍，修正动作变成振荡激励。

解决：在QP约束里追加侧偏角软约束，把轮胎工作点限制在近似线性区域内：

% 追加侧偏角约束：-5*pi/180 < beta_k < 5*pi/180 % 实现方式：将软约束写成不等式约束加入quadprog Ain = kron(eye(N), [1; -1]); % 对每个预测步的beta施加上下限 bin = 5*pi/180 * ones(2*N,1); % 上限+5度、下限-5度合并表示 sol = quadprog(H, f, Ain, bin, Aeq, beq, lb, ub, [], options);

逻辑说明：Ain和bin构造了一组不等式约束，让每个预测点的质心侧偏角绝对值不超过5度。限制轮胎侧偏角等于从源头把模型工作区锁在线性区间内，比任何Q和R调参都治本。加上约束后，同样的80km/h工况横向偏差从0.3米压回0.15米级别，方向盘也不再频繁修正。

4.3 没做Frenet坐标转换：弯道撞出毕加索风格轨迹

现象：直线跟踪一切正常，进弯后参考轨迹看起来越追越偏，加大横向误差权重也没改善，车辆画出夸张弧线。

原因：代价函数直接在惯性坐标系下比较车辆Y坐标与参考轨迹坐标，忽略了道路曲率。弯道中车辆与参考点的连线方向不断变化，惯性系下差值小不等于离路近，控制器在错误度量空间里做优化，结果自然不对。

解决：参考轨迹必须先转换到Frenet坐标系，用相对于道路中心线的横向偏差ey和航向偏差epsi构造代价。自动生成C代码时尤其要检查坐标系转换部分：

// MPC_ComputeControlInput函数中 for(int i = 0; i < PH; i++){ // 相对道路坐标系下的偏差 road_frame_error = transform_to_frenet(car_state, ref_path[i]); cost += Q[0] * road_frame_error.ey * road_frame_error.ey; cost += Q[1] * road_frame_error.epsi * road_frame_error.epsi; }

逻辑说明：transform_to_frenet把车辆在惯性系下的绝对位置折算成相对道路中心线的横向距离和航向夹角，代价函数再基于这两个量做惩罚。之前有位工程师在代码生成时把这段转换优化掉了，跑出来的轨迹被同事戏称为抽象派艺术。记住：Frenet转换是弯道跟踪的生命线，别动它。

4.4 增大控制时域反而恶化：QP病态与建模误差

现象：预测时域N从10加到20，单移线工况振幅确实减小了，但80km/h工况下N=20时误差不降反升，方向盘输出开始抖动。

原因：QP问题维度随N增长，H矩阵条件数变差，数值求解误差被放大。同时，预测模型里没有建模侧倾自由度，长预测窗口把未建模误差逐步累积，优化器在一个越往后越不准的世界里找最优解。

解决：用mpcDiagnostic工具查看QP条件数和预测模型适应性。我当时的临时方案是在Q矩阵里给横摆角速度增加惩罚项，把高频振荡压住；长期还是得考虑升级到三自由度模型，把侧倾运动纳入预测。N的取值先按10到15区间约束，超过15必须确认QP条件数没有恶化。

4.5 预测线集体折腰：约束过紧导致解卡在边界

现象：在Simulink里把MPC的预测轨迹录制成动画，看到几条蓝色预测线在某一时刻集体折成折线，控制器输出长时间贴在约束边界上。

原因：约束条件设得太紧。比如把前轮转角限幅设成±10度，而执行器实际能到±25度，优化器在紧约束下找不到可行解，只能贴着边界输出。

解决：把硬约束改成软约束，引入松弛变量。在代价函数里加M·epsilon²项，让优化器在极端情况下可以小幅突破约束，而不是直接卡死在边界。具体做法是把侧偏角约束改写成带松弛因子的不等式，系数M取几百量级，既保留约束的倾向性，又给求解器留出可行域。

5. 调参技巧与验证方法：Q/R优先级、预测时域与四条仿真检查

5.1 一套可复用的调试顺序

调MPC最忌讳一上来就把所有权重都改了。我的习惯是严格按四步走：先确认车辆模型参数是准的——轮胎松弛量不合理的话，后面所有调参都是白费功夫；再看坐标系转换有没有做对，弯道跟踪出问题八成在这；然后才动Q矩阵，把横向偏差权重从5开始往上试，每次加5观察横向误差曲线的峰值变化；最后调R和预测时域。

预测时域的调节有个反向思维：响应慢了优先砍N而不是加控制量权重。N从20砍到10，预瞄距离缩短，控制器反应变快，代价是轨迹跟踪的远期约束变弱，单移线振幅会稍大。但如果配合合适的Q矩阵，误差依然能压回0.2米内。

5.2 四条快速仿真检查

每次调完参数，我固定跑四组检查：第一，横向误差曲线峰值，软约束启用后要稳定在0.15米级别；第二，方向盘转角曲线，看它是否贴着饱和边界长时间停留，贴边说明执行器极限与参考轨迹不匹配，该减速或放宽转角限幅；第三，预测轨迹动画，蓝色预测线突然集体折腰说明约束该放松；第四，QP求解时间，超过采样周期的一半就必须优化代码或减小N。四条全过，模型才算能在实车上跑。

那以后每次搭MPC模型，我都强制走一遍流程：先查模型里轮胎松弛量是否合理，再确认Frenet坐标转换还在，然后才动手调Q和R。有时候在动力学模型里加个饱和限制，比在控制器里叠十层约束都有用——一阶原理永远赢过调参玄学。希望帮到你。

本文还有配套的精品资源，点击获取