基于MATLAB编程实现无人机编队仿真

无人机编队仿真 功能：九架无人机从不同初始位置集结为正方形编队，之后变换为三角形编队，并进行编队转弯、直线飞行以及爬升。 算法：领航跟随+人工势场+PID MATLAB编程实现，代码注释清楚 以下文字及示例代码仅供参考

文章目录

• • • 1 效果抢先看
    • 2 算法框架
    • 3 文件结构
    • 4 数学模型（节选）
    • 5 核心代码
    • • 5.1 主脚本 `main.m`
      • 5.2 领航机航迹 `leaderTraj.m`
      • 5.3 编队控制 `formationCtrl.m`

1 效果抢先看

2 算法框架

• 领航机：给定全局航迹（直线/圆弧/爬升）；
• 跟随机：
• 领航-跟随 期望相对位置；
• 人工势场 防止碰撞；
• PID 输出期望加速度 → 积分得速度、位置。

3 文件结构

Quad9_Formation/
├─ main.m % 一键运行
├─ leaderTraj.m % 领航机航迹生成
├─ formationCtrl.m % 编队控制（领航+势场+PID）
├─ potentialField.m % 人工势场
├─ plot3D.m % 实时三维动画
└─ README.pdf % 公式 & 调参

4 数学模型（节选）

• 领航机状态 [ \\boldsymbol{p}L(k)=\\begin{bmatrix}x_L\\y_L\\z_L\\end{bmatrix},\\quad \\psi_L(k)=\\text{atan2}(v{Ly},v_{Lx}) ]

• 跟随机期望位置 正方形： [ \\boldsymbol{d}i=R(\\psi_L)\\cdot \\underbrace{\\begin{bmatrix}s_x(i)\\s_y(i)\\0\\end{bmatrix}}{\\text{相对坐标}} +\\boldsymbol{p}_L ] 三角形： [ \\boldsymbol{d}_i=R(\\psi_L)\\cdot \\begin{bmatrix}r\\cos\\theta_i\\r\\sin\\theta_i\\0\\end{bmatrix} +\\boldsymbol{p}_L,\\quad \\theta_i=\\frac{2\\pi}{9}(i-1) ]

• 人工势场力（机间排斥） [ \\boldsymbol{F}{ij}= \\begin{cases} k{\\text{rep}}\\left(\\frac{1}{d_{ij}}-\\frac{1}{d_{\\text{safe}}}\\right)\\frac{\\boldsymbol{p}i-\\boldsymbol{p}j}{d{ij}},& d{ij}<d_{\\text{safe}}\\[4pt] \\boldsymbol{0},& \\text{otherwise} \\end{cases} ]

• PID 控制 [ \\boldsymbol{u}_i=K_p\\boldsymbol{e}_i+K_i\\int\\boldsymbol{e}i+K_d\\dot{\\boldsymbol{e}}i+\\sum{j\\neq i}\\boldsymbol{F}{ij} ]

5 核心代码

5.1 主脚本 main.m

clc; clear; close all;
addpath(genpath(pwd));

%% 参数
N = 9; % 无人机数量
dt = 0.05; % 步长 (s)
Tsim = 120; % 总时长
Rsafe = 1.5; % 势场安全距离
kp = [2 2 1]; ki = [0.1 0.1 0.05]; kd = [1 1 0.5]; % PID

%% 初始状态
p = 10*randn(3,N); % 随机位置
v = zeros(3,N);

%% 阶段时间线
phaseTime = [0 20 40 70 120]; % 时间分界
% 0-20: 正方形；20-40: 三角形；40-70: 左转90°；70-120: 直线+爬升

%% 仿真主循环
figure('Name','9-UAV Formation','Color','w'); view(3); grid on; axis equal
hL = plot3(NaN,NaN,NaN,'ko','MarkerSize',10,'MarkerFaceColor','b');
hF = plot3(NaN,NaN,NaN,'ro','MarkerSize',8);
hold on

for k = 1:round(Tsim/dt)
t = k*dt;

% 阶段切换
phase = find(t>=phaseTime,1,'last');

% 领航机航迹
[pL,vL,psiL] = leaderTraj(t,phase);

% 编队控制
[p,v] = formationCtrl(p,v,pL,vL,psiL,phase,N,kp,ki,kd,Rsafe,dt);

% 绘图
set(hL,'XData',pL(1),'YData',pL(2),'ZData',pL(3));
set(hF,'XData',p(1,:),'YData',p(2,:),'ZData',p(3,:));
drawnow; pause(0.01);
end

5.2 领航机航迹 leaderTraj.m

function [pL,vL,psiL] = leaderTraj(t,phase)
switch phase
case 1 % 正方形阶段，静止
pL = [0 0 10]'; vL = [0 0 0]'; psiL = 0;
case 2 % 三角形阶段，静止
pL = [0 0 10]'; vL = [0 0 0]'; psiL = 0;
case 3 % 左转圆弧
R = 30; omega = 0.2;
psiL = omega*t;
pL = [R*sin(psiL); R*(1–cos(psiL)); 10];
vL = [R*omega*cos(psiL); R*omega*sin(psiL); 0];
case 4 % 直线 + 爬升
vL = [5 0 0.5]';
pL = [0 0 10] + vL*(t–70);
end

5.3 编队控制 formationCtrl.m

function [p,v] = formationCtrl(p,v,pL,vL,psiL,phase,N,kp,ki,kd,Rsafe,dt)
persistent int_e
if isempty(int_e), int_e = zeros(3,N); end

% 生成期望相对位置
switch phase
case 1 % 正方形
side = 8; offset = side/2;
rel = [ –offset –offset 0; offset –offset 0; offset offset 0; –offset offset 0;
–offset 0 0; offset 0 0; 0 –offset 0; 0 offset 0; 0 0 0]';
case 2 % 三角形
r = 6; theta = linspace(0,2*pi,N+1); theta(end) = [];
rel = [r*cos(theta); r*sin(theta); zeros(1,N)];
case {3,4}
rel = [r*cos(theta); r*sin(theta); zeros(1,N)];
end

% 旋转到领航机坐标系
Rpsi = [cos(psiL) –sin(psiL) 0; sin(psiL) cos(psiL) 0; 0 0 1];
pd = pL + Rpsi*rel;

% 势场力
Frep = zeros(3,N);
for i = 1:N
for j = [1:i–1,i+1:N]
d = norm(p(:,i)–p(:,j));
if d < Rsafe
Frep(:,i) = Frep(:,i) + 1*(1/d–1/Rsafe)*(p(:,i)–p(:,j))/d;
end
end
end

% PID
e = pd – p;
int_e = int_e + e*dt;
de = (kp.*e + ki.*int_e – v) – kd.*(v – vL);
a = de + Frep;
v = v + a*dt;
p = p + v*dt;
end