异构固定翼无人机集群协同搜索的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用仿真调参确定。我试过很多组配比，最终选的是λ1=0.6、λ2=0.8，集群整体覆盖率收敛速度最均衡。这个贴士后面参数部分细说。

2.3 避障设计：为什么直接套势场法会翻车

初次复现时，我图省事直接用了经典人工势场法做避障：障碍物产生斥力场，目标点产生引力场，合力决定飞行方向。跑起来才发现问题很大——固定翼有最小转弯半径和速度下限，势场法输出的方向变化可能是瞬时的，飞机根本无法执行；而且在障碍物密集区域，势场容易陷入局部极小点，飞机原地打转。

这套方法里避障的解决思路是“先安全后最优”：路径规划阶段直接用Dubins路径做几何约束下的避让，把转弯半径约束前置到规划环节；运行阶段再加一个前视检测，动态障碍或者突发障碍出现时，在当前航迹上找最近的碰撞点，触发局部重规划。

局部重规划我用的是拉格朗日几何法生成绕障圆弧，具体来说：当检测到前方障碍时，根据当前航向、障碍位置和半径，规划一条由圆弧段连接绕障点再回到原目标航迹点的路径。这条路径天然满足最小转弯半径约束，不需要后期再平滑。这个方案在障碍不算密集时很稳，密集时我会把决策周期缩短，让重规划更频繁。

2.4 异构协同的分工逻辑

集群协同方面，复现时参考了市场拍卖机制的思路，但做了简化。

任务分配不是集中式的，而是交互式的：每架飞机广播自己的“意向航点”和对应效用值，其他飞机收到后，如果发现另一架飞机对自己当前倾向的航点有更高效用，就判断是否需要让步。Step是排序竞争，不涉及复杂博弈。每架飞机维护一个“已分配给自己的目标集合”，确保同一区域不会被两架飞机同时过度覆盖。

在异构能力利用上，我的实现是给每个候选点计算“能力适配度”：速度快的飞机倾向信息增益大但距离远的航点，精度高的飞机倾向局部精细确认，机动性好的飞机则被分配到靠近障碍区域的航点。这个适配度直接乘在效用函数上，用来调整不同飞机对不同候选点的偏好。

3. Matlab实现的关键细节

3.1 环境建模与初始化

环境用一个结构体统一管理，核心字段包括地图尺寸、栅格尺寸、障碍多边形列表、禁飞区列表、目标真实位置列表、先验概率图。栅格尺寸我选的是5米一格，搜索区域大小设为2km×2km，共400×400=160000个栅格。这个精度对固定翼任务够用，计算量也在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，传感器带更高精度但探测距离较短，约300m
• C型：速度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 关键参数对照表

复现过程中我整理了最影响仿真效果的一组参数，这里直接给出我最终采用的经验值：

这些参数之间存在耦合，调参顺序建议是：先定决策周期，再定信息增益和风险代价的比例，最后微调航向跟踪增益和障碍安全余量。不要一次性同时调几个参数，否则出问题了根本分不清是哪个参数引起的。

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 常见问题速查表

这套复现做完，最大的体会是：搜索协同的算法框架看起来简单，真正的难点都在约束耦合里。固定翼的机动约束、异构带来的能力差异、信息传播的受限，这三者交织在一起时，任何一环节用“理想化假设”带过去，后面仿真都会以各种奇怪的方式回报你。如果你也在复现类似方法，建议先从单机避障调稳，再加第二架飞机测试协同，最后再扩展到异构集群，步骤越稳，后期排错越轻松。