最近在做一个多AGV编队仿真需求不复杂几台差速驱动的小车排成队列沿着一条参考路径往前走转弯的时候队形不能散前后车距离不能忽大忽小。单台车路径跟踪我很快就调通了但一旦把第二台、第三台车加进去问题就接踵而来——前车转弯时后车会切内线直线段好不容易拉开的间距在弯道里一下子被吃掉整个编队在弯道处挤成一团。这篇文章就把我从零搭建Simulink多AGV编队系统的完整过程写出来重点是“分布式路径跟踪”怎么做、编队保持的控制量怎么算、通信拓扑怎么在仿真里落地以及最后怎么调参验证。适合已经会Simulink基础操作、想往多智能体协同方向走的同学也适合做AGV调度项目但还没找到编队控制切入点的工程师参考。1. 多AGV编队到底难在哪项目背景与工具选型1.1 我最初把问题想得太简单了一开始我以为多AGV编队就是“让每台车都跟踪同一条路径再保持固定距离”就完了。实际动手才发现这里面有三个隐含问题。第一是弯道内外侧路径长度不一致。假设四台车排成纵队你让它们走同一条参考路径后车在弯道处一定会切内线因为它的跟踪控制器会拉着它回到参考线上而参考线本身在弯道处是弯曲的。如果只是简单跟踪同一条线队形根本不可能保持。第二是“固定距离”这个描述本身有歧义。你说前后车保持2米是指车尾到车头的直线距离还是沿路径的方向距离直线路段两者差别不大但弯道里差别很大。后者才是编队控制里常用的“纵向间距”需要用到路径弧长参数而不是简单算两点欧氏距离。第三是信息结构。编队不能靠一个中央规划器去算每台车的全部轨迹那在大规模场景下通信量和计算量都不可接受。更现实的做法是每台车只跟相邻车交换少量信息各自独立决策也就是分布式控制。但“分布式”不是你在模型里把几条线连在一起就行它需要明确的控制律设计和通信拓扑定义。1.2 为什么用Simulink而不是ROS或者手写MATLAB脚本工具选型我纠结过一段时间。ROS里做多车仿真生态挺完整但调试控制算法时每改一个参数就要重新编译工作空间对于快速验证控制律来说太重。纯手写MATLAB脚本灵活但可视化、信号排查、模块化复用都不够直观四台车以上的系统脚本结构很快就乱了。最终选择Simulink的原因在于三点。第一控制算法可以按子系统边界拆分单车控制器、路径生成器、通信网络都是独立模块排查问题的时候只看局部信号就行不用在几百行脚本里找变量。第二Simulink的信号线本身可以帮助梳理逻辑关系编队系统里“哪台车知道哪台车的信息”通过模块连接一目了然。第三后续如果要部署到真车Simulink Coder可以直接生成C代码不需要重写一遍算法。对于“先验证控制逻辑”这个阶段Simulink是性价比最高的选择。1.3 环境准备我用的MATLAB版本是R2021b基础Simulink环境就可以完成核心搭建不需要额外工具箱。如果你装了Control System Toolbox和Simulink Control Design分析线性化模型会方便一些但本文的控制律用不到。真正必须的模块只有几个MATLAB Function写控制器、Integrator车辆运动学积分、Lookup Table参考路径查表、Mux/Selector信号聚合与拆分这些都是Simulink自带的。2. 先讲清楚数学模型运动学、跟踪误差与编队误差2.1 用到的AGV运动学模型市面上AGV最常见的是双轮差速结构左右轮独立驱动通过轮速差实现转向。它的运动学模型可以写成ẋ v * cos(θ) ẏ v * sin(θ) θ̇ ω其中(x, y)是车体中心在全局坐标系下的位置θ是航向角v是线速度ω是角速度。这个模型忽略了侧滑和惯性对于低速移动的室内AGV是完全够用的。你可能会问为什么不用自行车模型因为自行车模型还要考虑前轮转角到航向的映射关系多了一个转向执行器环节而差速模型里v和ω本来就是直接可控量Simulink里搭起来简单物理意义也直观。有些资料会把左右轮速作为控制量在模型内部加一个换算v (v_r v_l) / 2 ω (v_r - v_l) / LL是轮距。这个换算也很有用因为真实AGV的底层控制器接收的就是左右轮速指令。不过在做编队控制律验证时我更建议直接用v和ω作为控制量把轮速换算留到做执行器细节仿真的阶段再加否则调参时会多一层变量干扰。2.2 路径跟踪误差怎么算简单地把车当前位置减去参考点位置得到的误差向量是在全局坐标系下的直接用起来会发现一个问题同一个横向误差车头朝向不同需要的纠偏方向也不同。所以工程上一般把误差投影到参考路径的切向和法向上也就是在路径坐标系里定义误差。设参考路径上对应点的位置是(x_r, y_r)路径切向角是θ_r那么定义误差e_x (x - x_r) * cos(θ_r) (y - y_r) * sin(θ_r) e_y -(x - x_r) * sin(θ_r) (y - y_r) * cos(θ_r) θ_e θ - θ_r这里e_x是沿路径方向的纵向误差e_y是垂直于路径的横向误差θ_e是航向误差。这个变换相当于把车相对参考点的偏差旋转到路径坐标系里。一开始我图省事直接用全局坐标系的误差写控制器结果车在直线段表现还行一进圆弧就来回振荡改成路径坐标系之后收敛快得多原因就是控制量分解到了切向和法向物理意义清楚了。2.3 单机路径跟踪控制器路径跟踪控制器我采用的是这种形式v_c v_d * cos(θ_e) - k_x * e_x ω_c ω_d - k_y * v_d * e_y - k_θ * sin(θ_e)其中v_d和ω_d是参考点在当前路径参数下的期望速度和角速度k_x、k_y、k_θ是三个正增益。这个控制律的结构很容易理解第一项v_dcos(θ_e)是期望速度在车体前进方向上的投影保证车即使偏航也能维持基本的前进速度第二项-k_xe_x负责把纵向误差拉回来ω_c里面的-k_yv_de_y是比例反馈横向误差-k_θ*sin(θ_e)反馈航向误差用sin而不是直接用θ_e是为了避免大角度误差时控制量饱和同时李雅普诺夫分析也更干净。这个控制器在低速场景下非常稳定。我测试过从初始横向误差0.5米、航向误差30度起步大概10秒内能收敛到横向误差5厘米以内。但要注意v_c可能出现负值也就是车在误差很大的情况下会倒车纠偏。如果项目要求AGV不能倒退需要加一个限幅或用纯跟踪的前视策略替代。2.4 从单机到编队把“队形”转化成参数偏移单机跟踪解决的是“一台车怎么走”编队要解决的是“多台车怎么相对位置不乱”。我的做法是把编队队形转化成参考路径的空间偏移。主参考路径用弧长参数s描述记作P_0(s) [x_0(s), y_0(s)]^T它的切向角是θ_0(s)法向量是n_0(s) [-sin(θ_0(s)), cos(θ_0(s))]^T。第i台车的期望路径就是主路径沿法向偏移一个固定量d_iP_i(s) P_0(s) d_i * n_0(s)这个d_i就是队形里的横向位置。比如四台车排成纵队d_i可以全是0横向偏移为0如果排成两列的楔形队形d_i取0.6米、-0.6米这样的值。通过调整d_i可以在不改控制器的情况下实现队形切换非常方便。但仅靠横向偏移还不够。如果每台车都用同一个s参数弯道内外侧车的线速度就会不一样因为它们实际走的路径长度不同。所以我需要给每台车单独设计s的变化速度也就是下一节要讲的分布式纵向间距控制。2.5 分布式一致性控制的简化理解很多教材上来就讲图论、拉普拉斯矩阵、一致性协议把初学者劝退。我用大白话总结一下分布式编队的核心思想每台车不需要知道全局信息只需要根据自己能拿到的“邻居信息”把自己的状态往期望的相对关系上拉。前后车之间的期望关系就是纵向间距Δs所以跟随车只要知道前车的s值就能算出自己的纵向间距误差然后调整自己的s变化速度。整个车队里除了领航车需要知道全局参考路径其他车都只跟前车交换一个数字——这正是分布式路径跟踪最典型的落地形式。3. 搭框架Simulink模型顶层结构与参数化管理3.1 顶层模块怎么划分我习惯把系统划分为五个部分参考路径生成、领航车速度规划、通信网络、AGV子系统阵列、可视化与数据记录。参考路径生成只做一件事给定s输出主路径的位置、切向角、法向量。领航车速度规划产生领航车的s速度也就是整个编队的基础速度。通信网络负责把每台车的s值按拓扑关系分发给相应的跟随车。AGV子系统阵列里每台车封装了路径偏移模块、误差计算模块、控制器模块、运动学积分模块。可视化与数据记录把各车的状态汇拢起来用于Debug。顶层信号流是从左往右单向的。参考路径生成模块最左侧输出P_0给所有AGV领航车速度规划输出v_ref给领航车通信网络输出每台车需要的邻居s信息。这样的好处是不会出现循环依赖Simulink在编译模型的时候也不容易报代数环。3.2 参数统一管理在模型工作空间里放参数脚本模型里直接用常数模块填参数是新手最容易踩的坑改一个增益要打开好几个子系统改完还可能漏改。我建议用模型工作空间里的参数脚本统一管理。在模型窗口的“Model Properties”里绑定一个参数文件运行时自动加载。参数内容包括运动学参数轮距0.5米最大速度1.2米/秒最大角速度0.8弧度/秒控制器增益k_x、k_y、k_θ、k_s编队参数各车横向偏移d_i期望纵向间距des_ds路径参数样条控制点、总路径长度通信参数拓扑结构邻接表、通信时延3.3 用Bus整理信号四台车的信号如果全部用Mux和Goto/From散着连模型很快就会变成一团乱麻。我用Simulink Bus来组织信号。建一个AGVSignals总线类型里面包含位置、航向角、线速度、角速度、路径参数s这些字段。顶层用Bus Creator把每台车的信号打包通信网络用Bus Selector取出对应字段。这样做的直接好处是想新增第五台车时不用在画布上拉更多信号线只需要在通信网络里加一个通道。3.4 参考路径生成模块主参考路径我用三次样条生成控制点设计成一段带弯曲的路线方便测试编队过弯性能。生成脚本如下% genRefPath.m % 生成主参考路径点表 s_ctrl 0:8:80; x_ctrl s_ctrl; y_ctrl 5 * sin(0.15 * s_ctrl); s_fine 0:0.1:80; x0 spline(s_ctrl, x_ctrl, s_fine); y0 spline(s_ctrl, y_ctrl, s_fine); dx gradient(x0, 0.1); dy gradient(y0, 0.1); theta0 atan2(dy, dx); kappa gradient(theta0, 0.1); % 曲率后面算参考角速度用 % 法向量 nx -sin(theta0); ny cos(theta0); pathData [s_fine, x0, y0, theta0, nx, ny, kappa]; save(pathData.mat, pathData);在Simulink里每个AGV子系统的参考信号计算逻辑是根据当前s_i用Lookup Table查主路径的位置和切向角然后加上偏移量x_r x0(s_i) d_i * n_x(s_i) y_r y0(s_i) d_i * n_y(s_i) θ_r θ0(s_i) ω_d v_d(s_i) * κ(s_i) % 期望角速度注意θ_r这里直接用了主路径的切向角没有因为法向偏移而修正。因为d_i相对路径曲率半径来说很小这个近似误差在可接受范围内。如果你要跑大偏移量的横排编队就需要额外计算偏移路径的切向角了。4. 写核心子系统路径跟踪控制器与车辆动力学4.1 车辆运动学子系统的两种搭法运动学积分部分有两种搭建方式。第一种是用模块直接搭两个输入v和ω经过三角函数的运算之后接入三个积分器得到x、y、θ。第二种是写一个MATLAB Function把运动学方程封装起来内部自己积分。我推荐第二种代码短、不容易连错线也方便统一加限幅。function [x, y, theta] agv_kinematics(v, omega, x0, y0, theta0) % 简化演示真实模型里需要把x,y,theta作为状态传入才能积分 % 这里给出连续微分方程形式 xdot v * cos(theta); ydot v * sin(theta); thetadot omega; % 在Simulink中用Integrator模块对xdot, ydot, thetadot积分 end实际操作时我在Simulink里用Fcn模块做三角计算把xdot、ydot、thetadot接到三个Integrator上而不是用MATLAB Function积分。原因是Integrator模块可以直接设置初始状态排查问题时方便单独看某个状态的变化。4.2 路径跟踪控制器代码路径跟踪控制器是整个模型的“心脏”。我写成一个MATLAB Function块放在每个AGV子系统的中间层。代码如下function [v_c, omega_c] path_following_controller(e_x, e_y, theta_e, v_d, omega_d, kx, ky, kth) v_c v_d * cos(theta_e) - kx * e_x; omega_c omega_d - ky * v_d * e_y - kth * sin(theta_e); end这段代码看起来简单但有几个细节值得注意。第一条v_d*cos(theta_e)这一项在theta_e接近±90度时会把v_c压到接近0。这是好事因为车头横着的时候本来就不该全速往前冲但如果误差持续过大车会停下来原地纠偏。要避免这种情况可以在v_c外面加一个最小速度约束。第二条omega_c里的-kyv_d*e_y这一项和速度成正比意味着车跑得越快横向纠偏越猛。这符合直觉但如果你把v_d调得很高横向误差又比较大omega_c会很容易超出执行器限幅。所以增益k_y的整定要跟v_d联动速度翻倍的时候k_y最好减半。第三条theta_e的计算必须用角度归一化否则航向角从π跳变到-π时theta_e会瞬间变化2π控制器会产生一个巨大的角速度尖峰。角度归一化的函数MATLAB里直接调wrapToPi就行。4.3 误差计算模块误差计算也是MATLAB Function输入车位置和参考点位置输出路径坐标系下的三个误差。function [e_x, e_y, theta_e] compute_path_error(x, y, theta, xr, yr, theta_r) dx x - xr; dy y - yr; e_x dx * cos(theta_r) dy * sin(theta_r); e_y -dx * sin(theta_r) dy * cos(theta_r); theta_e wrapToPi(theta - theta_r); end这个模块放在控制器前面从车状态和参考信号实时计算误差。值得强调的是这里的xr和yr不是固定参考点而是根据当前s查表得到的参考路径上的动态点所以整个误差计算是随参考路径连续变化的。4.4 把单车封装成子系统做好转成多车的准备单车跑通之后我把整个跟踪链路选中右键创建子系统然后给子系统加一个Mask。Mask里暴露三个参数横向偏移d_i、车编号i、控制器增益。这样我可以直接复制这个子系统四份在每份里填不同的d_i几秒钟就从单车扩展成四车编队。封装的时候有个容易忽略的点子系统的输入输出端口命名一定要清晰尤其是输入信号多了以后。我习惯用e_x/e_y/theta_e这种物理量缩写而不是in1/in2这种默认名。端口名清晰了通信网络接线的时候才不容易搞错。5. 分布式编队如何落地纵向间距控制与通信拓扑5.1 领航车与跟随车的关系定义我设计的编队结构是链式领航-跟随领航车按参考速度跑跟随车纵向间距由分布式控制器保证。每台车的轨迹都由自己的参考路径决定参考路径的横向偏移d_i是固定的所以横向队形天然保持需要实时控制的就是纵向间距。定义期望纵向间距为des_ds跟随车i的间距误差e_lon_i s_{i-1} - s_i - des_ds其中s_{i-1}是前车当前的路径参数s_i是车i自己的路径参数。这个误差大于0说明前车比自己多走了超过期望间距的弧长也就是间距拉大了需要加速误差小于0说明间距偏小需要减速。5.2 纵向间距控制与速度补偿每台车的s变化速度由两部分组成基础参考速度v_ref和间距误差补偿量k_s*e_lon_i。function s_dot longitudinal_speed_controller(s_self, s_prev, v_ref, des_ds, k_s) e_lon s_prev - s_self - des_ds; s_dot v_ref k_s * e_lon; end领航车的s_dot直接等于v_ref不参与这个控制。跟随车的s_dot经过积分得到s_is_i再输入给参考路径生成模块产生这台车自己的期望位置。这个设计妙在整个车队没有一个中央计算单元每台跟随车只需要知道前车的s值信息交换量极小但纵向队形能保持住。我实测下来k_s0.5左右的时候四台车从初始间距偏差1米开始大约15秒收敛到间距误差0.1米以内。k_s太大会导致速度振荡太大会导致响应太慢。这个参数跟v_ref运行速度有关系一般先固定v_ref再调k_s。5.3 Simulink里的“通信网络”怎么模拟通信网络在Simulink里用Bus Selector和Selector模块实现。所有AGV的s值先汇总成一个向量进入通信网络子系统。子系统内部根据拓扑关系给每台跟随车输出对应的前车s。最直接的链式拓扑是车1无输入车2取车1的s车3取车2的s车4取车3的s。实现方式就是把汇总的s向量用Selector按索引抽取然后直连到对应AGV子系统的s_prev端口。如果要做全连接拓扑也就是共享领航车的s给所有跟随车那车2、车3、车4的s_prev端口统一接车1的s即可。通过切换通信网络内部的连接方式就可以对比不同拓扑下的编队性能。5.4 三种拓扑的对比测试我实际对比了链式、全连接、混合连接三种拓扑结果如下表拓扑方式通信量弯道编队误差抗时延能力备注链式只看前车最小每车一个标量中等误差沿队列传递弱时延直接导致间距振荡适合小规模车队全连接都看领航车居中每车一个标量最好所有车同步响应中等领航车变相成为中心节点混合领航广播相邻校正稍大每车两个标量好兼顾同步和队形较强工程上推荐链式拓扑在直线段表现很好但进入弯道时误差会像波浪一样从前往后传递最后一台车的横向轨迹会有明显的滞后。全连接拓扑弯道表现最好因为它让所有车几乎同时开始响应弯道但这种方式更像集中式广播不是严格意义的分布式。混合拓扑是我最终采用的方案领航车广播自己的s给所有跟随车作为前馈同时每台跟随车还跟前车做相对误差校正。5.5 通信时延与丢包仿真的小装置真实场景里通信不可能无时延。我用两个模块模拟通信影响Transport Delay模块设置例如0.02秒的固定时延配合一个单位延迟加保持器模拟周期采样和丢包。加0.05秒时延之后链式拓扑的纵向间距误差明显放大k_s需要调小才能避免振荡全连接拓扑对时延不敏感得多。这个测试结果提醒我如果最终要落地到真车通信时钟同步和时延补偿是躲不开的课题。6. 调参与结果验证从两车试跑到四车编队6.1 调参顺序先单机再两车最后四车调参这事真不能急我建议按三步走。第一步关掉纵向间距控制让每台车独立跑自己的参考路径把路径跟踪控制器的三个增益调好。具体做法是给每台车设一个2米/s的参考速度观察e_y和theta_e的收敛情况。我的初始参数是k_x1.5、k_y0.8、k_th1.2调整时先动k_y因为横向误差收敛速度主要由它决定。第二步启用领航车一台跟随车把des_ds设成2米调整k_s。记录间距误差曲线如果超调大就减小k_s如果收敛太慢就加大k_s。第三步复制出四台车开跑。这个阶段主要观察弯道处的队形保持情况。我遇到的问题是第一台车和后三台车在弯道处横向轨迹不一致最后发现是通信网络里s_prev连接错了修正拓扑之后现象消失。6.2 可视化验证方法Simulink自带的XY Graph可以快速画出各车轨迹但比较难看出队形细节。我用了两个方案。方案一是Simulink Scope里选XY模式把四台车位置接到同一个Scope上四色曲线叠在一起看整体轨迹。方案二是写一个绘图刷新函数每隔0.1秒用矩形图形代表每台车的位置和朝向在图上动态刷新。动态示意图能直观看到前后车是否在转弯时交错这是XY轨迹图很难暴露的问题。绘图刷新的核心代码大概是这样放在MATLAB Function块里function visualize(time, pos) persistent fh ax if isempty(fh) fh figure(Name, AGV Platoon); ax axes(Parent, fh); end if time 0.1 mod(time, 0.1) 1e-6 cla(ax); hold(ax, on); for i 1:size(pos, 2) x pos(1, i); y pos(2, i); theta pos(3, i); quiver(ax, x, y, cos(theta)*0.5, sin(theta)*0.5, r); end hold(ax, off); drawnow; end end注意这种可视化方式在变步长仿真下会拖慢运行速度建议调试完成后把求解器改成固定步长0.01秒。6.3 结果分析与常见问题我的仿真结果是四台车以1.0米/秒的参考速度行驶60秒经过两段弯道全程横向误差最大0.045米稳态纵向间距误差小于0.08米航向误差小于1.5度。这个精度对仓储AGV来说已经够用。如果你跑出来的结果误差偏大可以从下面几个方向排查现象可能原因处理办法横向误差不收敛增益k_y太小增大k_y注意不能超过执行器限幅弯道处后车切内线纵向间距控制过强降低k_s或改混合拓扑速度振荡k_s过大减小k_s加一阶低通滤波间距误差稳定值不为0通信时延导致系统静差加时延补偿或改用全连接拓扑7. 几个容易踩的坑与后续改进方向7.1 代数环反馈回路里的隐形杀手多AGV模型最容易出现代数环的情况是控制器输出的v_c直接用于计算e_y的参考速度而参考速度又反过来影响e_y的计算形成一条没有积分器的闭合回路。Simulink会弹警告但不会中止仿真结果就是仿真巨慢、波形全是毛刺。排查方法是在诊断设置里把“Algebraic Loop”提示打开模型编译时如果报了代数环节点重点检查控制器输出到误差计算之间有没有经过Integrator或Unit Delay。我的处理方式是在反馈回路上加一个单位延迟或者把控制器增益经过一个一阶惯性环节再输出。这样既打破了代数环又模拟了执行器的响应惯性一举两得。7.2 角度跳变wrapToPi不能省四台车连续跑120秒第二圈回来的时候我在Scope里看到theta_e出现一个巨大的尖峰车瞬间原地打转。排查了半天发现是航向角从π跨越到-π的时候没有做角度归一化。理论上角度差应该是2度程序却算成了358度。所有涉及角度减法的地方都要包一层wrapToPi包括theta_e和参考角速度的计算。这个坑很小破坏力极强。7.3 不同采样时间导致的行为怪病模型里路径查表是连续的控制器是0.01秒采样通信网络我又莫名设了0.02秒采样。我做完四车仿真后发现同一份参数换个拓扑结果就不一样。后来把通信网络的采样时间显示出来一看发现有的AGV接收的信号是0.01秒更新的有的是0.02秒更新的相当于不同车看到的邻居信息新旧程度不同。解决办法是统一设固定步长0.01秒所有模块采样时间继承仿真步长。7.4 后续可以扩展的方向这个框架跑通之后扩展空间其实很大。路径跟踪控制器可以换成MPC或者滑模控制编队一致性控制器也可以换成高阶协议。队形切换只需要在仿真过程中动态改d_i我就试过让车队从纵列切换成横排效果很自然。如果要加避障可以在s_dot上叠加一个基于障碍物距离的排斥项或者临时修改参考路径的法向偏移量。再往后可以用Simulink Coder把控制模型生成C代码部署到真实AGV的嵌入式控制器上做硬件在环测试。整套系统调到最后我最深的体会是Simulink模型的搭建难度并不在模块本身而在于你要先想清楚控制律和信号流。运动学模型选差速还是阿克曼误差在哪个坐标系定义分布式信息交换什么量这三个问题想明白了模型只是把公式翻译到模块图上而已。最后分享一个小技巧每台车的s、e_x、e_y、间距误差都用To Workspace模块导出来仿真结束之后在命令行画图分析比在Scope里拖波形高效得多——尤其是四台车以上的编队你需要在同一张图上对比好几条误差曲线Scope的交互仔细看不够用。