固定翼无人机集群协同搜索：自适应决策与避障的Matlab复现

前阵子帮一个师弟复现了一篇论文的代码题目是“复杂环境下自适应决策和避障的异构固定翼无人机集群协同搜索方法研究”。说实话这类题目第一眼看上去信息量很大但其实核心就四件事异构、协同、自适应决策、避障最后全都要落到Matlab仿真上。干活之前我也是觉得这玩意很唬人等真正把模型拆开、把代码一跑发现只要把每个模块的边界理清楚复现难度远没有标题看起来那么高。这篇文章就围绕这个复现项目来写。我会从问题拆解讲到算法原理再到Matlab代码架构和核心实现最后把调试过程中踩过的坑一并列出来。适合正在做无人机集群搜索方向的研究生、对多智能体协同控制感兴趣的开发者以及想找一套能直接改改参数就跑的仿真框架的Matlab用户。我尽量把每一步的“为什么”讲透毕竟光对着论文抄公式后面调不出来才是最痛苦的。1. 复现前先看清问题的本质1.1 异构固定翼集群的“异”到底体现在哪“异构”这个词很容易被轻描淡写地带过但在实际算法设计里它几乎是所有复杂度的来源。同一支集群里的无人机如果飞行速度、转弯半径、传感器探测距离、飞行高度甚至任务约束都不一样那么所有假设“所有无人机能力相等”的做法从根上就站不住了。我复现时把异构性分成三组参数来定义运动学异构最大平飞速度、最小转弯半径、最大爬升率。这决定了每架无人机能飞多急的弯、能跑多快。感知异构探测半径、最大探测距离、有效探测视场角。有的无人机可能在高层大范围扫有的低空精确探视觉范围差异很大。任务异构剩余航时、载荷类型、最小安全高度。搜索和侦察任务的权重分配也因此不同。为什么要强调这个因为异构导致的问题不是“多跑几架飞机”那么简单。同样的搜索区域速度快、转弯半径大的无人机适合执行大范围的粗略扫描速度慢、小转弯半径的无人机适合在重要区域精细搜索。如果决策算法没有意识到这种差异就会让所有飞机都往一块儿挤或者让某些飞机频繁做出根本飞不出来的轨迹。1.2 自适应决策解决的是“计划赶不上变化”复杂环境里的“复杂”主要有两层一是先验信息不足目标位置不确定只能靠逐步搜索来更新认知二是环境动态变化包括突发障碍、威胁区域移动、队友飞行状态异常。固定翼无人机又有个天然限制——不能悬停转弯需要半径。所以计划好的路径一旦遇到意外修改的成本比多旋翼高得多必须提前把避让空间算出来。自适应决策做的是这样一件事在每个决策周期根据当前各无人机的位置、已有搜索信息、障碍分布在线重新计算下一步怎么飞。它和传统“预先规划全局路径、然后跟踪”的思路最大的区别是这里没有一份执行到底的固定计划。我复现下来发现整个系统的核心循环可以概括成感知更新 - 收益评估 - 分配决策 - 生成可飞行轨迹 - 避障修正 - 执行然后在下一个决策时刻重复。所谓自适应就是让这个循环跑得足够快、足够稳定让系统在环境发生变化后不用重新做全局规划靠局部调整就能维持整体搜索效率。2. 核心模块到底怎么建模2.1 搜索图与目标信息更新协同搜索的第一步是把“目标在哪”表示成一个空间概率分布。常用做法是把搜索区域离散成栅格每个格子上存储一个“存在目标”的概率值这就是搜索引擎地图。这个地图是所有决策层共享的状态。每次无人机飞过一个格子传感器返回“发现”或“未发现”结果地图会随之更新。我代码里用的是标准的贝叶斯更新假设栅格 c 的策略概率为 p(c|z_{1:t})当传感器在时刻 t 得到测量 z_t ∈ {0,1}更新公式为p(c|z_{1:t}) p(c|z_{1:t-1}) * p(z_t|c) / Σ p(z_t|c) p(c|z_{1:t-1})如果探测模型认为“目标存在时检测到目标的概率为 Pd目标不存在时误检概率为 Pf”那么检测到目标z_t 1时概率会上升未检测到z_t 0时概率会下降。问题在于如果一个区域长期没被访问目标概率既不应该无限升高也不应该完全归零。我加了一个时间衰减项p(c|t) p(c|t-1) * e^{-λ·Δt} μ·(1-e^{-λ·Δt})。这里的 λ 决定旧信息衰减速度μ 是平均背景概率用来防止某些格子概率被推到极端值后丧失重新搜索的价值。这个更新模型直接关系到搜索效率。如果你把 Pd 设得过高系统会非常自信地认为“这里没目标”然后就再也不来了如果设得过低地图会一直处于高不确定状态无人机反复扫同一个区域。我最终用的 Pd 0.7Pf 0.05λ 0.02μ 0.05效果相对平衡。2.2 任务收益函数及其物理含义自适应决策的基础是“每一步都在最大化某种收益”。但收益函数设计不好就会出现所有飞机都去抢同一个高概率区域或者长时间不搜索新区域的问题。我拆出来的收益函数包含四部分目标存在概率项对应格子当前的目标概率值概率越高搜索价值越大。信息增益项某区域如果长时间没被访问不确定性高去那里获得的信息量更大。距离代价项决策航向选择后飞抵候选格子需要的时间越远收益折减越多。转弯惩罚项固定翼无人机不能急转弯候选航向和当前航向的夹角越大飞行代价越高。总收益的表达式类似J w_p · P(c) w_i · I(c) w_d · (D_max - D(c)) w_t · (Δψ_max - Δψ)其中 P(c) 是目标概率I(c) 是信息不确定性可以用熵或者访问间隔来表达D(c) 是到候选格子的距离Δψ 是航向变化量。所有权重都需要归一化处理否则量纲完全不同直接相加没有意义。实际调参时我建议先固定 w_p 为主导把基础的搜索行为调出来再逐步加入 I 和 Δψ。如果你一开始就把转弯惩罚权重调得巨大飞机会变得极其“懒惰”老是沿着一条直线飞最后区域覆盖效果差到没法看。2.3 避障的两种实现路径避障在固定翼集群里要比地面机器人复杂因为不能急刹车。我复现时采用了两种避障策略配合使用静态障碍避障。对地图中的已知障碍比如山峰、禁飞区直接把对应栅格标记为不可通行然后在生成候选航向时把会撞上障碍的航向排除掉。但只排除还不够因为固定翼有转弯半径即使当前航向不直接指向障碍转弯过程中也可能擦到障碍边缘。我额外给障碍区域做了一层“膨胀”膨胀半径近似等于无人机最小转弯半径这样能避免常见的刮边问题。动态障碍/无人机互避。这部分我用了简化版的速度障碍法Velocity Obstacle思路。原理很简单把本机和对方的速度都放到速度空间里如果本机当前速度落在一个“会导致碰撞”的扇区内就改变速度方向让速度向量脱离这个扇区。扇区怎么算假设本机位置为 p_i速度为 v_i对方位置为 p_j速度为 v_j两机之间的相对位置 Δp p_i - p_j相对碰撞锥可以表示成一组速度条件如果 v_i - v_j 的方向落在以 Δp 方向为中心、半角为 arcsin(R_collision / |Δp|) 的锥体中就意味着未来可能碰撞。其中 R_collision 取两机安全距离之和。我实现时没有用完整的VO库计算而是做了一个简化对每个候选航向估算未来 T 秒内与所有动态障碍的相对距离如果最小距离小于安全阈值这个候选航向直接剔除如果无法避免全部冲突就选择冲突时间最晚的那个候选航向。这种方法牺牲了理论上的最优性但工程实现简单实时性好跑仿真完全够用。2.4 搜索和避障冲突时怎么决策实际仿真中最容易忽略的问题是避障和搜索目标冲突时怎么办。比如某个区域的目标概率非常高但到达该区域的航向会穿越障碍区。我的处理原则是安全优先级永远最高。候选航向先做安全性筛选剩下的航向里再挑收益最大。如果你连安全都保证不了搜索性能再高也没有意义因为飞机已经撞了。具体到实现上我在决策评分阶段给每个候选航向加了一个“避障惩罚项”如果该候选航向对应的预估轨迹与障碍的最小距离小于安全阈值惩罚设为无穷大直接排除如果大于安全阈值但小于松弛阈值则按比例降低收益。这样就能保证“尽量搜、安全飞”。3. Matlab代码架构与核心实现3.1 总体框架把代码分成四层来写我复现时没有把代码写成一坨“主循环里堆公式”而是分成四层后续调试省了非常多时间。环境层environment负责生成地图、初始搜索概率场、静态障碍、目标位置、动态威胁。地图用矩阵存目标概率场用一个和地图等大的矩阵。代理层agent定义无人机对象。每架无人机就是一个类实例属性包括位置、速度、航向角、探测半径、最大转弯速率、剩余航时以及它各自的探测模型参数。决策层decision核心逻辑所在。根据当前环境信息、所有代理状态计算每个代理的候选航向评分返回最优航向。主循环main负责时间推进调用环境更新、代理更新、决策更新并实时保存状态数据用于后处理。这样的分层带来一个直接好处你可以先单独测试决策层。比如只放两架无人机在空旷地图上跑看收益计算对不对再加静态障碍最后加动态互避。每层独立调试问题定位非常快。3.2 固定翼运动学建模固定翼不能原地转向这点必须在代码里建模。我用的简化三自由度运动模型x(t1) x(t) V · cos(ψ(t)) · dt y(t1) y(t) V · sin(ψ(t)) · dt ψ(t1) ψ(t) u_turn(t) · dt z(t1) z(t) V_z(t) · dt其中 V 是巡航速度异构不同无人机不同ψ 是航向角u_turn 是航向角速度控制量受最大转弯角速率约束|u_turn| ≤ V / R_minR_min 是最小转弯半径。这个约束直接体现在候选航向生成上下一个决策周期的可选航向只能落在 [ψ - Δψ_max, ψ Δψ_max] 区间内不能随便指向任何方向。实际写代码时我建议把物理量都统一在标准单位下坐标用米、速度用米/秒、时间用秒避免后面调参时因为单位混乱导致看起来“参数合理”但动力学异常。下面是运动学更新函数的骨架function new_state updateFixedWing(state, turn_rate_cmd, dt) % state: [x, y, z, psi, V] V state(5); max_turn V / 300; % 最小转弯半径300m示例 turn_cmd max(-max_turn, min(max_turn, turn_rate_cmd)); new_state zeros(1,5); new_state(1) state(1) V * cos(state(4)) * dt; new_state(2) state(2) V * sin(state(4)) * dt; new_state(3) state(3); new_state(4) state(4) turn_cmd * dt; new_state(5) V; end注意这里我对航向角做了归一化处理吗没有。如果仿真时间很长ψ 会无限累加可能超过2π导致三角函数计算精度下降。最好每次更新后统一对 ψ 取模 2πstate(4) mod(state(4), 2*pi);这种小问题在长时仿真里很容易反向坑你比如飞了十几分钟后忽然出现“无人机不受控”的奇怪现象先查是不是角度溢出。3.3 搜索收益计算核心函数收益计算是决策层最核心的部分。我写了一个独立函数输入包括当前搜索概率图、候选栅格列表、无人机自身参数和当前航向输出每个候选栅格的收益值。核心逻辑如下function gain computeSearchGain(map_p, agent, cell_idx, t) % map_p: 目标概率图 % agent: 无人机状态结构体 % cell_idx: 候选格子线性索引 p_target map_p(cell_idx); % 信息增益: 使用访问间隔时间衰减后的不确定度 last_visit agent.visit_map(cell_idx); info_gain 1 - exp(-(t - last_visit) / agent.time_decay); % 距离代价: 无人机当前位置到候选格子的欧氏距离 [cell_x, cell_y] ind2sub(size(map_p), cell_idx); dist sqrt((cell_x - agent.x)^2 (cell_y - agent.y)^2); dist_gain max(0, agent.max_range - dist) / agent.max_range; % 转弯代价: 当前航向与目标方向的夹角 desired_psi atan2(cell_y - agent.y, cell_x - agent.x); psi_diff abs(angdiff(agent.psi, desired_psi)); turn_gain max(0, (pi - psi_diff) / pi); % 加权求和权重在外部传入 gain agent.w_p * p_target agent.w_i * info_gain ... agent.w_d * dist_gain agent.w_t * turn_gain; end这个函数本身不复杂但有两个细节值得注意。第一angdiff 函数在Matlab的Aerospace Toolbox里有如果没有这个工具箱需要自己写一个角度差函数。我因为不想额外依赖工具箱直接手写了角度差处理function d angdiff(a, b) d mod(a - b pi, 2*pi) - pi; end第二权重归一化很重要。如果 p_target 的取值范围是 0 到 1info_gain 也是 0 到 1但 dist_gain 如果等于 1000 米 / 2000 米 0.5量纲是一致的。最容易被忽视的是 turn_gain它是角度比值无量纲。如果统一成 0~1 范围权重调整才有意义。3.4 自适应决策与避障的融合实现决策部分我采用的是“候选航向采样 加权评估 安全性筛选”框架。这也是目前论文复现中性价比最高的方案原因有两个一是固定翼的运动学约束是非线性的解析优化又慢又复杂二是候选采样法天然适合和障碍检测融合每个候选航向都可以独立评估安全性。候选航向怎么采我的做法是以当前航向为基准在最大转弯范围内均匀采样 N 个候选航向num_cand 13; delta_max agent.turn_rate_max * decision_dt; headings linspace(-delta_max, delta_max, num_cand); cand_psi agent.psi headings;然后对每个候选航向计算一个“前视轨迹”。所谓前视轨迹就是以当前速度为恒定值沿候选航向向前推 T_p 秒的圆弧路径。因为速度恒定、航向角速度固定轨迹很容易生成。安全性的判断是在这段前视轨迹上进行的。对轨迹上的每个采样点检查三件事是否越过地图边界是否落入静态障碍含膨胀层与其他无人机在该时间窗口内的预估位置是否距离过近。如果三项都通过计算这个候选航向的搜索收益再加上一个避障裕度奖励项得到该候选航向的最终得分最后取最高分对应的航向作为本周期控制指令。下面是决策主过程的简化代码function cmd selectHeading(agent, env, fleet, dt) cand_psi agent.psi linspace(-agent.delta_max, agent.delta_max, agent.num_cand); scores -inf(1, numel(cand_psi)); for k 1:numel(cand_psi) traj generateLookAheadTrajectory(agent, cand_psi(k), dt); if checkCollision(traj, env, fleet) continue; % 有碰撞风险直接剔除 end scores(k) evaluateTrajectoryGain(agent, traj, env.search_map, env.time); end [~, best_k] max(scores); cmd cand_psi(best_k); end这样写的好处是决策周期作为一个整体可调。我测试时发现决策频率 1Hz 到 2Hz 比较合适。太快了收益计算量大而且和运动学更新不同步会造成抖动太慢了环境变化可能来不及反应。3.5 异构协同任务分配异构带来的一个麻烦是收益函数的最优解对每架无人机都不一样。同一片区域大飞机可能因为转弯半径大飞不进去小飞机却可以轻松进入。所以不能只算“全局最优”必须分别计算每架飞机自己的可达候选集再各自决策。我采用的是一种非常工程化的方案基于“边际收益优先”的任务分配机制。具体做法是先把所有无人机按照搜索收益初值排序收益高的先选区域然后将已选区域的目标概率临时降低避免大家扎堆之后再让后续无人机选择剩余收益最高的区域。这种分配的好处是实现简单、计算开销小非常适合在线执行缺点是理论上不是全局最优。但对于搜索任务来说实际效果已经很接近复杂优化方法了。你如果对数学最优性有执念可以替换成一致性拍卖算法但牺牲的是实时性在纯仿真里倒也无所谓。4. 复现过程中的参数设置与调试心得4.1 我用的关键参数列表为了让你有个可对照的起点我列出最后调稳定的一组参数参数取值说明仿真区域2000m × 2000m栅格分辨率 20m栅格尺寸20m × 20m100×100个格子无人机数量4 架2架快速型 2架慢速型快速型速度35 m/s最小转弯半径 300m慢速型速度22 m/s最小转弯半径 150m探测半径偏移量快400m慢250m对应异构感知差异搜索目标概率初始值0.05全图均匀分布目标真实生成数6 个位置随机决策周期1 s运动学更新周期 0.1 s避障安全距离80m两机之间的最小间距这些参数不是一次调出来的。最初我用同一个转弯半径结果发现慢速无人机的轨迹“浪费”了大量航向调整空间搜索覆盖率反而下降。后来把异构参数拉开各司其职整体搜索效率明显上涨。4.2 从搜索覆盖率曲线看调参效果我习惯用两个指标评估仿真效果区域覆盖率访问过的网格占总网格比例和目标发现数量随时间的变化。调参有没有效果看这两条曲线就够不需要凭感觉。第一次跑通时覆盖率曲线两条平得离谱表示有大量无人机反复扫同一块区域。原因是信息增益项的权重设太低目标概率高的区域一直占据主导导致所有飞机往一个地方挤。把 w_i 从 0.2 调到 0.5 之后飞机开始主动访问长时间没扫的区域覆盖率曲线从第 200 秒开始有了明显抬升。第二次问题是目标全部被发现后飞机会陷入“漫无目的”的游荡状态能耗浪费严重。我加了一个搜索终止判定如果所有栅格的平均概率下降到阈值之下说明已经基本排除了目标存在可能可以降低速度进入区域巡航模式。4.3 动态威胁测试为了验证自适应决策的效果我在第 100 秒临时在地图中间插入了一块动态禁飞区形状随时间缓慢移动。最开始所有的无人机都没有意识到这个威胁在收益引导下仍然按原方向飞。当有无人机接近时避障模块触发重新选了绕行航向。但因为我设置的是候选航向采样绕行半径受限于当前转弯能力有几架飞机被迫绕了一大圈。这个测试让我意识到一个重要问题决策层的搜索收益和避障模块是并行关系但决策频率不能太低。如果决策周期 3 秒意味着无人机在 3 秒后才能响应动态威胁可能已经飞进去很远了。把决策周期降到 1 秒后动态避障响应就平滑很多。5. 复现中的常见问题与排查技巧实录5.1 常见问题速查表复现过程中我记录了一堆问题归纳成一张速查表希望你少走点弯路问题现象可能原因解决办法无人机频繁急转弯、航向震荡决策周期太短或转弯惩罚权重太小拉长决策周期或加大 w_t 权重所有飞机聚在同一个区域搜索信息增益权重太低目标概率项主导调高 w_i并检查visit_map是否正常更新覆盖率曲线长期不变时间衰减系数太小旧信息不衰减增大 λ让目标概率场随时间回潮无人机莫名其妙撞障碍障碍膨胀半径不足或轨迹采样点太稀疏增加膨胀半径、加密前视轨迹采样点两机互撞但避障模块没触发动态互避没有考虑对方未来位置互避检测时同时预测双方 t 秒后的位置代码越跑越慢每帧都在画图或者地图矩阵频繁复制画图降频到每5秒一次预分配数组这些问题看起来零散其实核心就一句话参数之间是强耦合的调了一个必须回头复测其他指标。不要只盯着目标发现数量要同时看覆盖率、碰撞次数、转弯频率。5.2 排查“假避障”问题的完整思路我特别想讲一下“假避障”这个坑。现象无人机已经避开了一个障碍但还是会在原地反复打转就是不往目标方向飞。一开始我以为避障逻辑错了最后打开轨迹图才发现原来是期望航向和目标方向之间隔着一个大型障碍候选航向里所有指向目标方向的角度都被安全筛选排除了剩下的候选全是远离目标方向的于是评分里距离项和概率项都不占优飞机就在障碍边缘来回试探。这个问题不能靠调整避障逻辑解决而需要引入“绕行规划”能力。我在避障模块之后加了一个“沿障碍切线方向绕行”的候选航向当现有候选航向全部不可用时选择当前航向朝障碍距离增加最快的方向偏转相当于强制导航绕行。加上这个之后系统行为一下子合理了。5.3 代码层面的调试经验Matlab做这类仿真调优有几个非常实用的经验第一绘图降频但数据不降频。一开始我在主循环里每次都画地图和所有无人机位置500 秒仿真能跑十几分钟。后来把绘图改成每 20 个时间步刷新一次再把状态数据尽可能记录成矩阵而不是cell数组性能立刻提上来跑一次仿真从 8 分钟降到 1 分钟以内。第二把关键状态导出成MAT文件用app Designer或者单独脚本回放。仿真画图看的是实时状态出问题根本来不及定位。我会把每个时刻所有无人机的 x, y, psi, 目标概率图, 决策得分都存下来跑完慢慢回放过程曲线一拉出来问题一眼就看到了。第三每次只改一个参数。听起来像废话但做的时候特别容易顺手改两个参数结果出问题不知道是谁引起的。我把参数集中放在一个 config.m 里每次实验前复制一份带时间戳保存改完只动一个值严格对照前后两组曲线的差异。第四善用matlab的断点和条件断点。查具体某架飞机异常时在 updateFixedWing 函数入口设定条件断点比如当 x 超出地图边界时暂停就能直接看到是哪个决策周期出了问题。5.4 长时仿真中的数值稳定性问题一个特别隐蔽的坑出现在长时仿真第 700 秒附近有一架无人机开始出现“僵尸状态”位置不再变化但状态显示还在更新。最终锁定原因是visit_map这个矩阵用的是 uint16 类型时间戳超过 65535 后溢出回绕成 0导致无人机认为自己刚扫过全场信息增益项几乎失效收益趋平后无人机失去搜索动力。解决方法是把时间相关变量全部改成 double 类型。这也提醒我在复现论文算法时数值类型的选择往往比算法本身更影响结果特别是在长时仿真里整数溢出、角度归一化、单位一致性这些问题都要提前想清楚。6. 从仿真代码到实际应用的一些补充思考6.1 这套代码能直接改造成什么样这套框架并不只是用来复现论文的。我把环境层换成随机生成的雷暴区域代理层换成不同性能参数的固定翼模型就能跑一个简单的“台风区域多无人机协同探测”场景。替换搜索目标概率场为目标初始分布图把避障层换成禁飞区约束也能用于航线规划类任务。也就是说这个复现项目的价值不仅是得到一份能跑的代码而是得到一整套可以二次开发的多无人机协同搜索基础框架。你只需要改环境模型、传感器模型和收益函数这三个部分就能适配很多实际问题。6.2 如果想进一步扩展到更真实的场景仿真和实飞之间还有很大的鸿沟。仿真里我假设了理想通信所有无人机可以实时共享地图和位置。实际中通信带宽受限、时延不稳定、数据丢包都是常事。如果要做实飞建议在代码里加入通信代价模型——比如限制地图同步频率、允许部分无人机携带未同步的信息参与决策这是更接近工程实战的方向。另外真实的固定翼无人机还有风场影响。我仿真里用的运动学模型是简化的实际侧风会影响航向、地速和转弯半径。如果后续往这个方向做建议把风速场作为环境输入加进运动学方程这会显著影响自适应决策的稳定性。6.3 最后说点实在的我复现项目的最大体会是论文里那些公式推导再漂亮落到代码上最先遇到的永远不是数学问题而是参数匹配、数值类型、决策时序这类“脏活”。不要被“复杂环境下自适应决策”这样的标题吓住核心逻辑拆开来本质就是不断重复三件事看一眼环境、算一下收益、选一个能飞的方向。从零复现下来最终框架加代码大概花了三周其中一半时间在调各种隐蔽的参数问题。但如果现在让我再复现一遍我会先把参数分层先跑通一个最简单的两机场景验证运动学和决策闭环没问题再一步步把静态障碍、动态威胁、异构属性加回去。我希望你不用再走一遍我那些弯路了。跑通之后你会发现这个框架不只是好看改参数、换场景、调策略都很快是真的能拿来用的那种代码。