馮 江，張 慧，張喜海，楊 方

（東北農(nóng)業(yè)大學電氣與信息學院，哈爾濱 150030）

近年來，隨著科技進步，植保無人機逐漸進入國內(nèi)農(nóng)業(yè)植物保護領域[1]，在林業(yè)防火、災后救援、電力巡線、無人機快遞投送、航拍攝影、三維建模等方面應用廣泛[2]。由于植保無人機需在不同地勢作業(yè)，負載較大，電機和螺旋槳需高速旋轉(zhuǎn)才能正常飛行，增加其發(fā)生故障風險[3]。因此，如何在植保無人機發(fā)生故障時，保證飛行姿態(tài)穩(wěn)定為無人機姿態(tài)控制研究熱點問題。當無人機姿態(tài)失衡時，容錯控制為保證無人機姿態(tài)平穩(wěn)關(guān)鍵，故障信息獲取為實現(xiàn)容錯控制有效手段[4]。何志輝等通過自抗擾模糊參數(shù)優(yōu)化控制策略，針對縱列式植保無人機發(fā)生故障問題，設計擴張狀態(tài)觀測器，采用模糊控制在線調(diào)整增益參數(shù)，并驗證所提算法魯棒性，但算法復雜增加飛行器反應時間[5]；張莉采用參數(shù)優(yōu)化方法，分析植保無人機田間作業(yè)工作原理，建立二次性能指標控制模型，保證植保無人機飛控系統(tǒng)性能穩(wěn)定，這種參數(shù)優(yōu)化方法需要一定前置條件，且飛行時外界環(huán)境與飛行器模型參數(shù)實時變化[6]；張紅欣等考慮植保無人機在噴灑農(nóng)藥時，極易受外界因素影響而改變飛行姿態(tài)，研究一種徑向基函數(shù)-比例-積分-微分控制方法，將神經(jīng)網(wǎng)絡、模糊控制、PID控制技術(shù)耦合，確保飛行姿態(tài)實現(xiàn)自適應平衡，此模型需考慮整機作業(yè)軌跡控制等因素，對模型精準度要求較高[7]。

本文基于反步法控制和滑模法控制，結(jié)合飛行器常用IBC 算法、SMC 算法和LQR 算法姿態(tài)控制特性，運用反步滑?？刂品椒ㄔO計容錯控制器，構(gòu)造反步滑模觀測器實時觀測植保無人機故障狀況，對四種算法分別在無故障和有加性故障干擾情況下作仿真對比，并對反步滑模算法開展試驗。

1 植保人機故障模型

圖1所示為植保無人機整體結(jié)構(gòu)，電機1和電機2 逆時針方向轉(zhuǎn)動，電機3 和電機4 順時針方向轉(zhuǎn)動。建立導航坐標系E（xe，ye，ze）、機體坐標系B（xb，yb，zb），其中（φ，θ，ψ）分別為滾轉(zhuǎn)角、俯仰角和偏航角。

圖1 植保無人機結(jié)構(gòu)Fig.1 Plant protection UAV structure

建立動力模型作出假設：

①假設無人機質(zhì)量為均勻分布，飛行時為剛體結(jié)構(gòu)[8]；

②假設無人機重心與質(zhì)心重合；

③假設無人機在行駛過程中不受地球自轉(zhuǎn)、公轉(zhuǎn)及地面效應影響。

從導航坐標系[9]到機體坐標系的旋轉(zhuǎn)變換矩陣如式（1）所示（其中c表示cos，s表示sin）：

由牛頓第二定律可知：

其中，F(xiàn)表示4 個旋翼總升力，即F=F1+F2+F3+F4，其中，F(xiàn)i=，k為升力系數(shù)，a為線加速度，m為四旋翼飛行器質(zhì)量。

得到四旋翼飛行器動態(tài)模型如式（3）所示：

式（3）中，x、y、z分別為導航坐標系3個位置分量；姿態(tài)角表示為φ、θ、ψ，依次為滾轉(zhuǎn)角、俯仰角和偏航角；Jr是旋翼轉(zhuǎn)動慣量；Jx、Jy、Jz依次為3個軸慣性力矩；m為機身重量；g為重力加速度；l為軸距；Ω為電機在同步過程中擾動；Ωi（i=1,2,3,4）表示4 個電機轉(zhuǎn)速。其中

考慮到四旋翼無人機在飛行過程中可能發(fā)生故障[10]，引起執(zhí)行機構(gòu)失效值近似為常數(shù)，將損失表達為各通道輸入量[11]，間接表示為俯仰、橫滾、偏航及高度通道輸入損失，結(jié)合式（3），可將此時狀態(tài)方程改寫為：

在四旋翼無人機動態(tài)方程中，在式（4）基礎上加入加性故障干擾，即為連續(xù)有界的一種加性故障干擾函數(shù)f（ii=1,2,3,4,5,6）。通常在容錯控制時，加性故障是典型描述故障的方法，在系統(tǒng)動態(tài)方程中添加一個干擾性，即添加一個故障函數(shù)[12]，表示在系統(tǒng)中故障的影響。此外，還考慮在飛行時系統(tǒng)受外界環(huán)境干擾影響。因此，在四旋翼動態(tài)模型函數(shù)中依次加入干擾函數(shù)g（ii=1,2,3,4,5,6），建立如式（5）所示四旋翼故障模型：

2 控制器設計

2.1 狀態(tài)觀測器簡化模型

電機、電調(diào)、螺旋槳失靈或未知環(huán)境因素干擾的不確定使執(zhí)行機構(gòu)發(fā)生加性故障，引發(fā)系統(tǒng)執(zhí)行機構(gòu)失效值近似為常數(shù)，將這種失效值間接轉(zhuǎn)為姿態(tài)角通道及高度通道的控制輸入損失[13]。其中（xd，yd，zd）和（x，y，z）依次分別為無人機位置的期望值和實際值，（φd，θd，ψd）和（φ，θ，ψ）依次為無人機姿態(tài)的期望值和實際值，（U1，U2，U3，U4）為控制觀測系統(tǒng)控制量。首先對系統(tǒng)故障模型作簡化處理，令系統(tǒng)控制輸入量為：

狀態(tài)變量為：

x為狀態(tài)變量，Y為輸出變量，u為輸入變量。

則四旋翼無人機模型簡化后，帶有加性故障的執(zhí)行器狀態(tài)空間矩陣如下：

式（9）中，a1=（Jy-Jz）/Jx，a2=（Jz-Jx）/Jy，a3=（Jx-Jy）/Jz，b1=l/Jx，b2=l/Jy，b3=1/Jz。

2.2 線性二次型調(diào)節(jié)器

線性二次型調(diào)節(jié)器在先行動態(tài)環(huán)節(jié)計算量小、硬件實現(xiàn)容易。通過構(gòu)造中間控制量，四旋翼系統(tǒng)可由線性動態(tài)和非線性靜態(tài)兩個環(huán)節(jié)組成[14]，對線性動態(tài)環(huán)節(jié)設計線性二次型調(diào)節(jié)器[15-16]，并設計LQR。解算非線性靜態(tài)環(huán)節(jié)，分配到實際系統(tǒng)中控制輸入通道。

將式（8）離散化，得如式（10）所示離散系統(tǒng)：

其中，Al，Bl和Cl分別為離散時間的狀態(tài)、輸入和輸出矩陣。“k”表示離散時間步長，k取整數(shù)。

LQR 的最優(yōu)控制時求反饋控制律[17]，令u=-kx，在區(qū)間[t,∞]中系統(tǒng)從非平衡狀態(tài)到零點附近。其性能指標為：

其中，Q和R分別為半正定與正定實對稱常數(shù)矩陣，且Q為狀態(tài)變量加權(quán)矩陣，R為控制變量加權(quán)矩陣。性能指標取極小值，根據(jù)Hamilton方程有：

最優(yōu)控制律為：

可得：

其中λ=Px。

本文使用Matlab 利用LQR（A,B,Q,R）函數(shù)直接對反饋系數(shù)求解，并驗證LQR（A,B,Q,R）函數(shù)系統(tǒng)性能。

2.3 反步法控制器設計

反步法控制被廣泛用于非線性系統(tǒng)，核心思想是將復雜系統(tǒng)劃分為多個子系統(tǒng)，且子系統(tǒng)數(shù)量不大于整個系統(tǒng)階數(shù)，減少計算時間[18]。每個子系統(tǒng)通過中間虛擬量或Lyapunov 函數(shù)作遞歸構(gòu)造，解決非線性因素的不確定性影響[19]。

以滾動通道為例，建立系統(tǒng)動態(tài)方程：

式（15）中，f4表示一個有界故障，g4作為一個常數(shù)，表示外界干擾，為限定條件，ρ3為外界干擾變化量上界。

狀態(tài)觀測器模型可簡化為：

步驟一：取選定的Lyapunov函數(shù)V1：

根據(jù)Lyapunov 穩(wěn)定理論分析可得，由于V.

1 ＜0，為此增加其虛擬控制量，使其作為e1子系統(tǒng)的虛擬控制，另（c1＞0為可調(diào)參數(shù)），則x2誤差e2為：

對式（17）求導并結(jié)合式（18）可得：

步驟二：構(gòu)造增廣Lyapunov函數(shù)V2：

式（20）中，ef=f4-ε4，其中f4表示已知滾轉(zhuǎn)角通道執(zhí)行器加性故障值，但f4值無法實際測量，在此使用ε4為估計值，定義積分項如下式：

對式（20）求導易知V.2＜0，說明步驟二設計中系統(tǒng)漸近收斂，則：

2.4 滑模控制器設計

基于反步法設計的控制器仍有不足，其被控對象要建立精準的數(shù)學模型。與反步法控制相比，滑?？刂莆锢砗唵吻铱刂菩阅軆?yōu)良。運動軌線被強迫為滑動流行狀態(tài)，并保持一段時間?；Ｗ兘Y(jié)構(gòu)控制與常規(guī)控制相比具有不連續(xù)控制特點，這種方式促使系統(tǒng)按照既定軌跡作高頻率、小幅度震蕩運動，為“滑?！边\動[20]。滑模運動的滑動模態(tài)可認為設計與外部環(huán)境參數(shù)變化及系統(tǒng)內(nèi)部環(huán)境變化無關(guān)，具有良好魯棒性。如果將反步法控制與滑模法控制結(jié)合，則可增大控制系統(tǒng)使用范圍，對于控制律設計的魯棒性較好。本文依據(jù)滑模變結(jié)構(gòu)原理，采用指數(shù)趨近控制律改變趨近運動的動態(tài)特性。具體設計如下：

與反步法控制一致，以滾轉(zhuǎn)通道為例，系統(tǒng)動態(tài)方程為：

取滑模趨近率為：

滑模控制易出現(xiàn)抖動，將sgn（s）函數(shù)改為sat（s）飽和函數(shù)可減少系統(tǒng)運行過程中抖動。

對應切換面為：

2.5 反步滑?？刂破髟O計

由于四旋翼具有欠驅(qū)動、強耦合控制特點，根據(jù)式（4）表示的故障模型方程設計容錯控制器總體容錯控制算法如圖2 所示，控制結(jié)構(gòu)包括位置控制和姿態(tài)控制，其中姿態(tài)控制期望的偏航角ψd被設定為已知，而期望的俯仰角φd、滾轉(zhuǎn)角θd需經(jīng)位置控制得到；通過姿態(tài)控制得到的通道控制量U2、U3、U4和高度通道控制量U1分配給執(zhí)行機構(gòu)控制輸入，完成四旋翼無人機控制。對于容錯控制器設計分兩步。首先通過滑動控制方法構(gòu)建滑模觀測器，有利于系統(tǒng)穩(wěn)定性分析；其次結(jié)合反步法控制，研究執(zhí)行器故障時容錯控制器的設計。

圖2 容錯控制結(jié)構(gòu)Fig.2 Fault-tolerant control structure

參照上述反步法控制器設計與滑模法控制器設計，則反步滑?？刂扑惴ㄝ斎肟刂茷椋?/p>

因此，其他通道電機輸入控制為：

3 仿真驗證與物理試驗

在Matlab/Simulink環(huán)境下對所設計的控制算法分別在無故障干擾和有加性故障干擾情況下開展仿真驗證。假使四旋翼無人機初始姿態(tài)為[0°0°0°]，期望姿態(tài)為[80°40°60°]，表1為四旋翼參數(shù)。

表1 植保無人機模型參數(shù)Table 1 Plant protection UAV model parameter

3.1 無干擾仿真試驗

首先，根據(jù)以上模型建立，設計相關(guān)仿真。表1所示為仿真參數(shù)。根據(jù)線性二次型算法中，通過多次試湊得到控制器Q和R分別為：

得出狀態(tài)反饋矩陣為：

在無故障干擾條件下，仿真3 種結(jié)果如圖3～5所示。

基于搭建的四旋翼無人機模型，圖3為4種控制算法下四旋翼無人機姿態(tài)響應曲線，圖4為4種控制算法下四旋翼無人機飛行速率仿真曲線，圖5是4 種控制算法下四旋翼無人機電機速率仿真曲線。圖3、4 響應曲線分別給出四旋翼橫滾、俯仰和偏航3 個方向響應狀態(tài)，圖5 給出4 個電機響應情況，得出以下分析結(jié)果。

圖5 LQR、SMC、IBC、BSC算法無干擾電機速率仿真結(jié)果Fig.5 LQR,SMC,IBC,BSC algorithm interference-free motor speed simulation results

由圖3可知，對于四旋翼系統(tǒng)，在受到相同系統(tǒng)輸入時，3種方法均可在一段時間趨于穩(wěn)定，具有良好控制效果。圖3a 中LQR 滾轉(zhuǎn)角收斂時間為200 s，圖3b中SMC滾轉(zhuǎn)角收斂時間為200 s，圖3c中IBC滾轉(zhuǎn)角收斂時間為200 s。3個控制算法穩(wěn)定時間一致，均可在短時間達到快速穩(wěn)定狀態(tài)。通過仿真曲線可知在趨于穩(wěn)定過程中，LQR和IBC響應均未出現(xiàn)振蕩，SMC算法有較小抖動。

圖3 LQR、SMC、IBC、BSC算法無干擾姿態(tài)仿真結(jié)果Fig.3 LQR,SMC,IBC,BSC algorithm interference-free attitude simulation results

圖4 中四旋翼飛行速率響應曲線均在200 s 時趨于0，響應時間及各通道最大響應角速度見表2，SMC算法最先到達最大角速度，以滾轉(zhuǎn)角為例，最大響應為1.121 deg·s-1。IBC算法在82 s時角速度達到峰值，滾轉(zhuǎn)角角速度峰值為0.08583 deg·s-1。

圖4 LQR、SMC、IBC、BSC算法無干擾飛行速率仿真結(jié)果Fig.4 LQR,SMC,IBC,BSC algorithm interference-free flight rate simulation results

圖5表示四旋翼每個電機角速度響應曲線，每種算法電機最大響應值如表3 所示，3 種算法電機轉(zhuǎn)速在78、72、82 s處達到響應峰值，這3個時間點分別對應表2中姿態(tài)響應時間，此時達到四旋翼最大響應峰值，電機轉(zhuǎn)速變化較大，以求快速跟上期望軌跡。

表2 3種算法姿態(tài)響應峰值參數(shù)Table 2 Three algorithms attitude response peak paremeter

表3 3種算法電機姿態(tài)響應峰值參數(shù)Table 3 Three algorithms motor attitude response peak paremeter

3.2 有界干擾仿真試驗

在實際飛行環(huán)境中，四旋翼無人機除系統(tǒng)本身出現(xiàn)故障，還會遇到外界干擾，如外界陣風等。在仿真模型中，于150 s 時加入階躍響應，驗證算法有效性。圖6～8為仿真結(jié)果，由此得到分析結(jié)果：

①由圖6～8 可知，在階躍響應信號干擾下，四旋翼無人機姿態(tài)響應、飛行速率、電機反作用力均受到一定程度影響，圖6～圖8a 中LQR 干擾試驗四旋翼姿態(tài)響應未收斂于平衡狀態(tài)；

②從圖6～圖8b 中，SMC 算法在150 s 時加入干擾，系統(tǒng)幾乎未受影響，可快速恢復到平衡狀態(tài)，魯棒性較好，但滾轉(zhuǎn)角在開始時有較小抖動；

圖8 LQR、SMC、IBC、BSC算法干擾下電機速率仿真結(jié)果Fig.8 Motor speed simulation results under the interference of LQR,SMC,IBC,and BSC algorithms

③同理，IBC 試驗在150 s 時加入干擾，系統(tǒng)姿態(tài)角變化較小，俯仰角、滾轉(zhuǎn)角、偏航角所受干擾影響較小，姿態(tài)響應產(chǎn)生較小抖動，電機響應在150 s后產(chǎn)生抖動，在100 s內(nèi)趨于穩(wěn)定，調(diào)節(jié)時間短，偏差小，魯棒性較好。

3.3 反步滑模算法驗證與物理試驗

基于以上3種算法試驗結(jié)果可得知，在無外界干擾情況下，3種算法均可快速達到平衡狀態(tài)；在加入外界干擾條件下，3 種算法均產(chǎn)生一定偏差，將反步法控制與滑模法控制方法結(jié)合，在Matlab/Simulink 下驗證反步滑?？刂葡到y(tǒng)，結(jié)果如圖3～8所示。

由圖3～5可知，在無干擾仿真環(huán)境下，反步滑模法可達到快速平穩(wěn)狀態(tài)，達到期望的姿態(tài)角；由圖6～8 可知，同樣在300 s 時加入階躍響應故障干擾信號，姿態(tài)角幾乎未受影響，無抖動和超調(diào)量，電機反作用姿態(tài)曲線較平穩(wěn)，可知，反步滑?？刂凭哂屑皶r響應特性與良好魯棒性，有較強容錯控制能力。

圖6 LQR、SMC、IBC、BSC算法干擾下姿態(tài)仿真結(jié)果Fig.6 Attitude simulation results under the interference of LQR,SMC,IBC,and BSC algorithms

圖9為將反步滑模算法寫入飛行控制器開展現(xiàn)場試飛試驗，在反步滑模控制系統(tǒng)下，植保無人機平穩(wěn)飛行，在加性故障環(huán)境下無人機受到影響較小，魯棒性較好。

圖9 BSC飛行試驗Fig.9 BSC flight test

通過Pixhawk飛行控制器MPU6050模塊可獲得飛行角速度和角速度數(shù)據(jù)，下圖是飛行過程中姿態(tài)曲線圖。從圖10中曲線1與曲線2可知，兩條線重合度較高，滾轉(zhuǎn)角目標值與實際值接近，驗證BSC算法性能較高，使得植保無人機很快達到預期角度。圖11為飛行控制器IMU姿態(tài)信息，由曲線3數(shù)值與曲線4 數(shù)值可知，AccX、AccY 兩個軸的姿態(tài)誤差盡在±2 m·s-2，飛行控制較穩(wěn)定，震動不明顯，BSC算法有良好的魯棒性。

圖10 BSC飛行姿態(tài)跟蹤曲線圖Fig.10 BSC flight attitude tracking curve

圖11 BSC飛行試驗IMU誤差Fig.11 BSC flight test IMU error

圖7 LQR、SMC、IBC、BSC算法干擾下飛行速率仿真結(jié)果Fig.7 Flight rate simulation results under the interference of LQR,SMC,IBC,and BSC algorithms

4 結(jié)論

針對植保無人機系統(tǒng)模型不確定或存在外界環(huán)境干擾情況下，導致作業(yè)時植保效率低下問題，對常用的3種控制算法開展仿真試驗。

分析中得知，LQR 控制算法在受到加性故障干擾后飛行器姿態(tài)無法保證快速恢復到平衡穩(wěn)定狀態(tài)；SMC 算法當滑動的軌跡到達滑模面后，易產(chǎn)生抖動；BSC算法在設計控制律的過程中對系統(tǒng)反饋性能要求較嚴格，算法復雜。

通過將反步法控制與滑模法控制結(jié)合，并驗證IBC算法有效性。從仿真實驗分析得知，反步滑模算法結(jié)合反步法控制與滑模法控制的性能，可以全局優(yōu)化系統(tǒng)參數(shù)，并自適應調(diào)整加權(quán)矩陣，在加入外界干擾的條件下電機的姿態(tài)較為穩(wěn)定，并能快速趨于穩(wěn)定狀態(tài)；從植保無人機現(xiàn)場的飛行情況可知，植保無人機使用反步法模算法，姿態(tài)的軌跡跟蹤性好，響應及時，遇到外界風力干擾時容錯控制性強，魯棒性良好，保證植保無人機能夠有效作業(yè)，提高無人機植保的工作效率。