1. 为什么单机路径规划解决不了多机协同的问题我最早接触多无人机协同路径规划是在一个模拟巡检项目里。当时想着用经典的 A* 或者 RRT 给每架飞机单独规划一条航线然后再加一个简单的距离检测如果靠太近就停下来等一等以为这样就算协同了。结果真到仿真环境里跑起来问题立刻暴露两架飞机在同一个走廊里迎面相遇各自的局部规划器都试图躲避对方结果一起往同一个方向闪不仅没避开反而撞在了一起。停下来等更是离谱——一架悬停等待另一架以为前方有障碍也悬停整个编队直接陷入死锁。这件事让我意识到一个关键点多机协同和单机路径规划在本质上不是同一个问题。单机规划只需要考虑从 A 到 B 怎么走不撞障碍物而多机协同需要考虑的是多架飞机共享同一时空资源时如何让每架飞机都能完成自己的任务同时保证彼此之间的安全间距。前者是几何问题后者是时空资源分配问题。动态环境又把问题复杂度推高了一截——障碍物是运动的其他无人机也是运动的每一时刻的可行路径都在变化。这篇文章想聊的就是我在这个方向上从零到一的过程包括协同架构怎么选、路径规划算法怎么改、防撞逻辑怎么设计以及最终 Matlab 仿真代码里的那些关键细节。如果你也在做多无人机路径规划相关的研究或项目尤其是卡在多机之间总打架这个阶段这篇文章应该能帮你省不少时间。代码层面的思路我会尽量拆开讲具体实现细节和调试经验才是真正值钱的地方。2. 协同架构选型集中式、分布式还是分层式2.1 三种架构的适用场景和代价差异先说结论架构选错了后面算法再怎么调都很难救回来。多无人机协同路径规划里最核心的第一个决策就是架构。常见的无非三种集中式、分布式、分层式。集中式架构是最直观的思路——地面站或某架领航机收集所有无人机的状态和任务信息统一计算全部路径然后把结果下发。这种方式的优点是全局最优性有保障所有冲突可以在规划阶段一次性消解。缺点是计算量大得惊人尤其是无人机数量超过 5 架之后状态空间的维度增长非常快更致命的是它对通信的依赖是单点式的一旦中心节点出问题整个编队就瘫痪。分布式架构则反过来每架无人机只根据自身感知到的局部信息做决策机间通过通信协商避免冲突。这种方式的实时性和鲁棒性都很好但代价是只能保证局部最优无法从全局视角判断这个决策是否会引发远期冲突。在实际仿真里很容易出现一个现象两架飞机局部协商后各自绕了一点路结果绕过去之后又迎面撞上了需要再次协商整个过程反复振荡。分层式架构是我个人在实践中最推荐的方向也是我最终采用的方案。它的核心理念是把协同任务拆解成不同时间尺度的问题来处理。上层的任务规划层负责宏观的航迹分配和目标点指派是分钟级别的决策中层的路径规划层在给定航迹点之间生成可行路径是秒级别的决策底层的轨迹跟踪与避撞层则处理实时避撞和速度调整是毫秒级别的决策。每一层只处理自己这个尺度的问题不需要跨尺度去操心别的层的事这样既避免了集中式架构中心节点压力过大的问题又比纯分布式协同多了一层全局约束。2.2 我为什么选了分层式架构在模拟项目里我需要让 5 架无人机在一片有动态障碍物的区域内完成多目标点的遍历任务目标的分配、路径的生成和实时避撞都必须同时满足而且中间要留出可调节的参数空间来做对比实验。集中式架构在一开始的实验里很快暴露了问题——当障碍物速度变快时全局重规划的频率越来越高中心节点的计算延迟拖累了整个系统最严重的时候会出现路径下发时飞机已经飞过头了的尴尬局面。分布式架构我做了个简化版测试效果也一般主要问题出在协商策略上没有全局信息做背书局部协商出来的路径经常会出现系统性偏差整个编队会慢慢偏离任务区域。分层式架构解决了我最头疼的问题。任务规划层只需要在目标点变化或者某架无人机失能时重规划频率很低可以接受较大的计算量路径规划层在动态障碍物逼近时做局部重规划频率中等算法有足够时间收敛底层避撞则是纯响应式逻辑速度快不依赖复杂计算。这样每一层的时间尺度不同计算资源的需求也被自然分摊开了。具体实现时我用了一个比较朴素的方案任务规划层用匈牙利算法做目标点分配路径规划层用改进后的 RRT* 生成路径底层避撞用一个优先级加速度调整的机制。后面我会详细说每一层的实现细节这里先提一句架构设计的核心是让不同时间尺度的决策互不干扰这一点在系统复杂度上来之后尤其重要。3. 动态环境建模时间维度才是协同的隐藏难点3.1 障碍物建模的三种方式和精度取舍所谓动态环境指的不是地图上多了几个移动的点而是路径规划的输入从一个静态地图变成了一个随着时间变化的状态集合。这一步做不好后面的所有算法都等于在打移动靶。我在项目里把动态环境建模分成了三类。第一类是已知轨迹的移动障碍物比如机场跑道上的服务车辆、执行固定航线的大型运输机它们的未来位置可以通过运动模型和时间参数精确推算。第二类是速度可测但轨迹未知的移动障碍物比如阵风区域里的随机湍流、无人机群中偏离航线的其他个体这些需要通过传感器持续跟踪用卡尔曼滤波或类似手段预测它未来一段时间内的位置。第三类是完全未知的突发障碍物比如突然出现在作业区域里的鸟类群或者信号干扰区这种只能在检测到之后尽快重规划。三类障碍物在建模时对计算资源的要求差距非常大。第一类最适合做精确的时空避让因为已知未来位置这个条件让冲突检测变成了确定性问题第二类需要引入不确定性描述通常是在预测位置上加一个锥形的不确定区域区域大小随预测时间增长而膨胀第三类只能用保守策略直接在检测到障碍物时预留更大的安全距离。我一开始贪图省事把所有障碍物都按第三类处理每架飞机都留了很大的安全半径结果导致可行通道被大幅压缩编队经常找不到合适的路径。后来改成按类型分层建模这才解决了问题。3.2 时空联合坐标系把时间变成第三个维度路径规划算法要处理动态环境最自然的一个思路就是把时间当作路径的一个维度来参与规划。具体做法是地图从二维平面 ((x, y)) 扩展成三维时空 ((x, y, t))障碍物在时空里变成了一个随时间推移的柱状或斜柱状区域路径规划问题就变成了在时空坐标系里找一条不穿过障碍物柱子的曲线。这个思路在理论上是优雅的但实现起来有一个坑必须注意时空路径规划对时序一致性有严格的要求。航迹上的每个点都必须严格单调地对应一个时间值不允许出现时间回溯因为飞机不能倒着飞。这意味着规划算法在采样或者扩展节点时必须强制约束时间维度是递增的。很多现成的规划算法比如 RRT、PRM默认各个维度是等价的直接套到时空坐标里会产生大量时间回溯的无效样本浪费计算资源。我最初的做法是把时间离散化成固定的时间步长然后在每个时间步上对空间位置做采样这样天然保证了时间的单调性。但问题也很明显——时间步长选大了障碍物位置变化快时会有穿模风险选小了时空网格的体量爆炸搜索效率极低。后来改成了自适应时间步长的方案在障碍物密集的区域缩小时间步长在开阔区域放宽时间步长效果好了不少。3.3 动态障碍物与无人机的相互作用还有一个容易被忽略的点是其他无人机本身就是动态障碍物的一部分。协同路径规划中的障碍不仅仅是环境中的移动物体还包括编队内部的其他成员。这两类动态障碍物在建模时有一个本质区别——环境障碍物是被动存在的只会按自己的轨迹移动而无人机是主动响应的它们会根据当前的环境和决策调整自己的速度与方向。如果只是简单地把其他无人机当作已知轨迹的动态障碍物来处理当对方突然变速或者转向时预测就会完全失真。在处理这个问题时我采用的方案是把无人机之间的防撞单独拉出来做一层不混进环境障碍物的建模里。环境障碍物的时空预测模型负责处理那些不会主动避让的威胁无人机之间的安全间距则通过速度障碍法Velocity Obstacle简称 VO配合优先级策略来保证。这样每层的假设和模型是自洽的不会出现把会主动躲避的无人机当成不会躲避的砖头来规划这种逻辑漏洞。4. 多机协同路径规划的核心算法实现4.1 改进 RRT*从单机路径到多机网络路径规划这一层我用了 RRT* 作为基础算法在此基础上做了两个关键改造让它能适配多机协同的需求。第一个改造是把代价从单纯的路程长度扩展为带风险惩罚的评估函数。传统 RRT* 在评估路径代价时只算欧氏距离但多机协同场景里路径的风险往往比长度更重要——一条路程稍长但远离动态障碍物密集区的路径实际执行起来可能比抄近道穿过障碍物密集区的路径更快到达目标因为不需要频繁降速避让。我在代价函数里加入了一个风险项计算路径上每个采样点附近动态障碍物的影响强度乘以一个权重系数叠加到总代价里。这个权重系数是一个重要的调参变量权重太高路径会过度绕远权重太低又会回到偏要穿越危险区的旧问题。第二个改造是引入了路径走廊Corridor的约束。多机协同中两架无人机的路径之间必须保持安全距离。但这种约束如果通过分别规划完再检查碰撞的方式来做效率很低——5 架飞机两两组合就有 10 对关系逐对检查起来既耗时又容易漏掉三个以上同时靠近的耦合场景。我的做法是在采样过程中直接约束新采样的节点如果与已规划路径的距离小于安全阈值则直接舍弃。这样一来路径生成阶段就已经天然避免了编队内部的近距离接触。改造成果立竿见影原先需要额外做两两碰撞检测的步骤直接被省掉了规划速度提升了很多。但代价也不是没有——强约束会牺牲部分路径的最优性在通道特别窄的环境里先到先占的机制会让后规划的无人机面临极其绕远的路线。所以我又补了一个策略在任务规划层做目标分配时尽量让各无人机的路径走廊交错开减少抢走廊的情况发生。4.2 速度障碍法与优先级防撞策略的配合路径规划层生成了一条理论无冲突的时空路径但实际执行过程中由于控制误差、风速扰动或者障碍物轨迹预测偏差无人机仍然可能偏离计划路径。这时候底层的实时防撞机制就要发挥作用了。我采用的底层防撞策略是速度障碍法VO结合优先级机制。速度障碍法的核心思想是根据对方无人机当前的位置和速度计算出哪些速度向量会让本机在未来一段时间内进入碰撞区域然后从合法速度集合里选一个最优解。这个方法的优点是实时性好、计算量小非常适合作为底层响应机制。但纯速度障碍法有一个问题——两架无人机可能同时把对方当作障碍物各自选择不同的避让方向反而导致碰撞。这就是著名的对向避让冲突问题。我用的解决方案是给每架无人机分配不同的优先级编号低优先级飞机在高优先级飞机接近时主动让路高优先级飞机保持航向不动。这个规则虽然简单但在实际仿真中确实有效解决了互相对闪的振荡问题。这里有一个值得注意的细节优先级不能一成不变。我在实验中发现如果任务规划层给某架飞机分配了很多任务点但它一直因为优先级太低而频繁让路整体任务完成效率会变得很差。所以我在每次任务重分配时会根据当前的执行进度动态调整优先级——任务落后较多的飞机在下一轮会获得更高的避让优先权。这个策略在小组规模3-5 架下运行得很顺畅。4.3 时间窗协调处理瞬时拥挤的补充机制即使有了路径走廊约束和 VO 防撞还有一个场景很难处理——多架无人机同时通过一个狭窄通道。路径走廊约束只能保证路径线段之间的距离但如果地形条件迫使两架飞机的路径在一段区域内必须近似重合走廊约束就会失效VO 防撞虽然能保证不碰撞但会导致后面的飞机频繁减速甚至悬停影响编队通过效率。针对这个场景我在中间层加了一个时间窗协调机制当规划层检测到两条路径在时空上有冲突时不直接丢弃路径重规划而是计算冲突区域的时间窗通过调整其中一架无人机在该区域的过境时间来解决冲突。具体做法是一架飞机按原计划通过冲突区域另一架在进入冲突区域之前的一个航迹点上增加等待时间或小幅减速确保两架飞机不同时处于冲突区域内。这个方法本质上是排队通行的思想。实际效果很不错——在仿真环境中狭窄通道场景下的编队通过时间比纯 VO 策略缩短了约 30%。代价是需要给路径规划层增加一个可等待航迹点的概念稍微增加了一点规划器的复杂度但相比重规划整条路径的开销这个代价完全可以接受。5. Matlab 代码实现的关键细节与调参经验5.1 模块划分与核心数据结构设计这个项目里我把整套代码分成五个模块这个划分本身就是一行行调试出来的值得说说。第一个模块是环境建模模块负责生成地图、动态障碍物的轨迹、以及障碍物的时空预测数据。我用了结构体数组的方式存储障碍物信息每个障碍物包含初始位置、速度向量、运动模型和预测轨迹序列。这种数据结构的优点是方便调试——你想看哪个障碍物未来 5 秒的位置直接调用预测函数返回对应的时空坐标即可。第二个模块是任务规划层输入是目标点集合和当前各无人机的状态输出是目标点分配矩阵。我用的是匈牙利算法的标准实现在 Matlab 里可以直接调用matchpairs函数底层实现不需要自己重造轮子。任务规划层还会附带计算每架无人机的优先级作为底层防撞的输入参数。第三个模块是路径规划层这是整个代码量最大的部分。我实现了改进的 RRT* 算法核心数据结构是节点数组和边的邻接表。RRT* 的节点扩展逻辑、代价函数计算、走廊约束检查、以及最终的路径平滑和剪枝都集中在这个模块里。为了保证代码可读性我把 RRT* 的步骤拆成了几个子函数采样、最近邻搜索、避障扩展、重连接、代价更新。第四个模块是轨迹跟踪与控制层负责让无人机按照规划好的路径执行并在必要时调用底层的 VO 防撞机制调整速度。这个模块是仿真环境中的无人机动力学模拟与规划层输出的参考路径之间的桥梁。我用了简化的质点模型来模拟无人机运动设定最大速度、最大加速度约束。第五个模块是主仿真循环负责统筹以上所有模块的定时调用管理仿真时钟、数据记录和可视化。这个模块划分有一个好处每个模块都能独立测试。我建议你在自己的实现中也保持这个习惯不要把所有逻辑都堆在一个大脚本里。分开写虽然初期麻烦一点但后面调试和出实验图的时候能省非常多时间。5.2 关键参数的初始值、调整方向和实际效果我把这个项目里影响最大的几个参数整理成了表格这些数值都是基于 5 机编队、1000m × 1000m 地图、20 个目标点的场景调试出来的仅供参考真正使用时必须结合你自己的场景来调整。参数名称初始值调整方向重要说明安全距离阈值 (d_s)30m障碍物密度大时适当减小太大会导致路径绕远太小会导致碰撞风险上升采样步长 (L_s)50m地图缩小时同步减小步长影响路径平滑度和规划时间一般保持为地图边长的 5%RRT* 最大迭代次数2000看规划结果是否收敛迭代不够时路径质量差够了之后继续增加只会徒增耗时代价函数风险权重 (w_r)0.3障碍物动态性强时增大权重太大路径绕远严重太小会频繁触发重规划VO 检测时间窗3s飞行速度慢时适当增大时间窗决定了避撞策略的前瞻性太短反应不过来时间窗协调容忍度5s通道狭窄时增大用来控制排队等待的最大时间超过则触发重规划这里重点说两个我踩过坑的参数。第一个是安全距离阈值 (d_s)它直接影响编队的整体行为——如果设置在 50m那么 5 架飞机在 1000m 的地图上编队飞行时几乎任何两条路径都会有走廊冲突RRT* 的扩展过程会频繁舍弃样本规划成功率大幅下降。我后来把 (d_s) 从 50m 逐步调小到 30m路径规划的成功率才回到正常水平。第二个是代价函数里的风险权重 (w_r)第一次实验时我设了 0.8结果所有无人机都倾向于远离障碍物绕了相当远的距离。下调到 0.3 之后效果好了很多。总结下来这两个参数都需要配合地图几何尺寸和障碍物密度来scale固定值思路容易翻车。5.3 三层避让逻辑的代码流程与衔接细节在代码层面我设计了一个三层递进的避让判定逻辑从路径规划层开始逐层往下触发。第一层路径规划阶段通过走廊约束尽量避开其他飞机的已规划路径这一层是防患于未然。第二层在航迹跟踪过程中如果检测到前方动态障碍物或他机在未来预测时间窗内可能进入安全距离则调用 VO 策略调整速度向量这一层是实时避让。第三层如果 VO 策略也无法避免碰撞比如对方速度突变或者通信延迟导致检测过晚则触发紧急策略让低优先级飞机悬停等待高优先级飞机通行这一层是兜底保命。三层逻辑的衔接有一个关键点第二层 VO 避让触发时会临时改写期望速度但不会修改规划层的参考路径只是暂缓沿路径前进的进度。当避让完成后无人机会自动加速追回参考路径的进度。这样设计的好处是规划层的路径不需要频繁重算计算负载很稳定。但要注意如果 VO 触发得太频繁会导致任务完成时间大幅拉长。所以我在每架无人机的控制器里加了一个避让次数统计器如果一段时间内某架飞机的 VO 避让次数超过阈值就主动触发路径重规划通过绕路来减少后续的临时避让次数。我这个统计器的逻辑比较简单设定每次避让会损失约 5 秒的航迹跟踪进度如果累计损失超过 20 秒就认为当前路径在动态环境下的可行性不佳触发重规划。在实际实验中这个策略让编队在动态环境下的任务完成时间缩短了约 15%效果明显。5.4 仿真中的可视化技巧Matlab 仿真最容易被忽视的部分是可视化但它几乎是调试效率的第一决定因素。纯看数值输出根本看不出路径轨迹之间的空间关系而一张图就能看出来大概在哪儿出了问题。我的做法是在仿真主循环里用 Matlab 的animatedline对象来绘制每架无人机的航迹每帧更新时用drawnow刷新画面。动态障碍物的预测轨迹用虚线绘制实际轨迹用实线绘制。当 VO 防撞触发时会以不同颜色标记该时刻的避让动作方便追踪哪一段路径频繁避让。这样一个可视化界面的好处是直观显性。有一次我调好参数后跑仿真发现 3 号机在某一区域频繁变黄避让标志看轨迹才发现它的路径规划经常穿过一个动态障碍物的往返路径——因为 RRT* 的随机采样在那一小片区域里恰好反复选出看似较短但实际风险很高的路径。后来我在代价函数里对这个区域的风险强度又做了二次加权现象就消失了。如果没有可视化这个问题光靠数值表几乎发现不了。6. 实测效果、性能瓶颈和那些文献里不会写的坑6.1 我的仿真实验配置与结果数据为了验证这套方案的有效性我设计了三个对比实验场景分别对应不同的任务目标。第一个场景是静态障碍物环境下的多目标遍历用来作为基线第二个场景是加入已知移动障碍物的动态环境第三个场景是全部未知突发障碍物的动态环境。指标我选了三个任务完成时间、碰撞次数、平均路径长度。在静态环境基线场景中5 架无人机去遍历 20 个目标点任务完成时间约 280 秒碰撞次数为 0平均路径长度约 3200m。加入已知轨迹移动障碍物后任务完成时间上升到 350 秒左右碰撞次数仍然为 0平均路径长度增加了 300m 左右——这部分增加是因为绕行和时空避让带来的。第三个场景全部换成未知突发障碍物后任务完成时间到了 420 秒附近触发了两次紧急策略但没有发生实际碰撞。统计数据背后暴露了两个规律。第一动态障碍物的可预测性比障碍物本身的速度更影响编队表现。已知轨迹障碍物即使速度很快对路径规划的影响也是可控的但未知障碍物哪怕数量少也会因为保守安全距离的大幅缩边而导致路径选择范围迅速收窄。第二编队规模从 3 架增加到 5 架时任务完成时间的增长不是线性的。因为每增加一架飞机配对冲突关系的数量是 (n(n-1)/2) 增长的路径走廊约束的紧迫程度也随之上升这直接影响 RRT* 的采样成功率。6.2 重规划频率失控动态环境里最隐蔽的坑如果说整个系统里哪个问题最折磨人我会毫不犹豫地说是重规划频率失控。不是规划算法崩了而是规划层被频繁触发以至于规划结果根本来不及被执行。这个问题的根因在于动态障碍物预测的误差会随时间增长。预测时间越长位置的不确定性越大。如果航迹跟踪层发现实际障碍物位置和预测值有偏差就触发重规划那么重规划会极度频繁。而且重规划的结果往往和原路径比较接近因为偏差还没大到需要重新选路但这已经足够消耗大量计算资源了。解决的办法说起来很简单但想明白用了很长时间给重规划设置一个死区。只有当预测轨迹与实际轨迹的偏差超过某个阈值时才触发重规划而不是每帧都检查。这个死区大小要配合安全距离来设置——如果安全距离是 30m那么偏差阈值设成安全距离的一半15m比较合理小于 15m 时先通过底层的 VO 避让来扛扛不住了再触发重规划。这个策略大幅降低了重规划频率从最坏情况下的每 2 秒一次降到了平均 15 秒一次规划层的计算负载问题顿时消失。后面我又想到一个更深的坑。重规划的触发不能只看位置偏差还得看速度方向偏差。有时候动态障碍物的位置偏差不大但速度方向已经完全不同了——这说明预测模型本身已经失真继续按原路径飞行迟早会出问题。这种情况即使在位置偏差阈值内也该触发重规划。我把两个条件改成了或逻辑位置偏差超阈值或速度方向偏差超阈值二者触发其一就重规划。6.3 通信延迟与丢包对协同的隐性影响多无人机系统的仿真很容易默认每架无人机都能实时获取其他飞机的精确位置。但只要你把通信延迟加入模型整个系统的行为会立刻变得面目全非。我做了一个极端测试给机间通信加入 0.5 秒的固定延迟和 5% 的丢包率模拟真实无线通信环境。结果系统出现了很多诡异的现象VO 避让经常慢半拍导致两架飞机已经进入了安全距离内才开始避让时间窗协调机制因为拿不到对方的实时位置排队逻辑经常错乱甚至出现了两架飞机互相谦让、都不肯先走的情况。应对的思路有两个层面。第一层面是在算法上补偿延迟——VO 检测时不要用对方当前的通信位置而是根据对方的通信位置加上预测时间外推位置这个改动很小但效果立竿见影。第二层面是设计上的妥协——把安全距离适当扩大以通信延迟为尺度适当增大安全半径。这个扩大会增加编队的空间占用但换来了系统对延迟和丢包的鲁棒性。我个人认为在真实应用中这个交换是值得的。6.4 参数敏感性分析与调参的迭代循环最后说一个系统性的问题这套方案的参数太多彼此之间有耦合关系调参过程如果不系统地组织会非常痛苦。我建议的做法是用分层逐步调参的策略。先把底层参数VO 检测时间窗、安全距离固定下来单独调路径规划层的参数采样步长、迭代次数、风险权重等路径规划层的输出质量稳定了再调任务规划层的参数目标分配策略、优先级动态调整逻辑最后在所有层都稳定后通盘检查一次耦合参数的影响。在耦合参数里我特别提醒一个容易忽略的组合安全距离阈值 代价函数风险权重 时间窗协调容忍度。这三者分别影响着路径走廊的可用宽度、路径绕远的倾向度、以及排队等待的最大耐心值。如果你发现编队排队等太久不要只想着调容忍度试着看看是不是安全距离设大了导致走廊太窄排队变普遍了。这种跨层的因果关系在仿真结果里表现得非常隐晦需要多试几组数据才能看清。还有个更实际的建议每次只改一个参数并且务必记录实验结果。我在做参数扫描实验时吃过亏——连续改了三四个参数结果效果变好了但完全不知道是哪个起的作用。后来老老实实一天改一个数据逐一记录效率高得多。7. 一点后续方向的思考整个项目做下来我的整体判断是在 5 架无人机这样的小规模编队里分层式的协同架构配合路径走廊约束 VO 实时避让 时间窗协调的组合方案已经足够工程可用。但如果你要扩展到 10 架以上这套方案会遇到明显的瓶颈——最核心的问题是路径规划层的走廊约束变成了强耦合约束后规划的无人机会越来越难找到可用路径。如果要继续往这个方向深入我个人觉得有两条路值得探索。一条是引入更灵活的优先级动态协调机制比如虚拟排序的思路让无人机在飞行过程中动态调整通过冲突区域的次序而不是固定优先级另一条是在路径规划层引入更高效的采样策略把已规划路径的信息作为采样引导减少无效采样的比例这样走廊约束的代价才能降下来。这两条路我都在断断续续地试如果有了新进展再写一篇后续总结分享出来。