四旋翼飞行器PD控制算法及其Matlab实现

四旋翼飞行器PD控制算法及其Matlab实现

引用

CSDN

1.

https://blog.csdn.net/Matlab88888/article/details/139558943

四旋翼飞行器是一种受到广泛应用的无人飞行器，其飞行稳定性和控制是实现各种任务的关键。PD（比例-微分）控制是一种常用的控制算法，用于稳定四旋翼飞行器的姿态（姿态角和角速度）。PD控制器由两部分组成：比例（P）和微分（D）部分。比例部分负责根据当前误差（期望值与实际值之间的差异）产生控制输出，其大小与误差成正比。微分部分则根据误差的变化率来调整控制输出，用于抑制系统的震荡和提高响应速度。综合考虑比例和微分部分的影响，PD控制器能够实现快速而稳定的系统响应。在四旋翼飞行器中，PD控制器通常用于控制姿态，即飞行器的倾斜角度和角速度，以实现期望的飞行姿态。通过测量飞行器的姿态和角速度，并与期望值进行比较，PD控制器可以生成适当的控制指令，通过调节电机的转速来稳定飞行器并实现期望的飞行动作。四旋翼飞行器PD控制通过比例和微分控制器的组合，能够实现稳定而高效的飞行姿态控制，为各种应用场景提供了可靠的飞行控制方案。

主函数部分代码：

clc
clear all
close all
global Jtp Ixx Iyy Izz b d l m g Kpz Kdz Kpp Kdp Kpt Kdt Kpps Kdps ZdF PhidF ThetadF PsidF ztime phitime thetatime psitime Zinit Phiinit Thetainit Psiinit Uone Utwo Uthree Ufour Ez Ep Et Eps
% Read the gain values
GainsK = importdata('Gains.mat');
Kpz = GainsK(1,1);  % Height P controller
Kdz = GainsK(1,2);  % Height D controller
Kpp = GainsK(1,3);  % Roll P controller
Kdp = GainsK(1,4);  % Roll D controller
Kpt = GainsK(1,5);  % Pitch P controller
Kdt = GainsK(1,6);  % Pitch D controller
Kpps = GainsK(1,7);  % Yaw P controller
Kdps = GainsK(1,8);  % Yaw D controller
% 
% % % Best Gains of the controllers
% 
% Kpp = 30
% Kdp = 5
% 
% Kpt = 30
% Kdt = 5
% 
% Kpps = 30
% Kdps = 5
% 
% Kpz = 40
% Kdz = 12
% % save gains data
% Gains = [Kpz Kdz Kpp Kdp Kpt Kdt Kpps Kdps];
% save('Gains.mat','Gains') % Write the optimal gains in a .mat file
% Quadrotor constants
Ixx = 8.1*10^(-3);  % Quadrotor moment of inertia around X axis
Iyy = 8.1*10^(-3);  % Quadrotor moment of inertia around Y axis
Izz = 14.2*10^(-3);  % Quadrotor moment of inertia around Z axis
Jtp = 104*10^(-6);  % Total rotational moment of inertia around the propeller axis
b = 54.2*10^(5-6);  % Thrust factor
d = 1.1*10^(-6);  % Drag factor
l = 0.24;  % Distance to the center of the Quadrotor
m = 1;  % Mass of the Quadrotor in Kg
g = 9.81;   % Gravitational acceleration
stepsize = 0.01;
t = 0.00000000000000:stepsize:5.00000000000000; % simulation time
% Initial conditions for the Quadrotor
x0 = [];
for i = 1:12,
    x0 = [x0;0];    % Initial position is the origin O(0,0,0), linear and angular velocities/accelerations = 0
end
% Initial values
Zinit = 0;
Phiinit = pi/3;
Thetainit = pi/2;
Psiinit = -pi/5;
x0(1) = 3; % Xinit
x0(3) = 4; % Yinit
x0(5) = Zinit; % Zinit
x0(7) = Phiinit; % Phiinit
x0(9) = Thetainit; % Thetainit
x0(11) = Psiinit; % Psiinit

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]杨瑞,李国洪,李智,等.基于改进蜣螂优化算法的四旋翼自切换控制[J/OL].天津理工大学学报:1-8[2024-06-09].http://kns.cnki.net/kcms/detail/12.1374.n.20240430.0926.014.html.

[2]黄捷,李泽毅.四旋翼无人机编队变换能耗优化仿真教学[J].实验技术与管理,2024,41(04):102-108.DOI:10.16791/j.cnki.sjg.2024.04.015.