1. 项目整体设计与思路拆解多智能体无人机群体控制这几年从实验室走向了工程落地尤其是编队飞行、协同搜索、区域覆盖这类场景对算法的鲁棒性和实时性要求越来越高。我这次做的这个项目核心目标很明确让一组无人机在存在静态障碍物的环境中既能保持预设的编队形状又能安全绕开障碍最终到达目标区域。整个方案在Matlab里实现方便快速验证算法逻辑也便于后续移植到实际飞控上。为什么选多智能体框架而不是单机控制因为群体行为的本质是“局部交互产生全局秩序”。每架无人机只知道自己和邻居的位置、速度以及附近障碍物的信息通过一致性协议和势场函数就能涌现出编队保持和避障的效果。这种分布式架构的好处是扩展性强增加或减少无人机数量不需要重新设计全局控制器而且单机故障不会导致整个群体崩溃。编队控制这块我选了基于Voronoi划分的方法。Voronoi图能把空间按最近邻原则划分成多个区域每个无人机负责一个Voronoi单元。这样做的好处是编队形状可以通过调整Voronoi单元的质心位置来间接控制而不是直接给每架无人机指定一个绝对坐标。这种“质心跟踪”的思路更符合分布式控制的精神每架无人机只需要计算自己Voronoi单元的质心然后朝质心移动就能自然形成均匀分布的编队。相比领航-跟随法Voronoi方法不需要指定领航者避免了单点故障相比虚拟结构法它不需要全局的虚拟刚体坐标通信量更小。障碍避让我用了人工势场法结合速度障碍法。人工势场法计算简单实时性好但有个经典问题就是局部极小值——无人机可能卡在障碍物前面来回震荡。我的解决办法是在势场函数里加入一个与目标点方向相关的旋转分量当检测到无人机陷入局部极小值时给它一个切向的速度扰动帮助它“滑”出陷阱。速度障碍法则是用来处理动态障碍的通过计算相对速度空间中的碰撞锥选择锥外的速度矢量作为避让方向。两种方法结合静态障碍用势场法动态障碍用速度障碍法实测下来在Matlab仿真里能稳定避障。整个项目的Matlab实现分为几个模块环境建模、Voronoi划分、编队控制器、避障控制器、动力学仿真和可视化。环境建模负责生成障碍物地图和无人机初始位置Voronoi划分用voronoin函数计算每个无人机的单元顶点编队控制器根据质心误差计算期望速度避障控制器叠加势场力和速度障碍约束动力学仿真用二阶积分模型更新无人机状态可视化用plot和quiver实时显示无人机位置、速度矢量和Voronoi边界。注意Matlab的voronoin函数在无人机数量较多时计算量会明显上升如果做大规模集群仿真建议用增量式Voronoi更新或者降采样处理。这个方案适合谁参考如果你正在做无人机编队、集群控制、或者多机器人协同的课题这个Matlab实现可以直接拿来改参数、换场景。如果你是对多智能体控制感兴趣的学生或工程师代码里的注释和模块划分也能帮你快速理解Voronoi编队和势场避障的底层逻辑。下面我会把每个模块的核心细节、参数选择、实操步骤和踩过的坑都摊开讲。2. 核心细节解析与实操要点2.1 Voronoi编队控制的数学原理与参数选择Voronoi划分的数学定义很简洁给定一组点$P {p_1, p_2, ..., p_n}$每个点$p_i$的Voronoi单元$V_i$是空间中所有满足$||x - p_i|| \leq ||x - p_j||$对所有$j \neq i$的点$x$的集合。在编队控制里$p_i$就是第$i$架无人机的位置。每架无人机计算自己Voronoi单元的质心$c_i$然后朝质心移动最终所有无人机会均匀分布在整个区域形成类似蜂窝的编队。但这里有个关键问题均匀分布不等于我们想要的编队形状。比如我想让无人机排成一条直线或者一个圆环直接朝Voronoi质心移动是做不到的。我的解决办法是引入一个“密度函数”$\phi(x)$用它来加权Voronoi单元的质心计算。密度函数在期望编队密集的地方取大值稀疏的地方取小值。这样Voronoi单元的面积会自动调整无人机就会聚集在密度高的区域形成想要的形状。具体实现时质心计算公式变成$$c_i \frac{\int_{V_i} x \phi(x) dx}{\int_{V_i} \phi(x) dx}$$在Matlab里这个积分我用网格离散化来近似。对每个Voronoi单元生成一个覆盖该单元的网格计算每个网格点的密度值然后加权平均得到质心。网格分辨率我设的是0.1米实测下来精度够用计算量也能接受。如果无人机数量超过20架建议把分辨率降到0.2米否则仿真速度会明显变慢。密度函数的设计直接决定编队形状。比如要形成圆形编队密度函数可以设为$\phi(x) \exp(-(||x - x_c|| - R)^2 / \sigma^2)$其中$x_c$是圆心$R$是半径$\sigma$控制圆环的宽度。这样密度在圆环上最高无人机就会聚集在圆环附近。如果要形成直线编队密度函数设为$\phi(x) \exp(-(y - y_0)^2 / \sigma^2)$密度在$y y_0$这条线上最高。实操心得密度函数的$\sigma$参数不要设得太小否则Voronoi单元会变得非常狭长质心计算容易受数值误差影响。我一般取$\sigma$为期望编队特征尺寸的0.3到0.5倍。2.2 人工势场法的函数设计与局部极小值处理人工势场法的核心思想是把无人机受到的总力分解为目标点的引力和障碍物的斥力。引力通常用二次函数$F_{att} k_{att} \cdot (x_{goal} - x)$其中$k_{att}$是引力增益。斥力用反比例函数$F_{rep} k_{rep} \cdot (1/d - 1/d_0) \cdot (1/d^2) \cdot \nabla d$其中$d$是无人机到障碍物的距离$d_0$是斥力作用范围$k_{rep}$是斥力增益。参数选择上$k_{att}$和$k_{rep}$的比例很关键。如果$k_{att}$太大无人机会“勇往直前”撞上障碍如果$k_{rep}$太大无人机会在障碍物附近过度保守甚至无法靠近目标。我一般先设$k_{att} 1$然后调整$k_{rep}$使得在$d d_0/2$时斥力大约是引力的2到3倍。$d_0$通常设为障碍物半径的1.5到2倍这样无人机在接近障碍物之前就开始避让。局部极小值是势场法的老毛病。当引力方向和斥力方向恰好相反且大小相等时合力为零无人机就卡住了。我的处理方法是加入一个“逃逸力”当检测到无人机速度持续低于某个阈值比如0.1 m/s超过一定时间比如2秒就施加一个垂直于引力方向的力方向随机选择左或右持续0.5秒。这个逃逸力不需要很大只要能打破平衡就行。实测下来这个方法能解决90%以上的局部极小值问题。还有一种情况是障碍物在目标点正前方无人机绕过去之后又会被引力拉回来形成震荡。我的解决办法是在势场函数里加入一个“记忆项”记录无人机最近经过的障碍物位置如果无人机在短时间内反复接近同一个障碍物就临时增大该障碍物的斥力范围迫使无人机绕得更远。这个技巧在Matlab里用一个简单的队列就能实现。2.3 速度障碍法处理动态障碍速度障碍法Velocity Obstacle是用来处理移动障碍物的。它的基本思想是在相对速度空间里如果无人机和障碍物的相对速度落在某个锥形区域内那么未来某个时刻它们就会碰撞。这个锥形区域就是速度障碍。无人机需要选择一个锥形区域外的速度作为避让速度。具体计算时设无人机位置为$p_a$速度为$v_a$障碍物位置为$p_b$速度为$v_b$障碍物半径为$r_b$。相对位置$p_{ab} p_b - p_a$相对速度$v_{ab} v_b - v_a$。速度障碍锥的顶点在$p_{ab}$方向半角为$\arcsin(r_b / ||p_{ab}||)$。如果$v_{ab}$落在锥内就需要调整$v_a$。调整策略我用了“最小偏转法”在速度空间里找到距离当前$v_a$最近且不在锥内的速度矢量。这个可以通过几何方法求解也可以用数值优化。Matlab里我用fmincon做一个小优化目标函数是$||v_a^{new} - v_a||^2$约束是$v_a^{new}$不在速度障碍锥内。优化变量是二维速度矢量计算量很小实时性没问题。注意速度障碍法假设障碍物速度恒定如果障碍物机动性强需要频繁重算速度障碍锥。我的做法是每0.1秒更新一次障碍物速度估计用卡尔曼滤波平滑避免速度估计噪声导致避让抖动。2.4 动力学模型与控制器设计无人机动力学我用了二阶积分模型$\dot{p} v$$\dot{v} u$其中$p$是位置$v$是速度$u$是加速度控制输入。这个模型忽略了姿态动力学适合做编队控制算法验证。如果要更真实可以换成四旋翼模型但计算量会大很多。控制器设计上我用了“期望速度速度跟踪”的双环结构。外环根据Voronoi质心误差和势场力计算期望速度$v_{des}$内环用PD控制器跟踪期望速度$u k_p (v_{des} - v) k_d (\dot{v}_{des} - \dot{v})$。$k_p$和$k_d$的选取影响响应速度和超调量。我一般设$k_p 2$$k_d 1$这样速度跟踪的上升时间大约0.5秒超调小于10%。期望速度的计算是$v_{des} v_{formation} v_{avoid}$其中$v_{formation}$是朝Voronoi质心移动的速度$v_{avoid}$是避障产生的修正速度。$v_{formation}$的大小我设了一个上限比如2 m/s避免无人机在编队误差大时速度过快。$v_{avoid}$则根据势场力和速度障碍法的输出合成。这里有个细节Voronoi质心计算和避障计算是解耦的但实际飞行中两者会相互影响。比如无人机为了避障偏离了Voronoi质心方向编队形状就会暂时变形。我的处理方法是给编队控制器一个较低的优先级避障控制器优先级更高。当避障力超过某个阈值时暂时忽略编队误差等避障完成后再恢复编队控制。这个优先级切换用一个简单的状态机实现效果很好。3. 实操过程与核心环节实现3.1 环境建模与初始化环境建模的第一步是定义仿真区域和障碍物。我设仿真区域为100米×100米的矩形障碍物用圆形表示每个障碍物有中心坐标和半径。障碍物数量我一般设5到10个随机分布在区域中间避免初始位置和目标点附近有障碍。无人机初始位置我用了两种方式一种是随机撒点另一种是均匀网格。随机撒点更接近实际部署场景但可能导致初始Voronoi单元大小差异很大编队收敛慢。均匀网格初始化收敛快但不够真实。我一般先用均匀网格验证算法再用随机撒点测试鲁棒性。目标点我设了一个矩形区域所有无人机需要最终覆盖这个区域。目标点的引力场用整个区域的平均位置作为虚拟目标点这样无人机会自然散开覆盖区域而不是全部挤到一个点上。Matlab代码里环境初始化大概是这样% 仿真区域 area [0 100 0 100]; % 障碍物 [x, y, radius] obstacles [30 30 8; 60 40 10; 45 70 7; 70 75 9; 20 60 6]; % 无人机数量 N 8; % 初始位置均匀网格 [X, Y] meshgrid(linspace(20, 80, 4), linspace(20, 80, 2)); positions [X(:), Y(:)]; % 目标区域 goal_area [70 90 70 90];实操心得障碍物半径不要设得太小否则势场斥力范围$d_0$也会很小无人机容易“擦边”撞上。我一般设障碍物半径至少3米$d_0$至少5米。3.2 Voronoi划分与质心计算Voronoi划分用Matlab的voronoin函数。输入是无人机位置矩阵输出是Voronoi顶点和单元索引。需要注意的是voronoin返回的顶点可能包含无穷远点用1表示需要特殊处理。我的做法是给仿真区域加一个大的边界框把无穷远点截断在边界上。质心计算我用网格离散化。对每个Voronoi单元生成一个覆盖该单元的网格计算每个网格点的密度值然后加权平均。网格生成用meshgrid密度函数根据编队形状定义。比如圆形编队% 计算Voronoi图 [v, c] voronoin(positions); % 对每个无人机计算质心 for i 1:N % 获取第i个Voronoi单元的顶点 vertices v(c{i}, :); % 生成网格 [Xg, Yg] meshgrid(linspace(min(vertices(:,1)), max(vertices(:,1)), 20), ... linspace(min(vertices(:,2)), max(vertices(:,2)), 20)); % 判断网格点是否在Voronoi单元内 in inpolygon(Xg, Yg, vertices(:,1), vertices(:,2)); % 计算密度 density exp(-((sqrt((Xg - center(1)).^2 (Yg - center(2)).^2) - R).^2) / sigma^2); % 加权质心 weights density .* in; centroid(i, :) [sum(Xg(:).*weights(:)) / sum(weights(:)), ... sum(Yg(:).*weights(:)) / sum(weights(:))]; end这段代码里inpolygon用来判断网格点是否在Voronoi单元内density是密度函数weights是加权系数。质心就是加权平均的位置。注意inpolygon在网格点很多时比较慢可以用convhull先求Voronoi单元的凸包再用凸包判断速度快很多。因为Voronoi单元本身就是凸的凸包就是单元本身。3.3 编队控制器与避障控制器实现编队控制器的输出是期望速度$v_{formation}$计算方式是% 编队控制 v_formation k_formation * (centroid - positions); % 限幅 v_max 2; speed sqrt(sum(v_formation.^2, 2)); for i 1:N if speed(i) v_max v_formation(i, :) v_formation(i, :) / speed(i) * v_max; end end避障控制器分两部分势场力和速度障碍修正。势场力计算% 势场避障 F_att k_att * (goal - positions); F_rep zeros(N, 2); for i 1:N for j 1:size(obstacles, 1) d norm(positions(i, :) - obstacles(j, 1:2)); if d d0 F_rep(i, :) F_rep(i, :) k_rep * (1/d - 1/d0) * (1/d^2) * ... (positions(i, :) - obstacles(j, 1:2)) / d; end end end F_total F_att F_rep; v_avoid k_avoid * F_total;速度障碍修正只在检测到动态障碍时启用。我的仿真里动态障碍用另一个移动的点表示速度恒定。速度障碍锥的计算和避让速度求解用fmincon% 速度障碍避让 if dynamic_obstacle_present p_ab obs_pos - positions(i, :); v_ab obs_vel - velocities(i, :); % 速度障碍锥半角 alpha asin(obs_radius / norm(p_ab)); % 检查v_ab是否在锥内 angle_v atan2(v_ab(2), v_ab(1)); angle_p atan2(p_ab(2), p_ab(1)); if abs(angle_v - angle_p) alpha % 需要避让用fmincon求解 v_new fmincon((v) norm(v - velocities(i, :))^2, velocities(i, :), ... [], [], [], [], [], [], (v) cone_constraint(v, p_ab, alpha)); v_avoid(i, :) v_new - velocities(i, :); end end实操心得fmincon在每次迭代时都会调用约束函数如果无人机数量多计算量会很大。我的优化是只在相对速度确实落在锥内时才调用fmincon而且用上一次的解作为初始值减少迭代次数。3.4 仿真主循环与可视化仿真主循环的时间步长我设的是0.05秒对应20Hz的控制频率。这个频率对编队控制来说足够了实际飞控通常跑50到100Hz但Matlab仿真20Hz就能看出效果。每步更新无人机状态% 主循环 dt 0.05; T 60; for t 0:dt:T % 计算Voronoi质心 centroid compute_voronoi_centroid(positions, density_func); % 编队控制 v_formation formation_control(positions, centroid); % 避障控制 v_avoid obstacle_avoidance(positions, velocities, obstacles, goal); % 合成期望速度 v_des v_formation v_avoid; % 速度跟踪 u kp * (v_des - velocities) kd * (-accelerations); % 更新状态 accelerations u; velocities velocities accelerations * dt; positions positions velocities * dt; % 可视化 plot_simulation(positions, velocities, obstacles, goal_area, v, c); drawnow; end可视化部分我用plot画无人机位置圆圈、速度矢量箭头、Voronoi边界线段和障碍物填充圆。drawnow用来实时刷新。如果仿真速度慢可以每10步画一次或者用pause(0.01)控制刷新率。注意Matlab的drawnow在循环里调用会显著降低仿真速度。如果做长时间仿真建议先跑完再画或者用drawnow limitrate限制刷新频率。4. 常见问题与排查技巧实录4.1 无人机在障碍物附近震荡不前进这是势场法最典型的问题。现象是无人机在障碍物前面来回移动速度方向反复变化但就是不绕过去。原因通常是引力方向和斥力方向接近相反合力在障碍物法线方向上来回切换。排查思路先打印无人机到障碍物的距离和合力方向看是否在某个位置合力突然反向。如果是说明陷入了局部极小值。解决办法有三个一是加入逃逸力检测到速度持续低于阈值时施加切向扰动二是调整$k_{att}$和$k_{rep}$的比例让引力稍大于斥力无人机就会“挤”过去三是用速度障碍法替代势场法速度障碍法没有局部极小值问题但计算量稍大。我一般先用逃逸力简单有效。逃逸力的方向垂直于引力方向大小设为引力的0.5倍持续0.3秒。实测下来大部分震荡都能在1秒内解决。4.2 Voronoi单元计算出现无穷远点voronoin函数在无人机位于凸包边界时会返回无穷远点索引为1。如果不处理质心计算会出错。我的处理方法是给仿真区域加一个大的边界框比如区域是100×100边界框设成-1000到1000。这样无穷远点会被截断在边界框上不会影响内部Voronoi单元的计算。还有一种情况是无人机数量少于3架时Voronoi图退化无法形成闭合单元。这时候编队控制要特殊处理比如两架无人机时直接用中点作为虚拟质心。我的代码里加了判断如果$N 3$就用简单的距离控制替代Voronoi控制。4.3 编队收敛慢或者不收敛编队收敛慢通常是因为$k_{formation}$太小或者密度函数$\sigma$太大导致质心计算不准确。排查时先看质心位置是否合理如果质心偏离Voronoi单元中心太远说明密度函数有问题。调整$\sigma$到期望编队尺寸的0.3到0.5倍重新计算质心。如果编队完全不收敛检查Voronoi单元是否为空。当无人机位置重合或者非常接近时Voronoi单元可能退化成一条线或一个点质心计算失效。解决办法是加一个最小距离约束无人机之间保持至少0.5米距离避免位置重合。实操心得编队收敛速度还和初始位置有关。如果初始位置随机撒点收敛时间可能比均匀网格长3到5倍。如果做实时性要求高的场景建议用均匀网格初始化或者用K-means先做一次粗聚类。4.4 速度障碍法导致避让抖动速度障碍法在障碍物速度估计有噪声时会导致避让方向频繁切换无人机抖动。我的解决办法是对障碍物速度做卡尔曼滤波平滑估计值。另外避让速度求解时加一个“惯性项”让新速度不要和上一次速度差太多$$v_{new} \arg\min ||v - v_{prev}||^2 \lambda ||v - v_{des}||^2$$其中$v_{prev}$是上一次的避让速度$\lambda$是权重。这样避让速度变化更平滑抖动明显减少。$\lambda$我一般设0.3到0.5。4.5 仿真速度慢的优化技巧Matlab仿真无人机集群时计算量主要来自Voronoi划分和质心计算。如果无人机数量超过20架仿真会明显变慢。我的优化技巧有用voronoin的增量更新只重新计算位置变化的无人机的Voronoi单元。质心计算的网格分辨率从0.1米降到0.2米精度损失不大速度提升4倍。可视化每10步画一次或者用drawnow limitrate。把势场力计算向量化避免双重循环。如果还是慢可以考虑用Matlab的parfor并行计算每个无人机的Voronoi质心。不过parfor有启动开销无人机数量少于10架时反而更慢。4.6 常见问题速查表问题现象可能原因排查方法解决办法无人机在障碍物前震荡局部极小值打印合力方向加逃逸力或调参数Voronoi单元计算错误无穷远点未处理检查顶点索引加边界框截断编队收敛慢密度函数参数不当检查质心位置调整$\sigma$避让抖动障碍物速度噪声检查速度估计卡尔曼滤波惯性项仿真速度慢计算量过大计时各模块降分辨率向量化无人机位置重合初始位置太近检查初始距离加最小距离约束速度跟踪超调PD参数过大看速度响应曲线减小$k_p$或增大$k_d$编队形状不对密度函数设计错误可视化密度分布重新设计密度函数最后再分享一个小技巧调试编队控制时先把避障关掉只跑编队控制确认编队能收敛到期望形状。然后再打开避障观察避障对编队的影响。这样分步调试问题定位快很多。我一开始就是编队和避障一起调结果编队不收敛排查了半天才发现是避障力太大把编队力盖过去了。分开调之后十分钟就搞定了。