简介：这套无人驾驶车辆模型预测控制（MPC）程序代码，面向学习智能车辆控制的研究者与开发者，解决从理论公式到可运行代码的落地问题。压缩包共16个文件，包含13个MATLAB脚本（.m）、2个说明文档（.txt）和1个Simulink模型（.mdl），整体仅38KB，轻量但脉络完整。代码按Chapter 2至Chapter 6分章组织，从车辆运动学与动力学模型出发，逐步展开预测框架、优化目标与约束处理、滚动优化更新等关键环节；其中.m文件覆盖核心算法实现，txt文件含C++参考代码与使用说明，mdl文件可支持Simulink环境下的MPC仿真验证。该资源尤其适合已有一定控制理论基础、希望对照程序理解MPC工程落地的读者，已有1069人学习下载，是研究无人驾驶路径规划与速度控制的实用参考。
1. 无人驾驶模型预测控制程序代码：把它当成黑匣子跑通，是最亏的用法
拿到一套无人驾驶车辆模型预测控制程序代码，多数人的第一反应是直接跑起来看效果。真跑起来才发现，MPC不像PID那样装上去就能动，它有一堆参数、模型和约束在背后互相牵制。这套程序代码解决的是无人车横纵向轨迹跟踪问题：输入一条参考轨迹，输出每一帧的加速度和前轮转角。它把车辆运动学模型、滚动时域优化、约束边界封装成一个最小闭环，适合做课题验证、毕业设计二次开发，也适合想从公式走到可执行代码的工程师。我的态度是：先确认它能不能跑通，再看懂它为什么这么设计，最后才动手改。
2. 先把MPC搞明白：车辆模型、代价函数与约束的三个关键决策
写无人驾驶MPC最忌讳上来就调参。你得先回答三个问题：为什么非得用MPC，LQR行不行；车辆模型要什么精度；约束到底怎么表达。这三个回答决定了后续所有代码的走向，也是判断一套MPC程序代码好不好的核心。
2.1 为什么是MPC：和PID、LQR比，优势在哪里
PID的思路是每个控制量单独闭环，转角环、速度环分开调。低速直线和小弯道没问题，但连续弯道里PID存在相位滞后，而且它处理不了“最大转角只能打这么多”这类约束。LQR把状态反馈一次性算出来，本质是无限时域最优控制，但它不显式处理约束，转角限幅只能在输出端加饱和函数。这里有个很隐蔽的问题：限幅生效时，控制已经不是最优的了。
MPC的做法是每一帧都滚动求解未来Np步的最优控制序列，把加速度边界、转角边界、转向变化率这些约束直接写进优化问题。它不是事后限幅，而是让优化器在可行域内找解。无人驾驶里约束特别多，这是选MPC而不是LQR的本质原因。你拿到这套程序代码时，第一步不是改参数，而是确认它的求解器、代价函数和约束边界是否符合这个思想。
2.2 车辆模型选型：运动学模型与动力学模型的取舍
MPC控制器的被控对象不是真车，是一个数学预测模型。模型精度直接决定预测准不准，但也直接决定求解速度。低速场景（约30 km/h以下）用运动学自行车模型就够，这个模型假设车辆没有侧偏，前后轮刚性，状态量是位置(x, y)、航向角theta、速度v，控制量是加速度a和前轮转角delta。高速场景必须换动力学模型，引入轮胎侧偏刚度、质心侧偏角、横摆角速度。
这套程序代码用的是运动学模型，速度和航向角单独积分，位置更新由航向角确定。它参数少、非线性度适中，适合课题验证和低速仿真。如果长时间把车速提到50 km/h以上，运动学模型的预测误差会明显变大，表象是控制量震荡。这时候优先换成动力学模型，而不是硬调权重矩阵。
import numpy as np def vehicle_update(state, control, dt, L): """ 运动学自行车模型单步离散化 state = [x, y, theta, v] control = [a, delta] """ x, y, theta, v = state a, delta = control # 航向角变化率由转向角和速度共同决定 theta_dot = v / L * np.tan(delta) # 半隐式积分：theta 用更新前的值算位置 x_new = x + v * np.cos(theta) * dt y_new = y + v * np.sin(theta) * dt theta_new = theta + theta_dot * dt v_new = v + a * dt return np.array([x_new, y_new, theta_new, v_new])逻辑说明：这段代码是后面所有预测的基础。位置更新用的是当前航向角theta，theta更新用的是积分后的theta_dot，这叫半隐式积分，比完全显式积分稳定，又比完全隐式简单。新手容易把theta_new直接代回位置计算，那就是全隐式，虽然更稳但多一次迭代，没必要。
参数说明：dt是采样周期，一般取0.05到0.1秒；L是轴距，对转向影响最大。L越大，同样转角下转弯半径越大。模型跑出来明显转不过弯时，先检查L，不要动Q矩阵。
2.3 优化问题的数学形式：从非线性模型到可求解的QP
MPC每一帧做三件事：测量当前状态，预测未来Np步，求解带约束的优化问题。代价函数写成三项相加：
J = sum (x_k - x_ref_k)^T Q (x_k - x_ref_k) + sum u_k^T R u_k + sum (u_k - u_{k-1})^T Rd (u_k - u_{k-1})
第一项惩罚状态偏差，第二项惩罚控制量大小，第三项惩罚控制增量。Q、R、Rd三个权重矩阵是调参重点。
这里有个坑：运动学自行车模型是非线性的，cvxpy这类QP求解器不能直接解非线性约束。常见做法有两个：一个是上CasADi做非线性MPC，学习成本高；另一个是在工作点线性化，做线性时变MPC（LTV-MPC）。我推荐后者，它保留运动学模型的大部分精度，又能用成熟QP求解器，代码量也可控。
def linearize_vehicle(state_lin, control_lin, L, dt): """ 在某个工作点线性化运动学模型 返回误差动力学的A、B矩阵 """ x, y, theta, v = state_lin a, delta = control_lin A = np.eye(4) A[0, 2] = -v * np.sin(theta) * dt A[0, 3] = np.cos(theta) * dt A[1, 2] = v * np.cos(theta) * dt A[1, 3] = np.sin(theta) * dt A[2, 3] = np.tan(delta) / L * dt B = np.zeros((4, 2)) B[2, 1] = v / (L * np.cos(delta) ** 2) * dt B[3, 0] = dt return A, B逻辑说明：每个工作点求一阶偏导，组成A、B矩阵。A前三行前三列来自位置与航向角的耦合，B的第三行第二列是转角对航向角变化率的影响，第四行第一列是加速度对速度的影响。误差状态e = x - x_ref，误差控制增量du = u - u_lin，误差动力学就变成e[k+1] = A e[k] + B du[k]。
参数说明：工作点从参考轨迹取，控制工作点用上一帧控制量。速度越高，B矩阵里转角相关项越大，说明高速下同样的转角输入对航向影响更强。这符合直觉。
import cvxpy as cp def build_ltv_mpc_problem(x0, A_list, B_list, ref_traj, Np, Nc, Q, R, Rd, u_prev, bounds): """ 构建LTV-MPC优化问题 A_list/B_list: Np个工作点的线性化矩阵 ref_traj: (Np, 4) 参考轨迹段 """ n_state = 4 n_control = 2 E = cp.Variable((Np + 1, n_state)) # 误差状态 U = cp.Variable((Np, n_control)) # 控制量 dU = cp.Variable((Np, n_control)) # 控制增量 cost = 0 constraints = [E[0, :] == x0 - ref_traj[0, :]] for k in range(Np): constraints.append( E[k + 1, :] == A_list[k] @ E[k, :] + B_list[k] @ dU[k, :] ) cost += cp.quad_form(E[k, :], Q) cost += cp.quad_form(U[k, :], R) for k in range(Nc): cost += cp.quad_form(dU[k, :], Rd) constraints.append(U[0, :] == u_prev + dU[0, :]) for k in range(1, Np): constraints.append(U[k, :] == U[k - 1, :] + dU[k, :]) constraints.append(U[:, 0] >= bounds["a_min"]) constraints.append(U[:, 0] <= bounds["a_max"]) constraints.append(U[:, 1] >= bounds["delta_min"]) constraints.append(U[:, 1] <= bounds["delta_max"]) problem = cp.Problem(cp.Minimize(cost), constraints) return problem, E, U, dU逻辑说明：这个优化问题的核心是把非线性车辆模型在每个预测步工作点线性化，然后解一个标准QP。误差状态E的初值是当前状态与参考起点的差，后续每一步由A、B和dU推进。控制量U由dU积分而来，这就是控制增量约束得以后置的原因。
参数说明：Np是预测时域，Nc是控制时域，通常Np取15到20，Nc取3到5。Q矩阵对角线通常给[1, 1, 1, 1]起步，R给[0.1, 0.1]，Rd给[0.5, 0.5]。Q和R是相对大小，Q太大会让你疯狂踩油门，R太大会让车跟不上轨迹。cvxpy的quad_form要求矩阵是对称半正定的，权重矩阵不要给负值。
3. 代码拆解：从主循环到求解器的每一帧执行逻辑
把原理看懂后，再去看程序代码结构就清晰了。这套代码不是一个大文件塞到底，而是按职责拆成模块。我拿到代码包后，习惯先看目录看main.py在做什么，再看vehicle_model里模型怎么写的，最后才看mpc_controller。
3.1 文件目录与模块职责
结构上大致分六块，每个文件只干一件事：
| 文件 | 职责 |
|---|---|
| main.py | 主仿真循环，初始化、加载轨迹、调用控制器、写日志 |
| vehicle_model.py | 车辆运动学模型、线性化函数、参考轨迹生成 |
| mpc_controller.py | MPC问题构建与求解封装 |
| params.yaml | 参数集中配置，Np、Q、R、边界都在这里 |
| visualize.py | 结果可视化，轨迹对比、误差曲线 |
| utils.py | 最近点查找、坐标变换等辅助函数 |
参数集中的好处是改参数不用翻代码。我上手第一步会把params.yaml里的参数从头到尾读一遍，对应到第2章讲的模型、代价、约束三件事上。
3.2 主循环：每帧都在做什么
主循环的逻辑比想象中简单：测量状态、找到参考轨迹段、线性化、求解MPC、下发控制量、推进模型、记录日志。但工程上容易踩坑的是参考轨迹段怎么切，以及求解失败时怎么兜底。
def find_nearest_index(state, traj): """在参考轨迹上找离当前位置最近的点索引""" diffs = traj[:, :2] - state[:2] dist = np.sum(diffs ** 2, axis=1) return int(np.argmin(dist)) def run_simulation(traj, x0, dt, Np, Nc, Q, R, Rd, bounds, L): state = x0.copy() log = {"t": [], "x": [], "y": [], "theta": [], "v": [], "a": [], "delta": []} t = 0.0 u_prev = np.array([0.0, 0.0]) total_steps = int(traj.shape[0] * dt * 10) # 按轨迹长度估算步数 for step in range(total_steps): # 1. 取参考轨迹段：从最近点向后取Np个点 ref_start = find_nearest_index(state, traj) ref_seg = traj[ref_start:ref_start + Np] if len(ref_seg) < Np: break # 2. 对参考段每个工作点做线性化 A_list, B_list = [], [] for k in range(Np): state_lin = ref_seg[k] ctrl_lin = u_prev if k == 0 else np.array([0.0, 0.0]) A_k, B_k = linearize_vehicle(state_lin, ctrl_lin, L, dt) A_list.append(A_k) B_list.append(B_k) # 3. 构建并求解MPC problem, E, U, dU = build_ltv_mpc_problem( state, A_list, B_list, ref_seg, Np, Nc, Q, R, Rd, u_prev, bounds ) problem.solve(solver="OSQP") # 4. 求解失败时兜底：用上一帧控制量 if problem.status != "optimal": control = u_prev else: control = U.value[0, :] # 5. 推进模型并记录 state = vehicle_update(state, control, dt, L) log["t"].append(t) log["x"].append(state[0]) log["y"].append(state[1]) log["theta"].append(state[2]) log["v"].append(state[3]) log["a"].append(control[0]) log["delta"].append(control[1]) u_prev = control t += dt return log逻辑说明：主循环的四个关键动作都很直白。找最近点用欧氏距离遍历，轨迹上万点时效率不高，但作为学习实现足够清楚。线性化在每帧重建，所以叫LTV-MPC。求解器返回optimal才更新控制量，失败时用上一帧兜底，这是实车上保安全的基本操作，直接让车停比乱打方向盘好。
参数说明：total_steps是我按轨迹长度粗略估的，实际写的时候你应该根据仿真时间dt精确计算。find_nearest_index有三点注意：轨迹必须按时间顺序排列；车辆速度过低时最近点可能跳变；高速时参考段可能回退。这些都会导致参考段突变，表现为控制量跳变。
3.3 可视化与结果调试
代码包里的visualize.py是排查问题最快的入口。先看轨迹贴不贴参考，再看横向误差收敛不收敛，最后看控制量曲线有没有毛刺。
import matplotlib.pyplot as plt def plot_results(log, traj): fig, axes = plt.subplots(2, 2, figsize=(12, 8)) # 轨迹对比 axes[0, 0].plot(traj[:, 0], traj[:, 1], "k--", label="参考轨迹") axes[0, 0].plot(log["x"], log["y"], "b-", label="实际轨迹") axes[0, 0].set_xlabel("x [m]") axes[0, 0].set_ylabel("y [m]") axes[0, 0].legend() # 速度曲线 axes[0, 1].plot(log["t"], log["v"]) axes[0, 1].set_xlabel("t [s]") axes[0, 1].set_ylabel("速度 [m/s]") # 加速度曲线 axes[1, 0].plot(log["t"], log["a"]) axes[1, 0].set_xlabel("t [s]") axes[1, 0].set_ylabel("加速度 [m/s^2]") # 转角曲线 axes[1, 1].plot(log["t"], log["delta"]) axes[1, 1].set_xlabel("t [s]") axes[1, 1].set_ylabel("转角 [rad]") plt.tight_layout() plt.savefig("mpc_result.png", dpi=150)逻辑说明：加速度曲线有毛刺，说明控制增量权重不够；转角曲线高频摆动，说明R或Rd偏小；轨迹贴合但速度不稳，要去看参考轨迹速度序列是否平滑。一张图从四个维度把控制器表现摊开，比单纯看轨迹贴不贴有用得多。
4. 参数标定：六个参数与一组调参顺序
我从一开始就强调：MPC参数之间存在强烈的耦合关系，没有哪个参数能单独调到位。这一章给一张参数表，再给一个从零开始的调试顺序。你拿到程序代码后，按这个顺序走，能少走很多弯路。
4.1 参数表：每个参数调大调小会发生什么
| 参数 | 含义 | 推荐初值 | 调大效果 | 调小效果 |
|---|---|---|---|---|
| Np | 预测时域 | 15 | 轨迹更平滑，求解变慢 | 响应更激进，易振荡 |
| Nc | 控制时域 | 3 | 控制自由度更多 | 更保守 |
| dt | 采样周期 | 0.05 s | 计算压力小 | 预测更精确 |
| L | 轴距 | 按车辆实际值 | 转弯半径偏大 | 转弯半径偏小 |
| Q | 状态偏差权重 | diag([1,1,1,1]) | 跟踪更紧，易震荡 | 跟踪松，控制平缓 |
| R | 控制量权重 | diag([0.1,0.1]) | 控制更平缓 | 控制更激进 |
| Rd | 控制增量权重 | diag([0.5,0.5]) | 消除抖动 | 响应变快，出毛刺 |
这里有一个重要原则：Q和R之间是相对关系，不是绝对值。你把Q和R同时乘10，控制效果基本不变。真正要关心的是权重比例，比如Q[0]=1, R[0]=0.1，表示状态位置偏差成本10倍于控制量成本。
4.2 调试顺序：先解锁再提速，先直线再弯道
我建议的调参顺序和大多数人直觉相反，不是拿最终场景直接调。
第一步固定模型参数。L按真实车辆填，dt按实时性选，这两个参数不能被Q矩阵补偿，填错了后面全白调。第二步验证基本闭环。Np先设8以下，R和Rd设大一点，跑一个简单直线或大半径弯，确认轨迹跟得上、求解器不报错。第三步调权重。先固定Q，R从0.2往下试，出现震荡就回调；震荡消失后，再逐步增大Rd控制增量平滑性。第四步增预测时域。Np从8往上加到15到20，每加一步看跟踪误差和求解时间。第五步才是最关键的：每次只改一个参数，记录现象。
这五步里最容易翻车的不是调参本身，而是改完参数不看曲线只看轨迹。轨迹看起来差不多，但加速度曲线已经剧烈抖动，这种状态上实车基本当场出事。
4.3 一个完整调参实例：从振荡到平稳
某开发者拿到代码包时默认参数是Np=20，R=diag([0.05, 0.05])，Rd=diag([0.1, 0.1])。跑低速弯道时横向误差保持在0.08米内，但转角曲线在±0.05弧度之间高频摆动。
第一步把Rd从0.1提到0.8，抖动幅度明显下降，但出弯时横向误差短暂增大。第二步把R从0.05提到0.2，控制量进一步平滑，误差增加约0.02米，可接受。第三步把Np从20降到12，求解时间从180ms降到80ms，跟踪误差没有明显恶化。最终参数是Np=12，R=diag([0.2, 0.2])，Rd=diag([0.8, 0.8])，横向误差保持在0.10米内，控制量平滑。
这套参数组合的关键是：增量权重高于控制权重，控制权重高于状态偏差权重里不关心的维度。如果轨迹出现大曲率弯道，Np还要再回调到15。
5. 避坑与排查：五个高频故障和通用调试方法
这章写的全是实操中反复踩过的坑，有一条算一条，都是血泪经验。每个坑都按现象、原因、解决三步写清楚。排查顺序不是从头到尾，而是看你的现象对号入座。
5.1 求解器报infeasible
现象：程序跑到某个弯道，求解器返回infeasible，控制量骤变或直接崩溃。
原因：约束边界太紧，状态落在可行域外。最常见的是参考轨迹带了一个不可达的急弯，而delta边界设得太小。其次是参考段不足Np个点，代码没判断就继续求解。
解决：先看轨迹生成代码，确认参考段长度够不够；再把delta边界放宽到±0.6弧度试一下；如果还是不可行，给边界加松弛变量。我给一个通用做法：
# 在build_ltv_mpc_problem的约束尾部加一段松弛 s = cp.Variable(Np, nonneg=True) cost += 1000 * cp.sum(s) constraints.append(U[:, 1] <= bounds["delta_max"] + s)逻辑说明：松弛变量让转角边界在代价足够大时可以轻微突破，代价是偏离约束。1000是松弛惩罚系数，越大越接近硬约束。如果加了松弛还是infeasible，问题多半在线性化工作点离真实状态太远，需要检查线性化时用的state_lin取值。
5.2 控制量高频抖动
现象：转角曲线在接近零的小幅范围内高频摆动，车开起来像蛇形。
原因：R和Rd都太小，优化器认为“多打一点方向”的代价几乎为零，于是每一帧的控制量都在小幅跳动。
解决：先增大Rd，再增大R，这是有顺序的。Rd惩罚的是变化率，能直接压住抖动；R惩罚绝对值，会让控制整体变小。先动Rd，再动R，两个权重交替试探，每次只改一个，观察加速度和转角曲线。
5.3 参考轨迹坐标混乱：全局坐标和车身坐标的换算
现象：直线轨迹跟得很好，换一条倾斜直线就偏，而且误差方向随着轨迹方向改变。
原因：误差计算直接在全局坐标里做差值，没有考虑车身坐标系。MPC的代价函数惩罚的是状态偏差，如果参考段和当前状态在全局坐标下有常量偏差，控制器会一直顶着这个偏差跑。
解决：把参考轨迹投影到车身坐标系再算误差，或者在生成参考段时确保参考起点和当前状态对齐。代码包里的utils.py里通常有坐标变换函数，排查时先确认这个函数被正确调用了。
5.4 实时性不足：Np和求解器选型的矛盾
现象：仿真时每帧要几百毫秒，实车根本跑不起来。
原因：Np设得太大，而且cvxpy每帧重建问题对象，构建开销比求解还大。
解决：先减Np，从20降到12通常能砍掉一半求解时间；求解器切到OSQP并开启early_terminate；每帧重复构建的对象如果结构固定，可以缓存模型只更新数值。最后还有一招，如果还是慢，检查是不是每次都对Np个点重新线性化，实际上参考段变化不大时，可以隔一帧再更新。
5.5 仿真与实车表现不一致
现象：仿真里跟踪完美，换到实车或半实物平台就飘了。
原因：运动学模型在低速下够用，速度一上来，轮胎侧偏被忽略，预测误差被放大。这是模型精度问题，不是参数问题，调Q矩阵救不回来。
解决：低速验证用运动学模型没问题；速度超过40 km/h就换动力学模型，标定轮胎侧偏刚度。实测前把速度、转角、加速度全量记录，和仿真曲线对比，找到误差开始拉大的那个时间点，再去分析是模型还是时间延迟。
5.6 通用调试方法：先开环再闭环
调参前先做一次开环验证，能帮你把模型问题和控制器问题切开。模型错了，再调MPC也没用。常见做法是给一组固定控制量序列，跑完和参考轨迹对比：
def run_open_loop(x0, u_seq, dt, L): """开环验证：固定控制量序列，看模型是否合理""" states = [x0] for u in u_seq: states.append(vehicle_update(states[-1], u, dt, L)) return np.array(states)逻辑说明：开环跑出来的轨迹如果本身就偏离参考很多，说明模型参数有问题，或者参考轨迹不可达。开环轨迹合理，再进闭环调MPC参数。这套流程能省掉一半排查时间。
提示：日志记录这一步别省。哪怕只是往列表里append，跑完再画图，都比只看最终轨迹靠谱。每次调参都要留一份日志，否则隔天自己都忘了之前改了什么。
6. 进阶用法：从固定轨迹推广到变道、停车和动态约束场景
固定轨迹跑通之后，下一步是把MPC从“跟一条线”推到“跟一个场景”。变道和停车是无人驾驶里最常碰到的两个场景，刚好能把这套代码的能力边界摸清楚。
变道的本质是参考轨迹不再固定不变，而是实时生成。生成方式不复杂，双曲正切函数或者五次多项式都行，关键是在每帧生成参考轨迹段时，把变道曲线切进来。MPC本身不用改，改的是轨迹生成模块。你要注意参考轨迹的速度序列也要同步变化，只换位置不换速度，会导致变道时加速度跳变。
停车的本质是末端约束。在预设的停车点前，把参考速度逐步降为零。但这里有个陷阱：代价函数惩罚的是状态偏差，接近终点时位置权重会强行拉低车速，结果可能出现车还没到点就几乎停住，然后又加速，形成反复调整。要诀是加一个末端状态约束：v[Np] == 0，同时把最后几帧的加速度限制为负值。我在实际写的时候喜欢在轨迹生成器里直接生成带末端刹车的速度曲线，而不是靠MPC强行收敛。
动态约束也值得试。代码包里边界参数目前是固定常量，你可以在每个弯道前动态调整速度上限，比如根据曲率计算允许的最大速度，再折算成加速度边界注入MPC。这比全局限制最高速度更能发挥MPC的潜力——它本来就是为了带约束优化设计的，你把约束变活了，它反而跑得更好。
从那以后，我每拿到一套MPC代码，第一件事永远是先把参考轨迹、车辆模型、约束边界完整写在笔记里，再开始跑循环。这个习惯救了我很多次，至少不会出现改了三天参数，最后发现是轴距填错的情况。希望帮到你。
本文还有配套的精品资源，点击获取
