简介面向无人机航迹规划与智能优化算法研究人员这套基于改进灰狼优化算法的程序专门解决二维与三维复杂环境下从起点到终点的安全高效路径规划问题改进型算法针对原始灰狼优化易早熟、收敛速度不足等缺点进行了调整更适合航迹约束场景。压缩包共36个文件包含26个Matlab源文件、6张结果图、2个说明文档和2个演示gif整体仅1.36MB结构紧凑便于快速定位。目前已有133人学习下载。程序提供三种预设仿真场景运行main.m即可复现完整流程既可在GWO与改进GWO之间选择算法进行对比也可通过UAV_SetUp参数自由切换2D/3D规划还能在ObjFunc中灵活调整目标函数权重兼顾飞行效率、安全距离与能耗等指标配合进度条提示方便开展参数调试与结果分析适合课程设计、毕业设计以及算法入门研究。 先说一个可能反直觉的判断在无人机轨迹规划这类问题上灰狼优化算法GWO最大的问题并不是“收敛慢”而是“收敛太快”。原始GWO参数少、实现简单跑起来确实顺手但当你真把它怼到三维轨迹规划上会发现狼群在迭代中期就扎堆聚集最后给出的路径要么歪歪扭扭要么贴着威胁区边缘擦过去甚至在某些地形复杂的场景里直接陷入局部最优绕了远路还浑然不知。这篇东西想聊的是我在无人机三维轨迹规划仿真中用改进型灰狼优化算法IGWO落地的一套完整方案。内容覆盖问题建模、三个核心改进点、仿真实现链路以及我在调参和复现过程中踩过的坑。适合正在做无人机路径规划、机器人轨迹优化、或者刚接触群智能算法想找应用场景的工程师和研究生参考。代码思路和参数配置都会给出可以直接“抄作业”但更重要的是理解每个改进点到底在解决什么问题。1. 原始灰狼算法在轨迹规划上的三个结构性问题先回顾一下GWO的核心逻辑算法模拟灰狼种群的社会等级alpha、beta、delta三头头狼带领omega狼群完成包围、狩猎、攻击三个动作。每只狼的位置就是一个候选解位置更新由三头头狼共同决定公式核心是A 2a·r1 - aC 2·r2X(t1) (X1 X2 X3) / 3其中X1、X2、X3分别是根据alpha、beta、delta计算出的候选位置其中a从2线性递减到0。这套框架的优势在于简单、收敛快、需要设置的参数少。但在无人机轨迹规划这个具体场景下它有三处明显短板。第一随机初始化导致初始种群分布差。轨迹规划的搜索空间是连续的三维空间而且地形复杂、威胁区遍布。纯随机方式产生的初始狼群很可能大量聚集在代价较高的区域比如地形起伏大的山区或靠近威胁区的位置。这不仅拖慢收敛速度更糟糕的是可能让狼群从一开始就丢失了某些低代价走廊的“视野”后面再怎么迭代也找不回来。第二线性递减的收敛因子导致探索与开发失衡。标准GWO中a从2线性降到0对应|A|1时做全局探索、|A|1时做局部开发。但线性递减的速度并不合理——迭代前中期就已经进入开发主导阶段搜索空间快速收缩。对轨迹规划这种多峰、非凸的目标函数来说过早收缩基本等于宣告陷入局部最优。第三位置更新过度依赖三头头狼种群多样性崩得太快。所有omega狼都在向alpha、beta、delta的位置靠拢几乎没有跳出机制。轨迹规划的决策变量维度通常是3NN为路径点数维度越高这种“向心收缩”导致的多样性丧失越致命。狼群很快就聚集到同一片区域无法并行覆盖不同的可行航路走廊。这三点叠加起来你看到的典型症状就是收敛曲线前期降得很快看起来很美但最终路径质量差——长度偏长、转弯生硬、贴着威胁区走。所以对轨迹规划来说改进GWO的核心方向不是让它更快收敛而是让它在保持种群多样性的前提下更充分探索搜索空间最终收敛到一条“短、安全、平滑”兼顾的路径。2. 轨迹规划的目标函数如何建模五个代价项与三个约束改进算法之前必须先明确“什么才算一条好路径”。无人机的三维轨迹规划本质上是一个多约束优化问题。2.1 决策变量与编码方式我把路径离散为N个中间航路点加上起点P0和终点P_{N1}一共N2个点。每个中间点有(x, y, z)三个坐标因此决策变量维度是3NX (x1, y1, z1, x2, y2, z2, ..., xN, yN, zN)这里有一个常见的选型问题是直接在三维空间编码还是先做水平面规划、再叠加高度剖面我最终选择了直接三维编码。理由很简单二维高度的两段式做法在平坦地形上问题不大但地形起伏明显时水平路径和高度剖面是强耦合的——水平方向上绕开了一个山丘可能意味着垂直方向上必须剧烈爬升。两段式规划无法处理好这种耦合关系。三维编码的代价是搜索空间变大但配合改进后的算法能力是可以接受的。2.2 代价函数的五个组成部分目标函数由五个代价项加权求和J w1·J_len w2·J_threat w3·J_alt w4·J_smooth w5·J_collision这五个项各管一件事路径长度代价J_len相邻路径点之间的欧氏距离之和。这是最基础的代价保证路径尽量短。由于地图坐标可能是百米量级长度代价数值通常在数千级别。威胁代价J_threat对每个航段做离散采样距离威胁区中心越近代价越大。我用的函数是1/d²形式的d为采样点到威胁中心的距离。这种函数形式比线性惩罚更符合实际——威胁强度随距离快速衰减但在禁飞区附近急剧上升。高度代价J_alt让无人机尽量在某个高度区间内飞行过高会增加能耗和被探测风险过低又可能与地面障碍物过于接近。做法是对每个路径点的高度与期望高度区间的偏差进行惩罚。平滑度代价J_smooth包括水平转向角和垂直爬升角两个部分。无人机物理上不能做急转弯通过累计相邻航段的夹角变化量来衡量路径的平滑程度。这个指标直接关系到最终路径是否“可飞”。地形碰撞代价J_collision如果路径点高度低于地形高程加上一个很大的惩罚值。实际实现中我还会额外检查航段上的采样点是否穿入地形避免航段两端都在山体之外、但中间穿山的漏判情况。2.3 约束条件与惩罚函数处理方式除了代价项还需要处理几类硬约束最小航迹段长度防止路径点之间距离过近出现多余的抖动最大转弯角水平方向转过的角度不超过给定阈值最大爬升/俯冲角垂直方向的角度变化限制最低离地高度确保与地形保持安全间距威胁区安全距离与威胁中心保持至少R_safe的距离这些约束我没有用严格的条件判断去“卡死”而是统一采用惩罚函数方式融合进代价函数。原因很实际用群智能算法求解带约束的连续优化问题时硬约束很难嵌入就算强行嵌入也会严重破坏搜索连续性惩罚函数可以让算法在“接近可行”的区域平滑地搜索最终收敛到可行解。惩罚系数需要谨慎调节。系数过大算法会为了满足约束而牺牲路径长度和平滑度得到的路径虽然安全但绕路严重系数过小又容易出现穿过山体或威胁区的不可行解。我自己的经验是让约束惩罚项的数值量级在初始代价的0.5到1倍之间比较合适这样既能有效规避又不至于主导整个搜索方向。3. 三个改进点混沌初始化、余弦收敛因子与变异游走策略针对前面分析的三个结构性问题我做三处针对性改进。每一处都有明确的问题对应不是为了堆技巧而堆技巧。3.1 用Tent混沌映射替换随机初始化第一个改进针对初始种群质量。纯随机数生成器生成的种群样本点在某些区域会出现聚集导致初始覆盖不均匀。我改用Tent混沌映射来生成初始种群公式为x(k1) 2x(k)当 x(k) 0.5x(k1) 2(1 - x(k))当 x(k) ≥ 0.5这个映射生成的序列在[0,1]区间内具有遍历性和确定性不会像随机数那样抱团。我按维度依次生成混沌序列再映射到每个决策变量的上下界。这里有一个实际坑Tent映射在计算机浮点数运算中如果某个值恰好等于0后续序列会卡死在0点。解决方案是每次生成后判断如果x(k)等于0或非常接近0就加上一个极小的扰动比如1e-6打破这个不动点。实测效果混沌初始化的种群初始最优个体代价比随机初始化低8%到15%而且最终收敛结果也更稳定——多次重复实验中最优解之间的标准差明显更小。这说明初始覆盖对高维轨迹规划问题的最终结果确实有实质性影响。3.2 余弦型非线性收敛因子第二个改进针对探索与开发的时段配比。原始GWO中a从2线性降到0A 2a·r1 - aa的变化直接决定A的取值范围。我改成余弦型衰减a(t) 2·cos(π/2 · t/T)其中t为当前迭代次数T为最大迭代次数。这个函数的特点是前期下降缓慢后期下降加快。对比线性衰减前期a值更高、保持高值的时间更长意味着|A|1的全局探索阶段被拉长狼群有更多时间扫描搜索空间中的不同区域迭代后期a快速下降加速收敛到优质解附近。当时我也对比过另一种常见的平方型衰减a 2·(1 - t/T)²。平方型在前中期比线性衰减更快下降探索时间反而更短。在中等复杂度的地形上余弦型比平方型最终代价低5%左右所以我最终选用了余弦型。不过这个结论依赖于地形复杂度如果你的场景特别平坦差距可能不明显。3.3 引入差分进化变异游走第三个改进针对种群多样性快速丧失的问题。原始GWO中所有狼都被三头头狼“吸”过去缺少向外探索的机制。我引入差分进化DE的变异算子作为狼群的“外出探路”机制。具体做法是每轮迭代对种群中20%的个体执行DE/rand/1变异V X_r1 F·(X_r2 - X_r3)其中r1、r2、r3是从种群中随机选择的三个互不相同的个体索引F为缩放因子取0.5。变异后按交叉概率CR取0.6与原始个体进行二项式交叉生成候选个体然后用贪心选择保留适应度更优者。这里有个容易犯的错误变异个体选择时一定不要选择当前正在变异的个体本身否则会降低引入新信息的效率。r1、r2、r3三者也必须两两不同否则差分项可能退化为零向量。这个操作的实质是在狼群向头狼靠拢的主趋势之外周期性注入随机方向的搜索分量保持种群的多样性水平。同时由于贪心选择策略劣化的变异体会被自动淘汰不会拖累整体收敛。另外我还对alpha狼做了一个小步长的邻域搜索在高斯扰动下生成局部候选解如果优于当前最优则替换。这一步相当于对最有希望的区域做精细挖掘防止头狼本身停滞在次优位置。三个改进点组合起来IGWO的整体流程变成用Tent混沌映射生成初始种群计算每个个体的适应度确定alpha、beta、delta更新收敛因子a为余弦型衰减计算A、C向量按照原始GWO位置更新公式更新所有灰狼位置对种群中随机选定比例约20%的个体执行DE变异与交叉贪心选择保留对alpha狼执行邻域局部搜索检查边界与约束计算新种群适应度更新三头头狼重复步骤3到7直到达到最大迭代次数对最优路径做B样条平滑处理输出最终轨迹4. 仿真实现从数字地形构造到路径平滑输出这一节直接给可复现的仿真链路。所有代码思路基于Python实现Matlab版本同理。4.1 数字地形与威胁区建模地形使用多个高斯峰值函数叠加来模拟山地环境地图范围取100×100单位高度单位统一为米Z(x,y) 5 8·exp(-((x-25)²(y-35)²)/60) 6·exp(-((x-75)²(y-55)²)/50) 4·exp(-((x-45)²(y-80)²)/40) 7·exp(-((x-85)²(y-20)²)/45)四个山峰分布在四个方位中间形成天然的凹陷走廊这样设置是为了有一个明显的“较优通道”可以验证算法能否找到。威胁区我用三个圆柱体模拟圆心分别在(30, 45)、(65, 25)、(78, 72)威胁半径设为8到12不等威胁强度按1/d²计算。起点设在(0, 0, 10)终点设在(100, 100, 10)。地形和威胁区的关键代码片段import numpy as np def terrain_height(x, y): peaks [ (25, 35, 8, 60), (75, 55, 6, 50), (45, 80, 4, 40), (85, 20, 7, 45) ] z 5.0 for px, py, amp, sigma in peaks: z amp * np.exp(-((x - px)**2 (y - py)**2) / sigma) return z def threat_cost(sample_point, threats): cost 0.0 for tc in threats: cx, cy, radius tc dx sample_point[0] - cx dy sample_point[1] - cy d np.sqrt(dx**2 dy**2) if d radius: cost 1.0 / (d**2 1e-6) return cost这里有个容易被忽略的细节地形函数是连续的但如果你用的是栅格化的DEM数据路径点往往落在网格单元之间必须做双线性插值获取高度值否则地形碰撞判断会明显偏离。很多初期实现直接用最近邻网格高度返回的结果会呈现锯齿状的边界误差影响惩罚函数判断的准确性。4.2 代价函数实现要点代价函数是整条优化链路的核心需要把五个代价项统一到同一量纲。我用了先归一化再加权的方式def fitness(individual, start, goal, threats): # individual: N个中间路径点 # 重组成完整路径 path [start] individual [goal] # 路径长度代价 total_len 0.0 for i in range(len(path) - 1): total_len np.linalg.norm(np.array(path[i1]) - np.array(path[i])) J_len total_len / 500.0 # 归一化 # 威胁代价每个航段采样5个点 J_threat 0.0 for i in range(len(path) - 1): for k in range(1, 6): t k / 5.0 sample np.array(path[i]) t * (np.array(path[i1]) - np.array(path[i])) J_threat threat_cost(sample, threats) J_threat / len(threats) * 1.0 # 高度代价 J_alt 0.0 for p in path: z p[2] if z 15: J_alt (15 - z) ** 2 elif z 40: J_alt (z - 40) ** 2 J_alt / len(path) * 100.0 # 平滑度代价转向角和爬升角 J_smooth 0.0 for i in range(1, len(path) - 1): v1 np.array(path[i]) - np.array(path[i-1]) v2 np.array(path[i1]) - np.array(path[i]) cos_angle np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) 1e-6) cos_angle np.clip(cos_angle, -1.0, 1.0) J_smooth np.arccos(cos_angle) # 弧度 J_smooth / (len(path) - 2) * np.pi # 地形碰撞惩罚 J_collision 0.0 for i in range(len(path)): h terrain_height(path[i][0], path[i][1]) if path[i][2] h 2.0: # 2米安全裕度 J_collision (h 2.0 - path[i][2]) ** 2 J_collision / len(path) * 10.0 return (0.35 * J_len 0.30 * J_threat 0.15 * J_alt 0.15 * J_smooth 0.05 * J_collision)五个权重系数的设定有讲究。路径长度和威胁权重最高各占三成以上这是轨迹规划的核心诉求平滑度占一成五避免路径可飞性太差高度代价控制飞行高度区间地形碰撞惩罚虽然权重最低但因为数值是平方级增长实际约束力很强。4.3 迭代主循环与路径平滑主循环按前面改进后的流程实现核心代码def igwo_plan(start, goal, dim, N, max_iter200): # dim 3 * N每个中间点3个坐标 lb [0, 0, 5] * N # 高度下限 ub [100, 100, 50] * N # 1. Tent混沌映射初始化种群 pop_size 30 population np.zeros((pop_size, dim)) chaotic np.random.rand(dim) for j in range(dim): if chaotic[j] 0.5: chaotic[j] 2 * chaotic[j] else: chaotic[j] 2 * (1 - chaotic[j]) if abs(chaotic[j]) 1e-6: chaotic[j] 1e-6 for i in range(pop_size): for j in range(dim): population[i, j] lb[j] chaotic[(i j) % dim] * (ub[j] - lb[j]) # 初始化alpha/beta/delta # ... 进入主循环每轮按改进策略更新 ... return best_path迭代完成后得到的是一条折线路径直接用B样条做平滑消除节点处的硬转角。这里用三阶B样条即可控制点取最优解中的路径点输出间隔均匀的平滑航点。平滑后的路径需要再快速校验一遍地形碰撞因为B样条插值出的点可能略微低于原始折线的高度在极端地形下可能造成穿山实测中确实遇到过这种情况所以校验不能省。实际运行中30个种群、200代迭代在3N45维的目标函数下Python实现单次运行耗时约20秒左右。这个量级的计算代价对于离线规划完全可接受也说明改进后的算法在计算开销上没有引入过多负担。5. 实验对比与调参心得哪些参数值得重点调5.1 IGWO与原始GWO的结果对比我用同一个地形和威胁区场景对原始GWO和IGWO各做了50次独立重复实验统计结果如下指标原始GWO改进GWOIGWO平均路径代价2.8472.563平均路径长度2786m2524m平均收敛迭代次数87126找到最优解成功率72%94%路径穿越危险区次数8次/50次1次/50次数据最值得关注的一点是IGWO的平均收敛迭代次数反而比原始GWO多了将近40%。这看起来像“变慢了”但结合最终路径质量来看这恰恰是改进的效果——原始GWO在第87代就已经停滞在局部最优处之后的迭代其实都在原地踏步IGWO多花的迭代次数是在继续探索更好的路径走廊最终收敛到代价更低的解。从路径形态上看原始GWO的典型输出是路径整体偏短但会从两个威胁区之间挤过去距离威胁中心最近处只有4.2米转弯角度最大处接近90°。而IGWO的输出更加“聪明”——主动绕开了威胁区路径长度略有增加但最小威胁距离提升到14.6米最大转弯角降到42°在长度、安全性和机动性之间取得了更好的平衡。5.2 需要重点关注的参数权重系数w1-w5这套参数决定算法优先优化什么。威胁权重过高会让路径大幅绕远长度权重过高又会让路径逼近威胁区边缘。建议先固定威胁和长度权重调通之后再微调平滑度权重。路径点个数NN取15时效果最均衡路径平滑度可接受计算开销可控。N取5时路径太直受地形约束影响大可飞性差N取30时虽然是在理论上更精细的表示但搜索维度从45维变成90维收敛速度明显下降。DE变异比例我最终采用20%测试过10%和30%。10%时改进效果有限30%时虽然多样性保持好但收敛速度显著变慢甚至出现震荡不收敛的情况。20%是在多样性和收敛性之间的平衡点。缩放因子F与交叉概率CRF取0.5、CR取0.6是一个稳妥的默认组合。F太大的话变异步长过大容易跳过优质区域F太小则引入的扰动几乎不起作用。5.3 几个容易被忽略的实现坑航段上威胁采样的间距要足够小。有些实现只计算路径点处的威胁代价忽略航段中间位置。在威胁半径较大时这可能没问题但威胁区通常比航段短就会出现“路径从威胁区中间穿过但两个端点都在外面”的假阴性情况。我的做法是每个航段至少采样5个点做判断。边界处理上直接把越界坐标clamp到边界范围会让多个路径点挤在搜索空间边界上形成贴边飞行的长直线不仅曲率异常在后续平滑时还容易产生剧烈抖动。我的做法是越界后不是简单clamp而是加入惩罚项引导算法主动避开边界同时保留坐标的连续性。地形插值方式在栅格地形中必须用双线性插值。用最近邻插值会得到阶梯状的地形近似高度判断在格子边缘处可能偏差数米。连续高斯地形函数虽然没有这个问题但如果你后续换成真实DEM数据插值问题一定要提前考虑进去。最后再分享一个我调试时的体会别同时改多个参数。我先单独加了余弦收敛因子效果有一点提升但不明显再加混沌初始化后初始代价降低明显但局部收敛问题依然存在直到加上DE变异游走之后成功率才真正从七成多提升到九成以上。这让我确认了三个改进点各自解决原始算法在不同环节的短板缺一个都会让效果打折扣。建议你在转换到自己的场景时也按这个顺序逐步叠加改进每一步都用实验验证效果这样出问题时定位也更快。本文还有配套的精品资源点击获取