融合粒子群改进鲸鱼优化算法，实现无人机三维航迹规划

做无人机航迹规划，尤其是三维航迹规划，踩过的坑远比想象中多。二维路径规划画一条不撞障碍物的线基本就完事了，可三维环境里要同时面对起伏地形、爬升俯冲角、转弯半径、最低离地高度这些约束，搜索空间一下子涨了好几个量级。我最近在一个模拟项目X里实现了“融合粒子群的改进鲸鱼优化算法”，用 Python 做无人机三维航迹规划，效果比标准鲸鱼算法和标准粒子群都要稳。这篇文章把整个项目的动机、建模、算法设计、代码实现、仿真结果和调参经验完整梳理一遍，适合正在做无人机航迹规划、机器人路径规划，或者想搞懂群智能算法怎么落地的人参考。如果你只是听说过鲸鱼优化算法，想看看它到底怎么和粒子群融合、怎么写代码跑通三维航迹，这篇内容正好对口。

1. 这个项目到底在解决什么问题

1.1 三维航迹规划难在哪

无人机三维航迹规划和传统二维路径规划有本质区别。二维里大部分算法只要保证路径不与障碍物相交，基本就能用；但三维环境里，地形高度本身就是变量，同一片区域高度不同，代价完全不同。比如同样从 A 点到 B 点，走山谷可能距离短但需要频繁爬升，走山脊可能距离长却相对平缓，到底哪条更优，取决于你对航程、油耗、姿态变化和飞行安全之间怎么权衡。

另外一个麻烦是搜索空间。如果地图是一个 1500×1500 的范围，高度范围 100 到 500，理论上任意连续坐标都是候选解，这是一个高维连续优化问题。传统A*、Dijkstra这类栅格搜索算法在离散栅格上能用，但栅格分辨率一高，存储和计算开销会爆炸。RRT类采样方法能处理高维空间，但生成的路径往往不够平滑，需要额外做轨迹优化。所以群智能算法天然适合这类问题：不依赖梯度信息，可以直接把航迹表示成决策变量，用适应度函数评价每条航迹的好坏，然后靠种群迭代逼近最优解。

还有一个容易忽略的点：三维航迹规划不止是“找一条安全路径”，还必须满足无人机动力学约束。比如相邻航迹点之间的距离不能太短，否则无人机来不及完成转弯；航迹的爬升角、俯冲角要限制在合理范围；最低高度要保证不与地面碰撞，同时也不能贴地飞行导致气流影响。这些约束如果没写进模型，算法给出的路径即使看起来平滑，实际飞也飞不了。

1.2 为什么选鲸鱼优化算法

鲸鱼优化算法（Whale Optimization Algorithm，WOA）是模拟座头鲸群体捕食行为的一种群智能算法，核心机制包括包围捕食、气泡网螺旋攻击和随机搜索三类位置更新策略。它最吸引人的地方是参数少，主循环里真正需要调的系数就一两个，实现起来非常快，适合做高维连续优化。

我在模拟项目X里选它做主算法，还有个重要原因是它对解空间的前期探索能力不错。WOA 在迭代前期会通过随机搜索机制让个体散布到不同区域，不容易像某些局部搜索算法那样一上来就陷入同一个坑里。航迹规划问题通常存在大量局部最优解，前期能够广撒网，对最终结果影响很大。

不过标准WOA也有明显短板。到迭代后期，个体都会向当前最优解靠拢，螺旋更新和包围捕食的步长会逐渐变小，种群多样性快速下降，一旦当前最优解落在某个局部极值附近，整个种群很难跳出来。这个问题在三维航迹这种几百维甚至上千维的优化任务里尤其突出。

1.3 融合粒子群的动机：不是叠加，而是互补

粒子群优化算法（PSO）是一种非常经典的群智能算法，核心是“记忆”机制：每个个体保存自己的历史最优位置，群体保存全局最优位置，速度更新时同时向这两个记忆点靠近。正因为有 pbest 和 gbest 的记忆引导，PSO 在局部精修和快速收敛方面表现得非常出色。

我当时想得很直接：WOA 负责把种群推到位，PSO 负责在候选解附近做精细搜索，两者如果配合好，就能兼顾全局探索和局部开发。实际做下来发现，关键不是简单把两个算法串在一起，而是要让它们互补。标准WOA的更新结果可能已经是一次有效的“方向探索”，此时如果直接丢弃或者仅靠螺旋修正，很容易丢失个体的历史经验；反过来，如果PSO完全接管位置更新，又可能让种群过早汇聚。所以最终方案是在WOA生成候选解之后，用PSO的速度公式结合 pbest 和 gbest 对候选解做进一步修正，然后再做贪婪选择，保证新位置不会比原来的差。

这种融合逻辑从直观上也站得住脚：一群侦察兵先用围猎策略锁定几个可疑区域，然后每个人回忆自己的最优位置，再互相参考群体最优位置，在可疑区域附近精细搜索。整个过程并不是把两个算法做简单的种群混合，而是在个体层面完成了一次“先探索、后精修”的协作。

2. 三维航迹规划的数学建模有多关键

2.1 地形、威胁和边界环境建模

在我的模拟场景里，地形用多个高斯山峰函数叠加生成，公式大致是：

z_terrain(x, y) = max(0, Σ A_i * exp( -((x - x_i)^2 / (2 * σx_i^2) + (y - y_i)^2 / (2 * σy_i^2) ) ))

每个山峰由峰高 A_i、中心位置 (x_i, y_i)、纵向和横向的展宽 σx_i、σy_i 控制。叠加之后再加上一个基础高度，就能得到比较自然的起伏地形。这种高斯山峰模型的好处是可以灵活控制地形难度，想造峡谷就放两座离得近的高峰，想验证算法避障能力就多放几个分散山峰。

威胁区用圆柱体近似，每个威胁区有自己的中心、半径和高度范围。无人机一旦进入威胁区，在代价函数里就会产生大额惩罚。实际飞行任务还有禁飞区、雷达探测区这类情况，都可以用类似的圆柱、球或长方体威胁模型去替代，不需要修改算法主体。边界约束也很直接：所有航迹点的 x、y 必须落在地图范围，z 必须在最低安全高度和最大飞行高度之间。超边界的位置要做越界修正，不能直接当作无效解丢掉，否则种群会在边界处带走大量个体，影响收敛。

2.2 航迹编码方式

航迹编码是整个项目里最先需要设计的一步。最直观的做法是把起点到终点之间插入 N 个航迹点，每个点有三个坐标，那么优化变量就是 3N 维。比如插入 10 个中间点，搜索维度就是 30，这对于群智能算法来说属于中等偏难的维度。

我在实际工程里没有把三个坐标全部当自由变量，而是做了一点简化：把 x 方向按等分长度固定下来，只优化每个点的 y 坐标和海拔 z。这样从一个航迹点到下一个航迹点，x 方向的投影距离是确定好的，转弯、爬升这些约束更容易控制，维度也从 3N 减少到 2N，收敛难度明显下降。

另一种技巧是使用相对坐标或增量编码：优化变量保存的是航迹点之间的水平夹角、爬升角和步长，而不是绝对坐标。这种编码更容易在解空间里保证航迹的平滑性，但计算航迹实际位置时需要积分，过程稍微复杂一些。对于项目里这个规模的三维航迹规划，固定 x 方向并使用 y、z 作为决策变量已经足够稳定，我最后采用的就是这种方案。

2.3 代价函数：把约束写成分数

适应度函数就是把约束和优化目标折算成一个标量，数值越小代表航迹越好。我在这个项目里用的代价包括：航程代价、爬升代价、平滑代价、威胁代价、最低高度惩罚。

航程代价是所有相邻航迹点之间的欧氏距离之和，这个值对应无人机的飞行时间或者耗油量。爬升代价统计相邻航迹点的高度变化绝对值之和，高度剧烈起伏不仅费电，还会让姿态控制压力变大。平滑代价考察相邻三段航迹之间的夹角或者转弯率，转弯过急要扣分。威胁代价沿航迹做密集插值，对插值点逐个判断是否进入威胁区，进入就给惩罚。最低高度惩罚则判断每个航迹点和插值点的高度是否低于对应地形高度加安全间隙，如果低于就按穿透深度的平方加权惩罚。

整个适应度函数可以写成：

J = λ1 * L_cost + λ2 * H_cost + λ3 * S_cost + λ4 * T_cost + P_terrain

这里的 λ1 到 λ4 是权重，P_terrain 是地形穿透和威胁区惩罚。权重设计有一个原则：惩罚项权重一定要远大于常规代价项。我在模拟项目X里把 P_terrain 的单次穿透惩罚设成 5000，常规航程代价的量级通常只有几百，这样即使某条航程极短但撞山，适应度也不会好看，算法才能把“安全可行”放在第一位。

2.4 约束处理方式：硬约束加软惩罚

约束不能全做成惩罚，否则会出问题。我自己的经验是：边界约束一定要硬处理，位置越界就强制拉回到边界；最低离地高度可以用硬约束结合软惩罚，如果单靠惩罚，可能出现航迹点刚好在地形上方一点点、但相邻插值段穿山的漏网情况。正确的做法是在计算代价前先对航迹做密集线性插值，逐个插值点检查是否低于地形高度，一旦发现穿透就返回一个很大的惩罚，同时在更新个体位置时把高度抬高到安全线以上。

像最大爬升角、最大转弯角这类动力学约束，我建议做软惩罚而不是硬截断。原因是群智能算法的随机性很强，如果直接把这些约束当成硬性条件卡死，大量生成的候选解都会无效，种群更新效率严重下降。先允许算法自由搜索，再用惩罚项把不可行区域“垫高”，效果比粗暴过滤好得多。后续如果需要严格限制角度，可以从最终优化结果里再做一步平滑处理。

3. 融合粒子群的改进鲸鱼算法设计

3.1 标准鲸鱼优化算法的三个更新策略

标准WOA的位置更新分成三部分。第一部分是包围捕食：当随机数小于 0.5 且系数 A 的绝对值小于 1 时，个体朝向当前全局最优位置收缩，更新公式为 X(t+1) = X_best - A * D，其中 D = |C * X_best - X(t)|。这里的 A 和 C 由控制参数 a 和随机数生成，a 在迭代过程中从 2 线性下降到 0。

第二部分是气泡网螺旋攻击：同样在随机数小于 0.5 但算法选择螺旋路径时，个体用公式 X(t+1) = D' * e^(b*l) * cos(2πl) + X_best 围绕最优位置螺旋逼近。第三部分是随机搜索：当 |A| ≥ 1 时，个体不再参考全局最优，而是随机选一个种群个体作为参照，用来增强全局探索能力。

这三个策略合起来的效果很有意思：前期 |A| 大，个体倾向于随机探索；后期 a 变小，个体逐渐收拢到最优解附近。标准WOA的问题就在这里，一旦前期错过真正的最优区域，后期不管螺旋更新还是包围捕食都只能在局部区域打转，很难跳出来。所以很多改进版WOA的论文都会针对这一块做文章，我这次融合粒子群的思路也是基于同一个痛点。

3.2 PSO 的强项与 WOA 的短板

粒子群优化的速度更新公式是 v = w * v + c1 * r1 * (pbest - x) + c2 * r2 * (gbest - x)，位置更新就是 x = x + v。从公式可以看出，每个粒子的飞行方向由三部分决定：自己之前的飞行惯性、个体历史最优的吸引力、全局最优的吸引力。这种“记忆”机制让PSO在求解精度和收敛速度上很有一手，个体即使当前质量不好，也能沿着历史最优方向不断修正。

但PSO的短板也很明显：一旦全体粒子的速度都偏向 gbest，种群多样性急剧下降，后续基本就围绕一个局部最优开抖。尤其是高维的航迹规划问题，PSO经常出现“早熟”，表现为收敛曲线很快下降但最终代价依然很高。WOA的好处是随机搜索机制保留了更强的探索能力，所以在整体框架上我用WOA维持种群广度，用PSO做局部深挖。

3.3 融合策略的具体设计

我最终实现的融合流程是这样的：对每一个个体，先按照标准WOA的方式生成一个候选位置 X_woa，这个候选位置可能是包围捕食、螺旋攻击或者随机搜索中的某一种产生。接下来把这个候选位置看作“粒子群更新中的当前位置”，代入PSO速度公式计算新的速度增量，用这个增量修正候选位置，得到最终候选 X_new。

速度更新我沿用经典PSO结构，但惯性权重 w 采用线性递减，从 0.9 降到 0.4，前中期保证探索，后期加强局部收敛。学习因子 c1 和 c2 我也做了一个不太复杂的变化：迭代前半程 c1 稍微大于 c2，强调个体历史经验，后半程反过来，让群体最优信息主导搜索。这个调整对收敛精度的提升其实比想象中明显。

得到 X_new 后，先对越界坐标做硬修正，对高度不足的点做抬升，然后计算适应度。如果 X_new 的适应度比当前个体 X 好，就替换，同时更新 pbest 和 gbest；如果更差，我会做一个速度回弹处理，也就是把速度向量取反而不是直接丢弃候选解，这样个体不会因为一次失败就完全丧失移动能力。这个细节帮了很大的忙，种群活跃度保持得比单纯贪婪选择好。

核心伪代码如下：

for t in range(max_iter): a = 2.0 - 2.0 * t / max_iter w = 0.9 - 0.5 * t / max_iter c1 = 2.5 - 1.5 * t / max_iter c2 = 0.5 + 1.5 * t / max_iter for i in range(pop_size): # 1. WO A 生成候选解 X_woa = woa_update(pop[i], gbest, population, a) # 2. PSO 修正候选解 v[i] = w * v[i] + c1 * r1 * (pbest[i] - X_woa) + c2 * r2 * (gbest - X_woa) X_new = X_woa + v[i] # 3. 边界与高度修正 X_new = bound_fix(X_new) # 4. 贪婪选择 + 速度回弹 if fitness(X_new) < fitness(pop[i]): pop[i] = X_new update_pbest_and_gbest(pop[i]) else: v[i] = -v[i]

这个融合方案不需要维护两套种群，也没有增加太多计算开销，核心代码量比标准WOA只多了十来行。但效果上，算法稳定性和最终解质量都有明显提升。

3.4 为什么这个融合方向站得住脚

很多人看到融合算法会怀疑是不是“拼积木”，实际测试下来，这种 WO A + PSO 的组合有效是有原因的。WOA 的三种更新策略决定了它在候选解空间里的“搜索骨架”，尤其随机搜索机制在高维问题里非常宝贵。PSO 的速度公式则提供了一种带记忆的“局部梯度方向”，它不等于真实梯度，但在 pbest 和 gbest 的引导下，搜索方向往往比纯随机巡游更有目的性。

简单说，WOA 提供了广泛性，PSO 提供了方向性，两种能力在同一个个体上叠加，相当于一个粗搜加一个精修同时发生。这也是为什么融合之后，标准WOA后期收敛慢、容易卡在局部最优的问题得到缓解，而标准PSO前期探索不足的问题也没有被放大。算下来，整个算法依然只依赖种群大小、迭代次数、惯性权重范围和学习因子范围这几个参数，可调性保留得很好。

4. Python 代码实现与关键片段解析

4.1 总体结构与主要数据结构

整个项目我用 Python 实现，代码组织成模块函数的形式：地形生成模块、航迹编码模块、适应度模块、WOA-PSO融合优化模块和可视化模块。主要数据结构包括种群位置数组 pos、速度数组 vel、个体历史最优 pbest、个体历史最优适应度 pbest_fitness、全局最优 gbest 和全局最优适应度。所有数组都用 numpy 表示，方便向量化计算。

因为航迹点是连续坐标，我把每个个体设计成二维数组：形状为 (n_points, 3)，每一行是一个航迹点的 (x, y, z)。整体种群就是一个三维数组：形状为 (pop_size, n_points, 3)。这样做的好处是适应度函数可以直接对整个个体操作，避免一个一个元素循环，方便后续扩展成并行计算。

标准WOA里的 A、C、l 这些参数每次迭代都要重新生成，我都是按 np.random.rand 的方式采样。为了让实验结果可以复现，我在项目里固定了随机种子，否则调参时很难判断改动是提升了还是纯随机偏差。

4.2 地形建模与适应度函数的实现

地形插值函数可以预先算成一个网格矩阵，所有个体计算时直接查表，避免反复调用 exp 函数。下面是我用的核心代码，计算航迹长度、高度变化、威胁代价和地形穿透惩罚。

import numpy as np def make_terrain(grid_x, grid_y, peaks): """ peaks: [(A, xc, yc, sigma_x, sigma_y), ...] """ terrain = np.zeros((len(grid_y), len(grid_x))) for i, y in enumerate(grid_y): for j, x in enumerate(grid_x): h = 0.0 for A, xc, yc, sx, sy in peaks: h += A * np.exp(-((x - xc) ** 2) / (2 * sx ** 2) - ((y - yc) ** 2) / (2 * sy ** 2)) terrain[i, j] = h return terrain def calc_fitness(points, terrain, x_grid, y_grid, threats, start, goal): # 航程代价 path = np.vstack([start, points, goal]) seg_len = np.linalg.norm(np.diff(path, axis=0), axis=1) length_cost = np.sum(seg_len) # 爬升代价 dz = np.abs(np.diff(path[:, 2])) climb_cost = np.sum(dz) # 平滑代价 turn_cost = 0.0 for i in range(1, len(path) - 1): v1 = path[i] - path[i - 1] v2 = path[i + 1] - path[i] cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-8) turn_cost += (1.0 - cos_angle) ** 2 # 威胁与地形惩罚 threat_penalty = 0.0 terrain_penalty = 0.0 for k in range(50): t = k / 49.0 for seg in range(len(path) - 1): px = path[seg][0] + (path[seg + 1][0] - path[seg][0]) * t py = path[seg][1] + (path[seg + 1][1] - path[seg][1]) * t pz = path[seg][2] + (path[seg + 1][2] - path[seg][2]) * t # 地形 gz = interp_terrain(px, py, terrain, x_grid, y_grid) if pz < gz + 8.0: terrain_penalty += 5000.0 * ((gz + 8.0 - pz) ** 2) # 威胁 for tc in threats: if ((px - tc[0]) ** 2 + (py - tc[1]) ** 2) < tc[2] ** 2 \ and tc[3][0] < pz < tc[3][1]: threat_penalty += 3000.0 return length_cost + 1.5 * climb_cost + 2.0 * turn_cost \ + terrain_penalty + threat_penalty

这里有一个细节我特别想强调：地形穿透惩罚必须对航迹段做密集插值，不能只判断优化变量那些点。因为优化变量本身的点可能都避开山峰了，但相邻两个点连成线段之后，线段完全可能在两个点之间穿山。我只在代码里做了一个 50 段插值示意，实际项目里可以根据地图大小和航迹点间距把插值密度提高到 100 甚至 200。

4.3 融合搜索主循环

主循环代码和前面伪代码一致，这里给出一个稍微完整一点的版本，包含种群初始化、WOA候选生成、PSO修正和最优解更新。

def woa_pso_optimize(terrain, x_grid, y_grid, threats, start, goal, pop_size=30, max_iter=200, n_points=8): dim = 2 * n_points # y 和 z 两个自由变量 pos = np.random.rand(pop_size, dim) pos[:, :n_points] = pos[:, :n_points] * (map_x[1] - map_x[0]) + map_x[0] # y范围 pos[:, n_points:] = pos[:, n_points:] * max_alt + safe_alt # z范围 vel = np.zeros((pop_size, dim)) pbest = pos.copy() gbest = pos[0].copy() fitness_val = np.array([calc_fitness(vec_to_points(pos[i]), ...) for i in range(pop_size)]) pbest_fitness = fitness_val.copy() gbest_fitness = fitness_val.min() for t in range(max_iter): a = 2.0 - 2.0 * t / max_iter w = 0.9 - 0.5 * t / max_iter c1 = 2.5 - 1.5 * t / max_iter c2 = 0.5 + 1.5 * t / max_iter for i in range(pop_size): r = np.random.rand() A = 2 * a * np.random.rand() - a C = 2 * np.random.rand() best_vec = vec_to_points(gbest) cur_vec = vec_to_points(pos[i]) if r < 0.5: if abs(A) < 1: X_woa = best_vec - A * abs(C * best_vec - cur_vec) else: rand_idx = np.random.randint(pop_size) rand_vec = vec_to_points(pos[rand_idx]) X_woa = rand_vec - A * abs(C * rand_vec - cur_vec) else: L = np.random.uniform(-1, 1) dist = abs(best_vec - cur_vec) X_woa = dist * np.exp(1.0 * L) * np.cos(2 * np.pi * L) + best_vec X_woa_flat = points_to_vec(X_woa) pbest_flat = points_to_vec(pbest[i]) gbest_flat = points_to_vec(gbest) vel[i] = w * vel[i] + c1 * np.random.rand() * (pbest_flat - X_woa_flat) \ + c2 * np.random.rand() * (gbest_flat - X_woa_flat) new_vec = X_woa_flat + vel[i] new_vec = bound_fix(new_vec) if calc_fitness(vec_to_points(new_vec), ...) < calc_fitness(cur_vec, ...): pos[i] = new_vec if calc_fitness(vec_to_points(new_vec), ...) < pbest_fitness[i]: pbest[i] = new_vec pbest_fitness[i] = calc_fitness(vec_to_points(new_vec), ...) else: vel[i] = -vel[i]

实际项目里我不会在适应度更新处重复调用多次 calc_fitness 那么浪费，而是先把新解适应度算一次存下来再比较。这里为了可读性做了点简化，正式工程需要注意缓存结果。

4.4 运行工程化的建议

运行速度是整个项目最容易翻车的地方。如果每次迭代都大范围调用地形插值函数，一次适应度计算要遍历几十个插值点，种群几十个个体，迭代几百代，慢是必然的。我建议先把地形网格插值矩阵提前算好，飞行过程中需要的三维坐标直接用双线性插值查表，不要反复调用高斯函数。

另外，航迹点数量和插值密度要平衡。在起点到终点之间插入 8 到 12 个中间点对于大多数场景已经够用，再多只会增加维度，并不一定能得到更好航迹。插值密度在 50 到 100 之间通常足够检测地形碰撞，如果地图存在又窄又高的山峰，再适当提高。实验调试阶段可以先用低分辨率地形和较少迭代，等参数确定后再提高分辨率跑最终结果。

可视化我用的是 matplotlib 的 3D 投影，把地形曲面、威胁圆柱体、起始点、目标点和最优航迹画在同一张图里。渲染时把地形网格步长调大一点，不然 3D 曲面旋转操作会很卡。也可以额外画一张高度剖面图，把地形高度和航迹高度投影到一条纵向剖面上，检查是否有穿透。

5. 仿真实验结果与收敛性分析

5.1 测试场景与参数配置

我在模拟项目X里构造了一个 1500×1500 的地图，放置了三个不同高度和展宽的高斯山峰，另加一个圆柱威胁区，起点设置在 (100, 100, 120)，终点设置在 (1400, 1400, 180)，中间插入 8 个可优化航迹点。种群规模 30，迭代 200 次，每个实验独立运行 10 次取平均值。对比对象是标准WOA、标准PSO和本文的 WO A-PSO 融合算法。

参数配置上，标准WOA除了种群和迭代，基本沿用原论文默认设置；标准PSO的惯性权重从 0.9 线性降到 0.4，c1=c2=1.5；融合算法的惯性权重和动态学习因子按前一节的方案。为了让对比公平，所有算法在相同地形和相同随机种子下测试，且使用同一个适应度函数。

5.2 与标准 WOA、PSO 的对比结果

下面是这一组仿真测试得到的典型数据：

融合算法的平均航迹长度最短，平均适应度最低，而且 10 次运行的标准差只有 45.6，说明鲁棒性明显更好。标准PSO虽然收敛得不算慢，但波动较大，有两次直接陷入带较大威胁惩罚的局部解。标准WOA整体稳定但收敛慢，后期改进不明显。需要说明的是，这些数据只是个人模拟环境下的测试结果，换地图、换参数，数值会有变化，但趋势在多次试验中保持一致：融合之后，解的质量和稳定性都会提升。

5.3 收敛曲线和航迹可视化怎么看

看收敛曲线时，我通常关注两个阶段。第一阶段是前 30 到 50 代，看算法能不能快速找到一个质量不错的可行解，这个阶段的下降速度反映全局探索效率；第二阶段是 150 代以后，看曲线是否还在有效下降、有没有出现长时间平台期。融合算法中前期下降速度接近PSO，后期又能利用WOA的随机搜索跳出局部平台，所以整体曲线更平滑，没有标准PSO那种“突然不动”的阶梯状。

航迹可视化则需要三维图加俯视图配合检查。三维图直观看出线的绕行是否合理、高度是否平滑；俯视图则方便检查是否进入威胁区。我还会单独绘制“高度-位置剖面”，把地形剖面和航迹高度叠在一起，如果航迹高度线始终在地形线上方且保持安全间隙，说明地形约束处理到位。这个方法在调试撞山问题时非常高效。

6. 常见问题与调参避坑指南

6.1 算法早熟，所有个体挤在一起

如果你看到收敛曲线很快下降，但最终还是有一个很高的代价，多半是早熟收敛。融合算法虽然有PSO的记忆引导，但高维下依然可能陷入局部最优。我常用的对策是：提高惯性权重的初始值，让个体前中期保持较高活跃度；适当降低 c2 的后期增幅，避免群体最优的吸引力太强；再就是每次检测到全局最优连续 20 代没有更新时，随机重新初始化 10% 的个体，可以明显增加跳出局部极值的概率。

另外可以尝试把航迹数量调整一下。插值点太少会让解空间过于简化，容易掉进某个固定形态的局部解；插值点太多维度太高，也容易早熟。通常从 6 到 10 个点开始试，再根据实际效果增减。

6.2 航迹总是撞山或者贴地飞行

这个问题绝大多数不是算法的问题，是适应度函数里地形检测做得不够密。你需要确保每个优化变量点和相邻点之间的线段都做了足够密的插值检测。我最初只检测航迹点本身，结果出现航迹点在两个山峰之间穿过山谷、看起来没问题，但连线直接穿山的情况，后来把插值密度提高到 100 才把问题彻底解决。

还有一个坑是惩罚权重设置。如果地形穿透惩罚和航程代价一个量级，算法会权衡“要不要穿山省点油”，这显然是不允许的。把穿透惩罚设为固定大值，尤其按穿透深度的平方加权，可以有效抑制这类行为。最低离地高度也建议设成硬下界，任何候选解一旦低于该值，直接修正回安全高度以上。

6.3 计算太慢

计算慢主要来自适应度函数。最耗时的部分是地形插值与威胁检测。地形插值可以用预计算网格查表法替代；威胁检测可以先把威胁区域投影成二维距离场，再直接查当前位置是否在威胁半径内。如果实在需要更快的速度，可以用 numba 给适应度函数加速，实际测试能带来几十倍提升。

另一个提速技巧是在种群层面做向量化。同一代里所有个体的适应度计算彼此独立，完全可以用 numpy 批量处理。如果核心逻辑允许，还可以把种群分成若干子组并行计算，每个子组处理部分个体，合并结果后统一更新 gbest。对于 30 到 50 的种群规模，并行化收益可能不大，但如果要跑到几百个个体，并行就是必须的了。

6.4 参数速查与推荐起点

下面这个表是我在类似项目里常用的参数起点，适合中等复杂度的三维地图。

我就以这个参数起点跑，基本都能在几十代内得到可行航迹，再根据具体地形微调权重。不要一开始就追求最优参数，先把功能跑通，再慢慢调，否则问题会全搅在一起，非常难排查。

这个项目做完之后，我最大的体会是：算法改进的收益很多时候不是来自设计了多少花哨机制，而是来自把问题建模做扎实。地形检测做密一点、惩罚权重定合理一点，比换一个更复杂的算法更有用。融合粒子群的思路本身不复杂，甚至代码只比标准WOA多十几行，但它确实解决了标准鲸鱼算法后期收敛精度低、容易早熟的核心痛点。你可以把这一步当作基础框架，继续扩展成多无人机协同航迹规划、动态威胁环境下的在线重规划，或者加入差分进化、模拟退火这种更精细的局部搜索机制。先把这套 Python 实现跑通，再往自己的任务场景里加东西，会省去大量从头摸索的时间。