基于Matlab的四旋翼无人机动力学PID控制仿真

Matlab算法工程师985计算机硕

于 2025-05-23 06:45:00 发布

阅读量919

点赞数 11

分类专栏： PID matlab 文章标签： matlab 无人机开发语言

本文链接：https://blog.csdn.net/2501_90884015/article/details/148129124

版权

matlab 同时被 2 个专栏收录

32 篇文章

订阅专栏

PID

10 篇文章

订阅专栏

基于Matlab的四旋翼无人机动力学PID控制仿真，
具体内容包括：

运用欧拉方程对地面坐标到机体坐标的转换矩阵进行了推导
在无人机动力学模型基础上，采用经典PID控制算法对其内环姿态和外环位置进行控制
说明文档：
①详细推导四旋翼飞行器的数学模型
②PID控制器的设计、位置回路控制器设计、姿态回路控制器设计
③PID参数调整
④仿真结果分析98

文章目录

在这里插入图片描述

基于 PID 控制 的四旋翼无人机动力学仿真 MATLAB 实现。该代码模拟了四旋翼无人机的姿态控制和位置控制，适用于无人机的动力学建模与控制研究。

程序说明

问题背景：
- 四旋翼无人机的动力学模型包括姿态控制（滚转、俯仰、偏航）和位置控制（高度、水平移动）。
- PID 控制器用于调节无人机的姿态角和位置。
四旋翼动力学模型：
- 基于牛顿-欧拉方程，考虑无人机的平动和旋转运动。
- 包括质量、惯性矩、重力、推力等参数。
PID 控制器：
- 用于调整无人机的姿态角（滚转角、俯仰角、偏航角）和位置（高度、水平坐标）。
- 每个控制通道（如滚转、俯仰、高度）都独立设计 PID 参数。
适用场景：
- 无人机的轨迹跟踪。
- 无人机的姿态稳定。

— 在这里插入图片描述

MATLAB 代码

% 四旋翼无人机动力学 PID 控制仿真
clc;
clear;

%% 参数定义
m = 1.0; % 质量 (kg)
g = 9.81; % 重力加速度 (m/s^2)
Ixx = 0.01; Iyy = 0.01; Izz = 0.02; % 惯性矩 (kg·m^2)
L = 0.2; % 旋翼臂长 (m)
kf = 1e-5; % 推力系数
km = 1e-7; % 力矩系数

% 时间参数
dt = 0.01; % 时间步长 (s)
T = 10; % 总仿真时间 (s)
time = 0:dt:T;

% 初始状态
x = [0; 0; 0]; % 位置 [x, y, z] (m)
v = [0; 0; 0]; % 速度 [vx, vy, vz] (m/s)
phi = 0; theta = 0; psi = 0; % 姿态角 [滚转, 俯仰, 偏航] (rad)
omega = [0; 0; 0]; % 角速度 [p, q, r] (rad/s)

% 目标值
target_pos = [1; 1; 2]; % 目标位置 [x, y, z] (m)
target_yaw = 0; % 目标偏航角 (rad)

% PID 参数
Kp_pos = [1.0, 1.0, 1.0]; % 位置 PID 参数
Ki_pos = [0.1, 0.1, 0.1];
Kd_pos = [0.5, 0.5, 0.5];

Kp_att = [10, 10, 1]; % 姿态 PID 参数
Ki_att = [0.1, 0.1, 0.1];
Kd_att = [2, 2, 0.5];

% 初始化变量
pos_error_integral = zeros(3, 1);
att_error_integral = zeros(3, 1);
prev_pos_error = zeros(3, 1);
prev_att_error = zeros(3, 1);

% 存储结果
pos_history = zeros(length(time), 3);
att_history = zeros(length(time), 3);

%% 主循环
for i = 1:length(time)
    % 计算位置误差
    pos_error = target_pos - x;
    pos_error_integral = pos_error_integral + pos_error * dt;
    pos_error_derivative = (pos_error - prev_pos_error) / dt;
    
    % 计算期望姿态角（根据位置误差）
    phi_des = Kp_pos(1) * pos_error(1) + Ki_pos(1) * pos_error_integral(1) + Kd_pos(1) * pos_error_derivative(1);
    theta_des = Kp_pos(2) * pos_error(2) + Ki_pos(2) * pos_error_integral(2) + Kd_pos(2) * pos_error_derivative(2);
    psi_des = target_yaw;
    
    % 计算姿态误差
    att_error = [phi_des; theta_des; psi_des] - [phi; theta; psi];
    att_error_integral = att_error_integral + att_error * dt;
    att_error_derivative = (att_error - prev_att_error) / dt;
    
    % PID 控制输出（角速度）
    p_des = Kp_att(1) * att_error(1) + Ki_att(1) * att_error_integral(1) + Kd_att(1) * att_error_derivative(1);
    q_des = Kp_att(2) * att_error(2) + Ki_att(2) * att_error_integral(2) + Kd_att(2) * att_error_derivative(2);
    r_des = Kp_att(3) * att_error(3) + Ki_att(3) * att_error_integral(3) + Kd_att(3) * att_error_derivative(3);
    
    % 动力学更新
    F = m * g + kf * sum([p_des; q_des; r_des]); % 总推力
    tau_phi = L * kf * (r_des(4) - r_des(2)); % 滚转力矩
    tau_theta = L * kf * (r_des(3) - r_des(1)); % 俯仰力矩
    tau_psi = km * (r_des(1) - r_des(2) + r_des(3) - r_des(4)); % 偏航力矩
    
    % 更新线速度
    v = v + dt * ([0; 0; -g] + (F / m) * [cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi); ...
        cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi); ...
        cos(phi)*cos(theta)]);
    
    % 更新位置
    x = x + dt * v;
    
    % 更新角速度
    omega_dot = [tau_phi/Ixx; tau_theta/Iyy; tau_psi/Izz] - ...
        [((Iyy-Izz)/Ixx)*omega(2)*omega(3); ...
         ((Izz-Ixx)/Iyy)*omega(3)*omega(1); ...
         ((Ixx-Iyy)/Izz)*omega(1)*omega(2)];
    omega = omega + dt * omega_dot;
    
    % 更新姿态角
    phi = phi + dt * (omega(1) + sin(phi)*tan(theta)*omega(2) + cos(phi)*tan(theta)*omega(3));
    theta = theta + dt * (cos(phi)*omega(2) - sin(phi)*omega(3));
    psi = psi + dt * ((sin(phi)/cos(theta))*omega(2) + (cos(phi)/cos(theta))*omega(3));
    
    % 存储结果
    pos_history(i, :) = x';
    att_history(i, :) = [phi, theta, psi];
    
    % 更新误差
    prev_pos_error = pos_error;
    prev_att_error = att_error;
end

%% 绘图
figure;
subplot(3, 1, 1);
plot(time, pos_history(:, 1), 'r', 'LineWidth', 1.5);
hold on;
plot(time, ones(size(time)) * target_pos(1), 'k--');
xlabel('时间 (s)');
ylabel('X 位置 (m)');
title('X 位置随时间变化');
legend('实际值', '目标值');
grid on;

subplot(3, 1, 2);
plot(time, pos_history(:, 2), 'g', 'LineWidth', 1.5);
hold on;
plot(time, ones(size(time)) * target_pos(2), 'k--');
xlabel('时间 (s)');
ylabel('Y 位置 (m)');
title('Y 位置随时间变化');
legend('实际值', '目标值');
grid on;

subplot(3, 1, 3);
plot(time, pos_history(:, 3), 'b', 'LineWidth', 1.5);
hold on;
plot(time, ones(size(time)) * target_pos(3), 'k--');
xlabel('时间 (s)');
ylabel('Z 位置 (m)');
title('Z 位置随时间变化');
legend('实际值', '目标值');
grid on;

figure;
subplot(3, 1, 1);
plot(time, att_history(:, 1), 'r', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('滚转角 (rad)');
title('滚转角随时间变化');
grid on;

subplot(3, 1, 2);
plot(time, att_history(:, 2), 'g', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('俯仰角 (rad)');
title('俯仰角随时间变化');
grid on;

subplot(3, 1, 3);
plot(time, att_history(:, 3), 'b', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('偏航角 (rad)');
title('偏航角随时间变化');
grid on;