前阵子有位做飞行控制的朋友问我多无人机编队控制在Matlab里到底怎么才能跑起来又不只是图个动画好看。我把自己之前整理的一套基于多智能体框架的仿真翻出来正好借这个机会说透。这个项目的名字虽然长但内容很聚焦基于多智能体无人机群体控制包含障碍避让和基于Voronoi的编队控制整体用Matlab实现。简单讲它解决的是这么一件事——一群无人机从分散位置起飞先按Voronoi划分的思路生成编队再保持队形飞向目标区域途中遇到圆形障碍能自动避让整个过程是分布式的每一架都只依赖自己和邻居的局部信息。这套东西适合谁来参考我觉得是三类人一是刚开始接触多智能体控制、想在Matlab里做编队仿真的研究生二是做实际无人系统集成的工程师需要一个能快速验证控制协议的原型三是对Voronoi图感兴趣想看看它除了画网格以外还能怎么用在机器人控制上的人。读完你会有一个能直接改参数跑起来的框架更重要的是知道每个环节为什么这么设计坑都在哪。1. 项目到底在做什么拆解多智能体编队控制的核心问题1.1 编队控制的三层需求多智能体无人机群体控制听起来高大上拆开看其实就三层需求。第一层是队形生成一群无人机不能像无头苍蝇一样随机分布它们要形成一个明确的几何构型比如三角形、五边形、直线纵队或者本项目的Voronoi覆盖形。第二层是队形保持在飞向目标过程中每架飞机和邻机之间的相对位置不能乱至少不能超过允许误差。第三层是安全约束机间不能碰撞遇到环境障碍要避让这两条是硬约束优先级高于队形精度。把这三层放到同一个控制律里最大的难点不是单层设计而是它们互相打架。比如严格保持队形时无人机往往没空间绕障碍绕障碍时队形又会被拉扯变形。所以我们在设计上的思路是分层底层用位置-速度反馈保证基本跟踪中间加上一致性项协调邻居关系最外层加势场避障让避障力只在危险距离内起作用。每层各司其职不会出现“为了安全把队形完全丢掉或者为了队形一头撞上障碍”的尴尬局面。另外要注意这个“分层”不是把控制器简单相加就完事。每一层都有自己起作用的阈值和增益层次之间还要做限幅。比如避障斥力我一般会限幅到最大加速度的70%避免局部避障力太大把编队拽得面目全非。这些细节看起来不起眼却是仿真能不能收敛的关键。1.2 为什么选Voronoi而不是别的编队方法编队控制方案不少常见的有领航者-跟随者、虚拟结构、基于行为以及本项目的Voronoi划分法。领航者-跟随者最简单一台领航机带着跑但缺点是领航机挂了整个队伍就完蛋而且后机容易滞后。虚拟结构把整个队伍当刚体约束精确但太死板碰到障碍时整体变形困难。基于行为法灵活但行为优先级和权重很难调容易出现“各走各的”。Voronoi划分的思路是给每架无人机分配一块“势力范围”范围边界的交点或者质心可以直接作为期望队形参考点。它的好处是天然分布式——每架飞机只要知道邻居位置就能算自己的单元不需要全局指挥而且队形本身有几何意义目标点到谁的单元最近就归谁管编队和任务分配可以合在一起做。用生活类比它不是让飞机排成固定几个格子而是相当于给每架飞机画“领地”领地的边界会随着飞机移动自动调整队形就有了弹性。把几种方案放在一起看会更清楚方法分布式程度队形精度避障扩展适用场景领航者-跟随者低低容易小规模、通信可靠虚拟结构低-中高难高精度编队基于行为高中容易动态环境Voronoi划分高中高容易区域覆盖、集群控制从表格能看出Voronoi没有明显短板尤其适合做“群体去中心化避障”的需求。当时我选它还有一个原因Voronoi单元能够自然地反映邻居分布密度单元面积大的飞机说明“周围空旷”控制律会把它往空地拉一点这正好和编队展开的需求吻合。1.3 障碍避让在整个系统中的位置障碍避让在这个项目里不是锦上添花是绕不开的痛点。很多编队仿真在空地跑得很漂亮一加障碍就乱了前机绕障后机还在按原队形冲结果撞在一起。所以我把避障设计成局部势场只有检测到障碍进入安全半径才启用正常巡航时完全不影响编队控制。这么设计的另一个原因是Voronoi编队本身对障碍有一定“天然容忍度”。当某架飞机被障碍挡住时它的Voronoi单元会被压扁变形参考点也会跟着挪这就相当于队形自动让出一条路。我们只需要在外层补一个短距离斥力防止飞机贴脸撞障碍剩下的交给编队协议去恢复。这个“底层避碰中层队形顶层Voronoi参考点”的结构是整个项目能跑通的关键。还有一个容易忽略的问题避障的优先级必须设计好。我的做法是给每个控制分量一个“作用标签”避障力一旦激活队形保持项可以暂时降权但不直接置零。完全置零会导致飞机绕障后找不到编队目标轨迹出现明显的“发飘”。降权之后的恢复过程更自然。2. 算法原理Voronoi划分与编队控制怎么结合2.1 Voronoi图到底怎么理解Voronoi图可能有些人只在几何课本里见过说白了就是平面最近邻划分。给定几个种子点把平面切分成若干个区域每个区域包含的点到对应种子点的距离比到其他任何种子点都近。区域之间的边界是相邻种子点的垂直平分线。数学上可以写成V_i {x | ||x - p_i|| ||x - p_j||, j ≠ i}每个“地盘”都是凸多边形所有地盘拼起来铺满整个平面。生活化的理解很简单每个种子点就是一个“基站”Voronoi图就是每个基站信号覆盖得最好的那片地盘边界处两个基站的信号强度相同。在无人机编队里种子点就是无人机当前位置。每架飞机维护自己的Voronoi单元单元的形状反映邻居分布单元质心则是一个“局部期望位置”。当飞机与邻居相对均匀时质心基本靠近自己当某侧邻居拥挤时质心会被推向空旷侧飞机就有向空旷侧运动的趋势这正好可以用来疏散、展开队形。Matlab里画Voronoi图很简单voronoi(px, py)一行就能出图但我们要的不是图而是每个单元的顶点集合和面积这就得用voronoin函数。它会返回顶点矩阵v和单元顶点索引c注意靠近无穷远处的顶点索引是1这些顶点坐标是无穷大需要专门处理。这个细节是后面所有目标点计算的基础很多新手在这里踩坑。2.2 从Voronoi划分到编队目标点分配我们需要想清楚“用Voronoi做编队”到底是怎么操作。最简单的思路先给整个编队设定一个期望队形位置集合比如6架飞机要停在某个圆形或方形区域里那么每架飞机的期望目标点可以取它当前Voronoi单元的某一特征点通常是质心。当分布均匀时质心会趋向于最终队形点当分布不均匀时质心会被邻居“推”到空旷处从而实现自动调整。具体来说每架无人机计算自己的Voronoi单元后求该多边形区域的质心记作c_i。然后控制目标是让飞机飞向c_i。这里有个看似矛盾的点质心由当前位置决定当前位置又由质心控制这不闭环了实际上算法会在迭代中收敛。大家熟悉的Lloyd算法就是这个思想不断把点移动到Voronoi单元质心最终会收敛到质心Voronoi划分CVT点分布趋于均匀。我们做的编队控制其实就是一个带动力学约束的Lloyd过程飞机不是瞬间跳到质心而是受速度、加速度限制地逼近它所以不会死锁也不会抖得很剧烈。关于特征点的选择除了质心也可以用距离所有邻居最远的点或者单元某个顶点。但质心最好实现也最平滑因为单元几何中心是位置的连续函数只要不穿越退化状态不会突然跳变。这一点对控制稳定性很重要。如果追求覆盖效率可以用面积加权的CVT目标点如果追求队形拓扑稳定可以用Voronoi顶点作为参考。我实测下来质心方案在6到20架规模内都表现稳定是性价比最高的选择。2.3 分布式控制协议设计有了目标点下一步就是设计控制力。系统简化成二维平面内的质点模型[p_xi; p_yi]是第i架无人机位置[v_xi; v_yi]是速度控制输入是加速度u_i。核心控制律我写成u_i -K_p * (p_i - c_i) - K_v * v_i u_i_cons u_i_rep其中-K_p * (p_i - c_i)是位置反馈把飞机拉向Voronoi质心-K_v * v_i是速度阻尼防止过冲和震荡u_i_cons是邻居一致性项让相邻飞机的速度趋同u_i_rep是避障斥力只在飞机进入障碍危险范围时才有值。用PID的类比来理解K_p是比例项K_v是微分项一致性项相当于把邻居状态也引入控制器让队伍整体“齐步走”。一致性项的具体形式u_i_cons K_c * sum_{j in N_i} (v_j - v_i)N_i是第i架飞机的邻居集合一般用通信半径来判断。当邻居速度和本机速度不一致时这个力会拉平速度差。它不直接改变位置但能大幅减少队伍内部的相对运动编队看起来更像一个整体而不是一群各自飞的机器。离散更新就用欧拉法v_i(k1) v_i(k) u_i * dtp_i(k1) p_i(k) v_i(k1) * dtdt一般取0.02到0.1秒。太快跑不动太慢容易在临界条件附近发散。我实测dt0.05在大多数场景都稳。2.4 障碍避让策略避障用人工势场法这是最经典也最好调试的方案。每个障碍物对外产生一个斥力场大小随距离衰减目标方向有一个引力场保证飞机不会停在某个角落。斥力公式我习惯用F_rep eta * (1/d - 1/d0) * (1/d^2) * n当d d0时才计算d是飞机到障碍中心的距离d0是斥力作用半径n是从障碍指向飞机的单位向量。d0比障碍物物理半径大一圈比如障碍半径0.5米d0取1米这样飞机在距离障碍1米时开始感受斥力逐步减速偏转。这里有个避坑点斥力只用一项的话容易在“目标点在障碍背后”时陷入局部最优飞机可能在障碍前左右抖动不前进。我的处理是在斥力之外加一个切向力或者当检测到距离缩短但速度仍指向障碍时让飞机先停止前冲再沿切向绕行。简单实现是给斥力加一个垂直于“障碍-飞机”连线的分量系数取主斥力的0.3到0.5。这个技巧能显著减少“顶着障碍往死里怼”的情况。还有一点要注意势场法并不是万能的在复杂迷宫环境中它一定会局部最优。但作为编队控制里的一个“安全层”它足够用了。真正的全局路径规划可以交给更高层的A*、RRT*这个项目只做局部避障所以不展开。3. Matlab实现整体架构与关键代码3.1 仿真环境搭建与参数定义我先说明环境我用的是Matlab R2022b代码基本兼容R2020b以后的版本主要依赖polyshape和voronoin这些基础功能不需要额外工具箱。整个仿真我建了一个主脚本和几个函数文件主脚本负责参数初始化、循环推进和画图函数文件负责控制律、Voronoi目标点计算和避障判断。第一步是参数区写在主脚本最前面clear; clc; close all; N 6; % 无人机数量 dt 0.05; % 仿真步长 T 30; % 仿真时长 Kp 1.8; Kv 1.2; Kc 0.8; % 控制增益 eta_rep 2.0; % 斥力增益 d0 1.0; % 斥力作用半径 r_obs 0.5; % 障碍物理半径 comm_range 2.5; % 通信半径这些参数是怎么来的Kp和Kv要匹配成类似二阶系统的阻尼关系如果假设单位质量闭环近似是Kp/(s^2 Kv*s Kp)。要让系统不震荡Kv大约在2*sqrt(Kp)附近。比如Kp1.8理论值约2.68但实际有Voronoi质心动态和离散化误差我取小一点1.2让系统偏“钝”而不是偏“冲”。eta_rep和d0需要和速度匹配速度上限约1.5m/s时eta_rep2能让斥力在0.8米处把飞机推偏。参数这个东西最忌讳的是照抄别人的。同样的控制律换一个仿真步长或者换一组初始位置最优参数就变了。所以我会在脚本顶部放一个参数结构体方便批量修改。后面第5章的调参方法也是围绕这个结构体展开的。3.2 Voronoi目标点计算的Matlab实现每个仿真步都要重新计算所有无人机的Voronoi单元。我封装了一个函数compute_voronoi_centroidfunction [cen, area] compute_voronoi_centroid(p) N size(p, 1); [v, c] voronoin(p); cen zeros(N, 2); area zeros(N, 1); for i 1:N verts v(c{i}, :); % 去掉无穷远顶点索引1对应的坐标可能为Inf finite isfinite(verts(:,1)) isfinite(verts(:,2)); if sum(finite) 3 cen(i, :) p(i, :); area(i) 0; continue; end poly polyshape(verts(finite, 1), verts(finite, 2)); [cx, cy] centroid(poly); cen(i, :) [cx, cy]; area(i) polyarea(verts(finite,1), verts(finite,2)); end end注意一个关键细节voronoin返回的单元顶点索引c{i}中如果包含1号顶点说明这个单元延伸到无穷远。直接算质心会有NaN或爆炸值。我先用isfinite把无穷顶点剔掉剔除后如果不足三个点说明这个单元退化就暂时用飞机自身位置当作质心等下一帧再修正。另一个值得提的地方是polyshape和centroid的用法。centroid是R2017b以后引入的函数用来算多边形质心。如果你用的是老版本可以自己写面积加权公式但务必要先用polyarea核对一下。还有当无人机数量很多时voronoin的计算量会随N^2增长。20架以内基本无感超过50架建议只计算局部邻居的Voronoi子图或者换用基于Delaunay三角剖分的近似算法。这个在工程上叫“分布式Voronoi”实现起来会复杂一些但思路是一致的。3.3 核心控制律代码有了质心控制律就很简单。我单独写了个函数输入当前状态和邻居状态输出加速度function u compute_control(p, v, c, adj) % p: N*2 位置 % v: N*2 速度 % c: N*2 Voronoi质心 % adj: N*N 邻接矩阵 N size(p,1); u zeros(N,2); Kp 1.8; Kv 1.2; Kc 0.8; for i 1:N u(i,:) Kp * (c(i,:) - p(i,:)) - Kv * v(i,:); % 一致性项 for j 1:N if adj(i,j) j ~ i u(i,:) u(i,:) Kc * (v(j,:) - v(i,:)); end end end end这里adj怎么建我按通信半径来两架飞机距离小于comm_range就算邻居。实际编队规模大时可以预先算好邻接矩阵N很小时每次循环都算也很快。注意不要把一致性项做得太强Kc一大队伍会像橡皮筋一样来回甩我调参时吃过这个亏。主循环只要做四件事计算Voronoi质心、算控制、更新状态、画图。状态更新可以直接用矢量写法for k 1:round(T/dt) c compute_voronoi_centroid(p); u compute_control(p, v, c, adj) compute_repulsion(p, obs, r_obs, d0, eta_rep); v v u*dt; p p v*dt; % 画图... end这一步看起来没什么技术含量但我在实际调试时发现状态的更新顺序很容易写反。一定要先用当前状态算控制量再更新速度再更新位置。顺序反了控制器看到的永远是上一帧的状态相当于给系统加了一个步长的延迟小增益还能跑增益一大直接发散。3.4 避障函数实现避障函数我写成独立函数方便单独调试。核心部分如下function u_rep compute_repulsion(p, obs, r_obs, d0, eta) N size(p,1); M size(obs,1); u_rep zeros(N,2); for i 1:N for j 1:M delta p(i,:) - obs(j,:); d norm(delta); if d d0 r_obs % 方向单位向量 n delta / max(d, 1e-6); % 从障碍表面起算的距离 factor eta * (1/(d - r_obs) - 1/d0) / (d - r_obs)^2; u_rep(i,:) u_rep(i,:) factor * n; % 切向分量防止局部最优 tangent [-n(2), n(1)]; u_rep(i,:) u_rep(i,:) 0.4 * factor * tangent; end end end end这里factor用的是(d - r_obs)而不是d相当于从障碍表面开始算距离物理意义更明确飞机越贴近障碍表面斥力增长越猛。max(d, 1e-6)是防止两架飞机完全重叠时除零。避障代码还有个小坑斥力可能会把飞机推出边界或者推到另一个障碍物脸上。多障碍场景下我通常在一个步长里把所有障碍的斥力线性叠加同时把合成的斥力限幅比如单轴不超过3m/s^2否则步长稍大就会数值爆炸。这个限幅值要根据最大速度来设一般取最大加速度的70%-80%比较合适。3.5 可视化与动画输出仿真好玩的地方在于可视化。我画了三个图层背景绘制障碍物圆、Voronoi边界、无人机节点和航迹。核心代码figure(Color,white); hold on; theta linspace(0, 2*pi, 80); plot(obs(:,1)r_obs*cos(theta), obs(:,2)r_obs*sin(theta), k); for k 1:round(T/dt) % 先更新状态 c compute_voronoi_centroid(p); u compute_control(p, v, c, adj) compute_repulsion(p, obs, r_obs, d0, eta_rep); v v u*dt; p p v*dt; % 再画图 cla; voronoi(p(:,1), p(:,2), --); hold on; plot(obs(:,1)r_obs*cos(theta), obs(:,2)r_obs*sin(theta), k); plot(p(:,1), p(:,2), ro, MarkerSize, 8); quiver(p(:,1), p(:,2), v(:,1), v(:,2), 0.3, r); title(sprintf(t %.2f s, k*dt)); drawnow; endvoronoi每次重画边界会闪但能直观看到编队变化。想看轨迹就把每步的p存到数组里然后用plot(traj(:,1), traj(:,2), b-)画出来。要输出GIF的话用exportgraphics逐帧保存PNG再做拼接R2020b以后也可以用内置的gif写文件但我在脚本里还是习惯存帧序列方便后期处理。还有一个细节画图也会消耗仿真时间导致实时动画很卡。我的做法是每5帧才刷新一次画面但计算每帧都做。这样既能看到趋势又不会让drawnow成为整个仿真的瓶颈。4. 实操记录完整运行一个编队避障场景4.1 场景设置我给一个可以直接复现的案例。6架无人机初始位置随机分布在原点附近编号初始位置1(0.2, 0.5)2(-0.8, 0.1)3(0.6, -0.4)4(-0.3, -0.9)5(0.9, 0.7)6(-0.6, 0.4)目标不是手写一个队形点而是设定整个编队要飞到(8, 8)附近、队形直径约3米。具体做法是在控制律的位置反馈里加一个群体目标项p_i和c_i的差值之外再加上(goal - c_i)的弱引力让整个Voronoi参考系往目标区域平移。相当于每架飞机不是飞向自己单元的绝对质心而是飞向“质心群体偏移量”。这个偏移量每架都相同所以不会破坏队形。障碍物我摆了一个圆圆心(5, 5)半径0.5米正好挡在从起点到(8,8)的必经之路上。无人机速度初值设为零仿真时间25秒。这里我要特意说明初始位置虽然叫“随机”但别真用完全随机的分布否则可能出现两架飞机初始重叠直接导致Voronoi退化。我的做法是先用均匀莲花撒点再叠加一个0.1量级的小扰动这样既随机又安全。4.2 调参过程第一次跑我用的Kp2.5, Kv0.8结果飞机在初始阶段就抖得厉害位置轨迹像锯齿。原因很简单Voronoi质心随位置变化很快Kp太高导致控制器对质心扰动反应过猛而Kv不足又压不住。我把Kp降到1.5Kv提到1.5震荡立刻缓解。之后又加了Kc0.6队形一致性明显改善。避障参数是另一个坎。eta_rep1时飞机以1.2m/s的速度靠近障碍在距离1米处开始转向但横向偏移不够几乎贴着障碍边蹭过去看起来悬。我把eta_rep提到3切向系数从0.4提到0.6飞机在距离障碍1.5米左右就开始清晰绕行路径也圆滑了。这里提醒一下过高的eta_rep会导致飞机离障碍老远就绕圈队形被拉得不成样子所以eta_rep最好按“紧急刹车距离”来算v_max * dt * 安全余量。调参时我还做了个笨但有效的操作把voronoi画图关掉只在终端打印每一帧的质心位置和面积。这样能排除绘图干扰更快定位是控制问题还是数值问题。等参数稳定了再打开画图体验会好很多。4.3 观察到的现象与分析跑通后的轨迹大致分三个阶段。第一阶段0-5秒飞机从随机分布逐渐形成以质心为中心的均匀展开队形Voronoi单元面积差异从大变小队形呈近似正六边形。第二阶段5-12秒编队整体平移接近障碍前侧飞机先感受到斥力开始减速并向外侧偏转随后整个梯队像一列火车一样逐架绕行队形被拉成弧形。第三阶段12秒后所有飞机越过障碍Voronoi质心重新拉平队形在目标区域附近收敛速度趋近于零。最让我意外的是绕障过程中没有一架飞机需要和邻居“商量”绕行方向纯粹靠Voronoi质心偏移和局部斥力整个梯队就自动完成了一次类似“S形变道”的动作。这说明把Voronoi参考点和势场避障分层叠加的设计是自洽的。当然前提是障碍不能太密集如果两个障碍间距小于编队直径这套简单方案还是会卡壳需要再加路径规划层。我还记录了各架飞机的最小机间距。在绕障最紧张的时刻机间距一度从初始的1.2米压缩到0.55米但仍然大于我们设的安全距离0.4米。这个结果说明即使避障力在起作用机间避碰也不能只靠Voronoi的“弹性”——尤其是N很大的时候两架飞机可能同时被同一个障碍推到同一侧。所以我在后续版本里又加了一个机间斥力项和障碍斥力并列专门防止飞机互相贴近。5. 常见问题与排查技巧实录5.1 编队发散或者原地抖动表现是位置曲线高频锯齿或者飞机越飞越远。原因九成是增益和步长不匹配。先检查Kp和KvKv太小阻尼不够再检查dt如果dt 0.1且Kp2二阶离散系统很容易失稳。我调试时习惯先关掉避障和一致性项只跑“单机追Voronoi质心”确认稳定后再逐项打开。这个“控制变量法”在复杂系统里真的能救命。还有一个隐患是Voronoi质心计算里混入Inf顶点导致NaN。只要某一架飞机位置输入了NaN后面全崩。我为这个写了保护在控制律入口检查all(isfinite(p))不是就直接报错并打印当前帧能省半天排查时间。建议你也这么做不要相信数值计算永远不出问题。5.2 Voronoi边界跳变导致目标点突变当两架飞机位置接近时它们的Voronoi边界会移动得非常快质心可能在一帧之间跳一下。如果不做平滑控制器就会感受到阶跃信号产生抖动。我的做法是对质心做指数滤波c_smooth alpha * c_new (1-alpha) * c_oldalpha取0.3到0.5。这样目标点变化圆滑代价是响应稍慢但编队整体更稳。如果场景里N很大比如20架以上建议不要每帧都重算全局Voronoi只算局部几个邻居的子图。这会牺牲一定的全局最优性但分布式控制本来就不要求全局最优只要局部行为符合预期跑起来效率高得多。这里我可以给出一个判断标准单帧计算超过10毫秒就要考虑优化了。5.3 避障失效或者穿模第一反应看d0和eta_rep是否匹配。如果飞机速度1.5m/s斥力作用半径1m留给飞机的反应时间不到0.7秒除非eta_rep极大否则必然蹭到障碍。正确做法把d0设为v_max * 1.5 r_obs比如v_max1.5d02.5左右。这样飞机在2米外就开始有反应轨迹自然柔和。还有一种“穿模”是数值原因dt太大飞机在一个步长内直接穿过障碍物圆心。这时斥力检测不到近距离因为采样点刚好跳过危险区域。解决方法是步长减半或者做连续碰撞检测判断线段和圆是否相交。后者实现不复杂但一般仿真用不到我建议先减步长简单有效。5.4 调试效率提升的几个小工具第一一定要把每帧的Voronoi单元面积记录下来。面积从大到小、趋于均匀的过程比肉眼看轨迹更能反映编队收敛还能帮你判断是不是有飞机卡在某处。第二用tic/toc统计单帧耗时如果超过0.05秒实时动画就是假的你得考虑减少画图频率。第三把关键参数做成脚本顶部的结构体方便批量跑参数扫描。我之前用for循环嵌套跑不同Kp和eta_rep的组合画出一个“收敛性-安全距离”的二维图调参从玄学变成了有数据支撑的选择。还有一个经验是不要在仿真中途频繁改代码尤其是正在画图的时候。Matlab的figure和drawnow对键盘交互响应很慢改个参数要等半天。我的习惯是把所有需要调试的参数放在一个param.m里每次只改这个文件主脚本保持稳定。这样既清晰又能避免误改。6. 进阶扩展从二维仿真到更真实场景的几个方向6.1 把质点模型换成四旋翼动力学当前仿真用的是质点模型直接输出加速度这是控制层的抽象实现。但做无人机控制的人都知道四旋翼不能直接输出水平加速度它要通过改变姿态角来产生水平推力。外面看起来是“期望加速度”里面其实是“期望滚转角/俯仰角”的映射。简单写法是把控制量u当作期望加速度然后转换roll_desired (u_x / g) * sign(推力方向)这个转换在Matlab里可以单独做一个函数后续接入飞控仿真时只需要改这一层。编队和避障的算法完全不用动因为它们都工作在位置-速度层。这就是分层的好处。6.2 动态障碍与时变编队如果障碍物会移动势场法也能用但需要每个仿真步更新障碍位置并且斥力作用的相对速度要加进判断。比如障碍朝飞机飞过来哪怕距离还很远也要提前避让障碍远离时斥力可以减弱。这个可以通过相对速度修正d0来实现d0_eff d0 relative_velocity * dt。Voronoi参考点也可以做成时变的比如在飞行中把队形从正六边形切换成直线或者三角形。切换的平滑方式就是慢慢移动虚拟目标点而不是瞬间改变每个Voronoi种子位置。我在实验里让编队从“菱形”过渡到“长蛇”在25秒内完成队形没有明显震荡。这个扩展非常适合做航拍、巡检等需要变换编队构型的任务。6.3 通信受限与丢包模拟分布式控制最依赖通信但真实环境里通信会丢包、会延迟。最简单的模拟方式是在每个控制步按概率随机丢弃部分邻居状态比如丢包率10%那么一致性项只使用收到的邻居信息。你会发现编队依然能维持但收敛速度和队形精度会下降。如果丢包率太高一致性项会退化编队可能解耦。我目前的做法是加一个“零阶保持”丢包时沿用上一帧收到的邻居状态这样能维持基本的一致性。更进阶的方案是加一个简单预测器用邻居上一帧的速度估计它的当前位置但这就涉及卡尔曼滤波的范畴了。先不要贪多能模拟丢包并观察系统鲁棒性已经是很好的扩展方向。最后再说一点个人体会。这套仿真做完我最深的感触是Voronoi在Matlab里实现起来其实不难真正的功夫都在那些看不见的细节上无穷顶点怎么剔除、质心要不要滤波、斥力作用半径怎么跟速度匹配、先关掉哪一层再调哪一层。如果你也准备做类似的多智能体编队仿真建议不要急着追求复杂算法先把一个6机避障场景在Matlab里从画图到闭环全部跑通再逐步加规模、加通信延迟、加模型误差。那些高级问题到了这一步自然会有新的解决思路。 前阵子有位做飞行控制的朋友问我多无人机编队控制在Matlab里到底怎么才能跑起来又不只是图个动画好看。我把自己之前整理的一套基于多智能体框架的仿真翻出来正好借这个机会说透。这个项目的名字虽然长但内容很聚焦基于多智能体无人机群体控制包含障碍避让和基于Voronoi的编队控制整体用Matlab实现。简单讲它解决的是这么一件事——一群无人机从分散位置起飞先按Voronoi划分的思路生成编队再保持队形飞向目标区域途中遇到圆形障碍能自动避让整个过程是分布式的每一架都只依赖自己和邻居的局部信息。这套东西适合谁来参考我觉得是三类人一是刚开始接触多智能体控制、想在Matlab里做编队仿真的研究生二是做实际无人系统集成的工程师需要一个能快速验证控制协议的原型三是对Voronoi图感兴趣想看看它除了画网格以外还能怎么用在机器人控制上的人。读完你会有一个能直接改参数跑起来的框架更重要的是知道每个环节为什么这么设计坑都在哪。1. 项目到底在做什么拆解多智能体编队控制的核心问题1.1 编队控制的三层需求多智能体无人机群体控制听起来高大上拆开看其实就三层需求。第一层是队形生成一群无人机不能像无头苍蝇一样随机分布它们要形成一个明确的几何构型比如三角形、五边形、直线纵队或者本项目的Voronoi覆盖形。第二层是队形保持在飞向目标过程中每架飞机和邻机之间的相对位置不能乱至少不能超过允许误差。第三层是安全约束机间不能碰撞遇到环境障碍要避让这两条是硬约束优先级高于队形精度。把这三层放到同一个控制律里最大的难点不是单层设计而是它们互相打架。比如严格保持队形时无人机往往没空间绕障碍绕障碍时队形又会被拉扯变形。所以我们在设计上的思路是分层底层用位置-速度反馈保证基本跟踪中间加上一致性项协调邻居关系最外层加势场避障让避障力只在危险距离内起作用。每层各司其职不会出现“为了安全把队形完全丢掉或者为了队形一头撞上障碍”的尴尬局面。另外要注意这个“分层”不是把控制器简单相加就完事。每一层都有自己起作用的阈值和增益层次之间还要做限幅。比如避障斥力我一般会限幅到最大加速度的70%避免局部避障力太大把编队拽得面目全非。这些细节看起来不起眼却是仿真能不能收敛的关键。1.2 为什么选Voronoi而不是别的编队方法编队控制方案不少常见的有领航者-跟随者、虚拟结构、基于行为以及本项目的Voronoi划分法。领航者-跟随者最简单一台领航机带着跑但缺点是领航机挂了整个队伍就完蛋而且后机容易滞后。虚拟结构把整个队伍当刚体约束精确但太死板碰到障碍时整体变形困难。基于行为法灵活但行为优先级和权重很难调容易出现“各走各的”。Voronoi划分的思路是给每架无人机分配一块“势力范围”范围边界的交点或者质心可以直接作为期望队形参考点。它的好处是天然分布式——每架飞机只要知道邻居位置就能算自己的单元不需要全局指挥而且队形本身有几何意义目标点到谁的单元最近就归谁管编队和任务分配可以合在一起做。用生活类比它不是让飞机排成固定几个格子而是相当于给每架飞机画“领地”领地的边界会随着飞机移动自动调整队形就有了弹性。把几种方案放在一起看会更清楚方法分布式程度队形精度避障扩展适用场景领航者-跟随者低低容易小规模、通信可靠虚拟结构低-中高难高精度编队基于行为高中容易动态环境Voronoi划分高中高容易区域覆盖、集群控制从表格能看出Voronoi没有明显短板尤其适合做“群体去中心化避障”的需求。当时我选它还有一个原因Voronoi单元能够自然地反映邻居分布密度单元面积大的飞机说明“周围空旷”控制律会把它往空地拉一点这正好和编队展开的需求吻合。1.3 障碍避让在整个系统中的位置障碍避让在这个项目里不是锦上添花是绕不开的痛点。很多编队仿真在空地跑得很漂亮一加障碍就乱了前机绕障后机还在按原队形冲结果撞在一起。所以我把避障设计成局部势场只有检测到障碍进入安全半径才启用正常巡航时完全不影响编队控制。这么设计的另一个原因是Voronoi编队本身对障碍有一定“天然容忍度”。当某架飞机被障碍挡住时它的Voronoi单元会被压扁变形参考点也会跟着挪这就相当于队形自动让出一条路。我们只需要在外层补一个短距离斥力防止飞机贴脸撞障碍剩下的交给编队协议去恢复。这个“底层避碰中层队形顶层Voronoi参考点”的结构是整个项目能跑通的关键。还有一个容易忽略的问题避障的优先级必须设计好。我的做法是给每个控制分量一个“作用标签”避障力一旦激活队形保持项可以暂时降权但不直接置零。完全置零会导致飞机绕障后找不到编队目标轨迹出现明显的“发飘”。降权之后的恢复过程更自然。2. 算法原理Voronoi划分与编队控制怎么结合2.1 Voronoi图到底怎么理解Voronoi图可能有些人只在几何课本里见过说白了就是平面最近邻划分。给定几个种子点把平面切分成若干个区域每个区域包含的点到对应种子点的距离比到其他任何种子点都近。区域之间的边界是相邻种子点的垂直平分线。数学上可以写成V_i {x | ||x - p_i|| ||x - p_j||, j ≠ i}每个“地盘”都是凸多边形所有地盘拼起来铺满整个平面。生活化的理解很简单每个种子点就是一个“基站”Voronoi图就是每个基站信号覆盖得最好的那片地盘边界处两个基站的信号强度相同。在无人机编队里种子点就是无人机当前位置。每架飞机维护自己的Voronoi单元单元的形状反映邻居分布单元质心则是一个“局部期望位置”。当飞机与邻居相对均匀时质心基本靠近自己当某侧邻居拥挤时质心会被推向空旷侧飞机就有向空旷侧运动的趋势这正好可以用来疏散、展开队形。Matlab里画Voronoi图很简单voronoi(px, py)一行就能出图但我们要的不是图而是每个单元的顶点集合和面积这就得用voronoin函数。它会返回顶点矩阵v和单元顶点索引c注意靠近无穷远处的顶点索引是1这些顶点坐标是无穷大需要专门处理。这个细节是后面所有目标点计算的基础很多新手在这里踩坑。2.2 从Voronoi划分到编队目标点分配我们需要想清楚“用Voronoi做编队”到底是怎么操作。最简单的思路先给整个编队设定一个期望队形位置集合比如6架飞机要停在某个圆形或方形区域里那么每架飞机的期望目标点可以取它当前Voronoi单元的某一特征点通常是质心。当分布均匀时质心会趋向于最终队形点当分布不均匀时质心会被邻居“推”到空旷处从而实现自动调整。具体来说每架无人机计算自己的Voronoi单元后求该多边形区域的质心记作c_i。然后控制目标是让飞机飞向c_i。这里有个看似矛盾的点质心由当前位置决定当前位置又由质心控制这不闭环了实际上算法会在迭代中收敛。大家熟悉的Lloyd算法就是这个思想不断把点移动到Voronoi单元质心最终会收敛到质心Voronoi划分CVT点分布趋于均匀。我们做的编队控制其实就是一个带动力学约束的Lloyd过程飞机不是瞬间跳到质心而是受速度、加速度限制地逼近它所以不会死锁也不会抖得很剧烈。关于特征点的选择除了质心也可以用距离所有邻居最远的点或者单元某个顶点。但质心最好实现也最平滑因为单元几何中心是位置的连续函数只要不穿越退化状态不会突然跳变。这一点对控制稳定性很重要。如果追求覆盖效率可以用面积加权的CVT目标点如果追求队形拓扑稳定可以用Voronoi顶点作为参考。我实测下来质心方案在6到20架规模内都表现稳定是性价比最高的选择。2.3 分布式控制协议设计有了目标点下一步就是设计控制力。系统简化成二维平面内的质点模型[p_xi; p_yi]是第i架无人机位置[v_xi; v_yi]是速度控制输入是加速度u_i。核心控制律我写成u_i -K_p * (p_i - c_i) - K_v * v_i u_i_cons u_i_rep其中-K_p * (p_i - c_i)是位置反馈把飞机拉向Voronoi质心-K_v * v_i是速度阻尼防止过冲和震荡u_i_cons是邻居一致性项让相邻飞机的速度趋同u_i_rep是避障斥力只在飞机进入障碍危险范围时才有值。用PID的类比来理解K_p是比例项K_v是微分项一致性项相当于把邻居状态也引入控制器让队伍整体“齐步走”。一致性项的具体形式u_i_cons K_c * sum_{j in N_i} (v_j - v_i)N_i是第i架飞机的邻居集合一般用通信半径来判断。当邻居速度和本机速度不一致时这个力会拉平速度差。它不直接改变位置但能大幅减少队伍内部的相对运动编队看起来更像一个整体而不是一群各自飞的机器。离散更新就用欧拉法v_i(k1) v_i(k) u_i * dtp_i(k1) p_i(k) v_i(k1) * dtdt一般取0.02到0.1秒。太快跑不动太慢容易在临界条件附近发散。我实测dt0.05在大多数场景都稳。2.4 障碍避让策略避障用人工势场法这是最经典也最好调试的方案。每个障碍物对外产生一个斥力场大小随距离衰减目标方向有一个引力场保证飞机不会停在某个角落。斥力公式我习惯用F_rep eta * (1/d - 1/d0) * (1/d^2) * n当d d0时才计算d是飞机到障碍中心的距离d0是斥力作用半径n是从障碍指向飞机的单位向量。d0比障碍物物理半径大一圈比如障碍半径0.5米d0取1米这样飞机在距离障碍1米时开始感受斥力逐步减速偏转。这里有个避坑点斥力只用一项的话容易在“目标点在障碍背后”时陷入局部最优飞机可能在障碍前左右抖动不前进。我的处理是在斥力之外加一个切向力或者当检测到距离缩短但速度仍指向障碍时让飞机先停止前冲再沿切向绕行。简单实现是给斥力加一个垂直于“障碍-飞机”连线的分量系数取主斥力的0.3到0.5。这个技巧能显著减少“顶着障碍往死里怼”的情况。还有一点要注意势场法并不是万能的在复杂迷宫环境中它一定会局部最优。但作为编队控制里的一个“安全层”它足够用了。真正的全局路径规划可以交给更高层的A*、RRT*这个项目只做局部避障所以不展开。3. Matlab实现整体架构与关键代码3.1 仿真环境搭建与参数定义我先说明环境我用的是Matlab R2022b代码基本兼容R2020b以后的版本主要依赖polyshape和voronoin这些基础功能不需要额外工具箱。整个仿真我建了一个主脚本和几个函数文件主脚本负责参数初始化、循环推进和画图函数文件负责控制律、Voronoi目标点计算和避障判断。第一步是参数区写在主脚本最前面clear; clc; close all; N 6; % 无人机数量 dt 0.05; % 仿真步长 T 30; % 仿真时长 Kp 1.8; Kv 1.2; Kc 0.8; % 控制增益 eta_rep 2.0; % 斥力增益 d0 1.0; % 斥力作用半径 r_obs 0.5; % 障碍物理半径 comm_range 2.5; % 通信半径这些参数是怎么来的Kp和Kv要匹配成类似二阶系统的阻尼关系如果假设单位质量闭环近似是Kp/(s^2 Kv*s Kp)。要让系统不震荡Kv大约在2*sqrt(Kp)附近。比如Kp1.8理论值约2.68但实际有Voronoi质心动态和离散化误差我取小一点1.2让系统偏“钝”而不是偏“冲”。eta_rep和d0需要和速度匹配速度上限约1.5m/s时eta_rep2能让斥力在0.8米处把飞机推偏。参数这个东西最忌讳的是照抄别人的。同样的控制律换一个仿真步长或者换一组初始位置最优参数就变了。所以我会在脚本顶部放一个参数结构体方便批量修改。后面第5章的调参方法也是围绕这个结构体展开的。3.2 Voronoi目标点计算的Matlab实现每个仿真步都要重新计算所有无人机的Voronoi单元。我封装了一个函数compute_voronoi_centroidfunction [cen, area] compute_voronoi_centroid(p) N size(p, 1); [v, c] voronoin(p); cen zeros(N, 2); area zeros(N, 1); for i 1:N verts v(c{i}, :); % 去掉无穷远顶点索引1对应的坐标可能为Inf finite isfinite(verts(:,1)) isfinite(verts(:,2)); if sum(finite) 3 cen(i, :) p(i, :); area(i) 0; continue; end poly polyshape(verts(finite, 1), verts(finite, 2)); [cx, cy] centroid(poly); cen(i, :) [cx, cy]; area(i) polyarea(verts(finite,1), verts(finite,2)); end end注意一个关键细节voronoin返回的单元顶点索引c{i}中如果包含1号顶点说明这个单元延伸到无穷远。直接算质心会有NaN或爆炸值。我先用isfinite把无穷顶点剔掉剔除后如果不足三个点说明这个单元退化就暂时用飞机自身位置当作质心等下一帧再修正。另一个值得提的地方是polyshape和centroid的用法。centroid是R2017b以后引入的函数用来算多边形质心。如果你用的是老版本可以自己写面积加权公式但务必要先用polyarea核对一下。还有当无人机数量很多时voronoin的计算量会随N^2增长。20架以内基本无感超过50架建议只计算局部邻居的Voronoi子图或者换用基于Delaunay三角剖分的近似算法。这个在工程上叫“分布式Voronoi”实现起来会复杂一些但思路是一致的。3.3 核心控制律代码有了质心控制律就很简单。我单独写了个函数输入当前状态和邻居状态输出加速度function u compute_control(p, v, c, adj) % p: N*2 位置 % v: N*2 速度 % c: N*2 Voronoi质心 % adj: N*N 邻接矩阵 N size(p,1); u zeros(N,2); Kp 1.8; Kv 1.2; Kc 0.8; for i 1:N u(i,:) Kp * (c(i,:) - p(i,:)) - Kv * v(i,:); % 一致性项 for j 1:N if adj(i,j) j ~ i u(i,:) u(i,:) Kc * (v(j,:) - v(i,:)); end end end end这里adj怎么建我按通信半径来两架飞机距离小于comm_range就算邻居。实际编队规模大时可以预先算好邻接矩阵N很小时每次循环都算也很快。注意不要把一致性项做得太强Kc一大队伍会像橡皮筋一样来回甩我调参时吃过这个亏。主循环只要做四件事计算Voronoi质心、算控制、更新状态、画图。状态更新可以直接用矢量写法for k 1:round(T/dt) c compute_voronoi_centroid(p); u compute_control(p, v, c, adj) compute_repulsion(p, obs, r_obs, d0, eta_rep); v v u*dt; p p v*dt; % 画图... end这一步看起来没什么技术含量但我在实际调试时发现状态的更新顺序很容易写反。一定要先用当前状态算控制量再更新速度再更新位置。顺序反了控制器看到的永远是上一帧的状态相当于给系统加了一个步长的延迟小增益还能跑增益一大直接发散。3.4 避障函数实现避障函数我写成独立函数方便单独调试。核心部分如下function u_rep compute_repulsion(p, obs, r_obs, d0, eta) N size(p,1); M size(obs,1); u_rep zeros(N,2); for i 1:N for j 1:M delta p(i,:) - obs(j,:); d norm(delta); if d d0 r_obs % 方向单位向量 n delta / max(d, 1e-6); % 从障碍表面起算的距离 factor eta * (1/(d - r_obs) - 1/d0) / (d - r_obs)^2; u_rep(i,:) u_rep(i,:) factor * n; % 切向分量防止局部最优 tangent [-n(2), n(1)]; u_rep(i,:) u_rep(i,:) 0.4 * factor * tangent; end end end end这里factor用的是(d - r_obs)而不是d相当于从障碍表面开始算距离物理意义更明确飞机越贴近障碍表面斥力增长越猛。max(d, 1e-6)是防止两架飞机完全重叠时除零。避障代码还有个小坑斥力可能会把飞机推出边界或者推到另一个障碍物脸上。多障碍场景下我通常在一个步长里把所有障碍的斥力线性叠加同时把合成的斥力限幅比如单轴不超过3m/s^2否则步长稍大就会数值爆炸。这个限幅值要根据最大速度来设一般取最大加速度的70%-80%比较合适。3.5 可视化与动画输出仿真好玩的地方在于可视化。我画了三个图层背景绘制障碍物圆、Voronoi边界、无人机节点和航迹。核心代码figure(Color,white); hold on; theta linspace(0, 2*pi, 80); plot(obs(:,1)r_obs*cos(theta), obs(:,2)r_obs*sin(theta), k); for k 1:round(T/dt) % 先更新状态 c compute_voronoi_centroid(p); u compute_control(p, v, c, adj) compute_repulsion(p, obs, r_obs, d0, eta_rep); v v u*dt; p p v*dt; % 再画图 cla; voronoi(p(:,1), p(:,2), --); hold on; plot(obs(:,1)r_obs*cos(theta), obs(:,2)r_obs*sin(theta), k); plot(p(:,1), p(:,2), ro, MarkerSize, 8); quiver(p(:,1), p(:,2), v(:,1), v(:,2), 0.3, r); title(sprintf(t %.2f s, k*dt)); drawnow; endvoronoi每次重画边界会闪但能直观看到编队变化。想看轨迹就把每步的p存到数组里然后用plot(traj(:,1), traj(:,2), b-)画出来。要输出GIF的话用exportgraphics逐帧保存PNG再做拼接R2020b以后也可以用内置的gif写文件但我在脚本里还是习惯存帧序列方便后期处理。还有一个细节画图也会消耗仿真时间导致实时动画很卡。我的做法是每5帧才刷新一次画面但计算每帧都做。这样既能看到趋势又不会让drawnow成为整个仿真的瓶颈。4. 实操记录完整运行一个编队避障场景4.1 场景设置我给一个可以直接复现的案例。6架无人机初始位置随机分布在原点附近编号初始位置1(0.2, 0.5)2(-0.8, 0.1)3(0.6, -0.4)4(-0.3, -0.9)5(0.9, 0.7)6(-0.6, 0.4)目标不是手写一个队形点而是设定整个编队要飞到(8, 8)附近、队形直径约3米。具体做法是在控制律的位置反馈里加一个群体目标项p_i和c_i的差值之外再加上(goal - c_i)的弱引力让整个Voronoi参考系往目标区域平移。相当于每架飞机不是飞向自己单元的绝对质心而是飞向“质心群体偏移量”。这个偏移量每架都相同所以不会破坏队形。障碍物我摆了一个圆圆心(5, 5)半径0.5米正好挡在从起点到(8,8)的必经之路上。无人机速度初值设为零仿真时间25秒。这里我要特意说明初始位置虽然叫“随机”但别真用完全随机的分布否则可能出现两架飞机初始重叠直接导致Voronoi退化。我的做法是先用均匀莲花撒点再叠加一个0.1量级的小扰动这样既随机又安全。4.2 调参过程第一次跑我用的Kp2.5, Kv0.8结果飞机在初始阶段就抖得厉害位置轨迹像锯齿。原因很简单Voronoi质心随位置变化很快Kp太高导致控制器对质心扰动反应过猛而Kv不足又压不住。我把Kp降到1.5Kv提到1.5震荡立刻缓解。之后又加了Kc0.6队形一致性明显改善。避障参数是另一个坎。eta_rep1时飞机以1.2m/s的速度靠近障碍在距离1米处开始转向但横向偏移不够几乎贴着障碍边蹭过去看起来悬。我把eta_rep提到3切向系数从0.4提到0.6飞机在距离障碍1.5米左右就开始清晰绕行路径也圆滑了。这里提醒一下过高的eta_rep会导致飞机离障碍老远就绕圈队形被拉得不成样子所以eta_rep最好按“紧急刹车距离”来算v_max * dt * 安全余量。调参时我还做了个笨但有效的操作把voronoi画图关掉只在终端打印每一帧的质心位置和面积。这样能排除绘图干扰更快定位是控制问题还是数值问题。等参数稳定了再打开画图体验会好很多。4.3 观察到的现象与分析跑通后的轨迹大致分三个阶段。第一阶段0-5秒飞机从随机分布逐渐形成以质心为中心的均匀展开队形Voronoi单元面积差异从大变小队形呈近似正六边形。第二阶段5-12秒编队整体平移接近障碍前侧飞机先感受到斥力开始减速并向外侧偏转随后整个梯队像一列火车一样逐架绕行队形被拉成弧形。第三阶段12秒后所有飞机越过障碍Voronoi质心重新拉平队形在目标区域附近收敛速度趋近于零。最让我意外的是绕障过程中没有一架飞机需要和邻居“商量”绕行方向纯粹靠Voronoi质心偏移和局部斥力整个梯队就自动完成了一次类似“S形变道”的动作。这说明把Voronoi参考点和势场避障分层叠加的设计是自洽的。当然前提是障碍不能太密集如果两个障碍间距小于编队直径这套简单方案还是会卡壳需要再加路径规划层。我还记录了各架飞机的最小机间距。在绕障最紧张的时刻机间距一度从初始的1.2米压缩到0.55米但仍然大于我们设的安全距离0.4米。这个结果说明即使避障力在起作用机间避碰也不能只靠Voronoi的“弹性”——尤其是N很大的时候两架飞机可能同时被同一个障碍推到同一侧。所以我在后续版本里又加了一个机间斥力项和障碍斥力并列专门防止飞机互相贴近。5. 常见问题与排查技巧实录5.1 编队发散或者原地抖动表现是位置曲线高频锯齿或者飞机越飞越远。原因九成是增益和步长不匹配。先检查Kp和KvKv太小阻尼不够再检查dt如果dt 0.1且Kp2二阶离散系统很容易失稳。我调试时习惯先关掉避障和一致性项只跑“单机追Voronoi质心”确认稳定后再逐项打开。这个“控制变量法”在复杂系统里真的能救命。还有一个隐患是Voronoi质心计算里混入Inf顶点导致NaN。只要某一架飞机位置输入了NaN后面全崩。我为这个写了保护在控制律入口检查all(isfinite(p))不是就直接报错并打印当前帧能省半天排查时间。建议你也这么做不要相信数值计算永远不出问题。5.2 Voronoi边界跳变导致目标点突变当两架飞机位置接近时它们的Voronoi边界会移动得非常快质心可能在一帧之间跳一下。如果不做平滑控制器就会感受到阶跃信号产生抖动。我的做法是对质心做指数滤波c_smooth alpha * c_new (1-alpha) * c_oldalpha取0.3到0.5。这样目标点变化圆滑代价是响应稍慢但编队整体更稳。如果场景里N很大比如20架以上建议不要每帧都重算全局Voronoi只算局部几个邻居的子图。这会牺牲一定的全局最优性但分布式控制本来就不要求全局最优只要局部行为符合预期跑起来效率高得多。这里我可以给出一个判断标准单帧计算超过10毫秒就要考虑优化了。5.3 避障失效或者穿模第一反应看d0和eta_rep是否匹配。如果飞机速度1.5m/s斥力作用半径1m留给飞机的反应时间不到0.7秒除非eta_rep极大否则必然蹭到障碍。正确做法把d0设为v_max * 1.5 r_obs比如v_max1.5d02.5左右。这样飞机在2米外就开始有反应轨迹自然柔和。还有一种“穿模”是数值原因dt太大飞机在一个步长内直接穿过障碍物圆心。这时斥力检测不到近距离因为采样点刚好跳过危险区域。解决方法是步长减半或者做连续碰撞检测判断线段和圆是否相交。后者实现不复杂但一般仿真用不到我建议先减步长简单有效。5.4 调试效率提升的几个小工具第一一定要把每帧的Voronoi单元面积记录下来。面积从大到小、趋于均匀的过程比肉眼看轨迹更能反映编队收敛还能帮你判断是不是有飞机卡在某处。第二用tic/toc统计单帧耗时如果超过0.05秒实时动画就是假的你得考虑减少画图频率。第三把关键参数做成脚本顶部的结构体方便批量跑参数扫描。我之前用for循环嵌套跑不同Kp和eta_rep的组合画出一个“收敛性-安全距离”的二维图调参从玄学变成了有数据支撑的选择。还有一个经验是不要在仿真中途频繁改代码尤其是正在画图的时候。Matlab的figure和drawnow对键盘交互响应很慢改个参数要等半天。我的习惯是把所有需要调试的参数放在一个param.m里每次只改这个文件主脚本保持稳定。这样既清晰又能避免误改。6. 进阶扩展从二维仿真到更真实场景的几个方向6.1 把质点模型换成四旋翼动力学当前仿真用的是质点模型直接输出加速度这是控制层的抽象实现。但做无人机控制的人都知道四旋翼不能直接输出水平加速度它要通过改变姿态角来产生水平推力。外面看起来是“期望加速度”里面其实是“期望滚转角/俯仰角”的映射。简单写法是把控制量u当作期望加速度然后转换roll_desired (u_x / g) * sign(推力方向)这个转换在Matlab里可以单独做一个函数后续接入飞控仿真时只需要改这一层。编队和避障的算法完全不用动因为它们都工作在位置-速度层。这就是分层的好处。6.2 动态障碍与时变编队如果障碍物会移动势场法也能用但需要每个仿真步更新障碍位置并且斥力作用的相对速度要加进判断。比如障碍朝飞机飞过来哪怕距离还很远也要提前避让障碍远离时斥力可以减弱。这个可以通过相对速度修正d0来实现d0_eff d0 relative_velocity * dt。Voronoi参考点也可以做成时变的比如在飞行中把队形从正六边形切换成直线或者三角形。切换的平滑方式就是慢慢移动虚拟目标点而不是瞬间改变每个Voronoi种子位置。我在实验里让编队从“菱形”过渡到“长蛇”在25秒内完成队形没有明显震荡。这个扩展非常适合做航拍、巡检等需要变换编队构型的任务。6.3 通信受限与丢包模拟分布式控制最依赖通信但真实环境里通信会丢包、会延迟。最简单的模拟方式是在每个控制步按概率随机丢弃部分邻居状态比如丢包率10%那么一致性项只使用收到的邻居信息。你会发现编队依然能维持但收敛速度和队形精度会下降。如果丢包率太高一致性项会退化编队可能解耦。我目前的做法是加一个“零阶保持”丢包时沿用上一帧收到的邻居状态这样能维持基本的一致性。更进阶的方案是加一个简单预测器用邻居上一帧的速度估计它的当前位置但这就涉及卡尔曼滤波的范畴了。先不要贪多能模拟丢包并观察系统鲁棒性已经是很好的扩展方向。最后再说一点个人体会。这套仿真做完我最深的感触是Voronoi在Matlab里实现起来其实不难真正的功夫都在那些看不见的细节上无穷顶点怎么剔除、质心要不要滤波、斥力作用半径怎么跟速度匹配、先关掉哪一层再调哪一层。如果你也准备做类似的多智能体编队仿真建议不要急着追求复杂算法先把一个6机避障场景在Matlab里从画图到闭环全部跑通再逐步加规模、加通信延迟、加模型误差。那些高级问题到了这一步自然会有新的解决思路。