【MATLAB例程】多无人机协同巡逻仿真：基于长机-僚机模型的编队保持与串级PID控制

基于 MATLAB 开发的多无人机（UAV）协同编队巡逻仿真系统。它采用了经典的长机-僚机（Leader-Follower）控制架构，深度融合了六自由度（6-DOF）非线性动力学模型与双闭环串级 PID 控制算法。通过集中式路径规划与分布式误差反馈机制，系统实现了多机在复杂巡逻航点下的菱形编队精准保持。

MATLAB打开后即可直接运行，包运行成功。
原创代码，仅供学习和研究，请勿翻卖

文章目录

程序介绍
- 物理模型与坐标系
- 长机-僚机协同策略
- [串级 PID 控制器逻辑](#串级 PID 控制器逻辑)
运行结果
MATLAB源代码

程序介绍

物理模型与坐标系

程序内置了非线性六自由度（6-DOF）无人机动力学模型 。通过旋转矩阵 R R R 将机体坐标系下的推力转换至惯性坐标系，并考虑了空气动力阻尼（线性阻尼与旋转阻尼），确保了仿真环境的逼真度。

长机-僚机协同策略

集中式路径规划：地面站预设leader_wps，长机根据当前位置与目标的距离自动判定并切换下一航点。
分布式队形反馈 ：僚机的期望位置不仅取决于长机广播的坐标和固定的 formation_offset，还引入了 formation_gain。
- 计算公式 ： P c m d = P l e a d e r + O f f s e t + K f ( O f f s e t − O f f s e t a c t u a l ) P_{cmd} = P_{leader} + Offset + K_{f}(Offset - Offset_{actual}) Pcmd=Pleader+Offset+Kf(Offset−Offsetactual)。
- 这种设计增强了僚机在长机剧烈转向时的跟随速度。

串级 PID 控制器逻辑

每一架无人机均独立运行一套级联控制算法：

外环（位置环）：输入空间误差，输出期望加速度。
姿态解算层：将期望加速度解算为目标横滚角（Roll）和俯仰角（Pitch）。
内环（姿态环）：输入角度误差，输出控制力矩（Torque）。

运行结果

本程序模拟了 1架长机（Leader）+ 3架僚机（Follower） 的协同巡逻任务。系统运行后将生成三张关键的分析图表：

多无人机三维轨迹图 ：直观展示四架无人机如何维持菱形编队完成起飞、前飞、直角转向及返航降落的全过程。图中清晰标出了长机的规划航点（Waypoints）。

轴向位置跟踪与误差图：详细对比了各架无人机在 X、Y、Z 三个轴向上的实际位置与期望位置，展示了级联 PID 控制器在高动态转向时的鲁棒性。

队形误差与推力对比图 ：
- 左侧记录了三架僚机相对于长机的实时位置偏差（欧氏距离），验证了在分布式反馈增益作用下，编队误差能迅速收敛至厘米级。
- 右侧展示了各机电机的总推力输出，反映了物理限幅对系统的实际约束。

命令行截图：

MATLAB源代码

部分代码如下：

matlab 复制代码

%  多无人机协同巡逻仿真 - 长机/僚机队形保持
%  控制架构：集中式调度 + 分布式执行
%    · 地面站（集中调度）：规划长机航点，广播队形偏移量
%    · 长机（Leader）    ：跟踪全局航点，串级PID控制
%    · 僚机（Follower）  ：跟踪"长机位置 + 队形偏移"，

% 作者:matlabfilter（V同号，除前期达成一致外，付费咨询）
% 2026-03-26/Ver1

clc; clear; close all;
rng(0);

%% 全局仿真参数
dt   = 0.01;           % 仿真步长 s
T    = 350;             % 总仿真时长 s
N    = round(T / dt);

%% 无人机物理参数（主机、僚机相同）
p.g      = 9.81;
p.mass   = 0.25;
p.Ixx    = 0.012;
p.Iyy    = 0.012;
p.Izz    = 0.020;
p.kd_lin = 0.10;
p.kd_ang = 0.05;

%% 长机航点（全局巡逻路径）
leader_wps = [
     0,   0,  0;    % WP1: 地面起始
     0,   0,  3;    % WP2: 起飞至3m
    10,   0,  3;    % WP3: 向前
    10,  10,  3;    % WP4: 右转
     0,  10,  3;    % WP5: 左转
     0,   0,  3;    % WP6: 返回
     0,   0,  0;    % WP7: 降落
];
switch_dist = 0.5;   % 航点切换阈值 [m]

%% 队形偏移量定义（机体坐标系偏移，相对长机）
%  菱形编队：长机居中，3架僚机分别在后左、后右、正后
%  偏移量 [dx, dy, dz]（m），dz可设不同高度分层避碰
n_followers = 3;
formation_offset = [
    -3,  2,  0;    % 僚机1：左后
    -3, -2,  0;    % 僚机2：右后
    -6,  0,  0;    % 僚机3：正后（双层备份机）
];

完整代码：
https://download.csdn.net/download/callmeup/92764324

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者