简介基于运动学车辆模型的开放式驾驶MPC横向控制算法完整MATLAB源程序主要面向自动驾驶、智能车路径跟踪方向的毕业设计与课程设计人群。资源共104个文件其中97个.m文件构成核心代码涵盖主程序、轨迹生成、代价函数计算、MPC求解等模块另有4个png结果示意图、2个asv备份文件及1个md说明文档便于对照运行效果与理解代码逻辑。压缩包整体仅1.4MB结构紧凑。目前已有198人学习浏览。程序基于运动学自行车模型描述车辆运动通过模型预测控制优化横向跟踪误差实现车道保持与预定路径跟踪同时开放式驾驶框架使算法可根据实时路况调整控制输入。通过研读源码读者可掌握MPC预测时域设计、约束处理及代价函数调参等关键技术快速复现仿真结果并迁移至自己的控制方案具有较好的工程参考价值。1. 基于运动学车辆模型的开放式MPC横向控制一套能直接跑通的路径跟踪实现这套源程序的核心是用运动学车辆模型也就是常说的自行车模型做模型预测控制MPC解决无人车沿参考路径行驶时的横向控制问题。和市面上大量基于动力学模型的MPC方案不同它把车辆简化为一个不考虑轮胎力和侧偏特性的几何模型换来的是更低的建模成本和更快的求解速度。程序包里的main_path_tracking.m是主入口配合calc_mpc.m、solve_mpc_problem.m、Autopilot_cost_function.m和generate_trajref.m组成了一个从参考轨迹生成、模型预测、代价函数定义到控制器求解的完整闭环。对正在做毕业设计或刚接触MPC横向控制的从业者来说这套代码的价值不在于算法多先进而在于它把MPC最核心的“预测、优化、滚动执行”讲得很清楚而且能直接在MATLAB里跑通方便改参数做对比实验。2. 运动学车辆模型与横向控制问题建模2.1 为什么用运动学模型而不是动力学模型车辆横向控制首先面对的问题是“拿什么模型来预测车辆行为”。动力学模型需要轮胎侧偏刚度、转动惯量、质心位置等参数这些参数要么查不到准确值要么随车速和路面附着变化标定成本很高。运动学模型把车辆简化为一个刚体认为车轮始终沿其指向方向运动忽略了轮胎侧偏角。在大曲率、低速工况下动力学模型和运动学模型的预测结果差别很小。但运动学模型的优势非常明显状态量少通常只有位置、航向角、速度、前轮转角模型结构简单微分方程容易写离散化后带入MPC优化问题不会带来过重的非线性负担。这个程序里采用的就是典型的运动学自行车模型状态方程可以写成% 运动学自行车模型状态方程 % 状态: x, y 为车辆后轴中心位置; psi 为航向角; v 为纵向速度; delta 为前轮转角 % 控制输入: a 为纵向加速度; omega 为前轮转角变化率 dxdt(1) v * cos(psi); % x 方向速度 dxdt(2) v * sin(psi); % y 方向速度 dxdt(3) v * tan(delta) / L; % 航向角变化率L 为轴距 dxdt(4) a; % 纵向加速度 dxdt(5) omega; % 前轮转角控制量变化率代码逻辑说明这里把前轮转角delta作为状态而不是直接作为控制输入控制量变成了前轮转角变化率omega。这么做的好处是MPC求解出的控制序列不会出现转角突变实际执行时更平滑。状态方程中dxdt(3)是关键它把前轮转角与航向角变化率联系起来是整个横向控制模型的核心。2.2 横向控制问题的数学描述横向控制的目标是让车辆后轴中心的位置(x, y)和航向角psi尽量贴近参考轨迹。代码里generate_trajref.m就是负责生成这条参考轨迹的它输出的通常是一个包含自变量位置和参考航向角的矩阵例如function traj_ref generate_trajref(t_horizon, dt, v_ref) % 生成参考轨迹这里以直线 正弦扰动为例实际可替换为双移线、圆轨迹等 N t_horizon / dt; traj_ref zeros(N, 3); % 每一列x_ref, y_ref, psi_ref for i 1:N t i * dt; traj_ref(i, 1) v_ref * t; % x 参考位置 traj_ref(i, 2) 0.5 * sin(0.2 * t); % y 参考位置横向偏移曲线 traj_ref(i, 3) atan2(0.1 * cos(0.2 * t), v_ref); % 参考航向角 end end参数说明t_horizon是预测时域的总时长dt是采样时间v_ref是参考纵向速度。traj_ref的第二列采用sin函数是为了让代码在初始状态就能看到明显的横向跟踪效果便于验证MPC的收敛能力。如果你要换成实际工况比如高速公路换道可以把第二列改成阶跃或斜线组合。2.3 模型离散化和预测模型构建连续状态方程不能直接用于MPC优化需要离散化。工程上常用的方法是一阶欧拉离散虽然精度不如龙格库塔法但对于运动学模型在dt较小时通常取0.05到0.2秒误差完全可接受而且计算量小。离散后的预测模型在每次控制周期从当前状态开始通过迭代状态方程往后推Np步得到预测轨迹function X_pred predict_trajectory(x0, u_seq, L, dt, Np) % 输入: x0 当前状态向量, u_seq 控制序列 [a; omega], L 轴距, dt 采样时间, Np 预测步数 X_pred zeros(Np, 5); % 预测的状态轨迹 xk x0; for k 1:Np a u_seq(1, k); omega u_seq(2, k); % 欧拉离散 xk(4) xk(4) a * dt; xk(3) xk(3) (xk(4) * tan(xk(5)) / L) * dt; xk(1) xk(1) xk(4) * cos(xk(3)) * dt; xk(2) xk(2) xk(4) * sin(xk(3)) * dt; xk(5) xk(5) omega * dt; X_pred(k, :) xk; end end这段代码建议照着calc_mpc.m内部实现的思路去理解。predict_trajectory前四行看似只是把状态方程按顺序迭代了一遍但顺序有讲究先更新速度再用更新后的速度更新航向角接着更新位置最后更新转角。如果把位置更新放在速度更新前面预测轨迹在高加速度工况下会出现偏差。这也是常见实现里容易踩坑的地方。3. MPC代价函数设计与参考轨迹适配横向控制的核心优化目标3.1 代价函数的层次跟踪精度、控制能量与平滑度MPC每次求解的本质是在满足约束的前提下找一组控制输入序列使得未来Np步内车辆状态与参考轨迹的偏差最小。这个“偏差”怎么量化完全由代价函数决定。Autopilot_cost_function.m实现的就是这个函数它的结构一般分为两层。第一层是跟踪误差代价针对横向位置偏差、航向角偏差、速度偏差分别加权第二层是控制代价针对加速度和转角变化率进行惩罚。这两个层次缺一不可如果只惩罚跟踪误差控制序列会剧烈震荡如果控制代价权重过大车辆又会缓慢地偏离轨迹。3.2 Autopilot_cost_function.m 的代价函数结构典型实现如下function cost Autopilot_cost_function(u_seq, x0, traj_ref, L, dt, Np, weights) % 拆解权重 q_y weights.q_y; % 横向位置偏差权重 q_psi weights.q_psi; % 航向角偏差权重 q_v weights.q_v; % 速度偏差权重 r_a weights.r_a; % 加速度权重 r_omega weights.r_omega; % 转角变化率权重 X_pred predict_trajectory(x0, u_seq, L, dt, Np); cost 0; for k 1:Np dy X_pred(k, 2) - traj_ref(k, 2); % 横向偏差 dpsi wrapToPi(X_pred(k, 3) - traj_ref(k, 3)); % 航向角偏差注意角度归一化 dv X_pred(k, 4) - traj_ref_velocity(k); % 速度偏差 cost cost q_y * dy^2 q_psi * dpsi^2 q_v * dv^2; end % 控制代价 for k 1:Np cost cost r_a * u_seq(1,k)^2 r_omega * u_seq(2,k)^2; end end这里有几个容易被忽略的细节。第一wrapToPi非常重要如果不把航向角偏差归一化到[-pi, pi]当车辆航向与参考方向相差接近2*pi时代价函数会给出一个绝对值巨大的偏差值导致MPC求解出完全错误的控制量这在环形路或连续弯道中会频繁出现。第二横向位置偏差直接平方隐含了“任何方向的横向偏差都不可接受”的假设适合路径跟踪如果是车道保持一般还要在横向偏差上叠加一个预瞄距离或车道中心线偏移量。3.3 权重参数的作用与取值范围权重矩阵的设计直接决定控制品质。下表是这套程序里比较实用的初始权重参考值权重符号含义推荐初始值调节方向q_y横向偏差权重10~50增大可收紧横向跟踪精度但过大容易引起振荡q_psi航向角偏差权重5~20增大可改善弯道中的航向跟随但与q_y需平衡q_v速度偏差权重0.1~1横向控制为主时建议设小值否则会过度干预纵向速度r_a加速度权重0.1~1增大则加速度变化平缓但跟踪响应变慢r_omega转角变化率权重1~10增大则转向更平顺但可能牺牲跟踪精度调参时的一个经验做法是先固定r_omega和r_a只调q_y直到横向偏差在一个可接受的范围再逐步增大q_psi观察弯道表现最后回头重新微调r_omega。程序中weights是一个结构体修改main_path_tracking.m里的初始化部分即可完成设置。3.4 参考轨迹适配从生成到误差计算的闭环generate_trajref.m生成的参考轨迹不只包含位置坐标还包含航向角甚至纵向速度参考。这里有一个关键点MPC在第k步计算代价时使用的是traj_ref(k, :)也就是和预测步序号对应的参考值。如果参考轨迹是定长的而预测时域Np接近轨迹末段就会遇到“参考点不足”的问题。常见做法是在轨迹末尾复制最后一个参考点或者把生成的长度设定为Np 安全余量。这段逻辑通常在calc_mpc.m中通过索引控制实现% 截取当前时刻到未来 Np 步的参考轨迹片段 idx k_current : min(k_current Np - 1, size(traj_ref, 1)); ref_segment traj_ref(idx, :); % 若预测窗口超出轨迹长度则复制末尾点填充 if length(idx) Np ref_segment [ref_segment; repmat(traj_ref(end,:), Np - length(idx), 1)]; end这里的k_current是当前仿真步序号。复制末尾点的做法虽然简单但如果在轨迹终点附近出现一个大转角MPC可能会因为参考航向角突变而输出剧烈转向。更稳健的做法是在轨迹生成阶段就规划好末段直线的航向角这也是实际工程中参考轨迹设计的一个常见要求。4. MPC求解器与闭环控制流程从优化问题到方向盘转角4.1 为什么用fmincon而不是其他求解器MPC控制器的最后一步是求解优化问题。这个程序通过solve_mpc_problem.m完成。运动学模型的MPC问题是非线性优化问题因为预测模型里有cos、sin、tan等非线性项。和线性MPC不同它不能预先转化为二次规划QP求解。在MATLAB环境中处理这类问题最直接的工具是fmincon用于求解带约束非线性多元函数最小值的函数。它支持非线性约束、边界约束还能通过指定梯度和 Hessian 加速收敛。对于运动学MPC这种决策变量维数不高控制时域Nc通常为5到20变量数在10到40之间的问题fmincon的响应速度足够支撑1:10以上的实时仿真。solve_mpc_problem.m中典型调用方式如下function u_opt solve_mpc_problem(x0, traj_ref, params) % 定义决策变量初值u_seq 包含两个控制量序列 (a, omega) Nc params.Nc; % 控制时域 u0 zeros(2, Nc); % 初始控制序列 % 变量上下界约束加速度 [-3, 3] m/s^2转角变化率 [-0.5, 0.5] rad/s lb [-3 * ones(1, Nc); -0.5 * ones(1, Nc)]; ub [ 3 * ones(1, Nc); 0.5 * ones(1, Nc)]; % 调用 fmincon 求解 options optimoptions(fmincon, Algorithm, sqp, MaxIterations, 50, ... MaxFunctionEvaluations, 2000, Display, off); u_opt fmincon((u) Autopilot_cost_function(u, x0, traj_ref, ... params.L, params.dt, params.Np, params.weights), ... u0, [], [], [], [], lb, ub, [], options); end参数说明Nc是控制时域表示MPC只优化前Nc步的控制输入Np是预测时域两者不相等。两者的关系是MPC算法的关键设计点预测长度决定控制器“看得有多远”控制长度决定“这一段路要走得多细”。fmincon的Algorithm选sqp序列二次规划是因为它处理非线性约束和边界约束时比interior-point更容易从不良初值收敛。4.2 SQP求解器的收敛特性与初值选择用fmincon做MPC求解最大的坑在于初值u0。MPC是滚动优化的上一时刻的解u_opt就是本时刻最好的初值。代码里如果每次都用零序列作为初值在较大曲率的弯道上很容易陷入局部极小或求解失败。工程上推荐把上一时刻的最优解做一次“移位”作为当前时刻初值% 用上一周期解作为当前初值热启动 u_init zeros(2, Nc); % u_prev 是上一次控制周期的解此处向左移动一位并复制最后一位 u_init(1, 1:end-1) u_prev(1, 2:end); u_init(1, end) u_prev(1, end); u_init(2, 1:end-1) u_prev(2, 2:end); u_init(2, end) u_prev(2, end);注意这里u_prev是上一周期的u_opt。热启动可以显著改善实时性在fmincon的MaxIterations被限制到50次以内时尤其重要。另一个细节是控制量边界lb、ub不要设得太紧否则优化器在障碍约束上花太多迭代次数。4.3 闭环仿真主流程main_path_tracking.m 的架构main_path_tracking.m是整个程序的主控脚本它把模型、代价函数、求解器串起来。仿真过程本身就是一个典型的MPC滚动时域循环% main_path_tracking.m 核心流程 x0 [0; 0; 0; v0; 0]; % 初始状态 [x, y, psi, v, delta] traj_ref generate_trajref(T_sim, dt, v_ref); % 全仿真时长的参考轨迹 u_prev zeros(2, Nc); % 初始控制序列 x_log zeros(N_steps, 5); % 日志存储 for k 1:N_steps % 1. 截取当前时刻之后的参考轨迹段 ref_seg traj_ref(k:min(kNp-1, end), :); % 2. 调用MPC求解当前最优控制序列 u_opt solve_mpc_problem(x0, ref_seg, params); % 3. 执行第一个控制量并更新车辆状态模拟车辆实际响应 a_cmd u_opt(1, 1); omega_cmd u_opt(2, 1); x0 vehicle_dynamics_update(x0, a_cmd, omega_cmd, L, dt); % 4. 记录状态并保留控制序列用于热启动 u_prev u_opt; x_log(k, :) x0; end这段代码的vehicle_dynamics_update就是前面2.1节状态方程的离散化版本它模拟了真实车辆在收到控制指令后的响应。这里的关键是“只执行第一个控制量”随后重新求解这正是MPC滚动优化Receding Horizon的本质。很多初学者会误把整个u_opt序列都执行完再求解下一轮那其实就退化成了开环最优控制抗干扰能力会大幅下降。4.4 仿真步数与实时性的权衡程序中的N_steps由T_sim / dt计算控制周期等于仿真步长dt。dt的选择直接影响求解实时性采样周期dt预测时域Np20物理时长求解压力适用场景0.05s1.0s高fmincon可能超时高速工况或硬件在环0.10s2.0s中等推荐初始调试城市道路、弯道仿真0.20s4.0s低低速泊车、概念验证在普通PC上Np20, Nc10, dt0.1时单步求解耗时在50到150毫秒之间接近实时运行。如果换成dt0.05单步求解时间可能超过控制周期这时就需要压缩Np或Nc了。5. 横向控制调参与验证把Np、权重和约束改对才有实用价值MPC项目里最尴尬的问题是“波形看起来是收敛的但真跑起来方向盘在抖”根因大多不是算法错误而是参数和约束配置脱离了车辆实际执行能力。先说预测时域Np的选择。Np太短比如小于10MPC只看得到眼前的路遇到弯道会晚打方向Np太长比如超过40优化问题的非线性程度增加fmincon容易陷入局部最优求解时间也会涨到不可控的程度。折中方案是让Np * dt约等于车速行驶两秒的距离。比如车速10m/sdt0.1sNp20刚好覆盖2秒的行驶距离这是比较好的初始选择。控制时域Nc则不必设置得和Np一样长。Nc通常取Np的1/2到1/3因为距离越远的控制量对未来轨迹的影响越弱而且减少Nc能显著降低决策变量维数。程序默认的Nc10、Np20就是这一原则的体现。权重参数里最容易出问题的不是q_y而是r_omega和转角变化率约束的配合。当r_omega过小且omega边界较大时MPC给出的转向命令会出现频繁切换方向的现象表现在仿真图上就是方向盘角度锯齿状跳变。此时应该先看omega轨迹是否触碰边界如果频繁触碰边界但控制量没有振荡考虑减小omega_cmd的边界值从0.5降到0.3如果没触碰边界但输出有高频抖动则把r_omega调大2到3倍。对于q_psi和q_y的平衡一个值得尝试的技巧是让q_psi与车速正相关。低速时以横向位置跟踪为主高速时以航向跟踪为主可以写成q_psi 0.5 * q_y * v / v_max。这也是代码里q_psi不设为常数而是随速度变化的原因之一。验证方法上除了看横向偏差曲线还要看两个细节第一是优化器退出标志fmincon的exitflag如果频繁等于0或负数说明求解失败对应的控制量不能执行需要在solve_mpc_problem.m里加入对exitflag的判断并回退到上一时刻的有效解第二是控制序列首尾是否平滑如果相邻两个控制周期的u_opt(1,1)突变量超过执行机构物理限制就应该进一步减小omega的边界而不是继续调大r_omega。最后一个建议是在跑main_path_tracking.m之前先单独测试generate_trajref.m生成的轨迹是否满足曲率连续。运动学MPC对参考轨迹的曲率突变非常敏感如果参考路径的航向角在某个点出现尖角即便权重调得再好车辆也会在该点出现明显的横向超调。如果轨迹是手工拼接的最好先用smooth函数或样条插值对航向角做一次平滑处理这也是很多开源方案里没有提到但实际调试时很关键的一步。本文还有配套的精品资源点击获取