基于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 模型按职责切成四块信号流向是单向的方便单独替换和测试模块职责关键输入/输出二自由度车辆模型模拟横向横摆运动输出 beta、r、Y、psi输入前轮转角 delta参考轨迹生成产生单移线/双移线/车道线中心轨迹输出 refY、refPsiMPC 控制器滚动优化求 delta输入状态 x0、参考轨迹执行器模型饱和速率限制逼近真实 EPS输出受限 delta这里有个设计取舍值得说清楚为什么用二自由度而不是七自由度因为横向控制关心的是质心侧偏角和横摆角速度这两个主导量二自由度模型参数少质量、轴距、前后侧偏刚度、转动惯量线性化后直接能导出状态空间给 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] endwrapToPi这步别省航向角跨过 ±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, -(CafCar)/(m*vx), (CafCar)/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*Caflr^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)*41:i*4, :) Ad^i; for j 1:i Theta((i-1)*41:i*4, (j-1)*11: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 和参考轨迹输出 deltafunction 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); % 只取第一个控制量滚动优化 endH和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 eps2rho取 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 加到 20QP 变量数翻倍quadprog的复杂度约 O(N^3)CPU 占用率飙升 50% 是正常现象。解决预测时域和控制时域解耦N20 但控制时域 Nc5只优化前 5 步的控制量后面保持最后一步不变。这样精度接近 N20计算量接近 N5。正文里“控制时域压到 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 基线本方案 MPC验收阈值单移线横向误差峰值0.55 m0.28 m≤ 0.3 m稳态车道保持误差0.22 m0.12 m≤ 0.15 m前轮转角最大变化率18°/0.1s9°/0.1s≤ 10°/0.1s单周期求解耗时 1 ms15 ms≤ 20 ms表里 PID 那列不是贬低 PID而是说明 MPC 用计算换来了约束处理和预判能力代价是实时性开销——这个 trade-off 讲清楚答辩就稳了。另外准备一个“参数敏感性”小实验把 Q 的横向权重从 10 调到 20展示误差下降但转角抖动上升证明调参不是玄学而是有明确方向的。血泪经验是答辩前一定把“预测时域增大为什么可能导致控制延迟”这个问题想透。正文留的那个思考题答案在于预测时域变长后QP 更倾向于用远期信息做平滑近期响应反而变“懒”加上求解耗时增加等效延迟上升。我一般会在答辩 PPT 里放一张 N10 和 N20 的阶跃响应对比用波形说话。从那以后我每次交付横向控制方案都强制走一遍“三张图 对比表 敏感性实验”的验收流程缺一张心里就没底。希望这套结构能帮到你把毕设或交付方案一次讲圆。本文还有配套的精品资源点击获取