几架无人机在同一片空域里干活麻烦从来不是飞得快不快而是你飞的时候别人也在飞。特别是一旦目标位置在移动、外部障碍物会突然出现在航线上、机间距离被压得很近单机那套“规划一条路走到底”的逻辑会瞬间失效。我自己在做某跨平台系统的无人机协同模块时被这个问题折腾过好几轮最后在动态环境下用了一套“全局路径规划 局部动态避障 机间优先级协商”的Matlab实现跑通了多机协同路径规划与防撞。这篇就把这个方案拆开聊从问题建模、算法选型到Matlab代码结构尽量让想复现这套仿真的朋友少踩一些坑。这个工作适合谁参考一是准备做多无人机协同路径规划课题的学生二是需要快速搭建多机防撞算法原型验证的工程师三是刚接触机器人路径规划、想用一个完整框架把A*、DWA、速度障碍这些概念串起来的学习者。方案本身不复杂但它的组合思路和对应工程细节是很多“算法Demo”不会直接告诉你的。1. 动态环境下多无人机协同与防撞的问题本质1.1 动态环境的“动”到底指什么动态环境这个词听着宽泛但在多无人机系统的仿真里一般指三类变化。第一类是目标点移动。侦察类任务里目标可能是一个正在行驶的车辆也可能是移动的救援对象无人机的“终点”不是固定坐标而是需要不断更新的实时候选点。第二类是外部障碍物移动。比如园区里突然驶入的卡车、低空飞行的其他飞行器这类障碍无法提前完整出现在静态地图里只能靠机载传感器或共享信息实时感知。第三类变化最容易被忽略就是同伴无人机本身也在移动。多机协同中其他无人机既是任务的协作成员也同时是各自规划器必须避让的动态障碍。这三类变化叠加在一起会让传统离线规划彻底失效。静态环境下我们可以在起飞前解出一条最优路径然后一路飞到终点。但动态环境下路径本身会过期甚至无人机当前的速度窗口都会过期必须满足“感知—预测—决策—执行”的闭环。所以做多机协同防撞不能只谈路径搜索算法的改进更要谈“在线重规划”和“速度空间选择”。1.2 “协同”不是一堆单机规划器的简单叠加很多人以为多机协同防撞就是每架无人机各自跑一套避障算法。实际上大多数单机算法把其他无人机当普通障碍物处理时会引出很尴尬的对称避让问题两架相向而行的无人机同时向左躲发现不行又同时向右躲路径不断振荡即使不发生碰撞飞行效率也非常低。单机算法为什么会这样因为它们不会“商量”。普通障碍物不会根据你的动作调整自身轨迹但无人机之间是有通信能力或至少可以共享状态的。协同防撞的关键不是让每架飞机都更聪明地绕开别人而是让整个系统在“我让”“他让”“我们约定怎么让”之间达成一致。协同通常体现在两个层面。第一是空间协同即通过协调机制避免多机同时占据同一空间。第二是时间协同即通过速度调整让各机按预定时间到达任务点避免汇聚冲突。这两个层面才是“协同路径规划”里的“协同”真意。如果只是每架飞机独立避障后再比对结果那不叫协同叫碰运气。所以设计多机系统时我建议首先把问题抽象成一句话在满足动态障碍物安全间距、无人机之间安全间距、运动学约束和时间协同约束的前提下为每架无人机找到从当前位置到目标位置的可飞行轨迹。清晰的问题定义能指导后面的算法选择。2. 方案选型为什么选用“A* 动态窗口法 优先级协商”2.1 常见规划算法的横向对比多无人机路径规划能选的算法很多我先把常见的几种放在一起比较。算法适用维度动态避障能力多机协同适配度Matlab实现复杂度主要短板A*2D/3D栅格弱需重规划中需额外协调低转折多、全局路径静态RRT/RRT*高维连续空间弱需重规划中中随机性强路径不稳定人工势场法连续空间强实时性好中低局部极值目标不可达动态窗口法DWA速度空间强在线避障中可加协同项中依赖全局参考路径速度障碍法VO速度空间强适合运动障碍强天然描述机间冲突中复杂环境容易无解冲突搜索CBS多智能体路径针对已知路径强较高动态重规划成本高从表格能看出来没有一种算法单打独斗就能完成动态环境下多机协同防撞。A*适合做全局参考路径但本体和静态地图都变化后需要重规划DWA擅长在局部输出符合运动学约束的速度但缺少对动态障碍的“预判”速度障碍法天然适合描述“两架无人机之间”的相对速度冲突但单独使用容易掉进局部无解。所以我的选择是把它们按角色组合起来各干各擅长的部分。2.2 三层框架如何分工这套方案的框架可以拆成三层。最底层是全局规划层我选用改进A*。它负责在已知静态地图上快速生成从起点到终点的参考路线。这一层不需要太高的刷新频率因为静态障碍不会频繁变化。它的作用主要是给局部规划器一个方向引导避免局部算法一直在优化“下一步”却没有整体方向感。第二层是局部感知与避障层我选用DWA。它从当前状态出发在速度空间采样一组候选速度并模拟未来一小段时间内的轨迹用评价函数选出一条最合适的。它天然处理运动学约束也能把周围障碍物、其他无人机投影到轨迹模拟里。DWA的动态能力正好补上A*对在线变化的反应盲区。第三层是机间协商层采用优先级避让加速度障碍法的融合策略。当两架无人机靠得太近时只靠DWA各自的评价函数可能产生“对称避让”问题因此引入协商机制。优先级的来源可以是任务紧急程度、剩余电量、到达时间要求等。协商结果会作为DWA评价函数里的一个硬约束或额外惩罚项改变原本的速度选择倾向。这种分层最大的好处是解耦。全局路径、局部避障、机间协同各自出问题时可以分别调参和排查不会被一个巨型神经网络的不可解释性绑架。2.3 这套方案在Matlab里的整体流程把三层框架落到Matlab里整体流程是这样的读入静态地图并生成栅格化代价地图2. 初始化每架无人机的位置、速度、目标点、优先级3. 对每架无人机执行改进A*得到全局参考路径4. 进入主仿真循环按时间步长推进5. 每个循环内对每架无人机先更新对动态障碍物和邻近无人机的感知信息6. 用DWA在当前速度空间采样生成候选轨迹7. 通过协商层判断候选轨迹是否会引起机间冲突若冲突则调整评价函数8. 选择评分最高的速度并执行9. 判断无人机是否到达目标点或是否需要重新调用全局规划。这个流程并不复杂但每层都需要处理一堆工程细节。下面我按模块讲代码实现时会把关键细节一起说清楚。3. Matlab实现要点与核心代码解读3.1 无人机与地图的建模方式写Matlab代码前我建议先把数据结构定清楚。如果只是做二三架无人机的仿真用结构体数组就够了。真实工程里可能有更多无人机但结构体数组是理解问题最好的起点。% 单架无人机的基本状态 drone.pos [0; 0; 0]; % 三维位置列向量 drone.vel [10; 0; 0]; % 三轴速度 drone.target [100; 80; 20]; % 当前目标点 drone.radius 0.5; % 无人机包络半径 drone.priority 1; % 数值越大优先级越高 drone.refPath []; % 存放A*生成的全局路径 drone.lastVelocity [0;0;0]; % 上一步速度用于平滑约束动态障碍物可以单独放在一个结构体数组里因为它们的状态会被感知模块更新。obs.pos [40; 30; 0]; obs.vel [5; 0; 0]; obs.radius 1.0; obs.active true;地图我习惯用二维栅格数组代表水平投影高度单独做层间避让约束。例如map zeros(200, 200); % 200x200栅格 map(50:60, 80:90) 1; % 静态障碍区域置1这种建模方式简单直接但有一个容易被忽略的点栅格单元大小和无人机半径的关系。无人机半径0.5米栅格分辨率不能设成2米否则路径会和实际碰撞风险脱节。我一般让栅格单元尺寸小于无人机包络半径然后在A*里额外给障碍物区域做膨胀编译代价地图时就把安全距离“吃进去”。3.2 改进A*全局路径代价与启发函数A*本身不复杂核心是节点扩展、代价累加、启发式搜索。Matlab里实现时我总结了三个改进点。第一是启发函数的选择。普通二维欧式距离可以正确引导搜索但无人机转向是有代价的。我在代价函数里额外加入转向惩罚路径就不再是“折线最短”而是“飞行代价较小”。代价函数可以写成cost g(n) h(n) turnPenalty; turnPenalty k * (1 - cos(deltaAngle));其中deltaAngle是父节点方向到当前扩展节点方向的角度变化。这样A*生成的路径会尽量避免频繁转弯同时也缩短了后期DWA跟随路径时的曲率变化。第二是安全走廊约束。扩展节点时不仅要检查当前栅格是否是障碍还要检查该栅格周围一定半径内是否存在障碍。这相当于在搜索过程中就加了一层安全缓冲区后续DWA不会动不动就把路径拉进贴墙状态。第三是路径平滑。A*出来的路径往往有明显的折角直接交给DWA做参考会引入不必要的震荡。我常用三次B样条曲线对路径做一次平滑或者用贝塞尔曲线做局部光顺。平滑后路径点数目可以适当抽稀但要保留足够密度供局部规划器投影。核心搜索部分可以这么写function path astar_plan(map, start, goal, safeDist) % map: 代价地图 % 启发函数采用欧式距离 openList PriorityQueue(); openList.insert(start, 0); gScore inf(size(map)); gScore(start(1), start(2)) 0; cameFrom containers.Map(); while ~openList.isEmpty() current openList.pop(); if norm(current - goal) 1e-3 path reconstructPath(cameFrom, current); path smoothPath(path, map, safeDist); return; end for neighbor getNeighbors(current, map, safeDist) tentative_g gScore(current) edgeCost(current, neighbor); if tentative_g gScore(neighbor) gScore(neighbor) tentative_g; priority tentative_g heuristic(neighbor, goal); openList.insert(neighbor, priority); cameFrom(num2str(neighbor)) current; end end end error(No path found); end代码里PriorityQueue需要自己写一个简单的最小堆或者用Matlab的sortrows配合struct数组也能实现数据量不大时性能足够。我调试时发现A*最影响效果的不是启发函数而是代价地图的膨胀半径处理。3.3 DWA局部防撞速度采样与评价函数DWA的核心是“动态窗口”的概念。它的思路不是在全速度空间搜索而是只在当前速度附近、且能在安全距离内刹停的窗口内搜索。这样生成的轨迹才符合无人机的运动学约束。先设计速度采样% 当前速度 vx drone.vel(1); vy drone.vel(2); v norm([vx vy]); w atan2(vy, vx); % 当前航向角假设无人机是平动模型 % 动态窗口范围 maxAccel 2.0; % 最大线加速度 maxOmegaAccel 1.0; % 最大角加速度 dt 0.1; % 控制周期 v_min max(0, v - maxAccel * dt); v_max v maxAccel * dt; w_min w - maxOmegaAccel * dt; w_max w maxOmegaAccel * dt; % 采样步长 v_res 0.2; w_res 0.05; for vc v_min:v_res:v_max for wc w_min:w_res:w_max traj simulateTrajectory(drone.pos, [vc, wc], dt, horizon); score evaluateTrajectory(traj, goal, map, otherDrones); bestScore max(bestScore, score); bestVelocity [vc, wc]; end end这里比较重要的是评价函数。我一般会同时纳入四个项目标导引、静态障碍距离、自身速度、机间协同。function score evaluateTrajectory(traj, goal, obstacleDist, otherDronesScore) headingScore 1 - angleDiff(traj(end,:), goal) / pi; distScore min(obstacleDist) / (obstacleDist 0.1); velocityScore norm(traj(end,:)) / vMax; coopScore otherDronesScore; % 与邻近无人机距离相关的函数 score alpha * headingScore beta * distScore gamma * velocityScore delta * coopScore; end其中coopScore需要特殊处理。我一般先计算候选轨迹末端位置与其他无人机的预测位置之间的距离如果距离小于安全阈值coopScore设为很大的负值或直接给一个高度惩罚。这样可以保证即使其他无人机突然切入航路DWA也会主动避开。不过DWA的问题也在这一刻暴露了所有无人机都在基于预测位置避让时可能出现“两边都让”的僵持。比如两机接近时每个DWA都倾向于向右多绕半圈整体就会变成镜像对称的逃生路径。这时如果没有协同协商层系统就会震荡。所以DWA适合做“感知后的第一反应”但最后的决定权要交给协商层。3.4 机间防撞速度障碍法和优先级避让速度障碍法VO的思路很直观。你把其他无人机当成一个运动中的圆然后通过相对速度判断如果当前相对速度指向碰撞区域就说明保持当前速度会有碰撞风险。在Matlab里我习惯把其他无人机的位置、速度投影到当前决策无人机的速度空间找出那些会进入碰撞圆锥的相对速度集合。核心代码如下function collisionFlag checkVelocityObstacle(posA, velA, posB, velB, safetyDist) relPos posB - posA; relVel velB - velA; relSpeed dot(relPos, relVel) / norm(relPos); % 径向相对速度 lateralDist norm(relPos) - safetyDist; if relSpeed 0 lateralDist safetyDist % 相对速度分量顺着视线方向且距离小于安全阈值 collisionFlag true; else collisionFlag false; end end单用VO能检测冲突但不能解决“谁让谁”的问题。这时候优先级机制就派上用场了。我的策略是在每架无人机的状态里维护一个priority值主循环每步检查所有无人机两两之间的距离一旦小于协商触发距离就进入协商流程。协商流程简单可靠的做法是“高优先级保速低优先级让行”。高优先级无人机在协作评价函数里不增加机间避让惩罚低优先级无人机会受到一个额外的“远离高优先级机”的速度约束。这样双方决策不对称避让方向就唯一了。代码示意function newV resolveConflict(drone, other, priority) if priority(drone.id) priority(other.id) % 本机让行 awayDir - (other.pos - drone.pos) / norm(other.pos - drone.pos); newV drone.vMax * awayDir drone.vel * 0.2; % 偏向远离方向 else newV drone.vel; % 保持原速度方向 end end这个逻辑虽然简单但在我的仿真里非常稳定。你还可以在进入协商时加入超时机制如果低优先级无人机在一个方向让让停停超过N秒就切换避让方向防止卡进障碍角落。3.5 主循环与时间戳推进把所有模块串成主循环是整个仿真的骨架。我建议把主循环写成一个脚本或函数统一管理时间推进和状态更新。for t 0:dt:Tsim for i 1:N % 1. 更新无人机感知信息 obstacles getVisibleObstacles(obs, drone(i)); neighbors getNearestDrones(drones, i, negotiationDist); % 2. 检查是否需要重新做全局规划 if shouldReplan(drone(i), map, globalObstacles) drone(i).refPath astar_plan(map, drone(i).pos, drone(i).target, safeDist); end % 3. 执行DWA局部规划 [vBest, wBest] dwaPlan(drone(i), drone(i).refPath, obstacles, neighbors, dt); % 4. 协商层调整速度 [vFinal, wFinal] resolveConflict(drone(i), neighbors, priorities); % 5. 更新无人机状态 drone(i).vel [vFinal * cos(wFinal), vFinal * sin(wFinal), 0]; drone(i).pos drone(i).pos drone(i).vel * dt; end % 记录仿真数据 recordData(t); end注意时间戳推进很重要的一点是感知、规划、执行必须用同一个时间戳不能A无人机已经把速度换成t1时刻的值B无人机还在用t时刻的位置计算避让。这会直接影响碰撞判断的准确性。我的做法是先做一次“规划前快照”所有无人机在规划期间使用快照状态等全部规划完成后再统一更新位置。4. 参数调优与典型仿真场景分析4.1 关键参数的前期设置参数设置是这套框架最容易“调崩”的地方。我给了自己一组基准参数作为起点再按场景调整。参数基准值调整方向影响后果安全距离2~3米场景越拥堵或速度越高设定越大过小碰撞风险高过大策略保守协商触发距离5~8米根据通信时延调整过小来不及避让过大频繁触发DWA预测时域1~3秒速度越快时域越短过长计算慢且轨迹偏差大最大线速度10~20 m/s根据任务速度和动力学约束过大会限制避让能力评价权重ratioalpha0.4, beta0.3, gamma0.15, delta0.15目标性/避障性/机动性/协同性权重失衡导致路径漂移栅格分辨率0.5米依据地图大小和无人机尺寸过粗丢失障碍细节过细慢我这里要特别强调一下安全距离。它不只是“两架飞机半径相加”还要考虑定位误差、通信延迟、执行器响应延迟。当速度是20m/s、通信延迟0.2秒时制动距离可能已经超过2米。所以安全距离要设成零状态基线上再加一个速度相关项safeDist baseDist k_speed * norm(drone.vel);这个和DWA中“动态窗口必须能在安全距离内停下”是配套的。4.2 典型场景双机交叉和三机会聚我测试时跑得最多的两个场景一个是双机交叉一个是三机会聚。双机交叉场景里两架无人机从起点出发航线在中点附近形成一个交叉点并且高度层相同。这个场景最能暴露对称避让问题。关闭协商层时两机通常会在交叉点附近反复左右抖动路径轨迹像一个打结的麻绳。打开协商层后优先级高的直线通过优先级低的绕出一个平滑弧线进入交叉点两机最近距离稳定在安全阈值以上。三机会聚场景更有意思。三架无人机从不同方向飞向同一目标点。这种情况下目标点本身是同一坐标如果不做时间协同即使机间避障成功三架也会在目标点附近撞成一团。我的处理方式是在目标点外设立一个“等待圈”或“时间走廊”。无人机到达等待圈后先检查其他无人机是否已经到达。如果有更高优先级的机还没离开本机就在等待圈内做圆周运动等待。这个措施有效保证了任务集结的稳定性。4.3 动态障碍物下的重规划表现动态障碍物测试我选择了一个“突然横穿”的场景。无人机以10m/s巡航一个移动障碍从侧面以6m/s速度穿过航线。其实局部DWA能实时避开它但只靠DWA会出现一个很有意思的情况每次重新规划都会把参考路径拉回原来贴着障碍的路线导致下一时刻又需要绕开整个路径比较碎。解决这个问题的方法不是在DWA层面使劲而是把动态障碍物也纳入全局重规划的判断条件。我在shouldReplan函数里加入了一个条件如果动态障碍物的运动趋势与当前参考路径存在持续冲突并且这个冲突需要绕开超过N秒就触发A*重规划。这样全局路径会主动绕到动态障碍物运动轨迹的后面DWA的负担就小多了。实测下来这个组合在“突然出现的障碍物”上效果不错路径长度只比理想情况多10%左右但飞行稳定性明显更好。5. 常见问题与排查技巧实录5.1 对称避让导致的死锁与震荡这是多机协同里最容易出现的问题现象是两机面对面靠近时同时左转、同时右转导致无法稳定分离。排查思路很固定先检查协商层是否正常工作优先级是否真实传入DWA。如果优先级仍然对称就说明协商层失效了。我自己踩过的一个坑是协商层只修改了评价函数里的coopScore但alpha、beta这些权重在冲突瞬间权重太低把协同项盖过去了。后来我把协同项从“加权项”改成“条件约束项”一旦相对距离进入协商触发区低优先级机的候选速度直接排除“朝向冲突方向”的部分效果立竿见影。5.2 路径贴障碍、局部规划救不回来A*规划的路径贴着障碍边缘走DWA执行时稍微受到扰动就会蹭到障碍甚至陷入局部反复。这个问题的根子不在DWA而在代价地图膨胀半径不够。我处理时会把安全距离的一半作为膨胀半径并且在A*扩展邻居时额外检查“距障碍物不足安全距离的栅格视为不可走”。很多时候只要膨胀参数一改路径自然就离墙远了不少。5.3 多机系统出现“你等我我等你”的停顿优先级协商适得其反时会形成“你比我优先级高我等你下一帧你让了我又尝试前进结果又冲突”的循环。这本质上是协商结果没有时间一致性。我在协商层加了“让行确认窗口”低优先级无人机一旦决定让行至少在0.5秒内不改变让行方向。这个窗口能有效消除高频协商带来的抖动。如果窗口加完还是停顿一般是让行速度太慢或协商触发距离太小。可以适当增大协商触发距离给低优先级机更充裕的避让空间。5.4 仿真运行效率的根本优化思路Matlab跑三层框架无人机一多就会很慢。我建议先定位瓶颈通常都出现在DWA的速度采样和轨迹模拟上。优化手段有三个方向一是减少采样数量比如把速度分辨率和角速度分辨率适当调粗二是用向量化计算代替内层循环一次生成所有候选轨迹矩阵再统一评分三是让每个无人机的DWA只关注“感知半径内的障碍”别拿全图障碍参与评分。我在自己的实现里把DWA采样从双重for循环改成了meshgrid生成采样点后一次性计算轨迹性能提高了4倍左右。如果还要更快可以把A*重规划放到独立函数里只在需要时调用不要每个时间步都重新搜索。如果在调试中还遇到其他诡异现象比如某架无人机突然“画出”一个不可思议的大回环先别急着调算法打印轨迹和速度选点记录八成是某一行代码时间戳没对齐或者规划器被意外传入了自己上一帧的位置做初始状态。这类问题排查起来并不难但需要耐心。这套方案算是多无人机协同路径规划里工程味比较重的一种组合。我自己最大的体会是算法选型不是越先进越好而是要在“可复现、可调参、可解释”这三件事之间找平衡。Matlab仿真的真正作用不是把论文里的理论公式抄一遍而是让你亲眼看到哪些环节会卡死哪些参数会让系统变得又呆又慢。如果你也准备从零搭一套多机协同仿真建议先把单机版A*加DWA跑稳再加第二架无人机最后引入动态障碍物。一层一层往上叠遇到问题才不会被一堆变量淹没。