如果你做过无人机的山区测绘或电力巡检大概会明白一个尴尬飞控调参可以靠手感但航迹规划没有那么多试错机会——飞机撞上山头就是实打实的损失。三维航迹规划说白了就是在一堆山峰、禁飞区、高度约束之间找出一条又短、又平滑、又安全的航线本质是一个高维非线性优化问题。我最近在做一个模拟项目时把粒子群算法PSO的速度记忆机制融进改进版鲸鱼优化算法WOA的位置更新流程中用纯Python写了一套可运行实现跑下来的收敛速度和航迹质量都明显优于单独的PSO和标准WOA。这篇文章把从问题建模、算法融合、代码落地到调参避坑的完整过程完整捋一遍给正在做无人机航迹规划、机器人避障或智能优化算法的朋友一份可直接参考的模板。1. 无人机三维航迹规划的核心拆解1.1 航迹规划的数学本质航迹规划在数学上可以看成“在三维空间中寻找一个满足约束的最优连续路径”。常规做法是把连续问题离散化在起点和终点之间插入若干航迹控制点每个控制点有x、y、z三个坐标再用这些控制点拟合出一条完整航线。如果有K个控制点优化变量就是3K维空间中的一个向量。当K取10维度就是30山峰和障碍物的存在让目标函数压根不是凸函数到处都是局部坑位。这类问题难在三个点。第一目标函数没有解析梯度路径长度、威胁程度、平滑性、高度偏差这些代价指标混合在一起导数根本算不出来。第二约束复杂有地形约束、障碍物碰撞约束、最大转弯角约束、飞行高度区间约束。第三维度较高搜索空间大传统枚举法完全不现实。所以实际的航迹规划工程里大家很少用纯数学规划更多是上启发式算法——遗传算法、粒子群、模拟退火、蚁群以及近年比较火的鲸鱼优化算法。启发式算法的好处是不需要梯度信息只要把代价函数定义清楚就能在解空间里瞎撞乱试最后试出一条可行航迹。1.2 标准启发式算法的三处硬伤我早前用过标准粒子群算法做航迹规划踩过不少老坑。第一处硬伤是后期收敛慢PSO的所有粒子都在向群体最优靠拢到后期速度越来越小种群多样性急剧下降搜索就接近于原地踏步。第二处硬伤是容易早熟尤其是在复杂地形下一旦某个粒子发现一条相对短的路径其它粒子会被快速吸引过去逼得航迹钻进局部最优解。第三处硬伤在标准WOA身上也有——它对初始种群位置比较敏感初始种群分布不好后面怎么优化都绕不出劣质区域。这三处硬伤直接催生了融合算法的思路。既然PSO靠个体历史最优和全局最优的“记忆引力”做搜索而WOA靠包围猎物的收缩机制和气泡网螺旋更新做开发那能不能让它们互相补短板答案是可以而且效果比我原本预想的好。1.3 自然启发式算法的适用逻辑你要问为什么偏偏用这类仿生算法而不是梯度下降或者牛顿法最直接的原因是航迹代价函数根本无法求导。地形是个山峰叠加的数字高程图威胁代价是离散点碰撞检测的结果平滑代价要逐段算夹角——这里面任何一个环节都让代价函数变得不可导、不连续。启发式算法不关心函数内部长什么样只关心输入一组航迹控制点后输出的代价数值这正好对上了航迹规划这种“黑箱优化”的需求。另外三维航迹规划的搜索过程本身和鲸鱼捕食有很强的相似性。起点到终点就好比猎物位置无人机航迹就是鲸鱼逼近猎物的路径。WOA里“随机探索-包围收缩-螺旋修正”三个阶段的节奏天然适配“先全局搜索找潜力区域、再局部细化打磨航迹”的规划逻辑。这也是我最终把改进WOA作为主框架而不是反过来用改进PSO做主框架的原因。2. WOA与PSO机制剖析及融合设计2.1 标准WOA的三种搜索策略鲸鱼优化算法模拟的是座头鲸的泡泡网捕食行为。三套位置更新机制里最核心的是包围猎物机制D |C · X*(t) − X(t)| X(t1) X*(t) − A · D这里X*(t)是当前最优个体位置A和C是由收敛因子a派生的系数向量a从2线性递减到0。当|A| 1时鲸鱼向最优个体收缩相当于局部开发当|A| ≥ 1时算法强制随机挑选一个个体来更新位置相当于全局探索。第二套机制是气泡网螺旋更新X(t1) X*(t) D · e^(bl) · cos(2πl)D是当前个体到最优个体的距离b是螺旋形状常数l是[-1,1]间的随机数。这一机制让鲸鱼一边收缩一边绕圈逼近猎物局部搜索能力很强。第三套是随机搜索猎物和|A| ≥ 1时的行为类似。标准WOA的问题在于螺旋更新全部以全局最优为锚点一旦全局最优陷入局部坑位整个种群都会跟着陷进去初始收敛因子线性递减也让算法在探索和开发之间切换得过于生硬。2.2 PSO为融合提供的“记忆引力”粒子群算法的核心基因是两个“最优记忆”——每个粒子保存自己的历史最优pbest整个群体共享一个全局最优gbest位置更新靠速度来完成v w·v c1·r1·(pbest − x) c2·r2·(gbest − x) x x vpbest相当于个体记忆gbest相当于群体经验c1和c2分别控制“相信自己还是相信大家”的比重。惯性权重w决定上一时刻的速度保留多少前期w大飞得远探索得多后期w小飞得稳收得更细。WOA缺的正是这套记忆机制。WOA每次迭代的位置更新只依赖当前最优位置和随机系数个体自己的历史经验完全被丢弃了。也就是说一条鲸鱼以前找到过一个不错的区域但下一轮它可能完全忘掉跳去其它地方瞎逛。融合PSO后pbest和速度项能帮WOA想起“以前哪个位置是有前途的”gbest则提供群体层面的引导搜索效率自然就上去了。2.3 PSO-ImWOA融合的整体思路我的融合方案通俗点说就是让WOA当主体框架在每一轮迭代里给每条鲸鱼两个选择——按PSO的方式更新一次或者按WOA的方式更新一次。选择的概率根据当前迭代进度动态变化。前期迭代让PSO的占比高一些充分利用速度项的漫游能力和pbest的分散记忆探索整个搜索空间种群不至于被过早吸到一起。后期迭代逐步提高WOA占比靠它的包围收缩和螺旋更新把探索到的优质区域打磨精细。此外WOA的包围猎物公式里原本只用当代最优X*(t)引导我把它改成“当代最优和个体历史最优的加权引导”相当于鲸鱼不仅跟着群体老大走也参考自己的成功经验——这才是“融合PSO的改进鲸鱼优化算法”的核心含义。3. 改进鲸鱼优化算法的四个关键设计3.1 佳点集初始化替代随机初始化标准WOA用均匀随机数生成初始种群大问题是种群在解空间里分布不均匀可能十条鲸鱼全挤在一半空间里另一半区域压根没被搜索到。我用佳点集初始化来替代随机初始化——佳点集的数学背景是数论里的低偏差序列它能保证有限个样本点在单位超立方体内分布得尽量均匀。实现上不复杂先生成均匀分布的随机数再按固定比例映射到搜索边界内。代码大概这样def good_point_set(n, dim, bounds): # 佳点集初始化n为种群规模dim为维度 alpha np.zeros(dim) for d in range(dim): alpha[d] 2 * np.cos(2 * np.pi * (d 1) / (dim 2)) points np.zeros((n, dim)) for i in range(n): r (i 1) * alpha % 1.0 points[i] bounds[:, 0] (bounds[:, 1] - bounds[:, 0]) * r return points佳点集初始化实测的效果是收敛代次提前了大概15%~20%代价函数均值也更稳定跑多次实验的方差明显低于随机初始化。3.2 余弦型收敛因子代替线性递减标准WOA里a从2匀速衰减到0问题在于搜索前期探索任务重a衰减太快会过早进入局部开发阶段搜索后期需要精细打磨a又降得太慢导致收敛曲线末尾拖尾明显。我改用余弦曲线来调aa 2 · cos(0.5 · π · t / T_max)t是当前迭代数T_max是最大迭代数。这条曲线的特点是前期衰减慢a保持在较高水平给了足够的全局探索空间中期快速下降切入开发阶段后期又平缓贴近0让算法在最优解附近做精细收缩。这个改动简单但非常有效代价函数下降曲线明显更均衡不再出现前期狂降后期趴窝的现象。3.3 精英引导与速度记忆融合这是整个改进的核心。在WOA的包围猎物机制里标准公式用全局最优X*做引导我加入了个体历史最优pbest做双参考D |C · (w_p · pbest (1 − w_p) · gbest) − X(t)| X(t1) (w_p · pbest (1 − w_p) · gbest) − A · Dw_p是历史最优权重随迭代进行逐渐从0.4降到0.2。前期多参考自己的历史成功经验保持个体探索的多样性后期多参考群体最优加快收敛到高精度解。螺旋更新同样做了改造锚点从单一gbest改为“gbest和pbest的中点”。这相当于鲸鱼绕圈逼近猎物时既不会完全丢掉自己的判断又不会偏离群体大方向太远。3.4 动态概率切换机制融合不是让每条鲸鱼每轮都同时用两种机制更新而是按概率二选一。我定义了一个切换概率p_switch 0.7 − 0.4 · (t / T_max)前代p_switch约为0.7多数个体走PSO更新路线利用速度惯性扩散开后期p_switch约0.3多数个体切换到WOA收缩路线。每次更新前生成一个随机数rr p_switch走PSO分支否则走WOA分支。这个设计的奥妙在于它给种群引入了“异步性”——同一时刻有的鲸鱼在全局探索有的在局部开发比全体同步切换更接近真实生物群体的行为也更能避免整个种群步调一致导致的振荡。4. 航迹规划建模与代价函数设计4.1 三维地形与障碍物建模航迹规划实验不可能真去飞无人机模拟环境是第一块地基。我用高斯峰函数叠加生成带起伏的三维地形def build_terrain(x_range, y_range, step5.0): xs np.arange(x_range[0], x_range[1] step, step) ys np.arange(y_range[0], y_range[1] step, step) X, Y np.meshgrid(xs, ys) Z np.zeros_like(X, dtypefloat) peaks [ (150, 180, 220, 160, 160), (360, 250, 180, 120, 150), (520, 620, 260, 190, 140), (680, 380, 200, 130, 130), (820, 780, 240, 150, 170), (260, 810, 160, 140, 120), ] for cx, cy, h, sx, sy in peaks: Z h * np.exp(-((X - cx) ** 2 / (2 * sx ** 2) (Y - cy) ** 2 / (2 * sy ** 2))) return X, Y, Z坐标里每个峰代表一座山h是峰值高度sx和sy控制山体胖瘦。实验空间取1000m×1000m飞行高度上限400m。除了山峰我还在地图上额外放置了几个圆柱体障碍高度150m至250m模拟建筑物与禁飞区域。4.2 航迹编码与B样条插值平滑航迹不直接用控制点的折线因为相邻控制点硬连起来会出现尖锐转角无人机根本飞不出来。我用B样条插值把控制点拟合成平滑曲线。插值点需要在地形采样点上逐一计算高度与碰撞信息。def bspline_interpolate(control_pts, segments_per_knot8): from scipy.interpolate import splprep, splev pts np.array(control_pts) tck, _ splprep([pts[:, 0], pts[:, 1], pts[:, 2]], s0) u np.linspace(0, 1, segments_per_knot * (len(pts) - 1)) x, y, z splev(u, tck) return np.column_stack([x, y, z])控制点数量选多少直接影响效果。我试过6个、10个、15个三种配置10个点是最佳平衡点太少的话样条拟合过于僵硬绕不开复杂障碍太多的话搜索维度涨到30以上算法收敛变慢且样条容易过拟合出现毫无意义的波浪形航迹。4.3 四分量代价函数与权重设计代价函数是整个算法的指挥棒航迹质量好不好完全看代价函数定义得是否准确。我用四个分量加权求和J w₁·J_length w₂·J_height w₃·J_smooth w₄·J_threatJ_length是插值后航迹的总长度短航迹省电省时间。J_height是全部航迹点的平均高度与预设巡航高度之差的绝对值平均飞太低危险、飞太高费电二次惩罚更合适。J_smooth是相邻航迹段的偏航角变化量平均值转弯太急的航迹会被惩罚。J_threat是威胁代价每个航迹点到障碍物中心的距离如果小于“障碍半径安全距离”就按二次函数产生一个巨大惩罚。权重分配是我反复调出来的经验值代价分量权重设计原因航迹长度2.0追求短路径但不希望以安全为代价高度偏差1.5保持经济巡航高度平滑程度2.5减少转弯角保证可飞性威胁代价4.0安全最高优先穿障惩罚最重威胁代价里的安全距离取30m。权重比例不是死的如果场景里障碍物密集我会把威胁权重调到6以上如果场景空旷就是拼路径长度长度权重可以提到4。4.4 约束处理与可行航迹判定航迹约束我分成硬约束和软约束。硬约束是位置边界和最低安全高度超出搜索边界的位置直接用反射法修正——把越界分量弹回边界内侧这样处理比裁剪法更平滑。软约束是最大转弯角和碰撞威胁统统折算进代价函数里让算法自己去权衡。可行航迹的判定规则是整条插值后的航迹没有任何一个点进入障碍物危险半径并且所有点高度不低于该位置地形高度加安全离地距离。如果算法输出结果不满足我就认为这次搜索失败。跑对比实验时每一轮的代价计算都会附带碰撞标记方便统计碰撞率。5. Python工程实现与实验对比5.1 工程代码结构整个项目我用模块化组织文件结构如下uav_planner/ ├── environment.py # 地形、障碍物、边界环境 ├── cost_function.py # 航迹代价计算 ├── pso_imwoa.py # PSO-ImWOA主算法 ├── baseline_woa.py # 标准WOA对照算法 ├── baseline_pso.py # 标准PSO对照算法 ├── visualize.py # 三维可视化 └── main.py # 实验主入口环境和算法分开写的好处是后面想换地形、换障碍物、换代价权重时不需要动算法代码只要改environment和cost_function的接口整个框架都复用。5.2 核心代码逐段解析PSO-ImWOA的主体更新逻辑在update方法里我摘出最核心的一段def update(self, t, max_iter): a 2 * np.cos(0.5 * np.pi * t / max_iter) p_switch 0.7 - 0.4 * (t / max_iter) for i in range(self.n): r np.random.rand() if r p_switch: # PSO分支速度记忆 双最优引导 w 0.95 - 0.65 * (t / max_iter) self.vel[i] (w * self.vel[i] self.c1 * np.random.rand() * (self.pbest[i] - self.pos[i]) self.c2 * np.random.rand() * (self.gbest - self.pos[i])) self.pos[i] self.pos[i] self.vel[i] else: # WOA分支动态概率选包围或螺旋 if np.random.rand() 0.5: A 2 * a * np.random.rand() - a C 2 * np.random.rand() anchor 0.2 * self.pbest[i] 0.8 * self.gbest if abs(A) 1: D np.abs(C * anchor - self.pos[i]) self.pos[i] anchor - A * D else: rand_whale self.pos[np.random.randint(self.n)] D np.abs(C * rand_whale - self.pos[i]) self.pos[i] rand_whale - A * D else: # 螺旋更新以双最优锚点为中心绕圈 anchor 0.3 * self.pbest[i] 0.7 * self.gbest D np.abs(anchor - self.pos[i]) l np.random.uniform(-1, 1) self.pos[i] D * np.exp(l) * np.cos(2 * np.pi * l) anchor # 边界反射 self.pos[i] self.reflect(self.pos[i])这里有两个实现细节值得多说。一是速度初始化我用了和位置同维度的零向量避免初始速度过大导致前期乱飞二是PSO分支里的惯性权重w也做了线性递减从0.95降到0.3这样即使走PSO分支后期也能保证局部搜索强度。主循环里每轮更新完就要算新位置的代价同时更新pbest和gbest。更新pbest时我用了“新代价优于历史代价才替换”的规则但如果新位置满足所有硬约束而历史最优不满足无条件替换——安全航迹永远优先于短航迹。5.3 三路算法对比实验结果我用同一块地形、同一组起终点、同样的种群规模50和迭代次数100对标准PSO、标准WOA、PSO-ImWOA各跑了30次独立实验取平均值和统计指标。起终点设置是越复杂的越好起点在(0, 0, 120)终点在(1000, 1000, 150)中间横着三座山峰和一个圆柱障碍。算法平均航迹代价平均航程(m)平均收敛代次穿障率标准PSO132.615475812%标准WOA124.81521468%PSO-ImWOA115.21489310%融合算法在航迹代价上比标准WOA低了约7.7%比标准PSO低了约13.1%收敛代次缩短到31代就逼近最优穿障率在所有实验中为零。更有说服力的是稳定性30次独立实验中融合算法的代价标准差是5.3标准WOA是11.2标准PSO是15.7融合算法的稳定性优势非常明显这在工程上是实打实的好处。收敛曲线的对比也很有意思。标准PSO前20代下降很快到60代后彻底平缓标准WOA前期慢40代后才开始大幅下降PSO-ImWOA是全程保持较快的下降速度到30代左右就已经接近最终水平没有出现明显的停滞平台。5.4 关键参数对收敛性能的影响迭代跑多了我总结出几个对结果影响最大的参数。第一个是种群数N。N从20增到80收敛质量逐步提高但超过80后收益递减计算耗时翻倍N取50左右是性价比较高的值。第二个是控制点数量K。K8时代价为121.3K10时降到115.2K12时不降反升到118.9原因就是30维以上样条过拟合算法在无用弯曲上浪费了搜索预算。第三个就是代价权重比。威胁权重低于2时穿障率明显上升高于5时算法过度保守航迹绕大圈总航程显著增长。我的建议是威胁权重与长度权重保持在21到31之间比较合理。5.5 常见问题与调参速查表实际操作中一定会遇到坑这里直接给你一张速查表现象可能原因排查思路收敛曲线前期就趴窝收敛因子a衰减太快换余弦型收敛因子或调大初始a航迹穿障碍物威胁权重太低提高w₄到5以上加大安全距离航迹振荡剧烈控制点太多导致样条过拟合减少控制点数量提高平滑权重多次实验结果方差大初始化不均匀换佳点集初始化固定随机种子后期收敛极慢惯性权重降得太快惯性权重下限调到0.3降速放缓航迹飞行高度过高高度代价权重偏低把J_height改为平方惩罚并提高权重排查顺序建议是先看代价函数是否正确——把一条已知安全航迹扔进去算代价如果输出不是明显低于随机航迹代价函数先出问题了。再看算法更新逻辑——单独跑一轮打印所有个体的代价分布如果全部相同说明种群多样性丢失了。最后才调参数每次只改一个变量记录对照结果。三维可视化用matplotlib的Axes3D就够了地形用plot_surface画成半透明网格航迹用红色粗线叠加障碍物用散点或圆柱体标出。一幅带航迹的三维地形图生成后能直观地看到算法是否真正绕开了山峰这比盯着代价数值判断更高效。写在最后的实操体会项目跑通后最大的感受是启发式算法组合不是简单把两个方法的公式搅在一起关键在明确各自的擅长阶段和互补点。PSO的长处是记忆与信息共享WOA的长处是收缩与螺旋开发的局部精细能力融合的核心是把“历史经验”注入到鲸鱼的捕食行为里而不是让两种算法你一轮我一轮地机械切换。如果你是第一次接触这类代码建议从标准WOA开始跑通后在它的基础上逐步加佳点集初始化、加余弦收敛因子再加PSO融合每加一步做一次对比实验观察收敛曲线的变化。这样能清楚地感受到每一个改进粒度的贡献。这套实现后续还可以往多目标方向扩展比如同时优化航迹代价和被发现概率或者引入实时重规划机制应对动态障碍物回旋余地很大。最后再分享一个小技巧设计代价函数时把分量量纲统一起来会省掉很多调参时间。比如长度以米为单位高度偏差也以米为单位但平滑代价是角度值威胁代价是等级值。最好先把每个分量归一化到同一数量级再控制权重比例否则权重调半天实际起作用的只有量纲最大的那个分量。这个坑我踩过代价函数看起来公式很严谨一跑起来算法全被某个分量的数值尺度牵着鼻子走。先归一化再谈调参你会少走很多弯路。