5架无人机在60秒内完成一字形 → 楔形 → 菱形两次队形切换的全过程仿真，采用**虚拟领机（Virtual Leader）**结构解耦编队控制问题：各跟随无人机独立跟踪各自相对领机的期望偏置位置，无需无人机之间直接通信。
原创代码，请勿翻卖，包运行成功
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击上方卡片联系作者文章目录
程序简介
队形偏置矩阵
三种队形均以 2 × n 2 \times n 2×n 矩阵描述每架无人机相对虚拟领机的期望偏置 ( Δ x i , Δ y i ) (\Delta x_i,\, \Delta y_i) (Δxi,Δyi)：
| 队形 | 几何特征 | 典型应用场景 |
|---|---|---|
| 一字形 | 5机横向等间距排列，间距 d = 3 d=3 d=3 m | 广域扫描/侦察 |
| 楔形 | V字形，前尖后宽，纵横双向偏置 | 突防/气动减阻 |
| 菱形 | 前后左右四角+中心，纵向拉伸 | 目标包围/协同探测 |
仿真流程
初始化 → 设置队形偏置 → 虚拟领机匀速飞行
↓
主循环（每步 dt = 0.05 s）：
计算当前/下一步期望偏置
→ 计算位置/速度误差
→ PD控制 + 饱和限幅
→ 欧拉积分更新速度、位置
↓
后处理：计算跟踪误差 → 绘图 → 动画演示 → 控制台统计输出关键参数说明
| 参数 | 符号 | 取值 | 说明 |
|---|---|---|---|
| 无人机数量 | n n n | 5 | 1架领机（虚拟）+ 4架跟随 |
| 仿真步长 | d t dt dt | 0.05 s | 满足控制带宽要求 |
| 比例增益 | K p K_p Kp | 3.5 | 位置误差收敛速度 |
| 微分增益 | K d K_d Kd | 2.5 | 阻尼，抑制超调 |
| 控制饱和 | u max u_{\max} umax | 8 m/s² | 模拟执行机构物理限制 |
| 基础间距 | d d d | 3.0 m | 队形几何缩放基准 |
| 领机速度 | v L v_L vL | 1.5 m/s | 匀速直线飞行 |
| 过渡时间 | T r T_r Tr | 5 s | 余弦插值窗口宽度 |
运行结果
仿真程序运行后将自动生成以下六张图表：
- 无人机飞行轨迹（俯视图）：展示5架无人机在整个60秒仿真过程中的完整飞行轨迹，并在关键时刻标注队形快照（一字形/楔形/菱形），红色虚线标记队形切换时刻，蓝色虚线标记平滑过渡完成时刻。
-
各无人机队形跟踪误差曲线 ：显示每架无人机实际位置与期望位置之间的偏差随时间的变化，直观体现PD控制器在队形切换过渡期间的调节性能。
-
各跟随无人机控制输入变化 ：以子图形式分别绘制4架跟随无人机在X轴与Y轴方向的控制加速度输出，展示控制量饱和约束（±8 m/s²）的限制效果。
-
队形快照（相对领机坐标系） ：在领机相对坐标系下，分别呈现t=8 s、t=25 s、t=52 s三个典型时刻的一字形、楔形、菱形队形几何构型。
-
各阶段平均跟踪误差统计柱状图：对三个飞行阶段的平均跟踪误差进行分组统计，定量评估每架无人机在不同队形下的控制精度。
- 编队切换动态演示（绝对坐标系） ：以动画形式实时演示编队在绝对坐标系下的飞行过程，视窗跟随虚拟领机滑动，动态显示当前所处阶段及过渡进度百分比。
同时，命令窗口将输出仿真参数摘要及各阶段平均误差、全局最大误差、两次切换峰值误差等关键性能指标。
MATLAB源代码
部分代码如下：
matlab
%% 多无人机队形切换控制仿真
% 一字形 → 楔形 → 菱形
% 方法: 虚拟领机 + PD控制 + 余弦平滑过渡函数
% 输出: 轨迹图 | 误差曲线 | 控制输入 | 队形快照 | 误差统计
% 需求: MATLAB R2018b+
% 作者:matlabfilter
% 2026-05-07
clear; clc; close all;
rng(0);
fprintf('正在初始化仿真...\n');
% 仿真参数
n = 5; % 无人机数量
dt = 0.05; % 仿真步长 [s]
T = 60; % 总仿真时间 [s]
t = (0:dt:T)'; % 时间向量 [N×1]
N = numel(t);
% 队形切换时刻
t1 = 15; % 一字形 → 楔形 切换开始 [s]
t2 = 35; % 楔形 → 菱形 切换开始 [s]
Tr = 5; % 平滑过渡持续时间 [s]
% PD控制器参数
Kp = 3.5; % 比例增益
Kd = 2.5; % 微分增益
umax = 8.0; % 控制输入饱和限制 [m/s²]
% 编队几何参数
d = 3.0; % 基础间距 [m]
vL = 1.5; % 虚拟领机速度 [m/s]
% 2. 队形偏置矩阵定义
% 一字形
F_line = [
0, 0, 0, 0, 0 ;
0, -d, -2*d, d, 2*d
];
% 楔形
F_wedge = [
0, -d, -2*d, -d, -2*d ;
0, -d, -2*d, d, 2*d
];
% 菱形
F_diamond = [
d, 0, -d, 0, 0 ;
0, -d, 0, d, 0
];
% 领机轨迹