无人机三维航迹规划这几年一直是个热门方向尤其把智能优化算法用进去之后论文和工程验证都出了不少成果。我在实际项目中试过粒子群、鲸鱼算法、差分进化这些常见套路说实话单用一种算法要么陷入局部最优要么后期收敛慢真正落地到三维航迹上总是差一口气。后来把粒子群优化PSO的位置记忆机制融合进鲸鱼优化算法WOA里做了个改进版本 PSO-ImWOA用 Python 完整实现了无人机三维航迹规划效果比单独用 WOA 或者 PSO 明显提升。这篇就把整个思路、建模过程、代码实现和调参踩坑经验完整写一遍给正在研究智能算法航迹规划的同学一个可以直接参考的工程样本。1. 整体设计思路三维航迹规划到底在优化什么1.1 航迹规划问题的本质无人机三维航迹规划说白了就是在三维空间中找一条从起点到终点的路径这条路径要满足飞行安全、航程短、平滑可飞这几个基本要求。和二维路径规划不同三维场景下多了高度维度的约束比如地形起伏、山峰障碍、禁飞区域飞行高度太低容易撞山太高又暴露目标或者油耗增加转弯太急物理上飞不出来。所以这个问题本质上是一个带约束的多目标优化问题。常规做法是把约束条件加权合并成一个适应度函数然后用优化算法去搜索一组航迹点使得适应度值最小。搜索的空间长什么样假设我们把航迹离散成 N 个中间航迹点每个点有 x、y、z 三个坐标那么优化变量的维度就是 3N。加上起点和终点是固定的整条航迹就是从起点出发依次经过这 N 个点最后到达终点。算法的任务就是找出一组最优的中间点坐标。1.2 为什么选择 WOA 作为基础算法鲸鱼优化算法是模拟座头鲸捕食行为的元启发式算法包括三个核心机制包围猎物、气泡网攻击和随机搜索。我选择它作为基础主要有几个原因。第一WOA 的参数相对少核心就是收敛因子 a 和螺旋常数 b调参压力比 PSO 小很多。第二WOA 的螺旋更新机制在搜索过程中天然有更好的探索能力不容易像标准 PSO 那样快速早熟。第三WOA 的代码结构清晰方便做改进和二次开发不管是后续加约束还是改编码方式改动成本都比较低。但 WOA 也有明显的短板。最突出的是它的位置更新完全依赖当前最优个体也就是种群中的头鲸如果这个头鲸落在局部最优区域整个种群都会被带着往局部收敛。另外WOA 缺少对个体历史经验的记忆机制每只鲸鱼只跟当前状态走不考虑自己曾经到过的更优位置这在复杂多峰的地形环境里是很吃亏的。1.3 PSO 的融合价值在哪里PSO 的核心思想就两条个体记忆和群体共享。每个粒子记住自己的历史最优位置pbest同时通过群体最优位置gbest实现信息共享。这个机制在收敛速度和局部开发能力上非常有优势但 PSO 的问题是后期多样性快速下降所有粒子都朝 gbest 靠拢容易早熟。那能不能把两者的优势拼在一起我在设计 PSO-ImWOA 的时候思路很明确保留 WOA 的螺旋搜索骨架同时在位置更新的时候引入 PSO 的速度惯性机制和全局最优引导。这样一来算法前期靠 WOA 的全局探索能力铺开搜索面后期靠 PSO 的 gbest 引导加快收敛同时用 pbest 保留个体的历史经验避免种群被单一个体带偏。这个融合思路的实现形式是在 WOA 的位置更新公式后面增加一个修正项相当于给鲸鱼加了一个速度向量当前位移的一部分朝自己的历史最优位置修正另一部分朝全局最优位置修正。这个修正项的强度由惯性权重和两个学习因子共同控制。2. 航迹建模与适应度函数设计2.1 航迹点的编码方式三维航迹规划的第一步是把航迹变成算法可以优化的变量。我采用最直接的坐标编码给定起点 S 和终点 E在 S 和 E 之间均匀插入 N 个待优化的中间点每个点包括 x、y、z 三个坐标。算法优化的就是这 N 个点的共 3N 个坐标值。为什么用这种编码方式而不是用距离-角度编码或者贝塞尔曲线控制点编码坐标编码实现最简单改造成本低调试方便。距离-角度编码虽然能天然满足一些运动学约束但维度之间的耦合性更强算法搜起来更吃力。贝塞尔曲线编码出来的航迹是很平滑但控制点的数量和位置跟实际航迹点的关系不直接后期检查威胁代价和高度约束的时候多一层转换反而不直观。不过坐标编码有一个必须处理的问题边界约束。每个航迹点的 x、y、z 都必须落在飞行区域范围内比如 x 在 0 到 2000 米之间z 在 50 到 500 米之间。越界处理我采用“吸附到边界”的策略而不是重新随机初始化。重新随机初始化会让种群在迭代中产生大量无意义的个体拖慢收敛吸附到边界则让个体始终在可行域内只是位置固定在边界上后续迭代中它还有机会被更新出去。2.2 适应度函数的四项加权指标适应度函数是航迹规划的灵魂设计得好不好直接决定算法能不能产生一条可飞的航迹。我用了四个指标的加权和。第一个是航迹长度代价。直接计算航迹所有相邻点之间的欧氏距离之和然后除以起点到终点的直线距离做归一化。这样处理的好处是长度代价的量纲稳定在 1 附近不会因为地图尺寸变化导致数值大小失真。第二个是威胁代价。在实际场景中威胁源可以建模成若干个以中心点为球心、具有作用半径的威胁区无人机进入威胁区就面临被探测或被击落的风险。威胁代价的计算方式是逐段检查航迹段到威胁源中心的最短距离如果距离小于安全半径就按距离的倒数累加一个较大的惩罚值。这个设计逻辑是离威胁源越近代价越大惩罚越剧烈算法自然会尽量让航迹远离威胁区。第三个是高度代价。无人机飞行高度不是越低越好也不是越高越好。低空容易撞地形高空耗油且暴露概率高。我在目标高度附近设了一个舒适区间比如 100 米到 150 米航迹高度落在这个区间内不产生高度代价超出区间则按差值线性增加代价。这样算法会尽可能让整条航迹维持在一个合理的高度带里。第四个是平滑度代价。无人机物理上无法完成直角转弯所以航迹中相邻航迹段的转角和爬升角都有上限。平滑度代价计算每个中间点处两个相邻航迹段的夹角包括水平转角和垂直爬升角如果超过最大允许值就累加惩罚。四个代价加权求和权重系数根据任务需求调整。如果任务更重视隐蔽性就把威胁代价的权重调高如果任务更重视飞行效率就把长度代价调高。def calc_fitness(route, env, weights): # route: 航迹点列表 [[x1,y1,z1], [x2,y2,z2], ...] # weights: [w_len, w_threat, w_alt, w_smooth] total_len sum(distance(route[i], route[i1]) for i in range(len(route)-1)) straight_len distance(route[0], route[-1]) len_cost total_len / straight_len threat_cost 0.0 for i in range(len(route)-1): threat_cost segment_threat_cost(route[i], route[i1], env) alt_cost sum(height_penalty(p[2], env.target_alt, env.alt_tol) for p in route[1:-1]) smooth_cost sum(smoothness_penalty(route[i-1], route[i], route[i1]) for i in range(1, len(route)-1)) return (weights[0]*len_cost weights[1]*threat_cost weights[2]*alt_cost weights[3]*smooth_cost)2.3 三维山地环境仿真测试算法的环境我用的是经典的山峰函数叠加模型。先生成一张数字高程图在上面叠加若干个高斯型山峰作为地形障碍再在特定位置布置威胁源。山峰函数的公式是[ z(x,y) \sum_{i1}^{M} h_i \cdot \exp\left(-\frac{(x-x_i)^2 (y-y_i)^2}{\sigma_i^2}\right) ]其中 (h_i) 是第 i 个山峰的高度((x_i, y_i)) 是山峰中心位置(\sigma_i) 是山峰的宽度系数。这个模型简单可控你可以随意调整山峰数量、位置和高度来构造不同难度的测试场景。实际验证时我一般生成 2000×2000 米的地图网格分辨率 10 米这样地形数据矩阵就是 200×200计算速度很快又能模拟足够的起伏细节。威胁源我在地图上放了几个球形区域半径 80 到 120 米不等。无人机航迹段如果进入威胁区域就按进入深度计算威胁代价。威胁模型也支持扩展成圆柱形、多层级风险区看具体场景需求。3. 算法核心机制WOA 与 PSO 的融合改进3.1 标准 WOA 的三大位置更新机制标准 WOA 有三种位置更新策略。第一种是包围猎物当 (|A| 1) 时鲸鱼向当前最优个体位置收缩靠近位置更新为[ X_{new} X_{best} - A \cdot |C \cdot X_{best} - X| ]第二种是气泡网攻击当 (p 0.5) 时采用螺旋收缩策略位置更新为[ X_{new} X_{best} D \cdot e^{bl} \cdot \cos(2\pi l) ]第三种是随机搜索当 (|A| \geq 1) 时鲸鱼随机选取一个个体作为参照进行全局探索[ X_{new} X_{rand} - A \cdot |C \cdot X_{rand} - X| ]这个机制是 WOA 探索能力的核心但问题也在其中。整套流程中没有任何一个环节让个体回顾自己的历史搜索经验。如果某只鲸鱼之前已经到过一个不错的位置但因为头鲸带偏导致它在后续迭代中偏离了那个位置它自己没有任何机制把它拉回来。这在多峰地形中是致命的。3.2 PSO 速度-位置模型如何嵌入 WOA我改进的核心操作是给 WOA 的三种位置更新结果都加一个“PSO 修正项”。具体做法是先按 WOA 的规则计算出 (X_{woa_new})然后以这一结果为基准叠加一个速度向量[ V_{new} w \cdot V c_1 \cdot r_1 \cdot (P_{pbest} - X) c_2 \cdot r_2 \cdot (P_{gbest} - X) ][ X_{new} X_{woa_new} V_{new} ]其中 (w) 是惯性权重(c_1)、(c_2) 是学习因子(r_1)、(r_2) 是 [0,1] 均匀随机数(P_{pbest}) 是每个个体自己历史上适应度最好的位置(P_{gbest}) 是整个种群的历史最优位置。这个设计的物理意义很直接鲸鱼在按 WOA 的策略游动的同时会朝自己曾经到过的最好位置和经验共享的最好位置各偏一点相当于在原来的搜索策略上叠加了一层记忆力和群体智慧。这样融合之后WOA 的全局探索能力保留了下来不容易像纯 PSO 那样早熟同时种群的收敛速度明显加快因为 gbest 的引导项提供了一条快速逼近最优区域的捷径。pbest 的加入则防止个体被头鲸带偏之后无法自拔等于多了一道保险。3.3 非线性收敛因子与惯性权重的自适应调整标准 WOA 的收敛因子 (a) 从 2 线性递减到 0这带来一个问题如果最大迭代次数设得比较大前期探索期过长后期开发期又太仓促如果设得小又容易过早进入开发阶段。我改用余弦形式非线性递减[ a a_{min} (a_{max} - a_{min}) \cdot \cos\left(\frac{\pi \cdot t}{2 \cdot T}\right) ]其中 (t) 是当前迭代次数(T) 是最大迭代次数。这样算法在前期维持较大的 (a) 值来保障全局探索中后期快速下降增强局部开发比线性递减更容易兼顾搜索广度和收敛精度。PSO 部分的惯性权重 (w) 我做了类似的调整从 0.9 线性递减到 0.4。前期较大的 (w) 保持个体速度惯性便于维持探索多样性后期较小的 (w) 让速度项衰减个体更快朝 pbest 和 gbest 收敛。实测下来这个组合比固定权重或者纯线性递减都要稳定多次独立实验的方差也更小。4. Python 代码实现与关键步骤解析4.1 算法主流程结构整体代码分四个模块环境建模、适应度计算、PSO-ImWOA 优化器、结果可视化。环境建模负责生成数字高程地图和威胁源适应度计算负责把一组航迹点转化为一个标量适应度值优化器是整个程序的核心负责种群初始化、迭代更新和边界处理可视化部分用 matplotlib 生成三维航迹图和收敛曲线。整个流程伪代码如下初始化种群随机生成 N 个航迹点组成的个体 计算每个个体的适应度初始化 pbest 和 gbest for t in range(max_iter): a cosine_decrease(t, max_iter) w linear_decrease(t, max_iter) for each whale in population: 按 WOA 规则更新位置包围/螺旋/随机搜索 计算 PSO 速度项 V 位置 WOA位置 V 边界吸附处理 计算新适应度 更新 pbest update gbest 记录收敛曲线 输出 gbest 对应的航迹绘制三维图4.2 PSO-ImWOA 核心更新代码下面是优化器中位置更新的核心代码片段我把 WOA 的三种更新策略和 PSO 修正项都放在了一起def update_position(self, whale, idx, t, a, w, gbest_pos): # whale: 当前个体shape (3*N,) # a: WOA收敛因子, w: PSO惯性权重 r1, r2 np.random.rand(2) A 2 * a * r1 - a C 2 * r2 p np.random.rand() b 1.0 l np.random.uniform(-1, 1) # 按WOA规则计算基准位置 if p 0.5: if abs(A) 1: # 包围猎物向当前最优个体收缩 X_woa self.best_pos - A * abs(C * self.best_pos - whale) else: # 随机搜索 rand_idx np.random.randint(0, self.pop_size) rand_whale self.population[rand_idx] X_woa rand_whale - A * abs(C * rand_whale - whale) else: # 气泡网螺旋更新 dist abs(self.best_pos - whale) X_woa self.best_pos dist * np.exp(b * l) * np.cos(2 * np.pi * l) # PSO速度修正项 v (w * self.velocities[idx] c1 * np.random.rand() * (self.pbest_pos[idx] - whale) c2 * np.random.rand() * (gbest_pos - whale)) self.velocities[idx] v # 融合位置更新 new_pos X_woa v return new_pos这个代码里有几个细节我特意处理过。第一WOA 公式里的绝对值算子我用 numpy 的 abs 实现注意和 np.linalg.norm 区分前者是逐元素绝对值后者才是范数。第二螺旋更新时用 np.exp(b*l) 来控制螺旋的螺距b 取 1.0 是经验值过大或过小都会让螺旋展开太快或太慢。第三PSO 的速度项直接加到 WOA 的更新结果上而不是替换 WOA 的位置这样保证 WOA 的探索骨架始终存在。4.3 三维航迹可视化可视化我用 matplotlib 的 3D 投影实现。除了画出航迹线本身我把数字高程地图也叠加上去用 plot_surface 绘制地形用 scatter 标记起点、终点和威胁源位置。这样生成的三维图可以直观地看到算法规划的航迹是否避开了山峰和威胁区。import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D fig plt.figure(figsize(12, 8)) ax fig.add_subplot(111, projection3d) X_map, Y_map np.meshgrid(range(map_w), range(map_h)) ax.plot_surface(X_map, Y_map, dem_map, alpha0.6, cmapterrain) route np.array(best_route) ax.plot(route[:,0], route[:,1], route[:,2], r-o, linewidth2.5, markersize4, labelPSO-ImWOA route) ax.scatter(route[0,0], route[0,1], route[0,2], colorg, s100, labelStart) ax.scatter(route[-1,0], route[-1,1], route[-1,2], colorb, s100, labelEnd) ax.legend() plt.show() plt.figure(figsize(8, 5)) plt.plot(convergence_curve, b-, linewidth2) plt.xlabel(Iteration) plt.ylabel(Best Fitness) plt.grid(True) plt.show()这里有个小技巧地形表面模型用 plot_surface 绘制的时候如果网格太密会非常卡。我实际测试时把 DEM 网格做了隔行采样降采样到 100×100 左右再绘制图形的视觉效果几乎不变但渲染速度快了好几倍。5. 实验对比与参数调优经验5.1 相同条件下的三组算法对比我做了三组对照实验分别用标准 WOA、标准 PSO、改进的 PSO-ImWOA在完全相同的三维地图和相同的起点终点条件下各跑 20 次。种群规模 30最大迭代次数 300航迹中间点数 N5也就是 15 个优化维度。结果很典型。标准 WOA 的平均最优适应度值最高而且 20 次实验之间的方差很大说明算法稳定性不好有的运气好能找到比较短的航迹有的就卡在某个山峰旁边。标准 PSO 的收敛速度是三者中最快的前期曲线下降很猛但后劲不足20 次实验中有相当比例的结果陷入局部最优绕过了山峰但航迹明显绕远路。PSO-ImWOA 的综合表现最好平均适应度和最优适应度都优于前面两者方差也控制在很小的范围。从收敛曲线看PSO-ImWOA 最明显的优势是后期。标准 PSO 在 100 代左右基本就停止下降了标准 WOA 在 200 代以后还有小幅波动而 PSO-ImWOA 在 200 代左右还能稳定地从局部区域中跳出来找到更短更安全的航迹。5.2 关键参数的经验取值先说种群规模。这个参数和问题维度直接相关。航迹点数量 N5 时维度是 15种群 30 就足够了如果 N 增加到 10维度升到 30种群规模建议调到 40 到 50。盲目加大种群规模效果有限还让每轮迭代的计算量成倍上升收敛速度反而会变慢。学习因子 (c_1) 和 (c_2) 我都取了 1.5。这个值在 PSO 类算法中属于经典取值你可以微调但尽量不要取太大。超过 2.5 之后速度修正项会主导整个位置更新WOA 的探索机制实际上被淹没了算法就退化成带 WOA 空壳的 PSO融合的意义就没了。惯性权重 (w) 的初值和终值我设成 0.9 和 0.4。这个区间是多次实验后的折中前期能保留足够的探索速度后期不会因为速度残余太大导致个体在 gbest 附近反复震荡。5.3 航迹质量评估的三个维度算法跑完之后怎么判断航迹质量我只看三个指标。第一是航迹总长度和直飞距离的比值这个比值越接近 1说明绕路越少。第二是威胁区进入情况检查航迹所有航迹段是否都在威胁半径之外如果有触碰看触碰深度是多少。第三是航迹的平滑性统计所有转角超过最大允许值的中间点数量。用这三个指标评估改进效果时我注意到一个现象融合 PSO 之后航迹总长度通常比标准 WOA 短 5% 到 10%而且航迹的平滑度更好。原因是 gbest 引导让群体更容易朝短路径方向收敛而 pbest 记忆机制让个体不太容易在局部区域反复绕圈转弯处的航迹点分布更均匀。6. 常见问题与避坑指南6.1 航迹点飞出了地图边界这个问题在初版代码里非常容易遇到。坐标编码方式下优化变量是连续实数算法在更新位置时不会自动考虑边界约束航迹点很容易飞出地图范围或者扎进地底下。我的处理方式是在每次位置更新之后立刻做边界吸附x、y、z 三个维度分别检查new_pos np.clip(new_pos, [x_min, y_min, z_min], [x_max, y_max, z_max])这样处理之后航迹点始终在可行区域内不会出现无意义的适应度值。但注意一个问题如果 z 的下界设得太高那么所有航迹点都被强制抬升到一个固定高度算法实际上是在二维平面上搜索三维优化的意义就打了折扣。z 的下界要设置得合理比如地形最高点以上少量余量让算法在高度维度上有足够的调整空间。6.2 适应度各项权重的失衡问题权重设置是另一个高频踩坑点。我之前试用过一组权重长度代价权重 1.0威胁代价权重 10结果算法把主要精力全放在躲威胁上了航迹绕了一个巨大的弧线长度代价严重失控。后来我一律采用归一化处理把每个代价项都归一到同一数量级再设置权重才有意义。具体做法是先用一个随机生成的可行个体集合统计每个代价项的均值然后用这个均值把对应代价项归一化。这样即使换了地图权重也不用重新调。如果你不想做归一化另一种务实办法是直接实验调权重先固定长度权重为 1调整其他项的权重观察航迹形态的变化。6.3 算法提前收敛到局部最优这是所有元启发式算法都会遇到的问题。我处理这个问题的经验有两条。一条是检查收敛曲线是否出现“断崖式”下跌后彻底平台化如果是说明种群多样性在早期就被破坏了这时需要降低 (c_2) 的取值减弱全局最优引导的强度或者把惯性权重 (w) 的减速度放缓。另一条是检查是否所有个体在空间中高度集中于某片区域如果是可以在每次迭代时对部分个体做随机扰动增加探索性。实际测试中发现PSO-ImWOA 比标准 WOA 和标准 PSO 发生早熟的概率都要低。原因是两个算法的机制互相制约WOA 的随机搜索策略保证了探索下限PSO 的记忆机制保证了开发上限叠加后不容易出现单一机制主导导致的失衡。6.4 高维场景下的维度灾难当航迹点数量设置得很大时算法效果会明显退化。我试过 N20也就是 60 维的优化问题同样的种群规模和迭代次数下收敛速度和航迹质量都比 N5 差不少。这个问题的根源不是算法本身而是高维空间搜索难度本身在指数级上升。处理方式有两种。一种是分层优化先用较少航迹点比如 N5做粗规划确定航迹的大致走向再在粗航迹的基础上插值细化为更多点用局部优化微调。另一种是改用分段规划把整条航迹切成若干段每段分别用 PSO-ImWOA 优化段与段之间用平滑算法衔接。两种方案我都试过分层优化的实现更简单效果也足够用。7. 代码的扩展思路与后续方向这套 PSO-ImWOA 代码框架的扩展性不错。如果要加新的约束条件比如禁飞区、动态威胁源只需要在适应度函数里再加一个代价项其他部分不用大改。如果要做多无人机协同航迹规划可以把适应度函数改成多目标形式或者用分解策略把问题拆成多个单目标子问题并行优化。我在实际项目里还做过一次扩展把静态威胁源改成移动威胁源每个威胁源的位置随时间变化航迹规划从一条静态路径问题变成了路径和时间双重优化问题。做法是把时间也编码进航迹点算法同时优化空间坐标和到达时间。虽然收敛速度明显变慢但 PSO-ImWOA 的搜索能力仍然能支撑起这个更高维度的求解。另外关于 Python 运行速度如果地图规模很大或种群规模调高可以直接把适应度计算函数做向量化去掉 for 循环改用 numpy 矩阵运算效果立竿见影。我在 200×200 的 DEM 地图上测试过向量化之后单次迭代时间从 1.2 秒降低到 0.3 秒300 代跑完只需要一分半钟。有个细节值得单独说一下如果你准备把代码发给别人复现环境依赖必须写清楚。重点就是 numpy 和 matplotlib还有 Python 3.8 以上版本。其他库尽量不要引入引入得越多别人复现环境的成本就越高。我见过很多开源代码卡在依赖问题上一跑就报错这个问题其实可以用 requirements.txt 轻松解决只是很多同学容易忽视。最后再分享一个自己用下来的心得第一版代码跑通之后先别急着调参和换算法先拿一组固定的随机种子把基线结果稳定住。我用 random seed 42 固定之后每次运行结果完全一致后面所有改进的对比都有确定的参照系了。这个习惯看起来朴素实际做算法对比实验时能帮你省掉大量重复调试的时间。