简介：这份资源面向无人驾驶与自动控制方向的学习者和开发者，提供模型预测控制（MPC）的完整程序代码实现，帮助理解车辆路径规划、速度控制与转向操作中的预测优化逻辑。压缩包共16个文件，约38KB，以13个m脚本文件为主，辅以2个txt说明文档和1个mdl仿真模型，覆盖车辆动力学建模、预测框架、优化求解、滚动优化及仿真测试等核心模块，并包含Pacejka轮胎模型等具体实现。目前已有1069人学习下载，适合具备一定控制理论基础与MATLAB编程能力、希望深入掌握MPC在无人驾驶中应用的中高级读者。通过研读代码，可系统梳理从车辆模型构建到控制输入更新的完整流程，理解约束处理、参数调节与仿真验证方法，为实际无人驾驶系统设计提供可复用的参考实现与排错思路。
1. 从一段跑不通的轨迹跟踪说起：这套 MPC 代码到底能干什么
如果你正在做无人驾驶车辆的轨迹跟踪，大概率遇到过这种场景：PID 调了半天，直道还行，一进弯就画龙；LQR 稍微好点，但一遇到执行器饱和、轮胎侧偏刚度变化，跟踪误差立刻发散。这时候很多人会转向模型预测控制（MPC），因为它能在滚动时域里显式处理约束，把前轮转角、加速度这些物理限制直接写进优化问题。但真到动手写代码，QP 求解器怎么选、预测时域和控制时域怎么配、车辆模型用运动学还是动力学，每一步都是坑。
这套「无人驾驶车辆模型预测控制程序代码」就是围绕这个痛点来的。它把车辆横向动力学模型、线性时变 MPC 的推导、QP 求解、轨迹跟踪仿真串成了一条可复现的链路，适合两类人：一类是刚接触 MPC、想找一个能跑通的参考实现来对照公式的；另一类是做控制算法落地、需要一套可改参数、可换场景的代码骨架的。下面我按「模型怎么建 → 代码怎么跑 → 参数怎么调 → 坑在哪」的顺序拆一遍，中间给可直接抄的代码块和参数表。
2. 车辆模型与 MPC 数学形式：从二自由度到 QP 标准型
2.1 为什么选二自由度动力学模型而不是运动学模型
运动学自行车模型在低速（<5 m/s）下够用，但无人驾驶车辆的横向控制往往要在 10～20 m/s 区间工作，这时候轮胎侧偏力进入非线性区，运动学模型忽略侧偏角，预测出来的横摆角速度会偏乐观。二自由度动力学模型把车辆简化为前、后两个等效轮，状态量取横向位置 y、横向速度 vy、横摆角 φ、横摆角速度 r，控制量取前轮转角 δ 和纵向加速度 a。常见做法是：
- 状态量 x = [y, vy, φ, r]^T
- 控制量 u = [δ, a]^T
- 输出 y_out = [y, φ]^T，用于和参考轨迹做误差
轮胎侧偏力用线性近似 F_yf = C_f * α_f，F_yr = C_r * α_r，其中 α_f = δ - (vy + ar)/vx，α_r = -(vy - br)/vx。这里 C_f、C_r 是前、后轮侧偏刚度，a、b 是质心到前、后轴距离。这套线性化在侧向加速度 < 0.4g 时误差可控，超过这个范围就要考虑非线性 MPC 或增益调度。
2.2 连续模型离散化与线性时变处理
连续状态方程写成 x_dot = A_c x + B_c u，其中 A_c、B_c 依赖纵向速度 vx。因为 vx 在预测时域内会变，所以每个控制周期都要用当前 vx 重新算 A_c、B_c，再离散化。常见做法是用前向欧拉或零阶保持器（ZOH）。前向欧拉简单，但步长大了会不稳定；ZOH 精度高，适合 Ts = 0.05～0.1 s 的场景。下面是一段离散化和预测方程构造的 Python 代码：
import numpy as np from scipy.linalg import expm def discretize_kinematic_dynamic(vx, Ts, params): """ 二自由度动力学模型离散化 vx: 当前纵向速度 m/s Ts: 采样周期 s params: 包含 m, Iz, a, b, Cf, Cr 的字典 """ m, Iz = params['m'], params['Iz'] a, b = params['a'], params['b'] Cf, Cr = params['Cf'], params['Cr'] # 连续系统矩阵 A_c (4x4) A_c = np.array([ [0, 1, 0, 0], [0, -(Cf+Cr)/(m*vx), (Cf+Cr)/m, (-a*Cf + b*Cr)/(m*vx)], [0, 0, 0, 1], [0, (-a*Cf + b*Cr)/(Iz*vx), (a*Cf - b*Cr)/Iz, -(a**2*Cf + b**2*Cr)/(Iz*vx)] ]) # 连续输入矩阵 B_c (4x2) B_c = np.array([ [0, 0], [Cf/m, 0], [0, 0], [a*Cf/Iz, 0] ]) # 零阶保持器离散化 n, m_in = A_c.shape[0], B_c.shape[1] M = np.zeros((n+m_in, n+m_in)) M[:n, :n] = A_c M[:n, n:] = B_c Md = expm(M * Ts) A_d = Md[:n, :n] B_d = Md[:n, n:] return A_d, B_d这段代码的关键参数是 Ts 和 vx。Ts 一般取 0.05 s 或 0.1 s，太小会让 QP 求解频率过高，太大则离散误差明显。vx 不能为 0，否则 A_c 里会出现除零，实际代码里要加一个下限，比如 max(vx, 1.0)。离散化之后，预测方程把未来 Np 步的状态堆成一个向量，形式为 Y = Ψ ξ + Θ ΔU，其中 ξ 是当前状态和控制量的增广，ΔU 是控制增量序列。这个形式直接对应 QP 标准型 min 0.5 ΔU^T H ΔU + f^T ΔU，约束是 ΔU 上下界和输出上下界。
2.3 QP 标准型构造与求解器选择
把预测方程代入代价函数 J = Σ (y_ref - y)^T Q (y_ref - y) + Σ Δu^T R Δu + ρ ε^2，展开后得到 H = Θ^T Q_bar Θ + R_bar，f = -Θ^T Q_bar E。约束包括控制增量约束、控制量约束、输出约束，以及松弛因子 ε 的非负约束。常见求解器有 OSQP、quadprog、CasADi。OSQP 对稀疏 QP 友好，适合嵌入式部署；quadprog 在 MATLAB 里用得多；CasADi 适合快速原型。下面是一个用 OSQP 求解的骨架：
import osqp import scipy.sparse as sp def solve_mpc(H, f, A_ineq, l, u, warm_start=None): """ H: 二次项矩阵 (n_u*Nc x n_u*Nc) f: 一次项向量 A_ineq: 不等式约束矩阵 l, u: 约束上下界 """ prob = osqp.OSQP() prob.setup(sp.csc_matrix(H), f, sp.csc_matrix(A_ineq), l, u, warm_start=True, verbose=False, max_iter=2000, eps_abs=1e-4) if warm_start is not None: prob.warm_start(x=warm_start) res = prob.solve() if res.info.status != 'solved': raise RuntimeError(f'QP 求解失败: {res.info.status}') return res.x这里 warm_start 很关键。MPC 每个周期都在解结构相似的 QP，把上一周期的解作为热启动，能把求解时间压到 1～3 ms。eps_abs 设 1e-4 是精度和速度的折中，再小会拖慢求解。max_iter 设 2000 是防止极端情况下死循环。
3. 代码跑通全流程：从参数配置到轨迹跟踪仿真
3.1 参数配置表与车辆参数取值
跑通之前先把参数理清楚。下面这张表是我在复现时用的典型值，来源是常见乘用车的量级，不是某一款具体车型：
| 参数 | 符号 | 取值 | 单位 | 说明 |
|---|---|---|---|---|
| 质量 | m | 1500 | kg | 整车质量 |
| 横摆转动惯量 | Iz | 2500 | kg·m² | 绕 z 轴 |
| 质心到前轴 | a | 1.2 | m | |
| 质心到后轴 | b | 1.6 | m | |
| 前轮侧偏刚度 | Cf | 80000 | N/rad | 线性区 |
| 后轮侧偏刚度 | Cr | 80000 | N/rad | 线性区 |
| 采样周期 | Ts | 0.05 | s | |
| 预测时域 | Np | 20 | 步 | 对应 1 s |
| 控制时域 | Nc | 5 | 步 | 对应 0.25 s |
| 前轮转角上限 | δ_max | 0.5 | rad | 约 28.6° |
| 转角增量上限 | Δδ_max | 0.05 | rad | 每步 |
| 纵向加速度上限 | a_max | 2.0 | m/s² |
这张表里最容易翻车的是 Cf、Cr。很多开源代码直接抄了 80000，但实际轮胎在侧偏角超过 5° 后刚度会下降，如果仿真里侧偏角经常超过这个值，跟踪会发散。稳妥做法是把 Cf、Cr 设成随侧偏角查表的函数，或者把侧向加速度限制在 0.3g 以内。
3.2 参考轨迹生成与误差计算
参考轨迹一般用五次多项式或贝塞尔曲线生成，保证曲率连续。下面这段代码生成一条双移线轨迹，并计算横向误差和航向误差：
def generate_double_lane_change(vx, total_time, Ts): """ 生成双移线参考轨迹 返回: t, x_ref, y_ref, phi_ref, vx_ref """ t = np.arange(0, total_time, Ts) # 纵向匀速 x_ref = vx * t # 横向用平滑函数，避免曲率突变 y_ref = 3.5 * (1 / (1 + np.exp(-0.8*(t-3))) - 1 / (1 + np.exp(-0.8*(t-8)))) # 航向角由横向速度/纵向速度近似 dy = np.gradient(y_ref, Ts) phi_ref = np.arctan2(dy, vx) return t, x_ref, y_ref, phi_ref def compute_error(state, ref): """ state: [y, vy, phi, r] ref: [y_ref, phi_ref] 返回横向误差和航向误差 """ e_y = state[0] - ref[0] e_phi = state[2] - ref[1] return e_y, e_phi注意 phi_ref 用 arctan2(dy, vx) 只是近似，更准的做法是用参考轨迹的切线方向。如果参考轨迹曲率大，这个近似会引入额外误差，表现为跟踪时车辆总是慢半拍。
3.3 主循环：预测、求解、施加、更新
主循环的逻辑是：每个周期读当前状态，算离散矩阵，构造 QP，求解，取第一个控制增量，更新状态，推进参考轨迹。下面是一个可运行的主循环骨架：
def run_mpc_simulation(): params = {'m':1500, 'Iz':2500, 'a':1.2, 'b':1.6, 'Cf':80000, 'Cr':80000} Ts, Np, Nc = 0.05, 20, 5 vx = 15.0 t, x_ref, y_ref, phi_ref = generate_double_lane_change(vx, 15.0, Ts) state = np.array([0.0, 0.0, 0.0, 0.0]) # [y, vy, phi, r] u_prev = np.zeros(2) # [delta, a] log = [] for k in range(len(t)-1): A_d, B_d = discretize_kinematic_dynamic(vx, Ts, params) # 构造预测矩阵 (此处省略 Ψ, Θ 的堆叠细节，按 2.2 节公式展开) H, f, A_ineq, l, u_b = build_qp(A_d, B_d, state, u_prev, y_ref[k:k+Np], phi_ref[k:k+Np], Np, Nc) du = solve_mpc(H, f, A_ineq, l, u_b) # 取第一个控制增量 delta = u_prev[0] + du[0] acc = u_prev[1] + du[1] # 施加约束 delta = np.clip(delta, -0.5, 0.5) acc = np.clip(acc, -2.0, 2.0) # 状态更新（用离散模型） state = A_d @ state + B_d @ np.array([delta, acc]) u_prev = np.array([delta, acc]) log.append([t[k], state[0], state[2], delta, acc]) return np.array(log)这段代码里 build_qp 是核心，需要把 2.2 节的 Ψ、Θ 按 Np、Nc 堆出来。堆叠时注意控制时域 Nc 小于预测时域 Np 时，Nc 之后的控制增量保持为 0，这是标准做法。如果 Nc 设成等于 Np，计算量会上去，但控制更灵活。一般 Nc 取 Np 的 1/4 到 1/3。
4. 避坑与排查：MPC 轨迹跟踪最常见的五类翻车
4.1 现象：车辆在直道就抖，转角来回跳
原因：控制增量权重 R 太小，或者 QP 求解精度不够，导致每个周期解出来的 Δδ 符号来回变。解决：把 R 从 0.1 提到 1.0 以上，同时检查 eps_abs 是否设得太松。如果还抖，把控制时域 Nc 从 5 降到 3，减少优化变量。
4.2 现象：进弯后横向误差越来越大，最后发散
原因：预测时域 Np 太短，车辆还没看到弯道就冲进去了；或者 Cf、Cr 线性区假设失效，侧偏角超过 5°。解决：把 Np 从 20 提到 30，同时把 vx 降下来，或者把侧向加速度限制在 0.3g。如果必须高速过弯，换非线性 MPC 或增益调度。
4.3 现象：QP 求解报 infeasible
原因：输出约束和状态约束冲突，比如参考轨迹要求横向位置在 ±0.5 m，但车辆当前状态离得太远，预测时域内根本回不来。解决：加松弛因子 ε，把硬约束变成软约束，代价函数里加 ρ ε^2，ρ 取 1000 以上。这样即使暂时不可行，也能给出一个最小违反的解。
4.4 现象：仿真步长 Ts 改小后反而更慢
原因：Ts 变小，QP 求解频率变高，单次求解时间没降，总时间就上去了。解决：Ts 不要低于 0.02 s，除非用 C 代码生成或嵌入式求解器。常见做法是 Ts = 0.05 s，配合 warm_start，单次求解 1～3 ms。
4.5 现象：跟踪直道没问题，一换轨迹就崩
原因：参考轨迹的曲率不连续，或者 phi_ref 计算用了差分导致噪声。解决：参考轨迹用五次多项式或贝塞尔曲线，保证曲率连续；phi_ref 用解析导数，不要用 np.gradient。如果轨迹来自规划模块，先做平滑滤波。
5. 进阶技巧：把 MPC 从仿真推到实时，以及一个验证习惯
5.1 用 C 代码生成替代 Python 求解
Python 跑 MPC 适合验证算法，但实时性不够。常见做法是用 CasADi 或 acados 生成 C 代码，把 QP 求解器换成 OSQP 的 C 版本或 HPIPM。生成后的单次求解能压到 0.5 ms 以内，足够 100 Hz 跑。下面是一个用 CasADi 定义 MPC 的片段：
import casadi as ca def build_casadi_mpc(Np, Nc, Ts, params): opti = ca.Opti() X = opti.variable(4, Np+1) U = opti.variable(2, Nc) X0 = opti.parameter(4) ref = opti.parameter(2, Np) Q = ca.diag([10, 1, 10, 1]) R = ca.diag([1, 0.1]) cost = 0 opti.subject_to(X[:,0] == X0) for k in range(Np): if k < Nc: u_k = U[:,k] else: u_k = U[:,-1] cost += (X[0,k]-ref[0,k])**2 * 10 + (X[2,k]-ref[1,k])**2 * 10 if k < Nc: cost += u_k[0]**2 * 1 + u_k[1]**2 * 0.1 # 动力学约束用离散模型 A_d, B_d = discretize_kinematic_dynamic(15.0, Ts, params) opti.subject_to(X[:,k+1] == A_d @ X[:,k] + B_d @ u_k) opti.minimize(cost) opti.subject_to(opti.bounded(-0.5, U[0,:], 0.5)) opti.subject_to(opti.bounded(-2.0, U[1,:], 2.0)) opts = {'ipopt.print_level':0, 'print_time':0} opti.solver('ipopt', opts) return opti, X, U, X0, ref这段代码用 IPOPT 求解，适合原型验证。如果要嵌入式部署，把 solver 换成 qpoases 或 osqp，并导出 C 代码。
5.2 一个验证习惯：先开环扫参数，再闭环跑
我自己的习惯是，每次改完模型或权重，先做开环扫参：固定一组状态和控制量，看预测输出和真实模型差多少。如果开环误差就大，闭环肯定崩。开环验证通过后，再跑闭环，先低速直道，再高速直道，再低速弯道，最后高速弯道。每一步都记录横向误差峰值和 QP 求解时间。下面这张表是我常用的验证清单：
| 阶段 | 速度 | 轨迹 | 通过标准 |
|---|---|---|---|
| 开环 | 任意 | 固定输入 | 预测输出与真实模型误差 < 1% |
| 闭环低速直道 | 5 m/s | 直线 | 横向误差峰值 < 0.05 m |
| 闭环高速直道 | 15 m/s | 直线 | 横向误差峰值 < 0.1 m |
| 闭环低速弯道 | 5 m/s | 双移线 | 横向误差峰值 < 0.1 m |
| 闭环高速弯道 | 15 m/s | 双移线 | 横向误差峰值 < 0.3 m，不发散 |
从那以后我每次调 MPC，都强制走一遍这个清单，尤其是开环那一步，能省掉大量闭环调试的玄学时间。希望帮到你。
本文还有配套的精品资源，点击获取
