用Python复现数学建模竞赛题：手把手教你用Dijkstra和蚁群算法搞定无人机协同避障

用Python实战数学建模Dijkstra与蚁群算法在无人机协同避障中的妙用数学建模竞赛中无人机协同避障问题一直是经典中的经典。这类问题不仅考验参赛者的数学功底更检验将抽象模型转化为实际代码的能力。今天我们就以2023年深圳杯C题为例抛开繁琐的数学推导直接动手用Python实现Dijkstra和蚁群算法解决无人机航迹规划难题。1. 问题理解与环境搭建首先我们需要明确题目要求两架无人机分别从A、B两站出发以10m/s的速度相向飞行途中必须避开一个半径500米的圆形障碍物且两机连线必须始终与障碍圆相交。此外无人机转弯半径不得小于30米。关键约束条件障碍圆半径500米A站距圆心1000米B站距圆心3500米无人机速度10 m/s最小转弯半径30米注意实际编程时需要将距离单位统一建议全部使用米(m)作为基本单位。1.1 Python环境准备我们需要以下Python库支持算法实现和数据可视化import numpy as np import matplotlib.pyplot as plt import heapq # Dijkstra算法优先队列 import random # 蚁群算法随机选择 from math import sqrt, sin, cos, atan2, pi # 几何计算2. Dijkstra算法实现最短路径规划Dijkstra算法是解决单源最短路径问题的经典算法。在无人机航迹规划中我们可以将飞行区域离散化为网格每个网格点作为图的一个顶点顶点间的距离作为边权重。2.1 网格化环境建模首先建立坐标系设圆心为原点(0,0)A站在x轴负方向(-1000,0)B站在x轴正方向(3500,0)。def create_environment(): # 定义关键点坐标 circle_center np.array([0, 0]) A_station np.array([-1000, 0]) B_station np.array([3500, 0]) circle_radius 500 # 创建网格 x np.linspace(-1500, 4000, 100) # x轴范围 y np.linspace(-1000, 1000, 80) # y轴范围 X, Y np.meshgrid(x, y) return X, Y, circle_center, circle_radius, A_station, B_station2.2 Dijkstra算法核心实现def dijkstra(graph, start): # 初始化距离字典 distances {vertex: float(infinity) for vertex in graph} distances[start] 0 priority_queue [(0, start)] while priority_queue: current_distance, current_vertex heapq.heappop(priority_queue) # 如果找到更短路径则跳过 if current_distance distances[current_vertex]: continue for neighbor, weight in graph[current_vertex].items(): distance current_distance weight # 发现更短路径时更新 if distance distances[neighbor]: distances[neighbor] distance heapq.heappush(priority_queue, (distance, neighbor)) return distances2.3 路径可视化def plot_path(path, X, Y, circle_center, circle_radius): plt.figure(figsize(12, 6)) # 绘制障碍圆 circle plt.Circle(circle_center, circle_radius, colorr, alpha0.3) plt.gca().add_patch(circle) # 绘制路径 path_x [p[0] for p in path] path_y [p[1] for p in path] plt.plot(path_x, path_y, b-, linewidth2) # 标记起点和终点 plt.plot(path[0][0], path[0][1], go, markersize10) plt.plot(path[-1][0], path[-1][1], ro, markersize10) plt.grid(True) plt.axis(equal) plt.show()3. 蚁群算法实现协同避障蚁群算法模拟蚂蚁觅食行为适合解决路径优化问题。对于无人机协同避障我们需要考虑两架无人机的路径协调。3.1 蚁群算法参数设置class ACOParameters: def __init__(self): self.ant_count 20 # 蚂蚁数量 self.iterations 100 # 迭代次数 self.alpha 1.0 # 信息素重要程度 self.beta 2.0 # 启发式信息重要程度 self.rho 0.5 # 信息素挥发系数 self.Q 100 # 信息素强度 self.min_tau 0.1 # 最小信息素量 self.max_tau 10 # 最大信息素量3.2 信息素矩阵初始化def init_pheromone_matrix(size): return np.ones((size, size)) * 0.13.3 蚂蚁路径构建def construct_path(ant, pheromone, distance_matrix, params): path [] visited set() current ant.start_node path.append(current) visited.add(current) while len(path) ant.node_count: next_node select_next_node(current, visited, pheromone, distance_matrix, params) path.append(next_node) visited.add(next_node) current next_node return path3.4 信息素更新def update_pheromone(pheromone, ants, params): # 信息素挥发 pheromone * (1 - params.rho) # 添加新信息素 for ant in ants: path ant.path path_length ant.path_length for i in range(len(path)-1): u, v path[i], path[i1] pheromone[u][v] params.Q / path_length pheromone[v][u] pheromone[u][v] # 对称矩阵 # 限制信息素范围 pheromone np.clip(pheromone, params.min_tau, params.max_tau) return pheromone4. 双机协同避障策略实现两架无人机协同飞行时除了各自避开障碍物还需保持连线与障碍圆相交。这需要特殊的协调策略。4.1 协同约束条件检查def check_collision_avoidance(path1, path2, circle_center, circle_radius): for p1, p2 in zip(path1, path2): # 计算两机连线中点 mid_point ((p1[0]p2[0])/2, (p1[1]p2[1])/2) # 检查中点是否在障碍圆内 distance sqrt((mid_point[0]-circle_center[0])**2 (mid_point[1]-circle_center[1])**2) if distance circle_radius: return False # 违反约束 return True # 满足约束4.2 协同路径优化算法def cooperative_path_planning(): # 初始化参数 params ACOParameters() # 为两架无人机分别初始化蚁群 aco1 ACOForDrone(drone1) aco2 ACOForDrone(drone2) best_path1 None best_path2 None best_time float(inf) for iteration in range(params.iterations): # 为每架无人机生成蚂蚁路径 paths1 [aco1.construct_path() for _ in range(params.ant_count)] paths2 [aco2.construct_path() for _ in range(params.ant_count)] # 评估所有路径组合 for p1 in paths1: for p2 in paths2: if check_collision_avoidance(p1, p2, circle_center, circle_radius): time1 calculate_path_time(p1) time2 calculate_path_time(p2) max_time max(time1, time2) if max_time best_time: best_time max_time best_path1 p1 best_path2 p2 # 更新信息素 aco1.update_pheromone(best_path1) aco2.update_pheromone(best_path2) return best_path1, best_path2, best_time5. 算法优化与性能提升在实际应用中我们需要考虑算法效率和路径质量之间的平衡。以下是几种优化策略5.1 启发式信息设计def heuristic_info(distance_matrix): # 使用距离的倒数作为启发式信息 eta 1.0 / (distance_matrix 1e-10) # 避免除以零 return eta5.2 并行计算加速from multiprocessing import Pool def parallel_ant_run(ant): return ant.construct_path() # 在主循环中使用 with Pool(processes4) as pool: paths pool.map(parallel_ant_run, ants)5.3 动态参数调整def adaptive_parameters(iteration, max_iterations): # 随着迭代动态调整参数 rho 0.1 0.4 * (iteration / max_iterations) # 逐渐增加挥发系数 alpha 1.0 - 0.5 * (iteration / max_iterations) # 降低信息素重要性 return rho, alpha6. 实际应用中的挑战与解决方案在实现无人机协同避障算法时会遇到各种实际问题。以下是几个典型挑战及应对方法6.1 转弯半径约束处理无人机的物理限制要求路径转弯半径不小于30米。我们可以在路径平滑阶段加入这一约束def smooth_path_with_turn_constraint(path, min_turn_radius): smoothed_path [path[0]] for i in range(1, len(path)-1): # 计算三点形成的转弯半径 p1, p2, p3 path[i-1], path[i], path[i1] radius calculate_turn_radius(p1, p2, p3) if radius min_turn_radius: smoothed_path.append(p2) else: # 需要调整路径点以满足转弯半径 adjusted_point adjust_point_for_turn(p1, p2, p3, min_turn_radius) smoothed_path.append(adjusted_point) smoothed_path.append(path[-1]) return smoothed_path6.2 实时避障策略当遇到动态障碍物时需要实时调整路径def dynamic_obstacle_avoidance(current_path, obstacle_position, obstacle_radius): # 检查路径是否与障碍物相交 collision_points find_collision_points(current_path, obstacle_position, obstacle_radius) if not collision_points: return current_path # 无需调整 # 在碰撞点前后插入避障点 new_path [] for i in range(len(current_path)-1): new_path.append(current_path[i]) # 检查当前线段是否与障碍物相交 if is_segment_intersecting_circle(current_path[i], current_path[i1], obstacle_position, obstacle_radius): # 计算避障点 avoid_point calculate_avoidance_point(current_path[i], current_path[i1], obstacle_position, obstacle_radius) new_path.append(avoid_point) new_path.append(current_path[-1]) return new_path6.3 多目标优化权衡在问题2中我们需要优化第二架无人机的到达时间。这需要权衡两架无人机的路径def multi_objective_optimization(paths): # 计算各目标函数值 time1 [calculate_path_time(p[0]) for p in paths] time2 [calculate_path_time(p[1]) for p in paths] # 寻找Pareto前沿 pareto_front [] for i in range(len(paths)): dominated False for j in range(len(paths)): if time1[j] time1[i] and time2[j] time2[i] and \ (time1[j] time1[i] or time2[j] time2[i]): dominated True break if not dominated: pareto_front.append(paths[i]) return pareto_front7. 完整案例演示让我们通过一个完整的例子展示如何使用上述算法解决深圳杯C题的问题1def solve_problem1(): # 初始化环境 X, Y, circle_center, circle_radius, A_station, B_station create_environment() # 构建图结构 graph build_graph(X, Y, circle_center, circle_radius) # 为无人机A(A→B)寻找路径 path_A dijkstra(graph, tuple(A_station), tuple(B_station)) # 为无人机B(B→A)寻找路径 path_B dijkstra(graph, tuple(B_station), tuple(A_station)) # 检查协同约束 if not check_collision_avoidance(path_A, path_B, circle_center, circle_radius): # 如果不满足约束使用蚁群算法优化 path_A, path_B, _ cooperative_path_planning() # 可视化结果 plot_two_paths(path_A, path_B, circle_center, circle_radius) # 计算到达时间 time_A calculate_path_time(path_A) time_B calculate_path_time(path_B) first_arrival min(time_A, time_B) print(f第一架无人机到达时间: {first_arrival:.2f}秒) print(f无人机A路径长度: {calculate_path_length(path_A):.2f}米) print(f无人机B路径长度: {calculate_path_length(path_B):.2f}米) return path_A, path_B在实际项目中我们发现Dijkstra算法虽然能找到最短路径但在处理协同约束时灵活性不足。而蚁群算法虽然计算量较大但能更好地处理复杂约束条件。将两种算法结合使用往往能取得更好的效果。