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

集群协同搜索这块论文不少但能跑起来、能复现出效果的不多。这个项目标题里有“异构”、“复杂环境”、“自适应决策”、“避障”几个词每一个都对应一块不太好啃的硬骨头。我前阵子刚把这篇论文的Matlab代码完整复现了一遍从参数初始化到最后的可视化踩了不少坑也搞清楚了不少设计上的门道。这篇就按我的实际复现过程来聊聊其中的关键设计、核心逻辑以及那些论文里根本不会写的注意事项。1. 项目拆解与关键思路1.1 为什么是异构固定翼先说“异构”这件事。很多人一开始没太在意这个词觉得无非是多几架无人机一起飞。但真正上手之后就会发现异构才是整个搜救场景里最贴近实际的假设。固定翼和旋翼不一样它没法悬停有个最小速度和最小转弯半径的约束。异构则意味着集群里不同飞机拥有不同的飞行包线大翼展的搜索范围广、巡航速度高但转弯半径更大机动性差小翼展的灵活一些但航程短、载荷有限。这样一个混合编队既能跑得快、覆盖广又能在复杂地形里灵活穿行。这在算法实现上的直接后果是所有跟路径、时序、协同相关的计算都不能再假设“大家都一样”。比如搜索带划分巡航速度高的飞机分配的面积就得更大否则会出现在同一个时间片内飞得快的飞机干完活等别人飞得慢的这边还在转圈。这在仿真里看着还好放到真实场景中就是完全不可用的方案。1.2 复杂环境到底复杂在哪环境复杂这个词经常被论文用烂了但在这个项目里它是实打实的。我复现时把环境定义成了三类要素的叠加静态障碍物山峰、楼宇、限飞区这些是已知的或者可以预探测的动态威胁区比如局部强风、雷暴云团它们会随着时间移动或扩大不确定性很强禁飞区与搜索热点一部分区域完全不能进一部分区域有更高的搜索优先级。这些要素叠加之后光做静态路径规划显然不够。难点在于“自适应”——飞机在飞行过程中动态威胁的位置变了怎么办某架飞机被障碍挡了路线原先分配给它的那块搜索区域空出来怎么重新分配如果你的算法里没有应对这些变化的重规划逻辑那代码就是死的反过来说只要把“环境更新→任务重分配→路径重规划”这个闭环打通整个系统就活过来了。这也是我在复现过程中觉得最值钱的部分。1.3 复现的关键目标我把这次复现的核心目标定为三点完整跑通从环境生成、任务分配到航迹执行的闭环流程验证异构机群在“马赛克式”搜索区域划分下的覆盖效率和避障能力把论文里的算法逻辑翻译成人能看懂的Matlab工程代码便于后续换场景、加功能。后面所有的工作都是围绕这三点展开的。2. 整体架构与各模块拆解2.1 模块分层整套代码我是按四个层次组织的这也是我建议你复现时采用的分层思路层次模块核心职责环境层地图生成、障碍建模生成静态障碍、动态威胁更新搜索区域信息决策层区域划分、任务分配、目标函数根据机群状态和环境信息决定每架飞机飞向哪里规划层航迹生成、避障重规划在下发目标点的基础上生成可飞航迹执行层飞机运动模型、状态更新按固定翼运动学模型推进仿真时钟这个分层的最大好处是算法逻辑和运动模型是解耦的。比如你想把动力学模型改成更复杂的六自由度模型只需要替换执行层就行决策层完全不用动。反过来你想把区域划分算法从“Voronoi割”换成“网格收益函数”也不会牵扯到飞机怎么飞的问题。2.2 固定翼运动学模型固定翼无人机在仿真里最常用的简化模型是二维Dubins模型。核心约束就两个最小转弯半径 R_min决定了航迹的最小曲率半径速度 V 和航向角 ψ 的变化关系。我用的运动学递推公式如下% 固定翼简化运动学模型 x x V * cos(psi) * dt; y y V * sin(psi) * dt; psi psi u * dt; % u为偏航角速度满足 |u| V / R_min这里u就是控制量你可以理解为自动驾驶仪给出的转向指令。在航迹跟踪时我的做法是先算出无人机当前位置相对目标航点的期望航向再用一个比例控制器把期望航向转换为偏航角速度desired_psi atan2(dy, dx); err_psi wrapToPi(desired_psi - psi); u min(max(kp * err_psi, -V/R_min), V/R_min);这个模型看似简单但它的意义在于它把“能不能飞到那个点”这个几何问题直接转化成了“转弯半径够不够”的物理约束。2.3 搜索收益函数协同搜索的决策核心是一个收益Reward函数。每架无人机会评估候选区域的价值选择收益最大的目标去执行。我在实现里把收益设计成三部分加权求和reward alpha * uncertainty_map(x,y) ... % 环境不确定性越高越值得去 beta * priority_map(x,y) ... % 热点区域的搜索优先级 - gamma * threat_map(x,y) ... % 威胁区域的惩罚 - delta * path_cost(x,y); % 路径代价这里uncertainty_map初始设为一个常数无人机每飞过一个栅格该栅格的不确定性就降低一次。这实际上模拟的是“信息增益”的思路还没被搜过的地方信息量最大优先搜索。threat_map则把动态威胁区考虑进去让收益函数天然排斥危险区域。3. Matlab代码实现与核心流程3.1 工程初始化与地图生成我先定义了一个参数结构体集中管理所有仿真参数这样后续调参不用翻遍整个项目% 仿真初始化 sim_params.dt 0.1; % 仿真步长 sim_params.T 300; % 仿真总时长 sim_params.n_uav 5; % 无人机数量 sim_params.R_min [60, 50, 40, 45, 55]; % 最小转弯半径异构 sim_params.V_cruise [30, 25, 20, 22, 28]; % 巡航速度异构 sim_params.comm_range 200; % 通信半径 sim_params.safe_dist 15; % 安全间距地图我用了最简单的栅格法地图尺寸为1000m×1000m栅格分辨率10m。静态障碍物手动设置几个多边形区域用inpolygon函数完成栅格级别的占用来判断。动态威胁区我用了一个随时间变化的圆形区域来表示圆心位置做缓慢漂移半径按正弦变化以此模拟移动性强雷暴或者临时空域管制区。3.2 区域分配区域分配这步是整个协同搜索的“大脑”。我实现了两种分配策略这里贴的是基于改进Voronoi划分的版本。基本思路是先用各无人机当前位置做一次标准Voronoi划分再根据各机的速度能力对划分结果做修正。修正方法很简单——速度快的飞机把它的Voronoi单元边界朝邻居方向“推进”一段位移等效于给它分配相对更大的区域。% 基于加权距离的Voronoi划分 for i 1:n_uav for j 1:n_uav if i ~ j d_ij norm(pos(i,:) - pos(j,:)); % 加权边界速度快的飞机拥有更大的势力范围 w_i V_cruise(i) / mean(V_cruise); w_j V_cruise(j) / mean(V_cruise); boundary_offset (d_ij / 2) * (1 - w_i w_j); % 更新该栅格归属 end end end3.3 航迹规划与控制对于每个子区域的目标航点我用Dubins路径生成飞机从当前状态到目标点的可飞航迹。Matlab里没有直接提供Dubins路径生成函数需要自己实现。function path dubins_path(start, goal, R) % start [x, y, psi]; % goal [x, y, psi]; % 计算三部分组成起始圆弧直线段终止圆弧 ... end这里最关键的是判断“最短Dubins路径”属于哪种类型CLC、CL、CC等。我实现的时候偷了个懒——完全枚举所有可能路径选最短的一条。计算量稍大但对于单机规划来说完全够用。3.4 主循环执行流程仿真主循环是整个代码骨架每一步做以下事情for t 0:dt:T % 1. 更新环境信息 threat_map update_threat_map(threat_map, t); % 2. 更新各机状态检查是否到达目标航点 for i 1:n_uav if reach_target(uav(i), target(i)) % 为目标无人机重新分配新目标 target(i) assign_new_target(uav(i), map_info); end end % 3. 避障判断与航迹修正 for i 1:n_uav if detect_obstacle(uav(i), obstacle_list) % 触发避障逻辑 obstacle_avoidance(uav(i)); end end % 4. 更新编队通信与协作信息 share_info(uav); % 5. 可视化实时绘制 plot_scene(uav, target, threat_map); end这个循环看着简单但里面最容易出问题的是避障和重调度之间的优先级关系。如果避障动作的优先级太低飞机可能直接撞上障碍物优先级太高又会导致任务频繁中断、搜索效率低下。我最后定的顺序是先更新环境再判断机间协同需求最后做避障——因为避障属于安全性动作必须在任何情况下都有最高优先级。3.5 结果可视化可视化是Matlab实现里比较讨巧的一部分。我的做法是生成一个实时俯视图背景用灰度图表示搜索概率地图静态障碍用红色多边形动态威胁用半透明圆各无人机用不同颜色的三角形指向当前航向已搜索区域用灰度渐变的覆盖层表示当某块区域被搜索过后颜色变亮。最终的覆盖率和搜索效率也会画成曲线便于评估算法性能。4. 自适应决策与避障的工程实现细节4.1 自适应决策的触发条件自适应决策不是每时每刻都在跑那样太耗算力而且可能在不需要重分配的时候引入额外的混乱。我的实现里重调度逻辑只有在以下情况之一出现时才触发某架无人机到达当前目标点需要得到新任务动态威胁区域覆盖了某架无人机的规划航迹导致它偏离过大某架无人机因为“故障”或“任务失败”退出集群仿真中模拟这种特殊情况搜索完成度超过一个阈值需要切换搜索策略从广域搜索转到重点区域搜索。这四个条件分别对应了“任务完成”“环境变化”“成员变化”“策略切换”四种典型场景。写代码的时候务必把这几个条件分开处理不要合一否则遇到“有飞机完成目标同时威胁区域变了”这种叠加场景逻辑就会乱。4.2 避障层级我把避障实现成了两个层级第一个层级是全局规划避障在任务分配时尽量让航迹不穿过已知静态障碍。这靠的是在搜索地图上做代价检测把障碍物区域设为高代价路径规划时自然避让。第二个层级是局部实时避障用于处理动态威胁和临时出现的障碍物。局部的避障我用了速度障碍法function [avoid_vel] velocity_obstacle(own_pos, own_vel, obstacle_pos, obstacle_vel, tau) % 计算当前状态的碰撞锥 rel_pos obstacle_pos - own_pos; rel_vel own_vel - obstacle_vel; dist norm(rel_pos); % 判断相对速度方向是否落在碰撞锥内 ... % 若在则选择一个远离碰撞锥的安全速度 end这个方法的物理意义很直观把对方的运动“冻结”在当前时刻计算当前速度方向是否会导致碰撞。如果会就改变自己的速度方向到碰撞锥之外同时保证新的速度方向不会偏离目标航向太远。实测下来速度障碍法比人工势场法更稳定。人工势场法在多个障碍物靠得很近时特别容易陷入局部极小值——飞机在几个障碍之间来回震荡,看起来就像蒙圈了。速度障碍法没有这个问题因为它考虑的本身就是速度方向不涉及位置空间的势场叠加。4.3 机间防碰撞策略无人机互相之间的防碰撞和避障还不一样。避障针对的是环境中的障碍物防碰撞处理的是机群内部的间距问题。我的处理方式非常传统设定一个安全距离safe_dist每架无人机会周期性地检查自己与邻居的距离。如果小于安全距离就沿着“彼此连线方向的反方向”增加一个速度修正量。% 机间避撞 for i 1:n_uav for j i1:n_uav rel_vec pos(i,:) - pos(j,:); dist_ij norm(rel_vec); if dist_ij safe_dist push_force (safe_dist - dist_ij) / safe_dist; dir_vec rel_vec / dist_ij; uav(i).vel_repulse uav(i).vel_repulse push_force * dir_vec; uav(j).vel_repulse uav(j).vel_repulse - push_force * dir_vec; end end end这类“排斥力”思路在编队防碰撞中很常用。它跟速度障碍法的配合是速度障碍法负责“不撞死物”、不撞动态威胁区排斥力负责“不撞队友”。4.4 参数敏感性分析经验上收益函数的四个权重最敏感直接决定算法表现。alpha环境不确定性权重太大飞机会反复搜索同一片区域beta热点优先级权重太大飞机会过早地扎堆到热点区域而搜索覆盖率不高。我的经验是alpha设为0.4保证基础搜索beta设为0.3允许热点区域有吸引力但不至于扎堆gamma设为0.2威胁区域惩罚适度不把飞机“吓”得绕太远delta设为0.1路径代价权重最低路径稍长可以接受。这套参数在1000m×1000m的场景里效果不错但如果你改地图尺寸或无人机数量参数还得重新调不要死套。5. 复现踩坑记录与调试技巧5.1 转弯半径离散化的坑我一开始在初始化时给每架无人机设了不同的巡航速度但转弯半径是用一个固定公式算的结果几个异构飞机的运动学特性区分度不够导致编队行为几乎和同构的一样。后来我把转弯半径按无人机的翼载荷和最大过载分别设置编队的差异化行为才真正显现。坑的根源任务分配算法依赖无人机的特性差异来生成不同决策如果你的飞机参数之间都差不多分配结果跟同构集群无异自然看不出算法优越性。5.2 Voronoi划分边界的噪声问题Voronoi划分在栅格地图上做时边界会呈现锯齿状。如果你直接把每个栅格的归属权扔给各无人机会发现有的无人机需要同时飞到很多互不相邻的小块区域路径来回折返效率反而降低。我的解决办法是在划分完成之后加一道区域平滑处理把面积过小、形状过碎的“碎片区域”就近合并到相邻的大块区域中。虽然这偏离了最理想的Voronoi划分但实际搜救场景里让飞机多飞几块小碎片区域远不如飞一块连续区域来得经济。5.3 动态威胁的更新频率动态威胁区如果更新得太频繁无人机的航线会被频繁打断仿真计算量很大而且飞机会一直在“重规划→还没飞几步→再重规划”的循环里打转。这种病态现象我从数据上直观看到的就是总航程变大、覆盖率反而没有提升。我把动态威胁的更新频率从每个仿真步更新改成了每2秒更新一次同时给重规划加了一个“冷却时间”——离上次重规划不足3秒的即使威胁区有变化也先继续飞当前航迹等下一个检测周期再处理。这两个措施一加覆盖率曲线立刻平稳了。5.4 通信限制下的信息共享现实中无人机之间的通信有范围限制不是所有飞机之间都能直接通信。这一点我在代码里专门做了模拟距离大于通信半径的两架无人机不能直接交换信息只能通过中间机转发。这里就有个很有意思的现象当通信拓扑不连通时不同无人机对“哪块区域已经搜索过”的认知会出现分歧可能导致两架飞机重复搜索同一块区域。面对这个问题我加了简单的历史信息融合每架无人机维护一个本地搜索代价地图通过通信时会和其他无人机交换但接收方只更新对方地图中“置信度更高”的栅格。这样即使通信受限整个集群的搜索行为依然基本一致。5.5 常用的三种调试手段复现过程中我觉得最有用的三个调试手段建议你也用起来第一画轨迹慢放。不要只盯着最后的覆盖率曲线。把仿真改成慢速播放逐帧看无人机轨迹不同颜色区分。很多时候问题是肉眼一眼就能看出来的比如某架飞机在原地转圈说明目标分配逻辑里有死循环某两架飞机轨迹始终间隔很近说明机间避撞力阈值过小。第二分模块单测。我在做整体联调之前会先把区域划分模块单独运行输入一组静态坐标打印出划分结果再把Dubins路径生成模块单独跑输入一组起终点看路径是否满足最小转弯半径约束。每个模块单独测都没问题之后再联调排查复杂度会降低很多。第三加仿真日志。在每个关键分支里写上日志输出输出当前状态和决策结果。不要觉得日志浪费算力一次复杂逻辑错误排查所花的时间远远超过日志带来的那点性能损耗。5.6 性能优化Matlab跑多机协同仿真在迭代次数上去之后有性能瓶颈。主要耗时点在两层一是实时绘图每帧重绘地图和无人机图标非常吃资源二是栅格地图的循环更新。我的优化方案很简单绘图频率降为每5个仿真步绘制一次地图的栅格更新用向量化操作替代循环在不需要可视化的批量实验中直接注释掉绘图代码仿真速度提升了将近10倍。如果你的场景里无人机数量和地图栅格数量很大可以考虑把核心计算用C MEX改写但就这个项目规模而言向量化优化已经完全够用。6. 最后再分享两个有用的小技巧第一个小技巧是关于结果评估指标。除了累计覆盖率一定要同时统计“平均单机搜索效率”——用总有效搜索栅格数除以所有飞机总航程。这个指标能直观反映算法有没有让飞机“瞎转”。只追求覆盖率而不关心效率很容易做出一个所有飞机在目标区域反复横跳的高覆盖低效方案。第二个小技巧比较实用把环境参数、飞机参数做成一个参数文件每次实验前只修改参数不要改主代码。我复现时因为这个吃了不少亏一开始参数散落在各个脚本里想对比一组实验改得晕头转向。后来痛定思痛把参数统一收敛到一个脚本里所有模块都从这个结构体读取参数。这样调参、跑对比实验都变成了改几行参数、跑一遍脚本的事。这个习惯养成之后做任何仿真项目都会省很多时间。复现这个项目本质上是在走一遍“从论文到工程”的完整路程。论文里一页纸的公式落到Matlab里往往是一百多行代码加无数次调试。但跑通的那一瞬间看到集群搜索曲线和避障轨迹稳稳地走出来之前踩的坑就全都值了。