✅作者简介：热爱科研的Matlab仿真开发者，擅长数学建模、数据处理、模型创新、算法改进、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：完整代码获取 定制创新 论文复现私信

 👇 关注我领取海量matlab电子书和数学建模资料 

🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。

🔥 内容介绍 

大家好，今天给大家带来一套完整、可直接在 MATLAB 中复现的二维多智能体仿真工程。这套工程集成了 A * 栅格路径规划、轨迹平滑、基于符号距离场 SDF 的障碍物膨胀、人工势场 APF 避障、PID 轨迹跟踪以及主从式编队控制，模拟 1 个领航智能体搭配 2 个跟随智能体的机器人集群运动。

它是作者整个研究链路的第一阶段基础版本，后续会以此为基础拓展三维空间、六自由度无人机动力学、模型预测控制 MPC 等更高级的方案。不管是课程设计、毕设入门、算法对比基线，还是做无人机集群仿真的前期验证，这套框架都非常合适。

很多刚入门多智能体协同的同学，容易把 “路径规划” 和 “编队控制” 割裂开学习：单独跑 A*、单独调势场避障、单独写 PID 跟踪。但真实集群任务，是领航者先规划全局可行路径，跟随者在跟随领航者的同时，维持相对队形、避开环境障碍物。这份工程恰好把这一整套流程串联起来，是非常好的学习范本。

---

一、项目整体介绍

这套仿真系统面向二维平面环境，主体是机器人 / 无人机质点动力学模型，采用经典的主从（Leader-Follower）编队架构：领航者负责全局路径规划，沿着规划好的平滑轨迹运动；两名跟随者在领航者的局部坐标系下，维持固定的相对偏移，形成稳定编队。

整个工程实现的模块清单：

1. 基于栅格地图的 A * 全局路径规划

2. 轨迹平滑与时间参数化处理

3. 类无人机质点动力学模型（二维）

4. PID 轨迹跟踪控制器

5. 符号距离场 SDF，用于障碍物膨胀与距离梯度计算

6. 人工势场 APF 局部避障算法

7. 主从编队控制逻辑

8. 集群运动实时动画可视化

仿真地图支持两种模式：固定确定地图，或是随机生成障碍物地图，方便大批量对比实验。

✅ 当前已经稳定实现的功能

• A * 算法能够搜索出无碰撞的可行全局路径

• 对原始折线路径做平滑处理，生成连续轨迹

• 领航者可以稳定跟踪平滑后的参考轨迹

• 跟随智能体能够保持预设的相对编队偏移量

• 完整动画输出，直观展示所有智能体的运动全过程

⚠️ 现有方案的固有短板（论文 / 毕设可以重点写）

虽然整套仿真可以正常运行，但这套基于经典人工势场 + PID 的方案存在经典局限性，也是作者后续研究的出发点：

• 智能体靠近障碍物拐角时，排斥力容易引发震荡；

• 在狭窄通道内，SDF 梯度引导的避障力不一定能引导跟随者顺利通行；

• 人工势场天然存在局部极小点问题，智能体有可能卡在局部位置无法到达目标。

这些问题不是代码 bug，而是 APF 基础方法本身的固有缺陷，也是我们后续引入速度障碍法 ORCA、MPC 模型预测控制等高级方案的动因。

二、工程目录结构（模块化设计，方便二次开发）

整套代码采用模块化拆分，每个功能单独封装函数，耦合度低，后续替换算法非常方便。

src/
 ├── mainsimulation.m          % 主入口脚本，一键启动仿真
 │
 ├── environment/
 │    └── environment.m        % 环境、障碍物、栅格地图生成
 │
 ├── planners/
 │    ├── planner.m            % A*规划器主体
 │    └── smooth_path.m        % 轨迹平滑、时间参数化
 │
 ├── dynamics/
 │    └── dynamics.m           % 智能体质点动力学模型
 │
 ├── controller/
 │    └── pidcontroller.m      % PID跟踪控制器
 │
 ├── obstacle avoidance/
 │    ├── avoidanceField.m     % 人工势场排斥力计算
 │    └── avoidance_sdf.m      % SDF距离场避障接口
 │
 └── utils/
      └── buildSDF.m           % 符号距离场SDF生成函数

visualization/
 └── animateswarm.m            % 集群动画绘制脚本

results/
 └── multi agent 2d.pdf        % 结果图、实验报告输出

模块化的好处很明显：如果你只想更换路径规划算法，只需要修改 planner 文件夹；想要换控制器，只改动 pidcontroller；想要升级避障策略，直接替换 avoidance 里的函数，不需要大面积改动主程序。这也是科研仿真工程推荐的规范写法。

三、仿真运行步骤

1. 打开 MATLAB；

2. 添加所有源码文件夹到搜索路径：

addpath(genpath('src'));
addpath('visualization');

1. 运行主脚本：

run('src/mainsimulation.m');

执行之后，程序会自动依次完成：

• 生成二维带障碍物的仿真环境；

• 调用 A * 规划出从起点到终点的无碰撞路径；

• 对原始路径做平滑、时间参数化；

• 启动领航者 + 两个跟随者的动力学仿真；

• 实时播放集群运动动画。

小提示：如果是第一次跑，建议先用固定地图调试；批量实验时，可以切换随机地图模式，统计算法的成功率。

四、核心模块原理拆解

1. A * 全局路径规划

A * 是机器人领域最经典的基于栅格的启发式搜索算法，也是本工程的顶层规划模块。

• 输入：二值占据栅格地图，栅格标记为可行或者障碍物；

• 代价函数：采用欧氏距离作为启发值，保证可采纳性；

• 邻域选择：支持 4 邻域或者 8 邻域搜索；

• 输出：由一系列栅格节点组成的折线路径。

这里要区分清楚：A * 只负责找到一条离散的节点序列，这条折线不能直接送给控制器跟踪，拐点处速度突变，很容易造成跟踪抖动。所以后面必须搭配轨迹平滑模块。

2. 轨迹平滑模块 smooth_path.m

A * 输出的是离散折线，不适合无人机、移动机器人连续运动。smooth_path.m 负责把折线转化为连续可跟踪的轨迹：

• 生成连续的参考轨迹；

• 按照时间均匀重采样；

• 保证位置、速度连续，大幅降低 PID 跟踪压力。 这一步是很多新手容易忽略的环节：很多人直接拿 A * 原始点给 PID，跟踪效果差、震荡严重，根源就在轨迹没有平滑和时间参数化。

3. 符号距离场 SDF，buildSDF.m

SDF（Signed Distance Field，符号距离场）是当前机器人避障里很热门的工具，替代传统的栅格膨胀。 函数 buildSDF.m 的作用：

• 根据机器人自身半径，对障碍物做安全膨胀；

• 计算地图上每一个点到最近障碍物的距离（距离变换）；

• 求解距离场的梯度，给人工势场提供排斥方向。

对比传统栅格膨胀：SDF 可以直接获取任意位置距离障碍物多远、朝哪个方向远离障碍物，梯度天然连续，比单纯的栅格膨胀更适合连续势场计算。

4. 人工势场 APF 避障 avoidanceField.m

人工势场法的思路非常直观：目标点产生吸引力，障碍物产生排斥力，合力决定智能体的运动方向。 在这个工程里，排斥势场依托 SDF 构建：

• 排斥力只在距离障碍物小于阈值 d0 的时候激活；

• 靠近目标点时，排斥力会逐步衰减，避免临近终点时被势场卡住；

• 避障输出的力，和 PID 跟踪控制信号叠加在一起，共同驱动智能体。

重点提醒：这就是前面提到的短板来源。经典 APF 只考虑局部距离信息，当多个障碍物合围、通道狭窄时，吸引力和排斥力可能抵消，形成局部极小，智能体停滞。

5. 主从编队控制逻辑

编队逻辑是整套工程的核心亮点： 领航者独立完成全局路径规划，沿着平滑参考轨迹运动。 跟随者不在全局坐标系跟踪固定坐标，而是在领航者的局部航向坐标系下，维持预设的相对偏移。 简单说：领航者往哪个方向转弯，跟随者的队形跟着同步旋转，队形保持相对不变，适合跟随机动目标的编队场景。 这种方式是无人机集群最常用的主从编队方案之一，实现简单、实时性好，缺点是依赖领航者的状态，如果领航者失效，编队容易崩溃。

⛳️ 运行结果

📣 部分代码

function [traj, traj_t] = smooth_path(map_pln, path, dt, v_nominal)

% SMOOTH_PATH - Path smoothing with collision check (planner frame)

% Inputs:

% map_pln : occupancy map (1 = obstacle)

% path : discrete waypoints [N x 2]

% dt : sample time

% v_nominal : nominal velocity (units/second)

%

% Outputs:

% traj : smoothed, dense trajectory [M x 2]

% traj_t : timestamps [M x 1]

if nargin < 3, dt = 0.1; end

if nargin < 4, v_nominal = 2.0; end

% --- Shortcut polyline using line-of-sight check ---

keep = 1; i = 1;

while i < size(path,1)

j = i + 2; jbest = i + 1;

while j <= size(path,1)

if has_line_of_sight(map_pln, path(i,:), path(j,:))

jbest = j; j = j + 1;

else

break

end

end

keep(end+1) = jbest; %#ok<AGROW>

i = jbest;

end

keep = unique(keep, 'stable');

poly = path(keep, :);

% --- Light arclength-based spline interpolation ---

dseg = [0; cumsum(sqrt(sum(diff(poly).^2,2)))];

if dseg(end) < 1e-6

traj = poly; traj_t = 0; return;

end

M = max(3, ceil((dseg(end)/v_nominal)/dt));

s = linspace(0, dseg(end), M).';

px = interp1(dseg, poly(:,1), s, 'pchip');

py = interp1(dseg, poly(:,2), s, 'pchip');

% --- Collision validation (planner indexing convention!) ---

ok = true;

[H,W] = size(map_pln);

for k = 1:M

x = round(min(max(px(k),1), W));

y = round(min(max(py(k),1), H));

if map_pln(y,x)

ok = false; break

end

end

if ok

traj = [px, py];

else

% fallback: densified polyline

traj = [];

for k = 2:size(poly,1)

nseg = 1 + ceil(norm(poly(k,:)-poly(k-1,:))/v_nominal/dt);

seg = [linspace(poly(k-1,1), poly(k,1), nseg)', ...

linspace(poly(k-1,2), poly(k,2), nseg)'];

if isempty(traj)

traj = seg;

else

traj = [traj; seg(2:end,:)];

end

end

end

total_time = dt * (size(traj,1)-1);

traj_t = linspace(0, total_time, size(traj,1)).';

end

% ====== Helper ======

function tf = has_line_of_sight(map_pln, a, b)

N = max(ceil(norm(b - a)), 1);

xs = linspace(a(1), b(1), N);

ys = linspace(a(2), b(2), N);

[H,W] = size(map_pln);

tf = true;

for k = 1:N

x = round(min(max(xs(k),1), W));

y = round(min(max(ys(k),1), H));

if map_pln(y,x)

tf = false; return;

end

end

end

🔗 参考文献

🎈 部分理论引用网络文献，若有侵权联系博主删除

#各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化、背包问题、 风电场布局、时隙分配优化、 最佳分布式发电单元分配、多阶段管道维修、 工厂-中心-需求点三级选址问题、 应急生活物质配送中心选址、 基站选址、 道路灯柱布置、 枢纽节点部署、 输电线路台风监测装置、 集装箱调度、 机组优化、 投资优化组合、云服务器组合优化、 天线线性阵列分布优化、CVRP问题、VRPPD问题、多中心VRP问题、多层网络的VRP问题、多中心多车型的VRP问题、 动态VRP问题、双层车辆路径规划（2E-VRP）、充电车辆路径规划（EVRP）、油电混合车辆路径规划、混合流水车间问题、 订单拆分调度问题、 公交车的调度排班优化问题、航班摆渡车辆调度问题、选址路径规划问题、港口调度、港口岸桥调度、停机位分配、机场航班调度、泄漏源定位

#机器学习和深度学习时序、回归、分类、聚类和降维

2.1 bp时序、回归预测和分类

2.2 ENS声神经网络时序、回归预测和分类

2.3 SVM/CNN-SVM/LSSVM/RVM支持向量机系列时序、回归预测和分类

2.4 CNN|TCN|GCN卷积神经网络系列时序、回归预测和分类

2.5 ELM/KELM/RELM/DELM极限学习机系列时序、回归预测和分类

2.6 GRU/Bi-GRU/CNN-GRU/CNN-BiGRU门控神经网络时序、回归预测和分类

2.7 ELMAN递归神经网络时序、回归\预测和分类

2.8 LSTM/BiLSTM/CNN-LSTM/CNN-BiLSTM/长短记忆神经网络系列时序、回归预测和分类

2.9 RBF径向基神经网络时序、回归预测和分类

2.10 DBN深度置信网络时序、回归预测和分类

2.11 FNN模糊神经网络时序、回归预测

2.12 RF随机森林时序、回归预测和分类

2.13 BLS宽度学习时序、回归预测和分类

2.14 PNN脉冲神经网络分类

2.15 模糊小波神经网络预测和分类

2.16 时序、回归预测和分类

2.17 时序、回归预测预测和分类

2.18 XGBOOST集成学习时序、回归预测预测和分类

2.19 Transform各类组合时序、回归预测预测和分类

方向涵盖风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、用电量预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

#图像处理方面

图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

#路径规划方面

旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、 充电车辆路径规划（EVRP）、 双层车辆路径规划（2E-VRP）、 油电混合车辆路径规划、 船舶航迹规划、 全路径规划规划、 仓储巡逻

#无人机应用方面

无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化、车辆协同无人机路径规划

#通信方面

传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化、水声通信、通信上传下载分配

# 信号处理方面

信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化、心电信号、DOA估计、编码译码、变分模态分解、管道泄漏、滤波器、数字信号处理+传输+分析+去噪、数字信号调制、误码率、信号估计、DTMF、信号检测

#电力系统方面

微电网优化、无功优化、配电网重构、储能配置、有序充电、MPPT优化、家庭用电

# 元胞自动机方面

交通流 人群疏散 病毒扩散 晶体生长 金属腐蚀

#雷达方面

卡尔曼滤波跟踪、航迹关联、航迹融合、SOC估计、阵列优化、NLOS识别

# 车间调度

零等待流水车间调度问题NWFSP 、 置换流水车间调度问题PFSP、 混合流水车间调度问题HFSP 、零空闲流水车间调度问题NIFSP、分布式置换流水车间调度问题 DPFSP、阻塞流水车间调度问题BFSP