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

前阵子把一套异构固定翼无人机集群协同搜索方法在Matlab里完整跑通了。简单说，这套方法解决的是这么一个问题：有多架气动参数和传感器配置都不一样的固定翼无人机，在部分已知、部分未知的复杂环境里，要在某个区域内搜索目标，同时还得保证不撞障碍物、不撞队友。标题里的“自适应决策”和“避障”不是两个独立模块，而是嵌在同一套滚动决策循环里的两个约束。复现完我最大的体会是，这类项目真正难的不是某个单点算法，而是固定翼不能悬停这一条硬约束，它会同时影响搜索、分配和避障三个环节。这篇文章把完整框架、关键函数和踩坑记录写下来，给同样在做集群搜索复现的朋友做个参照。

1. 先把标题掰开揉碎：异构、自适应决策、协同搜索分别指什么

1.1 异构固定翼平台到底“异”在哪

“异构”在标题里不是用来凑字数的概念，它直接改变了算法的输入输出结构。我复现时用了三类平台：

这个表看起来简单，但在算法里影响非常大。最小转弯半径决定了同一架飞机能不能进入某个窄区域；探测半径决定了它飞过一片栅格时能把哪些位置的“未搜索概率”抹掉；巡航速度又直接决定了单位时间内的覆盖面积。所以异构平台的协同，本质上是把“快机大范围粗扫”和“慢机小范围精查”组合起来，但代价是不能再用统一的欧氏距离去做任务分配，必须为每架飞机单独估计路径代价和到达时间。

固定翼还有一条旋翼机没有的约束：不能原地转弯，不能悬停等待。转弯半径和速度的关系是：

R_min = v^2 / (g * tan(phi_max))

其中 g 是重力加速度，phi_max 是最大滚转角。这意味着飞得越快，转弯半径越大，能去的地方越少。很多搜索算法默认无人机可以随意调整航向，这在固定翼上根本不成立。我复现时把所有决策都建立在这条约束之上，后面章节的避障和分配都会反复用到。

1.2 自适应决策是为了解决什么痛点

如果环境静态、目标先验已知、障碍物不变，那最简单的方法就是给每架飞机画一个子区域，让它按扫描线飞完，这也是很多覆盖搜索的基线方案。但复杂环境里这些假设全不成立：某个区域突然出现高价值线索，附近出现新禁飞区，某架飞机因故障退出集群，这些情况都会让预设航线立刻失效。

自适应决策的本质，是“每隔一段时间重新评估一次：我接下来该去哪，去哪最划算”。复现时我把它具体成四个机制：

• 概率地图实时更新，而不是一次初始化后不变；
• 滚动时域决策，每隔 N 个仿真步长重新算一次任务分配；
• 搜索收益函数随地图变化动态调整；
• 避障优先级在遇到突发障碍时高于搜索收益。

举个例子，一架中型机原本沿着直线搜索一片低概率区域，突然另一架飞机在远处发现了一个强信号区域。如果不做自适应，它会继续飞完当前区域；做了自适应，它会在下一个决策时刻比较“继续当前区域”和“转向强信号区域”的边际收益，如果后者明显更高，就立即规划一条Dubins路径转过去。这个“比较”不是拍脑袋，而是基于概率地图里每个栅格当前的未搜索概率、目标存在概率和覆盖代价算出来的，下一章详细讲。

2. 我复现这套方法时采用的整体框架

2.1 分层决策框架：环境层、任务层、轨迹层

把所有问题塞进一个目标函数里，让优化器一次算完，在规模稍大时基本不可行。我实际采用的是三层解耦结构，每层只做一件事，但层与层之间有明确的输入输出接口。

这个分层结构的好处是每一层都可以单独测试。我在复现初期只调环境层和轨迹层，让所有飞机按固定分配的扫描线飞，先把地图更新和避障跑通；确认无误之后再加入任务层的动态分配。否则一旦出问题，很难判断是搜索收益算错了，还是Dubins路径生成错了。

环境层和任务层的交互频率也需要注意。我设置的决策周期是 2 秒，也就是说每2秒重新做一次任务分配，但轨迹层每个仿真步长 0.1 秒都在更新位置和避障判断。这样做的好处是决策不会太频繁导致机群“抖动”，又能在环境变化时做出及时反应。

2.2 概率地图更新逻辑：为什么它是自适应的“心脏”

概率地图是整个系统的核心状态量。我给每个栅格维护一个“未搜索概率”，或者叫不确定度。初始时，如果没有任何先验信息，所有栅格都设为 0.5；如果有目标可能出现的高概率区，就把对应栅格设得更高，比如 0.8。每当某架飞机的探测半径覆盖到某个栅格，该栅格的不确定度就降低。

单个栅格的更新公式，我用了这种形式：

P_new = P_old * (1 - detection_prob * coverage_factor)

其中 detection_prob 是传感器对该类型目标的单次探测概率，coverage_factor 是本次探测对栅格的覆盖程度，范围在 0 到 1 之间。coverage_factor 由栅格中心到航迹线的距离决定，距离越近覆盖越充分，超过探测半径就为 0。检测后如果不确定度降到某个阈值以下，比如 0.1，就认为该栅格已被充分搜索，不再纳入收益计算。

Matlab里对应函数如下：

function mapNew = updateMap(map, x, y, psi, detectRadius, detectionProb, gridSize) mapNew = map; h = gridSize; n = round(detectRadius / h); ix = round(x / h) + 1; iy = round(y / h) + 1; for dx = -n:n for dy = -n:n gx = ix + dx; gy = iy + dy; if gx < 1 || gy < 1 || gx > size(map,2) || gy > size(map,1) continue; end dist = sqrt((dx*h)^2 + (dy*h)^2); if dist > detectRadius continue; end % 简化覆盖率：探测圆内默认全覆盖 coverage = 1 - dist / (detectRadius + eps); mapNew(gy, gx) = map(gy, gx) * (1 - detectionProb * coverage); end end mapNew = max(mapNew, 0.01); end

我特意加了max(mapNew, 0.01)，避免不确定度变成0后，数值误差导致栅格“复活”。实际中多架飞机同时覆盖同一个栅格时，我建议按时间顺序依次更新，而不是同时用乘积融合，因为同一决策周期内两次探测之间飞机已经移动了，简单乘积会低估不确定度。多机信息融合时再统一做加权平均。

3. Matlab实现中几个关键函数的写法

3.1 固定翼运动模型：质点加Dubins约束

轨迹层首先要有一个准确但不复杂的固定翼运动模型。我采用二维常高度假设，状态量为[x, y, psi]，控制量为滚转角phi。每步更新时，先根据当前速度计算转弯角速度，再更新航向，最后用更新后的航向更新位置。

function [xNew, yNew, psiNew] = updateFixedWing(x, y, psi, v, phi, dt) g = 9.8; omega = g * tan(phi) / v; % 转弯角速度 psiNew = psi + omega * dt; psiAvg = psi + 0.5 * omega * dt; % 用平均航向计算位置更精确 xNew = x + v * cos(psiAvg) * dt; yNew = y + v * sin(psiAvg) * dt; end

之所以用psiAvg而不是psiNew，是因为在一整个 dt 内航向是连续变化的，直接用终点航向会让轨迹向外侧偏大。这个误差在单步很小，但1000步之后累计起来可能相差好几个机身长度，对避障判断会造成不必要的虚警。

真正的路径规划我用的是Dubins路径。固定翼从当前状态到某个目标点，总共有4种基本组合：右转-直线-右转、右转-直线-左转、左转-直线-右转、左转-直线-左转。我不需要把完整Dubins路径的全部坐标都算出来，只需要得到路径长度和末段航向，用来计算任务分配里的路径代价值。

一个简化版的Dubins路径长度计算如下：

function L = dubinsLength(x0, y0, psi0, xt, yt, Rmin) psi0 = wrapToPi(psi0); dx = xt - x0; dy = yt - y0; D = sqrt(dx^2 + dy^2); if D < 2 * Rmin psi1 = atan2(dy, dx); else psi1 = atan2(dy, dx) + acos(2*Rmin / D); end turn1 = abs(wrapToPi(psi1 - psi0)) * Rmin; L = turn1 + D + Rmin * pi; % 保守估计，等号成立时对应特定进入角 end

这个不是严格精确公式，但作为成本估计已经够用。如果追求精确，可以调用成熟工具箱，但要注意代码里必须自带最短路径比选，不要只算一种组合。

3.2 任务分配：基于边际收益的贪心分配法

任务层的核心是给每架飞机选一个“下一块搜索区域”。我没有做全局最优分配，而是用了基于边际收益的贪心方法，理由是大规模集群里全局最优的求解时间不可控，而贪心配合滚动时域已经能把覆盖效率做到接近最优。

每架飞机对每个候选区域计算一个收益：

gain = unsearchedProbability * area + detectionValue - lambda * pathCost

其中：

• unsearchedProbability 是该区域内所有栅格不确定度之和；
• area 是区域面积，用来避免系统偏向小块高密度区；
• detectionValue 是区域内目标的先验存在概率加权；
• pathCost 是从当前位置到该区域入口的Dubins路径长度；
• lambda 是路径代价系数，控制“远距离高价值区域”和“近距离低价值区域”之间的平衡。

代码如下：

function gain = estimateSearchGain(x, y, psi, v, candidate, probMap, lambda, Rmin) cx = candidate.centerX; cy = candidate.centerY; pathCost = dubinsLength(x, y, psi, cx, cy, Rmin); % 候选区域内的未搜索概率累加 regionProb = sum(probMap(candidate.yGrid, candidate.xGrid), 'all'); area = numel(candidate.yGrid) * candidate.gridSize^2; detectionValue = candidate.priorTargetProb * area; gain = regionProb + detectionValue - lambda * pathCost; end

异构在这里的体现方式，是不同飞机有不同的v和Rmin，所以即使它们当前位置相同、候选区域相同，算出来的 pathCost 也不同。中型机可能觉得这个区域很近，大型高速机却因为转弯半径大，需要绕一大圈才能进去，收益自然被压低。

分配顺序我采用“一轮一轮挑”：先让所有飞机计算所有候选区域的收益，选出全局最大值分配给那架飞机，然后把这个区域从其他飞机的候选列表里删掉，重复直到所有飞机都有任务。这样避免了多架飞机选中同一个区域。如果某架飞机已经接近任务区域内部，就不分配新的外部区域，让它继续在当前区域精细搜索。

3.3 避障约束：速度障碍法的工程化剪裁

避障部分我用了速度障碍法（Velocity Obstacle）的思想，但没有用完整的连续求解，而是离散采样航向角。

核心逻辑是：对于每个候选航向，先判断沿该方向飞行是否会和障碍物或者队友发生碰撞，剔除不安全方向，再从剩余方向里选搜索收益最高的。

对静态障碍物，判断方式比较简单。把障碍物看成半径为rObs的圆，把本机看成半径为rSafe的圆，那么从本机位置到障碍物中心的方向角记为thetaObs，安全航线必须与thetaObs的夹角大于某个临界角：

thetaObs = atan2(obsY - y, obsX - x); criticalAngle = asin((rObs + rSafe) / distToObs); for heading = candidateHeadings delta = abs(wrapToPi(heading - thetaObs)); if delta > criticalAngle safeHeadings(end+1) = heading; end end

机间避撞原理类似，但要考虑对方也在运动。我把对方飞机膨胀成一个更大的圆，然后用相对位置和相对速度构造动态碰撞锥。简化做法是假设对方匀速直线飞行，预测未来T_horizon秒内的最小距离，如果低于安全距离就判为危险。这个预测窗口一般取 5 到 10 秒，太长会过度保守，太短又来不及避让。

固定翼避障最大的限制是：航向不能突变。所以候选航向集合不能是-pi:0.1:pi随便取，必须限制在当前航向的可达范围内：

maxDelta = (v / Rmin) * dt * k; % k 为决策周期内的控制步数 headings = psi + linspace(-maxDelta, maxDelta, 21);

如果所有候选航向都被判定为危险，就说明当前决策周期内没有可行方案，此时需要降低速度或者增大预测窗口。但固定翼低速是有下限的，失速速度之上才能维持飞行，所以最稳妥的做法是在更早的决策周期就发现风险，而不是等到最后一步才处理。

4. 仿真参数怎么配才能让结果可信

4.1 时间步长、栅格尺寸与搜索半径必须互相匹配

复现这类算法，最容易出现的问题不是算法本身，而是参数配比不合理。我第一次跑的时候，决策周期设成 10 秒，飞机速度 25 m/s，一个决策周期要飞 250 米，传感器半径只有 80 米，结果就是每隔 10 秒才更新一次地图，覆盖轨迹像锯齿一样，大量区域被漏掉。

我后来总结了一套经验准则：

此外，所有单位必须统一。Matlab里最常见的错误是把角度单位搞混，wrapToPi处理的是弧度，而有人在初始化航向时直接写了 90、180 这样的度数，导致飞机朝着完全错误的方向飞。我建议在仿真开始前写一个断言函数，检查所有角度变量的范围是否在[-pi, pi]内。

4.2 性能指标怎么定才能说明“协同搜索有效”

只盯着最终覆盖率是不够的。一片区域谁飞都能覆盖，关键是多少时间内、用多少飞行距离、是否安全。我复现时用了几个指标：

• 覆盖率：已搜索面积占可搜索面积的比例，按不确定度阈值判定；
• 搜索效率：单位飞行距离带来的覆盖率增量，用于比较不同策略；
• 安全指标：整个仿真过程中任意两机之间的最小距离、无人机与障碍物之间的最小距离；
• 分配均衡度：每架飞机的累计飞行距离是否有明显差距。

这些指标不能只看单次仿真。我每次跑一组参数，至少做 20 次蒙特卡洛，随机初始化机群起点和目标分布，然后取平均值和方差。如果某次运行出现碰撞，那次数据不能丢弃，要单独统计碰撞率。集群搜索算法的价值，恰恰体现在大量随机场景下都能保持安全，而不是某个精心调好的场景里表现完美。

5. 复现过程中我踩过的几个坑

5.1 收益函数归一化顺序不对，机群扎堆

第一个比较大的坑出现在任务分配收益函数上。最开始我把路径代价做了全局归一化：

gain = regionProb / maxRegionProb - pathCost / maxPathCost

这个写法看起来合理，实际上会让机群全部涌向最高概率区。因为最高概率区的regionProb / maxRegionProb接近 1，而其他区域被压到很小的值，距离项又归一化后拉不开差距，低概率区域根本不会被选中。最后形成的结果就是几架飞机在同一片高概率区反复绕圈，覆盖率曲线很快进入平台期。

解决方法是去掉分母归一化，改成固定权重：

gain = regionProb + detectionValue - lambda * pathCost

lambda 通过实验调整，我最终取值 0.3 左右，此时机群既不会扎堆，也不会为了省路程而忽略高价值区域。这个坑提醒我：归一化可以用在展示指标上，但不能用在优化目标里，因为它会破坏不同量纲之间的真实权衡。

5.2 用“目标点”给固定翼下达指令，导致航线交叉和浪费

第二个坑是任务表达方式。一开始我把任务分配的结果定义成一个目标点坐标，飞机飞向那个点，到达之后再重新分配。结果固定翼飞过目标点之后不能停下来，只能继续绕圈，下一轮分配时它还在绕圈，导致轨迹出现大量交叉和重叠，搜索效率非常低。

后来我把任务点改成了目标圆。每架飞机分配到的不是一个点，而是一个以某点为中心、半径不小于自身最小转弯半径的圆。任务层只需要给出圆心和圆弧方向，轨迹层生成Dubins路径，让飞机以某个切线方向进入这个圆，然后沿圆弧飞行。这样做的好处是飞机始终处于持续转弯的状态，不会出现“飞过来了但还要等下一指令”的尴尬情形。

5.3 避障和搜索同时算，结果两个都顾不上

第三个坑是决策顺序。我一开始把避障和搜索放在同一个目标函数里，加了一个权重系数：

totalScore = searchGain - avoidWeight * collisionRisk

看起来是“既要又要”，实际上优化器经常选出一些中间方案，搜索收益一般、避障也一般。尤其是在狭窄通道里，这种折中方案可能让飞机贴着障碍物飞，风险很高。

我后来改成级联决策：

1. 先生成当前航向可达的所有候选航向；
2. 剔除会和障碍物、队友碰撞的航向；
3. 如果剩余航向集合为空，优先转向风险最小的方向，并降低速度；
4. 从剩余集合中选择搜索收益最大的航向。

这个顺序的本质是：安全是硬约束，搜索是软目标。先保证有解，再追求优化。运行时你会发现，避障动作会显得“果断”很多，因为它不再被搜索收益稀释。

5.4 通信丢失时的地图一致性

最后一个小坑是关于通信的。集群协同搜索默认所有飞机共享概率地图，但实际场景里通信链路不可能永远稳定。如果某架飞机断连了几秒，它本地地图的老旧信息会污染全局地图，导致其他飞机以为某个区域已经搜索过，实际并没有。

我处理的办法是在每条地图更新消息里带上时间戳和飞机ID，接收端只融合比自己本地地图版本更新的数据。如果某架飞机长时间失联，就把它之前贡献的置信度逐步衰减，防止过期信息永远留在图里。这个处理在Matlab仿真里只多写十几行，但对整个系统在通信受限场景下的稳定性提升显著。

6. 这套方法还能往哪个方向延伸

6.1 参数自适应：从固定系数到在线调整

我在复现时发现，搜索收益函数里的 lambda 是一个非常难调的参数。场景密集时，路径代价权重应该更高，避免飞机绕远路；场景稀疏时，搜索收益权重应该更高，鼓励飞机去探索未搜索区域。固定一个 lambda 很难同时适配两种场景。

一个可行的扩展是给 lambda 加自适应性：每隔N个决策周期统计一下当前未搜索区域占比，如果占比高，说明还有很多地方没搜，就适当降低 lambda，让飞机更愿意飞远路；如果占比低，说明剩余区域比较集中，就提高 lambda，让飞机优先去近处剩余区域。这种自适应策略和标题里的“自适应决策”可以形成更好的呼应，实现起来也不复杂。

6.2 从Matlab到半实物仿真

Matlab里跑通不代表实际能飞。真实固定翼无人机还有风场扰动、通信延迟、传感器噪声和控制延迟等问题。如果后续要做硬件验证，我建议先在Simulink里加入风场模型和延时模块，对比无风和有风两种条件下最小转弯半径的变化。侧风会显著改变飞机相对地面的转弯轨迹，原本规划好的Dubins路径可能不再可行。

更接近工程化的做法是把Matlab里的决策模块生成C代码，部署到机载计算单元上，地面站只负责显示和监控。这样可以避免Matlab原型和实机代码不一致的问题。不过这一步工作量大，建议先把地图更新、避障筛选这两个模块的数值稳定性和边界条件测试充分再考虑。

6.3 一个实操层面的建议

如果你也准备复现类似方法，我建议不要一开始就追求和标题完全一致的大而全系统。先实现单机覆盖搜索，确认概率地图更新和Dubins路径没问题；再加第二架飞机，观察它们的任务分配是否均衡；然后加入障碍物和避障；最后才引入异构参数和自适应决策。每一步都用一个简单的二维动画可视化中间结果，比如绘制每架飞机的航迹、概率地图热力图、候选航向的安全区间。看到画面，你才能发现数据表里看不出来的问题，尤其是飞机在目标圆附近反复绕圈这类轨迹层问题，没有图几乎不可能定位到。

我个人的体会是，这类集群搜索算法百分之八十的难度在系统集成，剩下的百分之二十才是单个算法本身。把框架拆分清楚、参数配比合理、决策顺序正确，复现工作至少能少走一半弯路。如果你现在正卡在某个模块上，优先检查运动模型、地图更新和任务分配三者之间的数据接口是否一致，大多数问题都出在这里。