前阵子把一套异构固定翼无人机集群协同搜索方法在Matlab里完整跑通了。简单说这套方法解决的是这么一个问题有多架气动参数和传感器配置都不一样的固定翼无人机在部分已知、部分未知的复杂环境里要在某个区域内搜索目标同时还得保证不撞障碍物、不撞队友。标题里的“自适应决策”和“避障”不是两个独立模块而是嵌在同一套滚动决策循环里的两个约束。复现完我最大的体会是这类项目真正难的不是某个单点算法而是固定翼不能悬停这一条硬约束它会同时影响搜索、分配和避障三个环节。这篇文章把完整框架、关键函数和踩坑记录写下来给同样在做集群搜索复现的朋友做个参照。1. 先把标题掰开揉碎异构、自适应决策、协同搜索分别指什么1.1 异构固定翼平台到底“异”在哪“异构”在标题里不是用来凑字数的概念它直接改变了算法的输入输出结构。我复现时用了三类平台平台类型巡航速度最小转弯半径探测半径典型用途轻型低速机15 m/s30 m40 m近距离精细确认中型中速机25 m/s60 m80 m主力覆盖搜索大型高速机35 m/s120 m150 m快速大范围初查这个表看起来简单但在算法里影响非常大。最小转弯半径决定了同一架飞机能不能进入某个窄区域探测半径决定了它飞过一片栅格时能把哪些位置的“未搜索概率”抹掉巡航速度又直接决定了单位时间内的覆盖面积。所以异构平台的协同本质上是把“快机大范围粗扫”和“慢机小范围精查”组合起来但代价是不能再用统一的欧氏距离去做任务分配必须为每架飞机单独估计路径代价和到达时间。固定翼还有一条旋翼机没有的约束不能原地转弯不能悬停等待。转弯半径和速度的关系是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(end1) 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 秒才更新一次地图覆盖轨迹像锯齿一样大量区域被漏掉。我后来总结了一套经验准则参数推荐取值依据仿真步长 dt0.1 s小于最短时间内飞机移动距离与栅格尺寸的比值决策周期 T_plan1~3 s应保证 T_plan * v 0.5 * 传感器直径栅格尺寸10~20 m远小于探测半径避免漏检探测半径40~150 m与平台类型匹配安全距离1.5~2 倍翼展或机身长度避免湍流干扰也可以放宽此外所有单位必须统一。Matlab里最常见的错误是把角度单位搞混wrapToPi处理的是弧度而有人在初始化航向时直接写了 90、180 这样的度数导致飞机朝着完全错误的方向飞。我建议在仿真开始前写一个断言函数检查所有角度变量的范围是否在[-pi, pi]内。4.2 性能指标怎么定才能说明“协同搜索有效”只盯着最终覆盖率是不够的。一片区域谁飞都能覆盖关键是多少时间内、用多少飞行距离、是否安全。我复现时用了几个指标覆盖率已搜索面积占可搜索面积的比例按不确定度阈值判定搜索效率单位飞行距离带来的覆盖率增量用于比较不同策略安全指标整个仿真过程中任意两机之间的最小距离、无人机与障碍物之间的最小距离分配均衡度每架飞机的累计飞行距离是否有明显差距。这些指标不能只看单次仿真。我每次跑一组参数至少做 20 次蒙特卡洛随机初始化机群起点和目标分布然后取平均值和方差。如果某次运行出现碰撞那次数据不能丢弃要单独统计碰撞率。集群搜索算法的价值恰恰体现在大量随机场景下都能保持安全而不是某个精心调好的场景里表现完美。5. 复现过程中我踩过的几个坑5.1 收益函数归一化顺序不对机群扎堆第一个比较大的坑出现在任务分配收益函数上。最开始我把路径代价做了全局归一化gain regionProb / maxRegionProb - pathCost / maxPathCost这个写法看起来合理实际上会让机群全部涌向最高概率区。因为最高概率区的regionProb / maxRegionProb接近 1而其他区域被压到很小的值距离项又归一化后拉不开差距低概率区域根本不会被选中。最后形成的结果就是几架飞机在同一片高概率区反复绕圈覆盖率曲线很快进入平台期。解决方法是去掉分母归一化改成固定权重gain regionProb detectionValue - lambda * pathCostlambda 通过实验调整我最终取值 0.3 左右此时机群既不会扎堆也不会为了省路程而忽略高价值区域。这个坑提醒我归一化可以用在展示指标上但不能用在优化目标里因为它会破坏不同量纲之间的真实权衡。5.2 用“目标点”给固定翼下达指令导致航线交叉和浪费第二个坑是任务表达方式。一开始我把任务分配的结果定义成一个目标点坐标飞机飞向那个点到达之后再重新分配。结果固定翼飞过目标点之后不能停下来只能继续绕圈下一轮分配时它还在绕圈导致轨迹出现大量交叉和重叠搜索效率非常低。后来我把任务点改成了目标圆。每架飞机分配到的不是一个点而是一个以某点为中心、半径不小于自身最小转弯半径的圆。任务层只需要给出圆心和圆弧方向轨迹层生成Dubins路径让飞机以某个切线方向进入这个圆然后沿圆弧飞行。这样做的好处是飞机始终处于持续转弯的状态不会出现“飞过来了但还要等下一指令”的尴尬情形。5.3 避障和搜索同时算结果两个都顾不上第三个坑是决策顺序。我一开始把避障和搜索放在同一个目标函数里加了一个权重系数totalScore searchGain - avoidWeight * collisionRisk看起来是“既要又要”实际上优化器经常选出一些中间方案搜索收益一般、避障也一般。尤其是在狭窄通道里这种折中方案可能让飞机贴着障碍物飞风险很高。我后来改成级联决策先生成当前航向可达的所有候选航向剔除会和障碍物、队友碰撞的航向如果剩余航向集合为空优先转向风险最小的方向并降低速度从剩余集合中选择搜索收益最大的航向。这个顺序的本质是安全是硬约束搜索是软目标。先保证有解再追求优化。运行时你会发现避障动作会显得“果断”很多因为它不再被搜索收益稀释。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路径没问题再加第二架飞机观察它们的任务分配是否均衡然后加入障碍物和避障最后才引入异构参数和自适应决策。每一步都用一个简单的二维动画可视化中间结果比如绘制每架飞机的航迹、概率地图热力图、候选航向的安全区间。看到画面你才能发现数据表里看不出来的问题尤其是飞机在目标圆附近反复绕圈这类轨迹层问题没有图几乎不可能定位到。我个人的体会是这类集群搜索算法百分之八十的难度在系统集成剩下的百分之二十才是单个算法本身。把框架拆分清楚、参数配比合理、决策顺序正确复现工作至少能少走一半弯路。如果你现在正卡在某个模块上优先检查运动模型、地图更新和任务分配三者之间的数据接口是否一致大多数问题都出在这里。
