先说个我自己的体会单机路径规划我当年用A*和RRT系列做得挺顺手但一旦把场景换成多无人机同时在动问题性质就完全变了。每一架无人机不仅要躲开环境里的动态障碍物还得随时提防队友而且队友本身也在主动改变位置——这就不是简单地把障碍物列表多加几个坐标的问题。更麻烦的是动态环境意味着之前的规划结果可能下一秒就失效必须在线重规划。这篇文章我想把一套基于Matlab实现的多无人机协同路径规划与防撞仿真框架掰开揉碎讲清楚从任务场景拆解、算法选型、代码架构设计到调参踩坑和结果评估给正在做无人机集群、多智能体路径规划或者毕设相关方向的朋友一份能直接上手的参考。1. 动态环境下多无人机协同任务场景与两层难题1.1 典型任务场景为什么单机规划不够用先看一个很常见的任务设定。假设一个区域巡检场景需要出动5架小型四旋翼无人机从不同出发点起飞每架要分别到达各自的巡检目标点同时全程保持安全间距还要避开空域里随机出现的未知移动障碍物比如飞鸟、临时施工的吊车、其他非合作飞行器。这个任务如果你用每架飞机独立规划一条从起点到终点的路径来做大概率会遇到两类问题两条路径在时空上交叉飞机同时到达交叉点附近机间距离小于安全阈值某一架为躲避障碍物临时改道结果闯入另一架的航线保护区内。所以多无人机协同的本质不是各自规划然后把路径拼在一起而是要在规划阶段就把其他无人机的存在作为一个动态约束条件纳入考虑。这也是我把这个项目定义成协同路径规划与防撞而不是多机路径规划的原因路径规划解决的是从A到B怎么走防撞解决的是走的过程中不能和任何东西相撞两者必须耦合在一起处理。1.2 动态环境带来的第二层不确定性静态环境下的多机路径规划其实已经有大量成熟方法可以直接把队友建模为障碍物然后用集中式规划统一求解。但动态环境把这个逻辑打碎了。动态环境的动态主要体现在三个维度非合作障碍物的运动不可精确预知只能通过感知获得当前时刻的位置和速度对下一步的位置只能做短时预测无人机自身的运动受动力学约束四旋翼不能像质点一样瞬间转向、加减速规划层必须要给控制层留出足够的响应余量任务的动态性目标点本身就可能是移动的比如跟踪一个移动目标或者任务需求在飞行过程中发生变化。这三者叠加最直接的结果就是预先计算的全局最优路径的价值大幅缩水。你算出来一条路径花了0.5秒但环境在这0.5秒内已经变了算出来的路径可能已经不满足安全约束。所以动态环境下的协同规划本质上是在计算速度和最优性之间做妥协——这也是为什么现在主流做法普遍采用全局规划局部避碰的分层结构而不是单层求解。1.3 机间协同的两个核心问题优先级与冲突消解除了环境动态性多机系统还有一个独有的问题多架无人机之间发生潜在冲突时谁让谁这个问题必须提前定义清楚否则防撞逻辑会出现循环避让导致系统死锁。我常用的设定有两种。一种是对等式协同所有无人机地位相同通过协商机制比如速度障碍法中的互惠规则双方各承担一半避让责任另一种是优先级式协同根据任务类型或紧急程度给无人机设置固定优先级低优先级让高优先级。两种方式在Matlab里实现难度都不大但对结果的稳定性影响很明显。我的建议是小规模集群用对等式协同视觉效果好大家都在动规模一大或者任务有明确主次时优先用优先级式逻辑清楚好调试。2. 协同路径规划与防撞算法的选型分析2.1 几个主流方案的基本逻辑与适用边界动态环境下做多无人机协同路径规划Matlab社区里常见的方案有这么几类人工势场法APF、速度障碍法VO/RVO、采样规划法RRT/RRT、模型预测控制MPC*。它们的核心逻辑差异很大不能随便选。人工势场法的思路最简单目标点产生引力障碍物和队友产生斥力无人机在势场中沿合力方向运动。优点是实现快、计算量小、适合实时在线规划缺点也公认容易陷入局部极小值在狭窄通道里会震荡而且对动态障碍物的处理比较粗暴。速度障碍法的核心是速度空间几何在某个时间窗口内如果本机以当前速度飞行会撞上障碍物则这些会撞的速度构成一个锥形区域在速度空间中绕过这个区域选择一个新的安全速度。它天然适合动态环境因为它直接处理的是相对速度和碰撞时间而不是位置的远近。RRT系列是采样类方法通过随机采样构建搜索树来找到可行路径。优点是能处理高维约束和复杂几何环境缺点是随机性导致路径不够平滑计算时间不稳定用于在线重规划时需要做加速处理。MPC是滚动优化思路每个控制周期基于当前状态和一个预测模型优化未来一段时间内的控制序列并把第一段控制量下发执行。它的优点是可以把动力学约束、安全约束、协同约束统一写进优化问题里效果很漂亮缺点也很明显计算量大多机联合建模的话维度爆炸对Matlab仿真来说实时性是个考验。2.2 我最终采用的组合方案及理由在这个项目里我采用的方案是**全局路径预先规划 局部速度障碍法实时避碰 优先级规则消解冲突**的组合架构而不是单纯死磕某一种算法。原因是这套组合对动态环境的适应性最好全局层用RRT*给每架无人机预先规划一条从起点到目标点的粗路径这条路径在环境发生重大变化时才重新规划平时不频繁触发局部层把全局路径上的下一个航点作为临时目标点用改进的速度障碍法输出实时安全速度处理障碍物和队友的避让协同层用优先级规则处理多机冲突降低整体逻辑复杂度。这样设计的好处是全局层保证了大方向上的合理性和路径长度优化的空间局部层保证了实时安全性协同层解决了多机之间的规则问题。三个层各管各的调试的时候可以分开验证。2.3 关键参数为什么这么设速度障碍法有几个参数直接影响效果不能拍脑袋填。安全时间窗口time horizon我一般设为3到5秒。太短的窗口反应不过来太长的窗口会过度保守无人机稍微远离一点就开始绕路路径会变得很啰嗦最小安全距离这个通常由任务要求和飞机实际尺寸决定。我做的仿真里设为2米也就是两架无人机质心距离小于2米就算碰撞。取值不能比飞机物理尺寸大太多否则空域利用率太低避让反应距离我设为6米即探测到对方在6米内也按趋势接近时触发避让。这个参数决定了避让的提前量要和安全时间窗口配合着调。这些参数之间是强耦合的。比如安全时间窗口从4秒调到8秒最小安全距离不变无人机也会更早开始避让路径平滑度提升但绕行距离增加。我后面的调参章节会专门说怎么配合着调。3. Matlab代码实现仿真框架、核心模块与关键代码逻辑3.1 仿真框架的整体结构写Matlab仿真最忌讳的就是把一堆脚本堆在一起改参数要翻半天代码。我的做法是分成四个模块环境模块、无人机模块、规划避碰模块、可视化模块。每个模块以函数或类的方式组织主程序只是调用它们的主循环。环境模块地图边界、静态障碍物坐标、动态障碍物生成与运动更新 无人机模块无人机状态结构体位置、速度、目标点、优先级、半径 规划避碰模块全局路径生成、局部速度障碍法计算、防撞逻辑 可视化模块实时绘制轨迹、障碍物、安全区域指示主循环的时间步长我设为0.1秒相当于10Hz的控制频率。这个频率对仿真足够同时每一步要做的事情也简单更新环境、更新无人机状态、调用局部规划器得到新速度、检查碰撞、绘图。3.2 数据结构的选定从结构体到面向对象我做第一版的时候用的是结构体数组每架无人机是结构体里的一个元素。后来发现代码越加越乱速度障碍法里要频繁访问所有无人机的状态写起来很不舒服。第二版改成Matlab的类classdef每架无人机是一个对象自身的状态、参数和状态更新方法都封装在一起。环境也单独封装成一个类。这看起来是件小事但对后期调试和扩展影响非常大。比如后来我想给部分无人机加不同的约束条件直接在类里加属性就行不用动主程序的任何逻辑。Matlab的类虽然性能上不如纯结构体但在这种逻辑复杂度下可维护性收益远大于性能开销。3.3 核心代码逻辑速度障碍法的实现要点速度障碍法的Matlab实现代码量其实不大关键在几何计算。我写了一段核心逻辑做了简化但保留了最重要的步骤。% 输入本机状态 obs, 对方状态 other, 安全距离 d_safe, 时间窗口 tau % 输出候选速度 v_new % 1. 计算相对位置和相对速度 p_rel other.pos - obs.pos; v_rel obs.vel - other.vel; % 2. 判断当前速度是否会导致碰撞 dist_close norm(p_rel); if dist_close d_safe collision true; else % 最短趋近距离计算相对位置在相对速度方向上的投影 u v_rel / norm(v_rel); proj dot(p_rel, u); closest norm(p_rel - proj * u); collision (proj 0) (closest d_safe) (proj norm(v_rel)*tau); end % 3. 若存在碰撞风险生成避让速度沿切线方向偏转 if collision % 计算远离向量 n p_rel / norm(p_rel); % 在垂直于n的方向选一个切向速度同时保留朝向目标的速度分量 v_desired normalize(obs.target - obs.pos) * obs.speed_lim; v_new v_desired 2.0 * n * (d_safe - closest); % 截断到最大速度 if norm(v_new) obs.speed_lim v_new v_new / norm(v_new) * obs.speed_lim; end else v_new normalize(obs.target - obs.pos) * obs.speed_lim; end这只是一个最简洁的核心骨架实际运行中还要加入一个关键处理当多个避让方向同时冲突时不能单纯叠加修正向量否则会出现震荡。我的做法是取冲突方向中最危险的那个作为主避让方向其他方向的修正量按权重衰减后再叠加。3.4 全局路径与局部避碰的衔接方式全局RRT*生成的是离散路径点序列关键是局部层怎么追这条路径。我用的方法是航点推进机制每架无人机维护一个变量current_waypoint_index每次局部规划器的目标点不是最终终点而是路径序列中距离当前位置足够近的那个航点。当无人机距离当前航点小于某个阈值比如0.5米时推进到下一个航点。这个机制有个细节值得注意阈值太小飞机会在航点附近反复绕圈阈值太大飞机会提前切目标点导致飞行轨迹过度平滑、偏离全局路径。实际仿真中还要配合航点间的空间插值让航点密度足够高。我的做法是指定路径后用样条插值加密到每0.5米一个子航点这样局部追踪的稳定性好很多。3.5 动态障碍物的建模与更新动态障碍物我设了两种一种是直线匀速运动另一种是做圆周运动。直线匀速的用来验证基本避碰能力圆周运动的用来考验算法应对非匀速障碍物时的表现。更新方式很简单每个时间步按运动方程计算新位置。这里有个容易被忽略的坑动态障碍物的速度信息在代码里是已知真值还是带噪声的观测值。如果直接用真值计算避让仿真实测效果会好得离谱如果加噪声避让性能会明显下降。显然实际系统中你只有观测值所以我建议在所有仿真中都给速度加高斯噪声否则后续换到实测工况时算法会直接失效。4. 调参与踩坑记录从能跑通到跑得稳4.1 问题一无人机会在目标点附近来回震荡这个现象我第一版代码就遇到了。原因不复杂人工势场法中的目标引力在接近目标时趋于零但避让修正量仍然存在于是速度方向在目标点附近不停摆动。解决的办法有两个一是引入到达判定逻辑当无人机距离目标点小于设定值比如1米时停止避让计算只输出减速控制二是对避让修正量施加阻尼只有当速度方向的修正角度在合理范围内时才生效。我更推荐第一种因为做法干净逻辑清晰。实际代码里每次规划前先判断dist_to_goal arrival_threshold满足就直接以目标点作为期望速度方向并缓慢减速。4.2 问题二多机交叉飞行时出现八字绕圈或死锁场景是这样的两架无人机相向而行都用速度障碍法避让。由于它们同时采用各让一半的互惠策略在理想对称条件下能平顺错开但一旦初始条件不对称哪怕差一点点速度大小就可能出现来回让对方、双方轨迹像绕八字一样反复纠缠甚至原地死锁。这个问题我排查了很久最终定位到两点一是速度障碍法的避让方向选择过于贪心总是选择几何上最快的脱离方向二是我的代码里缺少对避让状态记忆。针对修复我给每架无人机增加了一个避让记忆计时器一旦进入避让状态后续若干秒内优先保持同一个避让方向直到确认冲突消除。这有点模仿人类避让时的决定方向就不轻易变的行为效果立竿见影。4.3 问题三全局路径重规划导致路径振荡动态环境里全局RRT*如果频繁触发重规划路径本身就会反复变化导致无人机大方向都不稳定。这个问题我建议从源头控制不要每次发现障碍物位置变化就重规划而是设定一个重规划条件比如全局路径距离障碍物最近点小于安全阈值且持续超过0.5秒才触发。这样既保留了全局规划的时效性又不会让规划结果乱跳。4.4 参数调节速查表的实践我整理了这份参数调节速查表完全基于这个项目的实际仿真调试经验。如果你在自己跑代码时遇到类似现象可以直接对照。现象相关参数调整方向碰撞频繁发生在近距离安全距离 d_safe增大避让过于提前路径绕行严重安全时间窗口 tau减小多机八字绕圈避让记忆时间增大目标点附近震荡到达判定阈值减小避让响应不及时避让反应距离增大整体路径曲折全局航点插值密度加密4.5 计算效率与仿真实时性Matlab仿真的运算效率是个绕不开的话题。我的代码一开始在主循环里直接用矩阵循环遍历所有配对5架无人机倒是没问题但一扩展到30架就明显卡顿。后来花了一晚上把所有配对计算全部向量化时间降了一个数量级。具体做法是不写双层for循环而是用meshgrid生成配对索引一条命令同时算出所有无人机之间的距离矩阵、相对速度矩阵。Matlab对这种矩阵运算的支持是非常偏心的能向量化就向量化这条经验在仿真规模扩大时特别值钱。5. 仿真结果评估衡量协同算法的几个硬指标5.1 指标一碰撞次数与最小安全距离裕度防撞效果最直接的指标是碰撞次数但这不够精细。两架飞机最小接近距离是3米另一组是1.5米安全距离阈值是2米后一组虽然没撞上但显然更危险。所以我除了统计碰撞次数还会统计最小安全距离裕度所有无人机两两之间的最近距离的分布情况。重点看有没有距离非常接近阈值但没有碰撞的时刻这代表系统擦着规则走鲁棒性较差。我在仿真里发现如果加了速度观测噪声最小接近距离会明显波动说明算法对感知误差是敏感的实际系统就更要预留安全裕度。5.2 指标二路径效率与能量消耗路径效率我统计两个量一是实际飞行距离与全局规划路径长度的比值这个比值越接近1说明局部避碰对全局路线的影响越小二是平均速度和能耗估计因为四旋翼的功耗和飞行速度不成线性关系整体绕行越少能耗越低。实测下来使用这套分层架构后在动态障碍物不多的情况下路径长度比值一般在1.1到1.3之间说明局部避碰没有过度牺牲全局效率。5.3 指标三系统收敛性与稳定性稳定性指标主要看两点一是在多次重复实验中无人机都能顺利到达目标点没有出现死锁或无限绕圈二是速度输出曲线没有高频震荡。我自己会额外做一个测试在仿真中途突然插入一个高速动态障碍物看系统能否在几个时间步内恢复稳定。这个压力测试比普通跑完一条航线更能反映算法在真实动态环境下的可靠性。5.4 典型场景验证的设计思路我设计了三个典型场景来验证算法的有效性这三个场景分别对应三种不同类型的动态冲突。场景名称场景设定主要验证目标交叉航线两队无人机呈十字交叉穿越空域机间互避的时序协调能力汇聚目标多架无人机飞向同一区域的不同目标点密集场景下的冲突消解能力动态突入中途突然加入高速移动障碍物实时响应与重规划能力这三个场景跑下来基本能覆盖动态协同路径规划的主要难点。我建议你在自己的项目中也按这个思路设计测试用例不要只跑单一场景就下结论。6. 从仿真到实飞的几个延伸问题仿真里跑得通的逻辑移植到真实无人机系统时至少还要处理三个落差。第一是通信延迟与丢包仿真里所有无人机状态默认是同步的实机系统里状态信息经过无线传输会有延迟速度障碍法里用的对方速度信息可能是几百毫秒前的旧数据避让效果会打折扣。应对思路是在估计对方状态时引入匀速运动预测模型用当前时刻的时间差外推位置。第二是定位误差与坐标系转换仿真里无人机坐标是全局坐标系下的精确值实机中位姿估计本身就带漂移。防撞算法直接使用这些带误差的值安全距离阈值必须放大否则会把估计误差当成真实距离。这也是我在仿真里坚持加观测噪声的原因。第三是动力学约束差异仿真里的速度输出在下一时间步就能实现实机四旋翼有加速度限制、姿态响应延迟。如果不做速度约束平滑算法输出的速度指令会触发控制器的剧烈响应。一个简单的做法是在速度输出端加一阶低通滤波代价是避让响应稍微变慢需要在安全距离上做补偿。最后说一个我在整个项目里体会最深的事动态环境下的协同路径规划难点往往不是某一层算法而是层与层之间的接口。全局规划、局部避碰、协同规则每一层单独跑都很顺畅一旦串起来各种奇奇怪怪的问题就出来了。所以调试时一定要分层验证先固定全局路径只测局部避碰再固定所有参数只改协同规则最后才做全链路联合仿真。这条路走通之后你再去看那些更复杂的方案——分布式MPC、强化学习之类的——会发现它们的核心还是同样的安全问题只是换了一套更精细的工具去描述约束。多无人机协同这块的东西工程细节永远比算法名字更有嚼头。
