当前位置：首页 > news >正文

《MATLAB实战训练营：从入门到工业级应用》高阶挑战篇-《用无人机仿真玩转PID控制：MATLAB四旋翼仿真建模全攻略》

news 2025/5/7 11:36:22

《MATLAB实战训练营：从入门到工业级应用》高阶挑战篇-✈️ 用无人机仿真玩转PID控制：MATLAB四旋翼仿真建模全攻略 🚁

欢迎来到这篇超级详细的MATLAB四旋翼无人机仿真教程！无论你是控制理论爱好者、无人机发烧友，还是MATLAB编程新手，这篇文章都将带你从零开始，一步步构建一个完整的四旋翼无人机PID控制系统仿真模型。准备好开始这场空中冒险了吗？让我们开始这段有趣的旅程吧！Let’s go! 🎯

1. 四旋翼无人机基础入门 🚀

1.1 四旋翼无人机的工作原理

四旋翼无人机，顾名思义就是有四个旋翼的飞行器 ✨。它的飞行原理其实非常有趣：

四个电机（M1-M4）呈十字形排列
对角电机旋转方向相同（M1和M3同向，M2和M4同向）
通过改变四个电机的转速来实现各种飞行动作

1.2 基本运动控制

四旋翼有6个自由度，但只有4个控制输入（四个电机的转速），属于欠驱动系统。基本运动控制包括：

运动类型	控制方法	示意图
垂直运动（升降）	同时增加/减少所有电机转速	⬆️⬇️
俯仰运动（前后）	增加前部/后部电机转速	✈️→
横滚运动（左右）	增加左侧/右侧电机转速	🚁←→
偏航运动（旋转）	增加对角线电机转速	🔄

💡 有趣的事实：四旋翼无人机本质上是一个不稳定的系统，这正是我们需要PID控制的原因！

2. 数学模型建立 📐

要仿真四旋翼，首先需要建立它的数学模型。我们采用牛顿-欧拉方程来描述四旋翼的动力学。

2.1 坐标系定义

我们定义两个坐标系：

地面坐标系（惯性系）：固定在地面，用于描述无人机的位置
机体坐标系：固定在无人机上，随无人机一起运动

2.2 动力学方程

四旋翼的6自由度动力学方程可以表示为：

平移运动：
$\frac{d^2x}{dt^2} = (\sin\psi \sin\phi + \cos\psi \sin\theta \cos\phi) U_1\$

$\frac{d^2y}{dt^2} = (-\cos\psi \sin\phi + \sin\psi \sin\theta \cos\phi) U_1$

$\frac{d^2z}{dt^2} = -mg + (\cos\theta \cos\phi) U_1$

旋转运动：
$I_{xx} \frac{d^2\phi}{dt^2} = \dot{\theta}\dot{\psi}(I_{yy} - I_{zz}) + l U_2$

$I_{yy} \frac{d^2\theta}{dt^2} = \dot{\phi}\dot{\psi}(I_{zz} - I_{xx}) + l U_3$

$I_{zz} \frac{d^2\psi}{dt^2} = \dot{\phi}\dot{\theta}(I_{xx} - I_{yy}) + U_4$

其中：

m：无人机质量
l：电机到质心的距离
Ixx, Iyy, Izz：转动惯量
U₁-U₄：控制输入

2.3 控制输入与电机转速关系

控制输入与电机转速的关系为：
$\begin{bmatrix} U_1 \\ U_2 \\ U_3 \\ U_4 \end{bmatrix} = \begin{bmatrix} 1 & 1 & 1 & 1 \\ 0 & -l & 0 & l \\ l & 0 & -l & 0 \\ c & -c & c & -c \end{bmatrix} \begin{bmatrix} F_1 \\ F_2 \\ F_3 \\ F_4 \end{bmatrix}$

其中F₁-F₄是四个电机产生的升力，c是反扭矩系数。

3. MATLAB仿真环境搭建 💻

现在让我们开始用MATLAB搭建仿真环境吧！

3.1 初始化参数

首先创建一个新的MATLAB脚本quadcopter_sim.m，并添加以下初始化代码：

clear all; close all; clc;%% 无人机物理参数
params.m = 1.2;      % 质量(kg)
params.g = 9.81;     % 重力加速度(m/s^2)
params.l = 0.25;     % 电机到质心的距离(m)% 转动惯量(kg·m^2)
params.Ixx = 0.0234;
params.Iyy = 0.0234;
params.Izz = 0.0468;params.c = 0.01;     % 反扭矩系数
params.k = 1.2e-5;   % 升力系数
params.b = 1e-6;     % 阻力系数%% 仿真参数
tspan = [0 20];      % 仿真时间范围
params.dt = 0.01;           % 时间步长(s)
t = tspan(1):params.dt:tspan(2);%% 初始状态
% [x y z x_dot y_dot z_dot phi theta psi phi_dot theta_dot psi_dot]
initial_state = zeros(12,1);  % 全部初始化为0

3.2 实现动力学方程

创建一个函数文件quadcopter_ode.m来实现动力学方程：

function state_dot = quadcopter_ode(t, state, params, U)% 解包状态变量x = state(1); y = state(2); z = state(3);x_dot = state(4); y_dot = state(5); z_dot = state(6);phi = state(7); theta = state(8); psi = state(9);phi_dot = state(10); theta_dot = state(11); psi_dot = state(12);% 解包控制输入U1 = U(1); U2 = U(2); U3 = U(3); U4 = U(4);% 平移加速度x_ddot = (sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi)) * U1 / params.m;y_ddot = (-cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi)) * U1 / params.m;z_ddot = -params.g + (cos(theta)*cos(phi)) * U1 / params.m;% 旋转加速度phi_ddot = theta_dot*psi_dot*(params.Iyy-params.Izz)/params.Ixx + params.l*U2/params.Ixx;theta_ddot = phi_dot*psi_dot*(params.Izz-params.Ixx)/params.Iyy + params.l*U3/params.Iyy;psi_ddot = phi_dot*theta_dot*(params.Ixx-params.Iyy)/params.Izz + U4/params.Izz;% 组装状态导数state_dot = zeros(12,1);state_dot(1) = x_dot;state_dot(2) = y_dot;state_dot(3) = z_dot;state_dot(4) = x_ddot;state_dot(5) = y_ddot;state_dot(6) = z_ddot;state_dot(7) = phi_dot;state_dot(8) = theta_dot;state_dot(9) = psi_dot;state_dot(10) = phi_ddot;state_dot(11) = theta_ddot;state_dot(12) = psi_ddot;
end

4. PID控制器设计 🎛️

现在我们来设计PID控制器，让无人机能够稳定飞行！

4.1 PID控制原理

PID控制器由三部分组成：

比例§：与当前误差成正比
积分(I)：与误差的积分成正比，消除稳态误差
微分(D)：与误差的变化率成正比，提供阻尼

PID控制器的输出公式为：
$K_p e(t) + K_i \int e(t) \, dt + K_d \frac{de(t)}{dt}$

4.2 实现PID控制器

创建一个函数文件pid_controller.m：

function [U, errors, integral_errors] = pid_controller(state, desired_state, ...params, prev_errors, integral_errors)% PID参数Kp_z = 10; Ki_z = 2; Kd_z = 5;      % 高度控制Kp_phi = 8; Ki_phi = 0.5; Kd_phi = 3; % 横滚控制Kp_theta = 8; Ki_theta = 0.5; Kd_theta = 3; % 俯仰控制Kp_psi = 8; Ki_psi = 0.5; Kd_psi = 3; % 偏航控制% 解包当前状态z = state(3); z_dot = state(6);phi = state(7); phi_dot = state(10);theta = state(8); theta_dot = state(11);psi = state(9); psi_dot = state(12);% 解包期望状态z_des = desired_state(3); z_dot_des = desired_state(6);phi_des = desired_state(7); phi_dot_des = desired_state(10);theta_des = desired_state(8); theta_dot_des = desired_state(11);psi_des = desired_state(9); psi_dot_des = desired_state(12);% 计算误差errors = zeros(4,1);errors(1) = z_des - z;           % 高度误差errors(2) = phi_des - phi;       % 横滚误差errors(3) = theta_des - theta;   % 俯仰误差errors(4) = psi_des - psi;       % 偏航误差% 计算误差导数error_dots = (errors - prev_errors) / params.dt;% 更新积分误差integral_errors = integral_errors + errors * params.dt;% 计算PID输出U1 = Kp_z * errors(1) + Ki_z * integral_errors(1) + Kd_z * error_dots(1);U2 = Kp_phi * errors(2) + Ki_phi * integral_errors(2) + Kd_phi * error_dots(2);U3 = Kp_theta * errors(3) + Ki_theta * integral_errors(3) + Kd_theta * error_dots(3);U4 = Kp_psi * errors(4) + Ki_psi * integral_errors(4) + Kd_psi * error_dots(4);% 限制控制输入U1 = max(min(U1, params.m*params.g*2), 0);  % U1不能为负U2 = max(min(U2, 5), -5);U3 = max(min(U3, 5), -5);U4 = max(min(U4, 5), -5);U = [U1; U2; U3; U4];
end

5. 仿真主循环 🔄

现在我们把所有部分整合到一起，完成仿真主循环：

%% 初始化
state = initial_state;
states = zeros(length(t), length(state));
states(1,:) = state';
desired_state = [0; 0; 2; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % 期望高度2米% PID控制器初始化
prev_errors = zeros(4,1);
integral_errors = zeros(4,1);%% 主仿真循环
for i = 1:length(t)-1% 获取当前控制输入[U, errors, integral_errors] = pid_controller(state, desired_state, ...params, prev_errors, integral_errors);prev_errors = errors;% 使用ode45求解ODE[~, temp_state] = ode45(@(t,state) quadcopter_ode(t, state, params, U), ...[t(i) t(i+1)], state);state = temp_state(end,:)';% 存储状态states(i+1,:) = state';% 显示进度if mod(i,100) == 0fprintf('仿真进度: %.1f%%\n', i/length(t)*100);end
endfprintf('仿真完成!\n');

6. 结果可视化 📊

仿真完成后，我们需要可视化结果来评估控制器的性能：

6.1 绘制3D轨迹

%% 3D轨迹图
figure('Name', '3D轨迹', 'Color', 'white');
plot3(states(:,1), states(:,2), states(:,3), 'b', 'LineWidth', 2);
hold on;
plot3(0, 0, desired_state(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
grid on;
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('四旋翼无人机3D轨迹');
legend('实际轨迹', '目标位置');
view(30,30);

在这里插入图片描述

6.2 绘制高度变化曲线

%% 高度变化图
figure('Name', '高度控制', 'Color', 'white');
plot(t, states(:,3), 'b', 'LineWidth', 2);
hold on;
plot(t, desired_state(3)*ones(size(t)), 'r--', 'LineWidth', 2);
grid on;
xlabel('时间 (s)'); ylabel('高度 (m)');
title('高度控制响应');
legend('实际高度', '期望高度');

在这里插入图片描述

6.3 绘制姿态角变化

%% 姿态角变化图
figure('Name', '姿态角', 'Color', 'white', 'Position', [100 100 1200 800]);subplot(3,1,1);
plot(t, rad2deg(states(:,7)), 'b', 'LineWidth', 2);
grid on;
xlabel('时间 (s)'); ylabel('横滚角 (deg)');
title('横滚角响应');subplot(3,1,2);
plot(t, rad2deg(states(:,8)), 'b', 'LineWidth', 2);
grid on;
xlabel('时间 (s)'); ylabel('俯仰角 (deg)');
title('俯仰角响应');subplot(3,1,3);
plot(t, rad2deg(states(:,9)), 'b', 'LineWidth', 2);
grid on;
xlabel('时间 (s)'); ylabel('偏航角 (deg)');
title('偏航角响应');

在这里插入图片描述

由上面三张图可以发现，这些轨迹比较理想，然而实际情况中比这会复杂很多，比如风的作用、传感器产生噪声数据等等，别急，我们继续往下看。

7. 参数调优技巧 🛠️

PID控制器的性能很大程度上取决于参数的选取。这里分享一些调参技巧：

7.1 调参步骤

先调P：将I和D设为0，逐渐增大P直到系统开始振荡
再调D：增加D来抑制振荡，使系统稳定
最后调I：如果需要消除稳态误差，适当增加I

7.2 Ziegler-Nichols方法

这是一种经典的PID参数整定方法：

先设Ki=Kd=0
增加Kp直到系统开始等幅振荡，记录此时的Kp=Ku和振荡周期Tu
根据下表设置PID参数：

控制器类型	Kp	Ti	Td
P	0.5Ku	-	-
PI	0.45Ku	0.83Tu	-
PID	0.6Ku	0.5Tu	0.125Tu

7.3 我们的参数建议

对于四旋翼无人机，以下参数可以作为起点：

% 高度控制
Kp_z = 10; Ki_z = 2; Kd_z = 5;% 姿态控制
Kp_angle = 8; Ki_angle = 0.5; Kd_angle = 3;

8. 进阶内容：添加风扰和噪声 🌬️

为了让仿真更接近现实，我们可以添加一些环境干扰：

8.1 修改ODE函数添加风扰

function state_dot = quadcopter_ode(t, state, params, U)% ... (之前的代码保持不变)% 添加随机风扰 (在x和y方向)wind_gain = 0.2;  % 风扰强度if t > 5  % 5秒后开始添加风扰x_ddot = x_ddot + wind_gain*randn();y_ddot = y_ddot + wind_gain*randn();end% ... (其余代码保持不变)
end

8.2 添加传感器噪声

在控制器中模拟传感器噪声：

function [U, errors, integral_errors] = pid_controller(state, desired_state, ...params, prev_errors, integral_errors)% 添加传感器噪声noise_gain = 0.02;state(3) = state(3) + noise_gain*randn();  % 高度噪声state(7:9) = state(7:9) + noise_gain*randn(3,1);  % 姿态角噪声% ... (其余代码保持不变)
end

9. 完整代码整合 🏗️

为了便于使用，这里提供完整的整合代码。创建一个名为quadcopter_sim.m的文件：

% 四旋翼无人机PID控制仿真 - 完整代码
% 作者: MATLAB技术大本营
% 日期: 2023年
% 版本: MATLAB 2016bfunction quadcopter_sim()%% 初始化参数clear all; close all; clc;% 无人机物理参数params.m = 1.2;      % 质量(kg)params.g = 9.81;     % 重力加速度(m/s^2)params.l = 0.25;     % 电机到质心的距离(m)% 转动惯量(kg·m^2)params.Ixx = 0.0234;params.Iyy = 0.0234;params.Izz = 0.0468;params.c = 0.01;     % 反扭矩系数params.k = 1.2e-5;   % 升力系数params.b = 1e-6;     % 阻力系数% 仿真参数tspan = [0 20];      % 仿真时间范围params.dt = 0.01;    % 时间步长(s)t = tspan(1):params.dt:tspan(2);% 初始状态 [x y z x_dot y_dot z_dot phi theta psi phi_dot theta_dot psi_dot]initial_state = zeros(12,1);% 期望状态 [x y z x_dot y_dot z_dot phi theta psi phi_dot theta_dot psi_dot]desired_state = [0; 0; 2; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % 期望高度2米%% 初始化仿真state = initial_state;states = zeros(length(t), length(state));states(1,:) = state';% PID控制器初始化prev_errors = zeros(4,1);integral_errors = zeros(4,1);%% 主仿真循环for i = 1:length(t)-1% 获取当前控制输入[U, errors, integral_errors] = pid_controller(state, desired_state, ...params, prev_errors, integral_errors);prev_errors = errors;% 使用ode45求解ODE[~, temp_state] = ode45(@(t,state) quadcopter_ode(t, state, params, U), ...[t(i) t(i+1)], state);state = temp_state(end,:)';% 存储状态states(i+1,:) = state';% 显示进度if mod(i,100) == 0fprintf('仿真进度: %.1f%%\n', i/length(t)*100);endendfprintf('仿真完成!\n');%% 可视化结果plot_results(t, states, desired_state);
endfunction state_dot = quadcopter_ode(t, state, params, U)% 解包状态变量x = state(1); y = state(2); z = state(3);x_dot = state(4); y_dot = state(5); z_dot = state(6);phi = state(7); theta = state(8); psi = state(9);phi_dot = state(10); theta_dot = state(11); psi_dot = state(12);% 解包控制输入U1 = U(1); U2 = U(2); U3 = U(3); U4 = U(4);% 平移加速度x_ddot = (sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi)) * U1 / params.m;y_ddot = (-cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi)) * U1 / params.m;z_ddot = -params.g + (cos(theta)*cos(phi)) * U1 / params.m;% 添加随机风扰 (在x和y方向)wind_gain = 0.2;  % 风扰强度if t > 5  % 5秒后开始添加风扰x_ddot = x_ddot + wind_gain*randn();y_ddot = y_ddot + wind_gain*randn();end% 旋转加速度phi_ddot = theta_dot*psi_dot*(params.Iyy-params.Izz)/params.Ixx + params.l*U2/params.Ixx;theta_ddot = phi_dot*psi_dot*(params.Izz-params.Ixx)/params.Iyy + params.l*U3/params.Iyy;psi_ddot = phi_dot*theta_dot*(params.Ixx-params.Iyy)/params.Izz + U4/params.Izz;% 组装状态导数state_dot = zeros(12,1);state_dot(1) = x_dot;state_dot(2) = y_dot;state_dot(3) = z_dot;state_dot(4) = x_ddot;state_dot(5) = y_ddot;state_dot(6) = z_ddot;state_dot(7) = phi_dot;state_dot(8) = theta_dot;state_dot(9) = psi_dot;state_dot(10) = phi_ddot;state_dot(11) = theta_ddot;state_dot(12) = psi_ddot;
endfunction [U, errors, integral_errors] = pid_controller(state, desired_state, ...params, prev_errors, integral_errors)% PID参数Kp_z = 10; Ki_z = 2; Kd_z = 5;      % 高度控制Kp_phi = 8; Ki_phi = 0.5; Kd_phi = 3; % 横滚控制Kp_theta = 8; Ki_theta = 0.5; Kd_theta = 3; % 俯仰控制Kp_psi = 8; Ki_psi = 0.5; Kd_psi = 3; % 偏航控制% 添加传感器噪声noise_gain = 0.02;state(3) = state(3) + noise_gain*randn();  % 高度噪声state(7:9) = state(7:9) + noise_gain*randn(3,1);  % 姿态角噪声% 解包当前状态z = state(3); z_dot = state(6);phi = state(7); phi_dot = state(10);theta = state(8); theta_dot = state(11);psi = state(9); psi_dot = state(12);% 解包期望状态z_des = desired_state(3); z_dot_des = desired_state(6);phi_des = desired_state(7); phi_dot_des = desired_state(10);theta_des = desired_state(8); theta_dot_des = desired_state(11);psi_des = desired_state(9); psi_dot_des = desired_state(12);% 计算误差errors = zeros(4,1);errors(1) = z_des - z;           % 高度误差errors(2) = phi_des - phi;       % 横滚误差errors(3) = theta_des - theta;   % 俯仰误差errors(4) = psi_des - psi;       % 偏航误差% 计算误差导数error_dots = (errors - prev_errors) / params.dt;% 更新积分误差integral_errors = integral_errors + errors * params.dt;% 计算PID输出U1 = Kp_z * errors(1) + Ki_z * integral_errors(1) + Kd_z * error_dots(1);U2 = Kp_phi * errors(2) + Ki_phi * integral_errors(2) + Kd_phi * error_dots(2);U3 = Kp_theta * errors(3) + Ki_theta * integral_errors(3) + Kd_theta * error_dots(3);U4 = Kp_psi * errors(4) + Ki_psi * integral_errors(4) + Kd_psi * error_dots(4);% 限制控制输入U1 = max(min(U1, params.m*params.g*2), 0);  % U1不能为负U2 = max(min(U2, 5), -5);U3 = max(min(U3, 5), -5);U4 = max(min(U4, 5), -5);U = [U1; U2; U3; U4];
endfunction plot_results(t, states, desired_state)%% 3D轨迹图figure('Name', '3D轨迹', 'Color', 'white', 'Position', [100 100 800 600]);plot3(states(:,1), states(:,2), states(:,3), 'b', 'LineWidth', 2);hold on;plot3(0, 0, desired_state(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');grid on;xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');title('四旋翼无人机3D轨迹');legend('实际轨迹', '目标位置');view(30,30);%% 高度变化图figure('Name', '高度控制', 'Color', 'white', 'Position', [200 200 800 400]);plot(t, states(:,3), 'b', 'LineWidth', 2);hold on;plot(t, desired_state(3)*ones(size(t)), 'r--', 'LineWidth', 2);grid on;xlabel('时间 (s)'); ylabel('高度 (m)');title('高度控制响应');legend('实际高度', '期望高度');%% 姿态角变化图figure('Name', '姿态角', 'Color', 'white', 'Position', [300 300 1200 800]);subplot(3,1,1);plot(t, rad2deg(states(:,7)), 'b', 'LineWidth', 2);grid on;xlabel('时间 (s)'); ylabel('横滚角 (deg)');title('横滚角响应');subplot(3,1,2);plot(t, rad2deg(states(:,8)), 'b', 'LineWidth', 2);grid on;xlabel('时间 (s)'); ylabel('俯仰角 (deg)');title('俯仰角响应');subplot(3,1,3);plot(t, rad2deg(states(:,9)), 'b', 'LineWidth', 2);grid on;xlabel('时间 (s)'); ylabel('偏航角 (deg)');title('偏航角响应');
end