一、四旋翼飛行器簡介
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控制器結構圖
二、部分源代碼及仿真圖
function varargout = GUI_Config(varargin)
% GUI_CONFIG MATLAB code for GUI_Config.fig
% GUI_CONFIG, by itself, creates a new GUI_CONFIG or raises the existing
% singleton*.
%
% H = GUI_CONFIG returns the handle to a new GUI_CONFIG or the handle to
% the existing singleton*.
%
% GUI_CONFIG('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in GUI_CONFIG.M_VALUE with the given input arguments.
%
% GUI_CONFIG('Property','Value',...) creates a new GUI_CONFIG or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before GUI_Config_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to GUI_Config_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help GUI_Config
% Last Modified by GUIDE v2.5 18-Dec-2020 21:15:22
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @GUI_Config_OpeningFcn, ...
'gui_OutputFcn', @GUI_Config_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
y_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function Izz_value_Callback(hObject, eventdata, handles)
% hObject handle to Izz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of Izz_value as text
% str2double(get(hObject,'String')) returns contents of Izz_value as a double
% --- Executes during object creation, after setting all properties.
function Izz_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to Izz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
% --- Executes on button press in structParamClear.
function structParamClear_Callback(hObject, eventdata, handles)
% hObject handle to structParamClear (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.m_value,'String','');
set(handles.g_value,'String','');
set(handles.L_value,'String','');
set(handles.Ixx_value,'String','');
set(handles.Iyy_value,'String','');
set(handles.Izz_value,'String','');
% --- Executes on button press in structParam_Default.
function structParam_Default_Callback(hObject, eventdata, handles)
% hObject handle to structParam_Default (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.m_value,'String','0.5');
set(handles.g_value,'String','9.8');
set(handles.L_value,'String','0.2');
set(handles.Ixx_value,'String','0.114');
set(handles.Iyy_value,'String','0.114');
set(handles.Izz_value,'String','0.158');
set(handles.progresslabel,'String','結構參數使用預設參數');
function xt_value_Callback(hObject, eventdata, handles)
% hObject handle to xt_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of xt_value as text
% str2double(get(hObject,'String')) returns contents of xt_value as a double
% --- Executes during object creation, after setting all properties.
function xt_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to xt_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function yt_value_Callback(hObject, eventdata, handles)
% hObject handle to yt_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of yt_value as text
% str2double(get(hObject,'String')) returns contents of yt_value as a double
% --- Executes during object creation, after setting all properties.
function yt_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to yt_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function zt_value_Callback(hObject, eventdata, handles)
% hObject handle to zt_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of zt_value as text
% str2double(get(hObject,'String')) returns contents of zt_value as a double
% --- Executes during object creation, after setting all properties.
function zt_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to zt_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
% --- Executes on button press in posParam_clear.
function posParam_clear_Callback(hObject, eventdata, handles)
% hObject handle to posParam_clear (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.xt_value,'String','');
set(handles.yt_value,'String','');
set(handles.zt_value,'String','');
% --- Executes on button press in posParam_Default.
function posParam_Default_Callback(hObject, eventdata, handles)
% hObject handle to posParam_Default (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.xt_value,'String','30');
set(handles.yt_value,'String','50');
set(handles.zt_value,'String','-15');
set(handles.progresslabel,'String','位置參數使用預設參數');
function kpx_value_Callback(hObject, eventdata, handles)
% hObject handle to kpx_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of kpx_value as text
% str2double(get(hObject,'String')) returns contents of kpx_value as a double
% --- Executes during object creation, after setting all properties.
function kpx_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to kpx_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
% --- Executes during object creation, after setting all properties.
function kpy_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to kpy_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function kiy_value_Callback(hObject, eventdata, handles)
% hObject handle to kiy_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of kiy_value as text
% str2double(get(hObject,'String')) returns contents of kiy_value as a double
% --- Executes during object creation, after setting all properties.
function kiy_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to kiy_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function kpz_value_Callback(hObject, eventdata, handles)
% hObject handle to kpz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of kpz_value as text
% str2double(get(hObject,'String')) returns contents of kpz_value as a double
% --- Executes during object creation, after setting all properties.
function kpz_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to kpz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function kiz_value_Callback(hObject, eventdata, handles)
% hObject handle to kiz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of kiz_value as text
% str2double(get(hObject,'String')) returns contents of kiz_value as a double
% --- Executes during object creation, after setting all properties.
function kiz_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to kiz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function kdz_value_Callback(hObject, eventdata, handles)
% hObject handle to kdz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of kdz_value as text
% str2double(get(hObject,'String')) returns contents of kdz_value as a double
% --- Executes during object creation, after setting all properties.
function kdz_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to kdz_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function kpphi_value_Callback(hObject, eventdata, handles)
% hObject handle to kpphi_value (see GCBO)
function kdtheta_value_Callback(hObject, eventdata, handles)
% hObject handle to kdtheta_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of kdtheta_value as text
% str2double(get(hObject,'String')) returns contents of kdtheta_value as a double
% --- Executes during object creation, after setting all properties.
function kdtheta_value_CreateFcn(hObject, eventdata, handles)
% hObject handle to kdtheta_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
set(hObject,'String','');
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function kppsi_value_Callback(hObject, eventdata, handles)
% hObject handle to kppsi_value (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of kppsi_value as text
% str2double(get(hObject,'String')) returns contents of kppsi_value as a double