改进鲸鱼优化算法在无人机三维航迹规划中的应用

1. 项目概述：无人机三维航迹规划与改进鲸鱼优化算法

在无人机应用领域，三维航迹规划一直是核心挑战之一。传统算法在复杂环境中容易陷入局部最优解，而基于生物启发式的优化算法为解决这一问题提供了新思路。这次我们要复现的北大核心论文，正是将改进后的鲸鱼优化算法（mWOA）应用于无人机三维航迹规划场景。

鲸鱼优化算法（WOA）模拟了座头鲸独特的"气泡网"捕食行为，通过螺旋包围和随机搜索机制实现全局优化。而论文中提出的PSO-mWOA算法，则融合了粒子群算法（PSO）的群体智能特性，进一步提升了算法的收敛速度和路径质量。用Python实现这个算法，不仅能深入理解生物启发式算法的精髓，还能掌握无人机路径规划的实际工程应用技巧。

2. 核心算法原理与技术解析

2.1 标准鲸鱼优化算法基础

鲸鱼优化算法的核心在于模拟三种捕食行为：

1. 包围猎物：鲸鱼识别猎物位置并逐渐靠近

   D = |C·X*(t) - X(t)| # 距离计算 X(t+1) = X*(t) - A·D # 位置更新

   其中A和C是系数向量，X*是当前最优解位置

2. 气泡网攻击：采用螺旋更新位置模拟气泡网捕食

   X(t+1) = D'·e^(bl)·cos(2πl) + X*(t)

   D'表示与最优解的距离，b是螺旋形状常数，l∈[-1,1]

3. 随机搜索：当|A|>1时进行全局探索

   X(t+1) = X_rand - A·|C·X_rand - X|

2.2 PSO-mWOA改进策略解析

论文提出的改进点主要体现在：

1. 纵横双向学习机制：

   • 纵向学习：保留WOA的精英个体引导特性
   • 横向学习：引入PSO的群体历史最优经验共享

   v_i = w*v_i + c1*r1*(pbest_i - x_i) + c2*r2*(gbest - x_i) x_i = x_i + v_i

2. 动态权重调整：

   w = w_max - (w_max-w_min)*(t/T_max)

   随着迭代次数t增加，惯性权重w线性递减

3. 多种群协同进化：

   • 将种群分为探索子群和开发子群
   • 定期进行信息交换避免早熟收敛

3. 无人机三维航迹规划实现

3.1 环境建模与约束处理

三维航迹规划需要建立包含以下要素的环境模型：

class Environment3D: def __init__(self): self.obstacles = [] # 障碍物坐标和半径 self.threat_zones = [] # 威胁区域 self.boundary = [] # 飞行边界 self.start_point = (0,0,0) self.target_point = (100,100,50)

关键约束条件处理：

1. 高度约束：z_min ≤ z ≤ z_max
2. 转弯角约束：Δθ ≤ θ_max
3. 爬升率约束：Δh/Δd ≤ rate_max
4. 障碍物避碰：√[(x-xo)²+(y-yo)²+(z-zo)²] > ro + safe_dist

3.2 适应度函数设计

适应度函数需要平衡路径长度、安全性和平滑度：

def fitness_function(path): length_cost = calculate_path_length(path) safety_cost = sum(obstacle_penalty(p) for p in path) smoothness_cost = sum(abs(angle_between(p1,p2,p3)) for p1,p2,p3 in zip(path,path[1:],path[2:])) return w1*length_cost + w2*safety_cost + w3*smoothness_cost

权重设置建议值（需根据任务调整）：

4. Python实现关键代码解析

4.1 算法主框架实现

class PSO_mWOA: def __init__(self, pop_size, dim, max_iter): self.pop_size = pop_size self.dim = dim # 三维路径点数量×3 self.max_iter = max_iter self.pop = np.random.uniform(low, high, (pop_size, dim)) self.velocity = np.zeros((pop_size, dim)) self.pbest = self.pop.copy() self.gbest = None def optimize(self): for iter in range(self.max_iter): # 1. 计算适应度 fitness = [fitness_function(decode_path(ind)) for ind in self.pop] # 2. 更新pbest和gbest for i in range(self.pop_size): if fitness[i] < self.pbest_fitness[i]: self.pbest[i] = self.pop[i].copy() # 3. 混合更新策略 a = 2 - 2*iter/self.max_iter # 线性递减 for i in range(self.pop_size): if np.random.rand() < 0.5: # PSO更新 r1, r2 = np.random.rand(2) self.velocity[i] = (self.w*self.velocity[i] + self.c1*r1*(self.pbest[i]-self.pop[i]) + self.c2*r2*(self.gbest-self.pop[i])) self.pop[i] += self.velocity[i] else: # WOA更新 p = np.random.rand() if p < 0.5: if abs(self.A) < 1: # 包围猎物 self.pop[i] = self.gbest - self.A*np.abs(self.C*self.gbest-self.pop[i]) else: # 随机搜索 rand_idx = np.random.randint(0, self.pop_size) self.pop[i] = self.pop[rand_idx] - self.A*np.abs(self.C*self.pop[rand_idx]-self.pop[i]) else: # 气泡网攻击 l = np.random.uniform(-1,1) self.pop[i] = np.abs(self.gbest-self.pop[i])*np.exp(self.b*l)*np.cos(2*np.pi*l) + self.gbest

4.2 路径解码与可视化

def decode_path(encoded_path): """将一维编码转换为三维路径点""" path = [] for i in range(0, len(encoded_path), 3): path.append([encoded_path[i], encoded_path[i+1], encoded_path[i+2]]) return np.array(path) def plot_3d_path(path, obstacles): fig = plt.figure(figsize=(10,8)) ax = fig.add_subplot(111, projection='3d') # 绘制路径 ax.plot(path[:,0], path[:,1], path[:,2], 'b-', linewidth=2, label='Planned Path') # 绘制障碍物 for obs in obstacles: u = np.linspace(0, 2*np.pi, 50) v = np.linspace(0, np.pi, 50) x = obs[0] + obs[3]*np.outer(np.cos(u), np.sin(v)) y = obs[1] + obs[3]*np.outer(np.sin(u), np.sin(v)) z = obs[2] + obs[3]*np.outer(np.ones(np.size(u)), np.cos(v)) ax.plot_surface(x, y, z, color='r', alpha=0.3) ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_zlabel('Z (m)') plt.legend() plt.show()

5. 参数调优与性能对比

5.1 关键参数设置建议

5.2 与其他算法对比测试

在相同环境下对比不同算法性能（典型结果）：

实测建议：对于实时性要求高的场景，可适当减少种群规模和迭代次数；对于安全性要求高的任务，则应增加约束条件的权重系数。

6. 工程实践中的常见问题与解决方案

6.1 路径振荡问题

现象：相邻路径点之间出现剧烈方向变化 解决方法：

1. 在适应度函数中增加平滑度惩罚项
2. 采用B样条曲线对原始路径进行平滑处理
3. 添加转弯角约束：

   def turn_angle_constraint(path): for i in range(1, len(path)-1): v1 = path[i] - path[i-1] v2 = path[i+1] - path[i] angle = np.arccos(np.dot(v1,v2)/(np.linalg.norm(v1)*np.linalg.norm(v2))) if angle > max_turn_angle: return float('inf') return 0

6.2 算法收敛过早

现象：种群多样性快速丧失，陷入局部最优 应对策略：

1. 采用多种群机制，定期交换个体
2. 引入动态变异概率：

   mutation_rate = 0.1 * (1 - iter/max_iter) # 随迭代递减 if random.random() < mutation_rate: individual += np.random.normal(0, sigma, size=dim)

3. 使用自适应参数调整：

   a = 2 * (1 - iter/max_iter) # 非线性递减 A = 2 * a * random.random() - a C = 2 * random.random()

6.3 三维可视化技巧

使用Matplotlib进行三维可视化时注意：

1. 设置合适的视角：

   ax.view_init(elev=30, azim=45) # 仰角30度，方位角45度

2. 添加高度参考平面：

   xx, yy = np.meshgrid(np.linspace(0,100,10), np.linspace(0,100,10)) zz = np.zeros_like(xx) ax.plot_surface(xx, yy, zz, alpha=0.2)

3. 使用不同颜色区分路径段：

   for i in range(len(path)-1): ax.plot([path[i][0],path[i+1][0]], [path[i][1],path[i+1][1]], [path[i][2],path[i+1][2]], color=cm.jet(i/len(path)))

7. 进阶优化方向

7.1 动态环境适应

对于移动障碍物场景，需要：

1. 建立环境预测模型
2. 采用滚动时域规划策略：

   while not reach_target: current_pos = get_actual_position() local_env = update_environment() partial_path = pso_mwoa_plan(current_pos, next_waypoint, local_env) execute_path(partial_path[0:look_ahead])

7.2 多机协同规划

扩展算法支持多无人机协同：

1. 在适应度函数中增加防撞约束：

   def collision_avoidance(path1, path2): min_dist = np.inf for p1, p2 in zip(path1, path2): dist = np.linalg.norm(p1-p2) if dist < min_dist: min_dist = dist return min_dist > safety_distance

2. 采用分层规划策略：
   • 上层：任务分配和粗略路径
   • 下层：单机精细路径规划

7.3 硬件在环测试

将算法部署到实际飞控系统的建议流程：

1. 使用AirSim或Gazebo进行仿真验证
2. 通过MAVLink协议与飞控通信：

   from pymavlink import mavutil connection = mavutil.mavlink_connection('udpin:localhost:14551') connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message( 10, connection.target_system, connection.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, int(0b110111111000), x, y, z, vx, vy, vz, 0, 0, 0, 0, 0))

3. 实地测试前务必设置安全保护：
   • 高度限制
   • 地理围栏
   • 紧急降落机制