简介面向自动驾驶与车辆控制领域的学习者与研发人员这份Matlab源码围绕三辆车的队列协同给出分布式模型预测控制的实现示例可用于理解多车队列在复杂交通环境下的稳定性与协同性设计。压缩包共2个文件体积约2KB其中.m文件承载核心算法与仿真入口.txt文件提供配套说明便于快速定位运行方式与参数含义。目前已有366人学习下载。读者可借此梳理分布式MPC在多车系统中的建模、预测、滚动优化与约束处理思路观察三车如何通过局部信息交互实现队列协调并在此基础上调整权重、预测时域或通信拓扑开展复现实验说明文档也有助于减少环境配置和代码阅读中的障碍。适合作为课程设计、算法预研和智能交通系统构建的轻量参考。1. 三辆车跟车为什么头车一脚刹车后面就乱某实验室的三车队列测试里头车以 60 km/h 巡航前方突然降到 40第二辆车在 0.8 秒后跟上减速第三辆车已经贴到 3 米以内——这是典型的队列纵向协同失效。集中式模型预测控制算得准但三辆车一个 QP、通信全连通扩到八辆车计算量就爆炸。这个 MATLAB 源码包做的事情很直接把一个大优化问题拆成三个小 QP用分布式模型预测控制DMPC解三车队列的间距保持与速度协同。它适合做自动驾驶纵向控制、车辆编队、分布式控制课程设计的人跑通之后能清楚看到每辆车的预测时域、约束边界和邻居信息是怎么进代价函数的。2. 三车队列的骨架目录结构、车辆模型与参数配置2.1 目录结构与各文件职责拿到源码包先看目录比急着按运行键有用。这类队列控制项目一般按「模型 / 优化 / 仿真」三层切分每层只干一件事改动的时候不容易牵一发动全身。我拆过的几个版本基本都长这样platoon_dmpc/ ├── main.m 仿真入口跑完直接出图 ├── init_params.m 所有参数集中在这里改车队规模改这里 ├── model/ │ ├── veh_model.m 三阶纵向动力学 离散化 │ └── spacing_policy.m 期望间距策略固定间距 / 固定时距 ├── mpc/ │ ├── build_prediction.m 构造预测矩阵 Phi、Gamma │ ├── build_qp.m 代价函数展开成标准二次型 │ ├── solve_dmpc.m 迭代协调 quadprog 求解 │ └── neighbor_info.m 从邻车取预测加速度序列 ├── sim/ │ ├── run_closed_loop.m 滚动时域主循环 │ └── plot_result.m 间距、速度、加速度三张图 └── config/ └── topo_3veh.m 三车通信拓扑与权重数据流是单向的main.m调init_params.m拿到结构体prm交给run_closed_loop.m循环里每辆车调一次solve_dmpc.m后者先问neighbor_info.m要邻居上一轮的预测加速度再让build_prediction.m和build_qp.m拼出 H、f、A、b扔给quadprog解出来的第一个控制量施加到车上状态推进一步进下一拍。把这条链路记住后面改哪一块心里都有数。2.2 三阶纵向动力学模型与离散化队列控制里车辆模型的选择直接决定仿真像不像真车。二阶模型状态只有位置和速度加速度当输入写起来省事但把执行器滞后整个忽略了做到 0.1 秒采样时后车会明显抖——这是踩过的坑。这个包用的是三阶模型把加速度也当状态输入是「期望加速度」经过一个一阶惯性环节才变成实际加速度时间常数 τ 取 0.5 秒左右接近乘用车纵向执行器的响应。连续时间下三阶模型的写法是位置导数等于速度、速度导数等于加速度、加速度导数等于 (-a u)/τ。离散化用前向欧拉就够采样 0.1 秒时精度完全够用没必要上矩阵指数那样反而让不熟 MATLAB 的人看代码时卡住。function [Ad, Bd, Cd] veh_model(Ts, tau) % 三阶纵向动力学状态 [位置; 速度; 加速度]输入为期望加速度 % 连续方程: dp v, dv a, da (-a u)/tau Ac [0, 1, 0; 0, 0, 1; 0, 0, -1/tau]; Bc [0; 0; 1/tau]; Cc [1, 0, 0]; % 前向欧拉离散Ts 取 0.05~0.1 时误差可接受 Ad eye(3) Ts * Ac; Bd Ts * Bc; Cd Cc; endtau是这套模型里最敏感的参数。设太小0.1 秒等于假设车能瞬间响应仿真里间距曲线漂亮得不真实设太大1.0 秒以上队列会明显迟钝后车容易压到安全距离。我一般先在 0.3 到 0.7 之间扫一遍看间距误差峰值落在哪再定死。Cd只取位置输出因为间距误差最终是用两车位置差算的速度差另外单独取这一点在下一章的增广状态里会讲清楚。2.3 参数配置与期望间距策略参数全部塞在init_params.m里返回一个结构体。这种写法看着土但改起来最省事——换车队规模、换权重、换约束只动一个文件不用满工程搜数字。核心参数如下参数含义典型取值Ts采样周期0.1 stau执行器时间常数0.5 sNp预测时域20 步2 sNc控制时域5 步L车长4.5 md0静止安全间距5 mh固定时距1.2 sQ间距/速度误差权重diag([1, 0.5])R控制量权重0.1S控制增量权重0.05a_lim加速度约束[-3, 2] m/s²u_lim期望加速度约束[-4, 2.5] m/s²期望间距用固定时距CTH比固定间距更贴近实际d_des d0 h * v_i。车速越高跟得越远这符合驾驶习惯也能避免高速时安全裕度不够。spacing_policy.m里就三行但它是整个代价函数的参考值来源符号搞反的话后面所有调试都是白费力气。预测时域 20 步覆盖 2 秒是为了一辆以 60 km/h 行驶的车在 2 秒内能把速度降下 15 m/s 左右这个窗口足够 MPC「看见」前车的减速动作。控制时域压到 5 步是为了减少决策变量个数三辆车每辆 5 个变量QP 规模小单步求解在 10 ms 量级实时性有余量。这个「Np 长、Nc 短」的搭配是队列控制里的常规做法别把两个都设成 20那样求解时间会翻好几倍而效果提升有限。3. 分布式 MPC 的核心预测矩阵、代价函数与 QP 构造3.1 相对状态增广把前车加速度当成可测扰动如果每辆车只优化自己的绝对位置和速度那三辆车就是三个互不相干的 MPC间距根本约束不住。正确做法是把「与前车的相对量」做成状态。定义间距误差e_i p_{i-1} - p_i - L - d_des速度误差dv_i v_{i-1} - v_i再拼上本车加速度a_i和前车加速度a_{i-1}得到一个四维增广状态z_i [e_i; dv_i; a_i; a_{i-1}]前车加速度进状态作用是把它当成一个「可测扰动」。在预测时域内先假设它保持不变写出转移矩阵function [Az, Bz, Cz] aug_model(Ad, Bd, Ts, tau) % 增广状态 z [间距误差; 速度误差; 本车加速度; 前车加速度] Az [1, Ts, -Ts^2/2, Ts^2/2; 0, 1, -Ts, Ts; 0, 0, 1-Ts/tau, 0; 0, 0, 0, 1]; Bz [0; 0; Ts/tau; 0]; Cz [1, 0, 0, 0; % 输出间距误差 0, 1, 0, 0]; % 输出速度误差 endAz的第一行是从运动学推出来的间距误差的变化量等于速度误差乘 Ts再减去两车加速度差带来的二阶项。第二行是速度误差的变化量。第三行是本车加速度自己的一阶惯性收敛。第四行恒等对应「前车加速度在预测窗口内不变」这个假设。Bz只有第三个分量非零因为控制输入只影响本车加速度。这个假设是整个分布式架构的代价前车其实也在动它的加速度在预测窗口里是变的。单靠这一步后车算出来的最优解会偏保守。第 4 章的迭代协调就是专门修这个偏差的别在这里就想着把模型做复杂那样会失去分布式拆分带来的计算优势。3.2 预测方程与 Gamma 矩阵的块构造有了Az、Bz把未来 Np 步的状态写成当前状态加控制序列的线性组合Z Phi * z0 Gamma * U。Phi是 Az 的幂次堆叠Gamma是下三角的分块 Toeplitz 矩阵第 k 行第 j 列块表示第 j 个控制量对第 k 步状态的影响等于Az^(k-j) * Bz。手写双重循环最直观也最不容易写错。当控制时域 Nc 小于预测时域 Np 时Nc 之后的控制量保持为最后一个用递推方式处理function [Phi, Gam] build_prediction(Az, Bz, Np, Nc) n size(Az, 1); m size(Bz, 2); Phi zeros(n*Np, n); Gam zeros(n*Np, m*Nc); for k 1:Np Phi((k-1)*n1 : k*n, :) Az^k; if k 1 Gam(1:n, 1:m) Bz; else % 先整体左乘 Az相当于把上一行块往前推一步 Gam((k-1)*n1 : k*n, :) Az * Gam((k-2)*n1 : (k-1)*n, :); if k Nc % 只有在前 Nc 步内才新增一个决策变量 Gam((k-1)*n1 : k*n, (k-1)*m1 : k*m) Bz; end end end endk从 1 到 Np 逐行填。第一行只有第一个控制块是Bz其余为零。之后每一行先把上一行整体左乘Az再在k Nc时把第 k 个块列设成Bz。这样当k Nc时不再引入新变量控制量自然保持住等价于「最后一个控制量持续施加」。这段代码看着短但它是整个 MPC 里最容易写错的地方。常见错误是把Az的幂次写反或者忘了Nc Np时的保持逻辑结果仿真出来车头的响应总是提前一步。写完拿Np3, Nc2手算一遍前三行去对五分钟能省掉半天的调试。3.3 代价函数展开成标准二次型代价函数是三项之和跟踪间距和速度误差、惩罚控制量幅值、惩罚控制量增量。写成矩阵形式是J (Z - Ref) * Qbar * (Z - Ref) U * Rbar * U (D*U - d0) * Sbar * (D*U - d0)Qbar是按预测步数块对角堆叠的QRbar、Sbar同理。D是差分矩阵用来把绝对控制量变成增量d0里装的是上一拍实际施加的控制量这样第一拍的增量惩罚才有参照。把Z Phi*z0 Gamma*U代进去按 U 整理得到标准形式J 0.5*U*H*U f*U constfunction [H, f] build_qp(Phi, Gam, Cz, Qbar, Rbar, Sbar, z0, ref, D, d0) % 输出矩阵在预测时域上堆叠 Cbar kron(eye(size(Phi,1)/size(Cz,1)), Cz); G Cbar * Gam; PhiY Cbar * Phi; Ref repmat(ref, size(Phi,1)/size(Cz,1), 1); H 2 * (G * Qbar * G Rbar D * Sbar * D); f 2 * (G * Qbar * (PhiY * z0 - Ref) - D * Sbar * d0); % 数值正则化避免 quadprog 报非正定 H H 1e-8 * eye(size(H)); endCbar把每步的二维输出扩展到2*Np维kron加单位阵就能做到比手写循环快。Ref是参考值间距误差的参考恒为 0保持期望间距速度误差的参考也是 0跟上前车速度。H前面乘 2 是因为quadprog求解的是0.5*xHx fx形式两边要对齐这个系数错了的话求解出来的控制量会整体偏大一倍而且从曲线上看不出明显异常只是车跟得特别「急」。最后那行 1e-8 正则化是我后加的。理论上G*Qbar*G是半正定的但浮点运算累积误差会让quadprog偶尔报 exitflag -2加一点点单位阵就能压住代价可以忽略。3.4 约束拼装与 quadprog 调用约束有三类间距误差不能小于负的安全裕度也就是不能贴太近、加速度不能超限、期望加速度不能超限。前两类都是状态约束要转成对 U 的线性不等式。function [Aineq, bineq, lb, ub] build_constraints(Phi, Gam, z0, Np, n, lim) % 只约束间距误差不约束速度误差 e_row zeros(1, n); e_row(1) 1; E kron(eye(Np), e_row); % 间距误差下限p_{i-1} - p_i - L - d_des -d_safe Aineq -E * Gam; bineq E * Phi * z0 lim.d_safe * ones(Np, 1); lb lim.u_min * ones(Np, 1); ub lim.u_max * ones(Np, 1); endd_safe取 3 到 5 米表示允许贴到的极限距离。注意这里只约束了间距误差的下限没有约束上限——后车落后太多不构成危险强行约束反而会让 QP 不可行。加速度约束通过lb、ub施加在控制量上比转成状态约束更直接quadprog处理边界比处理一般不等式快。求解调用很短opts optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); [U, fval, exitflag] quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], opts); if exitflag 0 U zeros(size(f)); % 求解失败就退回零控制别让 NaN 污染状态 endexitflag一定要判。曾经有一次约束冲突导致返回全 NaNNaN 又进了车辆状态整个仿真跑出满屏的NaN曲线查了两个小时才发现是这里没兜住。退化成零控制虽然不优但至少不会把仿真炸掉。4. 通信拓扑与迭代协调邻车信息怎么进代价函数4.1 三车拓扑的邻接矩阵与权重分布式的前提是每辆车只能拿到邻居的信息。三车队列里定义领航车为 1 号跟随车为 2、3 号。邻接矩阵W的第 i 行第 j 列表示「i 车能从 j 车拿到信息」取 1 表示有链路0 表示没有function W topo_3veh() % 1 领航2、3 跟随车道内单向通信允许后车听前车 W [0, 0, 0; % 领航车不听任何车只跟参考速度曲线 1, 0, 0; % 2 号车只拿 1 号车的信息 0, 1, 0]; % 3 号车只拿 2 号车的信息 end这是最稀疏的链式拓扑也是实际车队里最常见的一种因为车际通信的带宽和可靠性都不允许全连通。稀疏拓扑的代价是信息传播慢1 号车刹车2 号车一拍之后知道3 号车要两拍。预测时域 20 步正好能覆盖这个传播延迟这也是前面Np不能设太短的原因之一。有些版本会用双向拓扑后车也把状态发给前车这样前车能「照顾」后车的跟车能力间距误差峰值能降一些。代价是通信量翻倍而且引入耦合后迭代收敛变慢。做纯跟随场景用单向链式就够别一上来就上全连通。4.2 Jacobi 式迭代两轮交换就够第 3 章留了个问题预测时域内把前车加速度当成常数可前车明明也在动。修法是迭代——每辆车先按上一轮的猜测算一遍把算出来的加速度序列广播给邻居下一轮邻居用这个新序列重新组装扰动再解一次。function [U, A_pred] solve_dmpc(z0, prm, A_guess) A_pred repmat(A_guess, 1, prm.Np); % 初始猜测上一拍的加速度保持不变 for iter 1:prm.maxIter % 用邻居的预测加速度序列替换掉 Az 第四行的常数假设 [Phi, Gam, dvec] build_prediction_with_disturbance(prm, A_pred); [H, f] build_qp(Phi, Gam, prm.Cz, prm.Qbar, prm.Rbar, ... prm.Sbar, z0, prm.ref, prm.D, prm.d0); [H, f] fold_disturbance(H, f, Gam, dvec, prm.Qbar); [Aineq, bineq, lb, ub] build_constraints(Phi, Gam, z0, prm.Np, 4, prm.lim); U quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], prm.opts); if isempty(U), U zeros(prm.Nc, 1); end A_new simulate_accel_seq(U, z0(3), prm); if norm(A_new(:) - A_pred(:), inf) prm.tol, break; end A_pred 0.5 * A_pred 0.5 * A_new; % 松弛更新防止来回震荡 end end几个关键点。A_guess是上一拍的实际加速度用它当初始猜测比用零好得多通常一两轮就收敛。maxIter设 3 到 5别设太大三车规模下两轮足够再迭代收益很小。tol取 1e-3 量级对应加速度的收敛精度。0.5那个松弛系数是血泪经验。直接用新序列替换旧序列在第 3 号车上容易出现两轮之间来回跳的现象——这一轮假设前车猛减速所以自己保守下一轮前车发现自己被假设减速了于是调整反复横跳加速度曲线出来是锯齿状。加个 0.5 的阻尼收敛慢一点但曲线干净。4.3 主循环滚动时域一步一步推外层仿真循环负责把上面的东西串起来。领航车不走 MPC直接跟一条预设的速度曲线跟随车逐个求解注意顺序——第 2 号车解完再解第 3 号车这样 3 号车能立刻拿到 2 号车这一拍的最新预测比并行求解收敛更快。x prm.x0; % N 辆车 × 3 维状态 A_hist zeros(prm.N, prm.Np); % 上一拍的预测加速度做迭代初值 U_last zeros(prm.N, 1); % 上一拍实际施加的控制量 for k 1:round(prm.T_total / prm.Ts) % 领航车跟踪参考速度 u1 prm.kp * (prm.v_ref(k) - x(1, 2)); x(1, :) (prm.Ad * x(1, :) prm.Bd * u1); for i 2:prm.N % 组装增广状态 e x(i-1,1) - x(i,1) - prm.L - (prm.d0 prm.h * x(i,2)); dv x(i-1,2) - x(i,2); z0 [e; dv; x(i,3); x(i-1,3)]; [U, A_hist(i,:)] solve_dmpc(z0, prm, A_hist(i,1)); u U(1); % 只施加第一个 x(i, :) (prm.Ad * x(i, :) prm.Bd * u); U_last(i) u; end prm.d0 U_last; % 增量惩罚用 log(k, :) x(:); endprm.d0每拍更新成上一拍的控制量喂给增量惩罚项这个细节漏了的话第一拍的增量惩罚会一直用初始零值导致起步瞬间控制量冲得特别猛。日志log存全部状态plot_result.m直接拿它画三条曲线间距误差、速度、加速度。看间距误差曲线是最快的体检方式收敛到零附近且没有持续振荡基本就说明参数配得没大问题。5. 避坑与排查跑不通先看这五处5.1 间距误差符号搞反车越跟越远现象仿真跑起来三辆车越跑越散间距一路涨到几百米但速度曲线看着正常。原因e p_{i-1} - p_i - L - d_des里前车减后车改成后车减前车之后代价函数里的参考方向还是按原来的写MPC 就朝着「拉大间距」的方向优化而且因为约束只卡下限它拉大间距完全不违反约束一路放飞。解决在init_params.m里加一句断言assert(e 0, e 应为正表示距离大于期望)这种思路早期就拦住。符号约定只在一处定义其他文件全部引用不要每个函数里各写一遍。5.2 quadprog 返回空解曲线全是 NaN现象跑到某一拍U变成空矩阵或 NaN之后所有状态都是 NaN图上什么都没有。原因两种情况最常见。一是间距约束和安全间距撞车前车急刹时后车在物理上已经无法满足d_safeQP 不可行二是H因为数值误差变成非正定求解器直接放弃。解决分两步。H加1e-8 * eye(size(H))正则化把d_safe从硬约束改成带松弛变量的软约束松弛项加大权重惩罚这样极端工况下求解器总能给出一个「尽力而为」的解而不是直接崩掉。另外exitflag 0时退化成零控制别让 NaN 进状态。5.3 队列出现固定频率的振荡现象间距误差曲线上叠加了一个 0.4 到 0.6 Hz 的正弦幅度不大但不衰减怎么调权重都压不下去。原因tau和实际执行器的响应不匹配。tau设得太小MPC 以为车能立刻响应于是给出比较激进的控制量实际执行器跟不上等响应过来时已经过头了下一拍再往回拉形成极限环。解决把tau提到接近实际值或者在控制量上加一阶低通滤波。我一般会把tau在 0.3 到 0.7 之间扫一遍看哪个值下振荡幅度最小通常 0.5 附近就干净了。别指望靠调Q、R解决这个问题那只是把振荡掩盖住。5.4 仿真很漂亮加个通信延迟就崩现象理想仿真里间距误差峰值 0.3 米加 100 ms 通信延迟之后峰值窜到 2 米以上甚至追尾。原因迭代协调里邻居的预测加速度是同一时刻拿到的隐含假设通信零延迟。实际链路有延迟时后车用的是过时信息前车已经在减速了它还在按老数据算。解决在neighbor_info.m里加一个长度为 1 到 2 的延迟缓冲区后车取 N 拍之前的数据同时把预测时域加长到覆盖延迟Np从 20 提到 25。想做得更细的话用状态预估补上延迟这段时间的变化效果会好不少。5.5 权重怎么调都不对劲现象Q里的间距误差权重调大超调调小跟不住。反复横跳找不到舒服的点。原因量纲不一致。间距误差单位是米数值在 0 到 10 之间速度误差单位是 m/s数值在 0 到 15 之间控制量是 m/s²数值在 -4 到 2 之间。直接拿这几个数当权重等于在比谁的数量级大。解决先归一化。Q diag([1/d_max^2, 1/dv_max^2])把每个误差除以其允许的最大值控制量同理然后再调相对比例。归一化之后权重就有物理意义了调起来是单调的不用再靠玄学试。6. 二次开发从三车扩到五车以及换掉线性模型扩车队规模是这套代码最常见的改造需求。改动点比想象中少topo_3veh.m里把 3×3 的邻接矩阵换成 5×5prm.N改成 5主循环的for i 2:prm.N不用动代价函数一个字都不用改。真正要跟着调的是预测时域——链式拓扑下信息每传一辆车延迟一拍五辆车首尾相隔四跳Np至少得从 20 提到 30 才能让最后一辆「看见」头车的动作。换非线性模型是另一个方向。三阶线性模型在常规工况下够用但急加速、大坡度时误差明显。改法是把veh_model.m换成一个非线性函数build_prediction.m里的线性预测矩阵失效需要用fmincon直接对控制序列做非线性优化或者每步在当前工作点重新线性化。前者代码改动小、求解慢后者快但实现复杂。规模不大时我倾向fmincon先在prm里加个开关两种求解器并存对比着看。验证改造是否成功我一般跑三条固定的速度曲线匀速巡航、正弦波动、阶跃减速各跑一次记录三项指标。指标合格线说明间距误差峰值 1.0 m阶跃工况下收敛时间 4 s从扰动到回到 ±0.2 m单步求解耗时 15 ms五车规模三项都过再去看加速度曲线有没有锯齿——锯齿说明迭代协调在振荡回去把松弛系数调小一点。曾经有一次扩到五车之后间距曲线看着挺好结果加速度曲线是锯齿状的查了半天才发现是maxIter设成了 8迭代次数多了反而在两组解之间来回跳。从那以后我每次改车队规模都会先把加速度曲线拉出来单独看一眼再去看间距——间距是给人看的加速度才是给执行器看的。希望这些能帮到你。本文还有配套的精品资源点击获取