去年我们把车道保持从PID换成了MPC原因很直白高速过弯时PID控制的方向盘在弯道入口总会抖一下过了弯心又感觉修正晚了一拍。我们当然也试过加大阻尼、换观测器、加前馈折腾了两周最后意识到根本不是参数没调好而是算法本身看不到未来的路。MPC这种预测控制算法本质上是把“跟踪误差最小化”变成“在满足物理约束前提下让未来一小段时间内车辆的运动轨迹尽量贴近参考轨迹”这不光是在做控制了更像在每一帧进行一个微型的局部规划。对于正在接触自动驾驶控制算法、想搞明白车辆怎么沿着目标轨迹走的朋友这篇文章会把MPC的原理、模型、调参和实车部署的坑一次讲透。1. 为什么最终选了MPC从PID和LQR的局限说起1.1 从一次过弯抖动开始的问题那次问题出在一个半径约200米的匝道车速稳定在60km/h左右。PID输出前轮转角参数已经按经验做过一轮整定直道上表现不错但一进弯方向盘会先轻微回摆随后像“犹豫”一样才进入稳定转向。这是因为PID本质上是根据当前误差做比例、积分、微分运算它对未来没有任何预见性。曲率突变时误差才刚刚开始增长PID必须要等误差累积到一定程度才会加大修正力度这在物理上就决定了它永远慢半拍。有人会说LQR不是比PID更“现代”吗LQR确实解决了多变量最优的问题可以同时考虑横向偏差、航向偏差、横摆角速度等多个状态计算出一组最优状态反馈增益。但它有两个硬伤。第一LQR很难显式处理约束。转向角有极限转向角变化率有极限期望加速度受路面附着条件限制这些在LQR里只能通过调权重重间接体现调出来往往是保守又平庸的效果。第二LQR是基于当前状态的一次性最优解没有“未来路长什么样”的概念。它和PID一样是站在当下做决策视线没有延伸到前方。MPC的不同之处在于它把控制问题写成“未来N个时刻的最优控制序列”来求解。每帧优化完只执行第一步下一帧拿到新的状态再重新优化一遍这就是滚动优化的含义。这种结构让它天然包含了对参考轨迹的预瞄能力也能把机械限位、舒适性、附着极限这些约束直接写进优化问题里。1.2 三种控制算法的核心对比用表格来对比会更直观。我把PID、LQR和MPC放在相同任务下考虑车道保持或者轨迹跟踪评价维度是预测能力、约束处理、计算量、实现难度、实车调试友好度。对比维度PIDLQRMPC是否预测未来轨迹否事后纠偏否当前状态反馈是有限时域预测是否显式处理约束不能很难可以约束可写入优化问题多变量耦合处理需要手动解耦可以用状态空间统一设计天然支持多变量联合优化在线计算量极小小较大取决于预测时域和约束数工程实现难度低中中高适合场景简单工况、底层执行器线性工作点附近高速、过弯、强约束、多目标场景从这张表能看出MPC并不是在所有方面都优于LQR或PID。它的计算量更大调试门槛也更高。但在自动驾驶场景里车辆本身是非线性系统工作范围多变约束处处存在尤其是高速下要兼顾跟踪精度、车身稳定和乘坐舒适MPC几乎是绕不开的选择。我个人的理解是MPC更像一个“带约束的在线规划器”控制周期内算出来的不一定是一成不变的反馈增益而是“考虑未来一段路况后的最优行动方针”。这也是我在后文所有调参和问题排查中反复强调的核心视角。2. 车辆模型选择运动学还是动力学差别很大2.1 低速场景常用的运动学自行车模型MPC一定要有预测模型没有模型就谈不上预测。预测模型可以简单也可以复杂但最简单的形式是运动学自行车模型。它的假设是车辆没什么侧滑轮胎始终沿运动方向滚动适合低速泊车、封闭园区、拥堵跟车这类工况。常见的运动学模型可以写成这样dx/dt v * cos(theta beta)dy/dt v * sin(theta beta)dtheta/dt v * cos(beta) * tan(delta) / L其中x、y是车辆后轴中心位置theta是航向角delta是前轮转角L是轴距v是车速beta是质心侧偏角。低速下beta很小很多工程实现直接忽略它把模型简化为dtheta/dt v * tan(delta) / L这个模型的好处是参数少、计算量小在低速场景下精度够用。用来做泊车路径跟踪或者低速记忆泊车几十毫秒的控制周期完全扛得住。我刚做车辆控制那会儿觉得模型越复杂越高级后来发现低速场景用简单模型反而更稳定因为参数越少对精度的影响因素就越少系统更容易收敛。2.2 高速为什么必须用动力学模型车速超过60km/h以后简单运动学模型就不太够用了。高速下轮胎会产生不可忽略的侧偏角车辆横摆动力学和侧向加速度强耦合转向角输入到横摆角速度的响应也不再是简单的几何关系。这时候需要用二自由度车辆动力学模型业内也叫自行车动力学模型。忽略俯仰、侧倾只考虑横向运动和横摆运动可以得到m * v * (beta_dot psi_dot) F_yf F_yrI_z * psi_ddot l_f * F_yf - l_r * F_yr其中beta是质心侧偏角psi是横摆角m是整车质量I_z是绕Z轴的转动惯量l_f和l_r是质心到前轴和后轴的距离。线性轮胎模型下轮胎侧向力近似为F_yf -C_af * alpha_fF_yr -C_ar * alpha_r这里C_af、C_ar分别代表前轴和后轴的等效侧偏刚度alpha_f、alpha_r是前后轮侧偏角。进一步可以把侧偏角表达成车辆状态和转向角的函数整理成标准的状态空间形式d/dt [y, psi, beta, psi_dot]^T A * x B * delta坐在车内直观感受是低速过弯方向盘一打车身跟着就转高速过弯方向盘打了之后车身要有一个建立横摆响应的过程这期间还会产生轻微的“外抛”趋势。这就是动力学效应。用动力学模型做MPC预测控制器才会提前知道横摆建立需要时间才会在入弯前适度提前转向而不是等误差已经变大再纠。2.3 模型不准怎么办反馈校正和状态估计兜底既然模型只是真实系统的近似就必然存在模型失配。路面附着系数变化、轮胎磨损、载重变化都会导致侧偏刚度变差。MPC本身有反馈校正环节每帧拿车辆实际状态去重新初始化优化问题这能吸收一部分模型误差。但如果误差太大预测轨迹会和实际轨迹明显偏离控制效果照样恶化。工程上常用两个手段兜底。一是引入高精度的状态估计比如扩展卡尔曼滤波或无迹卡尔曼滤波把GPS/IMU/轮速信号融合起来得到更准确的横摆角速度、质心侧偏角估计值。二是做简单的参数自适应在线估计轮胎侧偏刚度等关键参数。实车量产程序里全参数自适应不一定现实但针对侧偏刚度做离线查表加在线修正已经是很成熟的策略。我做项目时还有个笨办法把模型预测的输出和实际测量做差长时间持续偏高或偏低就说明模型参数可能要更新而不是一味调大Q矩阵去硬抗。模型错误和权重不合理是两码事先用数据确认是哪一类再动手改。3. 手把手搭建横向MPC状态、目标函数、约束和求解3.1 误差状态空间怎么写自动驾驶轨迹跟踪控制里MPC最常处理的状态不是绝对坐标而是相对于参考轨迹的误差。横向偏差e_y表示车辆质心到参考轨迹的横向距离航向偏差e_psi表示车辆航向与参考轨迹切线方向的夹角。小角度假设下横向偏差和航向偏差的导数可以写成e_y_dot v * e_psie_psi_dot v / L * tan(delta) - v * kappa_refkappa_ref是参考轨迹的曲率可以理解为路弯不弯。对纵向车速v做定常假设把状态设为[e_y, e_psi, e_y_dot, e_psi_dot]的某种组合控制量取前轮转角delta就能得到线性化的状态空间模型x(k1) A * x(k) B * delta(k) C(k)C(k)是由参考曲率带来的附加项在线计算时根据前方参考点插值得到。这里有个细节状态里的速度项究竟怎么处理。低速运动学模型可以把v拿进A矩阵变成时变系统高速动力学模型则要把速度变化也考虑进去。工程实现往往会对不同车速分段选择模型再对模型矩阵做线性化处理。只要A、B矩阵随控制周期更新求解器仍能保证不错的实时性。3.2 目标函数和权重矩阵的物理意义MPC每一帧要优化的目标函数一般写成如下形式J sum_{i0}^{N-1} (x_i - x_ref_i)^T * Q * (x_i - x_ref_i) delta_i^T * R * delta_i (x_N - x_ref_N)^T * P * (x_N - x_ref_N)N是预测时域Q是状态误差权重矩阵R是控制量权重矩阵P是终端惩罚矩阵。Q里面给横向偏差的权重越大控制器就越“着急”贴近参考线后果是转向动作更猛。R给得越大控制器就越“偷懒”转向动作更平缓但过弯误差会变大。终端惩罚P是为了保证预测时域结束时车辆状态也能趋向参考否则优化只会顾头不顾尾。理解Q和R的物理意义是调参的基础。比如在高速变道场景横向偏差权重太高会导致方向盘快速切入带来明显横向加速度冲击航向偏差权重太高则会让车辆变得“轴”转弯时转向不够及时。设计目标函数时我会先想清楚一个优先级保障安全约束、兼顾跟踪精度、最后才追求舒适性权重设置要和这个优先级一致。3.3 一个可运行的简化横向MPC代码骨架下面给出一段非常简化的Python代码展示在已知参考轨迹ref_x、ref_y、ref_yaw时如何调用OSQP求解QP问题。实际工程里会用C实现但逻辑是一致的。import numpy as np import osqp from scipy import sparse class LateralMPC: def __init__(self, N20, dt0.05, Qnp.diag([10.0, 1.0, 1.0, 0.5]), Rnp.array([[0.1]]), u_max0.6, du_max0.3): self.N N self.dt dt self.Q Q self.R R self.u_max u_max self.du_max du_max def build_model(self, v, kappa_ref): # 简化状态 [e_y, e_psi, e_y_dot, e_psi_dot] A np.array([ [1.0, v * self.dt, self.dt, 0.0], [0.0, 1.0, 0.0, self.dt], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0] ]) B np.array([0.0, v * self.dt / 3.0, 0.0, v * self.dt / 3.0]).reshape(4, 1) # 参考曲率的补偿项实际由轨迹插值得到 C np.array([0.0, -v * kappa_ref * self.dt, 0.0, 0.0]) return A, B, C def solve(self, x0, ref_states, v, kappa_ref): A, B, C self.build_model(v, kappa_ref) n_x 4 n_u 1 # 构造QP的P矩阵 P np.zeros((self.N * (n_x n_u), self.N * (n_x n_u))) for i in range(self.N): P[i * (n_x n_u): i * (n_x n_u) n_x, i * (n_x n_u): i * (n_x n_u) n_x] self.Q P[i * (n_x n_u) n_x: i * (n_x n_u) n_x n_u, i * (n_x n_u) n_x: i * (n_x n_u) n_x n_u] self.R P sparse.csc_matrix(P) # 线性约束矩阵包含状态递推和控制增量约束 # 这里省略了完整拼装工程实现可以参考OSQP示例 # 后文给出约束拼装的思路 q np.zeros(self.N * (n_x n_u)) lower -np.inf * np.ones(self.N * (n_x n_u)) upper np.inf * np.ones(self.N * (n_x n_u)) prob osqp.OSQP() prob.setup(P, q, None, lower, upper, warm_startTrue) res prob.solve() return res.x这段代码框架把“模型预测”和“优化求解”分开了。现实中QP的约束拼装是重头戏要把状态递推等式Ax Bu C x_next写进等式约束把|delta| u_max和|delta_k - delta_{k-1}| du_max写进不等式约束。这些约束能用稀疏矩阵高效表示直接用CSR或CSC格式拼装会快很多。3.4 求解器选型实测对比MPC求解器的选型直接影响能否跑进实时控制周期。我实测过几种常用方案列一个表供参考求解器算法类型适合场景实测体验OSQP交替方向乘子法凸QP、中小规模、嵌入式开源免费内存占用小收敛速度快最推荐作为起步qpOASES活跃集法小型QP、无约束或简单约束冷启动快但约束多时效率下降CVXGEN自动代码生成固定规模小问题早期嵌入式MPC常用问题结构一变就要重新生成代码灵活性差CasADi IPOPT内点法离线仿真、非线性MPC研究精度高但实时性不够不适合量产控制我见过不少人一上来就用CasADi搭非线性MPC仿真里效果很好看但部署到嵌入式平台后计算时间严重超标。我的建议是第一版先做成线性时变MPC用OSQP这类轻量求解器把控制周期稳定跑进30ms以内再去考虑要不要上非线性MPC。4. 调参到底在调什么权重、预测时域、控制周期和约束4.1 权重矩阵标定的经验路径新手拿到MPC的第一反应是把Q和R调成对角阵这没错但不知道初值是什么就很痛苦。我常用的初始标定是Q取diag([10.0, 1.0, 1.0, 0.5])R取0.1预测时域20步控制周期50ms。这个组合在我们多个项目中都能作为安全起点既不会抖动也不会迟钝。接下来的调参顺序我建议先调R再调Q。R决定控制量的“活跃度”太小会让方向指令频繁跳动太大让响应发闷。先把R固定到一个不让方向发颤的值再去动Q的横向偏差和航向偏差权重。横向偏差权重增加车辆过弯会更贴近参考线航向偏差权重增加车辆切线和参考线的方向会更一致。这里面需要记住转弯时横向偏差和航向偏差本身是耦合的不要独自拉大某一项否则容易过冲。权重标定一定要分阶段走。先在仿真里用公共数据集或者场景回放做批量扫参确定大致区间再做硬件在环把底盘延时和执行器特性加进去最后才是实车微调。实车阶段尽量只改几个关键权重不要同时改预测时域和约束否则出了问题根本没法定位。4.2 预测时域N和控制周期dt怎么搭配预测时域N乘以控制周期dt等于控制器“预瞄”的总时间窗口。这个窗口太短控制器只能看到眼前一小段路高速下等于没有预瞄窗口太长远处的参考轨迹不一定可靠而且优化变量变多计算量成倍增长。我的一些经验值是这样的低速园区dt可以取100msN取20到30总窗口2到3秒高速道路dt取50ms以内N取30到60总窗口1.5到3秒紧急避障这类工况如果想看到更远的轨迹可以在线调整N或者缩短dt。控制周期小于20ms时求解器必须在极短时间内完成求解对硬件要求高一般底盘执行器也没必要那么快所以别一味追求小周期。还有一个很容易忽略的点预测时域内部的参考轨迹点必须与真实前方轨迹对齐。也就是说优化的第k步参考点应该是按当前车速往前推k*dt米处的轨迹点而不是机械地把轨迹数组的前N个点拿过来。这个细节如果错了车辆过弯会出现固定偏置看起来像又把权重调坏了。4.3 约束的松紧软约束比硬约束更实用MPC最亮的点是约束处理但约束不是越紧越好。约束过紧可能导致每一帧都没有可行解求解器直接报错车辆失去控制指令。这时候最常用的做法是把部分约束做成软约束J ... lambda_s * sum(s_i^2)也就是在目标函数里加入松弛变量s_i并给一个大惩罚系数lambda_s。这样优化问题不再硬性要求所有状态严格满足边界而是允许在极限情况下“轻轻越界”换取问题有解。转向角限位这种机械硬约束应当保留硬约束因为它由执行器物理极限决定但横向误差边界、舒适性加速度边界这种软性指标更适合做成软约束。实际调参中我建议先看车上日志统计正常行驶时转向角的分布范围。不要在仿真里觉得2弧度很酷结果实车方向盘根本转不到那么快。把约束设置得和真实执行能力匹配才能避免求解器在线性化精度不足时给出冒进的控制指令。5. 从仿真到实车部署和上下游模块的协同5.1 控制算法在整车软件架构里的位置MPC不是独立模块。它在自动驾驶软件栈里处于决策规划层和执行层之间。上游感知模块输出车道线、目标障碍物规划模块输出参考轨迹参考轨迹通常是一条带时间戳的曲线包含位置、航向角、速度建议甚至曲率。下游是线控底盘MPC把方向盘转角指令发给转向执行器把加速度指令发给动力和制动系统。我画了一条简化链路文字描述就是感知 - 融合估计 - 预测 - 决策规划 - 运动控制(MPC) - 底盘执行器 - 车辆运动MPC所在的位置决定了它必须同时理解两条数据流一条是“理想轨迹应该是怎样的”来自上游规划一条是“车辆现在实际在哪”来自定位和状态估计。所以MPC调参调到最后经常不是在调控制器本身的参数而是在调上游轨迹平滑度和下游执行器响应延迟。5.2 横纵向耦合与解耦布局上有两种常见方案。一种是横向纵向完全解耦横向MPC只管方向盘转角纵向控制单独做期望加速度通常也是MPC或者PID分层控制。解耦方案实现简单、调试方便现阶段量产项目里用得最多。另一种是横纵向联合MPC在同一优化问题里同时求解转向角和加速度。理论上更优因为高速过弯时纵向减速和横向转向是耦合的联合优化能更好地控制速度与转向的协调。代价是状态量翻倍、非线性更强、求解时间明显上升。我做过的项目里联合MPC更多用于研究验证和高性能场景量产落地还是解耦为主。如果你做解耦方案有一点要特别注意上游轨迹的时间戳和速度建议要与MPC内部预测周期保持同步。否则横向MPC按60km/h预瞄纵向控制实际上已经减速到40km/h路径跟踪就会在前方出现固定偏差。5.3 从仿真到实车的坑仿真里跑得好不代表实车稳。我们踩过最大的坑是执行器延迟。仿真里方向盘转角指令立刻生效实车从指令发出到转向机真正执行到位通常有50到150ms延迟。这个延迟会让MPC产生“控制过晚”的效果过弯时方向修正偏晚出现振荡或切弯。解决办法之一是延迟补偿。在模型预测中把执行器延迟建模进去把当前实际转角而不是指令转角作为状态反馈初始值。另一个办法是在标定时把执行器延迟测出来做成动态查表再把这个延迟写进参考轨迹的匹配上让控制器“提前半拍”动作。实车测试前我强烈建议先在仿真里用回放数据跑一遍MPC看看指令是否频繁撞击限位、是否有求解失败时刻。公共数据集和场地录制的log都行。回放仿真可以过滤掉大量低级问题省下的实车调试时间非常可观。6. 常见问题与排查实录6.1 问题速查表把我在不同项目里遇到过的典型问题整理成一张速查表方便大家对照排查现象常见原因排查方向方向盘高频抖动R过小、控制周期过短、执行器延迟补偿不足增大R、增大控制周期、检查延迟补偿过弯切弯偏内预测时域太短、横向偏差权重偏高、参考轨迹曲率不平滑增大N、适当调整Q、平滑轨迹曲率响应迟钝过弯误差大R过大、模型增益偏小、速度匹配错误降低R、检查车辆参数和速度系数求解器频繁报不可行约束过紧、状态估计异常、轨迹跳变软约束化、检查状态输入、加松弛变量直道有小幅波动权重或线性化精度问题、参考线噪声平滑参考轨迹、检查状态信号滤波换道后回正过冲航向偏差权重过高、终端惩罚不足调整Q、增大P或延长N遇到问题先定位现象属于哪一类再决定改结构还是改参数。不要一上来就怀疑MPC原理不行多数情况下是某个接线层面的问题。6.2 我的几个额外调试心得MPC和传统控制算法最大的不同是它把很多设计决策从“写控制律”变成了“写优化问题”。所以我调试时最先看的不是代码循环而是目标函数里每一项到底在多大尺度上起作用。横向偏差单位是米数值通常零点几航向偏差单位是弧度高速下可能只有0.01量级控制量单位是弧度可能也是0.1量级。如果权重不按量纲缩放航向偏差那一项基本形同虚设。另一个经验是每帧求解器的迭代次数要设上限并加一个超时兜底。实时系统里绝对不能出现“等求解器算完再发指令”这种逻辑。如果本帧超时我会直接用上一帧解的第一项控制量或者用最近一次成功解的的控制序列做递推。安全第一控制指令不能断。最后说一点心态上的体会。MPC确实比PID复杂调试周期也长但它带来的“预判能力”和“约束表达能力”在自动驾驶场景里价值太大了。我后来把MPC也用到了纵向速度控制里效果同样比纯PID好。如果你正在从PID往MPC切换建议先在仿真里把问题想透再上实车这个顺序能省下大量时间。