1. 从赛题到实战无人机协同避障航迹规划的核心挑战去年深圳杯数学建模C题一出来圈子里不少朋友都聊过题目本身挺有意思把无人机协同和航迹规划这两个热点捏在了一起。很多人第一反应是去找现成的算法套比如A*、RRT什么的但真做起来就会发现单纯调包跑个仿真和做出一个能逻辑自洽、经得起推敲的数学模型中间隔着一道鸿沟。这道题本质上考的不是你会不会用某个算法而是你能不能把一个复杂的工程问题抽象成清晰的数学问题并设计出合理的求解策略。无人机协同意味着多机之间不再是独立的个体它们共享空域任务可能交织还得互相让路避障航迹规划则要求在充满静态障碍物甚至可能是动态障碍的环境中为每一架无人机找出一条从起点到终点安全、高效的飞行路径。这听起来像是电影里的场景但实际上它的核心是优化问题——在无数条可能的路径中找到满足所有约束不撞墙、不撞机、符合动力学的那一条或一组最优解。我参与过类似的仿真项目也看过不少优秀论文发现大家最容易栽跟头的地方往往不是算法本身而是问题定义的清晰度和约束条件处理的严谨性。题目给了起点、终点、障碍物位置但无人机怎么飞是质点模型还是考虑尺寸转弯有最小半径限制吗速度能不能突变协同的“协同”到底指什么是仅仅保证不碰撞还是要求保持特定队形、同时到达这些细节题目可能不会完全明确但你的模型必须做出合理且自洽的假设并在论文中明确阐述。这恰恰是数学建模的精髓用数学语言描述世界并解决其中的问题。接下来我就结合常见的思路和实战中的一些坑拆解一下如何系统性地攻克这类题目。2. 问题拆解与模型构建把天空变成数学公式面对“协同避障航迹规划”这个复合问题直接上手编码是灾难性的。我们必须先把它拆解成几个可以建模的子问题并建立它们之间的逻辑联系。2.1 环境与智能体建模一切计算的基础首先我们需要定义无人机和它所在的世界。通常我们会做以下假设环境建模将三维空间离散化或者用连续的坐标表示。障碍物通常建模为立方体、圆柱体或多面体。对于规划而言我们需要一种快速判断任意点是否在障碍物内的方法。一个常见技巧是使用符号距离函数SDF或直接进行几何相交检测。在数学建模中为了简化常将障碍物视为球体或长方体其内部区域用不等式约束表示例如对于球心在(x0, y0, z0)半径为R的障碍物约束条件为(x-x0)²(y-y0)²(z-z0)² ≥ R²。无人机建模最简单的模型是质点模型即忽略无人机尺寸只用一个点代表其位置。但为了更真实的避障通常采用包络球模型或包围盒模型。例如将无人机视为一个半径为r的球体那么实际的避障约束就需要在障碍物半径上加上这个r。更复杂的模型会考虑无人机的动力学比如微分平坦特性但这在数学建模竞赛中可能过于复杂。一个折中的方法是引入运动学约束比如限制最大速度v_max、最大加速度a_max和最大转弯角速度。协同关系定义这是关键。“协同”可以指防碰撞协同任何两架无人机在任何时刻都必须保持一个最小安全距离d_safe。这是最基本的协同要求。时序协同例如要求多架无人机同时到达各自目标点或者按照特定时间顺序到达。通信与感知协同假设无人机之间可以共享位置信息用于分布式避障决策。在集中式规划中我们通常假设有一个全局规划器知晓所有信息。基于以上我们可以建立最基础的数学模型框架。设我们有N架无人机规划时间 horizon 为T。对于第i架无人机其轨迹可以表示为关于时间t的函数P_i(t) [x_i(t), y_i(t), z_i(t)]。那么核心的约束条件包括起点终点约束P_i(0) Start_i,P_i(T) Goal_i。障碍物避碰约束对于所有时间t和所有障碍物Obs_jdistance(P_i(t), Obs_j) ≥ R_obs r_uav。无人机间防撞约束对于所有时间t和任意i≠kdistance(P_i(t), P_k(t)) ≥ d_safe。动力学约束||v_i(t)|| ≤ v_max,||a_i(t)|| ≤ a_max其中v和a是位置的一阶和二阶导数。目标函数通常是最小化总飞行时间、总路径长度或总能量消耗。多目标情况下如既想时间短又想路径平滑可以使用加权和法。2.2 路径表示与优化变量选择轨迹P_i(t)是连续函数但计算机无法处理无限维对象。我们需要将其参数化。常见方法有分段线性折线将轨迹表示为一系列路径点Waypoints的连线。优化变量就是这些路径点的坐标。优点是简单直观但生成的路径不平滑无人机需要停在各点转向。多项式轨迹如Minimum Snap将每一段轨迹表示为一个高阶多项式通常是5次或7次通过优化多项式系数来使轨迹平滑最小化加加速度的平方积分。这是目前无人机轨迹规划的主流方法之一能生成非常平滑、可直接被飞控执行的轨迹。其优化变量是多项式系数。B样条曲线B样条具有局部修改性和凸包性质非常适合用于避障规划。通过优化B样条的控制点来调整轨迹形状。在数学建模中考虑到时间和复杂度采用路径点作为优化变量是一种务实的选择。我们可以通过在目标函数中增加一项来惩罚相邻路径点间的距离差相当于鼓励直线或是在后处理中进行平滑。2.3 将连续问题离散化如何让计算机求解我们的约束条件是对所有连续时间t成立的这无法直接求解。必须进行时间离散化。我们将总时间T划分为K个时间步t0, t1, ..., tK。然后我们只要求约束在每一个离散的时间点t_k上成立。这样无穷多个约束就变成了有限个K * (障碍物数 无人机间约束对)。优化变量也从连续函数变成了所有无人机在所有时间步上的位置集合{P_i(t_k)}。这是一个非常关键的步骤。离散化的粒度时间步长Δt需要权衡步长太大可能无法捕捉到碰撞发生的瞬间步长太小优化变量激增求解会非常慢。通常需要根据无人机的最大速度和安全距离来经验性地选择。至此我们得到了一个大规模的、带有非线性约束距离约束的数学优化问题。接下来就是如何求解它。3. 核心算法思路从集中式优化到分布式协调对于离散化后的问题直接调用标准优化器如IPOPT、SNOPT可能因为问题规模和非凸性而失败。因此我们需要设计更巧妙的算法思路。下面介绍几种在研究和竞赛中常用的方法。3.1 集中式规划基于优化的方法集中式方法假设一个中央处理器拥有全局信息为所有无人机一次性规划出所有轨迹。序列化规划这是最简单粗暴但有效的策略。先为第一架无人机规划一条无视其他无人机的避障路径。然后在规划第二架无人机时将第一架无人机的轨迹视为动态障碍物。以此类推。这种方法实现简单但效果严重依赖于规划顺序可能导致后规划的无人机路径非常冗长甚至无解。联合优化将所有无人机的路径点变量放在一起构建一个统一的优化问题。目标函数可能是所有无人机路径长度之和约束包括每架无人机自身的起终点、障碍物约束以及所有无人机两两之间的防撞约束。这个问题非常庞大且非凸。可以采用序列凸规划SCP或交替方向乘子法ADMM来求解。SCP的思路是在当前轨迹估计值附近将非凸的距离约束线性化或二阶锥近似从而将原问题转化为一系列凸优化子问题迭代求解直至收敛。这是目前处理这类问题比较前沿和有效的方法。注意在数学建模论文中如果你采用联合优化SCP的思路你需要清晰地阐述线性化过程。例如非凸约束||P_i - P_j|| ≥ d可以在点(P_i0, P_j0)处线性化为(P_i0 - P_j0)^T (P_i - P_j) / ||P_i0 - P_j0|| ≥ d。这需要一定的数学推导能力。3.2 分布式/分散式规划基于反应式规则的方法集中式方法计算负担重且需要全局通信。分布式方法则让每架无人机基于局部信息如自身状态、邻近无人机状态、局部障碍物信息自主决策。人工势场法APF这是一个经典方法。目标点产生引力障碍物和其他无人机产生斥力无人机的运动方向由合力决定。方法简单计算快但容易陷入局部最优比如在狭窄通道口振荡且可能产生“死锁”两机迎面相遇斥力使它们僵持。在数学建模中你可以用APF作为基线方法然后分析其缺陷并提出改进比如增加 tangential force 来逃离局部最优或者引入速度障碍法来更好地解决死锁。速度障碍法VO及其变种RVO, ORCA这是更鲁棒的分布式避障方法。其核心思想是每架无人机根据他机的当前位置和速度计算出在速度空间中会导致未来某一时间内碰撞的“速度障碍区”然后选择一个不属于该区域的速度通常是最接近其期望速度的速度。最优互惠避障ORCA算法能保证为每架无人机求解出一个线性规划问题从而高效地生成无碰撞速度。在建模中你可以将VO/ORCA与全局路径规划结合先用A*等算法规划一条粗略的全局路径然后无人机沿着路径飞行时用VO/ORCA进行局部实时避障。3.3 基于智能搜索的路径规划这类方法不直接求解优化问题而是在构型空间C-space中搜索一条无碰撞路径。A及其变种D, LPA*在离散的网格地图中非常有效。你需要将三维空间体素化把包含障碍物的体素标记为不可通行。A通过启发式函数如到终点的欧氏距离引导搜索能找到最短路径。但对于多机协同简单的A无法直接处理动态的机间避碰约束。一个方案是为多机系统构建一个联合状态空间**所有无人机位置的笛卡尔积然后在这个高维空间中进行搜索。但空间维度随无人机数量指数增长仅适用于2-3架无人机的小规模场景。快速随机探索树RRT适用于连续空间。通过随机采样和向树中扩展节点来探索空间。RRT* 是其渐进最优版本。对于多机协同同样可以构建联合状态空间的RRT树。为了提高效率可以采用一些策略如先为每架无人机单独规划一条粗略路径可能碰撞然后在这些路径的“隧道”内进行局部采样和优化。在实际建模中混合方法往往更受青睐。例如使用A*或RRT为每架无人机生成一条初始的、忽略其他无人机的无碰撞路径。然后将这些路径作为初始解送入一个基于优化的轨迹优化器如使用SCP框架同时优化平滑性、动力学可行性和无人机间的防撞。这样既利用了搜索算法的全局探索能力又发挥了优化方法在局部精细化方面的优势。4. 求解策略与编程实现要点有了模型和算法思路下一步就是将其转化为可运行的代码。这里有几个关键的实战要点。4.1 编程语言与工具选择Python是数学建模的绝对主流。其生态丰富NumPy/SciPy用于科学计算Matplotlib用于可视化CVXOPT、CasADi或Pyomo用于建模和求解优化问题。对于A*、RRT等搜索算法Python实现起来也很方便。MATLAB同样强大特别是在优化工具箱和可视化方面上手快。但处理复杂逻辑和大型项目时Python的灵活性更胜一筹。关键工具推荐CasADi这是一个用于非线性优化和最优控制的强大框架。它支持符号计算可以自动微分并方便地连接到IPOPT等求解器。如果你想实现SCP或直接转录法求解轨迹优化问题CasADi是一个极佳的选择。OpenAI Gym / Pybullet如果你想进行更逼真的动力学仿真可以借助这些机器人仿真环境。但这通常超出了纯数学建模的要求除非你的模型特别强调动力学。4.2 一个具体的参考实现框架假设我们采用“分段线性路径点 联合优化 软约束处理”的简化框架。以下是实现步骤数据输入与预处理读取起点、终点、障碍物坐标和半径。确定无人机数量N、安全距离d_safe、最大速度v_max、规划时间步数K。初始化轨迹为每架无人机生成一条初始轨迹。最简单的是直线连接起点和终点。如果直线穿障则使用A*搜索一条粗略的无碰撞路径并将其离散化为K个路径点。构建优化问题变量一个(N * K * 3)维的向量代表所有无人机所有时间点的三维坐标。目标函数最小化总路径长度相邻时间点位置差的和。硬约束起点和终点的等式约束。软约束惩罚项将障碍物避碰和无人机防撞约束作为惩罚项加入目标函数而不是严格的等式/不等式约束。这是因为严格的非凸约束会使问题很难求解。例如对于每个约束distance d我们添加一个惩罚项weight * max(0, d - distance)^2。这样当发生“碰撞”时目标函数值会增大优化器会试图调整轨迹来减少惩罚。通过逐渐增大惩罚权重障碍罚函数法可以逼近一个满足约束的解。调用优化求解器使用scipy.optimize.minimize函数选择方法如SLSQP或trust-constr来求解这个带约束的非线性优化问题。将软约束作为目标函数的一部分。后处理与可视化得到优化后的路径点后可以用样条插值使其平滑。最后用matplotlib绘制三维轨迹动画直观展示协同避障效果。# 以下是一个高度简化的代码框架示意核心思路 import numpy as np from scipy.optimize import minimize import matplotlib.pyplot as plt def objective(x, starts, goals, obstacles, N, K, weight_obs, weight_uav): 目标函数路径长度 碰撞惩罚 x: 优化变量形状为 (N*K*3, )已被拉平 # 1. 将x重构成 (N, K, 3) 的数组 traj x.reshape(N, K, 3) length_cost 0 for i in range(N): for k in range(K-1): length_cost np.linalg.norm(traj[i, k1] - traj[i, k]) # 2. 障碍物碰撞惩罚 obs_penalty 0 for obs in obstacles: obs_center, obs_radius obs for i in range(N): for k in range(K): dist np.linalg.norm(traj[i, k] - obs_center) if dist obs_radius: obs_penalty weight_obs * (obs_radius - dist)**2 # 3. 无人机间碰撞惩罚 uav_penalty 0 safe_dist 2.0 # 示例安全距离 for i in range(N): for j in range(i1, N): for k in range(K): dist np.linalg.norm(traj[i, k] - traj[j, k]) if dist safe_dist: uav_penalty weight_uav * (safe_dist - dist)**2 return length_cost obs_penalty uav_penalty def constraint_start_end(x, starts, goals, N, K): 起点终点约束 traj x.reshape(N, K, 3) constraints [] for i in range(N): # 起点约束traj[i, 0] - starts[i] 0 constraints.extend(list(traj[i, 0] - starts[i])) # 终点约束traj[i, -1] - goals[i] 0 constraints.extend(list(traj[i, -1] - goals[i])) return np.array(constraints) # 主程序流程 N 2 # 2架无人机 K 20 # 20个时间点 starts np.array([[0,0,0], [5,0,0]]) goals np.array([[10,10,5], [0,10,5]]) obstacles [ (np.array([5,5,2]), 1.5) ] # (中心半径) # 初始化轨迹直线 initial_guess [] for i in range(N): lin_traj np.linspace(starts[i], goals[i], K) initial_guess.append(lin_traj) initial_guess np.array(initial_guess).flatten() # 优化 cons {type: eq, fun: constraint_start_end, args: (starts, goals, N, K)} result minimize(objective, initial_guess, args(starts, goals, obstacles, N, K, 100.0, 100.0), constraintscons, methodSLSQP, options{maxiter: 500, disp: True}) optimized_traj result.x.reshape(N, K, 3) # ... 后续可视化 ...重要提示上述代码框架非常初级仅用于演示思想。实际应用中直接使用软约束全局优化可能因变量多、非凸性强而难以收敛到好解。更稳健的做法是结合前面提到的SCP或分布式方法。4.3 可视化让结果自己说话在数学建模论文中出色的可视化能极大提升表现力。静态三维图使用matplotlib的Axes3D绘制三维空间用散点图或曲面表示障碍物用不同颜色的线条表示各无人机的轨迹。在关键点如近距离交汇处可以标注出距离。动态动画使用matplotlib.animation.FuncAnimation制作轨迹跟踪动画可以清晰展示无人机如何随时间运动并相互避让。动画可以保存为GIF或视频嵌入论文。性能指标图绘制每架无人机与最近障碍物/其他无人机的距离随时间变化的曲线确保其始终高于安全阈值。绘制速度、加速度曲线以验证动力学可行性。5. 论文写作与模型评估如何脱颖而出完成了模型和求解最后一步是将你的工作清晰、有说服力地呈现出来。5.1 模型评估与灵敏度分析一个完整的模型必须经过评估。你需要设计实验来回答有效性你的算法能在各种给定场景下如不同障碍物布局、不同无人机数量规划出无碰撞路径吗展示2-3个有代表性的复杂场景。最优性你的解质量如何与基准方法如序列化规划、简单APF对比总路径长度、总飞行时间等指标。鲁棒性如果无人机的最大速度、安全距离等参数发生变化你的模型表现如何进行灵敏度分析。例如逐渐减小安全距离d_safe观察是否还能找到解或者解的质量如何变化。这能体现模型对参数变化的稳定程度。计算效率记录求解时间。分析无人机数量N、时间步K与求解时间的关系。这对于评估算法可扩展性至关重要。5.2 论文写作的核心章节与技巧你的论文应该讲一个好故事问题重述与分析不要照抄题目要用自己的话精炼概括问题核心并明确列出你的合理假设无人机模型、协同定义、环境假设等。模型建立这是重中之重。清晰地定义所有符号分小节阐述环境模型、无人机模型、约束条件、目标函数、离散化方法。公式要编号并辅以必要的文字说明。算法设计详细说明你采用的求解策略。如果是混合方法说明各模块如何衔接。画出算法流程图能让逻辑更清晰。求解过程介绍编程环境、使用的工具包、关键参数设置。可以简要描述一下调试过程中遇到的主要问题及解决方法如优化不收敛时如何调整初始值或惩罚权重。结果分析与可视化用图表说话。除了展示漂亮的轨迹动画还要有定量分析表格和曲线图。对结果进行讨论为什么在这个地方轨迹出现了绕行为什么这两架无人机的避让方式是这样的模型评价与推广客观评价自己模型的优点如考虑了协同避碰、求解相对高效和缺点如未考虑动力学细节、对初值敏感等。并提出可能的改进方向如引入更精确的动力学模型、尝试分布式算法提高实时性等。5.3 常见“坑”与应对策略优化问题不收敛这是最常见的问题。可能原因初始轨迹太差如直接穿障、惩罚权重设置不当太小则约束无效太大会导致数值病态。策略先用A*等搜索算法生成一个可行的初始路径采用障碍罚函数法从一个较小的权重开始逐步增加用上一次优化的结果作为下一次的初值。计算时间过长当无人机数量或时间步增多时变量维度过高。策略考虑降维比如减少时间步数或在优化后期再加密时间步尝试分布式算法将大问题分解如果使用集中式优化检查是否能利用问题的稀疏结构。死锁Deadlock在多机对向飞行等场景中可能陷入僵局。策略在反应式算法如APF中引入随机扰动或“让路规则”在优化框架中可以尝试引入轻微的优先级或者修改目标函数鼓励其中一架无人机做出稍大的绕行。轨迹不光滑使用路径点模型容易产生折线。策略在目标函数中加入平滑项惩罚加速度或加加速度或者在得到路径点后用B样条或多项式进行平滑插值并重新检查碰撞。解决这类问题没有银弹需要根据具体场景在最优性、计算复杂度和实现难度之间做出权衡。数学建模竞赛中一个逻辑清晰、实现完整、分析深入的“实用”模型往往比一个追求极致性能但漏洞百出的“复杂”模型更能获得好评。关键在于展示你系统化解决问题的能力从问题定义、模型构建、算法设计、求解实现到分析评估的完整链条。
