简介：这份资料是北京理工大学「无人驾驶车辆模型预测控制」第二版配套程序代码，面向智能交通、自动驾驶方向的学习者与研究人员，用于理解并实践MPC在无人车路径规划、速度控制与避障中的落地细节。压缩包共349个文件，约1.52MB，以h头文件、m脚本、mat数据、c源码、obj目标文件、lib库、mak工程配置及mdl/sim仿真模型为主，另含bat批处理、mexw64与mol等文件，覆盖车辆动力学建模、预测模型、在线优化求解、控制律生成与仿真验证等模块。内容预览可见双积分速度控制、非线性设定距离、MISO与噪声场景等MPC仿真入口，便于对照代码梳理算法结构。目前已有780人学习下载，适合希望从理论走向工程实现、锻炼建模与优化求解能力的读者研读与复现。
1. 从公式到方向盘：模型预测控制到底在车里干什么
很多人第一次翻开《无人驾驶车辆模型预测控制》这类教材，翻到第二版新增的动力学推导部分就卡住了——公式能看懂，但不知道这些矩阵到底对应车上哪个零件。我当年也是这么过来的，后来在仿真环境里把代码跑通、看着车辆轨迹从画面上歪歪扭扭地收敛到参考线，才算真正理解 MPC 在无人驾驶里的位置。它本质上是一个"滚动优化"的控制器：每个控制周期用当前状态往前预测未来 N 步，解一个带约束的优化问题，只取第一步控制量执行，下一周期再重新来一遍。这套逻辑解决的是轨迹跟踪里的核心矛盾——既要跟得紧，又要满足前轮转角限制、加速度限制这些物理约束。适合谁看？做自动驾驶规控方向的工程师、车辆工程专业做毕设的学生、以及想把 MPC 从 MATLAB 搬到 Python 或 C++ 的开发者。下面我按"建模→离散化→求解→调参→避坑"的顺序，把这条链路拆开讲。
2. 车辆运动学与动力学建模：两种模型怎么选、参数怎么定
2.1 运动学自行车模型的最小实现
做 MPC 的第一步是选模型。运动学自行车模型把车辆简化为前后两个轮子，假设轮胎无侧滑，状态量取[x, y, θ]，控制量取[v, δ]（速度与前轮转角）。它的连续时间微分方程是：
ẋ = v·cos(θ) ẏ = v·sin(θ) θ̇ = v·tan(δ)/L其中 L 是轴距。这个模型在低速（一般 5 m/s 以下）场景足够用，泊车、园区低速巡航都能跑。我一般会先用它把 MPC 框架搭起来，因为状态量少、雅可比矩阵好推、调试时容易定位问题。下面是一个 Python 里做前向欧拉离散化的最小代码：
import numpy as np def kinematic_model(state, control, L, dt): """ 运动学自行车模型离散化 state: [x, y, theta] control: [v, delta] L: 轴距 (m) dt: 采样周期 (s) """ x, y, theta = state v, delta = control # 前向欧拉，dt 较小时精度够用 x_next = x + v * np.cos(theta) * dt y_next = y + v * np.sin(theta) * dt theta_next = theta + v * np.tan(delta) / L * dt return np.array([x_next, y_next, theta_next])逻辑说明：这里用前向欧拉而不是龙格库塔，是因为 MPC 内部要反复调用这个函数做预测，计算量敏感；当 dt 取 0.05~0.1 s 时，欧拉法的截断误差在厘米级，对轨迹跟踪影响可忽略。参数上，L 必须和实际车辆一致，差 0.1 m 在高速下就会带来明显跟踪偏差；dt 不建议大于 0.1 s，否则离散化误差会累积。
2.2 动力学模型什么时候必须上
当车速超过 10 m/s，或者路面附着系数低、需要处理侧偏时，运动学模型就不够了。第二版教材里重点补充了动力学模型，状态量扩展到[x, y, θ, vx, vy, ω]，控制量仍是[前轮转角, 纵向力]。它的核心是把轮胎侧偏力用线性或魔术公式建模，再代入车身动力学方程。选型判断标准很简单：如果你的仿真场景里出现了明显甩尾、或者参考轨迹曲率对应的侧向加速度超过 0.3g，就该换动力学模型。代价是状态维度翻倍，QP 求解时间大概增加 2~3 倍，实时性要重新评估。
2.3 参数标定的三个关键量
不管用哪种模型，有三个参数必须实测或查车辆手册确认：轴距 L、质心到前轴距离 a、质心到后轴距离 b。很多人直接拿默认值凑，结果仿真里跟得好好的，上车就偏。我一般会做一组定圆测试，让车以固定转角跑一圈，用轨迹半径反推等效轴距，误差能压到 2% 以内。
3. 把优化问题写成代码：目标函数、约束与求解器落地
3.1 目标函数的三项权重怎么配
MPC 的目标函数通常由三部分组成：跟踪误差项、控制量变化率项、控制量幅值项。写成矩阵形式是：
J = Σ (x_k - x_ref)ᵀ Q (x_k - x_ref) + Σ u_kᵀ R u_k + Σ Δu_kᵀ S Δu_kQ 管跟踪精度，R 管控制量大小，S 管控制平滑度。经验配比：先设 Q 为单位阵，R 取 0.1~1，S 取 R 的 5~10 倍。如果轨迹跟踪出现高频抖动，优先加大 S；如果转弯时明显切内弯，加大 Q 里对应横向误差的权重。下面是一个用cvxpy搭 QP 的骨架：
import cvxpy as cp import numpy as np def build_mpc(N, nx, nu, A, B, Q, R, S, x0, x_ref): """ N: 预测时域 nx, nu: 状态/控制维度 A, B: 离散化后的状态空间矩阵 Q, R, S: 权重矩阵 x0: 当前状态 x_ref: 参考轨迹序列, shape (N+1, nx) """ x = cp.Variable((nx, N + 1)) u = cp.Variable((nu, N)) cost = 0 constraints = [x[:, 0] == x0] for k in range(N): cost += cp.quad_form(x[:, k] - x_ref[k], Q) cost += cp.quad_form(u[:, k], R) if k > 0: cost += cp.quad_form(u[:, k] - u[:, k-1], S) constraints += [x[:, k+1] == A @ x[:, k] + B @ u[:, k]] # 控制量约束示例 constraints += [cp.abs(u[0, :]) <= 0.5] # 前轮转角限幅 constraints += [u[1, :] >= -3, u[1, :] <= 2] # 加速度限幅 prob = cp.Problem(cp.Minimize(cost), constraints) prob.solve(solver=cp.OSQP, warm_start=True) return u[:, 0].value逻辑说明：这里用warm_start=True是关键，MPC 相邻周期的解高度相似，热启动能把求解时间压到冷启动的 30%~50%。约束里前轮转角限幅 0.5 rad 对应约 28 度，是常见乘用车的物理极限；加速度范围按舒适性取的，急刹场景可以放宽到 -5。注意x_ref的长度是 N+1，因为状态从 k=0 到 k=N 共 N+1 个点。
3.2 离散化方法的选择
连续模型转离散，常用零阶保持（ZOH）或前向欧拉。ZOH 精度高但需要矩阵指数运算，前向欧拉简单但要求 dt 小。我一般用scipy.signal.cont2discrete做 ZOH，代码如下：
from scipy.signal import cont2discrete import numpy as np def discretize(Ac, Bc, dt): """零阶保持离散化""" C = np.eye(Ac.shape[0]) D = np.zeros((Ac.shape[0], Bc.shape[1])) Ad, Bd, _, _, _ = cont2discrete((Ac, Bc, C, D), dt, method='zoh') return Ad, Bd参数说明：method='zoh'适合采样周期固定的数字控制器；如果 dt 大于系统最小时间常数的 1/10，ZOH 也会失真，这时要么减小 dt，要么改用更精确的模型。
3.3 求解器选型与实时性
常见 QP 求解器有 OSQP、qpOASES、HPIPM。OSQP 对稀疏问题友好、Python 接口成熟，适合原型验证；qpOASES 在嵌入式上更常见，C++ 版本实时性稳定。实测在 N=20、状态维度 6 的情况下，OSQP 单次求解约 2~5 ms，满足 100 Hz 控制周期。如果超过 10 ms，先查预测时域是不是设太大，N 从 20 降到 15 往往能省一半时间，跟踪精度损失有限。
4. 避坑与排查：那些让轨迹画歪的细节
4.1 现象：车辆在参考线附近高频抖动
原因：控制量变化率权重 S 太小，或者预测时域 N 太短导致控制器"短视"。解决：先把 S 调到 R 的 10 倍，再把 N 从 10 加到 20，抖动基本消失。如果还抖，检查参考轨迹是不是本身有噪声，做一次滑动平均滤波。
4.2 现象：转弯时车辆切内弯，稳态误差大
原因：Q 矩阵里横向误差权重不足，或者模型里没考虑曲率前馈。解决：把 Q 中对应 y 的权重从 1 提到 5~10；同时在参考轨迹里加入曲率前馈项，让控制器提前打方向。
4.3 现象：求解器报 infeasible
原因：约束太紧，比如前轮转角限幅 0.3 rad 但参考轨迹需要 0.5 rad 才能跟上。解决：先放宽约束验证可行性，再逐步收紧；或者把硬约束改成软约束，加松弛变量并惩罚松弛量。
4.4 现象：仿真跟得好，实车偏差大
原因：模型参数和实车不一致，最常见的是轴距和质量分布。解决：做定圆和直线加减速测试，反推等效参数；同时检查控制周期是否和仿真一致，实车抖动会导致 dt 不稳。
4.5 现象：低速时正常，高速时发散
原因：用了运动学模型但车速超过适用范围。解决：切换到动力学模型，或者把运动学模型里的速度项做线性化处理，但后者只在 8 m/s 以下有效。
5. 从仿真到落地的进阶技巧：热启动、变时域与代码移植
把 MPC 跑通只是第一步，真正上车还要解决实时性和鲁棒性。我常用的两个技巧：一是热启动加变时域，低速时 N 取 20 保证精度，高速时降到 10 保证实时性，切换时用上一周期的解做初值，求解时间能稳定在 3 ms 以内。二是把 Python 原型移植到 C++ 时，用 Eigen 做矩阵运算、qpOASES 做求解，注意 Python 里的numpy切片在 C++ 里要改成块操作，否则性能会掉一个数量级。验证方法上，我习惯先做开环测试：给定一组控制序列，看预测轨迹和实际轨迹是否重合；再做闭环测试：跑双移线和蛇形工况，记录横向误差 RMS，一般要求小于 0.1 m。最后提醒一句，别在没做参数标定的情况下直接上车，我见过太多因为轴距填错导致原地画圈的案例。希望帮到你。
本文还有配套的精品资源，点击获取
