最近我在重构一套多无人机协同路径规划与防撞的MATLAB仿真代码场景设定在动态环境下障碍物会移动、目标点可能变化、多架飞机同时执行任务。这个项目表面上是“路径规划”真正跑起来之后才发现难点几乎全在“协同”和“防撞”这两个词上。如果你也在做无人机集群、无人车编队或者任何多智能体路径规划这篇文章应该能帮你少走不少弯路。我会把整个技术路线、防撞策略、代码模块和调参过程都拆开讲所有代码思路都可以直接参考捡起来就能改。先说结论单机静态路径规划只要给定地图和目标点一条无碰撞路径很容易找但一旦变成多机、动态环境问题规模立刻从“寻路”升级成“时序协调实时避障决策冲突解除”。MATLAB做原型验证非常合适因为算法迭代快、可视化方便又能方便地导出成C代码用于后续移植。下面我从问题定界、算法选型、防撞实现、代码拆解到调参踩坑完整分享一遍。1. 为什么动态环境下的协同路径规划比单机路径规划高一个难度等级1.1 静态任务居然不是“热身”而是“前提”很多人一开始都会从静态路径规划入手。A*、RRT、人工势场这类算法都有一个隐含前提障碍物位置在一段时间内不变任务目标点也不变。单机情况下只要从起点到终点找到一条几何上无碰撞的路径剩下的事情就是沿着路径走。但这个前提放到多机动态环境里一下就撑不住了。我最初的做法是“先全局规划再让飞机错开出发时间”。思路很简单给每架无人机单独规划一条静态路径然后通过时间偏移避免它们在交汇走廊里撞上。可是动态障碍物一出现这个方案的脆弱性就暴露了。障碍物在动意味着原来规划出的路径可能几十秒之后就被堵死目标点如果也变化整条路径的参考价值就更低。所以动态协同环境下路径规划不能是“离线算一次就完事”必须变成“规划—执行—再规划”的闭环。多机协同还引入了时间维度上的耦合一架飞机的路径决策会影响其他飞机的安全。因为飞机之间有交互A为了避让障碍物突然减速可能和B的距离就小于安全间隔了。于是每个控制周期都必须同时考虑障碍物状态和邻居飞机的状态这不是简单的“两个单机问题相加”。1.2 动态环境下绕不开的三类挑战我梳理了一下动态协同路径规划的核心难点集中在三块。第一块是环境感知的不确定性。动态障碍物往往来自传感器检测而传感器有更新频率和噪声障碍物的预测位置也不可能完全准确。这意味着算法不能只处理当前帧的静态快照还要预测未来一段时间内障碍物的运动趋势。第二块是无人机之间的碰撞避免。通信延迟、飞行控制延迟、编队中的相互挤压都会让“理论上安全”变成“实际危险”。尤其是多架飞机同时收到避障指令时如果各自随机转向很容易出现震荡甚至互相逼近。这里需要明确的协调机制比如优先级、速度一致性或者双向避让规则。第三块是计算实时性。路径规划从离线秒级变成在线毫秒级。地面站或者机载处理器必须在很短的时间内完成所有飞机的避障计算。如果算法复杂度太高飞得快的时候算出来的速度指令已经过期了那整个系统就是不稳定的。这也是很多demo能跑通但实际试飞就崩溃的原因。1.3 为什么选MATLAB做原型验证很多人问我为什么不用Python、C或者直接Simulink。我的看法是动态多机协同问题算法逻辑的验证比运行速度重要得多。MATLAB的矩阵运算和工具箱让数据处理非常简洁画图和动画输出几乎是零成本这对于快速观察避障效果很有帮助。尤其是人工势场、速度障碍法这类需要频繁操作向量、判断几何关系的算法MATLAB写起来非常接近数学表达式不容易出错。后续如果要上机MATLAB Coder或者Simulink模型生成C代码是成熟路径。我先在纯脚本里验证控制逻辑再改造出实时接口这个流程在我看来是效率最高的。当然Python也能做但涉及大规模矩阵运算和可视化时MATLAB确实更顺手。2. 建模与算法选型我为什么选了“人工势场速度障碍法一致性协议”2.1 环境建模连续坐标比栅格更适合动态场景动态环境下我建议直接用连续坐标建模而不是栅格。栅格地图虽然搜索方便但障碍物一旦移动每个控制周期都要更新占用情况计算量不小。更麻烦的是栅格表达不了速度信息。一个正在快速靠近的障碍物和一个静止障碍物在栅格上可能长得一样但这两种情况的避让策略完全不同。我的建模方式如下无人机看作二维平面上的质点叠加安全半径R_safe。多旋翼在高空定高巡航时这种简化是合理的低空复杂地形再单独考虑三维。动态障碍物表示为一个圆形区域同时携带速度和运动方向。每架无人机状态包括位置、速度、目标点、优先级编号。仿真时间用固定步长dt推进典型值0.1s。感知范围设置为R_sense只处理范围内障碍物和邻居控制周期内计算量可控。2.2 算法选型对比在定方案之前我认真对比了四类常见算法各有优劣。算法核心思想动态环境适用性主要缺点A* / D* Lite在图或栅格上搜索路径动态环境需要频繁重规划开销大路径不平滑实时性差RRT / RRT*随机采样扩展树高维空间有效但动态障碍处理成本高随机性强结果不稳定人工势场(APF)目标引力障碍斥力反应快适合局部实时避障局部极小点、震荡速度障碍法(VO)基于相对速度判断碰撞非常契合移动障碍物只解决速度选择不解决全局路径最后我选择了组合方案全局采用稀疏A*做参考路径低频重规划局部采用改进人工势场速度障碍法做实时防撞多机协调部分用一致性协议保持速度同步和间距。这个组合的好处是全局负责“往哪走”局部负责“怎么避开”一致性协议负责“多机不打架”。2.3 整体框架分层规划与控制实践中我把整个系统分成三层。第一层是全局路径层。每2秒左右用当前目标点和障碍物状态规划一条参考路径。因为规划的频率低可以采用比局部算法更复杂的搜索。第二层是局部避障层。每个控制周期都运行输入是参考路径的下一航点、周围的动态障碍物信息和邻居无人机状态输出是期望速度向量。第三层是协同修正层。把期望速度与邻居速度做一致性校正保证编队内不会出现“你左我右”的局势。这种分层结构的关键好处是可以独立调参。动态障碍物响应好不好主要调局部避障参数多机间距保持好不好主要调协同参数遇到长期堵塞通道则靠全局重规划绕过去。3. 防撞策略的工程实现从公式到MATLAB代码3.1 动态障碍物的速度障碍圆锥速度障碍法(Velocity Obstacle)是我用来处理移动障碍物防撞的核心。它的直觉可以这样理解如果我把自身速度减掉障碍物速度得到一个“相对速度”那么障碍物在我眼里就变成了静止的。我可以把障碍物膨胀成一个圆然后从我的位置向这个膨胀圆引两条切线两条切线围成的区域就是一个圆锥。只要相对速度的方向落在这个圆锥里按现在的速度走下去就会撞上。数学上设本机位置为p_i速度为v_i动态障碍物位置为p_o速度为v_o。定义相对速度v_rel v_i - v_o如果相对速度方向指向以障碍物圆心为中心、半径为R_vo R_safe R_obs的膨胀圆那么存在碰撞风险。判断的核心代码可以写成下面这样function [inRisk, ttc] checkVO(p_i, v_i, p_o, v_o, R_vo, tau) p_rel p_o - p_i; if norm(p_rel) R_vo inRisk true; ttc 0; return; end v_rel v_i - v_o; % 相对速度在连线方向上的分量 proj dot(v_rel, p_rel) / norm(p_rel); % 只要相对速度在未来 tau 秒内让两圆距离小于安全距离就认为有风险 if proj 0 inRisk false; ttc inf; return; end % 最近距离估算|p_rel v_rel * t| 最小值 t_hit dot(p_rel, v_rel) / dot(v_rel, v_rel); if t_hit tau inRisk false; ttc inf; return; end closestDist norm(p_rel v_rel * t_hit); if closestDist R_vo inRisk true; ttc t_hit; else inRisk false; ttc inf; end end注意这里的tau是一个关键参数它表示防撞算法只关心未来多少秒内的碰撞风险。tau太小避障会反应迟钝tau太大无人机会对远处的障碍物也提前绕行路径变得很绕。我后面调参时专门处理了这个量让它与相对速度自适应。具体做法在第五节会讲。3.2 加入“相对速度”的人工势场传统人工势场只考虑位置斥力F_rep k_rep * (1/d - 1/d0) * (1/d^2) * (p_i - p_o)/d其中d norm(p_i - p_o)d0是斥力作用范围。这个公式对静态障碍物很好用但对动态障碍物有个明显缺陷没有考虑障碍物的速度。如果一个障碍物正高速远离我即使距离很近其实不用避让反过来如果障碍物高速冲向我即使距离还远也需要提前反应。所以在改进版里我在斥力中加入了相对速度项F_rep_vel k_vel * saturate(v_rel_along) * (p_i - p_o) / d; function s saturate(x) s max(-1, min(1, x)); end这里的v_rel_along是相对速度在连线方向上的投影。当相对速度朝向本机时v_rel_along为负但斥力会增强当相对速度远离时斥力反而减弱甚至变成吸力。这样无人机就能区分“即将撞上”和“已经远离”两种情况避障动作更智能。同时我加了一个切向分力。只沿连线方向推无人机会径直后退很容易造成队形混乱。切向分力让无人机绕向障碍物的侧面既完成避让又不偏离目标方向。切向大小一般设为斥力的0.3到0.5倍具体看飞行器的转弯灵活性。3.3 多机协同避让优先级与一致性协议处理完“无人机对障碍物”的防撞还要处理“无人机对无人机”。无人机之间的防撞不能每架飞机都执行同样的转向策略否则两架飞机可能同时向左或同时向右最后还是撞在一起。我的做法是引入优先级机制。每架无人机在任务开始时分配一个IDID小的优先级高。当两架飞机距离小于通信半径时只让优先级低的那一架执行主动避让优先级高的保持原有期望速度。这样决策唯一不产生歧义。优先级不是固定的也可以根据任务阶段动态调整比如执行紧急任务的飞机临时提升优先级。除了优先级我还加了一层一致性协议。一致性协议的思想是让每架飞机的速度状态向邻居飞机的平均速度靠拢同时保持安全间距。可以理解为编队飞行中的“默契配合”。function v_consensus updateConsensus(i, state, adjMatrix, v_des, k_p) neighbors find(adjMatrix(i, :) 0); v_base v_des; for j neighbors p_diff state(j).pos - state(i).pos; d norm(p_diff); if d R_comm d R_safe % 速度一致性项 v_base v_base k_p * (state(j).vel - state(i).vel); % 间距保持项距离过近时加入排斥速度 if d R_safe * 1.5 v_base v_base 0.1 * (1/d - 1/(R_safe*1.5)) * p_diff / d; end end end v_consensus v_base; end这里有个细节一致性项不宜加得太强。如果k_p太大所有飞机都会被“拉”成一个整体导致单机避障动作被抵消。我通常把一致性增益控制在避障增益的十分之一以下。4. 核心代码模块拆解完整跑通一版多机协同仿真4.1 主循环框架先搞清楚每个周期要做什么仿真主程序的核心是时间步进。下面是简化后的主循环框架% 初始化 dt 0.1; T_max 60; N 4; state initDrones(N, starts, targets); obstacles initDynamicObstacles(...); adjMatrix buildAdjacency(state, R_comm); timeStep 1; for t 0:dt:T_max % 更新动态障碍物位置和目标点 obstacles updateObstacles(obstacles, t); targets updateTargets(targets, t); for i 1:N % 1. 获取全局参考航点 wp globalPlanner(state(i), targets(i), obstacles); % 2. 局部期望速度 v_des computeDesiredVelocity(state(i), wp); % 3. 障碍物防撞修正 v_avoid avoidObstacles(state(i), obstacles); % 4. 多机协同修正 v_final updateConsensus(i, state, adjMatrix, v_des v_avoid, k_p); % 5. 限制速度和加速度 [v_final] limitSpeedAccel(v_final, v_max, a_max, dt); % 更新位置 state(i).vel v_final; state(i).pos state(i).pos v_final * dt; end % 检测碰撞和任务完成 checkCollisions(state, obstacles); if allTargetsReached(state, targets) break; end % 绘图 plotFrame(state, obstacles, t); pause(0.01); end这个框架的好处是模块清晰。每个函数只做一件事换算法也方便。比如把avoidObstacles从人工势场换成模型预测控制主循环完全不用大改。4.2 全局规划模块不能太笨也不能太重全局规划我用的是稀疏A*但只作为“灯塔”航点生成器。和传统A*不同的是我不往栅格地图上探路而是直接在连续空间里采样若干候选航点再搜索出一条参考路径。采样密度不需要大因为局部避障会处理细节。全局规划每两秒重跑一次如果当前路径被动态障碍物完全封堵就触发重规划。我遇到的坑是全局规划太频繁会导致路径跳变无人机来回大幅转向。解决方法是只有当新路径比当前路径长度减少超过10%或者当前路径被障碍物阻挡时才接受重规划。这个阈值让路径保持稳定又不会放过真正的堵塞。4.3 防撞检测函数多障碍物优先级的处理之前的checkVO只判断单个障碍物。实际仿真中一个无人机周围可能有多个动态障碍物避让速度需要综合计算。我采用的策略是找出风险最高的那个障碍物针对它计算避让速度然后用这个候选速度去检查其他障碍物是否也安全如果不安全再迭代。function v_avoid avoidObstacles(state_i, obstacles) v_avoid [0 0]; riskList []; for o 1:length(obstacles) [inRisk, ttc] checkVO(state_i.pos, state_i.vel, ... obstacles(o).pos, obstacles(o).vel, ... R_safe obstacles(o).radius, tau); if inRisk riskList(end1) o; %#okAGROW end end if isempty(riskList) return; end % 选择 ttc 最小的障碍物作为最紧急障碍 [~, idx] min(arrayfun((o) getTTC(state_i, obstacles(o)), riskList)); obs obstacles(riskList(idx)); % 计算避让速度方向垂直于相对位置方向并偏向远离方向 p_rel state_i.pos - obs.pos; d norm(p_rel); tangent [-p_rel(2), p_rel(1)] / d; away p_rel / d; v_avoid state_i.vel 0.5 * away 0.5 * tangent; end这段代码比较朴实地展示了避让方向生成思路。实际项目中会把避让速度限制到最大速度范围内并且加平滑滤波防止指令突变。4.4 可视化与数据记录看得见才能调得准MATLAB做仿真的一大优势是可视化。我用quiver画速度箭头用viscircles画安全圆和障碍物圆用plot画历史轨迹。每帧动画再加上飞行时间能够直观看到避让是否平滑。除了画面我还记录三个关键指标最小安全距离、任务完成时间、碰撞次数。最小安全距离用于评估防撞策略的保守程度任务完成时间用于评估算法是否过于绕路碰撞次数则是底线指标。调参时我习惯盯着这三个指标看而不是只看画面“感觉还行”。画面能发现问题数据能验证改善。5. 仿真实验与调参心得跑通之后踩过的几个坑5.1 参数调节的优先级先保证不撞再追求效率参数太多时最忌讳一把抓。我的经验是按优先级顺序来调。参数作用调节顺序感知半径R_sense决定能看到多远先设为最大避免漏障安全半径R_safe防撞底线第一优先保证时间窗口tau预测未来风险第二个调斥力增益k_rep避障力度第三个调速度增益k_vel动态响应灵敏度第四个调一致性增益k_p编队协调强度最后调有一次实验我把安全半径设得偏小想看看能不能提升飞行效率。结果两架无人机在交叉点几乎擦着飞过去最小距离只有0.2米虽然没“碰撞”但一点余量都没有。后来我把安全半径从0.8米调到1.2米代价是任务完成时间增加了8%但最小距离提升到0.6米以上。这个交换是非常值得的。5.2 局部极小点与多机震荡问题改进APFVO组合跑通后最让人头疼的是狭窄通道里的阻塞。两架无人机相对飞行谁都让谁最后停在原地形成死锁。这是经典的人工势场局部极小点问题在动态多机环境下的扩展版。我的解决思路有三步。第一检测停滞如果某个无人机连续2秒速度模长小于0.1m/s就判定停滞。第二停滞时引入临时扰动让无人机沿垂直于目标方向的方向偏移一段距离打破对称。第三如果扰动后仍无法脱离就触发全局重规划从另一个方向绕行。这套机制在实际仿真中几乎消除了死锁。多机震荡则是我用优先级机制解决的。原来所有飞机遇到邻居都执行相互避让结果两机在走廊里互相推搡轨迹像蛇一样。改成低优先级让高优先级之后轨迹立刻平滑下来。5.3 时间窗口tau与障碍物速度的自适应我最初把tau固定为3秒效果很差。障碍物慢速移动时3秒时间窗口根本没有风险无人机却提前绕行。障碍物高速冲过来时3秒窗口又显得太短避让动作晚了半拍。后来我改成了自适应公式tau clamp( R_safe / max(0.1, v_rel_proj), tau_min, tau_max )其中v_rel_proj是相对速度在障碍物连线上的投影。相对速度越大预测时间窗口越短相对速度越小窗口越长但也不会长到绕远路。tau_min我设为0.5秒tau_max设为4秒。这个调整让避让动作的“提前量”和“范围”都合理了很多。5.4 计算效率向量化之后才敢说实时最开始我写成嵌套循环每架无人机遍历所有障碍物里面还做很多norm和if。仿真1000步要几十秒控制周期根本扛不住。我把坐标和速度存成矩阵一次计算所有相对距离和相对速度。比如pos_all是N×2矩阵用pdist2批量计算距离再用向量化逻辑判断风险。优化后单步计算时间降了一个数量级。对于更大规模的集群MATLAB毕竟不是为实时控制设计的但做地面站仿真已经足够了。6. 后续扩展与个人体会6.1 从二维到三维从质点到真实动力学目前这套仿真是二维平面内的质点模型。真实无人机飞行还有高度维度有最大爬升率和转弯半径。我在后续版本里加入了高度层概念只在水平面做避障垂直方向用一个简单PID保持高度。如果要做更真实的避障可以把三维空间的速度障碍扩展成球体圆锥复杂度会上去但基本思路一致。动力学约束也很重要。质点模型可以瞬间转向真实飞机不行。我后来加了最大加速度限制和速率限制避让路径立刻变得更像真实行迹。在算法调试初期不建议加太多动力学约束否则很难判断问题是算法还是动力学引起的。6.2 分布式与集中式的取舍我在仿真里用的是半分布式策略每架飞机计算自己的避让速度但协作时共享位置和速度。严格意义上这不是纯分布式因为还存在优先级这种全局信息。如果要完全分布式可以用市场协商或一致性迭代让每架无人机在本地达成速度分配。代价是通信次数增加实时性会受影响。现在我更关注嵌入式验证。下一步打算把这套MATLAB算法通过代码生成移植到某低成本自驾仪上加入通信延迟和丢包模型在室内简单场景做小规模试飞。仿真和真实环境的差距只有飞起来才知道。最后分享一点个人体会这类多无人机协同项目最容易卡住的往往不是某个算法的数学推导而是“安全边界”的定义。你觉得安全半径1米够了吗动态障碍物预测误差0.5米行不行这些问题没有标准答案只能通过一次次仿真和真机试飞去逼近。所以无论算法论文写得多么漂亮落地时一定要先把仿真环境和评价指标做扎实否则后面每一步都在空中楼阁上盖房子。