做无人机集群路径规划有一段时间了。相比单机路径规划，集群最让人挠头的是“路径之间还会打架”：你单独给每架无人机规划的航迹都没问题，可几十个航迹放到同一张地图上，空中相遇、间隔不足、同一时刻扎堆过窄走廊的问题全冒出来。最近我把 BKA、CO、PSO、PIO 四种优化算法挨个塞进同一个集群路径规划模型里跑了个遍，把建模、代价函数、算法更新、仿真对比的完整过程整理成了 Matlab 代码。这篇博文就是把这套过程拆开来讲，适合正在做无人机集群航迹规划的研究生、参加智能算法竞赛的团队，以及想把群智能优化算法用在实际路径规划问题上的开发者。

先说结论：没有哪一种算法在所有场景下都绝对胜出。BKA 和 CO 这类新算法在复杂障碍环境下更容易找到更平滑的集群路径，但代价是计算时间更长、稳定性需要多次运行验证；PSO 速度快、实现简单，却可能早早陷入局部最优；PIO 的“先罗盘后地标”机制比较贴合路径规划的需求，在多数中等规模算例上表现均衡。下面我会把整个思路展开，包括怎么建模、怎么写代价函数、四种算法各自怎么实现、对比实验怎么设计，以及我在实际调试中踩过的几个典型的坑。

1. 无人机集群路径规划到底在解什么问题

1.1 单机路径规划与集群路径规划的本质区别

很多人以为集群路径规划就是把单机路径规划重复做 N 遍，实际完全不是一回事。单机任务里，一条路径只对障碍物负责，路径规划的核心是“从起点到终点找一条无碰撞的路线”；集群任务里，每条路径不仅要避开环境中的障碍物，还要避开其他无人机，甚至要考虑同一时间点上 multi-agent 之间的最小安全距离、编队保持、时序协同等因素。

当无人机数量只有 2 到 3 架时，还可以把机间碰撞当作一条额外约束加到单机规划里；但当数量到 6 架、8 架甚至更多时，问题性质就变了。因为任意两架无人机之间都存在距离约束，组合数量会按平方级别增长。如果再考虑三维空域、转弯半径、速度约束，直接在栅格图或者树搜索算法里扩展状态空间，会面临状态爆炸的问题。所以现在很多研究都把集群路径规划建模成优化问题，用元启发式算法去搜索一组可行且优质的航迹。

1.2 数学上把“路径质量”拆成四类代价

一个标准的集群路径规划优化问题，可以写成最小化代价函数的形式。假设有 N 架无人机，每架无人机有一条路径，我们把每架无人机的路径用 D 个中间航迹点来描述，那么整个问题的决策变量就是 N×D×dim 维向量，dim 是 2 或 3，代表二维平面或三维空间。

总代价通常由四部分构成：

totalCost = w1 * lengthCost + w2 * obstacleCost + w3 * smoothCost + w4 * collisionCost

其中 lengthCost 是全部无人机路径长度之和，体现飞行距离；obstacleCost 是路径点与障碍物之间的距离惩罚，体现安全性；smoothCost 是相邻航迹段的夹角惩罚，体现可飞性；collisionCost 是任意两架无人机在相同时间点上距离过近的惩罚，体现协同性。

这四个代价加起来，就把“路径规划得怎么样”变成了一个数值。数值越小，说明路径越短、越安全、越平滑、集群之间越不打架。这个思路看起来简单，但实际写代价函数时会发现很多细节，后面会专门讲。

1.3 为什么优化算法是主流做法

有人会问：经典的 A* 搜索、RRT* 采样算法不是也能做路径规划吗？为什么还要用 BKA、CO、PSO、PIO 这类优化算法？

A* 在二维栅格地图上效率很好，但集群场景下状态空间几乎是组合爆炸的。每个无人机都有位置状态，所有无人机的状态组合起来，搜索空间维度急剧上升。RRT* 能处理几何障碍，但很难把“路径平滑度”“机间最小间距”“多机协同”这类连续指标自然写进采样过程。而智能优化算法只要有明确的代价函数，就能通过种群迭代去搜索近似最优解，多机协同约束可以直接变成惩罚项。这也是为什么在科研和竞赛中，这类算法频繁出现在无人机集群路径规划里。

2. 把路径规划变成一个可计算的优化问题：Matlab建模实践

2.1 用 struct 描述环境和任务

在 Matlab 里跑优化算法，第一步不是写算法，而是把问题环境定义清楚。我习惯用一个 model 结构体把地图、无人机数量、路径点数、起点终点、障碍物全部装进去。这样后面四种算法调用同一个数据结构，公平性也好保证。

model = struct; model.N = 4; % 无人机数量 model.D = 8; % 每架无人机的中间航迹点数 model.dim = 2; % 2表示二维平面，改成3就是三维路径规划 model.xmin = 0; model.xmax = 100; model.ymin = 0; model.ymax = 100; model.start = repmat([0 0], model.N, 1); model.target = repmat([100 100], model.N, 1); model.obs = [30 40 8; 60 30 10; 40 70 8; 80 60 9]; % 每个障碍物为 [x, y, r] model.safeDist = 5;

障碍物用圆形表示只是演示用，实际换成长方形、多边形或者数字高程模型生成的栅格地图都可以，只需要对应修改障碍代价的计算函数。圆形障碍的好处是计算点到圆心的欧几里得距离特别方便，适合先跑通算法流程。

2.2 路径编码：每一只“鸟”到底代表什么

优化算法的每个个体，本质上是一个候选解。在这个问题里，一个候选解就是所有无人机的全部中间航迹点拼起来的一个长向量。假设有 4 架无人机，每架 8 个中间点，二维平面，那么个体长度就是 4×8×2=64。这个向量在 Matlab 中可以用一行代码 reshape 成三维数组，后续处理非常方便。

X = reshape(individual, model.N, model.D, model.dim); % 对第 k 架无人机，完整路径为： path_k = [model.start(k, :); squeeze(X(k, :, :)); model.target(k, :)];

这里要注意 reshape 的顺序问题。第一个维度是无人机编号，第二个维度是航迹点序号，第三个维度是坐标分量。这样设计的好处是，后面无论写 PSO 的速度更新还是 BKA 的扰动操作，都只需要对这个长向量整体处理，不需要为每架无人机维护单独的变量，代码简洁很多。

2.3 代价函数的 Matlab 骨架

代价函数是整套代码里最核心的部分。下面给出一个简洁但可运行的骨架，每一种代价都单独计算，最后按权重相加。

function val = pathCost(individual, model) N = model.N; D = model.D; dim = model.dim; X = reshape(individual, N, D, dim); lengthCost = 0; obstacleCost = 0; smoothCost = 0; collisionCost = 0; for k = 1:N path = [model.start(k, :); squeeze(X(k, :, :)); model.target(k, :)]; % 1) 路径长度代价 lengthCost = lengthCost + sum(sqrt(sum(diff(path, 1, 1).^2, 2))); % 2) 障碍物代价 obstacleCost = obstacleCost + obstaclePenalty(path, model.obs); % 3) 平滑代价：用相邻线段夹角惩罚 smoothCost = smoothCost + smoothPenalty(path); end % 4) 集群碰撞代价：在不同时间点上检查距离 collisionCost = collisionPenalty(X, model.start, model.target, model.safeDist); w = [1, 1000, 300, 500]; val = w(1)*lengthCost + w(2)*obstacleCost + w(3)*smoothCost + w(4)*collisionCost; end

写代价函数时我有一条经验：尽量不要把碰撞判断写成 if 穿墙 then 返回 inf。否则所有穿墙解代价都是一样的，算法完全得不到梯度信息，后期很难把解“拉”回安全区域。更好的做法是给障碍物区域一个连续的高惩罚，比如越靠近障碍中心惩罚越大，让算法能感知到移动方向。

2.4 约束边界怎么处理

路径规划里最常见的硬约束是地图边界：中间航迹点不能超出 xmin、xmax、ymin、ymax。优化算法生成了随机新解以后，经常会有分量越界。最简单且稳的方法不是罚函数，而是边界修复，把越界的坐标直接拉回边界或者随机放到边界内部。

function x = boundaryRepair(x, lb, ub) x = min(max(x, lb), ub); end

有些算法研究者喜欢用“随机重置”来处理越界分量，也就是越界的那个维度重新生成一个随机值。这种方法在低维问题上不错，但在路径规划这种高维问题上会引入很大的随机扰动，导致收敛变慢。我实测下来，直接裁剪边界最稳定。

3. 四种优化算法的核心机理与 Matlab 代码实现

3.1 所有算法都套在同一个搜索框架里

虽然 BKA、CO、PSO、PIO 的机制不同，但代码框架完全可以共用。先用随机种群初始化每一个个体，然后进入迭代：在每一代里，用代价函数评估所有个体，更新种群的个体最优和历史最优，再按各自的更新公式生成新个体，最后做边界修复和选择。

function [bestSol, bestCost, curve] = runAlgorithm(updateHandle, model, opt) dims = model.N * model.D * model.dim; lb = repmat([model.xmin, model.ymin], 1, dims/2); ub = repmat([model.xmax, model.ymax], 1, dims/2); pop = rand(opt.NP, dims) .* (ub - lb) + lb; fit = zeros(opt.NP, 1); for i = 1:opt.NP fit(i) = pathCost(pop(i, :), model); end [bestCost, idx] = min(fit); bestSol = pop(idx, :); curve = zeros(opt.MaxIt, 1); for t = 1:opt.MaxIt [pop, fit] = updateHandle(pop, fit, bestSol, bestCost, model, opt, t); [newBestCost, idx] = min(fit); if newBestCost < bestCost bestCost = newBestCost; bestSol = pop(idx, :); end curve(t) = bestCost; end end

这个框架写好后，四种算法只需要各自实现 updateHandle，也就是“根据当前种群和历史最优生成新种群”的那段逻辑。这样结构清晰，调 Bug 也容易定位问题在算法层还是代价函数层。

3.2 BKA：模拟黑翅鸢飞行与攻击的新算法

BKA 是新出现不久的自然启发式优化算法，核心是模拟黑翅鸢的飞行巡航和捕食攻击。飞行阶段可以看作全局勘探，个体围绕当前最优解做大范围的搜索；攻击阶段则是局部开发，个体快速向目标方向俯冲，还会引入长尾扰动防止早熟。我在实现时把它简化成下面的形式：

function [pop, fit] = bkaUpdate(pop, fit, bestSol, bestCost, model, opt, t) dims = model.N * model.D * model.dim; lb = repmat([model.xmin, model.ymin], 1, dims/2); ub = repmat([model.xmax, model.ymax], 1, dims/2); for i = 1:opt.NP if rand < 0.6 % 飞行阶段：全局探索 newInd = pop(i, :) + randn(1, dims) .* (bestSol - pop(i, :)); else % 攻击阶段：利用柯西长尾扰动跳出现有区域 step = 0.5 * tan(pi * (rand(1, dims) - 0.5)); newInd = bestSol + step .* (bestSol - pop(i, :)); end newInd = boundaryRepair(newInd, lb, ub); newCost = pathCost(newInd, model); if newCost < fit(i) pop(i, :) = newInd; fit(i) = newCost; end end end

这里我必须提醒一句：正式论文里的 BKA 公式比我这个简化版本复杂，比如飞行阶段可能包含翅膀摆动、迎角变化等参数。我这里给的只是“保留核心逻辑”的演示代码，目的是让团队能快速把它接进路径规划框架。如果要用到论文里，请务必回到原始文献按照完整公式实现。

3.3 CO：浣熊追逐猎物与逃逸的两阶段策略

CO 算法（浣熊优化算法）是我试过的几款新算法里比较好理解的，它把一次迭代分成两个阶段。第一阶段模拟浣熊群体捕猎鬣蜥：一部分爬到树上，另一部分在树下等待，本质上是让整个种群向当前最优位置靠近，实现全局搜索。第二阶段模拟浣熊遭遇捕食者后逃逸：个体随机跳到地图中的新位置，给种群注入随机性，避免大家全挤在同一个局部最优。

function [pop, fit] = coUpdate(pop, fit, bestSol, bestCost, model, opt, t) dims = model.N * model.D * model.dim; lb = repmat([model.xmin, model.ymin], 1, dims/2); ub = repmat([model.xmax, model.ymax], 1, dims/2); if t <= opt.MaxIt / 2 % 阶段1：追捕猎物，向最优解靠近 for i = 1:opt.NP I = randi([1 2]); if rand < 0.5 newInd = pop(i, :) + I * (bestSol - pop(i, :)); else newInd = pop(i, :) + (bestSol - pop(i, :)); end newInd = boundaryRepair(newInd, lb, ub); newCost = pathCost(newInd, model); if newCost < fit(i) pop(i, :) = newInd; fit(i) = newCost; end end else % 阶段2：逃逸，随机探索新区域 for i = 1:opt.NP newInd = lb + rand(1, dims) .* (ub - lb); newCost = pathCost(newInd, model); if newCost < fit(i) pop(i, :) = newInd; fit(i) = newCost; end end end end

实际使用中我发现，CO 第二阶段的纯随机逃逸虽然能探索新区域，但会把已经搜到的不错路径打得比较碎，导致收敛曲线不够平滑。后面我给 CO 加了一个精英保留操作，每次迭代结束后，把最差的一个个体替换成历史最优解，稳定性明显改善。

3.4 PSO：经典粒子群的“经验共享”机制

PSO 是不会有争议的经典算法。它的每个个体被视为一个粒子，粒子在搜索空间中飞行，飞行方向由三部分决定：当前速度、个体历史最优 pbest、群体历史最优 gbest。标准的更新公式是：

v = w * v + c1 * rand * (pbest - x) + c2 * rand * (gbest - x) x = x + v

对应的 Matlab 核心代码：

function [pop, fit] = psoUpdate(pop, fit, bestSol, bestCost, model, opt, t) dims = model.N * model.D * model.dim; lb = repmat([model.xmin, model.ymin], 1, dims/2); ub = repmat([model.xmax, model.ymax], 1, dims/2); w = 0.9 - 0.5 * (t / opt.MaxIt); % 惯性权重线性下降 v = opt.v; for i = 1:opt.NP v(i, :) = w * v(i, :) ... + opt.c1 * rand(1, dims) .* (opt.pbest(i, :) - pop(i, :)) ... + opt.c2 * rand(1, dims) .* (bestSol - pop(i, :)); v(i, :) = max(min(v(i, :), opt.vmax), -opt.vmax); pop(i, :) = pop(i, :) + v(i, :); pop(i, :) = boundaryRepair(pop(i, :), lb, ub); newCost = pathCost(pop(i, :), model); if newCost < fit(i) fit(i) = newCost; opt.pbest(i, :) = pop(i, :); end end opt.v = v; end

PSO 在路径规划问题里最大的问题是容易产生“锯齿状路径”。原因是粒子速度更新状态笨，相邻中间点可能互相拉扯。解决办法很简单：在代价函数里加入平滑代价项，或者最后对最优路径做一次 B 样条平滑。这样 PSO 依然是一个很能打的基线算法。

3.5 PIO：鸽子归巢的双机制切换

PIO 是模拟鸽子归巢行为设计的算法，分成两个完全不同的阶段。第一阶段叫地图指南针算子，模拟鸽子依靠太阳和地磁场判断回家方向，速度会随时间衰减；第二阶段叫地标算子，模拟鸽子接近目的地时改用熟悉地标导航，每次迭代先把适应度较差的个体淘汰掉一半，以优秀个体的中心作为地标，所有鸽子向这个中心位置移动。

function [pop, fit] = pioUpdate(pop, fit, bestSol, bestCost, model, opt, t) dims = model.N * model.D * model.dim; lb = repmat([model.xmin, model.ymin], 1, dims/2); ub = repmat([model.xmax, model.ymax], 1, dims/2); if t <= opt.MaxIt / 2 % 地图指南针阶段 for i = 1:opt.NP opt.v(i, :) = opt.v(i, :) * exp(-opt.R * t) ... + rand(1, dims) .* (bestSol - pop(i, :)); pop(i, :) = pop(i, :) + opt.v(i, :); pop(i, :) = boundaryRepair(pop(i, :), lb, ub); newCost = pathCost(pop(i, :), model); if newCost < fit(i) fit(i) = newCost; end end else % 地标阶段：淘汰一半较差个体，向中心飞 [~, idx] = sort(fit); keepNum = max(2, ceil(opt.NP / 2)); center = mean(pop(idx(1:keepNum), :), 1); for i = 1:opt.NP pop(i, :) = pop(i, :) + rand(1, dims) .* (center - pop(i, :)); pop(i, :) = boundaryRepair(pop(i, :), lb, ub); newCost = pathCost(pop(i, :), model); if newCost < fit(i) fit(i) = newCost; end end end end

PIO 的阶段性切换和路径规划需求很契合：前期大范围探索找大方向，后期集中资源精细搜索。注意地标阶段如果完全按论文把种群减半，在高维问题里会损失太多多样性。我一般不完全淘汰一半，而是保留 70% 左右的个体作为优秀集合计算中心，效果更好。

3.6 四种算法一张表看懂

实现完之后我最大的体会：四种算法看似差异很大，但只要写好公共框架，替换更新逻辑只花一晚上的时间。真正费时间的永远是代价函数和参数调试，算法本身反而是最容易搬动的一块。

4. 无人机集群场景下的算法适配与仿真结果

4.1 统一实验配置才能公平对比

跑算法对比最怕不公平。如果每种算法用的种群规模、迭代次数、初始种群都不一样，那图表做得再漂亮也不能说明问题。我在做对比实验时，统一设置种群数量 100、最大迭代 200 代，地图和障碍物完全相同。更重要的是，设置相同的随机种子基线，让每次重复实验都可复现。

rng(2024, 'twister');

每种算法独立跑 20 次，因为元启发式算法初始化随机，单次结果没有统计意义。另外还有一个更严谨的做法：不用迭代次数作为停止条件，而是用“适应度评估次数”作为停止条件。因为有的算法虽然每代迭代快，但单代评估次数少，直接比较迭代次数会不公平。

4.2 典型收敛曲线与路径质量观察

在 4 架无人机、8 个中间点、4 个圆形障碍物的测试场景中，我记录了一次典型的收敛曲线和最终路径质量。以下数字只来自某组实验中的一次结果，不代表所有地图上的绝对排名，但趋势有参考价值：

从曲线走势上看，PSO 前期下降最快，但经常在中后期进入平台期，说明它容易早熟；BKA 和 CO 前期略慢，却能在绕行复杂障碍时找到更短的绕行路线；PIO 的下降曲线最“稳健”，前期速度居中，后期地标机制触发后会有一段明显下降。

4.3 从单次最优解到多次统计

单次结果只能讲故事，多次统计才能下结论。我把四种算法各跑 20 次，记录每次的最优代价，然后把数值画成箱线图。在这个图上通常能看到：PSO 的中位数可能不是最差，但容易出现离群点；CO 的箱体比较宽，说明稳定性差一些；BKA 和 PIO 的箱体相对窄，整体表现更稳。

如果项目要求稳定交付，我大概率优先选 PIO；如果目标是复杂地形下冲更低的目标函数值，BKA 和 CO 值得多花时间调参。所谓“谁最优”必须绑定具体的代价函数和地图结构，离开场景谈算法强弱没有意义。

4.4 多指标评估：代价小不一定能飞

目标函数值只反映加权后的结果，有时会出现路径总长度很漂亮但转弯特别急、或者贴着障碍物边缘走的情况。因此我在仿真后还会额外检查几项指标：

• 路径最大转弯角，用于判断物理可飞性；
• 任意两架无人机最小间距，用于判断集群安全性；
• 路径与障碍物平均距离，用于判断安全裕度；
• 同一路径点的平滑性，用于判断后期是否要做曲线拟合。

多指标评估可以帮助你发现代价函数权重设置是否合理。比如某个算法的目标代价最低，但最大转弯角超过无人机实际机动限制，那说明平滑惩罚项系数太小，而不是算法本身厉害。

5. 跑仿真时最容易踩的五个坑

5.1 个体维度算错，算法永远学不会“飞”

这是我最常遇到的新手错误。假设 N=4、D=8、dim=2，个体维度是 64，不是 32。如果只写成 N×D=32，reshape 就会把坐标分量错配，算法搜来搜去都在一个乱了套的空间里，代价函数值根本降不下去。调试办法很简单：在 cost 函数里加一行 size 检查，或者把最优解 reshape 后单独画一条路径出来，肉眼看一眼路径是否连续。

5.2 惩罚项量级失控，最优解照样穿墙

代价函数里，障碍惩罚的量级必须明显大于路径长度。一条绕行路径的长度通常也就 300 到 400，如果你把穿墙惩罚写成每次加 10，那么穿墙解的总代价可能比绕行还低，算法当然选择穿墙。我会在初始调参时把障碍惩罚系数设成 1000 甚至 10000，跑通之后再慢慢降低到一个离墙更近但不穿墙的值。关键是让“穿墙”和“不穿墙”在代价数值上有明显分层。

5.3 只跑一次就下结论，随机性会骗你

元启发式算法初始化种群全靠随机数，一次运行的低代价可能只是运气好。我在正式对比中至少跑 20 次，记录平均值、标准差和最好值。这里要注意，每次实验开始时调用rng(k, 'twister')，确保别人拿到代码能够复现你的结果。否则过两个星期你自己都可能复现不出来。

5.4 迭代次数相同，但计算预算不对等

PIO 的地标算子会淘汰一半种群，所以它每一代实际评估的次数比其他算法少。如果只比较迭代次数，PIO 看似优秀，但可能只是因为算得少。更公平的横轴是函数评估次数 FEs，也就是 pathCost 被调用的总次数。实现时可以在每个算法外面包一个计数器，每次调用代价函数就计数。

5.5 边界修复和精英保留做不好，新算法会“飞出去”

BKA 和 CO 的更新公式里经常出现较大的扰动步长，如果不做边界修复，很多个体会跑到地图外面，代价函数直接失效。PSO 还需要做速度限幅，否则粒子震荡发散。另外，所有算法都应该做精英保留：每一代把最优解单独存下来，即使种群里其他个体被随机扰动搅乱，最终输出依然是最优解。这个小动作能在不影响搜索的情况下大幅提高稳定性。

6. Matlab 代码组织方式与调参经验

6.1 文件怎么拆，测试才不混乱

整套代码我建议按功能拆成六个文件：init_problem.m 负责初始化地图和模型参数；path_cost.m 负责计算代价；bka_update.m、co_update.m、pso_update.m、pio_update.m 分别实现四种算法的更新逻辑；run_experiment.m 负责批量对比；plot_results.m 负责画收敛曲线和路径图。这样做的最大好处是无论替换代价函数还是增加新算法，改动范围都很小。

6.2 一套简单的批量实验脚本

批量实验脚本可以这样写：

rngBase = 2024; algos = {'BKA', 'CO', 'PSO', 'PIO'}; repeatTimes = 20; for a = 1:4 for r = 1:repeatTimes rng(rngBase + r, 'twister'); [bestCost, bestSol, curve] = runAlgorithm(algos{a}, model); result(a, r).cost = bestCost; result(a, r).curve = curve; end end

跑完以后把 result 保存成 mat 文件，再统一画图。这里我特意用了rngBase + r而不是每次都固定同一个数，是为了让同一轮重复的四个算法从同一个随机种子出发，但又有不同的随机轨迹。实验记录里要写清楚种子值，方便回看。

6.3 调参心得

调参这件事，我的原则是从少到多。先用默认参数跑一遍，观察收敛曲线有没有明显下降；如果前期就平，有可能是种群太小，或者初始解范围不对。PSO 的惯性权重最好线性下降，初值 0.9、终值 0.4 是通用起点；PIO 的地标阶段分界点不要固定为 MaxIt/2，可以试 0.4MaxIt 和 0.6MaxIt；CO 的精英替换比例我一般取 5% 到 10%，可以显著提升稳定度。

如果追求论文级的实验，不要为了某一张地图反复把参数调到最好，那样会过拟合到具体环境。更好的做法是取一组中等参数，在多个不同地图上测试，说明算法的一般性。另一个小技巧是收敛曲线的横轴换成 FEs，纵轴用 log 或者普通坐标都行，关键是每一条曲线都要是多次运行的平均值，否则锯齿会很大。

最后说点我个人操作习惯。我一般先用 PSO 快速跑一遍，看代价函数有没有明显 bug，再上 BKA 和 PIO 做正式对比；CO 因为随机性强，在需要多次重复实验时有点费时间。路径规划算法这件事，大多数情况下不是“哪个算法更高级”胜出，而是“哪个算法和你的代价函数匹配得更好”。你把代价函数的结构、惩罚系数的量级、边界处理都做扎实了，哪怕用经典 PSO 也能得到工程可用的路径。希望这篇整理能让你少踩几个坑，后面也能在自己的场景里跑出漂亮的集群路径。