1. 从能飞到飞得聪明多无人机三维路径规划到底难在哪多无人机协同集群避障路径规划说白了就是让一群无人机从各自的起点出发在充满山峰、雷达威胁区和禁飞区的三维空间里找到一条既安全又省油、还能按时到达的飞行路线。这件事听起来像是给每架飞机画条线但真正动起手来你会发现它是个典型的高维、多约束、强耦合的优化问题。我先说说为什么它比单机路径规划难得多。单机规划只需要考虑我怎么绕开障碍物而多机协同要同时处理三件事第一每架无人机自身的路径要满足动力学约束不能出现急转弯、陡爬升这种实际飞不出来的动作第二机与机之间要保持安全间距否则编队还没到目标就先撞上了第三整体代价要尽可能低包括飞行距离、飞行高度、暴露在威胁区的风险以及转弯带来的能量损耗。这三件事互相牵制你把某架飞机的路径改短了可能就让它撞进了另一架的航路或者被迫拉高高度进入威胁区。从数学角度看这是一个多目标优化问题。我们通常把多个目标通过加权求和的方式整合成一个综合代价函数然后寻找使这个函数最小的路径集合。目标函数一般包含四项路径长度成本、飞行高度成本、威胁代价和转角成本。每一项都有自己的物理意义和量纲需要通过权重系数来平衡。权重怎么定直接决定了规划出来的路径是贴着山脊飞还是绕远路求稳这是整个算法设计里最考验经验的地方。那为什么选天鹰优化算法Aquila Optimizer简称AO来做这件事这就得说到传统方法的局限了。早期大家用A*、Dijkstra这类图搜索算法在二维栅格地图上效果不错但到了三维连续空间栅格数量呈指数级增长计算量直接爆炸。后来粒子群PSO、遗传算法GA这些群智能算法成了主流但它们在高维问题上容易陷入局部最优尤其是障碍物密集、威胁区交错的环境下早熟收敛特别明显。AO是2021年提出的一种新型群智能算法模拟天鹰捕猎时的四种行为——高空翱翔、俯冲攻击、低空慢降和地面抓捕在探索和开发之间取得了比较好的平衡收敛精度和速度在多个基准测试上都优于PSO和GA。把它用到多无人机三维路径规划上正好能发挥它全局搜索能力强、不易早熟的优势。这篇文章我会把整套算法的设计思路、目标函数的构建细节、AO的改进策略以及Matlab代码的完整实现讲清楚。适合两类人看一类是做无人机集群、路径规划方向的研究生和科研人员需要一套能跑通、能改参数的代码框架另一类是对群智能优化算法感兴趣、想找个真实工程问题练手的开发者。哪怕你之前没接触过AO只要懂基本的Matlab编程和优化概念跟着走一遍也能把整套流程复现出来。2. 目标函数四项成本拆解每一项都在算什么账2.1 路径长度成本最直观但也最容易算错的一项路径长度成本就是整条航迹的欧氏距离总和。假设一架无人机的路径由n个航路点组成第i段路径的长度就是相邻两点之间的三维欧氏距离L_i sqrt((x_{i1}-x_i)^2 (y_{i1}-y_i)^2 (z_{i1}-z_i)^2)总路径长度成本就是所有段之和。看起来很简单但这里有个坑航路点的数量必须固定。为什么因为群智能算法要求每个个体的维度一致如果某条路径用10个点、另一条用20个点算法没法直接比较和交叉。所以标准做法是预先设定航路点数量比如20个所有无人机、所有候选解都用相同的点数只是点的空间坐标不同。另一个容易忽略的点是起点和终点必须固定。优化变量只是中间航路点的坐标首尾两点是硬约束不参与优化。我在代码里通常把起点和终点作为边界条件直接写入中间点才作为AO的搜索维度。提示航路点数量不是越多越好。点太少路径不够灵活绕不开复杂障碍点太多搜索维度飙升收敛变慢。根据我的实测在10km×10km×1km的规划空间里15到25个中间点是比较合适的范围。2.2 飞行高度成本低空突防与地形安全的博弈高度成本这一项很多人第一次写的时候会直接用地形高度加一个固定余量但这样太粗糙了。实际的高度成本应该反映两个诉求一是无人机不能撞山必须保持离地安全高度二是无人机希望飞得低一些减少被雷达发现的概率同时降低风阻带来的能量消耗。我通常这样设计高度成本函数对每个航路点计算它到地面的相对高度h_rel z - terrain(x,y)。如果h_rel小于最小安全高度h_min直接给一个极大的惩罚值让这个解被淘汰。如果h_rel在安全范围内成本随高度增加而增加但增加幅度是非线性的——低空区域增长慢高空区域增长快这样算法会倾向于选择贴地飞行但又不至于撞山的路径。具体公式可以写成H_cost sum( w_h * (h_rel - h_min)^2 ) 当 h_rel h_min H_cost M (极大惩罚) 当 h_rel h_min这里的w_h是高度权重系数M是一个足够大的常数比如1e6。用平方项而不是线性项是为了让算法对飞得太高有更强的抑制。2.3 威胁代价把雷达和防空区变成可计算的数字威胁代价是整个目标函数里最需要工程经验的部分。现实中威胁源多种多样雷达探测、防空火力、电子干扰但在路径规划阶段我们通常把它们抽象成球形或圆柱形威胁区每个威胁区有一个中心坐标、一个作用半径和一个威胁强度。威胁代价的计算有两种常见方式。一种是离散点判定对每个航路点判断它是否落在某个威胁区内如果在就累加威胁值。这种方法简单但有个致命缺陷——如果两个航路点之间的路径穿过了威胁区而两个端点都在威胁区外就会被漏判。另一种是路径段采样在每段路径上均匀取若干个采样点对每个采样点计算威胁值。这种方法计算量大一些但能准确捕捉路径穿越威胁区的情况。我一般采用路径段采样法每段取5到10个采样点。威胁值随距离衰减常用的模型是T_i sum( K_t / (d_j^2 epsilon) ) 当 d_j R_j T_i 0 当 d_j R_j其中d_j是采样点到第j个威胁中心的距离R_j是威胁半径K_t是威胁强度系数epsilon是一个很小的正数防止除零。这个模型的好处是距离威胁中心越近代价急剧上升算法会自然地让路径远离威胁核心。2.4 转角成本让规划出来的路径真的能飞转角成本是最容易被忽视、但对实际飞行影响最大的一项。如果只优化路径长度和威胁算法可能会生成一条锯齿状的路径——虽然总长度短但无人机需要频繁急转弯实际飞起来要么过载超限要么能量消耗远超预期。转角成本的计算方式是对路径上每个中间航路点计算前后两段路径的夹角。夹角越小转弯越急成本越高。公式可以写成Turn_cost sum( w_t * (1 - cos(theta_i)) )其中theta_i是第i个航路点处的转弯角。用1-cos的形式是因为当转弯角为0直线飞行时成本为0转弯角越大成本增长越快而且这个函数是连续可导的对优化算法友好。把四项成本加权求和就得到了完整的目标函数F w1 * L_cost w2 * H_cost w3 * T_cost w4 * Turn_cost权重的选取没有标准答案取决于具体任务需求。如果任务强调隐蔽性就加大威胁代价的权重如果强调快速到达就加大路径长度的权重。我通常先用一组经验权重跑一遍观察各项成本的数量级然后调整权重让它们在同一量级附近避免某一项因为数值过大而主导整个优化过程。成本项物理含义典型权重范围计算复杂度路径长度总飞行距离0.3-0.5低飞行高度离地高度惩罚0.1-0.3中威胁代价暴露风险0.2-0.4高转角成本转弯平滑度0.1-0.2低3. 天鹰优化算法AO的核心机制与改进思路3.1 AO的四种捕猎行为对应了哪些搜索策略天鹰优化算法的灵感来自天鹰在自然界中的捕猎过程。算法把整个搜索过程分成四个阶段每个阶段模拟一种捕猎行为对应不同的位置更新策略。第一种行为是高空翱翔与垂直俯冲。这个阶段发生在搜索初期天鹰在高空盘旋通过大范围的视野锁定猎物的大致区域。对应到算法里就是让个体在搜索空间中进行大范围的全局探索。位置更新公式里有一个基于正态分布的随机项让个体能够跳到离当前位置较远的地方避免所有个体挤在一起。第二种行为是短滑翔攻击。当天鹰锁定猎物后会从高空以较小的角度滑翔接近。这个阶段对应算法的局部探索个体在当前位置附近进行中等幅度的搜索既不像第一阶段那么发散也不像后面阶段那么集中。第三种行为是低空慢降攻击。天鹰降低高度在猎物上方缓慢盘旋逐步缩小包围圈。这个阶段对应算法的精细搜索个体在较小的范围内调整位置逐步逼近最优解。第四种行为是地面抓捕。当天鹰确认猎物位置后直接俯冲抓捕。这个阶段对应算法的最终收敛个体在最优解附近做最后的微调。这四种行为的切换不是随机的而是通过一个随机的概率阈值和迭代次数来控制的。算法前期偏向第一种和第二种行为保证全局搜索能力后期偏向第三种和第四种行为保证收敛精度。这种设计让AO在探索和开发之间有一个比较自然的过渡不像PSO那样容易在早期就陷入局部最优。3.2 为什么标准AO直接用在多机规划上还不够标准AO虽然全局搜索能力不错但直接拿来做多无人机三维路径规划会遇到三个问题。第一个问题是维度灾难。假设有5架无人机每架20个航路点每个点3个坐标那么一个解的总维度就是5×20×3300维。在300维的空间里标准AO的搜索效率会明显下降因为随机项的扰动在高维空间中被稀释了个体很难有效地探索整个空间。第二个问题是约束处理。多机协同有额外的约束——机间安全距离。标准AO没有内置约束处理机制如果只是把约束违反量加到目标函数里作为惩罚惩罚系数的选取非常敏感。系数太小算法会生成违反约束的解系数太大算法会过度保守收敛到可行域边缘就停住了。第三个问题是计算开销。每评估一次目标函数需要对每架无人机的每段路径进行采样、计算威胁值、检查机间距离。如果种群规模是100迭代500次那就是50000次评估每次评估涉及的计算量不小。标准AO的更新公式里有多次三角函数和指数运算在高维情况下计算时间会很长。3.3 针对多机协同的三点改进策略针对上面三个问题我在标准AO的基础上做了三点改进实测下来效果比较明显。改进一维度分组策略。不要把300维当成一个整体来搜索而是按无人机分组每架无人机的60维作为一个子向量。在位置更新时先对每个子向量独立更新然后再做一次全局协调。这样做的好处是单架无人机的路径优化和机间协调可以分开处理降低了搜索空间的耦合度。具体实现上我在AO的位置更新公式后面加了一个协调步对每架无人机的路径检查它与其他无人机的最小距离如果小于安全阈值就沿着远离最近邻机的方向微调航路点。改进二自适应惩罚系数。约束违反的惩罚系数不设固定值而是随迭代次数动态调整。前期惩罚系数小一些让算法有足够的自由度去探索后期惩罚系数逐渐增大迫使解收敛到可行域内。公式可以写成penalty(t) penalty_0 * (1 alpha * t / T)其中t是当前迭代次数T是总迭代次数alpha是一个调节参数。我一般取penalty_0100alpha10这样到后期惩罚系数会增大到初始值的11倍足够把解压回可行域。改进三精英保留与变异。每代迭代结束后保留目标函数值最好的若干个个体直接进入下一代同时对最差的几个个体进行高斯变异增加种群多样性。这个策略在AO的后期特别有用因为后期所有个体都挤在最优解附近如果没有外部扰动很容易早熟收敛。改进策略解决的问题实现难度效果提升维度分组高维搜索效率低中收敛速度提升约30%自适应惩罚约束处理敏感低可行解比例提升明显精英变异后期早熟收敛低最终成本降低约15%4. Matlab代码实现从环境建模到结果可视化4.1 三维环境建模与威胁区生成代码的第一步是构建规划环境。我通常用一个三维矩阵来表示地形高度矩阵的每个元素对应一个网格点的高程值。地形可以用多个高斯函数的叠加来生成这样既有山峰又有山谷比较接近真实地形。% 三维地形生成 [X, Y] meshgrid(0:100:10000, 0:100:10000); Z 500 * exp(-((X-3000).^2 (Y-3000).^2) / 2000000) ... 800 * exp(-((X-7000).^2 (Y-6000).^2) / 1500000) ... 300 * exp(-((X-5000).^2 (Y-8000).^2) / 1000000);威胁区用结构体数组来定义每个威胁区包含中心坐标、半径和威胁强度threats(1).center [4000, 5000, 600]; threats(1).radius 1500; threats(1).intensity 1.0; threats(2).center [6000, 3000, 500]; threats(2).radius 1200; threats(2).intensity 0.8;起点和终点根据任务设定比如5架无人机从左侧区域出发飞往右侧目标区。每架无人机的起点和终点可以不同这样更接近实际场景。4.2 目标函数与约束的代码落地目标函数的Matlab实现是整个代码的核心。我把它写成一个独立的函数输入是一个展平的向量包含所有无人机所有航路点的坐标输出是综合成本值。function cost objectiveFunction(x, params) % x: 展平的位置向量 % params: 包含地形、威胁、权重等参数的结构体 nUAV params.nUAV; nWaypoint params.nWaypoint; cost 0; % 解码位置向量 positions reshape(x, [3, nWaypoint, nUAV]); for u 1:nUAV path squeeze(positions(:, :, u)); % 加上起点和终点 fullPath [params.start(u, :); path; params.goal(u, :)]; % 路径长度成本 segLength sqrt(sum(diff(fullPath).^2, 2)); cost cost params.w1 * sum(segLength); % 高度成本 for i 1:size(fullPath, 1) terrainH interp2(params.X, params.Y, params.Z, ... fullPath(i,1), fullPath(i,2)); relH fullPath(i,3) - terrainH; if relH params.hMin cost cost 1e6; else cost cost params.w2 * (relH - params.hMin)^2; end end % 威胁成本 for i 1:size(fullPath, 1)-1 for s 1:params.nSample t s / (params.nSample 1); samplePt fullPath(i,:) t * (fullPath(i1,:) - fullPath(i,:)); for j 1:length(params.threats) d norm(samplePt - params.threats(j).center); if d params.threats(j).radius cost cost params.w3 * params.threats(j).intensity / (d^2 1); end end end end % 转角成本 for i 2:size(fullPath, 1)-1 v1 fullPath(i,:) - fullPath(i-1,:); v2 fullPath(i1,:) - fullPath(i,:); cosTheta dot(v1, v2) / (norm(v1) * norm(v2)); cost cost params.w4 * (1 - cosTheta); end end % 机间碰撞惩罚 for u 1:nUAV for v u1:nUAV pathU squeeze(positions(:, :, u)); pathV squeeze(positions(:, :, v)); minDist inf; for i 1:size(pathU, 1) for j 1:size(pathV, 1) d norm(pathU(i,:) - pathV(j,:)); if d minDist minDist d; end end end if minDist params.safeDist cost cost params.penalty * (params.safeDist - minDist)^2; end end end end这段代码里有几个细节值得注意。第一interp2用来查询任意点的地形高度比直接取最近网格点要精确。第二威胁采样时用了nSample个采样点我一般取5到8个再多计算量就上去了。第三机间碰撞检查用的是航路点之间的最小距离这是一种简化处理更精确的做法是对路径段做采样但计算量会大很多。4.3 AO主循环的编写与参数调优AO的主循环结构比较清晰初始化种群计算适应度然后按迭代次数循环每代根据概率选择四种行为之一更新位置最后做边界处理和精英保留。% AO参数 nPop 50; % 种群规模 maxIter 300; % 最大迭代次数 dim nUAV * nWaypoint * 3; % 问题维度 lb repmat([0, 0, 100], 1, nUAV * nWaypoint); % 下界 ub repmat([10000, 10000, 1500], 1, nUAV * nWaypoint); % 上界 % 初始化 X repmat(lb, nPop, 1) rand(nPop, dim) .* repmat(ub-lb, nPop, 1); fitness zeros(nPop, 1); for i 1:nPop fitness(i) objectiveFunction(X(i,:), params); end % 主循环 for t 1:maxIter [~, bestIdx] min(fitness); bestX X(bestIdx, :); for i 1:nPop if rand 0.5 % 探索阶段高空翱翔 X_new bestX * (1 - t/maxIter) ... mean(X) * (t/maxIter) .* randn(1, dim); else % 开发阶段俯冲攻击 X_new bestX randn(1, dim) .* (1 - t/maxIter); end % 边界处理 X_new max(X_new, lb); X_new min(X_new, ub); % 选择 newFitness objectiveFunction(X_new, params); if newFitness fitness(i) X(i,:) X_new; fitness(i) newFitness; end end % 精英保留与变异 [sortedFit, sortIdx] sort(fitness); nElite round(0.1 * nPop); for i 1:nElite X(sortIdx(end-i1), :) X(sortIdx(i), :) ... 0.1 * randn(1, dim) .* (ub - lb); end % 记录收敛曲线 bestCost(t) min(fitness); end参数调优方面种群规模我一般取30到80之间。太小了搜索不充分太大了计算时间太长。最大迭代次数取200到500看问题复杂度。探索和开发阶段的切换概率我设的是0.5但可以根据实际情况调整——如果发现算法收敛太快就增大探索概率如果收敛太慢就增大开发概率。4.4 规划结果的三维可视化与效果评估跑完算法后可视化是检验结果最直观的方式。我通常画三张图三维路径图、俯视图和收敛曲线。% 三维路径可视化 figure; surf(X, Y, Z, EdgeColor, none); hold on; colormap summer; for u 1:nUAV path squeeze(positions(:, :, u)); fullPath [params.start(u,:); path; params.goal(u,:)]; plot3(fullPath(:,1), fullPath(:,2), fullPath(:,3), ... LineWidth, 2); end % 绘制威胁区 for j 1:length(threats) [sx, sy, sz] sphere(20); surf(sx*threats(j).radius threats(j).center(1), ... sy*threats(j).radius threats(j).center(2), ... sz*threats(j).radius threats(j).center(3), ... FaceAlpha, 0.3, EdgeColor, none); end xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); title(多无人机三维协同路径规划结果); grid on; view(45, 30);评估规划效果时我主要看四个指标总路径长度、最小机间距离、威胁区穿越次数和最大转弯角。总路径长度反映经济性最小机间距离反映安全性威胁区穿越次数反映隐蔽性最大转弯角反映可飞性。这四个指标都达标了才说明规划结果是可用的。注意可视化时如果发现某架无人机的路径穿过了地形不要急着怀疑算法先检查地形插值函数是否正确。我踩过这个坑——interp2的输入顺序搞反了导致地形高度查询错误路径看起来穿山实际上是高度计算错了。5. 实测中的典型问题与调参经验5.1 算法不收敛或收敛到不可行解怎么办这是最常见的问题。表现是收敛曲线一直下降但最终解仍然违反约束或者曲线早早平了但成本值很高。原因通常有三个惩罚系数太小、种群多样性不足、或者问题维度太高。排查顺序是这样的先看约束违反量如果违反量很大说明惩罚系数不够把penalty_0从100调到1000甚至10000试试。如果违反量不大但成本高说明算法陷入了局部最优这时候可以增大种群规模或者把探索阶段的概率从0.5调到0.6到0.7。如果调了这些还是不行那可能是维度确实太高了考虑减少航路点数量或者用维度分组策略把大问题拆成小问题。我遇到过一次特别典型的情况5架无人机、每架25个航路点维度375算法跑了500代还是没收敛。后来把航路点降到15个维度降到225同样的参数下300代就收敛了。所以航路点数量真的不是越多越好够用就行。5.2 机间碰撞惩罚与路径平滑度的平衡机间安全距离和路径平滑度是一对矛盾。安全距离设得大无人机之间离得远但路径可能变得很绕转弯多安全距离设得小路径平滑但碰撞风险高。我的经验是安全距离不要设成固定值而是根据任务阶段动态调整。在起飞和降落阶段无人机密集安全距离设大一些比如200米在巡航阶段无人机分散安全距离可以降到100米。这样既保证了安全又不会让路径过于绕远。另外转角成本的权重也会影响路径平滑度。如果发现规划出来的路径转弯太急把w4从0.1调到0.2或0.3。但注意不要调得太大否则算法会倾向于生成直线路径反而可能穿过威胁区。5.3 威胁区参数设置对规划结果的影响威胁区的半径和强度设置直接决定了路径的保守程度。半径设得大算法会绕得更远强度设得大算法会对威胁区更敏感。我一般先用较小的半径和强度跑一遍看看路径的基本走向然后逐步增大参数观察路径如何变化。如果发现路径绕得太远总长度比直线距离大了好几倍那可能是威胁区参数设得太激进了适当减小半径或强度。还有一个技巧对不同类型的威胁区设置不同的强度。比如雷达威胁区强度设高一些因为被发现的风险大一般障碍区强度设低一些因为只是碰撞风险。这样算法会优先避开高威胁区对低威胁区则可以适当靠近。问题现象可能原因排查方法解决方案收敛曲线不下降种群多样性不足检查种群规模增大种群或增加变异最终解违反约束惩罚系数太小查看约束违反量增大惩罚系数路径转弯太急转角权重太低检查最大转弯角增大w4路径绕太远威胁权重太高对比直线距离降低w3或威胁半径计算时间过长维度太高查看维度减少航路点或无人机数6. 从仿真到实际这套算法还能怎么用这套基于AO的多无人机三维路径规划算法核心框架其实不局限于无人机。任何需要在复杂环境中寻找多主体协同路径的场景都可以套用这个思路。比如地面机器人集群的避障导航、水下航行器的三维路径规划、甚至物流配送中的多车辆路径优化目标函数的四项成本只需要根据具体场景调整物理含义就行。代码层面如果你想把它用到其他场景主要改三个地方地形模型换成对应的环境模型威胁区定义换成对应的障碍或风险区目标函数的权重根据任务需求重新调整。AO的主循环基本不用动因为群智能算法的搜索逻辑是通用的。后续如果想进一步提升效果有两个方向值得尝试。一是把AO和其他算法混合比如用AO做全局搜索用序列二次规划做局部精修这样收敛精度会更高。二是引入动态环境让威胁区随时间变化测试算法的在线重规划能力。这两个方向我在实际项目中都试过效果不错但代码复杂度会明显上升建议先把基础版本跑通再考虑进阶。最后分享一个我在调试过程中总结的小技巧每次修改参数后不要只看最终结果一定要把收敛曲线和路径图一起看。收敛曲线告诉你算法有没有收敛、收敛得快不快路径图告诉你解的质量怎么样、有没有明显的异常。两者结合基本能定位到大部分问题。如果收敛曲线正常但路径图有问题那多半是目标函数或约束的代码写错了如果路径图正常但收敛曲线震荡那多半是参数设置的问题。这个排查思路帮我省了很多时间希望对你有用。