简介这份资源面向无人驾驶与自动控制方向的学习者和开发者提供模型预测控制MPC的完整程序代码实现帮助理解车辆路径规划、速度控制与转向操作中的预测优化逻辑。压缩包共16个文件约38KB以13个m脚本为主体辅以2个txt说明文档和1个mdl仿真模型覆盖车辆动力学建模、预测框架、优化求解、滚动优化及仿真测试等关键环节并涉及Pacejka轮胎模型等具体实现。目前已有1069人学习下载适合具备一定控制理论基础与编程能力、希望深入掌握MPC在无人驾驶中应用的中高级读者。通过研读这些代码读者可对照章节模块理解线性化与非线性模型差异、数值积分方法、约束条件处理及参数调优思路为实际系统设计提供可复用的参考实现。1. 从仿真到实车这套 MPC 代码到底能跑出什么效果如果你正在做无人驾驶轨迹跟踪相关的课题或项目大概率绕不开模型预测控制MPC这个词。但真正让人头疼的不是理解 MPC 的数学推导而是找到一份能直接跑起来、结构清晰、方便改参数的代码。我手里这份无人驾驶车辆模型预测控制程序代码就是冲着这个痛点来的。它用 Python 实现了一套完整的 MPC 控制器覆盖车辆运动学建模、预测模型构建、滚动优化求解、控制量输出这条完整链路配套有仿真环境和可视化脚本。适合谁正在做路径跟踪算法验证的开发者、需要快速搭建 MPC 原型的学生、以及想把 MPC 从仿真往实车迁移的工程师。代码不依赖复杂的自动驾驶框架装好 NumPy、SciPy、Matplotlib 就能跑这一点对新手非常友好。2. 项目结构与技术选型为什么用 Python 而不是 C2.1 目录结构一览拿到代码后先别急着跑花两分钟把目录结构看清楚后面改起来会省很多事。整个项目的组织方式比较扁平没有过度工程化核心文件就几个mpc_vehicle_control/ ├── main.py # 主入口跑仿真循环 ├── mpc_controller.py # MPC 控制器核心实现 ├── vehicle_model.py # 车辆运动学/动力学模型 ├── reference_path.py # 参考轨迹生成 ├── utils.py # 工具函数角度归一化、绘图等 ├── config.py # 参数配置集中管理 └── simulation/ ├── simulator.py # 仿真环境封装 └── visualize.py # 可视化与动画这个结构的好处是职责分明vehicle_model.py只管车辆怎么动mpc_controller.py只管怎么算控制量main.py负责把两者串起来。你要换车辆模型只动一个文件要换优化求解器也只动一个文件。常见做法是把参数散落在各个文件里但这个项目把关键参数都抽到了config.py改起来不用满项目搜。2.2 技术栈与依赖项目用的是 Python 3.8核心依赖只有三个依赖用途版本建议NumPy矩阵运算、状态向量操作≥1.21SciPy调用优化求解器SLSQP≥1.7Matplotlib轨迹绘制与动画≥3.4没有用 CasADi 或 OSQP 这类专门的优化库而是直接用scipy.optimize.minimize做滚动优化。这样做的好处是依赖少、安装快缺点是求解速度不如专用 QP 求解器。对于仿真验证和算法理解来说够用了如果你要上实车跑高频率控制后面我会讲怎么替换求解器。2.3 为什么选运动学模型而不是动力学模型代码默认用的是车辆运动学自行车模型状态量是[x, y, θ, v]控制量是[a, δ]加速度和前轮转角。这个选择很务实运动学模型在低速场景下精度足够而且状态维度低MPC 的优化问题规模小求解快。如果你做的是高速避障或极限工况那确实需要换成动力学模型但那是另一个复杂度级别的事了。运动学模型的离散递推公式在vehicle_model.py里import numpy as np def kinematic_bicycle_model(state, control, dt, L2.5): 车辆运动学自行车模型离散递推 state: [x, y, theta, v] control: [a, delta] 加速度, 前轮转角 dt: 采样时间 L: 轴距 x, y, theta, v state a, delta control # 欧拉法离散化 x_next x v * np.cos(theta) * dt y_next y v * np.sin(theta) * dt theta_next theta v / L * np.tan(delta) * dt v_next v a * dt # 角度归一化到 [-pi, pi] theta_next (theta_next np.pi) % (2 * np.pi) - np.pi return np.array([x_next, y_next, theta_next, v_next])这里有几个细节值得注意。L2.5是轴距默认值对应一辆紧凑型轿车你可以根据实际车辆改。角度归一化那行很容易被忽略但不做的话 theta 会一直累加导致 cos/sin 计算出问题。欧拉法离散化在 dt 较小比如 0.1s时精度够用如果你把 dt 加到 0.5s 以上建议换成龙格库塔法。3. MPC 控制器核心拆解预测、优化、滚动执行3.1 预测模型的构建MPC 的核心思想是在每个控制周期基于当前状态预测未来 N 步的系统行为然后求解一个优化问题只执行第一步控制量下一步再重新预测和优化。这个“滚动”的过程在mpc_controller.py里实现。预测模型就是把运动学模型往前推 N 步def predict_trajectory(self, state, control_sequence): 根据控制序列预测未来 N 步的状态轨迹 state: 当前状态 [x, y, theta, v] control_sequence: shape (N, 2) 的控制序列 返回: shape (N1, 4) 的状态轨迹 predicted_states [state] current_state state.copy() for i in range(self.horizon): current_state kinematic_bicycle_model( current_state, control_sequence[i], self.dt, self.wheelbase ) predicted_states.append(current_state) return np.array(predicted_states)horizon是预测时域默认设的是 20 步配合 dt0.1s相当于预测未来 2 秒。这个参数很关键太短了车辆反应不过来太长了优化问题规模大、求解慢。我一般会从 15 到 30 之间试根据车速调整——车速快就取大一点。3.2 代价函数的设计代价函数决定了 MPC 的“性格”。这个项目用的代价函数包含三部分跟踪误差、控制量变化率、控制量幅值。def cost_function(self, control_flat, state, ref_trajectory): 代价函数跟踪误差 控制平滑 控制幅值 control_flat: 展平后的控制序列 state: 当前状态 ref_trajectory: 参考轨迹点 control_sequence control_flat.reshape(self.horizon, 2) predicted_states self.predict_trajectory(state, control_sequence) cost 0.0 for i in range(self.horizon): # 跟踪误差位置偏差 航向偏差 dx predicted_states[i1][0] - ref_trajectory[i][0] dy predicted_states[i1][1] - ref_trajectory[i][1] dtheta predicted_states[i1][2] - ref_trajectory[i][2] dtheta (dtheta np.pi) % (2 * np.pi) - np.pi cost self.w_pos * (dx**2 dy**2) cost self.w_theta * dtheta**2 # 控制平滑相邻控制量之差 if i 0: da control_sequence[i][0] - control_sequence[i-1][0] ddelta control_sequence[i][1] - control_sequence[i-1][1] cost self.w_smooth * (da**2 ddelta**2) # 控制幅值惩罚 cost self.w_control * (control_sequence[i][0]**2 control_sequence[i][1]**2) return cost权重参数w_pos、w_theta、w_smooth、w_control都在config.py里。调参的血泪经验是先调位置权重让跟踪跟上再调航向权重消除震荡最后加平滑权重让控制量别抖。如果控制量抖得厉害优先加w_smooth如果跟踪滞后加w_pos。3.3 滚动优化与约束处理优化求解用的是 SciPy 的 SLSQP支持等式和不等式约束。代码里对控制量做了幅值约束from scipy.optimize import minimize def solve(self, state, ref_trajectory): 求解 MPC 优化问题 返回最优控制序列的第一步 # 初始猜测全零控制 u0 np.zeros(self.horizon * 2) # 控制量边界约束 bounds [] for _ in range(self.horizon): bounds.append((-self.max_accel, self.max_accel)) # 加速度 bounds.append((-self.max_steer, self.max_steer)) # 前轮转角 # 求解 result minimize( self.cost_function, u0, args(state, ref_trajectory), methodSLSQP, boundsbounds, options{maxiter: 100, ftol: 1e-4} ) if not result.success: # 求解失败时返回上一帧控制量或零控制 print(fMPC 求解失败: {result.message}) return np.array([0.0, 0.0]) optimal_control result.x.reshape(self.horizon, 2) return optimal_control[0] # 只执行第一步max_accel默认 2.0 m/s²max_steer默认 0.5 rad约 28.6°。这两个值要根据你的车辆实际能力改设太大了优化问题可能无解设太小了车辆响应不够。maxiter100对 20 步时域来说一般够用如果你发现经常求解失败先检查约束是不是太紧。注意SLSQP 是局部优化算法对初始猜测敏感。如果求解经常失败可以把初始猜测改成上一帧的最优解而不是全零。4. 跑起来从安装依赖到看到轨迹图4.1 环境准备与依赖安装先把环境弄好。建议用虚拟环境避免和系统 Python 冲突# 创建虚拟环境 python -m venv venv # 激活Linux/Mac source venv/bin/activate # 激活Windows venv\Scripts\activate # 安装依赖 pip install numpy scipy matplotlib如果你用的是 Anaconda也可以直接conda create -n mpc python3.9然后激活。依赖就三个装起来很快。4.2 配置参数怎么改跑之前先看一眼config.py把关键参数过一遍# config.py 关键参数 class MPCConfig: # 预测时域 horizon 20 # 采样时间 (s) dt 0.1 # 轴距 (m) wheelbase 2.5 # 最大加速度 (m/s^2) max_accel 2.0 # 最大前轮转角 (rad) max_steer 0.5 # 代价函数权重 w_pos 10.0 # 位置跟踪权重 w_theta 5.0 # 航向跟踪权重 w_smooth 1.0 # 控制平滑权重 w_control 0.1 # 控制幅值权重第一次跑建议先用默认值看到效果后再调。如果你想跑圆形轨迹把reference_path.py里的轨迹类型改成circle想跑直线就改成straight。4.3 运行仿真与结果解读直接跑主程序python main.py跑完之后会弹出一个窗口显示三张图第一张是 XY 平面上的参考轨迹和实际轨迹对比第二张是跟踪误差随时间的变化第三张是控制量加速度和前轮转角随时间的变化。怎么看结果好不好先看第一张图实际轨迹和参考轨迹基本重合就说明跟踪效果不错。再看第二张图位置误差应该收敛到 0.1m 以内航向误差在 0.05rad 以内。如果误差震荡或者发散大概率是权重没调好或者预测时域太短。第三张图看控制量是否平滑如果加速度在正负之间高频切换说明w_smooth太小了。4.4 换个参考轨迹试试想验证控制器对不同轨迹的适应性改reference_path.py里的轨迹生成函数就行。比如生成一个正弦形轨迹def generate_sine_path(num_points200, amplitude3.0, frequency0.05): 生成正弦形参考轨迹 amplitude: 振幅 (m) frequency: 频率 x np.linspace(0, 100, num_points) y amplitude * np.sin(frequency * x) # 计算航向角轨迹切线方向 dy np.gradient(y, x) theta np.arctan(dy) # 速度设为恒定 v np.ones(num_points) * 5.0 return np.column_stack([x, y, theta, v])把main.py里的轨迹生成调用换成这个函数再跑一遍看看控制器能不能跟上。正弦轨迹的曲率变化比较剧烈如果跟踪效果不好可以适当增大horizon或者降低车速。5. 避坑与排查那些让我调了一整晚的问题5.1 求解失败控制量输出为零现象仿真跑着跑着车辆突然不动了终端打印“MPC 求解失败”。原因SLSQP 在约束边界附近容易找不到可行解尤其是参考轨迹曲率突变的时候。另外初始猜测全零也可能导致求解器陷入局部不可行区域。解决把初始猜测改成上一帧的最优控制序列这样求解器从一个已知可行的点出发成功率高很多。具体做法是在控制器里维护一个last_solution变量每次求解前用它来初始化。5.2 轨迹跟踪震荡车辆左右摇摆现象实际轨迹在参考轨迹两侧来回摆动控制量高频切换。原因位置权重w_pos太大而平滑权重w_smooth太小控制器过于激进地纠正误差导致过冲和震荡。解决先把w_smooth从 1.0 加到 5.0 甚至 10.0观察震荡是否减弱。如果减弱了但跟踪变慢再适当加w_pos。调参顺序一定是先平滑后跟踪反过来会越调越乱。5.3 角度跳变导致跟踪误差计算错误现象车辆航向角在 ±π 附近时跟踪误差突然变得很大控制器输出异常。原因角度没有归一化比如参考航向是 3.14实际航向是 -3.14直接相减得到 6.28但实际误差只有 0.003。解决所有涉及角度差值的地方都要做归一化公式是(dtheta pi) % (2*pi) - pi。这个坑我在三个不同的项目里都踩过现在写代码第一件事就是检查角度处理。5.4 仿真速度慢跑一次要等很久现象main.py跑一次要几十秒甚至几分钟调参效率极低。原因SLSQP 每次求解都要数值计算梯度20 步时域就是 40 个优化变量计算量不小。而且 Python 循环本身也慢。解决短期方案是把horizon从 20 降到 10求解速度能快一倍多。长期方案是换用 CasADi 或 OSQP 这类带自动微分和稀疏求解的库速度能提升一个数量级。如果只是调参还可以把maxiter从 100 降到 50。5.5 换车辆参数后轨迹完全不对现象改了wheelbase或dt之后车辆行为变得很奇怪甚至画出来的轨迹是发散的。原因dt和wheelbase是耦合的。运动学模型里v/L*tan(delta)*dt这一项如果 dt 变大而 L 没变航向角变化会过大导致模型失真。另外dt改了之后参考轨迹的速度也要相应调整。解决改dt的时候同步检查horizon * dt是否还在合理范围一般 1 到 3 秒。改wheelbase的时候确认max_steer是否还合理——轴距大的车转弯半径大同样的前轮转角对应的航向变化更小。6. 二次开发换求解器、加约束、上实车6.1 替换优化求解器如果你要把这套代码往实车迁移SLSQP 的速度肯定不够。我一般会换成 OSQP它是专门解二次规划问题的速度快而且有 Python 接口。改造思路是把代价函数写成二次型形式约束写成线性形式然后调用 OSQP 求解。具体来说把mpc_controller.py里的solve方法替换掉保持输入输出接口不变这样main.py不用改。import osqp import scipy.sparse as sp def solve_osqp(self, state, ref_trajectory): 用 OSQP 求解 MPC 问题 需要把代价函数和约束写成标准 QP 形式 # 构建 Hessian 矩阵和梯度向量 # 这里省略具体推导核心是把代价函数展开成 0.5 * x^T * P * x q^T * x P sp.csc_matrix(self._build_hessian()) q self._build_gradient(state, ref_trajectory) # 约束控制量上下界 A sp.eye(self.horizon * 2, formatcsc) lower np.tile([-self.max_accel, -self.max_steer], self.horizon) upper np.tile([self.max_accel, self.max_steer], self.horizon) # 求解 prob osqp.OSQP() prob.setup(P, q, A, lower, upper, verboseFalse) res prob.solve() if res.info.status ! solved: return np.array([0.0, 0.0]) optimal_control res.x.reshape(self.horizon, 2) return optimal_control[0]替换之后同样的 horizon 和 dt求解时间能从几十毫秒降到几毫秒实车跑 10Hz 控制频率没问题。6.2 添加避障约束原始代码只做了控制量约束没有考虑障碍物。要加避障可以在代价函数里加一个惩罚项或者在约束里加不等式约束。惩罚项的做法更简单对每个预测步计算车辆到最近障碍物的距离如果小于安全距离就加大惩罚。def add_obstacle_penalty(self, predicted_states, obstacles, safe_dist2.0): 避障惩罚项 obstacles: 障碍物位置列表 [(x1,y1), (x2,y2), ...] safe_dist: 安全距离 penalty 0.0 for state in predicted_states: for obs in obstacles: dist np.sqrt((state[0]-obs[0])**2 (state[1]-obs[1])**2) if dist safe_dist: penalty 1000.0 * (safe_dist - dist)**2 return penalty这个惩罚项加到代价函数里就行。注意惩罚系数要给得足够大否则优化器会“冒险”穿过障碍物。但也不能太大太大会导致数值问题。6.3 实车部署的注意事项从仿真到实车有几个地方必须改。第一状态估计不能用仿真里的真值要接 GPS/IMU 或者 SLAM 的输出而且要考虑噪声和延迟。第二控制量输出要经过执行器的死区补偿和限幅仿真里加速度说给多少就给多少实车油门刹车有响应延迟。第三加一个安全监控模块当 MPC 求解失败或者跟踪误差超过阈值时自动切换到紧急制动。我自己的习惯是每次改完控制器参数先在仿真里跑三组不同曲率的轨迹确认跟踪误差都在阈值内再上车测试。上车第一件事不是跑轨迹而是原地打方向盘验证前轮转角标定对不对。这个习惯帮我省了好几次现场返工。希望帮到你。本文还有配套的精品资源点击获取