【飛行器】基於matlab多源資訊融合演算法多旋翼無人機組合導航系統【含Matlab原始碼 1267期】

一、四旋翼飛行器簡介

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控制器結構圖

二、部分原始碼

```c clear all

demo_0 = 1; rad = pi/180; deg = 180/pi;

pos = [30pi/180, 120pi/180, 200]'; vn = [10, 10, 10];

att0 = [10, 10, 10]'*rad; v0 = [10, 20, 30]'; % ans = euler2dcm(att0)

b = fir1(20, 0.01, 'low'); b = b/sum(b); % x = repmat([att0; vby]', length(b), 1);

% rv = [10, 2, 1]'pi/180; % sqrt(rv'rv)

wm = [1, 1, 1 2, 2, 2 3, 3, 3 4, 4, 4 5, 5, 5]; %
% wmm = wm(1:2, :) % wm1 = wm(2:3, :) % cross(wmm,wm1, 2) % cs = [ [ 2, 0, 0, 0, 0]/3 % [ 9, 27, 0, 0, 0]/20 % [ 54, 92, 214, 0, 0]/105 % [ 250, 525, 650, 1375, 0]/504 % [2315, 4558, 7296, 7834, 15797]/4620 ]; %
% cs(3, 1:3)*wm(1:3, :) % wm(1:3, :)

% eth = earth(pos, vn) % skew(v0)

% u = [1, 2, 3]'; % u = u/norm(u); % theta = 0.8pi; % % phi1 = theta; % phi2 = -(2pi - theta) ; % % PHI1 = phi1u; % PHI2 = phi2u; % % norm(PHI) % qq1 = rv2q(PHI1)' % qq2 = rv2q(PHI2)' % % PHI1'*PHI1 % rk = [[0.1; 0.1; 0.1]; [10; 10; 10]]'; % Rk = diag(rk)^2;

% R =1 + 0.1.*randn(10000, 1); % mean(R) % var(R) % % clearvars -except R % imu_err = imuerror(); name = 'selfdefine'; exist('name', 'var') && ~strcmp(name, '') && ~strcmp(name, 'zero')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 名稱：Opitiaml Based Alignment (GPS velocity aided) version 1.0 % 功能： % % 縮寫的含義： % msr: measurement; % ref: reference; 特指真實值 % sv: save; % prv: previous; 對應變數在上次解算後的值或在本次解算開始時刻的值 % opt: optimal

% Author: Kun Gan, Tongji University % Email : c[email protected] % Date : 2020/12/1 %% % 全域性變數 close all gvar_earth;

% 下載資料 % 資料包含 % 1. trajectory_ref.(pos, vn, att) % 2. imu.ref.(acc, gyr) % 軌跡模擬程式生成的資料，trajectory的長度會比imu長1。trajectory首個數據為 % t0時刻的avp_ref, imu首個數據為理想imu在t1時刻輸出的(t0, t1]內角度和速 % 度增量。 load('trj_and_imu.mat');

% 與測量資料相關的量 % imu輸出頻率 (Hz) f_imu = 100; ts_imu = 1/f_imu; % gps輸出頻率 (Hz) f_gps = f_imu; ts_gps = 1/f_gps; % 總資料長度 (個) num_imu_data = size(imu_ref.acc, 1);

% 給imu資料新增誤差 imu_err = imuerrorset('selfdefine'); % add imu error [imu_msr.gyr, imu_msr.acc] = imuadderr(imu_ref.gyr, imu_ref.acc, ... imu_err.eb, imu_err.web, imu_err.db, imu_err.wdb, ts_imu);

% 設定全域性變數 % 第一個"讀入"的資料所對應的時刻以及在imu_ref和trajectory_ref中的位置 (s) % 可以認為導航系統是在第start_time秒啟動的 start_time = 0; first_data_index = round(start_time/ts_imu) + 1; % 對準時間長度 (s) total_alignment_time = 92;

% 更新演算法子樣數 (個) num_subsample = 2; nts = num_subsample*ts_imu;

% 初始位置、速度、姿態 pos0 = trajectory_ref.pos(first_data_index, :)'; vn0 = trajectory_ref.vn(first_data_index, :)'; att0 = trajectory_ref.att(first_data_index, :)';

att0_ref = att0; Cbn0_ref = a2mat(att0); q0_ref = a2qua(att0);

% 一些變數在上一時刻的值 % 速度、位置、姿態 % 目前還沒有準備對初始時刻值新增誤差 pos_prv = pos0; vn_prv = vn0; att_prv = att0;

% eth儲存慣導解算需要的變數，例如wien, winn等。 eth_prv = earth(pos_prv, vn_prv);

% bt系和nt系相對慣性空間變化量 Cntn0_prv = eye(3); Cbtb0_prv = eye(3); qbtb0 = [1, 0, 0, 0]';

% 分配動態變數儲存空間 phi_sv = zeros(round(total_alignment_time/ts_imu), 3); phi0_sv = zeros(round(total_alignment_time/ts_imu), 3);

% 計數變數 % 當前時刻 s current = 0; % 當前迴圈時第i次迴圈 i = 0; % index k = 0; %% % 我們的終極目標是讓演算法能夠在實際中執行，實際中情況大概是這樣的： % 1. t=0，慣導開機 % 2. t=ts_imu，imu在此時輸出第一組速度增量vm(1)和角增量wm(1) % 3. t=nts,imu輸出湊足了執行一次慣導更新所需要的子樣數，導航計算機立即執行 % 慣導更新程式並輸出導航結果p(t),vn(t),att(t) % 4. 導航計算機繼續等待imu測得n組測量資訊再進行慣導更新 while current < total_alignment_time
% 每過nts秒，imu就會測量出num_subsample組取樣結果。獲得足夠imu輸出後立 % 即在current時刻（亦稱：當前時刻）進行導航更新 current = current + nts;

% current時刻，載體的位置、速度、姿態參考值
pos_ref = trajectory_ref.pos(k, :)';
vn_ref = trajectory_ref.vn(k, :)';
att_ref = trajectory_ref.att(k, :)';
qbn_ref = a2qua(att_ref);

% 用參考速度模擬當前時刻GPS速度,其中位置資訊暫時不加誤差
vn_gps = vn_ref + 0.*randn(3, 1);
pos_gps = pos_ref;

vn_gps_sv(i, :) = vn_gps;
% 用gps輸出的速度作為組合導航系統給出的速度
vn = vn_gps;
pos = pos_gps;

% 從imu_ref中讀出(current - nts, current]這段時間內imu輸出的n組量測資訊
wm = imu_msr.gyr(k-num_subsample+1 : k, :);
vm = imu_msr.acc(k-num_subsample+1 : k, :);

wm_sv(2*i-1:2*i, :) = wm;
vm_sv(2*i-1:2*i, :) = vm;
% 步長圓錐/划槳誤差
[phim, dvbm] = cnscl(wm, vm);


% *** 計算alpha(n0)和beta(b0) ***
% alpha(n0)
% 1. 目前用vn0_ref計算alpha, 暫時不考慮滑動視窗。
% 2. 用gps輸出的位置計算wien和gn
alpha_sigma = alpha_sigma + ... 
              Cntn0_prv*(cross(eth_prv.wien, vn_prv) - eth_prv.gn)*nts;
alpha = Cntn0*vn - vn0 + alpha_sigma;
% beta(b0)
beta = beta_prv + Cbtb0_prv*dvbm;

% QUEST 方法計算qbn0
[ qbn0, K ] = QUEST( beta, alpha, K_prv );

% 姿態解算
Cbn0 = q2mat(qbn0);
Cbn = Cntn0'*Cbn0*Cbtb0;
att = m2att(Cbn);
phi0_sv(i, :) = atterrnorml(q2att(qbn0) - att0_ref)*deg;
phi_sv(i, :) = atterrnorml(att - att_ref)*deg;

% 計算導航解算時所需要的相關引數
eth = earth(pos, vn);

% 將本次更新後的導航引數作為下次更新初值
pos_prv = pos;
vn_prv = vn;
att_prv = att;
eth_prv = eth;
Cntn0_prv = Cntn0;
Cbtb0_prv = Cbtb0;
alpha_prv = alpha;
beta_prv = beta;
K_prv = K;

end

% 注意：如果end前面加了空格就會出錯! phi_sv(i+1:end, :) = []; phi0_sv(i+1:end, :) = []; %% 繪圖 time_axis = (1:1:i)nts; time_axis_imu = (1:1:2i)*ts_imu;

% Cbn誤差 msplot(311, time_axis, phi_sv(:, 1), 'pitch error / \circ'); msplot(312, time_axis, phi_sv(:, 2), 'roll error / \circ'); msplot(313, time_axis, phi_sv(:, 3), 'yaw error / \circ');

%Cbn0 誤差 msplot(311, time_axis, phi0_sv(:, 1), 'pitch error / \circ'); msplot(312, time_axis, phi0_sv(:, 2), 'roll error / \circ'); msplot(313, time_axis, phi0_sv(:, 3), 'yaw error / \circ');

```

三、執行結果

四、matlab版本及參考文獻

1 matlab版本 2014a

2 參考文獻 [1] 包子陽,餘繼周,楊杉.智慧優化演算法及其MATLAB例項（第2版）[M].電子工業出版社，2016. [2]張巖,吳水根.MATLAB優化演算法原始碼[M].清華大學出版社，2017. [3]張萍.四旋翼飛行器姿態控制建模與模擬[J].電機與控制應用. 2019,46(12) [4]劉巖,楊牧.四旋翼飛行器飛行控制系統研究與設計[J].山東工業技術. 2019,(07)