matlab 路径规划，灰狼算法GWO
有三种算法对比，分别是GWO，MP_GWO，CSGWO，有两种改进的算法，
三维地图环境，算法选择架构清晰，有程序流程图。
多机分布式规划，集群协同任务框架。

以下文字及示例代码仅供参考

---

1 研究背景与目标

在复杂三维山地、城市峡谷或灾后救援场景中，多无人机（UAV）需要同时完成：

• 单机无碰撞最优航迹
• 集群协同（时间同步、队形保持、通信连通）

传统 GWO 易早熟、收敛慢。本文给出 MP-GWO（多种群）与 CS-GWO（布谷鸟混合）两种改进算法，并搭建 “单机-集群”可切换架构，一键完成：

• 三维栅格/数字高程地图建模
• 三种算法对比（收敛曲线、路径长度、实时性）
• 分布式任务分配接口（支持后续扩展 Consensus/MPC）

---

2 算法选型与改进要点

算法	核心改进	适用场景	代码文件
GWO	标准 α-β-δ 等级狩猎	小规模、单峰	GWO.m
MP-GWO	多种群并行 + 信息迁移	多峰、协同规划	MP_GWO.m
CS-GWO	莱维飞行 + 随机抛弃	高维、复杂障碍	CS_GWO.m

参考文献：[1]

---

3 程序整体架构

├─ main.m               % 一键运行（算法选择、地图、集群数量）
├─ Map3D/               % 三维地图生成
│   ├─ digitalTerrain.m % 数字高程(DEM)
│   └─ addObstacles.m   % 障碍模型
├─ Algorithm/           % 三种算法封装
│   ├─ GWO.m
│   ├─ MP_GWO.m
│   └─ CS_GWO.m
├-- Swarm/              % 多机协同
│   ├─ TaskAlloc.m      % 分布式任务分配（拍卖/一致性）
│   └─ Formation.m      % 队形约束
├─ Plot3D/              % 三维可视化
│   ├─ plotPath3D.m
│   └─ plotConvergence.m
└─ Result/              % 自动保存 .fig & .mat

流程图如下（可在 README 中插入高清 SVG）

---

4 关键代码精要

4.1 三维目标函数（高度+威胁+转角）

function cost = Obj3D(path, model)
% path: [x,y,z] 航迹点
% model 包含 DEM、威胁球、最大转角/爬升
L = sum(sqrt(sum(diff(path).^2,2)));       % 路径长度
H = sum(abs(diff(path(:,3))));             % 高度变化
T = sum(model.threat(path(:,1),path(:,2),path(:,3))); % 威胁指数
A = sum(abs(atan2(diff(path(:,2)),diff(path(:,1)))));
cost = L + 0.3*H + 0.5*T + 0.2*A;
end

4.2 算法选择入口（main.m）

clc; clear; close all;
%% 用户配置
ALG = 2;            % 1-GWO 2-MP-GWO 3-CS-GWO
UAV_N = 4;          % 集群数量
MaxIter = 200;
PopSize = 40;

%% 三维地图 & 任务
model = Map3D();
tasks = SwarmTask(model, UAV_N); % 返回起点/终点列表

%% 并行或串行求解
parfor u = 1:UAV_N
    switch ALG
        case 1, path{u} = GWO(tasks(u), PopSize, MaxIter);
        case 2, path{u} = MP_GWO(tasks(u), PopSize, MaxIter);
        case 3, path{u} = CS_GWO(tasks(u), PopSize, MaxIter);
    end
end

%% 结果展示
plotPath3D(model, path);
plotConvergence(model, path);
save('Result/Result.mat','path');

4.3 MP-GWO 核心片段（多种群迁移）

function [Best] = MP_GWO(task, N, MaxIter)
% 分为 3 个子群
nPop = floor(N/3);
for g = 1:3
    pop{g} = initPop(nPop, task);
end
for iter = 1:MaxIter
    for g = 1:3
        pop{g} = GWO_core(pop{g}, task); % 标准 GWO 内部迭代
    end
    % 每 20 代迁移一次最优 α
    if mod(iter,20)==0
        [~,bestIdx] = min(cellfun(@(x)x.alpha.fit, pop));
        target = mod(bestIdx,3)+1;
        pop{target}.wolf(end,:) = pop{bestIdx}.wolf(1,:);
    end
end
[~,Best] = min(cellfun(@(x)x.alpha.fit, pop));
end

---