多无人机协同航迹规划这词听起来挺唬人但拆开看就两件事一是让每架无人机安全穿过威胁区、精准落到目标点二是让多架无人机在共享空间里别互相打架、还能同时到位。我早期做单机规划的时候觉得在二维地图里找一条像样航迹已经够烦了等把问题升级到多机协同才发现难度完全不是一个量级。这篇笔记不聊虚的就把我从问题建模、代价函数设计、粒子群改进到Matlab代码落地的完整过程写出来里面包括我实际踩过的坑、调参的血泪经验以及可直接复现的代码思路。适合谁看如果你正在做无人机路径规划方向的课题或者刚接触粒子群算法、想把它用到带约束的实际项目里这篇笔记能帮你省掉大把试错时间。1. 项目整体设计与思路拆解1.1 单机规划升级到多机协同之后复杂度为什么陡增很多教程里单无人机航迹规划做得挺漂亮给定一张栅格地图从起点一路搜到终点A*、RRT都能出结果。可一旦引入多架无人机问题性质就变了不是说把单机规划重复M遍就行因为存在几个单机场景根本不会出现的硬约束机间防撞两架无人机在各自的最优航迹上可能共享同一片空域规划结果需要保证任意时刻各机之间的距离都大于安全阈值。时间协同多机协同任务里各机往往需要在同一时间窗口抵达目标区域单纯把航迹长度算出来远不够还要考虑速度调节后能否同时到达。维度灾难假设有M架无人机每架有N个自由控制点那这个粒子的维度就是M*N比单机问题直接大了M倍搜索空间扩大的速度比你想象中快得多。我以前拿单机场景的代码直接套多机结果跑出来的航迹每一条单独看都合理一叠加就出现交叉冲突后来才意识到协同约束必须直接写进优化目标里而不是指望事后排查。1.2 为什么选粒子群算法而不是A*、遗传算法航迹规划可用方法一大把我在选型的时候把几个主流方案摆在一起比了一轮方法优势劣势适合场景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.9wMin取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 改进粒子群完整流程把上面三块改进拼起来整个改进粒子群的执行流程是这样的读取地图、威胁区、无人机起终点数据用Tent混沌映射生成初始粒子群计算初始适应度初始化个体最优pbest和全局最优gbest主迭代循环按自适应策略计算每个粒子的惯性权重更新速度并限幅更新位置并做边界约束修正计算适应度更新pbest和gbest每5代执行子群间信息交换检测早熟条件若满足则触发混沌变异达到最大迭代次数后输出全局最优的航迹控制点用样条插值生成最终航迹绘制航迹图整个流程就是在一层层约束和目标函数的包裹下不断逼近可行、安全、平滑的航迹。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 i1: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改进PSO平均航迹总长度312公里285公里威胁代价0.280.09碰撞惩罚次数6次0次收敛到稳定的迭代代数89代47代终值标准差2.30.6标准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 调参速查表参数建议范围备注种群规模40~80多机问题至少40起步规模太小容易陷入局部最优最大迭代数150~300以收敛曲线进入平台期为判断依据惯性权重范围0.4~0.9wMax过大导致后期震荡wMax过小容易早熟学习因子c1、c21.5~2.0多机场景c2略大于c1强化全局最优引导碰撞安全距离5~10根据地图尺寸合理设定碰撞惩罚系数50~150过小约束失效过大航迹过度保守7. 后续可扩展方向这个项目的框架搭好以后后续扩展其实非常自然很多方向不用改动核心算法就能延伸。第一个方向是三维航迹规划。把控制点坐标从二维扩展成三维适应度函数里增加高度代价项比如限制最大爬升角度、禁止超低空飞行等就能从平面绕行升级为立体空域规划更贴近实际任务场景。第二个方向是动态威胁环境。当前威胁源位置固定属于离线规划范畴。如果无人机在飞行过程中通过传感器发现新的威胁源就需要引入在线重规划机制把改进PSO作为局部重规划器周期性触发。第三个方向是和差分进化、遗传算法做混合。粒子群收敛快但容易早熟遗传算法全局搜索能力强但收敛慢两者结合可以让局部搜索和全局探索的切换更自然。在做多机场景时混合算法往往能在复杂威胁分布下找到更优解。回头说一个我自己调试这个项目时的小心得。多无人机协同航迹规划表面上是算法问题、代码问题实际上最难的是约束建模。你把代价函数和约束罚项设计得越贴近物理实际算法收敛后得到的航迹就越能用这一步花的时间往往比调算法参数还要多。建议你动手写代码之前先拿纸把每种约束的数学表达式画清楚再进Matlab逐段实现比我当初着急写完再回头改要高效得多。 
长河君 · 2026/10/21 9:41:22 · 阅读 1.2k 多无人机协同航迹规划：改进粒子群算法与Matlab实现
多无人机协同航迹规划这词听起来挺唬人但拆开看就两件事一是让每架无人机安全穿过威胁区、精准落到目标点二是让多架无人机在共享空间里别互相打架、还能同时到位。我早期做单机规划的时候觉得在二维地图里找一条像样航迹已经够烦了等把问题升级到多机协同才发现难度完全不是一个量级。这篇笔记不聊虚的就把我从问题建模、代价函数设计、粒子群改进到Matlab代码落地的完整过程写出来里面包括我实际踩过的坑、调参的血泪经验以及可直接复现的代码思路。适合谁看如果你正在做无人机路径规划方向的课题或者刚接触粒子群算法、想把它用到带约束的实际项目里这篇笔记能帮你省掉大把试错时间。1. 项目整体设计与思路拆解1.1 单机规划升级到多机协同之后复杂度为什么陡增很多教程里单无人机航迹规划做得挺漂亮给定一张栅格地图从起点一路搜到终点A*、RRT都能出结果。可一旦引入多架无人机问题性质就变了不是说把单机规划重复M遍就行因为存在几个单机场景根本不会出现的硬约束机间防撞两架无人机在各自的最优航迹上可能共享同一片空域规划结果需要保证任意时刻各机之间的距离都大于安全阈值。时间协同多机协同任务里各机往往需要在同一时间窗口抵达目标区域单纯把航迹长度算出来远不够还要考虑速度调节后能否同时到达。维度灾难假设有M架无人机每架有N个自由控制点那这个粒子的维度就是M*N比单机问题直接大了M倍搜索空间扩大的速度比你想象中快得多。我以前拿单机场景的代码直接套多机结果跑出来的航迹每一条单独看都合理一叠加就出现交叉冲突后来才意识到协同约束必须直接写进优化目标里而不是指望事后排查。1.2 为什么选粒子群算法而不是A*、遗传算法航迹规划可用方法一大把我在选型的时候把几个主流方案摆在一起比了一轮方法优势劣势适合场景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.9wMin取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 改进粒子群完整流程把上面三块改进拼起来整个改进粒子群的执行流程是这样的读取地图、威胁区、无人机起终点数据用Tent混沌映射生成初始粒子群计算初始适应度初始化个体最优pbest和全局最优gbest主迭代循环按自适应策略计算每个粒子的惯性权重更新速度并限幅更新位置并做边界约束修正计算适应度更新pbest和gbest每5代执行子群间信息交换检测早熟条件若满足则触发混沌变异达到最大迭代次数后输出全局最优的航迹控制点用样条插值生成最终航迹绘制航迹图整个流程就是在一层层约束和目标函数的包裹下不断逼近可行、安全、平滑的航迹。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 i1: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改进PSO平均航迹总长度312公里285公里威胁代价0.280.09碰撞惩罚次数6次0次收敛到稳定的迭代代数89代47代终值标准差2.30.6标准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 调参速查表参数建议范围备注种群规模40~80多机问题至少40起步规模太小容易陷入局部最优最大迭代数150~300以收敛曲线进入平台期为判断依据惯性权重范围0.4~0.9wMax过大导致后期震荡wMax过小容易早熟学习因子c1、c21.5~2.0多机场景c2略大于c1强化全局最优引导碰撞安全距离5~10根据地图尺寸合理设定碰撞惩罚系数50~150过小约束失效过大航迹过度保守7. 后续可扩展方向这个项目的框架搭好以后后续扩展其实非常自然很多方向不用改动核心算法就能延伸。第一个方向是三维航迹规划。把控制点坐标从二维扩展成三维适应度函数里增加高度代价项比如限制最大爬升角度、禁止超低空飞行等就能从平面绕行升级为立体空域规划更贴近实际任务场景。第二个方向是动态威胁环境。当前威胁源位置固定属于离线规划范畴。如果无人机在飞行过程中通过传感器发现新的威胁源就需要引入在线重规划机制把改进PSO作为局部重规划器周期性触发。第三个方向是和差分进化、遗传算法做混合。粒子群收敛快但容易早熟遗传算法全局搜索能力强但收敛慢两者结合可以让局部搜索和全局探索的切换更自然。在做多机场景时混合算法往往能在复杂威胁分布下找到更优解。回头说一个我自己调试这个项目时的小心得。多无人机协同航迹规划表面上是算法问题、代码问题实际上最难的是约束建模。你把代价函数和约束罚项设计得越贴近物理实际算法收敛后得到的航迹就越能用这一步花的时间往往比调算法参数还要多。建议你动手写代码之前先拿纸把每种约束的数学表达式画清楚再进Matlab逐段实现比我当初着急写完再回头改要高效得多。