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

《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自由度动力学方程可以表示为：

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

$$m\frac{d^{2}y}{d{t}^2}=(-\cos \psi \sin \varphi +\sin \psi \sin \theta \cos \varphi )U_1$$

$$m\frac{d^{2}z}{d{t}^2}=-mg+(\cos \theta \cos \varphi )U_1$$

旋转运动：
$$I_{xx}\frac{d^2\varphi }{d{t}^2}=\dot{\theta }\dot{\psi }(I_{yy}-I_{zz})+l{U}_2$$

$$I_{yy}\frac{d^2\theta }{d{t}^2}=\dot{\varphi }\dot{\psi }(I_{zz}-I_{xx})+l{U}_3$$

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

其中：

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

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

控制输入与电机转速的关系为：
$$\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{cccc}1& 1& 1& 1\\ 0& -l& 0& l\\ l& 0& -l& 0\\ c& -c& c& -c\end{array}\right]\left[\begin{array}{c}{F}_1\\ F_2\\ F_3\\ F_4\end{array}\right]$$

其中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控制器的输出公式为：
$$u(t)=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) == 0
        fprintf('仿真进度: %.1f%%\n', i/length(t)*100);
    end
end

fprintf('仿真完成!\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 调参步骤

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

7.2 Ziegler-Nichols方法

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

1. 先设Ki=Kd=0
2. 增加Kp直到系统开始等幅振荡，记录此时的Kp=Ku和振荡周期Tu
3. 根据下表设置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 2016b

function 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) == 0
            fprintf('仿真进度: %.1f%%\n', i/length(t)*100);
        end
    end
    
    fprintf('仿真完成!\n');
    
    %% 可视化结果
    plot_results(t, states, desired_state);
end

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;
    
    % 添加随机风扰 (在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;
end

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; % 偏航控制
    
    % 添加传感器噪声
    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];
end

function 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

以下是添加风扰和传感器噪声之后的效果，我们会发现轨迹图确实更贴合实际效果一些呢～

3D轨迹图

高度变化图

姿态角变化图

10. 总结与展望 🎯

恭喜你完成了这个四旋翼无人机PID控制的MATLAB仿真！🎉 通过这个项目，我们学到了：

1. 四旋翼无人机的动力学建模
2. PID控制器的原理与实现
3. MATLAB仿真环境的搭建
4. 控制参数的调优方法
5. 如何处理现实中的干扰和噪声

未来改进方向

这只是一个起点，你还可以考虑以下扩展：

• 实现轨迹跟踪：让无人机跟随特定路径飞行
• 添加图像识别：结合计算机视觉实现目标跟踪
• 改用更先进的控制算法：如LQR、MPC或神经网络控制
• 硬件在环仿真：连接实际飞控硬件进行测试

希望这篇教程对你有所帮助！如果有任何问题或建议，欢迎留言讨论。Happy coding! 🚀