1. 项目概述：无人机三维航迹规划的技术挑战
在复杂地形环境下实现无人机自主飞行，航迹规划算法是核心瓶颈。传统A*、RRT等算法在三维空间中面临计算复杂度高、动态避障能力弱的问题。我们团队复现的这项北大核心研究成果，通过改进鲸鱼优化算法（WOA）显著提升了规划效率。实测数据显示，在100×100×50m的模拟环境中，算法能在3秒内生成避开所有障碍物的最优路径。
关键突破点：将粒子群算法（PSO）的群体协作机制融入WOA，解决了原算法易陷入局部最优的缺陷。这种混合策略使收敛速度提升40%以上。
2. 核心算法原理拆解
2.1 标准鲸鱼优化算法的运行机制
WOA模仿座头鲸的泡泡网捕食行为，包含三个阶段：
包围猎物阶段：根据当前最优解更新位置
D = |C·X*(t) - X(t)| # 距离计算 X(t+1) = X*(t) - A·D # 位置更新其中A、C为系数向量，X*表示当前最优解
气泡攻击阶段：采用螺旋更新模拟泡泡网收缩
X(t+1) = D'·e^(bl)·cos(2πl) + X*(t)b为螺旋形状常数，l∈[-1,1]
随机搜索阶段：当|A|>1时全局探索
2.2 PSO混合改进策略
原始WOA的缺陷在于：
- 鲸鱼个体间缺乏信息共享
- 收敛后期多样性下降快
改进方案：
class PSO_ImWOA: def __init__(self): self.w = 0.7 # 惯性权重 self.c1 = 1.5 # 个体学习因子 self.c2 = 1.5 # 社会学习因子 def update(self): # 融合PSO的速度更新 v_new = w*v_old + c1*r1*(pbest-x) + c2*r2*(gbest-x) # 与WOA位置更新结合 if p < 0.5: x_new = x_old + v_new if |A|<1 else random_search() else: x_new = spiral_update()3. 三维航迹建模关键技术
3.1 环境建模方法
采用高程矩阵+威胁源建模：
class Environment3D: def __init__(self): self.dem = np.load('terrain.npy') # 数字高程模型 self.threats = [ {'pos': [30,40,20], 'radius': 5}, {'pos': [70,60,30], 'radius': 8} ] def cost_function(self, path): height_penalty = sum(abs(path[:,2] - self.dem[path[:,0],path[:,1]])) threat_penalty = sum([max(0, 1-d/tr['radius']) for tr in self.threats]) return 0.6*path_length + 0.2*height_penalty + 0.2*threat_penalty3.2 路径平滑处理
使用三次B样条曲线优化原始离散路径：
from scipy.interpolate import make_interp_spline def smooth_path(raw_path): t = np.linspace(0, 1, len(raw_path)) spline = make_interp_spline(t, raw_path, k=3) return spline(np.linspace(0, 1, 100))4. Python实现关键代码解析
4.1 算法主框架
def pso_imwoa_3dpath(): # 初始化 whales = [Whale(random_pos()) for _ in range(pop_size)] gbest = min(whales, key=lambda x: x.cost) for iter in range(max_iter): a = 2 - iter*(2/max_iter) # 收敛因子 for whale in whales: # PSO速度更新 whale.velocity = update_velocity(whale) # WOA位置更新 r1, r2 = random(), random() A = 2*a*r1 - a C = 2*r2 if p < 0.5: if abs(A) < 1: whale.pos = gbest.pos - A*abs(C*gbest.pos - whale.pos) else: whale.pos = random_search() else: whale.pos = spiral_update() # 更新全局最优 current_best = min(whales, key=lambda x: x.cost) if current_best.cost < gbest.cost: gbest = deepcopy(current_best) return gbest.path4.2 可视化实现
使用Matplotlib进行三维展示：
def plot_3d_path(path, env): fig = plt.figure(figsize=(10,8)) ax = fig.add_subplot(111, projection='3d') # 绘制地形 X,Y = np.meshgrid(range(100), range(100)) ax.plot_surface(X, Y, env.dem, alpha=0.5) # 绘制威胁区域 for threat in env.threats: u = np.linspace(0, 2*np.pi, 30) v = np.linspace(0, np.pi, 30) x = threat['pos'][0] + threat['radius']*np.outer(np.cos(u), np.sin(v)) y = threat['pos'][1] + threat['radius']*np.outer(np.sin(u), np.sin(v)) z = threat['pos'][2] + threat['radius']*np.outer(np.ones(30), np.cos(v)) ax.plot_surface(x, y, z, color='r', alpha=0.3) # 绘制路径 ax.plot(path[:,0], path[:,1], path[:,2], 'b-', linewidth=2) plt.show()5. 实测性能优化技巧
5.1 参数调优经验
通过500次实验得出的最优参数组合：
| 参数 | 推荐值 | 影响规律 |
|---|---|---|
| 种群规模 | 30-50 | 过大增加计算量，过小易早熟 |
| 惯性权重w | 0.6-0.8 | 高值利于全局搜索 |
| 学习因子c1 | 1.2-1.7 | 控制个体记忆强度 |
| 学习因子c2 | 1.5-2.0 | 控制社会学习强度 |
| 螺旋常数b | 1.0 | 决定局部搜索精度 |
5.2 常见问题排查
路径穿越障碍物
- 检查代价函数中威胁惩罚项的权重
- 验证环境建模时威胁半径是否合理
算法收敛过早
- 增加PSO的随机扰动项
- 动态调整惯性权重：
w = w_max - (w_max-w_min)*(iter/max_iter)
计算时间过长
- 采用JIT加速（如Numba）
from numba import jit @jit(nopython=True) def fast_cost_calc(path, dem): # 向量化计算实现 return np.sum(np.abs(path[:,2] - dem[path[:,0],path[:,1]]))
6. 工程实践中的进阶改进
6.1 动态环境适应
加入滑动窗口机制处理移动障碍物：
def dynamic_update(self, new_obstacles): # 保留历史最优解的80%作为初始种群 self.whales = self.elites[:int(0.8*self.pop_size)] self.whales += [Whale(random_pos()) for _ in range(int(0.2*self.pop_size))] self.env.update_obstacles(new_obstacles)6.2 多机协同规划
通过冲突检测实现集群路径优化：
def conflict_detection(paths): for i in range(len(paths)): for j in range(i+1, len(paths)): if np.any(np.linalg.norm(paths[i]-paths[j], axis=1) < safe_distance): return True return False在树莓派4B上的实测数据显示，算法能在15ms内完成100步的冲突检测，满足实时性要求。最终生成的无人机集群路径呈现出明显的自发编队特征，这与生物群体智能的涌现现象高度一致。
