做自动驾驶路径跟踪的基本都绕不开MPC这个词。早些年我在仿真里用固定参数的MPC跑直线和缓弯觉得效果还行可一旦把车速提上去或者路径里塞进一段双移线那样的紧急变道工况固定参数方案就开始露馅横向误差拉大、转向抖动、甚至直接发散。这阵子我花了两周多时间把一套论文里比较常见的组合方案完完整整复现了一遍用神经网络做MPC的预测模型做成NN-MPC再用ANFIS在线调整MPC的代价权重组成自适应MPC最后在同一个仿真平台里对比标准MPC、NN-MPC和自适应MPC三者的表现。这篇文章就是整个复现过程的技术笔记从思路拆解、公式推导到代码结构、参数调节再到我踩过的坑全部摊开讲。先说清楚这篇文章适合谁。如果你已经搭过一个基础MPC知道预测时域Np和控制时域Nc大概是什么概念想在此基础上加入神经网络或模糊自适应模块那按这篇文章的操作走一遍是完全可以的。如果你只是听过MPC的名字还没跑通第一个版本我建议先把文中的车辆运动学模型和优化求解部分搞清楚再往上加学习模块。因为整个系统的地基还是MPC网络和ANFIS都只是给它“换脑子”和“调性格”地基不稳上层加什么都是白搭。1. 整体方案设计与思路拆解1.1 传统MPC在路径跟踪里究竟卡在哪里模型预测控制的本质是“每一拍都解一个有限时域优化问题”。在每个控制周期里控制器根据当前状态、参考轨迹和预测模型找出一段最优控制序列然后把序列第一个控制量送到执行器下一个周期重新求解一遍。这种滚动优化的好处是能处理约束坏处是对预测模型的准确性极其敏感。传统做法用的是运动学自行车模型加固定权重。低速工况下运动学近似够用但车速一高轮胎侧偏、横摆动态、加速带来的负载转移都会让模型和真实系统之间产生偏差。模型偏差一变大MPC算出来的“最优”控制量就不是真正的最优横向误差会明显上升严重时优化问题都解不出可行解。另一个问题在代价函数里横向误差、航向误差、控制量这三类指标共用一套固定Q/R权重这套权重在直道上很舒服到急弯里就嫌“纠错不够快”在低速大曲率路段好用到高速小曲率路段又容易抖动。所以传统MPC不是不能跑而是它把“模型精度”和“参数匹配”两个难题都押在了一个固定结构上。我们要做的就是用神经网络去补强预测模型用ANFIS去做代价权重的在线调度。1.2 两条技术路线NN-MPC与ANFIS自适应MPC这套复现方案里其实有两条独立技术路线它们解决的痛点不一样。第一条是NN-MPC。思路很直接既然手写动力学模型困难、精度有限那就让神经网络从数据里学一个预测模型。网络输入历史状态和控制量输出未来状态。预测模型换成了神经网络MPC的滚动优化框架不变只是内部“推演未来”的那一步由网络来完成。这个路线适合车辆动力学模型未知、参数难以标定、但数据采集相对容易的场景。数据充足的情况下网络对非线性特征的表达力比固定参数模型强很多。第二条是ANFIS自适应MPC。这条路线不改预测模型而是改代价函数里的权重。ANFIS全称是自适应神经模糊推理系统本质是一个带可学习参数的模糊逻辑系统。它有模糊规则的可解释性也有神经网络的学习能力。在路径跟踪里ANFIS根据当前跟踪偏差、车速、曲率等特征实时输出Q矩阵和R矩阵的缩放系数相当于给MPC配了一个“驾驶风格调节器”。横向偏差大了就加大纠错权重高速小偏差时就降低控制敏感度。两条路线可以合在一起理解NN负责“算得准”ANFIS负责“调得稳”。这里要强调一下复现时不要把两个模块硬塞进同一个优化器里。最稳妥的做法是分模块设计用ANFIS在外面调节MPC参数用NN在模型层做校准或预测每一层可以单独验证。1.3 方案选型时的取舍很多初学者看到“神经网络ANFIS自适应MPC”这几个词堆在一起第一反应是全都上结果调试量爆炸。实际做下来我的经验是先看你的痛点在哪里。如果车辆模型本身结构清楚只是参数不准用ANFIS在线调Q/R就够了没必要上NN。因为NN-MPC的训练数据、特征选择、网络结构调优每一步都在增加不确定性调试成本不低。如果模型结构复杂、轮胎非线性强、工况变化大NN的预测能力才有明显的不可替代性。从控制工程角度看ANFIS方案更容易保证稳定性和实时性因为它没有改变MPC本身的优化结构NN-MPC要解决网络输出可导性和求解器嵌入问题复杂一个量级。还有一个取舍维度是计算资源。ANFIS在线调参的计算量很小基本就是几次隶属度计算和加权平均NN-MPC如果真把神经网络放进优化器里做预测每一步求解都要经过网络的正向传播甚至反向求导实时性压力会非常大。所以复现时我给NN的定位是“离线训练、在线校准”而不是把一个大网络整体嵌进MPC的求解器内部。2. 核心原理梳理与关键方程2.1 车辆运动学模型和误差定义路径跟踪里最常用的预测模型是自行车模型也叫单轨模型。假设车辆左右轮合并为一个前轮和一个后轮忽略侧偏角状态取横向位置x、纵向位置y、横摆角φ和纵向速度v控制量取前轮转角δ和加速度a。离散化后的运动学方程可以写成x(k1) x(k) v(k) * cos(φ(k)) * dt y(k1) y(k) v(k) * sin(φ(k)) * dt φ(k1) φ(k) v(k) / L * tan(δ(k)) * dt v(k1) v(k) a(k) * dt其中L是轴距dt是控制周期。这个模型在低速和中等速度下精度够用是绝大多数MPC路径跟踪demo的默认选择。MPC里的跟踪目标通常不直接用绝对坐标而是用相对参考轨迹的误差。设参考轨迹在当前位置给出参考状态[x_ref, y_ref, φ_ref]那么误差向量为e_x x - x_ref e_y y - y_ref e_φ φ - φ_ref代价函数里对三个误差分量分别加权。注意这里的量纲差别很大x和y是米φ是弧度直接放进同一个对角阵如果不做无量纲归一化很容易出现一个分量主导整个优化目标的情况。后面我会专门讲这个坑。2.2 标准MPC的优化问题表达标准MPC在每个控制周期求解如下形式的优化问题min J Σ_{k1}^{Np} e(k)^T Q e(k) Σ_{k1}^{Nc-1} u(k)^T R u(k) Σ_{k1}^{Nc-1} Δu(k)^T S Δu(k)约束条件包括状态递推方程x(k1) f(x(k), u(k))初始状态x(0) x_current控制输入上下限δ_min ≤ δ ≤ δ_maxa_min ≤ a ≤ a_max控制增量约束|Δδ| ≤ Δδ_max这里每一项都有实际意义。Q矩阵决定跟踪误差在优化目标里的“地位”R矩阵决定控制量本身的“代价”S矩阵则是对控制量变化率的惩罚用于抑制转向突变。很多人只调Q和R忽略了Δu项结果控制量幅值不大但抖得很厉害加上S以后情况立刻改善。Np和Nc也是关键参数。Np是预测时域表示一次优化往前看多少步Nc是控制时域表示优化决策多少个控制量。Np太短控制器“目光短浅”来不及提前制动或转向Np太长求解变量增多实时性下降而且远处轨迹的不确定性会被过度放大。一般复现时Np取10到30Nc取Np的50%左右然后根据实际曲线再微调。2.3 NN-MPC里的神经网络预测模型怎么定义NN-MPC的核心是把2.1节里的解析递推方程替换成神经网络映射。最常见的设计是NARX结构即带外生输入的非线性自回归模型。输入当前时刻及过去若干时刻的状态和控制量输出下一时刻的状态或状态增量。这里有个工程细节直接预测绝对状态难度大训练误差稍微积累几步就会飘掉。稳妥做法是预测状态增量Δs(k) s(k) - s(k-1)网络只学习增量部分预测时用上一时刻的真实状态加上网络输出的增量。这样即使网络存在小误差也不会在递推过程中被反复放大。训练数据由车辆模型在随机控制输入下滚动生成。采集格式大致为输入特征[Δs(k-1), Δs(k-2), ..., s(k-1), δ(k-1), a(k-1)] 输出标签Δs(k)这里的状态一般取[x, y, φ, v]四维控制量取[δ, a]两维。网络训练好后MPC优化过程中做多步预测时就用网络输出增量逐个外推。理论上讲这个预测模型比固定参数运动学模型更贴近真实车辆的非线性特征尤其是轮胎接近附着极限的时候。2.4 ANFIS如何在线调整MPC参数ANFIS的完整结构分为五层输入模糊化层、规则前件层、归一化层、规则后件层、去模糊化输出层。它本质上是在用模糊规则搭一个可微的近似函数训练时通过反向传播去调整隶属度函数参数和规则后件参数。在自适应MPC里ANFIS的输入可以取三种特征横向误差e_y、航向误差e_φ、纵向车速v。输出是两个缩放系数kQ和kR用来修改MPC代价矩阵Q_now kQ * Q0 R_now kR * R0当|e_y|很大时我们希望MPC更看重纠错所以kQ取大一点、kR取小一点当|e_y|很小时为了不产生抖动应该减小Q、增大R让控制器更“保守”。同样车速提高时R的权重应该适当增大避免高速下控制量过于激进。规则写成模糊语言就是如果误差大且车速低则kQ大、kR小如果误差小且车速高则kQ中、kR大。需要注意ANFIS输出的缩放系数不能没有上下限。我一开始没限制误差大时kQ飙到十几MPC直接变成高增益控制器最优解剧烈变化系统震荡。后来把kQ限制在0.5到2.0之间效果立刻平稳下来。3. 仿真环境与数据准备3.1 平台选择与配置复现方案可以选MATLAB/Simulink也可以选Python生态。我把两者的特点列个表对比维度MATLAB/SimulinkPython CasADi建模便利性图形化建模模型库全代码建模灵活但需要自己写神经网络支持有Deep Learning ToolboxPyTorch/TensorFlow/scikit-learnMPC求解有MPC Toolbox封装好用CasADi手写优化问题实时性调试相对抽象黑盒多透明可控方便查问题学习成本工具链成熟但授权贵免费开源社区活跃我是用PythonCasADi跑的原因是整个链路完全透明。车辆模型、参考轨迹、优化约束、网络训练都在同一个环境里排查问题比在MATLAB里来回切换工具箱舒服很多。CasADi负责定义优化变量、约束和代价函数底层求解器用IPOPT神经网络部分用scikit-learn的MLPRegressor就够跑通流程不用一开始就上PyTorch。3.2 参考轨迹生成路径跟踪没有一个标准测试轨迹等于白跑。我的做法是生成三类轨迹直线、S形弯道、双移线变道。双移线是验证车辆变道工况的经典参考可以用一个周期函数来生成平滑的横向偏移。代码大致如下import numpy as np def double_lane(s, A3.5, Lc60): # s是沿参考线的累计弧长A是变道幅度Lc是变道长度 y np.zeros_like(s) for i, si in enumerate(s): if si Lc: y[i] A * (si / Lc - np.sin(2 * np.pi * si / Lc) / (2 * np.pi)) else: y[i] A return yS形弯道就是把双移线的后半段反向偏移让车辆连续变两次道。生成参考轨迹以后还要计算每个点的参考横摆角φ_ref一般直接对y关于s求一阶导数再取反正切。这一步容易被忽略但MPC误差项里需要φ_ref少了它航向误差就算不对。3.3 训练数据集的生成与预处理NN-MPC的训练数据不用实车采集至少复现阶段不需要。你可以用一个“更精细的动力模型”当作被控对象在这个模型上跑随机控制序列记录状态转移样本。也可以用标准运动学模型直接生成但网络学到的就是运动学模型的映射起不到“补强”作用。我的做法是给运动学模型叠加一些轮胎非线性修正项模拟真实车辆和简化模型之间的差距这样网络才有机会学到运动学之外的补偿信息。数据集字段基本如下字段含义示例state_in当前状态 [x, y, φ, v][10.2, 1.4, 0.08, 5.0]control_in控制量 [δ, a][0.05, 0.3]state_out下一时刻状态[10.5, 1.7, 0.09, 5.2]预处理必须做归一化。x和y是米级别φ是弧度级别v是米每秒级别量纲都不一样如果不归一化神经网络的损失函数会被大数值变量主导训练过程非常难收敛。我用z-score归一化把训练集的均值和标准差存下来在线使用时再用同一套参数归一化。训练集和验证集按8:2划分。验证集不能只取同一段轨迹的后半段要从不同速度、不同曲率的样本里重采样否则验证误差很低换条轨迹跑就露馅。4. 逐步复现与核心代码说明4.1 标准MPC的CasADi实现先搭一个标准MPC作为整个系统基线后续所有模块都在这套代码上扩展。使用CasADi定义优化问题时关键是三个对象状态变量X、控制变量U、参考参数Ref。代码结构如下import numpy as np import casadi as ca dt 0.1 # 控制周期 L 2.9 # 轴距 Np 20 # 预测时域 Nc 10 # 控制时域 Q np.diag([10.0, 10.0, 0.5, 1.0]) R np.diag([0.1, 0.1]) opti ca.Opti() X opti.variable(4, Np 1) U opti.variable(2, Nc) X0 opti.parameter(4, 1) Ref opti.parameter(4, Np 1) def dynamics(x, u): x_next x[0] x[3] * ca.cos(x[2]) * dt y_next x[1] x[3] * ca.sin(x[2]) * dt phi_next x[2] x[3] / L * ca.tan(u[0]) * dt v_next x[3] u[1] * dt return ca.vertcat(x_next, y_next, phi_next, v_next) opti.subject_to(X[:, 0] X0) for k in range(Nc): opti.subject_to(X[:, k 1] dynamics(X[:, k], U[:, k])) for k in range(Nc, Np): opti.subject_to(X[:, k 1] dynamics(X[:, k], U[:, Nc - 1])) cost 0 for k in range(Np 1): e X[:, k] - Ref[:, k] cost ca.mtimes([e.T, Q, e]) for k in range(Nc): cost ca.mtimes([U[:, k].T, R, U[:, k]]) if k 0: delta_u U[:, k] - U[:, k - 1] cost ca.mtimes([delta_u.T, S, delta_u]) opti.minimize(cost) # 约束前轮转角-30°到30°加速度-2到2 opti.subject_to(opti.lb U opti.ub) opti.solver(ipopt, {print_level: 0}, {max_iter: 100})主循环里每个周期给参数赋值求解后取第一个控制量while running: s_meas get_vehicle_state() ref_block get_reference_block(s_meas) opti.set_value(X0, s_meas) opti.set_value(Ref, ref_block) sol opti.solve() u_opt sol.value(U) apply_control(u_opt[:, 0])这里有个细节求解器不是每次都能成功。约束太紧或初始点不好会返回无解。工程上我会缓存上一个可行解本轮无解时用上一轮控制量顶一下避免控制器中断引发车辆失控。4.2 神经网络预测器的训练与嵌入在我复现的NN-MPC里网络并没有直接塞进CasADi优化器内部而是做“残差预测参数校准”。训练流程分三步。第一步准备样本。用运动学模型加非线性修正生成一批轨迹每条轨迹的状态转移样本构成输入输出对。这里我是用滑动窗口方式构造输入窗口长度取10步历史信息。窗口太短网络看不到动态趋势太长训练维度爆炸。第二步训练网络。我用scikit-learn的MLPRegressor先跑通流程from sklearn.neural_network import MLPRegressor from sklearn.preprocessing import StandardScaler scaler StandardScaler() X_train scaler.fit_transform(raw_input) model MLPRegressor(hidden_layer_sizes(64, 64), activationtanh, max_iter500, early_stoppingTrue) model.fit(X_train, y_train)第三步嵌入。每个MPC周期开始前调用网络预测当前时刻之后的状态增量序列根据预测结果修正运动学模型里的等效速度或等效横摆角速度。比如网络预测出弯道里实际横摆偏差比运动学模型小我就把运动学模型里的横摆项乘以一个校准系数。这种“软校准”不需要让网络在优化器内部参与求导稳定性好很多实时性也够。4.3 ANFIS在线调参模块的实现ANFIS模块不一定非得上深度学习框架我用一个简化版本就能完成在线权重调节。核心是用高斯隶属度函数定义输入模糊集合再用加权平均去模糊化。def gauss(x, center, width): return np.exp(-0.5 * ((x - center) / width) ** 2) def anfis_weights(e_y, e_phi, v): # 输入偏差范围按经验设定 mu_large gauss(abs(e_y), 0.0, 0.6) mu_small gauss(abs(e_y), 0.0, 0.15) # 输出kQ和kR kQ 1.0 0.8 * mu_large - 0.4 * mu_small kR 1.0 - 0.4 * mu_large 0.5 * mu_small # 限幅防止调节过猛 kQ np.clip(kQ, 0.5, 2.0) kR np.clip(kR, 0.5, 2.0) return kQ, kR这个实现看起来很简单但已经具备ANFIS的核心逻辑隶属度计算对应模糊化加权求和对应输出去模糊化。如果需要做完整的ANFIS训练可以在MATLAB里用模糊逻辑工具箱也可以用Python的anfis库来离线学习规则后件参数。我的建议是先用固定规则跑通整体控制链路确认自适应权重确实带来改善再上训练环节。每个周期得到kQ和kR之后更新MPC的代价矩阵Q_now Q * kQ R_now R * kR rebuilt_cost.update(QQ_now, RR_now)为了平滑最好对kQ做一个一阶低通滤波kQ_filtered 0.7 * kQ_filtered 0.3 * kQ不然ANFIS每次输出的波动会直接传导到MPC权重里控制量跟着一起抖。4.4 主循环整合串起来整个系统的控制周期我设成100毫秒对应10Hz的控制频率。每个周期依次执行读取状态、计算误差、运行ANFIS、更新权重、求解MPC、施加控制。while running: s get_vehicle_state() ref, ref_idx get_nearest_reference(s) e_y s[1] - ref[1] e_phi s[2] - ref[2] kQ, kR anfis_weights(e_y, e_phi, s[3]) set_mpc_weight_scale(kQ, kR) if use_nn_correction: pred_delta nn_predictor.predict(s, future_control) correct_dynamics(pred_delta) u solve_mpc(s, ref_block) apply_control(u[:, 0])主循环顺序不是随便定的。ANFIS必须在求解MPC之前执行因为它要修改本次优化用的代价矩阵NN校准也要在求解前完成因为它影响的是预测递推。如果顺序反了模块的作用就延迟了一个控制周期这在高速场景里影响很大。5. 实验效果对比与结果分析5.1 三种方案在典型工况下的表现我选了三种工况低速直线接弯、中速S弯、高速双移线。下表是一组在复现仿真里得到的示例数据只代表本次复现时的相对趋势不是通用结论方案平均横向误差(m)最大横向误差(m)转向抖动程度平均求解耗时(ms)固定参数MPC0.210.78中等35NN-MPC0.090.35偏大52ANFIS自适应MPC0.060.22很小41NN校准ANFIS自适应MPC0.050.18很小55固定参数MPC在直线段表现不错但进双移线的第二个弯道时横向误差迅速变大原因是运动学预测模型在高横摆工况下出现偏差。NN-MPC把平均误差降下来不少说明网络学到的补偿信息确实有效但它在入弯瞬间偶尔出现转向抖动我怀疑是训练数据里变道工况占比不够网络在边界区域的外推能力不足。ANFIS自适应MPC是表现最稳定的。误差大了它知道加重纠错权重误差小了知道让控制器“松油门”整体控制动作明显平滑。把NN校准和ANFIS结合起来以后误差还有小幅下降但计算耗时也上去了实时性要根据你的平台性能再权衡。5.2 为什么自适应权重比固定权重更有效这个结果不难理解。固定Q/R相当于给控制器固定了一个“性格”某些场景下过于急躁某些场景下过于迟钝。ANFIS相当于让控制器根据当前误差状态实时切换性格。误差大时kQ变大控制器更愿意牺牲控制平滑性去换跟踪精度误差小时kR变大控制器避免为微小偏差做出剧烈反应。从控制理论角度看这是一种增益调度思想只是调度规则不是人工分段查表而是用模糊推理平滑连接。相比直接给MPC换增益ANFIS的优势在于切换是连续的不会出现控制量突变。5.3 关于计算代价的一点提醒我看到很多人复现这类文章时最关注“神经网络多厉害”却忽略实时性。仿真里跑得动不代表实车控制器跑得动。NN-MPC如果把网络放进优化器里每步都要做网络正向传播甚至反向求导求解耗时很容易超过控制周期。我做软校准方案本质上是用一定的模型表达力换取实时性。如果你的目标是把方案部署到真实平台一定要先卡时间和算力预算再考虑模型的复杂度。6. 常见问题与调试避坑记录6.1 神经网络过拟合导致泛化不足这个坑我在复现时踩得最深。第一版只用双移线轨迹生成训练数据网络在双移线上预测效果很好换到S弯直接误差翻倍。原因很简单训练数据分布太窄网络只记住了双移线附近的状态转移特征。解决办法有两个思路。一是扩充数据来源把直线、S弯、不同车速、不同控制幅度都放进去必要时对状态量加高斯噪声。二是把网络定位调整为“残差学习”让它只学简化模型和精细模型之间的差而不是学习完整的状态转移。残差学习的训练目标更简单泛化性也更好。6.2 MPC求解失败或求解时间抖动MPC无解是路径跟踪复现里最常见的事故。常见诱因有三个预测时域太长导致优化变量太多、约束设置过紧、参考轨迹突然跳变。我调试时会把Np从30慢慢减到10每减一次测一次表现。约束方面方向盘角度限幅不能只给上限还要给增量约束用Δδ限制来平滑转向。还有一个很实用的兜底方案维护一个最优解缓存。上一周期求解成功后把控制序列存下来当前周期如果求解器报错或超时就退而求其次把缓存里的第二个控制量继续用一两拍同时给上层告警。这不能根治问题但能防止控制器在求解异常时直接崩溃。6.3 ANFIS调节过猛引起振荡ANFIS输出不加限制的话很容易变成高增益自适应。误差稍微大一点kQ就被推到很大下一拍误差变小kQ又回落MPC权重跟着来回跳控制量自然振荡。我加了三层防御输出限幅、低通滤波、规则死区。死区特别重要误差小于某个阈值时让kQ和kR基本不动避免MPC在已经跟踪得很好的情况下做无意义调整。6.4 量纲不一致导致代价函数失衡Q矩阵里的横向误差单位是米航向误差单位是弧度控制量里转向角是弧度加速度是米每平方秒。这些量纲放在同一个平方和代价函数里数值天然就有数量级差异。比如航向误差0.1弧度看起来不大但平方后和0.1米的横向误差平方一样贡献实际意义却完全不同。解决办法有两个一是把所有状态和控制量做归一化后再算代价二是在Q和R矩阵里按物理量纲换算权重。我一般先用归一化方式把状态量处理到类似量纲然后Q矩阵四个对角线元素设成不同比例最后看仿真曲线的灵敏度再微调。6.5 逐层验证模块加得越多效果越差怎么办这是很打击人的情况加NN效果差再加ANFIS效果更差。我的经验是不要同时开两个模块调试必须逐层验证。先只跑标准MPC调好Q/R/Np/Nc记录基线指标加入ANFIS查看误差是否下降、控制是否平滑跑稳之后再加NN校准每次只改变一个变量。如果加某个模块后指标集体变差先看这个模块的输入输出是否符合预期。ANFIS输出饱和了吗NN训练集和验证集分布一致吗大概率不是控制框架的问题而是模块本身哪一步没做对。最后分享一点个人体会这次复现下来我最深的感受是这类“神经网络MPC”的工程项目真正的难点不在网络结构有多花哨而在怎么把学习和优化两个体系拼接得稳定可靠。直接拿一个大网络替换MPC预测模型听起来很前卫实际调试会很痛苦先做离线训练再在线上做软校准反而效果又快又稳。另外数据驱动的模块最怕训练数据和实际工况不一致所以数据采集时别只图省事跑一条轨迹速度、曲率、噪声都要铺开。如果你也正在做类似的自适应MPC复现我建议严格按“基线MPC → ANFIS调权 → NN校准”的顺序推进每一步都用误差曲线和控制平滑度来验证收益这样整个系统就算后面加了再多的模块也不会失控。
