★ FEATURED ARTICLE 最近在做一个多无人机协同路径规划的项目踩了不少坑也沉淀了不少经验。这个方向市面上开源代码不少但真正能跑通动态环境、多机协同、防撞一整套流程的还是需要自己把逻辑捋清楚。我基于Matlab从零实现了一套动态环境下多无人机协同路径规划与防撞系统包含全局路径规划、局部动态避障、多机协同防撞和可视化仿真四大部分整体在离线仿真中达到了预期的规划精度和实时性目标。这套实现的价值在于它不是把几个现成的工具箱函数拼在一起而是把“协同”真正落到了规划层每架无人机不仅走自己的路还要在动态威胁下与其他无人机相互避让。对于正在学习无人机集群算法、需要完成课程设计或仿真验证的开发者这套代码提供了一个可以直接修改、扩展的工程框架按自己的场景换地图、换模型、加约束都能快速跑起来。1. 问题拆解与整体设计思路1.1 多无人机协同规划解决的是什么拆开“多无人机协同路径规划与防撞”这句话核心就三件事第一每架无人机要从起点到终点找到一条可行路径。这个“可行”包含两层意思一是不能碰到障碍物二是路径要符合无人机的飞行约束最小转弯半径、最大速度、飞行高度等。第二多架无人机之间不能互相碰撞。单机路径规划只需要对静态地图负责多机环境下每架飞机对别人来说都是一个移动障碍物而且这个障碍物本身的未来轨迹不确定因为它也在动态决策。第三在动态环境中原来的路径可能随时失效。比如环境中突然出现新的静态障碍或者其他飞机切入本机航线这时候需要局部重规划而不只是简单走全局路径。这三件事如果分开做每件都不难难的是同时做并且保证计算效率。我最初尝试把全局路径规划结果固定下来再靠底层速度控制强行避让结果发现全局路径频繁失效飞机经常绕大圈。后来换成分层架构情况才显著改善。1.2 动态环境带来的三个额外麻烦静态环境下做多机规划本质上是在几何空间里做“找路防撞”但一旦引入动态障碍三个麻烦会立刻出现第一个是时间维度。静态规划只需要回答“走哪条路”动态规划必须回答“什么时候在哪个位置”。路径规划空间从XY二维或XYZ三维扩展成XYT或XYZ-T四维搜索复杂度成指数级上升不能再简单地套静态A*或RRT去暴力搜索。第二个是预测不确定性。动态障碍物的运动状态只能通过一定时间窗口内观测来估计短期内可以假设匀速或匀加速度长期预测误差快速累积。规划算法必须把“预测的时间跨度”限制在可控范围内。第三个是计算实时性。多机协同下每架飞机的规划结果都会影响其他飞机的输入。如果规划耗时太长等算出来局势早就变了。所以算法必须在几十毫秒到几百毫秒内完成一次重规划这就逼着你在最优性和实时性之间做权衡。1.3 方案选型集中式还是分布式协同规划在架构上有两类主流思路集中式和分布式。集中式是有一个中枢节点统一接收所有飞机的位置、目标点、状态算出全部无冲突路径后统一下发。好处是全局最优性好冲突可以直接在规划阶段消解。坏处是计算量随飞机数量指数上升而且对通信依赖极高中枢一旦故障整个集群瘫痪。适合飞机数量少比如3-5架、通信条件好的场景。分布式则是每架飞机独立规划路径依靠局部感知和机间通信来协调避让比如优先级机制、协商协议。优点是扩展性好适合大规模集群缺点是需要精心设计协调策略否则会出现“死锁”——两架飞机互相让路让个没完。我在这个项目中综合考虑了实时性和扩展需求选择了集中规划与分布式避让结合的混合方案全局规划阶段用集中式协调生成带时间信息的初始路径局部防撞阶段用分布式速度障碍法实时避让。这样既能保留全局协调能力又能应对动态变化整体结构也更贴合实际工程中的“地面站机载自主”模式。2. 核心算法选型与技术原理2.1 全局路径规划栅格地图RRT*的选型逻辑全局规划层我用了基于栅格地图的RRT算法。选它的理由很朴素A在稠密栅格地图上虽然能给出较优路径但地图尺寸一大、维度一高内存占用和计算时间膨胀得厉害。RRT系列作为采样规划算法在高维空间里尤其灵活而且RRT*相比基础RRT增加了重连rewire步骤能渐进优化出接近最短的路径。具体做法是先把环境栅格化成二值地图再用map binaryOccupancyMap(width, height, resolution)建立占据栅格场。RRT*在这个栅格场里采样节点同时用stateSpaceDubins约束两点之间的连接曲线让规划出的路径天生满足最小转弯半径约束不至于产生折线型的不合理航线。不过RRT*有一个用户要特别注意的问题随机采样导致每次规划的路径都不完全一样在多机协同中路径初始化不稳定会带来额外的协调麻烦。我的处理办法是在目标点附近增加采样偏置同时规划完成后对路径做剪枝和插值平滑。剪枝能去掉冗余航点插值则保证路径足够密给下层控制器提供连续参考轨迹。2.2 局部动态避障速度障碍区的判定逻辑动态环境下最核心的局部避障算法我选的是速度障碍法也就是常说的VO。它处理“如何避开移动障碍物”的思路非常直观把所有移动障碍物在速度空间里“膨胀”成一个危险区域如果本机当前速度落入这个区域就意味着按这个速度飞下去迟早会撞上需要立刻寻找区域外的速度。举一个简化的计算例子。本机位置在A点速度Va一个动态障碍物在B点速度Vb。把相对速度Vr Va - Vb画出来再根据两机半径和相对位置判断相对速度是否指向碰撞圆锥内部。如果指向内部就把Vr朝圆锥边缘旋转一个安全裕度角度再换算回本机速度。在Matlab中构造一个几何函数collisionCone(self, obstacle, safetyMargin)输入两机状态输出当前速度是否在碰撞锥内以及候选逃逸速度方向。这个方法在工程上的最大优点是计算量极低而且不依赖障碍物的长期轨迹预测——只需要短时状态估计非常适合实时决策。但它的缺点也明显只考虑瞬时状态没有考虑本机动力学约束可能给出一个无人机物理上飞不出来的速度指令。所以在代码里我把VO的输出接入了一个低通滤波和加速度限幅环节模拟真实控制器的响应特性。2.3 协同机制优先级和时间窗怎么配合多机之间不只是避障碍物还要互相避让。速度障碍法处理动态障碍没问题但两架都在自主规划的飞机会出现“双向互让”的振荡。解决这个问题的常见思路是给每架飞机分配一个固定的优先级或者实时协商动态优先级。我采用的方法是固定编号优先级加时间窗约束。全局规划时先给每架飞机分配编号编号小的优先级高。规划器按优先级从高到低依次规划各机路径低优先级飞机在规划时把高优先级飞机的轨迹离散成一连串带时间的占位点作为动态障碍物输入。这样规划出来的所有路径天然无冲突。局部防撞层也用到优先级当VO检测到两机速度冲突时低优先级飞机主动避让高优先级飞机维持原速。这样虽然损失了一点对称性但换来的是决策结果的确定性在仿真中从未出现死锁或震荡。时间窗约束的作用则是保证路径的时间一致性。全局规划器输出每条航路点时会附带一个期望到达时间所有飞机按这个时间表飞行一旦某架飞机因为躲避动态障碍延误超过阈值就触发全区域重规划把时间基准重新对齐。3. Matlab代码实现与核心环节解析3.1 环境建模与无人机运动学模型环境建模这一步在所有规划算法之前它的质量直接决定后续规划结果是否可信。我使用Matlab Navigation Toolbox中的功能函数搭建占据栅格地图核心代码如下% 建立地图尺寸10km x 10km分辨率50m/格 width 10000; height 10000; resolution 50; map binaryOccupancyMap(width, height, resolution); % 手动添加静态障碍物矩形区域 obs1 [2000, 1500, 800, 600]; % [x, y, width, height] setOccupancy(map, [obs1(1):obs1(3):obs1(1)obs1(3); ... obs1(2):obs1(4):obs1(2)obs1(4)], 1); % 查看地图 show(map)这段代码里几个细节值得注意。resolution取50米意味着每个栅格代表50m×50m的区域分辨率过高会拖慢RRT的碰撞检测过低又会让小型障碍物的边缘失真。我试过10米和50米两档10米分辨率下RRT单次规划耗时接近一秒在动态环境下很难接受50米下基本在两百毫秒内可以完成对于仿真验证足够。无人机运动学模型我做了简化使用Dubins模型描述平面运动。它把飞机看作一个只能在二维平面内以固定最小转弯半径运动的质点路径由直线段和圆弧组成。这个模型简单但抓住了固定翼无人机路径规划的核心约束——最小转弯半径而且能直接配合MATLAB的stateSpaceDubins使用。% 创建Dubins状态空间设置最小转弯半径 ss stateSpaceDubins; ss.MinTurningRadius 500; % 单位米转弯半径的取值需要参考实际机型参数。我按一般小型固定翼无人机的飞行性能估算了500米如果你换用四旋翼平台可以直接把半径缩小到100米甚至更低代码不用改结构。3.2 协同规划主流程整体协同规划流程的骨架并不复杂核心是“按优先级依次规划轨迹离散化作为后续约束”的循环。我把它整理成如下的主循环伪代码% 输入地图map、各机起点starts、各机终点goals、无人机数量N % 输出每架飞机的航路点序列paths paths cell(1, N); for i 1:N % 1. 把已规划飞机的航路点按速度离散化为动态障碍轨迹 dynObs []; for j 1:i-1 traj discreteTrajectory(paths{j}, speeds(j), dt); dynObs [dynObs; traj]; end % 2. 构造状态验证器融合静态地图 动态轨迹占位 validator validatorDynamicObstacles(map, dynObs); % 3. 在规划空间里搜索无冲突路径 path planSingleUAV(ss, validator, starts{i}, goals{i}); % 4. 剪枝 平滑 paths{i} smoothPath(path); end第1步的discreteTrajectory是协同的关键。它把上一架飞机的路径按期望速度和固定时间步长离散成一系列“位置时刻”的集合。这些位置在后续规划中会被当作占据区域处理——某时刻某位置是占用状态路径规划器就必须绕开。第3步的planSingleUAV内部调用RRT搜索但验证器变成动态验证器。实现时我写了一个自定义的validatorDynamicObstacles类继承自nav.StateValidator在isStateValid方法里同时检查静态栅格占用和动态轨迹占位。这样RRT每扩展一个节点都会检查该节点对应的时刻是否与其他飞机轨迹有重叠。3.3 防撞逻辑实现距离检测与速度避让防撞逻辑在每一控制周期执行输入是当前所有飞机位置、速度以及地图信息输出是每架飞机的期望速度修正。核心是两个环节碰撞提前检测和速度避让计算。碰撞提前检测主要计算两架飞机的最短相遇时间。如果任意两架飞机在预测时间内距离小于安全半径就判定为有碰撞风险进入避让流程function [risk, ttc] checkCollisionRisk(pos1, vel1, pos2, vel2, safeDist) relPos pos1 - pos2; relVel vel1 - vel2; % 相对位置在相对速度方向上的投影 a dot(relVel, relVel); b 2 * dot(relPos, relVel); c dot(relPos, relPos) - safeDist^2; % 求解最小相遇时间 disc b^2 - 4*a*c; if disc 0 || a 0 risk false; ttc inf; return; end ttc max(0, (-b - sqrt(disc)) / (2*a)); risk ttc 10; % 10秒为预警门限 end这个函数的数学本质是求解二次方程判断相对运动轨迹是否进入以安全距离为半径的“保护圆”。代码里ttc 10的预警门限可以根据场景调整越小越激进越大越保守。我实测下来10秒门限对本次仿真中等密度场景比较合适超过15秒会出现过度避让飞机会走很多不必要的弯路。如果检测到风险低优先级飞机会触发速度障碍避让调整速度方向避开碰撞圆锥function newVel velocityObstacleAvoidance(posSelf, velSelf, posObs, velObs, maxSpeed) % 计算相对位置和相对速度 relPos posSelf - posObs; relVel velSelf - velObs; % 计算到碰撞圆锥边界的角度偏移 relDist norm(relPos); coneAngle asin(safeRadius / relDist); refAngle atan2(relVel(2), relVel(1)); avoidAngle refAngle coneAngle deg2rad(10); % 额外裕量10度 newDir [cos(avoidAngle), sin(avoidAngle)]; newVel newDir * maxSpeed; end这段代码是我实现中反复调试最多的部分。 deg2rad(10)这个额外裕量并不是来自理论推导而是仿真经验——如果没有这10度裕量避让轨迹会贴着碰撞圆锥边缘走一旦障碍物有微小非线性运动立刻又会重新触发避让造成频繁换向。3.4 仿真可视化与数据记录可视化看似是锦上添花实际上在协同规划开发中是刚需。没有逐帧动画你很难看出两架飞机到底是在“主动协同”还是“碰巧没撞上”。Matlab中我用figure plot加定时器驱动的方式把每一时刻所有飞机的位置、历史和目标点都画在同一张图上。核心结构如下% 初始化图形 figure; hold on; grid on; axis([0 10000 0 10000]); plotStaticObstacles(map); % 为每架飞机创建轨迹句柄 h_traj cell(1, N); for i 1:N h_traj{i} plot(nan, nan, -, LineWidth, 1.5); end % 每帧刷新飞机位置与轨迹 for t 1:numSteps % 更新每个飞机的真实位置由控制逻辑产生 for i 1:N set(h_traj{i}, XData, trajX{i}(1:t), YData, trajY{i}(1:t)); end drawnow; pause(0.05); end除了图形输出我把每一时刻各机位置、速度、碰撞检测结果都记录到结构体变量中仿真结束后统一导出方便后续量化分析“最小间距”、“平均绕行距离”等指标。这些指标在写实验报告或论文时非常重要千万别只留动画不留数据。4. 参数标定与实验效果分析4.1 各模块关键参数速查表参数标定是整个项目中花费时间最多的环节没有之一。我把关键参数整理为一张速查表方便后续调试时对照修改参数本实现取值调参方向影响栅格分辨率50 m降低则规划快升高则更精细规划耗时、边界精度最小转弯半径500 m按机型调整四旋翼可大幅缩小路径平滑度、可达性安全半径250 m增大则更保守减小则更激进航线经济性、安全性碰撞预警门限10 s增大则提前避让避让时机、绕行幅度速度避让裕量10°增大则更远离碰撞锥抖动频率、路径冗余优先级时间间隔2 s过大会脱离实际轨迹协同精度、规划效率这张表里最容易被忽略的是“优先级时间间隔”。它表示低优先级飞机规划时高优先级飞机轨迹被离散化时使用的时间粒度。2秒意味着每隔2秒采一个占位点点与点之间通过线性插值判断占用。间隔太大低优先级飞机会“钻空档”间隔太小计算量又大实时性变差。我测试了0.5秒到5秒的区间最终落在2秒是比较平衡的选择。4.2 实验场景与量化结果我用一个典型场景做了验证5架无人机起点分布在地图四周目标点是中心附近的5个不同位置环境中预先放置了3个静态障碍区另外设置了两条移动障碍轨迹模拟动态威胁。量化结果上5架飞机全部在规划时间内到达目标点全程任意两机间距保持在安全半径以上最小间距记录出现在第3架和第4架飞机交汇时为276米安全半径250米没有触发实际碰撞。整体路径长度相比单机最短路径总和增加了约18%动态避让造成的额外绕行在合理范围内。从实时性看全局协同规划总耗时约3.1秒每架飞机平均240毫秒完成路径搜索局部速度障碍避让单次计算均在毫秒级控制周期20Hz下完全跑得动。这说明混合架构的动态负载是可接受的具备实际嵌入式平台部署的潜力。4.3 与单机规划的对比思考把协同结果和单纯做5次单机规划对比能更清楚看到协同层到底付出了多少代价、换来了什么。单机独立规划时每架飞机都会选择对自己最短的路径但两条最短路径很可能会在某些区域交叉甚至重合。在我这个场景中5条独立最短路径在地图中心形成了一个明显的汇聚区域如果不加协同这个区域就是潜在的“碰撞雷区”。协同规划本质上是在每个交汇点附近增加时间错开。代价是多飞了18%的距离换来的是全程无冲突且不需要任何急停急转。这个“时间换空间”的思路在无人机集群里非常关键——硬碰硬的空间分离往往成本过高合理的时间错位是更经济的方案。5. 常见问题与调试避坑记录5.1 运行报警与解决方案对照实际调试过程中必然遇到一堆报错和异常现象我把最常见的几类整理成对照表现象根因解决方案RRT*规划超时栅格分辨率过高或动态障碍点过多降低分辨率、增大采样步长两架飞机在狭窄通道互锁双向避让导致死锁启用固定优先级低优先级等待路径在目标点附近振荡RRT*随机性导致终点连接不稳定目标点加偏置采样、规划后插值规划路径穿墙状态验证器只查节点没查连线使用isMotionValid检查整段运动两机间距刚好卡在安全半径附近防撞检测忽略了动力学响应延迟安全半径乘1.2的冗余系数最后一条是最容易被忽略的。理论检测的安全距离经常在真实动态仿真中被“突破”一点原因就是速度改变需要时间飞机会在加速阶段向前“滑”一段距离。这个“滑行距离”在小速度下不明显但在高速或大惯量场景中很危险。最简单有效的做法就是给安全半径加上一个冗余系数而不是追求理论上的紧致界。5.2 调试方法从单机到多机的分步推进多机协同系统的调试复杂度远大于单机如果直接把5架飞机一起怼进去出了问题根本没法定位。我的调试习惯是先分层次推进第一步单机静态环境调试。先关掉所有动态障碍和其他飞机只验证单机全局规划路径是否平滑、是否满足转弯半径约束。此阶段解决的是RRT*本身的问题。第二步单机动态环境调试。加入移动障碍物验证速度障碍法能否响应动态威胁。此阶段需要重点观察避让路径是否有抖动或反复横跳有就调避让角度裕量和预警门限。第三步双机协同调试。只保留两架飞机验证优先级机制、时间窗配合是否正常观察交汇时的避让行为。此阶段最容易发现问题因为两机交互模式简单一眼就能看出谁在让谁。第四步逐步扩展至5架。每加一架都观察是否引入新的冲突模式尤其关注狭窄通道、多机交汇等高难度场景。这套分层调试的方法在项目后期帮我节省了大量定位时间。不要嫌前期流程长越急越容易在后期被一个隐蔽问题拖住。5.3 实际执行过程中的几个易错点有几个细节我一定要专门提醒。第一个是动态障碍物轨迹的坐标系必须与地图坐标系完全一致这听起来是废话但我在代码里就犯过把局部坐标当成全局坐标用的低级错误结果飞机绕着空转。第二是RRT*的samples数量不能盲目加大采样数加倍虽然可能找到更优解但耗时增长更明显在动态场景里得不偿失。第三是固定优先级虽然稳定但要定期检查低优先级飞机是否长期处于避让状态如果多次仿真中某一架飞机总是绕行说明优先级分配不合理需要基于起点到终点的路径交叉度动态调整。6. 扩展方向与后续思路这套Matlab实现最大的优势是扩展性。如果后续要做三维环境只需要把状态空间换为stateSpaceSE3或基于Dubins的三维扩展同时在地图中增加高度维度栅格如果要做真实飞行器接入可以把当前的速度避让输出换成MAVLink指令云端协同保持离线规划全球路径下发给机载飞控。我还在尝试把局部避让层从几何方法换成模型预测控制用滚动时域优化同时考虑速度约束和加速度约束可能进一步降低多余绕行但对计算性能的要求会提高不少。另外把强化学习引入协同避让决策也是当前一个很有意思的方向不过就目前的工程落地而言基于几何和优化结合的方法稳定性和可解释性仍然是最强的。回看这个项目的全过程我最深的体会是多无人机协同路径规划的关键瓶颈从来不是单个算法不够聪明而是整套系统的环节太多、耦合太紧。把每个环节的参数都理解透并且建立一种分步调试、层层验证的工作习惯才是真正把代码跑好、把系统做稳的核心能力。这套思路你现在复制过去换成自己的地图和参数也能较快拿到可用的结果。
阅读完成 · 觉得有帮助？
