企业官网建设流程全解析

去年我们把车道保持从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放在相同任务下考虑：车道保持或者轨迹跟踪，评价维度是预测能力、约束处理、计算量、实现难度、实车调试友好度。

从这张表能看出，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_yr
I_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_f
F_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_psi
e_psi_dot = v / L * tan(delta) - v * kappa_ref

kappa_ref是参考轨迹的曲率，可以理解为路弯不弯。对纵向车速v做定常假设，把状态设为[e_y, e_psi, e_y_dot, e_psi_dot]的某种组合，控制量取前轮转角delta，就能得到线性化的状态空间模型：

x(k+1) = A * x(k) + B * delta(k) + C(k)

C(k)是由参考曲率带来的附加项，在线计算时根据前方参考点插值得到。

这里有个细节，状态里的速度项究竟怎么处理。低速运动学模型可以把v拿进A矩阵变成时变系统，高速动力学模型则要把速度变化也考虑进去。工程实现往往会对不同车速分段选择模型，再对模型矩阵做线性化处理。只要A、B矩阵随控制周期更新，求解器仍能保证不错的实时性。

3.2 目标函数和权重矩阵的物理意义

MPC每一帧要优化的目标函数，一般写成如下形式：

J = sum_{i=0}^{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, N=20, dt=0.05, Q=np.diag([10.0, 1.0, 1.0, 0.5]), R=np.array([[0.1]]), u_max=0.6, du_max=0.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_start=True) 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求解器的选型，直接影响能否跑进实时控制周期。我实测过几种常用方案，列一个表供参考：

我见过不少人一上来就用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可以取100ms，N取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 问题速查表

把我在不同项目里遇到过的典型问题整理成一张速查表，方便大家对照排查：

遇到问题先定位现象属于哪一类，再决定改结构还是改参数。不要一上来就怀疑MPC原理不行，多数情况下是某个接线层面的问题。

6.2 我的几个额外调试心得

MPC和传统控制算法最大的不同是，它把很多设计决策从“写控制律”变成了“写优化问题”。所以我调试时最先看的不是代码循环，而是目标函数里每一项到底在多大尺度上起作用。横向偏差单位是米，数值通常零点几；航向偏差单位是弧度，高速下可能只有0.01量级；控制量单位是弧度，可能也是0.1量级。如果权重不按量纲缩放，航向偏差那一项基本形同虚设。

另一个经验是每帧求解器的迭代次数要设上限，并加一个超时兜底。实时系统里绝对不能出现“等求解器算完再发指令”这种逻辑。如果本帧超时，我会直接用上一帧解的第一项控制量，或者用最近一次成功解的的控制序列做递推。安全第一，控制指令不能断。

最后说一点心态上的体会。MPC确实比PID复杂，调试周期也长，但它带来的“预判能力”和“约束表达能力”在自动驾驶场景里价值太大了。我后来把MPC也用到了纵向速度控制里，效果同样比纯PID好。如果你正在从PID往MPC切换，建议先在仿真里把问题想透，再上实车，这个顺序能省下大量时间。