1. 从数学建模到真实飞控无人机协同避障航迹规划的核心挑战最近几年无论是全国大学生数学建模竞赛还是深圳杯这类地区性赛事无人机相关的题目热度一直居高不下。2023年深圳杯数学建模C题就是一个典型代表它把“无人机协同避障航迹规划”这个既前沿又充满工程挑战的问题抛给了参赛者。很多同学拿到题目后第一反应可能是去搜索“路径优化算法”、“A*算法”、“遗传算法”这些关键词然后试图套用一个现成的模型。但如果你真的在无人机行业里摸爬滚打过就会知道把数学建模论文里的那条“最优路径”变成天空中无人机实际飞行的轨迹中间隔着一道巨大的鸿沟。这道鸿沟就是理论与工程实践的差距。这道题目的价值远不止于完成一篇论文。它本质上是在考察我们如何将一个复杂的、多约束的工程问题抽象成一个可计算的数学模型并最终能指导或仿真实际系统的行为。协同意味着多架无人机避障意味着环境感知与动态决策航迹规划则是在满足动力学约束、时间约束、安全约束下的空间路径寻优。这几乎涵盖了无人机系统从上层决策到底层控制的所有核心环节。因此讨论这个问题不能只停留在“我用什么算法得了高分”更要深入去想“这个算法为什么有效”、“它的假设在现实中成立吗”、“代码跑出来的路径真能飞吗”。这就是我想在这篇分享里重点聊的如何穿透数学建模的表象去理解无人机航迹规划里那些真正棘手的问题并给出一些具有实操性的思路和代码设计哲学。2. 问题拆解协同避障航迹规划到底在规划什么在动手建模型、写代码之前我们必须把问题本身掰开揉碎看清楚它的每一个约束和边界。题目通常会给一个场景比如多架无人机从不同起点出发前往各自终点区域中存在静态障碍物如山丘、建筑物甚至可能有动态障碍如其他飞行器要求规划出满足最小安全距离、最短时间或最节能的飞行路径。2.1 核心约束条件分析首先我们要明确无人机不是质点它的飞行受到物理规律的严格限制。这是许多纯数学路径规划算法如传统的图搜索算法容易忽略的第一点。动力学约束无人机有最大速度、最大加速度包括线加速度和角加速度的限制。这意味着路径不能有急转弯曲率必须连续且在一定范围内。你规划出一条理论上最短的、包含直角转弯的折线无人机是根本飞不出来的强行跟踪会导致失稳甚至坠毁。因此规划出的路径必须是可飞行的通常要求是C1或C2连续即一阶导数连续或二阶导数连续。避障约束这不仅仅是“不撞上”。障碍物通常被建模为圆柱体、长方体或凸多面体。安全距离约束要求无人机与所有障碍物表面的距离必须大于一个阈值例如10米。这个阈值不仅要考虑无人机本身的尺寸还要考虑定位误差、控制误差和风扰等不确定性。所以我们规划的其实是一个安全走廊而不仅仅是一条线。协同约束多机之间必须保持安全距离防止碰撞。这引入了时空维度的耦合。两架无人机的路径在空间上交叉并不可怕可怕的是它们在同一时刻到达交叉点。因此协同规划的本质是空间与时间的联合分配可能需要引入优先级、预约机制或实时协商。任务约束可能包括到达时间窗口、总航程限制、能量消耗与速度、加速度相关等。在数学建模中这些往往成为优化问题的目标函数或附加约束条件。2.2 环境建模从连续世界到离散图计算机无法直接处理连续空间中的无限可能路径。因此航迹规划的第一步总是环境建模与离散化。常见的方法有栅格法将三维空间划分为均匀的立方体栅格Voxel。每个栅格标记为“自由”、“障碍”或“未知”。这种方法直观易于实现碰撞检测只需判断无人机占据的栅格状态但缺点也很明显精度与计算开销矛盾精细栅格导致维度爆炸且路径只能是栅格中心的连线不够平滑。注意在三维栅格中规划搜索空间是O(n³)急剧膨胀。通常需要结合分层规划先在粗粒度栅格找到大致路径再在局部进行精细化平滑。采样法如快速随机扩展树RRT及其变种RRT* Informed RRT*。通过在自由空间中随机采样点并尝试将其与树中现有节点连接来构建一棵路径树。这种方法在高维空间如包含姿态的规划中效率很高能快速找到可行解并渐进趋向最优。对于本题的静态环境RRT* 是一个强有力的候选算法。图搜索法在栅格或特定路点Waypoint构成的状态空间图上进行搜索如A*、D*、Dijkstra算法。A*算法通过启发式函数引导搜索方向效率很高。但关键是如何设计“状态”。对于无人机状态可能不仅仅是(x, y, z)位置还应包括速度矢量(vx, vy, vz)甚至时间t。这就引出了状态栅格或时空状态空间的概念在这个空间里进行搜索可以自然地满足动力学和协同避障约束但计算量更大。选择建议对于数学建模竞赛由于计算时间有限环境通常被简化。我推荐采用分层规划策略上层使用A*在粗粒度栅格上规划一条无碰撞的几何路径下层采用轨迹优化方法如Minimum Snap轨迹生成将这条折线路径优化成一条平滑、满足动力学约束的多项式轨迹。这样既能保证可行性又能在模型中体现深度。3. 算法工具箱哪些模型能扛起协同避障的大旗明确了问题和环境模型后我们来盘点一下核心的算法工具箱。没有一种算法是万能的关键在于根据问题特点进行选择和组合。3.1 单机航迹规划基础算法这是协同规划的基础。假设环境中只有静态障碍物。A搜索算法*这是必须掌握的经典算法。其核心是代价函数 f(n) g(n) h(n)。g(n)是从起点到节点n的实际代价h(n)是从节点n到终点的估计代价启发函数。在三维栅格中h(n)常采用欧几里得距离或曼哈顿距离。关键点启发函数h(n)必须满足可采纳性admissible即不高估实际代价才能保证找到最优解。在三维中欧几里得距离是可采纳的。实操改进对于无人机g(n)不能只是路径长度还应考虑转向代价、爬升代价这可以通过在节点扩展时赋予不同方向的移动以不同代价来实现。# A* 算法核心逻辑的简化伪代码示意 import heapq def a_star(start, goal, grid): open_set [] heapq.heappush(open_set, (0, start)) came_from {} g_score {start: 0} f_score {start: heuristic(start, goal)} while open_set: current heapq.heappop(open_set)[1] if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(current, grid): tentative_g_score g_score[current] cost(current, neighbor) if neighbor not in g_score or tentative_g_score g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g_score f_score[neighbor] g_score[neighbor] heuristic(neighbor, goal) heapq.heappush(open_set, (f_score[neighbor], neighbor)) return None # 路径不存在RRT算法*相较于基础RRTRRT* 具有渐进最优性。它在找到初始路径后会不断地对路径进行“重布线”和“重选择父节点”操作从而逐步优化路径代价。优势无需离散化环境在高维空间和复杂障碍物环境下非常有效能直接产生连续空间中的路径。劣势收敛到最优解需要大量采样点耗时较长路径可能不够平滑仍需后处理。竞赛应用在建模时可以将其作为一个对比算法展示其在复杂狭窄空间中的优势同时指出其实时性不足的问题。3.2 从路径到轨迹满足动力学约束的关键一步通过A或RRT得到的一系列路径点Waypoints只是一串空间坐标。无人机控制器如PID或模型预测控制需要的是随时间变化的、平滑的轨迹参考信号。这一步就是轨迹生成。目前最主流的方法是使用多项式轨迹特别是Minimum Snap轨迹最小化加加速度的积分或Minimum Jerk轨迹最小化加速度的积分。对于多旋翼无人机其平动动力学与姿态解耦控制输入与加速度相关因此Minimum Snap最小化推力变化率能带来更平稳的飞行。核心思想将整个轨迹按路径点分段每一段用一个高阶多项式如7次多项式表示。通过设定每一段起点和终点的位置、速度、加速度、加加速度等边界条件并保证在路径点处这些状态连续可以构造一个庞大的线性方程组。最小化“Snap”四阶导数的积分平方可以转化为一个二次规划问题有高效的封闭解。# Minimum Snap轨迹生成的核心步骤概念性描述 import numpy as np from scipy.optimize import minimize # 假设我们有3个路径点分成2段轨迹每段用7次多项式表示 # 状态量每段轨迹的8个系数 (c0, c1, ..., c7)其中 p(t) c0 c1*t c2*t^2 ... c7*t^7 # 约束包括 # 1. 起点和终点的位置、速度、加速度、加加速度已知或设为0。 # 2. 中间路径点的位置已知以及通过该点时的速度、加速度连续性。 # 目标函数最小化所有段Snap的平方积分和。 def objective(coefficients): # coefficients 包含所有段的系数 total_cost 0 for seg_coef in segments: # 计算该段多项式第4阶导数的系数 snap_coef ... # 对多项式系数求4次导得到的新系数 # 计算该段上snap平方的积分这是一个关于多项式系数的二次型 cost seg_coef.T Q_matrix seg_coef # Q_matrix由时间区间和多项式阶次决定 total_cost cost return total_cost # 构建线性等式约束边界条件和连续性条件 A_eq, b_eq build_equality_constraints(...) # 构建线性不等式约束如速度、加速度限幅 A_ineq, b_ineq build_inequality_constraints(...) # 求解二次规划问题 result minimize(objective, x0, constraints{type: eq, fun: lambda x: A_eq x - b_eq}, bounds...) optimal_coefficients result.x这个过程的数学推导和矩阵构建是建模的难点和亮点。在论文中清晰地展示如何将物理约束转化为数学约束是获得高分的关键。3.3 多机协同规划的核心策略当有多架无人机时问题复杂度呈指数增长。主要策略有三类集中式规划将所有无人机视为一个整体在联合状态空间所有无人机状态的笛卡尔积中进行规划。这能得到全局最优解但状态空间维度是单机的N倍计算完全不可行只适用于2-3架无人机的理论分析。分布式规划每架无人机独立规划自己的路径但通过通信共享意图并在检测到潜在冲突时进行协商调整。这是目前研究的热点也更贴近实际应用。基于优先级的路径规划为无人机分配固定的优先级。高优先级无人机按单机规划其最优路径。低优先级无人机在规划时将高优先级无人机的时空轨迹视为动态障碍物进行避让。这种方法简单但可能导致低优先级无人机路径严重次优。基于时空预约的规划将空域和时间划分为网格时空体素。每架无人机在规划时先“预约”它将要占据的时空体素。如果发生冲突两架无人机预约了同一时空体素后规划的无人机需要绕行或等待。这类似于交通信号灯或预约系统。解耦式规划这是数学建模中最实用的方法。先进行路径规划忽略时间只考虑空间冲突。得到多条空间路径后再进行速度规划为每架无人机安排通过路径上各点的时间从而避免在同一时间到达空间交叉点。这通常通过调整无人机的飞行速度在最大最小速度范围内来实现可以建模为一个线性规划或约束优化问题。对于深圳杯C题这类赛题我强烈推荐采用“解耦式规划”思路。因为它将复杂的时空联合优化问题分解为两个相对简单的子问题大大降低了建模和求解难度同时又能清晰地体现在论文中。具体步骤可以是步骤一为每架无人机独立规划一条避开所有静态障碍物的空间路径使用改进的A或RRT。步骤二检查任意两条路径是否存在空间交叉点。如果存在标记这些交叉区域。步骤三在交叉区域附近将无人机视为在一条“单行道”上行驶的车辆。通过为每架无人机分配不同的通过时间例如引入微小的速度调整或等待确保它们不会同时进入交叉区域。这可以转化为一个冲突消解的时间表优化问题。4. 建模实战构建一个可求解的优化模型理论说再多不如一个具体的模型。我们尝试为“多无人机、静态障碍、最小化总飞行时间”这个典型场景建立一个优化模型。4.1 模型假设与符号定义假设1无人机简化为一个质点但其飞行需满足最大速度V_max、最大加速度a_max的约束。假设2障碍物为已知位置和大小的圆柱体。安全距离为d_safe。假设3所有无人机在同一高度层飞行或飞行高度已预先确定简化问题为二维。假设4无人机匀速飞行不这太理想。我们假设其在路径段上可以加速/减速但整体运动用分段匀加速模型近似以便于计算时间。符号U: 无人机集合i ∈ U。O: 障碍物集合k ∈ O。P_i: 无人机i的路径由一系列路径点p_i^m(m0,1,...,M_i)组成其中p_i^0为起点p_i^{M_i}为终点。t_i^m: 无人机i到达路径点p_i^m的时刻。s_i^m: 无人机i在路径段m从点p_i^m到p_i^{m1}上计划行驶的距离。v_i^m: 无人机i在路径段m上的平均速度决策变量之一。4.2 目标函数与约束条件目标函数最小化最后一架无人机到达终点的时间makespan或所有无人机飞行时间之和。Minimize T max_i (t_i^{M_i}) 或 Minimize Σ_i (t_i^{M_i} - t_i^0)约束条件运动学约束速度上下限0 v_i^m V_max加速度近似约束相邻段速度差限制|v_i^{m1} - v_i^m| / ΔT_i^m a_max其中ΔT_i^m是飞过段m的估计时间。这个约束是非线性的因为时间本身取决于速度。一个常见的线性化技巧是固定路径段的飞行时间或采用更精细的离散化。避障约束静态对于每个路径点p_i^m其与每个障碍物k中心o_k的距离必须大于障碍物半径R_k加上安全距离d_safe。|| p_i^m - o_k ||_2 R_k d_safe。这是非线性约束欧几里得范数。防碰撞约束无人机间这是最核心的协同约束。对于任意两架无人机i, j(i≠j)以及任意时刻t或任意对应的路径点/段它们之间的距离必须大于安全距离d_safe。|| pos_i(t) - pos_j(t) ||_2 d_safe。这是一个连续的时空约束直接处理极其困难。关键简化解耦法我们只在已知的路径交叉点施加此约束。假设无人机i和j的路径在空间点C交叉。设它们到达C点的时间分别为t_i^C和t_j^C。则约束可写为| t_i^C - t_j^C | Δt_min。其中Δt_min是一个最小安全时间间隔其值取决于无人机在交叉点附近的速度和安全距离。Δt_min d_safe / (v_i^C v_j^C)这里v_i^C是无人机i在交叉点附近的速度。这又将速度变量耦合了进来。进一步简化竞赛实用假设所有无人机以恒定速度V飞行或每架无人机一个恒定速度V_i。那么路径点之间的飞行时间与距离成正比。防碰撞约束就简化为对到达关键路径点交叉点的时间差的约束。我们可以通过调整每架无人机的出发时间或在非关键路径段的速度来满足这个时间差约束。这就转化为了一个混合整数线性规划问题为每一对在交叉点可能冲突的无人机引入一个0-1决策变量δ_{ij}表示谁先通过。约束变为t_i^C M * (1 - δ_{ij}) t_j^C Δt_mint_j^C M * δ_{ij} t_i^C Δt_min其中M是一个很大的正数。这组约束确保了t_i^C和t_j^C至少相差Δt_min。4.3 模型求解思路这个模型包含了非线性约束避障和整数变量冲突顺序是一个混合整数非线性规划问题直接求解非常困难。在数学建模竞赛中我们需要采用启发式或分层求解策略第一阶段固定路径的空间规划。使用A*算法为每架无人机生成一条初始无碰撞路径仅针对静态障碍。此时只考虑空间不考虑时间和无人机间冲突。得到一系列路径点P_i。第二阶段冲突检测与关键点识别。遍历所有无人机路径检测两两之间路径线段是否相交在二维平面中可通过计算几何方法判断。记录下所有交叉点C。第三阶段基于时间窗口的冲突消解。这是核心优化步骤。假设每架无人机以恒定速度V_i飞行V_i可在[V_min, V_max]范围内作为连续变量优化或固定为V_max以最小化时间。根据路径长度和速度可以计算出每架无人机到达每个交叉点C的时间窗口[earliest_i^C, latest_i^C]。最早时间由最大速度计算最晚时间由最小速度计算或加入等待时间。问题转化为为每个交叉点C上的每一对冲突无人机(i, j)分配一个通过顺序并调整它们的速度或出发时间使得实际通过时间满足顺序和安全间隔要求同时优化总时间。这可以建模为一个约束规划或混合整数线性规划问题。虽然规模可能不小但借助ortools、PuLP等优化库对于赛题规模如5-10架无人机数个交叉点是可解的。第四阶段轨迹平滑与输出。根据最终确定的路径点和速度或时间利用前面提到的Minimum Snap方法生成平滑的轨迹作为最终输出。5. 参考代码框架与关键实现细节这里不可能给出全部代码但我会勾勒出核心模块的框架和实现时容易踩坑的细节。5.1 环境与路径规划模块Python示例import numpy as np import matplotlib.pyplot as plt from scipy.spatial import KDTree import heapq from typing import List, Tuple class Drone: def __init__(self, start, goal, id): self.id id self.start np.array(start) self.goal np.array(goal) self.path [] # 路径点列表 (x, y, z) self.timestamps [] # 到达每个路径点的计划时间 class Obstacle: def __init__(self, center, radius): self.center np.array(center) self.radius radius class PathPlanner: def __init__(self, grid_size, bounds): self.grid_size grid_size self.bounds bounds # ((x_min, x_max), (y_min, y_max), (z_min, z_max)) self.obstacles [] def add_obstacle(self, obs: Obstacle): self.obstacles.append(obs) def is_collision_free(self, point): 检查一个点是否与任何障碍物碰撞 for obs in self.obstacles: if np.linalg.norm(point[:2] - obs.center[:2]) obs.radius: # 假设2D障碍 return False return True def a_star_plan(self, start, goal): 在二维/三维栅格上进行A*搜索 # 将连续坐标离散化为栅格索引 start_idx self.continuous_to_index(start) goal_idx self.continuous_to_index(goal) open_set [] heapq.heappush(open_set, (0, start_idx)) came_from {} g_score {tuple(start_idx): 0} f_score {tuple(start_idx): self.heuristic(start_idx, goal_idx)} while open_set: _, current heapq.heappop(open_set) if current tuple(goal_idx): return self.reconstruct_path(came_from, current, start_idx) for neighbor in self.get_neighbors(current): if not self.is_index_collision_free(neighbor): continue tentative_g g_score[current] self.distance(current, neighbor) if tuple(neighbor) not in g_score or tentative_g g_score[tuple(neighbor)]: came_from[tuple(neighbor)] current g_score[tuple(neighbor)] tentative_g f_score[tuple(neighbor)] tentative_g self.heuristic(neighbor, goal_idx) heapq.heappush(open_set, (f_score[tuple(neighbor)], neighbor)) return None # 未找到路径 def heuristic(self, a, b): # 欧几里得距离作为启发函数 return np.linalg.norm(np.array(a) - np.array(b)) def distance(self, a, b): # 实际代价可以加入转向惩罚 return np.linalg.norm(np.array(a) - np.array(b)) # ... 其他方法continuous_to_index, get_neighbors, reconstruct_path, is_index_collision_free关键细节1碰撞检测的保守性。is_collision_free函数检查的是点是否在障碍物内。但无人机有尺寸应该检查整个机体是否碰撞。一个实用的方法是在路径点处检查点在路径段两点之间进行采样检查比如在连接线上每隔0.1米取一个点进行碰撞检测。更严格的方法是计算线段到障碍物圆心的最短距离。关键细节2启发函数的选择。在三维中欧几里得距离是可采纳的但可能不是最有效的。如果无人机主要在同一高度飞行可以先用二维距离计算再考虑高度差这能加快搜索速度。5.2 冲突检测模块def detect_path_conflicts(paths: List[List[np.ndarray]], conflict_threshold5.0): 检测多条路径之间的空间交叉点。 paths: 列表的列表每个内层列表是一个无人机的路径点序列。 conflict_threshold: 判定为交叉的距离阈值米。 返回: 一个列表每个元素为 (i, j, point_approx)表示无人机i和j的路径在point_approx附近交叉。 conflicts [] n_drones len(paths) for i in range(n_drones): for j in range(i1, n_drones): path_i paths[i] path_j paths[j] # 检查路径i的每一段与路径j的每一段是否相交 for seg_i in range(len(path_i)-1): for seg_j in range(len(path_j)-1): p1, p2 path_i[seg_i], path_i[seg_i1] q1, q2 path_j[seg_j], path_j[seg_j1] intersect_point, dist segment_distance(p1, p2, q1, q2) if dist conflict_threshold: # 线段最小距离小于阈值认为潜在冲突 conflicts.append((i, j, intersect_point)) return conflicts def segment_distance(p1, p2, q1, q2): 计算两条三维线段之间的最短距离和最近点。 简化这里可以只考虑二维xy平面或使用现有几何库。 返回: (最近点坐标, 最短距离) # 实现略可使用向量投影方法计算 pass关键细节3冲突判定的模糊性。两条路径不一定精确相交于一点。更常见的情况是它们以一定距离“擦肩而过”。因此使用一个conflict_threshold如安全距离的1.5倍来判断更为合理。计算线段间最短距离比判断是否相交更通用。5.3 基于时间窗口的冲突消解优化模型ortools示例这是整个协同规划的大脑。我们使用Google的ortools库来建模和求解这个调度问题。from ortools.sat.python import cp_model def resolve_conflicts_with_cp(drones: List[Drone], conflicts: List, V_nom5.0, delta_t_min2.0): 使用约束规划解决冲突。 drones: 无人机对象列表其中包含初始路径和路径段长度。 conflicts: 冲突列表每个冲突包含 (drone_i_id, drone_j_id, conflict_point, seg_idx_i, seg_idx_j)。 V_nom: 名义飞行速度 (m/s)。 delta_t_min: 最小安全时间间隔 (s)。 model cp_model.CpModel() num_drones len(drones) # 决策变量每架无人机的出发时间偏移量或每段的速度比例因子 # 这里简化为每架无人机定义一个总的时间缩放因子 k_i 实际速度 V_nom * k_i, k_i在[0.8, 1.2]之间 k {} for i in range(num_drones): k[i] model.NewIntVar(80, 120, fk_{i}) # 用整数表示避免浮点数实际值为 k/100 # 计算每架无人机到达其各个冲突点的时间基于路径长度和速度 # 首先需要知道从起点到冲突点经过的路径总长度 # 假设我们已经有了一个函数 calc_distance_to_conflict(drone_id, conflict_info) arrival_times {} # 存储每个冲突点对应的到达时间表达式 for (i, j, cp, seg_i, seg_j) in conflicts: dist_i calc_distance_to_conflict(drones[i], seg_i, cp) dist_j calc_distance_to_conflict(drones[j], seg_j, cp) # 到达时间 距离 / (V_nom * k_i/100) # 由于ortools CP-SAT主要处理整数我们可以将时间放大100倍来避免浮点数 arrival_i model.NewIntVar(0, 10000, farrival_i_{i}_{j}) model.Add(arrival_i (dist_i * 100) // (V_nom * k[i])) # 整数除法近似 arrival_j model.NewIntVar(0, 10000, farrival_j_{i}_{j}) model.Add(arrival_j (dist_j * 100) // (V_nom * k[j])) # 添加顺序约束时间差必须大于 delta_t_min # 引入0-1变量 b b1 表示 i 先于 j 通过 b model.NewBoolVar(fb_{i}_{j}) M 10000 # 大M # 如果 bTrue (1)则约束 arrival_j arrival_i delta_t_min model.Add(arrival_j arrival_i int(delta_t_min * 100)).OnlyEnforceIf(b) # 如果 bFalse (0)则约束 arrival_i arrival_j delta_t_min model.Add(arrival_i arrival_j int(delta_t_min * 100)).OnlyEnforceIf(b.Not()) arrival_times[(i, j, cp)] (arrival_i, arrival_j, b) # 目标函数最小化最大完成时间makespan # 计算每架无人机的总飞行时间 finish_times [] for i in range(num_drones): total_dist drones[i].total_path_length finish model.NewIntVar(0, 10000, ffinish_{i}) model.Add(finish (total_dist * 100) // (V_nom * k[i])) finish_times.append(finish) makespan model.NewIntVar(0, 10000, makespan) model.AddMaxEquality(makespan, finish_times) model.Minimize(makespan) # 求解 solver cp_model.CpSolver() solver.parameters.max_time_in_seconds 30.0 # 设置求解时间限制 status solver.Solve(model) if status cp_model.OPTIMAL or status cp_model.FEASIBLE: print(Solution found.) for i in range(num_drones): k_val solver.Value(k[i]) / 100.0 print(fDrone {i}: speed factor {k_val}) # 根据求解出的k值重新计算各无人机精确的时间表更新到drone对象中 # ... else: print(No solution found.)关键细节4模型线性化与近似。上面的模型做了大量简化用整数近似浮点数、用固定速度比例因子代替分段速度、用整数除法近似除法运算。在数学建模中这完全是可以接受的但必须在论文中说明这些简化及其潜在影响。更精确的模型可能需要使用ortools的线性规划求解器或更专业的优化工具但复杂度会大大增加。关键细节5求解器调参与退火。约束规划或混合整数规划求解器可能找不到最优解尤其是在规模较大时。需要设置合理的求解时间限制并准备好接受可行解。也可以考虑使用局部搜索或遗传算法等元启发式算法来求解这个调度问题这类算法更容易编码且对模型形式要求更灵活非常适合数学建模。6. 从仿真到现实的鸿沟模型局限性与进阶思考完成建模和代码跑出一个漂亮的仿真动画论文就可以收尾了吗对于一个有经验的从业者来说这才是思考的开始。我们的模型建立在诸多理想化假设之上真实世界要残酷得多。不确定性无处不在我们的模型假设无人机定位绝对精准、环境地图完全已知、控制指令被完美执行。现实中GPS有误差视觉/激光雷达感知有噪声和延迟风会干扰无人机电机响应有波动。因此规划出的轨迹必须具有鲁棒性。一种方法是在规划时考虑不确定性比如将障碍物膨胀得更大增加安全距离或者使用随机模型预测控制。在建模论文中可以讨论“如果无人机定位存在1米误差我们的方案如何保证安全”并提出增加安全冗余、在线重规划等策略。通信与分布式协调我们的集中式调度模型假设所有信息全局可知。在真实多机系统中通信可能受限、延迟甚至中断。这就需要分布式协同算法例如基于一致性协议或市场拍卖机制。每架无人机只根据局部信息自身状态、邻居状态做出决策通过迭代协商达成一致。在论文中可以将其作为一个重要的扩展方向进行讨论。动态障碍物题目可能只说了静态障碍但现实环境是动态的。处理动态障碍物如其他无人机、飞鸟需要感知-预测-规划的闭环。常用的方法是速度障碍法或动态窗口法它们根据障碍物的当前速度和位置预测其未来轨迹并为本机规划出一个无碰撞的速度矢量。这通常作为局部实时避障层叠加在全局规划层之上。计算实时性我们前面讨论的优化模型对于在线实时规划来说可能太慢。在实际飞控中通常采用分层架构顶层进行低频的全局重规划如每秒1次底层进行高频的局部轨迹优化和控制如每秒100次。局部规划器使用更轻量级的算法如人工势场法或模型预测控制来跟踪全局路径并规避突然出现的障碍。所以当你完成这个题目的建模后真正有价值的部分是在论文的“模型评价与推广”部分坦诚地讨论这些局限性并指出在实际工程中需要如何改进。这能体现出你对问题理解的深度远远超过仅仅给出一个算法和结果。7. 参赛建议与代码资源利用对于参加数学建模竞赛的同学结合这个题目我有几条非常具体的建议第一合理分配时间抓住主要矛盾。三天或四天时间不可能实现一个完美的、工程级的系统。你们的重点是展示建模思想、求解过程和结果分析。因此Day1彻底理解题目完成问题分析、模型假设、符号说明。确定你们的技术路线例如采用“解耦式规划A* 冲突消解调度”。完成环境建模和单机A*路径规划的代码。Day2实现冲突检测和基于时间窗口的调度模型。使用优化求解器如ortools或自己编写启发式算法如遗传算法求解调度问题。得到初步的协同航迹。Day3进行大量仿真实验分析不同参数无人机数量、障碍物密度、安全距离对结果总时间、成功率的影响。绘制漂亮的路径图和时空图。撰写论文主体部分。Day4完善论文撰写摘要总结模型优缺点提出改进方向。检查全文。第二善用开源代码但必须理解并改造。GitHub上有大量无人机路径规划的代码例如PythonRobotics项目包含了A*、RRT、RRT*等算法的清晰实现。MotionPlanning相关仓库也有许多样例。但切记不要直接复制粘贴。竞赛论文需要你们自己的代码和结果。重点理解算法的核心逻辑然后根据你们自己的模型假设进行修改。比如把二维A*改成三维在代价函数中加入高度惩罚。参考代码的架构但数据结构和接口要重写成适合你们问题的方式。第三可视化是王道。评委看论文的时间很短一张直观的图胜过千言万语。一定要绘制二维/三维路径规划图用不同颜色线条表示不同无人机用圆圈或方块表示障碍物。绘制时空图。这是体现协同避障精髓的图。横轴是时间纵轴是路径长度或关键路径点的位置每条线代表一架无人机的进度。在交叉点处确保线条没有重叠直观展示出时间上的错开。制作仿真动画。用matplotlib.animation可以制作简单的2D动画展示无人机如何沿着规划路径飞行并避免碰撞。这将是论文的一大亮点。第四模型检验要全面。不要只展示一个完美案例。要设计多种场景简单场景验证算法基础功能。复杂密集场景测试算法的鲁棒性和效率。极端场景如通道非常狭窄必须严格排序通过。分析算法是否仍然有效瓶颈在哪里。参数敏感性分析改变安全距离、无人机最大速度等参数观察对总航行时间的影响并用图表展示。无人机协同避障航迹规划是一个迷人的问题它站在理论数学、计算机科学和工程实践的交叉点上。深圳杯的这道题给了我们一个绝佳的契机去深入探索这个领域。通过这次建模你收获的不仅仅是一篇论文和可能的奖项更是一套解决复杂系统优化问题的思维框架——如何分解问题、如何权衡简化与精确、如何将算法转化为代码、如何评价和推广你的方案。这才是数学建模竞赛乃至我们解决任何工程问题时最宝贵的财富。 无人机协同避障航迹规划：从A*算法到轨迹优化的工程实践
1. 从数学建模到真实飞控无人机协同避障航迹规划的核心挑战最近几年无论是全国大学生数学建模竞赛还是深圳杯这类地区性赛事无人机相关的题目热度一直居高不下。2023年深圳杯数学建模C题就是一个典型代表它把“无人机协同避障航迹规划”这个既前沿又充满工程挑战的问题抛给了参赛者。很多同学拿到题目后第一反应可能是去搜索“路径优化算法”、“A*算法”、“遗传算法”这些关键词然后试图套用一个现成的模型。但如果你真的在无人机行业里摸爬滚打过就会知道把数学建模论文里的那条“最优路径”变成天空中无人机实际飞行的轨迹中间隔着一道巨大的鸿沟。这道鸿沟就是理论与工程实践的差距。这道题目的价值远不止于完成一篇论文。它本质上是在考察我们如何将一个复杂的、多约束的工程问题抽象成一个可计算的数学模型并最终能指导或仿真实际系统的行为。协同意味着多架无人机避障意味着环境感知与动态决策航迹规划则是在满足动力学约束、时间约束、安全约束下的空间路径寻优。这几乎涵盖了无人机系统从上层决策到底层控制的所有核心环节。因此讨论这个问题不能只停留在“我用什么算法得了高分”更要深入去想“这个算法为什么有效”、“它的假设在现实中成立吗”、“代码跑出来的路径真能飞吗”。这就是我想在这篇分享里重点聊的如何穿透数学建模的表象去理解无人机航迹规划里那些真正棘手的问题并给出一些具有实操性的思路和代码设计哲学。2. 问题拆解协同避障航迹规划到底在规划什么在动手建模型、写代码之前我们必须把问题本身掰开揉碎看清楚它的每一个约束和边界。题目通常会给一个场景比如多架无人机从不同起点出发前往各自终点区域中存在静态障碍物如山丘、建筑物甚至可能有动态障碍如其他飞行器要求规划出满足最小安全距离、最短时间或最节能的飞行路径。2.1 核心约束条件分析首先我们要明确无人机不是质点它的飞行受到物理规律的严格限制。这是许多纯数学路径规划算法如传统的图搜索算法容易忽略的第一点。动力学约束无人机有最大速度、最大加速度包括线加速度和角加速度的限制。这意味着路径不能有急转弯曲率必须连续且在一定范围内。你规划出一条理论上最短的、包含直角转弯的折线无人机是根本飞不出来的强行跟踪会导致失稳甚至坠毁。因此规划出的路径必须是可飞行的通常要求是C1或C2连续即一阶导数连续或二阶导数连续。避障约束这不仅仅是“不撞上”。障碍物通常被建模为圆柱体、长方体或凸多面体。安全距离约束要求无人机与所有障碍物表面的距离必须大于一个阈值例如10米。这个阈值不仅要考虑无人机本身的尺寸还要考虑定位误差、控制误差和风扰等不确定性。所以我们规划的其实是一个安全走廊而不仅仅是一条线。协同约束多机之间必须保持安全距离防止碰撞。这引入了时空维度的耦合。两架无人机的路径在空间上交叉并不可怕可怕的是它们在同一时刻到达交叉点。因此协同规划的本质是空间与时间的联合分配可能需要引入优先级、预约机制或实时协商。任务约束可能包括到达时间窗口、总航程限制、能量消耗与速度、加速度相关等。在数学建模中这些往往成为优化问题的目标函数或附加约束条件。2.2 环境建模从连续世界到离散图计算机无法直接处理连续空间中的无限可能路径。因此航迹规划的第一步总是环境建模与离散化。常见的方法有栅格法将三维空间划分为均匀的立方体栅格Voxel。每个栅格标记为“自由”、“障碍”或“未知”。这种方法直观易于实现碰撞检测只需判断无人机占据的栅格状态但缺点也很明显精度与计算开销矛盾精细栅格导致维度爆炸且路径只能是栅格中心的连线不够平滑。注意在三维栅格中规划搜索空间是O(n³)急剧膨胀。通常需要结合分层规划先在粗粒度栅格找到大致路径再在局部进行精细化平滑。采样法如快速随机扩展树RRT及其变种RRT* Informed RRT*。通过在自由空间中随机采样点并尝试将其与树中现有节点连接来构建一棵路径树。这种方法在高维空间如包含姿态的规划中效率很高能快速找到可行解并渐进趋向最优。对于本题的静态环境RRT* 是一个强有力的候选算法。图搜索法在栅格或特定路点Waypoint构成的状态空间图上进行搜索如A*、D*、Dijkstra算法。A*算法通过启发式函数引导搜索方向效率很高。但关键是如何设计“状态”。对于无人机状态可能不仅仅是(x, y, z)位置还应包括速度矢量(vx, vy, vz)甚至时间t。这就引出了状态栅格或时空状态空间的概念在这个空间里进行搜索可以自然地满足动力学和协同避障约束但计算量更大。选择建议对于数学建模竞赛由于计算时间有限环境通常被简化。我推荐采用分层规划策略上层使用A*在粗粒度栅格上规划一条无碰撞的几何路径下层采用轨迹优化方法如Minimum Snap轨迹生成将这条折线路径优化成一条平滑、满足动力学约束的多项式轨迹。这样既能保证可行性又能在模型中体现深度。3. 算法工具箱哪些模型能扛起协同避障的大旗明确了问题和环境模型后我们来盘点一下核心的算法工具箱。没有一种算法是万能的关键在于根据问题特点进行选择和组合。3.1 单机航迹规划基础算法这是协同规划的基础。假设环境中只有静态障碍物。A搜索算法*这是必须掌握的经典算法。其核心是代价函数 f(n) g(n) h(n)。g(n)是从起点到节点n的实际代价h(n)是从节点n到终点的估计代价启发函数。在三维栅格中h(n)常采用欧几里得距离或曼哈顿距离。关键点启发函数h(n)必须满足可采纳性admissible即不高估实际代价才能保证找到最优解。在三维中欧几里得距离是可采纳的。实操改进对于无人机g(n)不能只是路径长度还应考虑转向代价、爬升代价这可以通过在节点扩展时赋予不同方向的移动以不同代价来实现。# A* 算法核心逻辑的简化伪代码示意 import heapq def a_star(start, goal, grid): open_set [] heapq.heappush(open_set, (0, start)) came_from {} g_score {start: 0} f_score {start: heuristic(start, goal)} while open_set: current heapq.heappop(open_set)[1] if current goal: return reconstruct_path(came_from, current) for neighbor in get_neighbors(current, grid): tentative_g_score g_score[current] cost(current, neighbor) if neighbor not in g_score or tentative_g_score g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g_score f_score[neighbor] g_score[neighbor] heuristic(neighbor, goal) heapq.heappush(open_set, (f_score[neighbor], neighbor)) return None # 路径不存在RRT算法*相较于基础RRTRRT* 具有渐进最优性。它在找到初始路径后会不断地对路径进行“重布线”和“重选择父节点”操作从而逐步优化路径代价。优势无需离散化环境在高维空间和复杂障碍物环境下非常有效能直接产生连续空间中的路径。劣势收敛到最优解需要大量采样点耗时较长路径可能不够平滑仍需后处理。竞赛应用在建模时可以将其作为一个对比算法展示其在复杂狭窄空间中的优势同时指出其实时性不足的问题。3.2 从路径到轨迹满足动力学约束的关键一步通过A或RRT得到的一系列路径点Waypoints只是一串空间坐标。无人机控制器如PID或模型预测控制需要的是随时间变化的、平滑的轨迹参考信号。这一步就是轨迹生成。目前最主流的方法是使用多项式轨迹特别是Minimum Snap轨迹最小化加加速度的积分或Minimum Jerk轨迹最小化加速度的积分。对于多旋翼无人机其平动动力学与姿态解耦控制输入与加速度相关因此Minimum Snap最小化推力变化率能带来更平稳的飞行。核心思想将整个轨迹按路径点分段每一段用一个高阶多项式如7次多项式表示。通过设定每一段起点和终点的位置、速度、加速度、加加速度等边界条件并保证在路径点处这些状态连续可以构造一个庞大的线性方程组。最小化“Snap”四阶导数的积分平方可以转化为一个二次规划问题有高效的封闭解。# Minimum Snap轨迹生成的核心步骤概念性描述 import numpy as np from scipy.optimize import minimize # 假设我们有3个路径点分成2段轨迹每段用7次多项式表示 # 状态量每段轨迹的8个系数 (c0, c1, ..., c7)其中 p(t) c0 c1*t c2*t^2 ... c7*t^7 # 约束包括 # 1. 起点和终点的位置、速度、加速度、加加速度已知或设为0。 # 2. 中间路径点的位置已知以及通过该点时的速度、加速度连续性。 # 目标函数最小化所有段Snap的平方积分和。 def objective(coefficients): # coefficients 包含所有段的系数 total_cost 0 for seg_coef in segments: # 计算该段多项式第4阶导数的系数 snap_coef ... # 对多项式系数求4次导得到的新系数 # 计算该段上snap平方的积分这是一个关于多项式系数的二次型 cost seg_coef.T Q_matrix seg_coef # Q_matrix由时间区间和多项式阶次决定 total_cost cost return total_cost # 构建线性等式约束边界条件和连续性条件 A_eq, b_eq build_equality_constraints(...) # 构建线性不等式约束如速度、加速度限幅 A_ineq, b_ineq build_inequality_constraints(...) # 求解二次规划问题 result minimize(objective, x0, constraints{type: eq, fun: lambda x: A_eq x - b_eq}, bounds...) optimal_coefficients result.x这个过程的数学推导和矩阵构建是建模的难点和亮点。在论文中清晰地展示如何将物理约束转化为数学约束是获得高分的关键。3.3 多机协同规划的核心策略当有多架无人机时问题复杂度呈指数增长。主要策略有三类集中式规划将所有无人机视为一个整体在联合状态空间所有无人机状态的笛卡尔积中进行规划。这能得到全局最优解但状态空间维度是单机的N倍计算完全不可行只适用于2-3架无人机的理论分析。分布式规划每架无人机独立规划自己的路径但通过通信共享意图并在检测到潜在冲突时进行协商调整。这是目前研究的热点也更贴近实际应用。基于优先级的路径规划为无人机分配固定的优先级。高优先级无人机按单机规划其最优路径。低优先级无人机在规划时将高优先级无人机的时空轨迹视为动态障碍物进行避让。这种方法简单但可能导致低优先级无人机路径严重次优。基于时空预约的规划将空域和时间划分为网格时空体素。每架无人机在规划时先“预约”它将要占据的时空体素。如果发生冲突两架无人机预约了同一时空体素后规划的无人机需要绕行或等待。这类似于交通信号灯或预约系统。解耦式规划这是数学建模中最实用的方法。先进行路径规划忽略时间只考虑空间冲突。得到多条空间路径后再进行速度规划为每架无人机安排通过路径上各点的时间从而避免在同一时间到达空间交叉点。这通常通过调整无人机的飞行速度在最大最小速度范围内来实现可以建模为一个线性规划或约束优化问题。对于深圳杯C题这类赛题我强烈推荐采用“解耦式规划”思路。因为它将复杂的时空联合优化问题分解为两个相对简单的子问题大大降低了建模和求解难度同时又能清晰地体现在论文中。具体步骤可以是步骤一为每架无人机独立规划一条避开所有静态障碍物的空间路径使用改进的A或RRT。步骤二检查任意两条路径是否存在空间交叉点。如果存在标记这些交叉区域。步骤三在交叉区域附近将无人机视为在一条“单行道”上行驶的车辆。通过为每架无人机分配不同的通过时间例如引入微小的速度调整或等待确保它们不会同时进入交叉区域。这可以转化为一个冲突消解的时间表优化问题。4. 建模实战构建一个可求解的优化模型理论说再多不如一个具体的模型。我们尝试为“多无人机、静态障碍、最小化总飞行时间”这个典型场景建立一个优化模型。4.1 模型假设与符号定义假设1无人机简化为一个质点但其飞行需满足最大速度V_max、最大加速度a_max的约束。假设2障碍物为已知位置和大小的圆柱体。安全距离为d_safe。假设3所有无人机在同一高度层飞行或飞行高度已预先确定简化问题为二维。假设4无人机匀速飞行不这太理想。我们假设其在路径段上可以加速/减速但整体运动用分段匀加速模型近似以便于计算时间。符号U: 无人机集合i ∈ U。O: 障碍物集合k ∈ O。P_i: 无人机i的路径由一系列路径点p_i^m(m0,1,...,M_i)组成其中p_i^0为起点p_i^{M_i}为终点。t_i^m: 无人机i到达路径点p_i^m的时刻。s_i^m: 无人机i在路径段m从点p_i^m到p_i^{m1}上计划行驶的距离。v_i^m: 无人机i在路径段m上的平均速度决策变量之一。4.2 目标函数与约束条件目标函数最小化最后一架无人机到达终点的时间makespan或所有无人机飞行时间之和。Minimize T max_i (t_i^{M_i}) 或 Minimize Σ_i (t_i^{M_i} - t_i^0)约束条件运动学约束速度上下限0 v_i^m V_max加速度近似约束相邻段速度差限制|v_i^{m1} - v_i^m| / ΔT_i^m a_max其中ΔT_i^m是飞过段m的估计时间。这个约束是非线性的因为时间本身取决于速度。一个常见的线性化技巧是固定路径段的飞行时间或采用更精细的离散化。避障约束静态对于每个路径点p_i^m其与每个障碍物k中心o_k的距离必须大于障碍物半径R_k加上安全距离d_safe。|| p_i^m - o_k ||_2 R_k d_safe。这是非线性约束欧几里得范数。防碰撞约束无人机间这是最核心的协同约束。对于任意两架无人机i, j(i≠j)以及任意时刻t或任意对应的路径点/段它们之间的距离必须大于安全距离d_safe。|| pos_i(t) - pos_j(t) ||_2 d_safe。这是一个连续的时空约束直接处理极其困难。关键简化解耦法我们只在已知的路径交叉点施加此约束。假设无人机i和j的路径在空间点C交叉。设它们到达C点的时间分别为t_i^C和t_j^C。则约束可写为| t_i^C - t_j^C | Δt_min。其中Δt_min是一个最小安全时间间隔其值取决于无人机在交叉点附近的速度和安全距离。Δt_min d_safe / (v_i^C v_j^C)这里v_i^C是无人机i在交叉点附近的速度。这又将速度变量耦合了进来。进一步简化竞赛实用假设所有无人机以恒定速度V飞行或每架无人机一个恒定速度V_i。那么路径点之间的飞行时间与距离成正比。防碰撞约束就简化为对到达关键路径点交叉点的时间差的约束。我们可以通过调整每架无人机的出发时间或在非关键路径段的速度来满足这个时间差约束。这就转化为了一个混合整数线性规划问题为每一对在交叉点可能冲突的无人机引入一个0-1决策变量δ_{ij}表示谁先通过。约束变为t_i^C M * (1 - δ_{ij}) t_j^C Δt_mint_j^C M * δ_{ij} t_i^C Δt_min其中M是一个很大的正数。这组约束确保了t_i^C和t_j^C至少相差Δt_min。4.3 模型求解思路这个模型包含了非线性约束避障和整数变量冲突顺序是一个混合整数非线性规划问题直接求解非常困难。在数学建模竞赛中我们需要采用启发式或分层求解策略第一阶段固定路径的空间规划。使用A*算法为每架无人机生成一条初始无碰撞路径仅针对静态障碍。此时只考虑空间不考虑时间和无人机间冲突。得到一系列路径点P_i。第二阶段冲突检测与关键点识别。遍历所有无人机路径检测两两之间路径线段是否相交在二维平面中可通过计算几何方法判断。记录下所有交叉点C。第三阶段基于时间窗口的冲突消解。这是核心优化步骤。假设每架无人机以恒定速度V_i飞行V_i可在[V_min, V_max]范围内作为连续变量优化或固定为V_max以最小化时间。根据路径长度和速度可以计算出每架无人机到达每个交叉点C的时间窗口[earliest_i^C, latest_i^C]。最早时间由最大速度计算最晚时间由最小速度计算或加入等待时间。问题转化为为每个交叉点C上的每一对冲突无人机(i, j)分配一个通过顺序并调整它们的速度或出发时间使得实际通过时间满足顺序和安全间隔要求同时优化总时间。这可以建模为一个约束规划或混合整数线性规划问题。虽然规模可能不小但借助ortools、PuLP等优化库对于赛题规模如5-10架无人机数个交叉点是可解的。第四阶段轨迹平滑与输出。根据最终确定的路径点和速度或时间利用前面提到的Minimum Snap方法生成平滑的轨迹作为最终输出。5. 参考代码框架与关键实现细节这里不可能给出全部代码但我会勾勒出核心模块的框架和实现时容易踩坑的细节。5.1 环境与路径规划模块Python示例import numpy as np import matplotlib.pyplot as plt from scipy.spatial import KDTree import heapq from typing import List, Tuple class Drone: def __init__(self, start, goal, id): self.id id self.start np.array(start) self.goal np.array(goal) self.path [] # 路径点列表 (x, y, z) self.timestamps [] # 到达每个路径点的计划时间 class Obstacle: def __init__(self, center, radius): self.center np.array(center) self.radius radius class PathPlanner: def __init__(self, grid_size, bounds): self.grid_size grid_size self.bounds bounds # ((x_min, x_max), (y_min, y_max), (z_min, z_max)) self.obstacles [] def add_obstacle(self, obs: Obstacle): self.obstacles.append(obs) def is_collision_free(self, point): 检查一个点是否与任何障碍物碰撞 for obs in self.obstacles: if np.linalg.norm(point[:2] - obs.center[:2]) obs.radius: # 假设2D障碍 return False return True def a_star_plan(self, start, goal): 在二维/三维栅格上进行A*搜索 # 将连续坐标离散化为栅格索引 start_idx self.continuous_to_index(start) goal_idx self.continuous_to_index(goal) open_set [] heapq.heappush(open_set, (0, start_idx)) came_from {} g_score {tuple(start_idx): 0} f_score {tuple(start_idx): self.heuristic(start_idx, goal_idx)} while open_set: _, current heapq.heappop(open_set) if current tuple(goal_idx): return self.reconstruct_path(came_from, current, start_idx) for neighbor in self.get_neighbors(current): if not self.is_index_collision_free(neighbor): continue tentative_g g_score[current] self.distance(current, neighbor) if tuple(neighbor) not in g_score or tentative_g g_score[tuple(neighbor)]: came_from[tuple(neighbor)] current g_score[tuple(neighbor)] tentative_g f_score[tuple(neighbor)] tentative_g self.heuristic(neighbor, goal_idx) heapq.heappush(open_set, (f_score[tuple(neighbor)], neighbor)) return None # 未找到路径 def heuristic(self, a, b): # 欧几里得距离作为启发函数 return np.linalg.norm(np.array(a) - np.array(b)) def distance(self, a, b): # 实际代价可以加入转向惩罚 return np.linalg.norm(np.array(a) - np.array(b)) # ... 其他方法continuous_to_index, get_neighbors, reconstruct_path, is_index_collision_free关键细节1碰撞检测的保守性。is_collision_free函数检查的是点是否在障碍物内。但无人机有尺寸应该检查整个机体是否碰撞。一个实用的方法是在路径点处检查点在路径段两点之间进行采样检查比如在连接线上每隔0.1米取一个点进行碰撞检测。更严格的方法是计算线段到障碍物圆心的最短距离。关键细节2启发函数的选择。在三维中欧几里得距离是可采纳的但可能不是最有效的。如果无人机主要在同一高度飞行可以先用二维距离计算再考虑高度差这能加快搜索速度。5.2 冲突检测模块def detect_path_conflicts(paths: List[List[np.ndarray]], conflict_threshold5.0): 检测多条路径之间的空间交叉点。 paths: 列表的列表每个内层列表是一个无人机的路径点序列。 conflict_threshold: 判定为交叉的距离阈值米。 返回: 一个列表每个元素为 (i, j, point_approx)表示无人机i和j的路径在point_approx附近交叉。 conflicts [] n_drones len(paths) for i in range(n_drones): for j in range(i1, n_drones): path_i paths[i] path_j paths[j] # 检查路径i的每一段与路径j的每一段是否相交 for seg_i in range(len(path_i)-1): for seg_j in range(len(path_j)-1): p1, p2 path_i[seg_i], path_i[seg_i1] q1, q2 path_j[seg_j], path_j[seg_j1] intersect_point, dist segment_distance(p1, p2, q1, q2) if dist conflict_threshold: # 线段最小距离小于阈值认为潜在冲突 conflicts.append((i, j, intersect_point)) return conflicts def segment_distance(p1, p2, q1, q2): 计算两条三维线段之间的最短距离和最近点。 简化这里可以只考虑二维xy平面或使用现有几何库。 返回: (最近点坐标, 最短距离) # 实现略可使用向量投影方法计算 pass关键细节3冲突判定的模糊性。两条路径不一定精确相交于一点。更常见的情况是它们以一定距离“擦肩而过”。因此使用一个conflict_threshold如安全距离的1.5倍来判断更为合理。计算线段间最短距离比判断是否相交更通用。5.3 基于时间窗口的冲突消解优化模型ortools示例这是整个协同规划的大脑。我们使用Google的ortools库来建模和求解这个调度问题。from ortools.sat.python import cp_model def resolve_conflicts_with_cp(drones: List[Drone], conflicts: List, V_nom5.0, delta_t_min2.0): 使用约束规划解决冲突。 drones: 无人机对象列表其中包含初始路径和路径段长度。 conflicts: 冲突列表每个冲突包含 (drone_i_id, drone_j_id, conflict_point, seg_idx_i, seg_idx_j)。 V_nom: 名义飞行速度 (m/s)。 delta_t_min: 最小安全时间间隔 (s)。 model cp_model.CpModel() num_drones len(drones) # 决策变量每架无人机的出发时间偏移量或每段的速度比例因子 # 这里简化为每架无人机定义一个总的时间缩放因子 k_i 实际速度 V_nom * k_i, k_i在[0.8, 1.2]之间 k {} for i in range(num_drones): k[i] model.NewIntVar(80, 120, fk_{i}) # 用整数表示避免浮点数实际值为 k/100 # 计算每架无人机到达其各个冲突点的时间基于路径长度和速度 # 首先需要知道从起点到冲突点经过的路径总长度 # 假设我们已经有了一个函数 calc_distance_to_conflict(drone_id, conflict_info) arrival_times {} # 存储每个冲突点对应的到达时间表达式 for (i, j, cp, seg_i, seg_j) in conflicts: dist_i calc_distance_to_conflict(drones[i], seg_i, cp) dist_j calc_distance_to_conflict(drones[j], seg_j, cp) # 到达时间 距离 / (V_nom * k_i/100) # 由于ortools CP-SAT主要处理整数我们可以将时间放大100倍来避免浮点数 arrival_i model.NewIntVar(0, 10000, farrival_i_{i}_{j}) model.Add(arrival_i (dist_i * 100) // (V_nom * k[i])) # 整数除法近似 arrival_j model.NewIntVar(0, 10000, farrival_j_{i}_{j}) model.Add(arrival_j (dist_j * 100) // (V_nom * k[j])) # 添加顺序约束时间差必须大于 delta_t_min # 引入0-1变量 b b1 表示 i 先于 j 通过 b model.NewBoolVar(fb_{i}_{j}) M 10000 # 大M # 如果 bTrue (1)则约束 arrival_j arrival_i delta_t_min model.Add(arrival_j arrival_i int(delta_t_min * 100)).OnlyEnforceIf(b) # 如果 bFalse (0)则约束 arrival_i arrival_j delta_t_min model.Add(arrival_i arrival_j int(delta_t_min * 100)).OnlyEnforceIf(b.Not()) arrival_times[(i, j, cp)] (arrival_i, arrival_j, b) # 目标函数最小化最大完成时间makespan # 计算每架无人机的总飞行时间 finish_times [] for i in range(num_drones): total_dist drones[i].total_path_length finish model.NewIntVar(0, 10000, ffinish_{i}) model.Add(finish (total_dist * 100) // (V_nom * k[i])) finish_times.append(finish) makespan model.NewIntVar(0, 10000, makespan) model.AddMaxEquality(makespan, finish_times) model.Minimize(makespan) # 求解 solver cp_model.CpSolver() solver.parameters.max_time_in_seconds 30.0 # 设置求解时间限制 status solver.Solve(model) if status cp_model.OPTIMAL or status cp_model.FEASIBLE: print(Solution found.) for i in range(num_drones): k_val solver.Value(k[i]) / 100.0 print(fDrone {i}: speed factor {k_val}) # 根据求解出的k值重新计算各无人机精确的时间表更新到drone对象中 # ... else: print(No solution found.)关键细节4模型线性化与近似。上面的模型做了大量简化用整数近似浮点数、用固定速度比例因子代替分段速度、用整数除法近似除法运算。在数学建模中这完全是可以接受的但必须在论文中说明这些简化及其潜在影响。更精确的模型可能需要使用ortools的线性规划求解器或更专业的优化工具但复杂度会大大增加。关键细节5求解器调参与退火。约束规划或混合整数规划求解器可能找不到最优解尤其是在规模较大时。需要设置合理的求解时间限制并准备好接受可行解。也可以考虑使用局部搜索或遗传算法等元启发式算法来求解这个调度问题这类算法更容易编码且对模型形式要求更灵活非常适合数学建模。6. 从仿真到现实的鸿沟模型局限性与进阶思考完成建模和代码跑出一个漂亮的仿真动画论文就可以收尾了吗对于一个有经验的从业者来说这才是思考的开始。我们的模型建立在诸多理想化假设之上真实世界要残酷得多。不确定性无处不在我们的模型假设无人机定位绝对精准、环境地图完全已知、控制指令被完美执行。现实中GPS有误差视觉/激光雷达感知有噪声和延迟风会干扰无人机电机响应有波动。因此规划出的轨迹必须具有鲁棒性。一种方法是在规划时考虑不确定性比如将障碍物膨胀得更大增加安全距离或者使用随机模型预测控制。在建模论文中可以讨论“如果无人机定位存在1米误差我们的方案如何保证安全”并提出增加安全冗余、在线重规划等策略。通信与分布式协调我们的集中式调度模型假设所有信息全局可知。在真实多机系统中通信可能受限、延迟甚至中断。这就需要分布式协同算法例如基于一致性协议或市场拍卖机制。每架无人机只根据局部信息自身状态、邻居状态做出决策通过迭代协商达成一致。在论文中可以将其作为一个重要的扩展方向进行讨论。动态障碍物题目可能只说了静态障碍但现实环境是动态的。处理动态障碍物如其他无人机、飞鸟需要感知-预测-规划的闭环。常用的方法是速度障碍法或动态窗口法它们根据障碍物的当前速度和位置预测其未来轨迹并为本机规划出一个无碰撞的速度矢量。这通常作为局部实时避障层叠加在全局规划层之上。计算实时性我们前面讨论的优化模型对于在线实时规划来说可能太慢。在实际飞控中通常采用分层架构顶层进行低频的全局重规划如每秒1次底层进行高频的局部轨迹优化和控制如每秒100次。局部规划器使用更轻量级的算法如人工势场法或模型预测控制来跟踪全局路径并规避突然出现的障碍。所以当你完成这个题目的建模后真正有价值的部分是在论文的“模型评价与推广”部分坦诚地讨论这些局限性并指出在实际工程中需要如何改进。这能体现出你对问题理解的深度远远超过仅仅给出一个算法和结果。7. 参赛建议与代码资源利用对于参加数学建模竞赛的同学结合这个题目我有几条非常具体的建议第一合理分配时间抓住主要矛盾。三天或四天时间不可能实现一个完美的、工程级的系统。你们的重点是展示建模思想、求解过程和结果分析。因此Day1彻底理解题目完成问题分析、模型假设、符号说明。确定你们的技术路线例如采用“解耦式规划A* 冲突消解调度”。完成环境建模和单机A*路径规划的代码。Day2实现冲突检测和基于时间窗口的调度模型。使用优化求解器如ortools或自己编写启发式算法如遗传算法求解调度问题。得到初步的协同航迹。Day3进行大量仿真实验分析不同参数无人机数量、障碍物密度、安全距离对结果总时间、成功率的影响。绘制漂亮的路径图和时空图。撰写论文主体部分。Day4完善论文撰写摘要总结模型优缺点提出改进方向。检查全文。第二善用开源代码但必须理解并改造。GitHub上有大量无人机路径规划的代码例如PythonRobotics项目包含了A*、RRT、RRT*等算法的清晰实现。MotionPlanning相关仓库也有许多样例。但切记不要直接复制粘贴。竞赛论文需要你们自己的代码和结果。重点理解算法的核心逻辑然后根据你们自己的模型假设进行修改。比如把二维A*改成三维在代价函数中加入高度惩罚。参考代码的架构但数据结构和接口要重写成适合你们问题的方式。第三可视化是王道。评委看论文的时间很短一张直观的图胜过千言万语。一定要绘制二维/三维路径规划图用不同颜色线条表示不同无人机用圆圈或方块表示障碍物。绘制时空图。这是体现协同避障精髓的图。横轴是时间纵轴是路径长度或关键路径点的位置每条线代表一架无人机的进度。在交叉点处确保线条没有重叠直观展示出时间上的错开。制作仿真动画。用matplotlib.animation可以制作简单的2D动画展示无人机如何沿着规划路径飞行并避免碰撞。这将是论文的一大亮点。第四模型检验要全面。不要只展示一个完美案例。要设计多种场景简单场景验证算法基础功能。复杂密集场景测试算法的鲁棒性和效率。极端场景如通道非常狭窄必须严格排序通过。分析算法是否仍然有效瓶颈在哪里。参数敏感性分析改变安全距离、无人机最大速度等参数观察对总航行时间的影响并用图表展示。无人机协同避障航迹规划是一个迷人的问题它站在理论数学、计算机科学和工程实践的交叉点上。深圳杯的这道题给了我们一个绝佳的契机去深入探索这个领域。通过这次建模你收获的不仅仅是一篇论文和可能的奖项更是一套解决复杂系统优化问题的思维框架——如何分解问题、如何权衡简化与精确、如何将算法转化为代码、如何评价和推广你的方案。这才是数学建模竞赛乃至我们解决任何工程问题时最宝贵的财富。