上个月课题组仿真环境里出现了一个经典画面8架无人机在空旷场地上排成整齐队形可一旦场地中央放上一根圆柱障碍物编队在绕行后迟迟恢复不了还有几架在障碍物背后反复绕圈。后来我把整个控制框架换成了多智能体分布式方案——用 Voronoi 图做空间划分让每架无人机自主决定目标点用人工势场做障碍避让。编队不再是我预先拼出来的固定形状而是随着障碍和任务区域自己长出来的分布。这篇文章把原理、Matlab 实现和调试过程完整复盘一遍适合正在搞无人机编队、多智能体覆盖控制或者相关课程设计的朋友参考。1. 为什么是 Voronoi把编队问题变成空间分配问题1.1 预设队形方案的三个硬伤先说为什么大多数课程设计和初版方案都用“预设队形 跟随”的套路。这类方案实现简单给每架无人机分配一个相对领航者的偏移量控制律收敛之后自然形成三角形、楔形或者横排队形用户手册里都能找到现成案例。但在有障碍的真实任务场景里这套思路有三个绕不开的硬伤。第一队形恢复慢。遇到障碍物时如果选择“整体绕行”相当于把整个刚性结构平移绕过障碍路径规划计算量会随队形规模非线性增长如果选择“拆开绕行”无人机各自从障碍两侧穿过队形一旦被分割重聚的判断逻辑相当复杂。我见过最常见的现象就是两架无人机在障碍背后互相等待谁都不想先偏离预期位置直到超时触发保护逻辑。第二队形形态与任务空间无关。预设队形只在空旷且密度均匀的场景里合理。实际任务里目标区域可能有热点区域需要多部署无人机、可能有禁飞区不允许进入、可能有通信盲区不能占用。固定队形不会随空间密度改变空间利用率自然很差。第三成员失效后的重构成本高。如果编队里有一架无人机断电退出靠预设偏移的方案需要重新为每个位置指定目标系统没有自我修复能力。1.2 Voronoi 划分让队形获得自适应性Voronoi 图做的事情非常简单给定平面上一组点每架无人机的位置把平面划分成与这些点一一对应的凸多边形区域每个区域内的任意位置到“自己”这点的距离都小于到其它任何一点的距离。数学定义一句话Vi {q ∈ Q : ‖q − p_i‖ ≤ ‖q − p_j‖, ∀j ≠ i}。这个划分天然适合多智能体群体控制场景每架无人机独立负责自己的 Voronoi 单元不需要中心节点统一指挥分布式特征明显。无人机位置一变Voronoi 划分立刻随动也就是说“队形”是位置信息的函数天然自适应。无人机之间的间距由划分边界决定两个相邻单元不会无限重叠自带了一部分避碰属性。那怎么把 Voronoi 和编队控制挂钩答案在单元质心。如果每架无人机都控制自己移动到当前 Voronoi 单元的质心整个群体的空间分布就会不断优化某个区域无人机过于集中这个区域的单元面积就会缩小质心向其它方向偏移无人机分布过散单元面积增大质心被拉向空白区域中心。整个系统最终收敛到一种稳定配置数学上叫质心 Voronoi 配置Centroidal Voronoi ConfigurationCVC。1.3 从覆盖控制到编队控制的衔接为什么“向质心移动”就会收敛这里有一个非常简洁的理论支撑。定义覆盖代价函数H Σ ∫_{Vi} ‖q − p_i‖² φ(q) dq其中 φ(q) 是任务空间密度函数重点区域取值大空白区域取值小。这个积分度量的是“任务空间中每个点到最近无人机的二次距离”直观说就是“需求点到最近服务点的平均距离”。对这个函数求梯度会得到∂H/∂p_i −2 M_{Vi} (C_{Vi} − p_i)其中 M_{Vi} 是单元的加权质量C_{Vi} 是加权质心。这说明只要令控制输入正比于 C_{Vi} − p_i覆盖代价就会单调递减最后收敛到 CVC。这段话有点数学密度但值得读两遍它证明了编队不一定要“预设”只要让每个无人机持续往自己 Voronoi 单元的质心移动队伍的分布在数学上就是最优的。当然要坦率说明一点这套方案天然生成的是覆盖式编队空间自适应分布不是那种要求角点坐标精确到厘米的刚性几何编队。如果你需要的是类似“人字形”的刚性编队可以把密度函数 φ(q) 设计成在期望位置附近有尖峰Voronoi 单元质心就会被“吸”到那些尖峰上从而实现柔性几何编队。本文项目采用的是均匀密度加障碍避让的覆盖式编队仿真任务是矩形区域内的多机巡逻覆盖。2. 系统模型与整体架构控制链路怎么搭2.1 无人机运动学模型的选择编队控制仿真里最常见的两种模型。一阶积分器 ẋ u控制输入是速度建模最简单适合协议层面的验证二阶积分器双积分器ẍ u控制输入是加速度更接近真实无人机能体现惯性和速度阻尼。我在这个项目里选双积分器。原因很直接后续要迁移到硬件在环仿真或者实飞纯一阶模型控制量直接给速度和飞控输出加速度的方式不一致会多一层转换双积分器模型只需要控制律里补偿速度阻尼项迁移时改动很小。状态定义p_i ∈ R² 是位置v_i ∈ R² 是速度控制输入为加速度 u_i ∈ R²。这里先做二维平面验证后面第 5 节再讲三维扩展。2.2 通信拓扑与一致性协议多智能体系统强调局部信息交互。通信拓扑用邻接矩阵 A 描述a_ij 1 表示无人机 i 能收到无人机 j 的位置/速度信息否则为 0。自环 a_ii 0。编队控制至少保证通信图连通也就是从任意一架无人机出发经过若干跳可以到达任意其它无人机。连通性判定用 Laplacian 矩阵 L D − A 的特征值第二小特征值 λ₂ 0 说明连通。λ₂ 越大网络收敛越快。一致性协议的基础形式ẋ_i −Σ_{j∈N_i} (x_i − x_j)这个协议可以实现所有无人机状态一致收敛。编队控制就是在一致性的基础上引入期望偏移u_i −Σ_{j∈N_i} [p_i − c_i−p_j − c_j] − K_v v_i本项目里更直接一点用 Voronoi 质心作为期望位置那么“编队项”就变成一个分布式跟踪控制律u_i K_p (C_i − p_i) − K_d v_i其中 C_i 是当前 Voronoi 单元质心K_p、K_d 是 PD 增益。K_d v_i 项抑制速度振荡保证二阶系统稳定。2.3 模块划分与仿真主循环整个 Matlab 工程按功能拆成几个独立模块setup.m参数定义包括无人机数量、初始位置、障碍物、PD 增益、通信拓扑。compute_voronoi_centroids.m输入所有无人机位置输出每个单元的顶点和质心。control_law.m输入某架无人机的位置、速度、质心、障碍物、邻居信息输出加速度指令。dynamics_update.m双积分器数值积分。plot_frame.m动画绘制。main_run.m主循环。主循环结构非常直白for t 0:dt:T % 1. 更新 Voronoi 划分计算每架无人机的单元质心 centroids compute_voronoi_centroids(pos, region); % 2. 对每架无人机计算控制输入 for i 1:N neigh pos([1:i-1 i1:end], :); u(i, :) control_law(pos(i,:), vel(i,:), centroids(i,:), obstacles, neigh, opts); end % 3. 半隐式积分更新速度、位置 vel vel u * dt; pos pos vel * dt; % 4. 每若干帧绘制一次 if mod(round(t/dt), 10) 0 plot_frame(pos, vel, obstacles); drawnow; end end模块拆开的好处改避障策略、换密度函数、换通信拓扑都只需要动对应文件不用重写整个工程。这是我做了三轮编队仿真后养成的习惯——算法研究阶段模块边界就是调试边界。3. 避障控制人工势场如何与编队力共存3.1 障碍建模与斥力场设计避障方法选择上我考虑过速度障碍法VO/ORCA和模型预测控制MPC。MPC 效果好但计算量大多智能体实时仿真明显变慢VO 适合动态障碍物但和 Voronoi 编队项融合时参数太多。最后选人工势场两条理由计算开销极小实现直观和 Voronoi 控制律天然同构——两边都是“力和梯度”的语言。障碍物建模为圆形用 (obs_x, obs_y, obs_r) 表示圆心和半径。无人机到障碍表面的距离ρ ‖p − p_obs‖ − r_obs设计斥力势场U_rep 0.5·η·(1/ρ − 1/ρ₀)²当 ρ ≤ ρ₀否则为 0对应的斥力F_rep η·(1/ρ − 1/ρ₀)·(1/ρ²)·(p − p_obs)/ρ公式里 (p − p_obs)/ρ 是单位方向向量1/ρ² 来自势场梯度它让无人机离障碍越近斥力增长越快避免直接撞上。ρ₀ 是斥力作用范围我只让它在局部范围生效防止远距离障碍物干扰编队收敛。3.2 编队项与避障项的权重协调这里有个很关键的设计问题编队项和避障项同时作用权重怎么给最简单的是固定权重u u_ff u_obs。这种做法在稀疏障碍场景下能跑通但一旦无人机逼近障碍避障力可能被较大的编队力淹没出现“怼墙”反过来避障权重如果设得很大无人机在空旷区域也会被远处障碍物吸引偏离队形甚至整个队伍停在半路不前进。我的做法是让权重随“到最近障碍物距离”连续变化d_min min(所有障碍表面距离); alpha 1 / (1 exp(5 * (d_min - opts.r_safe))); u (1 - alpha) * u_ff alpha * u_obs;当 d_min 远大于 r_safe 时 alpha 约等于 0编队项完全生效进入危险区域时 alpha 快速逼近 1避障接管中间区域平滑过渡不会出现突变导致的抖动。实际调参时指数里的系数 5 控制切换带宽度我调到 5 时过渡区间大约 0.6 米左右比较合适。另外无人机之间也是移动障碍物。我在 u_obs 里额外加了一项“邻居软排斥”当两机距离小于 2 倍安全半径时施加与距离成反比的排斥力。这一项写法和障碍物斥力一致只是把障碍半径替换成无人机安全半径。加上之后编队在避障绕行过程中不会互相挤压。3.3 预防局部极小与抖动人工势场法最大的问题就是局部极小点当引力与斥力大小相等方向相反时无人机受力为零会停在某个非目标点。经典画面就是无人机在障碍物“背后”悬停既不撞也不走。我的处理方式比较务实三招并用记忆扰动记录无人机过去 N 步的位置如果连续 N 步位置变化小于阈值而速度又不为零说明进入局部极小区这时往当前受力方向的垂直方向叠加一个持续 2 秒的小扰动。实测下来8 架无人机、3 个障碍物的场景里加入扰动后没有一次被困住。斥力方向的旋转修正当无人机运动方向与障碍物连线夹角过小时几乎正对障碍把斥力方向顺时针旋转一个角度让无人机从侧面绕行。这个技巧在很多论文里叫“势场切线分量”实现起来就是在斥力方向上加一个切向单位向量分量。代码实现只有几行但对绕行效果的提升非常明显。绕行完成后自然回编队alpha 的平滑过渡本身解决了一部分“绕完再回”的问题。我观察到只要斥力作用范围 ρ₀ 设在 2.0 左右无人机绕到障碍侧面后斥力自然消失alpha 自动回到约 0编队项接管恢复路径很平滑。4. Matlab 实现拆解从 voronoin 到动画输出4.1 Voronoi 计算与无限顶点裁剪Matlab 自带的 voronoin 是最直接的起点[V, C] voronoin(pos);pos 是 N×2 的位置矩阵V 是所有顶点的坐标C 是每个单元对应的顶点索引。但这里有一个经典坑位于凸包外侧的单元顶点包含无穷远点V 中坐标会出现 Inf。如果直接把 V(C{i}, :) 传给 polyshape 算质心polyshape 立刻报错。实际项目里我用的技巧是虚拟点裁剪法在真实无人机位置之外把任务区域四角向外扩展一段距离加入 4 个虚拟位置点再调用 voronoin。这样所有真实无人机的单元都会被虚拟点的单元“挤”住不再有无限顶点。后续只需要从 C 中取前 N 个单元顶点即可。function centroids compute_voronoi_centroids(pos, region) % region [xmin, xmax, ymin, ymax] N size(pos, 1); margin 10; corners [region(1)-margin, region(3)-margin; region(2)margin, region(3)-margin; region(1)-margin, region(4)margin; region(2)margin, region(4)margin]; ext [pos; corners]; [V, C] voronoin(ext); centroids zeros(N, 2); for i 1:N verts V(C{i}, :); poly polyshape(verts(:, 1), verts(:, 2)); [cx, cy] centroid(poly); centroids(i, :) [cx, cy]; end end这个技巧省去了手写多边形裁剪算法的功夫强烈建议采用。注意虚拟点不要放在离区域太近的位置一般外扩距离设为区域宽度的一半以上才保险否则真实无人机靠近边界时单元仍然会被压得过扁。4.2 单元质心与 Lloyd 迭代计算质心用 polyshape 的 centroid 方法非常直接。但有两点提醒。polyshape 要求顶点按顺时针或逆时针顺序排列。voronoin 输出的顶点本身是环绕顺序一般可以直接用如果遇到某些退化单元报错就先转成逆时针再构造。另外质心不是几何中心而是面积一阶矩的结果。单元形状越不规则质心和顶点平均值的差异越明显——这直接影响能否收敛到 CVC所以不要图省事直接平均顶点坐标。整个 Lloyd 迭代过程给定无人机位置 p计算 Voronoi 划分计算每个单元质心 C_V控制无人机向质心移动回到第 1 步。这个迭代在时间离散化之后就是主循环。密度函数 φ(q) 如果取常数质心就是普通多边形质心如果任务区有重点区域φ(q) 取非均匀分布比如高斯峰质心会偏向重点区域编队分布也随之变密。这步零成本就能让编队形态随任务需求变化非常推荐试一试。4.3 核心控制律代码与主循环避障和编队融合后的控制律代码层面长这样function u control_law(p, v, c_des, obstacles, neigh, opts) % 编队项PD 跟踪 Voronoi 质心 u_ff opts.kp * (c_des - p) - opts.kd * v; % 避障项人工势场斥力 u_obs zeros(1, 2); d_min inf; for k 1:size(obstacles, 1) op obstacles(k, 1:2); or_ obstacles(k, 3); d norm(p - op); rho d - or_; d_min min(d_min, rho); if rho opts.rho0 rho 1e-4 dir (p - op) / d; u_obs u_obs opts.eta * (1/rho - 1/opts.rho0) * (1/rho^2) * dir; end end % 邻居软排斥无人机之间防碰撞 for j 1:size(neigh, 1) d norm(p - neigh(j, :)); if d 2 * opts.r_safe d 1e-4 u_obs u_obs opts.rep_n * (1/d - 1/(2*opts.r_safe)) * (p - neigh(j, :)) / d; end end % 编队与避障的权重协调 alpha 1 / (1 exp(5 * (d_min - opts.r_safe))); u (1 - alpha) * u_ff alpha * u_obs; end主循环里每步先重新计算 Voronoi 质心再对每个个体调用这个函数最后更新状态。数值积分我用的是半隐式——先更新速度再更新位置比显式欧拉稳定很多。dt 一般取 0.02 到 0.05 秒太大了无人机容易“穿透”斥力场。4.4 可视化与结果判读可视化这一块用到的都是 Matlab 基础绘图函数voronoi(pos(:,1), pos(:,2)) 绘制 Voronoi 边界或者用 patch 自定义绘制单元边线scatter 画无人机当前位置quiver 画速度向量障碍物用 rectangle 结合 Curvature 属性画圆每帧 drawnow最后用 VideoWriter 存视频。判断仿真是否成功的量化指标我建议看四个覆盖代价 H 是否持续下降并趋于稳定无人机最小间距是否始终大于安全半径最终无人机坐标与对应质心的误差是否收敛到阈值内alpha 均值在到达稳态后是否接近 0——说明没有持续避障占用控制资源。5. 参数调试与三处踩坑记录5.1 仿真参数速查表把我最终调出来的一组有效参数列出来方便读者作为起点参数数值说明N8无人机数量任务区域20m × 15m矩形巡逻区域dt0.05s仿真步长T40s仿真时长K_p1.8位置增益K_d2.2速度阻尼r_safe1.0m安全半径ρ₀2.5m斥力作用范围η8.0斥力增益rep_n3.0邻居软排斥增益通信拓扑k-近邻图k3保证连通性这组参数下8 架无人机从随机位置出发约 15 秒内覆盖代价 H 下降到稳定值的 90% 左右绕行 3 个障碍物过程中没有发生碰撞也没有陷入局部极小。5.2 坑一voronoin 返回无限坐标导致 polyshape 崩溃现象运行到第二帧polyshape 报错错误信息类似“Polyshape 必须包含至少 3 个顶点”。原因单元包含 Inf 顶点。排查思路打印 V(C{i}, :) 看是否存在 Inf。一旦看到 Inf其实就是凸包外侧单元的问题。解决上文的虚拟点裁剪法。这是最省事也最稳的方案不要去自己写凸包裁剪。我最早自己写过一版点集外边界裁剪代码处理边界相交、退化单元各种情况花了整整一个晚上最后不如四个虚拟点好用。5.3 坑二斥力增益调大后无人机在障碍物附近疯狂抖动现象η 从 5 调到 10 后无人机一进入障碍物斥力范围就开始来回震荡轨迹呈锯齿状。原因斥力增益太大加上离散时间步长不够小导致下一时刻的指令方向反复倒转。解决分三步处理。第一步增大 K_d让速度阻尼吃掉多余动能第二步对 u_obs 做幅值限制我这边限到 3.0 m/s²第三步时间步长从 0.05 降到 0.02。三步组合后抖动问题消失。实际上大部分“调参数调不出来”的振荡都是时间步长太粗和阻尼不足同时造成的优先检查这两项比盲目调避障权重有效得多。5.4 坑三通信拓扑不连通队伍散成两群现象8 架无人机初始位置分两堆仿真几秒后两队各自向各自质心收敛但两堆之间始终有一块真空区整个队伍没有形成统一编队。原因环形拓扑按编号连接编号前 3 架连在一起后 5 架连在一起两堆之间没有通信链路。虽然 Voronoi 质心计算用的全局信息但控制协议实际依赖邻域信息缺了链路就是两个独立子系统。解决先计算 Laplacian 矩阵的第二小特征值 λ₂。λ₂ 小于 1e-6 就是不连通。把通信拓扑改成 k-近邻图k3后队伍立刻统一。这里有个更隐蔽的细节即使通信拓扑保证连通初始位置分太散时外侧区域的 Voronoi 单元可能长时间不更新无人机和质心之间隔着很长距离收敛时间被拉长。解决方法是初始位置用随机均匀采样不要让两堆间距超过区域短边的 0.6 倍。5.5 从 2D 到 3D 的扩展路径最后聊聊迁移到三维的问题。3D 场景下 Voronoi 变成三维凸多面体划分Matlab 里有两条路用 mpt3 工具箱Multi-Parametric Toolbox 3直接生成有界 Voronoi 单元或者用 convhulln 计算每个单元的凸包顶点再用 volume 和 centroid 计算质心。控制律本身不需要改位置和速度各加一个 z 轴分量即可。障碍物从圆盘变成球体斥力公式里的距离改成三维范数ρ₀ 保持不变。飞行高度如果固定机间距离按 xy 平面投影算如果高度可变上下两架无人机的避碰逻辑要同时考虑 z 轴。我实测下来3D 场景主要多花时间在可视化环节——patch 画三维多面体、调相机视角控制层的改动基本为零。这也是为什么我建议先在 2D 里把所有控制逻辑跑通再往上加维度而不是一步到位直接写三维。 
长河君 · 2026/10/25 11:14:04 · 阅读 1.2k 多无人机编队控制：Voronoi图与人工势场Matlab仿真实现
上个月课题组仿真环境里出现了一个经典画面8架无人机在空旷场地上排成整齐队形可一旦场地中央放上一根圆柱障碍物编队在绕行后迟迟恢复不了还有几架在障碍物背后反复绕圈。后来我把整个控制框架换成了多智能体分布式方案——用 Voronoi 图做空间划分让每架无人机自主决定目标点用人工势场做障碍避让。编队不再是我预先拼出来的固定形状而是随着障碍和任务区域自己长出来的分布。这篇文章把原理、Matlab 实现和调试过程完整复盘一遍适合正在搞无人机编队、多智能体覆盖控制或者相关课程设计的朋友参考。1. 为什么是 Voronoi把编队问题变成空间分配问题1.1 预设队形方案的三个硬伤先说为什么大多数课程设计和初版方案都用“预设队形 跟随”的套路。这类方案实现简单给每架无人机分配一个相对领航者的偏移量控制律收敛之后自然形成三角形、楔形或者横排队形用户手册里都能找到现成案例。但在有障碍的真实任务场景里这套思路有三个绕不开的硬伤。第一队形恢复慢。遇到障碍物时如果选择“整体绕行”相当于把整个刚性结构平移绕过障碍路径规划计算量会随队形规模非线性增长如果选择“拆开绕行”无人机各自从障碍两侧穿过队形一旦被分割重聚的判断逻辑相当复杂。我见过最常见的现象就是两架无人机在障碍背后互相等待谁都不想先偏离预期位置直到超时触发保护逻辑。第二队形形态与任务空间无关。预设队形只在空旷且密度均匀的场景里合理。实际任务里目标区域可能有热点区域需要多部署无人机、可能有禁飞区不允许进入、可能有通信盲区不能占用。固定队形不会随空间密度改变空间利用率自然很差。第三成员失效后的重构成本高。如果编队里有一架无人机断电退出靠预设偏移的方案需要重新为每个位置指定目标系统没有自我修复能力。1.2 Voronoi 划分让队形获得自适应性Voronoi 图做的事情非常简单给定平面上一组点每架无人机的位置把平面划分成与这些点一一对应的凸多边形区域每个区域内的任意位置到“自己”这点的距离都小于到其它任何一点的距离。数学定义一句话Vi {q ∈ Q : ‖q − p_i‖ ≤ ‖q − p_j‖, ∀j ≠ i}。这个划分天然适合多智能体群体控制场景每架无人机独立负责自己的 Voronoi 单元不需要中心节点统一指挥分布式特征明显。无人机位置一变Voronoi 划分立刻随动也就是说“队形”是位置信息的函数天然自适应。无人机之间的间距由划分边界决定两个相邻单元不会无限重叠自带了一部分避碰属性。那怎么把 Voronoi 和编队控制挂钩答案在单元质心。如果每架无人机都控制自己移动到当前 Voronoi 单元的质心整个群体的空间分布就会不断优化某个区域无人机过于集中这个区域的单元面积就会缩小质心向其它方向偏移无人机分布过散单元面积增大质心被拉向空白区域中心。整个系统最终收敛到一种稳定配置数学上叫质心 Voronoi 配置Centroidal Voronoi ConfigurationCVC。1.3 从覆盖控制到编队控制的衔接为什么“向质心移动”就会收敛这里有一个非常简洁的理论支撑。定义覆盖代价函数H Σ ∫_{Vi} ‖q − p_i‖² φ(q) dq其中 φ(q) 是任务空间密度函数重点区域取值大空白区域取值小。这个积分度量的是“任务空间中每个点到最近无人机的二次距离”直观说就是“需求点到最近服务点的平均距离”。对这个函数求梯度会得到∂H/∂p_i −2 M_{Vi} (C_{Vi} − p_i)其中 M_{Vi} 是单元的加权质量C_{Vi} 是加权质心。这说明只要令控制输入正比于 C_{Vi} − p_i覆盖代价就会单调递减最后收敛到 CVC。这段话有点数学密度但值得读两遍它证明了编队不一定要“预设”只要让每个无人机持续往自己 Voronoi 单元的质心移动队伍的分布在数学上就是最优的。当然要坦率说明一点这套方案天然生成的是覆盖式编队空间自适应分布不是那种要求角点坐标精确到厘米的刚性几何编队。如果你需要的是类似“人字形”的刚性编队可以把密度函数 φ(q) 设计成在期望位置附近有尖峰Voronoi 单元质心就会被“吸”到那些尖峰上从而实现柔性几何编队。本文项目采用的是均匀密度加障碍避让的覆盖式编队仿真任务是矩形区域内的多机巡逻覆盖。2. 系统模型与整体架构控制链路怎么搭2.1 无人机运动学模型的选择编队控制仿真里最常见的两种模型。一阶积分器 ẋ u控制输入是速度建模最简单适合协议层面的验证二阶积分器双积分器ẍ u控制输入是加速度更接近真实无人机能体现惯性和速度阻尼。我在这个项目里选双积分器。原因很直接后续要迁移到硬件在环仿真或者实飞纯一阶模型控制量直接给速度和飞控输出加速度的方式不一致会多一层转换双积分器模型只需要控制律里补偿速度阻尼项迁移时改动很小。状态定义p_i ∈ R² 是位置v_i ∈ R² 是速度控制输入为加速度 u_i ∈ R²。这里先做二维平面验证后面第 5 节再讲三维扩展。2.2 通信拓扑与一致性协议多智能体系统强调局部信息交互。通信拓扑用邻接矩阵 A 描述a_ij 1 表示无人机 i 能收到无人机 j 的位置/速度信息否则为 0。自环 a_ii 0。编队控制至少保证通信图连通也就是从任意一架无人机出发经过若干跳可以到达任意其它无人机。连通性判定用 Laplacian 矩阵 L D − A 的特征值第二小特征值 λ₂ 0 说明连通。λ₂ 越大网络收敛越快。一致性协议的基础形式ẋ_i −Σ_{j∈N_i} (x_i − x_j)这个协议可以实现所有无人机状态一致收敛。编队控制就是在一致性的基础上引入期望偏移u_i −Σ_{j∈N_i} [p_i − c_i−p_j − c_j] − K_v v_i本项目里更直接一点用 Voronoi 质心作为期望位置那么“编队项”就变成一个分布式跟踪控制律u_i K_p (C_i − p_i) − K_d v_i其中 C_i 是当前 Voronoi 单元质心K_p、K_d 是 PD 增益。K_d v_i 项抑制速度振荡保证二阶系统稳定。2.3 模块划分与仿真主循环整个 Matlab 工程按功能拆成几个独立模块setup.m参数定义包括无人机数量、初始位置、障碍物、PD 增益、通信拓扑。compute_voronoi_centroids.m输入所有无人机位置输出每个单元的顶点和质心。control_law.m输入某架无人机的位置、速度、质心、障碍物、邻居信息输出加速度指令。dynamics_update.m双积分器数值积分。plot_frame.m动画绘制。main_run.m主循环。主循环结构非常直白for t 0:dt:T % 1. 更新 Voronoi 划分计算每架无人机的单元质心 centroids compute_voronoi_centroids(pos, region); % 2. 对每架无人机计算控制输入 for i 1:N neigh pos([1:i-1 i1:end], :); u(i, :) control_law(pos(i,:), vel(i,:), centroids(i,:), obstacles, neigh, opts); end % 3. 半隐式积分更新速度、位置 vel vel u * dt; pos pos vel * dt; % 4. 每若干帧绘制一次 if mod(round(t/dt), 10) 0 plot_frame(pos, vel, obstacles); drawnow; end end模块拆开的好处改避障策略、换密度函数、换通信拓扑都只需要动对应文件不用重写整个工程。这是我做了三轮编队仿真后养成的习惯——算法研究阶段模块边界就是调试边界。3. 避障控制人工势场如何与编队力共存3.1 障碍建模与斥力场设计避障方法选择上我考虑过速度障碍法VO/ORCA和模型预测控制MPC。MPC 效果好但计算量大多智能体实时仿真明显变慢VO 适合动态障碍物但和 Voronoi 编队项融合时参数太多。最后选人工势场两条理由计算开销极小实现直观和 Voronoi 控制律天然同构——两边都是“力和梯度”的语言。障碍物建模为圆形用 (obs_x, obs_y, obs_r) 表示圆心和半径。无人机到障碍表面的距离ρ ‖p − p_obs‖ − r_obs设计斥力势场U_rep 0.5·η·(1/ρ − 1/ρ₀)²当 ρ ≤ ρ₀否则为 0对应的斥力F_rep η·(1/ρ − 1/ρ₀)·(1/ρ²)·(p − p_obs)/ρ公式里 (p − p_obs)/ρ 是单位方向向量1/ρ² 来自势场梯度它让无人机离障碍越近斥力增长越快避免直接撞上。ρ₀ 是斥力作用范围我只让它在局部范围生效防止远距离障碍物干扰编队收敛。3.2 编队项与避障项的权重协调这里有个很关键的设计问题编队项和避障项同时作用权重怎么给最简单的是固定权重u u_ff u_obs。这种做法在稀疏障碍场景下能跑通但一旦无人机逼近障碍避障力可能被较大的编队力淹没出现“怼墙”反过来避障权重如果设得很大无人机在空旷区域也会被远处障碍物吸引偏离队形甚至整个队伍停在半路不前进。我的做法是让权重随“到最近障碍物距离”连续变化d_min min(所有障碍表面距离); alpha 1 / (1 exp(5 * (d_min - opts.r_safe))); u (1 - alpha) * u_ff alpha * u_obs;当 d_min 远大于 r_safe 时 alpha 约等于 0编队项完全生效进入危险区域时 alpha 快速逼近 1避障接管中间区域平滑过渡不会出现突变导致的抖动。实际调参时指数里的系数 5 控制切换带宽度我调到 5 时过渡区间大约 0.6 米左右比较合适。另外无人机之间也是移动障碍物。我在 u_obs 里额外加了一项“邻居软排斥”当两机距离小于 2 倍安全半径时施加与距离成反比的排斥力。这一项写法和障碍物斥力一致只是把障碍半径替换成无人机安全半径。加上之后编队在避障绕行过程中不会互相挤压。3.3 预防局部极小与抖动人工势场法最大的问题就是局部极小点当引力与斥力大小相等方向相反时无人机受力为零会停在某个非目标点。经典画面就是无人机在障碍物“背后”悬停既不撞也不走。我的处理方式比较务实三招并用记忆扰动记录无人机过去 N 步的位置如果连续 N 步位置变化小于阈值而速度又不为零说明进入局部极小区这时往当前受力方向的垂直方向叠加一个持续 2 秒的小扰动。实测下来8 架无人机、3 个障碍物的场景里加入扰动后没有一次被困住。斥力方向的旋转修正当无人机运动方向与障碍物连线夹角过小时几乎正对障碍把斥力方向顺时针旋转一个角度让无人机从侧面绕行。这个技巧在很多论文里叫“势场切线分量”实现起来就是在斥力方向上加一个切向单位向量分量。代码实现只有几行但对绕行效果的提升非常明显。绕行完成后自然回编队alpha 的平滑过渡本身解决了一部分“绕完再回”的问题。我观察到只要斥力作用范围 ρ₀ 设在 2.0 左右无人机绕到障碍侧面后斥力自然消失alpha 自动回到约 0编队项接管恢复路径很平滑。4. Matlab 实现拆解从 voronoin 到动画输出4.1 Voronoi 计算与无限顶点裁剪Matlab 自带的 voronoin 是最直接的起点[V, C] voronoin(pos);pos 是 N×2 的位置矩阵V 是所有顶点的坐标C 是每个单元对应的顶点索引。但这里有一个经典坑位于凸包外侧的单元顶点包含无穷远点V 中坐标会出现 Inf。如果直接把 V(C{i}, :) 传给 polyshape 算质心polyshape 立刻报错。实际项目里我用的技巧是虚拟点裁剪法在真实无人机位置之外把任务区域四角向外扩展一段距离加入 4 个虚拟位置点再调用 voronoin。这样所有真实无人机的单元都会被虚拟点的单元“挤”住不再有无限顶点。后续只需要从 C 中取前 N 个单元顶点即可。function centroids compute_voronoi_centroids(pos, region) % region [xmin, xmax, ymin, ymax] N size(pos, 1); margin 10; corners [region(1)-margin, region(3)-margin; region(2)margin, region(3)-margin; region(1)-margin, region(4)margin; region(2)margin, region(4)margin]; ext [pos; corners]; [V, C] voronoin(ext); centroids zeros(N, 2); for i 1:N verts V(C{i}, :); poly polyshape(verts(:, 1), verts(:, 2)); [cx, cy] centroid(poly); centroids(i, :) [cx, cy]; end end这个技巧省去了手写多边形裁剪算法的功夫强烈建议采用。注意虚拟点不要放在离区域太近的位置一般外扩距离设为区域宽度的一半以上才保险否则真实无人机靠近边界时单元仍然会被压得过扁。4.2 单元质心与 Lloyd 迭代计算质心用 polyshape 的 centroid 方法非常直接。但有两点提醒。polyshape 要求顶点按顺时针或逆时针顺序排列。voronoin 输出的顶点本身是环绕顺序一般可以直接用如果遇到某些退化单元报错就先转成逆时针再构造。另外质心不是几何中心而是面积一阶矩的结果。单元形状越不规则质心和顶点平均值的差异越明显——这直接影响能否收敛到 CVC所以不要图省事直接平均顶点坐标。整个 Lloyd 迭代过程给定无人机位置 p计算 Voronoi 划分计算每个单元质心 C_V控制无人机向质心移动回到第 1 步。这个迭代在时间离散化之后就是主循环。密度函数 φ(q) 如果取常数质心就是普通多边形质心如果任务区有重点区域φ(q) 取非均匀分布比如高斯峰质心会偏向重点区域编队分布也随之变密。这步零成本就能让编队形态随任务需求变化非常推荐试一试。4.3 核心控制律代码与主循环避障和编队融合后的控制律代码层面长这样function u control_law(p, v, c_des, obstacles, neigh, opts) % 编队项PD 跟踪 Voronoi 质心 u_ff opts.kp * (c_des - p) - opts.kd * v; % 避障项人工势场斥力 u_obs zeros(1, 2); d_min inf; for k 1:size(obstacles, 1) op obstacles(k, 1:2); or_ obstacles(k, 3); d norm(p - op); rho d - or_; d_min min(d_min, rho); if rho opts.rho0 rho 1e-4 dir (p - op) / d; u_obs u_obs opts.eta * (1/rho - 1/opts.rho0) * (1/rho^2) * dir; end end % 邻居软排斥无人机之间防碰撞 for j 1:size(neigh, 1) d norm(p - neigh(j, :)); if d 2 * opts.r_safe d 1e-4 u_obs u_obs opts.rep_n * (1/d - 1/(2*opts.r_safe)) * (p - neigh(j, :)) / d; end end % 编队与避障的权重协调 alpha 1 / (1 exp(5 * (d_min - opts.r_safe))); u (1 - alpha) * u_ff alpha * u_obs; end主循环里每步先重新计算 Voronoi 质心再对每个个体调用这个函数最后更新状态。数值积分我用的是半隐式——先更新速度再更新位置比显式欧拉稳定很多。dt 一般取 0.02 到 0.05 秒太大了无人机容易“穿透”斥力场。4.4 可视化与结果判读可视化这一块用到的都是 Matlab 基础绘图函数voronoi(pos(:,1), pos(:,2)) 绘制 Voronoi 边界或者用 patch 自定义绘制单元边线scatter 画无人机当前位置quiver 画速度向量障碍物用 rectangle 结合 Curvature 属性画圆每帧 drawnow最后用 VideoWriter 存视频。判断仿真是否成功的量化指标我建议看四个覆盖代价 H 是否持续下降并趋于稳定无人机最小间距是否始终大于安全半径最终无人机坐标与对应质心的误差是否收敛到阈值内alpha 均值在到达稳态后是否接近 0——说明没有持续避障占用控制资源。5. 参数调试与三处踩坑记录5.1 仿真参数速查表把我最终调出来的一组有效参数列出来方便读者作为起点参数数值说明N8无人机数量任务区域20m × 15m矩形巡逻区域dt0.05s仿真步长T40s仿真时长K_p1.8位置增益K_d2.2速度阻尼r_safe1.0m安全半径ρ₀2.5m斥力作用范围η8.0斥力增益rep_n3.0邻居软排斥增益通信拓扑k-近邻图k3保证连通性这组参数下8 架无人机从随机位置出发约 15 秒内覆盖代价 H 下降到稳定值的 90% 左右绕行 3 个障碍物过程中没有发生碰撞也没有陷入局部极小。5.2 坑一voronoin 返回无限坐标导致 polyshape 崩溃现象运行到第二帧polyshape 报错错误信息类似“Polyshape 必须包含至少 3 个顶点”。原因单元包含 Inf 顶点。排查思路打印 V(C{i}, :) 看是否存在 Inf。一旦看到 Inf其实就是凸包外侧单元的问题。解决上文的虚拟点裁剪法。这是最省事也最稳的方案不要去自己写凸包裁剪。我最早自己写过一版点集外边界裁剪代码处理边界相交、退化单元各种情况花了整整一个晚上最后不如四个虚拟点好用。5.3 坑二斥力增益调大后无人机在障碍物附近疯狂抖动现象η 从 5 调到 10 后无人机一进入障碍物斥力范围就开始来回震荡轨迹呈锯齿状。原因斥力增益太大加上离散时间步长不够小导致下一时刻的指令方向反复倒转。解决分三步处理。第一步增大 K_d让速度阻尼吃掉多余动能第二步对 u_obs 做幅值限制我这边限到 3.0 m/s²第三步时间步长从 0.05 降到 0.02。三步组合后抖动问题消失。实际上大部分“调参数调不出来”的振荡都是时间步长太粗和阻尼不足同时造成的优先检查这两项比盲目调避障权重有效得多。5.4 坑三通信拓扑不连通队伍散成两群现象8 架无人机初始位置分两堆仿真几秒后两队各自向各自质心收敛但两堆之间始终有一块真空区整个队伍没有形成统一编队。原因环形拓扑按编号连接编号前 3 架连在一起后 5 架连在一起两堆之间没有通信链路。虽然 Voronoi 质心计算用的全局信息但控制协议实际依赖邻域信息缺了链路就是两个独立子系统。解决先计算 Laplacian 矩阵的第二小特征值 λ₂。λ₂ 小于 1e-6 就是不连通。把通信拓扑改成 k-近邻图k3后队伍立刻统一。这里有个更隐蔽的细节即使通信拓扑保证连通初始位置分太散时外侧区域的 Voronoi 单元可能长时间不更新无人机和质心之间隔着很长距离收敛时间被拉长。解决方法是初始位置用随机均匀采样不要让两堆间距超过区域短边的 0.6 倍。5.5 从 2D 到 3D 的扩展路径最后聊聊迁移到三维的问题。3D 场景下 Voronoi 变成三维凸多面体划分Matlab 里有两条路用 mpt3 工具箱Multi-Parametric Toolbox 3直接生成有界 Voronoi 单元或者用 convhulln 计算每个单元的凸包顶点再用 volume 和 centroid 计算质心。控制律本身不需要改位置和速度各加一个 z 轴分量即可。障碍物从圆盘变成球体斥力公式里的距离改成三维范数ρ₀ 保持不变。飞行高度如果固定机间距离按 xy 平面投影算如果高度可变上下两架无人机的避碰逻辑要同时考虑 z 轴。我实测下来3D 场景主要多花时间在可视化环节——patch 画三维多面体、调相机视角控制层的改动基本为零。这也是为什么我建议先在 2D 里把所有控制逻辑跑通再往上加维度而不是一步到位直接写三维。