实现四旋翼无人机的物理建模、控制算法、3D可视化和用户交互功能。
系统架构
matlab
四旋翼仿真系统
├── GUI界面 (MATLAB App Designer)
│ ├── 参数设置面板
│ ├── 控制面板
│ ├── 状态显示
│ └── 3D视图集成
├── Simulink模型
│ ├── 四旋翼动力学模型
│ ├── 控制器模块
│ ├── 环境模型
│ └── 3D动画模块
└── 数据处理与通信
├── 参数传递
├── 状态更新
└── 数据记录完整实现代码
1. GUI界面 (appdesigner)
matlab
classdef QuadrotorSimulator < matlab.apps.AppBase
% 四旋翼无人机仿真系统主界面
properties (Access = public)
UIFigure matlab.ui.Figure
TabGroup matlab.ui.container.TabGroup
ControlTab matlab.ui.container.Tab
ParamsTab matlab.ui.container.Tab
ViewTab matlab.ui.container.Tab
% 控制面板组件
StartButton matlab.ui.control.Button
StopButton matlab.ui.control.Button
ResetButton matlab.ui.control.Button
PositionLabel matlab.ui.control.Label
VelocityLabel matlab.ui.control.Label
AttitudeLabel matlab.ui.control.Label
BatteryLabel matlab.ui.control.Label
% 参数面板组件
MassEdit matlab.ui.control.NumericEditField
InertiaEdit matlab.ui.control.NumericEditField
ArmLengthEdit matlab.ui.control.NumericEditField
PropellerEdit matlab.ui.control.NumericEditField
PIDKpEdit matlab.ui.control.NumericEditField
PIDKiEdit matlab.ui.control.NumericEditField
PIDKdEdit matlab.ui.control.NumericEditField
WindSpeedEdit matlab.ui.control.NumericEditField
GravityEdit matlab.ui.control.NumericEditField
% 3D视图组件
SceneView matlab.ui.control.UIAxes
TrajectoryView matlab.ui.control.UIAxes
% 状态变量
SimulinkModelRunning logical = false
SimHandle % Simulink模型句柄
LogData % 仿真数据记录
end
methods (Access = private)
function startupFcn(app)
% 初始化参数
app.MassEdit.Value = 1.2; % kg
app.InertiaEdit.Value = 0.02; % kg·m²
app.ArmLengthEdit.Value = 0.25; % m
app.PropellerEdit.Value = 0.1; % m
app.PIDKpEdit.Value = 1.0;
app.PIDKiEdit.Value = 0.1;
app.PIDKdEdit.Value = 0.5;
app.WindSpeedEdit.Value = 0.5; % m/s
app.GravityEdit.Value = 9.81; % m/s²
% 初始化3D视图
init3DView(app);
end
function init3DView(app)
% 创建3D场景
cla(app.SceneView);
hold(app.SceneView, 'on');
% 绘制地面网格
[X, Y] = meshgrid(-5:0.5:5, -5:0.5:5);
Z = zeros(size(X));
surf(app.SceneView, X, Y, Z, 'FaceAlpha', 0.3, 'EdgeColor', [0.5 0.5 0.5]);
% 绘制四旋翼模型
drawQuadrotor(app, [0, 0, 0], [0, 0, 0], 0.8);
% 设置视角
view(app.SceneView, 3);
axis(app.SceneView, 'equal');
xlim(app.SceneView, [-5 5]);
ylim(app.SceneView, [-5 5]);
zlim(app.SceneView, [0 5]);
grid(app.SceneView, 'on');
xlabel(app.SceneView, 'X (m)');
ylabel(app.SceneView, 'Y (m)');
zlabel(app.SceneView, 'Z (m)');
title(app.SceneView, '四旋翼无人机3D视图');
end
function drawQuadrotor(app, pos, att, color)
% 绘制四旋翼无人机模型
[x, y, z] = ellipsoid(0, 0, 0, 0.1, 0.1, 0.05);
h_body = surf(app.SceneView, x+pos(1), y+pos(2), z+pos(3), 'FaceColor', color, 'EdgeColor', 'none');
% 绘制机臂
arm_length = 0.25;
arm_width = 0.02;
for i = 1:4
angle = (i-1)*pi/2;
x_arm = [0, arm_length*cos(angle)];
y_arm = [0, arm_length*sin(angle)];
z_arm = [0, 0];
h_arm = plot3(app.SceneView, x_arm+pos(1), y_arm+pos(2), z_arm+pos(3), 'b-', 'LineWidth', 3);
end
% 绘制螺旋桨
prop_radius = 0.1;
for i = 1:4
angle = (i-1)*pi/2;
center = [arm_length*cos(angle), arm_length*sin(angle), 0];
[x_prop, y_prop] = cylinder(prop_radius, 16);
z_prop = zeros(size(x_prop));
h_prop = surf(app.SceneView, x_prop+center(1)+pos(1), ...
y_prop+center(2)+pos(2), z_prop+center(3)+pos(3), ...
'FaceColor', [0.7 0.7 0.7], 'EdgeColor', 'none');
end
% 应用姿态变换
rotate(h_body, [1 0 0], rad2deg(att(1)), [pos(1), pos(2), pos(3)]);
rotate(h_body, [0 1 0], rad2deg(att(2)), [pos(1), pos(2), pos(3)]);
rotate(h_body, [0 0 1], rad2deg(att(3)), [pos(1), pos(2), pos(3)]);
end
function updateScene(app, pos, att)
% 更新3D场景中的无人机位置
delete(findall(app.SceneView, 'Type', 'Surface'));
delete(findall(app.SceneView, 'Type', 'Line'));
% 重绘场景
init3DView(app);
drawQuadrotor(app, pos, att, [0.2 0.6 1.0]);
end
function startSimulation(app)
if app.SimulinkModelRunning
return;
end
% 设置Simulink模型参数
assignin('base', 'droneMass', app.MassEdit.Value);
assignin('base', 'droneInertia', app.InertiaEdit.Value);
assignin('base', 'armLength', app.ArmLengthEdit.Value);
assignin('base', 'propRadius', app.PropellerEdit.Value);
assignin('base', 'pidKp', app.PIDKpEdit.Value);
assignin('base', 'pidKi', app.PIDKiEdit.Value);
assignin('base', 'pidKd', app.PIDKdEdit.Value);
assignin('base', 'windSpeed', app.WindSpeedEdit.Value);
assignin('base', 'gravity', app.GravityEdit.Value);
% 启动Simulink模型
app.SimHandle = sim('quadrotor_model.slx', 'SimulationMode', 'normal', 'SaveOutput', 'on');
app.SimulinkModelRunning = true;
% 启动定时器更新界面
app.Timer = timer(...
'ExecutionMode', 'fixedRate', ...
'Period', 0.05, ...
'TimerFcn', @(~,~) updateDisplay(app));
start(app.Timer);
end
function stopSimulation(app)
if app.SimulinkModelRunning
% 停止Simulink模型
evalin('base', 'set_param(''quadrotor_model'', ''SimulationCommand'', ''stop'')');
app.SimulinkModelRunning = false;
% 停止定时器
if isvalid(app.Timer)
stop(app.Timer);
delete(app.Timer);
end
end
end
function resetSimulation(app)
app.stopSimulation();
app.init3DView();
app.PositionLabel.Text = '位置: [0.00, 0.00, 0.00] m';
app.VelocityLabel.Text = '速度: [0.00, 0.00, 0.00] m/s';
app.AttitudeLabel.Text = '姿态: [0.00, 0.00, 0.00] °';
app.BatteryLabel.Text = '电池: 100%';
end
function updateDisplay(app)
if ~app.SimulinkModelRunning
return;
end
try
% 从Simulink模型获取数据
t = app.SimHandle.get('tout');
if isempty(t)
return;
end
lastIdx = length(t);
pos = app.SimHandle.get('position');
vel = app.SimHandle.get('velocity');
att = app.SimHandle.get('attitude');
battery = app.SimHandle.get('batteryLevel');
% 更新界面显示
app.PositionLabel.Text = sprintf('位置: [%.2f, %.2f, %.2f] m', pos(lastIdx,1), pos(lastIdx,2), pos(lastIdx,3));
app.VelocityLabel.Text = sprintf('速度: [%.2f, %.2f, %.2f] m/s', vel(lastIdx,1), vel(lastIdx,2), vel(lastIdx,3));
app.AttitudeLabel.Text = sprintf('姿态: [%.1f, %.1f, %.1f] °', rad2deg(att(lastIdx,1)), rad2deg(att(lastIdx,2)), rad2deg(att(lastIdx,3)));
app.BatteryLabel.Text = sprintf('电池: %.0f%%', battery(lastIdx));
% 更新3D视图
app.updateScene(pos(lastIdx,:), att(lastIdx,:));
% 更新轨迹视图
plot(app.TrajectoryView, pos(:,1), pos(:,3), 'b-');
hold(app.TrajectoryView, 'on');
plot(app.TrajectoryView, pos(1,1), pos(1,3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(app.TrajectoryView, pos(lastIdx,1), pos(lastIdx,3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
xlabel(app.TrajectoryView, 'X (m)');
ylabel(app.TrajectoryView, 'Z (m)');
title(app.TrajectoryView, '飞行轨迹 (XZ平面)');
grid(app.TrajectoryView, 'on');
axis(app.TrajectoryView, 'equal');
catch ME
disp(['更新显示错误: ' ME.message]);
end
end
end
methods (Access = public)
function app = QuadrotorSimulator
% 创建UI界面
app.UIFigure = uifigure('Name', '四旋翼无人机仿真系统', 'Position', [100 100 900 600]);
% 创建选项卡组
app.TabGroup = uitabgroup(app.UIFigure, 'Position', [20 20 860 560]);
% 控制选项卡
app.ControlTab = uitab(app.TabGroup, 'Title', '控制');
app.StartButton = uibutton(app.ControlTab, 'push', ...
'Text', '开始仿真', ...
'Position', [50 450 100 30], ...
'ButtonPushedFcn', @(btn,event) startSimulation(app));
app.StopButton = uibutton(app.ControlTab, 'push', ...
'Text', '停止仿真', ...
'Position', [170 450 100 30], ...
'ButtonPushedFcn', @(btn,event) stopSimulation(app));
app.ResetButton = uibutton(app.ControlTab, 'push', ...
'Text', '重置仿真', ...
'Position', [290 450 100 30], ...
'ButtonPushedFcn', @(btn,event) resetSimulation(app));
% 状态显示
uilabel(app.ControlTab, 'Text', '当前状态:', 'Position', [50 400 80 22]);
app.PositionLabel = uilabel(app.ControlTab, 'Text', '位置: [0.00, 0.00, 0.00] m', 'Position', [140 400 250 22]);
app.VelocityLabel = uilabel(app.ControlTab, 'Text', '速度: [0.00, 0.00, 0.00] m/s', 'Position', [140 370 250 22]);
app.AttitudeLabel = uilabel(app.ControlTab, 'Text', '姿态: [0.00, 0.00, 0.00] °', 'Position', [140 340 250 22]);
app.BatteryLabel = uilabel(app.ControlTab, 'Text', '电池: 100%', 'Position', [140 310 250 22]);
% 3D视图
app.SceneView = uiaxes(app.ControlTab, 'Position', [400 250 450 300]);
app.TrajectoryView = uiaxes(app.ControlTab, 'Position', [400 50 450 180]);
% 参数选项卡
app.ParamsTab = uitab(app.TabGroup, 'Title', '参数设置');
uilabel(app.ParamsTab, 'Text', '质量 (kg):', 'Position', [50 450 100 22]);
app.MassEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [160 450 100 22], 'Value', 1.2);
uilabel(app.ParamsTab, 'Text', '转动惯量 (kg·m²):', 'Position', [50 410 120 22]);
app.InertiaEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [180 410 100 22], 'Value', 0.02);
uilabel(app.ParamsTab, 'Text', '机臂长度 (m):', 'Position', [50 370 100 22]);
app.ArmLengthEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [160 370 100 22], 'Value', 0.25);
uilabel(app.ParamsTab, 'Text', '螺旋桨半径 (m):', 'Position', [50 330 120 22]);
app.PropellerEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [180 330 100 22], 'Value', 0.1);
uilabel(app.ParamsTab, 'Text', 'PID参数:', 'Position', [50 280 80 22]);
uilabel(app.ParamsTab, 'Text', 'Kp:', 'Position', [50 250 30 22]);
app.PIDKpEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [80 250 100 22], 'Value', 1.0);
uilabel(app.ParamsTab, 'Text', 'Ki:', 'Position', [200 250 30 22]);
app.PIDKiEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [230 250 100 22], 'Value', 0.1);
uilabel(app.ParamsTab, 'Text', 'Kd:', 'Position', [350 250 30 22]);
app.PIDKdEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [380 250 100 22], 'Value', 0.5);
uilabel(app.ParamsTab, 'Text', '风速 (m/s):', 'Position', [50 200 100 22]);
app.WindSpeedEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [160 200 100 22], 'Value', 0.5);
uilabel(app.ParamsTab, 'Text', '重力加速度 (m/s²):', 'Position', [50 160 150 22]);
app.GravityEdit = uieditfield(app.ParamsTab, 'numeric', 'Position', [210 160 100 22], 'Value', 9.81);
% 视图选项卡
app.ViewTab = uitab(app.TabGroup, 'Title', '数据分析');
% 这里可以添加数据分析图表
% 注册启动函数
app.startupFcn();
end
end
end2. Simulink模型 (quadrotor_model.slx)
模型结构描述
matlab
四旋翼模型
├── 输入模块
│ ├── 目标位置 [x_d, y_d, z_d]
│ ├── 目标姿态 [roll_d, pitch_d, yaw_d]
│ └── 风速 [vx_w, vy_w, vz_w]
├── 控制器模块
│ ├── 位置PID控制器
│ ├── 姿态PID控制器
│ └── 电机分配器
├── 四旋翼动力学模型
│ ├── 电机模型
│ ├── 力与力矩计算
│ ├── 刚体动力学
│ └── 环境模型（风、重力）
├── 传感器模型
│ ├── IMU（加速度计、陀螺仪）
│ ├── GPS（位置、速度）
│ └── 电池模型
├── 3D动画模块
│ └── VR Sink
└── 输出模块
├── 位置 [x, y, z]
├── 速度 [vx, vy, vz]
├── 姿态 [roll, pitch, yaw]
├── 角速度 [wx, wy, wz]
└── 电池电量关键MATLAB函数代码
四旋翼动力学模型 (quadrotor_dynamics.m)
matlab
function [dx, dy, dz, dvx, dvy, dvz, droll, dpitch, dyaw, dwx, dwy, dwz] = quadrotor_dynamics(t, state, u, params)
% 四旋翼无人机动力学模型
% 状态: [x, y, z, vx, vy, vz, roll, pitch, yaw, wx, wy, wz]
% 控制输入: [T, tau_roll, tau_pitch, tau_yaw]
% 参数: 结构体包含质量、惯性矩等
% 解包状态
x = state(1); y = state(2); z = state(3);
vx = state(4); vy = state(5); vz = state(6);
roll = state(7); pitch = state(8); yaw = state(9);
wx = state(10); wy = state(11); wz = state(12);
% 解包参数
m = params.mass; % 质量 (kg)
Ixx = params.Ixx; % 转动惯量 (kg·m²)
Iyy = params.Iyy;
Izz = params.Izz;
g = params.gravity; % 重力加速度 (m/s²)
L = params.arm_length; % 机臂长度 (m)
k = params.k_thrust; % 推力系数
b = params.k_drag; % 扭矩系数
% 解包控制输入
T = u(1); % 总推力
tau_roll = u(2); % 滚转力矩
tau_pitch = u(3); % 俯仰力矩
tau_yaw = u(4); % 偏航力矩
% 旋转矩阵 (机体到世界坐标系)
R = [cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+cos(yaw)*sin(pitch)*cos(roll);
sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll);
-sin(pitch), cos(pitch)*sin(roll), cos(pitch)*cos(roll)];
% 外力 (重力 + 空气阻力)
F_gravity = [0; 0; -m*g];
F_drag = -params.drag_coeff * [vx; vy; vz]; % 简化的空气阻力模型
F_total = R * [0; 0; T] + F_gravity + F_drag;
% 力矩
tau_total = [tau_roll; tau_pitch; tau_yaw];
% 动力学方程 (牛顿-欧拉方程)
dvx = F_total(1)/m;
dvy = F_total(2)/m;
dvz = F_total(3)/m;
dwx = (tau_total(1) + (Iyy - Izz)*wy*wz)/Ixx;
dwy = (tau_total(2) + (Izz - Ixx)*wx*wz)/Iyy;
dwz = (tau_total(3) + (Ixx - Iyy)*wx*wy)/Izz;
% 运动学方程
dx = vx;
dy = vy;
dz = vz;
droll = wx + sin(roll)*tan(pitch)*wy + cos(roll)*tan(pitch)*wz;
dpitch = cos(roll)*wy - sin(roll)*wz;
dyaw = (sin(roll)/cos(pitch))*wy + (cos(roll)/cos(pitch))*wz;
% 输出导数
dx = [dx; dy; dz; dvx; dvy; dvz; droll; dpitch; dyaw; dwx; dwy; dwz];
endPID控制器 (pid_controller.m)
matlab
function u = pid_controller(setpoint, measurement, prev_error, integral, Kp, Ki, Kd, dt)
% PID控制器实现
% setpoint: 设定值
% measurement: 测量值
% prev_error: 上一次误差
% integral: 积分项累积
% Kp, Ki, Kd: PID参数
% dt: 时间步长
% 计算当前误差
error = setpoint - measurement;
% 比例项
P = Kp * error;
% 积分项 (带抗饱和)
integral = integral + error * dt;
I = Ki * integral;
% 微分项
derivative = (error - prev_error) / dt;
D = Kd * derivative;
% 计算控制输出
u = P + I + D;
% 更新状态
prev_error = error;
end电机分配器 (motor_mixer.m)
matlab
function motor_speeds = motor_mixer(T, tau_roll, tau_pitch, tau_yaw, params)
% 将控制指令转换为电机转速
% T: 总推力
% tau_roll: 滚转力矩
% tau_pitch: 俯仰力矩
% tau_yaw: 偏航力矩
% params: 包含参数的结构体
L = params.arm_length; % 机臂长度
k = params.k_thrust; % 推力系数
b = params.k_drag; % 扭矩系数
% 解线性方程
% [1, 1, 1, 1;
% -L, L, L, -L;
% -L, -L, L, L;
% -b, b, -b, b] * [ω1², ω2², ω3², ω4²]^T = [T, tau_roll, tau_pitch, tau_yaw]^T
A = [1, 1, 1, 1;
-L, L, L, -L;
-L, -L, L, L;
-b, b, -b, b];
cmd = [T; tau_roll; tau_pitch; tau_yaw];
omega_sq = A \ cmd;
% 转换为实际转速 (rad/s)
motor_speeds = sqrt(max(0, omega_sq)); % 确保非负
end3. 3D动画模块 (VR Sink配置)
matlab
% 创建VR世界文件 (quadrotor.wrl)
% 文件内容:
% #VRML V2.0 utf8
% Transform {
% children [
% DEF Quadrotor Transform {
% translation 0 0 0
% children [
% Shape {
% appearance Appearance { material Material { diffuseColor 0 0.5 1 } }
% geometry Box { size 0.2 0.2 0.1 }
% }
% Transform { translation 0.25 0 0 children [Shape { ... }] } % 机臂1
% Transform { translation 0 -0.25 0 children [Shape { ... }] } % 机臂2
% Transform { translation -0.25 0 0 children [Shape { ... }] } % 机臂3
% Transform { translation 0 0.25 0 children [Shape { ... }] } % 机臂4
% ]
% }
% ]
% }
% 在Simulink中使用VR Sink
% 1. 添加VR Sink模块
% 2. 加载quadrotor.wrl文件
% 3. 连接输入:
% - Quadrotor.translation: [x, y, z]
% - Quadrotor.rotation: [roll, pitch, yaw] (欧拉角)系统功能说明
1. GUI功能
-
参数设置：调整无人机物理参数、PID控制器参数和环境参数
-
控制面板：启动、停止和重置仿真
-
状态显示：实时显示位置、速度、姿态和电池状态
-
3D可视化：实时显示无人机在3D环境中的位置和姿态
-
轨迹显示：显示XZ平面的飞行轨迹
2. Simulink模型功能
- 精确动力学模型：基于牛顿-欧拉方程的刚体动力学
- 环境模型：重力、空气阻力和风力影响
- 控制器：位置环和姿态环PID控制器
- 电机模型：将控制指令转换为电机转速
- 传感器模型：模拟IMU、GPS和电池状态
- 3D动画：使用VR Sink实现实时3D可视化
3. 仿真场景示例
- 悬停测试：设置目标位置为[0,0,1]，观察无人机稳定悬停
- 轨迹跟踪：设置圆形或方形轨迹，测试控制器性能
- 抗风测试：增加风速参数，观察无人机在风中的稳定性
- 负载变化：增加质量参数，测试控制器适应性
参考代码 带有GUI、Simulink 3D模拟的四旋翼仿真程序 www.youwenfan.com/contentcss/45895.html
使用说明
-
启动系统：
matlabapp = QuadrotorSimulator; -
参数设置：
- 在"参数设置"选项卡调整无人机物理参数
- 调整PID参数优化控制性能
- 设置风速模拟风场环境
-
运行仿真：
- 点击"开始仿真"按钮启动仿真
- 在"控制"选项卡观察实时状态
- 使用"停止仿真"和"重置仿真"按钮控制仿真过程
-
数据分析：
- 在"数据分析"选项卡查看详细数据（需扩展实现）
- 导出仿真数据进行进一步分析
扩展功能建议
- 高级控制器 ：
- 实现LQR或MPC控制器
- 添加自适应控制算法
- 故障模拟 ：
- 电机故障模拟
- 传感器失效模拟
- 通信延迟模拟
- 路径规划 ：
- 集成A*或RRT路径规划算法
- 添加避障功能
- 多机协同 ：
- 扩展为多无人机仿真系统
- 实现编队控制和协同导航
- 硬件在环 ：
- 添加PX4或DJI SDK接口
- 实现硬件在环仿真