MATLAB/Simulink仿真 四旋翼无人机轨迹追踪 无人机仿真爱好者看过来！
MATLAB/Simulink仿真 四旋翼无人机轨迹追踪无人机仿真爱好者看过来！MATLAB/Simulink工具包，专为四旋翼UAV设计，实现三维位置、姿态角度等多参数实时跟踪。仿真过程包括参考位置与实际位置对比，直观反馈飞行状态。
MATLAB/Simulink仿真 四旋翼无人机轨迹追踪
无人机仿真爱好者看过来！MATLAB/Simulink工具包，专为四旋翼UAV设计，实现三维位置、姿态角度等多参数实时跟踪。仿真过程包括参考位置与实际位置对比，直观反馈飞行状态。
X轴响应：蓝色曲线，有轻微超调后稳定
Y轴响应：红色曲线，上升快，略有振荡
Z轴响应：紫色曲线，存在较大超调和调节时间
这说明您的目标是实现一个 基于 MATLAB/Simulink 的四旋翼无人机轨迹追踪仿真系统，能够实时跟踪参考位置并反馈飞行状态。
MATLAB + Simulink 代码示例，用于实现如下功能：
四旋翼动力学建模（非线性）
双环 PID 控制器（外环位置控制 + 内环姿态控制）
三维轨迹追踪（阶跃/正弦/斜坡等）
实时绘制参考 vs 实际位置对比图
支持 Simulink 模块化仿真
注释详细，适合新手学习！
🔧 第一步：主控脚本 quadcopter_sim.m
matlab
%% 四旋翼无人机轨迹追踪仿真（MATLAB/Simulink）
% 作者：Qwen
% 时间：2025年10月23日
% 功能：实现 X/Y/Z 三维位置与姿态角实时跟踪
% 输出：参考 vs 实际位置对比图（如您截图所示）
clear; clc; close all;
%% 参数初始化
m = 0.8; % 质量 (kg)
g = 9.81; % 重力加速度
Ixx = 0.007; % 惯性矩
Iyy = 0.007;
Izz = 0.012;
J = diag([Ixx, Iyy, Izz]);
L = 0.2; % 臂长
kf = 1e-5; % 升力系数
km = 1.5e-6; % 力矩系数
% 初始状态 [x y z phi theta psi xdot ydot zdot p q r]
initial_state = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];
% 目标轨迹类型：‘step’, ‘sinusoidal’, ‘ramp’
trajectory_type = ‘step’;
% PID 参数（外环位置）
Kp_xy = 8; Kd_xy = 5;
Kp_z = 10; Kd_z = 6; Ki_z = 1;
% PID 参数（内环姿态）
Kp_phi = 40; Kd_phi = 10; Ki_phi = 2;
Kp_theta = 40; Kd_theta = 10; Ki_theta = 2;
Kp_psi = 30; Kd_psi = 8; Ki_psi = 1;
% 存储到工作区
assignin(‘base’, ‘m’, m);
assignin(‘base’, ‘g’, g);
assignin(‘base’, ‘J’, J);
assignin(‘base’, ‘L’, L);
assignin(‘base’, ‘kf’, kf);
assignin(‘base’, ‘km’, km);
assignin(‘base’, ‘initial_state’, initial_state);
assignin(‘base’, ‘trajectory_type’, trajectory_type);
assignin(‘base’, ‘Kp_xy’, Kp_xy); assignin(‘base’, ‘Kd_xy’, Kd_xy);
assignin(‘base’, ‘Kp_z’, Kp_z); assignin(‘base’, ‘Kd_z’, Kd_z); assignin(‘base’, ‘Ki_z’, Ki_z);
assignin(‘base’, ‘Kp_phi’, Kp_phi); assignin(‘base’, ‘Kd_phi’, Kd_phi); assignin(‘base’, ‘Ki_phi’, Ki_phi);
assignin(‘base’, ‘Kp_theta’, Kp_theta); assignin(‘base’, ‘Kd_theta’, Kd_theta); assignin(‘base’, ‘Ki_theta’, Ki_theta);
assignin(‘base’, ‘Kp_psi’, Kp_psi); assignin(‘base’, ‘Kd_psi’, Kd_psi); assignin(‘base’, ‘Ki_psi’, Ki_psi);
%% 设置仿真参数
sim(‘quadcopter_tracking_model’); % 运行 Simulink 模型
%% 绘图分析
t = tout;
x_ref = positions_ref(:,1);
y_ref = positions_ref(:,2);
z_ref = positions_ref(:,3);
x_act = states_log(:,1);
y_act = states_log(:,2);
z_act = states_log(:,3);
figure(‘Position’, [100, 100, 1200, 800]);
subplot(2,2,1);
plot(t, x_act, ‘b’, t, x_ref, ‘–r’, ‘LineWidth’, 1.5);
xlabel(‘Time (s)’); ylabel(‘X Position (m)’);
title(‘X轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);
subplot(2,2,2);
plot(t, y_act, ‘r’, t, y_ref, ‘–r’, ‘LineWidth’, 1.5);
xlabel(‘Time (s)’); ylabel(‘Y Position (m)’);
title(‘Y轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);
subplot(2,2,3);
plot(t, z_act, ‘m’, t, z_ref, ‘–r’, ‘LineWidth’, 1.5);
xlabel(‘Time (s)’); ylabel(‘Z Position (m)’);
title(‘Z轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);
subplot(2,2,4);
plot(t, states_log(:,6)180/pi, ‘k’);
xlabel(‘Time (s)’); ylabel(‘Yaw Angle (deg)’);
title(‘偏航角响应’); grid on;
🧩 第二步：Simulink 模型 quadcopter_tracking_model.slx（结构说明）
由于无法直接发送 .slx 文件，以下是 模块化设计结构 和 关键子系统逻辑，您可以轻松重建模型。
📦 主模型结构
±--------------------------------------------+
quadcopter_tracking_model
±-----------+ ±--------------------+
Trajectory -> Controller ->
±-----------+ ±--------------------+
±--------------------------------------+
Quadrotor Dynamics (S-Function) <-
±--------------------------------------+
±---------------+ ±-----------------+
Scope / To Workspace
±---------------+ ±-----------------+
±--------------------------------------------+
🎛️ 子系统1：Trajectory Generator（轨迹生成器）
根据 trajectory_type 生成参考信号：
matlab
function [x_d, y_d, z_d] = fcn(type, t)
switch type
case ‘step’
x_d = 1.5 (t >= 1);
y_d = 1.5 (t >= 1);
z_d = 2.0 (t >= 1);
case ‘sinusoidal’
x_d = 1.5 sin(0.5t);
y_d = 1.5 cos(0.5t);
z_d = 1 + 0.5sin(0.3t);
case ‘ramp’
x_d = 0.02 t;
y_d = 0.02 t;
z_d = 0.01 t;
end
end
🎛️ 子系统2：Controller（双环PID控制器）
外环：位置控制 → 期望姿态
matlab
% 输入：实际位置 r = [x,y,z], 速度 v = [vx,vy,vz]
% 输出：U1（总升力）、phi_d、theta_d、psi_d
ex = x_d - x; evx = 0 - vx;
ey = y_d - y; evy = 0 - vy;
ez = z_d - z; evz = 0 - vz;
theta_d = Kp_xy ex + Kd_xy evx;
phi_d = -(Kp_xy ey + Kd_xy evy);
e_int_z = e_int_z + ez dt;
U1 = m (g + Kp_zez + Kd_zevz + Ki_ze_int_z);
U4 = Kp_psi(0-psi) + Kd_psi(0-r) + Ki_psie_int_psi;
内环：姿态控制 → 力矩 U2,U3,U4
matlab
% 输入：欧拉角 eta = [phi,theta,psi], 角速度 omega = [p,q,r]
% 输出：U2, U3, U4
e_phi = phi_d - phi;
e_theta = theta_d - theta;
e_psi = wrapToPi(0 - psi);
U2 = Kp_phie_phi + Kd_phi(0-p) + Ki_phie_int_phi;
U3 = Kp_thetae_theta + Kd_theta(0-q) + Ki_thetae_int_theta;
U4 = Kp_psie_psi + Kd_psi(0-r) + Ki_psie_int_psi;
🧱 子系统3：Quadrotor Dynamics（动力学模型）——使用 S-Function 或 State-Space
方法一：S-Function（推荐）
创建 quad_dynamics.m：
matlab
function sys = mdlOutputs(t, x, u)
% 状态: x = [r; eta; v; omega]
% 输入: u = [U1, U2, U3, U4]
r = x(1:3); eta = x(4:6); v = x(7:9); omega = x(10:12);
U1 = u(1); U2 = u(2); U3 = u(3); U4 = u(4);
phi = eta(1); theta = eta(2); psi = eta(3);
% 旋转矩阵
R = [cos(psi)cos(theta), cos(psi)sin(theta)sin(phi)-sin(psi)cos(phi), cos(psi)sin(theta)cos(phi)+sin(psi)sin(phi);
sin(psi)cos(theta), sin(psi)sin(theta)sin(phi)+cos(psi)cos(phi), sin(psi)sin(theta)cos(phi)-cos(psi)sin(phi);
-sin(theta), cos(theta)sin(phi), cos(theta)cos(phi)];
acc_e = R [0; 0; U1] / m - [0; 0; g];
tau = [U2; U3; U4];
omega_dot = J \ (tau - cross(omega, J omega));
📥 数据记录设置
在 Simulink 中添加 To Workspace 模块：
states_log：记录状态向量 [x,y,z,phi,theta,psi,…]
positions_ref：记录参考位置 [x_d,y_d,z_d]
tout：自动记录时间
📊 输出效果
运行 quadcopter_sim.m 后将生成：
X轴：蓝色实线（实际），红色虚线（期望）
Y轴：红色实线，快速响应
Z轴：紫色实线，有超调但最终收敛
第四张图：偏航角变化（可选）
✅ 如何使用？
- 新建 Simulink 模型，命名为 quadcopter_tracking_model
- 添加上述子系统（可用 MATLAB Function 模块实现）
- 设置求解器为 ode4 (Runge-Kutta)，固定步长 0.01
- 运行主脚本 quadcopter_sim.m
- 自动生成四张响应图
📌 扩展建议
添加 三维动画 显示飞行轨迹（使用 animatedline）
改用 LQR 或 MPC 控制器
加入 风扰、传感器噪声
导出为 ROS 节点 或 硬件在环（HIL）
四旋翼无人机（Quadcopter）的控制架构图，包含以下关键模块：
- 位置环（Position Loop）
- 姿态环（Attitude Loop）
- 电机控制（Motor Control）
- 动力学模型（Dynamics Model）
这是一套典型的 双闭环控制结构：
👉 外环：位置控制器 → 输出期望姿态角
👉 内环：姿态控制器 → 输出电机转速
实现四旋翼无人机的 三维轨迹追踪
使用 双环 PID 控制器（外环位置 + 内环姿态）
支持 阶跃/正弦/斜坡参考轨迹
可视化 X/Y/Z 轴响应曲线（如您截图所示）
模块化设计，易于理解与修改
MATLAB + Simulink 代码：四旋翼无人机控制仿真
🔧 第一步：主控脚本 quadcopter_control.m
matlab
%% 四旋翼无人机控制仿真（MATLAB/Simulink）
% 作者：Qwen
% 时间：2025年10月25日
% 功能：实现 X/Y/Z 三维位置与姿态角实时跟踪
% 输出：参考 vs 实际位置对比图（如您截图所示）
clear; clc; close all;
%% 参数初始化
m = 0.8; % 质量 (kg)
g = 9.81; % 重力加速度
Ixx = 0.007; % 惯性矩
Iyy = 0.007;
Izz = 0.012;
J = diag([Ixx, Iyy, Izz]);
L = 0.2; % 臂长
kf = 1e-5; % 升力系数
km = 1.5e-6; % 力矩系数
% 初始状态 [x y z phi theta psi xdot ydot zdot p q r]
initial_state = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];
% 目标轨迹类型：‘step’, ‘sinusoidal’, ‘ramp’
trajectory_type = ‘step’;
% PID 参数（外环位置）
Kp_xy = 8; Kd_xy = 5;
Kp_z = 10; Kd_z = 6; Ki_z = 1;
% PID 参数（内环姿态）
Kp_phi = 40; Kd_phi = 10; Ki_phi = 2;
Kp_theta = 40; Kd_theta = 10; Ki_theta = 2;
Kp_psi = 30; Kd_psi = 8; Ki_psi = 1;
% 存储到工作区
assignin(‘base’, ‘m’, m);
assignin(‘base’, ‘g’, g);
assignin(‘base’, ‘J’, J);
assignin(‘base’, ‘L’, L);
assignin(‘base’, ‘kf’, kf);
assignin(‘base’, ‘km’, km);
assignin(‘base’, ‘initial_state’, initial_state);
assignin(‘base’, ‘trajectory_type’, trajectory_type);
assignin(‘base’, ‘Kp_xy’, Kp_xy); assignin(‘base’, ‘Kd_xy’, Kd_xy);
assignin(‘base’, ‘Kp_z’, Kp_z); assignin(‘base’, ‘Kd_z’, Kd_z); assignin(‘base’, ‘Ki_z’, Ki_z);
assignin(‘base’, ‘Kp_phi’, Kp_phi); assignin(‘base’, ‘Kd_phi’, Kd_phi); assignin(‘base’, ‘Ki_phi’, Ki_phi);
assignin(‘base’, ‘Kp_theta’, Kp_theta); assignin(‘base’, ‘Kd_theta’, Kd_theta); assignin(‘base’, ‘Ki_theta’, Ki_theta);
assignin(‘base’, ‘Kp_psi’, Kp_psi); assignin(‘base’, ‘Kd_psi’, Kd_psi); assignin(‘base’, ‘Ki_psi’, Ki_psi);
%% 设置仿真参数
sim(‘quadcopter_control_model’); % 运行 Simulink 模型
%% 绘图分析
t = tout;
x_ref = positions_ref(:,1);
y_ref = positions_ref(:,2);
z_ref = positions_ref(:,3);
x_act = states_log(:,1);
y_act = states_log(:,2);
z_act = states_log(:,3);
figure(‘Position’, [100, 100, 1200, 800]);
subplot(2,2,1);
plot(t, x_act, ‘b’, t, x_ref, ‘–r’, ‘LineWidth’, 1.5);
xlabel(‘Time (s)’); ylabel(‘X Position (m)’);
title(‘X轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);
subplot(2,2,2);
plot(t, y_act, ‘r’, t, y_ref, ‘–r’, ‘LineWidth’, 1.5);
xlabel(‘Time (s)’); ylabel(‘Y Position (m)’);
title(‘Y轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);
subplot(2,2,3);
plot(t, z_act, ‘m’, t, z_ref, ‘–r’, ‘LineWidth’, 1.5);
xlabel(‘Time (s)’); ylabel(‘Z Position (m)’);
title(‘Z轴跟踪响应’); grid on; legend(‘实际’, ‘期望’);
subplot(2,2,4);
plot(t, states_log(:,6)180/pi, ‘k’);
xlabel(‘Time (s)’); ylabel(‘Yaw Angle (deg)’);
title(‘偏航角响应’); grid on;
🧩 第二步：Simulink 模型 quadcopter_control_model.slx（结构说明）
📦 主模型结构
±--------------------------------------------+
quadcopter_control_model
±-----------+ ±--------------------+
Trajectory -> Controller ->
±-----------+ ±--------------------+
±--------------------------------------+
Quadrotor Dynamics (S-Function) <-
±--------------------------------------+
±---------------+ ±-----------------+
Scope / To Workspace
±---------------+ ±-----------------+
±--------------------------------------------+
🎛️ 子系统1：Trajectory Generator（轨迹生成器）
根据 trajectory_type 生成参考信号：
matlab
function [x_d, y_d, z_d] = fcn(type, t)
switch type
case ‘step’
x_d = 1.5 (t >= 1);
y_d = 1.5 (t >= 1);
z_d = 2.0 (t >= 1);
case ‘sinusoidal’
x_d = 1.5 sin(0.5t);
y_d = 1.5 cos(0.5t);
z_d = 1 + 0.5sin(0.3t);
case ‘ramp’
x_d = 0.02 t;
y_d = 0.02 t;
z_d = 0.01 t;
end
end
🎛️ 子系统2：Controller（双环PID控制器）
外环：位置控制 → 期望姿态
matlab
% 输入：实际位置 r = [x,y,z], 速度 v = [vx,vy,vz]
% 输出：U1（总升力）、phi_d、theta_d、psi_d
ex = x_d - x; evx = 0 - vx;
ey = y_d - y; evy = 0 - vy;
ez = z_d - z; evz = 0 - vz;
theta_d = Kp_xy ex + Kd_xy evx;
phi_d = -(Kp_xy ey + Kd_xy evy);
e_int_z = e_int_z + ez dt;
U1 = m (g + Kp_zez + Kd_zevz + Ki_ze_int_z);
U4 = Kp_psi(0-psi) + Kd_psi(0-r) + Ki_psie_int_psi;
内环：姿态控制 → 力矩 U2,U3,U4
matlab
% 输入：欧拉角 eta = [phi,theta,psi], 角速度 omega = [p,q,r]
% 输出：U2, U3, U4
e_phi = phi_d - phi;
e_theta = theta_d - theta;
e_psi = wrapToPi(0 - psi);
U2 = Kp_phie_phi + Kd_phi(0-p) + Ki_phie_int_phi;
U3 = Kp_thetae_theta + Kd_theta(0-q) + Ki_thetae_int_theta;
U4 = Kp_psie_psi + Kd_psi(0-r) + Ki_psie_int_psi;
🧱 子系统3：Quadrotor Dynamics（动力学模型）——使用 S-Function 或 State-Space
方法一：S-Function（推荐）
创建 quad_dynamics.m：
matlab
function sys = mdlOutputs(t, x, u)
% 状态: x = [r; eta; v; omega]
% 输入: u = [U1, U2, U3, U4]
r = x(1:3); eta = x(4:6); v = x(7:9); omega = x(10:12);
U1 = u(1); U2 = u(2); U3 = u(3); U4 = u(4);
phi = eta(1); theta = eta(2); psi = eta(3);
% 旋转矩阵
R = [cos(psi)cos(theta), cos(psi)sin(theta)sin(phi)-sin(psi)cos(phi), cos(psi)sin(theta)cos(phi)+sin(psi)sin(phi);
sin(psi)cos(theta), sin(psi)sin(theta)sin(phi)+cos(psi)cos(phi), sin(psi)sin(theta)cos(phi)-cos(psi)sin(phi);
-sin(theta), cos(theta)sin(phi), cos(theta)cos(phi)];
acc_e = R [0; 0; U1] / m - [0; 0; g];
tau = [U2; U3; U4];
omega_dot = J \ (tau - cross(omega, J omega));
T = [1, sin(phi)tan(theta), cos(phi)tan(theta);
0, cos(phi), -sin(phi);
0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
eta_dot = T * omega;
sys = [v; eta_dot; acc_e; omega_dot];
end
📥 数据记录设置
在 Simulink 中添加 To Workspace 模块：
states_log：记录状态向量 [x,y,z,phi,theta,psi,…]
positions_ref：记录参考位置 [x_d,y_d,z_d]
tout：自动记录时间
运行 quadcopter_control.m 后将生成：
X轴：蓝色实线（实际），红色虚线（期望）
Y轴：红色实线，快速响应
Z轴：紫色实线，有超调但最终收敛
第四张图：偏航角变化（可选）
✅ 如何使用？
- 新建 Simulink 模型，命名为 quadcopter_control_model
- 添加上述子系统（可用 MATLAB Function 模块实现）
- 设置求解器为 ode4 (Runge-Kutta)，固定步长 0.01
- 运行主脚本 quadcopter_control.m
- 自动生成四张响应图
📌 扩展建议
添加 三维动画 显示飞行轨迹（使用 animatedline）
改用 LQR 或 MPC 控制器
加入 风扰、传感器噪声
导出为 ROS 节点 或 硬件在环（HIL）
电商企业物流数字化转型必备！快递鸟 API 接口，72 小时快速完成物流系统集成。全流程实战1V1指导，营造开放的API技术生态圈。
更多推荐
15
0- 0
已为社区贡献1条内容
所有评论(0)