多无人机协同路径规划：星雀优化算法与Matlab实现

1. 多无人机协同路径规划到底在解决什么问题1.1 从单机避障到集群协同的跨越单架无人机的路径规划本质上是在一张三维地图上找一个从起点到终点的可行曲线避开障碍物、满足飞行约束就行。但一旦变成多架无人机同时执行任务问题的性质就完全变了。你要考虑的不再是一条线而是多条线之间的时空关系——它们会不会在某个时刻撞上会不会同时挤进同一条狭窄通道会不会因为某架飞机的避让动作导致另一架被迫偏离安全走廊。这就是多无人机协同集群路径规划的核心难点。它要同时处理三个维度的耦合空间维度上每架飞机各自的三维航迹时间维度上到达同一区域的时间窗口以及任务维度上各自的目标点分配。任何一维处理不好轻则任务效率下降重则发生空中冲突。我接触这个方向是从一个模拟项目开始的当时的需求是让一个小型四旋翼编队在城市楼宇之间完成区域巡检。最初想得很简单每架飞机单独跑一遍A*算法不就行了实测下来问题一大堆两架飞机在同一个路口对头飞谁都不让有的飞机为了绕开障碍物飞到了另一架的正上方垂直间隔不到五米还有的飞机路径拐来拐去能耗比直线飞行高了将近一倍。这些问题的根源在于单机规划器根本不知道其他飞机的存在。1.2 为什么目标函数要同时管路径、高度、威胁和转角很多人第一次看到这个目标函数会疑惑为什么要把路径长度、飞行高度、威胁代价和转角代价放在一起优化分开处理不行吗分开处理的问题在于这些指标之间存在强烈的耦合关系。举个实际例子你想缩短路径长度最直接的办法是降低飞行高度从楼宇之间穿过去。但低空飞行意味着地面威胁更大同时楼宇间的湍流和障碍物密度更高转角次数也会增加。反过来你为了减少转角把路径拉直路径长度可能反而变长因为直线路径未必经过威胁最低的区域。所以这四个指标必须放在一个统一的目标函数里联合优化。路径长度决定了任务的总航程和能耗飞行高度影响安全性和被探测概率威胁代价量化了经过危险区域的代价转角代价则直接关系到飞行器的机动能力和能量消耗。四者加权求和权重系数根据具体任务场景调整才能得到一条综合最优的航迹。1.3 星雀优化算法为什么适合这个问题星雀优化算法NOA是一种群智能优化算法它的核心灵感来自星雀群体的觅食和迁徙行为。相比遗传算法、粒子群算法这些经典方法NOA有几个特点让它特别适合多无人机路径规划。第一NOA的种群更新机制包含探索和开发两个阶段的动态平衡。在路径规划中前期需要大范围搜索可能的航迹空间后期需要在最优解附近精细调整NOA的机制天然契合这个需求。第二NOA对高维优化问题的收敛性比较好。多无人机路径规划的参数空间维度很高——三架无人机、每架20个航路点每个点三个坐标那就是180个优化变量传统方法很容易陷入局部最优。第三NOA的种群多样性保持能力较强这对于多机协同问题很重要因为你需要同时维护多条有差异的候选路径避免所有飞机都收敛到同一条航线上。当然NOA也不是万能的。它的参数调校对结果影响很大种群规模和迭代次数的选择需要根据问题规模来定。后面我会详细讲参数怎么调。2. 目标函数与约束条件的数学建模细节2.1 路径长度代价的计算与归一化路径长度代价是最直观的一项。假设第i架无人机的航迹由n个航路点组成那么路径长度就是相邻航路点之间欧氏距离的累加L_i Σ_{j1}^{n-1} ||P_{j1} - P_j||其中P_j (x_j, y_j, z_j)是第j个航路点的三维坐标。但这里有个坑不同无人机的路径长度量级可能差很多。比如一架飞直线距离500米另一架绕路飞了1200米如果直接相加长路径的那架会主导整个目标函数短路径的优化就被淹没了。所以必须做归一化处理。常见的做法是除以该无人机起点到终点的直线距离得到一个无量纲的比值。这样每架无人机的路径代价都在1到3之间量级统一。注意归一化基准的选择会影响优化结果的偏向性。用直线距离做基准优化器会倾向于让所有飞机都尽量走直线用平均路径长度做基准则更注重整体均衡。我一般用直线距离因为物理意义更明确。2.2 飞行高度代价的设计逻辑高度代价的设计要看具体任务。如果是低空巡检任务你希望无人机飞得越低越好因为越低分辨率越高、越省电。但太低又容易撞楼、撞树、受地面人员干扰。所以高度代价通常设计成一个分段函数当飞行高度低于最低安全高度H_min时代价急剧上升惩罚项当高度在H_min到H_opt之间时代价随高度增加而缓慢上升当高度超过H_opt后代价快速上升因为高空风大、能耗高、被探测概率大数学上可以写成C_h(z) k1 * (H_min - z)^2, z H_min k2 * (z - H_opt)^2, z H_opt 0, H_min z H_opt其中k1远大于k2确保无人机不会飞到最低安全高度以下。H_opt一般取任务区域最高障碍物高度加20到30米。2.3 威胁代价的量化方法威胁代价是最难量化的一项因为“威胁”本身是个模糊概念。在实际工程中我们通常把威胁源建模成一个个球形或圆柱形的危险区域每个区域有一个威胁等级。无人机经过这些区域时根据距离中心的远近和威胁等级计算代价。具体做法是对每个航路点计算它到所有威胁源中心的距离d。如果d小于威胁半径R则代价为C_t Σ W_k * exp(-d_k^2 / (2 * σ_k^2))其中W_k是第k个威胁源的权重σ_k控制衰减速度。这个高斯型函数的好处是平滑可导优化算法处理起来比较稳定。如果d大于R代价为零。实操心得威胁半径不要设得太小否则优化器会贴着威胁边缘飞实际飞行中稍微有点风就进去了。我一般把威胁半径放大1.2到1.5倍作为规划时的安全裕度。2.4 转角代价与飞行可行性约束转角代价直接关系到无人机能不能飞出来。固定翼无人机的转弯半径很大转角超过一定角度就转不过去了。多旋翼虽然灵活但急转弯会导致速度骤降、能耗激增而且相机云台会抖动影响任务效果。转角代价的计算方式是对航迹上每个中间航路点计算前后两段航迹的夹角θ。如果θ小于最大允许转角θ_max代价为零否则代价为C_θ k_θ * (θ - θ_max)^2k_θ是一个较大的权重确保优化器不会产生不可飞的急转弯。θ_max根据机型确定多旋翼一般取60到90度固定翼取15到30度。除了转角还有几个硬约束必须满足最小航路段长度太短的航段飞控来不及响应、最大爬升率、最大飞行速度。这些通常作为惩罚项加入目标函数或者在优化过程中直接裁剪不可行解。2.5 多机协同的防碰撞约束多机协同最核心的约束就是防碰撞。两架无人机之间的安全距离d_safe通常取5到10米根据定位精度和机身尺寸确定。在目标函数中碰撞代价可以写成C_collision Σ_i Σ_{ji} max(0, d_safe - ||P_i(t) - P_j(t)||)^2注意这里用的是时间对齐后的位置。因为两架飞机可能在不同时间经过同一空间点如果时间错开就不算碰撞。所以协同规划必须同时优化空间路径和时间分配。实际实现时我通常把时间离散成若干段每段检查一次所有飞机的位置对。如果任意一对飞机在任意时刻的距离小于d_safe就产生碰撞代价。这个代价的权重设得非常大确保优化器优先消除碰撞。3. 星雀优化算法的实现与调参实录3.1 种群初始化怎么让初始路径不那么离谱NOA的种群初始化直接影响收敛速度和解的质量。如果随机生成一堆乱七八糟的路径算法要花很多代才能把它们拉回可行区域。我的做法是采用“引导随机初始化”先在起点和终点之间生成一条直线作为参考路径在直线附近随机扰动生成初始航路点扰动幅度随航路点序号呈正弦变化中间大、两端小对每个航路点检查是否在障碍物内部如果在就沿梯度方向推到障碍物表面这样生成的初始种群既有多样性又不会偏离可行区域太远。实测下来比纯随机初始化的收敛速度快30%到40%。% 引导随机初始化示例 for i 1:pop_size for j 1:num_waypoints t j / (num_waypoints 1); base_point start_pos t * (end_pos - start_pos); perturbation sin(pi * t) * randn(1, 3) .* max_deviation; waypoint base_point perturbation; % 障碍物推离 waypoint push_out_of_obstacles(waypoint, obstacles); population(i, j, :) waypoint; end end3.2 适应度函数的设计与惩罚项处理适应度函数就是目标函数的倒数或者负值。但直接优化目标函数有个问题不可行解的适应度可能比可行解还好因为惩罚项还没大到足以压制它。我的处理方式是分层惩罚第一层硬约束违反碰撞、飞出边界、低于最低高度惩罚系数取1e6第二层软约束违反转角过大、航段过短惩罚系数取1e3第三层目标函数本身路径、高度、威胁、转角加权和这样确保任何可行解都优于不可行解优化器会先找到可行域再在里面精调。踩过的坑惩罚系数不是越大越好。如果硬约束惩罚设成1e12适应度函数的数值范围会跨越十几个数量级浮点精度不够优化器会失去梯度信息。1e6是个比较稳妥的选择。3.3 探索与开发阶段的平衡策略NOA的核心更新公式包含探索和开发两个阶段。探索阶段模拟星雀在未知区域的大范围搜索开发阶段模拟它们在已知食物源附近的精细觅食。两个阶段的切换由自适应因子控制A 2 * (1 - t / T_max)其中t是当前迭代次数T_max是最大迭代次数。A从2线性衰减到0。当A较大时算法偏向探索当A较小时偏向开发。但这个线性衰减有个问题前期探索太慢后期开发不够精细。我改成非线性衰减A 2 * (1 - (t / T_max)^2)这样前期A下降得慢给探索更多时间后期A下降得快快速进入精细开发。实测收敛精度提升了约15%。3.4 参数调优的实操经验NOA的主要参数有种群规模N、最大迭代次数T_max、探索开发切换因子、以及各种权重系数。我整理了一个调参参考表参数推荐范围影响调参建议种群规模N50-200太小易早熟太大计算慢无人机数量×20起步最大迭代T_max500-2000太小不收敛太大浪费时间看适应度曲线拐点路径权重w_L0.3-0.5越大路径越短任务时间紧就调大高度权重w_H0.1-0.3越大飞得越保守安全要求高就调大威胁权重w_T0.2-0.4越大越远离威胁根据威胁等级调转角权重w_θ0.1-0.2越大路径越平滑固定翼调大多旋翼可小调参的顺序建议是先调种群规模和迭代次数确保算法能收敛再调四个目标权重根据任务需求平衡最后微调探索开发因子优化收敛速度。4. 完整实操流程与Matlab代码解析4.1 环境建模与参数配置第一步是建立三维环境模型。我通常用一个三维栅格地图表示每个栅格存储障碍物信息和威胁等级。栅格分辨率取1到2米太粗会漏掉小障碍物太细计算量爆炸。% 环境参数 map_size [1000, 1000, 200]; % 长宽高单位米 grid_res 2; % 栅格分辨率米 num_drones 3; % 无人机数量 num_waypoints 15; % 每架无人机的航路点数 % 障碍物定义长方体 obstacles [ 200, 300, 0, 150, 200, 80; % x起点, y起点, z起点, x长度, y长度, z长度 500, 600, 0, 200, 150, 100; 700, 200, 0, 100, 300, 60; ]; % 威胁源定义球形 threats [ 400, 500, 50, 100, 0.8; % x, y, z, 半径, 威胁等级 600, 700, 80, 120, 0.6; ]; % 无人机参数 drone_params.max_turn_angle 60; % 最大转角度 drone_params.min_segment_length 10; % 最小航段长度米 drone_params.safe_distance 8; % 安全距离米 drone_params.min_height 30; % 最低飞行高度米 drone_params.max_height 150; % 最高飞行高度米4.2 目标函数计算的完整实现目标函数是优化的核心我把它拆成几个子函数分别计算最后加权求和。function cost evaluate_path(path, drone_params, obstacles, threats, other_paths) % path: num_waypoints x 3 矩阵 % other_paths: 其他无人机的路径用于碰撞检测 % 1. 路径长度代价 seg_lengths sqrt(sum(diff(path).^2, 2)); L sum(seg_lengths); L_ref norm(path(end,:) - path(1,:)); % 直线距离 cost_length L / L_ref; % 2. 高度代价 z path(:,3); cost_height 0; for i 1:length(z) if z(i) drone_params.min_height cost_height cost_height 100 * (drone_params.min_height - z(i))^2; elseif z(i) drone_params.max_height cost_height cost_height 10 * (z(i) - drone_params.max_height)^2; end end cost_height cost_height / length(z); % 3. 威胁代价 cost_threat 0; for i 1:size(path,1) for k 1:size(threats,1) d norm(path(i,:) - threats(k,1:3)); if d threats(k,4) cost_threat cost_threat threats(k,5) * exp(-d^2/(2*(threats(k,4)/2)^2)); end end end cost_threat cost_threat / size(path,1); % 4. 转角代价 cost_turn 0; for i 2:size(path,1)-1 v1 path(i,:) - path(i-1,:); v2 path(i1,:) - path(i,:); cos_theta dot(v1, v2) / (norm(v1) * norm(v2)); theta acosd(max(-1, min(1, cos_theta))); if theta drone_params.max_turn_angle cost_turn cost_turn (theta - drone_params.max_turn_angle)^2; end end % 5. 碰撞代价 cost_collision 0; for j 1:length(other_paths) other other_paths{j}; min_len min(size(path,1), size(other,1)); for t 1:min_len d norm(path(t,:) - other(t,:)); if d drone_params.safe_distance cost_collision cost_collision 1e6 * (drone_params.safe_distance - d)^2; end end end % 加权求和 w [0.4, 0.2, 0.3, 0.1]; % 路径、高度、威胁、转角权重 cost w(1)*cost_length w(2)*cost_height w(3)*cost_threat w(4)*cost_turn cost_collision; end4.3 NOA主循环的实现要点NOA的主循环包括种群更新、适应度评估、最优解记录三个部分。多无人机的情况下每架无人机独立优化但适应度评估时要考虑其他无人机当前的最优路径。% NOA主循环 for iter 1:max_iter % 计算自适应因子 A 2 * (1 - (iter/max_iter)^2); % 对每架无人机分别优化 for drone_idx 1:num_drones % 获取其他无人机当前最优路径 other_paths {}; for j 1:num_drones if j ~ drone_idx other_paths{end1} best_paths{j}; end end % 种群更新 for i 1:pop_size % 探索阶段 if rand A/2 new_pos population(drone_idx).pos(i,:) ... randn(1, dim) .* exploration_range; else % 开发阶段向最优解靠近 new_pos population(drone_idx).pos(i,:) ... randn(1, dim) .* (best_paths{drone_idx}(:) - population(drone_idx).pos(i,:)) * 0.1; end % 边界处理 new_pos max(min(new_pos, ub), lb); % 评估适应度 new_path reshape(new_pos, num_waypoints, 3); new_cost evaluate_path(new_path, drone_params, obstacles, threats, other_paths); % 更新个体最优 if new_cost population(drone_idx).cost(i) population(drone_idx).pos(i,:) new_pos; population(drone_idx).cost(i) new_cost; end end % 更新全局最优 [min_cost, min_idx] min(population(drone_idx).cost); if min_cost best_costs(drone_idx) best_costs(drone_idx) min_cost; best_paths{drone_idx} reshape(population(drone_idx).pos(min_idx,:), num_waypoints, 3); end end % 记录收敛曲线 convergence(iter) sum(best_costs); end4.4 结果可视化与航迹平滑优化出来的航路点直接连起来是折线实际飞行需要平滑处理。我通常用三次B样条曲线拟合航路点生成光滑可飞的航迹。% 航迹平滑 t linspace(0, 1, num_waypoints); t_smooth linspace(0, 1, num_waypoints * 10); for drone_idx 1:num_drones path best_paths{drone_idx}; % 三次样条插值 xx spline(t, path(:,1), t_smooth); yy spline(t, path(:,2), t_smooth); zz spline(t, path(:,3), t_smooth); smooth_paths{drone_idx} [xx, yy, zz]; end % 三维可视化 figure; hold on; % 画障碍物 for i 1:size(obstacles,1) draw_cuboid(obstacles(i,:)); end % 画威胁源 for i 1:size(threats,1) draw_sphere(threats(i,1:3), threats(i,4)); end % 画航迹 colors {r, g, b, m, c}; for drone_idx 1:num_drones plot3(smooth_paths{drone_idx}(:,1), smooth_paths{drone_idx}(:,2), ... smooth_paths{drone_idx}(:,3), colors{mod(drone_idx-1,5)1}, LineWidth, 2); end grid on; xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); title(多无人机协同路径规划结果); view(45, 30);5. 常见问题排查与避坑指南5.1 算法不收敛或收敛到局部最优这是最常见的问题。表现是适应度曲线早早平了但解的质量明显不行。原因通常有三个种群多样性不足、探索开发平衡没调好、惩罚系数设置不当。排查步骤先看初始种群的适应度分布如果所有个体的适应度都差不多说明初始化太集中了需要增大扰动幅度。再看收敛曲线如果前50代就平了说明探索阶段太短把A的衰减改慢一点。最后检查惩罚系数如果不可行解的适应度跟可行解差不多说明惩罚太小了。我的经验是多无人机路径规划至少需要100个种群、500代迭代。如果问题规模大5架以上种群要加到200以上。5.2 多机碰撞无法完全消除有时候优化结果里两架飞机还是靠得太近。原因可能是安全距离设得太大可行域太窄或者时间对齐没做好两架飞机其实在不同时间经过同一区域但被误判为碰撞。解决办法第一检查时间对齐逻辑。如果两架飞机的速度不同同一序号航路点的时间戳是不一样的需要按实际时间插值后再比较位置。第二适当减小安全距离或者给每架飞机分配不同的高度层用高度差来保证安全。第三增加碰撞代价的权重让它成为主导项。实操心得我通常给每架无人机分配一个基准高度比如第一架飞80米第二架飞100米第三架飞120米。这样即使水平投影重叠垂直方向也有20米的安全间隔。当然这要求高度层之间有足够的空间。5.3 航迹转角过大导致不可飞优化出来的路径有时候会出现锐角转弯多旋翼勉强能飞但很危险固定翼根本飞不了。这是因为转角代价的权重太小或者最大转角约束没加进去。处理方式把转角代价的权重提高到0.2以上同时把最大转角作为硬约束超过就直接判定为不可行解。另外航迹平滑那一步也能缓解这个问题B样条曲线会把尖角磨圆。但如果原始航路点的转角就超过90度平滑也救不回来必须在优化阶段就压制住。5.4 计算时间过长多无人机路径规划的计算量随无人机数量和航路点数量呈指数增长。三架飞机、15个航路点跑一次大概要几分钟五架飞机、20个航路点可能要跑半小时以上。加速的办法有几个一是用并行计算每架无人机的优化可以并行跑Matlab的parfor直接支持。二是减少航路点数量用10到12个点通常就够了平滑阶段再插值加密。三是降低种群规模和迭代次数但这样解的质量会下降需要权衡。四是把适应度函数里的循环向量化Matlab的矩阵运算比循环快很多。问题现象可能原因排查方法解决方案适应度曲线早平种群多样性不足检查初始种群分布增大初始化扰动解的质量差探索阶段太短看A因子衰减曲线改非线性衰减碰撞消不掉安全距离过大检查最小间距减小安全距离或分层转角过大转角权重太小检查转角代价占比提高权重或加硬约束计算太慢种群/迭代过大计时各模块并行化或向量化路径贴障碍物威胁半径太小检查威胁代价放大威胁半径1.5倍5.5 权重系数怎么调才合理权重系数没有标准答案取决于任务需求。我的一般原则是安全相关的权重威胁、碰撞、高度优先保证性能相关的权重路径长度、转角其次。具体操作时先把安全权重设大跑一遍看结果是否安全然后逐步减小安全权重、增大性能权重直到找到安全性和效率的平衡点。一个实用的技巧是先单独优化每个目标看看每个目标能达到的最优值是多少然后根据任务需求给每个目标分配一个合理的期望值权重反比于期望值。比如路径长度期望是1.2倍直线距离高度期望是100米威胁代价期望是0.1转角期望是0那么权重可以设为1/1.2、1/100、1/0.1、1/1再归一化。这个内容后续还可以这样扩展把静态威胁换成动态威胁比如移动的禁飞区或者其他飞行器把二维规划扩展到三维空间中的编队保持把单次任务规划扩展到多任务序列规划。这些方向都有实际需求算法框架基本不变主要是目标函数和约束条件需要调整。