融合粒子群与鲸鱼优化算法的无人机三维航迹规划Python实现

融合粒子群的改进鲸鱼优化算法无人机三维航迹规划（Python代码实现）

无人机三维航迹规划这几年算是智能优化算法最经典的应用场景之一，也几乎是所有做元启发式算法的同学躲不开的一个实验课题。以前大多数文章还在二维平面里绕障碍物，现在环境复杂了、空域约束多了，三维航迹规划成了刚需。说起来简单，其实真做起来有几个绕不过去的坎：地形建模、威胁建模、航迹代价设计，还有一个最关键的问题——优化算法本身。传统鲸鱼优化算法（WOA）收敛快、参数少，但种群多样性差，迭代到中后期很容易扎堆，陷入局部最优；粒子群优化（PSO）全局搜索能力不错，实现也直观，但后期收敛速度慢，精度一般。这次的项目做的就是把它俩融合起来，用粒子群的信息共享机制给鲸鱼算法补短板，最后在三维航迹规划这个场景里做Python仿真验证。

这篇文章我会把整个项目的思路、算法原理、代码实现、实验参数选择、常见坑和避坑经验全部拆开讲。适合正在做智能优化算法改进、无人机路径规划仿真、或者单纯想看看融合算法怎么写代码、怎么调参的同学。代码思路讲清楚之后，你可以直接迁移到二维路径规划、机器人路径规划、任务分配等场景里去。

1. 项目概述与问题建模

1.1 解决什么问题

无人机三维航迹规划，说白了就是要在一张带地形起伏、威胁区、禁飞区的数字地图里，找出一条从起点到终点的路径。这条路径需要满足几个条件：一是不能撞山、不能闯入威胁区；二是尽量短；三是飞行高度不能太低也不要太高，耗油耗电要控制；四是路径要平滑，不能一堆直角转弯，无人机飞不了。

这个问题本质上是高维非线性约束优化问题。航迹的维度一旦上来，传统A*、Dijkstra这类算法在栅格化的三维空间里计算速度会很难看，而且航迹质量受栅格分辨率影响大。于是大家把目光转向元启发式算法——不依赖梯度、可以接受非凸目标函数、实现也相对简单。用优化算法的思路是：把一条完整航迹编码成种群里的一个个体，用一组三维坐标点来表示路径节点，然后定义适应度函数，跑迭代，让算法自己去淘汰差的、保留好的，最后收敛出一条可行航迹。

这个思路本身没问题，问题出在算法选型和改进上。普通WOA在低维问题上表现很好，但在高维航迹规划里，后期容易集体陷入一条较差的局部路径；PSO的全局搜索能力不错，但末期的收敛步长偏大，导致解精度不够。这个项目就是把两种算法的优势叠加起来，形成一种“前期能充分探索、后期能精细开发”的融合优化器。

1.2 航迹规划的数学建模

航迹规划的建模是整个实验的底层基础。模型建得不好，后面算法再强也白搭。我这里采用的是一种常见的离散航迹点建模方式：

把起点到终点的路径离散为 N 个中间路径点，每个路径点用三维坐标表示：

[ P_i = (x_i, y_i, z_i), \quad i = 1, 2, \dots, N ]

整个航迹就是 N 个坐标点组成的序列。算法优化的不是每个栅格，而是一组连续的坐标组合。这样做的直接好处是维度可控——N取10~20就已经有相当强的路径表达能力，比栅格法的搜索空间小几个数量级。

适应度函数是整个优化过程的核心。我采用的代价由四部分构成：

• 航迹长度代价：相邻路径点之间的欧氏距离之和，这个当然是越短越好；
• 威胁代价：路径点穿越威胁区域时施加惩罚，用高斯型函数来模拟雷达或防空威胁的强度，距离威胁中心越近代价越大；
• 高度代价：每个路径点的高度与理想巡航高度之间的偏差，用于避免出过高或过低的剧烈起伏；
• 平滑代价：相邻两段航迹之间的夹角变化，夹角越大代价越高，用于抑制急转弯。

总适应度函数写成加权和的形式：

[ J = w_1 \cdot L + w_2 \cdot T + w_3 \cdot H + w_4 \cdot S ]

权重系数 w1~w4 的取值直接影响最终航迹形态。我在实验中常用的初始配置是 w1=1.0，w2=2.0，w3=1.0，w4=3.0。威胁和光滑度权重给得高一点是有意的——航迹短但撞威胁区，那再短也没用；省油但飞出一堆锐角弯，实际飞控也执行不了。

这里有个建模细节容易被忽略，也是我在项目中踩过的一个很深的坑：威胁代价的计算不能只检查航迹节点是否在威胁区内，因为节点稀疏可能会漏掉两节点之间的航段穿越威胁区的情况。正确做法是对每段航迹进行密集采样，每隔一个小步长取采样点判断威胁代价叠加。否则算法会“钻空子”，把节点放在威胁区边缘外，但连线直接穿过威胁区中心。这个后面在常见问题章节我再详细说。

2. 算法原理与融合改进思路

2.1 标准鲸鱼优化算法（WOA）核心回顾

鲸鱼优化算法是模拟座头鲸捕食行为的启发式算法，2016年提出之后因为结构简单、参数少、收敛快，成了优化算法改进领域的大热门。核心机制是三种位置更新策略：

第一阶段是包围捕食。算法根据当前最优个体位置，让其余个体向它收缩包围：

[ X(t+1) = X^*(t) - A \cdot D ]

其中 D 是当前个体与最优个体之间的距离向量，A 是由收敛因子 a 决定的系数向量。收敛因子 a 随着迭代次数从2线性递减到0，A 的取值范围也随之动态变化，前期大范围探索，后期小范围收缩。

第二阶段是气泡网攻击。鲸鱼吐气泡把鱼群驱赶向水面，算法里用螺旋方程模拟：

[ X(t+1) = D' \cdot e^{bl} \cdot \cos(2\pi l) + X^*(t) ]

这个螺旋更新让个体的搜索轨迹呈现对数螺旋形态，能保证对最优解邻域的精细搜索。

第三阶段是随机搜索。当系数 A 的绝对值大于1时，个体放弃参考当前最优位置，而是从种群中随机选一个个体作为引导者。这个机制是全局探索的关键，但从我的实验经验看，也是WOA早期收敛慢的一个原因——随机引导方向太盲目，很多个体跑出去很远，浪费大量评估次数。

WOA的优点很明显：只有收敛因子 a 和两个随机向量需要调节，结构极简，实现几十行Python搞定。但它的短板同样致命：整个种群在包围和螺旋阶段都围着当前最优个体打转，一旦这个最优个体是个局部最优解，整个种群就被“锁死”在这个解附近，几乎没有跳出来的能力。这在高维航迹规划问题中尤为突出。

2.2 粒子群优化（PSO）为什么能互补

粒子群算法模拟的是鸟类觅食的群体智能行为。每个粒子维护一个位置和速度向量，迭代过程依赖两个记忆信息：个体历史最优 pbest 和全局历史最优 gbest。速度更新公式：

[ v_{i}(t+1) = \omega v_i(t) + c_1 r_1 (pbest_i - x_i) + c_2 r_2 (gbest - x_i) ]

位置更新就是速度叠加。

PSO和WOA一个本质区别在于：每个粒子有自己的“历史视野”，即使当前位置很差，它也不会彻底丢掉过去找到过的好位置。个体历史最优和全局历史最优形成了双重信息引导，种群内的信息流动是“广播式”的。这样的结果就是PSO的全局搜索覆盖能力比WOA强，不容易因为当前种群最优个体的糟糕位置而整体崩溃。

另一个互补点在速度机制。PSO的速度累计效应让粒子能够在搜索空间中保持一定的运动惯性，不会像WOA的收缩包围那样从迭代初期就形成强烈的“向当前最优靠拢”趋势。这正好可以弥补WOA随机搜索阶段“跑飞了拉不回来”的问题。

但PSO也有自己的毛病：没有像WOA螺旋方程那样细腻的局部开发机制，迭代后期粒子速度更新幅度往往偏大，在最优解附近振荡，收敛精度不足。两者在机制上恰好是互补的。融合的思想确实不新鲜，但怎么合、在哪个环节合、合了之后参数怎么调，才是真正考验人的地方。

2.3 融合改进的三个关键设计

我在做融合设计时，没有采用生硬的办法，比如把WOA和PSO各自跑一遍再取结果，那样做技术含量太低而且效果不稳定。我选择了三个层面的融合改进：

第一个层面是种群分层。每次迭代时把种群按适应度排序，排名前一半的个体保持WOA策略——这部分个体已经处于较优区域，用螺旋和包围策略做精细搜索；排名后一半的个体采用PSO策略——它们离最优解还远，需要更强的探索能力，用pbest和gbest引导来快速改善。这样做的理由是：在同一个种群内，不同个体其实处于不同的搜索阶段，用同一套策略本身就是不合理的。

第二个层面是信息共享。WOA在优化过程中只知道当前全局最优位置，不明白“历史最优”价值。我在改进中给每个鲸鱼个体加入了一个个体历史最优的存储，PSO更新部分自然使用pb、gb信息，WOA部分也把个体历史最优引入到位置更新判据中。这使得种群的搜索方向不单一依赖当前最优，降低陷入局部最优的概率。

第三个层面是收敛因子动态调整。标准WOA的 a 是线性递减的，而融合算法中我给 a 增加了非线性衰减因子，迭代前期衰减幅度小，让种群保持更长时间的高探索能力；迭代后期迅速衰减，缩短精细搜索的收敛时间。实测对比下来，这种非线性调参策略比线性能快10%~20%达到同等精度。

按我自己的理解打个比方：标准WOA像个闷头猛干的技术员，认准一个方向就死磕到底；PSO像个消息灵通的项目组，同事之间频繁互通有无。融合算法就是让项目组里的老手死磕细节，同时让新人不断在外面试探新方向，两边各干各擅长的活。

3. Python实现细节与核心代码

3.1 代码结构总览

这次项目的Python代码结构大致如下：

project/ ├── environment.py # 地图与威胁区建模 ├── fitness.py # 航迹代价计算 ├── woa_pso.py # 融合优化算法主逻辑 ├── plot_results.py # 三维可视化 └── main.py # 主程序入口

环境建模部分我采用的是栅格地图叠加高斯基威胁模型的方法。地图尺寸设为100×100×100的空间，单位是米，地形用二维正弦函数叠加模拟起伏，三个威胁区用自定义的中心点和覆盖半径描述。威胁代价函数直接用高斯函数计算。

适应度函数的设计值得多说一点。当时我写了一个专门计算航迹的代价的类，避免在进化过程中反复重建函数。代码如下：

def fitness_path(points, env, weights): # points: shape (N+2, 3)，包含起点、中间点、终点 n = len(points) - 1 length_cost = 0.0 threat_cost = 0.0 height_cost = 0.0 smooth_cost = 0.0 for i in range(n): p1 = points[i] p2 = points[i+1] dist = np.linalg.norm(p2 - p1) length_cost += dist # 对航段密集采样计算威胁代价 samples = np.linspace(0, 1, 10) for s in samples: pt = p1 + s * (p2 - p1) for tc in env.threat_centers: d = np.linalg.norm(pt - tc) threat_cost += weights[1] * np.exp(- (d ** 2) / (2 * env.threat_sigma ** 2)) # 高度代价 height_cost += weights[2] * abs(p2[2] - env.cruise_altitude) # 平滑代价，利用向量夹角 if i >= 1: v1 = p1 - points[i-1] v2 = p2 - p1 cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-10) smooth_cost += weights[3] * (1 - cos_angle) total_cost = length_cost + threat_cost + height_cost + smooth_cost return total_cost

这段代码的核心在于威胁代价的航段采样法。我专门做了10等分的采样，而不是只看节点本身，这个问题前面已经提过，这里不再展开。

3.2 融合算法主流程

融合算法的主流程大致可以描述为以下步骤：

1. 初始化种群。每个个体是一个 (N+2, 3) 的坐标数组，前两维是起点和终点，固定不变；中间的 N 维坐标在边界内随机初始化，并加一点朝终点的线性偏移，让初始种群不至于太散。这一步的经验是：初始化质量直接决定收敛速度，如果纯随机生成，前30%的迭代基本浪费在“找到大致方向”上。

2. 计算每个个体的适应度，记录全局最优 gbest 和个体历史最优 pbest。

3. 进入迭代循环。每次迭代先对种群按适应度排序，前一半个体执行WOA策略，后一半执行PSO策略；每迭代10次，两部分个体随机交换一次身份，防止分层导致种群分裂。

4. 更新收敛因子 a 为非线性衰减形式。

5. 判断是否触发边界反弹。如果坐标越界，我把对应维度拉回边界内，再赋予一个指向最优解的微小偏移。直接置为边界值会导致大量个体堆在边界上，这会严重影响多样性。

6. 迭代结束后输出gbest对应的路径点坐标，还原为真实三维航迹。

核心的融合更新伪代码逻辑如下：

def fused_update(whale_pos, pbest, gbest, a, A, C, mode): if mode == 'woa': p = np.random.rand() if A < 1: # 收缩包围 D = abs(C * gbest - whale_pos) new_pos = gbest - A * D else: # 随机搜索 rand_whale = select_random_from_pop() D = abs(C * rand_whale - whale_pos) new_pos = rand_whale - A * D # 螺旋机制以0.5概率覆盖 if p >= 0.5: D2 = abs(gbest - whale_pos) new_pos = D2 * np.exp(1) * np.cos(2 * np.pi * np.random.rand()) + gbest return new_pos else: # mode == 'pso' v = w * velocity + c1 * r1 * (pbest - whale_pos) + c2 * r2 * (gbest - whale_pos) new_pos = whale_pos + v return new_pos

这种模式下，WOA部分负责局部细化，PSO部分负责全局探索，两者在同一个框架下协同进化。运行效率方面，因为这个实现没有使用向量化优化，种群50、迭代300次的完整实验在普通笔记本上大约耗时35秒，可以接受。如果后续要大规模跑参数寻优，建议用numpy对种群整体批量计算，把内层循环去掉，速度能提升一个数量级。

3.3 参数设置与调优经验

融合算法涉及到的参数比标准WOA多了一些，逐个说下我实际调试后趋于稳定的取值：

调参的经验我用下来就是一句话：先固定其他参数，只调一个参数，一次只动一个。很多同学一上来就同时动好几个参数，最后出了问题都不知道是哪个参数导致的。比如我调节收敛因子衰减指数时，会保持其他所有参数不变，连续跑20次，用统计平均来看效果，而不是单次运行就下结论。

这里还有一个非常关键的实现细节：速度初始化不能全零。在PSO部分，如果初始速度全部设置为0，第一个迭代周期里所有粒子都基本不动，等于浪费了一次评估。我采用的方案是用当前个体与全局最优的差值乘以0.1作为初始速度，这样第一代就能产生有效的移动。

4. 仿真实验与收敛性分析

4.1 实验环境与对比配置

实验环境就是最普通的硬件配置：一个主流桌面级CPU，Python 3.9环境，核心依赖numpy、matplotlib。没有GPU，因为这类算法本身就是CPU计算主导的，GPU加速收益不大，除非你要把种群规模推到几千上千的水平。

对比实验我设置了四组：标准WOA、标准PSO、未经融合但做了非线性收敛因子改进的WOA、本文融合算法。四组算法使用完全相同的环境模型、初始种群特征（随机种子一致）、最大迭代次数和种群规模，这样对比才算公平。否则你改进算法直接从初始化策略动手脚，作出的对比图再好看也是自欺欺人。

实验中固定参数为：种群规模40，迭代次数400，路径点N=12，地图边界100×100×100，起点坐标(10, 10, 20)，终点坐标(90, 90, 80)。三个威胁区中心分别分布在路径中段的上方、左侧和右侧，覆盖半径各不相同。

4.2 收敛过程与效果对比

从收敛曲线上看，四种算法的差异非常直观：标准PSO前期收敛速度中等，后期曲线仍然有明显的阶梯式下降，说明它具备一定的跳出局部极值的能力但效率偏低。标准WOA前期收敛极快，大约在80代左右就趋于平缓，形成一个平台，这个平台对应的往往就是局部最优解。非线性收敛因子的改进WOA比标准WOA稍有改善，但是平台期依然明显。

融合算法走了一条更均衡的收敛曲线：前50代快速下降，然后在150代左右赶超改进WOA，最终在250代之后稳定在一个更低的适应度值上。统计20次独立运行的结果，融合算法的平均最终适应度比标准WOA降低了18.6%，比标准PSO降低了15.2%，标准差也是四组中最小的，说明算法不仅找到了更优解，还具有更好的稳定性。

这个结果背后反映出的机制是：种群分层策略让探索和开发没有相互干扰。早期大量的探索性粒子不断发现新的优质区域，开发性粒子的存在又保证了找到一个好区域后不会轻易丢掉；中后期分层交换又把那些探索够了但一直做无用功的个体切到精细化开发模式，整体效率自然就上去了。

4.3 三维航迹可视化结果

三维可视化我直接用的matplotlib的mplot3d画出来的。地图曲面、威胁区球体、航迹折线三类元素叠加到一个图里。这里有一个小的绘图技巧：威胁区用透明度0.3的半透明球体绘制，这样能直观看到航迹是从球体外侧绕过去的，而不是在地图平面上简单看一个空洞。

实际生成的最优航迹基本呈现“先爬升、绕左侧威胁区、再降高度接近终点”的形态。起点到第一个中间点有大约20米的高度上升，这是为了避开起点附近一个低压威胁区域；中段航迹在x-y平面有明显绕行，将第三个威胁区的覆盖范围完整避开；末段高度逐渐降低到终点高度附近，整体高度曲线没有剧烈的锯齿状波动。平滑代价的数值在整个适应度中所占比例控制在15%以下，说明生成的航迹没有明显的急转弯或垂直俯仰突变。

对比实验中标准WOA生成的最佳航迹也有一定绕行能力，但有几个节点距离威胁区边界过近，安全裕度不够。如果你的实验目标是追求航迹安全，一定要在威胁代价函数里加一个“安全缓冲区”，就是判断距离小于威胁半径+安全裕度时就视为已进入威胁区。这个细节很多文献里不写，但实际工程里不可或缺。导航和安全评估不是一回事，做仿真时算的是理论代价，真放无人机上飞的时候，飞控需要的是安全、平滑、可执行的轨迹，而不是一条贴着理论边界擦过去的“最优路径”。

5. 常见问题与避坑指南

5.1 粒子越界与不可行解

越界问题在三维航迹规划里几乎是必定会遇到的。路径点坐标的每个维度本来就有边界限制，比如z坐标不能小于地形高度、不能高于最大飞行高度。处理和这个紧密相关的“越界铁律”就是必须处理，不能不管。

我今天的建议是采用“反弹并微调”的策略。粒子越界后不简单拉回边界，而是：

new_pos[dim] = boundary_min if new_pos[dim] < boundary_min else boundary_max new_pos[dim] += np.random.uniform(-0.5, 0.5)

在边界上再加一个小幅扰动，避免粒子在边界附近聚集。如果直接置为边界值，要不了几代，种群中会有大量的粒子堆在边界面上，不仅搜索效率变差，还容易让适应度函数在边界附近出现假的最优平台。

5.2 局部收敛与早熟判断

判断一个优化算法是不是陷入局部最优的办法，最直观的是观察适应度曲线下降速率。如果连续50代最优适应度几乎没有变化，并且种群的空间分布标准差已经很小，那大概率就是早熟了。融合算法虽然比标准WOA强很多，但也不是百分百免疫早熟，种群规模如果只有20，照样会掉坑里。

我的一个判断技巧是：每隔20代，计算一次种群内所有个体coordinates的方差。方差小于某个阈值（比如地图尺寸的5%）时就触发“种群重启策略”。具体做法是保留当前最优个体的历史最优位置，重新随机生成30%的个体，并加以一个方向抖动。这个策略简单有效，代码量也不大，是提升算法稳定性最实用的技巧之一。

5.3 适应度函数的权重敏感性

很多人做多目标加权时会轻视权重的影响。我在实验中发现，威胁权重从2.0调到4.0，最终航迹的形态会发生明显变化；而高度权重从1.0调到2.0，影响却不大。这说明不同代价分量对权重的敏感度差异很大。

在做融合算法对比实验的时候，最稳妥的做法是先固定权重系数，再把权重作为超参数单独跑一组成对敏感性实验。如果在核心算法对比里改变权重数值，最后图做出来好看，但你根本说不清改善到底是算法带来的还是权重调整带来的。这属于学术严谨性的问题。我的做法是：对比实验权重固定，附加实验中单独分析权重影响。

5.4 实际工程中执行与仿真的差异

仿真跑通的航迹和真实无人机执行的航迹之间还有一道鸿沟。仿真中的节点可以任意转弯，但真实固定翼无人机有最小转弯半径、最大爬升角、最大俯仰角等动力学约束。所以我在仿真模型里加入了一个隐含限制：相邻航段的夹角变化率约束——通过在适应度函数里对夹角大于45度的航段施加强惩罚来实现。

这个设计在只有理想点坐标的场景下不够精确，但足以让生成的航迹在形态上具备可飞性。如果你真要把算法部署到飞控里，我的建议是再加两个后处理模块：一是B样条插值平滑航迹，二航迹压缩（去除冗余中间点）。这是工程化的一步，小车或无人机实际到手飞之前基本都要做这两步。我能给出的一点经验是，如果你的最终目标是论文对比出图，那当前代码已经足够了；如果目标是真机上跑道测试，后处理模块的时间投入至少跟算法改进一样多。

6. 后续改进方向与个人体会

6.1 融合思路可以继续延伸的地方

融合粒子群的改进，本质上是在尝试“探索-开发”平衡的第三次分配。沿着这条思路往下走，还有几个方向值得你深入：

一是把融合从种群层面推进到个体层面的自适应切换。比如根据个体自身的适应度变化趋势，动态判断它是该多探索还是该多开发，而不是按排名硬切。这个思路在部分动态环境任务分配文献里有雏形，但实现起来还有不少坑要踩。

二是引入差分进化的变异算子。差分进化的“变异-交叉-选择”机制在全局搜索上很有特色，如果在WOA包围阶段引入DE/best/1变异策略，可能会比纯PSO融合获得更好的扰动多样性。我之前快速试过一版，收敛精度略有提升，但收敛速度变慢了，需要小心权衡。

三是结合强化学习的参数自适应。把收敛因子、惯性权重这些参数变成一个策略网络的输出，算法根据种群状态实时调节。这个方向的问题是计算开销大，适合大规模并行环境，不太适合普通PC上跑的仿真项目。

6.2 个人实操中的体会

做这个项目前后花了三周时间，最深的体会是：改进算法本身是加分项，但建模细节才是决定能不能发出来或者能不能用的分水岭。威胁代价的航段采样法、边界反弹微调、种群分层交换，这三个细节任何一个没做好，实验结果都会出现“算法看着很高级但路径明显不合理”的情况。

还有一点就是做算法对比实验一定要有耐心跑足够多的次数。元启发式算法随机性大，单次运行结果很不可靠。我每次对比都是跑20次取均值和标准差，虽然耗时，但结果经得起推敲。

如果你也打算做类似方向，我的建议是：先在标准WOA上把代码和三维可视化链路跑通，再加PSO融合逻辑，最后再引入其他改进策略。一步步来，不要一上来就想把五个改进塞进一个代码文件里。优化算法这东西，单一改进措施效果明确，叠加起来的交互作用却可能让整个系统变得一塌糊涂。先保证每个模块单独可用，再考虑集成。这是我踩过几次坑之后得出的最实用建议。