无人驾驶MPC轨迹跟踪代码实战：从仿真到实车避坑指南

简介：这份资源面向无人驾驶与自动控制方向的学习者和开发者，提供模型预测控制（MPC）的完整程序代码实现，帮助理解MPC在车辆路径规划、速度控制与转向操作中的落地方式。压缩包共16个文件，以13个m脚本为主，辅以2个txt说明文档和1个mdl仿真模型，整体约38KB，覆盖车辆动力学建模、预测框架、优化问题求解、滚动优化控制更新及仿真测试等关键环节，并涉及Pacejka轮胎模型等具体实现。代码按章节组织，从基础运动学模型到非线性预测控制逐步展开，便于对照控制理论逐层研读。目前已有1069人学习下载，适合具备一定控制理论基础与MATLAB编程能力、希望深入掌握MPC原理并应用于实际无人驾驶系统设计的读者参考。

1. 从仿真到实车：这套 MPC 代码到底能跑通什么

如果你正在做无人驾驶车辆的轨迹跟踪控制，大概率绕不开模型预测控制（MPC）这个词。但真正让人头疼的不是理解它的原理，而是找到一份能跑通、能改参数、能看出每一步在干什么的代码。这套无人驾驶车辆模型预测控制程序代码，核心解决的就是这个问题：它把车辆动力学建模、线性化、预测方程推导、二次规划求解、控制量下发这条链路完整地串了起来，不是伪代码，也不是只给一个求解器调用示例。

它适合两类人：一类是刚接触车辆控制的研究生或工程师，需要一份能逐行调试的参考实现来建立直觉；另一类是在做轨迹跟踪算法对比的从业者，想拿一个干净的 MPC 基线去替换自己的控制器做 benchmark。代码结构围绕自行车模型展开，控制目标通常是横向位置误差和航向角误差的收敛，输出前轮转角或加速度指令。下面我从模型怎么建、代码怎么跑、参数怎么调、哪里容易翻车几个角度，把这份资源拆开讲清楚。

2. 车辆动力学建模与线性化：MPC 的起点在哪

2.1 为什么用自行车模型而不是四轮模型

做车辆 MPC，第一个选择就是模型精度和计算量的平衡。四轮模型能描述侧倾、载荷转移，但状态维度高，线性化后矩阵大，QP 求解时间在嵌入式平台上很难压到 10ms 以内。自行车模型把左右轮合并，假设车辆低速时轮胎侧偏特性在线性区间，用前轮转角和纵向速度作为输入，状态量取横向位置、横向速度、航向角、航向角速度，四维状态加两维控制，已经能覆盖大部分轨迹跟踪场景。

常见做法是：如果车速低于 60km/h 且路面附着良好，自行车模型的预测误差在可接受范围内；如果要做高速换道或低附着路面，才需要引入侧偏刚度非线性或魔术公式轮胎模型。这份代码默认用的是线性自行车模型，状态方程写成矩阵形式后，离散化用前向欧拉或零阶保持，步长一般取 0.05s 到 0.1s。

2.2 线性化与离散化的代码落点

MPC 每步都要在当前状态点对非线性模型做线性化，得到雅可比矩阵 A 和 B，然后离散化。代码里这部分通常封装成一个vehicle_model函数，输入当前状态和控制量，输出 Ad、Bd、Cd。下面是一个典型的实现片段：

import numpy as np def linearize_and_discretize(x, u, dt, L, Cf, Cr, m, Iz, vx): """ 在当前状态 x 和控制 u 处线性化自行车模型并离散化 x = [y, vy, psi, r] 横向位置, 横向速度, 航向角, 航向角速度 u = [delta, a] 前轮转角, 纵向加速度 """ y, vy, psi, r = x delta, a = u # 非线性动力学方程对状态的雅可比 A = np.zeros((4, 4)) A[0, 1] = 1.0 A[1, 1] = -(Cf + Cr) / (m * vx) A[1, 2] = (Cf + Cr) / m A[1, 3] = (-Cf * Lf + Cr * Lr) / (m * vx) - vx A[2, 3] = 1.0 A[3, 1] = (-Cf * Lf + Cr * Lr) / (Iz * vx) A[3, 2] = (Cf * Lf - Cr * Lr) / Iz A[3, 3] = -(Cf * Lf**2 + Cr * Lr**2) / (Iz * vx) # 对控制的雅可比 B = np.zeros((4, 2)) B[1, 0] = Cf / m B[3, 0] = Cf * Lf / Iz B[1, 1] = 0.0 B[3, 1] = 0.0 # 前向欧拉离散化 Ad = np.eye(4) + A * dt Bd = B * dt return Ad, Bd

这段代码的逻辑是：先写出连续时间下的状态空间矩阵，再用一阶保持离散化。参数里Lf和Lr是质心到前轴和后轴的距离，Cf、Cr是前后轮侧偏刚度，m是整车质量，Iz是绕 z 轴转动惯量。这些值需要根据实际车辆参数填写，如果随便填一组，跟踪效果会直接崩掉。注意vx出现在分母上，低速时矩阵元素会变得很大，所以代码里通常要加一个速度下限保护，比如vx = max(vx, 1.0)。

2.3 预测方程与 QP 标准型构造

线性化之后，下一步是把预测时域内的状态展开成关于当前状态和控制增量的表达式。假设预测时域 Np，控制时域 Nc，构造一个大矩阵，把未来 Np 步的状态写成初始状态加控制序列的线性组合。然后定义代价函数：横向误差、航向误差、控制量变化率、控制量幅值，各自加权。最后整理成标准二次规划形式：

from qpsolvers import solve_qp def build_qp(Ad, Bd, x0, ref, Np, Nc, Q, R, Rd): """ 构造 MPC 的 QP 问题并求解 ref: 参考轨迹在预测时域内的序列, 形状 (Np, 4) """ nx, nu = Ad.shape[0], Bd.shape[1] # 构造预测矩阵 Psi 和 Theta Psi = np.zeros((nx * Np, nx)) Theta = np.zeros((nx * Np, nu * Nc)) Ad_pow = np.eye(nx) for i in range(Np): Ad_pow = Ad_pow @ Ad if i > 0 else Ad Psi[i*nx:(i+1)*nx, :] = Ad_pow for j in range(min(i+1, Nc)): Theta[i*nx:(i+1)*nx, j*nu:(j+1)*nu] = np.linalg.matrix_power(Ad, i-j) @ Bd # 代价函数展开 H = Theta.T @ Q @ Theta + R f = (Psi @ x0 - ref.flatten()).T @ Q @ Theta # 控制增量约束 lb = -0.5 * np.ones(nu * Nc) ub = 0.5 * np.ones(nu * Nc) U = solve_qp(H, f, lb=lb, ub=ub, solver='quadprog') return U[:nu] if U is not None else np.zeros(nu)

这里Q是状态权重矩阵，通常横向误差权重给大一些，R是控制增量权重，Rd是控制量本身权重。Np一般取 20 到 30，Nc取 5 到 10。如果 QP 求解返回 None，说明约束冲突或矩阵非正定，代码里要有兜底逻辑，否则实车会突然丢控制。

3. 代码跑起来：从环境配置到闭环仿真

3.1 依赖安装与目录结构

拿到代码包后，先看依赖。常见的是 Python 3.8 以上，需要 numpy、scipy、matplotlib，QP 求解器可能用 quadprog、osqp 或 cvxpy。如果代码里用的是qpsolvers这个统一接口，那底层求解器可以换，但要注意不同求解器对矩阵正定性和约束类型的支持不一样。安装命令一般长这样：

pip install numpy scipy matplotlib qpsolvers quadprog

如果 quadprog 装不上，可以换 osqp：

pip install osqp

然后在代码里把solver='quadprog'改成solver='osqp'。注意 osqp 对等式约束的处理方式和 quadprog 不同，如果原代码里有硬等式约束，换求解器后可能需要改成软约束或调整容差。

目录结构通常分三块：model/放车辆动力学和线性化，controller/放 MPC 主循环和 QP 构造，simulation/放参考轨迹生成和闭环仿真。先跑simulation里的主脚本，确认能出图，再去改model里的参数。

3.2 参考轨迹生成与闭环仿真主循环

闭环仿真的逻辑是：给定一条参考轨迹，每个控制周期读取当前车辆状态，计算 MPC 控制量，把控制量作用到车辆模型上，更新状态，循环。参考轨迹常见用双移线或正弦曲线，代码里一般会有一个generate_reference函数。主循环的骨架如下：

def closed_loop_simulation(): dt = 0.05 T = 20.0 N = int(T / dt) x = np.array([0.0, 0.0, 0.0, 0.0]) # 初始状态 trajectory = [] for k in range(N): t = k * dt ref = generate_reference(t, dt, Np=20) Ad, Bd = linearize_and_discretize(x, u_prev, dt, L, Cf, Cr, m, Iz, vx) u = build_qp(Ad, Bd, x, ref, Np=20, Nc=5, Q, R, Rd) x = vehicle_step(x, u, dt) # 用非线性模型推进一步 trajectory.append(x.copy()) u_prev = u plot_trajectory(trajectory)

这里有个关键点：MPC 内部用的是线性化模型做预测，但仿真推进一步时用的是非线性模型，这叫“模型失配”。如果失配太大，跟踪误差会累积，这时候要么减小步长，要么在 MPC 里加反馈校正项。我一般会在vehicle_step里用完整的非线性自行车模型，确保仿真更接近真实。

3.3 参数表：哪些值不能随便填

下面这张表是代码里最常改的参数，填错一个就可能翻车：

这些值不是拍脑袋来的，最好从车辆参数表里查，或者做系统辨识。如果手头没有精确值，先用典型值跑通，再微调。

4. 避坑与排查：那些让跟踪效果崩掉的细节

4.1 现象：车辆画龙，横向误差来回震荡

原因通常是 Q 矩阵里横向误差权重给得太大，而 R 矩阵控制增量权重太小，导致控制器对噪声过度反应。另一个可能是vx在分母上，低速时矩阵元素爆炸，求解出的控制量跳变。解决方法是把vx加下限保护，同时把 R 调大，让控制量变化更平滑。我一般会先设 Q = diag([100, 1, 100, 1])，R = diag([10, 1])，跑一遍看曲线，再微调。

4.2 现象：QP 求解返回 None 或报 infeasible

这是血泪经验里最常见的一条。原因可能是约束上下界设反了，或者预测时域内参考轨迹跳变太大，导致可行域为空。先检查lb和ub的符号，再检查参考轨迹有没有阶跃。如果参考轨迹本身不连续，MPC 会在跳变点附近无解。解决办法是对参考轨迹做平滑滤波，或者把硬约束改成软约束，加松弛变量。

4.3 现象：仿真跑通但实车跟踪滞后

仿真里模型和控制器用同一套参数，实车上轮胎侧偏刚度随载荷和温度变化，模型失配导致滞后。常见做法是在 MPC 里加一个前馈项，或者用扰动观测器估计模型误差。另一个容易忽略的点是执行器延迟，前轮转角指令下发到实际转角有几十毫秒延迟，代码里如果没补偿，高速时跟踪误差会明显增大。我一般会在控制量输出后加一个一阶延迟环节，模拟执行器动态，再调 MPC 参数。

4.4 现象：换求解器后结果完全不一样

不同 QP 求解器对同一问题的数值处理不同，osqp 对稀疏矩阵友好，quadprog 对稠密矩阵快。如果换求解器后控制量跳变，先检查 H 矩阵是否对称正定，再看约束的容差设置。osqp 的eps_abs和eps_rel默认 1e-3，对车辆控制来说太松，建议调到 1e-5。另外，osqp 返回的解可能不满足硬约束，需要加一步投影或后处理。

4.5 现象：预测时域加长后求解时间暴涨

Np 从 20 加到 40，QP 维数翻倍，求解时间可能从 2ms 涨到 20ms，嵌入式平台直接超时。这时候要么降 Nc，要么用显式 MPC 离线算表，要么换更高效的求解器。我一般会先测一下单步求解时间的分布，看最坏情况能不能满足控制周期，再决定要不要优化。

5. 进阶技巧：怎么验证 MPC 真的在干活

5.1 用开环预测对比验证线性化精度

一个很实用的验证方法是：在某个状态点，用线性化模型预测未来 Np 步的状态，再用非线性模型推同样步数，对比两条轨迹。如果偏差在几步内就超过阈值，说明线性化点选得不好，或者步长太大。代码里可以加一个validate_linearization函数，每次线性化后自动跑一遍对比，偏差大就报警。

def validate_linearization(x0, u, dt, Np, threshold=0.1): Ad, Bd = linearize_and_discretize(x0, u, dt, ...) x_lin = x0.copy() x_nonlin = x0.copy() for i in range(Np): x_lin = Ad @ x_lin + Bd @ u x_nonlin = nonlinear_step(x_nonlin, u, dt) if np.linalg.norm(x_lin - x_nonlin) > threshold: print(f"线性化偏差在第 {i} 步超限: {np.linalg.norm(x_lin - x_nonlin):.3f}") return False return True

这个检查在调参阶段特别有用，能帮你快速定位是模型问题还是权重问题。

5.2 控制量变化率约束的工程整定

很多代码只约束控制量幅值，不约束变化率，结果实车方向盘打得像抽筋。加变化率约束其实很简单，把控制增量作为优化变量，约束delta_u的上下界。但要注意，变化率约束太紧会导致跟踪滞后，太松又没效果。我一般会先设一个物理极限，比如前轮转角变化率不超过 0.5 rad/s，再根据跟踪误差微调。如果跟踪误差在弯道处明显变大，说明变化率约束太紧，适当放宽。

5.3 从仿真到实车的检查清单

实车部署前，我每次都会强制走一遍这几项：第一，确认所有参数单位一致，角度用弧度还是度，速度用 m/s 还是 km/h，混用必翻车；第二，检查 QP 求解失败时的兜底逻辑，能不能安全停车；第三，用录制好的实车数据回放，对比 MPC 输出和实际执行器响应，看延迟和死区；第四，在低速下先试，确认无误再提速。从那以后我每次部署新控制器都强制走一遍这个清单，希望帮到你。

本文还有配套的精品资源，点击获取