前段时间接了一个多无人机协同路径规划的验证项目地图是三维山地场景几架无人机要从不同起点飞到指定目的地中间有雷达威胁源和地形遮挡要求机群整体成本最低。这个“成本”在工程上不是单一数值项目里明确要算路径长度、高度偏离、威胁暴露和转角幅度四项。我选型时对比了好几种群智能算法最后用鳄鱼伏击算法CAOA做核心优化器Matlab完整跑通三维路径能绕开威胁机间也不会撞。这篇文章把问题建模、算法选型、Matlab实现、参数调整和踩坑记录完整整理出来给正在做CAOA三维路径规划或多无人机协同避障路径规划的朋友一份可直接参考的实践笔记。1. 问题建模把“最低成本”拆成四个能算的东西1.1 多无人机协同路径规划到底在优化什么做路径规划时很容易被“最优路径”这个概念带偏以为就是最短路径。放到多无人机协同三维场景里事情远不是这样。路径是三维空间里一串航迹点序列不是一条直线要绕开已知威胁源和地形遮挡多架无人机同时飞彼此还要保持安全距离。如果每架飞机只顾自己最短很可能大家都挤到同一走廊或者绕飞时互相穿插实际根本没法飞。我习惯把问题定义成在连续三维空间中给定每架无人机的起点和终点找一组航迹点序列使总代价最小。代价来自四块路径长度、高度偏离、威胁暴露、转角幅度。机间碰撞和越界则转入惩罚项。这样一个多目标问题被转换成单目标优化只要权重设置合理智能优化算法就能直接跑也比加权后做归一化容易。目标函数是所有无人机代价之和既体现了“协同”又保留了计算上的可并行性。这里需要强调路径规划不是一次性画一条线就行。真实飞行中还要考虑航迹是否平滑、高度是否频繁变化、是否长时间暴露在威胁区。所以四个成本项缺一不可而且每一项都要能量化才能交给CAOA这种启发式算法去迭代优化。1.2 四项成本指标与归一化方法这四项成本在项目里都需要归一化否则路径长度可能是千米量级转角只有个位数加权时长度项直接把其他项淹没了。我采用的方案是各算各的标量再除以各自的最大可能值。具体如下成本项含义计算方式归一化参考路径长度成本 L所有航段长度之和相邻航点欧氏距离之和除以地图对角线长度高度成本 H与期望巡航高度偏差航点高度与期望高度差的绝对值的均值除以最大允许高度差威胁成本 T经过威胁源附近的暴露程度在每个采样点按威胁强度累计除以采样点总数转角成本 S机动平滑程度航向角和俯仰角变化量的均值除以最大允许转角路径长度成本直接反映能耗但只优化它会导致路径贴着威胁边缘走所以需要威胁项平衡。高度成本用于限制爬升和俯冲因为无人机频繁升降既耗能又容易被雷达捕获。威胁项我的做法是每个威胁源用球体建模半径内有一个逐渐增强的惩罚距离越近代价越高不直接设成硬禁飞区。这样算法会感受到“越靠近越危险”的梯度既能贴着边缘试探也能找到合适的绕飞通道。转角成本是为了航迹可飞性连续弯折的折线虽然在长度上占优但无人机根本转不过去。最终适应度函数写成Fitness wL * L_cost wH * H_cost wT * T_cost wS * S_cost lambda * CollisionPenalty BoundaryPenalty我第一版权重设的是0.4、0.2、0.3、0.1跑出来路径偏绕因为威胁权重太高。后来改成0.35、0.2、0.3、0.15效果好了不少。权重整定没有统一公式要结合地图和威胁分布微调后面第4节再展开说。1.3 约束条件与惩罚项处理除了四个成本约束条件也很关键。工程中必须考虑三类约束无人机的运动学约束包括最大转角、最大爬升角、最大航程环境约束包括禁飞区、地形边界机间协同约束包括最小安全距离。把这些硬约束直接在算法里实现非常麻烦尤其CAOA这种启发式算法碰撞判断本身代价很高我最终统一用惩罚函数法。比如机间距离约束我会把每条候选路径按相同的时间步长插值然后检查同一时刻两两飞机之间的三维距离一旦小于安全距离dSafe就把超出量按比例加入惩罚。边界约束同理航点跑到地图外就直接给一个很大的惩罚值。惩罚系数要足够大保证最优解必然满足约束但又不能大到让算法失去探索能力。我实测中lambda取1000到10000之间比较合适太小会出现路径钻到威胁源里太大会让早期种群几乎不会尝试高风险区域。用惩罚函数法还有一个好处CAOA在迭代前期可以放心探索那些“看起来有风险”的走廊只要惩罚项不至于让适应度变成无穷大算法就有机会发现绕过威胁的更优路径。如果你把威胁区设成硬约束算法一旦生成一条穿越威胁区的路径就直接判死种群里很多个体都会无效收敛效率反而更差。2. 算法选型为什么用鳄鱼伏击算法CAOA而不是PSO/GWO2.1 群智能算法在三维路径规划上的短板多无人机协同三维路径规划本质是个高维、非线性、多峰值的优化问题。传统粒子群PSO收敛快但很容易早熟种群里所有粒子被某一个局部最优吸引后很难跳出来。遗传算法GA全局搜索能力强但操作算子复杂参数又多做航路规划时编码、交叉、变异都要专门设计工程量不小。灰狼算法GWO在连续函数优化上表现不错但一旦目标函数里有大量威胁峰和碰撞惩罚灰狼的收敛速度会变慢而且容易出现后期收敛到同一条绕远路径的情况。我做选型的时候不是看谁的理论漂亮而是看它能不能在几千次目标函数评估内找到可飞路径。CAOA模拟的是鳄鱼捕猎时的伏击行为最大的特点是搜索策略有“观察-逼近-攻击”三个阶段这跟路径规划里“大范围探索-确定走廊后细化-最终收敛”的需求非常匹配。项目里测试下来CAOA比PSO平均成本低10%到20%比GWO的稳定性也好一些。你可能会问智能优化算法那么多为什么非要换新算法我的答案是CAOA不是换了一个名字的PSO而是平衡机制确实不一样。多无人机协同航路规划如果要同时处理五个威胁源和三架飞机适应度函数里有很多局部陷阱。CAOA的“高位伏击”阶段用长尾随机游走保证探索能力“低位潜行”阶段用小步调整保证开发精度这种分工让它在多峰值问题上的鲁棒性明显好很多。2.2 CAOA的搜索机制与仿生逻辑CAOA的核心思想我理解是鳄鱼在水中埋伏猎物时不会一直盲目游动而是先在高位观察周围动静发现目标后慢慢潜入靠近最后从水底发起致命一击。放到算法里这三个阶段分别对应不同的位置更新策略。高位伏击阶段负责全局探索。这个阶段里个体会参考种群中随机个体和最优个体生成随机扰动步长同时引入类似Levy飞行的长尾随机游走一些个体会跳到较远的区域避免种群很快聚集。低位潜行阶段负责局部开发。鳄鱼靠近猎物后不会直接冲过去而是根据种群当前中心位置和最优位置做小步调整像围猎一样把目标压缩在包围圈内。最后攻击阶段个体快速向最优解收敛用更小的步长精细搜索把路径打磨光滑。得益于这种机制CAOA在大范围禁飞区和连续威胁源同时存在的时候仍然能保持较高的种群多样性这也是它适合多无人机高维路径优化的原因。实现时不需要太多参数主要就是种群规模、迭代次数、探索与开发概率的比例以及Levy步长因子。我自己的实现里探索概率从0.6开始随着迭代次数线性降到0.3让前期多探索、后期多开发。2.3 多无人机协同“编码”方式用CAOA解多无人机问题第一步是解决个体编码。每只鳄鱼的位置向量不是只代表一条路径而是代表整个机群的路径集合。我采用一个固定的航点数nK每架无人机有nK个航点每个航点有x、y、z三个维度所以位置向量长度就是nUAV * nK * 3。起点和终点通常固定不在优化变量里优化的是中间的nK-2个航点。解码的时候把向量reshape成nUAV × nK × 3的三维数组然后逐架无人机依次算四项成本。协同体现在两个地方一是适应度函数是所有无人机成本的累加单架飞机绕远会导致总成本升高算法会自动平衡各机路径二是机间安全距离惩罚只要两架飞机在某个时间点距离过近整个个体的适应度就会变大。这样CAOA在更新位置时天然会避开机间碰撞而不是等到最后路径规划完再做一次防碰撞调整。这种编码方式有一个好处算法不需要知道哪一段路径属于哪架飞机它只负责优化一个长向量。只要适应度函数里解码逻辑正确CAOA就能自己找到每架飞机的合理路径。缺点是维度会随无人机数量和航点数增加而变大所以航点数不能设太多我一般取6到10个。3. Matlab实现从目标函数到完整跑通3.1 代码结构与主流程我习惯把Matlab工程拆成几个独立文件单个文件不要超过200行这样改起来效率高很多。项目主要文件如下main_caoa_3d.m主脚本设置地图参数、算法参数调用求解画图。initQuadPopulation.m初始化种群生成每只鳄鱼的位置向量。fitnessMultiDrone.m多无人机协同适应度函数返回总成本和各项分量。threatModel.m威胁源建模与威胁强度计算。caoaUpdate.mCAOA的位置更新主循环。plotPaths3D.m三维绘图和收敛曲线绘制。主流程分六步加载地图参数和威胁源初始化航点种群迭代评估适应度按CAOA阶段更新位置检查边界和越界值记录每一代最优适应度。迭代结束后把最优位置解码成各机航点用样条插值生成平滑轨迹再画图验证。Matlab里我习惯把地图、威胁、航点这些结构体统一放到params里方便函数间传递。比如params.nUAV、params.nK、params.threats、params.mapRange这样主函数和适应度函数之间传参很简单也不会因为全局变量污染工作区。3.2 地图与威胁建模三维路径规划里地图不需要像栅格那样精细我用的是连续空间加“威胁强度函数”的建模。地形高度用一个峰值函数生成然后每个威胁源定义一个球体中心位置、半径、强度三个参数。某一点受到的总威胁强度是每个威胁源的贡献之和ThreatIntensity(p) sum( K_i / (1 exp(a_i * (dist_i - r_i))) )这个式子是Sigmoid型好处是在威胁半径外迅速衰减到接近0进入半径内快速上升不会出现阶跃跳变算法在优化时能够感受到“越来越危险”的梯度。实际执行中如果地图范围是1000×1000×500米我设置了5个威胁源各半径80到150米强度系数不完全相同。地形部分我不建议用真实DEM数据一开始就参与调试因为数据量大会拖慢适应度计算。先用Matlab自带的peaks函数生成一个简单的连续地形验证算法和代码没问题之后再把真实高程图替换进去。这样分层验证排错容易得多。3.3 多无人机适应度函数关键代码适应度函数是整个工程的核心直接决定算法能不能收敛到好路径。下面这个是我项目里简化后的代码保留了最核心的逻辑function [cost, detail] fitnessMultiDrone(X, map, threats, params) % X: 优化变量长度为 nUAV*nK*3 nUAV params.nUAV; nK params.nK; X reshape(X, nUAV, nK, 3); cost 0; detail zeros(1, 4); for i 1:nUAV path squeeze(X(i,:,:)); [Li, Hi, Ti, Si] calSinglePathCost(path, map, threats, params); cost cost params.wL*Li params.wH*Hi params.wT*Ti params.wS*Si; detail detail [Li, Hi, Ti, Si]; end % 机间碰撞惩罚按航程等分插值检测 colPenalty calcCollisionPenalty(X, params); cost cost params.lambda * colPenalty; end这里calSinglePathCost会依次算路径长度项、高度项、威胁项、转角项。威胁项需要从起点到终点采样很多点逐个求威胁强度转角项需要先把路径投影到水平面计算航向角变化也要算俯仰角变化。关键点是所有输入输出都用行向量或三维数组避免Matlab中维度混乱。在单条路径成本计算里我还会做一次边界检查。如果某个航点的x或y超出地图范围或者z低于地形高度就直接返回一个很大的成本比如1e6。这样做可以防止CAOA在边界外生成“看起来更短但实际不可飞”的路径。3.4 CAOA主循环与参数配置我这里贴一下CAOA主循环的简化代码。按我用的版本探索和开发阶段是根据随机数切换的全局探索阶段使用随机个体指导步长局部开发阶段使用最优个体和种群中心指导更新for iter 1:maxIter for i 1:popSize if rand pExplore j randi([1, popSize]); X_new X(i,:) A * (X(j,:) - X(i,:)) levyStep .* randn(1, dim); else X_best popBest; X_center mean(X, 1); X_new X(i,:) B * (X_best - X(i,:)) C * (X_center - X(i,:)); end % 边界修正 X_new max(X_new, lb); X_new min(X_new, ub); % 计算适应度贪心保留 newCost fitnessMultiDrone(X_new, map, threats, params); if newCost costList(i) X(i,:) X_new; costList(i) newCost; end end [bestCost(iter), idx] min(costList); popBest X(idx,:); end需要特别注意levyStep不能设得太大。我开始设成1.2导致探索阶段跳得太远大部分个体飞出边界种群多样性反而下降。后来改成0.5效果好很多。A、B、C三个系数会影响搜索步长我一般设为0.5到1.0之间也可以随迭代次数逐渐减小。还要注意种群初始化。我一开始用纯随机生成航点导致大量航点落在地形下面或者威胁源正中心适应度全是惩罚值算法前几十代基本在无效搜索。后来改成在起点和终点的连线上叠加正态扰动初始种群的适应度立刻正常了收敛速度也快了很多。3.5 多机协同避障的一个容易被漏掉的点很多人在做多机协同的时候只检查最终路径中航点之间的距离这非常容易漏碰撞。因为两架飞机的航点即使不相交在航段之间也可能相交。我的做法是先把每条路径用样条插值成400个点再按飞行距离归一化成相同的时间轴然后逐点检查两两飞机之间的距离。只要任意采样点距离小于安全阈值立即返回一个较大的惩罚值。这个检查会在适应度函数里被调用成千上万次所以性能很重要。我建议用矩阵化批量算不要用三层for循环一个点一个点去算。实际代码里可以用pdist2快速计算两两距离矩阵然后统计小于阈值的数量一次调用就能完成所有飞机所有采样点的距离判断。有一个细节时间轴归一化要按路径长度做不能按航点序号做。因为两架飞机的航点密度可能不同按序号比较会导致错位。我在实现里先累加每段航段长度再在累计长度上等分插值这样不同飞机在同一“航程比例”处比较距离才是合理的。如果无人机速度相同航程比例也能近似对应时间比例。4. 实验场景与结果对比4.1 三无人机测试场景设置为了验证CAOA的效果我搭了一个三架无人机协同场景。地图范围1000×1000×500米起点分别放在地图三个角终点集中在目标区域附近中间设置五个威胁源每个半径100到150米同时地形高度在200到450米之间起伏。航点数固定为8种群规模50迭代200代。每架无人机期望巡航高度是300米安全距离设置为100米转角限制水平方向45度垂直方向30度。理论上每只鳄鱼的位置向量长度是3×8×372维属于中等偏上的维度。这种规模下CAOA每代要跑50次适应度计算每次适应度里包含三架飞机的插值和碰撞检测在Matlab里我做了矩阵化处理200代大概需要几十秒时间可以接受。我还做了对比把威胁源数量从3个加到7个CAOA仍然能稳定找到可飞路径但最优成本会上升因为可用走廊变窄了。如果威胁源增加到10个以上航点数要适当增加否则航点之间跨度太大插值出来的路径很容易穿进威胁区。4.2 CAOA与PSO、GWO的收敛对比我单独跑了CAOA、PSO、GWO三种算法各20次统计最终适应度均值和标准差。结果大致如下算法平均成本最好成本最差成本碰撞次数PSO0.6820.6410.7393GWO0.6650.6220.7111CAOA0.5730.5360.6110CAOA平均成本比PSO低了约16%比GWO低了约14%而且20次运行中没有任何一次出现机间碰撞。收敛曲线上CAOA前50代下降很快全局探索阶段能够让个体不断跳出局部最优到150代左右基本进入开发阶段后续优化幅度变小。PSO在40代左右就会陷入一个局部最优后面提升非常有限。三维路径图里CAOA生成的路径没有直接穿过威胁源而是规划出了绕行通道高度变化上大多数航点保持在280到320米之间高度成本控制得比较好。四条路径相互之间保持了一定间距没有出现交叉拥挤。GWO虽然也能找到可飞路径但路径的边缘绕行距离明显更长有几次还出现了两架飞机在终点附近距离过近的情况。4.3 参数敏感性与调参心得我经验里最重要的一项是威胁权重wT。权重设太高时路径会绕得非常远长度成本大幅上升设太低时算法倾向于贴着威胁边缘走虽然没有大量钻入威胁区但现实中雷达暴露概率已经不能接受。推荐的做法是先用默认权重跑一版看路径是否在边缘“擦边”有擦边就微调wT不要一次调太大。航点数量也和地图复杂度有关。地图威胁多、地形起伏大时8到10个航点比较合适简单场景下5个航点就够了。航点越多路径平滑潜力越大但优化维度也更高收敛会变慢。合理的插值密度也很关键威胁采样点多目标函数评估就精确但太密会每次计算都慢我一般每个航段等分30到50个点采样威胁碰撞检测用统一的400点插值。还有一个容易被忽略的参数是种群初始化范围。如果初始航点离终点太远CAOA需要很多代才能把路径拉过去。我在初始化时以起点到终点的连线为基准加一个±100米的正态扰动这样初始路径已经具备大致方向算法只需要优化细节。5. 踩坑记录与Matlab调试经验5.1 中文注释乱码问题这个项目用的是Matlab R2023b默认脚本编码和Windows系统编码偶尔不一致打开别人给的代码时中文注释会变成乱码。处理办法是把Matlab预设里的文件编码改成UTF-8或者在保存代码时选“其他编码”里的UTF-8。我自己写代码的习惯是文件头加一行英文说明正文中文注释只在关键段落使用避免整个文件全是中文导致编码问题影响运行。如果你拿到一份已经是乱码的代码不要急着手工改先用Matlab的“编辑”菜单把文件另存为UTF-8再重新打开大部分情况下能恢复。如果还不行就用命令行工具做一次编码转换。小技巧是写代码时尽量减少中文注释多用英文变量名和结构清晰的函数名代码本身就能表达大部分意图。5.2 同一段代码多次运行结果差异很大启发式算法本身有随机性但如果差异大到路径完全不是一个等级往往是随机数种子和初始种群没固定。调参阶段建议在main文件里固定rng(2025)这样每次运行结果可复现方便对比不同参数。最终交付版本再把种子去掉或者把种子作为参数传入这样别人也能复现结果。我在实际调试中吃过一次亏没有固定种子连续两次运行CAOA一次找到了一条很漂亮的路径另一次却一直绕远路。我怎么调权重都没用后来才发现是随机种子不同导致初始种群分布差异太大。固定种子之后每次只改一个参数做对比才能判断改动到底是有效还是随机波动。5.3 机间碰撞检查精度不够最开始我只检查了航点之间的距离结果路径图看起来没问题但用精细化仿真一跑两架飞机差点相撞。后来改成插值检查后虽然单次适应度计算时间多了一些但结果可靠性明显提高。如果你用CAOA算多机路径一定要在适应度函数里加入插值后的碰撞检测这个是整个项目里最容易被低估的模块。碰撞检查的阈值也要和无人机尺寸、飞行速度匹配。我用的安全距离是100米如果你的无人机是小机型可以缩小到30米但注意算法生成路径的精度会更高航点数可能需要增加。否则两个航点之间距离过大插值点也救不回来。5.4 目标函数尺度不一致导致权重无效如果路径长度项计算出的是2000多威胁项计算出的是0.8转角项是0.6那么权重再合理路径长度项也主导了优化方向。解决办法是所有成本项都做归一化。路径长度除以地图对角线威胁项除以采样点数转角除以最大可能转角。归一化做不好调权重基本是白调。我建议在适应度函数里加一个detail输出每次调用后可以查看四个成本项的量级。如果发现某一项长期比其他项大十倍以上优先调整归一化而不是调权重。权重是最后微调用的量级问题应该靠归一化解决。5.5 性能优化小技巧适应度函数是整个算法里调用最频繁的函数。能用矩阵运算就不用for循环比如计算路径长度直接用diff和vecnorm计算威胁强度用向量化方式一次求出所有采样点的距离。Matlab的parfor也可以用来并行计算种群适应度但要注意随机数生成和共享变量问题复杂项目里建议先把串行版本跑稳定再考虑并行。我的一个实战经验是先把插值点数设小一点调试逻辑比如威胁采样点先设10个等代码跑通了再增加到50个。这样调试阶段单次运行只要几秒钟改参数试错成本很低。等到结果稳定后再提高插值密度做最终精度验证能省下大量时间。5.6 常见问题排查速查表现象可能原因处理办法路径穿过威胁源威胁权重过低或威胁建模半径太小调大wT或增大威胁源半径多机路径交叉碰撞惩罚系数不够增大lambda检查插值点数路径转折过多转角权重过低或航点数太多调大wS减少航点数收敛曲线长时间不变种群早熟探索概率太低增大pExplore或引入变异操作航点落到地形下边界惩罚不完善在适应度函数中加入地形高度判断代码运行很慢插值点数过多或循环冗余减少插值点向量化距离计算6. 后续扩展思路与个人体会6.1 从静态规划到动态避障的扩展思路现在这个CAOA实现处理的是静态威胁源如果把它往真实场景推下一步就要考虑动态威胁和运动中的障碍物。一个可行的做法是采用滚动时域优化每隔一段时间根据最新威胁分布重新规划一次路径每次只执行前几个航点然后继续重规划。CAOA作为全局规划器单次求解时间在几十秒量级动态场景里必须加快目标函数评估速度才能满足在线重规划的需求。另一个方向是引入无人机动力学约束。现在的航点是纯几何上的没有考虑最小转弯半径和加速度限制。你可以用贝塞尔曲线或B样条对航点做光滑处理再把曲线在物理约束下做可行性检查。这样转角项成本会更低最终交给飞控的轨迹也更友好。6.2 给初学者的实现顺序建议如果你刚开始接触CAOA三维路径规划别一上来就拼多无人机协同。我的建议是先做单机单目标固定起点终点只用路径长度和威胁成本把CAOA主循环跑通。然后再加入高度和转角成本调整归一化。单机稳定后再扩展到多机协同这时候你只需要改适应度函数里的“循环所有无人机”和“机间碰撞检测”两部分。调试时先在简单地图上验证比如一个威胁源、地形用平面。如果算法连简单场景都找不到好路径问题大概率在适应度函数而非CAOA本身。可以用一个手工构造的已知最优解去测试适应度函数看输出数值是否符合预期。这一步能帮你省下很多后期排错时间。6.3 我对CAOA实现的三点体会第一CAOA不是银弹它和所有启发式算法一样需要你先把目标函数设计对。归一化、惩罚项、威胁建模这三件事做好了算法效果自然就出来。第二参数要少而精。我最终只调了权重、探索概率和Levy步长因子三个参数其他都用固定值反而比参数更多的版本稳定。第三多机协同的关键是碰撞惩罚设计而不是花哨的算法。CAOA只是帮你搜索能不能搜到安全路径取决于你告诉它什么是有价值的信息。这个项目目前告一段落但CAOA在动态威胁避障、多目标优化和更复杂约束场景下还有很大空间。我个人接下来的计划是先把动态威胁源加进来用滚动时域的方式跑通一个更接近实飞的原型。等跑出了稳定结果再回来继续分享具体实现和踩坑记录。
