企业官网建设流程全解析

多无人机协同航迹规划这词听起来挺唬人，但拆开看就两件事：一是让每架无人机安全穿过威胁区、精准落到目标点，二是让多架无人机在共享空间里别互相打架、还能同时到位。我早期做单机规划的时候，觉得在二维地图里找一条像样航迹已经够烦了，等把问题升级到多机协同，才发现难度完全不是一个量级。这篇笔记不聊虚的，就把我从问题建模、代价函数设计、粒子群改进到Matlab代码落地的完整过程写出来，里面包括我实际踩过的坑、调参的血泪经验，以及可直接复现的代码思路。

适合谁看？如果你正在做无人机路径规划方向的课题，或者刚接触粒子群算法、想把它用到带约束的实际项目里，这篇笔记能帮你省掉大把试错时间。

1. 项目整体设计与思路拆解

1.1 单机规划升级到多机协同之后，复杂度为什么陡增

很多教程里单无人机航迹规划做得挺漂亮：给定一张栅格地图，从起点一路搜到终点，A*、RRT都能出结果。可一旦引入多架无人机，问题性质就变了，不是说把单机规划重复M遍就行，因为存在几个单机场景根本不会出现的硬约束：

• 机间防撞：两架无人机在各自的最优航迹上可能共享同一片空域，规划结果需要保证任意时刻各机之间的距离都大于安全阈值。
• 时间协同：多机协同任务里，各机往往需要在同一时间窗口抵达目标区域，单纯把航迹长度算出来远不够，还要考虑速度调节后能否同时到达。
• 维度灾难：假设有M架无人机，每架有N个自由控制点，那这个粒子的维度就是M*N，比单机问题直接大了M倍，搜索空间扩大的速度比你想象中快得多。

我以前拿单机场景的代码直接套多机，结果跑出来的航迹每一条单独看都合理，一叠加就出现交叉冲突，后来才意识到协同约束必须直接写进优化目标里，而不是指望事后排查。

1.2 为什么选粒子群算法而不是A*、遗传算法

航迹规划可用方法一大把，我在选型的时候把几个主流方案摆在一起比了一轮：

最终选粒子群，本质原因是航迹规划的目标函数没有梯度信息，而且粒子在连续空间里飞行的更新方式，跟航迹控制点的数值优化结构天然契合。你不需要设计复杂的编码解码方案，直接把一组控制点坐标拼成一个粒子向量，适应度函数一算就能迭代。

1.3 标准粒子群的两大痛点，改进方向从这里出发

标准粒子群（PSO）虽然好用，但我在多机场景里很快就发现两个致命问题。

第一是早熟收敛。所有粒子都朝着一个全局最优的位置压过去，一旦这个全局最优是局部最优，整个种群就困在里面出不来，航迹会明显绕远路。第二是后期收敛慢。标准PSO在迭代后期粒子速度衰减，精细搜索能力不够，想提升航迹平滑度很吃力。

这两点正好对应我在符号【改进粒子群】里做的三件事：动态惯性权重调节探索与开发；混沌初始化让初始种群分布更均匀；早熟检测加变异策略把困住的粒子踢出局部最优。后面第三节我会逐个讲实现细节。

2. 航迹规划建模与目标函数设计

2.1 飞行环境建模与威胁区处理

先把仿真环境抽象出来，我用的连续坐标系，地图范围100公里乘100公里，威胁区用圆形威胁源模型建模，每个威胁源有圆心坐标和威胁半径，对应雷达探测区或者防空火力区。

粒子穿越威胁区时，威胁代价与无人机到威胁源距离成反比。这样处理比简单判定“是否落在威胁区”更合理，因为真实雷达信号强度本来就是随着距离衰减的。

我看很多新手在这里容易犯一个错：把地图栅格化得特别细，导致粒子维度爆炸，收敛极慢。其实连续坐标建模更贴近无人机航迹规划的本质，而且粒子群算法本来就是在连续空间里做的，没必要强行套栅格。

2.2 单机航迹代价函数的设计

每架单机的代价函数我分成了三块：航程代价、威胁代价、平滑代价。

航程代价就是所有相邻控制点之间的欧氏距离之和，这个很好理解，无人机飞得越短越省油。威胁代价则是沿航迹均匀采样若干点，计算每个采样点到各威胁源的距离，如果距离小于威胁半径就累加惩罚值：

function threatCost = calThreatCost(trackPoints, threatList) n = size(trackPoints, 1); threatCost = 0; for i = 1:n p = trackPoints(i, :); for j = 1:size(threatList, 1) c = threatList(j, 1:2); r = threatList(j, 3); d = norm(p - c); if d < r threatCost = threatCost + (r - d) / r; end end end threatCost = threatCost / n; end

平滑代价算的是相邻三段航迹之间的夹角变化，转弯越剧烈惩罚越大。很多做规划的人只看航程和威胁，结果跑出来的航迹弯来弯去跟心电图似的，无人机根本没法按这个航线飞，所以平滑约束必须从一开始就写进目标函数里。

2.3 协同约束怎么融入目标函数，这里有个关键设计

多机协同的难点就在于把碰撞约束和时间协同约束科学地塞进粒子群优化的目标函数。我的做法是罚函数法，对不满足约束的个体施加高额惩罚，让粒子群自然避开不可行区域。

机间防撞检测的具体做法是：对每条航迹采样等间隔的点序列，计算任意两架无人机在同一时刻采样点的空间距离，只要有任何一对距离小于安全阈值，就累计一个碰撞惩罚项。

时间协同的处理更巧妙一点，无人机实际速度可调，航迹规划阶段只需要保证各机航迹长度在可允许的同步速度范围内。我会加一个同时到达代价项，计算各机航迹长度方差，方差越小越容易通过速度控制实现同时到达。这个做法比直接固定各机速度要灵活得多。

2.4 粒子编码方式，一条航迹如何映射成粒子位置

粒子维度设计是整个项目最核心的一步。假设有M架无人机，每架无人机航迹由K个控制点构成，起点和终点固定不动，中间K-2个点是可优化变量。每个控制点有二维坐标，那么每个粒子的维度就是M*(K-2)*2。

粒子位置向量的物理意义是所有无人机中间控制点坐标拼接成的长向量。相邻控制点之间用三次样条插值细化，得到平滑航迹。

3. 改进粒子群算法的核心实现

3.1 标准PSO公式回顾与参数敏感性

先过一遍标准粒子群的更新公式，因为后面所有改进都是在这个基础之上叠加的：

v(i,:) = w * v(i,:) + c1 * rand * (pbest(i,:) - x(i,:)) + c2 * rand * (gbest - x(i,:)); x(i,:) = x(i,:) + v(i,:);

w是惯性权重，c1和c2是学习因子，前一项衡量粒子保持原有速度的趋势，后两项分别是向个体历史最优位置和全局最优位置学习的程度。w大粒子飞得散，全局搜索能力强但难以精细收敛；w小则相反，局部搜索能力强但容易陷进去。

这个公式看似简单，实际参数敏感度极高。我见过不少同学直接把w设成0.6就开跑，结果多机场景下粒子飞了几十代就缩成一团，后面怎么迭代都没变化。

3.2 改进一：自适应惯性权重，让粒子该飞散时飞散、该收敛时收敛

第一步改进是做惯性权重的动态调节，核心思想是让惯性权重随迭代进程和粒子适应度变化：迭代早期权重偏大，保持种群探索能力；迭代后期权重偏小，让粒子围绕最优解精细搜索。

具体实现我试过两种策略。第一种是线性递减：

w = wMax - (wMax - wMin) * (iter / maxIter);

这个最简单，wMax取0.9，wMin取0.4，实测下来整体表现稳定。第二种是适应度排序自适应权重，对每一代粒子的适应度排序，适应度好的粒子给较小权重，保护其精细搜索能力，适应度差的粒子给较大权重，强迫它们扩大探索范围。这个比线性递减更聪明，但逻辑复杂度高一些。

我的经验是先实现线性递减，跑通整个流程后再换成自适应版本，这样调试时能快速定位问题出在算法框架还是参数选择上。

3.3 改进二：混沌初始化与早熟变异机制

标准PSO用随机数生成初始种群，随机序列有一个隐蔽问题：分布可能不均匀，粒子扎堆在某个区域，导致前期搜索方向就偏了。这里我引入了混沌映射做初始化。混沌序列有随机序列不具备的遍历性和均匀性，能让初始粒子更均匀地铺满整个解空间。

Tent映射的表达式是：

% Tent映射生成混沌序列 function x = tentMap(seed, n) x = zeros(1, n); x(1) = seed; for i = 2:n if x(i-1) < 0.5 x(i) = 2 * x(i-1); else x(i) = 2 * (1 - x(i-1)); end end end

注意用Tent映射要避开0和0.5这两个固有点，否则序列就卡住了。混沌初始化后，再把混沌序列线性映射到控制点坐标的可取范围内。

早熟变异机制的设计思路是监控种群状态，当检测到全局最优连续若干代没有改善，或者种群适应度方差变得很小，就判断为早熟，触发变异操作。变异方式是把一部分表现差的粒子的位置用混沌序列重新生成，但保留它们的历史最优pbest位置。这样既保持种群多样性，又不丢失已经搜索到的信息。

3.4 改进三：多子群协同与信息交互拓扑

第三个改进方向参考了多子群协同的思路。标准PSO所有粒子共享一个全局最优，信息传递太快，多样性格局容易早早崩塌；而每个粒子只和邻居交换信息的环形拓扑，收敛慢但多样性保持好。

我的实现是主从双子群结构：主群负责全局搜索，从群负责局部精细搜索。两个子群各自维护局部最优，每隔几次迭代互相交换一次最优位置信息，相当于在探索能力和开发能力之间做平衡。

这个策略在多无人机场景里实测增益明显，尤其是威胁区障碍比较多的时候，标准PSO容易所有粒子绕到同一侧，多子群协同能找到不同的可行绕行通道，最后选出全局最优的那条。

3.5 改进粒子群完整流程

把上面三块改进拼起来，整个改进粒子群的执行流程是这样的：

1. 读取地图、威胁区、无人机起终点数据
2. 用Tent混沌映射生成初始粒子群，计算初始适应度
3. 初始化个体最优pbest和全局最优gbest
4. 主迭代循环：
   • 按自适应策略计算每个粒子的惯性权重
   • 更新速度并限幅，更新位置并做边界约束修正
   • 计算适应度，更新pbest和gbest
   • 每5代执行子群间信息交换
   • 检测早熟条件，若满足则触发混沌变异
5. 达到最大迭代次数后输出全局最优的航迹控制点
6. 用样条插值生成最终航迹，绘制航迹图

整个流程就是在一层层约束和目标函数的包裹下不断逼近可行、安全、平滑的航迹。

4. Matlab工程实现细节

4.1 工程文件结构与主框架

写Matlab代码之前先把工程结构理清楚，不然随着函数增多会乱成一团。我最终的项目文件组织方式是：

• main_run.m：主脚本，设置仿真环境并调用算法
• initMap.m：地图与威胁区初始化
• initPSO_chaos.m：混沌初始化粒子群
• improvedPSO_iter.m：改进粒子群主迭代循环
• calFitnessMulti.m：多机协同适应度计算
• splineSmooth.m：三次样条插值平滑航迹
• plotRoutes.m：航迹与威胁区可视化

4.2 主脚本与适应度函数的关键代码

主脚本里最核心的部分就是把仿真参数定义清楚，一段能直接跑通的框架长这样：

%% 环境与任务参数 mapRange = [0, 100, 0, 100]; % 地图范围 threatList = [20, 30, 12; 70, 65, 15; 50, 20, 10]; % 威胁圆心坐标和半径 startPos = [10, 10, 10, 80, 20, 60]; % 三机起点 targetPos = [90, 90, 90, 15, 85, 40]; % 三机终点 safeDist = 5; % 机间安全距离 %% PSO参数 nPop = 60; % 种群数 maxIter = 200; % 最大迭代数 dim = 3 * 5 * 2; % 3架无人机，每架5个中间控制点，二维坐标

适应度函数是整个算法的核心，里面包含单机代价和多机协同惩罚：

function fitVal = calFitnessMulti(x, threatList, startPos, targetPos, safeDist) M = 3; % 无人机数量 Kc = 5; % 中间控制点数量 % 解析粒子向量：M * (Kc*2) 的矩阵 tracks = reshape(x, M, Kc*2); totalLenCost = 0; totalThreatCost = 0; totalSmoothCost = 0; samplePoints = zeros(M, 50, 2); % 每条航迹采样50个点用于碰撞检测 for m = 1:M pts = [startPos(m,:); reshape(tracks(m,:), Kc, 2); targetPos(m,:)]; smoothPts = splineSmooth(pts, 50); % 样条插值得到50个点 totalLenCost = totalLenCost + sum(sqrt(sum(diff(smoothPts,1).^2, 2))); totalThreatCost = totalThreatCost + calThreatCost(smoothPts, threatList); totalSmoothCost = totalSmoothCost + calSmoothCost(pts); samplePoints(m, :, :) = smoothPts; end % 机间碰撞惩罚 collisionPenalty = 0; for i = 1:M for j = i+1:M dists = sqrt(sum((squeeze(samplePoints(i,:,:)) - squeeze(samplePoints(j,:,:))).^2, 2)); collisionPenalty = collisionPenalty + sum(max(0, safeDist - dists)); end end fitVal = 0.4 * totalLenCost + 0.3 * totalThreatCost + 0.15 * totalSmoothCost + 100 * collisionPenalty; end

这段代码有两点值得注意。第一是totalLenCost和totalThreatCost的数量级差异，直接把权重定为0.4和0.3之前，必须先单独调试观察各分量原本的量级，否则可能一个分量的权重虽小，实际数值却绝对主导了整个适应度函数。第二是碰撞惩罚的系数，我取的是100，太小则粒子会无视碰撞去追求更短航程，太大则整个搜索过程会变得极其保守，航迹会绕得离谱。

4.3 速度更新与边界约束处理

粒子速度更新时，直接限制速度的范围很重要。我把速度限幅设置为控制点坐标范围的20%，防止粒子飞出地图后再反弹回来。

v(i, :) = w * v(i, :) + c1 * rand * (pbest(i, :) - x(i, :)) + c2 * rand * (gbest - x(i, :)); v(i, :) = max(min(v(i, :), velMax), -velMax); x(i, :) = x(i, :) + v(i, :); x(i, :) = max(min(x(i, :), xMax), xMin);

边界约束处理我踩过一个坑：早期直接用截断法，粒子撞到边界就强制放到边界上，结果边界处聚集了大量粒子，变成一种隐性的局部最优。后来改成越界后随机反弹，效果好了很多。

4.4 可视化与结果呈现

多无人机航迹规划如果没有合适的可视化，调试效率会很低。我每次跑完都会画三张图：第一张是三条航迹和威胁区圆分布图，能看到整体绕行效果；第二张是收敛曲线，用来判断算法有没有早熟；第三张是粒子间距随时间变化的曲线，专门观察碰撞约束有没有被满足。

5. 实验设计与结果对比

5.1 仿真场景设定

我用三架无人机做验证场景。地图100乘100公里，三架无人机起点分别在地图三个角，目标点分布在另一侧，中间夹着三个圆形威胁区，保证航线必然需要绕行。

每架无人机设置5个中间控制点，三次样条插值后生成50个航迹采样点。粒子群参数设成种群规模60，迭代次数200，安全距离设为5公里。

5.2 标准PSO与改进PSO的对比

标准PSO跑出来的航线虽然整体也能到终点，但存在两个问题：部分航迹从威胁区边缘擦过，威胁代价偏高；收敛曲线在40代以后几乎平直，明显早熟。改进PSO的收敛速度更快，而且由于混沌变异机制的存在，中后期依然能跳出局部最优，找到更平滑、威胁代价更小的航迹。

这套对比结果在我的多项试验里保持一致，改进粒子群的航迹往往绕行更为合理，不会出现贴着威胁区边缘飞行或急速转弯这类危险动作。

5.3 参数敏感性分析

我还做了一组控制变量实验，分别测试种群规模、迭代次数、惯性权重范围对结果的影响。种群规模从20加到60，结果稳定性明显提升，继续加到100时收益递减。迭代次数在200代就能收敛到稳定解，加到500代主要收益是航迹平滑度微升，但计算时间翻倍。

惯性权重范围的影响最敏感，wMax取0.9、wMin取0.4时结果最优。wMax过小容易早熟，wMax过大会导致粒子乱飞，后期收敛困难。这些参数经验可以直接复用，但换地图后建议重新跑一遍小范围参数扫描。

6. 踩坑记录与调参经验

6.1 粒子集体飞出地图边界，连有效航迹都找不到

这是我第一次跑多机版本遇到的第一个严重问题。当时把威胁代价权重设得极高，粒子只要进入威胁区就被罚得头破血流，于是大量粒子干脆往地图边缘跑，冲出边界后又被随机拉回，整个种群几乎无法形成有效搜索。

解决思路是在罚函数的设计上给一个渐进梯度，而不是一进威胁区就重罚。威胁区边缘的小幅穿越给一个小惩罚，深入核心区才给重罚，这样粒子能逐步探索出绕行路径，而不是一味逃离。

6.2 碰撞惩罚项让优化地形变得极端

多机碰撞约束本质是一个高度非连续的函数，你在地图上稍微挪一点距离，碰撞从无到有，惩罚可能从0跳到几百，适应度地形变得非常陡峭。粒子一旦尝到碰撞惩罚的苦头，就会躲到距离其他航迹非常远的区域，导致整体航迹变成外圈大绕行。

后来我从两个角度做了缓解：一是把碰撞惩罚从硬判定的阶跃函数改成连续的探测距离函数，比如距离小于安全阈值时，罚值随距离线性增长而不是瞬间爆表；二是先跑一遍完全不考虑碰撞约束的版本，把每架无人机的大致航迹区域确定下来，再加入碰撞约束精调，相当于分阶段优化。

6.3 收敛曲线像是心电图，一直在跳不下降

这个现象往往出现在混沌变异触发太频繁的场景里。早熟检测的阈值设得过于敏感，刚找到一点下降趋势就触发了变异，把表现好的粒子扰动掉。

我的经验是早熟检测连续代数至少设到15代，并且变异只作用于表现排名后30%的粒子，前70%的粒子位置完全不动。这样既保留搜索成果，又给种群注入新鲜多样性。

6.4 调参速查表

7. 后续可扩展方向

这个项目的框架搭好以后，后续扩展其实非常自然，很多方向不用改动核心算法就能延伸。

第一个方向是三维航迹规划。把控制点坐标从二维扩展成三维，适应度函数里增加高度代价项，比如限制最大爬升角度、禁止超低空飞行等，就能从平面绕行升级为立体空域规划，更贴近实际任务场景。

第二个方向是动态威胁环境。当前威胁源位置固定，属于离线规划范畴。如果无人机在飞行过程中通过传感器发现新的威胁源，就需要引入在线重规划机制，把改进PSO作为局部重规划器周期性触发。

第三个方向是和差分进化、遗传算法做混合。粒子群收敛快但容易早熟，遗传算法全局搜索能力强但收敛慢，两者结合可以让局部搜索和全局探索的切换更自然。在做多机场景时，混合算法往往能在复杂威胁分布下找到更优解。

回头说一个我自己调试这个项目时的小心得。多无人机协同航迹规划表面上是算法问题、代码问题，实际上最难的是约束建模。你把代价函数和约束罚项设计得越贴近物理实际，算法收敛后得到的航迹就越能用，这一步花的时间往往比调算法参数还要多。建议你动手写代码之前，先拿纸把每种约束的数学表达式画清楚，再进Matlab逐段实现，比我当初着急写完再回头改要高效得多。