无人机集群路径规划：五种智能优化算法对比与Matlab实现

无人机集群路径规划最近问的人确实多尤其是想用智能优化算法跑三维航迹的同学动不动就要求对比五六种算法再加Matlab代码。这类问题看着唬人其实拆开之后就是三件事怎么建模、用什么算法搜、怎么写代码把结果画出来。这篇我想结合一份同时跑SFOA、APO、GOOSE、CO、PIO五种算法的无人机集群路径规划工程把从问题建模到代码实现再到结果对比和常见坑完整讲一遍。先回答一个大家最常问的问题为什么非要对比这么多种算法原因不复杂。无人机集群路径规划本质是一个带约束的多目标优化问题无人机数量多一点、地图复杂一点传统规划方法就非常吃力。群智能优化算法的好处是不需要梯度信息能在一个巨大的搜索空间里不断逼近可行解。但不同算法的全局搜索和局部开发能力差异很大有的在简单场景里收敛飞快一进复杂威胁区就早熟有的前期探索强但后期收敛慢。单独拿一种算法说它最优基本没有说服力放在同一个地图、同一个适应度函数、同样迭代次数下横向对比才能真正看出各自的本事。这篇内容适合正在做无人机路径规划课题、需要跑对比实验拿图表写论文的人也适合刚入门Matlab、想快速理解优化算法和航迹规划怎么结合的同学。看完你能知道代码每个模块在干什么、五种算法各自的思路和实现要点、实验中容易踩的坑以及怎么把你自己的场景塞进去跑通。1. 场景建模无人机集群路径规划到底在解什么1.1 飞行代价不只是“距离最短”很多人一上来就想着用算法找最短路径但真实无人机集群规划绝对不是只找最短。飞行距离短当然重要但代价函数里通常至少要包含三部分航迹长度代价、威胁代价、平滑代价。航迹长度代价很好理解就是相邻两个航迹点之间的距离累加能保证规划的路径尽量经济。威胁代价是无人机在飞行过程中进入威胁区或禁飞区的惩罚。实际战场或城市任务里威胁区可能代表雷达探测范围、防空火力覆盖区、障碍物区域这些区域概化成一个或者多个球体或圆柱体。如果航迹点落在威胁范围内或者两个航迹点之间的连线穿过威胁范围就要增加惩罚。平滑代价则约束无人机不能频繁大角度转弯因为固定翼无人机有最小转弯半径旋翼机虽然能悬停但频繁转向也意味着能量损耗和姿态调整成本。把这三项加权求和就得到了一个完整的适应度函数。这里有必要提醒一句权重系数并不是一拍脑袋定的。距离权重和威胁权重如果失衡常见的结果就是算法费了半天劲最后得出一条贴着威胁区边缘走的理论安全路径实际飞行时一个扰动就进去了。我在实验中习惯先把威胁代价的基准值放大到和距离代价同一个量级再微调惩罚系数这样收敛稳定很多。1.2 约束条件威胁区、禁飞区、机间防撞单机路径规划只需要考虑威胁区和自身性能约束集群规划多了一个最难处理的约束就是机间防撞。多架无人机同时起飞如果直接各飞各的很容易在某个时间段扎堆在某一片空域里。机间防撞在代价函数里的实现比较直接每一代更新完位置后检查任意两架无人机的对应航迹点之间的三维欧氏距离。如果这个距离小于设定的安全间距就增加一个很大的惩罚值。安全间距通常设置为旋翼直径的几倍或者根据任务需求直接给一个固定值比如20米、30米。注意这里检查的是同一航迹点序号上的两机距离而不是整条路径上任意时刻的距离因为路径规划时所有无人机的航迹点数量是相同的一一对应就可以近似表示同一时间切片的间距。这种方法严格来说是一种近似但对于大多数工程验证来说已经够用。另外还要考虑无人机本身的物理约束比如最大转弯角、最大爬升角、最大飞行速度。如果在代价函数里不限制算法可能规划出锯齿状航迹让人看着就想摇头。这部分通常加在平滑代价里用相邻三个点的夹角来约束。1.3 编码方式从航迹点矩阵到适应度接下来是编码问题。假设有 N 架无人机每条路径由 D 个航迹点组成那么一个个体就是 N × D 的矩阵矩阵里每个元素代表一个三维坐标点。实际Matlab实现时经常把三维坐标拆开用一个向量表示所有无人机的所有坐标再用 reshape 还原成矩阵。我见过不少同学在编码这里绕晕其实理解起来很简单算法每生成一个新位置向量就把它 reshape 成 [N, D, 3] 的三维数组第一维是无人机编号第二维是航迹点编号第三维是x、y、z三个坐标。这样代价函数接收一个个体向量reshape之后就能很方便地做距离计算、威胁检测、间距检测。起点和终点通常是固定的不需要算法去搜索所以真正被优化的只是中间航迹点。初始化时可以把中间点在起点到终点的直线连线附近做随机偏移这样初始种群就带有一定的基础可行性算法收敛速度快不少。如果初始点完全随机撒在一个大空间里你大概率会看到前面几十代代价居高不下白白浪费迭代次数。2. 五种优化算法的机制差异与Matlab实现要点2.1 SFOA借太阳朝向做全局寻优SFOA在很多资料里写作向日葵优化算法核心思想是模拟向日葵在生长过程中始终朝向太阳、同时避免个体之间过度遮挡的行为。它的搜索策略有种中心开花的感觉种群中每一株向日葵都会有一个朝向最优解的方向同时个体之间保持着一定的排斥距离防止重叠。在Matlab里实现SFOA时最需要注意的就是方向向量更新。SFOA里每个个体不仅依赖全局最优还依赖个体之间的间距调节这个间距因子如果设置得太小所有个体很快聚集到同一个点探索能力丧失设置得太大又会拖慢收敛速度。我的建议是先保持默认参数用简单测试函数观察种群多样性再根据你自己的地图比例微调那个间距常数。另外SFOA对边界处理的写法比较敏感位置更新后必须把越界坐标拉回边界范围内否则后面迭代可能出现NaN。2.2 APO双阶段搜索的平衡设计APO这个名字在多个仓库里对应过不同算法我这里采用的是北极海鹦优化。这种算法模拟海鹦在海上长距离飞行寻找鱼群、再俯冲入水捕食的过程天然就有两个阶段全局广域探索和局部深度开采。长距离飞行阶段用的是大步长随机扰动让个体能跳到搜索空间中较远的位置入水捕食阶段则在小范围内精细搜索步长逐渐缩小。这种双阶段结构对路径规划问题非常友好。三维航迹规划最优解的形态可能是一条贴着威胁区边缘绕行的曲线也可能是一条相对平直但稍远的路径最优解可能藏在地图上不太起眼的区域里。APO前期的广域搜索能把种群带到有希望的区域后期缩小步长则负责在局部细化路径。实现时阶段切换通常用一个自适应系数来控制比如迭代次数占比前一半偏探索、后一半偏开发。要注意的坑是阶段切换太生硬会导致收敛曲线出现明显的断层跳跃建议用连续递减的步长因子过渡平滑一些。2.3 GOOSE编队防守策略的启发GOOSE算法的灵感来自鹅群的防守行为这两年相关论文热度挺高。它比较特别的地方在于将种群的搜索过程分成了防守和攻击两种模式。防守模式下种群个体模拟鹅群在警戒状态下的响应行为保持警觉并进行位置调整攻击模式下模拟群体驱赶入侵者的集体行为个体之间协同逼近目标。这个两阶段群体协同的机制放在无人机集群上很有代入感。无人机编队本身就是群体协同GOOSE里个体之间会互相参考位置信息形成一种类似编队保持的搜索方式。实现GOOSE时一个常见问题是参数里有两个记忆矩阵需要维护一个是每个个体自身的历史信息另一个是群体共享的协同信息。如果协同信息的更新频率太高容易让种群过早统一意见陷入局部最优频率太低算法又有点各跑各的。我在调试时会把协同更新的权重调低一点让种群保持足够的多样性后期再逐步提高协同权重。2.4 CO种群社会文化更新机制CO这个缩写同样有重名情况在这次的工程里指的是郊狼优化算法模拟郊狼群体的社会文化、成长和迁徙行为。它和一般粒子群最大的区别是个体不直接拿最优个体替换自己而是通过种群内的文化差异来影响每个临时个体的生成。换句话说每只郊狼不是单纯向最好的那只学习而是综合考虑群体里的多样性和社会结构生成一种更稳妥的更新方式。这种机制让CO在面对多模态、多约束问题时不容易早早丧失多样性。无人机路径规划里威胁区分布不均匀经常存在多个局部看起来不错的走廊传统粒子群很容易全部涌向其中一个。CO因为更新规则相对保守种群成员分布比较分散反倒更有可能在后期发现更优路径。Matlab实现时郊狼优化需要维护分组结构每一代里个体属于不同组组内产生文化倾向因子。分组的数量会直接影响多样性建议按种群规模的一半左右来设置组数别让一个组里个体太多。2.5 PIO两个经典算子如何接力搜索PIO鸽群优化是这里面最经典的算法很多做航迹规划的人第一个想到的就是它。鸽群优化有两个核心算子地图罗盘算子和地标算子。地图罗盘算子模拟鸽子通过太阳和地磁感知方向返回巢穴的能力在迭代前期负责大范围搜索地标算子模拟鸽子接近目的地时依靠熟悉地标进行精细导航的行为在迭代后期负责局部收敛。PIO实现起来逻辑非常清晰但需要仔细处理两个算子的切换。简单做法是设定一个迭代比例阈值比如前70%用地图罗盘算子后30%用地标算子。不过我更推荐动态切换让地标算子的作用逐步增强避免前期搜得不够充分就过早进入局部细化。另一个细节是地标算子中种群规模会随着迭代逐渐减半因为更靠近目标的鸽子会作为地标引导其他个体学习因子也要跟着调整。PIO的收敛速度通常很快但在复杂威胁区场景里容易掉进局部最优所以有人会把PIO作为后期局部搜索嵌入到其他算法里混合并行使用效果往往比单独跑更好。3. 代码框架解析一份可直接跑的Matlab工程3.1 主程序执行流程这类Matlab工程的代码框架通常很统一主程序大致可以拆成六个模块参数设置、地图与威胁区初始化、种群初始化、主循环迭代、结果输出、可视化绘图。第一次拿到代码不要急着逐行读。先找到参数设置区把地图尺寸、无人机数量、航迹点数、威胁区信息、种群规模、最大迭代次数这些改清楚然后直接跑一遍看效果。我自己的习惯是先不改算法用默认参数跑通确认能出三维路径图和收敛曲线之后再动代码逻辑。如果跑通都费劲说明初始化或者维度设置有地方对不上这时候别继续往下调参。主循环部分结构基本一致每个迭代步里对当前种群中的每个个体做位置更新、边界处理、计算适应度、更新历史最优和全局最优。五种算法在这个框架里唯一的区别就是位置更新那一小段也就是算法核心机制对应的Matlab代码。所以代码里通常会把更新策略封装成不同的函数或分支你换算法时只需要切换函数句柄代价函数完全不用动。3.2 代价函数是算法能否收敛的“定盘星”代价函数是整个工程里最重要的部分。很多同学跑出来的结果很离谱比如路径直接穿过威胁区但代价却很好看十有八九是代价函数里威胁代价没写好。威胁代价比较规范的做法是对每条航迹段的若干插值点逐一检测判断是否落入某个威胁区。如果只检测航迹点本身很可能出现航迹点刚好在威胁区外、但连线穿过威胁区的情况。航迹插值检测就是在相邻两点之间均匀插出若干个中间点再对这些点逐一判断。插值密度越大检测越精确但计算量也会上来。在Matlab里用 linspace 插值再配合 for 循环检测是最直接的方式不用担心效率问题路径规划的地图通常不大。平滑代价实现时要用到航向角和爬升角的计算。简单做法是遍历每个连续三点组计算三点构成的角度变化量如果角度变化超过阈值则按差值平方累加惩罚。这里需要强调一个细节角度要用弧度再换算成度很多bug都是单位混用造成的。机间防撞代价则是在所有无人机的当前路径上计算两两距离矩阵这里可以提前预分配矩阵别在循环里反复动态扩容不然数据量大时慢得让人怀疑人生。3.3 三种可视化结果的读取方法Matlab工程跑完通常会输出三张图三维航迹图、二维俯视图、收敛曲线图。三维航迹图是主图会把地图边界、威胁区球体/圆柱、每架无人机的航迹用不同颜色画出来直观展示路径是否绕开了威胁区、多机路径是否有交叉。二维俯视图更适合快速检查路径在水平面上的分布尤其是威胁区投影之后一眼就能看出路径是否贴着危险边界。收敛曲线图横轴是迭代次数纵轴是每代最优适应度值用来观察算法收敛的速度和最终精度。看三维图时建议顺手打开Rotate 3D工具转几圈正面看路径可能觉得没问题换个角度可能发现无人机爬升角过陡或者贴地飞行。在论文里放图时一般把三维图作为主图俯视图放旁边收敛曲线放底部或单独一栏。需要注意的是收敛曲线通常画的是全局最优适应度的变化不要只保存最后一代的结果运行过程中每一代的最优值都必须存下来不然画不了曲线。4. 五种算法对比实验参数设置与结果解读4.1 统一的实验参数与公平性设置做算法对比最怕的是各算法参数设置不公平。打个比方你让A算法跑200代B算法跑100代然后说A算法更优这没意义。所以实验前一定要设定统一的基准参数。我常用的参数是无人机数量3架路径航迹点数量10个左右种群规模30最大迭代次数200地图范围200×200×80米威胁区设置三到四个分布在不同的高度层。每个算法独立运行多次取平均值和最好值进行统计因为群智能算法带有随机性单次运行结果没有说服力。至少跑10次记录每次的最优适应度、平均适应度、收敛代数、运行耗时然后把这些数据整理成表格。这里有个容易被忽略的点种群规模和迭代次数一样时不同算法的时间复杂度并不一样。CO的分组结构、GOOSE的两个记忆矩阵、PIO的种群减半机制都会让单次迭代耗时不同。除了看最终适应度也要记录运行时间有时候一个算法比其他算法多花了一倍时间最终结果却只好了百分之几那是否值得选择就要结合你的实际场景来判断了。4.2 收敛曲线的真实含义收敛曲线是横轴迭代次数、纵轴最优代价的连线。一条理想的收敛曲线应该是前期快速下降中期缓慢下降后期趋于平缓稳定。快速下降说明算法的全局搜索能力强很快找到了有希望的区域平缓稳定说明算法已经趋于收敛修改也不会带来明显提升。如果曲线在迭代中期出现突然的平台期然后某一次迭代又突然下降这种阶梯状下降通常意味着算法在一次随机扰动中偶然跳出了局部最优。这个现象在PIO上很常见因为它的地标算子会让种群规模动态变化每次减半可能带来一次状态突变。如果曲线一直不下降或者下降幅度极小先别急着怪算法很有可能是初始化就把所有个体都撒在了一个很差的区域或者代价函数里距离项和威胁项量级差太多梯度都被大的那一项吞了。这时候把每代的种群平均适应度也画出来如果平均值在下降但最优值不动说明种群多样性不够要去检查算法对全局最优的利用强度是否过大。4.3 各算法的表现差异与机制解释在我跑同一组实验的结果里PIO通常收敛最快前50代就能把代价压到比较低的水平但后期容易陷入局部最优最终结果往往不是最好的。SFOA的搜索行为比较温和收敛稳定在简单地图上表现不错但复杂威胁区场景下容易停在次优解附近。APO由于双阶段结构前期探索很充分在中等复杂度的地图上往往能找到比较理想的路径收敛曲线比较平滑。GOOSE的协同搜索机制让它在多无人机场景下表现均衡防撞效果比较稳定代价函数也能持续下降。CO的多样性保持能力强在多威胁区、多局部最优的场景里往往能找到更优的路径但相对耗时较长收敛速度是五个里偏慢的。这个结果不是我瞎编的是符合算法机制逻辑的。PIO两个算子接力决定了它前期快后期容易冲过头APO的双阶段设计天然适合中等复杂度的地形CO的群体文化机制本质上是把搜索步长限制在一个更稳定的范围内牺牲速度换全局性。理解了机制差异你在文章里写对比分析时就不用简单复述实验结果而是能解释为什么这个算法在这个场景表现好这才是审稿人和导师想看到的东西。5. 常见问题排查与避坑清单5.1 报错维度不一致这类工程最常见的报错就是矩阵维度不匹配。报错信息通常出现在代价函数里比如Matrix dimensions must agree。根因几乎都是初始化解码成三维数组时某个维度的定义和你预设参数对不上。排查方法很简单在初始化解码后加一行 size() 输出确认个体向量reshape成 [N, D, 3] 的成功性。然后单独调用一次代价函数传入一个初始化个体看能不能正常算出数值。如果代价函数能算出数再放入算法主循环就基本没问题。我遇到过一个同学他把D理解成航迹点数量但初始化时生成的是D-2个中间点因为起点和终点固定不需要搜索。解码时如果还按D来reshape最后一个维度的数据就会错位报错或结果错乱是必然的。这种情况多检查代码注释里的维数定义就行。5.2 收敛曲线不下降明明代码没报错但收敛曲线平平的像心电图一样上下抖动完了。这个问题通常是初始化范围太小或者适应度函数里惩罚项过重导致的。初始化时所有个体都落在起点和终点连线的极窄范围内算法再怎么搜索都逃不出一个小空间自然收敛不动。换句话说你给算法的发挥空间根本不够。另一个常见原因是惩罚系数设置过大。威胁惩罚是距离量级的几十倍甚至上百倍在算法看来目标函数表面就像一个巨大的深坑和四周的高墙每一步更新都会产生巨额惩罚最优值再怎么变都拉不回来。把惩罚系数调小一些让距离项在早期能够主导搜索等算法找到一条基本可行的路径后再加强威胁惩罚细化路径。这种方式在Matlab实现里就是所谓的罚函数随迭代递增简单有效。5.3 路径穿过威胁区路径直接穿过威胁区尤其是一条路径刚好从两个紧挨着的威胁区中间穿过说明两个问题一是威胁检测的插值点不够密二是威胁惩罚权重不够大。插值点太少时连线上的某个小段穿过了球体但插值点都刚好落在球外检测不出来。把每个线段上的插值点增加到10个以上基本就能消除这类漏检。还有一种情况是路径贴着威胁区的边界走代价很低看起来就像擦边球。这其实是算法在利用惩罚函数的空隙严格来说威胁区的建模应该有硬约束或更大的边界惩罚带。实用做法是给威胁区半径乘一个1.1到1.2的安全系数让规划路径自动与真正的危险边界保持一个安全缓冲。5.4 多机间距失效多无人机路径规划里机间防撞惩罚失效是个隐蔽问题。明明代码里写了间距检查但画出来的路径中两架无人机在某个航迹点几乎重合。检查后发现是在计算两两距离时用了错误的维度索引比如比较的是同一架无人机不同航迹点之间的距离而不是不同无人机同一航迹点之间的距离。如果你自己修改多机场景代码务必在每次迭代更新后手动算一下无人机之间的最小距离打印出来观察。同时要注意安全距离阈值不要设得太小建议在代价函数里把机间防撞的惩罚权重设成动态值距离小于阈值时不仅加固定惩罚还可以把超出部分按平方累积距离越近惩罚增长越快这样能形成更强的排斥力。5.5 代码运行太慢五种算法都跑相同的迭代次数和种群规模时间确实不短。最常见的瓶颈就是代价函数里那两层for循环一层遍历所有个体一层遍历所有航迹点和威胁区。Matlab写for循环本来就慢解决办法是尽量向量化操作。检测威胁区时可以把所有航迹点构造成一个大矩阵利用向量化同时计算到每个威胁球心的距离再矩阵化判断是否落入威胁区而不是逐点检测。机间防撞同理用nchoosek生成无人机编号对一次性计算所有两个无人机在所有航迹点上的距离矩阵。向量化之后同样的实验时间能缩短到原来的五分之一甚至十分之一。如果你只是要跑通对比实验不想花时间重构代码也可以从减少重复运行次数入手每个算法独立运行次数从10次降到5次然后用平均值出图效率提升一半。我个人在实际操作中的体会是这类多算法对比工程看起来复杂其实只要建模清楚了算法只是不同的搜索策略在同一个代价函数上做文章。真正花时间的永远是测试函数的设计、维度对齐和结果解读这也恰恰是写论文和做项目最核心的部分。最后再分享一个小技巧跑对比实验前把五种算法的种群初始个体设置为同一个随机种子生成这样能剔除随机性对结果公平性的干扰对比曲线会好看很多也更加有说服力。