四旋翼无人机仿真matlab/simulink仿真
四旋翼自抗扰 姿态控制仿真模型 ADRC模型对比PID轨迹跟踪

以下文字及示例代码仅供参考

以下是一个完整的 四旋翼无人机 MATLAB/Simulink 仿真模型，包含：

---

✅ 功能概述

1. 四旋翼动力学建模（6自由度）
2. 姿态控制策略对比：
   • ADRC（自抗扰控制器）
   • PID 控制器
3. 轨迹跟踪功能实现
4. Simulink 模型 + MATLAB 脚本 + 参数配置文件
5. 可视化界面：3D动画、状态曲线显示

---

🧠 一、四旋翼动力学模型（6-DOF）

状态变量定义：

$$x=[x,y,z,\varphi ,\theta ,\psi ,\dot{x},\dot{y},\dot{z},p,q,r]$$

其中：

• $ x, y, z $: 位置
• $ \phi, \theta, \psi $: 欧拉角（Roll, Pitch, Yaw）
• $ p, q, r $: 角速度
• $ \dot{x}, \dot{y}, \dot{z} $: 速度

四旋翼受力与力矩关系：

设四个电机转速为 $ \omega_1, \omega_2, \omega_3, \omega_4 $

总升力：
$$F=b({\omega }_1^2+{\omega }_2^2+{\omega }_3^2+{\omega }_4^2)$$

滚转力矩：
$${\tau }_{\varphi }=lb({\omega }_4^2-{\omega }_2^2)$$

俯仰力矩：
$${\tau }_{\theta }=lb({\omega }_3^2-{\omega }_1^2)$$

偏航力矩：
$${\tau }_{\psi }=d({\omega }_1^2-{\omega }_2^2+{\omega }_3^2-{\omega }_4^2)$$

其中 b 是升力系数，d 是阻力系数，l 是臂长

---

🧠 二、控制器设计

1. PID 控制器（姿态环 + 位置环）

% PID 控制器参数
Kp_pos = [4, 4, 8];
Kd_pos = [2, 2, 4];
Ki_pos = [0.1, 0.1, 0.2];

Kp_att = [5, 5, 3];
Kd_att = [1.5, 1.5, 1];
Ki_att = [0.01, 0.01, 0.01];

% 姿态控制输出
u_phi = pid_control(phi_ref, phi, Kp_att(1), Ki_att(1), Kd_att(1));
u_theta = pid_control(theta_ref, theta, Kp_att(2), Ki_att(2), Kd_att(2));
u_psi = pid_control(psi_ref, psi, Kp_att(3), Ki_att(3), Kd_att(3));

% 位置控制输出
thrust = pid_control(z_ref, z, Kp_pos(3), Ki_pos(3), Kd_pos(3));
phi_ref = pid_control(x_ref, x, Kp_pos(1), Ki_pos(1), Kd_pos(1));
theta_ref = pid_control(y_ref, y, Kp_pos(2), Ki_pos(2), Kd_pos(2));

---

2. ADRC 控制器（以姿态控制为例）

核心模块：

• TD（跟踪微分器）：提取参考信号的微分
• ESO（扩张状态观测器）：估计系统扰动
• NLSEF（非线性状态误差反馈律）

function u = adrc_controller(ref, y, v, beta01, beta02, beta03, h, w0)

% TD
v(1) = v(1) + h * v(2);
v(2) = v(2) + h * (-beta01 * fal(v(1)-ref, 0.5, 0.01) - beta02 * abs(v(2))^(0.5)*sign(v(2)));

% ESO
e = z1 - y;
z1 = z1 + h*(z2 - beta1*e + bu);
z2 = z2 + h*(-beta2*fal(e, 0.25, 0.01) + w);

% NLSEF
e1 = ref - y;
e2 = v(2) - z2;
u0 = beta03 * fal(e1, 0.5, 0.01) + beta04 * fal(e2, 0.25, 0.01);
u = (u0 - z2)/b0;

end

注：需要自定义 fal() 函数，常用于ADRC中的非线性函数。

---

📦 三、Simulink模型结构图

[Reference Trajectory] --> [Position Controller] --> [Attitude Controller] --> [Quadrotor Dynamics]
                                                  ↑
                                             [State Feedback]

Simulink模块说明：

模块	功能
Reference Path Generator	生成轨迹（如圆周/直线/螺旋）
Position Controller	位置控制（PID 或 ADRC）
Attitude Controller	姿态控制（PID 或 ADRC）
Quadrotor Dynamics	6自由度动力学模型
Thrust to Motor Speed	将力和力矩转换为电机转速
Scope & Visualization	显示状态、轨迹跟踪效果
3D Animation Block	使用 MATLAB 的 Aero.Animation 实现飞行动画

---

📈 四、轨迹跟踪示例

% 参考轨迹：螺旋上升
t = 0:Ts:T_sim;
x_ref = 5*sin(t);
y_ref = 5*cos(t);
z_ref = 2 + 0.5*t;
psi_ref = 0;

---

📊 五、控制器性能对比

你可以通过如下指标进行对比分析：

指标	PID	ADRC
上升时间	✅	✅
超调量	❗️较大	✅较小
抗扰能力	❌弱	✅强
参数整定	✅简单	❗️复杂
鲁棒性	❌	✅

---

📂 六、代码结构建议

/project_root/
├── quad_dynamics.m               % 四旋翼动力学函数
├── controller_pid.m              % PID控制器函数
├── controller_adrc.m             % ADRC控制器函数
├── fal_function.m                % 非线性函数fal()
├── main_quad_sim.m               % 主程序启动脚本
├── quad_model.slx                % Simulink模型
├── animation_script.m            % 3D动画绘制脚本
└── reference_path.mat            % 参考轨迹数据

---

📚 七、推荐参考文献

1. Gao, Z. (2003). Scaling and bandwidth-parameterization based controller tuning.
2. Han, J. (2009). From PID to active disturbance rejection control.
3. Lee, T., Leok, M., & McClamroch, N. H. (2010). Geometric tracking control of a quadrotor UAV on SE(3).
4. IEEE论文：“ADRC Based Attitude Control for Quadrotor UAV”

---

🧪 八、进阶扩展方向

• 加入风扰或外部干扰模型
• 加入视觉SLAM融合导航
• 使用LQR/MPC优化轨迹跟踪
• 在ROS/Gazebo中联合仿真
• 应用于集群无人机编队控制