1. 从“单打独斗”到“协同作战”无人机集群航迹规划的核心挑战去年带队参加深圳杯数学建模竞赛选的正是C题“无人机协同避障航迹规划”。这个题目乍一看是经典的路径规划问题但深入下去你会发现它真正考验的是如何让一群无人机像一支训练有素的队伍在复杂的三维空间里既能高效抵达各自目标又能避免互相碰撞、避开障碍物还得考虑飞行时间和能耗。这和我们平时玩的单机航拍无人机或者网上那些“手搓”一架F450、调通APM飞控的乐趣完全是两个维度的挑战。单机飞行你只需要关心它自己别撞墙而集群协同你要操心的是整个团队的队形、通信、决策和容错。为什么这个问题如此重要又如此棘手想象一下未来城市物流的场景几十架配送无人机在楼宇间穿梭如果每架都只规划自己的最优路径很可能会在某个十字路口“堵车”甚至“撞机”。再比如大面积农田的植保作业多架无人机协同喷洒如何保证覆盖无死角且不重复作业同时避免相互干扰这些实际应用都指向了“协同避障航迹规划”这个核心技术。它不仅仅是算法问题更是系统工程涉及飞控如STM32、Pixhawk、通信如数传电台、可能的5G或自组网、感知如视觉避障、激光雷达和决策路径规划算法的深度融合。网络上相关的热词如“无人机路径规划算法”、“无人机编队”、“无人机群组通信”都从不同侧面描绘了这幅技术拼图。而“无人机视觉感知”、“无人机目标检测”则是实现实时避障的关键感官。我们当时的解题过程实际上就是尝试用数学模型和算法来模拟并优化这个复杂的多智能体系统。本文将结合我们的实战经验抛开论文的学术外壳聊聊如何从工程和算法角度一步步拆解并实现无人机的协同避障航迹规划其中会涉及模型建立、算法选型、仿真验证以及那些容易踩坑的细节。2. 问题拆解把协同规划分解为可计算的模块面对“协同避障航迹规划”这样一个宏大命题直接上手建模很容易无从下手。我们的策略是将其分解为几个相对独立又相互关联的子问题就像搭积木一样先处理好每一块再考虑如何拼装。### 2.1 环境建模把真实世界变成算法能看懂的地图这是所有规划的基础。竞赛题目通常会给出障碍物的位置、形状可能是圆柱体、长方体等以及无人机的起点、终点坐标。我们的第一步就是建立三维空间的环境模型。离散化与网格法最直观的方法是将三维空间划分为均匀的立方体网格Voxel Grid。每个网格单元标记为“自由”、“障碍”或“无人机占用”。这种方法简单便于计算但精度和计算量是一对矛盾。网格划分太粗可能无法精确描述障碍物边界导致规划路径过于保守或发生碰撞划分太细计算复杂度会呈立方级增长特别是对于大规模空间。在仿真中比如使用“无人机仿真”环境或自己编写我们需要根据障碍物的最小尺寸和无人机自身尺寸安全半径来权衡网格分辨率。几何模型法如果障碍物是规则几何体如题目中常见的圆柱我们可以直接用其数学方程如圆柱体的方程来描述。在路径搜索或优化时通过计算无人机位置到这些几何体表面的距离来判断是否碰撞。这种方法精度高内存占用小但碰撞检测计算相对复杂且对于不规则障碍物描述困难。势场法建模这是一种在规划中常用的间接环境表示。将目标点设为“引力场”障碍物设为“斥力场”无人机在合力作用下运动。这种方法在建模阶段就融合了规划思想适合作为后续优化算法的初始解生成器。注意在编程实现时务必建立统一的世界坐标系并将所有无人机、障碍物的坐标转换到该坐标系下。这是后续所有计算不出错的前提。一个常见的坑是忽略了坐标系的转换导致看似正确的算法在实际空间中产生完全错误的路径。### 2.2 无人机建模它不是质点是有体积和动力学的智能体很多初学者会把无人机简化为一个质点这在不考虑姿态和动力学约束的初步规划中或许可行但要实现可靠的协同避障必须考虑更多因素。动力学约束无人机不是幽灵不能瞬间移动或直角转弯。它有最大速度、最大加速度、最大角速度等限制。在规划时生成的路径必须是动力学可行的。例如路径的曲率不能超过无人机的最小转弯半径。这通常需要在路径平滑Path Smoothing阶段引入或者直接使用符合动力学模型的方法如状态栅格进行规划。几何尺寸与安全包络无人机本身有物理尺寸。在碰撞检测时我们不能只检测其中心点而应该用一个包围球或包围盒安全包络来代表它。这个包络的半径通常大于无人机实际半径以留出安全余量。协同避障时检查的是无人机A的安全包络与无人机B的安全包络、以及障碍物膨胀后的区域是否相交。通信与感知模型协同的前提是彼此“知道”对方的状态。我们需要假设一个通信模型例如每架无人机可以周期性如每秒10次广播自己的位置、速度、意图给一定范围内的邻居无人机。同时每架无人机也可能搭载传感器模拟“无人机视觉感知”来探测未知障碍物。在模型简化时我们可以假设全局信息已知集中式规划或仅邻居信息已知分布式规划。### 2.3 优化目标定义什么才是“好”的航迹规划不仅要“能走通”还要“走得好”。我们需要用数学语言定义什么是“好”。首要目标无碰撞。这是硬约束必须满足。包括无人机与静态障碍物的碰撞UAV_Obstacle_Collision 0以及无人机之间的碰撞UAV_UAV_Collision 0。核心优化目标总飞行时间最短对于物流等场景时间就是效率。目标函数可以是所有无人机从起点到终点时间的总和最小化或最晚到达无人机的到达时间makespan最小化。总路径长度最短与时间相关但在匀速假设下等价。减少路径长度能直接节省能耗。能耗最低更复杂的模型会考虑飞行能耗与速度、加速度的关系。通常可以近似为与路径长度和机动动作如加减速、转弯正相关。协同相关目标队形保持对于编队飞行如“无人机编队”表演需要最小化无人机与其期望队形位置的偏差。通信连通性确保无人机集群在飞行过程中始终保持必要的通信链路避免个体失联。平滑性路径应尽可能平滑减少急剧的转向和速度变化以提高飞行稳定性和乘客/货物的舒适度如果载人载货。在我们的模型中最终构建了一个多目标优化函数以总路径长度为主要优化目标将无碰撞作为约束条件同时将最大飞行时间作为次要优化目标或约束进行处理。3. 核心武器库路径规划算法选型与实战适配算法是解决规划问题的灵魂。没有一种算法是万能的我们需要根据问题特点进行选择和组合。### 3.1 全局规划器为每架无人机找一条“骨架”路径在已知全局地图的情况下首先需要为每架无人机规划一条从起点到终点的初始路径。A算法及其变种*这是最经典的图搜索算法。在三维网格地图上表现良好。其关键是启发函数h(n)的设计好的启发函数能大幅提高搜索效率。对于三维空间欧几里得距离是最常用的启发函数。A* 保证找到最短路径但在高维精细网格中计算量很大。变种算法如Weighted A*通过牺牲最优性来换取速度。快速随机探索树RRT系列特别适合高维空间和复杂约束下的路径规划。RRT通过随机采样快速探索空间能有效找到可行路径但不保证最优。RRT*是其渐进最优版本通过后续迭代优化路径。对于无人机集群可以为每架无人机独立运行RRT但这样完全忽略了协同容易导致路径冲突。协同A(CA) 与 冲突搜索CBS**这是解决多智能体路径规划MAPF的经典算法框架特别适合我们这个问题。CA* 的基本思想是为所有无人机同时搜索路径在搜索树的每个节点都包含所有无人机的联合状态。当扩展节点发现冲突两架无人机同时占据同一位置时就为该节点生成约束例如禁止无人机A在时刻t进入格子G并分别继续搜索。这种方法能保证找到最优解但联合状态空间随无人机数量指数增长计算成本极高通常只适用于无人机数量较少10的场景。CBS是一种更高效的两层算法。高层搜索Conflict Tree负责解决冲突低层搜索例如为单个无人机运行A*负责为每个无人机规划满足当前约束集的路径。当低层规划出的路径有冲突时高层就为冲突的双方分别添加新的约束如无人机1在时间t不能位于位置x或无人机2在时间t不能位于位置x并重新进行低层规划。CBS通过约束的传递来协调多机路径比CA* 更灵活、更高效是解决此类问题的首选算法之一。### 3.2 局部避障与协同调整让路径“活”起来全局规划出的路径是静态的但真实环境可能变化无人机也可能存在控制误差。因此需要局部实时避障和协同调整。人工势场法APF如前所述将目标和障碍物转化为势场。在协同场景中每架无人机不仅受到静态障碍物和目标点的势场力还会受到其他无人机产生的斥力。这种方法计算简单反应快适合实时微调。但其致命缺点是容易陷入局部最优比如在两个障碍物之间震荡或“卡”在狭窄通道口。通常不单独使用而是作为全局路径的跟踪和局部修正器。速度障碍法VO与最优互惠避碰ORCA这是更高级、更数学化的实时协同避障方法。VO的核心思想是在速度空间中计算出会导致未来某一时间窗内发生碰撞的所有速度集合然后选择不在这个集合内的速度。ORCA在VO的基础上通过线性优化为每架无人机分配一个互不冲突的“安全速度”并且这个分配过程是互惠Reciprocal的即双方共同承担责任来避让避免了传统VO中一方主动避让导致的抖动和不自然运动。ORCA算法在密集动态环境下表现非常出色是当前多机器人协同避障的研究热点。基于优化的方法将整个集群一段时间内的轨迹表示为一组参数化的曲线如B样条将动力学约束、避障约束转化为不等式、协同目标如间距都转化为优化问题的约束和目标函数然后使用非线性优化求解器如IPOPT、SNOPT进行求解。这种方法能直接生成平滑、动力学可行的轨迹质量很高但计算量大对初值敏感且可能陷入局部最优。常与全局规划器结合用全局路径提供优化初值。### 3.3 我们的实战策略分层混合架构在竞赛有限的时间和计算资源下我们采用了一种分层混合的架构这也是工程上常见的思路顶层任务分配与粗规划首先根据无人机的起点和终点使用改进的CBS算法进行全局路径规划。为了降低计算复杂度我们对环境进行了适度粗粒度的网格划分并采用了“优先规划”策略为预计路径冲突可能性大的无人机优先规划并将其路径作为障碍物固定下来再为后续无人机规划。这虽然不是最优但在有限时间内能得到一个可行的无冲突初始方案。中层轨迹优化将CBS输出的离散路径点用B样条曲线进行连接和平滑使其符合无人机的最小转弯半径和最大曲率约束。同时在这个阶段引入时间轴为每个路径点分配时间戳确保在相同时间点无人机间留有足够的安全距离。底层局部反应在仿真执行时为每架无人机配置一个基于ORCA思想的局部避障模块。这个模块会实时根据邻居无人机和附近突发障碍物如果题目有设定的预测轨迹微调当前的速度指令以应对模型误差和未预见的干扰。这套架构的核心思想是全局保证可行性局部保证鲁棒性。4. 仿真验证从数学公式到看得见的飞行算法设计得再精妙不经仿真验证都是纸上谈兵。我们使用Python作为主要工具结合一些强大的库搭建了完整的仿真验证环境。### 4.1 仿真环境搭建我们没有使用复杂的“无人机仿真”平台如Gazebo虽然它们更真实而是选择了轻量级的自定义仿真以便快速迭代算法。核心库NumPy用于数值计算Matplotlib用于二维/三维可视化。对于更复杂的三维动态展示可以引入Plotly或PyGame。环境绘制在三维坐标系中用不同颜色的圆柱体或立方体表示障碍物用线条或带箭头的轨迹表示无人机路径用动态移动的点或小模型表示无人机实时位置。动力学仿真我们采用了简化的质点动力学模型。无人机状态包括位置(x, y, z)和速度(vx, vy, vz)。控制输入为加速度(ax, ay, az)。通过积分dt时间更新状态新速度 旧速度 加速度 * dt新位置 旧位置 新速度 * dt。同时限制速度和加速度在最大值范围内。### 4.2 关键性能指标KPI可视化仿真不仅要看无人机是否“飞过去”还要定量评估飞得“好不好”。路径长度与时间曲线为每架无人机绘制其飞行路径长度随时间变化的曲线可以直观看出是否同时到达、是否有等待。最小间隔距离监控实时计算并绘制任意两架无人机之间的欧氏距离。设置一条安全阈值线如2倍无人机半径确保整个飞行过程中所有间距都大于阈值。这是验证避障有效性的最直接证据。速度与加速度曲线绘制每架无人机的速度和加速度分量随时间的变化。检查是否超出最大限制加速度曲线是否平滑突变的加速度意味着不舒适的急刹或猛冲。能量消耗估算根据一个简化的模型例如能耗与速度平方和加速度的绝对值成正比估算总能耗并进行对比。### 4.3 典型场景测试与结果分析我们设计了几个典型场景来测试算法的鲁棒性对称交叉场景四架无人机从正方形的四个角出发飞向对角。这是检验冲突解决能力的经典场景。我们的CBS层成功规划出了“错峰通行”的路径一架无人机稍作盘旋等待其他三架依次通过中心区域避免了死锁。狭窄通道场景所有无人机需要通过一个比无人机安全包络宽不了多少的通道。这考验路径规划的精细度和协同性。算法成功规划出了“排队通过”的序列并通过轨迹优化确保了在通道内的精确控制。动态干扰场景在飞行途中随机加入一个小的移动障碍物。底层的ORCA局部避障模块成功生效使附近的无人机做出了平滑的避让动作之后又回归到原定路径附近。踩坑实录在最初实现CBS时我们忽略了“等待”也是一种有效的冲突解决策略。我们只考虑了空间上的避让你走左边我走右边导致在密集场景中搜索树爆炸无法在时限内得到解。后来引入了“时间轴”和“等待动作”即在某个节点停留一个时间步大大提升了算法的求解能力和效率。这个“等待”动作在数学上等价于在路径中插入一个与上一时刻相同的位置点。5. 算法优化与工程调参在理想与现实间权衡数学建模和算法实现给出了骨架而让整个系统真正高效、稳定运行离不开大量的优化和“调参”。这部分工作充满了工程经验的味道。### 5.1 搜索效率优化让算法跑得更快CBS或A* 在高分辨率网格上慢是主要瓶颈。启发函数Heuristic的魔力对于A*一个接近真实代价的启发函数能极大减少搜索节点。除了欧氏距离我们可以考虑障碍物的信息。例如计算当前点到终点的直线距离如果连线穿过障碍物则给一个惩罚值加到启发函数中引导搜索尽早绕开障碍物密集区。跳跃点搜索JPS在均匀网格中JPS可以跳过大量不必要的中间节点直接“跳”到关键转折点能将搜索速度提升一个数量级。我们尝试将JPS思想应用到三维网格取得了不错的效果。分层规划先在一个非常粗糙的网格上规划一条大致的“走廊”然后再在这个走廊限定的精细网格内进行详细规划。这相当于缩小了搜索范围。剪枝与约束传播在CBS的高层搜索中如果某个节点的代价如路径总长已经超过当前找到的最好解则直接剪掉该分支。同时当为一个无人机添加了新的约束如“不能在t时刻位于(x,y,z)”可以尝试推导出一些派生约束如“在t-1或t1时刻也应避免该区域附近”提前避免冲突。### 5.2 轨迹平滑与动力学可行性从网格中心点连成的折线路径无人机是无法直接跟踪的。B样条曲线拟合我们采用三次B样条来平滑路径。B样条局部可控、光滑连续的特性非常适合轨迹表示。将规划出的路径点作为B样条的控制点通过调整控制点可以确保曲线不穿过障碍物满足凸包性质。同时计算曲线的曲率确保其小于无人机最大曲率限制。时间分配与匀速化为平滑后的曲线分配时间。一种简单的方法是假设无人机以恒定速度飞行根据曲线段长度分配时间。更高级的方法是根据曲率动态分配时间在直线段可以加速在弯道处减速以保证向心加速度不超过限制。我们采用了后者使得生成的轨迹不仅空间上平滑时间上也符合动力学。微分平坦性对于多旋翼无人机这类系统其动力学模型具有微分平坦特性。这意味着我们可以将复杂的全状态轨迹规划简化为对少数几个平坦输出通常是位置和偏航角及其导数的规划。这大大降低了轨迹优化的维度。我们最终的轨迹优化模块就基于此思想确保了生成的位置、速度、加速度甚至加加速度Jerk都是连续的飞行非常平稳。### 5.3 参数调优没有银弹只有权衡整个系统充满了需要调节的参数它们相互影响需要反复试验。安全距离这是最重要的参数之一。设置得太大会导致规划无解或路径非常保守设置得太小则容易发生碰撞。它应该略大于无人机物理半径加上定位和控制误差的估计值。我们将其设为物理半径的1.5倍。时间步长dt在离散时间规划中dt决定了时间分辨率。dt小规划精细冲突检测准但搜索空间大dt大则相反。我们需要在精度和计算量之间折衷。通常dt应小于无人机穿过自身尺寸所需的时间。规划频率与重规划周期全局规划器多久运行一次局部避障器多久计算一次新的速度指令全局规划耗时长频率低如1Hz局部避障快频率高如10Hz。当局部避障长时间无法解决冲突如陷入局部最优时应触发全局重规划。优化算法的权重在构建多目标优化函数时各项路径长度、时间、平滑度的权重需要仔细调整。我们的经验是初期以保证无碰撞为首要目标权重极高在确保安全的基础上再逐步调整其他权重大小以得到更优的综合性能。6. 从模型到现实的鸿沟竞赛未涉及但至关重要的环节竞赛模型是高度简化的而真实的无人机协同系统要复杂得多。在论文的最后部分我们探讨了模型与现实的差距这也是未来工作的方向。通信延迟与丢包我们的模型假设通信是即时、完美的。现实中无线通信如Wi-Fi、数传存在延迟、抖动和丢包。这可能导致无人机基于过时信息做出决策引发碰撞。解决方案包括在状态估计中引入预测、使用具有容错能力的协同算法如一致性算法、甚至采用部分去中心化的架构减少对连续通信的依赖。定位误差无人机通过GPS、视觉里程计或UWB进行定位都存在厘米级甚至分米级的误差。我们的安全距离必须包容这个误差。更先进的做法是进行不确定性感知规划Uncertainty-aware Planning在规划时直接考虑状态估计的不确定性椭球。外部扰动风是无人机飞行最大的外部扰动。强风可能使无人机偏离计划轨迹。这需要在控制层设计鲁棒控制器如滑模控制、自适应控制或者在规划层就生成抗扰动的轨迹如增加冗余空间。异构无人机集群竞赛中假设所有无人机性能相同。现实中集群可能由不同型号、不同载荷、不同能力的无人机组成。规划时需要考虑到异质性例如让机动性强的无人机执行更复杂的避让动作。人机交互与伦理安全特别是在城市空域无人机需要被“无人机Remote ID”系统追踪需要遵守空域规则需要具备紧急情况下的“无人机反制”或失效安全机制如自动降落。这些都属于更高层面的系统设计问题。完成这个项目最大的体会是协同规划是一个典型的“系统级”问题。任何一个单一的算法都无法解决所有问题需要将感知、通信、规划、控制等多个模块有机整合并在算法的理想性与工程的实时性、鲁棒性之间做出精巧的权衡。从A*、RRT到CBS、ORCA从势场法到优化理论工具很多但更重要的是理解问题本质并选择或组合出最适合当前约束计算资源、时间、精度要求的方案。这个过程远比调通一个“无人机飞控”或跑通一个“目标检测”模型要复杂但也正是其挑战和魅力所在。 无人机集群协同避障航迹规划：从算法原理到工程实践
1. 从“单打独斗”到“协同作战”无人机集群航迹规划的核心挑战去年带队参加深圳杯数学建模竞赛选的正是C题“无人机协同避障航迹规划”。这个题目乍一看是经典的路径规划问题但深入下去你会发现它真正考验的是如何让一群无人机像一支训练有素的队伍在复杂的三维空间里既能高效抵达各自目标又能避免互相碰撞、避开障碍物还得考虑飞行时间和能耗。这和我们平时玩的单机航拍无人机或者网上那些“手搓”一架F450、调通APM飞控的乐趣完全是两个维度的挑战。单机飞行你只需要关心它自己别撞墙而集群协同你要操心的是整个团队的队形、通信、决策和容错。为什么这个问题如此重要又如此棘手想象一下未来城市物流的场景几十架配送无人机在楼宇间穿梭如果每架都只规划自己的最优路径很可能会在某个十字路口“堵车”甚至“撞机”。再比如大面积农田的植保作业多架无人机协同喷洒如何保证覆盖无死角且不重复作业同时避免相互干扰这些实际应用都指向了“协同避障航迹规划”这个核心技术。它不仅仅是算法问题更是系统工程涉及飞控如STM32、Pixhawk、通信如数传电台、可能的5G或自组网、感知如视觉避障、激光雷达和决策路径规划算法的深度融合。网络上相关的热词如“无人机路径规划算法”、“无人机编队”、“无人机群组通信”都从不同侧面描绘了这幅技术拼图。而“无人机视觉感知”、“无人机目标检测”则是实现实时避障的关键感官。我们当时的解题过程实际上就是尝试用数学模型和算法来模拟并优化这个复杂的多智能体系统。本文将结合我们的实战经验抛开论文的学术外壳聊聊如何从工程和算法角度一步步拆解并实现无人机的协同避障航迹规划其中会涉及模型建立、算法选型、仿真验证以及那些容易踩坑的细节。2. 问题拆解把协同规划分解为可计算的模块面对“协同避障航迹规划”这样一个宏大命题直接上手建模很容易无从下手。我们的策略是将其分解为几个相对独立又相互关联的子问题就像搭积木一样先处理好每一块再考虑如何拼装。### 2.1 环境建模把真实世界变成算法能看懂的地图这是所有规划的基础。竞赛题目通常会给出障碍物的位置、形状可能是圆柱体、长方体等以及无人机的起点、终点坐标。我们的第一步就是建立三维空间的环境模型。离散化与网格法最直观的方法是将三维空间划分为均匀的立方体网格Voxel Grid。每个网格单元标记为“自由”、“障碍”或“无人机占用”。这种方法简单便于计算但精度和计算量是一对矛盾。网格划分太粗可能无法精确描述障碍物边界导致规划路径过于保守或发生碰撞划分太细计算复杂度会呈立方级增长特别是对于大规模空间。在仿真中比如使用“无人机仿真”环境或自己编写我们需要根据障碍物的最小尺寸和无人机自身尺寸安全半径来权衡网格分辨率。几何模型法如果障碍物是规则几何体如题目中常见的圆柱我们可以直接用其数学方程如圆柱体的方程来描述。在路径搜索或优化时通过计算无人机位置到这些几何体表面的距离来判断是否碰撞。这种方法精度高内存占用小但碰撞检测计算相对复杂且对于不规则障碍物描述困难。势场法建模这是一种在规划中常用的间接环境表示。将目标点设为“引力场”障碍物设为“斥力场”无人机在合力作用下运动。这种方法在建模阶段就融合了规划思想适合作为后续优化算法的初始解生成器。注意在编程实现时务必建立统一的世界坐标系并将所有无人机、障碍物的坐标转换到该坐标系下。这是后续所有计算不出错的前提。一个常见的坑是忽略了坐标系的转换导致看似正确的算法在实际空间中产生完全错误的路径。### 2.2 无人机建模它不是质点是有体积和动力学的智能体很多初学者会把无人机简化为一个质点这在不考虑姿态和动力学约束的初步规划中或许可行但要实现可靠的协同避障必须考虑更多因素。动力学约束无人机不是幽灵不能瞬间移动或直角转弯。它有最大速度、最大加速度、最大角速度等限制。在规划时生成的路径必须是动力学可行的。例如路径的曲率不能超过无人机的最小转弯半径。这通常需要在路径平滑Path Smoothing阶段引入或者直接使用符合动力学模型的方法如状态栅格进行规划。几何尺寸与安全包络无人机本身有物理尺寸。在碰撞检测时我们不能只检测其中心点而应该用一个包围球或包围盒安全包络来代表它。这个包络的半径通常大于无人机实际半径以留出安全余量。协同避障时检查的是无人机A的安全包络与无人机B的安全包络、以及障碍物膨胀后的区域是否相交。通信与感知模型协同的前提是彼此“知道”对方的状态。我们需要假设一个通信模型例如每架无人机可以周期性如每秒10次广播自己的位置、速度、意图给一定范围内的邻居无人机。同时每架无人机也可能搭载传感器模拟“无人机视觉感知”来探测未知障碍物。在模型简化时我们可以假设全局信息已知集中式规划或仅邻居信息已知分布式规划。### 2.3 优化目标定义什么才是“好”的航迹规划不仅要“能走通”还要“走得好”。我们需要用数学语言定义什么是“好”。首要目标无碰撞。这是硬约束必须满足。包括无人机与静态障碍物的碰撞UAV_Obstacle_Collision 0以及无人机之间的碰撞UAV_UAV_Collision 0。核心优化目标总飞行时间最短对于物流等场景时间就是效率。目标函数可以是所有无人机从起点到终点时间的总和最小化或最晚到达无人机的到达时间makespan最小化。总路径长度最短与时间相关但在匀速假设下等价。减少路径长度能直接节省能耗。能耗最低更复杂的模型会考虑飞行能耗与速度、加速度的关系。通常可以近似为与路径长度和机动动作如加减速、转弯正相关。协同相关目标队形保持对于编队飞行如“无人机编队”表演需要最小化无人机与其期望队形位置的偏差。通信连通性确保无人机集群在飞行过程中始终保持必要的通信链路避免个体失联。平滑性路径应尽可能平滑减少急剧的转向和速度变化以提高飞行稳定性和乘客/货物的舒适度如果载人载货。在我们的模型中最终构建了一个多目标优化函数以总路径长度为主要优化目标将无碰撞作为约束条件同时将最大飞行时间作为次要优化目标或约束进行处理。3. 核心武器库路径规划算法选型与实战适配算法是解决规划问题的灵魂。没有一种算法是万能的我们需要根据问题特点进行选择和组合。### 3.1 全局规划器为每架无人机找一条“骨架”路径在已知全局地图的情况下首先需要为每架无人机规划一条从起点到终点的初始路径。A算法及其变种*这是最经典的图搜索算法。在三维网格地图上表现良好。其关键是启发函数h(n)的设计好的启发函数能大幅提高搜索效率。对于三维空间欧几里得距离是最常用的启发函数。A* 保证找到最短路径但在高维精细网格中计算量很大。变种算法如Weighted A*通过牺牲最优性来换取速度。快速随机探索树RRT系列特别适合高维空间和复杂约束下的路径规划。RRT通过随机采样快速探索空间能有效找到可行路径但不保证最优。RRT*是其渐进最优版本通过后续迭代优化路径。对于无人机集群可以为每架无人机独立运行RRT但这样完全忽略了协同容易导致路径冲突。协同A(CA) 与 冲突搜索CBS**这是解决多智能体路径规划MAPF的经典算法框架特别适合我们这个问题。CA* 的基本思想是为所有无人机同时搜索路径在搜索树的每个节点都包含所有无人机的联合状态。当扩展节点发现冲突两架无人机同时占据同一位置时就为该节点生成约束例如禁止无人机A在时刻t进入格子G并分别继续搜索。这种方法能保证找到最优解但联合状态空间随无人机数量指数增长计算成本极高通常只适用于无人机数量较少10的场景。CBS是一种更高效的两层算法。高层搜索Conflict Tree负责解决冲突低层搜索例如为单个无人机运行A*负责为每个无人机规划满足当前约束集的路径。当低层规划出的路径有冲突时高层就为冲突的双方分别添加新的约束如无人机1在时间t不能位于位置x或无人机2在时间t不能位于位置x并重新进行低层规划。CBS通过约束的传递来协调多机路径比CA* 更灵活、更高效是解决此类问题的首选算法之一。### 3.2 局部避障与协同调整让路径“活”起来全局规划出的路径是静态的但真实环境可能变化无人机也可能存在控制误差。因此需要局部实时避障和协同调整。人工势场法APF如前所述将目标和障碍物转化为势场。在协同场景中每架无人机不仅受到静态障碍物和目标点的势场力还会受到其他无人机产生的斥力。这种方法计算简单反应快适合实时微调。但其致命缺点是容易陷入局部最优比如在两个障碍物之间震荡或“卡”在狭窄通道口。通常不单独使用而是作为全局路径的跟踪和局部修正器。速度障碍法VO与最优互惠避碰ORCA这是更高级、更数学化的实时协同避障方法。VO的核心思想是在速度空间中计算出会导致未来某一时间窗内发生碰撞的所有速度集合然后选择不在这个集合内的速度。ORCA在VO的基础上通过线性优化为每架无人机分配一个互不冲突的“安全速度”并且这个分配过程是互惠Reciprocal的即双方共同承担责任来避让避免了传统VO中一方主动避让导致的抖动和不自然运动。ORCA算法在密集动态环境下表现非常出色是当前多机器人协同避障的研究热点。基于优化的方法将整个集群一段时间内的轨迹表示为一组参数化的曲线如B样条将动力学约束、避障约束转化为不等式、协同目标如间距都转化为优化问题的约束和目标函数然后使用非线性优化求解器如IPOPT、SNOPT进行求解。这种方法能直接生成平滑、动力学可行的轨迹质量很高但计算量大对初值敏感且可能陷入局部最优。常与全局规划器结合用全局路径提供优化初值。### 3.3 我们的实战策略分层混合架构在竞赛有限的时间和计算资源下我们采用了一种分层混合的架构这也是工程上常见的思路顶层任务分配与粗规划首先根据无人机的起点和终点使用改进的CBS算法进行全局路径规划。为了降低计算复杂度我们对环境进行了适度粗粒度的网格划分并采用了“优先规划”策略为预计路径冲突可能性大的无人机优先规划并将其路径作为障碍物固定下来再为后续无人机规划。这虽然不是最优但在有限时间内能得到一个可行的无冲突初始方案。中层轨迹优化将CBS输出的离散路径点用B样条曲线进行连接和平滑使其符合无人机的最小转弯半径和最大曲率约束。同时在这个阶段引入时间轴为每个路径点分配时间戳确保在相同时间点无人机间留有足够的安全距离。底层局部反应在仿真执行时为每架无人机配置一个基于ORCA思想的局部避障模块。这个模块会实时根据邻居无人机和附近突发障碍物如果题目有设定的预测轨迹微调当前的速度指令以应对模型误差和未预见的干扰。这套架构的核心思想是全局保证可行性局部保证鲁棒性。4. 仿真验证从数学公式到看得见的飞行算法设计得再精妙不经仿真验证都是纸上谈兵。我们使用Python作为主要工具结合一些强大的库搭建了完整的仿真验证环境。### 4.1 仿真环境搭建我们没有使用复杂的“无人机仿真”平台如Gazebo虽然它们更真实而是选择了轻量级的自定义仿真以便快速迭代算法。核心库NumPy用于数值计算Matplotlib用于二维/三维可视化。对于更复杂的三维动态展示可以引入Plotly或PyGame。环境绘制在三维坐标系中用不同颜色的圆柱体或立方体表示障碍物用线条或带箭头的轨迹表示无人机路径用动态移动的点或小模型表示无人机实时位置。动力学仿真我们采用了简化的质点动力学模型。无人机状态包括位置(x, y, z)和速度(vx, vy, vz)。控制输入为加速度(ax, ay, az)。通过积分dt时间更新状态新速度 旧速度 加速度 * dt新位置 旧位置 新速度 * dt。同时限制速度和加速度在最大值范围内。### 4.2 关键性能指标KPI可视化仿真不仅要看无人机是否“飞过去”还要定量评估飞得“好不好”。路径长度与时间曲线为每架无人机绘制其飞行路径长度随时间变化的曲线可以直观看出是否同时到达、是否有等待。最小间隔距离监控实时计算并绘制任意两架无人机之间的欧氏距离。设置一条安全阈值线如2倍无人机半径确保整个飞行过程中所有间距都大于阈值。这是验证避障有效性的最直接证据。速度与加速度曲线绘制每架无人机的速度和加速度分量随时间的变化。检查是否超出最大限制加速度曲线是否平滑突变的加速度意味着不舒适的急刹或猛冲。能量消耗估算根据一个简化的模型例如能耗与速度平方和加速度的绝对值成正比估算总能耗并进行对比。### 4.3 典型场景测试与结果分析我们设计了几个典型场景来测试算法的鲁棒性对称交叉场景四架无人机从正方形的四个角出发飞向对角。这是检验冲突解决能力的经典场景。我们的CBS层成功规划出了“错峰通行”的路径一架无人机稍作盘旋等待其他三架依次通过中心区域避免了死锁。狭窄通道场景所有无人机需要通过一个比无人机安全包络宽不了多少的通道。这考验路径规划的精细度和协同性。算法成功规划出了“排队通过”的序列并通过轨迹优化确保了在通道内的精确控制。动态干扰场景在飞行途中随机加入一个小的移动障碍物。底层的ORCA局部避障模块成功生效使附近的无人机做出了平滑的避让动作之后又回归到原定路径附近。踩坑实录在最初实现CBS时我们忽略了“等待”也是一种有效的冲突解决策略。我们只考虑了空间上的避让你走左边我走右边导致在密集场景中搜索树爆炸无法在时限内得到解。后来引入了“时间轴”和“等待动作”即在某个节点停留一个时间步大大提升了算法的求解能力和效率。这个“等待”动作在数学上等价于在路径中插入一个与上一时刻相同的位置点。5. 算法优化与工程调参在理想与现实间权衡数学建模和算法实现给出了骨架而让整个系统真正高效、稳定运行离不开大量的优化和“调参”。这部分工作充满了工程经验的味道。### 5.1 搜索效率优化让算法跑得更快CBS或A* 在高分辨率网格上慢是主要瓶颈。启发函数Heuristic的魔力对于A*一个接近真实代价的启发函数能极大减少搜索节点。除了欧氏距离我们可以考虑障碍物的信息。例如计算当前点到终点的直线距离如果连线穿过障碍物则给一个惩罚值加到启发函数中引导搜索尽早绕开障碍物密集区。跳跃点搜索JPS在均匀网格中JPS可以跳过大量不必要的中间节点直接“跳”到关键转折点能将搜索速度提升一个数量级。我们尝试将JPS思想应用到三维网格取得了不错的效果。分层规划先在一个非常粗糙的网格上规划一条大致的“走廊”然后再在这个走廊限定的精细网格内进行详细规划。这相当于缩小了搜索范围。剪枝与约束传播在CBS的高层搜索中如果某个节点的代价如路径总长已经超过当前找到的最好解则直接剪掉该分支。同时当为一个无人机添加了新的约束如“不能在t时刻位于(x,y,z)”可以尝试推导出一些派生约束如“在t-1或t1时刻也应避免该区域附近”提前避免冲突。### 5.2 轨迹平滑与动力学可行性从网格中心点连成的折线路径无人机是无法直接跟踪的。B样条曲线拟合我们采用三次B样条来平滑路径。B样条局部可控、光滑连续的特性非常适合轨迹表示。将规划出的路径点作为B样条的控制点通过调整控制点可以确保曲线不穿过障碍物满足凸包性质。同时计算曲线的曲率确保其小于无人机最大曲率限制。时间分配与匀速化为平滑后的曲线分配时间。一种简单的方法是假设无人机以恒定速度飞行根据曲线段长度分配时间。更高级的方法是根据曲率动态分配时间在直线段可以加速在弯道处减速以保证向心加速度不超过限制。我们采用了后者使得生成的轨迹不仅空间上平滑时间上也符合动力学。微分平坦性对于多旋翼无人机这类系统其动力学模型具有微分平坦特性。这意味着我们可以将复杂的全状态轨迹规划简化为对少数几个平坦输出通常是位置和偏航角及其导数的规划。这大大降低了轨迹优化的维度。我们最终的轨迹优化模块就基于此思想确保了生成的位置、速度、加速度甚至加加速度Jerk都是连续的飞行非常平稳。### 5.3 参数调优没有银弹只有权衡整个系统充满了需要调节的参数它们相互影响需要反复试验。安全距离这是最重要的参数之一。设置得太大会导致规划无解或路径非常保守设置得太小则容易发生碰撞。它应该略大于无人机物理半径加上定位和控制误差的估计值。我们将其设为物理半径的1.5倍。时间步长dt在离散时间规划中dt决定了时间分辨率。dt小规划精细冲突检测准但搜索空间大dt大则相反。我们需要在精度和计算量之间折衷。通常dt应小于无人机穿过自身尺寸所需的时间。规划频率与重规划周期全局规划器多久运行一次局部避障器多久计算一次新的速度指令全局规划耗时长频率低如1Hz局部避障快频率高如10Hz。当局部避障长时间无法解决冲突如陷入局部最优时应触发全局重规划。优化算法的权重在构建多目标优化函数时各项路径长度、时间、平滑度的权重需要仔细调整。我们的经验是初期以保证无碰撞为首要目标权重极高在确保安全的基础上再逐步调整其他权重大小以得到更优的综合性能。6. 从模型到现实的鸿沟竞赛未涉及但至关重要的环节竞赛模型是高度简化的而真实的无人机协同系统要复杂得多。在论文的最后部分我们探讨了模型与现实的差距这也是未来工作的方向。通信延迟与丢包我们的模型假设通信是即时、完美的。现实中无线通信如Wi-Fi、数传存在延迟、抖动和丢包。这可能导致无人机基于过时信息做出决策引发碰撞。解决方案包括在状态估计中引入预测、使用具有容错能力的协同算法如一致性算法、甚至采用部分去中心化的架构减少对连续通信的依赖。定位误差无人机通过GPS、视觉里程计或UWB进行定位都存在厘米级甚至分米级的误差。我们的安全距离必须包容这个误差。更先进的做法是进行不确定性感知规划Uncertainty-aware Planning在规划时直接考虑状态估计的不确定性椭球。外部扰动风是无人机飞行最大的外部扰动。强风可能使无人机偏离计划轨迹。这需要在控制层设计鲁棒控制器如滑模控制、自适应控制或者在规划层就生成抗扰动的轨迹如增加冗余空间。异构无人机集群竞赛中假设所有无人机性能相同。现实中集群可能由不同型号、不同载荷、不同能力的无人机组成。规划时需要考虑到异质性例如让机动性强的无人机执行更复杂的避让动作。人机交互与伦理安全特别是在城市空域无人机需要被“无人机Remote ID”系统追踪需要遵守空域规则需要具备紧急情况下的“无人机反制”或失效安全机制如自动降落。这些都属于更高层面的系统设计问题。完成这个项目最大的体会是协同规划是一个典型的“系统级”问题。任何一个单一的算法都无法解决所有问题需要将感知、通信、规划、控制等多个模块有机整合并在算法的理想性与工程的实时性、鲁棒性之间做出精巧的权衡。从A*、RRT到CBS、ORCA从势场法到优化理论工具很多但更重要的是理解问题本质并选择或组合出最适合当前约束计算资源、时间、精度要求的方案。这个过程远比调通一个“无人机飞控”或跑通一个“目标检测”模型要复杂但也正是其挑战和魅力所在。