馮 江,張 慧,張喜海,楊 方
(東北農(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ī)需在不同地勢(shì)作業(yè),負(fù)載較大,電機(jī)和螺旋槳需高速旋轉(zhuǎn)才能正常飛行,增加其發(fā)生故障風(fēng)險(xiǎn)[3]。因此,如何在植保無人機(jī)發(fā)生故障時(shí),保證飛行姿態(tài)穩(wěn)定為無人機(jī)姿態(tài)控制研究熱點(diǎn)問題。當(dāng)無人機(jī)姿態(tài)失衡時(shí),容錯(cuò)控制為保證無人機(jī)姿態(tài)平穩(wěn)關(guān)鍵,故障信息獲取為實(shí)現(xiàn)容錯(cuò)控制有效手段[4]。何志輝等通過自抗擾模糊參數(shù)優(yōu)化控制策略,針對(duì)縱列式植保無人機(jī)發(fā)生故障問題,設(shè)計(jì)擴(kuò)張狀態(tài)觀測(cè)器,采用模糊控制在線調(diào)整增益參數(shù),并驗(yàn)證所提算法魯棒性,但算法復(fù)雜增加飛行器反應(yīng)時(shí)間[5];張莉采用參數(shù)優(yōu)化方法,分析植保無人機(jī)田間作業(yè)工作原理,建立二次性能指標(biāo)控制模型,保證植保無人機(jī)飛控系統(tǒng)性能穩(wěn)定,這種參數(shù)優(yōu)化方法需要一定前置條件,且飛行時(shí)外界環(huán)境與飛行器模型參數(shù)實(shí)時(shí)變化[6];張紅欣等考慮植保無人機(jī)在噴灑農(nóng)藥時(shí),極易受外界因素影響而改變飛行姿態(tài),研究一種徑向基函數(shù)-比例-積分-微分控制方法,將神經(jīng)網(wǎng)絡(luò)、模糊控制、PID控制技術(shù)耦合,確保飛行姿態(tài)實(shí)現(xiàn)自適應(yīng)平衡,此模型需考慮整機(jī)作業(yè)軌跡控制等因素,對(duì)模型精準(zhǔn)度要求較高[7]。
本文基于反步法控制和滑模法控制,結(jié)合飛行器常用IBC 算法、SMC 算法和LQR 算法姿態(tài)控制特性,運(yùn)用反步滑??刂品椒ㄔO(shè)計(jì)容錯(cuò)控制器,構(gòu)造反步滑模觀測(cè)器實(shí)時(shí)觀測(cè)植保無人機(jī)故障狀況,對(duì)四種算法分別在無故障和有加性故障干擾情況下作仿真對(duì)比,并對(duì)反步滑模算法開展試驗(yàn)。
圖1所示為植保無人機(jī)整體結(jié)構(gòu),電機(jī)1和電機(jī)2 逆時(shí)針方向轉(zhuǎn)動(dòng),電機(jī)3 和電機(jī)4 順時(shí)針方向轉(zhuǎn)動(dòng)。建立導(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
建立動(dòng)力模型作出假設(shè):
①假設(shè)無人機(jī)質(zhì)量為均勻分布,飛行時(shí)為剛體結(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 個(gè)旋翼總升力,即F=F1+F2+F3+F4,其中,F(xiàn)i=,k為升力系數(shù),a為線加速度,m為四旋翼飛行器質(zhì)量。
得到四旋翼飛行器動(dòng)態(tài)模型如式(3)所示:
式(3)中,x、y、z分別為導(dǎo)航坐標(biāo)系3個(gè)位置分量;姿態(tài)角表示為φ、θ、ψ,依次為滾轉(zhuǎn)角、俯仰角和偏航角;Jr是旋翼轉(zhuǎn)動(dòng)慣量;Jx、Jy、Jz依次為3個(gè)軸慣性力矩;m為機(jī)身重量;g為重力加速度;l為軸距;Ω為電機(jī)在同步過程中擾動(dòng);Ωi(i=1,2,3,4)表示4 個(gè)電機(jī)轉(zhuǎn)速。其中
考慮到四旋翼無人機(jī)在飛行過程中可能發(fā)生故障[10],引起執(zhí)行機(jī)構(gòu)失效值近似為常數(shù),將損失表達(dá)為各通道輸入量[11],間接表示為俯仰、橫滾、偏航及高度通道輸入損失,結(jié)合式(3),可將此時(shí)狀態(tài)方程改寫為:
在四旋翼無人機(jī)動(dòng)態(tài)方程中,在式(4)基礎(chǔ)上加入加性故障干擾,即為連續(xù)有界的一種加性故障干擾函數(shù)f(ii=1,2,3,4,5,6)。通常在容錯(cuò)控制時(shí),加性故障是典型描述故障的方法,在系統(tǒng)動(dòng)態(tài)方程中添加一個(gè)干擾性,即添加一個(gè)故障函數(shù)[12],表示在系統(tǒng)中故障的影響。此外,還考慮在飛行時(shí)系統(tǒng)受外界環(huán)境干擾影響。因此,在四旋翼動(dòng)態(tài)模型函數(shù)中依次加入干擾函數(shù)g(ii=1,2,3,4,5,6),建立如式(5)所示四旋翼故障模型:
電機(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)為控制觀測(cè)系統(tǒng)控制量。首先對(duì)系統(tǒng)故障模型作簡(jiǎn)化處理,令系統(tǒng)控制輸入量為:
狀態(tài)變量為:
x為狀態(tài)變量,Y為輸出變量,u為輸入變量。
則四旋翼無人機(jī)模型簡(jiǎn)化后,帶有加性故障的執(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。
線性二次型調(diào)節(jié)器在先行動(dòng)態(tài)環(huán)節(jié)計(jì)算量小、硬件實(shí)現(xiàn)容易。通過構(gòu)造中間控制量,四旋翼系統(tǒng)可由線性動(dòng)態(tài)和非線性靜態(tài)兩個(gè)環(huán)節(jié)組成[14],對(duì)線性動(dòng)態(tài)環(huán)節(jié)設(shè)計(jì)線性二次型調(diào)節(jié)器[15-16],并設(shè)計(jì)LQR。解算非線性靜態(tài)環(huán)節(jié),分配到實(shí)際系統(tǒng)中控制輸入通道。
將式(8)離散化,得如式(10)所示離散系統(tǒng):
其中,Al,Bl和Cl分別為離散時(shí)間的狀態(tài)、輸入和輸出矩陣?!発”表示離散時(shí)間步長(zhǎng),k取整數(shù)。
LQR 的最優(yōu)控制時(shí)求反饋控制律[17],令u=-kx,在區(qū)間[t,∞]中系統(tǒng)從非平衡狀態(tài)到零點(diǎn)附近。其性能指標(biāo)為:
其中,Q和R分別為半正定與正定實(shí)對(duì)稱常數(shù)矩陣,且Q為狀態(tài)變量加權(quán)矩陣,R為控制變量加權(quán)矩陣。性能指標(biāo)取極小值,根據(jù)Hamilton方程有:
最優(yōu)控制律為:
可得:
其中λ=Px。
本文使用Matlab 利用LQR(A,B,Q,R)函數(shù)直接對(duì)反饋系數(shù)求解,并驗(yàn)證LQR(A,B,Q,R)函數(shù)系統(tǒng)性能。
反步法控制被廣泛用于非線性系統(tǒng),核心思想是將復(fù)雜系統(tǒng)劃分為多個(gè)子系統(tǒng),且子系統(tǒng)數(shù)量不大于整個(gè)系統(tǒng)階數(shù),減少計(jì)算時(shí)間[18]。每個(gè)子系統(tǒng)通過中間虛擬量或Lyapunov 函數(shù)作遞歸構(gòu)造,解決非線性因素的不確定性影響[19]。
以滾動(dòng)通道為例,建立系統(tǒng)動(dòng)態(tài)方程:
式(15)中,f4表示一個(gè)有界故障,g4作為一個(gè)常數(shù),表示外界干擾,為限定條件,ρ3為外界干擾變化量上界。
狀態(tài)觀測(cè)器模型可簡(jiǎn)化為:
步驟一:取選定的Lyapunov函數(shù)V1:
根據(jù)Lyapunov 穩(wěn)定理論分析可得,由于V.
1 <0,為此增加其虛擬控制量,使其作為e1子系統(tǒng)的虛擬控制,另(c1>0為可調(diào)參數(shù)),則x2誤差e2為:
對(duì)式(17)求導(dǎo)并結(jié)合式(18)可得:
步驟二:構(gòu)造增廣Lyapunov函數(shù)V2:
式(20)中,ef=f4-ε4,其中f4表示已知滾轉(zhuǎn)角通道執(zhí)行器加性故障值,但f4值無法實(shí)際測(cè)量,在此使用ε4為估計(jì)值,定義積分項(xiàng)如下式:
對(duì)式(20)求導(dǎo)易知V.2<0,說明步驟二設(shè)計(jì)中系統(tǒng)漸近收斂,則:
基于反步法設(shè)計(jì)的控制器仍有不足,其被控對(duì)象要建立精準(zhǔn)的數(shù)學(xué)模型。與反步法控制相比,滑??刂莆锢砗?jiǎn)單且控制性能優(yōu)良。運(yùn)動(dòng)軌線被強(qiáng)迫為滑動(dòng)流行狀態(tài),并保持一段時(shí)間。滑模變結(jié)構(gòu)控制與常規(guī)控制相比具有不連續(xù)控制特點(diǎn),這種方式促使系統(tǒng)按照既定軌跡作高頻率、小幅度震蕩運(yùn)動(dòng),為“滑模”運(yùn)動(dòng)[20]。滑模運(yùn)動(dòng)的滑動(dòng)模態(tài)可認(rèn)為設(shè)計(jì)與外部環(huán)境參數(shù)變化及系統(tǒng)內(nèi)部環(huán)境變化無關(guān),具有良好魯棒性。如果將反步法控制與滑模法控制結(jié)合,則可增大控制系統(tǒng)使用范圍,對(duì)于控制律設(shè)計(jì)的魯棒性較好。本文依據(jù)滑模變結(jié)構(gòu)原理,采用指數(shù)趨近控制律改變趨近運(yùn)動(dòng)的動(dòng)態(tài)特性。具體設(shè)計(jì)如下:
與反步法控制一致,以滾轉(zhuǎn)通道為例,系統(tǒng)動(dòng)態(tài)方程為:
取滑模趨近率為:
滑??刂埔壮霈F(xiàn)抖動(dòng),將sgn(s)函數(shù)改為sat(s)飽和函數(shù)可減少系統(tǒng)運(yùn)行過程中抖動(dòng)。
對(duì)應(yīng)切換面為:
由于四旋翼具有欠驅(qū)動(dòng)、強(qiáng)耦合控制特點(diǎn),根據(jù)式(4)表示的故障模型方程設(shè)計(jì)容錯(cuò)控制器總體容錯(cuò)控制算法如圖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ī)控制。對(duì)于容錯(cuò)控制器設(shè)計(jì)分兩步。首先通過滑動(dòng)控制方法構(gòu)建滑模觀測(cè)器,有利于系統(tǒng)穩(wěn)定性分析;其次結(jié)合反步法控制,研究執(zhí)行器故障時(shí)容錯(cuò)控制器的設(shè)計(jì)。
圖2 容錯(cuò)控制結(jié)構(gòu)Fig.2 Fault-tolerant control structure
參照上述反步法控制器設(shè)計(jì)與滑模法控制器設(shè)計(jì),則反步滑??刂扑惴ㄝ斎肟刂茷椋?/p>
因此,其他通道電機(jī)輸入控制為:
在Matlab/Simulink環(huán)境下對(duì)所設(shè)計(jì)的控制算法分別在無故障干擾和有加性故障干擾情況下開展仿真驗(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
首先,根據(jù)以上模型建立,設(shè)計(jì)相關(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 個(gè)方向響應(yīng)狀態(tài),圖5 給出4 個(gè)電機(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可知,對(duì)于四旋翼系統(tǒng),在受到相同系統(tǒng)輸入時(shí),3種方法均可在一段時(shí)間趨于穩(wěn)定,具有良好控制效果。圖3a 中LQR 滾轉(zhuǎn)角收斂時(shí)間為200 s,圖3b中SMC滾轉(zhuǎn)角收斂時(shí)間為200 s,圖3c中IBC滾轉(zhuǎn)角收斂時(shí)間為200 s。3個(gè)控制算法穩(wěn)定時(shí)間一致,均可在短時(shí)間達(dá)到快速穩(wěn)定狀態(tài)。通過仿真曲線可知在趨于穩(wěn)定過程中,LQR和IBC響應(yīng)均未出現(xiàn)振蕩,SMC算法有較小抖動(dòng)。
圖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 時(shí)趨于0,響應(yīng)時(shí)間及各通道最大響應(yīng)角速度見表2,SMC算法最先到達(dá)最大角速度,以滾轉(zhuǎn)角為例,最大響應(yīng)為1.121 deg·s-1。IBC算法在82 s時(shí)角速度達(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表示四旋翼每個(gè)電機(jī)角速度響應(yīng)曲線,每種算法電機(jī)最大響應(yīng)值如表3 所示,3 種算法電機(jī)轉(zhuǎn)速在78、72、82 s處達(dá)到響應(yīng)峰值,這3個(gè)時(shí)間點(diǎn)分別對(duì)應(yīng)表2中姿態(tài)響應(yīng)時(shí)間,此時(shí)達(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
在實(shí)際飛行環(huán)境中,四旋翼無人機(jī)除系統(tǒng)本身出現(xiàn)故障,還會(huì)遇到外界干擾,如外界陣風(fēng)等。在仿真模型中,于150 s 時(shí)加入階躍響應(yīng),驗(yàn)證算法有效性。圖6~8為仿真結(jié)果,由此得到分析結(jié)果:
①由圖6~8 可知,在階躍響應(yīng)信號(hào)干擾下,四旋翼無人機(jī)姿態(tài)響應(yīng)、飛行速率、電機(jī)反作用力均受到一定程度影響,圖6~圖8a 中LQR 干擾試驗(yàn)四旋翼姿態(tài)響應(yīng)未收斂于平衡狀態(tài);
②從圖6~圖8b 中,SMC 算法在150 s 時(shí)加入干擾,系統(tǒng)幾乎未受影響,可快速恢復(fù)到平衡狀態(tài),魯棒性較好,但滾轉(zhuǎn)角在開始時(shí)有較小抖動(dòng);
圖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 時(shí)加入干擾,系統(tǒng)姿態(tài)角變化較小,俯仰角、滾轉(zhuǎn)角、偏航角所受干擾影響較小,姿態(tài)響應(yīng)產(chǎn)生較小抖動(dòng),電機(jī)響應(yīng)在150 s后產(chǎn)生抖動(dòng),在100 s內(nèi)趨于穩(wěn)定,調(diào)節(jié)時(shí)間短,偏差小,魯棒性較好。
基于以上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 時(shí)加入階躍響應(yīng)故障干擾信號(hào),姿態(tài)角幾乎未受影響,無抖動(dòng)和超調(diào)量,電機(jī)反作用姿態(tài)曲線較平穩(wěn),可知,反步滑??刂凭哂屑皶r(shí)響應(yīng)特性與良好魯棒性,有較強(qiáng)容錯(cuò)控制能力。
圖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)場(chǎng)試飛試驗(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 兩個(gè)軸的姿態(tài)誤差盡在±2 m·s-2,飛行控制較穩(wěn)定,震動(dòng)不明顯,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
針對(duì)植保無人機(jī)系統(tǒng)模型不確定或存在外界環(huán)境干擾情況下,導(dǎo)致作業(yè)時(shí)植保效率低下問題,對(duì)常用的3種控制算法開展仿真試驗(yàn)。
分析中得知,LQR 控制算法在受到加性故障干擾后飛行器姿態(tài)無法保證快速恢復(fù)到平衡穩(wěn)定狀態(tài);SMC 算法當(dāng)滑動(dòng)的軌跡到達(dá)滑模面后,易產(chǎn)生抖動(dòng);BSC算法在設(shè)計(jì)控制律的過程中對(duì)系統(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)場(chǎng)的飛行情況可知,植保無人機(jī)使用反步法模算法,姿態(tài)的軌跡跟蹤性好,響應(yīng)及時(shí),遇到外界風(fēng)力干擾時(shí)容錯(cuò)控制性強(qiáng),魯棒性良好,保證植保無人機(jī)能夠有效作業(yè),提高無人機(jī)植保的工作效率。