代码在MATLAB中搭建了一套多架无人机编队协同飞行的三维仿真场景。1 架领航机负责按照预设航路点自主导航，另外 4 架跟随机在领航机周围保持楔形队形跟飞。整个系统同时具备障碍物规避和无人机间防碰撞能力，最终输出轨迹、误差和控制量等多维度结果。
原创代码，请勿翻卖
文章目录
程序简介
核心功能模块
领航机依次飞向 6 个预设航路点，到达后自动切换下一个目标。跟随机（从机）通过位置+速度双环控制律维持楔形队形，实时跟踪领航机运动。基于人工势场法，对 3 个球形障碍物施加斥力，领航机和跟随机均参与避障。无人机之间距离过近时自动产生互斥力，防止飞行过程中发生碰撞。
控制策略
领航机采用比例-阻尼控制律，根据当前位置与目标航路点的偏差计算期望速度方向，叠加障碍物斥力后输出控制加速度。
跟随机在领航机坐标系下计算期望位置，同样采用比例-阻尼控制，并在其基础上叠加防碰撞斥力和障碍物斥力，三项合力经限幅后作为最终指令。控制加速度上限、速度上限。
关键参数一览
自定义函数说明
详见源代码 m 文件。 详见源代码m文件。 详见源代码m文件。
运行结果
三维轨迹：
编队误差曲线：
无人机控制输入：
命令行窗口的截图：
程序结构：
MATLAB源代码
部分代码如下：
matlab
%% 多无人机编队协同控制与三维轨迹规划仿真
% 作者:matlabfilter（V同号，除前期达成一致外，付费咨询）
% 2026-05-18/Ver1
clear; clc; close all;
rng(0);
%% 基本参数
dt = 0.05; % 步长
T = 80; % 总仿真时间
t = 0:dt:T;
N = length(t);
numUAV = 5; % 无人机总数
leaderID = 1; % 领航机编号
% 控制增益
kp_leader = 1.8; % 领航机位置增益
kv_leader = 1.2; % 领航机速度阻尼
kp_form = 2.4; % 编队位置增益
kv_form = 1.6; % 编队速度阻尼
k_obs = 4.0; % 障碍物斥力增益
obsInfluence = 8.0; % 障碍物影响半径
uMax = 6.0; % 最大控制加速度
vMax = 8.0; % 最大速度
% 领航机的预设航路点
waypoints = [
0, 0, 10;
25, 0, 12;
45, 20, 15;
65, 10, 18;
85, 35, 16;
110, 30, 14
];
% 障碍物：[x, y, z, 半径]
obstacles = [
35, 8, 13, 5;
60, 18, 16, 6;
82, 28, 15, 5
];
% 编队队形：每架跟随机相对领航机的偏移量，这里用楔形
formationOffset = [
0, 0, 0;
-6, -5, -1;
-6, 5, -1;
-12, -10, -2;
-12, 10, -2
];
%% 状态初始化
pos = zeros(numUAV, 3, N);
vel = zeros(numUAV, 3, N);
acc = zeros(numUAV, 3, N);
% 给每架无人机一个初始位置，加点随机扰动模拟真实起飞完整代码：https://download.csdn.net/download/callmeup/92882247
如需帮助，或有导航、定位滤波相关的代码定制需求，可通过下方卡片联系我