做车辆队列控制仿真这段时间我接触最多的就是基于MATLAB的异构车辆队列分布式模型预测控制DMPC。所谓“异构”不是简单说车队里车型不同而是每辆车的动力学特性、响应滞后、加速与制动能力都彼此不同所谓“分布式”则是放弃一个中央控制器包揽全队优化的传统思路改为每辆车各算各的MPC再用通信把邻居车辆的预测轨迹“借”过来协调。这篇文章就把这套仿真的完整落地过程讲清楚从车辆模型、代价函数、约束设计到MATLAB里的主循环、QP求解、通信协议和调参避坑适合正在做智能网联车辆、队列控制或毕设项目的同学直接参考。1. 先把思路理清分布式MPC为什么适合异构车队1.1 异构到底“异”在哪里很多人一开始会把车型不同简单理解成“名字不同”真正落到模型里才知道差别有多大。队列控制的常用纵向模型是三阶模型状态是位置、速度、加速度输入是期望加速度\dot{s_i}v_i, \dot{v_i}a_i, \dot{a_i}(u_i-a_i)/\tau_i这里的\tau_i就是一阶惯性时间常数。它直接反映了车辆对加速度指令的响应速度轿车的时间常数通常只有0.4秒左右重卡可能要到0.8秒甚至1秒以上。也就是说同样的期望加速度指令重卡真正达到这个加速度要比轿车慢得多。除了时间常数车辆长度、最大加速度、最大制动减速度、期望时距这些参数也不一样。我做仿真时常用的一组对比参数如下参数小轿车模型重卡模型时间常数 τ0.4s0.8s最大加速度2.0 m/s²0.8 m/s²最大制动减速度-4.5 m/s²-2.0 m/s²车辆长度4m12m期望时距 h0.6s1.0s如果忽略这些差异把所有车辆都当同一个动力学模型来控制最典型的现象就是队列尾部“波浪式振荡”前车轻踩油门轿车很快跟上重卡半天没动后车为了补间距又猛加速结果整个队列像手风琴一样来回伸缩。这就是异构性没有真正进到控制模型里的后果。所以异构车辆队列DMPC的第一步不是先写算法而是先把每一辆车的这些参数明确列出来做成一个参数表。后面所有优化问题都要针对每辆车的实际参数单独构建而不是全队共用一个模型矩阵。1.2 集中式、分散式、分布式MPC怎么选MPC在做队列控制时常见方案有集中式MPC、分散式MPC和分布式MPC。三者的区别我习惯用一个类比来解释集中式像一个总指挥所有人听他调度全局最优但别人一挂就全乱分散式是各管各既不问别人也不理别人看起来自由实际容易乱套分布式则像一套小组协作每个人自己拿主意但做决定前会先问一下旁边的队友打算怎么走然后把自己的计划调整得跟他兼容。集中式MPC把整个车队所有车辆状态堆在一起构建一个大优化问题。假设车队有N辆车每辆车3个状态那么优化变量的维度就是3N × 预测时域。车队从4辆加到10辆时求解时间会明显上升而且所有车辆模型参数、约束、初始状态都要集中到一起任何一辆车的数据出错整个优化就解不出来。更麻烦的是一旦中央控制器通信断开全队控制都会瘫痪。分散式MPC则是每辆车只优化自己的问题完全不考虑其他车的影响代价函数里没有邻居的预测信息。这样做计算量确实小但当车辆之间物理上存在强耦合时比如必须保持安全间距这种方案就会暴露出致命问题每辆车都只顾自己很容易产生“串车”不稳哪怕前车已经刹车后车还要先加速拉间距反应滞后。分布式MPC跟前两者最大的区别在于它在每辆车的局部优化中显式地引入了邻居车辆的预测轨迹。我用的方案是第i辆车在做当前时刻的优化时把前车i-1在未来Np步的预测轨迹作为已知参数放进代价函数和约束中同时把领导车辆的参考速度也广播给全队。这样局部优化仍然是自己的问题但通过通信把车辆间的耦合关系纳入了决策。它的计算复杂度基本是线性的车队数量增加不会带来指数级爆炸而且通信断开时还能沿用上一次的预测轨迹做保守控制不会立刻崩溃。1.3 DMPC的“协商协议”到底是什么很多人对分布式MPC的理解停留在“每辆车各自跑一个MPC”这个层面但如果是这样那其实就是分散式MPC还不算真正的分布式控制。分布式MPC里必须有一个信息交互和迭代机制才能让各自独立的优化最终收敛到一致、协调的行动。我的做法是定义一套“假设轨迹”机制。每辆车维护一条预测轨迹比如从当前时刻起未来20步的位置序列这代表“我打算这样走”。每次迭代时每辆车读取前车最新发布的轨迹把它代入自己的优化问题算出一条新的最优轨迹并覆盖发布。这一轮迭代结束后下一轮再读邻居更新后的轨迹重新优化。经过若干轮迭代整个车队的轨迹会逐步逼近一个协调解。在程序上这体现为一个双层循环。外层是仿真时间步内层是DMPC协商迭代。我通常设2到3轮迭代就够了不是越多越好因为车辆队列的纵向耦合相对简单迭代太多不仅增加计算时间还容易让控制器对邻居轨迹的变化过度敏感产生不必要的振荡。需要特别说明的是节点之间的交互信息不是原始状态的全部而是预测轨迹。为什么这样设计因为MPC的核心优势就是能“看到未来”如果只交换当前状态而不交换未来轨迹那每辆车对邻居行为的预判能力就没了控制效果会退化成普通的反馈控制。同时预测轨迹的长度Np要跟局部MPC的预测时域一致否则轨迹在末端截断会造成信息失真。2. 核心细节建模、代价函数与约束2.1 车辆纵向动力学建模队列控制里的车辆模型可以简化成纵向运动也就是不考虑横向转向只看车辆沿道路方向的位置、速度和加速度。状态向量选择为x_i [s_i; v_i; a_i]三个分量分别表示位置、速度、加速度。连续时间动态方程是\dot{s_i}v_i\dot{v_i}a_i\dot{a_i}(u_i-a_i)/\tau_i这里的u_i是车辆的控制输入可以理解为下层执行器要去跟踪的期望加速度。\tau_i体现了执行机构的滞后也就是前面说到的那组异构参数。为什么用三阶模型而不用更简单的一阶或二阶模型因为队列控制里既要控制间距又要控制速度一致性还需要限制加速度变化如果模型阶次太低控制器算出来的轨迹会过于激进实际执行器根本跟不上。三阶模型已经能覆盖大部分纵向控制的动态特性同时状态维度只有3在MPC的预测计算中仍然非常高效。2.2 离散化处理和模型矩阵构建MPC本质上是离散时间的优化控制问题所以连续模型要先离散化。采样周期Ts的选择需要兼顾控制精度和计算量。我在仿真里常用Ts 0.1s这个值既不会因为太大而丢失动态细节也不会因为太小导致预测时域内步数过多。用前向欧拉法离散后状态空间表达式是A_d [1 Ts 0; 0 1 Ts; 0 0 1-Ts/τ_i]B_d [0; 0; Ts/τ_i]这个A_d矩阵每个对角块的形式都一样但τ_i不同会让第三行第三列的值不同这就是异构车辆在模型矩阵层面的体现。在MATLAB里构建时我不会手动输入矩阵而是写成buildModelMatrix(tau, Ts)函数把时间常数传进去自动生成这样后续批量切换车型参数会很方便。还有个细节容易忽略控制输入u_i和状态中的加速度a_i并不一样。u_i是期望加速度a_i是实际加速度。由于惯性滞后两者之间存在一阶动态延迟。设计代价函数时我会同时惩罚u_i和a_i前者是为了防止控制指令过于激进而损坏执行机构后者是为了限制车辆实际加速度过大避免乘客不舒服、货物移位。2.3 代价函数如何设计代价函数是整个DMPC优化问题的核心。常见的队列控制目标有三个保持期望间距、保持速度一致、减少控制器的激烈动作。对应的代价项如下J_i Σ_{j1}^{Np} [w_s * e_{s,i}(kj)^2 w_v * (v_i(kj) - v_lead(kj))^2 w_u * u_i(kj)^2 w_a * a_i(kj)^2]这里的e_{s,i}(kj)是间距误差定义方式是e_{s,i} (s_{i-1} - s_i - L_{i-1}) - (d0 h_i * v_i)第一项(s_{i-1} - s_i - L_{i-1})表示前车尾与自车头的实际间距减去(d0 h_i * v_i)表示期望间距这个期望间距由固定安全距离d0和与速度相关的时距项h_i * v_i组成。速度越高期望间距越大这在高速队列场景下能避免后车跟得太近。权重系数w_s、w_v、w_u、w_a的取值需要根据车辆特性分别设置这也是异构控制的关键。重卡因为惯性大、响应慢如果同样用轿车的w_a控制器会频繁发出大幅加速度指令实际车辆根本执行不了结果就是过度修正。我后面会在调参章节给出具体经验。值得强调的是代价函数中v_lead(kj)的获取方式决定了控制器的信息结构。在带头车辆智能、其他车辆只负责跟随的场景里头车的未来参考速度会通过广播发送给所有跟随车。如果是无头队列那么v_lead可以替换为前车的预测速度。两种方式在DMPC框架下都可以实现关键是保持信息格式统一。2.4 约束条件如何处理约束分三类控制输入约束、状态约束、间距约束。控制输入约束比较简单u_min ≤ u_i ≤ u_max对重卡来说u_max可能只有0.8 m/s²而小轿车可以到2.0 m/s²约束边界必须按车辆实际能力设置否则DMPC优化出来的轨迹重卡根本执行不了。状态约束主要是加速度边界和速度边界a_min ≤ a_i ≤ a_max0 ≤ v_i ≤ v_max间距约束是最能体现“耦合”的约束也是DMPC分布式特性的关键。对第i辆车来说s_{i-1}(kj) - s_i(kj) - L_{i-1} ≥ d_safe这个式子看起来简单但注意s_{i-1}(kj)不是第i辆车的优化变量而是来自前车的预测轨迹。在分布式求解时我们把它当作已知参数也就是在本次迭代中从前车那里读到的数值。这样约束仍然是线性约束可以直接写进标准QP问题里。如果前车轨迹每次迭代都在变间距约束就可能出现“一会儿满足一会儿不满足”的情况。这时要注意不能用太硬的安全距离否则QP很容易因为约束冲突而不可行。我的经验是把安全间距约束改成软约束也就是允许在极特殊情况下降级到d_safe - ξ同时把ξ作为一个非负变量加入代价函数并乘以一个很大的罚系数。这样既保证了正常情况下间距约束严格满足又避免了极端情况下优化直接失败。3. 实操环节基于MATLAB把DMPC跑起来3.1 参数初始化与车辆编组仿真开始前的参数初始化占了我整个项目调试时间的三分之一。这里有个教训参数表写得越清楚后面排查问题越省事。我习惯把所有车辆参数放进结构体数组veh而不是散落在不同变量里。例如Ts 0.1; % 采样周期 Np 20; % 预测时域 N 8; % 车队车辆数量 Niter 2; % DMPC协商迭代次数 Ksim 400; % 仿真总步数 veh(1).tau 0.4; veh(1).len 4; veh(1).amax 2.0; veh(1).h 0.6; veh(2).tau 0.8; veh(2).len 12; veh(2).amax 0.8; veh(2).h 1.0; veh(3).tau 0.5; veh(3).len 6; veh(3).amax 1.5; veh(3).h 0.7;这里的车队设计故意让第2辆、第4辆是重卡其他是轿车或商务车模拟真实的混编场景。初始位置按期望间距排好初始速度全部设为20 m/s这样仿真一开始系统就处于平衡点附近不会因为初始误差过大直接让优化器进入不可行状态。我踩过的一个坑是刚开始把初始间距设成固定车长加一个常数而没有考虑速度相关的期望间距。结果仿真第一帧就开始剧烈调整车队的初始误差被MPC当作一个必须迅速消除的偏差输出一个很大的加速度指令。解决方法是让初始位置满足期望间距公式也就是把d0 h_i * v_i作为初始间距的一部分。3.2 仿真主循环预测、优化、通信DMPC的仿真主循环结构是for k 1:Ksim % 更新头车轨迹 lead_traj leader_reference(k, Np, Ts); % DMPC协商迭代 for iter 1:Niter for i 1:N % 读取前车最新预测轨迹 if i 1 front_traj lead_traj; else front_traj traj_buf(i-1, :); end % 构建并求解局部QP问题 [H, f, Aineq, bineq] buildQP(veh(i), x_current(i,:), front_traj); % 求解 u_sol quadprog(H, f, Aineq, bineq, [], [], lb, ub); % 用求解结果更新自车预测轨迹 traj_buf(i, :) simulate_local(veh(i), x_current(i,:), u_sol); end end % 所有车应用第一步控制量更新真实状态 for i 1:N u_true(i) traj_buf(i, 1); % 注意这里取了第一条 x_current(i, :) update_state(veh(i), x_current(i, :), u_true(i)); end end这个结构里最关键的是traj_buf缓冲区。它存放的是每辆车“当前对外宣布的预测轨迹”每次迭代都会更新。第i辆车优化时读的是前车i-1上一轮迭代发布的最新轨迹而不是本轮自己刚算出的轨迹。为什么顺序上从第1辆开始而不是从最后往前这取决于邻接关系如果每辆车只需要前车信息从前到后依次算第i辆车就能用上前车最新算完的结果收敛更快如果每辆车还需要后车的反馈那就可能要换成从后往前或者加双轮迭代。quadprog返回的u_sol是整个预测时域内的控制序列。注意我只取了第一个分量作为实际控制量这是MPC最典型的“滚动时域”特性算未来Np步但只执行第一步下一步再重新优化。有的新手在第一次写代码时会把整个控制序列都执行完那样仿真就变成开环控制误差会一路累积必须避免。3.3 把优化问题写成quadprog标准型MATLAB内置的quadprog是求解凸二次规划的主力工具使用形式是[u, fval, exitflag] quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options);所以关键是构造H、f、Aineq、bineq。变量是预测时域内的控制序列U [u(1); u(2); ...; u(Np)]。状态预测可以写成矩阵形式X M * x0 N * U其中M是零输入状态下从初始状态推进Np步的矩阵N是控制输入对预测状态的贡献矩阵。在MATLAB里我不会真的去推导这个矩阵的闭式表达式而是用循环构建梯度矩阵然后组装代价函数。伪代码如下function [H, f, Aineq, bineq] buildQP(veh, x0, front_traj) Ts 0.1; A_d [1 Ts 0; 0 1 Ts; 0 0 1-Ts/veh.tau]; B_d [0; 0; Ts/veh.tau]; % 预先分配 Gs zeros(Np, Np); % 间距误差对U的敏感矩阵 Gv zeros(Np, Np); % 速度误差对U的敏感矩阵 Ga zeros(Np, Np); % 加速度对U的敏感矩阵 Gu eye(Np); % 预测状态展开 x_pred zeros(3, Np1); x_pred(:,1) x0; for j 1:Np x_pred(:,j1) A_d*x_pred(:,j) B_d*U(j); end % 这里用符号推导代替实际循环核心是把 % x_pred 分成 初始状态贡献 U 的线性贡献 % 然后代入代价函数 J U*H*U f*U end为了在实际运行时不踩坑我建议先在命令行里用小规模Np5跑通,确认quadprog能正常返回结果再放大到Np20。小规模下可以打印出每一步的中间变量看看H矩阵是否奇异。我遇到过的最典型问题是代价函数里如果完全没加u的正则项H会半正定但不严格正定quadprog会报“Hessian is not positive definite”警告。解决办法很简单在u的二次项上加一个很小的系数比如1e-4让H始终严格正定。3.4 仿真结果怎么评估程序跑完不是结束还要用几个指标判断DMPC到底表现如何。我习惯记录三类数据一是间距误差序列。把每辆车在每个时刻的e_s画出来看是否有收敛趋势、有没有持续振荡。正常情况应该是头车加速阶段间距误差短暂增大后很快收敛到零附近重卡跟车误差会比轿车大但不应持续发散。二是加速度曲线。看重卡的加速度指令是否一直顶在边界上。如果长时间顶着u_max运行说明前车轨迹太激进或者跟车间距过小需要调整头车的参考轨迹或者增大期望时距h_i。三是计算耗时。用tic/toc记录每个仿真步的求解时间。当车队规模到10辆以上、Np到30时如果单步耗时超过0.2秒就需要考虑缩短预测时域或减少迭代次数。这些指标要在一次仿真中同时记录并保存到.mat文件里。我见过不少同学只看一两张曲线图就判断算法“有效”结果换一个初值又失败就是因为没有系统性记录这些用于后续排查的中间数据。4. 常见问题与排查技巧实录4.1 现象与原因速查表调试DMPC时问题往往不是“算法理论复杂”而是“细节坑太多”。我把实际遇到的典型案例整理成了一个速查表现象可能原因解决方式某辆车QP直接无解约束过紧间距约束不可行改为软约束加入松弛变量和罚因子队列尾部振荡剧烈权重失衡间距误差权重过大降低w_s适当提高w_v增加控制量变化惩罚重卡与大车跟车滞后严重控制器仍用了统一的小车模型确保A_d、B_d按各自tau实时生成仿真第一步就产生很大抖动初始位置不满足期望间距初始化时按d0 h*v_i排列间距曲线像波浪一样向后传播缺少前车加速度前馈在代价函数加入前车加速度跟踪项或广播领导轨迹迭代收敛慢DMPC只做了一轮迭代信息没充分传播增加Niter到2或3观察曲线是否趋于不变控制量频繁跳变没有限制控制增量加入Δu惩罚项或对相邻时刻输入差做约束这个表我不能说覆盖了所有情况但覆盖了我在异构车辆队列DMPC中最常遇到的七成问题。后面单独讲几个处理思路比较典型的点。4.2 不可行问题与软约束QP不可行是DMPC里最让人头疼的问题。车辆队列动力学本身是耦合的当两辆车之间既有硬性的安全间距约束又有各自独立的加速度约束时如果前车突然急刹后车即使全力制动也可能无法满足安全间距此时QP的解集就是空的。我处理不可行问题的标准做法是引入松弛变量。把原来硬性的间距约束s_{i-1} - s_i - L_{i-1} ≥ d_safe改成s_{i-1} - s_i - L_{i-1} ξ_i ≥ d_safe, ξ_i ≥ 0同时在代价函数中加一项M * ξ_i^2其中M是一个很大的正数比如1×10^6。这样做的好处是正常情况下ξ_i等于0约束退化为原来的硬约束一旦出现无法满足的情况优化器允许以一定代价突破安全间距而不是直接让整个仿真崩溃。在仿真中看到ξ_i长时间大于0就说明车队进入了危险工况此时要回头检查前车轨迹的激进程度和车辆制动能力。还有一个经验软约束的罚系数不要一开始就设得过高。因为过高的M会让QP数值条件变差反而增加求解时间。我是从1×10^4开始观察ξ是否收敛到0再逐步提高。4.3 通信延迟与预测信息陈旧DMPC依赖车辆之间的预测轨迹交换理想情况下每次迭代都能拿到邻居最新轨迹。但实际仿真中通信总会存在延迟哪怕延迟只有一个仿真步长也会让车辆基于“过时”的信息做决策。我处理这个问题的一个实用技巧是不直接使用邻居上一时刻发布的原始轨迹而是做一步外推。简单做法是traj_extrapolate front_traj; traj_extrapolate(1:end-1) front_traj(2:end); traj_extrapolate(end) front_traj(end);这相当于假设前车保持最后一段加速度不变往前推一步。这种做法虽然不严谨但在通信延迟只有一个仿真步长时非常有效能明显减少队列末端的相位滞后。更稳妥的方案是在代价函数中降低对前车远近未来轨迹的信任权重对近端前5步给高权重对远端给低权重。这样做的好处是即使邻居远端预测信息不那么准确也不会对当前决策产生过大影响。4.4 调参时的小技巧调参是整个DMPC项目里最磨人的环节。我总结了一条经验不要一上来就调整个车队先把单辆车闭环调稳再把多车耦合调稳。先让一辆车在给定前车轨迹下能稳定跟车这时只调w_s和w_v看间距误差和速度误差的响应然后接入两辆车观察前车扰动向后传播的情况最后再扩展到全车队。针对异构车型我建议参数按车型分组设置轿车组用比较激进的参数重卡组用保守参数。怎么算保守关键是降低w_a因为重卡本身的加速能力有限过大的w_a会让控制器为了抑制加速度而产生剧烈的控制量变化反而是帮倒忙。轿车的w_s可以高一些因为它的执行器响应快能跟上较大的间距修正指令。另一个容易被忽略的参数是期望时距h_i。h越小队列跟车越紧道路通行效率越高但稳定性裕度越小。当h小于0.5秒时队列尾部非常容易出现振荡。这不是控制器调参能完全解决的是物理上感应延迟和车辆响应延迟带来的稳定性极限。所以我在仿真中会刻意把h设成0.6到1.2秒优先保证稳定再考虑压缩间距。关于迭代次数Niter也不要越大越好。我第一次做DMPC时设了5轮迭代结果发现第3轮和第5轮的结果几乎没有区别反而因为过多的信息交换导致控制量在局部振荡。现在我对大多数纵向队列场景就设2轮只有在强耦合或需要很紧间距时才设3轮。判断标准很简单算完后把相邻迭代轮次的轨迹差值画出来如果已经小到可以忽略就没必要继续增加迭代。最后再分享一个小技巧仿真时把每辆车的权重参数、约束边界、初始状态都保存在一个TABLE变量里导出出问题时能直接对照排查。我遇到过几次“算法没问题但结果不对”的情况最后发现只是某辆车的时间常数填错了一位小数。别小看这种低级错误在四到八辆车的车队里一个参数填错整个车队的曲线都会怪起来而且看起来特别像算法问题最容易浪费时间。
