【飞行器】基于matlab四旋翼无人机控制仿真【含Matlab源码 2238期】

⛄一、四旋翼飞行器简介

0 引言
四旋翼飞行器由于具有可垂直起降、机动性强、操作方便等诸多优点,在军事和民用场合得到广泛应用,从而成为众多学者的研究热点。四旋翼飞行器是具有四输入、六输出的欠驱动、非线性、强耦合系统。其姿态控制精度和抗干扰问题一直是研究重点。目前国内较普遍的飞行器控制算法主要包括:反步法、自适应控制、H控制、滑模控制、自抗扰控制等,对实现四旋翼飞行器的姿态控制具有重要的理论和实践意义。文献提出应用反步算法为飞行器上下、前后、左右、偏航4个子系统配置控制律,实现了四旋翼飞行器对设定轨迹的精确跟踪。该算法在构造Lyapunov函数的过程中,是以其导数小于零为前提,因此应用受到局限。文献针对传统的离散线性滑模应用于四旋翼飞行器控制具有跟踪误差大、响应速度慢、不能有限时间收敛等问题,提出了干扰观测器补偿的自适应离散终端滑模控制,使响应时间更快、跟踪效果更理想、鲁棒性更强。文献利用线性扩张状态观测器对四旋翼飞行器内部不确定干扰和外部干扰进行实时估计,进而采取线性状态反馈控制对扰动的估计值进行在线补偿,以实现四旋翼飞行器的姿态控制。

1 四旋翼飞行器动力学模型的建立
1. 1 四旋翼飞行器受力分析
对于飞行器的每个旋翼,剖面呈非对称,一旦旋翼旋转,由于上表面空气流速比下表面快,故上表面受到的空气压力小于下表面,上下表面受到的压差形成升力,如图1所示。旋翼1、3逆时针旋转,旋翼2、4顺时针旋转。由叶素动量理论可知,每个旋翼产生的升力Fi与电机转速ωi的平方成正比,即Fi=kFω2i(i=1,2,3,4),其中kF为升力系数。
在这里插入图片描述
图1 四旋翼飞行器受力分析图
当旋翼旋转时,空气阻力会阻碍其旋转。这种阻力形成施加在机体上的反扭转力矩,当4个旋翼转速相等时,旋翼产生的反扭矩作用相互抵消。四旋翼飞行器通过改变2对正反螺旋桨的转速实现对其运动控制。在4个旋翼转速相等的情况下,同时增加或者减少4个旋翼转速,可以实现飞行器上升或者下降。如果4个旋翼产生的升力之和等于机体重力,可以实现飞行器空中悬停。保持旋翼2、4的转速不变,改变旋翼1或者旋翼3的转速,飞行器在力矩l(F3-F1)或l(F1-F3)的作用下(其中l为电机轴线到飞行器中心距离),可实现俯仰运动。保持旋翼1和旋翼3的转速不变,改变旋翼2或者旋翼4的转速,飞行器在力矩l(F4-F2)或l(F2-F4)的作用下,可实现横滚运动。如果同时改变旋翼1和旋翼3的转速,或者同时改变旋翼2和旋翼4的转速,并保持飞行器总升力与机体重力相等,飞行器会在反扭矩的作用下实现偏航运动。由此可见,实现飞行器垂直运动的升力,以及实现俯仰、横滚、偏航运动的旋转力矩可以表示为
在这里插入图片描述
式中: F——飞行器垂直运动的升力;
Mx、My、Mz——四旋翼俯仰、横滚和偏航运动的旋转力矩;
kF——升力系数;
kM——旋转力矩系数。

1. 2 动力学模型建立
为了描述飞行器的姿态和运动状态,需要引入地理坐标系n(X,Y,Z)与载体坐标系b(x,y,z)。地理坐标系又称为东北天坐标系,载体坐标系与飞行器固连,原点为飞行器中心。将地理坐标系与载体坐标系原点重合,并将地理坐标系分别绕X、Y、Z轴旋转3次之后可得到载体坐标系,地理坐标系到载体坐标系的转换矩阵可表示为
在这里插入图片描述

由式(5)和式(2)可求得地理坐标系n中飞行器在X、Y、Z轴方向所受旋翼升力向量:

在低速飞行过程中,角速度矢量较小,式(8)中左边第二项可近似认为为零,则式(8)可化简为

将式(10)转换可得:

由式(7)和式(11)可得四旋翼飞行器在低速飞行情况下的非线性动力学模型:
在这里插入图片描述
由式(12)可知,四旋翼飞行器线运动不影响角运动,但是角运动会影响线运动。以u1、u2、u3、u4为系统输入,通过改变这4个输入变量的值,可以改变飞行器的3个线位移和3个角位移,从而实现对飞行器的运动控制。

2 四旋翼飞行器的控制系统构建与仿真
经典PID算法结构简单,基于偏差设计反馈律,不依赖受控对象的具体数学模型,在很多过程控制中均有良好表现。尽管各种新的控制算法不断涌现,但是并没有改变PID控制算法在工业控制中的主导地位。本文根据四旋翼在飞行过程中经常会遇到不确定外界干扰等情况,设计了基于小扰动的PID控制器,如图2所示。
在这里插入图片描述
图2 PID控制器结构图

⛄二、部分源代码

%% Clear Workspace
close all
clear all
clc

addpath(‘utils’)
addpath(‘trajectories’)

OUTPUT_TO_VIDEO = 0;
videoName = ‘circle’;

%% Reference Trajectory
% Select the Wanted Trajectory

% trajhandle = @hoverTrajectory;
% trajhandle = @circleTrajectory;
trajhandle = @diamondTrajectory;

%% Regulator
controlhandle = @controller;

%% **************************** FIGURES *****************************
fprintf(‘Initializing figures…\n’)
h_fig = figure;
h_3d = gca;
axis equal;
grid on;
view(3);
xlabel(‘x [m]’); ylabel(‘y [m]’); zlabel(‘z [m]’)
quadcolors = lines(1);

set(gcf,‘Renderer’,‘OpenGL’)

%% *********************** INITIAL CONDITIONS ***********************
fprintf(‘Setting initial conditions…\n’)
real_time = true;
params = crazyflie();
time_tol = 5000; % max time
maxIter = 5000; % max iteration
starttime = 0; % start of simulation in seconds
tstep = 0.01; % this determines the time step at which the solution is given
cstep = 0.05; % image capture time interval
nstep = cstep/tstep;
time = starttime; % current time
err = []; % runtime errors

% Get start and stop position
startTraj = trajhandle(0);
stopTraj = trajhandle(inf);
stopPose = stopTraj.pos;
x0 = initializeState(startTraj.pos, 0);
xtraj = zeros(maxIternstep, length(x0));
ttraj = zeros(maxIternstep, 1);
x = x0;

pos_tol = 0.01;
vel_tol = 0.01;

%% ************************* RUN SIMULATION *************************
if OUTPUT_TO_VIDEO == 1
v = VideoWriter(videoName);
open(v);
end

fprintf(‘Simulation Running…\n’)
% Main loop
for iter = 1:maxIter
tic;

timeint = time:tstep:time+cstep;
% Initialize quad plot
if iter == 1QP = QuadPlot(1, x0, 0.1, 0.04, quadcolors(1,:), maxIter, h_3d);desiredState = trajhandle(time);QP.UpdateQuadPlot(x, [desiredState.pos; desiredState.vel], time);h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time));
end% Run simulation
[tsave, xsave] = ode45(@(t,s) quadEOM(t, s, 1, controlhandle, trajhandle, params), timeint, x);
x = xsave(end, :)';% Save to traj
xtraj((iter-1)*nstep+1:iter*nstep,:) = xsave(1:end-1,:);
ttraj((iter-1)*nstep+1:iter*nstep) = tsave(1:end-1);% Update quad plot
desiredState = trajhandle(time + cstep);
QP.UpdateQuadPlot(x, [desiredState.pos; desiredState.vel], time + cstep);
set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep))
if OUTPUT_TO_VIDEO == 1im = frame2im(getframe(gcf));writeVideo(v,im);
endtime = time + cstep; % Update simulation time
t = toc;% Check to make sure ode45 is not timing out
if(t > cstep*50)err = 'Ode45 Unstable';break;
end% Pause to make real-time
if real_time && (t < cstep)pause(cstep - t);
end% Check termination criteria
if terminateCheck(x, time, stopPose, pos_tol, vel_tol, time_tol)break
end

end

if OUTPUT_TO_VIDEO == 1
close(v);
end

%% ************************* POST PROCESSING *************************
% Truncate xtraj and ttraj
xtraj = xtraj(1:iternstep,:);
ttraj = ttraj(1:iternstep);

% Plot the saved position and velocity of each robot
% Truncate saved variables
QP.TruncateHist();
% Plot position for each quad
h_pos = figure(‘Name’, ’ position’);
plotState(h_pos, QP.state_hist(1:3,:), QP.time_hist, ‘pos’, ‘vic’);
plotState(h_pos, QP.state_des_hist(1:3,:), QP.time_hist, ‘pos’, ‘des’);
% Plot velocity for each quad
h_vel = figure(‘Name’, ‘velocity’);
plotState(h_vel, QP.state_hist(4:6,:), QP.time_hist, ‘vel’, ‘vic’);
plotState(h_vel, QP.state_des_hist(4:6,:), QP.time_hist, ‘vel’, ‘des’);

if(~isempty(err))
error(err);
end

fprintf(‘Finished!\n’)