简介基于改进型灰狼优化算法GWO的无人机轨迹规划MATLAB代码包面向无人机路径规划方向的研究者、学生与算法入门者。针对复杂飞行环境下安全高效航迹生成问题提供从种群初始化、边界约束到航迹检测的完整可运行框架。包内共36个文件以26个.m源码为主包括main.m主程序、三套UAV_SetUp.m仿真参数、GWO与MP_GWO两种启发搜索算法、ObjFun.m目标函数及绘图辅助脚本另有6张结果图、2份说明文档、2个演示动图压缩包仅1.36MB。程序支持2D/3D轨迹规划自由切换可自行设计仿真环境并调整目标权重运行进度条提示降低了等待焦虑。已有133人进入学习。代码按功能模块拆分结构清晰便于对比改进型GWO与传统GWO的收敛性和解质量适合用于算法改进实验、课程设计或科研预研。 把无人机轨迹规划这件事做好其实是个“两头堵”的活——既要保证算法在数学上能收敛到一条好路线又得让这条路线下发给飞控后飞机真的飞得出来、飞得稳。我最近做的一个项目就是用改进型灰狼优化算法来做无人机三维轨迹规划效果比原版GWO和粒子群算法都要好。这篇就把整个思路、改进点、参数设置和踩坑过程完整记录一下适合正在做无人机路径规划相关课题、或者想把元启发式算法落地的同学参考。1. 项目整体思路为什么是灰狼优化算法1.1 无人机轨迹规划到底在做什么先说清楚一个概念轨迹规划和路径规划不是一回事。路径规划解决的是“从A点到B点怎么走”输出通常是一系列航路点轨迹规划在路径的基础上还要加上时间信息也就是说每个航路点什么时候到、速度怎么变化、转弯的时候姿态大概什么样都得交代清楚。拿飞控的视角来看路径规划告诉你“走哪条路”轨迹规划告诉你“以什么速度、什么姿态沿着这条路走”。我做的这个项目场景是城市或山区环境下无人机要从起点飞到目标点中间有若干禁飞区威胁源还需要尽量降低飞行高度、保持航迹平滑。这是一个典型的三维全局规划问题。规划出来的航路点序列会交给下层的飞行控制器执行。所以算法输出的路线不仅要短、要安全还得满足无人机的动力学约束比如最小转弯半径、最大爬升角这些。1.2 为什么选GWO而不是A*、RRT、PSO传统方法里A*和RRT在二维平面路径规划中确实很经典但它们在高维、连续、多约束环境下往往需要大量采样或者网格化处理计算量会涨得很快。人工势场法实时性好可局部极小值问题比较麻烦。智能优化算法里遗传算法全局搜索能力强但编码复杂、调参麻烦粒子群算法实现简单但容易早熟。灰狼优化算法是Mirjalili在2014年提出的模拟的是灰狼群体的等级制度和捕猎行为。它有四个等级α是最优解代表狼群的头狼β是第二优、δ是第三优辅助决策剩下的ω是普通狼。整个搜索过程就是狼群围绕猎物包围、追捕、攻击的过程。这个算法的优势很明显结构简单、控制参数少核心参数就收敛因子a一个、实现容易、收敛速度快。我用它做轨迹规划主要看中的是它参数少的特性——在工程上少一个参数就少一个需要调的坑。不过直接把原版GWO搬来用效果并不理想。后面会详细讲问题出在它的搜索机制上早期收敛因子线性下降导致探索能力不足后期狼群多样性下降太快极其容易陷入局部最优。所以这个项目的核心工作就是针对轨迹规划这个具体场景对GWO做定向改造。2. 改进点设计把经典GWO改造成轨迹规划专用算法2.1 原版GWO在轨迹规划中的两个致命短板第一个短板是收敛因子a的线性衰减策略。a从2线性降到0直接影响位置更新步长。这种策略在早期会让狼群快速向当前最优解靠拢但问题在于如果α狼本身处于一个局部较优解整个狼群就会被“带偏”后面很难逃出来。在无人机轨迹规划这种多局部极小值的问题上这种早熟现象尤其明显。第二个短板是位置更新方式过于简单。原版算法里ω狼的位置是由α、β、δ三匹狼的位置加权平均决定的权重固定没有考虑迭代过程中不同阶段对“探索”和“开发”的需求差异。这就导致算法前期探索能力不够后期开发能力又过剩。我做的改进就是用非线性收敛因子替换线性衰减同时引入动态权重来调整三匹头狼对位置更新的影响比例。2.2 改进一非线性收敛因子与动态权重收敛因子的改进公式为a 2 * (1 - (t/T)^k)其中t是当前迭代次数T是最大迭代次数k是一个非线性调节系数我取的是1.5。这样a的下降速度在前期比较慢狼群的搜索步长大能保证充分的全局探索后期a快速下降步长变小算法转入精细开发提高收敛精度。动态权重的思路是迭代早期更信任α狼因为α代表当前最优方向但同时不能忽略β和δ的多样性迭代后期如果α长期不变说明可能卡在局部最优这时候应该增大β和δ的权重引入更多可能性。具体实现我用了基于迭代次数变化的余弦权重。实测下来这种改进方式比固定权重在收敛速度上慢了不到5%但最终航迹长度平均短了8%到12%说明它有效避免了早熟。2.3 改进二Tent混沌映射初始化种群原版GWO的种群初始化是随机生成种群分布均匀性没法保证。如果初始狼群恰好都集中在一个区域算法从起跑线上就输了。我在初始化阶段引入Tent混沌映射让初始解尽可能均匀地分布在整个搜索空间。Tent映射的公式为x(n1) x(n) / μ, 当 x(n) μ x(n1) (1 - x(n)) / (1 - μ), 当 x(n) μ这里μ取0.5混沌序列就会在[0,1]区间内产生一组遍历性极好的值。把这些值映射到航路点的坐标范围初始种群的多样性就有了保障。这个改进虽然不起眼但对最终结果的影响很大——它决定了整个搜索过程的起点质量。2.4 改进三差分进化扰动机制即使加了混沌初始化和动态权重算法在复杂威胁环境下还是可能陷入局部最优。我加了一个“保险丝”每迭代10次就对狼群中的一部分较差个体执行一次差分进化DE风格的变异交叉操作引入新个体替换掉适应度排名靠后的狼。这个操作的本质是周期性打破狼群的“思想统一”。在实际调试中如果不加这个扰动大约有三成的概率会收敛到一条明显绕远路的航线加了之后这个问题基本消失。代价是每次扰动多消耗约5%的计算量对现代计算机来说完全可接受。3. 轨迹编码与适应度函数设计3.1 航路点编码与三次样条插值在GWO算法里每只“狼”代表一条候选轨迹。我把三维轨迹用N个航路点表示每个点包含x、y、z三个坐标狼的位置向量维度就是3N。起点和终点是固定的不参与编码。N的取值视任务范围而定一般选6到10个中间点。点太少轨迹灵活性不够点太多搜索维度会爆炸收敛变慢。裸的航路点序列是不能直接飞的转折太生硬飞控根本执行不了。所以得到航路点之后我用三次样条插值对轨迹做平滑处理。插值后的轨迹是连续且二阶可导的在相邻航路点之间过渡平滑曲率变化也不会太剧烈。这一步和机械臂轨迹规划里用的平滑思想是相通的——想想机械臂关节空间里做轨迹规划的时候也都需要保证速度和加速度连续。实际编码时的坑在于狼的位置向量的每个维度都有物理意义不能随便瞎改。比如z坐标的下界就是地形高度加上安全飞行高度x、y的范围也要限制在地图边界内。这些边界约束我会在狼群初始化时直接写入避免生成太多无意义的越界解。3.2 适应度函数长度代价、威胁代价、平滑代价与高度代价适应度函数是整个算法的“指挥棒”设计得好不好直接决定最终轨迹质量。我的适应度函数由四部分组成长度代价相邻航路点之间的欧氏距离之和除以最远直线距离归一化到[0,1]。威胁代价每个威胁源建模为圆柱体如果航路点落入威胁半径内代价增加再考虑整段路径是否穿过了威胁圆柱使用线段到圆心距离来检测。威胁代价权重设得最高。平滑代价计算相邻两个转向角的角度变化量角度变化越大代价越高。这能直接抑制“折线型”路径。高度代价为了降低被探测概率和节省能耗轨迹的平均高度要尽量低但最低不能低于地形安全高度。总适应度是四个代价的加权和。权重我分别为威胁0.4、长度0.25、平滑0.2、高度0.15。这个权重组合不是拍脑袋定的是跑了十几组对照实验挑出来的。威胁权重必须最高这是安全问题长度权重不必过高因为稍微多绕几十米远比穿越风险区要划算得多。3.3 约束处理方式约束条件包括最小航路段长度、最大转弯角、最大俯仰角、最低飞行高度。我用的是罚函数法当某条轨迹不满足约束时在适应度上加上一个惩罚项。惩罚项的值设计成比正常代价高一个数量级确保不满足约束的轨迹在迭代中被快速淘汰。之前踩过的一个坑是惩罚力度太小算法会“钻空子”——拿一条明显不可飞的轨迹和一条可飞但稍长的轨迹比前者反而得分更高。后来我把惩罚系数做了自适应设计让它在迭代后期越来越大这样到最后留下来的基本都是满足约束的解。4. 仿真参数与实操流程4.1 仿真环境搭建算法验证我是在MATLAB里做的。原因很简单矩阵运算方便画图和调参都快。如果你坚持用Python建议搭配numpy和matplotlib效果也一样只是矩阵操作写起来稍稍啰嗦一点。后面要接真机的话可以再把算法移植到Python或C和PX4飞控的仿真环境对接起来。仿真地图我设计成1000m x 1000m x 200m的三维空间。地图里有6个威胁源分布在从起点到终点连线的两侧附近故意摆放成一个“走廊”结构让算法必须做非平凡的绕行决策。这是验证全局规划能力最直接的方式。4.2 完整流程与关键参数表整个项目的实现步骤如下初始化地图、威胁源、起点终点坐标。用Tent混沌映射初始化狼群每只狼代表一条候选轨迹。计算每只狼的适应度选出α、β、δ三匹头狼。进入主循环更新非线性收敛因子a动态计算权重更新每只狼的位置。每10次迭代执行一次差分进化扰动替换较差个体。重新计算适应度更新α、β、δ。达到最大迭代次数后输出α狼对应的航路点。用三次样条插值平滑检查转弯角、转弯半径是否满足动力学约束。输出最终航迹和收敛曲线。关键参数设置如下表格参数名称数值说明种群规模50狼群数量太少会早熟太多计算慢最大迭代次数200后期收敛曲线已变平200次足够收敛因子非线性系数k1.5控制a的衰减速率中间航路点数量N8每个点3维搜索维度24重规划间隔未启用全局静态规划不涉及实时重规划威胁源数量6圆柱体威胁模型惩罚系数自适应递增从10线性增加到10004.3 实验对比改进GWO vs 原版GWO vs PSO为了公平对比三种算法的种群规模和迭代次数完全一致。每个算法独立运行15次取平均值算法平均航迹长度(m)威胁代价平均收敛迭代次数标准差(m)原版GWO14320.188763粒子群PSO13880.1210651改进型GWO12850.037422改进型GWO的平均航迹比原版短了大约10%威胁代价降到接近零收敛速度还更快。最明显的区别在稳定性上——原版GWO跑15次能有四五次掉进局部最优改进型只出现了一次轻微绕路。这说明改进方案确实有效不是只在某一次运气好。4.4 平滑后轨迹的可飞性验证得到航路点后平滑和动力学检查这一步不能省。我用三次样条插值生成平滑轨迹后逐点计算曲率再换算成转弯半径。无人机最小转弯半径设为80m检查结果全部满足要求。最大俯仰角也做了检查在指定的高度范围内没有问题。这里分享一个经验如果你发现平滑后的轨迹在某些点曲率过激不要急着去改样条系数优先回头检查航路点的分布——大概率是有两个相邻航路点靠得太近导致样条为了穿过它们被迫产生剧烈弯曲。适当调整航路点之间的最小间距约束问题通常就消失了。5. 常见问题与真机落地避坑5.1 算法早熟怎么办早熟的表现是收敛曲线早早变平最终轨迹明显绕远路或者穿过了本可以躲开的威胁区。排查方向有三个首先看收敛因子a的衰减曲线如果a很快降到了0探索阶段太短需要增大非线性系数k其次检查种群多样性如果多次迭代后所有狼的位置向量都几乎一样说明多样性丢失需要提高差分进化扰动的频率或者增加变异幅度最后看权重设计如果α的权重始终过高也会加速早熟。5.2 生成的轨迹飞控执行不了这是算法到真机之间最大的一个鸿沟。仿真里觉得很好的平滑轨迹下发给飞控后飞机却飞不出同样的效果。原因多半在于算法只考虑了运动学约束没有考虑无人机动力学特性。比如爬升率和下降率限制、最大水平速度、转弯时的滚转角限制这些。我之前就遇到过仿真轨迹的最大爬升角是30度但实际飞机最大只能飞20度的情况。解决思路有两条一是在适应度函数里把动力学约束建模得更完整不要只用运动学约束二是做分层规划——上层用改进GWO生成全局航路点下层交给飞控自带的轨迹规划器或LQR控制器去跟踪这些航路点。实际工程里第二种思路更稳健也更好调。5.3 Python和MATLAB结果不一致同一个算法换成不同语言实现结果可能差很多但关键指标应该基本一致。如果你发现差异很大先检查两件事一是随机数种子是否统一混沌初始化本身是确定性的但后续的差分进化扰动里如果有随机操作不固定种子结果必然不同二是浮点精度问题MATLAB默认双精度Python如果不注意类型转换可能会降到单精度。统一设置random seed并把数据接口对齐通常就能解决。5.4 常见问题速查表现象可能原因解决措施收敛过快且轨迹差收敛因子下降太快增大非线性系数k收敛慢但轨迹好探索过度开发不足减小k适当增大后期α权重轨迹穿越威胁区威胁代价权重太低提高威胁代价权重至0.4以上轨迹折线感强平滑代价不够增加平滑代价权重或加大航路点间距多次运行结果差异很大种群初始化不够均匀使用混沌映射初始化轨迹无法被飞控执行缺少动力学约束增加约束项或采用分层规划架构6. 扩展方向单机到集群再到学习类方法结合6.1 从单机轨迹规划到多机协同避障单机轨迹规划跑通之后很自然会往集群方向走。多机场景里除了环境威胁还要考虑机间避碰和任务时间协同。GWO在这个方向上的扩展方式通常是把所有无人机的航路点拼接在一起编码同一只狼代表整个机群的规划方案然后适应度函数里增加机间距离代价和任务时间同步代价。如果做的是多无人机协同避障这种思路能直接套用。但要注意维度灾难问题3架无人机、每架8个中间航路点搜索维度就是72普通GWO迭代200次可能不够。这种情况下可以考虑离线预规划加在线局部避障的两层架构全局部分用改进GWO局部快速避障交给人工势场或者深度强化学习策略。6.2 与学习类方法结合从结构上看全局规划用启发式搜索算法局部反应式控制用深度强化学习是目前比较成熟的工程组合。全局算法给出长时间尺度上的最优路径局部策略负责应对动态威胁。这样既保留了GWO这类算法的稳定性和全局搜索能力又借助学习类方法的实时响应特性补上了动态避障的短板。真机系统里这种混合架构比单纯依赖任何一种方法都靠谱。关于PX4这类飞控仿真环境的搭建网上已经有很多成熟的教程可以直接在该项目链接基础上展开。建议先把本文的算法在MATLAB里跑通再移植到Python做仿真最后对接飞控做半实物仿真。一步一步来不要跳步。7. 实操体会与总结最后聊点实际的。改进型GWO这套方案在静态全局轨迹规划问题上确实够用代码量不大调参难度低效果比原版GWO和PSO都好。它在我的项目里最大的价值是稳定——同一张地图跑15次结果一致性很高这对工程交付来说比单次最优解重要得多。踩过几次坑之后我的体会是这类元启发式算法项目60%的精力应该花在建模和适应度函数设计上算法本身反而是相对标准化的。威胁建模、约束设计、权重标定每一个环节都直接影响最终能不能飞。千万别一上来就猛调算法参数把地图和代价模型吃透效果立刻就不一样。另外一个心得很多人在算法验证时只画一条“漂亮”的轨迹就收工了没有检查速度和加速度曲线也没有考虑到转角处的动力学冲击。建议在输出轨迹后一定要补一步曲线检查把所有运动学、动力学指标打印出来逐项核对。只有把这些细节都补齐算法才能从论文里的图变成真机上飞出来的航线。场景稍微变化比如换个地图或者换种威胁分布权重都需要重新标定。把权重标定这一步做成一个可配置的脚本会给你后续的维护省下一大堆时间。本文还有配套的精品资源点击获取