基于MPC的二自由度车辆轨迹跟踪与车道保持Simulink方案

简介：面向汽车工程专业学生、自动驾驶研发人员与控制工程师的MPC横向控制实践资料，聚焦二自由度车辆动力学建模、轨迹跟踪与车道保持的Simulink实现，帮助读者理解预测时域、控制时域与权重矩阵等关键参数的整定逻辑，并掌握执行器饱和限制下的控制可行性验证方法。压缩包共1个PDF文件，约114KB，内容围绕状态方程线性化、MPC控制器搭建、单移线工况仿真与调参经验展开，兼顾理论推导与工程落地。已有62人学习，适合希望从零搭建横向控制仿真链路、对照复现并排查常见问题的读者参考，也可作为课程设计或算法预研的入门材料。

1. 从一次方向盘抖动说起：这套 MPC 车道保持方案到底解决什么问题

方向盘在 80 km/h 时传来一阵细微的周期性抖动，仪表上的车道线图标开始闪烁——这是横向控制里最典型的“画龙”现场。很多同学做车辆轨迹跟踪的毕设或课设，第一反应是上 PID：横向误差进比例项，航向误差进微分项，调一调也能跑。但 PID 是事后补偿，它不知道车辆下一秒会往哪偏，更没法把“前轮转角不超过 ±25°”“质心侧偏角别超过 5°”这类物理约束写进控制律里。这套基于模型预测控制（MPC）的二自由度车辆轨迹跟踪与车道保持 Simulink 方案，要解决的就是这件事：用可预测的车辆动力学模型，在每个控制周期滚动求解一个带约束的二次规划，提前把方向盘“拽”回车道中心。它适合正在做自动驾驶横向控制方向毕设/课设的学生，也适合需要一份可交付横向控制原型的从业者——下面从需求、架构、实现到验收，按一份完整方案的节奏拆开讲。

2. 需求与总体架构：二自由度模型加 MPC 的四模块划分

2.1 先把控制目标量化成可验收的指标

做横向控制方案，最怕一上来就写代码，最后答辩时被问“你这控制器到底算不算合格”答不上来。我一般会先把需求落成三组可测指标，后面所有设计都围着它们转。

第一组是跟踪精度：单移线工况下横向误差峰值不超过 0.3 m，稳态车道保持误差不超过 0.15 m，航向角误差峰值控制在 2° 以内。第二组是执行器友好度：前轮转角幅值限制在 ±25°，转角变化率不超过 10°/0.1 s，避免方向盘“抽风”。第三组是实时性：单个控制周期（取 Ts = 0.1 s）内 QP 求解耗时不超过 20 ms，保证在普通工控机或快速原型上能跑起来。

这三组指标不是拍脑袋，而是从项目正文里“偏差不超过 0.3 米”“方向盘转角超过 ±25 度进入非线性区”“预测时域从 10 调到 20 步 CPU 占用率飙升 50%”这些实测结论反推出来的。把指标写进方案第一章，后面调参才有靶子，答辩时也有话可说。

2.2 四模块架构：被控对象、预测模型、控制器、执行器

整套 Simulink 模型按职责切成四块，信号流向是单向的，方便单独替换和测试：

这里有个设计取舍值得说清楚：为什么用二自由度而不是七自由度？因为横向控制关心的是质心侧偏角和横摆角速度这两个主导量，二自由度模型参数少（质量、轴距、前后侧偏刚度、转动惯量），线性化后直接能导出状态空间，给 MPC 当预测模型最合适。七自由度适合做垂向和轮胎联合仿真，但塞进 MPC 里会让 QP 规模爆炸，实时性直接翻车。通常的做法是：控制用二自由度，验证阶段再拿 CarSim 之类的高保真模型做对比，这套方案里先用二自由度把闭环跑通。

2.3 坐标系约定：惯性系与道路坐标系的转换必须前置

项目正文里反复强调“坐标系转换要是没做，车子在弯道突然发疯”，这不是夸张。MPC 的代价函数惩罚的是相对道路的横向误差 ey 和航向误差 epsi，而车辆模型输出的是惯性系下的 Y 和 psi。两者之间差一个参考轨迹的切向角，必须显式转换：

% 惯性系状态 -> 道路坐标系误差（Frenet 转换简化版） function [ey, epsi] = toRoadFrame(Y, psi, refY, refPsi) % refY/refPsi 为参考轨迹在当前弧长处的横向位置与切向角 dy = Y - refY; ey = dy * cos(refPsi); % 横向偏差投影到道路法向 epsi = wrapToPi(psi - refPsi); % 航向偏差，归一化到 [-pi, pi] end

wrapToPi这步别省，航向角跨过 ±180° 时如果不归一化，代价函数会突然跳变，QP 解出来的转角会瞬间打满。这个函数放在参考轨迹模块和 MPC 之间，属于“看不见但缺了就出事”的胶水层。

3. 车辆动力学与预测模型：从两行方程到离散状态空间

3.1 二自由度模型的方程与参数含义

二自由度车辆模型的核心就两个方程：横向力平衡和横摆力矩平衡。项目正文给了两版写法，一版用 beta 和 r 作状态，一版展开成含纵向速度 vx 的形式。我倾向用后者，因为变车速场景更贴近真实工况：

function dx = vehicleModel(t, x, u) % 状态: x = [beta; r; Y; psi; vx] 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); r = x(2); psi = x(4); vx = x(5); 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 = [a11*beta + a12*r + Caf/(m*vx)*delta; a21*beta + a22*r + lf*Caf/Iz*delta; vx*sin(psi + beta); % 惯性系 Y 方向速度 r; % 横摆角 0]; % 本方案暂定匀速 end

参数区这几个数不是随便填的：Caf和Car差 4000 左右，体现前后轴载荷转移；lf比lr短，说明质心偏前，这是前驱轿车的典型布置。换车型时只改这六个参数，模型“性格”就变了——正文里提到把 1.5 吨小车改成 5 吨卡车横向误差飙到 1 米开外，本质是质量和惯量变了但侧偏刚度没跟着匹配，轮胎线性区假设提前失效。

3.2 线性化与离散化：c2d 这一步不能省

MPC 需要线性预测模型，所以在每个工作点（当前 vx）对上面的非线性方程做雅可比线性化，得到连续状态空间 A、B、C、D，再用c2d离散：

Ts = 0.1; % 控制周期 0.1 s A = [0, 1, 0, 0; 0, -(Caf+Car)/(m*vx), (Caf+Car)/m, -(lf*Caf-lr*Car)/(m*vx); 0, 0, 0, 1; 0, -(lf*Caf-lr*Car)/(Iz*vx), (lf*Caf-lr*Car)/Iz, -(lf^2*Caf+lr^2*Car)/(Iz*vx)]; B = [0; Caf/m; 0; lf*Caf/Iz]; C = eye(4); D = zeros(4,1); sys_d = c2d(ss(A,B,C,D), Ts, 'zoh'); % 零阶保持器离散

'zoh'零阶保持器对应实际执行器在一个周期内保持转角不变，比'tustin'更贴近真实。正文里说“连续系统直接怼进 MPC 仿真速度掉到 0.5 倍速”，原因就是连续模型在变步长求解器下反复积分，离散化后 QP 的预测矩阵是常数，求解快一个量级。离散后的 A_d、B_d 直接喂给预测矩阵构造。

3.3 预测矩阵构造：kron 快速拼装与权重设计

预测时域 N 步内，状态递推可以写成 X = Phix0 + ThetaU，其中 Phi 和 Theta 用kron拼：

N = 10; % 预测时域 [Ad, Bd] = deal(sys_d.A, sys_d.B); Phi = zeros(size(Ad,1)*N, size(Ad,1)); Theta = zeros(size(Ad,1)*N, size(Bd,2)*N); for i = 1:N Phi((i-1)*4+1:i*4, :) = Ad^i; for j = 1:i Theta((i-1)*4+1:i*4, (j-1)*1+1:j*1) = Ad^(i-j)*Bd; end end Q = diag([10, 5, 10, 5]); % 状态权重: beta, r, Y, psi R = 0.1; % 控制量权重

Q 的对角项对应四个状态的惩罚力度。横向位置 Y 和航向 psi 给 10 和 5，侧偏角 beta 给 10 是为了压住车身姿态，横摆角速度 r 给 5 是防止过度甩尾。R = 0.1 相对 Q 小一个量级，意思是“宁可多打方向也要跟住线”，但也不能太小，否则方向盘高频抖动。正文里提到“Q(1,1) 太大导致转向延迟，太小压不住振荡，建议从 [5,20] 区间试”，这个经验值可以直接拿来当调参起点。

4. Simulink 落地：控制器搭建、约束注入与代码生成

4.1 MPC 模块的 Simulink 实现路径

Simulink 里搭 MPC 有两条路：用 Model Predictive Control Toolbox 的现成模块，或者手写 MATLAB Function 块调quadprog。毕设阶段我建议先手写，因为每一步都看得见，答辩时能讲清楚 QP 怎么来的；交付阶段再换成工具箱或生成 C 代码。

手写版的核心是一个 MATLAB Function 块，输入 x0 和参考轨迹，输出 delta：

function delta = MPC_Controller(x0, refY, refPsi) % 预测矩阵 Phi/Theta 与权重 Q/R 在 InitFcn 里预计算，此处省略 N = 10; % 参考轨迹在预测时域内展开 Yref = refY * ones(N,1); Psiref = refPsi * ones(N,1); % 代价: (X - Xref)'*Qbar*(X - Xref) + U'*Rbar*U H = Theta' * Qbar * Theta + Rbar; f = Theta' * Qbar * (Phi*x0 - Xref); % 约束: 前轮转角 ±25° lb = -25*pi/180 * ones(N,1); ub = 25*pi/180 * ones(N,1); options = optimoptions('quadprog','Display','off','Algorithm','interior-point-convex'); U = quadprog(H, f, [], [], [], [], lb, ub, [], options); delta = U(1); % 只取第一个控制量，滚动优化 end

H和f是标准 QP 形式，quadprog的interior-point-convex算法对中小规模问题稳定。注意delta = U(1)这行——MPC 的精髓是只执行第一步，下一周期重新用新状态 x0 再解一次，这就是“滚动时域”。如果直接把整段 U 都执行，就退化成开环最优控制，模型失配时立刻发散。

4.2 执行器饱和与速率限制：把物理约束写进模型

正文里用set_param给 SteeringActuator 设上下限，这个思路对，但更稳妥的做法是在 Simulink 里显式放 Saturation 和 Rate Limiter 两个块串联：

% 执行器参数（可写成脚本批量配置） maxAngle = 25*pi/180; % 最大前轮转角 maxRate = 10*pi/180/0.1; % 最大转角速率 10°/0.1s set_param('mpc_lka/SteeringActuator/Saturation', 'UpperLimit', num2str(maxAngle)); set_param('mpc_lka/SteeringActuator/Saturation', 'LowerLimit', num2str(-maxAngle)); set_param('mpc_lka/SteeringActuator/RateLimiter', 'RisingSlewLimit', num2str(maxRate)); set_param('mpc_lka/SteeringActuator/RateLimiter', 'FallingSlewLimit', num2str(-maxRate));

Saturation 管幅值，Rate Limiter 管变化率，两者串联才逼近真实 EPS。只加 Saturation 的话，QP 解出来的转角序列可能在相邻周期跳变，仿真里看着没事，上实车就是方向盘“打手”。正文提到“35 米处误差突增，前轮转角触达 ±25° 限制”，本质是执行器饱和后控制器还在按未饱和的预测算，形成 windup，解决办法要么提前减速，要么把饱和后的实际转角反馈回控制器——后者就是 anti-windup 的思路。

4.3 软约束与松弛因子：QP 无解时的后悔药

硬约束在极端工况下会让 QP 无可行解，quadprog返回空，控制器直接“死机”。工程上的标准做法是加松弛因子，把状态约束软化：

% 在 QP 里追加松弛变量 epsilon，惩罚其范数 rho = 1e4; % 松弛惩罚权重，远大于 Q H_aug = blkdiag(H, rho*eye(2)); % 两个松弛变量对应 beta 上下限 % 约束: beta_min - eps1 <= beta <= beta_max + eps2

rho取 1e4 量级，意思是“除非万不得已，别用松弛”。正文里说“QP 求解器偶尔抽风，加个松弛因子像吃了镇定剂”，说的就是这个。侧偏角约束-5° < beta < 5°也建议走软约束，因为轮胎进入非线性区后 beta 本来就会超，硬卡住反而让控制器输出异常。

4.4 代码生成前的三个检查点

如果方案要往快速原型或嵌入式走，MPC_ComputeControlInput生成 C 代码前必须核对三件事。第一，坐标系转换是否在 C 里也做了——正文那段 C 代码里transform_to_frenet就是干这个的，漏了就是“毕加索轨迹”。第二，quadprog是否换成了定制的 QP 求解器（如 OSQP 或 HPIPM），因为 MATLAB 的quadprog不生成嵌入式代码。第三，预测矩阵 Phi/Theta 是否预计算成常量数组，别在 C 里每周期重算kron，那是纯浪费算力。

5. 避坑与排查：五个最容易翻车的点

5.1 现象：弯道轨迹突然发散，直道正常

原因：惯性系和道路坐标系没转换，或者wrapToPi漏了。直道时 refPsi 接近 0，两者差不多；弯道时 refPsi 一大，误差计算全错。

解决：在参考轨迹输出后强制过一遍toRoadFrame，并在 Simulink 里加 Scope 同时看 ey 和原始 dy，两者在直道应重合、弯道应分离，分离才说明转换生效。

5.2 现象：提高预测时域后跟踪更准，但仿真卡成幻灯片

原因：N 从 10 加到 20，QP 变量数翻倍，quadprog的复杂度约 O(N^3)，CPU 占用率飙升 50% 是正常现象。

解决：预测时域和控制时域解耦，N=20 但控制时域 Nc=5，只优化前 5 步的控制量，后面保持最后一步不变。这样精度接近 N=20，计算量接近 N=5。正文里“控制时域压到 5 步防止计算量爆炸”就是这个意思。

5.3 现象：方向盘周期性抖动，横向误差曲线呈锯齿

原因：R 太小或 Q 里横摆角速度权重不足，控制器对模型噪声过度反应；也可能是执行器 Rate Limiter 没加，QP 解出的转角序列本身就在跳。

解决：先把 R 从 0.1 提到 0.5 试，抖动减弱说明是控制量惩罚不够；若无效，检查 Rate Limiter 是否生效。正文里“Q 矩阵给横摆角速度加惩罚项”是临时方案，根因往往是预测模型和实际车辆动态失配（比如未建模的侧倾）。

5.4 现象：高速工况下画龙，低速正常

原因：轮胎线性区假设在高速大转角下失效，侧偏刚度实际值偏离标称值，预测模型“骗”了控制器。

解决：两条路——换魔术公式轮胎模型做非线性 MPC（计算量大），或者在 MPC 里加侧偏角软约束-5° < beta < 5°，让控制器主动避开非线性区。正文实测“加软约束瞬间治好画龙”，属于性价比最高的方案。

5.5 现象：QP 偶尔返回空解，仿真中断

原因：硬约束在极端状态组合下不可行，比如 beta 已经超限还要满足转角约束。

解决：加松弛因子（见 4.3），并给quadprog设MaxIterations上限和失败回退逻辑——无解时沿用上一周期转角，同时打日志。这个回退逻辑在实车上就是“控制器降级”，答辩时能讲出来是加分项。

6. 验收与答辩：用三张图和一个对比表把方案讲圆

验收这套 MPC 横向控制方案，我一般不看单一指标，而是让三张图同时说话。第一张是单移线工况的轨迹对比图：目标轨迹、实际轨迹、PID 基线轨迹画在同一坐标系，重点标出 35 米处误差突增点和最大横向误差 0.3 m 的位置。第二张是前轮转角时序图，看幅值是否卡在 ±25°、变化率是否平滑，这张图直接回应“执行器友好度”指标。第三张是 QP 求解耗时曲线，横轴仿真时间、纵轴每周期耗时，证明 20 ms 以内——答辩时评委最爱问实时性，这张图比嘴说管用。

对比表建议这样列，把“为什么选 MPC 不选 PID”用数据钉死：

表里 PID 那列不是贬低 PID，而是说明 MPC 用计算换来了约束处理和预判能力，代价是实时性开销——这个 trade-off 讲清楚，答辩就稳了。另外准备一个“参数敏感性”小实验：把 Q 的横向权重从 10 调到 20，展示误差下降但转角抖动上升，证明调参不是玄学而是有明确方向的。

血泪经验是：答辩前一定把“预测时域增大为什么可能导致控制延迟”这个问题想透。正文留的那个思考题，答案在于预测时域变长后，QP 更倾向于用远期信息做平滑，近期响应反而变“懒”，加上求解耗时增加，等效延迟上升。我一般会在答辩 PPT 里放一张 N=10 和 N=20 的阶跃响应对比，用波形说话。从那以后我每次交付横向控制方案，都强制走一遍“三张图 + 对比表 + 敏感性实验”的验收流程，缺一张心里就没底。希望这套结构能帮到你，把毕设或交付方案一次讲圆。

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