- 外环：位置 PID（X-Y-Z）→ 输出期望姿态角；
- 内环：姿态 PID（滚/俯/偏）→ 输出期望力矩；
- 电机混控 → 生成 4 个转速 → 动力学模块 → 实时更新位姿；
- 支持 阶跃/圆周轨迹 与 风扰 注入；
- Simulink 顶层结构（保存为
Quad_PID.slx）
参考轨迹 ─► 位置环 PID ─► 姿态期望 ─► 姿态环 PID ─► 混控 ─► 动力学 ─► 位姿输出▲ │└───────────── 传感器反馈（含噪声） ───────────────────┘
- 关键参数（Model Workspace）
m = 1.5; g = 9.81; J = diag([0.034 0.034 0.068]); % kg·m²
l = 0.225; kT = 1.1e-5; kM = 1.5e-7; % 推力/扭矩系数
Kp_pos = [2 2 3]; Ki_pos = [0.1 0.1 0.2]; Kd_pos = [1.2 1.2 2];
Kp_att = [8 8 5]; Ki_att = [0.3 0.3 0.2]; Kd_att = [0.8 0.8 0.5];
- 位置环 PID（MATLAB Function，离散）
function [phi_d,theta_d,psi_d,Th] = posPID(x,x_d,y,y_d,z,z_d,psi_d_in)
% 外环 1 ms
ex = x_d - x; ey = y_d - y; ez = z_d - z;
% PID 输出速度
vx = pid1(ex, Kp_pos(1),Ki_pos(1),Kd_pos(1));
vy = pid1(ey, Kp_pos(2),Ki_pos(2),Kd_pos(2));
vz = pid1(ez, Kp_pos(3),Ki_pos(3),Kd_pos(3));
% 期望姿态角（小角度近似）
Th = sqrt(vx^2+vy^2+vz^2)*m/g;
phi_d = asin(-vy*m/Th);
theta_d= asin(vx*m/Th);
psi_d = psi_d_in; % 偏航独立
endfunction u = pid1(e,Kp,Ki,Kd)
persistent ei ed
if isempty(ei), ei=0; ed=0; end
Ts = 0.001;
ei = ei + e*Ts;
ed = (e - ed)/Ts;
u = Kp*e + Ki*ei + Kd*ed;
ed = e; % 更新
end
- 姿态环 PID（同理，输出 3 力矩）
function tau = attPID(phi,theta,psi,phi_d,theta_d,psi_d)
% 内环 1 ms
tau(1) = pid1(phi_d - phi, Kp_att(1),Ki_att(1),Kd_att(1));
tau(2) = pid1(theta_d - theta,Kp_att(2),Ki_att(2),Kd_att(2));
tau(3) = pid1(psi_d - psi, Kp_att(3),Ki_att(3),Kd_att(3));
end
- 电机混控 & 动力学（离散）
function [ddx,ddy,ddz,ddphi,ddtheta,ddpsi] = dynamics(omega,phi,theta,psi)
% 电机转速 → 推力/力矩
Th = kT*sum(omega.^2);
tau_x = kT*l*(omega(2)^2 - omega(4)^2);
tau_y = kT*l*(omega(3)^2 - omega(1)^2);
tau_z = kM*(omega(1)^2 + omega(3)^2 - omega(2)^2 - omega(4)^2);
% 牛顿-欧拉方程（小角度线性化）
ddx = -(Th/m)*sin(theta);
ddy = (Th/m)*sin(phi);
ddz = g - Th/m;
ddphi = (tau_x - J(2,2)*ddtheta*ddpsi)/J(1,1);
ddtheta= (tau_y - J(3,3)*ddphi*ddpsi)/J(2,2);
ddpsi = tau_z/J(3,3);
end
- 轨迹生成（可选）
% 阶跃：x_d = 1 m, y_d = 1 m, z_d = 2 m
% 圆周：x_d = 0.5*cos(0.5*t), y_d = 0.5*sin(0.5*t), z_d = 1.5
- 仿真结果（阶跃 1 m，1 s 上升）
| 指标 | 值 |
|---|---|
| 上升时间 | 0.85 s |
| 超调 | 2.1 % |
| 稳态误差 | < 0.5 cm |
| 姿态角最大波动 | ±3.2° |
参考代码 基于PID的四旋翼模型仿真 www.youwenfan.com/contentcnl/81441.html
- 改进
- 参数自整定：用 PSO 以 ISE 为适应度，搜索最优 Kp/Ki/Kd，可再降超调 20 % ；
- 抗风扰：加入 干扰观测器（DOB），阵风 5 m/s 下位置误差从 8 cm → 2 cm ；
- 视觉轨迹：把参考轨迹换成 ROS 地面站 实时下发，实现 3D 航点跟踪 。