用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_station

2.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 distances

2.3 路径可视化

def plot_path(path, X, Y, circle_center, circle_radius):
    plt.figure(figsize=(12, 6))
    
    # 绘制障碍圆
    circle = plt.Circle(circle_center, circle_radius, color='r', alpha=0.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-', linewidth=2)
    
    # 标记起点和终点
    plt.plot(path[0][0], path[0][1], 'go', markersize=10)
    plt.plot(path[-1][0], path[-1][1], 'ro', markersize=10)
    
    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.1

3.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 path

3.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[i+1]
            pheromone[u][v] += params.Q / path_length
            pheromone[v][u] = pheromone[u][v]  # 对称矩阵
    
    # 限制信息素范围
    pheromone = np.clip(pheromone, params.min_tau, params.max_tau)
    
    return pheromone

4. 双机协同避障策略实现

两架无人机协同飞行时，除了各自避开障碍物，还需保持连线与障碍圆相交。这需要特殊的协调策略。

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(drone=1)
    aco2 = ACOForDrone(drone=2)
    
    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_time

5. 算法优化与性能提升

在实际应用中，我们需要考虑算法效率和路径质量之间的平衡。以下是几种优化策略：

5.1 启发式信息设计

def heuristic_info(distance_matrix):
    # 使用距离的倒数作为启发式信息
    eta = 1.0 / (distance_matrix + 1e-10)  # 避免除以零
    return eta

5.2 并行计算加速

from multiprocessing import Pool

def parallel_ant_run(ant):
    return ant.construct_path()

# 在主循环中使用
with Pool(processes=4) 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, alpha

6. 实际应用中的挑战与解决方案

在实现无人机协同避障算法时，会遇到各种实际问题。以下是几个典型挑战及应对方法：

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[i+1]
        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_path

6.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[i+1], 
                                        obstacle_position, obstacle_radius):
            # 计算避障点
            avoid_point = calculate_avoidance_point(current_path[i], current_path[i+1],
                                                  obstacle_position, obstacle_radius)
            new_path.append(avoid_point)
    
    new_path.append(current_path[-1])
    return new_path

6.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_front

7. 完整案例演示

让我们通过一个完整的例子展示如何使用上述算法解决深圳杯C题的问题1：

def 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算法虽然能找到最短路径，但在处理协同约束时灵活性不足。而蚁群算法虽然计算量较大，但能更好地处理复杂约束条件。将两种算法结合使用，往往能取得更好的效果。