融合粒子群的改进鲸鱼优化算法无人机三维航迹规划Python代码实现这个标题一拿出来懂行的朋友大概心里已经有数了又是一套智能优化算法套航迹规划的活儿。但网上这类代码满天飞大都是把标准算法跑一遍、画个3D图就完事实际用到项目里往往一塌糊涂——要么收敛慢要么路径穿山要么重复绕圈。我这篇不打算复读算法论文直接聊聊我实际落地这个项目时踩过的坑、改过的逻辑以及一套能直接跑出结果的Python实现思路。适合正在做无人机航迹规划、智能优化算法对比或者课程设计的朋友参考尤其是卡在“算法融合后结果反而更差”这个怪圈里的人。1. 先说清楚这个项目到底解决什么问题1.1 无人机三维航迹规划的任务拆解无人机航迹规划本质上是一个多约束的多目标优化问题。什么叫多目标你当然想让飞行路径最短但光短不够你还得避开威胁区、禁飞区不能一头扎进山坡里还得考虑飞行高度不能太低被树撞上也不能太高暴露在雷达下转弯还不能太急否则固定翼无人机根本飞不过去。这些要求互相打架路径最短往往就要贴着威胁区边缘走高度最稳往往路径就不是最平滑的那一条。所以我们要做的是把它转成一个单目标优化问题——把所有约束加权成一个综合代价函数再用智能优化算法去搜这个代价函数的最小值。这就是整个项目的核心框架。实际项目中大部分时间不是耗在算法本身上而是耗在“把真实环境抽象成可计算的代价函数”这一步。地形数据来了怎么插值威胁区怎么建模航迹用多少个点表示这些没想清楚再好的优化算法也是空转。1.2 为什么选WOA又为什么非要融合PSO鲸鱼优化算法WOA是这几年的热门算法模拟座头鲸的泡泡网捕食行为结构简单、参数少、易于实现一旦绕开早熟问题全局搜索能力确实不错。但它有一个很明显的短板标准WOA在迭代后期个体的位置更新高度依赖当前最优鲸鱼一旦这个最优解是局部极值算法极容易陷入停滞种群多样性很快崩溃。粒子群算法PSO则相反它靠“个体历史最优”和“群体历史最优”两个记忆来引导搜索信息共享机制非常直接。但PSO对参数敏感一开始容易收敛快后期更容易陷入局部最优而且速度更新的随机性一旦控制不好粒子会在局部区域来回振荡。你单独用哪个效果都一般。所以这里我把两者融合起来用PSO的“个体记忆社会经验”机制来修正WOA的位置更新公式同时改造WOA的收敛因子和种群初始化方式让算法前期能更充分探索后期还能保持一定的跳出能力。这就是标题里“融合粒子群的改进鲸鱼优化算法”的来由——不是简单地把两种算法交替跑一遍而是在更新机制层面互相渗透。2. 算法融合不是简单叠加2.1 标准WOA的捕食机制回顾标准WOA有三类位置更新方式。第一种是包围猎物也就是向当前最优个体收缩第二种是螺旋气泡网更新从当前位置向最优个体螺旋靠近第三种是随机搜索随机选一个个体来更新这是为了保持探索性。具体公式我就不展开抄了只强调几个关键点。包围和螺旋更新都有参数A和C其中A由收敛因子a控制。a在迭代过程中从2线性递减到0前期探索、后期开发。问题就出在这个“线性递减”上很多复杂地形下前期探索时间根本不够后期a逼近0之后鲸鱼就死死贴着最优解一点跳出去的能力都没有。此外标准WOA的所有位置更新都没有使用“个体自己的历史最优位置”这个信息。也就是说一条鲸鱼飞到一个比较好的位置但后来被算法带到另一个差位置它不会记得自己曾经在哪走过这对多峰地形而言是非常浪费的。这正是PSO可以帮上忙的地方。2.2 PSO的“经验分享”机制给WOA补了什么PSO的每个粒子维护两个记忆pbest自身历史最优和gbest全局历史最优。每次速度更新等于“惯性 个体认知 社会认知”三部分加权。把这种机制借到WOA里就等于给每条鲸鱼装了个“导航记忆”——除了跟着全局最优跑还能被自己的历史最佳位置拉一把。这种混合方式能显著提升两个场景下的表现第一种是当前全局最优陷入局部极值时个体历史最优分布在整个搜索空间中依然能给种群提供多元的探索方向第二种是当某条鲸鱼自己曾经找到过比当前邻域更好的位置时它不会完全被全局最优带偏保持了局部搜索的多样性。我实际测试下来改进后算法在恶劣地形下的收敛概率比标准WOA高出不少尤其是维度稍高比如30维、45维的时候优势会更明显。2.3 融合改进的三个关键动作我的融合方案没有搞得很花哨核心就是三个动作。第一个动作种群初始化。原始WOA是纯随机初始化我是用逻辑混沌映射生成初始种群。这个方法代码简单但能保证初始解摊得更均匀。很多人容易忽略这一步其实初始种群质量直接决定后面所有迭代的上限尤其是地图是复杂高斯的场景下随机初始化经常一上来就把所有个体堆在某个角落里。第二个动作收敛因子非线性化。把原来的线性递减改成“先慢后快”的余弦式递减前期给足探索时间后期快速收敛。公式是 a 2 * cos(π * t / (2 * T))或者用 a 2 * (1 - t / T)^2 也行。我测试时两种都比线性递减稳得多。第三个动作位置更新融合PSO。具体做法下面代码里会写在每一次迭代中不是生硬地“一半用WOA、一半用PSO”而是用概率p判断每条鲸鱼走哪条路同时把PSO的个体认知项直接加到WOA的包围公式中。p的值随迭代从0.8慢慢降到0.4前期偏WOA的全局探索后期偏PSO的局部开发两边优势都能用上。3. 三维航迹规划建模与约束处理3.1 地图建模地形、威胁区、边界三维航迹规划不是凭空画一条曲线得先把环境变成可以计算的数据。我用的是一种比较通用的做法用一个离散网格地图x轴、y轴方向划分出若干网格z轴是高度每个网格点存储一个地形高度值。地形可以用多个高斯峰叠加来模拟山体也可以在真实地形数据上插值。威胁区模拟成若干个球体或者圆柱体圆心坐标和半径固定代价值按照无人机到威胁中心的距离指数上升。目的就是让优化算法明白离威胁区越近代价越高但又不是完全禁入——如果完全不能进入那约束直接是硬性的很多算法会很难求解我们就用软惩罚把路径推走。边界约束更简单路径的所有航迹点都必须落在地图范围内x、y、z各自有上下界。在代码里我用惩罚函数处理越界个体也就是越界越多适应度越差但同时又保留一点点灵活性避免算法因越界而大片死亡。3.2 航迹编码方式节点式与内插平滑航迹编码方式特别重要甚至比算法迭代次数对结果的影响还大。我这里采用的是“固定节点数编码”假设无人机从起点P_start飞到终点P_end把起点到终点的x轴方向分成N-1段每一段对应一个x坐标固定、y和z坐标待优化的节点。这样一来待优化的变量就是N-2个中间节点的y和z坐标维度是(N-2)*2。举个例子如果N10那优化维度就是16每个鲸鱼个体就是一个16维的向量。这个做法的好处是x轴保证单调递增天然规避了来回绕圈、走回头路的问题。坏处是如果航路中有必须绕开的障碍物节点数太少会导致路径不够灵活。所以我一般取N20左右效果和速度比较平衡。航迹求出来后再用贝塞尔曲线或者三次样条做一次平滑处理让转弯半径连续这才真正符合无人机运动学约束。3.3 适应度函数设计与权重调优适应度函数是灵魂设计得好不好直接影响算法最终搜出来的路径能不能实际飞。我的适应度函数由四个部分加权组成路径长度代价相邻节点之间的欧氏距离之和。这个没什么好说核心是让路径尽量短。威胁代价每个节点到每个威胁中心的距离高斯衰减和再加一个路径线段中点处的威胁采样防止节点之间穿过威胁区。高度代价让路径尽量保持一个参考巡航高度避免忽高忽低。平滑代价相邻两条线段夹角的大小夹角越小越平滑用单位向量点积转成惩罚项。四个代价的权重不是固定不变的。我通常的做法是先让路径长度权重为1威胁代价权重初期给很大比如20保证路径不穿威胁区迭代稳定后再逐步降低威胁权重、略微提高长度权重。这个过程可以做成动态权重但为了简单直接用固定权重也能跑出不错的结果。这里有个经验威胁代价一定要做“节点 线段中点采样”否则算法会出现一个经典作弊行为——节点全部绕开威胁区但节点之间的直线从威胁区正中间穿过去。我第一次试的时候就踩过这个坑。4. Python工程实现逐步拆解4.1 项目文件结构与依赖我的实现主要依赖numpy、matplotlib和mpl_toolkits.mplot3d环境管理用conda或者venv都行。整个项目文件拆成三个模块跑起来不绕。map.py地图生成、威胁区设置、约束定义。woa_pso.py改进鲸鱼算法主体包括初始化、更新、融合逻辑。main.py主程序负责跑仿真、画图、输出收敛曲线和最终路径。这样做的好处是调试方便改地图参数不用动算法代码改算法参数不用动主程序。实际项目里如果加了真实地形数据只要换成读文件接进map.py就行后续维护成本很低。4.2 环境初始化与种群编码我先把地图和个体编码结构写出来。下面是简化后的map.py核心部分import numpy as np class GridMap: def __init__(self, x_range, y_range, z_range, grid_size1.0): self.x_range x_range # (xmin, xmax) self.y_range y_range self.z_range z_range self.grid_size grid_size self.threats [] def add_threat(self, center, radius, strength1.0): self.threats.append({center: np.array(center), radius: radius, strength: strength}) def terrain_height(self, x, y): # 用多个高斯峰模拟山地实际项目可替换为真实地形插值 h 60 20 * np.exp(-((x - 120) ** 2 (y - 80) ** 2) / 800) \ 15 * np.exp(-((x - 80) ** 2 (y - 160) ** 2) / 400) return h def threat_cost(self, pos): cost 0.0 for t in self.threats: dist np.linalg.norm(pos - t[center]) if dist t[radius]: cost t[strength] * (1.0 / (dist 1e-6)) else: cost t[strength] * np.exp(-(dist - t[radius]) / 10.0) return cost这里threat_cost用了一个很简单的策略威胁区内代价极高威胁区外按指数衰减。实际使用时这个公式需要根据威胁区类型调整比如禁飞区就直接给一个非常大的常数惩罚。个体编码在woa_pso.py里。每个个体是一个维度为(n_nodes - 2) * 2的向量前一半是中间节点的y坐标后一半是z坐标x坐标由起点终点均匀插值得到。def decode_position(individual, start, end, n_nodes): xs np.linspace(start[0], end[0], n_nodes) ys np.concatenate(([start[1]], individual[:(n_nodes - 2)], [end[1]])) zs np.concatenate(([start[2]], individual[(n_nodes - 2):], [end[2]])) return np.stack([xs, ys, zs], axis-1)注意这里的 z 是绝对高度不是相对地形高度。我后来改成“地形基准 相对偏移”要更稳避免算法把 z 搜到地形以下太多纯靠惩罚收敛得慢。4.3 适应度函数实现适应度函数在woa_pso.py里写输入一个个体解码后的路径点数组输出总代价。def fitness(path, grid, weights): length_cost 0.0 threat_c 0.0 height_c 0.0 smooth_c 0.0 ref_alt 80.0 n len(path) for i in range(n - 1): seg_len np.linalg.norm(path[i 1] - path[i]) length_cost seg_len # 节点威胁 线段中点威胁防止穿威胁区 threat_c grid.threat_cost(path[i]) mid (path[i] path[i 1]) / 2.0 threat_c grid.threat_cost(mid) * seg_len if i 0: v1 path[i] - path[i - 1] v2 path[i 1] - path[i] cosine np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) 1e-6) smooth_c 1.0 - cosine # 高度偏离参考值惩罚 height_c np.sum((path[:, 2] - ref_alt) ** 2) / n # 地形穿透惩罚 terrain_c 0.0 for p in path: if p[2] grid.terrain_height(p[0], p[1]): terrain_c (grid.terrain_height(p[0], p[1]) - p[2]) ** 2 total (weights[len] * length_cost weights[thr] * threat_c weights[hei] * height_c weights[smo] * smooth_c weights[ter] * terrain_c) return total这里的权重weights是直接传入的字典方便调参。terrain_c这一项我不希望太大因为它只是约束而非目标但如果不加算法很容易直接把路径贴着地面走。4.4 WOA-PSO融合主循环代码重头戏来了融合主循环。这里有两种常见的实现方式我选了第二种。方式一每次迭代分成两批一半用WOA一半用PSO交替更新。这个实现简单但两个群体之间信息没有及时共享效果一般。方式二每只个体在每次迭代中按概率走WOA更新或PSO更新同时把pbest和gbest引入WOA公式。这种方式信息融合度高我采用的是这个。核心代码如下class IWOAPSO: def __init__(self, dim, lb, ub, n_whales50, max_iter200): self.dim dim self.lb np.array(lb) self.ub np.array(ub) self.n n_whales self.max_iter max_iter # 混沌初始化种群 r 0.7 for i in range(n): r 3.7 * r * (1 - r) self.positions[i] self.lb r * (self.ub - self.lb) self.velocities np.zeros_like(self.positions) self.pbest self.positions.copy() self.fitness_pbest np.full(n, np.inf) self.gbest self.positions[0].copy() self.fitness_gbest np.inf def evolve(self, fitness_func): T self.max_iter for t in range(T): a 2 * np.cos(np.pi * t / (2 * T)) # 非线性收敛因子 for i in range(self.n): # 判断是否触发越界修正 self.positions[i] self.boundary_check(self.positions[i]) f fitness_func(self.positions[i]) if f self.fitness_pbest[i]: self.fitness_pbest[i] f self.pbest[i] self.positions[i].copy() if f self.fitness_gbest: self.fitness_gbest f self.gbest self.positions[i].copy() for i in range(self.n): r1 np.random.rand(3) p 0.8 - 0.4 * (t / T) # 融合概率前期偏WOA后期偏PSO if np.random.rand() p: # WOA更新 引入PSO认知项 A 2 * a * np.random.rand(self.dim) - a C 2 * np.random.rand(self.dim) if np.random.rand() 0.5: # 包围猎物 D np.abs(C * self.gbest - self.positions[i]) new_pos self.gbest - A * D else: # 螺旋更新 dist np.abs(self.gbest - self.positions[i]) l np.random.uniform(-1, 1) new_pos dist * np.exp(1.0 * l) * np.cos(2 * np.pi * l) self.gbest else: # PSO更新 w 0.9 - 0.5 * (t / T) c1 1.8 c2 1.8 r1 np.random.rand(self.dim) r2 np.random.rand(self.dim) v (w * self.velocities[i] c1 * r1 * (self.pbest[i] - self.positions[i]) c2 * r2 * (self.gbest - self.positions[i])) self.velocities[i] v new_pos self.positions[i] v # 小概率变异防止完全早熟 if np.random.rand() 0.05: idx np.random.randint(self.dim) new_pos[idx] self.lb[idx] np.random.rand() * (self.ub[idx] - self.lb[idx]) self.positions[i] self.boundary_check(new_pos) return self.gbest, self.fitness_gbest简单解释一下这段代码里的关键点a用余弦递减一步步从2降到接近0WOA更新里不再用随机个体X_rand而是直接用gbest同时保留了螺旋机制这是为了更快向当前最优点靠拢PSO分支保留了完整的速度更新这样个体更替有“记忆”而非盲搜。p从0.8逐步降到0.4意味着前期大部分个体用WOA探索后期有更多个体切到PSO精调。这是这个融合方案里最值得品味的一个超参数调得好不好直接决定收敛曲线是平滑下降还是锯齿抖动。4.5 参数配置与运行方式在主程序里我通常这样配置参数import numpy as np from map import GridMap from woa_pso import IWOAPSO from utils import decode_position, fitness np.random.seed(42) grid GridMap(x_range(0, 200), y_range(0, 200), z_range(0, 200)) grid.add_threat([70, 70, 50], radius20, strength30.0) grid.add_threat([140, 120, 20], radius15, strength25.0) start (0.0, 0.0, 80.0) end (200.0, 200.0, 90.0) n_nodes 20 dim (n_nodes - 2) * 2 lb [0] * dim # 实际应该根据y/z范围设置这里简化 ub [200] * dim weights {len: 1.0, thr: 1.5, hei: 0.01, smo: 0.5, ter: 5.0} def fit_func(ind): path decode_position(ind, start, end, n_nodes) return fitness(path, grid, weights) algo IWOAPSO(dim, lb, ub, n_whales50, max_iter200) best_ind, best_f algo.evolve(fit_func) best_path decode_position(best_ind, start, end, n_nodes)这里lb、ub简单全填了0和200实际按y/z范围做组合就行。重点是运行时要记住设随机种子否则算法结果不稳定你很难判断是改进带来的效果还是随机波动。4.6 仿真可视化与收敛曲线画图部分我用matplotlib的三维散点、地形网格和威胁球。威胁球不直接画球体画成半透明等值面或者直接画圆心周围的一圈点这样快很多。三维图里主要看三点路径是否绕开威胁区、是否贴地飞行、起点终点衔接是否顺滑。收敛曲线则看从多少代开始进入平稳区平稳区的高度越低说明找到的解越好。我这里再补一句光看总代价值下降还不够最好把四项代价都打印出来观察威胁代价在100代之后有没有回升。如果回升说明路径长度权重过大把路径往威胁区里拽了这种“穿盾”问题单看总代价值容易被蒙混过去。5. 调参与避坑我从失败实验里总结的细节5.1 常见问题与排查速查表我先列一个速查表这些问题我基本都遇到过而且每一条背后都有对应的实验记录。现象可能原因解决办法收敛曲线一开始就贴着最优值不动初始种群太集中或者权重中威胁代价过小换混沌初始化检查威胁代价占比路径总是穿过威胁区威胁代价只在节点上计算线段穿威胁区增加线段中点采样或提高threat权重路径高度忽上忽下几何上很丑高度代价权重太小拉大height权重或者改成参考高度平方惩罚路径严重穿山terrain惩罚没生效z下界过松加大terrain权重或限制z最小值为地形高度迭代后期收敛放缓结果还行但总不够好融合概率p下降太快调整p的终值为0.3~0.5不要让全个体都走PSO不同随机种子下结果差异悬殊随机性太大、种群数少增加n_whales或者做多次运行取最优运行速度极慢适应度函数里多次遍历威胁区预先计算威胁区域网格距离场用查表代替实时计算这张表里最重要的就是第一行。早熟问题在标准WOA里几乎是必然的尤其是连续地形平滑时很多个体初始就聚在近处算法根本搜不到远处的低代价区。混沌初始化可以缓解但不能根治所以融合PSO的速度记忆就显得特别关键。5.2 两个特别容易踩的坑第一个坑把位置更新里所有r1用同一个随机数。这是新手最容易犯的错误会在A 2*a*np.random.rand(self.dim) - a这行上省事写成一个标量。表面上看代码能跑实际上会导致所有维度同步缩放整个搜索空间变成一条直线维度一高算法就废了。务必让每个维度都独立采样随机数。第二个坑越界后再把个体直接“硬拉”回边界。如果你用clip把每个维度的值截断到边界会人为地在边界上堆积大量个体前期还好后期会严重压缩搜索多样性。我一般用“重新生成”或者“部分反弹”的方式处理越界。比如越界后的新位置 边界 - 0.5 * (越界量)这样会反弹回可行域内部而不是贴边冻结。这两个坑都属于那种“代码一眼看不出问题跑起来效果怎么都不对”的典型情况。我第一次做这个项目时就因为第一个坑浪费了两天时间。5.3 参数调节的经验法则这里给出一套我的经验参数起点适配本项目的三维地图环境种群数 n_whales50我一般不推荐超过100性能提升有限但耗时翻倍。最大迭代次数 max_iter200~300三维航迹规划的维度不算高300代足够收敛。融合概率p初始0.8最终0.4采用线性或余弦递减都可以。权重len1.0thr设置成1.5~2.0hei设置成0.01~0.05smo设置成0.5ter设置成3~8。如果你发现路径穿山优先加ter而不是减hei因为两者作用点不同。调节参数时有个技巧每次只动一个参数把其他参数固定住记录收敛曲线和路径图。不要“感觉这不好就同时改两个”否则你永远不知道是哪个改动起了作用。6. 这个项目还能往哪扩展6.1 加入动态威胁与实时重规划上面这套实现是静态环境下的离线规划。真实任务中无人机起飞后可能会碰到新威胁比如临时禁飞区、移动目标。扩展方案是把算法改成滚动时域规划无人机每飞若干步就重新以当前位置为起点、下一目标点为重点调用一次改进WOA重规划路径。这么做有个好处不需要追求全局最优只要每段路径在局部可行即可。计算量可控而且融合后的算法收敛速度快很适合这类实时重规划场景。但要注意重规划时初始种群不能用随机初始化而是用当前航迹继续迭代否则路径容易抖动。6.2 多无人机协同多无人机协同是另一个方向。把单机的N个航迹节点扩展成M架无人机的联合编码代价函数里加上机间碰撞约束和任务分配约束维度会暴涨。这时候融合PSO的优势会更明显因为个体记忆能帮助每架无人机保持自己的航线稳定不会因为全局最优的轻微变动而反复横跳。我建议从小规模开始比如2架无人机、每架15个节点先调通碰撞检测和协同代价再逐步增加无人机数量。6.3 落地到实际飞行前的最后一步仿真路径最终要转成飞行控制系统的航点指令。这里别忘了两点一是用贝塞尔曲线或Dubins曲线做平滑因为优化出来的折线转弯太急二是检查每个航点的yaw角连续变化避免无人机在空中“甩头”。这些不在算法代码里但在工程落地里跟算法同样重要。我当时做完仿真后有一段平滑路径直接导入地面站试飞结果在第三个航点转弯时抖动得很厉害原因就是角度跳变超过了控制系统的角速度上限。后来我在代码里加了一步约束检测确保相邻两个航段夹角不超过30度飞起来就顺了。个人体会是算法融合只是走出了第一步后面的约束建模、参数调优、工程适配才是真正决定这套代码能不能用的关键。希望这篇分享能让你在实现类似项目时少踩几个我已经踩平了的坑。