MATLAB/Simulink小型四旋翼智能控制

MATLAB/Simulink小型四旋翼智能控制

1. 粒子群（PSO）-PID
2. 遗传算法（GA）-PID
3. BP 神经网络 PID
   三种策略，可直接对比性能。

---

一、目录结构

QuadCopter_SmartPID/
 ├─ main.m                 % 一键脚本（选择算法）
 ├─ model/
 │   ├─ quad_dynamics.m    % 6-DOF 刚体动力学
 │   └─ simulink/
 │       ├─ quad_pso.slx   % PSO-PID
 │       ├─ quad_ga.slx    % GA-PID
 │       └─ quad_bp.slx    % BP-PID
 ├─ optimizer/
 │   ├─ pso_pid.m          % 粒子群优化
 │   ├─ ga_pid.m           % 遗传算法
 │   └─ bp_pid.m           % BP 在线整定
 ├─ evaluation/
 │   └─ itae.m             % ITAE/ISE/超调计算
 ├─ plot/
 │   └─ compare.m          % 三种算法结果对比
 └─ README.md

---

二、四旋翼动力学模型（精简公式）

% quad_dynamics.m
function [xdot] = quad_dynamics(x,u,param)
% 状态 x = [x,y,z,φ,θ,ψ,u,v,w,p,q,r]'
% 输入 u = [u1,u2,u3,u4]  总拉力+三力矩
g  = 9.81; m = 1.2; I = diag([0.01 0.01 0.02]);
% 位置
xdot(1:3) = x(7:9);                       % 速度
xdot(7)   = (cos(x(5))*cos(x(4)))*u(1)/m;
xdot(8)   = (sin(x(4))*cos(x(5)))*u(1)/m;
xdot(9)   = (sin(x(5)))*u(1)/m - g;
% 姿态
xdot(4:6) = [x(10) x(11) x(12)];
xdot(10:12) = I\(u(2:4) - cross([x(10) x(11) x(12)]',I*[x(10) x(11) x(12)]'));
end

---

三、三种智能 PID 设计

算法	优化变量	适应度	特点
PSO-PID	Kp, Ki, Kd (3×3)	ITAE	全局搜索，收敛快
GA-PID	同上	超调+上升时间	全局+局部，鲁棒
BP-PID	在线权重	预测误差	实时整定，自适应

---

四、关键代码片段

1. PSO-PID 优化器 pso_pid.m

function [Kp,Ki,Kd] = pso_pid(model)
lb = [0 0 0]; ub = [5 5 2]; dim = 3;
opts = optimoptions('particleswarm','SwarmSize',30,'MaxIterations',50);
[K,~] = particleswarm(@(x) cost_fun(x,model),dim,lb,ub,opts);
Kp=K(1); Ki=K(2); Kd=K(3);
end

function J = cost_fun(x,model)
simOut = sim('quad_pso','StopTime','5','SrcWorkspace','current');
J = itae(simOut);      % 积分时间绝对误差
end

2. GA-PID 优化器 ga_pid.m

function [Kp,Ki,Kd] = ga_pid(model)
lb=[0 0 0]; ub=[5 5 2];
opts = optimoptions('ga','PopulationSize',40,'MaxGenerations',80);
[K,~] = ga(@(x) cost_fun(x,model),3,[],[],[],[],lb,ub,[],opts);
Kp=K(1); Ki=K(2); Kd=K(3);
end

3. BP-PID 在线整定 bp_pid.m

% 网络结构 6-20-3  输入=[e de ∫e] 输出=[Kp Ki Kd]
net = feedforwardnet([20 10]);
net.trainFcn='trainlm';
net.trainParam.epochs=200;
net = train(net,inputs,targets);   % 在线滚动训练

---

五、Simulink 模型（3 个 .slx）

• quad_pso.slx：PSO 优化后 PID 参数固定 → 姿态/位置跟踪
• quad_ga.slx：GA 全局整定 → 对比超调与上升时间
• quad_bp.slx：BP 在线输出 Kp,Ki,Kd → 扰动自适应

---

六、主代码 main.m

clc; clear; close all;
model = load('quad_param.mat');   % 四旋翼参数

%% 1) PSO
[Kp_pso,Ki_pso,Kd_pso] = pso_pid(model);
sim('quad_pso'); metrics_pso = itae(simout);

%% 2) GA
[Kp_ga,Ki_ga,Kd_ga] = ga_pid(model);
sim('quad_ga'); metrics_ga = itae(simout);

%% 3) BP
sim('quad_bp'); metrics_bp = itae(simout);

%% 4) 对比
compare('PSO',metrics_pso,'GA',metrics_ga,'BP',metrics_bp);

---

七、仿真结果示例

指标	PSO	GA	BP
ITAE	1.23	1.47	0.91
超调量	4 %	7 %	2 %
上升时间	0.6 s	0.8 s	0.45 s
抗扰恢复	0.9 s	1.2 s	0.6 s

参考代码 小型四旋翼的智能控制 www.youwenfan.com/contentcnf/45792.html

八、如何替换为自己的四旋翼

1. 修改 quad_param.mat：质量、惯量、电机参数
2. 更新 cost_fun：增加风速扰动、测量噪声
3. 调整 BP 网络结构：输入增加陀螺/加速度计信号