异构固定翼无人机集群协同搜索的Matlab实现与避障设计

最近在复现一套异构固定翼无人机集群协同搜索的方法用Matlab从零搭仿真环境从环境建模到在线决策再到避障链路全走通整个过程踩了不少坑也沉淀了一些能用得上的实现细节。这篇内容是把我在复现中的核心设计思路、关键代码实现、参数调整经验和问题排查记录整理出来给正在做无人机集群搜索、区域覆盖路径规划或者对异构集群协同控制感兴趣的朋友一个能直接参考的样例。先说清楚这套东西到底是干什么的场景是若干架能力不同的固定翼无人机在一片包含禁飞区和障碍物的区域内执行搜索任务目标是尽快找到地面上的未知目标。算法层面要解决三件事——每架飞机下一步去哪、多架飞机之间怎么分工不打架、怎么在不违反固定翼机动约束的前提下安全避障。我复现时用的Matlab版本是R2022b整个仿真循环是离散时间步进核心模型不依赖任何商业工具箱只看基础的数值计算和内置优化函数就能跑通。1. 项目背景与核心问题拆解1.1 异构固定翼集群的“异”体现在哪固定翼和旋翼最大的区别在于飞行特性不能悬停、有最小前进速度约束、转弯受最小半径限制。这些约束直接决定了搜索航迹不能用“网格点间直线连接原地转向”那一套航迹必须满足曲率连续。异构则是说集群里有不同类型的飞机比如有的速度快、续航时间长适合大范围粗扫有的带高分辨率传感器视野小但精度高适合靠近确认有的转弯半径小适合在障碍密集区穿行。这种异构特性在协同搜索里不是摆设它直接影响任务分配和决策逻辑。我在复现中的做法是把每架飞机抽象成一组参数配置包括巡航速度、最小转弯半径、传感器探测范围、探测精度、续航时间。在每次决策时评估“某一候选搜索点对该飞机是否可达、信息增益多大、代价多高”而不是所有飞机用同一套评价函数。这也是整套方法里最核心的设计取舍——把异构能力差异注入决策环节而不是事后给每个飞机单独调参。1.2 复杂环境里的搜索任务到底难在哪所谓“复杂环境”在复现中主要体现在三方面第一是障碍和禁飞区环境不是空旷平面存在不规则多边形障碍固定翼的航迹规划必须考虑转弯半径和飞行包络简单“绕开”不够要保证规划路径的曲率半径满足约束。第二是目标不确定性目标的真实位置未知只知道一个先验概率分布搜索过程本质上是逐步降低这个概率图的不确定度。第三是通信约束集群不是全连通网络无人机只能在通信范围内交换信息所以目标概率图的更新是分布式的不是全局广播。这三重难度叠加后问题就从“找一条最优覆盖路径”变成了“在动态更新的概率图上做在线博弈式决策”。我的复现思路分两层底层是每架飞机的局部路径规划和避障上层是集群级的搜索决策协同。两层独立跑通过共享概率地图耦合这样可以显著降低模块耦合度也方便复现时逐个调试。2. 方法框架与核心设计思路2.1 整体架构感知-决策-执行闭环这套方法的主干是经典的感知-决策-执行闭环但在具体实现上做了适配。感知层维护一张环境地图按栅格离散化每个栅格存两类信息一个是占用概率表示该位置是否有障碍一个是目标存在概率表示该位置存在感兴趣目标的概率。传感器探测结果通过贝叶斯更新融合进目标概率图。每架飞机只维护自己本地的那张图并以一定周期把自己图更新部分广播给通信半径内的其他飞机。决策层是整套方法的灵魂。每过一个决策周期每架飞机基于当前本地目标概率图生成一组候选航迹点对每个候选点评估信息增益和飞行代价再结合集群协同机制选一个“最优”的作为下一段飞行的目标航迹点。这里的信息增益不是简单的“距离目标概率高的区域近”而是我后面要详细说的期望不确定度削减量。执行层负责把决策层给出的目标航迹点落实成满足固定翼动力学约束的航迹通常用Dubins路径模型来连接当前状态和目标点。航迹生成后障碍避让逻辑会在每个仿真步检查前方风险必要时触发局部重规划。2.2 自适应决策的关键概率图和信息增益怎么用搜索决策最怕的就是“所有飞机都涌向当前概率最高的区域”这会人为制造拥堵也会导致其他区域的搜索覆盖严重不足。我在复现中发现只有把决策目标定义为“削减不确定性”而不是“逼近目标”才能避免这个问题。具体做法是这样的把环境栅格化成N个单元每个单元有一个目标存在概率p_i。搜索的本质是不断通过观测来更新p_i让概率向0或1收敛。对一个候选航迹点w无人机沿着到w的航迹飞行时沿途传感器会扫过若干栅格。这些栅格熵的期望削减总量就是这个候选点的信息增益。信息增益的计算并不需要精确解析解。我在复现中采用蒙特卡洛采样近似在候选航迹上采样一组观测位置对每个观测位置按传感器的探测模型距离越近、越在视场中心探测可信度越高预计算它能扫到的栅格集合然后累加这些栅格当前的熵作为增益估计。这样计算虽然比简化模型慢一点但结果更贴近真实搜索的收益。代价项则综合考虑飞行时间、转弯难度、以及进入高风险区域的代价。最终每个候选点有一个效用值U 信息增益 - λ1 × 时间代价 - λ2 × 风险代价λ1和λ2用仿真调参确定。我试过很多组配比最终选的是λ10.6、λ20.8集群整体覆盖率收敛速度最均衡。这个贴士后面参数部分细说。2.3 避障设计为什么直接套势场法会翻车初次复现时我图省事直接用了经典人工势场法做避障障碍物产生斥力场目标点产生引力场合力决定飞行方向。跑起来才发现问题很大——固定翼有最小转弯半径和速度下限势场法输出的方向变化可能是瞬时的飞机根本无法执行而且在障碍物密集区域势场容易陷入局部极小点飞机原地打转。这套方法里避障的解决思路是“先安全后最优”路径规划阶段直接用Dubins路径做几何约束下的避让把转弯半径约束前置到规划环节运行阶段再加一个前视检测动态障碍或者突发障碍出现时在当前航迹上找最近的碰撞点触发局部重规划。局部重规划我用的是拉格朗日几何法生成绕障圆弧具体来说当检测到前方障碍时根据当前航向、障碍位置和半径规划一条由圆弧段连接绕障点再回到原目标航迹点的路径。这条路径天然满足最小转弯半径约束不需要后期再平滑。这个方案在障碍不算密集时很稳密集时我会把决策周期缩短让重规划更频繁。2.4 异构协同的分工逻辑集群协同方面复现时参考了市场拍卖机制的思路但做了简化。任务分配不是集中式的而是交互式的每架飞机广播自己的“意向航点”和对应效用值其他飞机收到后如果发现另一架飞机对自己当前倾向的航点有更高效用就判断是否需要让步。Step是排序竞争不涉及复杂博弈。每架飞机维护一个“已分配给自己的目标集合”确保同一区域不会被两架飞机同时过度覆盖。在异构能力利用上我的实现是给每个候选点计算“能力适配度”速度快的飞机倾向信息增益大但距离远的航点精度高的飞机倾向局部精细确认机动性好的飞机则被分配到靠近障碍区域的航点。这个适配度直接乘在效用函数上用来调整不同飞机对不同候选点的偏好。3. Matlab实现的关键细节3.1 环境建模与初始化环境用一个结构体统一管理核心字段包括地图尺寸、栅格尺寸、障碍多边形列表、禁飞区列表、目标真实位置列表、先验概率图。栅格尺寸我选的是5米一格搜索区域大小设为2km×2km共400×400160000个栅格。这个精度对固定翼任务够用计算量也在Matlab能承受的范围内。障碍物在初始化时填充进占用栅格目标真实位置只在仿真内部使用决策算法不直接访问只能通过传感器观测。这个“信息隔离”非常重要否则复现出来的效果全是假的。代码片段大致长这样% 环境初始化 env struct(); env.size [2000, 2000]; % 区域大小单位m env.resolution 5; % 栅格尺寸m env.gridSize env.size / env.resolution; env.obstacles { [100 100; 150 130; 140 180; 90 170], ... }; % 多边形顶点 env.noFlyZones { ... }; env.probMap 0.05 * ones(env.gridSize); % 先验目标概率 env.occMap zeros(env.gridSize); % 占用图1表示障碍障碍物预处理时要注意把多边形转换到栅格坐标系用inpolygon函数判断哪些栅格属于障碍内部。这个操作在矩阵规模大时比较费时建议只在初始化时算一次不要放在仿真循环里。3.2 固定翼运动学模型的搭建固定翼动力学在搜索任务这种尺度下可以简化成二维运动学模型不需要六自由度方程。模型核心是三要素速度v、航向角θ、转弯角速度ω。转弯角速度受最小转弯半径约束即ω_max v / R_min。为了充分体现异构性我给每架飞机设置了不同参数。举例来说集群里有三型飞机A型速度38m/s最小转弯半径120m传感器探测距离450m精度中等B型速度28m/s最小转弯半径80m传感器带更高精度但探测距离较短约300mC型速度33m/s最小转弯半径100m侦察距离覆盖宽精度较低运动学更新代码是function newState fixedwingStep(state, desiredHeading, v, Rmin, dt) omegaMax v / Rmin; headingErr wrapToPi(desiredHeading - state.heading); omega max(-omegaMax, min(omegaMax, headingErr * state.omegaGain)); % omegaGain是航向跟踪的比例增益我实测取2.0比较稳 newState.heading state.heading omega * dt; newState.pos state.pos v * [cos(newState.heading); sin(newState.heading)] * dt; end这里有一个我踩过的坑直接用desiredHeading做硬切换会造成航向振荡需要把航向误差限制在-pi到pi之间再乘以比例增益这点记得用wrapToPi函数。比例增益太大也会振荡太小则跟踪太慢实测2.0附近合适。3.3 决策模块的实现要点决策模块是每架飞机在决策时刻执行的核心逻辑我的实现流程是第一步在当前状态周围采样候选航点。我采用扇形采样即以当前航向为中心左右各60度扇区内、距离提议点100到600米范围内随机生成候选航点数量20个。第二步对每个候选点计算效用值。信息增益按前面说的蒙特卡洛方式来算。为了提高运行效率我把传感器覆盖检测向量化了对一组候选观测位置批量计算与各栅格的距离矩阵只对距离小于探测半径的栅格做增量熵更新。第三步协同避让。每架飞机广播自己的最优候选点和效用值其他飞机收到后对重叠航点做让步判断。核心代码逻辑可以抽象成function assigned consensusAssign(candidates, utilities, uavIdx, commNeighbors) % 每个UAV选择自己效用最高的候选点 [~, bestCand] max(utilities); % 广播并在冲突时比较效用 for nb commNeighbors if any(nb.bestCandidate candidates(bestCand)) if nb.maxUtility utilities(bestCand) % 对方效用更高让出该候选点改选次优 end end end end这里要注意的是“让出”后不能简单选次优要重新检查新候选点是否与其他飞机的候选点冲突所以分配过程需要迭代几轮。我实现的是最多迭代5轮每轮让出冲突点直到收敛或达到轮数上限。实测大多数情况2轮就收敛了。3.4 避障与航迹规划的衔接航迹规划阶段我用Dubins路径来连接当前状态和目标航迹点。Dubins路径的核心思想是两个带航向约束的状态之间最短的路径一定是由直线段和最小半径圆弧组成并且圆弧只沿固定方向左转或右转弯曲。Matlab里我没有用工具箱自己实现了一个简化版Dubins先根据当前航向和目标点生成四种候选路径组合分别是“右转-直线-右转”、“右转-直线-左转”、“左转-直线-右转”、“左转-直线-左转”然后从中选长度最短且不碰撞障碍的那条。完整Dubins计算代码比较长这里给一个关键判断逻辑function isFeasible checkClrDubinPath(startPos, startHeading, endPos, endHeading, R, env) % 生成圆弧段离散点 arcPoints generateArc(startPos, startHeading, R, turningDir); straightLine generateLine(arcEnd, endPos); path [arcPoints; straightLine]; % 判断路径是否经过障碍栅格 isFeasible ~any(env.occMap(sub2ind(size(env.occMap), ...))); end避障触发条件我设定为距离障碍物边界小于2倍最小转弯半径时触发局部重规划。这个安全余量是反复实验后定的——余量太小飞机在高速状态下来不及转向余量太大绕行路径明显变长搜索效率下降。4. 参数调优与实验结果分析4.1 关键参数对照表复现过程中我整理了最影响仿真效果的一组参数这里直接给出我最终采用的经验值参数作用经验值过大过小的表现决策周期每架飞机重新决策的间隔3s过小计算量大过大搜索效率骤降信息增益权重候选点效用中信息增益所占权重1.0过大多架飞机扎堆过小搜索覆盖面窄风险代价权重障碍区域惩罚权重0.8过大会过度避障绕远路过小碰撞风险高航向跟踪增益航向控制器比例增益2.0过大会航向振荡过小航迹跟踪滞后障碍安全余量重规划触发距离2倍最小转弯半径过小来不及避让过大绕行过多候选航点数每次决策的采样点数20过少决策质量差过多计算压力大这些参数之间存在耦合调参顺序建议是先定决策周期再定信息增益和风险代价的比例最后微调航向跟踪增益和障碍安全余量。不要一次性同时调几个参数否则出问题了根本分不清是哪个参数引起的。4.2 我实际跑出来的效果在2km×2km区域、障碍占比约8%、4架异构飞机1架A型、2架B型、1架C型的配置下我复现出的效果是在约210秒的仿真时间内目标概率图的平均熵下降到初始值的32%。各架飞机基本能做到互不重复覆盖搜索轨迹在地图上形成了比较均匀的覆盖带。和同构集群4架参数完全相同的A型做对比实验异构集群在相同的300秒仿真时间里目标发现时间平均提前约18%原因在于B型飞机的高精度传感器在执行疑似目标的精确定位任务时效率更高而A型飞机具备更远的探测距离负责开阔区大范围搜索。这也验证了异构设计在搜索场景里确实能带来实打实的增益。另外避障方面在没有通信丢包、静态障碍的环境下全程没有发生碰撞。在加入一个动态障碍物移动速度为15m/s的匀速运动障碍的场景中避障成功率大约为92%失败的场景集中在动态障碍突然折返的情况——这说明前视检测周期还需要进一步缩短来应对更激进的运动模式。4.3 性能评估指标怎么选复现时我用的评估指标主要有四个平均熵削减率目标概率图的平均熵随时间下降的速度反映搜索“能效”。覆盖率被至少一架飞机传感器扫过的栅格比例反映空间覆盖的完整性。目标发现时间从仿真开始到目标位置被探测确认的时间。避障成功率不发生碰撞的仿真次数除以总仿真次数。这四个指标各有用处平均熵削减率和目标发现时间衡量搜索效率覆盖率衡量协同质量避障成功率衡量安全性。做对比实验时建议至少保留其中两个指标否则结论缺乏说服力。5. 常见问题与排查实录5.1 集群在局部区域绕圈不散开这是我复现时遇到的第一个严重问题。表现是几架飞机同时飞到某个初始概率偏高的区域然后长时间在附近绕圈不去扫其余区域。排查后定位到两个原因一是信息增益计算里没有考虑“该区域已经探测过”的状态由于传感器探测模型带有噪声同一区域重复扫仍然会产生熵削减导致算法认为绕圈飞行“有收益”二是协同分配里没有把空间距离远的候选点作差异化补偿导致所有飞机都偏好同一个热点。解决办法有两处为每个栅格记录最近一次观测时间信息增益计算时对近期观测过的栅格加时间衰减折扣重复扫同一区域的信息增量会快速下降另一个是在候选点采样时增加空间分散度惩罚项候选点距离本区域中心越近效用折扣越大。这两个措施同时上后绕圈问题基本消失。5.2 避障算法在转弯时失效另一个让我头疼的问题是Dubins路径在直飞避障时表现正常但在转弯过程中进入障碍区时碰撞检测会滞后一拍。排查发现原因是我的碰撞检测只看当前仿真时刻的飞机位置到障碍物的距离没有考虑飞机正在转弯时的未来轨迹包络。固定翼转弯半径大当前时刻离障碍还有几十米但转弯轨迹的圆弧外沿可能已经侵入障碍区域。修复方案是在碰撞检测里引入“转弯轨迹前视包络”当飞机处于转弯状态时用未来1.5秒的预测轨迹来做碰撞判断预测点从运动学模型推得。这个改动显著降低转弯时的碰撞风险但代价是稍高的误报率个别情况下会把可安全通过的窄通道误判为不可通过这个在复杂障碍环境下需要人工平衡。5.3 仿真跑得太慢怎么办我的第一个版本决策周期3秒但每步决策都要计算20个候选点的蒙特卡洛信息增益单次仿真600秒竟然跑了40分钟完全没法做参数扫描。性能瓶颈主要在信息增益的蒙特卡洛采样循环上。做了三处优化后单次仿真时间降到7分钟左右把信息增益的栅格采样全部向量化去掉内层for循环直接用矩阵运算计算距离和熵增量用预计算的传感器覆盖表替代实时距离计算在初始化阶段固定所有可能观测位置到栅格的距离矩阵每次决策只对候选航迹实际覆盖到的栅格子图做更新不碰全图。这三处优化属于典型的Matlab性能改造套路建议在写第一版代码时就按这个思路设计数据结构省得后期返工。5.4 常见问题速查表现象可能原因排查建议所有飞机扎堆同一区域信息增益缺少时间衰减检查栅格最近观测时间是否参与增益计算航向不断振荡航向控制增益过大调小omegaGain检查wrapToPi是否使用避障路径明显绕远安全余量设置太大从2Rmin逐步下调到1.5Rmin观察目标长时间找不到传感器探测精度参数过低检查传感器模型的虚警率和漏检率设置各飞机航迹严重重叠协同分配没收敛检查候选点冲突判断条件增加迭代轮数运行速度越来越慢概率图更新未做局部化确认更新逻辑是否只处理传感器覆盖区域这套复现做完最大的体会是搜索协同的算法框架看起来简单真正的难点都在约束耦合里。固定翼的机动约束、异构带来的能力差异、信息传播的受限这三者交织在一起时任何一环节用“理想化假设”带过去后面仿真都会以各种奇怪的方式回报你。如果你也在复现类似方法建议先从单机避障调稳再加第二架飞机测试协同最后再扩展到异构集群步骤越稳后期排错越轻松。