简介一套将 Apollo 开源框架中横向 MPC模型预测控制算法移植到 MATLAB 环境的代码包面向自动驾驶控制方向的工程师、研究者与高年级学生主要用于轨迹跟踪与转向控制算法的仿真验证。压缩包内共 9 个文件包括 5 个 .m 源文件mpc_control.m、UpdateErrorDynamics.m、GetControlLimit.m、InterPolate.m、main.m与 4 个 .asv 自动保存备份前者分别承担主控制、误差更新、控制量限制、路径插值和入口流程后者便于在编辑过程中恢复对照整体大小仅 9KB结构轻量注释清晰适合直接在 MATLAB 中阅读运行。实现中利用车辆侧滑角、横摆角速度等动态特性构建状态空间模型通过预测时域内的约束最优化生成当前转向角指令完整呈现 MPC 的滚动优化与反馈校正机制。开发者还可修改成本函数、约束条件或车辆参数进行二次开发并结合 Simulink 开展系统级仿真。已有 1733 人下载学习是一份兼具学习与研究价值的横向控制算法示例。1. 为什么横向控制要选MPC1.1 横向控制在自动驾驶链路中的位置先梳理一下自动驾驶的控制链路感知层给出障碍物和车道线信息决策规划层生成一条期望轨迹而控制层要做的就是让车精确地跟随这条轨迹。控制层又分为纵向控制油门/刹车和横向控制方向盘转角我这次要说的就是横向控制中的MPC方案。很多朋友问我横向控制用PID或者LQR不也能做吗为什么非得用MPC我直接说结论PID适合简单工况LQR适合无约束的线性系统但真实车辆有转向机构物理极限、有轮胎附着极限、有执行器响应延迟。MPC最大的优势是它能在每个控制周期内基于预测模型往前看一段时域并且把转向角约束、转向角变化率约束直接放进优化问题里求解。这就像开车的时候老司机不会盯着眼前两米看而是会预判弯道走势提前打方向——MPC干的正是这件事。如果你是刚开始接触这个方向我的建议是别一上来就啃复杂的非线性MPC从本文这种基于线性时变模型预测控制的方案入手把整个流程跑通再去逐步加复杂度。1.2 PID/LQR/MPC三者的取舍逻辑为了说清楚选型逻辑我把三种算法放在同一个维度下对比算法是否需要预测模型能否显式处理约束计算开销适用场景PID否否极低低速、小曲率、工况简单LQR是线性定常否低高速巡航、无约束的线性区间MPC是可线性可非线性能较高弯道、避障、极限工况表格里最值得关注的是能否显式处理约束这一行。LQR虽然是很好的状态反馈控制器但当你告诉它方向盘最多只能打30度的时候它并不知道怎么把这个限制纳入计算只能靠事后限幅这就会导致控制品质下降甚至系统失稳。MPC把约束写进优化问题求解出的控制量天然满足限制这是质的差别。不过话说回来MPC也不是万能的。它的计算量确实更大所以工程上常见做法是简单工况切LQR复杂工况切MPC这就是所谓的混合控制策略。本文我们先把MPC这条线彻底吃透。2. 车辆模型推导与离散化2.1 从自行车模型说起MPC的预测能力依赖模型模型精度和复杂度要平衡。横向控制里最经典的是自行车模型Bicycle Model它把车辆的前后轮各合并成一个轮子忽略左右轮差异假设车辆只有横摆和侧向两个自由度。低速场景用运动学模型就够了但车速超过某个阈值比如30km/h之后轮胎侧偏特性会明显影响车辆行为这时候必须用动力学模型。我用的是线性二自由度动力学模型状态量取横向偏差、横摆角偏差、横摆角速度和侧向速度。核心方程是下面这组整车侧向力平衡方程横摆力矩平衡方程这两个方程把轮胎侧偏刚度、前后轴距、整车质量、横摆惯量都耦合在一起。很多初学者在这里犯迷糊我建议你把公式推导看两遍之后直接跳到代码里对应矩阵的赋值部分看代码比看公式快得多。建模的核心目的只有一个建立一个从前轮转角输入到车辆状态变化的数学映射这个映射越准MPC的预测就越准。2.2 线性化处理真实的车辆动力学是非线性的直接拿来做非线性MPC求解器的负担很大实时性很难保证。工程上最成熟的做法是线性时变MPCLTV-MPC在当前工作点附近做泰勒展开忽略高阶项得到一个线性状态空间方程。具体来说把参考轨迹上的每个点都作为工作点在每个采样时刻重新线性化一次得到时变的A、B矩阵。这意味着每个控制周期都要重新构建预测矩阵计算量会比传统LQR大但对于现代工控机和MATLAB的mex加速来说完全在可接受范围内。线性化之后状态空间方程形式如下状态方程关注横向偏差、横摆角偏差的动态输出方程关注实际测量量这里有个实操细节必须提醒你线性化之后的状态空间模型只在工作点附近有效如果参考轨迹曲率突变过大或车辆状态偏离参考太远模型的预测会失真。所以MPC控制器通常会配合一个前馈项把稳态误差提前消掉。2.3 离散化与代码落地连续模型不能直接在数字控制器里用需要离散化。我用的是前向欧拉法离散公式很简单A_d I ATsB_d BTs。Ts是控制周期横向控制一般取20ms到50ms取太小求解压力大取太大响应迟钝我常用的折中是20ms。在实际MATLAB代码中这部分通常放在mpcSolve函数之前每步先根据当前状态和参考点更新A、B矩阵然后调用quadprog求解。下面是离散化部分的简化示例% 连续状态矩阵A, B由车辆参数计算 A_d eye(nx) A * Ts; B_d B * Ts; % 构建预测矩阵Phi和Gamma Phi zeros(nx*Np, nx); Gamma zeros(nx*Np, nu*Nc); for k 1:Np Phi(k*nx-(nx-1):k*nx, :) A_d^k; for j 1:Nc if j k Gamma(k*nx-(nx-1):k*nx, j*nu-(nu-1):j*nu) ... A_d^(k-j) * B_d; end end end这段代码是MPC预测模型的核心骨架Phi矩阵负责把当前状态推到未来Np步Gamma矩阵负责把未来的控制序列映射到状态轨迹上。后面所有QP求解都建立在这两个矩阵的基础之上。3. MATLAB代码实现与核心模块拆解3.1 总体文件结构我习惯把整个仿真工程分成四个文件主脚本、模型参数文件、MPC求解函数、可视化脚本。这样分层的逻辑很清晰——主脚本负责仿真循环模型文件只做参数传递求解函数专注优化问题构建与求解可视化单独拉出去方便调试时反复调用。主循环大致长这样先初始化车辆状态和参考轨迹然后每一步做三件事——更新参考点和线性化模型、构建QP问题、调用quadprog求解出最优前轮转角再把这个转角输入给车辆模型更新状态最后把数据存下来用于绘图。3.2 预测模型构建的代码细节预测模型是整个MPC的地基。这里要重点说一个常见错误很多人的预测矩阵维度对不上导致quadprog报错。维度问题根本原因是对Np、Nc、nx、nu四个变量的乘法关系没理清。我整理了一个维度速查表变量含义常见取值Np预测时域20~40Nc控制时域5~15nx状态量个数4横向偏差、横摆角偏差、横摆角速度、侧向速度nu控制量个数1前轮转角Phi矩阵Np*nx行nx列维度必须匹配Gamma矩阵Npnx行Ncnu列维度必须匹配这个表我建议你打印出来贴在屏幕边调试报错的时候对照着查能省下大量时间。3.3 目标函数与约束的QP标准形式MPC的核心优化问题本质是一个带约束的二次规划QP。要把MPC问题喂给MATLAB的quadprog必须把目标函数化成标准形式目标函数里的误差加权项对应QP中的H矩阵控制量加权项对应H矩阵中的控制分量约束条件对应Aineq、bineq、lb、ub标准的quadprog调用格式如下options optimoptions(quadprog, Display, off, Algorithm, active-set); [u_opt, fval, exitflag] quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options);这里有几个关键点展开说一下。第一H矩阵必须是对称正定的否则求解器直接报错或输出无界解我在代码里会加一行H (H H) / 2来保证对称性。第二f向量包含了参考状态的信息如果参考轨迹是定值参考f就是个常数向量但如果参考轨迹每个点都不一样f必须每步重新计算。第三lb和ub是对控制量的硬约束比如前轮转角最大30度换算成弧度就是±0.5236。3.4 仿真主循环的完整流程仿真主循环的完整逻辑我用伪代码来说明因为完整代码太长贴一段逻辑清晰的主循环更利于理解for k 1:length(t) % 1. 取当前车辆状态 x_current % 2. 从参考轨迹中找到最近点及参考状态 x_ref % 3. 在当前工作点线性化得到A_d, B_d % 4. 构建预测矩阵Phi, Gamma % 5. 构建QP目标函数H, f, 约束lb, ub % 6. 调用quadprog求解取第一个控制量 u_opt(1) % 7. 用车辆模型更新状态记录数据 end主循环里第2步的找最近点容易被忽略但它直接决定控制精度。最简单的方法是用欧氏距离遍历参考轨迹所有点复杂度O(n)轨迹点不多时完全够用轨迹点很多时建议用KD树或者按弧长预索引。还有一个细节找到最近点之后参考横摆角应该取该点处的切线方向而不是用全局坐标的反正切否则大弯道会出现明显的跟随误差。4. 参数整定与仿真调优4.1 权重矩阵Q和R的调节方法MPC的调参核心就是调目标函数里的Q和R两个权重矩阵。Q惩罚状态偏差R惩罚控制量大小。直觉理解Q的数值越大控制器对偏差越敏感响应越激进R越大控制器越怕打方向转向越保守。我给出一个经验初值组合方便你快速起跑Q diag([30, 10, 5, 1]); % 横向偏差、横摆角偏差、横摆角速度、侧向速度 R 0.1; % 前轮转角权重这个组合的逻辑是横向偏差的权重最大因为这是我们要追求的核心指标横摆角偏差其次因为它影响车辆姿态横摆角速度和侧向速度权重更小因为它们是内部动态量不需要施加很强的惩罚。实际操作中先把R固定从小到大调Q观察横向偏差是否收敛再把Q固定从小到大调R感受控制量变化是否平滑。这个顺序很重要同时调两个参数很容易陷入这调调那调调的困局。4.2 预测时域Np和控制时域Nc的选择预测时域Np决定了控制器能看多远。Np太小控制器视野太短入弯前提前打方向的意图体现不出来Np太大预测矩阵维度暴增求解时间急剧上升。我的经验值是车速低时Np可以取大一些如30车速高时Np取20左右即可。控制时域Nc决定了控制器未来规划几步一般取5到10就够。Nc继续增大对效果提升不明显但维度增大导致求解变慢。这里有个实用技巧如果你发现控制器在大曲率弯道中转向不足先加Np而不是加Nc通常立竿见影。我整理了一张调参速查表做仿真时可以对照排查现象优先调整参数调整方向入弯晚、切弯过度Np增大控制量抖动剧烈R增大横向偏差收敛慢Q(1,1)增大求解时间超标Np、Nc减小高速震荡权重预测时域Q增大同时Np略减4.3 约束处理的工程技巧约束设置看起来简单但实际操作有几个坑。第一个坑是约束过紧导致QP无解比如把前轮转角约束设成±0.1弧度而实际跟踪大弯道需要0.4弧度求解器无论如何都找不到满足条件的解。这种情况下exitflag会返回负值很多人在这里卡住。解决方法是加松弛因子在目标函数里加一个惩罚项允许约束在某些极端工况下被轻微违反代码里表现为在H矩阵对角线上对松弛变量加一个大权重。第二个坑是控制量的变化率约束。光限制前轮转角本身还不够如果相邻两个周期转角突变执行机构跟不上实际效果会打折扣。我一般在QP里额外加一维转角增量作为控制变量把角度和角速度都约束起来。这会让状态方程多一列维度更复杂但控制品质提升非常明显。实际经验是加了变化率约束之后方向盘抖动问题几乎消失。第三个坑是关于约束的软硬属性。硬约束是必须满足的而软约束可以在大权重惩罚下轻微越界。在工程上我会把转向机构物理极限设为硬约束把控制量舒适性上限设为软约束这样既保证安全又给求解器留了余量。5. 常见问题与排查技巧实录5.1 求解器报错矩阵维度不匹配这是新手最容易遇到的一类问题报错信息通常指向H或f向量维度与quadprog要求不一致。我在调试时有一个笨但有效的方法在调用quadprog前打印所有矩阵的size逐一和维度速查表比对。还有一个高发原因当Nc小于Np时Gamma矩阵中超出Nc的部分没有正确定义导致目标函数矩阵H出现零行或NaN。这类问题还有一种隐蔽的表现形式是约束矩阵Aineq的行数和f向量的列数对不上报错信息可能不太直接。我的建议是写一个小的调试辅助函数专门用于校验所有矩阵维度在仿真循环里每步调用一次这样可以把问题精确定位到具体是哪一步出错。5.2 状态发散或数值爆炸如果仿真跑着跑着状态量飞了先不用怀疑MPC理论有问题大概率是数值问题。我总结出三个最常见的数值爆炸原因。第一是离散化步长太大。如果Ts超过系统最小时间常数的十分之一前向欧拉法会失真试试把Ts减半看是否好转。第二是模型矩阵条件数过大。线性化之后的A矩阵如果条件数很大预测矩阵的数值稳定性会很差。解决方法是单位归一化把毫米、度等单位统一成米、弧度或者对状态做缩放处理。第三是参考轨迹突变。如果参考轨迹本身有阶跃式跳变MPC的预测会出现严重的瞬态响应。解决办法是对参考轨迹做平滑预处理我常用的方法是B样条拟合或移动平均滤波。5.3 跟踪效果不错但求解过慢实时性是MPC工程落地的关键指标。如果你在仿真中发现一个控制周期求解要花掉200ms而控制周期只有20ms这是致命的。我提供几条经过验证的提速路径第一条减少预测时域的同时保持性能。把Np从40减到25求解时间大约能降一半配合把Q调大一点性能几乎不下降。第二条用有效集算法代替内点法。quadprog的Algorithm选项里active-set适合中小规模问题Interior-point适合大规模问题我实际测试active-set在Np20的场景下速度快约三倍。第三条编译成mex封装。MATLAB自带的codegen可以把MPC求解函数编译成mex文件实测提速五到十倍。如果你有MATLAB Coder工具箱这几乎是一劳永逸的解法。6. 从仿真到实际应用的扩展思路代码调通、仿真验证完成之后下一步怎么走我分享几条个人经验。第一如果你打算把算法部署到实车或硬件在环平台建议把MATLAB代码移植成C二次规划求解库推荐OSQP或acados这两个库在嵌入式平台上有成熟支持。不要试图在MATLAB环境里做实时控制虽然mex能提速但硬件平台独立部署才是工程常态。第二模型可以从线性时变往非线性推进。本文用的LTV-MPC适合大多数工况但在冰雪路面、漂移工况、轮胎进入强非线性区的时候LTV模型误差会显著增大。你可以考虑更高精度的动力学模型但代价是求解速度和系统复杂度建议作为优化方向而不是第一步目标。第三传感器噪声和时间同步问题在仿真中完全体现不出来但在实车上是必然要面对的。横向控制依赖横摆角速度信号和横向偏差观测值如果你的IMU噪声偏大建议加上卡尔曼滤波或互补滤波做状态估计。我在实际调试中最大的体会是MPC调参的过程非常考验耐性参数互相耦合很难一步到位。建议你每次只动一个参数记录实验数据做一个简单的参数与效果对照表时间久了就能积累出感觉。更重要的是先保证基础的模型正确性和代码逻辑正确性再追求控制性能的极致顺序不要搞反了。最后分享一个我常用的工作习惯我会把每一组调参得到的轨迹误差曲线截图保存并在文件命名里注明参数组合和工况信息。这样回头找起来非常方便也能避免重复试错。做算法规范化和记录往往比追求一次性的完美表现更重要。本文还有配套的精品资源点击获取