基于反步滑模算法的植保無人機(jī)姿態(tài)控制研究

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

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

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

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

1 植保人機(jī)故障模型

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

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

建立動力模型作出假設(shè)：

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

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

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

從導(dǎo)航坐標(biāo)系[9]到機(jī)體坐標(biāo)系的旋轉(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分別為導(dǎo)航坐標(biāo)系3個位置分量；姿態(tài)角表示為φ、θ、ψ，依次為滾轉(zhuǎn)角、俯仰角和偏航角；Jr是旋翼轉(zhuǎn)動慣量；Jx、Jy、Jz依次為3個軸慣性力矩；m為機(jī)身重量；g為重力加速度；l為軸距；Ω為電機(jī)在同步過程中擾動；Ωi（i=1,2,3,4）表示4 個電機(jī)轉(zhuǎn)速。其中

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

在四旋翼無人機(jī)動態(tài)方程中，在式（4）基礎(chǔ)上加入加性故障干擾，即為連續(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 控制器設(shè)計

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

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

狀態(tài)變量為：

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

則四旋翼無人機(jī)模型簡化后，帶有加性故障的執(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é)計算量小、硬件實(shí)現(xiàn)容易。通過構(gòu)造中間控制量，四旋翼系統(tǒng)可由線性動態(tài)和非線性靜態(tài)兩個環(huán)節(jié)組成[14]，對線性動態(tài)環(huán)節(jié)設(shè)計線性二次型調(diào)節(jié)器[15-16]，并設(shè)計LQR。解算非線性靜態(tài)環(huán)節(jié)，分配到實(shí)際系統(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)到零點(diǎn)附近。其性能指標(biāo)為：

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

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

可得：

其中λ=Px。

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

2.3 反步法控制器設(shè)計

反步法控制被廣泛用于非線性系統(tǒng)，核心思想是將復(fù)雜系統(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）求導(dǎo)并結(jié)合式（18）可得：

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

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

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

2.4 滑模控制器設(shè)計

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

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

取滑模趨近率為：

滑?？刂埔壮霈F(xiàn)抖動，將sgn（s）函數(shù)改為sat（s）飽和函數(shù)可減少系統(tǒng)運(yùn)行過程中抖動。

對應(yīng)切換面為：

2.5 反步滑模控制器設(shè)計

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

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

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

因此，其他通道電機(jī)輸入控制為：

3 仿真驗(yàn)證與物理試驗(yàn)

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

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

3.1 無干擾仿真試驗(yàn)

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

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

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

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

圖5 LQR、SMC、IBC、BSC算法無干擾電機(jī)速率仿真結(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)定時間一致，均可在短時間達(dá)到快速穩(wěn)定狀態(tài)。通過仿真曲線可知在趨于穩(wěn)定過程中，LQR和IBC響應(yīng)均未出現(xiàn)振蕩，SMC算法有較小抖動。

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

圖4 中四旋翼飛行速率響應(yīng)曲線均在200 s 時趨于0，響應(yīng)時間及各通道最大響應(yīng)角速度見表2，SMC算法最先到達(dá)最大角速度，以滾轉(zhuǎn)角為例，最大響應(yīng)為1.121 deg·s-1。IBC算法在82 s時角速度達(dá)到峰值，滾轉(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表示四旋翼每個電機(jī)角速度響應(yīng)曲線，每種算法電機(jī)最大響應(yīng)值如表3 所示，3 種算法電機(jī)轉(zhuǎn)速在78、72、82 s處達(dá)到響應(yīng)峰值，這3個時間點(diǎn)分別對應(yīng)表2中姿態(tài)響應(yīng)時間，此時達(dá)到四旋翼最大響應(yīng)峰值，電機(jī)轉(zhuǎn)速變化較大，以求快速跟上期望軌跡。

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

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

3.2 有界干擾仿真試驗(yàn)

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

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

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

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

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

3.3 反步滑模算法驗(yàn)證與物理試驗(yàn)

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

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

圖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)場試飛試驗(yàn)，在反步滑?？刂葡到y(tǒng)下，植保無人機(jī)平穩(wěn)飛行，在加性故障環(huán)境下無人機(jī)受到影響較小，魯棒性較好。

圖9 BSC飛行試驗(yàn)Fig.9 BSC flight test

通過Pixhawk飛行控制器MPU6050模塊可獲得飛行角速度和角速度數(shù)據(jù)，下圖是飛行過程中姿態(tài)曲線圖。從圖10中曲線1與曲線2可知，兩條線重合度較高，滾轉(zhuǎn)角目標(biāo)值與實(shí)際值接近，驗(yàn)證BSC算法性能較高，使得植保無人機(jī)很快達(dá)到預(yù)期角度。圖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飛行試驗(yàn)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é)論

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

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

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