简介这份资源面向无人驾驶与自动控制方向的学习者和开发者提供模型预测控制MPC的完整程序代码实现帮助理解车辆路径规划、速度控制与转向操作中的预测优化逻辑。压缩包共16个文件约38KB以13个m脚本为主体辅以2个txt说明文档和1个mdl仿真模型覆盖车辆动力学建模、预测框架、优化求解、滚动优化及仿真测试等关键环节并涉及Pacejka轮胎模型等具体内容。目前已有1069人学习下载适合具备一定控制理论基础与编程能力、希望深入掌握MPC在无人驾驶中应用的读者。通过研读这些代码可对照章节结构梳理从车辆模型到控制更新的完整流程理解约束处理、参数调节与仿真验证方法为实际系统设计提供可复用的参考实现与排错思路。1. 从一段仿真轨迹说起这套 MPC 代码到底能跑出什么如果你手头正好有一份无人驾驶车辆模型预测控制程序代码第一反应大概率是先别管理论能不能让车在仿真里沿着参考轨迹跑起来。这套代码解决的就是这件事——它把车辆动力学模型、预测模型、滚动优化和约束处理串成一条完整的控制链路输入一条期望轨迹输出前轮转角和纵向加速度让车辆在双移线、换道、圆形轨迹这些典型工况下跟踪上去。适合两类人一类是刚接触模型预测控制、想找个能跑通的工程实现对照论文公式的另一类是已经在做底盘控制或轨迹跟踪想拿一套可改参数、可换模型的骨架做二次开发的。代码本身不依赖实车纯仿真环境就能验证这对没有测试车辆的开发者来说省掉了最大的门槛。2. 先看清骨架MPC 控制器的模块划分与数据流2.1 从参考轨迹到控制量的完整链路这套代码的顶层逻辑并不复杂但模块之间的数据流如果没理清改起来很容易牵一发动全身。我一般会把它拆成四层轨迹层、预测层、优化层、执行层。轨迹层负责生成或读取参考路径输出的是离散的 (x, y, θ, v) 序列预测层拿当前状态和参考轨迹用车辆模型往前推 N 步得到预测时域内的状态偏差优化层把偏差、控制增量、约束条件组装成二次规划问题交给求解器执行层只取优化结果的第一个控制量作用于车辆然后进入下一个控制周期。这个“只取第一步”的滚动机制是 MPC 的核心特征也是很多人第一次看代码时容易忽略的地方——明明算了 N 步为什么只用第一步因为下一步状态更新后预测会重新做一遍这就是滚动优化的含义。2.2 目录结构里藏着的工程习惯一份能直接跑的 MPC 代码目录通常不会太深但每个文件夹的职责很明确。常见做法是分成model/、controller/、simulation/、utils/四块。model/放车辆动力学或运动学模型包括状态方程和参数controller/放 MPC 主循环、预测模型构建、约束组装和目标函数simulation/放工况脚本和可视化utils/放参考轨迹生成、矩阵工具、绘图函数。我见过不少初学者把模型和控制器混在一个文件里跑是能跑但想换一个车辆模型时就得动主循环改起来很痛苦。所以如果你拿到的代码结构清晰先别急着改参数花十分钟把每个文件被谁调用、输出什么搞清楚后面能省很多时间。2.3 车辆模型运动学还是动力学选哪个代码里最核心的底层模块就是车辆模型。运动学模型简单状态量少适合低速和路径跟踪验证动力学模型考虑轮胎侧偏、横摆动力学适合高速和极限工况。这套代码如果面向无人驾驶场景大概率用的是动力学模型至少也是带侧偏刚度的单轨模型。常见做法是用状态空间形式写# 单轨车辆动力学模型离散化示例 # 状态量: [x, y, theta, vx, vy, omega] # 控制量: [delta_f, a_x] def vehicle_dynamics(state, control, dt, params): x, y, theta, vx, vy, omega state delta_f, a_x control m, Iz, lf, lr, Cf, Cr params[m], params[Iz], params[lf], params[lr], params[Cf], params[Cr] # 纵向和侧向速度在车身坐标系下的变化 vx_dot a_x vy * omega vy_dot (Cf * (delta_f - (vy lf * omega) / max(vx, 0.1)) Cr * (-(vy - lr * omega) / max(vx, 0.1))) / m - vx * omega omega_dot (lf * Cf * (delta_f - (vy lf * omega) / max(vx, 0.1)) - lr * Cr * (-(vy - lr * omega) / max(vx, 0.1))) / Iz # 全局坐标更新 x_dot vx * np.cos(theta) - vy * np.sin(theta) y_dot vx * np.sin(theta) vy * np.cos(theta) theta_dot omega return np.array([x_dot, y_dot, theta_dot, vx_dot, vy_dot, omega_dot])这段代码里max(vx, 0.1)是为了防止低速时除零这是工程实现里很常见的一个保护。参数Cf、Cr是前后轮侧偏刚度lf、lr是质心到前后轴的距离m是质量Iz是横摆转动惯量。这些参数直接决定模型精度如果你拿到的代码跑出来轨迹发散先检查这几个值是不是和你的车辆匹配。离散化一般用前向欧拉或四阶龙格库塔前向欧拉简单但步长不能太大龙格库塔精度高但计算量翻倍仿真里通常 0.02 到 0.05 秒的步长用前向欧拉就够了。3. 把代码跑起来环境、参数与第一个仿真工况3.1 依赖安装与求解器选择这套代码大概率是 Python 写的核心依赖就几个NumPy 做矩阵运算SciPy 或 CVXPY 做二次规划Matplotlib 做可视化。如果用的是 OSQP 或 qpOASES 这类专用求解器安装会稍微麻烦一点但速度比通用求解器快不少。我一般会先建一个干净的虚拟环境然后按顺序装# 创建虚拟环境并安装核心依赖 python -m venv mpc_env source mpc_env/bin/activate # Windows 下用 mpc_env\Scripts\activate pip install numpy scipy matplotlib pip install osqp # 如果代码用的是 OSQP 求解器装完之后先别急着跑主程序用python -c import numpy, scipy, osqp确认没有报错。如果代码里用的是 CVXPY那还要装cvxpy但 CVXPY 在 Windows 上偶尔会有编译器依赖问题遇到的话换 OSQP 或直接上 Linux 环境会省事很多。这一步的坑在于求解器版本和代码里调用的 API 不匹配比如 OSQP 新版本改了接口老代码可能直接报AttributeError这时候要么降版本要么改调用方式。3.2 关键参数怎么改预测时域、控制时域、权重参数文件通常是config.py或params.py里面最需要关注的是预测时域Np、控制时域Nc、采样时间dt和权重矩阵Q、R。Np决定往前看多远太小了跟踪滞后太大了计算量上去而且对模型误差更敏感Nc一般小于等于Np控制时域太长会导致优化变量爆炸。我一般会从Np20、Nc5、dt0.05开始试这个组合在低速路径跟踪里比较稳。权重矩阵Q管状态偏差R管控制量大小Q给大了跟踪紧但控制抖R给大了控制平滑但跟踪松。下面是一个典型的参数配置# MPC 参数配置示例 params { Np: 20, # 预测时域步数 Nc: 5, # 控制时域步数 dt: 0.05, # 采样时间单位秒 Q: np.diag([10, 10, 1, 1, 1, 1]), # 状态偏差权重 R: np.diag([0.1, 0.1]), # 控制量权重 delta_f_max: 0.5, # 前轮转角上限单位弧度 a_x_max: 2.0, # 纵向加速度上限 }改完参数后先跑一个最简单的工况比如直线跟踪或者小曲率圆形轨迹确认车辆能跟上再换双移线。如果一上来就跑双移线参数不对的话轨迹会画得乱七八糟你根本分不清是模型问题还是参数问题。3.3 跑通第一个工况从直线到双移线仿真脚本一般长这样初始化状态生成参考轨迹进入循环每个周期调用 MPC 控制器更新状态记录数据最后画图。下面是一个最小可运行的主循环# 仿真主循环示例 state np.array([0, 0, 0, 5.0, 0, 0]) # 初始状态: x, y, theta, vx, vy, omega trajectory generate_reference_trajectory() # 生成参考轨迹 history [] for step in range(total_steps): # 获取当前参考点 ref get_reference_point(trajectory, state, step) # 调用 MPC 控制器求解 control mpc_controller(state, ref, params) # 更新车辆状态 state update_state(state, control, params[dt]) history.append(state.copy()) plot_results(history, trajectory)这段代码里mpc_controller是核心它内部做了预测、优化、取第一步控制量三件事。update_state用车辆模型做积分。跑完之后看两张图一张是 x-y 平面轨迹对比一张是控制量随时间变化。如果轨迹跟踪上了但控制量抖得厉害回去调R如果轨迹切内弯或者跟不上调Q或者增大Np。双移线工况对 MPC 来说是个不错的测试因为它同时考验跟踪精度和控制平滑性。4. 核心代码拆解预测模型、目标函数与约束组装4.1 预测模型怎么从连续方程变成离散矩阵MPC 的预测模型本质上是把连续状态方程离散化后写成矩阵形式方便一次性算出未来 N 步的状态。常见做法是用前向欧拉离散化然后构造预测矩阵。假设离散后的状态空间是x(k1) A x(k) B u(k)那预测方程可以写成# 构造预测矩阵 def build_prediction_matrices(A, B, Np, Nc): n A.shape[0] # 状态维度 m B.shape[1] # 控制维度 # 构造增广状态矩阵 A_pow [np.eye(n)] for i in range(1, Np 1): A_pow.append(A_pow[-1] A) # 构造预测矩阵 Phi 和 Gamma Phi np.vstack([A_pow[i] for i in range(1, Np 1)]) Gamma np.zeros((Np * n, Nc * m)) for i in range(Np): for j in range(min(i 1, Nc)): Gamma[i * n:(i 1) * n, j * m:(j 1) * m] A_pow[i - j] B return Phi, GammaPhi描述初始状态对预测状态的影响Gamma描述控制序列对预测状态的影响。有了这两个矩阵预测状态就可以写成Y Phi x0 Gamma U其中U是待优化的控制序列。这一步是 MPC 从理论到代码的关键转换很多论文里一笔带过但实际写代码时矩阵维度对不上是家常便饭建议构造完先打印形状确认。4.2 目标函数偏差和控制增量怎么加权目标函数通常是最小化状态偏差和控制增量的加权平方和。写成二次规划标准形式就是min 0.5 U^T H U f^T U。下面是对应的代码# 构造二次规划的目标函数矩阵 def build_cost_function(Phi, Gamma, x0, ref, Q, R, Np, Nc): n Q.shape[0] m R.shape[0] # 构造块对角权重矩阵 Q_bar np.kron(np.eye(Np), Q) R_bar np.kron(np.eye(Nc), R) # 预测偏差 E Phi x0 - ref.flatten() # Hessian 矩阵和梯度向量 H Gamma.T Q_bar Gamma R_bar f Gamma.T Q_bar E return H, fH和f直接丢给求解器就能解出最优控制序列。这里有个细节ref的维度和Phi x0要对齐否则E算出来是错的。我见过有人把参考轨迹的维度搞错结果车往反方向跑排查了半天才发现是ref少了一维。4.3 约束处理前轮转角、加速度和终端约束约束是 MPC 区别于 LQR 的核心优势之一。前轮转角有物理极限加速度有舒适性上限这些都要写成不等式约束lb A_ineq U ub。代码里通常这样组装# 构造控制量约束 def build_constraints(Nc, delta_f_max, a_x_max): m 2 # 控制量维度 # 控制量上下界 lb np.tile([-delta_f_max, -a_x_max], Nc) ub np.tile([delta_f_max, a_x_max], Nc) # 控制增量约束可选 A_ineq np.eye(Nc * m) return A_ineq, lb, ub如果代码里还加了控制增量约束那A_ineq会变成一个差分矩阵lb和ub对应增量上下限。终端约束一般是在预测时域最后一步加一个状态约束保证稳定性但工程实现里为了简化很多人只加控制量约束靠增大Np来近似终端效果。如果你拿到的代码有终端约束别急着删先跑一下对比看看稳定性有没有差别。5. 避坑与排查仿真跑不通时先看这几处5.1 轨迹发散或车辆画龙现象是车辆轨迹越来越偏或者左右摆动。原因通常是权重矩阵Q和R比例不对或者预测时域太短。解决方法是先把R调大让控制平滑再逐步增大Q提高跟踪精度同时把Np从 20 加到 30 试试。如果还不行检查车辆模型参数是不是和参考轨迹的生成模型不一致——用运动学模型生成轨迹、用动力学模型跟踪本身就有模型失配低速下不明显高速下会放大。5.2 求解器报 infeasible现象是求解器返回不可行或者直接抛异常。原因一般是约束太紧比如前轮转角上限设成了 0.1 弧度而参考轨迹需要更大的转角才能跟上。解决方法是先放宽约束跑一遍确认能跑通后再逐步收紧找到临界值。另外检查初始状态是不是在约束范围内如果初始状态本身就违反约束求解器也会报 infeasible。5.3 控制量抖动严重现象是前轮转角或加速度曲线毛刺很多。原因可能是dt太小导致数值噪声放大或者R给得太小。解决方法是适当增大dt到 0.05 或 0.1同时增大R。如果代码里用了控制增量作为优化变量检查增量权重是不是太小。还有一种可能是参考轨迹本身不平滑先对参考轨迹做一次低通滤波再喂给 MPC。5.4 仿真速度慢到无法接受现象是跑一个工况要等好几分钟。原因通常是求解器每次都在重新构造矩阵或者用了通用求解器解小规模问题。解决方法是把Phi、Gamma、H这些不随周期变化的矩阵提前算好循环里只更新和状态相关的部分。如果用的是 CVXPY换成 OSQP 或 qpOASES 会有明显提升。另外Np和Nc别设太大仿真验证阶段Np20、Nc5足够了。5.5 换工况后跟踪效果骤降现象是直线跑得好好的换双移线就跟踪不上。原因是双移线曲率变化快对预测时域和控制时域的要求不同。解决方法是针对不同工况调参或者加一个曲率前馈。常见做法是把参考轨迹的曲率也作为前馈项加到控制量里这样 MPC 只需要处理偏差负担小很多。如果代码里没有前馈通道可以在控制量输出后手动叠加一个曲率相关的转角。6. 二次开发换模型、加约束、接实车的改造思路拿到这套代码之后最值钱的二次开发方向有三个换车辆模型、加新约束、往实车迁移。换模型最简单把model/里的状态方程和参数替换掉保持输入输出接口不变就行。比如从单轨动力学换成双轨模型状态量从 6 维变成更多维预测矩阵的构造代码不用动只要A、B矩阵维度对得上。加约束稍微麻烦一点要在build_constraints里增加不等式同时确认求解器支持。比如加一个侧向加速度约束|a_y| a_y_max需要把侧向加速度写成状态和控制的线性组合再塞进约束矩阵。往实车迁移是另一个量级的工程。仿真里状态量直接能拿到实车上很多状态要靠估计比如侧向速度vy和横摆角速度omega通常用卡尔曼滤波或者观测器补上。控制量输出也要经过执行器接口前轮转角要转成 CAN 报文加速度要转成驱动和制动扭矩。我一般会先在仿真里把控制周期和实车对齐比如实车 20ms 一个周期仿真也改成 0.02 秒看看求解时间够不够。如果单次求解超过 10ms实车上就会丢周期这时候要么简化模型要么换更快的求解器。验证改造是否成功我习惯用三步第一步同一个工况下和原始代码对比轨迹偏差在可接受范围内第二步换两到三个不同曲率的工况确认没有工况特异性崩溃第三步把控制量打印出来看有没有超出物理极限。这三步走完基本能判断改造是不是稳了。从那以后我每次改 MPC 代码都会先把预测矩阵的形状打印一遍再跑一个最小直线工况确认没问题才动其他参数。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/18 15:04:18 · 阅读 1.2k 无人驾驶MPC模型预测控制代码实战：从仿真轨迹到轨迹跟踪
简介这份资源面向无人驾驶与自动控制方向的学习者和开发者提供模型预测控制MPC的完整程序代码实现帮助理解车辆路径规划、速度控制与转向操作中的预测优化逻辑。压缩包共16个文件约38KB以13个m脚本为主体辅以2个txt说明文档和1个mdl仿真模型覆盖车辆动力学建模、预测框架、优化求解、滚动优化及仿真测试等关键环节并涉及Pacejka轮胎模型等具体内容。目前已有1069人学习下载适合具备一定控制理论基础与编程能力、希望深入掌握MPC在无人驾驶中应用的读者。通过研读这些代码可对照章节结构梳理从车辆模型到控制更新的完整流程理解约束处理、参数调节与仿真验证方法为实际系统设计提供可复用的参考实现与排错思路。1. 从一段仿真轨迹说起这套 MPC 代码到底能跑出什么如果你手头正好有一份无人驾驶车辆模型预测控制程序代码第一反应大概率是先别管理论能不能让车在仿真里沿着参考轨迹跑起来。这套代码解决的就是这件事——它把车辆动力学模型、预测模型、滚动优化和约束处理串成一条完整的控制链路输入一条期望轨迹输出前轮转角和纵向加速度让车辆在双移线、换道、圆形轨迹这些典型工况下跟踪上去。适合两类人一类是刚接触模型预测控制、想找个能跑通的工程实现对照论文公式的另一类是已经在做底盘控制或轨迹跟踪想拿一套可改参数、可换模型的骨架做二次开发的。代码本身不依赖实车纯仿真环境就能验证这对没有测试车辆的开发者来说省掉了最大的门槛。2. 先看清骨架MPC 控制器的模块划分与数据流2.1 从参考轨迹到控制量的完整链路这套代码的顶层逻辑并不复杂但模块之间的数据流如果没理清改起来很容易牵一发动全身。我一般会把它拆成四层轨迹层、预测层、优化层、执行层。轨迹层负责生成或读取参考路径输出的是离散的 (x, y, θ, v) 序列预测层拿当前状态和参考轨迹用车辆模型往前推 N 步得到预测时域内的状态偏差优化层把偏差、控制增量、约束条件组装成二次规划问题交给求解器执行层只取优化结果的第一个控制量作用于车辆然后进入下一个控制周期。这个“只取第一步”的滚动机制是 MPC 的核心特征也是很多人第一次看代码时容易忽略的地方——明明算了 N 步为什么只用第一步因为下一步状态更新后预测会重新做一遍这就是滚动优化的含义。2.2 目录结构里藏着的工程习惯一份能直接跑的 MPC 代码目录通常不会太深但每个文件夹的职责很明确。常见做法是分成model/、controller/、simulation/、utils/四块。model/放车辆动力学或运动学模型包括状态方程和参数controller/放 MPC 主循环、预测模型构建、约束组装和目标函数simulation/放工况脚本和可视化utils/放参考轨迹生成、矩阵工具、绘图函数。我见过不少初学者把模型和控制器混在一个文件里跑是能跑但想换一个车辆模型时就得动主循环改起来很痛苦。所以如果你拿到的代码结构清晰先别急着改参数花十分钟把每个文件被谁调用、输出什么搞清楚后面能省很多时间。2.3 车辆模型运动学还是动力学选哪个代码里最核心的底层模块就是车辆模型。运动学模型简单状态量少适合低速和路径跟踪验证动力学模型考虑轮胎侧偏、横摆动力学适合高速和极限工况。这套代码如果面向无人驾驶场景大概率用的是动力学模型至少也是带侧偏刚度的单轨模型。常见做法是用状态空间形式写# 单轨车辆动力学模型离散化示例 # 状态量: [x, y, theta, vx, vy, omega] # 控制量: [delta_f, a_x] def vehicle_dynamics(state, control, dt, params): x, y, theta, vx, vy, omega state delta_f, a_x control m, Iz, lf, lr, Cf, Cr params[m], params[Iz], params[lf], params[lr], params[Cf], params[Cr] # 纵向和侧向速度在车身坐标系下的变化 vx_dot a_x vy * omega vy_dot (Cf * (delta_f - (vy lf * omega) / max(vx, 0.1)) Cr * (-(vy - lr * omega) / max(vx, 0.1))) / m - vx * omega omega_dot (lf * Cf * (delta_f - (vy lf * omega) / max(vx, 0.1)) - lr * Cr * (-(vy - lr * omega) / max(vx, 0.1))) / Iz # 全局坐标更新 x_dot vx * np.cos(theta) - vy * np.sin(theta) y_dot vx * np.sin(theta) vy * np.cos(theta) theta_dot omega return np.array([x_dot, y_dot, theta_dot, vx_dot, vy_dot, omega_dot])这段代码里max(vx, 0.1)是为了防止低速时除零这是工程实现里很常见的一个保护。参数Cf、Cr是前后轮侧偏刚度lf、lr是质心到前后轴的距离m是质量Iz是横摆转动惯量。这些参数直接决定模型精度如果你拿到的代码跑出来轨迹发散先检查这几个值是不是和你的车辆匹配。离散化一般用前向欧拉或四阶龙格库塔前向欧拉简单但步长不能太大龙格库塔精度高但计算量翻倍仿真里通常 0.02 到 0.05 秒的步长用前向欧拉就够了。3. 把代码跑起来环境、参数与第一个仿真工况3.1 依赖安装与求解器选择这套代码大概率是 Python 写的核心依赖就几个NumPy 做矩阵运算SciPy 或 CVXPY 做二次规划Matplotlib 做可视化。如果用的是 OSQP 或 qpOASES 这类专用求解器安装会稍微麻烦一点但速度比通用求解器快不少。我一般会先建一个干净的虚拟环境然后按顺序装# 创建虚拟环境并安装核心依赖 python -m venv mpc_env source mpc_env/bin/activate # Windows 下用 mpc_env\Scripts\activate pip install numpy scipy matplotlib pip install osqp # 如果代码用的是 OSQP 求解器装完之后先别急着跑主程序用python -c import numpy, scipy, osqp确认没有报错。如果代码里用的是 CVXPY那还要装cvxpy但 CVXPY 在 Windows 上偶尔会有编译器依赖问题遇到的话换 OSQP 或直接上 Linux 环境会省事很多。这一步的坑在于求解器版本和代码里调用的 API 不匹配比如 OSQP 新版本改了接口老代码可能直接报AttributeError这时候要么降版本要么改调用方式。3.2 关键参数怎么改预测时域、控制时域、权重参数文件通常是config.py或params.py里面最需要关注的是预测时域Np、控制时域Nc、采样时间dt和权重矩阵Q、R。Np决定往前看多远太小了跟踪滞后太大了计算量上去而且对模型误差更敏感Nc一般小于等于Np控制时域太长会导致优化变量爆炸。我一般会从Np20、Nc5、dt0.05开始试这个组合在低速路径跟踪里比较稳。权重矩阵Q管状态偏差R管控制量大小Q给大了跟踪紧但控制抖R给大了控制平滑但跟踪松。下面是一个典型的参数配置# MPC 参数配置示例 params { Np: 20, # 预测时域步数 Nc: 5, # 控制时域步数 dt: 0.05, # 采样时间单位秒 Q: np.diag([10, 10, 1, 1, 1, 1]), # 状态偏差权重 R: np.diag([0.1, 0.1]), # 控制量权重 delta_f_max: 0.5, # 前轮转角上限单位弧度 a_x_max: 2.0, # 纵向加速度上限 }改完参数后先跑一个最简单的工况比如直线跟踪或者小曲率圆形轨迹确认车辆能跟上再换双移线。如果一上来就跑双移线参数不对的话轨迹会画得乱七八糟你根本分不清是模型问题还是参数问题。3.3 跑通第一个工况从直线到双移线仿真脚本一般长这样初始化状态生成参考轨迹进入循环每个周期调用 MPC 控制器更新状态记录数据最后画图。下面是一个最小可运行的主循环# 仿真主循环示例 state np.array([0, 0, 0, 5.0, 0, 0]) # 初始状态: x, y, theta, vx, vy, omega trajectory generate_reference_trajectory() # 生成参考轨迹 history [] for step in range(total_steps): # 获取当前参考点 ref get_reference_point(trajectory, state, step) # 调用 MPC 控制器求解 control mpc_controller(state, ref, params) # 更新车辆状态 state update_state(state, control, params[dt]) history.append(state.copy()) plot_results(history, trajectory)这段代码里mpc_controller是核心它内部做了预测、优化、取第一步控制量三件事。update_state用车辆模型做积分。跑完之后看两张图一张是 x-y 平面轨迹对比一张是控制量随时间变化。如果轨迹跟踪上了但控制量抖得厉害回去调R如果轨迹切内弯或者跟不上调Q或者增大Np。双移线工况对 MPC 来说是个不错的测试因为它同时考验跟踪精度和控制平滑性。4. 核心代码拆解预测模型、目标函数与约束组装4.1 预测模型怎么从连续方程变成离散矩阵MPC 的预测模型本质上是把连续状态方程离散化后写成矩阵形式方便一次性算出未来 N 步的状态。常见做法是用前向欧拉离散化然后构造预测矩阵。假设离散后的状态空间是x(k1) A x(k) B u(k)那预测方程可以写成# 构造预测矩阵 def build_prediction_matrices(A, B, Np, Nc): n A.shape[0] # 状态维度 m B.shape[1] # 控制维度 # 构造增广状态矩阵 A_pow [np.eye(n)] for i in range(1, Np 1): A_pow.append(A_pow[-1] A) # 构造预测矩阵 Phi 和 Gamma Phi np.vstack([A_pow[i] for i in range(1, Np 1)]) Gamma np.zeros((Np * n, Nc * m)) for i in range(Np): for j in range(min(i 1, Nc)): Gamma[i * n:(i 1) * n, j * m:(j 1) * m] A_pow[i - j] B return Phi, GammaPhi描述初始状态对预测状态的影响Gamma描述控制序列对预测状态的影响。有了这两个矩阵预测状态就可以写成Y Phi x0 Gamma U其中U是待优化的控制序列。这一步是 MPC 从理论到代码的关键转换很多论文里一笔带过但实际写代码时矩阵维度对不上是家常便饭建议构造完先打印形状确认。4.2 目标函数偏差和控制增量怎么加权目标函数通常是最小化状态偏差和控制增量的加权平方和。写成二次规划标准形式就是min 0.5 U^T H U f^T U。下面是对应的代码# 构造二次规划的目标函数矩阵 def build_cost_function(Phi, Gamma, x0, ref, Q, R, Np, Nc): n Q.shape[0] m R.shape[0] # 构造块对角权重矩阵 Q_bar np.kron(np.eye(Np), Q) R_bar np.kron(np.eye(Nc), R) # 预测偏差 E Phi x0 - ref.flatten() # Hessian 矩阵和梯度向量 H Gamma.T Q_bar Gamma R_bar f Gamma.T Q_bar E return H, fH和f直接丢给求解器就能解出最优控制序列。这里有个细节ref的维度和Phi x0要对齐否则E算出来是错的。我见过有人把参考轨迹的维度搞错结果车往反方向跑排查了半天才发现是ref少了一维。4.3 约束处理前轮转角、加速度和终端约束约束是 MPC 区别于 LQR 的核心优势之一。前轮转角有物理极限加速度有舒适性上限这些都要写成不等式约束lb A_ineq U ub。代码里通常这样组装# 构造控制量约束 def build_constraints(Nc, delta_f_max, a_x_max): m 2 # 控制量维度 # 控制量上下界 lb np.tile([-delta_f_max, -a_x_max], Nc) ub np.tile([delta_f_max, a_x_max], Nc) # 控制增量约束可选 A_ineq np.eye(Nc * m) return A_ineq, lb, ub如果代码里还加了控制增量约束那A_ineq会变成一个差分矩阵lb和ub对应增量上下限。终端约束一般是在预测时域最后一步加一个状态约束保证稳定性但工程实现里为了简化很多人只加控制量约束靠增大Np来近似终端效果。如果你拿到的代码有终端约束别急着删先跑一下对比看看稳定性有没有差别。5. 避坑与排查仿真跑不通时先看这几处5.1 轨迹发散或车辆画龙现象是车辆轨迹越来越偏或者左右摆动。原因通常是权重矩阵Q和R比例不对或者预测时域太短。解决方法是先把R调大让控制平滑再逐步增大Q提高跟踪精度同时把Np从 20 加到 30 试试。如果还不行检查车辆模型参数是不是和参考轨迹的生成模型不一致——用运动学模型生成轨迹、用动力学模型跟踪本身就有模型失配低速下不明显高速下会放大。5.2 求解器报 infeasible现象是求解器返回不可行或者直接抛异常。原因一般是约束太紧比如前轮转角上限设成了 0.1 弧度而参考轨迹需要更大的转角才能跟上。解决方法是先放宽约束跑一遍确认能跑通后再逐步收紧找到临界值。另外检查初始状态是不是在约束范围内如果初始状态本身就违反约束求解器也会报 infeasible。5.3 控制量抖动严重现象是前轮转角或加速度曲线毛刺很多。原因可能是dt太小导致数值噪声放大或者R给得太小。解决方法是适当增大dt到 0.05 或 0.1同时增大R。如果代码里用了控制增量作为优化变量检查增量权重是不是太小。还有一种可能是参考轨迹本身不平滑先对参考轨迹做一次低通滤波再喂给 MPC。5.4 仿真速度慢到无法接受现象是跑一个工况要等好几分钟。原因通常是求解器每次都在重新构造矩阵或者用了通用求解器解小规模问题。解决方法是把Phi、Gamma、H这些不随周期变化的矩阵提前算好循环里只更新和状态相关的部分。如果用的是 CVXPY换成 OSQP 或 qpOASES 会有明显提升。另外Np和Nc别设太大仿真验证阶段Np20、Nc5足够了。5.5 换工况后跟踪效果骤降现象是直线跑得好好的换双移线就跟踪不上。原因是双移线曲率变化快对预测时域和控制时域的要求不同。解决方法是针对不同工况调参或者加一个曲率前馈。常见做法是把参考轨迹的曲率也作为前馈项加到控制量里这样 MPC 只需要处理偏差负担小很多。如果代码里没有前馈通道可以在控制量输出后手动叠加一个曲率相关的转角。6. 二次开发换模型、加约束、接实车的改造思路拿到这套代码之后最值钱的二次开发方向有三个换车辆模型、加新约束、往实车迁移。换模型最简单把model/里的状态方程和参数替换掉保持输入输出接口不变就行。比如从单轨动力学换成双轨模型状态量从 6 维变成更多维预测矩阵的构造代码不用动只要A、B矩阵维度对得上。加约束稍微麻烦一点要在build_constraints里增加不等式同时确认求解器支持。比如加一个侧向加速度约束|a_y| a_y_max需要把侧向加速度写成状态和控制的线性组合再塞进约束矩阵。往实车迁移是另一个量级的工程。仿真里状态量直接能拿到实车上很多状态要靠估计比如侧向速度vy和横摆角速度omega通常用卡尔曼滤波或者观测器补上。控制量输出也要经过执行器接口前轮转角要转成 CAN 报文加速度要转成驱动和制动扭矩。我一般会先在仿真里把控制周期和实车对齐比如实车 20ms 一个周期仿真也改成 0.02 秒看看求解时间够不够。如果单次求解超过 10ms实车上就会丢周期这时候要么简化模型要么换更快的求解器。验证改造是否成功我习惯用三步第一步同一个工况下和原始代码对比轨迹偏差在可接受范围内第二步换两到三个不同曲率的工况确认没有工况特异性崩溃第三步把控制量打印出来看有没有超出物理极限。这三步走完基本能判断改造是不是稳了。从那以后我每次改 MPC 代码都会先把预测矩阵的形状打印一遍再跑一个最小直线工况确认没问题才动其他参数。希望帮到你。本文还有配套的精品资源点击获取