1. 从一群无人机各飞各的到像雁群一样协同这个项目到底在解决什么如果你玩过多旋翼大概率经历过这样的场景三架无人机同时起飞你手里只有一个遥控器想让它们保持一个三角形编队往前飞结果要么是间距忽大忽小要么是其中一架撞上了突然出现的电线杆。单机控制靠PID调参还能凑合一旦数量上去问题就从怎么飞稳变成了怎么让它们互相知道对方在哪、该往哪飞、遇到障碍怎么让。这个项目要干的事情就是给一个无人机群体装上集体大脑和局部反射神经。具体来说它包含两个核心能力第一是障碍避让让每架无人机在飞行过程中能感知并绕开障碍物而不是傻乎乎地按预设轨迹撞上去第二是基于Voronoi的编队控制用Voronoi图把空间划分成若干责任区域每架无人机负责自己那块区域通过区域几何关系来维持整体队形。整个实现跑在Matlab里意味着你不需要真机就能先把算法逻辑验证清楚这对做研究和前期方案论证的人来说非常友好。适合谁来参考这篇内容如果你正在做多智能体协同控制相关的课题、需要快速搭一个仿真验证环境、或者单纯对群体智能怎么落地到无人机这件事好奇那接下来的内容会对你有直接帮助。我会把Voronoi编队的几何直觉、避障的势场设计、Matlab里的实现骨架以及我自己踩过的几个坑全部摊开讲清楚。需要先说明一点这个项目本质是一个仿真层面的算法验证框架不是直接能烧进飞控的工程代码。它的价值在于帮你把控制律和协同逻辑跑通、看明白至于后面怎么移植到真实平台那是另一个话题我会在最后一节聊几句思路。2. Voronoi编队控制的几何直觉为什么用责任田来管一群无人机2.1 从每架飞机追一个目标点说起最朴素的编队控制思路是给每架无人机分配一个期望位置让它飞过去。比如三架飞机要组成等边三角形那就设定三个目标点各自用位置控制器飞过去。这个方法在无障碍、无干扰的环境下能用但有两个致命问题。第一个问题是耦合性差。如果其中一架因为避障偏离了位置其他两架并不知道编队就散了。第二个问题是扩展性差。十架飞机你要手动分配十个目标点而且目标点之间的相对关系是写死的一旦整体需要旋转或缩放你得重新算一遍。Voronoi的思路完全不同。它不直接告诉每架飞机你该在哪而是告诉它你负责哪块区域。区域的边界由它和邻居之间的垂直平分线决定。当所有无人机都朝着自己区域的某个特征点通常是质心移动时整个群体就会自然形成一种稳定的空间分布。这就像给每个人分了一块责任田你管好自己那块整体秩序就出来了。2.2 Voronoi图是怎么长出来的给定平面上若干点这里就是无人机的位置Voronoi图把平面划分成若干个凸多边形区域每个区域内的任意一点到该区域对应无人机的距离都小于到其他任何无人机的距离。用大白话说离谁近就归谁管。这个性质带来一个非常好的结果——区域边界就是无人机之间的势力分界线。如果两架无人机靠得太近它们的区域会被压缩得很小质心位置会发生变化控制律就会产生一个让它们分开的力。反过来如果某架无人机离群体太远它的区域会变得很大质心会把它往回拉。这种自组织的间距维持机制是Voronoi编队最迷人的地方。在Matlab里计算Voronoi图可以直接用voronoi函数画图但要做控制你得拿到每个区域的顶点坐标。这时候更实用的是voronoin函数它返回的是顶点矩阵和区域索引你可以据此算出每个多边形的质心。这里有个细节voronoin返回的区域索引里第一个顶点是无穷远点用1表示处理边界区域时必须把它剔除否则质心计算会出错。我第一次跑的时候就因为这个原因边缘那架无人机的目标点直接飞到了天边。2.3 质心Voronoi剖分与编队收敛标准的Voronoi编队控制目标不是让无人机停在Voronoi区域的几何质心而是让它们收敛到质心Voronoi剖分Centroidal Voronoi Tessellation, CVT的状态。CVT的定义是每个区域的质心恰好与该区域对应的生成点重合。换句话说无人机停的位置就是它自己那块区域的重心。为什么CVT是个好目标因为在这个状态下整个群体对空间的覆盖是最均匀的。如果你给每架无人机一个密度函数比如某些区域需要更密集的覆盖CVT还能实现非均匀分布。对于编队控制来说你只需要调整密度函数的形状就能让群体自然形成你想要的队形轮廓而不需要显式地指定每个目标点。控制律的形式其实很简洁每架无人机的速度指令等于它当前位置到其Voronoi区域质心的向量乘以一个增益。用公式表示就是u_i k * (C_i - p_i)其中C_i是第i个Voronoi区域的质心p_i是无人机当前位置。这个控制律的稳定性可以用Lyapunov方法证明但作为实操者你只需要知道它会让整个系统朝着CVT状态收敛而且收敛过程是平滑的。2.4 编队形状怎么调出来有人会问我想让它们排成一条直线或者一个圆环Voronoi能办到吗答案是能但需要一点技巧。纯Voronoi编队在没有密度函数调制的情况下趋向于均匀分布形状取决于初始位置和边界条件。要得到特定形状通常有两种做法。一种是在密度函数上做文章。比如你想让无人机排成环形可以构造一个在环上密度高、其他地方密度低的函数CVT会自然把生成点推向高密度区域。另一种更直接的做法是混合控制在Voronoi控制的基础上叠加一个朝向期望编队形状的参考力。这样既保留了Voronoi的避碰和自组织特性又能精确控制队形。我在实际项目里更倾向第二种因为调参直观形状误差也容易量化。3. 障碍避让的势场设计让无人机怕障碍但不怕队友3.1 人工势场的基本框架障碍避让最经典的方案是人工势场法。核心思想很简单把目标点设成引力源把障碍物设成斥力源无人机在合力作用下运动。引力保证它往目标飞斥力保证它不撞障碍。这个方法的优点是计算量小、响应快、容易和其他控制律叠加缺点是容易陷入局部极小值也就是引力和斥力刚好抵消无人机卡在原地不动。在多无人机场景下势场还要多一层机间斥力。每架无人机不仅要躲障碍还要躲其他无人机。但这里有个微妙的平衡——机间斥力太强编队会被撑散太弱又会撞机。我的经验是机间斥力的作用范围应该略大于期望编队间距强度要低于障碍斥力这样在正常编队飞行时它几乎不生效只有在间距异常缩小时才介入。3.2 斥力函数的选型与参数计算斥力函数常见的有两种形式一种是基于距离倒数的一种是基于指数衰减的。倒数形式的斥力在距离趋近于零时会趋于无穷大理论上能保证绝对不碰撞但数值上容易爆炸仿真步长稍大就会发散。指数形式更平滑但需要仔细调参才能保证在安全距离外及时产生足够的斥力。我一般用改进的倒数形式加一个饱和上限function F_rep repulsive_force(p, p_obs, rho0, eta) d norm(p - p_obs); if d rho0 F_rep [0; 0]; else direction (p - p_obs) / d; magnitude eta * (1/d - 1/rho0) / d^2; magnitude min(magnitude, 50); % 饱和上限 F_rep magnitude * direction; end end这里的rho0是斥力作用半径eta是斥力增益。rho0怎么定它应该大于无人机的最小安全间距加上一个反应余量。假设你的无人机半径0.3米安全间距要求1米飞行速度2米/秒控制周期0.05秒那么一个周期内飞行0.1米。rho0至少要是1.3米留出反应时间的话建议设到2米左右。eta的取值要让最大斥力在合理范围内通常通过试凑从1开始往上加直到避障响应足够快但不震荡。3.3 局部极小值的破解思路局部极小值是势场法的老毛病。无人机、障碍、目标三点共线时引力和斥力方向相反合力为零无人机就卡住了。破解方法有几种我在项目里用的是虚拟目标点法当检测到合力接近零且未到达目标时在垂直于当前运动方向的一侧生成一个临时目标点打破对称性。这个临时目标点持续一小段时间后撤销让无人机重新评估。另一种更优雅的方法是旋转势场在斥力上叠加一个切向分量让无人机自然地绕过障碍而不是正面硬顶。切向分量的方向选择要一致比如始终顺时针否则会在障碍两侧来回震荡。这个方法在障碍物比较规则圆形、矩形时效果很好但遇到凹形障碍可能会把无人机引进死胡同。3.4 避障与编队的优先级仲裁避障和编队这两个目标有时候会冲突。比如编队要求你往左飞但左边有障碍你该怎么办我的处理原则是安全优先编队次之但编队不能完全放弃。具体实现上我用一个优先级权重避障力乘以一个较大的权重编队力乘以一个较小的权重两者叠加。当避障力很大时编队力被淹没无人机优先避障当避障力消失后编队力重新主导无人机回归队形。这里有个坑如果避障结束后无人机直接猛拉回编队位置可能会引起队形震荡。我的做法是给编队力加一个恢复速率限制让回归过程平滑一些。另外避障期间编队误差会累积如果障碍密集可能需要临时放宽编队误差的容忍度否则系统会一直处于高增益修正状态反而容易失稳。4. Matlab实现骨架从初始化到仿真循环的完整链路4.1 仿真环境与参数初始化Matlab做这类仿真的优势在于矩阵运算和可视化都很顺手。我通常把整个仿真写成一个主脚本加几个函数文件的结构。主脚本负责参数设置、循环推进和绘图函数文件负责Voronoi计算、势场计算、动力学更新这些模块化的工作。参数初始化部分我习惯用一个结构体把所有参数装起来这样调参的时候不用满脚本找变量params.N 5; % 无人机数量 params.dt 0.05; % 仿真步长 params.T 60; % 仿真总时长 params.area [0 20; 0 20]; % 仿真区域 params.v_max 3; % 最大速度 params.k_formation 1.2; % 编队增益 params.k_avoid 2.5; % 避障增益 params.rho0 2.0; % 斥力作用半径 params.eta 3.0; % 斥力增益 params.safe_dist 1.0; % 机间安全距离初始位置我一般随机撒点但会加一个最小间距约束避免一开始就有两架重叠。障碍物用结构体数组表示每个障碍有位置和半径两个字段。如果是多边形障碍可以拆成多个圆形近似或者用距离函数直接算点到多边形的距离。4.2 Voronoi区域计算与质心提取这是整个项目里最容易出bug的地方。Matlab的voronoin返回的索引格式需要仔细处理function [centroids, regions] compute_voronoi_centroids(positions, area) [v, r] voronoin(positions); centroids zeros(size(positions)); for i 1:size(positions, 1) region_idx r{i}; if isempty(region_idx) || any(region_idx 1) % 无穷远区域用边界裁剪 region_idx region_idx(region_idx ~ 1); if isempty(region_idx) centroids(i, :) positions(i, :); continue; end end poly_x v(region_idx, 1); poly_y v(region_idx, 2); % 用polyshape计算质心 ps polyshape(poly_x, poly_y); [cx, cy] centroid(ps); centroids(i, :) [cx, cy]; end end对于无穷远区域严格来说需要用仿真区域的边界去裁剪但我在实际项目里发现如果无人机初始位置都在区域内且分布合理无穷远区域很少出现。真出现了直接用当前位置作为质心也能凑合不会导致发散。当然如果你追求严谨可以用polybool或者自己写一个Sutherland-Hodgman裁剪算法把无穷远区域截断到仿真边界内。4.3 控制律合成与动力学更新控制律的合成就是把编队力和避障力加起来然后做限幅function u compute_control(i, positions, centroids, obstacles, params) % 编队力 F_form params.k_formation * (centroids(i, :) - positions(i, :)); % 障碍斥力 F_obs [0, 0]; for j 1:length(obstacles) F_obs F_obs repulsive_force(positions(i, :), ... obstacles(j).pos, obstacles(j).radius params.rho0, params.eta); end % 机间斥力 F_int [0, 0]; for j 1:size(positions, 1) if j i, continue; end d norm(positions(i, :) - positions(j, :)); if d params.safe_dist * 1.5 F_int F_int repulsive_force(positions(i, :), ... positions(j, :), params.safe_dist * 1.5, params.eta * 0.5); end end u F_form F_obs F_int; % 速度限幅 if norm(u) params.v_max u u / norm(u) * params.v_max; end end动力学更新我用的是最简单的单积分模型p p u * dt。这个模型假设速度可以瞬时改变对于算法验证足够了。如果你想更接近真实无人机可以换成双积分模型把控制量当作加速度再加一个速度阻尼项。双积分模型下编队增益需要重新调通常要小一些否则会超调震荡。4.4 可视化与调试技巧Matlab的voronoi函数可以直接画图但在仿真循环里每帧重画整个图会很慢。我的做法是每帧只更新无人机位置和Voronoi边界的句柄用set函数修改数据而不是重新创建对象。障碍物用viscircles画编队误差用实时曲线显示。调试的时候我强烈建议把每架无人机的控制力分量分开画出来。编队力、障碍斥力、机间斥力各用不同颜色这样一眼就能看出是哪个力在主导。我遇到过编队一直震荡的问题画出来才发现是机间斥力在正常间距下就一直在作用把编队增益抵消了一部分。把机间斥力的作用范围调小之后震荡立刻消失。5. 实测中暴露的四个典型问题与排查过程5.1 边缘无人机飞出去无穷远区域处理不当第一次跑通仿真的时候我发现最外面那架无人机总是往仿真区域外面飞最后停在角落里不动了。排查过程是这样的先看位置曲线发现它的目标点Voronoi质心一直在往边界外移动再看Voronoi区域发现它的区域是一个无穷远区域voronoin返回的索引里包含1而我的代码没有正确处理导致质心计算用了一个包含无穷远点的多边形算出来的质心自然在区域外。修复方法就是前面代码里写的把索引1剔除如果剔除后区域为空就用当前位置作为质心。更稳妥的做法是用仿真边界裁剪无穷远区域但需要额外实现裁剪算法。对于大多数编队场景简单剔除就够了因为只要无人机不飞出区域无穷远区域很少出现。5.2 编队收敛但间距不均匀密度函数缺失有一次我设了五架无人机期望它们均匀分布在一个圆形区域内。结果收敛后中间那架周围挤了三架边上两架离得很远。原因是我用的是标准Voronoi没有加密度函数系统收敛到的是均匀覆盖的CVT但均匀覆盖不等于均匀间距——在凸区域边界附近Voronoi区域会被边界截断面积偏小质心偏向内部导致边界附近的无人机被往里拉。解决办法是引入一个密度函数在边界附近降低密度值让CVT的生成点往外推。密度函数可以简单设为到区域中心的距离的函数也可以根据期望的编队形状来构造。我试过用高斯混合模型来定义密度效果不错但调参比较麻烦。如果只是想要均匀间距一个更直接的方法是在控制律里加一个边界斥力把无人机往区域内部推。5.3 避障时编队短暂散开但恢复缓慢避障过程中编队散开是正常的但我的问题是避障结束后恢复太慢有时候要十几秒才能回到期望队形。分析下来有两个原因一是编队增益设得偏小二是避障结束后无人机的速度方向还残留着避障时的横向分量需要时间衰减。改进措施有三个第一把编队增益从0.8提到1.2恢复速度明显加快第二在避障力消失后加一个速度阻尼项加速横向速度衰减第三给编队力加一个前馈项根据编队误差的变化率提前修正。第三个措施效果最好但需要小心处理前馈增益太大会引起振荡。5.4 仿真步长与控制周期的匹配问题我一开始用dt 0.1跑仿真结果避障时无人机经常穿过障碍物。原因是步长太大一个步长内无人机移动了0.3米而障碍物半径只有0.5米斥力还没来得及把它推开它已经到障碍物内部了。把步长降到0.02之后问题消失但仿真时间变长。这里的原则是步长乘以最大速度必须远小于障碍物的特征尺寸。经验上单步位移不超过障碍物半径的十分之一比较安全。如果计算资源有限可以用变步长远离障碍时用大步长接近障碍时自动减小步长。Matlab的ode45支持事件检测可以在接近障碍时触发步长调整但实现起来比固定步长复杂不少。6. 从仿真到落地的几个现实考量仿真跑得再漂亮真机上又是另一回事。我在这个项目里总结了几条经验给打算往真机移植的人参考。第一通信延迟是最大的敌人。仿真里所有无人机的位置信息是瞬时共享的真机上每架飞机的位置要经过估计、打包、传输、解包才能被邻居使用延迟几十毫秒很正常。Voronoi计算对位置精度很敏感延迟会导致区域划分抖动进而引起控制量抖动。缓解办法是在控制律里加低通滤波或者用预测算法补偿延迟。第二定位精度决定编队精度。仿真里位置是精确值真机上GPS误差可能到米级室内视觉定位好一些但也有厘米级误差。如果编队间距要求是两米定位误差半米那编队效果会大打折扣。实际项目里编队间距通常要留出定位误差的余量或者用相对定位比如机间测距来辅助。第三计算资源要留余量。Voronoi计算在Matlab里跑五架飞机很轻松但真机上的飞控计算机性能有限而且还要跑姿态控制、状态估计等任务。如果无人机数量增加到十几架Voronoi的计算量会上升需要考虑分布式计算或者简化算法。一个实用的简化是只考虑邻居的Voronoi区域而不是全局计算这样计算量随邻居数量而不是总数量增长。第四安全机制必须独立于编队逻辑。编队算法再可靠也不能保证不出问题。真机上应该有独立的安全监控模块检测到间距过近或者飞出安全区域时直接接管控制执行悬停或返航。这个模块不能依赖编队算法的输出必须是独立的传感器和控制通道。7. 写在最后几个让我少走弯路的实操习惯做这类多智能体仿真我最大的体会是先把单机跑通再上群体。很多人一上来就写五架飞机的协同结果出了问题根本不知道是哪架的逻辑错了。我的习惯是先写一架飞机的避障确认它能稳定绕开障碍再加第二架确认机间斥力生效然后才逐步增加数量。每一步都可视化每一步都记录关键变量的曲线。另一个习惯是参数分组调试。编队增益、避障增益、斥力半径这些参数不要一起调一次只动一个观察它对系统行为的影响。我一般先调编队增益让编队能收敛再调避障增益让避障响应够快最后调斥力半径微调安全距离。调参过程中把每次的参数和对应的行为记下来形成自己的参数直觉下次遇到类似场景就不用从头试了。最后一个习惯是保留中间版本的代码。Voronoi编队的实现有好几种变体势场函数也有不同形式我每次尝试新方案都会另存一个版本而不是在原文件上改。这样当新方案效果不好时可以快速回退到之前能跑的版本而不是从头重写。这个习惯在项目后期帮我省了很多时间。